(57)【特許請求の範囲】
【請求項１】
対象物を運搬する複数のアーム部と、
複数の前記アーム部にそれぞれ設けられた、前記アーム部に作用する外力を検出する検出部と、
前記各検出部からの検出情報に基づき、複数の前記アーム部を制御する制御部と、
を備え、
前記制御部は、
複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、
前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
ことを特徴とするロボット制御装置。
【請求項２】
複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された前記所定の点は、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点である
ことを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記座標系の原点は、前記対象物に設定されている
ことを特徴とする請求項２に記載のロボット制御装置。
【請求項４】
前記制御部は、
前記Ｐの運動と前記Ｒの運動とを、複数の前記アーム部間に設定された仮想的なインピーダンスに基づいて算出し、
前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット制御装置。
【請求項５】
前記制御部は、
前記Ｐの運動と前記Ｒの運動とに基づき、複数の前記アーム部の各々の目標位置及び姿勢を算出し、
前記算出した複数の前記アーム部の各々の目標位置及び姿勢を示す情報に基づき、複数の前記アーム部を制御する
ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット制御装置。
【請求項６】
前記制御部は、
前記Ｐの運動と前記Ｒの運動とを、前記設定された仮想的なインピーダンスと前記対象物のインピーダンスとに基づき算出する
ことを特徴とする請求項１から請求項５のいずれか１項に記載のロボット制御装置。
【請求項７】
前記制御部は、
複数の前記アーム部が、前記対象物に対して押し付ける力が所望の値になるように、当該対象物のインピーダンスを考慮して、前記Ｒの運動の仮想的なインピーダンスを設定する
ことを特徴とする請求項１から請求項６のいずれか１項に記載のロボット制御装置。
【請求項８】
前記制御部は、
前記対象物が剛体である場合、前記対象物のインピーダンスを考慮せずに、前記Ｒの運動の仮想的なインピーダンスを設定し、前記各アーム部から前記対象物への所望の押し付け力を制御する
ことを特徴とする請求項７に記載のロボット制御装置。
【請求項９】
前記制御部は、
前記Ｐの運動とＲの運動とに基づき、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ１、ｐ２、・・・ｐｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御する
ことを特徴とする請求項１から請求項８のいずれか１項に記載のロボット制御装置。
【請求項１０】
前記制御部は、
複数の前記アーム部の基準点同士の相対位置に基づいて、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ１、ｐ２、・・・ｐｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御する
ことを特徴とする請求項１から請求項９のいずれか１項に記載のロボット制御装置。
【請求項１１】
前記制御部は、
複数の前記アーム部が、前記対象物を運搬する各々の予め定められた目標軌道に対する補正量を、前記Ｐの運動と前記Ｒの運動とに基づき算出し、算出した補正量に基づき複数の前記アーム部を制御する
ことを特徴とする請求項１から請求項１０のいずれか１項に記載のロボット制御装置。
【請求項１２】
前記制御部は、
前記Ｐの運動と前記Ｒの運動の振る舞いを定義した運動方程式を、時間に関する高次の多項式を用いて解くことで前記Ｐの運動と前記Ｒの運動とを算出する
ことを特徴とする請求項１から請求項１１のいずれか１項に記載のロボット制御装置。
【請求項１３】
複数の前記アーム部は、各々６自由度を有する６軸アーム部である
ことを特徴とする請求項１から請求項１２のいずれか１項に記載のロボット制御装置。
【請求項１４】
前記請求項１から請求項１３のいずれか１項のロボット制御装置を備える
ことを特徴とするロボット。
【請求項１５】
対象物を運搬する複数のアーム部を有するロボット制御装置におけるロボット制御方法において、
検出部が、複数の前記アーム部にそれぞれ設けられた、外力を検出する検出工程と、
制御部が、複数の前記アーム部を制御する制御工程と、
を有し、
前記制御工程は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
ことを特徴とするロボット制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置、ロボット、およびロボット制御方法に関する。
【背景技術】
【０００２】
近年、製造現場等では、スカラロボットや多軸ロボット等が、製品の組み立てや検査等に用いられている。製品の組み立てや検査を行うときに物体を運搬する場合、ロボットは、物体を吸着したり、アームにより把持したりする。
【０００３】
例えば、特許文献１には、２本のアームを有する双腕ロボットにおいて、物体を運搬するロボットの制御技術が開示されている。双腕ロボットは、２本のアームのうち、片方をマスターアーム、一方をスレーブアームに設定して、２本のアームに物体を把持させる。そして、２本のアームに物体を把持させた状態で、マスターアームは、物体を運搬する目標軌道に対し把持部に加わる外力を力センサで検出し、その値に応じて目標軌道を、例えばインピーダンス制御を用いて弾力的に変化させる。また、２本のアームに物体を把持させた状態で、スレーブアームは、把持部に加わる外力を力センサで検出し、その値に応じてマスターアームに追従する目標軌道を生成して、生成した軌道で物体を運搬する。
【０００４】
また、例えば、非特許文献１には、インピーダンス制御を行うことで、２本のアームの協調制御を行う制御技術が開示されている。
【０００５】
図３０は、特許文献１に記載の従来技術に係るロボット制御装置の動作の概要を説明する図である。図３０のように、ロボットの制御部９１１は、アーム１のコンプライアンス成分算出部９１１−１、メモリ９１１−２、アーム１の設定軌道算出部９１１−３、アーム１の目標位置算出部９１１−４、アーム２の現在位置推定部９１１−１１、及びアーム２の目標位置算出部９１１−１２から構成されている。
ロボットは、不図示の２本のアームに各々力センサ１、２（９６１、９６２）を有している。この力センサ１、２は、不図示の把持部に加わる外力を検出する。制御部９１１は、アーム１の力センサ１が検出した外力に基づき、コンプライアンス成分を算出する。なお、コンプライアンス成分とは、把持部の位置および把持部に作用する力に基づき、ロボットを制御するときに用いるコンプライアンスの成分である。
また、制御部９１１は、メモリ９１１−２に記憶されている情報に基づき、アーム１の予定されている軌道（設定軌道）を算出する。そして、制御部９１１は、算出されたコンプライアンス成分と設定軌道とを用いて、アーム１の位置などの目標値を算出する。制御部９１１は、算出された目標値に基づき、アーム１を制御している。一方、制御部９１１は、アーム２の力センサ２が検出した外力に基づき、アーム１の現在位置を推定する。制御部９１１は、推定されたアーム１の現在位置に基づき、アーム２を制御している。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−１８３８０号公報
【非特許文献】
【０００７】
【非特許文献１】小菅一弘, 吉田英博, 福田敏男, 蟹谷清, 酒井勝, 針木和夫, ”インピーダンス制御に基づく双腕マニピュレータの協調制御”, [日本ロボット学会誌 13 (3) (1995) 404-410]. K. Kosuge, H. Yoshida, T. Fukuda, Msaru Sakai, K. Kanitani, K. Hariki, ”Unified Control for Dynamic Cooperative Manipulation”, [Proceedings of the 1994 IEEE/RSJ International Workshop on Intelligent Robotics and Systems (1994) 1042-1047].
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１に記載の従来技術では、スレーブアームの目標軌道について、マスターアームの把持部と対向する方向と直行する方向については把持部と対象物の摩擦や、回転成分を考慮していないため、正確な軌道を生成することは困難である。このため、特許文献１に記載の従来技術では、対象物を落下させてしまう、もしくは対象物に過大な力を加えて破壊してしまうといった課題があった。また、特許文献１に記載の従来技術では、スレーブアームの目標軌道を把持部に加わる外力を用いて生成するため、対象物が落下した際には、外力が加わるようにマスターアームに向かう目標軌道が生成され、アーム同士が衝突してしまうといった課題があった。
更に、特許文献１に記載の従来技術では、マスターアーム及びスレーブアームそれぞれの運動が独立に設定されるため、それらの相対運動で決定される対象物（物体）に対する押し付け力を直接制御することが出来ない。このため、対象物を落下させてしまう、もしくは対象物に過大な力を加えて破壊してしまうといった課題があった。
【０００９】
本発明は、上記の課題に鑑みてなされたものであって、対象物を確実に把持して運搬するロボット制御装置、ロボット、およびロボット制御方法を提供することを目的としている。
【課題を解決するための手段】
【００１０】
上記目的を達成するため、本発明のロボット制御装置は、対象物を運搬する複数のアーム部と、複数の前記アーム部にそれぞれ設けられた、前記アーム部に作用する外力を検出する検出部と、前記各検出部からの検出情報に基づき、複数の前記アーム部を制御する制御部と、を備え、前記制御部は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御することを特徴としている。
【００１１】
本発明によれば、アーム部を含むアームが対象物を把持して搬送しているときに、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００１３】
本発明によれば、複数の前記アーム部のそれぞれの先端に対する位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を、複数の前記アーム部の作業空間上に任意の位置に設定するようにしたので、対象物を把持して所定の部品に組み付ける場合においても、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００１４】
上記目的を達成するため、本発明のロボット制御装置において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された前記所定の点は、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点であるようにしてもよい。
【００１５】
本発明によれば、複数の前記アーム部のそれぞれの先端に対する位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点の位置に設定するようにしたので、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができるロボット制御装置を提供できる。また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００１６】
上記目的を達成するため、本発明のロボット制御装置において、前記座標系の原点は、前記対象物に設定されているようにしてもよい。
【００１７】
本発明によれば、座標系の原点は、前記対象物に設定したので、アームが把持して運搬する所定の部品に組み付ける場合でも、部品の配置がずれていたりしても対象物を落下させないように各アーム部を制御することができ、対象物を確実に部品に組み付けることができるロボット制御装置を提供できる。
【００１８】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動を、複数の前記アーム部間に設定された仮想的なインピーダンスに基づいて算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御するようにしてもよい。
【００１９】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。
【００２０】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動とに基づき、複数の前記アーム部の各々の運動を算出し、前記算出した複数の前記アーム部の各々の運動に基づき、複数の前記アーム部を制御するようにしてもよい。
【００２１】
本発明によれば、アームが対象物を把持して搬送しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００２２】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動とを、前記設定された仮想的なインピーダンスと前記対象物のインピーダンスとに基づき算出するようにしてもよい。
【００２３】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように、かつ障害物に倣うような動作を行うように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。
【００２４】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部が、前記対象物に対して押し付ける力が所望の値になるように、当該対象物のインピーダンスを考慮して、前記Ｒの運動の仮想的なインピーダンスを設定するようにしてもよい。
【００２５】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように、かつ障害物に倣うような動作を行うように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。
【００２６】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記対象物が剛体である場合、前記対象物のインピーダンスを考慮せずに、前記Ｒの運動の仮想的なインピーダンスを設定し、前記各アーム部から前記対象物への所望の押し付け力を制御するようにしてもよい。
【００２７】
本発明によれば、複数のアーム部が、対象物に対して押し付ける力は、対象物の弾性係数ｋと無関係に決定されるため、弾性係数ｋが未知であっても、押し付け力ｆｈを設定し、それを満たす弾性係数を用いることで所望の押し付け力を実現することが出来き、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００２８】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動とＲの運動とに基づき、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ１、ｐ２、・・・ｐｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御するようにしてもよい。
【００２９】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００３０】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部の基準点同士の相対位置に基づいて、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ１、ｐ２、・・・ｐｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御するようにしてもよい。
【００３１】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００３２】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部が、前記対象物を運搬する各々の予め定められた目標軌道に対する補正量を、前記Ｐの運動と前記Ｒの運動とに基づき算出し、算出した補正量に基づき複数の前記アーム部を制御するようにしてもよい。
【００３３】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部の目標軌道を補正することで、各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。
【００３４】
上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動の振る舞いを定義した運動方程式を、時間に関する高次の多項式を用いて解くことで前記Ｐの運動と前記Ｒの運動とを算出するようにしてもよい。
【００３５】
本発明によれば、ルンゲクッタ法などの高次の多項式を用いて対象物の中心の位置及び姿勢Ｐと、前記対象物の相対の位置及び姿勢Ｒとを算出するようにしたので、制御周期が長い場合でも、精度の高い制御を行うことができるロボット制御装置を提供できる。ここで高次の多項式とは、一般的に４次以上である。例えばルンゲクッタ法であれば４次までとることで、一般的な物理現象の運動方程式を精度良く解くことが可能である。
【００３６】
上記目的を達成するため、本発明のロボット制御装置において、複数の前記アーム部は、各々６自由度を有する６軸アーム部であるようにしてもよい。
【００３７】
本発明によれば、ロボットのアーム部を各々６軸にしたため、６つの変数を制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）、姿勢（α、β、γ）に対応して制御することができるロボット制御装置を提供できる。
【００３８】
上記目的を達成するため、本発明のロボットは、前記請求項１から請求項１３のいずれか１項のロボット制御装置を備えることを特徴としている。
【００３９】
本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を備えるロボットを提供できる。
【００４０】
上記目的を達成するため、本発明は、対象物を運搬する複数のアーム部を有するロボット制御装置におけるロボット制御方法において、検出部が、複数の前記アーム部にそれぞれ設けられた、外力を検出する検出工程と、制御部が、複数の前記アーム部を制御する制御工程と、を有し、前記制御工程は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御することを特徴としている。
【００４１】
本発明によれば、アーム部を含むアームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御方法を提供できる。
【図面の簡単な説明】
【００４２】
【図１】第１実施形態に係るロボット１の概略斜視図である。
【図２】同実施形態に係るアーム部２０−１及び２０−２と、第１力センサ６１及び第２力センサ６２の概念図である。
【図３】同実施形態に係る制御部１１のブロック図である。
【図４】同実施形態に係るロボット制御方法を適用するロボット１のアームの概念図である。
【図５】同実施形態に係るインピーダンス制御の概念を説明する図である。
【図６】同実施形態に係る対象物が障害物に当たった場合を説明する図である。
【図７】同実施形態に係る対象物が障害物に当たった後、各アームが制御される場合を説明する図である。
【図８】同実施形態に係る対象物１３０の斜視図である。
【図９】同実施形態に係る対象物１３０と障害物１４０との位置関係を説明する図である。
【図１０】同実施形態に係る障害物１４０が対象物１３０を押す力を説明する図である。
【図１１】同実施形態に係るアーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクを説明する図である。
【図１２】同実施形態に係るベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動を説明する図である。
【図１３】同実施形態に係るベクトルＰの姿勢成分ベクトルＰｄの運動について説明する図である。
【図１４】同実施形態に係る回転に応じた手先の中心位置及び姿勢を示す情報のベクトルＲの運動について説明する図である。
【図１５】同実施形態に係るインピーダンスの制御変数を説明する図である。
【図１６】同実施形態に係るΔＲｘ及びｆｘの振る舞いを説明する図である。
【図１７】同実施形態に係るＲＴｘを５次式で表した場合のΔＲｘ及びｆｘの振る舞いを表す図である。
【図１８】同実施形態に係る動作の一例を説明するフローチャートである。
【図１９】第２実施形態に係る対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。
【図２０】同実施形態に係るフレームがずれた場合、対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。
【図２１】第３実施形態に係る対象物（部品）を把持して、所定の部品３１１に組み付ける一例を説明する図である。
【図２２】第４実施形態に係る３本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。
【図２３】同実施形態に係る３本のアームによる座標系を説明する図である。
【図２４】同実施形態に係る制御部４１１のブロック図である。
【図２５】第５実施形態に係る３本のアームによる座標系を説明する図である。
【図２６】第６実施形態に係る３本のアームによる座標系を説明する図である。
【図２７】第７実施形態に係る４本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。
【図２８】同実施形態に係る４本のアームによる座標系を説明する図である。
【図２９】第８実施形態に係るｎ本のアームによる座標系を説明する図である。
【図３０】従来技術に係るロボット制御装置９１１の動作の概要を説明する図である。
【発明を実施するための形態】
【００４３】
以下、本発明の実施形態について図面を参照して詳細に説明する。なお、本発明は斯かる実施形態に限定されず、その技術思想の範囲内で種々の変更が可能である。
【００４４】
［第１実施形態］
図１は、本実施形態に係るロボット１の概略斜視図である。図１に示すように、ロボット１は、２本のアーム２、３を備えている。アーム２は、固定部１０、アーム部２０−１、第１力センサ６１、及び指部４０−１を備える。また、アーム３は、固定部１０、アーム部２０−２、第２力センサ６２、及び指部４０−２を備える。
【００４５】
固定部１０は、例えば床、壁、天井、移動可能な台車の上などに固定される。固定部１０は、内部に、制御部１１（破線にて図示）を備える。
制御部１１は、アーム部２０−１及び２０−２、指部４０−１及び４０−２を制御する。制御部１１の詳細は後述する。なお、制御部１１は、固定部１０の内部ではなく、固定部１０の外部に設けるようにしてもよい。
【００４６】
アーム部２０−１及び２０−２は、第１フレーム２１、第２フレーム２２、第３フレーム２３、第４フレーム２４および第５フレーム２５から構成されている。第１フレーム２１は、回転屈折軸を介して、固定部１０に回転可能または屈折可能に接続されている。第２フレーム２２は、回転屈折軸を介して、第１フレーム２１および第３フレーム２３に接続されている。第３フレーム２３は、回転屈折軸を介して、第２フレーム２２および第４フレーム２４に接続されている。第４フレーム２４は、回転屈折軸を介して、第３フレーム２３および第５フレーム２５に接続されている。第５フレーム２５は、回転屈折軸を介して、第４フレーム２４に接続されている。アーム部２０は、制御部１１の制御によって、各フレーム２１〜２５が各回転屈折軸を中心に複合的に回転または屈折し、動く。
【００４７】
アーム部２０−１の第５フレームのうち第４フレームが設けられた他方には、指部４０−１が設けられている。アーム部２０−２の第５フレームのうち第４フレームが設けられた他方には、指部４０−２が設けられている。指部４０−１及び４０−２は、直接的に、対象物を把持する。なお、指部４０−１及び４０−２は、例えば、複数のフレームを有し、対象物を把持できる構造である。
【００４８】
アーム部２０−１の第５フレーム２５と指部４０−１の間には、第１力センサ６１が介挿されている。第１力センサ６１は、アーム２の指部４０−１に加わる力の各空間成分（力成分という）とトルクの各空間成分（トルク成分という）を検出する。第１力センサ６１は、検出したアーム２の指部４０−１に加わる力成分とトルク成分を含む情報を制御部１１に出力する。
また、アーム部２０−２の第５フレーム２５と指部４０−２の間には、第２力センサ６２が介挿されている。第２センサ６２は、アーム３の指部４０−２に加わる力成分とトルク成分を検出する。第２力センサ６２は、検出したアーム３の指部４０−２に加わる力成分とトルク成分を含む情報を制御部１１に出力する。
【００４９】
図２は、本実施形態に係るアーム部２０−１及び２０−２と、第１力センサ６１及び第２力センサ６２の概念図である。図２のように、アーム２のアーム部２０−１は、回転部５２、５３及び５５と、屈折部５１、５４及び５６とを有している。そして、アーム２のアーム部２０−２は、指部４０−１との間に第１力センサ６１を有している。
また、アーム３のアーム部２０−１は、回転部５２、５３及び５５と、屈折部５１、５４及び５６とを有している。そして、アーム３のアーム部２０−２は、指部４０−２との間に第２力センサ６２を有している。
また、屈折部５１は、固定部１０と第１フレーム２１との間に設けられている。回転部５２は、第１フレーム２１と第２フレーム２２との間に設けられている。回転部５３は、第２フレーム２２と第３フレーム２３との間に設けられている。屈折部５４は、第３フレーム２３と第４フレーム２４との間に設けられている。回転部５５は、第４フレーム２４と第５フレーム２５との間に設けられている。屈折部５４は、第５フレームのうち第４フレームが設けられた他方に設けられている。
図２のように、アーム部２０−１及び２０−２は、６自由度を有する６軸アームである。すなわち、アーム部２０−１の先端の指部４０−１は、これらの各軸の回転部と屈折部の動作により、移動する。そして、指部４０−１が対象物１３０を把持している場合、第１力センサ６１が検出した外力には、このような各軸の回転部と屈折部の動作による成分が含まれている。このため、アーム部２０−１及びアーム２０−２が６軸アームであるため、６つの変数で制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）の３つの変数（成分）、姿勢（α、β、γ）の３つの成分（変数）に対応して制御することができる。
【００５０】
図３は、本実施形態に係る制御部１１のブロック図である。図３のように、制御部１１は、中心運動に対してはたらく力の算出部１１−１、中心運動のコンプライアンス成分算出部１１−２、第１記憶部１１−３、中心運動の設定軌道算出部１１−４、中心運動の目標位置算出部１１−５、アーム１１０の目標位置算出部１１−６、相対運動に対してはたらく力の算出部１１−１１、相対運動のコンプライアンス成分算出部１１−１２、第２記憶部１１−１３、相対運動の設定軌道算出部１１−１４、相対運動の目標位置算出部１１−１５、及びアーム１２０の目標位置算出部１１−１６を備えている。また、制御部１１は、不図示の内部タイマーを備えている。なお、アーム１１０とは、図１および図２において第１力センサ６１が取り付けられているアーム部２０−１である。また、アーム１２０とは、図１および図２において第２力センサ６２が取り付けられているアーム部２０−２である。
なお、各算出部が行う算出手順については、後述する。また、本実施形態では、第１記憶部１１−３及び１１−１３のように記憶部を２つ備える例を説明したが、記憶部は１つでもよい。
【００５１】
中心運動に対してはたらく力の算出部１１−１には、各アームに取り付けられている第１力センサ６１（検出部）及び第２力センサ６２（検出部）から、力成分とトルク成分を含む情報が入力される。中心運動に対してはたらく力の算出部１１−１は、後述するように、入力された力成分とトルク成分を含む情報に基づき、対象物１３０の中心運動に対してはたらく力を算出する。中心運動に対してはたらく力の算出部１１−１は、算出した対象物１３０の中心運動に対してはたらく力を中心運動のコンプライアンス成分算出部１１−２に出力する。
なお、対象物１３０の中心運動に対してはたらく力とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心において働く力である。すなわち、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２とが、対象物１３０を把持している場合、把持されている対象物１３０の中心がどのように運動するか、あるいは運動しているかを示すものである。
【００５２】
中心運動のコンプライアンス成分算出部１１−２は、後述するように、中心運動に対してはたらく力の算出部１１−１から出力された対象物１３０の中心運動に対してはたらく力を示す情報に基づき、対象物１３０の中心運動のコンプライアンス成分を算出する。中心運動のコンプライアンス成分算出部１１−２は、算出した対象物１３０の中心運動のコンプライアンス成分を中心運動の目標位置算出部１１−５に出力する。
なお、中心運動のコンプライアンス成分とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心におけるコンプライアンスの各成分である。また、コンプライアンスの各成分とは、後述するアーム２の手先とアーム３の手先との中心位置成分と姿勢成分である。また、コンプライアンス（ｃｏｍｐｌｉａｎｃｅ）とは、剛性の逆数のことであり、固定方法が剛結ではなく、入力に対し、ある程度の変形・変位を許す状態である。
【００５３】
第１記憶部１１−３には、予め対象物１３０に対する中心運動の軌道に関する情報が記憶されている。
【００５４】
中心運動の設定軌道算出部１１−４は、第１記憶部１１−３に記憶されている対象物１３０に対する中心運動の軌道に関する情報を読み出し、読み出した情報に基づき、後述するように、対象物１３０に対する中心運動の軌道を算出する。中心運動の設定軌道算出部１１−４は、算出した対象物１３０の中心運動の軌道を中心運動の目標位置算出部１１−５に出力する。
なお、中心運動の設定軌道とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心の搬送時の軌道である。
【００５５】
中心運動の目標位置算出部１１−５には、中心運動のコンプライアンス成分算出部１１−２から出力された対象物１３０の中心運動のコンプライアンス成分を示す情報と、中心運動の設定軌道算出部１１−４から出力された対象物１３０の中心運動の設定軌道を示す情報とが入力される。中心運動の目標位置算出部１１−５は、入力された対象物１３０の中心運動のコンプライアンス成分を示す情報と、対象物１３０の中心運動の設定軌道を示す情報とに基づき、後述するように、対象物１３０の中心運動の目標値を算出する。中心運動の目標位置算出部１１−５は、算出した対象物１３０の中心運動の目標値をアーム１１０の目標位置算出部１１−６及びアーム１２０の目標位置算出部１１−１６に出力する。
なお、中心運動の目標値とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心の搬送時の位置及び姿勢の目標制御値である。
【００５６】
アーム１１０の目標位置算出部１１−６には、中心運動の目標位置算出部１１−５から対象物１３０の中心運動の目標値を示す情報と、相対運動の目標位置算出部１１−１５から対象物１３０の相対運動の目標値を示す情報とが入力される。アーム１１０の目標位置算出部１１−６は、入力された対象物１３０の中心運動の目標値を示す情報と、対象物１３０の相対運動の目標値を示す情報とに基づき、後述するように、アーム１１０の目標値を算出する。アーム１１０の目標位置算出部１１−６は、算出したアーム１１０の目標値をアーム１１０の制御信号として、アーム１１０に出力する。
なお、アーム１１０の目標値とは、アーム１１０の先端の指部４０−１が対象物１３０の動作に追従して動作する位置及び姿勢を示すアーム１１０への制御量の目標値である。
【００５７】
相対運動に対してはたらく力の算出部１１−１１には、各アームに取り付けられている第１力センサ６１及び第２力センサ６２から、力成分とトルク成分を含む情報が入力される。相対運動に対してはたらく力の算出部１１−１１は、入力された力成分とトルク成分を含む情報の出力とに基づき、後述するように、相対運動に対してはたらく力を算出する。相対運動に対してはたらく力の算出部１１−１１は、算出した相対運動に対してはたらく力を相対運動のコンプライアンス成分算出部１１−１２に出力する。
なお、相対運動に対してはたらく力とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の相対的な力である。すなわち、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２とが、対象物１３０を把持している場合、アーム部２０−１の先端の指部４０−１に対して、アーム部２０−２の先端の指部４０−２がどのように運動するか、あるいは運動しているかを示すものである。
【００５８】
相対運動のコンプライアンス成分算出部１１−１２は、相対運動に対してはたらく力の算出部１１−１１から出力された対象物１３０の相対運動に対してはたらく力を示す情報に基づき、後述するように、対象物１３０の相対運動のコンプライアンス成分を算出する。相対運動のコンプライアンス成分算出部１１−１２は、算出した対象物１３０の相対運動のコンプライアンス成分を相対運動の目標位置算出部１１−１５に出力する。
なお、相対運動のコンプライアンス成分とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２のコンプライアンスの各成分である。また、コンプライアンスの各成分とは、後述するアーム２とアーム３との手先の相対位置成分と姿勢成分である。
【００５９】
第２記憶部１１−１３には、予め相対運動の軌道に関する情報が記憶されている。
【００６０】
相対運動の設定軌道算出部１１−１４は、第２記憶部１１−１３に記憶されている対象物１３０に対する相対運動の軌道に関する情報を読み出し、読み出した情報に基づき、後述するように、対象物１３０に対する相対運動の軌道を算出する。相対運動の設定軌道算出部１１−１４は、算出した対象物１３０の相対運動の軌道を相対運動の目標位置算出部１１−１５に出力する。
なお、相対運動の設定軌道とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の搬送時の軌道である。
【００６１】
相対運動の目標位置算出部１１−１５には、相対運動のコンプライアンス成分算出部１１−１２から出力された対象物１３０の相対運動のコンプライアンス成分を示す情報と、相対運動の設定軌道算出部１１−１４から出力された対象物１３０の相対運動の設定軌道を示す情報とが入力される。相対運動の目標位置算出部１１−１５は、入力された対象物１３０の相対運動のコンプライアンス成分を示す情報と、対象物１３０の相対運動の設定軌道を示す情報とに基づき、後述するように、対象物１３０の相対運動の目標値を算出する。相対運動の目標位置算出部１１−１５は、算出した対象物１３０の相対運動の目標値をアーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する。
なお、相対運動の目標値とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の搬送時の位置及び姿勢の目標制御値である。
【００６２】
アーム１２０の目標位置算出部１１−１６には、中心運動の目標位置算出部１１−５から対象物１３０の中心運動の目標値を示す情報と、相対運動の目標位置算出部１１−１５から対象物１３０の相対運動の目標値を示す情報とが入力される。アーム１２０の目標位置算出部１１−１６は、入力された対象物１３０の中心運動の目標値を示す情報と、対象物１３０の相対運動の目標値を示す情報とに基づき、後述するように、アーム１２０の目標値を算出する。アーム１２０の目標位置算出部１１−１６は、算出したアーム１２０の目標値をアーム１２０の制御信号として、アーム１２０に出力する。
なお、アーム１２０の目標値とは、アーム１２０の先端の指部４０−２が対象物１３０の動作に追従して動作する位置及び姿勢を示すアーム１２０への制御量の目標値である。
【００６３】
なお、本実施形態においては、対象物１３０は、密度が一様であり、対象物１３０の中心が重心と一致している場合についての例について説明を行っている。
【００６４】
［本実施形態の概要の説明］
次に、本実施形態に係るアーム１１０及びアーム１２０の制御の概要について、図４を用いて説明する。図４は、本実施形態のロボット制御方法を適用するロボット１のアームの概念図である。なお、図４のように、アーム１１０は、図１の一方のアーム部２０−１を模式的に表し、図１の指部４０−１を模式的に表した把持部１１１を有している。また、アーム１２０は、図１の他方のアーム部２０−２を模式的に表し、図１の指部４０−２を模式的に表した把持部１２１を有している。対象物１３０は、アーム１１０とアーム１２０により把持され、運搬される物体である。
【００６５】
図４において、ロボット１は、３つの座標系Σ１、Σ２、Σを有している。座標系Σ１（Ｏ１−ｘ１ｙ１ｚ１）は、アーム１１０の運動を記述するベース座標系であり、アーム１１０の手先の位置及び姿勢をベクトルｐ１で表す。また、座標系Σ２（Ｏ２−ｘ２ｙ２ｚ２）は、アーム１２０の運動を記述するベース座標系であり、アーム１２０の手先の位置及び姿勢をベクトルｐ２で表す。図４において、符号Ｏ１は、座標系Σ１の原点Ｏ１であり、符号Ｏ２は、座標系Σ２の原点Ｏ２である。
以下で説明する各式（１）〜式（２０）は、制御部１１がアーム１１０及び１２０の目標値を算出するときに用いる式であり、予め制御部１１内の後述する算出部に定義されて記憶されている。
制御部１１は、ベクトルｐ１を、位置成分ベクトルｐ１ｐと、姿勢成分ベクトルｐ１ｄとにより、次式（１）式のように表される。また、制御部１１は、ベクトルｐ２を、位置成分ベクトルのｐ２ｐと、姿勢成分ベクトルｐ２ｄとにより、次式（２）式のように表される。
また、符号α、β及びγは、３次元空間におけるＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角である。
【００６６】
【数１】
0005825589.tif 000002
【００６７】
【数２】
0005825589.tif 000003
【００６８】
式（１）と式（２）において、記号Ｔは、転置行列を表している。また、以下の式でも同様に、記号Ｔは、転置行列を表している。また、式（１）において、ｐ１ｘ、ｐ１ｙ及びｐ１ｚは、ベクトルｐ１ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１）において、ｐ１α、ｐ１β及びｐ１γは、ベクトルｐ１ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
式（２）において、ｐ２ｘ、ｐ２ｙ及びｐ２ｚは、ベクトルｐ２ｐのｘ軸成分、ｙ軸成分及びｚ軸成分であるまた、式（２）において、ｐ２α、ｐ２β及びｐ２γは、ベクトルｐ２ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
【００６９】
また、座標系Σ（Ｏ−ｘｙｚ）は、アーム１１０及びアーム１２０の運動を統一して記述する作業用座標系であり、この座標系においてアーム１１０の手先とアーム１２０の手先との中心の位置成分及び姿勢成分（以下、アームの手先の中心の位置及び姿勢という）を、ベクトルＰで表す。すなわち、ベクトルＰは、アーム１１０及びアーム１２０のそれぞれの先端の把持部１１１と把持部１２１との中心の位置及び姿勢を示す情報である。図４において、符号Ｏは、座標系Σの原点Ｏである。
また、この座標系Σにおいてアーム１１０の手先に対するアーム１２０の相対的な手先位置成分及び姿勢成分（以下、アームの手先同士の相対の位置及び姿勢という）を、ベクトルＲで表す。すなわち、ベクトルＲは、アーム１１０の先端の把持部１１１に対するアーム１２０の把持部１２１の相対の位置及び姿勢を示す情報である。
このような２本のアーム１１０及び１２０のそれぞれの先端から中心の位置をコンプライアンスの中心（剛性中心）、コンプライアンス・センターという。本実施形態では、対象物１３０の中心、すなわちベクトルＰをベクトルＲの中間にした例である。
ベクトルＰは、位置成分ベクトルＰｐと、姿勢成分ベクトルＰｄとにより、次式（３）式のように表される。また、ベクトルＲは、位置成分ベクトルＲｐと、姿勢成分ベクトルＲｄとにより、次式（４）式のように表される。
【００７０】
【数３】
0005825589.tif 000004
【００７１】
【数４】
0005825589.tif 000005
【００７２】
式（３）において、Ｐｘ、Ｐｙ及びＰｚは、ベクトルＰｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（３）において、Ｐα、Ｐβ及びＰγは、ベクトルＰｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
式（４）において、Ｐｘ、Ｐｙ及びＰｚは、ベクトルＰｐのｘ軸成分、ｙ軸成分及びｚ軸成分であるまた、式（４）において、Ｐα、Ｐβ及びＰγは、ベクトルＰｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
【００７３】
なお、図４のように、各座標系Σ１、Σ２およびΣの相対位置および姿勢は、３つの座標系Σ１、Σ２及びΣのｘ軸が同一直線上にあり、ｙ軸及びｚ軸の向きが等しく、作業座標系において原点間（Ｏ１、Ｏ２及びＯ）の関係が次式（５）を満たすように表される。
ただし、図４に示した各座標系の相対位置関係は一例であり、任意に配置することが可能である。
【００７４】
【数５】
0005825589.tif 000006
【００７５】
次に、第１力センサ６１から出力は、力成分ベクトルｆ１ｐとトルク成分ベクトルｆ１ｄとにより、次式（６）のように表される。また、第２力センサ６２の出力は、力成分ベクトルｆ２ｐとトルク成分ベクトルｆ２ｄとにより、次式（７）のように表される。
【００７６】
【数６】
0005825589.tif 000007
【００７７】
【数７】
0005825589.tif 000008
【００７８】
式（６）において、ｆ１ｘ、ｆ１ｙ及びｆ１ｚは、ベクトルｆ１ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（６）において、τ１α、τ１β及びτ１γは、アーム１１０のトルク成分ベクトルｆ１ｄを、ＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角で表したものである。
式（７）において、ｆ２ｘ、ｆ２ｙ及びｆ２ｚは、ベクトルｆ２ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（７）において、τ２α、τ２β及びτ２γは、アーム１２０のトルク成分ベクトルｆ２ｄを、ＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角で表したものである。
【００７９】
次に、これらを用いて、アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰに加わる力ベクトルＦは、次式（８）のように表される。また、アームの手先同士の相対位置及び姿勢を示す情報であるベクトルＲに加わる力ベクトルｆは、次式（９）のように表される。なお、説明を簡単にするため、アーム１１０と１２０の各力センサの出力ベクトルｆ１及びベクトルｆ２については、把持部に加わる重力、対象物１３０に加わる重力、及び把持部４０−１と４０−２とが加速度運動することによって加わる慣性力は取り除かれていて、更に作業座標系における表現に変換されているものとする。
まず、力ベクトルＦを、次式（８）のように表す。
【００８０】
【数８】
0005825589.tif 000009
【００８１】
式（８）において、Ｆｘ、Ｆｙ及びＦｚは、力ベクトルＦの位置成分ベクトルＦｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（８）において、Ｔα、Ｔβ及びＴγは、力ベクトルＦの姿勢成分ベクトルＦｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。なお、式（８）において、手先を対向させて物体を把持するため、互いに回転角γの回転方向が逆になるため、τγ＝−τ２γ−τ１γとなる。
【００８２】
次に、力ベクトルｆを、次式（９）のように表す。
【００８３】
【数９】
0005825589.tif 000010
【００８４】
式（９）において、ｆｘ、ｆｙ及びｆｚは、ベクトルｆｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（９）において、τα、τβ及びτγは、トルク成分ベクトルｆｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
【００８５】
アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰは、把持した対象物１３０の位置及び姿勢を制御して搬送するようにあらかじめ設定された軌道ベクトルＰＴに対して、上述した力ベクトルＦに応じてベクトルΔＰだけ軌道を変更したものとして表される。これにより、例えば障害物１４０などから物体に加わる力に応じて、受動的にその位置及び姿勢を変更してコンプライアンスを守りながら、所定の位置及び姿勢に物体を操作することを可能とする。
【００８６】
また、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲは、物体を把持するようにあらかじめ設定された軌道ベクトルＲＴに対して、上述した力ベクトルｆに応じてベクトルΔＲだけ軌道を変更したものとして表される。これにより、アームが物体に加える力について、コンプライアンスを守りながら物体を把持することを可能とする。従って、ある時刻ｔにおけるアームの手先の中心の位置及び姿勢を示す情報であるベクトルＰ、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲは、次式（１０）と次式（１１）のように表される。
【００８７】
【数１０】
0005825589.tif 000011
【００８８】
【数１１】
0005825589.tif 000012
【００８９】
式（１０）において、ベクトルΔＰは、障害物１４０に加わる外力に基づき、力ベクトルＦに応じて変更した軌道を示すベクトルである。式（１１）において、ベクトルΔＲは、障害物１４０に加える内力に基づき、力ベクトルｆに応じて、変更した軌道を示すベクトルである。
次に、式（１０）と式（１１）において、ベクトルＰＴおよびベクトルＲＴは、各々、次式（１２）と次式（１３）のように、位置成分と姿勢成分とに分けて表される。
【００９０】
【数１２】
0005825589.tif 000013
【００９１】
【数１３】
0005825589.tif 000014
【００９２】
式（１２）において、ＰＴｘ、ＰＴｙ及びＴＴｚは、ベクトルＰＴｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１２）において、ＰＴα、ＰＴβ及びＰＴγは、トルク成分ベクトルＰＴｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
式（１３）において、ＲＴｘ、ＲＴｙ及びＲＴｚは、ベクトルＲＴｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１３）において、ＲＴα、ＲＴβ及びＲＴγは、トルク成分ベクトルＲＴｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
【００９３】
同様に、式（１０）と式（１１）において、ベクトルΔＰおよびベクトルΔＲは、各々、次式（１４）と次式（１５）のように、位置成分と姿勢成分とに分けて表される。
【００９４】
【数１４】
0005825589.tif 000015
【００９５】
【数１５】
0005825589.tif 000016
【００９６】
式（１４）において、ΔＰｘ、ΔＰｙ及びΔＴｚは、ベクトルΔＰｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１４）において、ΔＰα、ΔＰβ及びΔＰγは、トルク成分ベクトルΔＰｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
式（１５）において、ΔＲｘ、ΔＲｙ及びΔＲｚは、ベクトルΔＲｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１５）において、ΔＲα、ΔＲβ及びΔＲγは、トルク成分ベクトルΔＲｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
【００９７】
次に、コンプライアンス制御の例としてインピーダンス制御について説明する。図５は、インピーダンス制御の概念を説明する図である。インピーダンス制御とは、図５のように、指部４０−１と指部４０−２との間に、仮想的な弾性係数ｋを有するばね、粘性係数Ｄを有するダンパー、重量Ｍを有する物体があるかのうように指部４０−１及び指部４０−２の運動を制御することである。図５のように、指部４０−１には、ばねの一端とダンパーの一端とが接続されている。ばねの他端とダンパーの他端は、物体の一端に接続されている。また、物体の他端は、指部４０−２に接続されている。
【００９８】
インピーダンス制御においては、ベクトルΔＰおよびベクトルΔＲをそれぞれベクトルＦ及びベクトルｆに応じて、仮想的なインピーダンスのもとで運動する対象物１３０の位置において設定する。ここで、インピーダンスは慣性力、ダンパーなどのように速度に依存して発生する力及びバネなどのように位置に依存して発生する力で構成されるとすると、ベクトルＰ及びベクトルＲは、次式（１６）と次式（１７）の運動方程式に従う。
【００９９】
【数１６】
0005825589.tif 000017
【０１００】
【数１７】
0005825589.tif 000018
【０１０１】
式（１６）において、Ｍｐ＾は、ベクトルＦの慣性行列（慣性係数）、Ｄｐ＾は、ベクトルＦの粘性行列（粘性係数）、Ｋｐ＾は、ベクトルＦの弾性行列（弾性係数）である。なお、式（１６）において、上付きに＾が表示されている各係数を、それぞれ便宜的にＭｐ＾、Ｄｐ＾およびＫｐ＾と表す。
また、式（１７）において、ＭＲ＾は、ベクトルｆの慣性行列（慣性係数）、ＤＲ＾は、ベクトルｆの粘性行列（粘性係数）、ＫＲ＾は、ベクトルｆの弾性行列（弾性係数）である。なお、式（１７）において、上に＾が表示されている各係数を、それぞれ便宜的にＭＲ＾、ＤＲ＾およびＫＲ＾と表す。なお、Ｐの運動とＲの運動の振る舞いを定義した運動方程式とは、式（１６）および式（１７）である。
各係数Ｍｐ＾、Ｄｐ＾、Ｋｐ＾、ＭＲ＾、ＤＲ＾およびＫＲ＾は、６行６列の対角行列である。対角成分にそれぞれ独立の値を設定することで、アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰとアームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲとの各軸に対する運動を、個別に制御することが可能となる。ただし、これは一例であり、Ｍｐ＾、Ｄｐ＾、Ｋｐ＾、ＭＲ＾、ＤＲ＾およびＫＲ＾は非対角要素を持つ行列であっても構わない。
なお、制御部１１は、仮想的なインピーダンスに加え、対象物のインピーダンスを用いて算出するようにしてもよい。
【０１０２】
次に、制御部１１は、式（１６）と式（１７）を解き、ベクトルΔＰとベクトルΔＲを求める。次に、制御部１１は、式（１０）および式（１１）に、求めたベクトルΔＰとベクトルΔＲを代入して、ベクトルＰとベクトルＲを求める。
次に、制御部１１は、求めたベクトルＰとベクトルＲを次式（１８）と次式（１９）に代入し、アーム１２０のベース座標系における手先位置及び姿勢を示す情報であるベクトルｐ１、アーム１２０のベース座標系における手先位置及び姿勢を示す情報であるベクトルｐ２を求める。
【０１０３】
【数１８】
0005825589.tif 000019
【０１０４】
【数１９】
0005825589.tif 000020
【０１０５】
式（１８）と式（１９）において、ベクトルＤは、次式（２０）で表される。
【０１０６】
【数２０】
0005825589.tif 000021
【０１０７】
式（２０）におけるＤは、図１のように各アーム１１０の原点Ｏ１または原点Ｏ２とΣの原点Ｏの間隔である。
制御部１１は、アーム部１１０と把持部１１１、アーム部１２０と把持部１２１に対して式（１８）及び式（１９）で求めた位置に移動するよう指令を出す。以上が本実施形態に係る制御の概要である。
【０１０８】
［障害物から対象物に力が加わった場合の説明］
次に、障害物から対象物に力が加わった場合の説明を、図６〜図１７を用いて行う。図６は、本実施形態に係る対象物が障害物に当たった場合を説明する図であり、図７は、本実施形態に係る対象物が障害物に当たった後、各アームが制御される場合を説明する図である。
【０１０９】
図６においては、アーム１１０とアーム１２０が対象物１３０を把持した状態で、対象物１３０に障害物１４０が衝突する。この場合、以下に詳細に説明する制御により、図７のように障害物１４０から受ける力に応じて、制御部１１の制御により、受動的にその位置及び姿勢を変更する。
【０１１０】
まず、対象物１３０を把持して操作している際に、対象物１３０が障害物１４０と接触した時に発生する力及びトルクについて、以下に説明する。
図６および図７において、アーム１１０及びアーム１２０の把持部１１１及び把持部１２１は、アーム１１０の手先の把持部１１１とアーム１２０の手先の把持部１２１との中心位置と、対象物１３０の中心が一致するように対象物１３０を把持している。ここで、対象物１３０を把持した状態で、アームの手先の中心の位置及び姿勢について、軌道ベクトルＰＴを、制御部１１は、式（１２）に基づき、次式（２１）のように算出する。なお、対象物１３０の中心がアーム１１０により搬送される軌道は、予めロボット制御装置１のオペレータにより各時刻における移動位置、移動速度が軌道に関する情報として設定される。そして、オペレータにより設定された軌道に関する情報が、予め制御部１１の第１記憶部１１−３に記憶されている。
【０１１１】
【数２１】
0005825589.tif 000022
【０１１２】
式（２１）の軌道ベクトルＰＴは、アームの手先の中心の位置及び姿勢について、ｘ軸及びｙ軸方向の位置及び姿勢を変化させずに、ｙ軸方向の位置を初期位置ｙ０から速度ｖで移動させる軌道である。
次に、アームの手先同士の相対位置及び姿勢について、対象物１３０を把持している状態において、軌道ベクトルＲＴを、制御部１１は、式（１３）に基づき、次式（２２）のように算出する。なお、搬送される対象物１３０において、アーム１１０に対するアーム１２０の手先同士の相互位置の軌道は、予めロボット制御装置１のオペレータにより各時刻における移動位置、移動速度が軌道に関する情報として設定される。そして、オペレータにより設定された相対的な軌道に関する情報が、予め制御部１１の第２記憶部１１−１３に記憶されている。
【０１１３】
【数２２】
0005825589.tif 000023
【０１１４】
式（２２）において、ｗ０は、アーム１１０及び１２０が、対象物１３０に加える力に応じて、制御部１１が選択する値であり、選択については後述する。
なお、対象物１３０は、図８の対象物１３０の斜視図に示すように幅ｗ、高さｈ、奥行きｌ、質量ｍの直方体であり、質量分布が一様でその重心が物体の中心にある。また、対象物１３０の中心を通り、作業座標系Σのｚ軸と平行な直線をｃとする。すなわち、対象物１３０の中心は、幅ｗ、高さｈ、奥行きｌ、質量ｍのそれぞれの中心が重なる位置である。
図９は、本実施形態に係る対象物１３０と障害物１４０との位置関係を説明する図である。障害物１４０は円筒形であり、対象物１３０と障害物１４０とは、図９のような位置関係で接している。すなわち、ｘｚ平面における対象物１３０の面は、障害物１４０の円周面と接する。より具体的には、ｘｙ平面の長手方向に対して、障害物１４０の長手方向が、直交する向きで接する。
図６に示したように、式（２１）の軌道に従って、制御部１１は、対象物１３０の位置を移動するように制御すると、対象物１３０が中心から距離ｄの位置で障害物１４０と接する。
【０１１５】
図１０は、本実施形態に係る障害物１４０が対象物１３０を押す力を説明する図である。図１０のように、障害物１４０が対象物１３０を押す力をベクトルｆｂとすると、対象物１３０と障害物１４０とが接した瞬間、ベクトルｆｂは、−ｙ方向を向いている。
ベクトルｆｂについて、対象物１３０の中心方向の成分をベクトルｆｂｃ、ベクトルｆｂｃと直行する方向の成分をベクトルｆｂ０とする。この場合、ベクトルｆｂｃは、対象物１３０を並進させる力となる。ベクトルｆｂ０は、直線ｃを回転軸として、対象物１３０を回転させるトルクのベクトルτｃを発生させている。
【０１１６】
次に力ベクトルｆｂｃ及びトルクのベクトルτｃが、アーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクについて、図１１を用いて説明する。図１１は、本実施形態に係るアーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクを説明する図である。図１１において、符号ｆｈは、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆｈである。
【０１１７】
アーム１１０及びアーム１２０は、制御部１１の制御により、対象物１３０を把持するために、それぞれの把持部１１１及び１２１を、それぞれｘ軸方向に−ｆｈ、ｆｈの力で対象物１３０に押し付けている。したがって、第１力センサ６１及び第２力センサ６２にはその反力が入力される。このようにアーム１１０及びアーム１２０が互いに押し付けあっているため、ベクトルｆｂｃのｘ成分−ｆｂｃｘはそれぞれの把持部１１１及び１２１に取り付けられた力センサ６１及び６２に入力される。
また、それぞれのｙ成分には力ベクトルｆｃのｙ成分−ｆｂｃｙが等分配されて、各力センサ６１及び６２に入力される。更に、第１力センサ６１及び第２力センサ６２のα成分には、トルクのベクトルτｃによって発生する−τｂが入力される。
【０１１８】
以上により、アーム１１０と把持部１１１との間に取り付けられた第１力センサ６１の出力ベクトルｆ１と、アーム１２０と把持部１２１との間に取り付けられた第２力センサ６２の出力ベクトルｆ２は、それぞれ次式（２３）と次式（２４）のようになる。
【０１１９】
【数２３】
0005825589.tif 000024
【０１２０】
【数２４】
0005825589.tif 000025
【０１２１】
次に、アームの手先の中心の位置及び姿勢を示す情報のベクトルＰに加わる力ベクトルＦの計算について説明する。次に、計算したベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動について説明する。また、それに応じたアーム１１０及びアーム１２０の位置及び姿勢を示す情報のベクトルｐ１、及びベクトルｐ２の運動について説明する。
まず、制御部１１は、式（８）に式（２３）式及び式（２４）を代入し、次式（２５）を算出する。
【０１２２】
【数２５】
0005825589.tif 000026
【０１２３】
次に、ベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動について、図１２を用いて説明する。図１２は、ベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動を説明する図である。
まず、制御部１１は、式（２５）に式（１６）を代入し、ベクトルΔＰを算出する。なお、式（１６）は非線形の微分方程式であるので、制御部１１は、例えば、オイラー法やルンゲクッタ法などの数値計算法を用いてベクトルΔＰを計算する。実際の制御においては、制御周期の長短に応じて計算方法を選択する必要があり、制御周期が長い場合は計算の精度が比較的高いルンゲクッタ法で算出する方が良い。すなわち、制御部１１は、高次の多項式を用いて近似して算出（時間に関する高次の多項式を用いて解く、とも言う）する。なお、高次の多項式とは、一般的に４次以上である。例えばルンゲクッタ法であれば４次までとることで、一般的な物理現象の運動方程式を精度良く解くことが可能である。
【０１２４】
次に、式（１６）の両辺をラプラス変換して、次式（２６）と次式（２７）を算出して、ΔＰの振る舞いを定性的に説明する。
【０１２５】
【数２６】
0005825589.tif 000027
【０１２６】
【数２７】
0005825589.tif 000028
【０１２７】
式（２６）と式（２７）は、ベクトルＦを入力ベクトルとし、ΔＰを出力とすると、それらが次式（２８）なる２次遅れ系で結ばれていると考えることが出来る。
【０１２８】
【数２８】
0005825589.tif 000029
【０１２９】
従って ベクトルΔＰは、ベクトルＦに２次遅れで追従する。ここではまず、ベクトルＰの位置成分ベクトルＰｐの運動について説明する。ベクトルΔＰはベクトルＦに２次遅れで追従することから、図１２に示すように、ベクトルΔＰはベクトルＦｐに倣う方向を向く。
従って、式（１０）及び式（２１）から、ベクトルＰｐは図１２に示すベクトルとなる。これにより式（１８）及び式（１９）から、アーム１２０及びアーム１２０の位置成分ベクトルｐ１ｐ及びベクトルｐ２ｐは、図１２に点線で示したベクトルｐ１’及びベクトルｐ２’となる。このため、対象物１３０を把持しながら障害物１４０との間に発生する力を緩和する方向に、アーム１１０及びアーム１２０は移動する。
【０１３０】
次にベクトルＰの姿勢成分ベクトルＰｄの運動について説明する。上述したように、ベクトルΔＰは、ベクトルＦに２次遅れで追従することから、図１３に示すように、ベクトルＰｄは、ｚ軸に関してｙ軸からｘ軸への向きである負方向に回転する。式（１８）及び式（１９）から、アーム１１０及びアーム１２０の位置成分ベクトルｐ１ｄ及びベクトルｐ２ｄも、図１３に示したようにｚ軸に関して負方向に回転する。図１３は、本実施形態に係るベクトルＰの姿勢成分ベクトルＰｄの運動について説明する図である。
【０１３１】
次に、ベクトルｆの計算について説明する。さらに、上述の回転に応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＲの運動について説明する。更にそれに応じたアーム１１０及びアーム１２０の位置及び姿勢を示す情報のベクトルｐ１、ベクトルｐ２の運動について説明する。
図１４は、本実施形態に係る回転に応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＲの運動について説明する図である。アーム１１０及びアーム１２０が、ｚ軸に関して負方向に回転するに伴い、図１４（ａ）に示すように、アーム１１０及びアーム１２０には、対象物１３０に対して加える力ベクトルｆｒが発生する。このベクトルｆｒを、ｘ軸方向及びｙ軸方向に分解すると、ｘ軸方向については、アーム１１０が加える力とアーム１２０が加える力とで相殺する。従って残るｙ軸方向について、それぞれその反力ｆｒｙ、−ｆｒｙが、アーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される。
【０１３２】
次に、制御部１１は、式（９）に式（２３）及び式（２４）を代入する。そして、上述した結果を考慮して制御部１１がベクトルｆを計算すると、ベクトルｆは、次式（２９）のように表される。
【０１３３】
【数２９】
0005825589.tif 000030
【０１３４】
式（２９）において、x軸方向の力は、対象物１３０をアーム１１０及びアーム１２０が把持する力に対する反力である。これに応じて対象物１３０を落下させないように、制御部１１が、アームの手先同士の相対の位置及び姿勢の軌道ベクトルＲＴのｘ成分について、式（２２）に示したようにｗ０に選択している。更に、制御部１１は、仮想的に設定するインピーダンスの制御変数も合わせて調整している。この制御変数の調整については後に詳述する。
【０１３５】
次に、式（２９）で示したベクトルｆのｙ成分のみを考慮して、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲの運動について説明する。式（１７）の両辺をラプラス変換すると、次式（３０）と次式（３１）が得られる。
【０１３６】
【数３０】
0005825589.tif 000031
【０１３７】
【数３１】
0005825589.tif 000032
【０１３８】
式（３１）において、ベクトルｆを入力、ベクトルΔＲを出力とすると、それらが次式（３２）のような２次遅れ系で結ばれていると考えることが出来る。
【０１３９】
【数３２】
0005825589.tif 000033
【０１４０】
従ってベクトルΔＲは、ベクトルｆに２次遅れで追従する。式（２９）に示したように、ベクトルｆの回転成分は０（ゼロ）であるから、ベクトルＲの位置成分ベクトルＲＰの運動について説明する。
ベクトルΔＲは、ベクトルｆに２次遅れで追従することから、図１２（ｂ）に示すように、ベクトルΔＲは、ベクトルｆＰのｙ成分−fｒｙに倣う方向を向く。従って、式（１１）及び式（２２）から、ベクトルＲＰは、図１４（ｂ）に示すベクトルとなる。更に式（１８）及び式（１９）から、アーム１１０及びアーム１２０の位置成分ベクトルｐ１Ｐ、ｐ２Ｐは、図１４（ｂ）に点線で示したベクトルｐ１’及びベクトルｐ２’となる。このため、制御部１１の制御により、対象物１３０を把持しながら障害物１４０との間に発生する力を緩和する方向にアーム１１０及びアーム１２０が移動する。
【０１４１】
この制御により、図６に示したようにアーム１１０及びアーム１２０が物体を把持した状態で、障害物１４０に対象物１３０が衝突すると、図７に示したように障害物１４０から受ける力に応じて受動的にその位置及び姿勢を変更して、コンプライアンスを守る。
【０１４２】
次に、アーム１１０及びアーム１２０が、把持する力に対する反力に応じて、対象物１３０を落下させないように、あらかじめ設定されるアームの手先同士の相対の位置及び姿勢の軌道ベクトルＲＴのｘ成分及び仮想的なインピーダンスの制御変数について図１５を用いて説明する。図１５は、本実施形態に係るインピーダンスの制御変数を説明する図である。
【０１４３】
まず、対象物１３０を弾性体として、それを把持する際の振舞いについて説明する。ベクトルｆのｘ成分ｆｘは、次式（３３）のように、相対位置のｘ成分Ｒｘが、対象物１３０の幅ｗに対して、それよりも内側に押し込んだ時に、その押し込んだ距離と対象物１３０の弾性係数ｋに比例した力を受ける。
【０１４４】
【数３３】
0005825589.tif 000034
【０１４５】
また、式（１１）より手先の相対位置のｘ成分Ｒｘは、次式（３４）のように表される。
【０１４６】
【数３４】
0005825589.tif 000035
【０１４７】
更に式（３１）より、ベクトルΔＲのｘ成分は、次式（３５）のように表される。
【０１４８】
【数３５】
0005825589.tif 000036
【０１４９】
次に、制御部１１は、式（３５）に式（３３）及び式（３４）を代入して、その振る舞いを解析する。この時、ベクトルＲＴのｘ成分ＲＴｘを、次式（３６）のように予め設定しておく。
【０１５０】
【数３６】
0005825589.tif 000037
【０１５１】
図１６は、ΔＲｘ及びｆｘの振る舞いを表す図である。図１６（ａ）に示したのは、ΔＲｘの振る舞いであり、縦軸がΔＲｘ、横軸が時間である。また、図１６（ｂ）に示したのは、ｆｘの振る舞いであり、縦軸がｆｘ、横軸が時間である。図１６（ａ）から、ΔＲｘはΔＲｘ＝ｗ−ｗ０、すなわちＲＴｘ＝ｗとなり、手先が対象物１３０の表面に接している位置から、ＫＲｘ（ｗ−ｗ０）／（ｋ＋ＫＲｘ）だけ押し込んだ位置で静止している。
それゆえ、図１６（ｂ）に示したように、ｆｘはそれに対象物１３０の弾性係数ｋを乗じた値になっている。
従って、アーム１１０及びアーム１２０が、対象物１３０に対して押し付ける力ｆｈは、次式（３７）のようになる。
【０１５２】
【数３７】
0005825589.tif 000038
【０１５３】
従って、対象物１３０の幅ｗ及び弾性係数ｋが既知であるとすれば、制御部１１は、押し付け力ｆｈを算出し、それを満たすＫＲｘ及びｗ０を用いることで所望の押し付け力を実現することが出来る。この時、ｗ０は、ｗを超えない範囲で大きな値とし、それに応じてＫＲｘを決定することで、アーム１１０に対するアーム１２０の相対的な目標位置を遠ざけることが可能である。
また、対象物１３０の弾性係数ｋが、ＫＲｘとの間に、ｋ＞＞ＫＲｘの関係が成り立つ時、すなわち、対象物１３０が固い場合、式（３７）は、次式（３８）のようになる。
【０１５４】
【数３８】
0005825589.tif 000039
【０１５５】
この場合、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆｈは、対象物１３０の弾性係数ｋと無関係に決定されるため、弾性係数ｋが未知であっても、押し付け力ｆｈを設定し、それを満たすＫＲｘ及びｗ０を用いることで所望の押し付け力を実現することが出来る。
次に、制御部１１は、式（３７）を解析的に求める。まず、制御部１１は、式（３５）に式（３３）及び式（３４）を代入し、ΔＲｘについて解き、次式（３９）を算出する。
【０１５６】
【数３９】
0005825589.tif 000040
【０１５７】
次に、時刻Ｔ以降の振る舞いを解析するため、式（３９）右辺のｋ（ｗ―ＲＴｘ）は、ｋ（ｗ−ｗ０）のステップ入力として扱う。このため、式（３９）は、式（３８）より、次式（４０）のように表される。
【０１５８】
【数４０】
0005825589.tif 000041
【０１５９】
式（３９）及びラプラス変換の最終値の定理を用いて、制御部１１は、次式（４１）のように、ΔＲｘのｔ→∞（無限大）の極限を求める。
【０１６０】
【数４１】
0005825589.tif 000042
【０１６１】
次に、制御部１１は、式（３３）に式（３４）及び式（４１）を代入して、次式（４２）のように、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆｈを求める。
【０１６２】
【数４２】
0005825589.tif 000043
【０１６３】
また、ベクトルＲＴのｘ成分ＲＴｘを、次式（４３）のように５次式で表す。
【０１６４】
【数４３】
0005825589.tif 000044
【０１６５】
なお、式（４３）において、各係数ｃ０、ｃ３、ｃ４、ｃ５は、次式（４４）である。
【０１６６】
【数４４】
0005825589.tif 000045
【０１６７】
式（４３）のようにＲＴｘを５次式で表すことで、アーム１１０及びアーム１２０の加速及び減速が緩やかになり、図１７に示すように、図１６で見られた急激な力の変化は抑制される。図１７は、ＲＴｘを５次式で表した場合のΔＲｘ及びｆｘの振る舞いを表す図である。図１７（ａ）に示したのは、ΔＲｘの振る舞いであり、縦軸がΔＲｘ、横軸が時間である。また、図１７（ｂ）に示したのは、ｆｘの振る舞いであり、縦軸がｆｘ、横軸が時間である。
【０１６８】
次に、本実施形態に係る制御方法を、図１８を用いて説明する。図１８は、本実施形態に係る動作の一例を説明するフローチャートである。
制御部１１は、作業指示が入力された後、ステップＳ１〜ステップＳ１０を行う。
制御部１１の中心運動の設定軌道算出部１１−４は、第１記憶部１１−３に記憶されているアーム１１０の手先及びアーム１２０の手先の中心の軌道に関する情報を読み出し、読み出した情報に基づき、作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰＴを生成する。
また、相対運動の設定軌道算出部１１−１４は、第２記憶部１１−１３に記憶されているアーム１１０に対するアーム１２０の手先同士の相対的なの軌道に関する情報に基づき、作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲＴを算出する。中心運動の設定軌道算出部１１−４は、算出した作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰＴを中心運動の目標位置算出部１１−５に出力する。また、相対運動の設定軌道算出部１１−１４は、算出した作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲＴを相対運動の目標位置算出部１１−１５に出力する（ステップＳ１）。
【０１６９】
次に、制御部１１は、内部タイマーの現在時刻ｔをリセット（ｔ＝０）する（ステップＳ２）。
【０１７０】
次に、中心運動に対してはたらく力の算出部１１−１は、第１力センサ６１からの出力であるベクトルｆ１を取得する。また、相対運動に対してはたらく力の算出部１１−１１は、第２力センサ６２からの出力であるベクトルｆ２を取得する（ステップＳ３）。
【０１７１】
次に、中心運動に対してはたらく力の算出部１１−１は、取得した第１力センサ６１からの出力であるベクトルｆ１に基づき、アームの手先の中心の位置及び姿勢に加わる力ベクトルＦを算出する。中心運動に対してはたらく力の算出部１１−１は、算出したアームの手先の中心の位置及び姿勢に加わる力ベクトルＦを中心運動のコンプライアンス成分算出部１１−２に出力する。また、相対運動に対してはたらく力の算出部１１−１１は、取得した第２力センサ６２からの出力であるベクトルｆ２に基づき、アームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆを算出する。相対運動に対してはたらく力の算出部１１−１１は、算出したアームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆを相対運動のコンプライアンス成分算出部１１−１２に出力する（ステップＳ４）。
【０１７２】
次に、中心運動のコンプライアンス成分算出部１１−２は、中心運動に対してはたらく力の算出部１１−１から出力されたアームの手先の中心の位置及び姿勢に加わる力ベクトルＦに基づき、手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰを算出する。中心運動のコンプライアンス成分算出部１１−２は、算出した手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰを中心運動の目標位置算出部１１−５に出力する。
相対運動のコンプライアンス成分算出部１１−１２は、相対運動に対してはたらく力の算出部１１−１１から出力されたアームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆに基づき、手先の相対の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲを算出する。相対運動のコンプライアンス成分算出部１１−１２は、算出した手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲを相対運動の目標位置算出部１１−１５に出力する（ステップＳ５）。
【０１７３】
次に、中心運動の目標位置算出部１１−５には、中心運動のコンプライアンス成分算出部１１−２から出力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰと、中心運動の設定軌道算出部１１−４から出力された作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰＴとが入力される。中心運動の目標位置算出部１１−５は、入力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰと、作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰＴとに基づき、現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰ（式（１０））を算出する。中心運動の目標位置算出部１１−５は、算出した現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰを、アーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する。
また、相対運動の目標位置算出部１１−１５には、相対運動のコンプライアンス成分算出部１１−１２から出力された手先の相対の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲと、相対運動の設定軌道算出部１１−１４から出力された作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲＴとが入力される。相対運動の目標位置算出部１１−１５は、入力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲと、作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲＴとに基づき、現在時刻の手先の相対の目標位置及び姿勢を示す情報であるベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲ（式（１１））を算出する。すなわち、相対運動の目標位置算出部１１−１５は、予定された軌道を、実際の動作に基づき補正した軌道に基づき、実際の動作に応じた現在時刻の手先の相対の目標位置及び姿勢を算出している。
相対運動の目標位置算出部１１−１５は、算出した現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲを、アーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する（ステップＳ６）。
【０１７４】
次に、アーム１１０の目標位置算出部１１−６には、中心運動の目標位置算出部１１−５から現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰと、相対運動の目標位置算出部１１−１５から現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲとが入力される。アーム１１０の目標位置算出部１１−６は、入力された現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰと、現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲとに基づき、アーム１１０の真の目標位置及び姿勢を示す情報のベクトルｐ１（式（１８））を算出する。
また、アーム１２０の目標位置算出部１１−１６には、中心運動の目標位置算出部１１−５から現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰと、相対運動の目標位置算出部１１−１５から現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲとが入力される。アーム１２０の目標位置算出部１１−１６は、入力された現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰＴ＋ベクトルΔＰと、現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲＴ＋ベクトルΔＲとに基づき、アーム１２０の真の目標位置及び姿勢を示す情報のベクトルｐ２（式（１９））を算出する（ステップＳ７）。
【０１７５】
次に、アーム１１０の目標位置算出部１１−６は、算出したアーム１１０の真の目標位置及び姿勢を示す情報のベクトルｐ１に基づき、アーム１１０の目標軌道の補正値である制御値を生成し、生成した制御値に基づき、アーム１１０を制御する。
また、アーム１２０の目標位置算出部１１−１６は、算出したアーム１２０の真の目標位置及び姿勢を示す情報のベクトルｐ２に基づき、アーム１２０の目標軌道の補正値である制御値を生成し、生成した制御値に基づき、アーム１２０を制御する（ステップＳ８）。
【０１７６】
次に、制御部１１は、内部タイマーの現在時刻ｔに制御周期Δｔを加算する（ステップＳ９）。
【０１７７】
次に、制御部１１は、加算後の内部タイマー値がｔ＝Ｔ（作業終了時刻）であるか否かを判定する（ステップＳ１０）。
ステップＳ１０の判定の結果、内部タイマー値がｔ＝Ｔ（作業終了時刻）ではない場合（ステップＳ１０；Ｎｏ）、制御部１１は、ステップＳ２に戻り、ステップＳ２〜ステップＳ１０の処理を繰り返す。
ステップＳ１０の判定の結果、内部タイマー値がｔ＝Ｔ（作業終了時刻）の場合（ステップＳ１０；Ｙｅｓ）、制御部１１は、制御処理を終了し、次の作業指示を待つ。
【０１７８】
以上のように、２本のアーム１１０及び１２０の運動を、アーム１１０の手先及びアーム１２０の手先の中心の位置及び姿勢を示す情報で表し、アーム１１０に対するアーム１２０の手先同士の相対の位置及び姿勢を示す情報で表した。更に予めアーム１１０の手先及びアーム１２０の手先の中心の目標軌道を設定して記憶部に記憶させ、予めアーム１１０に対するアーム１２０の手先の相対的な目標軌道を設定して記憶部に記憶させることで、対象物１３０を落下させてしまう、もしくは過大な力を加えて破壊してしまうといった課題を解決した。
また、制御部１１は、２本のアーム１１０及び１２０に取り付けられた第１力センサ６１及び第２力センサ６２の値から上述の２つの運動に対する力を表し、目標軌道に対する補正量を、それらの力に対して仮想的なインピーダンスを受けて運動する対象物の位置として計算した。この結果、対象物１３０が落下しても、補正量が０となり、第１記憶部１１−３及び第２記憶部１１−１３に記憶されている予め設定された目標軌道に、２本のアーム１１０及び１２０の手先が移動するだけであって、両者が衝突することを防ぐことができる。
更に、上述のアームの手先同士の相対の位置及び姿勢の運動に対して、仮想的に設定するインピーダンスは、対象物１３０を把持する際に設定する押し付け力から算出することが可能であり、所望の押し付け力を実現可能とした。
【０１７９】
また、本実施形態では、物体１３０のインピーダンスを仮想的に設定して制御する例を説明した。物体１３０が剛体である場合、物体１３０のインピーダンスを考慮せずに、制御部１１は、相対位置及び姿勢Ｒの仮想的なインピーダンスを設定し、所望の押し付け力を制御するようにしてもよい。
【０１８０】
［従来技術との比較］
次に、従来技術と本実施形態との比較を行う。
従来技術の場合、図３０のような構成のため、アーム１の目標値は、アーム１の力センサ９６１の出力と予め設定されているアーム１の軌道データのみに基づいて算出されている。また、アーム２の目標値は、アーム２の力センサ９６２に出力に基づいてアーム１の現在位置を推定し、推定したアーム１の位置に基づいてアーム２の目標値を算出していた。このため、対象物１３０に障害物１４０が当たった場合、対象物１３０を落下させてしまう場合もあった。さらに、対象物１３０を落下させてしまった場合、アーム２の力センサ９６２に加わる力がなくなるため、アーム２は、アーム１に向けて移動するような目標値が算出される。この結果、アーム２が、アーム１に衝突してしまう場合もあった。
【０１８１】
これに対し、本実施形態の場合、第１アーム１１０と、第２アーム１２０との間にインピーダンスが仮想的にあるとして、インピーダンス制御する。アーム１１０及び１２０の第１力センサ６１及び第２力センサ６２の出力に基づき、対象物１３０の中心運動に対してはたらく力に基づき中心運動のコンプライアンス成分を算出し、相対運動に対してはたらく力に基づき相対運動のコンプライアンス成分を算出している。さらに、中心運動及び相対運動の各時刻における設定軌道を算出している。そして、中心運動のコンプライアンス成分と中心運動の各時刻における設定軌道とに基づき中心運動の目標値を算出している。また、相対運動のコンプライアンス成分と相対運動の各時刻における設定軌道とに基づき相対運動の目標値を算出している。さらに、中心運動の目標値と相対運動の目標値とに基づき、アーム１１０及び１２０の目標値を算出して、アーム１１０及び１２０とを制御している。
この結果、対象物１３０が、障害物１４０に当たった場合でも、対象物１３０を落下させないように適切な押しつけ力で、アーム１１０及び１２０を制御することができる。また、仮に対象物１３０が、障害物１４０に当たり落下した場合においても、従来技術とは異なり、アーム１１０及び１２０との間には、インピーダンス制御により対象物１３０を仮想的に把持されているため、第１力センサ６１及び第２力センサ６２が外力を検出できない状態になっても、アーム１２０がアーム１１０に衝突することを防ぐことができる。
【０１８２】
次に、非特許文献１と本実施形態との比較を行う。
非特許文献１では、対象物のインピーダンスは、次式（４５）〜（４７）（非特許文献１の式（２１）〜（２３））である。これらの式（４５）〜（４７）を用いて、次式（４８）（非特許文献１の式（１３））を書き換えると、アームの相対位置の運動方程式は、次式（４９）のようになる。
【０１８３】
【数４５】
0005825589.tif 000046
【０１８４】
【数４６】
0005825589.tif 000047
【０１８５】
【数４７】
0005825589.tif 000048
【０１８６】
【数４８】
0005825589.tif 000049
【０１８７】
【数４９】
0005825589.tif 000050
【０１８８】
式（４５）〜式（４９）において、ＦＲは、対象物間に働く力であり、Δｘは、対象物間に働く力に対する対象物の相対変位であり、ＭＲ、ＤＲ、Ｋはそれぞれ正定の慣性行列、減衰係数行列及びバネ定数行列である。
また、アームの重心位置の運動方程式は、次式（５０）（非特許文献１の式（１））である。
【０１８９】
【数５０】
0005825589.tif 000051
式（５０）において、Ｆｅｘｔは、対象物に加わる外力であり、Δｘは、対象物に加わる外力Ｆｅｘｔに対する対象物の位置及び姿勢の変位であり、Ｍ、Ｄ、Ｋはそれぞれ対象物の見かけの慣性行列、減衰係数行列及びバネ定数行列である。
式（５０）と式（４９）とを比べるとゲインは異なるが、周波数特性は同じである。すなわち、物体のコンプライアンス・センターに関する運動と、部品間の相対運動とは同じ運動特性を持つ必要がある。
【０１９０】
一方、本実施形態では、物体のコンプライアンス・センターに関する運動と、部品間（エンドエフェクタ間）の相対運動とで異なる運動特性を設定することが可能である。このため、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
【０１９１】
［第２実施形態］
第１実施形態では、ロボット１は、対象物１３０を搬送する例を説明した。第２実施形態においては、ロボット１は、２本のアーム１１０及び１２０で、対象物２０１を把持して、所定の部品２１１に組み付ける場合について説明する。
図１９は、本実施形態に係る対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。  図２０は、本実施形態に係るフレームがずれた場合、対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。図１９及び図２０において、対象物２０１は、突起部の先端部２０２を有する凸状の部品であり、突起部以外の両側を２本のアーム１１０及び１２０で把持されている。また、フレーム２１１は、凹状の物体であり、凹部２１２を有する。ロボット１は、このフレーム２１１の凹部２１２に、対象物２０１の突起部２０２を組み込むように対象物を移動させるように制御する。
なお、本実施形態に係るロボット１及び制御部１１の構成は、第１実施形態と同様のため、説明を省略する。
【０１９２】
第１実施形態では、コンプライアンス・センターを、図４のように対象物１３０の中心、すなわちベクトルＰをベクトルＲの中間にした例を説明した。本実施形態では、コンプライアンス・センターを、２本のアーム１１０及び１２０のそれぞれの先端から所定の位置に設定する。本実施形態においては、所定の位置は、任意の位置に設定する。なお、コンプライアンス・センターの位置は、作業を行う前に、オペレータが当該作業において最適な場所にあらかじめ決めておく。
例えば、図１９のように、コンプライアンス・センターを対象物２０１の凸部の先端２０２に予めオペレータが設定する。図１９のように、対象物２０１の先端２０２にコンプライアンス・センターを置くと、対象物２０１の先端２０２が空間に対して仮想的に持つインピーダンス特性を設定することが可能となる。なお、インピーダンス特性とは、慣性係数、減衰係数及びバネ定数により式（１６）及び式（１７）で表される特性である。
フレーム２１１と対象物２０１が接触した際に受ける力を、両方のアーム１１０及び１２０に取り付けられている第１力センサ６１及び第２力センサ６２の値から算出し、それに応じて第１実施形態と同様に、インピーダンス特性に応じて、倣い動作をさせることで対象物２０１をフレーム２１１へと組み付けることが可能となる。
【０１９３】
例えば、図１９のように、フレーム２１１が所定の位置に配置されている場合、両方のアーム１１０及び１２０に取り付けた第１力センサ６１及び第２力センサ６２は、対象物２０１を把持している場合の力ベクトルｆ１と力ベクトルｆ２とが検出する。
一方、図２０のように、フレーム２１１ａが、ｘ方向に距離ｄ１ずれている場合、対象物２０１の先端２０２の面が、フレーム２１１ａの凹部２１２の角２１３に当たる。この結果、両方のアーム１１０及び１２０に取り付けた第１力センサ６１及び第２力センサ６２には、対象物２０１の先端２０２の面が、フレーム２１１ａの凹部２１２の角２１３に当たったことによる力ベクトルｆ１と力ベクトルｆ２とが検出される。
【０１９４】
第１実施形態では、制御部１１は、式（１６）と式（１７）のインピーダンス制御による運動方程式を解いてベクトルΔＰとベクトルΔＲを求めた後、式（１０）と式（１１）に結果を代入して、式（１８）と式（１９）によりベクトルＰとベクトルＲを算出した。本実施形態では、制御部１１は、式（１６）と式（１７）のインピーダンス制御による運動方程式に加え、ダンピング制御の制御則である次式（５１）と次式（５２）を用いてアーム１１０及びアーム１２０の目標値を算出する。
【０１９５】
【数５１】
0005825589.tif 000052
【０１９６】
【数５２】
0005825589.tif 000053
【０１９７】
なお、式（５１）及び式（５２）において、上付き＾ので表しているＤｐ＾は、粘性係数（ダンピング係数）である。制御部１１は、式（５１）、（５２）、（１６）及び（１７）の式を解いてベクトルΔＰとベクトルΔＲを求めた後、式（１０）と式（１１）に結果を代入して、ベクトルＰとベクトルＲを算出する。又、ベクトルＦｄｅｓは目標外力、ベクトルｆｄｅｓは目標内力である。
【０１９８】
式（５１）及び式（５２）において、ベクトルＦｄｅｓを０とし、ベクトルｆｄｅｓを、対象物を把持するために必要な押し付け力に設定すると、対象物は把持しながらも、外力に対して位置に追従しなくなるため、ダンピング制御も加えることで、物体が障害物に衝突した際に、あらかじめ設定された軌道には追従せず、障害物に倣うような動作を行う。この結果、高いコンプライアンス性能を実現することが可能となる。
【０１９９】
次に、制御部１１は、次式（５３）と次式（５４）に、算出したベクトルＰとベクトルＲを代入して、ベクトルｐ１及びベクトルｐ２を算出する。すなわち、次式（５３）が、第１実施形態における式（１８）であり、次式（５４）が、第１実施形態における式（１９）である。
【０２００】
【数５３】
0005825589.tif 000054
【０２０１】
【数５４】
0005825589.tif 000055
【０２０２】
なお、式（５３）と式（５４）において、ベクトルＤは、第１実施形態の式（２０）で表される。また、ベクトルＲ１Ｔは、ベクトルＰから見たアーム１１０（マニピュレータ１）先端の予め設定された軌道である。ベクトルＲ２Ｔは、ベクトルＰから見たアーム１２０（マニピュレータ２）先端の予め設定された軌道である。ベクトルΔＲ１は、ベクトルＰから見たアーム１１０（マニピュレータ１）先端のコンプライアンス成分である。ベクトルΔＲ２は、ベクトルＰから見たアーム１１０（マニピュレータ１）先端のコンプライアンス成分である。
【０２０３】
この場合においても、予め算出した各時刻の手先の中心の目標位置及び姿勢軌道と、各時刻の相対の手先の目標位置及び姿勢軌道とを、第１力センサ６１及び第１力センサ６２により検出された力ベクトルＦと力ベクトルｆとを用いて、第１実施形態の制御と同様に、図１８のフローチャートのように制御することで、対象物１３０を把持して、所定の部品２１１に組み付けることができる。
【０２０４】
以上のように、本実施形態においても、物体のコンプライアンス・センターに関する運動と、部品間（エンドエフェクタ間）の相対運動とで異なる運動特性を表すことが可能である。このため、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
【０２０５】
［第３実施形態］
第２実施形態では、ベクトルＰを空間上の任意の位置に置いて、各アームを制御する例を説明した。すなわち、ベクトルＰは、空間に固定された座標系Σで記述している。本実施形態では、例えば、対象物（部品）の先端に原点を設定し、対象物の位置及び姿勢に応じて変化する座標系Σ’で記述して、各アームの制御を行う。すなわち、本実施形態においては、コンプライアンス・センターは、対象物の位置及び姿勢に応じて変化する。
なお、本実施形態に係るロボット１及び制御部１１の構成は、第１実施形態と同様のため、説明を省略する。
【０２０６】
図２１は、本実施形態に係る対象物（部品）を把持して、所定の部品３１１に組み付ける一例を説明する図である。図２１において、符号３０１は対象物３０１（部品）であり、符号３１１は対象物３０１（部品）を取り付けるフレーム３１１である。図２１において、対象物３０１は、突起部の先端部３０２を有する凸状の部品であり、突起部以外の両側を２本のアーム１１０及び１２０で把持されている。また、フレーム３１１は、凹状の物体であり、凹部３１２を有する。ロボット１は、このフレーム３１１の凹部３１２に、対象物３０１の突起部３０２を組み込むように対象物を移動させるように制御する。
例えば、対象物３０１の先端３０２に原点Ｏ’を設定し、ｘ’軸、ｙ’軸、ｚ’軸をそれぞれ図２１のように設定する。このように座標系Σ’（Ｏ’−ｘ’ｙ’ｚ’）を定義する。
【０２０７】
第１実施形態では、制御部１１は、式（１６）と式（１７）を解いた後、式（１８）と式（１９）により各アームの目標位置であるベクトルＰとベクトルＲを算出した。本実施形態では、制御部１１は、座標系Σ’（Ｏ’−ｘ’ｙ’ｚ’）に基づき、ベクトルｐ１Ｔ及びベクトルｐ２Ｔを、次式（５５）〜式（５８）のように表し、アーム１１０及びアーム１２０の目標値を算出する。
【０２０８】
【数５５】
0005825589.tif 000056
【０２０９】
【数５６】
0005825589.tif 000057
【０２１０】
【数５７】
0005825589.tif 000058
【０２１１】
【数５８】
0005825589.tif 000059
【０２１２】
なお、式（５５）において、ベクトルｐ１Ｔは、アーム１１０の手先の位置及び姿勢を示すベクトルｐ１の把持した対象物３０１の位置及び姿勢を操作するように予め設定され第１記憶部１１−３に記憶されている軌道である。また、式（５６）において、ベクトルｐ２Ｔは、アーム１２０の手先の位置及び姿勢を示すベクトルｐ２の把持した対象物３０１の位置及び姿勢を操作するように予め設定され第１記憶部１１−３に記憶されている軌道である。
式（５５）〜式（５８）を用いて、式（１８）と式（１９）は、次式（５９）と次式（６０）のように置き換える。
【０２１３】
【数５９】
0005825589.tif 000060
【０２１４】
【数６０】
0005825589.tif 000061
【０２１５】
このように本実施形態では、式（５９）と式（６０）により、制御部１１は、各アーム１１０及び１２０の目標位置であるベクトルＰとベクトルＲを算出し、算出した目標位置の制御値に基づき各アームを、第１実施形態のフローチャートの図１８と同様に制御する。
【０２１６】
以上のように、部品の形状に応じて部品の先端に物体座標系とすると、部品の先端が空間に対して仮想的に持つインピーダンス特性を、部品の位置及び姿勢に関係なく、その形状に応じて算出することが可能となる。この結果、フレームと部品が接触した際に受ける力を、両方のアームに取り付けた力センサの値から算出し、それに応じて上述のインピーダンス特性に応じて、倣い動作をさせることで部品をフレームへと組み付けることが可能となる。
【０２１７】
なお、ベクトルＰＴ及びベクトルＲＴは、予め設定される軌道のため、ベクトルｐ１Ｔ及びベクトルｐ２Ｔを予め制御部１１が計算しておき、第１記憶部１１−３及び第２記憶部１１−１３に記憶させておくことが可能である。これにより、演算効率を向上することができる。
また、制御部１１は、コンプライアンス成分を、式（５７）と式（５８）を用いて算出し、式（５８）及び式（６０）を用いて各アームの目標値を算出することで、重心位置ベクトルＰ及び相対位置ベクトルＲを経由することなく各アーム１１０及び１２０の目標位置を計算することが可能になり、計算時間の短縮が可能になる効果がある。
【０２１８】
［第４実施形態］
第１実施形態〜第３実施形態では、２本のアームにより制御する例を説明したが、本実施形態は３本のアームを用いて制御する例である。
図２２は、本実施形態に係る３本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。図２２のように、対象物４３０（物体）を、３本のアーム４０１〜４０３が、各指部４０５〜４０７を介して把持している。なお、図２３では、説明のために、対象物４３０を二次元的に三箇所、把持している図を示したが、これに限らず、３次元的に三箇所を把持するようにしてもよい。
【０２１９】
図２３は、本実施形態に係る３本のアームによる座標系を説明する図である。図２３において、第１アーム４０１（マニピュレーター１）の手先位置及び姿勢をベクトルｘ１（図４のベクトルｐ１に相当）、第２アーム４０２（マニピュレーター２）の手先位置及び姿勢をベクトルｘ２（図４のベクトルｐ２に相当）、第３アーム４０３（マニピュレーター３）の手先位置及び姿勢をベクトルｘ３として表している。また、図２３において、ベクトルｘｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ２ｒは、第１アーム４０１の先端位置ｘ１と、第２アーム４０２の先端位置ｘ２との相対位置を表している。ｘ３ｒは、第１アーム４０１の先端位置ｘ１と、第３アーム４０３の先端位置ｘ３との相対位置を表している。ベクトルｘ２ｒ、ｘｒ３及びｘ１ｐは、図４のベクトルＲに相当する。
また、図２３においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。
【０２２０】
これらのベクトルｘ１、ｘ２、ｘ３、ｘｐ、ｘ１ｐ、ｘ２ｒ及びｘ３ｒを用いて、ベクトルｘｐは、次式（６１）のように表される。
【０２２１】
【数６１】
0005825589.tif 000062
【０２２２】
図２３のように、ベクトルｘ２ｒは、次式（６２）のように表され、ベクトルｘ３ｒは、次式（６３）のように表される。
【０２２３】
【数６２】
0005825589.tif 000063
【０２２４】
【数６３】
0005825589.tif 000064
【０２２５】
式（６１）、式（６２）及び式（６３）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘ３は、次式（６４）のように表される。
【０２２６】
【数６４】
0005825589.tif 000065
【０２２７】
制御部４１１は、この式（６４）を、式（１８）及び式（１９）の代わりに用い、式（６４）の各ベクトルｘ１、ｘ２及びｘ３を算出し、各アームの目標位置及び姿勢を算出することで、各アーム４０１〜４０３を制御する。
【０２２８】
図２４は、本実施形態に係る制御部４１１のブロック図である。図２４のように、制御部４１１は、重心運動に対してはたらく力の算出部４１１−１、重心運動のコンプライアンス成分算出部４１１−２、第１記憶部４１１−３、重心運動の設定軌道算出部４１１−４、重心運動の目標位置算出部４１１−５、第１アームの目標位置算出部４１１−２０、相対運動に対してはたらく力の算出部４１１−１１、相対運動のコンプライアンス成分算出部４１１−１２、第２記憶部４１１−１３、相対運動の設定軌道算出部４１１−１４、相対運動の目標位置算出部４１１−１５、第２アームの目標位置算出部４１１−２１、及び第３アームの目標位置算出部４１１−２２を備えている。また、制御部４１１は、不図示の内部タイマーを備えている。なお、本実施形態では、第１記憶部４１１−３と第２記憶部４１１−１３のように２つ備える例を説明したが、記憶部は１つでもよい。
【０２２９】
重心運動に対してはたらく力の算出部４１１−１には、各アーム４０１〜４０３に取り付けられている第１力センサ６１（検出部）、第２力センサ６２（検出部）及び第３力センサ４６３（検出部）から、力成分とトルク成分を含む情報が入力される。重心運動に対してはたらく力の算出部４１１−１は、第１力センサ６１、第２力センサ６２及び第３センサ４６３との出力とに基づき、対象物の重心運動に対してはたらく力を算出する。重心運動に対してはたらく力の算出部４１１−１は、算出した対象物の重心運動に対してはたらく力を重心運動のコンプライアンス成分算出部４１１−２に出力する。
なお、重心運動に対してはたらく力とは、アーム４０１の手先、アーム４０２の手先及びアーム４０３の手先の間に把持されている対象物４３０の重心位置に対して働く力である。対象物４３０の密度が一様であれば、重心位置は対象物４３０の中心の位置である。
【０２３０】
重心運動のコンプライアンス成分算出部４１１−２は、重心運動に対してはたらく力の算出部４１１−１から出力された対象物の重心運動に対してはたらく力を示す情報に基づき、対象物の重心運動のコンプライアンス成分を算出する。重心運動のコンプライアンス成分算出部１１−２は、算出した対象物の重心運動のコンプライアンス成分を重心運動の目標位置算出部４１１−５に出力する。
【０２３１】
第１記憶部４１１−３には、予め対象物に対する重心運動の軌道に関する情報が記憶されている。
【０２３２】
重心運動の設定軌道算出部４１１−４は、第１記憶部４１１−３に記憶されている対象物に対する重心運動の軌道に関する情報を読み出し、読み出した情報に基づき、対象物に対する重心運動の軌道を算出する。重心運動の設定軌道算出部４１１−４は、算出した対象物の重心運動の軌道を重心運動の目標位置算出部４１１−５に出力する。
【０２３３】
重心運動の目標位置算出部４１１−５には、重心運動のコンプライアンス成分算出部４１１−２から出力された対象物の重心運動のコンプライアンス成分を示す情報と、重心運動の設定軌道算出部４１１−４から出力された対象物の重心運動の設定軌道を示す情報とが入力される。重心運動の目標位置算出部４１１−５は、入力された対象物の重心運動のコンプライアンス成分を示す情報と、対象物の重心運動の設定軌道を示す情報とに基づき、対象物の重心運動の目標値を算出する。重心運動の目標位置算出部４１１−５は、算出した対象物の重心運動の目標値を第１アームの目標位置算出部４１１−２０、第２アームの目標位置算出部４１１−２１及び第３アームの目標位置算出部４１１−２２に出力する。
【０２３４】
相対運動に対してはたらく力の算出部４１１−１１には、各アーム４０１〜４０３に取り付けられている第１力センサ６１、第２力センサ６２及び第３力センサ４６３から、力成分とトルク成分を含む情報が入力される。相対運動に対してはたらく力の算出部４１１−１１は、入力された第１力センサ６１、第２力センサ６２及び第３力センサ４６３との出力とに基づき、対象物の相対運動に対してはたらく力を算出する。相対運動に対してはたらく力の算出部４１１−１１は、算出した対象物の相対運動に対してはたらく力を相対運動のコンプライアンス成分算出部４１１−１２に出力する。
なお、相対運動に対してはたらく力とは、アーム４０１に対するアーム４０２、アーム４０３の相対的にはたらく力である。
【０２３５】
相対運動のコンプライアンス成分算出部４１１−１２は、相対運動に対してはたらく力の算出部４１１−１１から出力された対象物の相対運動に対してはたらく力を示す情報に基づき、対象物の相対運動のコンプライアンス成分を算出する。相対運動のコンプライアンス成分算出部４１１−１２は、算出した対象物の相対運動のコンプライアンス成分を相対運動の目標位置算出部４１１−１５に出力する。
【０２３６】
第２記憶部４１１−１３には、予め相対運動の軌道に関する情報が記憶されている。
【０２３７】
相対運動の設定軌道算出部４１１−１４は、第２記憶部４１１−１３に記憶されている対象物に対する相対運動の軌道に関する情報を読み出し、読み出した情報に基づき、対象物に対する相対運動の軌道を算出する。相対運動の設定軌道算出部４１１−１４は、算出した対象物の相対運動の軌道を相対運動の目標位置算出部４１１−１５に出力する。
【０２３８】
相対運動の目標位置算出部４１１−１５には、相対運動のコンプライアンス成分算出部４１１−１２から出力された対象物の相対運動のコンプライアンス成分を示す情報と、相対運動の設定軌道算出部４１１−１４から出力された対象物の相対運動の設定軌道を示す情報とが入力される。相対運動の目標位置算出部４１１−１５は、入力された対象物の相対運動のコンプライアンス成分を示す情報と、対象物の相対運動の設定軌道を示す情報とに基づき、対象物の相対運動の目標値を算出する。算出した対象物の相対運動の目標値を第１アームの目標位置算出部４１１−２０、第２アームの目標位置算出部４１１−２１及び第３アームの目標位置算出部４１１−２２に出力する。
【０２３９】
第１アームの目標位置算出部４１１−２０には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第１アームの目標位置算出部４１１−２０は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第１アームの目標値を算出する。第１アームの目標位置算出部４１１−２０は、算出した第１アームの目標値を第１アーム４０１の制御信号として、第１アーム４０１に出力する。
【０２４０】
第２アームの目標位置算出部４１１−２１には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第２アームの目標位置算出部４１１−２０は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第２アームの目標値を算出する。第２アームの目標位置算出部４１１−２０は、算出した第２アームの目標値を第２アーム４０２の制御信号として、第２アーム４０２に出力する。
【０２４１】
第３アームの目標位置算出部４１１−２２には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第３アームの目標位置算出部４１１−２２は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第３アームの目標値を算出する。第３アームの目標位置算出部４１１−２２は、算出した第３アームの目標値を第３アーム４０３の制御信号として、第３アーム４０３に出力する。
【０２４２】
以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本である場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。
【０２４３】
［第５実施形態］
本実施形態は、３本のアームを制御する別の実施形態である。図２５は、本実施形態に係る３本のアームによる座標系を説明する図である。図２５のように、第１アーム４０１の手先位置及び姿勢をベクトルｘ１（図４のベクトルｐ１に相当）、第２アーム４０２の手先位置及び姿勢をベクトルｘ２（図４のベクトルｐ２に相当）、第３アーム４０３の手先位置及び姿勢をベクトルｘ３として表している。また、図２５において、ベクトルｘｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ２ｒは、第１アーム４０１の先端位置ｘ１と、第２アーム４０２の先端位置ｘ２との相対位置を表している。ｘ３ｒは、第２アーム４０２の先端位置ｘ２と、第３アーム４０３の先端位置ｘ３との相対位置を表している。ベクトルｘ２ｒ、ｘｒ３及びｘ１ｐは、図４のベクトルＲに相当する。
また、図２５においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。
【０２４４】
これらのベクトルｘ１、ｘ２、ｘ３、ｘｐ、ｘ１ｐ、ｘ２ｒ及びｘ３ｒを用いて、ベクトルｘｐは、第４実施形態と同様に、式（６１）のように表される。
【０２４５】
また、図２５のように、ベクトルｘ２ｒは、次式（６５）のように表され、ベクトルｘ３ｒは、次式（６６）のように表される。
【０２４６】
【数６５】
0005825589.tif 000066
【０２４７】
【数６６】
0005825589.tif 000067
【０２４８】
式（６１）、式（６５）及び式（６６）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘ３は、次式（６７）のように表される。
【０２４９】
【数６７】
0005825589.tif 000068
【０２５０】
制御部４１１は、この式（６７）を、式（１８）及び式（１９）の代わりに用い、式（６７）の各ベクトルｘ１、ｘ２及びｘ３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。
【０２５１】
なお、第４実施形態の式（６４）を用いて各アームの目標位置及び姿勢を算出するか、本実施形態の式（６７）を用いて各アームの目標位置及び姿勢を算出するかは、例えば対象物の形状などに応じてこれらを使い分けるようにしてもよい。すなわち、対象物の形状に応じて、力を制御すべき軸が変わるため、それに応じてこれらのパターンを使い分けるようにしてもよい。
【０２５２】
以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。
【０２５３】
［第６実施形態］
本実施形態は、３本のアームを制御する別の実施形態である。図２６は、本実施形態に係る３本のアームによる座標系を説明する図である。図２６のように、第１アーム４０１の手先位置及び姿勢をベクトルｘ１（図４のベクトルｐ１に相当）、第２アーム４０２の手先位置及び姿勢をベクトルｘ２（図４のベクトルｐ２に相当）、第３アーム４０３の手先位置及び姿勢をベクトルｘ３として表している。また、図２６において、ベクトルｘｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ２ｒは、第３アーム４０３の先端位置ｘ３と、第２アーム４０２の先端位置ｘ２との相対位置を表している。ｘ３ｒは、第１アーム４０１の先端位置ｘ１と、第３アーム４０３の先端位置ｘ３との相対位置を表している。ベクトルｘ２ｒ、ｘｒ３及びｘ１ｐは、図４のベクトルＲに相当する。
また、図２６においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。
【０２５４】
これらのベクトルｘ１、ｘ２、ｘ３、ｘｐ、ｘ１ｐ、ｘ２ｒ及びｘ３ｒを用いて、ベクトルｘｐは、第３実施形態と同様に、式（６１）のように表される。
【０２５５】
また、図２６のように、ベクトルｘ２ｒは、次式（６８）のように表され、ベクトルｘ３ｒは、次式（６９）のように表される。
【０２５６】
【数６８】
0005825589.tif 000069
【０２５７】
【数６９】
0005825589.tif 000070
【０２５８】
式（６１）、式（６８）及び式（６９）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘ３は、次式（７０）のように表される。
【０２５９】
【数７０】
0005825589.tif 000071
【０２６０】
制御部４１１は、この式（７０）を、式（１８）及び式（１９）の代わりに用い、式（７０）の各ベクトルｘ１、ｘ２及びｘ３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。
【０２６１】
なお、第４実施形態の式（６４）を用いて各アームの目標位置及び姿勢を算出するか、第５実施形態の式（６７）を用いて各アームの目標位置及び姿勢を算出するか、あるいは、本実施形態の式（７０）を用いて各アームの目標位置及び姿勢を算出するかは、例えば対象物の状などに応じてこれらを使い分けるようにしてもよい。すなわち、対象物の形状に応じて、力を制御すべき軸が変わるため、それに応じてこれらのパターンを使い分けるようにしてもよい。
【０２６２】
以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。
【０２６３】
［第７実施形態］
本実施形態では、４本のアームを制御する実施形態について説明する。
図２７は、本実施形態に係る４本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。図２７のように、対象物５３０（物体）を、４本のアーム５０１〜５０４が、各指部５０５〜５０８を介して把持している。なお、図２７では、説明のために、対象物５３０を二次元的に四箇所、把持している図を示したが、これに限らず、３次元的に四箇所を把持するようにしてもよい。
【０２６４】
各アームの制御を行う制御部の構成は、図２４の構成において、重心運動に対してはたらく力の算出部４１１−１に、４本のアームに設けられている各々の力センサー（第１力センサ〜第４力センサ）の出力が入力される。ここでは、第３のアームの他に、第４のアームが設けられ、この第４のアームに第４力センサが設けられている。同様に、図２４の構成において、相対運動に対してはたらく力の算出部４１１−１１に、４本のアームに設けられている各々の力センサー（第１力センサ〜第４力センサ）の出力が入力される。
また、図２４の構成に加え、不図示の第４アームの目標位置算出部を備える。第４アームの目標位置算出部には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第４アームの目標位置算出部は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第４アームの目標値を算出する。第４アームの目標位置算出部は、算出した第４アームの目標値を第４アームの制御信号として、第４アームに出力する。
【０２６５】
図２８は、本実施形態に係る４本のアームによる座標系を説明する図である。図２８のように、第１アーム５０１の手先位置及び姿勢をベクトルｘ１（図４のベクトルｐ１に相当）、第２アーム５０２の手先位置及び姿勢をベクトルｘ２（図４のベクトルｐ２に相当）、第３アーム５０３の手先位置及び姿勢をベクトルｘ３、第４アーム５０４の手先位置及び姿勢をベクトルｘ４として表している。また、図２８において、ベクトルｘｐは、４本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ２ｒは、第１アーム５０１の先端位置ｘ１と、第２アーム５０２の先端位置ｘ２との相対位置を表している。ｘ３ｒは、第１アーム５０１の先端位置ｘ１と、第３アーム５０３の先端位置ｘ３との相対位置を表している。ｘ４ｒは、第１アーム５０１の先端位置ｘ１と、第４アーム５０４の先端位置ｘ４との相対位置を表している。すなわち、ベクトルｘ２ｒ〜ｘｎｒ（ｎは、３以上４以下の自然数）は、第１アーム５０１の先端位置ｘ１と、第ｎアームの先端位置ｘｎとの相対位置を表している。
また、ベクトルｘ２ｒ、ｘｒ３、ｘ４ｒ及びｘ１ｐは、図４のベクトルＲに相当する。また、図２８においては、４本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。
【０２６６】
これらのベクトルｘ１、ｘ２、ｘ３、ｘｐ、ｘ１ｐ、ｘ２ｒ、ｘ３ｒ及びｘ４ｒを用いて、ベクトルｘｐは、次式（７１）のように表される。
【０２６７】
【数７１】
0005825589.tif 000072
【０２６８】
図２８のように、ベクトルｘ２ｒは、次式（７２）のように表され、ベクトルｘ３ｒは、次式（７３）のように表され、ベクトルｘ４ｒは、次式（７４）のように表される。
【０２６９】
【数７２】
0005825589.tif 000073
【０２７０】
【数７３】
0005825589.tif 000074
【０２７１】
【数７４】
0005825589.tif 000075
【０２７２】
式（７１）〜式（７４）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘ４は、次式（７５）のように表される。
【０２７３】
【数７５】
0005825589.tif 000076
【０２７４】
制御部４１１は、この式（７５）を、式（１８）及び式（１９）の代わりに用い、式（７５）の各ベクトルｘ１、ｘ２及びｘ３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。
【０２７５】
なお、本実施形態では、アームが４本の場合における制御の一例を説明したが、アームが３本の場合と同様に、対象物の形状等に応じて、各ベクトルｘ１ｐ、ｘ２ｒ、ｘ３ｒ及びｘ４ｒの設定を第４実施形態〜第６実施形態のように設定してもよい。すなわち、各ベクトルｘ１ｐ、ｘ２ｒ、ｘ３ｒ及びｘ４ｒは、ベクトルｘ２ｒ〜ｘｎｒ（ｎは、３以上４以下の自然数）は、第１アームの先端位置ｘ１と、第ｎアームの先端位置ｘｎとの相対位置に限らない。例えば、ｘ２ｒは、第２アームの先端位置ｘ２と、第ｎアームの先端位置ｘｎとの相対位置であってもよい。
【０２７６】
以上のように、４本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、４本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。
【０２７７】
［第８実施形態］
本実施形態では、ｎ（ｎは２以上の自然数）本のアームを制御する実施形態について説明する。
図２９は、本実施形態に係るｎ本のアームによる座標系を説明する図である。図２９のように、第１のアームの手先位置及び姿勢をベクトルｘ１（図４のベクトルｐ１に相当）、第２のアームの手先位置及び姿勢をベクトルｘ２（図４のベクトルｐ２に相当）、第３のアームの手先位置及び姿勢をベクトルｘ３、第４のアームの手先位置及び姿勢をベクトルｘ４、・・・第ｎのアームの手先位置及び姿勢をベクトルｘｎとして表している。また、図２９において、ベクトルｘｐは、複数のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ２ｒは、第１アームの先端位置ｘ１と、第２アームの先端位置ｘ２との相対位置を表している。ｘ３ｒは、第１アームの先端位置ｘ１と、第３アームの先端位置ｘ３との相対位置を表している。ｘ４ｒは、第１アームの先端位置ｘ１と、第４アームの先端位置ｘ４との相対位置を表している。ｘｎｒは、第１アームの先端位置ｘ１と、第ｎアームの先端位置ｘｎとの相対位置を表している。すなわち、ベクトルｘ２ｒ〜ｘｎｒは、第１アームの先端位置ｘ１と、第ｎアームの先端位置ｘｎとの相対位置を表している。また、ベクトルｘ２ｒ、ｘｒ３、ｘ４ｒ・・・ｘｎｒ及びｘ１ｐは、図４のベクトルＲに相当する。
また、図２９においては、ｎ本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。
【０２７８】
これらのベクトルｘ１、ｘ２、ｘ３、ｘｐ、ｘ１ｐ、ｘ２ｒ、ｘ３ｒ・・・ｘｎｒ及びｘ４ｒを用いて、ベクトルｘｐは、次式（７６）のように表される。
【０２７９】
【数７６】
0005825589.tif 000077
【０２８０】
図２９のように、ベクトルｘ２ｒ、・・・、ベクトルｘｎｒは、次式（７７）のように表される。
【０２８１】
【数７７】
0005825589.tif 000078
【０２８２】
式（７６）と式（７７）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘｎは、次式（７８）のように表される。
【０２８３】
【数７８】
0005825589.tif 000079
【０２８４】
制御部４１１は、この式（７８）を、式（１８）及び式（１９）の代わりに用い、式（７８）の各ベクトルｘ１〜ｘｎを算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。
【０２８５】
各アームの制御を行う制御部４１１は、図２４の構成において、重心運動に対してはたらく力の算出部４１１−１に、ｎ本のアームに設けられている各々の力センサー（第１力センサ〜第ｎ力センサ）の出力が入力される。同様に、図２４の構成において、相対運動に対してはたらく力の算出部４１１−１１に、ｎ本のアームに設けられている各々の力センサー（第１力センサ〜第ｎ力センサ）の出力が入力される。
また、図２４の構成に加え、不図示の第４アーム〜第ｎアームの目標位置算出部を備える。第ｎアームの目標位置算出部には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第ｎアームの目標位置算出部は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第ｎアームの目標値を算出する。第ｎアームの目標位置算出部は、算出した第ｎアームの目標値を第ｎアームの制御信号として、第ｎアームに出力する。
【０２８６】
なお、本実施形態では、アームがｎ本の場合における制御の一例を説明したが、アームが３本の場合と同様に、対象物の形状等に応じて、各ベクトルｘ１ｐ、ｘ２ｒ、ｘ３ｒ及びｘ４ｒの設定を第４実施形態〜第６実施形態のように変えてもよい。
【０２８７】
以上のように、ｎ本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、４本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。
【０２８８】
なお、本実施形態では、各アームが対象物を把持して、搬送あるいはフレームに組み付ける例を説明したが、例えば、アーム部２０−１の第５フレーム２５には、カメラを設けられるようにしてもよい。カメラは、制御部１１または４１１の制御によって、対象物またはフレームを撮像し、取り付け位置を撮影した画像に基づき認識するようにしてもよい。そして、認識した結果に基づき、対象物をフレームの取り付け位置に移動するように制御部が制御する。この場合においても、フレームの取り付け部分の位置が予め定められている位置からずれていても、対象物とフレームとが接触したりして、対象物やフレームが破損することを防ぐことができる。
【０２８９】
なお、本実施形態では、各アームに設けられた力センサを用いて、各アームにかかる外力を検出する例を説明したが、本発明はこれに限定されない。例えば、各アームを接続する各関節（屈折部、回転部）のモータを駆動する駆動電流値から、アーム部２０−１及び２０−２に作用する外力を検出するようにしてもよい。さらに、力センサを用いず、各関節の角速度から、各アーム部２０−１及び２０−２に作用する外力を検出するようにしてもよい。
【０２９０】
なお、本実施形態では、ロボット制御装置１をロボットに適用する例を説明したが、例えば、本実施形態のロボット制御装置１を、組み立て装置や搬送装置などに適用してもよい。また、本実施形態では、対象物を把持して搬送、または組み込む例を説明したが、例えば、物体の分解や検査など所定の動作を行うようにしてもよい。
【０２９１】
なお、実施形態の図３および図２４の制御部の各部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＣＤ−ＲＯＭ等の可搬媒体、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ  Ｓｅｒｉａｌ  Ｂｕｓ）  Ｉ／Ｆ（インタフェース）を介して接続されるＵＳＢメモリ、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、サーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
【符号の説明】
【０２９２】
１・・・ロボット
１０・・・固定部
１１、４１１・・・制御部
２０・・・アーム部
４０−１、４０−２・・・指部
６１・・・第１センサ（検出部）
６２・・・第２センサ（検出部）
１１０、１２０・・・アーム
１１１、１２１・・・把持部（アーム部の先端）
１３０・・・対象物
１４０・・・障害物
ベクトルＰ（Ｐ）・・・アーム１１０及びアーム１２０のアームの手先の中心の位置及び姿勢
ベクトルＲ（Ｒ）・・・アーム１１０に対するアーム１２０の手先同士の相対的な手先位置及び姿勢
ｐ１・・・アーム１１０の手先位置及び姿勢
ｐ２・・・アーム１２０の手先位置及び姿勢
【図１】
0005825589.tif 000080
【図２】
0005825589.tif 000081
【図３】
0005825589.tif 000082
【図４】
0005825589.tif 000083
【図５】
0005825589.tif 000084
【図６】
0005825589.tif 000085
【図７】
0005825589.tif 000086
【図８】
0005825589.tif 000087
【図９】
0005825589.tif 000088
【図１０】
0005825589.tif 000089
【図１１】
0005825589.tif 000090
【図１２】
0005825589.tif 000091
【図１３】
0005825589.tif 000092
【図１４】
0005825589.tif 000093
【図１５】
0005825589.tif 000094
【図１６】
0005825589.tif 000095
【図１７】
0005825589.tif 000096
【図１８】
0005825589.tif 000097
【図１９】
0005825589.tif 000098
【図２０】
0005825589.tif 000099
【図２１】
0005825589.tif 000100
【図２２】
0005825589.tif 000101
【図２３】
0005825589.tif 000102
【図２４】
0005825589.tif 000103
【図２５】
0005825589.tif 000104
【図２６】
0005825589.tif 000105
【図２７】
0005825589.tif 000106
【図２８】
0005825589.tif 000107
【図２９】
0005825589.tif 000108
【図３０】
0005825589.tif 000109