JP5825589B2 - ロボット制御装置、ロボット、およびロボット制御方法 - Google Patents

Description

本発明は、ロボット制御装置、ロボット、およびロボット制御方法に関する。

近年、製造現場等では、スカラロボットや多軸ロボット等が、製品の組み立てや検査等に用いられている。製品の組み立てや検査を行うときに物体を運搬する場合、ロボットは、物体を吸着したり、アームにより把持したりする。

例えば、特許文献１には、２本のアームを有する双腕ロボットにおいて、物体を運搬するロボットの制御技術が開示されている。双腕ロボットは、２本のアームのうち、片方をマスターアーム、一方をスレーブアームに設定して、２本のアームに物体を把持させる。そして、２本のアームに物体を把持させた状態で、マスターアームは、物体を運搬する目標軌道に対し把持部に加わる外力を力センサで検出し、その値に応じて目標軌道を、例えばインピーダンス制御を用いて弾力的に変化させる。また、２本のアームに物体を把持させた状態で、スレーブアームは、把持部に加わる外力を力センサで検出し、その値に応じてマスターアームに追従する目標軌道を生成して、生成した軌道で物体を運搬する。

また、例えば、非特許文献１には、インピーダンス制御を行うことで、２本のアームの協調制御を行う制御技術が開示されている。

図３０は、特許文献１に記載の従来技術に係るロボット制御装置の動作の概要を説明する図である。図３０のように、ロボットの制御部９１１は、アーム１のコンプライアンス成分算出部９１１−１、メモリ９１１−２、アーム１の設定軌道算出部９１１−３、アーム１の目標位置算出部９１１−４、アーム２の現在位置推定部９１１−１１、及びアーム２の目標位置算出部９１１−１２から構成されている。
ロボットは、不図示の２本のアームに各々力センサ１、２（９６１、９６２）を有している。この力センサ１、２は、不図示の把持部に加わる外力を検出する。制御部９１１は、アーム１の力センサ１が検出した外力に基づき、コンプライアンス成分を算出する。なお、コンプライアンス成分とは、把持部の位置および把持部に作用する力に基づき、ロボットを制御するときに用いるコンプライアンスの成分である。
また、制御部９１１は、メモリ９１１−２に記憶されている情報に基づき、アーム１の予定されている軌道（設定軌道）を算出する。そして、制御部９１１は、算出されたコンプライアンス成分と設定軌道とを用いて、アーム１の位置などの目標値を算出する。制御部９１１は、算出された目標値に基づき、アーム１を制御している。一方、制御部９１１は、アーム２の力センサ２が検出した外力に基づき、アーム１の現在位置を推定する。制御部９１１は、推定されたアーム１の現在位置に基づき、アーム２を制御している。

特開２００９−１８３８０号公報

小菅一弘, 吉田英博, 福田敏男, 蟹谷清, 酒井勝, 針木和夫, "インピーダンス制御に基づく双腕マニピュレータの協調制御", [日本ロボット学会誌 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].

しかしながら、特許文献１に記載の従来技術では、スレーブアームの目標軌道について、マスターアームの把持部と対向する方向と直行する方向については把持部と対象物の摩擦や、回転成分を考慮していないため、正確な軌道を生成することは困難である。このため、特許文献１に記載の従来技術では、対象物を落下させてしまう、もしくは対象物に過大な力を加えて破壊してしまうといった課題があった。また、特許文献１に記載の従来技術では、スレーブアームの目標軌道を把持部に加わる外力を用いて生成するため、対象物が落下した際には、外力が加わるようにマスターアームに向かう目標軌道が生成され、アーム同士が衝突してしまうといった課題があった。
更に、特許文献１に記載の従来技術では、マスターアーム及びスレーブアームそれぞれの運動が独立に設定されるため、それらの相対運動で決定される対象物（物体）に対する押し付け力を直接制御することが出来ない。このため、対象物を落下させてしまう、もしくは対象物に過大な力を加えて破壊してしまうといった課題があった。

本発明は、上記の課題に鑑みてなされたものであって、対象物を確実に把持して運搬するロボット制御装置、ロボット、およびロボット制御方法を提供することを目的としている。

上記目的を達成するため、本発明のロボット制御装置は、対象物を運搬する複数のアーム部と、複数の前記アーム部にそれぞれ設けられた、前記アーム部に作用する外力を検出する検出部と、前記各検出部からの検出情報に基づき、複数の前記アーム部を制御する制御部と、を備え、前記制御部は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御することを特徴としている。

本発明によれば、アーム部を含むアームが対象物を把持して搬送しているときに、対象物を確実に把持して運搬するロボット制御装置を提供できる。

本発明によれば、複数の前記アーム部のそれぞれの先端に対する位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を、複数の前記アーム部の作業空間上に任意の位置に設定するようにしたので、対象物を把持して所定の部品に組み付ける場合においても、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された前記所定の点は、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点であるようにしてもよい。

本発明によれば、複数の前記アーム部のそれぞれの先端に対する位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点の位置に設定するようにしたので、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができるロボット制御装置を提供できる。また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記座標系の原点は、前記対象物に設定されているようにしてもよい。

本発明によれば、座標系の原点は、前記対象物に設定したので、アームが把持して運搬する所定の部品に組み付ける場合でも、部品の配置がずれていたりしても対象物を落下させないように各アーム部を制御することができ、対象物を確実に部品に組み付けることができるロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動を、複数の前記アーム部間に設定された仮想的なインピーダンスに基づいて算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動とに基づき、複数の前記アーム部の各々の運動を算出し、前記算出した複数の前記アーム部の各々の運動に基づき、複数の前記アーム部を制御するようにしてもよい。

本発明によれば、アームが対象物を把持して搬送しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動とを、前記設定された仮想的なインピーダンスと前記対象物のインピーダンスとに基づき算出するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように、かつ障害物に倣うような動作を行うように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部が、前記対象物に対して押し付ける力が所望の値になるように、当該対象物のインピーダンスを考慮して、前記Ｒの運動の仮想的なインピーダンスを設定するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように、かつ障害物に倣うような動作を行うように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。また、仮に、対象物を落下させた場合においても、アーム同士が衝突しないように制御できるロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記対象物が剛体である場合、前記対象物のインピーダンスを考慮せずに、前記Ｒの運動の仮想的なインピーダンスを設定し、前記各アーム部から前記対象物への所望の押し付け力を制御するようにしてもよい。

本発明によれば、複数のアーム部が、対象物に対して押し付ける力は、対象物の弾性係数ｋと無関係に決定されるため、弾性係数ｋが未知であっても、押し付け力ｆ_ｈを設定し、それを満たす弾性係数を用いることで所望の押し付け力を実現することが出来き、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動とＲの運動とに基づき、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ_１、ｐ_２、・・・ｐ_ｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部の基準点同士の相対位置に基づいて、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ_１、ｐ_２、・・・ｐ_ｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、複数の前記アーム部が、前記対象物を運搬する各々の予め定められた目標軌道に対する補正量を、前記Ｐの運動と前記Ｒの運動とに基づき算出し、算出した補正量に基づき複数の前記アーム部を制御するようにしてもよい。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部の目標軌道を補正することで、各アーム部を制御することができ、また、対象物に過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボット制御装置において、前記制御部は、前記Ｐの運動と前記Ｒの運動の振る舞いを定義した運動方程式を、時間に関する高次の多項式を用いて解くことで前記Ｐの運動と前記Ｒの運動とを算出するようにしてもよい。

本発明によれば、ルンゲクッタ法などの高次の多項式を用いて対象物の中心の位置及び姿勢Ｐと、前記対象物の相対の位置及び姿勢Ｒとを算出するようにしたので、制御周期が長い場合でも、精度の高い制御を行うことができるロボット制御装置を提供できる。ここで高次の多項式とは、一般的に４次以上である。例えばルンゲクッタ法であれば４次までとることで、一般的な物理現象の運動方程式を精度良く解くことが可能である。

上記目的を達成するため、本発明のロボット制御装置において、複数の前記アーム部は、各々６自由度を有する６軸アーム部であるようにしてもよい。

本発明によれば、ロボットのアーム部を各々６軸にしたため、６つの変数を制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）、姿勢（α、β、γ）に対応して制御することができるロボット制御装置を提供できる。

上記目的を達成するため、本発明のロボットは、前記請求項１から請求項１３のいずれか１項のロボット制御装置を備えることを特徴としている。

本発明によれば、アームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御装置を備えるロボットを提供できる。

上記目的を達成するため、本発明は、対象物を運搬する複数のアーム部を有するロボット制御装置におけるロボット制御方法において、検出部が、複数の前記アーム部にそれぞれ設けられた、外力を検出する検出工程と、制御部が、複数の前記アーム部を制御する制御工程と、を有し、前記制御工程は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御することを特徴としている。

本発明によれば、アーム部を含むアームが対象物を把持して運搬しているときに、対象物が障害物にあたった場合でも、対象物を落下させないように各アーム部を制御することができ、また、過大な力を加えて破壊することを防ぐように制御することができ、対象物を確実に把持して運搬するロボット制御方法を提供できる。

第１実施形態に係るロボット１の概略斜視図である。 同実施形態に係るアーム部２０−１及び２０−２と、第１力センサ６１及び第２力センサ６２の概念図である。 同実施形態に係る制御部１１のブロック図である。 同実施形態に係るロボット制御方法を適用するロボット１のアームの概念図である。 同実施形態に係るインピーダンス制御の概念を説明する図である。 同実施形態に係る対象物が障害物に当たった場合を説明する図である。 同実施形態に係る対象物が障害物に当たった後、各アームが制御される場合を説明する図である。 同実施形態に係る対象物１３０の斜視図である。 同実施形態に係る対象物１３０と障害物１４０との位置関係を説明する図である。 同実施形態に係る障害物１４０が対象物１３０を押す力を説明する図である。 同実施形態に係るアーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクを説明する図である。 同実施形態に係るベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動を説明する図である。 同実施形態に係るベクトルＰの姿勢成分ベクトルＰ_ｄの運動について説明する図である。 同実施形態に係る回転に応じた手先の中心位置及び姿勢を示す情報のベクトルＲの運動について説明する図である。 同実施形態に係るインピーダンスの制御変数を説明する図である。 同実施形態に係るΔＲｘ及びｆｘの振る舞いを説明する図である。 同実施形態に係るＲ_Ｔｘを５次式で表した場合のΔＲ_ｘ及びｆ_ｘの振る舞いを表す図である。 同実施形態に係る動作の一例を説明するフローチャートである。 第２実施形態に係る対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。 同実施形態に係るフレームがずれた場合、対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。 第３実施形態に係る対象物（部品）を把持して、所定の部品３１１に組み付ける一例を説明する図である。 第４実施形態に係る３本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。 同実施形態に係る３本のアームによる座標系を説明する図である。 同実施形態に係る制御部４１１のブロック図である。 第５実施形態に係る３本のアームによる座標系を説明する図である。 第６実施形態に係る３本のアームによる座標系を説明する図である。 第７実施形態に係る４本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。 同実施形態に係る４本のアームによる座標系を説明する図である。 第８実施形態に係るｎ本のアームによる座標系を説明する図である。 従来技術に係るロボット制御装置９１１の動作の概要を説明する図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、本発明は斯かる実施形態に限定されず、その技術思想の範囲内で種々の変更が可能である。

［第１実施形態］
図１は、本実施形態に係るロボット１の概略斜視図である。図１に示すように、ロボット１は、２本のアーム２、３を備えている。アーム２は、固定部１０、アーム部２０−１、第１力センサ６１、及び指部４０−１を備える。また、アーム３は、固定部１０、アーム部２０−２、第２力センサ６２、及び指部４０−２を備える。

固定部１０は、例えば床、壁、天井、移動可能な台車の上などに固定される。固定部１０は、内部に、制御部１１（破線にて図示）を備える。
制御部１１は、アーム部２０−１及び２０−２、指部４０−１及び４０−２を制御する。制御部１１の詳細は後述する。なお、制御部１１は、固定部１０の内部ではなく、固定部１０の外部に設けるようにしてもよい。

アーム部２０−１及び２０−２は、第１フレーム２１、第２フレーム２２、第３フレーム２３、第４フレーム２４および第５フレーム２５から構成されている。第１フレーム２１は、回転屈折軸を介して、固定部１０に回転可能または屈折可能に接続されている。第２フレーム２２は、回転屈折軸を介して、第１フレーム２１および第３フレーム２３に接続されている。第３フレーム２３は、回転屈折軸を介して、第２フレーム２２および第４フレーム２４に接続されている。第４フレーム２４は、回転屈折軸を介して、第３フレーム２３および第５フレーム２５に接続されている。第５フレーム２５は、回転屈折軸を介して、第４フレーム２４に接続されている。アーム部２０は、制御部１１の制御によって、各フレーム２１〜２５が各回転屈折軸を中心に複合的に回転または屈折し、動く。

アーム部２０−１の第５フレームのうち第４フレームが設けられた他方には、指部４０−１が設けられている。アーム部２０−２の第５フレームのうち第４フレームが設けられた他方には、指部４０−２が設けられている。指部４０−１及び４０−２は、直接的に、対象物を把持する。なお、指部４０−１及び４０−２は、例えば、複数のフレームを有し、対象物を把持できる構造である。

アーム部２０−１の第５フレーム２５と指部４０−１の間には、第１力センサ６１が介挿されている。第１力センサ６１は、アーム２の指部４０−１に加わる力の各空間成分（力成分という）とトルクの各空間成分（トルク成分という）を検出する。第１力センサ６１は、検出したアーム２の指部４０−１に加わる力成分とトルク成分を含む情報を制御部１１に出力する。
また、アーム部２０−２の第５フレーム２５と指部４０−２の間には、第２力センサ６２が介挿されている。第２センサ６２は、アーム３の指部４０−２に加わる力成分とトルク成分を検出する。第２力センサ６２は、検出したアーム３の指部４０−２に加わる力成分とトルク成分を含む情報を制御部１１に出力する。

図２は、本実施形態に係るアーム部２０−１及び２０−２と、第１力センサ６１及び第２力センサ６２の概念図である。図２のように、アーム２のアーム部２０−１は、回転部５２、５３及び５５と、屈折部５１、５４及び５６とを有している。そして、アーム２のアーム部２０−２は、指部４０−１との間に第１力センサ６１を有している。
また、アーム３のアーム部２０−１は、回転部５２、５３及び５５と、屈折部５１、５４及び５６とを有している。そして、アーム３のアーム部２０−２は、指部４０−２との間に第２力センサ６２を有している。
また、屈折部５１は、固定部１０と第１フレーム２１との間に設けられている。回転部５２は、第１フレーム２１と第２フレーム２２との間に設けられている。回転部５３は、第２フレーム２２と第３フレーム２３との間に設けられている。屈折部５４は、第３フレーム２３と第４フレーム２４との間に設けられている。回転部５５は、第４フレーム２４と第５フレーム２５との間に設けられている。屈折部５４は、第５フレームのうち第４フレームが設けられた他方に設けられている。
図２のように、アーム部２０−１及び２０−２は、６自由度を有する６軸アームである。すなわち、アーム部２０−１の先端の指部４０−１は、これらの各軸の回転部と屈折部の動作により、移動する。そして、指部４０−１が対象物１３０を把持している場合、第１力センサ６１が検出した外力には、このような各軸の回転部と屈折部の動作による成分が含まれている。このため、アーム部２０−１及びアーム２０−２が６軸アームであるため、６つの変数で制御することが可能であり、例えば、位置（ｘ、ｙ、ｚ）の３つの変数（成分）、姿勢（α、β、γ）の３つの成分（変数）に対応して制御することができる。

図３は、本実施形態に係る制御部１１のブロック図である。図３のように、制御部１１は、中心運動に対してはたらく力の算出部１１−１、中心運動のコンプライアンス成分算出部１１−２、第１記憶部１１−３、中心運動の設定軌道算出部１１−４、中心運動の目標位置算出部１１−５、アーム１１０の目標位置算出部１１−６、相対運動に対してはたらく力の算出部１１−１１、相対運動のコンプライアンス成分算出部１１−１２、第２記憶部１１−１３、相対運動の設定軌道算出部１１−１４、相対運動の目標位置算出部１１−１５、及びアーム１２０の目標位置算出部１１−１６を備えている。また、制御部１１は、不図示の内部タイマーを備えている。なお、アーム１１０とは、図１および図２において第１力センサ６１が取り付けられているアーム部２０−１である。また、アーム１２０とは、図１および図２において第２力センサ６２が取り付けられているアーム部２０−２である。
なお、各算出部が行う算出手順については、後述する。また、本実施形態では、第１記憶部１１−３及び１１−１３のように記憶部を２つ備える例を説明したが、記憶部は１つでもよい。

中心運動に対してはたらく力の算出部１１−１には、各アームに取り付けられている第１力センサ６１（検出部）及び第２力センサ６２（検出部）から、力成分とトルク成分を含む情報が入力される。中心運動に対してはたらく力の算出部１１−１は、後述するように、入力された力成分とトルク成分を含む情報に基づき、対象物１３０の中心運動に対してはたらく力を算出する。中心運動に対してはたらく力の算出部１１−１は、算出した対象物１３０の中心運動に対してはたらく力を中心運動のコンプライアンス成分算出部１１−２に出力する。
なお、対象物１３０の中心運動に対してはたらく力とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心において働く力である。すなわち、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２とが、対象物１３０を把持している場合、把持されている対象物１３０の中心がどのように運動するか、あるいは運動しているかを示すものである。

中心運動のコンプライアンス成分算出部１１−２は、後述するように、中心運動に対してはたらく力の算出部１１−１から出力された対象物１３０の中心運動に対してはたらく力を示す情報に基づき、対象物１３０の中心運動のコンプライアンス成分を算出する。中心運動のコンプライアンス成分算出部１１−２は、算出した対象物１３０の中心運動のコンプライアンス成分を中心運動の目標位置算出部１１−５に出力する。
なお、中心運動のコンプライアンス成分とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心におけるコンプライアンスの各成分である。また、コンプライアンスの各成分とは、後述するアーム２の手先とアーム３の手先との中心位置成分と姿勢成分である。また、コンプライアンス（ｃｏｍｐｌｉａｎｃｅ）とは、剛性の逆数のことであり、固定方法が剛結ではなく、入力に対し、ある程度の変形・変位を許す状態である。

第１記憶部１１−３には、予め対象物１３０に対する中心運動の軌道に関する情報が記憶されている。

中心運動の設定軌道算出部１１−４は、第１記憶部１１−３に記憶されている対象物１３０に対する中心運動の軌道に関する情報を読み出し、読み出した情報に基づき、後述するように、対象物１３０に対する中心運動の軌道を算出する。中心運動の設定軌道算出部１１−４は、算出した対象物１３０の中心運動の軌道を中心運動の目標位置算出部１１−５に出力する。
なお、中心運動の設定軌道とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心の搬送時の軌道である。

中心運動の目標位置算出部１１−５には、中心運動のコンプライアンス成分算出部１１−２から出力された対象物１３０の中心運動のコンプライアンス成分を示す情報と、中心運動の設定軌道算出部１１−４から出力された対象物１３０の中心運動の設定軌道を示す情報とが入力される。中心運動の目標位置算出部１１−５は、入力された対象物１３０の中心運動のコンプライアンス成分を示す情報と、対象物１３０の中心運動の設定軌道を示す情報とに基づき、後述するように、対象物１３０の中心運動の目標値を算出する。中心運動の目標位置算出部１１−５は、算出した対象物１３０の中心運動の目標値をアーム１１０の目標位置算出部１１−６及びアーム１２０の目標位置算出部１１−１６に出力する。
なお、中心運動の目標値とは、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２との中心の搬送時の位置及び姿勢の目標制御値である。

アーム１１０の目標位置算出部１１−６には、中心運動の目標位置算出部１１−５から対象物１３０の中心運動の目標値を示す情報と、相対運動の目標位置算出部１１−１５から対象物１３０の相対運動の目標値を示す情報とが入力される。アーム１１０の目標位置算出部１１−６は、入力された対象物１３０の中心運動の目標値を示す情報と、対象物１３０の相対運動の目標値を示す情報とに基づき、後述するように、アーム１１０の目標値を算出する。アーム１１０の目標位置算出部１１−６は、算出したアーム１１０の目標値をアーム１１０の制御信号として、アーム１１０に出力する。
なお、アーム１１０の目標値とは、アーム１１０の先端の指部４０−１が対象物１３０の動作に追従して動作する位置及び姿勢を示すアーム１１０への制御量の目標値である。

相対運動に対してはたらく力の算出部１１−１１には、各アームに取り付けられている第１力センサ６１及び第２力センサ６２から、力成分とトルク成分を含む情報が入力される。相対運動に対してはたらく力の算出部１１−１１は、入力された力成分とトルク成分を含む情報の出力とに基づき、後述するように、相対運動に対してはたらく力を算出する。相対運動に対してはたらく力の算出部１１−１１は、算出した相対運動に対してはたらく力を相対運動のコンプライアンス成分算出部１１−１２に出力する。
なお、相対運動に対してはたらく力とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の相対的な力である。すなわち、アーム部２０−１の先端の指部４０−１とアーム部２０−２の先端の指部４０−２とが、対象物１３０を把持している場合、アーム部２０−１の先端の指部４０−１に対して、アーム部２０−２の先端の指部４０−２がどのように運動するか、あるいは運動しているかを示すものである。

相対運動のコンプライアンス成分算出部１１−１２は、相対運動に対してはたらく力の算出部１１−１１から出力された対象物１３０の相対運動に対してはたらく力を示す情報に基づき、後述するように、対象物１３０の相対運動のコンプライアンス成分を算出する。相対運動のコンプライアンス成分算出部１１−１２は、算出した対象物１３０の相対運動のコンプライアンス成分を相対運動の目標位置算出部１１−１５に出力する。
なお、相対運動のコンプライアンス成分とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２のコンプライアンスの各成分である。また、コンプライアンスの各成分とは、後述するアーム２とアーム３との手先の相対位置成分と姿勢成分である。

第２記憶部１１−１３には、予め相対運動の軌道に関する情報が記憶されている。

相対運動の設定軌道算出部１１−１４は、第２記憶部１１−１３に記憶されている対象物１３０に対する相対運動の軌道に関する情報を読み出し、読み出した情報に基づき、後述するように、対象物１３０に対する相対運動の軌道を算出する。相対運動の設定軌道算出部１１−１４は、算出した対象物１３０の相対運動の軌道を相対運動の目標位置算出部１１−１５に出力する。
なお、相対運動の設定軌道とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の搬送時の軌道である。

相対運動の目標位置算出部１１−１５には、相対運動のコンプライアンス成分算出部１１−１２から出力された対象物１３０の相対運動のコンプライアンス成分を示す情報と、相対運動の設定軌道算出部１１−１４から出力された対象物１３０の相対運動の設定軌道を示す情報とが入力される。相対運動の目標位置算出部１１−１５は、入力された対象物１３０の相対運動のコンプライアンス成分を示す情報と、対象物１３０の相対運動の設定軌道を示す情報とに基づき、後述するように、対象物１３０の相対運動の目標値を算出する。相対運動の目標位置算出部１１−１５は、算出した対象物１３０の相対運動の目標値をアーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する。
なお、相対運動の目標値とは、アーム部２０−１の先端の指部４０−１に対するアーム部２０−２の先端の指部４０−２の搬送時の位置及び姿勢の目標制御値である。

アーム１２０の目標位置算出部１１−１６には、中心運動の目標位置算出部１１−５から対象物１３０の中心運動の目標値を示す情報と、相対運動の目標位置算出部１１−１５から対象物１３０の相対運動の目標値を示す情報とが入力される。アーム１２０の目標位置算出部１１−１６は、入力された対象物１３０の中心運動の目標値を示す情報と、対象物１３０の相対運動の目標値を示す情報とに基づき、後述するように、アーム１２０の目標値を算出する。アーム１２０の目標位置算出部１１−１６は、算出したアーム１２０の目標値をアーム１２０の制御信号として、アーム１２０に出力する。
なお、アーム１２０の目標値とは、アーム１２０の先端の指部４０−２が対象物１３０の動作に追従して動作する位置及び姿勢を示すアーム１２０への制御量の目標値である。

なお、本実施形態においては、対象物１３０は、密度が一様であり、対象物１３０の中心が重心と一致している場合についての例について説明を行っている。

［本実施形態の概要の説明］
次に、本実施形態に係るアーム１１０及びアーム１２０の制御の概要について、図４を用いて説明する。図４は、本実施形態のロボット制御方法を適用するロボット１のアームの概念図である。なお、図４のように、アーム１１０は、図１の一方のアーム部２０−１を模式的に表し、図１の指部４０−１を模式的に表した把持部１１１を有している。また、アーム１２０は、図１の他方のアーム部２０−２を模式的に表し、図１の指部４０−２を模式的に表した把持部１２１を有している。対象物１３０は、アーム１１０とアーム１２０により把持され、運搬される物体である。

図４において、ロボット１は、３つの座標系Σ_１、Σ_２、Σを有している。座標系Σ_１（Ｏ_１−ｘ_１ｙ_１ｚ_１）は、アーム１１０の運動を記述するベース座標系であり、アーム１１０の手先の位置及び姿勢をベクトルｐ_１で表す。また、座標系Σ_２（Ｏ_２−ｘ_２ｙ_２ｚ_２）は、アーム１２０の運動を記述するベース座標系であり、アーム１２０の手先の位置及び姿勢をベクトルｐ_２で表す。図４において、符号Ｏ_１は、座標系Σ_１の原点Ｏ_１であり、符号Ｏ_２は、座標系Σ_２の原点Ｏ_２である。
以下で説明する各式（１）〜式（２０）は、制御部１１がアーム１１０及び１２０の目標値を算出するときに用いる式であり、予め制御部１１内の後述する算出部に定義されて記憶されている。
制御部１１は、ベクトルｐ_１を、位置成分ベクトルｐ_１ｐと、姿勢成分ベクトルｐ_１ｄとにより、次式（１）式のように表される。また、制御部１１は、ベクトルｐ_２を、位置成分ベクトルのｐ_２ｐと、姿勢成分ベクトルｐ_２ｄとにより、次式（２）式のように表される。
また、符号α、β及びγは、３次元空間におけるＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角である。

式（１）と式（２）において、記号^Ｔは、転置行列を表している。また、以下の式でも同様に、記号^Ｔは、転置行列を表している。また、式（１）において、ｐ_１ｘ、ｐ_１ｙ及びｐ_１ｚは、ベクトルｐ_１ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１）において、ｐ_１α、ｐ_１β及びｐ_１γは、ベクトルｐ_１ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
式（２）において、ｐ_２ｘ、ｐ_２ｙ及びｐ_２ｚは、ベクトルｐ_２ｐのｘ軸成分、ｙ軸成分及びｚ軸成分であるまた、式（２）において、ｐ_２α、ｐ_２β及びｐ_２γは、ベクトルｐ_２ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。

また、座標系Σ（Ｏ−ｘｙｚ）は、アーム１１０及びアーム１２０の運動を統一して記述する作業用座標系であり、この座標系においてアーム１１０の手先とアーム１２０の手先との中心の位置成分及び姿勢成分（以下、アームの手先の中心の位置及び姿勢という）を、ベクトルＰで表す。すなわち、ベクトルＰは、アーム１１０及びアーム１２０のそれぞれの先端の把持部１１１と把持部１２１との中心の位置及び姿勢を示す情報である。図４において、符号Ｏは、座標系Σの原点Ｏである。
また、この座標系Σにおいてアーム１１０の手先に対するアーム１２０の相対的な手先位置成分及び姿勢成分（以下、アームの手先同士の相対の位置及び姿勢という）を、ベクトルＲで表す。すなわち、ベクトルＲは、アーム１１０の先端の把持部１１１に対するアーム１２０の把持部１２１の相対の位置及び姿勢を示す情報である。
このような２本のアーム１１０及び１２０のそれぞれの先端から中心の位置をコンプライアンスの中心（剛性中心）、コンプライアンス・センターという。本実施形態では、対象物１３０の中心、すなわちベクトルＰをベクトルＲの中間にした例である。
ベクトルＰは、位置成分ベクトルＰ_ｐと、姿勢成分ベクトルＰ_ｄとにより、次式（３）式のように表される。また、ベクトルＲは、位置成分ベクトルＲ_ｐと、姿勢成分ベクトルＲ_ｄとにより、次式（４）式のように表される。

式（３）において、Ｐ_ｘ、Ｐ_ｙ及びＰ_ｚは、ベクトルＰ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（３）において、Ｐ_α、Ｐ_β及びＰ_γは、ベクトルＰ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。
式（４）において、Ｐ_ｘ、Ｐ_ｙ及びＰ_ｚは、ベクトルＰ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分であるまた、式（４）において、Ｐ_α、Ｐ_β及びＰ_γは、ベクトルＰ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。

なお、図４のように、各座標系Σ_１、Σ_２およびΣの相対位置および姿勢は、３つの座標系Σ_１、Σ_２及びΣのｘ軸が同一直線上にあり、ｙ軸及びｚ軸の向きが等しく、作業座標系において原点間（Ｏ_１、Ｏ_２及びＯ）の関係が次式（５）を満たすように表される。
ただし、図４に示した各座標系の相対位置関係は一例であり、任意に配置することが可能である。

次に、第１力センサ６１から出力は、力成分ベクトルｆ_１ｐとトルク成分ベクトルｆ_１ｄとにより、次式（６）のように表される。また、第２力センサ６２の出力は、力成分ベクトルｆ_２ｐとトルク成分ベクトルｆ_２ｄとにより、次式（７）のように表される。

式（６）において、ｆ_１ｘ、ｆ_１ｙ及びｆ_１ｚは、ベクトルｆ_１ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（６）において、τ_１α、τ_１β及びτ_１γは、アーム１１０のトルク成分ベクトルｆ_１ｄを、ＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角で表したものである。
式（７）において、ｆ_２ｘ、ｆ_２ｙ及びｆ_２ｚは、ベクトルｆ_２ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（７）において、τ_２α、τ_２β及びτ_２γは、アーム１２０のトルク成分ベクトルｆ_２ｄを、ＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角で表したものである。

次に、これらを用いて、アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰに加わる力ベクトルＦは、次式（８）のように表される。また、アームの手先同士の相対位置及び姿勢を示す情報であるベクトルＲに加わる力ベクトルｆは、次式（９）のように表される。なお、説明を簡単にするため、アーム１１０と１２０の各力センサの出力ベクトルｆ_１及びベクトルｆ_２については、把持部に加わる重力、対象物１３０に加わる重力、及び把持部４０−１と４０−２とが加速度運動することによって加わる慣性力は取り除かれていて、更に作業座標系における表現に変換されているものとする。
まず、力ベクトルＦを、次式（８）のように表す。

式（８）において、Ｆ_ｘ、Ｆ_ｙ及びＦ_ｚは、力ベクトルＦの位置成分ベクトルＦ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（８）において、Ｔ_α、Ｔ_β及びＴ_γは、力ベクトルＦの姿勢成分ベクトルＦ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角の成分である。なお、式（８）において、手先を対向させて物体を把持するため、互いに回転角γの回転方向が逆になるため、τ_γ＝−τ_２γ−τ_１γとなる。

次に、力ベクトルｆを、次式（９）のように表す。

式（９）において、ｆ_ｘ、ｆ_ｙ及びｆ_ｚは、ベクトルｆ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（９）において、τ_α、τ_β及びτ_γは、トルク成分ベクトルｆ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。

アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰは、把持した対象物１３０の位置及び姿勢を制御して搬送するようにあらかじめ設定された軌道ベクトルＰ_Ｔに対して、上述した力ベクトルＦに応じてベクトルΔＰだけ軌道を変更したものとして表される。これにより、例えば障害物１４０などから物体に加わる力に応じて、受動的にその位置及び姿勢を変更してコンプライアンスを守りながら、所定の位置及び姿勢に物体を操作することを可能とする。

また、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲは、物体を把持するようにあらかじめ設定された軌道ベクトルＲ_Ｔに対して、上述した力ベクトルｆに応じてベクトルΔＲだけ軌道を変更したものとして表される。これにより、アームが物体に加える力について、コンプライアンスを守りながら物体を把持することを可能とする。従って、ある時刻ｔにおけるアームの手先の中心の位置及び姿勢を示す情報であるベクトルＰ、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲは、次式（１０）と次式（１１）のように表される。

式（１０）において、ベクトルΔＰは、障害物１４０に加わる外力に基づき、力ベクトルＦに応じて変更した軌道を示すベクトルである。式（１１）において、ベクトルΔＲは、障害物１４０に加える内力に基づき、力ベクトルｆに応じて、変更した軌道を示すベクトルである。
次に、式（１０）と式（１１）において、ベクトルＰ_ＴおよびベクトルＲ_Ｔは、各々、次式（１２）と次式（１３）のように、位置成分と姿勢成分とに分けて表される。

式（１２）において、Ｐ_Ｔｘ、Ｐ_Ｔｙ及びＴ_Ｔｚは、ベクトルＰ_Ｔｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１２）において、Ｐ_Ｔα、Ｐ_Ｔβ及びＰ_Ｔγは、トルク成分ベクトルＰ_ＴｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
式（１３）において、Ｒ_Ｔｘ、Ｒ_Ｔｙ及びＲ_Ｔｚは、ベクトルＲ_Ｔｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１３）において、Ｒ_Ｔα、Ｒ_Ｔβ及びＲ_Ｔγは、トルク成分ベクトルＲ_ＴｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。

同様に、式（１０）と式（１１）において、ベクトルΔＰおよびベクトルΔＲは、各々、次式（１４）と次式（１５）のように、位置成分と姿勢成分とに分けて表される。

式（１４）において、ΔＰ_ｘ、ΔＰ_ｙ及びΔＴ_ｚは、ベクトルΔＰ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１４）において、ΔＰ_α、ΔＰ_β及びΔＰ_γは、トルク成分ベクトルΔＰ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。
式（１５）において、ΔＲ_ｘ、ΔＲ_ｙ及びΔＲ_ｚは、ベクトルΔＲ_ｐのｘ軸成分、ｙ軸成分及びｚ軸成分である。また、式（１５）において、ΔＲ_α、ΔＲ_β及びΔＲ_γは、トルク成分ベクトルΔＲ_ｄのＺＹＺ固定角表現におけるｚ軸、ｙ軸、ｚ軸に対する各回転角成分である。

次に、コンプライアンス制御の例としてインピーダンス制御について説明する。図５は、インピーダンス制御の概念を説明する図である。インピーダンス制御とは、図５のように、指部４０−１と指部４０−２との間に、仮想的な弾性係数ｋを有するばね、粘性係数Ｄを有するダンパー、重量Ｍを有する物体があるかのうように指部４０−１及び指部４０−２の運動を制御することである。図５のように、指部４０−１には、ばねの一端とダンパーの一端とが接続されている。ばねの他端とダンパーの他端は、物体の一端に接続されている。また、物体の他端は、指部４０−２に接続されている。

インピーダンス制御においては、ベクトルΔＰおよびベクトルΔＲをそれぞれベクトルＦ及びベクトルｆに応じて、仮想的なインピーダンスのもとで運動する対象物１３０の位置において設定する。ここで、インピーダンスは慣性力、ダンパーなどのように速度に依存して発生する力及びバネなどのように位置に依存して発生する力で構成されるとすると、ベクトルＰ及びベクトルＲは、次式（１６）と次式（１７）の運動方程式に従う。

式（１６）において、Ｍ_ｐ＾は、ベクトルＦの慣性行列（慣性係数）、Ｄ_ｐ＾は、ベクトルＦの粘性行列（粘性係数）、Ｋ_ｐ＾は、ベクトルＦの弾性行列（弾性係数）である。なお、式（１６）において、上付きに＾が表示されている各係数を、それぞれ便宜的にＭ_ｐ＾、Ｄ_ｐ＾およびＫ_ｐ＾と表す。
また、式（１７）において、Ｍ_Ｒ＾は、ベクトルｆの慣性行列（慣性係数）、Ｄ_Ｒ＾は、ベクトルｆの粘性行列（粘性係数）、Ｋ_Ｒ＾は、ベクトルｆの弾性行列（弾性係数）である。なお、式（１７）において、上に＾が表示されている各係数を、それぞれ便宜的にＭ_Ｒ＾、Ｄ_Ｒ＾およびＫ_Ｒ＾と表す。なお、Ｐの運動とＲの運動の振る舞いを定義した運動方程式とは、式（１６）および式（１７）である。
各係数Ｍ_ｐ＾、Ｄ_ｐ＾、Ｋ_ｐ＾、Ｍ_Ｒ＾、Ｄ_Ｒ＾およびＫ_Ｒ＾は、６行６列の対角行列である。対角成分にそれぞれ独立の値を設定することで、アームの手先の中心の位置及び姿勢を示す情報であるベクトルＰとアームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲとの各軸に対する運動を、個別に制御することが可能となる。ただし、これは一例であり、Ｍ_ｐ＾、Ｄ_ｐ＾、Ｋ_ｐ＾、Ｍ_Ｒ＾、Ｄ_Ｒ＾およびＫ_Ｒ＾は非対角要素を持つ行列であっても構わない。
なお、制御部１１は、仮想的なインピーダンスに加え、対象物のインピーダンスを用いて算出するようにしてもよい。

次に、制御部１１は、式（１６）と式（１７）を解き、ベクトルΔＰとベクトルΔＲを求める。次に、制御部１１は、式（１０）および式（１１）に、求めたベクトルΔＰとベクトルΔＲを代入して、ベクトルＰとベクトルＲを求める。
次に、制御部１１は、求めたベクトルＰとベクトルＲを次式（１８）と次式（１９）に代入し、アーム１２０のベース座標系における手先位置及び姿勢を示す情報であるベクトルｐ_１、アーム１２０のベース座標系における手先位置及び姿勢を示す情報であるベクトルｐ_２を求める。

式（１８）と式（１９）において、ベクトルＤは、次式（２０）で表される。

式（２０）におけるＤは、図１のように各アーム１１０の原点Ｏ_１または原点Ｏ_２とΣの原点Ｏの間隔である。
制御部１１は、アーム部１１０と把持部１１１、アーム部１２０と把持部１２１に対して式（１８）及び式（１９）で求めた位置に移動するよう指令を出す。以上が本実施形態に係る制御の概要である。

［障害物から対象物に力が加わった場合の説明］
次に、障害物から対象物に力が加わった場合の説明を、図６〜図１７を用いて行う。図６は、本実施形態に係る対象物が障害物に当たった場合を説明する図であり、図７は、本実施形態に係る対象物が障害物に当たった後、各アームが制御される場合を説明する図である。

図６においては、アーム１１０とアーム１２０が対象物１３０を把持した状態で、対象物１３０に障害物１４０が衝突する。この場合、以下に詳細に説明する制御により、図７のように障害物１４０から受ける力に応じて、制御部１１の制御により、受動的にその位置及び姿勢を変更する。

まず、対象物１３０を把持して操作している際に、対象物１３０が障害物１４０と接触した時に発生する力及びトルクについて、以下に説明する。
図６および図７において、アーム１１０及びアーム１２０の把持部１１１及び把持部１２１は、アーム１１０の手先の把持部１１１とアーム１２０の手先の把持部１２１との中心位置と、対象物１３０の中心が一致するように対象物１３０を把持している。ここで、対象物１３０を把持した状態で、アームの手先の中心の位置及び姿勢について、軌道ベクトルＰ_Ｔを、制御部１１は、式（１２）に基づき、次式（２１）のように算出する。なお、対象物１３０の中心がアーム１１０により搬送される軌道は、予めロボット制御装置１のオペレータにより各時刻における移動位置、移動速度が軌道に関する情報として設定される。そして、オペレータにより設定された軌道に関する情報が、予め制御部１１の第１記憶部１１−３に記憶されている。

