JP3968536B2 - 冗長自由度マニピュレータのコンプライアンス制御方法 - Google Patents
冗長自由度マニピュレータのコンプライアンス制御方法 Download PDFInfo
- Publication number
- JP3968536B2 JP3968536B2 JP27280297A JP27280297A JP3968536B2 JP 3968536 B2 JP3968536 B2 JP 3968536B2 JP 27280297 A JP27280297 A JP 27280297A JP 27280297 A JP27280297 A JP 27280297A JP 3968536 B2 JP3968536 B2 JP 3968536B2
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- macro
- micromanipulator
- tip
- micro
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1643—Programme controls characterised by the control loop redundant control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、冗長自由度マニピュレータのコンプライアンス制御および軌道追従制御に関するものである。
【０００２】
【従来の技術】
従来、環境との接触に伴う冗長自由度マニピュレータのコンプライアンス制御方式として、文献「冗長マクロ・マイクロ・マニピュレータのインピーダンス制御、日本ロボット学会誌Ｖｏｌ・１２、Ｎｏ・５、ｐｐ．７６６−７２２、１９９４」に見られる永井、吉川らの方式がある。この方式は、マニピュレータ（マクロ部）の先端に小型軽量のマニピュレータ（マイクロ部）をシリアルに接続した冗長マニピュレータ（以降、マクロ・マイクロ・マニピュレータと呼ぶ）に対して、マクロ部ベースに対するマイクロ部先端の運動、マクロ部ベースに対するマクロ部先端の運動それぞれにコンプライアンスモデル（運動モデル）を設定するものである。
マイクロ部先端に加わる力信号を用いてマイクロ部先端をコンプライアンス制御し、マイクロ部からマクロ部先端に伝わる機構内部の力を用いて、マクロ部先端をコンプライアンス制御している。マクロ部、マイクロ部に対するコンプライアンスモデルの仮想剛性を等しく設定すれば、定常的にはマイクロ部が基準位置に戻るため、マイクロ部の狭い動作範囲を拡張することができる。更に、マクロ部に対するコンプライアンスモデルの共振周波数をマイクロ部のそれよりも低く設定することにより、周波数の高い運動をマイクロ部に、周波数の低い運動をマクロ部に分配し、エネルギー効率が良い。
また、冗長自由度マクロ／マイクロマニピュレータの軌道追従（位置制御）方式としては、例えば、文献「Ｏ．Ｋｈａｔｉｂ：Ｒｅｄｕｃｅｄ Ｅｆｆｅｃｔｉｖｅ Ｉｎｅｒｔｉａ ｉｎ Ｍａｃｒｏ−／ｍｉｎｉ−Ｍａｎｉｐｕｌａｔｏｒ Ｓｙｓｔｅｍｓ、Ｉｎｔ．Ｓｙｍｐ．ｏｎ Ｒｏｂｏｔｉｃ Ｒｅｓｅａｒｃｈ、Ｖｏｌ．５、ｐｐ．２７９−２８４、１９９０」の方式がある。このＫｈａｔｉｂの方式は、マクロ／マイクロマニピュレータを１つの冗長自由度マニピュレータとみなし、１つのコントローラで制御している。
【０００３】
【発明が解決しようとする課題】
しかしながら、上記従来例では、先の永井、吉川らの方式は、文献「Ｉｍｐｅａｄａｎｃｅ Ｃｏｎｔｒｏｌ：Ａｎ Ａｐｐｒｏａｃｈ ｔｏ Ｍａｎｉｐｕｌａｔｉｏｎ：Ｐａｒｔ．１−３、Ｔｒａｎｓ．ｏｆ ｔｈｅ ＡＳＭＥ Ｊ．ｏｆ Ｄｙｎａｍｉｃ Ｓｙｓｔｅｍｓ、Ｍｅａｓｕｒｅｍｅｎｔ、ａｎｄ Ｃｏｎｔｒｏｌ、Ｖｏｌ．１０７、ｐｐ．１−２４、１９８５」のＨｏｇａｎの加速度ベースに基づく方法により、コンプライアンス制御を実現しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じるが、この永井、吉川らの方式は、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければならないという問題があった。
更に、後者のＫｈａｔｉｂらの方式は、マクロ／マイクロマニピュレータのヤコビ行列の一般化逆行列を用いて制御系を構成しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じる。このＫｈａｔｉｂの方式も、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければらないという問題があった。
そこで、本発明は、制御則の実装が大変に容易で、マイクロマニピュレータのハードウェアの変更時にはマイクロマニピュレータの制御部のみを変更すれば済むように、マイクロマニピュレータの変更が容易な、冗長自由度マニピュレータのコンプライアンス、軌道追従制御方法および装置を提供することを目的としている。
【０００４】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載の発明は、冗長自由度マニピュレータのコンプライアンス制御方法に係り、マニピュレータにそのベース座標系を基準にした先端の位置および姿勢の制御系を構成し、前記マニピュレータ先端に加わる力およびモーメントを計測する計測手段を設けて、先端に力またはモーメントが加わった際の前記マニピュレータの参照位置を基準にした運動がマス、ダンパおよびバネ要素で構成される運動モデルの運動に一致するように前記計測手段から得られる外力信号と前記運動モデルを用いて前記参照位置の修正量を計算し、前記修正量を前記参照位置に加えた値を前記位置制御系の位置指令とするコンプライアンス制御系を構成したマニピュレータを２個直列に接続した冗長自由度マニピュレータのコンプライアンス制御方法において、ベース部のマニピュレータをマクロマニピュレータ、前記マクロマニピュレータ先端に接続されたマニピュレータをマイクロマニピュレータと表わしたとき、前記マイクロマニピュレータの参照位置における仮想的な先端位置あるいは仮想的な把持物体位置の運動が前記マクロマニピュレータの前記運動モデルの運動に一致するように、前記マクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マイクロマニピュレータの実際の先端位置あるいは把持物体位置の運動が前記マイクロマニピュレータの前記運動モデルの運動に一致するように、前記マイクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マイクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マクロマニピュレータの前記運動モデルによって前記マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が前記冗長自由度マニピュレータのベース座標系を基準にした前記マイクロマニピュレータ先端あるいは前記把持物体の位置に現れないように、前記マイクロマニピューレータの位置指令に前記マクロマニピュレータの前記位置変位を相殺する補正を加え、
前記マイクロマニピュレータの計測手段から得られる外力信号を用いて、前記マクロマニピュレータの前記運動モデルから前記マクロマニピュレータの参照位置を修正して位置指令を作成することを特徴としている。
【０００５】
また、請求項２記載の発明は、請求項１記載の冗長自由度マニピュレータのコンプライアンス制御方法において、前記マクロマニピュレータと前記マイクロマニピュレータの前記運動モデルの仮想的なマス、ダンパおよびバネのパラメータを全く同一とすることにより前記運動モデルを一つとし、前記一つの運動モデルから得られる参照位置修正量を用いて、前記マクロマニピュレータと前記マイクロマニピュレータの位置指令を作成すること、を特徴としている。
【０００７】
【発明の実施の形態】
（第１の実施の形態）
以下、本発明の第１の実施の形態について図を参照して説明する。
図１〜図３は本発明の第１の実施の形態に係る図である。
図１は本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図２は図１に示す冗長自由度マニピュレータの概形図である。
図３は図２に示すマクロマニピュレータのコンプライアンス制御の概念説明図である。
図２はマクロ／マイクロマニピュレータの概形と座標系設定を示す図である。
図２において、２０１はマクロマニピュレータ、２０２はマイクロマニピュレータである。２０３、２０４はそれぞれマクロマニピュレータ先端の力（モーメントも含む）計測手段、マイクロマニピュレータの先端の力計測手段である。２０５はマイクロマニピュレータのエンドエフェクタ、２０６は把持物体である。
Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系である。すなわちそれはマイクロマニピュレータのベース座標系でもある。
Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。
Σｔｃｐはエンドエフェクタ先端の座標系、
Σｏｂｊは把持物体に固定された物体座標系である。
ここでＰを３次元ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準としたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にした把持物体位置（すなわちΣｏｂｊ原点位置）、Σｂを基準にした把持物体位置をそれぞれ、
【０００８】
【数１】
また、Ｒを３×３回転行列とし、Σｅ１からΣｂ、Σｅ２からΣｅ１、ΣｏｂｊからΣｅ２、ΣｏｂｊからΣｂへの回転変換行列をそれぞれ、
【数２】
他の位置ベクトルや座標系間の回転変換行列も同様の規則に基づいて表すものとする。
Σｂを基準にした把持物体位置、ΣｏｂｊからΣｂへの回転変換行列はつぎの関係式を満たす。
【数３】
以降、上式のＰ２、Ｒｅ２等に付加した添え記号（〜）は参照位置であることを表すものとする。
マクロ、マイクロマニピュレータの運動モデル（インピーダンスモデル）をそれぞれつぎのように表す。
【数４】
ここで、Ｍはインピーダンスモデルの３×３仮想慣性行列、Ｄは３×３仮想粘性行列、Ｋは３×３仮想剛性行列である。Ｍ、Ｄ、Ｋに関して右下の添字のｐ、ｒはそれぞれ並進運動、回転運動に関するものであることを表し、同じく右下の添字Ｍ、ｍはそれぞれマクロ、マイクロマニピュレータに関するものであることを表している。
△ｐ、△ｒはそれぞれ並進運動、回転運動に関する参照位置の修正量である。右下の添字Ｍ、ｍはＭ、Ｄ、Ｋの場合と同様な意味を表す。△ｒは姿勢変化を表す３次元ベクトルで、例えば、ＺＹＺオイラー相対角、ロール、ピッチ、ヨー角（ＺＹＸオイラー相対角）などで表現される。また、Ｆｅ、Ｍｅはそれぞれ、把持物体あるいはマイクロマニピュレータのエンドエフェクタに加わる力、モーメントである。Ｆｄ、Ｍｄはそれぞれ力、モーメントの目標値である。なお、姿勢変化の３次元ベクトル△ｒを回転変換行列で表現したものを△Ｒと表すことにする。
つぎにマクロマニピュレータのコンプライアンス制御系について説明する。
先ず、図３の説明図に示すように、マクロマニピュレータ先端から先は仮想的な固定ツールが取付けられていると考える。
図３において、３０１はマクロマニピュレータ、３０２、３０３はそれぞれ仮想的な固定ツール、仮想的な把持物体である。
Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数５】
と表される仮想的な把持物体座標系、〜Σｏｂｊの原点の運動が式（５）、（６）のインピーダンス特性を持つようにマクロマニピュレータ先端を制御する。
そのためにはマクロマニピュレータへの位置指令は次式を満足する必要がある。
【数６】
マクロマニピュレータの位置制御系への位置姿勢指令である。
以降、記号（∧）は位置制御系への指令値を表すものとする。
上の式（２）、（４）、（１２）より次式が成り立つ。
【数７】
以上より、式（１３）、（１４）に基づいてマクロマニピュレータの参照位置を修正して、位置指令値を計算する。
つぎにマイクロマニピュレータのコンプライアンス制御系について説明する。
図２の実際の把持物体（Σｏｂｊの原点）の運動が式（７）、（８）のインピーダンス特性を持つようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【０００９】
【数８】
以上より、式（１７）、（１８）に基づいてマイクロマニピュレータの参照位置を修正して、位置指令を計算する。また、式（１７）、（１８）より、マクロマニピュレータのインピーダンスモデルによる運動の影響がΣｂを基準にしてマイクロマニピュレータの把持物体に現れないように、その運動を相殺する補正をマイクロマニピュレータの参照位置に加えていることがわかる。この補正機能により、マクロマニピュレータの位置制御系の追従遅れをマイクロマニピュレータで補償して、把持物体により高速なインピーダンスモデルの運動を実現することが可能となる。
マイクロマニピュレータが何も把持していない状態ではΣｏｂｊではなく、Σｔｃｐに対して全く同様の制御を行えばよい。
ここで、把持物体に一定外力が加わった際の定常状態におけるマクロ、マイクロマニピュレータの挙動について説明する。
十分時間が経った定常状態では、
【数９】
マイクロマニピュレータのインピーダンスモデルの仮想剛性Ｋpm、Ｋrmを、マクロマニピュレータのインピーダンスモデルの仮想剛性ＫpM、ＫrMに等しく設定した場合を考える。この場合、
【数１０】
となる。上式に式（１３）を代入すると次式が成り立つ。
【数１１】
上式に式（１４）、（２０）を代入して式（３）を用いると、次式が成り立つ。
【数１２】
従って、式（２０）、（２２）より定常状態で、
【数１３】
マイクロマニピュレータ先端はその参照位置に戻ることがわかる。
通常、マイクロマニピュレータの可動範囲はマクロマニピュレータに比べて非常に狭いものとなる。本実施の形態の方式では、定常状態でマイクロマニピュレータがその参照位置に戻ることが保証されるため、マイクロマニピュレータの動作範囲を拡大することができる。
つぎに、マクロ、マイクロマニピュレータ先端の力計測手段（力センサ）から得られる力モーメント信号の処理について説明する。
図２の２０３、２０４の力センサの座標系はそれぞれΣｅ１、Σｅ２に一致しているものとする。物体に加わる外力およびモーメントの作用点の位置ベクトルをｐａｃｔ、任意に設定するモーメント（インピーダンス）中心の位置ベクトルをｐｉｃと表す。作用点ｐａｃｔに加わる外力およびモーメントをそれぞれｆｅ、ｍｅとする。このときモーメント中心から見た把持物体に加わる力ＦｅおよびモーメントＭｅは次のように表される。
Ｆｅ＝ｆｅ （２３）
Ｍｅ＝ｍｅ＋（Ｐａｃｔ−Ｐｉｃ）×ｆｅ （２４）
先ず、マクロマニピュレータ先端の力センサから得られる力モーメント信号の処理を考える。力センサから得られる力モーメント信号からセンサのオフセットおよびマイクロマニピュレータと把持物体の重力による影響を補償した力をｆＭ、モーメントをｍＭと表すと、次式が成り立つ。
【００１０】
【数１４】
式（２６）よりＭｅを求めるためには、モーメント中心の相違を考慮してｍＭを次のように変換する。
【数１５】
座標変換する。同様に、マイクロマニピュレータ先端の力センサから得られる力モーメント信号から、センサのオフセットおよび把持物体の重力による影響を補償した力、モーメントをｆｍ、ｍｍと表すと、次式が成り立つ。
【数１６】
式（２９）より、Ｍｅを求めるためには、モーメント中心の相違を考慮してｍｍを次のように変換する。
【数１７】
以上説明した本実施の形態の冗長自由度マクロ／マイクロマニピュレータに対するコンプライアンス制御方式の制御ブロック図を図１に示す。図１には、制御アルゴリズムの処理と情報の流れを示している。
図１において、１０１、１０２はそれぞれマクロ、マイクロマニピュレータのコンプライアンス制御系である。１０３は制御系１０１と１０２の間の情報受け渡しを行う通信手段あるいはデータバスである。１０４、１０５はそれぞれマクロ、マイクロマニピュレータの位置制御系である。１０６、１０７はそれぞれマクロ、マイクロマニピュレータの関節角計測器である。１０８、１０９はそれぞれマクロ、マイクロマニピュレータ先端の力計測器である。１１０、１１１はそれぞれマクロ、マイクロマニピュレータの順キネマティクス演算部である。
１１２、１１３はそれぞれの逆キネマティクス演算部である。１１４、１１５はそれぞれマクロ、マイクロマニピュレータ先端の力計測器から得た力モーメント信号の変換処理部である。１１６、１１７はそれぞれマクロ、マイクロマニピュレータのインピーダンスモデルの演算部である。１１８、１１９はそれぞれマクロ、マイクロマニピュレータの位置指令演算部である。１２０、１２１はそれぞれｐｏｂｊ、ｐｉｃのフレーム変換演算部である。
図１中の太い実線はマクロ、マイクロマニピュレータのコンプライアンス制御系１０１と１０２間で受け渡しを行なう情報を示している。
つぎに動作について説明する。
マクロ側１０１では、エンコーダ等によるマクロマニピュレータの関節角計測器１０６により関節角θ１を検出して、順キネマティクス演算部１１０において位置情報として並進運動Ｐ１、回転運動Ｒｅ１に変換する。一方、マクロマニピュレータ先端の力計測器１０８により力モーメントｆＭ、ｍＭを検出し、力モーメント信号の変換処理部１１４によりフレーム変換して
【数１８】
として、インピーダンスモデル演算部１１６において（５）、（６）式に示したようなインピーダンスモデル演算を行い、それぞれ並進運動、回転運動に関する参照位置の修正量△ＰＭ、△ＲＭを算出する。
マクロマニピュレータの位置指令演算部１１８では、
【数１９】
インピーダンスモデル演算部１１６からの参照位置修正量によって、式（１３）、（１４）のような演算を行って修正して、
【００１１】
【数２０】
この指令値を逆キネマティクス演算部１１２で再び関節角θに変換して、位置制御系１０４より関節を制御する。
以上がマクロマニピュレータのコンプライアンス制御系１０１の概略シーケンスであるが、マイクロマニピュレータのコンプライアンス制御系１０２も制御ブロックは殆ど同一構成なのでシーケンスも殆ど同じであり、マイクロマニピュレータのインピーダンスモデル演算部１１７では、式（７）、（８）に示すようなＭ、Ｄ、Ｋ演算を行って、参照位置修正量△Ｒｍ、△Ｐｍを求め、マイクロマニピュレータの位置指令演算部１１９では式（１７）、（１８）に示す修正演算を行って、位置指令値を出力していることと、ｅ２をｅ１フレームに変換する変換演算部１２０、１２１が付加されている。
こうした構成の両コンプライアンス制御系１０１、１０２では、更に、両制御系１０１、１０２間の通信回線１０３により協調制御系を構成し、例えば、マクロマニピュレータ側の順キネマティクス演算部１１０からの検出位置情報、あるいは回転参照位置信号等はマイクロマニピュレータへ位置指令演算部１１９の演算情報として送信し、マイクロマニピュレータ側からはｅ２、ｅ１フレーム変換信号等をマクロ側へ送信している。こうした情報の受け渡しによって、式（１８）に示すようなマクロマニピュレータの先端が移動した位置変位の影響が、マイクロマニピュレータの先端あるいは把持物体に現れないようにする、補正機能を実現している。
このような、本実施の形態の制御方式を、冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御に適用し、マクロおよびマイクロマニピュレータのインピーダンスモデルの仮想剛性、仮想慣性などを適切に設定することにより、動作範囲の狭いマイクロマニピュレータの動作範囲を拡大したり、マクロおよびマイクロマニピュレータの外力に対する運動の周波数分離が可能となる。
更にマクロおよびマイクロマニピュレータにおける従来の位置制御ベースのコンプライアンス制御系に簡単な修正を加え、マクロおよびマイクロマニピュレータのコンプライアンス制御系間に信号の通信手段あるいはデータバスを設けただけの非常に簡単な構成であるため、両コンプライアンス制御系の分離性に優れ、制御装置への実装が容易で、マイクロマニピュレータの変更にも容易に対処できる。
【００１２】
（第２の実施の形態）
次に本発明の第２の実施の形態について図を参照して説明する。
図４は本発明の第２の実施の形態に係る多指多関節ハンドの概形図である。
図４において、４０１、４０２、４０３はそれぞれ第１、２、３の指である。４０４、４０５、４０６はそれぞれ第１、２、３の指先に加わる力の計測手段である。４０７はベース部、４０８は把持物体である。
この構成で、多指多関節ハンドの各指の制御を把持物体４０８の把持力を制御する部分と、把持物体４０８に加わる外力に対する協調制御部分に分割し、把持物体４０８に加わる外力に対する協調制御に運動モデルを設け、位置制御系をベースにしたコンプライアンス制御を構成することにより、第１の実施の形態による制御方式を容易に適用することが可能である。その際、各指先力計測手段４０４、４０５、４０６から得られる各指の力信号を、各指が発生している把持物体４０８の把持力と、把持物体４０８に加わる外力とに分解すればよい。
【００１３】
（第３の実施の形態）
次に本発明の第３の実施の形態について図を参照して説明する。
図５は本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図５に示す装置は図１に示した第１の実施の形態より、マクロマニピュレータ側の力計測器１０８と、信号変換処理部１１４を省略した構成で、マイクロマニピュレータの力計測器１０９、１１５で得られる、変換された力モーメント信号を通信手段１０３を介してマクロ側へ送信することによって、第１の実施例の場合と同じ機能の制御系を構成できる。
【００１４】
（第４の実施の形態）
次に本発明の第４の実施の形態について図を参照して説明する。
図６は本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図６の場合は第３の実施の形態の図５の構成から、更に、マクロ側のインピーダンスモデル演算部１１６も省略した例である。各インピーダンスモデル演算部１１６と１１７のパラメータを全く同一とした場合には、マイクロ側の演算による参照位置修正量△Ｐｍ、△Ｒｍをマクロ側位置指令演算部１１８へ送信して同様に制御系を構成できる。
以上、図１、図５、６に示した制御装置では、逆キネマティクスを解いて、関節空間でマクロ、マイクロマニピュレータの位置フィードバックループを構成しているが、作業座標空間で直接位置フィードックを構成してもよい。
更に、ここまではΣｂを基準に把持物体にインピーダンス特性を与える場合を示しているが、新たな座標変換手順を加えることにより全く同様の方法で任意の座標系を基準に把持物体に対してインピーダンス特性を与えることもできる。
【００１５】
（第５の実施の形態）
次に本発明の第５の実施の形態について図を参照して説明する。
図７は本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図８は図７に示す冗長自由度マニピュレータの概形図である。
図９は図８に示すマクロマニピュレータの位置制御の概念説明図である。
図８において、６０１、６０２はそれぞれマクロマニピュレータ、マイクロマニピュレータである。６０３はマイクロマニピュレータのエンドエフェクタであり、図２の把持物体を把持していない場合の構成である。Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系であり、それはマイクロマニピュレータのベース座標系でもある。Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。Σｔｃｐはエンドエフェクタ先端の座標系である。ここでＰを３次元位置ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準にしたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にしたエンドエフェクタ先端位置（すなわちΣｔｃｐ原点位置）、Σｂを基準にしたエンドエフェクタ先端位置をそれぞれ、
【数２１】
またＲを３×３回転行列とし、Σｅ１からΣｂ、Σｅ２からΣｅ１、ΣｔｃｐからΣｅ２、ΣｔｃｐからΣｂへの回転変換行列をそれぞれ、
【数２２】
他の位置ベクトルや座標系間の回転変換行列も同様の規則に基づいて表すものとする。Σｂを基準にしたエンドエフェクタ先端位置、ΣｔｃｐからΣｂへの回転変換行列はつぎの関係式を満たす。
【数２３】
マイクロマニピュレータのエンドエフェクタの目標位置軌道を、
【数２４】
と表す。
なお、以降、記号（〜）は第１の実施の形態では参照位置を表したが、本実施の形態では以降記号（〜）は目標位置軌道であることを表すものとする。
また、マイクロマニピュレータの基準位置を
【数２５】
以降、記号＃は基準位置であることを表すものとする。
【００１６】
先ず、マクロマニピュレータの位置制御系について説明する。
図９のように、マクロマニピュレータ先端から先は仮想的な固定ツールが取り付けられていると考える。
図９において、７０１はマクロマニピュレータ、７０２は仮想的な固定ツールである。Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数２６】
と表される仮想的なエンドエフェクタ座標系＃Σｔｃｐ原点の運動が目標位置軌道に一致するようにマクロマニピュレータ先端を制御する。そのためには、マクロマニピュレータへの位置指令は次式を満足する必要がある。
【数２７】
マクロマニピュレータの位置制御系への位置指令である。
なお以降、記号（∧）は位置制御系への指令値を表すものとする。式（３６）、（３８）、（４０）より次式が成り立つ。
【数２８】
また式（３５）、（３７）、（３９）、（４１）より次式が成り立つ。
【数２９】
以上より、式（４１）、（４２）に基づいてマクロマニピュレータの位置指令値を計算する。
次に、マイクロマニピュレータの位置制御系について説明する。
図８の実際のエンドエフェクタ位置（Σｔｃｐの原点）が目標軌道に一致するようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【数３０】
式（３４）、（３６）、（４１）、（４４）より次式が成り立つ。
【００１７】
【数３１】
また、式（３３）、（３５）、（４１）、（４２）、（４３）、（４５）より次式が成り立つ。
【数３２】
以上より、式（４５）、（４６）に基づいてマイクロマニピュレータの基準位置を修正して、位置指令を計算する。
また、式（４５）、（４６）よりマクロマニピュレータの追従偏差による影響がΣｂを基準にしてマイクロマニピュレータのエンドエフェクタ先端に現れないように、その運動を相殺する補正をマイクロマニピュレータの基準位置に加えていることが分かる。この補正機能により、マクロマニピュレータの位置制御系の追従遅れをマイクロマニピュレータで補償して、エンドエフェクタ先端により正確で高速な軌道追従特性を実現できる。
【００１８】
以上説明した本実施の形態の冗長自由度マクロ／マイクロマニュピレータに対する軌道追従制御方式の制御ブロック図を図７に示す。図７にはその制御アルゴリズムの処理と情報の流れを示している。
図７において、５０１、５０２はそれぞれマクロ、マイクロマニピュレータの位置制御系である。５０３は５０１、５０２間の情報の通信手段あるいはデータバスである。５０４、５０５はそれぞれマクロ、マイクロマニピュレータの関節角位置制御系である。５０６、５０７はそれぞれマクロ、マイクロマニピュレータの関節角計測手段である。５０８、５０９はそれぞれマクロ、マイクロマニピュレータの逆キネマティクス演算部である。 ５１０はマクロマニピュレータの順キネマティクス演算部である。５１１は式（３５）、（３６）の演算を行うマイクロマニピュレータのエンドエフェクタの目標位置演算部である。５１２は式（４１）、（４２）の演算を行うマクロマピュレータの位置指令演算部である。５１３は式（４５）、（４６）の演算を行うマイクロマニピュレータの位置指令演算部である。
なお、図７中の太い実線はマクロ、マイクロマニピュレータの位置制御系５０１と５０２間で受け渡しが行なわれる情報を表し、これによって式（４５）、（４６）に示した補正機能が実現される。
このような、本実施の形態によれば、従来のマニピュレータの位置制御系に簡単な修正を加えた軌道追従方式で、マクロ、マイクロマニピュレータの位置制御系間に通信手段又はデータバスを設けた簡単な構成のために、両位置制御系の分離性に優れ、制御装置への実装が容易でマイクロマニピュレータの変更にも容易に対処できる。
【００１９】
（第６の実施の形態）
次に本発明の第６の実施の形態について図を参照して説明する。
図１０は本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図１０が、図７の構成図と異なる点は基準位置データの送信方法のみで、マイクロマニピュレータの基準位置データがマクロ側に与えられ、通信手段５０３を介してマイクロマニピュレータの位置制御系５０２側へ送信している。マイクロマニピュレータの基準位置は、頻繁に値が変更されるわけては無く、通常はマクロマニピュレータの動作範囲の中心に設定された定数値と考えてよいので、図１０のような構成で、図７に示した第５の実施の形態と同一機能の制御系を構成できる。
【００２０】
【発明の効果】
以上説明したように、本発明によれば、冗長自由度マニピュレータのコンプライアンス制御において、マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が、冗長自由度マニピュレータのベース座標を基準にしてマイクロマニピュレータ先端あるいは把持物体に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御方式に比べて、制御則が非常に簡潔で制御装置の分散化が可能になり、制御則の実装が容易でマイクロマニピュレータの変更も容易になって、実用的な冗長自由度マニピュレータのコンプライアンス制御系が実現できるという効果がある。
更に、冗長自由度マニピュレータの軌道追従制御において、マクロマニピュレータ先端の位置指令値からの追従偏差の影響がベース座標系を基準にマイクロマニピュレータ先端に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピユレータの軌道追従方式に比べて、制御則の実装が容易で、マイクロマニピュレータの変更も容易になり、実用的な冗長自由度マニピュレータの軌道追従制御系が実現できるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
【図２】図１に示す冗長自由度マニピュレータの概形図。
【図３】図１に示すマクロマニピュレータのコンプライアンス制御の概念説明図。
【図４】本発明の第２の実施の形態に係る多指多関節ハンドの概形図。
【図５】本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図６】本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図７】本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【図８】図７に示す冗長自由度マニピュレータの概形図。
【図９】図７に示すマクロマニピュレータの位置制御の概念説明図。
【図１０】本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【符号の説明】
１０１ マクロマニピュレータのコンプライアンス制御系
１０２ マイクロマニピュレータのコンプライアンス制御系
１０３、５０３ 通信手段
１０４ マクロマニピュレータの位置制御系
１０５ マイクロマニピュレータの位置制御系
１０６、５０６ マクロマニピュレータの関節角計測器
１０７、５０７ マイクロマニピュレータの関節角計測器
１０８ マクロマニピュレータ先端の力モーメント計測器
１０９ マイクロマニピュレータ先端の力モーメント計測器
１１０、５１０ マクロマニピュレータの順キネマティクス演算部
１１１ マイクロマニピュレータの順キネマティクス演算部
１１２、５０８ マクロマニピュレータの逆キネマティクス演算部
１１３、５０９ マイクロマニピュレータの逆キネマティクス演算部
１１４ マクロマニピュレータの力モーメントの変換部
１１５ マイクロマニピュレータの力モーメントの変換部
１１６ マクロマニピュレータのインピーダンスモデル演算部
１１７ マイクロマニピュレータのインピーダンスモデル演算部
１１８、５１２ マクロマニピュレータの位置指令演算部
１１９、５１３ マイクロマニピュレータの位置指令演算部
１２０、１２１ フレーム変換部
２０１、３０１、６０１、７０１ マクロマニピュレータ
２０２、３０２、６０２、７０２ マイクロマニピュレータ
２０３ マクロマニピュレータ先端に加わる力モーメントの計測器
２０４ マイクロマニピュレータ先端に加わる力モーメントの計測器
２０５、６０３ マイクロマニピュレータのエンドエフェクタ
２０６、４０８ 把持物体
３０３ 仮想的な把持物体
４０１ 第１の指
４０２ 第２の指
４０３ 第３の指
４０４ 第１の指先の力モーメント計測器
４０５ 第２の指先の力モーメント計測器
４０６ 第３の指先の力モーメント計測器
４０７ 多指多関節ハンドのベース部
５０１ マクロ側位置制御系
５０２ マイクロ側位置制御系
５０４ マクロ側関節角位置制御系
５０５ マイクロ側関節角位置制御系
５１１ マクロ側エンドエフェクタの目標位置演算部
【発明の属する技術分野】
本発明は、冗長自由度マニピュレータのコンプライアンス制御および軌道追従制御に関するものである。
【０００２】
【従来の技術】
従来、環境との接触に伴う冗長自由度マニピュレータのコンプライアンス制御方式として、文献「冗長マクロ・マイクロ・マニピュレータのインピーダンス制御、日本ロボット学会誌Ｖｏｌ・１２、Ｎｏ・５、ｐｐ．７６６−７２２、１９９４」に見られる永井、吉川らの方式がある。この方式は、マニピュレータ（マクロ部）の先端に小型軽量のマニピュレータ（マイクロ部）をシリアルに接続した冗長マニピュレータ（以降、マクロ・マイクロ・マニピュレータと呼ぶ）に対して、マクロ部ベースに対するマイクロ部先端の運動、マクロ部ベースに対するマクロ部先端の運動それぞれにコンプライアンスモデル（運動モデル）を設定するものである。
マイクロ部先端に加わる力信号を用いてマイクロ部先端をコンプライアンス制御し、マイクロ部からマクロ部先端に伝わる機構内部の力を用いて、マクロ部先端をコンプライアンス制御している。マクロ部、マイクロ部に対するコンプライアンスモデルの仮想剛性を等しく設定すれば、定常的にはマイクロ部が基準位置に戻るため、マイクロ部の狭い動作範囲を拡張することができる。更に、マクロ部に対するコンプライアンスモデルの共振周波数をマイクロ部のそれよりも低く設定することにより、周波数の高い運動をマイクロ部に、周波数の低い運動をマクロ部に分配し、エネルギー効率が良い。
また、冗長自由度マクロ／マイクロマニピュレータの軌道追従（位置制御）方式としては、例えば、文献「Ｏ．Ｋｈａｔｉｂ：Ｒｅｄｕｃｅｄ Ｅｆｆｅｃｔｉｖｅ Ｉｎｅｒｔｉａ ｉｎ Ｍａｃｒｏ−／ｍｉｎｉ−Ｍａｎｉｐｕｌａｔｏｒ Ｓｙｓｔｅｍｓ、Ｉｎｔ．Ｓｙｍｐ．ｏｎ Ｒｏｂｏｔｉｃ Ｒｅｓｅａｒｃｈ、Ｖｏｌ．５、ｐｐ．２７９−２８４、１９９０」の方式がある。このＫｈａｔｉｂの方式は、マクロ／マイクロマニピュレータを１つの冗長自由度マニピュレータとみなし、１つのコントローラで制御している。
【０００３】
【発明が解決しようとする課題】
しかしながら、上記従来例では、先の永井、吉川らの方式は、文献「Ｉｍｐｅａｄａｎｃｅ Ｃｏｎｔｒｏｌ：Ａｎ Ａｐｐｒｏａｃｈ ｔｏ Ｍａｎｉｐｕｌａｔｉｏｎ：Ｐａｒｔ．１−３、Ｔｒａｎｓ．ｏｆ ｔｈｅ ＡＳＭＥ Ｊ．ｏｆ Ｄｙｎａｍｉｃ Ｓｙｓｔｅｍｓ、Ｍｅａｓｕｒｅｍｅｎｔ、ａｎｄ Ｃｏｎｔｒｏｌ、Ｖｏｌ．１０７、ｐｐ．１−２４、１９８５」のＨｏｇａｎの加速度ベースに基づく方法により、コンプライアンス制御を実現しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じるが、この永井、吉川らの方式は、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければならないという問題があった。
更に、後者のＫｈａｔｉｂらの方式は、マクロ／マイクロマニピュレータのヤコビ行列の一般化逆行列を用いて制御系を構成しており、制御則が複雑で演算量が多く、制御装置への実装が難しいという問題があった。
また、マイクロマニピュレータは一種のエンドエフェクタであり、作業内容に応じて取換える必要が生じる。このＫｈａｔｉｂの方式も、マクロ部とマイクロ部の制御装置の独立性を考慮していないため、作業内容に応じてマイクロ部を取換える場合に、制御装置全体に修正を加えなければらないという問題があった。
そこで、本発明は、制御則の実装が大変に容易で、マイクロマニピュレータのハードウェアの変更時にはマイクロマニピュレータの制御部のみを変更すれば済むように、マイクロマニピュレータの変更が容易な、冗長自由度マニピュレータのコンプライアンス、軌道追従制御方法および装置を提供することを目的としている。
【０００４】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載の発明は、冗長自由度マニピュレータのコンプライアンス制御方法に係り、マニピュレータにそのベース座標系を基準にした先端の位置および姿勢の制御系を構成し、前記マニピュレータ先端に加わる力およびモーメントを計測する計測手段を設けて、先端に力またはモーメントが加わった際の前記マニピュレータの参照位置を基準にした運動がマス、ダンパおよびバネ要素で構成される運動モデルの運動に一致するように前記計測手段から得られる外力信号と前記運動モデルを用いて前記参照位置の修正量を計算し、前記修正量を前記参照位置に加えた値を前記位置制御系の位置指令とするコンプライアンス制御系を構成したマニピュレータを２個直列に接続した冗長自由度マニピュレータのコンプライアンス制御方法において、ベース部のマニピュレータをマクロマニピュレータ、前記マクロマニピュレータ先端に接続されたマニピュレータをマイクロマニピュレータと表わしたとき、前記マイクロマニピュレータの参照位置における仮想的な先端位置あるいは仮想的な把持物体位置の運動が前記マクロマニピュレータの前記運動モデルの運動に一致するように、前記マクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マイクロマニピュレータの実際の先端位置あるいは把持物体位置の運動が前記マイクロマニピュレータの前記運動モデルの運動に一致するように、前記マイクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マイクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マクロマニピュレータの前記運動モデルによって前記マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が前記冗長自由度マニピュレータのベース座標系を基準にした前記マイクロマニピュレータ先端あるいは前記把持物体の位置に現れないように、前記マイクロマニピューレータの位置指令に前記マクロマニピュレータの前記位置変位を相殺する補正を加え、
前記マイクロマニピュレータの計測手段から得られる外力信号を用いて、前記マクロマニピュレータの前記運動モデルから前記マクロマニピュレータの参照位置を修正して位置指令を作成することを特徴としている。
【０００５】
また、請求項２記載の発明は、請求項１記載の冗長自由度マニピュレータのコンプライアンス制御方法において、前記マクロマニピュレータと前記マイクロマニピュレータの前記運動モデルの仮想的なマス、ダンパおよびバネのパラメータを全く同一とすることにより前記運動モデルを一つとし、前記一つの運動モデルから得られる参照位置修正量を用いて、前記マクロマニピュレータと前記マイクロマニピュレータの位置指令を作成すること、を特徴としている。
【０００７】
【発明の実施の形態】
（第１の実施の形態）
以下、本発明の第１の実施の形態について図を参照して説明する。
図１〜図３は本発明の第１の実施の形態に係る図である。
図１は本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図２は図１に示す冗長自由度マニピュレータの概形図である。
図３は図２に示すマクロマニピュレータのコンプライアンス制御の概念説明図である。
図２はマクロ／マイクロマニピュレータの概形と座標系設定を示す図である。
図２において、２０１はマクロマニピュレータ、２０２はマイクロマニピュレータである。２０３、２０４はそれぞれマクロマニピュレータ先端の力（モーメントも含む）計測手段、マイクロマニピュレータの先端の力計測手段である。２０５はマイクロマニピュレータのエンドエフェクタ、２０６は把持物体である。
Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系である。すなわちそれはマイクロマニピュレータのベース座標系でもある。
Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。
Σｔｃｐはエンドエフェクタ先端の座標系、
Σｏｂｊは把持物体に固定された物体座標系である。
ここでＰを３次元ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準としたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にした把持物体位置（すなわちΣｏｂｊ原点位置）、Σｂを基準にした把持物体位置をそれぞれ、
【０００８】
【数１】
【数２】
Σｂを基準にした把持物体位置、ΣｏｂｊからΣｂへの回転変換行列はつぎの関係式を満たす。
【数３】
マクロ、マイクロマニピュレータの運動モデル（インピーダンスモデル）をそれぞれつぎのように表す。
【数４】
△ｐ、△ｒはそれぞれ並進運動、回転運動に関する参照位置の修正量である。右下の添字Ｍ、ｍはＭ、Ｄ、Ｋの場合と同様な意味を表す。△ｒは姿勢変化を表す３次元ベクトルで、例えば、ＺＹＺオイラー相対角、ロール、ピッチ、ヨー角（ＺＹＸオイラー相対角）などで表現される。また、Ｆｅ、Ｍｅはそれぞれ、把持物体あるいはマイクロマニピュレータのエンドエフェクタに加わる力、モーメントである。Ｆｄ、Ｍｄはそれぞれ力、モーメントの目標値である。なお、姿勢変化の３次元ベクトル△ｒを回転変換行列で表現したものを△Ｒと表すことにする。
つぎにマクロマニピュレータのコンプライアンス制御系について説明する。
先ず、図３の説明図に示すように、マクロマニピュレータ先端から先は仮想的な固定ツールが取付けられていると考える。
図３において、３０１はマクロマニピュレータ、３０２、３０３はそれぞれ仮想的な固定ツール、仮想的な把持物体である。
Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数５】
そのためにはマクロマニピュレータへの位置指令は次式を満足する必要がある。
【数６】
以降、記号（∧）は位置制御系への指令値を表すものとする。
上の式（２）、（４）、（１２）より次式が成り立つ。
【数７】
つぎにマイクロマニピュレータのコンプライアンス制御系について説明する。
図２の実際の把持物体（Σｏｂｊの原点）の運動が式（７）、（８）のインピーダンス特性を持つようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【０００９】
【数８】
マイクロマニピュレータが何も把持していない状態ではΣｏｂｊではなく、Σｔｃｐに対して全く同様の制御を行えばよい。
ここで、把持物体に一定外力が加わった際の定常状態におけるマクロ、マイクロマニピュレータの挙動について説明する。
十分時間が経った定常状態では、
【数９】
【数１０】
【数１１】
【数１２】
【数１３】
通常、マイクロマニピュレータの可動範囲はマクロマニピュレータに比べて非常に狭いものとなる。本実施の形態の方式では、定常状態でマイクロマニピュレータがその参照位置に戻ることが保証されるため、マイクロマニピュレータの動作範囲を拡大することができる。
つぎに、マクロ、マイクロマニピュレータ先端の力計測手段（力センサ）から得られる力モーメント信号の処理について説明する。
図２の２０３、２０４の力センサの座標系はそれぞれΣｅ１、Σｅ２に一致しているものとする。物体に加わる外力およびモーメントの作用点の位置ベクトルをｐａｃｔ、任意に設定するモーメント（インピーダンス）中心の位置ベクトルをｐｉｃと表す。作用点ｐａｃｔに加わる外力およびモーメントをそれぞれｆｅ、ｍｅとする。このときモーメント中心から見た把持物体に加わる力ＦｅおよびモーメントＭｅは次のように表される。
Ｆｅ＝ｆｅ （２３）
Ｍｅ＝ｍｅ＋（Ｐａｃｔ−Ｐｉｃ）×ｆｅ （２４）
先ず、マクロマニピュレータ先端の力センサから得られる力モーメント信号の処理を考える。力センサから得られる力モーメント信号からセンサのオフセットおよびマイクロマニピュレータと把持物体の重力による影響を補償した力をｆＭ、モーメントをｍＭと表すと、次式が成り立つ。
【００１０】
【数１４】
【数１５】
【数１６】
【数１７】
図１において、１０１、１０２はそれぞれマクロ、マイクロマニピュレータのコンプライアンス制御系である。１０３は制御系１０１と１０２の間の情報受け渡しを行う通信手段あるいはデータバスである。１０４、１０５はそれぞれマクロ、マイクロマニピュレータの位置制御系である。１０６、１０７はそれぞれマクロ、マイクロマニピュレータの関節角計測器である。１０８、１０９はそれぞれマクロ、マイクロマニピュレータ先端の力計測器である。１１０、１１１はそれぞれマクロ、マイクロマニピュレータの順キネマティクス演算部である。
１１２、１１３はそれぞれの逆キネマティクス演算部である。１１４、１１５はそれぞれマクロ、マイクロマニピュレータ先端の力計測器から得た力モーメント信号の変換処理部である。１１６、１１７はそれぞれマクロ、マイクロマニピュレータのインピーダンスモデルの演算部である。１１８、１１９はそれぞれマクロ、マイクロマニピュレータの位置指令演算部である。１２０、１２１はそれぞれｐｏｂｊ、ｐｉｃのフレーム変換演算部である。
図１中の太い実線はマクロ、マイクロマニピュレータのコンプライアンス制御系１０１と１０２間で受け渡しを行なう情報を示している。
つぎに動作について説明する。
マクロ側１０１では、エンコーダ等によるマクロマニピュレータの関節角計測器１０６により関節角θ１を検出して、順キネマティクス演算部１１０において位置情報として並進運動Ｐ１、回転運動Ｒｅ１に変換する。一方、マクロマニピュレータ先端の力計測器１０８により力モーメントｆＭ、ｍＭを検出し、力モーメント信号の変換処理部１１４によりフレーム変換して
【数１８】
マクロマニピュレータの位置指令演算部１１８では、
【数１９】
【００１１】
【数２０】
以上がマクロマニピュレータのコンプライアンス制御系１０１の概略シーケンスであるが、マイクロマニピュレータのコンプライアンス制御系１０２も制御ブロックは殆ど同一構成なのでシーケンスも殆ど同じであり、マイクロマニピュレータのインピーダンスモデル演算部１１７では、式（７）、（８）に示すようなＭ、Ｄ、Ｋ演算を行って、参照位置修正量△Ｒｍ、△Ｐｍを求め、マイクロマニピュレータの位置指令演算部１１９では式（１７）、（１８）に示す修正演算を行って、位置指令値を出力していることと、ｅ２をｅ１フレームに変換する変換演算部１２０、１２１が付加されている。
こうした構成の両コンプライアンス制御系１０１、１０２では、更に、両制御系１０１、１０２間の通信回線１０３により協調制御系を構成し、例えば、マクロマニピュレータ側の順キネマティクス演算部１１０からの検出位置情報、あるいは回転参照位置信号等はマイクロマニピュレータへ位置指令演算部１１９の演算情報として送信し、マイクロマニピュレータ側からはｅ２、ｅ１フレーム変換信号等をマクロ側へ送信している。こうした情報の受け渡しによって、式（１８）に示すようなマクロマニピュレータの先端が移動した位置変位の影響が、マイクロマニピュレータの先端あるいは把持物体に現れないようにする、補正機能を実現している。
このような、本実施の形態の制御方式を、冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御に適用し、マクロおよびマイクロマニピュレータのインピーダンスモデルの仮想剛性、仮想慣性などを適切に設定することにより、動作範囲の狭いマイクロマニピュレータの動作範囲を拡大したり、マクロおよびマイクロマニピュレータの外力に対する運動の周波数分離が可能となる。
更にマクロおよびマイクロマニピュレータにおける従来の位置制御ベースのコンプライアンス制御系に簡単な修正を加え、マクロおよびマイクロマニピュレータのコンプライアンス制御系間に信号の通信手段あるいはデータバスを設けただけの非常に簡単な構成であるため、両コンプライアンス制御系の分離性に優れ、制御装置への実装が容易で、マイクロマニピュレータの変更にも容易に対処できる。
【００１２】
（第２の実施の形態）
次に本発明の第２の実施の形態について図を参照して説明する。
図４は本発明の第２の実施の形態に係る多指多関節ハンドの概形図である。
図４において、４０１、４０２、４０３はそれぞれ第１、２、３の指である。４０４、４０５、４０６はそれぞれ第１、２、３の指先に加わる力の計測手段である。４０７はベース部、４０８は把持物体である。
この構成で、多指多関節ハンドの各指の制御を把持物体４０８の把持力を制御する部分と、把持物体４０８に加わる外力に対する協調制御部分に分割し、把持物体４０８に加わる外力に対する協調制御に運動モデルを設け、位置制御系をベースにしたコンプライアンス制御を構成することにより、第１の実施の形態による制御方式を容易に適用することが可能である。その際、各指先力計測手段４０４、４０５、４０６から得られる各指の力信号を、各指が発生している把持物体４０８の把持力と、把持物体４０８に加わる外力とに分解すればよい。
【００１３】
（第３の実施の形態）
次に本発明の第３の実施の形態について図を参照して説明する。
図５は本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図５に示す装置は図１に示した第１の実施の形態より、マクロマニピュレータ側の力計測器１０８と、信号変換処理部１１４を省略した構成で、マイクロマニピュレータの力計測器１０９、１１５で得られる、変換された力モーメント信号を通信手段１０３を介してマクロ側へ送信することによって、第１の実施例の場合と同じ機能の制御系を構成できる。
【００１４】
（第４の実施の形態）
次に本発明の第４の実施の形態について図を参照して説明する。
図６は本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
図６の場合は第３の実施の形態の図５の構成から、更に、マクロ側のインピーダンスモデル演算部１１６も省略した例である。各インピーダンスモデル演算部１１６と１１７のパラメータを全く同一とした場合には、マイクロ側の演算による参照位置修正量△Ｐｍ、△Ｒｍをマクロ側位置指令演算部１１８へ送信して同様に制御系を構成できる。
以上、図１、図５、６に示した制御装置では、逆キネマティクスを解いて、関節空間でマクロ、マイクロマニピュレータの位置フィードバックループを構成しているが、作業座標空間で直接位置フィードックを構成してもよい。
更に、ここまではΣｂを基準に把持物体にインピーダンス特性を与える場合を示しているが、新たな座標変換手順を加えることにより全く同様の方法で任意の座標系を基準に把持物体に対してインピーダンス特性を与えることもできる。
【００１５】
（第５の実施の形態）
次に本発明の第５の実施の形態について図を参照して説明する。
図７は本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図８は図７に示す冗長自由度マニピュレータの概形図である。
図９は図８に示すマクロマニピュレータの位置制御の概念説明図である。
図８において、６０１、６０２はそれぞれマクロマニピュレータ、マイクロマニピュレータである。６０３はマイクロマニピュレータのエンドエフェクタであり、図２の把持物体を把持していない場合の構成である。Σｂはマクロマニピュレータのベース座標系である。すなわちそれはマクロ／マイクロマニピュレータ全体のベース座標系でもある。
Σｅ１はマクロマニピュレータ先端の座標系であり、それはマイクロマニピュレータのベース座標系でもある。Σｅ２はマイクロマニピュレータ先端の座標系である。マクロマニピュレータにはΣｂを基準にΣｅ１原点の位置制御系が構成されており、マイクロマニピュレータにはΣｅ１を基準にΣｅ２原点の位置制御系が構成されている。Σｔｃｐはエンドエフェクタ先端の座標系である。ここでＰを３次元位置ベクトルとし、Σｂを基準にしたマクロマニピュレータ先端位置（すなわちΣｅ１原点位置）、Σｅ１を基準にしたマイクロマニピュレータ先端位置（すなわちΣｅ２原点位置）、Σｅ２を基準にしたエンドエフェクタ先端位置（すなわちΣｔｃｐ原点位置）、Σｂを基準にしたエンドエフェクタ先端位置をそれぞれ、
【数２１】
【数２２】
【数２３】
【数２４】
なお、以降、記号（〜）は第１の実施の形態では参照位置を表したが、本実施の形態では以降記号（〜）は目標位置軌道であることを表すものとする。
また、マイクロマニピュレータの基準位置を
【数２５】
【００１６】
先ず、マクロマニピュレータの位置制御系について説明する。
図９のように、マクロマニピュレータ先端から先は仮想的な固定ツールが取り付けられていると考える。
図９において、７０１はマクロマニピュレータ、７０２は仮想的な固定ツールである。Σｅ１からの位置ベクトル、姿勢の回転行列が、
【数２６】
【数２７】
なお以降、記号（∧）は位置制御系への指令値を表すものとする。式（３６）、（３８）、（４０）より次式が成り立つ。
【数２８】
【数２９】
次に、マイクロマニピュレータの位置制御系について説明する。
図８の実際のエンドエフェクタ位置（Σｔｃｐの原点）が目標軌道に一致するようにマイクロマニピュレータ先端を制御する。そのためには、マイクロマニピュレータへの位置指令は次式を満足する必要がある。
【数３０】
【００１７】
【数３１】
【数３２】
また、式（４５）、（４６）よりマクロマニピュレータの追従偏差による影響がΣｂを基準にしてマイクロマニピュレータのエンドエフェクタ先端に現れないように、その運動を相殺する補正をマイクロマニピュレータの基準位置に加えていることが分かる。この補正機能により、マクロマニピュレータの位置制御系の追従遅れをマイクロマニピュレータで補償して、エンドエフェクタ先端により正確で高速な軌道追従特性を実現できる。
【００１８】
以上説明した本実施の形態の冗長自由度マクロ／マイクロマニュピレータに対する軌道追従制御方式の制御ブロック図を図７に示す。図７にはその制御アルゴリズムの処理と情報の流れを示している。
図７において、５０１、５０２はそれぞれマクロ、マイクロマニピュレータの位置制御系である。５０３は５０１、５０２間の情報の通信手段あるいはデータバスである。５０４、５０５はそれぞれマクロ、マイクロマニピュレータの関節角位置制御系である。５０６、５０７はそれぞれマクロ、マイクロマニピュレータの関節角計測手段である。５０８、５０９はそれぞれマクロ、マイクロマニピュレータの逆キネマティクス演算部である。 ５１０はマクロマニピュレータの順キネマティクス演算部である。５１１は式（３５）、（３６）の演算を行うマイクロマニピュレータのエンドエフェクタの目標位置演算部である。５１２は式（４１）、（４２）の演算を行うマクロマピュレータの位置指令演算部である。５１３は式（４５）、（４６）の演算を行うマイクロマニピュレータの位置指令演算部である。
なお、図７中の太い実線はマクロ、マイクロマニピュレータの位置制御系５０１と５０２間で受け渡しが行なわれる情報を表し、これによって式（４５）、（４６）に示した補正機能が実現される。
このような、本実施の形態によれば、従来のマニピュレータの位置制御系に簡単な修正を加えた軌道追従方式で、マクロ、マイクロマニピュレータの位置制御系間に通信手段又はデータバスを設けた簡単な構成のために、両位置制御系の分離性に優れ、制御装置への実装が容易でマイクロマニピュレータの変更にも容易に対処できる。
【００１９】
（第６の実施の形態）
次に本発明の第６の実施の形態について図を参照して説明する。
図１０は本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図である。
図１０が、図７の構成図と異なる点は基準位置データの送信方法のみで、マイクロマニピュレータの基準位置データがマクロ側に与えられ、通信手段５０３を介してマイクロマニピュレータの位置制御系５０２側へ送信している。マイクロマニピュレータの基準位置は、頻繁に値が変更されるわけては無く、通常はマクロマニピュレータの動作範囲の中心に設定された定数値と考えてよいので、図１０のような構成で、図７に示した第５の実施の形態と同一機能の制御系を構成できる。
【００２０】
【発明の効果】
以上説明したように、本発明によれば、冗長自由度マニピュレータのコンプライアンス制御において、マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が、冗長自由度マニピュレータのベース座標を基準にしてマイクロマニピュレータ先端あるいは把持物体に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピュレータのコンプライアンス制御方式に比べて、制御則が非常に簡潔で制御装置の分散化が可能になり、制御則の実装が容易でマイクロマニピュレータの変更も容易になって、実用的な冗長自由度マニピュレータのコンプライアンス制御系が実現できるという効果がある。
更に、冗長自由度マニピュレータの軌道追従制御において、マクロマニピュレータ先端の位置指令値からの追従偏差の影響がベース座標系を基準にマイクロマニピュレータ先端に現れないように補正しているので、従来の冗長自由度マクロ／マイクロマニピユレータの軌道追従方式に比べて、制御則の実装が容易で、マイクロマニピュレータの変更も容易になり、実用的な冗長自由度マニピュレータの軌道追従制御系が実現できるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図である。
【図２】図１に示す冗長自由度マニピュレータの概形図。
【図３】図１に示すマクロマニピュレータのコンプライアンス制御の概念説明図。
【図４】本発明の第２の実施の形態に係る多指多関節ハンドの概形図。
【図５】本発明の第３の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図６】本発明の第４の実施の形態に係る冗長自由度マニピュレータのコンプライアンス制御装置の構成図。
【図７】本発明の第５の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【図８】図７に示す冗長自由度マニピュレータの概形図。
【図９】図７に示すマクロマニピュレータの位置制御の概念説明図。
【図１０】本発明の第６の実施の形態に係る冗長自由度マニピュレータの軌道追従制御装置の構成図。
【符号の説明】
１０１ マクロマニピュレータのコンプライアンス制御系
１０２ マイクロマニピュレータのコンプライアンス制御系
１０３、５０３ 通信手段
１０４ マクロマニピュレータの位置制御系
１０５ マイクロマニピュレータの位置制御系
１０６、５０６ マクロマニピュレータの関節角計測器
１０７、５０７ マイクロマニピュレータの関節角計測器
１０８ マクロマニピュレータ先端の力モーメント計測器
１０９ マイクロマニピュレータ先端の力モーメント計測器
１１０、５１０ マクロマニピュレータの順キネマティクス演算部
１１１ マイクロマニピュレータの順キネマティクス演算部
１１２、５０８ マクロマニピュレータの逆キネマティクス演算部
１１３、５０９ マイクロマニピュレータの逆キネマティクス演算部
１１４ マクロマニピュレータの力モーメントの変換部
１１５ マイクロマニピュレータの力モーメントの変換部
１１６ マクロマニピュレータのインピーダンスモデル演算部
１１７ マイクロマニピュレータのインピーダンスモデル演算部
１１８、５１２ マクロマニピュレータの位置指令演算部
１１９、５１３ マイクロマニピュレータの位置指令演算部
１２０、１２１ フレーム変換部
２０１、３０１、６０１、７０１ マクロマニピュレータ
２０２、３０２、６０２、７０２ マイクロマニピュレータ
２０３ マクロマニピュレータ先端に加わる力モーメントの計測器
２０４ マイクロマニピュレータ先端に加わる力モーメントの計測器
２０５、６０３ マイクロマニピュレータのエンドエフェクタ
２０６、４０８ 把持物体
３０３ 仮想的な把持物体
４０１ 第１の指
４０２ 第２の指
４０３ 第３の指
４０４ 第１の指先の力モーメント計測器
４０５ 第２の指先の力モーメント計測器
４０６ 第３の指先の力モーメント計測器
４０７ 多指多関節ハンドのベース部
５０１ マクロ側位置制御系
５０２ マイクロ側位置制御系
５０４ マクロ側関節角位置制御系
５０５ マイクロ側関節角位置制御系
５１１ マクロ側エンドエフェクタの目標位置演算部
Claims (2)
- マニピュレータにそのベース座標系を基準にした先端の位置および姿勢の制御系を構成し、前記マニピュレータ先端に加わる力およびモーメントを計測する計測手段を設けて、先端に力またはモーメントが加わった際の前記マニピュレータの参照位置を基準にした運動がマス、ダンパおよびバネ要素で構成される運動モデルの運動に一致するように前記計測手段から得られる外力信号と前記運動モデルを用いて前記参照位置の修正量を計算し、前記修正量を前記参照位置に加えた値を前記位置制御系の位置指令とするコンプライアンス制御系を構成したマニピュレータを２個直列に接続した冗長自由度マニピュレータのコンプライアンス制御方法において、
ベース部のマニピュレータをマクロマニピュレータ、前記マクロマニピュレータ先端に接続されたマニピュレータをマイクロマニピュレータと表わしたとき、前記マイクロマニピュレータの参照位置における仮想的な先端位置あるいは仮想的な把持物体位置の運動が前記マクロマニピュレータの前記運動モデルの運動に一致するように、前記マクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マイクロマニピュレータの実際の先端位置あるいは把持物体位置の運動が前記マイクロマニピュレータの前記運動モデルの運動に一致するように、前記マイクロマニピュレータの計測手段から得られる外力信号と前記運動モデルを用いて前記マイクロマニピュレータの参照位置を修正してその位置指令を作成し、
前記マクロマニピュレータの前記運動モデルによって前記マクロマニピュレータ先端がその参照位置から移動した位置変位の影響が前記冗長自由度マニピュレータのベース座標系を基準にした前記マイクロマニピュレータ先端あるいは前記把持物体の位置に現れないように、前記マイクロマニピューレータの位置指令に前記マクロマニピュレータの前記位置変位を相殺する補正を加え、
前記マイクロマニピュレータの計測手段から得られる外力信号を用いて、前記マクロマニピュレータの前記運動モデルから前記マクロマニピュレータの参照位置を修正して位置指令を作成すること、を特徴とする冗長自由度マニピュレータのコンプライアンス制御方法。 - 前記マクロマニピュレータと前記マイクロマニピュレータの前記運動モデルの仮想的なマス、ダンパおよびバネのパラメータを全く同一とすることにより前記運動モデルを一つとし、前記一つの運動モデルから得られる参照位置修正量を用いて、前記マクロマニピュレータと前記マイクロマニピュレータの位置指令を作成すること、を特徴とする請求項１記載の冗長自由度マニピュレータのコンプライアンス制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27280297A JP3968536B2 (ja) | 1997-10-06 | 1997-10-06 | 冗長自由度マニピュレータのコンプライアンス制御方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27280297A JP3968536B2 (ja) | 1997-10-06 | 1997-10-06 | 冗長自由度マニピュレータのコンプライアンス制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH11104985A JPH11104985A (ja) | 1999-04-20 |
| JP3968536B2 true JP3968536B2 (ja) | 2007-08-29 |
Family
ID=17518964
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP27280297A Expired - Fee Related JP3968536B2 (ja) | 1997-10-06 | 1997-10-06 | 冗長自由度マニピュレータのコンプライアンス制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3968536B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2954986B1 (de) * | 2014-06-10 | 2020-05-06 | Siemens Aktiengesellschaft | Vorrichtung und Verfahren zum Steuern und Regeln eines Mehrkörpersystems |
-
1997
- 1997-10-06 JP JP27280297A patent/JP3968536B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH11104985A (ja) | 1999-04-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Wang et al. | Dynamic control of multisection three-dimensional continuum manipulators based on virtual discrete-jointed robot models | |
| Butterfaß et al. | DLR-Hand II: Next generation of a dextrous robot hand | |
| KR101549818B1 (ko) | 로봇 핸드 및 그 제어방법 | |
| Ren et al. | Adaptive hybrid position/force control of dual-arm cooperative manipulators with uncertain dynamics and closed-chain kinematics | |
| Wu et al. | Architecture optimization of a parallel Schönflies-motion robot for pick-and-place applications in a predefined workspace | |
| JP2010120124A (ja) | ロボットアームの教示システム及び方法 | |
| Su et al. | Robust grasping for an under-actuated anthropomorphic hand under object position uncertainty | |
| JP3369351B2 (ja) | 多関節マニピュレータの弾性設定方法および制御装置 | |
| Sadun et al. | An overview of active compliance control for a robotic hand | |
| Almusawi et al. | Online teaching of robotic arm by human–robot interaction: end effector force/torque sensing | |
| Wimbock et al. | Analysis and experimental evaluation of the intrinsically passive controller (IPC) for multifingered hands | |
| Reis et al. | Modeling and control of a multifingered robot hand for object grasping and manipulation tasks | |
| JP3968536B2 (ja) | 冗長自由度マニピュレータのコンプライアンス制御方法 | |
| Griffin | Shared control for dexterous telemanipulation with haptic feedback | |
| Pereira et al. | Control of flexible manipulators. theory and practice | |
| JP3884249B2 (ja) | 人間型ハンドロボット用教示システム | |
| Lambert et al. | A novel 5 DOF fully parallel robot combining 3T1R motion and grasping | |
| CN107414826B (zh) | 一种腱驱动机械手张力约束末端操作控制方法 | |
| Wang et al. | A discrete-jointed robot model based control strategy for spatial continuum manipulators | |
| Wang et al. | A mapping method for telemanipulation of the non-anthropomorphic robotic hands with initial experimental validation | |
| Jiang et al. | An inverse-kinematics table-based solution of a humanoid robot finger with nonlinearly coupled joints | |
| JP3817721B2 (ja) | 冗長自由度マニピュレータのコンプライアンス制御装置 | |
| Cortesao et al. | Haptic control design for robotic-assisted minimally invasive surgery | |
| JPH05337860A (ja) | ロボットハンドの教示装置およびロボットハンド | |
| Kurnicki et al. | Implementation and evaluation of a bilateral teleoperation with use of wave variables in the ReMeDi system for remote medical examination |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040910 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20060324 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20061114 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20061122 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070122 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20070214 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070305 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20070410 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070509 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070522 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |