(57)【特許請求の範囲】
【請求項１】
マニピュレータにそのベース座標系を基準にした先端の位置および姿勢の制御系を構成し、前記マニピュレータ先端に加わる力およびモーメントを計測する計測手段を設けて、先端に力またはモーメントが加わった際の前記マニピュレータの参照位置を基準にした運動がマス、ダンパおよびバネ要素で構成される運動モデルの運動に一致するように前記計測手段から得られる外力信号と前記運動モデルを用いて前記参照位置の修正量を計算し、前記修正量を前記参照位置に加えた値を前記位置制御系の位置指令とするコンプライアンス制御系を構成したマニピュレータを２個直列に接続した冗長自由度マニピュレータのコンプライアンス制御方法において、
ベース部のマニピュレータをマクロマニピュレータ、前記マクロマニピュレータ先端に接続されたマニピュレータをマイクロマニピュレータと表わしたとき、前記マイクロマニピュレータの参照位置における仮想的な先端位置あるいは仮想的な把持物体位置の運動が前記マクロマニピュレータの前記運動モデルの運動に一致するように、前記マクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マイクロマニピュレータの実際の先端位置あるいは把持物体位置の運動が前記マイクロマニピュレータの前記運動モデルの運動に一致するように、前記マイクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マイクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マクロマニピュレータの前記運動モデルによって前記マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が前記冗長自由度マニピュレータのベース座標系を基準にした前記マイクロマニピュレータ先端あるいは前記把持物体の位置に現れないように、前記マイクロマニピューレータの位置指令に前記マクロマニピュレータの前記位置変位を相殺する補正を加え、
前記マイクロマニピュレータの計測手段から得られる外力信号を用いて、前記マクロマニピュレータの前記運動モデルから前記マクロマニピュレータの参照位置を修正して位置指令を作成すること、を特徴とする冗長自由度マニピュレータのコンプライアンス制御方法。
【請求項２】
前記マクロマニピュレータと前記マイクロマニピュレータの前記運動モデルの仮想的なマス、ダンパおよびバネのパラメータを全く同一とすることにより前記運動モデルを一つとし、前記一つの運動モデルから得られる参照位置修正量を用いて、前記マクロマニピュレータと前記マイクロマニピュレータの位置指令を作成すること、を特徴とする請求項１記載の冗長自由度マニピュレータのコンプライアンス制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、冗長自由度マニピュレータのコンプライアンス制御および軌道追従制御に関するものである。
【０００２】
【従来の技術】
従来、環境との接触に伴う冗長自由度マニピュレータのコンプライアンス制御方式として、文献「冗長マクロ・マイクロ・マニピュレータのインピーダンス制御、日本ロボット学会誌Ｖｏｌ・１２、Ｎｏ・５、ｐｐ．７６６−７２２、１９９４」に見られる永井、吉川らの方式がある。この方式は、マニピュレータ（マクロ部）の先端に小型軽量のマニピュレータ（マイクロ部）をシリアルに接続した冗長マニピュレータ（以降、マクロ・マイクロ・マニピュレータと呼ぶ）に対して、マクロ部ベースに対するマイクロ部先端の運動、マクロ部ベースに対するマクロ部先端の運動それぞれにコンプライアンスモデル（運動モデル）を設定するものである。
マイクロ部先端に加わる力信号を用いてマイクロ部先端をコンプライアンス制御し、マイクロ部からマクロ部先端に伝わる機構内部の力を用いて、マクロ部先端をコンプライアンス制御している。マクロ部、マイクロ部に対するコンプライアンスモデルの仮想剛性を等しく設定すれば、定常的にはマイクロ部が基準位置に戻るため、マイクロ部の狭い動作範囲を拡張することができる。更に、マクロ部に対するコンプライアンスモデルの共振周波数をマイクロ部のそれよりも低く設定することにより、周波数の高い運動をマイクロ部に、周波数の低い運動をマクロ部に分配し、エネルギー効率が良い。
また、冗長自由度マクロ／マイクロマニピュレータの軌道追従（位置制御）方式としては、例えば、文献「Ｏ．Ｋｈａｔｉｂ：Ｒｅｄｕｃｅｄ  Ｅｆｆｅｃｔｉｖｅ  Ｉｎｅｒｔｉａ  ｉｎ  Ｍａｃｒｏ−／ｍｉｎｉ−Ｍａｎｉｐｕｌａｔｏｒ  Ｓｙｓｔｅｍｓ、Ｉｎｔ．Ｓｙｍｐ．ｏｎ  Ｒｏｂｏｔｉｃ  Ｒｅｓｅａｒｃｈ、Ｖｏｌ．５、ｐｐ．２７９−２８４、１９９０」の方式がある。このＫｈａｔｉｂの方式は、マクロ／マイクロマニピュレータを１つの冗長自由度マニピュレータとみなし、１つのコントローラで制御している。
【０００３】
【発明が解決しようとする課題】
しかしながら、上記従来例では、先の永井、吉川らの方式は、文献「Ｉｍｐｅａｄａｎｃｅ  Ｃｏｎｔｒｏｌ：Ａｎ  Ａｐｐｒｏａｃｈ  ｔｏ  Ｍａｎｉｐｕｌａｔｉｏｎ：Ｐａｒｔ．１−３、Ｔｒａｎｓ．ｏｆ  ｔｈｅ  ＡＳＭＥ  Ｊ．ｏｆ  Ｄｙｎａｍｉｃ  Ｓｙｓｔｅｍｓ、Ｍｅａｓｕｒｅｍｅｎｔ、ａｎｄ  Ｃｏｎｔｒｏｌ、Ｖｏｌ．１０７、ｐｐ．１−２４、１９８５」のＨｏｇａｎの加速度ベースに基づく方法により、コンプライアンス制御を実現しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じるが、この永井、吉川らの方式は、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければならないという問題があった。
更に、後者のＫｈａｔｉｂらの方式は、マクロ／マイクロマニピュレータのヤコビ行列の一般化逆行列を用いて制御系を構成しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じる。このＫｈａｔｉｂの方式も、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければらないという問題があった。
そこで、本発明は、制御則の実装が大変に容易で、マイクロマニピュレータのハードウェアの変更時にはマイクロマニピュレータの制御部のみを変更すれば済むように、マイクロマニピュレータの変更が容易な、冗長自由度マニピュレータのコンプライアンス、軌道追従制御方法および装置を提供することを目的としている。
【０００４】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載の発明は、冗長自由度マニピュレータのコンプライアンス制御方法に係り、マニピュレータにそのベース座標系を基準にした先端の位置および姿勢の制御系を構成し、前記マニピュレータ先端に加わる力およびモーメントを計測する計測手段を設けて、先端に力またはモーメントが加わった際の前記マニピュレータの参照位置を基準にした運動がマス、ダンパおよびバネ要素で構成される運動モデルの運動に一致するように前記計測手段から得られる外力信号と前記運動モデルを用いて前記参照位置の修正量を計算し、前記修正量を前記参照位置に加えた値を前記位置制御系の位置指令とするコンプライアンス制御系を構成したマニピュレータを２個直列に接続した冗長自由度マニピュレータのコンプライアンス制御方法において、ベース部のマニピュレータをマクロマニピュレータ、前記マクロマニピュレータ先端に接続されたマニピュレータをマイクロマニピュレータと表わしたとき、前記マイクロマニピュレータの参照位置における仮想的な先端位置あるいは仮想的な把持物体位置の運動が前記マクロマニピュレータの前記運動モデルの運動に一致するように、前記マクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マイクロマニピュレータの実際の先端位置あるいは把持物体位置の運動が前記マイクロマニピュレータの前記運動モデルの運動に一致するように、前記マイクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マイクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マクロマニピュレータの前記運動モデルによって前記マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が前記冗長自由度マニピュレータのベース座標系を基準にした前記マイクロマニピュレータ先端あるいは前記把持物体の位置に現れないように、前記マイクロマニピューレータの位置指令に前記マクロマニピュレータの前記位置変位を相殺する補正を加え、
前記マイクロマニピュレータの計測手段から得られる外力信号を用いて、前記マクロマニピュレータの前記運動モデルから前記マクロマニピュレータの参照位置を修正して位置指令を作成することを特徴としている。
【０００５】
また、請求項２記載の発明は、請求項１記載の冗長自由度マニピュレータのコンプライアンス制御方法において、前記マクロマニピュレータと前記マイクロマニピュレータの前記運動モデルの仮想的なマス、ダンパおよびバネのパラメータを全く同一とすることにより前記運動モデルを一つとし、前記一つの運動モデルから得られる参照位置修正量を用いて、前記マクロマニピュレータと前記マイクロマニピュレータの位置指令を作成すること、を特徴としている。
【０００７】
【発明の実施の形態】
（第１の実施の形態）
以下、本発明の第１の実施の形態について図を参照して説明する。
図１〜図３は本発明の第１の実施の形態に係る図である。
図１は本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図２は図１に示す冗長自由度マニピュレータの概形図である。
図３は図２に示すマクロマニピュレータのコンプライアンス制御の概念説明図である。
図２はマクロ／マイクロマニピュレータの概形と座標系設定を示す図である。
図２において、２０１はマクロマニピュレータ、２０２はマイクロマニピュレータである。２０３、２０４はそれぞれマクロマニピュレータ先端の力（モーメントも含む）計測手段、マイクロマニピュレータの先端の力計測手段である。２０５はマイクロマニピュレータのエンドエフェクタ、２０６は把持物体である。
Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系である。すなわちそれはマイクロマニピュレータのベース座標系でもある。
Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。
Σｔｃｐはエンドエフェクタ先端の座標系、
Σｏｂｊは把持物体に固定された物体座標系である。
ここでＰを３次元ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準としたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にした把持物体位置（すなわちΣｏｂｊ原点位置）、Σｂを基準にした把持物体位置をそれぞれ、
【０００８】
【数１】
0003968536.tif 000002
また、Ｒを３×３回転行列とし、Σｅ１からΣｂ、Σｅ２からΣｅ１、ΣｏｂｊからΣｅ２、ΣｏｂｊからΣｂへの回転変換行列をそれぞれ、
【数２】
0003968536.tif 000003
他の位置ベクトルや座標系間の回転変換行列も同様の規則に基づいて表すものとする。
Σｂを基準にした把持物体位置、ΣｏｂｊからΣｂへの回転変換行列はつぎの関係式を満たす。
【数３】
0003968536.tif 000004
以降、上式のＰ２、Ｒｅ２等に付加した添え記号（〜）は参照位置であることを表すものとする。
マクロ、マイクロマニピュレータの運動モデル（インピーダンスモデル）をそれぞれつぎのように表す。
【数４】
0003968536.tif 000005
ここで、Ｍはインピーダンスモデルの３×３仮想慣性行列、Ｄは３×３仮想粘性行列、Ｋは３×３仮想剛性行列である。Ｍ、Ｄ、Ｋに関して右下の添字のｐ、ｒはそれぞれ並進運動、回転運動に関するものであることを表し、同じく右下の添字Ｍ、ｍはそれぞれマクロ、マイクロマニピュレータに関するものであることを表している。
△ｐ、△ｒはそれぞれ並進運動、回転運動に関する参照位置の修正量である。右下の添字Ｍ、ｍはＭ、Ｄ、Ｋの場合と同様な意味を表す。△ｒは姿勢変化を表す３次元ベクトルで、例えば、ＺＹＺオイラー相対角、ロール、ピッチ、ヨー角（ＺＹＸオイラー相対角）などで表現される。また、Ｆｅ、Ｍｅはそれぞれ、把持物体あるいはマイクロマニピュレータのエンドエフェクタに加わる力、モーメントである。Ｆｄ、Ｍｄはそれぞれ力、モーメントの目標値である。なお、姿勢変化の３次元ベクトル△ｒを回転変換行列で表現したものを△Ｒと表すことにする。
つぎにマクロマニピュレータのコンプライアンス制御系について説明する。
先ず、図３の説明図に示すように、マクロマニピュレータ先端から先は仮想的な固定ツールが取付けられていると考える。
図３において、３０１はマクロマニピュレータ、３０２、３０３はそれぞれ仮想的な固定ツール、仮想的な把持物体である。
Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数５】
0003968536.tif 000006
と表される仮想的な把持物体座標系、〜Σｏｂｊの原点の運動が式（５）、（６）のインピーダンス特性を持つようにマクロマニピュレータ先端を制御する。
そのためにはマクロマニピュレータへの位置指令は次式を満足する必要がある。
【数６】
0003968536.tif 000007
マクロマニピュレータの位置制御系への位置姿勢指令である。
以降、記号（∧）は位置制御系への指令値を表すものとする。
上の式（２）、（４）、（１２）より次式が成り立つ。
【数７】
0003968536.tif 000008
以上より、式（１３）、（１４）に基づいてマクロマニピュレータの参照位置を修正して、位置指令値を計算する。
つぎにマイクロマニピュレータのコンプライアンス制御系について説明する。
図２の実際の把持物体（Σｏｂｊの原点）の運動が式（７）、（８）のインピーダンス特性を持つようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【０００９】
【数８】
0003968536.tif 000009
以上より、式（１７）、（１８）に基づいてマイクロマニピュレータの参照位置を修正して、位置指令を計算する。また、式（１７）、（１８）より、マクロマニピュレータのインピーダンスモデルによる運動の影響がΣｂを基準にしてマイクロマニピュレータの把持物体に現れないように、その運動を相殺する補正をマイクロマニピュレータの参照位置に加えていることがわかる。この補正機能により、マクロマニピュレータの位置制御系の追従遅れをマイクロマニピュレータで補償して、把持物体により高速なインピーダンスモデルの運動を実現することが可能となる。
マイクロマニピュレータが何も把持していない状態ではΣｏｂｊではなく、Σｔｃｐに対して全く同様の制御を行えばよい。
ここで、把持物体に一定外力が加わった際の定常状態におけるマクロ、マイクロマニピュレータの挙動について説明する。
十分時間が経った定常状態では、
【数９】
0003968536.tif 000010
マイクロマニピュレータのインピーダンスモデルの仮想剛性Ｋpm、Ｋrmを、マクロマニピュレータのインピーダンスモデルの仮想剛性ＫpM、ＫrMに等しく設定した場合を考える。この場合、
【数１０】
0003968536.tif 000011
となる。上式に式（１３）を代入すると次式が成り立つ。
【数１１】
0003968536.tif 000012
上式に式（１４）、（２０）を代入して式（３）を用いると、次式が成り立つ。
【数１２】
0003968536.tif 000013
従って、式（２０）、（２２）より定常状態で、
【数１３】
0003968536.tif 000014
マイクロマニピュレータ先端はその参照位置に戻ることがわかる。
通常、マイクロマニピュレータの可動範囲はマクロマニピュレータに比べて非常に狭いものとなる。本実施の形態の方式では、定常状態でマイクロマニピュレータがその参照位置に戻ることが保証されるため、マイクロマニピュレータの動作範囲を拡大することができる。
つぎに、マクロ、マイクロマニピュレータ先端の力計測手段（力センサ）から得られる力モーメント信号の処理について説明する。
図２の２０３、２０４の力センサの座標系はそれぞれΣｅ１、Σｅ２に一致しているものとする。物体に加わる外力およびモーメントの作用点の位置ベクトルをｐａｃｔ、任意に設定するモーメント（インピーダンス）中心の位置ベクトルをｐｉｃと表す。作用点ｐａｃｔに加わる外力およびモーメントをそれぞれｆｅ、ｍｅとする。このときモーメント中心から見た把持物体に加わる力ＦｅおよびモーメントＭｅは次のように表される。
Ｆｅ＝ｆｅ                                （２３）
Ｍｅ＝ｍｅ＋（Ｐａｃｔ−Ｐｉｃ）×ｆｅ    （２４）
先ず、マクロマニピュレータ先端の力センサから得られる力モーメント信号の処理を考える。力センサから得られる力モーメント信号からセンサのオフセットおよびマイクロマニピュレータと把持物体の重力による影響を補償した力をｆＭ、モーメントをｍＭと表すと、次式が成り立つ。
【００１０】
【数１４】
0003968536.tif 000015
式（２６）よりＭｅを求めるためには、モーメント中心の相違を考慮してｍＭを次のように変換する。
【数１５】
0003968536.tif 000016
座標変換する。同様に、マイクロマニピュレータ先端の力センサから得られる力モーメント信号から、センサのオフセットおよび把持物体の重力による影響を補償した力、モーメントをｆｍ、ｍｍと表すと、次式が成り立つ。
【数１６】
0003968536.tif 000017
式（２９）より、Ｍｅを求めるためには、モーメント中心の相違を考慮してｍｍを次のように変換する。
【数１７】
0003968536.tif 000018
以上説明した本実施の形態の冗長自由度マクロ／マイクロマニピュレータに対するコンプライアンス制御方式の制御ブロック図を図１に示す。図１には、制御アルゴリズムの処理と情報の流れを示している。
図１において、１０１、１０２はそれぞれマクロ、マイクロマニピュレータのコンプライアンス制御系である。１０３は制御系１０１と１０２の間の情報受け渡しを行う通信手段あるいはデータバスである。１０４、１０５はそれぞれマクロ、マイクロマニピュレータの位置制御系である。１０６、１０７はそれぞれマクロ、マイクロマニピュレータの関節角計測器である。１０８、１０９はそれぞれマクロ、マイクロマニピュレータ先端の力計測器である。１１０、１１１はそれぞれマクロ、マイクロマニピュレータの順キネマティクス演算部である。
１１２、１１３はそれぞれの逆キネマティクス演算部である。１１４、１１５はそれぞれマクロ、マイクロマニピュレータ先端の力計測器から得た力モーメント信号の変換処理部である。１１６、１１７はそれぞれマクロ、マイクロマニピュレータのインピーダンスモデルの演算部である。１１８、１１９はそれぞれマクロ、マイクロマニピュレータの位置指令演算部である。１２０、１２１はそれぞれｐｏｂｊ、ｐｉｃのフレーム変換演算部である。
図１中の太い実線はマクロ、マイクロマニピュレータのコンプライアンス制御系１０１と１０２間で受け渡しを行なう情報を示している。
つぎに動作について説明する。
マクロ側１０１では、エンコーダ等によるマクロマニピュレータの関節角計測器１０６により関節角θ１を検出して、順キネマティクス演算部１１０において位置情報として並進運動Ｐ１、回転運動Ｒｅ１に変換する。一方、マクロマニピュレータ先端の力計測器１０８により力モーメントｆＭ、ｍＭを検出し、力モーメント信号の変換処理部１１４によりフレーム変換して
【数１８】
0003968536.tif 000019
として、インピーダンスモデル演算部１１６において（５）、（６）式に示したようなインピーダンスモデル演算を行い、それぞれ並進運動、回転運動に関する参照位置の修正量△ＰＭ、△ＲＭを算出する。
マクロマニピュレータの位置指令演算部１１８では、
【数１９】
0003968536.tif 000020
インピーダンスモデル演算部１１６からの参照位置修正量によって、式（１３）、（１４）のような演算を行って修正して、
【００１１】
【数２０】
0003968536.tif 000021
この指令値を逆キネマティクス演算部１１２で再び関節角θに変換して、位置制御系１０４より関節を制御する。
以上がマクロマニピュレータのコンプライアンス制御系１０１の概略シーケンスであるが、マイクロマニピュレータのコンプライアンス制御系１０２も制御ブロックは殆ど同一構成なのでシーケンスも殆ど同じであり、マイクロマニピュレータのインピーダンスモデル演算部１１７では、式（７）、（８）に示すようなＭ、Ｄ、Ｋ演算を行って、参照位置修正量△Ｒｍ、△Ｐｍを求め、マイクロマニピュレータの位置指令演算部１１９では式（１７）、（１８）に示す修正演算を行って、位置指令値を出力していることと、ｅ２をｅ１フレームに変換する変換演算部１２０、１２１が付加されている。
こうした構成の両コンプライアンス制御系１０１、１０２では、更に、両制御系１０１、１０２間の通信回線１０３により協調制御系を構成し、例えば、マクロマニピュレータ側の順キネマティクス演算部１１０からの検出位置情報、あるいは回転参照位置信号等はマイクロマニピュレータへ位置指令演算部１１９の演算情報として送信し、マイクロマニピュレータ側からはｅ２、ｅ１フレーム変換信号等をマクロ側へ送信している。こうした情報の受け渡しによって、式（１８）に示すようなマクロマニピュレータの先端が移動した位置変位の影響が、マイクロマニピュレータの先端あるいは把持物体に現れないようにする、補正機能を実現している。
このような、本実施の形態の制御方式を、冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御に適用し、マクロおよびマイクロマニピュレータのインピーダンスモデルの仮想剛性、仮想慣性などを適切に設定することにより、動作範囲の狭いマイクロマニピュレータの動作範囲を拡大したり、マクロおよびマイクロマニピュレータの外力に対する運動の周波数分離が可能となる。
更にマクロおよびマイクロマニピュレータにおける従来の位置制御ベースのコンプライアンス制御系に簡単な修正を加え、マクロおよびマイクロマニピュレータのコンプライアンス制御系間に信号の通信手段あるいはデータバスを設けただけの非常に簡単な構成であるため、両コンプライアンス制御系の分離性に優れ、制御装置への実装が容易で、マイクロマニピュレータの変更にも容易に対処できる。
【００１２】
（第２の実施の形態）
次に本発明の第２の実施の形態について図を参照して説明する。
図４は本発明の第２の実施の形態に係る多指多関節ハンドの概形図である。
図４において、４０１、４０２、４０３はそれぞれ第１、２、３の指である。４０４、４０５、４０６はそれぞれ第１、２、３の指先に加わる力の計測手段である。４０７はベース部、４０８は把持物体である。
この構成で、多指多関節ハンドの各指の制御を把持物体４０８の把持力を制御する部分と、把持物体４０８に加わる外力に対する協調制御部分に分割し、把持物体４０８に加わる外力に対する協調制御に運動モデルを設け、位置制御系をベースにしたコンプライアンス制御を構成することにより、第１の実施の形態による制御方式を容易に適用することが可能である。その際、各指先力計測手段４０４、４０５、４０６から得られる各指の力信号を、各指が発生している把持物体４０８の把持力と、把持物体４０８に加わる外力とに分解すればよい。
【００１３】
（第３の実施の形態）
次に本発明の第３の実施の形態について図を参照して説明する。
図５は本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図５に示す装置は図１に示した第１の実施の形態より、マクロマニピュレータ側の力計測器１０８と、信号変換処理部１１４を省略した構成で、マイクロマニピュレータの力計測器１０９、１１５で得られる、変換された力モーメント信号を通信手段１０３を介してマクロ側へ送信することによって、第１の実施例の場合と同じ機能の制御系を構成できる。
【００１４】
（第４の実施の形態）
次に本発明の第４の実施の形態について図を参照して説明する。
図６は本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図６の場合は第３の実施の形態の図５の構成から、更に、マクロ側のインピーダンスモデル演算部１１６も省略した例である。各インピーダンスモデル演算部１１６と１１７のパラメータを全く同一とした場合には、マイクロ側の演算による参照位置修正量△Ｐｍ、△Ｒｍをマクロ側位置指令演算部１１８へ送信して同様に制御系を構成できる。
以上、図１、図５、６に示した制御装置では、逆キネマティクスを解いて、関節空間でマクロ、マイクロマニピュレータの位置フィードバックループを構成しているが、作業座標空間で直接位置フィードックを構成してもよい。
更に、ここまではΣｂを基準に把持物体にインピーダンス特性を与える場合を示しているが、新たな座標変換手順を加えることにより全く同様の方法で任意の座標系を基準に把持物体に対してインピーダンス特性を与えることもできる。
【００１５】
（第５の実施の形態）
次に本発明の第５の実施の形態について図を参照して説明する。
図７は本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図８は図７に示す冗長自由度マニピュレータの概形図である。
図９は図８に示すマクロマニピュレータの位置制御の概念説明図である。
図８において、６０１、６０２はそれぞれマクロマニピュレータ、マイクロマニピュレータである。６０３はマイクロマニピュレータのエンドエフェクタであり、図２の把持物体を把持していない場合の構成である。Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系であり、それはマイクロマニピュレータのベース座標系でもある。Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。Σｔｃｐはエンドエフェクタ先端の座標系である。ここでＰを３次元位置ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準にしたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にしたエンドエフェクタ先端位置（すなわちΣｔｃｐ原点位置）、Σｂを基準にしたエンドエフェクタ先端位置をそれぞれ、
【数２１】
0003968536.tif 000022
またＲを３×３回転行列とし、Σｅ１からΣｂ、Σｅ２からΣｅ１、ΣｔｃｐからΣｅ２、ΣｔｃｐからΣｂへの回転変換行列をそれぞれ、
【数２２】
0003968536.tif 000023
他の位置ベクトルや座標系間の回転変換行列も同様の規則に基づいて表すものとする。Σｂを基準にしたエンドエフェクタ先端位置、ΣｔｃｐからΣｂへの回転変換行列はつぎの関係式を満たす。
【数２３】
0003968536.tif 000024
マイクロマニピュレータのエンドエフェクタの目標位置軌道を、
【数２４】
0003968536.tif 000025
と表す。
なお、以降、記号（〜）は第１の実施の形態では参照位置を表したが、本実施の形態では以降記号（〜）は目標位置軌道であることを表すものとする。
また、マイクロマニピュレータの基準位置を
【数２５】
0003968536.tif 000026
以降、記号＃は基準位置であることを表すものとする。
【００１６】
先ず、マクロマニピュレータの位置制御系について説明する。
図９のように、マクロマニピュレータ先端から先は仮想的な固定ツールが取り付けられていると考える。
図９において、７０１はマクロマニピュレータ、７０２は仮想的な固定ツールである。Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数２６】
0003968536.tif 000027
と表される仮想的なエンドエフェクタ座標系＃Σｔｃｐ原点の運動が目標位置軌道に一致するようにマクロマニピュレータ先端を制御する。そのためには、マクロマニピュレータへの位置指令は次式を満足する必要がある。
【数２７】
0003968536.tif 000028
マクロマニピュレータの位置制御系への位置指令である。
なお以降、記号（∧）は位置制御系への指令値を表すものとする。式（３６）、（３８）、（４０）より次式が成り立つ。
【数２８】
0003968536.tif 000029
また式（３５）、（３７）、（３９）、（４１）より次式が成り立つ。
【数２９】
0003968536.tif 000030
以上より、式（４１）、（４２）に基づいてマクロマニピュレータの位置指令値を計算する。
次に、マイクロマニピュレータの位置制御系について説明する。
図８の実際のエンドエフェクタ位置（Σｔｃｐの原点）が目標軌道に一致するようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【数３０】
0003968536.tif 000031
式（３４）、（３６）、（４１）、（４４）より次式が成り立つ。
【００１７】
【数３１】
0003968536.tif 000032
また、式（３３）、（３５）、（４１）、（４２）、（４３）、（４５）より次式が成り立つ。
【数３２】
0003968536.tif 000033
以上より、式（４５）、（４６）に基づいてマイクロマニピュレータの基準位置を修正して、位置指令を計算する。
また、式（４５）、（４６）よりマクロマニピュレータの追従偏差による影響がΣｂを基準にしてマイクロマニピュレータのエンドエフェクタ先端に現れないように、その運動を相殺する補正をマイクロマニピュレータの基準位置に加えていることが分かる。この補正機能により、マクロマニピュレータの位置制御系の追従遅れをマイクロマニピュレータで補償して、エンドエフェクタ先端により正確で高速な軌道追従特性を実現できる。
【００１８】
以上説明した本実施の形態の冗長自由度マクロ／マイクロマニュピレータに対する軌道追従制御方式の制御ブロック図を図７に示す。図７にはその制御アルゴリズムの処理と情報の流れを示している。
図７において、５０１、５０２はそれぞれマクロ、マイクロマニピュレータの位置制御系である。５０３は５０１、５０２間の情報の通信手段あるいはデータバスである。５０４、５０５はそれぞれマクロ、マイクロマニピュレータの関節角位置制御系である。５０６、５０７はそれぞれマクロ、マイクロマニピュレータの関節角計測手段である。５０８、５０９はそれぞれマクロ、マイクロマニピュレータの逆キネマティクス演算部である。  ５１０はマクロマニピュレータの順キネマティクス演算部である。５１１は式（３５）、（３６）の演算を行うマイクロマニピュレータのエンドエフェクタの目標位置演算部である。５１２は式（４１）、（４２）の演算を行うマクロマピュレータの位置指令演算部である。５１３は式（４５）、（４６）の演算を行うマイクロマニピュレータの位置指令演算部である。
なお、図７中の太い実線はマクロ、マイクロマニピュレータの位置制御系５０１と５０２間で受け渡しが行なわれる情報を表し、これによって式（４５）、（４６）に示した補正機能が実現される。
このような、本実施の形態によれば、従来のマニピュレータの位置制御系に簡単な修正を加えた軌道追従方式で、マクロ、マイクロマニピュレータの位置制御系間に通信手段又はデータバスを設けた簡単な構成のために、両位置制御系の分離性に優れ、制御装置への実装が容易でマイクロマニピュレータの変更にも容易に対処できる。
【００１９】
（第６の実施の形態）
次に本発明の第６の実施の形態について図を参照して説明する。
図１０は本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図１０が、図７の構成図と異なる点は基準位置データの送信方法のみで、マイクロマニピュレータの基準位置データがマクロ側に与えられ、通信手段５０３を介してマイクロマニピュレータの位置制御系５０２側へ送信している。マイクロマニピュレータの基準位置は、頻繁に値が変更されるわけては無く、通常はマクロマニピュレータの動作範囲の中心に設定された定数値と考えてよいので、図１０のような構成で、図７に示した第５の実施の形態と同一機能の制御系を構成できる。
【００２０】
【発明の効果】
以上説明したように、本発明によれば、冗長自由度マニピュレータのコンプライアンス制御において、マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が、冗長自由度マニピュレータのベース座標を基準にしてマイクロマニピュレータ先端あるいは把持物体に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御方式に比べて、制御則が非常に簡潔で制御装置の分散化が可能になり、制御則の実装が容易でマイクロマニピュレータの変更も容易になって、実用的な冗長自由度マニピュレータのコンプライアンス制御系が実現できるという効果がある。
更に、冗長自由度マニピュレータの軌道追従制御において、マクロマニピュレータ先端の位置指令値からの追従偏差の影響がベース座標系を基準にマイクロマニピュレータ先端に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピユレータの軌道追従方式に比べて、制御則の実装が容易で、マイクロマニピュレータの変更も容易になり、実用的な冗長自由度マニピュレータの軌道追従制御系が実現できるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
【図２】図１に示す冗長自由度マニピュレータの概形図。
【図３】図１に示すマクロマニピュレータのコンプライアンス制御の概念説明図。
【図４】本発明の第２の実施の形態に係る多指多関節ハンドの概形図。
【図５】本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図６】本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図７】本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【図８】図７に示す冗長自由度マニピュレータの概形図。
【図９】図７に示すマクロマニピュレータの位置制御の概念説明図。
【図１０】本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【符号の説明】
１０１  マクロマニピュレータのコンプライアンス制御系
１０２  マイクロマニピュレータのコンプライアンス制御系
１０３、５０３  通信手段
１０４  マクロマニピュレータの位置制御系
１０５  マイクロマニピュレータの位置制御系
１０６、５０６  マクロマニピュレータの関節角計測器
１０７、５０７  マイクロマニピュレータの関節角計測器
１０８  マクロマニピュレータ先端の力モーメント計測器
１０９  マイクロマニピュレータ先端の力モーメント計測器
１１０、５１０  マクロマニピュレータの順キネマティクス演算部
１１１  マイクロマニピュレータの順キネマティクス演算部
１１２、５０８  マクロマニピュレータの逆キネマティクス演算部
１１３、５０９  マイクロマニピュレータの逆キネマティクス演算部
１１４  マクロマニピュレータの力モーメントの変換部
１１５  マイクロマニピュレータの力モーメントの変換部
１１６  マクロマニピュレータのインピーダンスモデル演算部
１１７  マイクロマニピュレータのインピーダンスモデル演算部
１１８、５１２  マクロマニピュレータの位置指令演算部
１１９、５１３  マイクロマニピュレータの位置指令演算部
１２０、１２１  フレーム変換部
２０１、３０１、６０１、７０１  マクロマニピュレータ
２０２、３０２、６０２、７０２  マイクロマニピュレータ
２０３  マクロマニピュレータ先端に加わる力モーメントの計測器
２０４  マイクロマニピュレータ先端に加わる力モーメントの計測器
２０５、６０３  マイクロマニピュレータのエンドエフェクタ
２０６、４０８  把持物体
３０３  仮想的な把持物体
４０１  第１の指
４０２  第２の指
４０３  第３の指
４０４  第１の指先の力モーメント計測器
４０５  第２の指先の力モーメント計測器
４０６  第３の指先の力モーメント計測器
４０７  多指多関節ハンドのベース部
５０１  マクロ側位置制御系
５０２  マイクロ側位置制御系
５０４  マクロ側関節角位置制御系
５０５  マイクロ側関節角位置制御系
５１１  マクロ側エンドエフェクタの目標位置演算部
【図１】
0003968536.tif 000034
【図２】
0003968536.tif 000035
【図３】
0003968536.tif 000036
【図４】
0003968536.tif 000037
【図５】
0003968536.tif 000038
【図６】
0003968536.tif 000039
【図７】
0003968536.tif 000040
【図８】
0003968536.tif 000041
【図９】
0003968536.tif 000042
【図１０】
0003968536.tif 000043