式（２１）の軌道ベクトルＰ_Ｔは、アームの手先の中心の位置及び姿勢について、ｘ軸及びｙ軸方向の位置及び姿勢を変化させずに、ｙ軸方向の位置を初期位置ｙ_０から速度ｖで移動させる軌道である。
次に、アームの手先同士の相対位置及び姿勢について、対象物１３０を把持している状態において、軌道ベクトルＲ_Ｔを、制御部１１は、式（１３）に基づき、次式（２２）のように算出する。なお、搬送される対象物１３０において、アーム１１０に対するアーム１２０の手先同士の相互位置の軌道は、予めロボット制御装置１のオペレータにより各時刻における移動位置、移動速度が軌道に関する情報として設定される。そして、オペレータにより設定された相対的な軌道に関する情報が、予め制御部１１の第２記憶部１１−１３に記憶されている。

式（２２）において、ｗ_０は、アーム１１０及び１２０が、対象物１３０に加える力に応じて、制御部１１が選択する値であり、選択については後述する。
なお、対象物１３０は、図８の対象物１３０の斜視図に示すように幅ｗ、高さｈ、奥行きｌ、質量ｍの直方体であり、質量分布が一様でその重心が物体の中心にある。また、対象物１３０の中心を通り、作業座標系Σのｚ軸と平行な直線をｃとする。すなわち、対象物１３０の中心は、幅ｗ、高さｈ、奥行きｌ、質量ｍのそれぞれの中心が重なる位置である。
図９は、本実施形態に係る対象物１３０と障害物１４０との位置関係を説明する図である。障害物１４０は円筒形であり、対象物１３０と障害物１４０とは、図９のような位置関係で接している。すなわち、ｘｚ平面における対象物１３０の面は、障害物１４０の円周面と接する。より具体的には、ｘｙ平面の長手方向に対して、障害物１４０の長手方向が、直交する向きで接する。
図６に示したように、式（２１）の軌道に従って、制御部１１は、対象物１３０の位置を移動するように制御すると、対象物１３０が中心から距離ｄの位置で障害物１４０と接する。

図１０は、本実施形態に係る障害物１４０が対象物１３０を押す力を説明する図である。図１０のように、障害物１４０が対象物１３０を押す力をベクトルｆ_ｂとすると、対象物１３０と障害物１４０とが接した瞬間、ベクトルｆ_ｂは、−ｙ方向を向いている。
ベクトルｆ_ｂについて、対象物１３０の中心方向の成分をベクトルｆ_ｂｃ、ベクトルｆ_ｂｃと直行する方向の成分をベクトルｆ_ｂ０とする。この場合、ベクトルｆ_ｂｃは、対象物１３０を並進させる力となる。ベクトルｆ_ｂ０は、直線ｃを回転軸として、対象物１３０を回転させるトルクのベクトルτ_ｃを発生させている。

次に力ベクトルｆ_ｂｃ及びトルクのベクトルτ_ｃが、アーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクについて、図１１を用いて説明する。図１１は、本実施形態に係るアーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される力及びトルクを説明する図である。図１１において、符号ｆ_ｈは、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆ_ｈである。

アーム１１０及びアーム１２０は、制御部１１の制御により、対象物１３０を把持するために、それぞれの把持部１１１及び１２１を、それぞれｘ軸方向に−ｆ_ｈ、ｆ_ｈの力で対象物１３０に押し付けている。したがって、第１力センサ６１及び第２力センサ６２にはその反力が入力される。このようにアーム１１０及びアーム１２０が互いに押し付けあっているため、ベクトルｆ_ｂｃのｘ成分−ｆ_ｂｃｘはそれぞれの把持部１１１及び１２１に取り付けられた力センサ６１及び６２に入力される。
また、それぞれのｙ成分には力ベクトルｆ_ｃのｙ成分−ｆ_ｂｃｙが等分配されて、各力センサ６１及び６２に入力される。更に、第１力センサ６１及び第２力センサ６２のα成分には、トルクのベクトルτ_ｃによって発生する−τ_ｂが入力される。

以上により、アーム１１０と把持部１１１との間に取り付けられた第１力センサ６１の出力ベクトルｆ_１と、アーム１２０と把持部１２１との間に取り付けられた第２力センサ６２の出力ベクトルｆ_２は、それぞれ次式（２３）と次式（２４）のようになる。

次に、アームの手先の中心の位置及び姿勢を示す情報のベクトルＰに加わる力ベクトルＦの計算について説明する。次に、計算したベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動について説明する。また、それに応じたアーム１１０及びアーム１２０の位置及び姿勢を示す情報のベクトルｐ_１、及びベクトルｐ_２の運動について説明する。
まず、制御部１１は、式（８）に式（２３）式及び式（２４）を代入し、次式（２５）を算出する。

次に、ベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動について、図１２を用いて説明する。図１２は、ベクトルＦに応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＰの運動を説明する図である。
まず、制御部１１は、式（２５）に式（１６）を代入し、ベクトルΔＰを算出する。なお、式（１６）は非線形の微分方程式であるので、制御部１１は、例えば、オイラー法やルンゲクッタ法などの数値計算法を用いてベクトルΔＰを計算する。実際の制御においては、制御周期の長短に応じて計算方法を選択する必要があり、制御周期が長い場合は計算の精度が比較的高いルンゲクッタ法で算出する方が良い。すなわち、制御部１１は、高次の多項式を用いて近似して算出（時間に関する高次の多項式を用いて解く、とも言う）する。なお、高次の多項式とは、一般的に４次以上である。例えばルンゲクッタ法であれば４次までとることで、一般的な物理現象の運動方程式を精度良く解くことが可能である。

次に、式（１６）の両辺をラプラス変換して、次式（２６）と次式（２７）を算出して、ΔＰの振る舞いを定性的に説明する。

式（２６）と式（２７）は、ベクトルＦを入力ベクトルとし、ΔＰを出力とすると、それらが次式（２８）なる２次遅れ系で結ばれていると考えることが出来る。

従って ベクトルΔＰは、ベクトルＦに２次遅れで追従する。ここではまず、ベクトルＰの位置成分ベクトルＰ_ｐの運動について説明する。ベクトルΔＰはベクトルＦに２次遅れで追従することから、図１２に示すように、ベクトルΔＰはベクトルＦ_ｐに倣う方向を向く。
従って、式（１０）及び式（２１）から、ベクトルＰ_ｐは図１２に示すベクトルとなる。これにより式（１８）及び式（１９）から、アーム１２０及びアーム１２０の位置成分ベクトルｐ_１ｐ及びベクトルｐ_２ｐは、図１２に点線で示したベクトルｐ_１’及びベクトルｐ_２’となる。このため、対象物１３０を把持しながら障害物１４０との間に発生する力を緩和する方向に、アーム１１０及びアーム１２０は移動する。

次にベクトルＰの姿勢成分ベクトルＰ_ｄの運動について説明する。上述したように、ベクトルΔＰは、ベクトルＦに２次遅れで追従することから、図１３に示すように、ベクトルＰ_ｄは、ｚ軸に関してｙ軸からｘ軸への向きである負方向に回転する。式（１８）及び式（１９）から、アーム１１０及びアーム１２０の位置成分ベクトルｐ_１ｄ及びベクトルｐ_２ｄも、図１３に示したようにｚ軸に関して負方向に回転する。図１３は、本実施形態に係るベクトルＰの姿勢成分ベクトルＰ_ｄの運動について説明する図である。

次に、ベクトルｆの計算について説明する。さらに、上述の回転に応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＲの運動について説明する。更にそれに応じたアーム１１０及びアーム１２０の位置及び姿勢を示す情報のベクトルｐ_１、ベクトルｐ_２の運動について説明する。
図１４は、本実施形態に係る回転に応じたアームの手先の中心の位置及び姿勢を示す情報のベクトルＲの運動について説明する図である。アーム１１０及びアーム１２０が、ｚ軸に関して負方向に回転するに伴い、図１４（ａ）に示すように、アーム１１０及びアーム１２０には、対象物１３０に対して加える力ベクトルｆ_ｒが発生する。このベクトルｆ_ｒを、ｘ軸方向及びｙ軸方向に分解すると、ｘ軸方向については、アーム１１０が加える力とアーム１２０が加える力とで相殺する。従って残るｙ軸方向について、それぞれその反力ｆ_ｒｙ、−ｆ_ｒｙが、アーム１１０及びアーム１２０と把持部１１１及び１２１との間に取り付けられた第１力センサ６１及び第２力センサ６２に入力される。

次に、制御部１１は、式（９）に式（２３）及び式（２４）を代入する。そして、上述した結果を考慮して制御部１１がベクトルｆを計算すると、ベクトルｆは、次式（２９）のように表される。

式（２９）において、x軸方向の力は、対象物１３０をアーム１１０及びアーム１２０が把持する力に対する反力である。これに応じて対象物１３０を落下させないように、制御部１１が、アームの手先同士の相対の位置及び姿勢の軌道ベクトルＲ_Ｔのｘ成分について、式（２２）に示したようにｗ_０に選択している。更に、制御部１１は、仮想的に設定するインピーダンスの制御変数も合わせて調整している。この制御変数の調整については後に詳述する。

次に、式（２９）で示したベクトルｆのｙ成分のみを考慮して、アームの手先同士の相対の位置及び姿勢を示す情報であるベクトルＲの運動について説明する。式（１７）の両辺をラプラス変換すると、次式（３０）と次式（３１）が得られる。

式（３１）において、ベクトルｆを入力、ベクトルΔＲを出力とすると、それらが次式（３２）のような２次遅れ系で結ばれていると考えることが出来る。

従ってベクトルΔＲは、ベクトルｆに２次遅れで追従する。式（２９）に示したように、ベクトルｆの回転成分は０（ゼロ）であるから、ベクトルＲの位置成分ベクトルＲ_Ｐの運動について説明する。
ベクトルΔＲは、ベクトルｆに２次遅れで追従することから、図１２（ｂ）に示すように、ベクトルΔＲは、ベクトルｆ_Ｐのｙ成分−f_ｒｙに倣う方向を向く。従って、式（１１）及び式（２２）から、ベクトルＲ_Ｐは、図１４（ｂ）に示すベクトルとなる。更に式（１８）及び式（１９）から、アーム１１０及びアーム１２０の位置成分ベクトルｐ_１Ｐ、ｐ_２Ｐは、図１４（ｂ）に点線で示したベクトルｐ_１’及びベクトルｐ_２’となる。このため、制御部１１の制御により、対象物１３０を把持しながら障害物１４０との間に発生する力を緩和する方向にアーム１１０及びアーム１２０が移動する。

この制御により、図６に示したようにアーム１１０及びアーム１２０が物体を把持した状態で、障害物１４０に対象物１３０が衝突すると、図７に示したように障害物１４０から受ける力に応じて受動的にその位置及び姿勢を変更して、コンプライアンスを守る。

次に、アーム１１０及びアーム１２０が、把持する力に対する反力に応じて、対象物１３０を落下させないように、あらかじめ設定されるアームの手先同士の相対の位置及び姿勢の軌道ベクトルＲ_Ｔのｘ成分及び仮想的なインピーダンスの制御変数について図１５を用いて説明する。図１５は、本実施形態に係るインピーダンスの制御変数を説明する図である。

まず、対象物１３０を弾性体として、それを把持する際の振舞いについて説明する。ベクトルｆのｘ成分ｆ_ｘは、次式（３３）のように、相対位置のｘ成分Ｒ_ｘが、対象物１３０の幅ｗに対して、それよりも内側に押し込んだ時に、その押し込んだ距離と対象物１３０の弾性係数ｋに比例した力を受ける。

また、式（１１）より手先の相対位置のｘ成分Ｒ_ｘは、次式（３４）のように表される。

更に式（３１）より、ベクトルΔＲのｘ成分は、次式（３５）のように表される。

次に、制御部１１は、式（３５）に式（３３）及び式（３４）を代入して、その振る舞いを解析する。この時、ベクトルＲ_Ｔのｘ成分Ｒ_Ｔｘを、次式（３６）のように予め設定しておく。

図１６は、ΔＲ_ｘ及びｆ_ｘの振る舞いを表す図である。図１６（ａ）に示したのは、ΔＲ_ｘの振る舞いであり、縦軸がΔＲ_ｘ、横軸が時間である。また、図１６（ｂ）に示したのは、ｆ_ｘの振る舞いであり、縦軸がｆ_ｘ、横軸が時間である。図１６（ａ）から、ΔＲ_ｘはΔＲ_ｘ＝ｗ−ｗ_０、すなわちＲ_Ｔｘ＝ｗとなり、手先が対象物１３０の表面に接している位置から、Ｋ_Ｒｘ（ｗ−ｗ_０）／（ｋ＋Ｋ_Ｒｘ）だけ押し込んだ位置で静止している。
それゆえ、図１６（ｂ）に示したように、ｆ_ｘはそれに対象物１３０の弾性係数ｋを乗じた値になっている。
従って、アーム１１０及びアーム１２０が、対象物１３０に対して押し付ける力ｆ_ｈは、次式（３７）のようになる。

従って、対象物１３０の幅ｗ及び弾性係数ｋが既知であるとすれば、制御部１１は、押し付け力ｆ_ｈを算出し、それを満たすＫ_Ｒｘ及びｗ_０を用いることで所望の押し付け力を実現することが出来る。この時、ｗ_０は、ｗを超えない範囲で大きな値とし、それに応じてＫ_Ｒｘを決定することで、アーム１１０に対するアーム１２０の相対的な目標位置を遠ざけることが可能である。
また、対象物１３０の弾性係数ｋが、Ｋ_Ｒｘとの間に、ｋ＞＞Ｋ_Ｒｘの関係が成り立つ時、すなわち、対象物１３０が固い場合、式（３７）は、次式（３８）のようになる。

この場合、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆ_ｈは、対象物１３０の弾性係数ｋと無関係に決定されるため、弾性係数ｋが未知であっても、押し付け力ｆ_ｈを設定し、それを満たすＫ_Ｒｘ及びｗ_０を用いることで所望の押し付け力を実現することが出来る。
次に、制御部１１は、式（３７）を解析的に求める。まず、制御部１１は、式（３５）に式（３３）及び式（３４）を代入し、ΔＲ_ｘについて解き、次式（３９）を算出する。

次に、時刻Ｔ以降の振る舞いを解析するため、式（３９）右辺のｋ（ｗ―Ｒ_Ｔｘ）は、ｋ（ｗ−ｗ_０）のステップ入力として扱う。このため、式（３９）は、式（３８）より、次式（４０）のように表される。

式（３９）及びラプラス変換の最終値の定理を用いて、制御部１１は、次式（４１）のように、ΔＲ_ｘのｔ→∞（無限大）の極限を求める。

次に、制御部１１は、式（３３）に式（３４）及び式（４１）を代入して、次式（４２）のように、アーム１１０及びアーム１２０が対象物１３０に対して押し付ける力ｆ_ｈを求める。

また、ベクトルＲ_Ｔのｘ成分Ｒ_Ｔｘを、次式（４３）のように５次式で表す。

なお、式（４３）において、各係数ｃ_０、ｃ_３、ｃ_４、ｃ_５は、次式（４４）である。

式（４３）のようにＲ_Ｔｘを５次式で表すことで、アーム１１０及びアーム１２０の加速及び減速が緩やかになり、図１７に示すように、図１６で見られた急激な力の変化は抑制される。図１７は、Ｒ_Ｔｘを５次式で表した場合のΔＲ_ｘ及びｆ_ｘの振る舞いを表す図である。図１７（ａ）に示したのは、ΔＲ_ｘの振る舞いであり、縦軸がΔＲ_ｘ、横軸が時間である。また、図１７（ｂ）に示したのは、ｆ_ｘの振る舞いであり、縦軸がｆ_ｘ、横軸が時間である。

次に、本実施形態に係る制御方法を、図１８を用いて説明する。図１８は、本実施形態に係る動作の一例を説明するフローチャートである。
制御部１１は、作業指示が入力された後、ステップＳ１〜ステップＳ１０を行う。
制御部１１の中心運動の設定軌道算出部１１−４は、第１記憶部１１−３に記憶されているアーム１１０の手先及びアーム１２０の手先の中心の軌道に関する情報を読み出し、読み出した情報に基づき、作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰ_Ｔを生成する。
また、相対運動の設定軌道算出部１１−１４は、第２記憶部１１−１３に記憶されているアーム１１０に対するアーム１２０の手先同士の相対的なの軌道に関する情報に基づき、作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲ_Ｔを算出する。中心運動の設定軌道算出部１１−４は、算出した作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰ_Ｔを中心運動の目標位置算出部１１−５に出力する。また、相対運動の設定軌道算出部１１−１４は、算出した作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲ_Ｔを相対運動の目標位置算出部１１−１５に出力する（ステップＳ１）。

次に、制御部１１は、内部タイマーの現在時刻ｔをリセット（ｔ＝０）する（ステップＳ２）。

次に、中心運動に対してはたらく力の算出部１１−１は、第１力センサ６１からの出力であるベクトルｆ_１を取得する。また、相対運動に対してはたらく力の算出部１１−１１は、第２力センサ６２からの出力であるベクトルｆ_２を取得する（ステップＳ３）。

次に、中心運動に対してはたらく力の算出部１１−１は、取得した第１力センサ６１からの出力であるベクトルｆ_１に基づき、アームの手先の中心の位置及び姿勢に加わる力ベクトルＦを算出する。中心運動に対してはたらく力の算出部１１−１は、算出したアームの手先の中心の位置及び姿勢に加わる力ベクトルＦを中心運動のコンプライアンス成分算出部１１−２に出力する。また、相対運動に対してはたらく力の算出部１１−１１は、取得した第２力センサ６２からの出力であるベクトルｆ_２に基づき、アームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆを算出する。相対運動に対してはたらく力の算出部１１−１１は、算出したアームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆを相対運動のコンプライアンス成分算出部１１−１２に出力する（ステップＳ４）。

次に、中心運動のコンプライアンス成分算出部１１−２は、中心運動に対してはたらく力の算出部１１−１から出力されたアームの手先の中心の位置及び姿勢に加わる力ベクトルＦに基づき、手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰを算出する。中心運動のコンプライアンス成分算出部１１−２は、算出した手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰを中心運動の目標位置算出部１１−５に出力する。
相対運動のコンプライアンス成分算出部１１−１２は、相対運動に対してはたらく力の算出部１１−１１から出力されたアームの手先同士の相対の位置及び姿勢に加わる力ベクトルｆに基づき、手先の相対の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲを算出する。相対運動のコンプライアンス成分算出部１１−１２は、算出した手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲを相対運動の目標位置算出部１１−１５に出力する（ステップＳ５）。

次に、中心運動の目標位置算出部１１−５には、中心運動のコンプライアンス成分算出部１１−２から出力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰと、中心運動の設定軌道算出部１１−４から出力された作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰ_Ｔとが入力される。中心運動の目標位置算出部１１−５は、入力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＰと、作業終了時刻Ｔまでの各時刻の手先の中心の目標位置及び姿勢軌道ベクトルＰ_Ｔとに基づき、現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰ（式（１０））を算出する。中心運動の目標位置算出部１１−５は、算出した現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰを、アーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する。
また、相対運動の目標位置算出部１１−１５には、相対運動のコンプライアンス成分算出部１１−１２から出力された手先の相対の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲと、相対運動の設定軌道算出部１１−１４から出力された作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲ_Ｔとが入力される。相対運動の目標位置算出部１１−１５は、入力された手先の中心の目標位置及び姿勢に対するコンプライアンス成分ベクトルΔＲと、作業終了時刻Ｔまでの各時刻の相対の手先の目標位置及び姿勢軌道ベクトルＲ_Ｔとに基づき、現在時刻の手先の相対の目標位置及び姿勢を示す情報であるベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲ（式（１１））を算出する。すなわち、相対運動の目標位置算出部１１−１５は、予定された軌道を、実際の動作に基づき補正した軌道に基づき、実際の動作に応じた現在時刻の手先の相対の目標位置及び姿勢を算出している。
相対運動の目標位置算出部１１−１５は、算出した現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲを、アーム１１０の目標位置算出部１１−６とアーム１２０の目標位置算出部１１−１６とに出力する（ステップＳ６）。

次に、アーム１１０の目標位置算出部１１−６には、中心運動の目標位置算出部１１−５から現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰと、相対運動の目標位置算出部１１−１５から現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲとが入力される。アーム１１０の目標位置算出部１１−６は、入力された現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰと、現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲとに基づき、アーム１１０の真の目標位置及び姿勢を示す情報のベクトルｐ_１（式（１８））を算出する。
また、アーム１２０の目標位置算出部１１−１６には、中心運動の目標位置算出部１１−５から現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰと、相対運動の目標位置算出部１１−１５から現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲとが入力される。アーム１２０の目標位置算出部１１−１６は、入力された現在時刻の手先の中心の真の目標位置及び姿勢を示す情報のベクトルＰ＝ベクトルＰ_Ｔ＋ベクトルΔＰと、現在時刻の手先の相対の真の目標位置及び姿勢を示す情報のベクトルＲ＝ベクトルＲ_Ｔ＋ベクトルΔＲとに基づき、アーム１２０の真の目標位置及び姿勢を示す情報のベクトルｐ_２（式（１９））を算出する（ステップＳ７）。

次に、アーム１１０の目標位置算出部１１−６は、算出したアーム１１０の真の目標位置及び姿勢を示す情報のベクトルｐ１に基づき、アーム１１０の目標軌道の補正値である制御値を生成し、生成した制御値に基づき、アーム１１０を制御する。
また、アーム１２０の目標位置算出部１１−１６は、算出したアーム１２０の真の目標位置及び姿勢を示す情報のベクトルｐ_２に基づき、アーム１２０の目標軌道の補正値である制御値を生成し、生成した制御値に基づき、アーム１２０を制御する（ステップＳ８）。

次に、制御部１１は、内部タイマーの現在時刻ｔに制御周期Δｔを加算する（ステップＳ９）。

次に、制御部１１は、加算後の内部タイマー値がｔ＝Ｔ（作業終了時刻）であるか否かを判定する（ステップＳ１０）。
ステップＳ１０の判定の結果、内部タイマー値がｔ＝Ｔ（作業終了時刻）ではない場合（ステップＳ１０；Ｎｏ）、制御部１１は、ステップＳ２に戻り、ステップＳ２〜ステップＳ１０の処理を繰り返す。
ステップＳ１０の判定の結果、内部タイマー値がｔ＝Ｔ（作業終了時刻）の場合（ステップＳ１０；Ｙｅｓ）、制御部１１は、制御処理を終了し、次の作業指示を待つ。

以上のように、２本のアーム１１０及び１２０の運動を、アーム１１０の手先及びアーム１２０の手先の中心の位置及び姿勢を示す情報で表し、アーム１１０に対するアーム１２０の手先同士の相対の位置及び姿勢を示す情報で表した。更に予めアーム１１０の手先及びアーム１２０の手先の中心の目標軌道を設定して記憶部に記憶させ、予めアーム１１０に対するアーム１２０の手先の相対的な目標軌道を設定して記憶部に記憶させることで、対象物１３０を落下させてしまう、もしくは過大な力を加えて破壊してしまうといった課題を解決した。
また、制御部１１は、２本のアーム１１０及び１２０に取り付けられた第１力センサ６１及び第２力センサ６２の値から上述の２つの運動に対する力を表し、目標軌道に対する補正量を、それらの力に対して仮想的なインピーダンスを受けて運動する対象物の位置として計算した。この結果、対象物１３０が落下しても、補正量が０となり、第１記憶部１１−３及び第２記憶部１１−１３に記憶されている予め設定された目標軌道に、２本のアーム１１０及び１２０の手先が移動するだけであって、両者が衝突することを防ぐことができる。
更に、上述のアームの手先同士の相対の位置及び姿勢の運動に対して、仮想的に設定するインピーダンスは、対象物１３０を把持する際に設定する押し付け力から算出することが可能であり、所望の押し付け力を実現可能とした。

また、本実施形態では、物体１３０のインピーダンスを仮想的に設定して制御する例を説明した。物体１３０が剛体である場合、物体１３０のインピーダンスを考慮せずに、制御部１１は、相対位置及び姿勢Ｒの仮想的なインピーダンスを設定し、所望の押し付け力を制御するようにしてもよい。

［従来技術との比較］
次に、従来技術と本実施形態との比較を行う。
従来技術の場合、図３０のような構成のため、アーム１の目標値は、アーム１の力センサ９６１の出力と予め設定されているアーム１の軌道データのみに基づいて算出されている。また、アーム２の目標値は、アーム２の力センサ９６２に出力に基づいてアーム１の現在位置を推定し、推定したアーム１の位置に基づいてアーム２の目標値を算出していた。このため、対象物１３０に障害物１４０が当たった場合、対象物１３０を落下させてしまう場合もあった。さらに、対象物１３０を落下させてしまった場合、アーム２の力センサ９６２に加わる力がなくなるため、アーム２は、アーム１に向けて移動するような目標値が算出される。この結果、アーム２が、アーム１に衝突してしまう場合もあった。

これに対し、本実施形態の場合、第１アーム１１０と、第２アーム１２０との間にインピーダンスが仮想的にあるとして、インピーダンス制御する。アーム１１０及び１２０の第１力センサ６１及び第２力センサ６２の出力に基づき、対象物１３０の中心運動に対してはたらく力に基づき中心運動のコンプライアンス成分を算出し、相対運動に対してはたらく力に基づき相対運動のコンプライアンス成分を算出している。さらに、中心運動及び相対運動の各時刻における設定軌道を算出している。そして、中心運動のコンプライアンス成分と中心運動の各時刻における設定軌道とに基づき中心運動の目標値を算出している。また、相対運動のコンプライアンス成分と相対運動の各時刻における設定軌道とに基づき相対運動の目標値を算出している。さらに、中心運動の目標値と相対運動の目標値とに基づき、アーム１１０及び１２０の目標値を算出して、アーム１１０及び１２０とを制御している。
この結果、対象物１３０が、障害物１４０に当たった場合でも、対象物１３０を落下させないように適切な押しつけ力で、アーム１１０及び１２０を制御することができる。また、仮に対象物１３０が、障害物１４０に当たり落下した場合においても、従来技術とは異なり、アーム１１０及び１２０との間には、インピーダンス制御により対象物１３０を仮想的に把持されているため、第１力センサ６１及び第２力センサ６２が外力を検出できない状態になっても、アーム１２０がアーム１１０に衝突することを防ぐことができる。

次に、非特許文献１と本実施形態との比較を行う。
非特許文献１では、対象物のインピーダンスは、次式（４５）〜（４７）（非特許文献１の式（２１）〜（２３））である。これらの式（４５）〜（４７）を用いて、次式（４８）（非特許文献１の式（１３））を書き換えると、アームの相対位置の運動方程式は、次式（４９）のようになる。

式（４５）〜式（４９）において、Ｆ_Ｒは、対象物間に働く力であり、Δｘは、対象物間に働く力に対する対象物の相対変位であり、Ｍ_Ｒ、Ｄ_Ｒ、Ｋはそれぞれ正定の慣性行列、減衰係数行列及びバネ定数行列である。
また、アームの重心位置の運動方程式は、次式（５０）（非特許文献１の式（１））である。

式（５０）において、Ｆ^ｅｘｔは、対象物に加わる外力であり、Δｘは、対象物に加わる外力Ｆ^ｅｘｔに対する対象物の位置及び姿勢の変位であり、Ｍ、Ｄ、Ｋはそれぞれ対象物の見かけの慣性行列、減衰係数行列及びバネ定数行列である。
式（５０）と式（４９）とを比べるとゲインは異なるが、周波数特性は同じである。すなわち、物体のコンプライアンス・センターに関する運動と、部品間の相対運動とは同じ運動特性を持つ必要がある。

一方、本実施形態では、物体のコンプライアンス・センターに関する運動と、部品間（エンドエフェクタ間）の相対運動とで異なる運動特性を設定することが可能である。このため、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。

［第２実施形態］
第１実施形態では、ロボット１は、対象物１３０を搬送する例を説明した。第２実施形態においては、ロボット１は、２本のアーム１１０及び１２０で、対象物２０１を把持して、所定の部品２１１に組み付ける場合について説明する。
図１９は、本実施形態に係る対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。 図２０は、本実施形態に係るフレームがずれた場合、対象物（部品）を把持して、所定の部品２１１に組み付ける一例を説明する図である。図１９及び図２０において、対象物２０１は、突起部の先端部２０２を有する凸状の部品であり、突起部以外の両側を２本のアーム１１０及び１２０で把持されている。また、フレーム２１１は、凹状の物体であり、凹部２１２を有する。ロボット１は、このフレーム２１１の凹部２１２に、対象物２０１の突起部２０２を組み込むように対象物を移動させるように制御する。
なお、本実施形態に係るロボット１及び制御部１１の構成は、第１実施形態と同様のため、説明を省略する。

第１実施形態では、コンプライアンス・センターを、図４のように対象物１３０の中心、すなわちベクトルＰをベクトルＲの中間にした例を説明した。本実施形態では、コンプライアンス・センターを、２本のアーム１１０及び１２０のそれぞれの先端から所定の位置に設定する。本実施形態においては、所定の位置は、任意の位置に設定する。なお、コンプライアンス・センターの位置は、作業を行う前に、オペレータが当該作業において最適な場所にあらかじめ決めておく。
例えば、図１９のように、コンプライアンス・センターを対象物２０１の凸部の先端２０２に予めオペレータが設定する。図１９のように、対象物２０１の先端２０２にコンプライアンス・センターを置くと、対象物２０１の先端２０２が空間に対して仮想的に持つインピーダンス特性を設定することが可能となる。なお、インピーダンス特性とは、慣性係数、減衰係数及びバネ定数により式（１６）及び式（１７）で表される特性である。
フレーム２１１と対象物２０１が接触した際に受ける力を、両方のアーム１１０及び１２０に取り付けられている第１力センサ６１及び第２力センサ６２の値から算出し、それに応じて第１実施形態と同様に、インピーダンス特性に応じて、倣い動作をさせることで対象物２０１をフレーム２１１へと組み付けることが可能となる。

例えば、図１９のように、フレーム２１１が所定の位置に配置されている場合、両方のアーム１１０及び１２０に取り付けた第１力センサ６１及び第２力センサ６２は、対象物２０１を把持している場合の力ベクトルｆ_１と力ベクトルｆ_２とが検出する。
一方、図２０のように、フレーム２１１ａが、ｘ方向に距離ｄ１ずれている場合、対象物２０１の先端２０２の面が、フレーム２１１ａの凹部２１２の角２１３に当たる。この結果、両方のアーム１１０及び１２０に取り付けた第１力センサ６１及び第２力センサ６２には、対象物２０１の先端２０２の面が、フレーム２１１ａの凹部２１２の角２１３に当たったことによる力ベクトルｆ_１と力ベクトルｆ_２とが検出される。

第１実施形態では、制御部１１は、式（１６）と式（１７）のインピーダンス制御による運動方程式を解いてベクトルΔＰとベクトルΔＲを求めた後、式（１０）と式（１１）に結果を代入して、式（１８）と式（１９）によりベクトルＰとベクトルＲを算出した。本実施形態では、制御部１１は、式（１６）と式（１７）のインピーダンス制御による運動方程式に加え、ダンピング制御の制御則である次式（５１）と次式（５２）を用いてアーム１１０及びアーム１２０の目標値を算出する。

なお、式（５１）及び式（５２）において、上付き＾ので表しているＤ_ｐ＾は、粘性係数（ダンピング係数）である。制御部１１は、式（５１）、（５２）、（１６）及び（１７）の式を解いてベクトルΔＰとベクトルΔＲを求めた後、式（１０）と式（１１）に結果を代入して、ベクトルＰとベクトルＲを算出する。又、ベクトルＦ_ｄｅｓは目標外力、ベクトルｆ_ｄｅｓは目標内力である。

式（５１）及び式（５２）において、ベクトルＦ_ｄｅｓを０とし、ベクトルｆ_ｄｅｓを、対象物を把持するために必要な押し付け力に設定すると、対象物は把持しながらも、外力に対して位置に追従しなくなるため、ダンピング制御も加えることで、物体が障害物に衝突した際に、あらかじめ設定された軌道には追従せず、障害物に倣うような動作を行う。この結果、高いコンプライアンス性能を実現することが可能となる。

次に、制御部１１は、次式（５３）と次式（５４）に、算出したベクトルＰとベクトルＲを代入して、ベクトルｐ_１及びベクトルｐ_２を算出する。すなわち、次式（５３）が、第１実施形態における式（１８）であり、次式（５４）が、第１実施形態における式（１９）である。

なお、式（５３）と式（５４）において、ベクトルＤは、第１実施形態の式（２０）で表される。また、ベクトルＲ_１Ｔは、ベクトルＰから見たアーム１１０（マニピュレータ１）先端の予め設定された軌道である。ベクトルＲ_２Ｔは、ベクトルＰから見たアーム１２０（マニピュレータ２）先端の予め設定された軌道である。ベクトルΔＲ_１は、ベクトルＰから見たアーム１１０（マニピュレータ１）先端のコンプライアンス成分である。ベクトルΔＲ_２は、ベクトルＰから見たアーム１１０（マニピュレータ１）先端のコンプライアンス成分である。

この場合においても、予め算出した各時刻の手先の中心の目標位置及び姿勢軌道と、各時刻の相対の手先の目標位置及び姿勢軌道とを、第１力センサ６１及び第１力センサ６２により検出された力ベクトルＦと力ベクトルｆとを用いて、第１実施形態の制御と同様に、図１８のフローチャートのように制御することで、対象物１３０を把持して、所定の部品２１１に組み付けることができる。

以上のように、本実施形態においても、物体のコンプライアンス・センターに関する運動と、部品間（エンドエフェクタ間）の相対運動とで異なる運動特性を表すことが可能である。このため、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。

［第３実施形態］
第２実施形態では、ベクトルＰを空間上の任意の位置に置いて、各アームを制御する例を説明した。すなわち、ベクトルＰは、空間に固定された座標系Σで記述している。本実施形態では、例えば、対象物（部品）の先端に原点を設定し、対象物の位置及び姿勢に応じて変化する座標系Σ’で記述して、各アームの制御を行う。すなわち、本実施形態においては、コンプライアンス・センターは、対象物の位置及び姿勢に応じて変化する。
なお、本実施形態に係るロボット１及び制御部１１の構成は、第１実施形態と同様のため、説明を省略する。

図２１は、本実施形態に係る対象物（部品）を把持して、所定の部品３１１に組み付ける一例を説明する図である。図２１において、符号３０１は対象物３０１（部品）であり、符号３１１は対象物３０１（部品）を取り付けるフレーム３１１である。図２１において、対象物３０１は、突起部の先端部３０２を有する凸状の部品であり、突起部以外の両側を２本のアーム１１０及び１２０で把持されている。また、フレーム３１１は、凹状の物体であり、凹部３１２を有する。ロボット１は、このフレーム３１１の凹部３１２に、対象物３０１の突起部３０２を組み込むように対象物を移動させるように制御する。
例えば、対象物３０１の先端３０２に原点Ｏ’を設定し、ｘ’軸、ｙ’軸、ｚ’軸をそれぞれ図２１のように設定する。このように座標系Σ’（Ｏ’−ｘ’ｙ’ｚ’）を定義する。

第１実施形態では、制御部１１は、式（１６）と式（１７）を解いた後、式（１８）と式（１９）により各アームの目標位置であるベクトルＰとベクトルＲを算出した。本実施形態では、制御部１１は、座標系Σ’（Ｏ’−ｘ’ｙ’ｚ’）に基づき、ベクトルｐ_１Ｔ及びベクトルｐ_２Ｔを、次式（５５）〜式（５８）のように表し、アーム１１０及びアーム１２０の目標値を算出する。

なお、式（５５）において、ベクトルｐ_１Ｔは、アーム１１０の手先の位置及び姿勢を示すベクトルｐ_１の把持した対象物３０１の位置及び姿勢を操作するように予め設定され第１記憶部１１−３に記憶されている軌道である。また、式（５６）において、ベクトルｐ_２Ｔは、アーム１２０の手先の位置及び姿勢を示すベクトルｐ_２の把持した対象物３０１の位置及び姿勢を操作するように予め設定され第１記憶部１１−３に記憶されている軌道である。
式（５５）〜式（５８）を用いて、式（１８）と式（１９）は、次式（５９）と次式（６０）のように置き換える。

このように本実施形態では、式（５９）と式（６０）により、制御部１１は、各アーム１１０及び１２０の目標位置であるベクトルＰとベクトルＲを算出し、算出した目標位置の制御値に基づき各アームを、第１実施形態のフローチャートの図１８と同様に制御する。

以上のように、部品の形状に応じて部品の先端に物体座標系とすると、部品の先端が空間に対して仮想的に持つインピーダンス特性を、部品の位置及び姿勢に関係なく、その形状に応じて算出することが可能となる。この結果、フレームと部品が接触した際に受ける力を、両方のアームに取り付けた力センサの値から算出し、それに応じて上述のインピーダンス特性に応じて、倣い動作をさせることで部品をフレームへと組み付けることが可能となる。

なお、ベクトルＰ_Ｔ及びベクトルＲ_Ｔは、予め設定される軌道のため、ベクトルｐ_１Ｔ及びベクトルｐ_２Ｔを予め制御部１１が計算しておき、第１記憶部１１−３及び第２記憶部１１−１３に記憶させておくことが可能である。これにより、演算効率を向上することができる。
また、制御部１１は、コンプライアンス成分を、式（５７）と式（５８）を用いて算出し、式（５８）及び式（６０）を用いて各アームの目標値を算出することで、重心位置ベクトルＰ及び相対位置ベクトルＲを経由することなく各アーム１１０及び１２０の目標位置を計算することが可能になり、計算時間の短縮が可能になる効果がある。

［第４実施形態］
第１実施形態〜第３実施形態では、２本のアームにより制御する例を説明したが、本実施形態は３本のアームを用いて制御する例である。
図２２は、本実施形態に係る３本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。図２２のように、対象物４３０（物体）を、３本のアーム４０１〜４０３が、各指部４０５〜４０７を介して把持している。なお、図２３では、説明のために、対象物４３０を二次元的に三箇所、把持している図を示したが、これに限らず、３次元的に三箇所を把持するようにしてもよい。

図２３は、本実施形態に係る３本のアームによる座標系を説明する図である。図２３において、第１アーム４０１（マニピュレーター１）の手先位置及び姿勢をベクトルｘ_１（図４のベクトルｐ_１に相当）、第２アーム４０２（マニピュレーター２）の手先位置及び姿勢をベクトルｘ_２（図４のベクトルｐ_２に相当）、第３アーム４０３（マニピュレーター３）の手先位置及び姿勢をベクトルｘ_３として表している。また、図２３において、ベクトルｘ_ｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ_２ｒは、第１アーム４０１の先端位置ｘ_１と、第２アーム４０２の先端位置ｘ_２との相対位置を表している。ｘ_３ｒは、第１アーム４０１の先端位置ｘ_１と、第３アーム４０３の先端位置ｘ_３との相対位置を表している。ベクトルｘ_２ｒ、ｘ_ｒ３及びｘ_１ｐは、図４のベクトルＲに相当する。
また、図２３においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。

これらのベクトルｘ_１、ｘ_２、ｘ_３、ｘ_ｐ、ｘ_１ｐ、ｘ_２ｒ及びｘ_３ｒを用いて、ベクトルｘｐは、次式（６１）のように表される。

図２３のように、ベクトルｘ_２ｒは、次式（６２）のように表され、ベクトルｘ_３ｒは、次式（６３）のように表される。

式（６１）、式（６２）及び式（６３）を用いて、各アームの手先位置ベクトルｘ１〜ベクトルｘ３は、次式（６４）のように表される。

制御部４１１は、この式（６４）を、式（１８）及び式（１９）の代わりに用い、式（６４）の各ベクトルｘ_１、ｘ_２及びｘ_３を算出し、各アームの目標位置及び姿勢を算出することで、各アーム４０１〜４０３を制御する。

図２４は、本実施形態に係る制御部４１１のブロック図である。図２４のように、制御部４１１は、重心運動に対してはたらく力の算出部４１１−１、重心運動のコンプライアンス成分算出部４１１−２、第１記憶部４１１−３、重心運動の設定軌道算出部４１１−４、重心運動の目標位置算出部４１１−５、第１アームの目標位置算出部４１１−２０、相対運動に対してはたらく力の算出部４１１−１１、相対運動のコンプライアンス成分算出部４１１−１２、第２記憶部４１１−１３、相対運動の設定軌道算出部４１１−１４、相対運動の目標位置算出部４１１−１５、第２アームの目標位置算出部４１１−２１、及び第３アームの目標位置算出部４１１−２２を備えている。また、制御部４１１は、不図示の内部タイマーを備えている。なお、本実施形態では、第１記憶部４１１−３と第２記憶部４１１−１３のように２つ備える例を説明したが、記憶部は１つでもよい。

重心運動に対してはたらく力の算出部４１１−１には、各アーム４０１〜４０３に取り付けられている第１力センサ６１（検出部）、第２力センサ６２（検出部）及び第３力センサ４６３（検出部）から、力成分とトルク成分を含む情報が入力される。重心運動に対してはたらく力の算出部４１１−１は、第１力センサ６１、第２力センサ６２及び第３センサ４６３との出力とに基づき、対象物の重心運動に対してはたらく力を算出する。重心運動に対してはたらく力の算出部４１１−１は、算出した対象物の重心運動に対してはたらく力を重心運動のコンプライアンス成分算出部４１１−２に出力する。
なお、重心運動に対してはたらく力とは、アーム４０１の手先、アーム４０２の手先及びアーム４０３の手先の間に把持されている対象物４３０の重心位置に対して働く力である。対象物４３０の密度が一様であれば、重心位置は対象物４３０の中心の位置である。

重心運動のコンプライアンス成分算出部４１１−２は、重心運動に対してはたらく力の算出部４１１−１から出力された対象物の重心運動に対してはたらく力を示す情報に基づき、対象物の重心運動のコンプライアンス成分を算出する。重心運動のコンプライアンス成分算出部１１−２は、算出した対象物の重心運動のコンプライアンス成分を重心運動の目標位置算出部４１１−５に出力する。

第１記憶部４１１−３には、予め対象物に対する重心運動の軌道に関する情報が記憶されている。

重心運動の設定軌道算出部４１１−４は、第１記憶部４１１−３に記憶されている対象物に対する重心運動の軌道に関する情報を読み出し、読み出した情報に基づき、対象物に対する重心運動の軌道を算出する。重心運動の設定軌道算出部４１１−４は、算出した対象物の重心運動の軌道を重心運動の目標位置算出部４１１−５に出力する。

重心運動の目標位置算出部４１１−５には、重心運動のコンプライアンス成分算出部４１１−２から出力された対象物の重心運動のコンプライアンス成分を示す情報と、重心運動の設定軌道算出部４１１−４から出力された対象物の重心運動の設定軌道を示す情報とが入力される。重心運動の目標位置算出部４１１−５は、入力された対象物の重心運動のコンプライアンス成分を示す情報と、対象物の重心運動の設定軌道を示す情報とに基づき、対象物の重心運動の目標値を算出する。重心運動の目標位置算出部４１１−５は、算出した対象物の重心運動の目標値を第１アームの目標位置算出部４１１−２０、第２アームの目標位置算出部４１１−２１及び第３アームの目標位置算出部４１１−２２に出力する。

相対運動に対してはたらく力の算出部４１１−１１には、各アーム４０１〜４０３に取り付けられている第１力センサ６１、第２力センサ６２及び第３力センサ４６３から、力成分とトルク成分を含む情報が入力される。相対運動に対してはたらく力の算出部４１１−１１は、入力された第１力センサ６１、第２力センサ６２及び第３力センサ４６３との出力とに基づき、対象物の相対運動に対してはたらく力を算出する。相対運動に対してはたらく力の算出部４１１−１１は、算出した対象物の相対運動に対してはたらく力を相対運動のコンプライアンス成分算出部４１１−１２に出力する。
なお、相対運動に対してはたらく力とは、アーム４０１に対するアーム４０２、アーム４０３の相対的にはたらく力である。

相対運動のコンプライアンス成分算出部４１１−１２は、相対運動に対してはたらく力の算出部４１１−１１から出力された対象物の相対運動に対してはたらく力を示す情報に基づき、対象物の相対運動のコンプライアンス成分を算出する。相対運動のコンプライアンス成分算出部４１１−１２は、算出した対象物の相対運動のコンプライアンス成分を相対運動の目標位置算出部４１１−１５に出力する。

第２記憶部４１１−１３には、予め相対運動の軌道に関する情報が記憶されている。

相対運動の設定軌道算出部４１１−１４は、第２記憶部４１１−１３に記憶されている対象物に対する相対運動の軌道に関する情報を読み出し、読み出した情報に基づき、対象物に対する相対運動の軌道を算出する。相対運動の設定軌道算出部４１１−１４は、算出した対象物の相対運動の軌道を相対運動の目標位置算出部４１１−１５に出力する。

相対運動の目標位置算出部４１１−１５には、相対運動のコンプライアンス成分算出部４１１−１２から出力された対象物の相対運動のコンプライアンス成分を示す情報と、相対運動の設定軌道算出部４１１−１４から出力された対象物の相対運動の設定軌道を示す情報とが入力される。相対運動の目標位置算出部４１１−１５は、入力された対象物の相対運動のコンプライアンス成分を示す情報と、対象物の相対運動の設定軌道を示す情報とに基づき、対象物の相対運動の目標値を算出する。算出した対象物の相対運動の目標値を第１アームの目標位置算出部４１１−２０、第２アームの目標位置算出部４１１−２１及び第３アームの目標位置算出部４１１−２２に出力する。

第１アームの目標位置算出部４１１−２０には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第１アームの目標位置算出部４１１−２０は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第１アームの目標値を算出する。第１アームの目標位置算出部４１１−２０は、算出した第１アームの目標値を第１アーム４０１の制御信号として、第１アーム４０１に出力する。

第２アームの目標位置算出部４１１−２１には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第２アームの目標位置算出部４１１−２０は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第２アームの目標値を算出する。第２アームの目標位置算出部４１１−２０は、算出した第２アームの目標値を第２アーム４０２の制御信号として、第２アーム４０２に出力する。

第３アームの目標位置算出部４１１−２２には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第３アームの目標位置算出部４１１−２２は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第３アームの目標値を算出する。第３アームの目標位置算出部４１１−２２は、算出した第３アームの目標値を第３アーム４０３の制御信号として、第３アーム４０３に出力する。

以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本である場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。

［第５実施形態］
本実施形態は、３本のアームを制御する別の実施形態である。図２５は、本実施形態に係る３本のアームによる座標系を説明する図である。図２５のように、第１アーム４０１の手先位置及び姿勢をベクトルｘ_１（図４のベクトルｐ_１に相当）、第２アーム４０２の手先位置及び姿勢をベクトルｘ_２（図４のベクトルｐ_２に相当）、第３アーム４０３の手先位置及び姿勢をベクトルｘ_３として表している。また、図２５において、ベクトルｘ_ｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ_２ｒは、第１アーム４０１の先端位置ｘ１と、第２アーム４０２の先端位置ｘ２との相対位置を表している。ｘ_３ｒは、第２アーム４０２の先端位置ｘ_２と、第３アーム４０３の先端位置ｘ_３との相対位置を表している。ベクトルｘ_２ｒ、ｘ_ｒ３及びｘ_１ｐは、図４のベクトルＲに相当する。
また、図２５においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。

これらのベクトルｘ_１、ｘ_２、ｘ_３、ｘ_ｐ、ｘ_１ｐ、ｘ_２ｒ及びｘ_３ｒを用いて、ベクトルｘ_ｐは、第４実施形態と同様に、式（６１）のように表される。

また、図２５のように、ベクトルｘ_２ｒは、次式（６５）のように表され、ベクトルｘ_３ｒは、次式（６６）のように表される。

式（６１）、式（６５）及び式（６６）を用いて、各アームの手先位置ベクトルｘ_１〜ベクトルｘ_３は、次式（６７）のように表される。

制御部４１１は、この式（６７）を、式（１８）及び式（１９）の代わりに用い、式（６７）の各ベクトルｘ_１、ｘ_２及びｘ_３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。

なお、第４実施形態の式（６４）を用いて各アームの目標位置及び姿勢を算出するか、本実施形態の式（６７）を用いて各アームの目標位置及び姿勢を算出するかは、例えば対象物の形状などに応じてこれらを使い分けるようにしてもよい。すなわち、対象物の形状に応じて、力を制御すべき軸が変わるため、それに応じてこれらのパターンを使い分けるようにしてもよい。

以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。

［第６実施形態］
本実施形態は、３本のアームを制御する別の実施形態である。図２６は、本実施形態に係る３本のアームによる座標系を説明する図である。図２６のように、第１アーム４０１の手先位置及び姿勢をベクトルｘ_１（図４のベクトルｐ_１に相当）、第２アーム４０２の手先位置及び姿勢をベクトルｘ_２（図４のベクトルｐ_２に相当）、第３アーム４０３の手先位置及び姿勢をベクトルｘ_３として表している。また、図２６において、ベクトルｘ_ｐは、３本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ_２ｒは、第３アーム４０３の先端位置ｘ_３と、第２アーム４０２の先端位置ｘ_２との相対位置を表している。ｘ_３ｒは、第１アーム４０１の先端位置ｘ_１と、第３アーム４０３の先端位置ｘ_３との相対位置を表している。ベクトルｘ_２ｒ、ｘ_ｒ３及びｘ_１ｐは、図４のベクトルＲに相当する。
また、図２６においては、３本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。

これらのベクトルｘ_１、ｘ_２、ｘ_３、ｘ_ｐ、ｘ_１ｐ、ｘ_２ｒ及びｘ_３ｒを用いて、ベクトルｘ_ｐは、第３実施形態と同様に、式（６１）のように表される。

また、図２６のように、ベクトルｘ_２ｒは、次式（６８）のように表され、ベクトルｘ_３ｒは、次式（６９）のように表される。

式（６１）、式（６８）及び式（６９）を用いて、各アームの手先位置ベクトルｘ_１〜ベクトルｘ_３は、次式（７０）のように表される。

制御部４１１は、この式（７０）を、式（１８）及び式（１９）の代わりに用い、式（７０）の各ベクトルｘ_１、ｘ_２及びｘ_３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。

なお、第４実施形態の式（６４）を用いて各アームの目標位置及び姿勢を算出するか、第５実施形態の式（６７）を用いて各アームの目標位置及び姿勢を算出するか、あるいは、本実施形態の式（７０）を用いて各アームの目標位置及び姿勢を算出するかは、例えば対象物の状などに応じてこれらを使い分けるようにしてもよい。すなわち、対象物の形状に応じて、力を制御すべき軸が変わるため、それに応じてこれらのパターンを使い分けるようにしてもよい。

以上のように、３本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、３本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。

［第７実施形態］
本実施形態では、４本のアームを制御する実施形態について説明する。
図２７は、本実施形態に係る４本のアームによるロボット制御方法を適用するロボット１のアームの概念図である。図２７のように、対象物５３０（物体）を、４本のアーム５０１〜５０４が、各指部５０５〜５０８を介して把持している。なお、図２７では、説明のために、対象物５３０を二次元的に四箇所、把持している図を示したが、これに限らず、３次元的に四箇所を把持するようにしてもよい。

各アームの制御を行う制御部の構成は、図２４の構成において、重心運動に対してはたらく力の算出部４１１−１に、４本のアームに設けられている各々の力センサー（第１力センサ〜第４力センサ）の出力が入力される。ここでは、第３のアームの他に、第４のアームが設けられ、この第４のアームに第４力センサが設けられている。同様に、図２４の構成において、相対運動に対してはたらく力の算出部４１１−１１に、４本のアームに設けられている各々の力センサー（第１力センサ〜第４力センサ）の出力が入力される。
また、図２４の構成に加え、不図示の第４アームの目標位置算出部を備える。第４アームの目標位置算出部には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第４アームの目標位置算出部は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第４アームの目標値を算出する。第４アームの目標位置算出部は、算出した第４アームの目標値を第４アームの制御信号として、第４アームに出力する。

図２８は、本実施形態に係る４本のアームによる座標系を説明する図である。図２８のように、第１アーム５０１の手先位置及び姿勢をベクトルｘ_１（図４のベクトルｐ１に相当）、第２アーム５０２の手先位置及び姿勢をベクトルｘ_２（図４のベクトルｐ_２に相当）、第３アーム５０３の手先位置及び姿勢をベクトルｘ_３、第４アーム５０４の手先位置及び姿勢をベクトルｘ_４として表している。また、図２８において、ベクトルｘ_ｐは、４本のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ_２ｒは、第１アーム５０１の先端位置ｘ_１と、第２アーム５０２の先端位置ｘ_２との相対位置を表している。ｘ_３ｒは、第１アーム５０１の先端位置ｘ_１と、第３アーム５０３の先端位置ｘ_３との相対位置を表している。ｘ_４ｒは、第１アーム５０１の先端位置ｘ_１と、第４アーム５０４の先端位置ｘ_４との相対位置を表している。すなわち、ベクトルｘ_２ｒ〜ｘ_ｎｒ（ｎは、３以上４以下の自然数）は、第１アーム５０１の先端位置ｘ_１と、第ｎアームの先端位置ｘ_ｎとの相対位置を表している。
また、ベクトルｘ_２ｒ、ｘ_ｒ３、ｘ_４ｒ及びｘ_１ｐは、図４のベクトルＲに相当する。また、図２８においては、４本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。

これらのベクトルｘ_１、ｘ_２、ｘ_３、ｘ_ｐ、ｘ_１ｐ、ｘ_２ｒ、ｘ_３ｒ及びｘ_４ｒを用いて、ベクトルｘ_ｐは、次式（７１）のように表される。

図２８のように、ベクトルｘ_２ｒは、次式（７２）のように表され、ベクトルｘ_３ｒは、次式（７３）のように表され、ベクトルｘ_４ｒは、次式（７４）のように表される。

式（７１）〜式（７４）を用いて、各アームの手先位置ベクトルｘ_１〜ベクトルｘ_４は、次式（７５）のように表される。

制御部４１１は、この式（７５）を、式（１８）及び式（１９）の代わりに用い、式（７５）の各ベクトルｘ_１、ｘ_２及びｘ_３を算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。

なお、本実施形態では、アームが４本の場合における制御の一例を説明したが、アームが３本の場合と同様に、対象物の形状等に応じて、各ベクトルｘ_１ｐ、ｘ_２ｒ、ｘ_３ｒ及びｘ_４ｒの設定を第４実施形態〜第６実施形態のように設定してもよい。すなわち、各ベクトルｘ_１ｐ、ｘ_２ｒ、ｘ_３ｒ及びｘ_４ｒは、ベクトルｘ_２ｒ〜ｘ_ｎｒ（ｎは、３以上４以下の自然数）は、第１アームの先端位置ｘ_１と、第ｎアームの先端位置ｘ_ｎとの相対位置に限らない。例えば、ｘ_２ｒは、第２アームの先端位置ｘ_２と、第ｎアームの先端位置ｘ_ｎとの相対位置であってもよい。

以上のように、４本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、４本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。

［第８実施形態］
本実施形態では、ｎ（ｎは２以上の自然数）本のアームを制御する実施形態について説明する。
図２９は、本実施形態に係るｎ本のアームによる座標系を説明する図である。図２９のように、第１のアームの手先位置及び姿勢をベクトルｘ_１（図４のベクトルｐ_１に相当）、第２のアームの手先位置及び姿勢をベクトルｘ_２（図４のベクトルｐ_２に相当）、第３のアームの手先位置及び姿勢をベクトルｘ_３、第４のアームの手先位置及び姿勢をベクトルｘ_４、・・・第ｎのアームの手先位置及び姿勢をベクトルｘ_ｎとして表している。また、図２９において、ベクトルｘ_ｐは、複数のアームの手先の中心の位置及び姿勢（図４のベクトルＰに相当）である。さらに、ベクトルｘ_２ｒは、第１アームの先端位置ｘ_１と、第２アームの先端位置ｘ_２との相対位置を表している。ｘ_３ｒは、第１アームの先端位置ｘ_１と、第３アームの先端位置ｘ_３との相対位置を表している。ｘ_４ｒは、第１アームの先端位置ｘ_１と、第４アームの先端位置ｘ_４との相対位置を表している。ｘ_ｎｒは、第１アームの先端位置ｘ_１と、第ｎアームの先端位置ｘ_ｎとの相対位置を表している。すなわち、ベクトルｘ_２ｒ〜ｘ_ｎｒは、第１アームの先端位置ｘ_１と、第ｎアームの先端位置ｘ_ｎとの相対位置を表している。また、ベクトルｘ_２ｒ、ｘ_ｒ３、ｘ_４ｒ・・・ｘ_ｎｒ及びｘ_１ｐは、図４のベクトルＲに相当する。
また、図２９においては、ｎ本のアームの統一的な座標系（作業座標系）の原点Ｏを交わらせて表している。

これらのベクトルｘ_１、ｘ_２、ｘ_３、ｘ_ｐ、ｘ_１ｐ、ｘ_２ｒ、ｘ_３ｒ・・・ｘ_ｎｒ及びｘ_４ｒを用いて、ベクトルｘ_ｐは、次式（７６）のように表される。

図２９のように、ベクトルｘ_２ｒ、・・・、ベクトルｘ_ｎｒは、次式（７７）のように表される。

式（７６）と式（７７）を用いて、各アームの手先位置ベクトルｘ_１〜ベクトルｘ_ｎは、次式（７８）のように表される。

制御部４１１は、この式（７８）を、式（１８）及び式（１９）の代わりに用い、式（７８）の各ベクトルｘ_１〜ｘ_ｎを算出し、各アームの目標位置及び姿勢を算出することで、各アームを制御する。

各アームの制御を行う制御部４１１は、図２４の構成において、重心運動に対してはたらく力の算出部４１１−１に、ｎ本のアームに設けられている各々の力センサー（第１力センサ〜第ｎ力センサ）の出力が入力される。同様に、図２４の構成において、相対運動に対してはたらく力の算出部４１１−１１に、ｎ本のアームに設けられている各々の力センサー（第１力センサ〜第ｎ力センサ）の出力が入力される。
また、図２４の構成に加え、不図示の第４アーム〜第ｎアームの目標位置算出部を備える。第ｎアームの目標位置算出部には、重心運動の目標位置算出部４１１−５から対象物の重心運動の目標値を示す情報と、相対運動の目標位置算出部４１１−１５から対象物の相対運動の目標値を示す情報とが入力される。第ｎアームの目標位置算出部は、入力された対象物の重心運動の目標値を示す情報と、対象物の相対運動の目標値を示す情報とに基づき、第ｎアームの目標値を算出する。第ｎアームの目標位置算出部は、算出した第ｎアームの目標値を第ｎアームの制御信号として、第ｎアームに出力する。

なお、本実施形態では、アームがｎ本の場合における制御の一例を説明したが、アームが３本の場合と同様に、対象物の形状等に応じて、各ベクトルｘ_１ｐ、ｘ_２ｒ、ｘ_３ｒ及びｘ_４ｒの設定を第４実施形態〜第６実施形態のように変えてもよい。

以上のように、ｎ本のアームを備えるロボットにおいても、各アームの把持部に取り付けられている各力センサの出力に基づき、各アームの目標位置及び姿勢を算出して各アームを制御するようにした。この結果、物体を把持しながら、落下させることなくその位置及び姿勢を変更させることが可能であり、障害物と衝突した際も落とすことなく、障害物に倣うように物体を移動させることが可能となる。
また、４本のアームを備えることで、対象物（物体）を把持する際の１本辺りの負荷が分散されるため、アームが２本の場合に比べて重量物を安定して把持し、搬送及び組み付けを行うことが可能になる。

なお、本実施形態では、各アームが対象物を把持して、搬送あるいはフレームに組み付ける例を説明したが、例えば、アーム部２０−１の第５フレーム２５には、カメラを設けられるようにしてもよい。カメラは、制御部１１または４１１の制御によって、対象物またはフレームを撮像し、取り付け位置を撮影した画像に基づき認識するようにしてもよい。そして、認識した結果に基づき、対象物をフレームの取り付け位置に移動するように制御部が制御する。この場合においても、フレームの取り付け部分の位置が予め定められている位置からずれていても、対象物とフレームとが接触したりして、対象物やフレームが破損することを防ぐことができる。

なお、本実施形態では、各アームに設けられた力センサを用いて、各アームにかかる外力を検出する例を説明したが、本発明はこれに限定されない。例えば、各アームを接続する各関節（屈折部、回転部）のモータを駆動する駆動電流値から、アーム部２０−１及び２０−２に作用する外力を検出するようにしてもよい。さらに、力センサを用いず、各関節の角速度から、各アーム部２０−１及び２０−２に作用する外力を検出するようにしてもよい。

なお、本実施形態では、ロボット制御装置１をロボットに適用する例を説明したが、例えば、本実施形態のロボット制御装置１を、組み立て装置や搬送装置などに適用してもよい。また、本実施形態では、対象物を把持して搬送、または組み込む例を説明したが、例えば、物体の分解や検査など所定の動作を行うようにしてもよい。

なお、実施形態の図３および図２４の制御部の各部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。なお、ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。
また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。
また、「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ−ＲＯＭ等の可搬媒体、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ） Ｉ／Ｆ（インタフェース）を介して接続されるＵＳＢメモリ、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。さらに「コンピュータ読み取り可能な記録媒体」とは、サーバやクライアントとなるコンピュータシステム内部の揮発性メモリのように、一定時間プログラムを保持しているものも含むものとする。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。

１・・・ロボット
１０・・・固定部
１１、４１１・・・制御部
２０・・・アーム部
４０−１、４０−２・・・指部
６１・・・第１センサ（検出部）
６２・・・第２センサ（検出部）
１１０、１２０・・・アーム
１１１、１２１・・・把持部（アーム部の先端）
１３０・・・対象物
１４０・・・障害物
ベクトルＰ（Ｐ）・・・アーム１１０及びアーム１２０のアームの手先の中心の位置及び姿勢
ベクトルＲ（Ｒ）・・・アーム１１０に対するアーム１２０の手先同士の相対的な手先位置及び姿勢
ｐ_１・・・アーム１１０の手先位置及び姿勢
ｐ_２・・・アーム１２０の手先位置及び姿勢

Claims (15)

1. 対象物を運搬する複数のアーム部と、
   複数の前記アーム部にそれぞれ設けられた、前記アーム部に作用する外力を検出する検出部と、
   前記各検出部からの検出情報に基づき、複数の前記アーム部を制御する制御部と、
   を備え、
   前記制御部は、
   複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、
   前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
   ことを特徴とするロボット制御装置。
2. 複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された前記所定の点は、複数の前記アーム部のそれぞれの先端を結ぶ線分の中点である
   ことを特徴とする請求項１に記載のロボット制御装置。
3. 前記座標系の原点は、前記対象物に設定されている
   ことを特徴とする請求項２に記載のロボット制御装置。
4. 前記制御部は、
   前記Ｐの運動と前記Ｒの運動とを、複数の前記アーム部間に設定された仮想的なインピーダンスに基づいて算出し、
   前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
   ことを特徴とする請求項１から請求項３のいずれか１項に記載のロボット制御装置。
5. 前記制御部は、
   前記Ｐの運動と前記Ｒの運動とに基づき、複数の前記アーム部の各々の目標位置及び姿勢を算出し、
   前記算出した複数の前記アーム部の各々の目標位置及び姿勢を示す情報に基づき、複数の前記アーム部を制御する
   ことを特徴とする請求項１から請求項４のいずれか１項に記載のロボット制御装置。
6. 前記制御部は、
   前記Ｐの運動と前記Ｒの運動とを、前記設定された仮想的なインピーダンスと前記対象物のインピーダンスとに基づき算出する
   ことを特徴とする請求項１から請求項５のいずれか１項に記載のロボット制御装置。
7. 前記制御部は、
   複数の前記アーム部が、前記対象物に対して押し付ける力が所望の値になるように、当該対象物のインピーダンスを考慮して、前記Ｒの運動の仮想的なインピーダンスを設定する
   ことを特徴とする請求項１から請求項６のいずれか１項に記載のロボット制御装置。
8. 前記制御部は、
   前記対象物が剛体である場合、前記対象物のインピーダンスを考慮せずに、前記Ｒの運動の仮想的なインピーダンスを設定し、前記各アーム部から前記対象物への所望の押し付け力を制御する
   ことを特徴とする請求項７に記載のロボット制御装置。
9. 前記制御部は、
   前記Ｐの運動とＲの運動とに基づき、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ_１、ｐ_２、・・・ｐ_ｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御する
   ことを特徴とする請求項１から請求項８のいずれか１項に記載のロボット制御装置。
10. 前記制御部は、
    複数の前記アーム部の基準点同士の相対位置に基づいて、複数の前記アーム部の各々の手先位置及び姿勢を示すｐ_１、ｐ_２、・・・ｐ_ｎ（ｎは２以上の自然数）を算出し、複数の前記アーム部を制御する
    ことを特徴とする請求項１から請求項９のいずれか１項に記載のロボット制御装置。
11. 前記制御部は、
    複数の前記アーム部が、前記対象物を運搬する各々の予め定められた目標軌道に対する補正量を、前記Ｐの運動と前記Ｒの運動とに基づき算出し、算出した補正量に基づき複数の前記アーム部を制御する
    ことを特徴とする請求項１から請求項１０のいずれか１項に記載のロボット制御装置。
12. 前記制御部は、
    前記Ｐの運動と前記Ｒの運動の振る舞いを定義した運動方程式を、時間に関する高次の多項式を用いて解くことで前記Ｐの運動と前記Ｒの運動とを算出する
    ことを特徴とする請求項１から請求項１１のいずれか１項に記載のロボット制御装置。
13. 複数の前記アーム部は、各々６自由度を有する６軸アーム部である
    ことを特徴とする請求項１から請求項１２のいずれか１項に記載のロボット制御装置。
14. 前記請求項１から請求項１３のいずれか１項のロボット制御装置を備える
    ことを特徴とするロボット。
15. 対象物を運搬する複数のアーム部を有するロボット制御装置におけるロボット制御方法において、
    検出部が、複数の前記アーム部にそれぞれ設けられた、外力を検出する検出工程と、
    制御部が、複数の前記アーム部を制御する制御工程と、
    を有し、
    前記制御工程は、複数の前記アーム部の作業空間上に任意に設定された座標系において、複数の前記アーム部のそれぞれの先端に対する相対的な位置及び姿勢が定義された所定の点の位置及び姿勢を示すＰの運動を前記各検出部からの検出情報に基づき算出し、複数の前記アーム部の先端同士の相対の位置及び姿勢を示すＲの運動を前記各検出部からの検出情報に基づき算出し、前記算出したＰの運動とＲの運動とに基づき、複数の前記アーム部を制御する
    ことを特徴とするロボット制御方法。

Images