JP6156236B2 - マスタスレーブマニピュレータの位置姿勢合わせ方法 - Google Patents

Description

本発明は、マスタスレーブマニピュレータの位置姿勢合わせ方法に関する。

従来、このような分野の技術として、特開平５−５０３８２号公報がある。この公報に記載されたマスタスレーブマニピュレータは、マスタ側マニピュレータ（以下、単にマスタという）を手動操作で粗い許容範囲に一致させたのち、スレーブ側マニピュレータ（以下、単にスレーブという）を小駆動力かつ低速で自動操作することで、マスタとスレーブの姿勢を一致させ、動作状態に遷移するものである。

以前の産業ロボット用のマニピュレータでは、主にコスト削減のために冗長自由度を持たない６自由度以下のマニピュレータが利用されていた。しかしながら近年の技術革新により、アクチュエータの小型化と低価格が進んだため、人体と同等の７自由度のマニピュレータも現実的となった。

特開平５−５０３８２号公報

スレーブは、システム起動時、動作状態に遷移する以前には駆動力が無い状態となっている事が多く、重力の影響で地面などに接触した状態にあることがほとんどである。そのため、マスタスレーブマニピュレータでは、動作状態に遷移する前にはマスタとスレーブがそれぞれ異なった位置、姿勢となっていることがほとんどであり、この状態からシステムを動作開始させようとするとスレーブが、マスタの位置、姿勢に近づこうと急激に動作する可能性がある。この状態において、特許文献１の手法による小駆動力で動作状態に遷移しようとしてもスレーブは動作を開始することができず、または動作開始までに長い時間がかかる場合がある。

また、従来の６自由度以下のマニピュレータでは、エンドエフェクタの空間上の位置と姿勢が決定すると、途中の各関節軸の角度が一意に決定するため、ユーザによる準備動作に特段の困難は発生しなかった。しかし、マスタスレーブマニピュレータが冗長度を持つ場合には、マスタを手動操作でスレーブの姿勢と一致させようとしたときに、許容範囲を粗くしたとしても、冗長性を持つ自由度を操作してマスタをスレーブの位置姿勢と一致させることは難易度が高く、熟練が必要となる。
本発明は、このような問題点を解決するためになされたものであり、マスタ側マニピュレータとスレーブ側マニピュレータの位置合わせを、容易に行うことができるマスタスレーブマニピュレータの位置姿勢合わせ方法を提供することを目的とする。

本発明にかかるマニピュレータの位置姿勢合わせ方法は、冗長自由度を有し、バイラテラルで動作するマスタスレーブマニピュレータの位置姿勢合わせ方法であって、マスタ側マニピュレータの肘位置、手首位置、手首角度の各パラメータについて、スレーブ側マニピュレータの位置、角度との偏差を算出し、前記演算部の演算結果により、前記偏差が指定範囲内になったときに、前記マスタ側マニピュレータの対応する部位が、スレーブ側マニピュレータの位置姿勢と一致するようにトルクを印加し、前記マスタ側マニピュレータの各部位の位置姿勢が、前記スレーブ側マニピュレータの位置姿勢と略一致した場合に、バイラテラル動作を開始する。
これにより、マスタ側マニピュレータの位置姿勢がスレーブ側マニピュレータの位置姿勢と一致するまで、マスタ側マニピュレータの部位ごとにトルクを発生させて位置合わせを実行する。

マスタ側とスレーブ側のマニピュレータの位置合わせを容易に実行できる。

実施の形態１にかかるマスタスレーブマニピュレータのブロック図である。 実施の形態１にかかるマスタスレーブマニピュレータの構成を示す図である。 実施の形態１にかかる４ｃｈ型のバイラテラルのマスタスレーブマニピュレータによる制御方式を示す図である。 実施の形態１にかかるマスタスレーブマニピュレータの動作フローチャートである。 実施の形態１にかかるゲインカーブの好適な設定の図である。 実施の形態２にかかるマスタスレーブマニピュレータの動作フローチャートである。 実施の形態２にかかるマスタスレーブマニピュレータの動作フローチャートである。

実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明にかかるマスタスレーブマニピュレータ１のブロック図である。また図２は、本発明にかかるマスタスレーブマニピュレータの構成を示す図である。マスタスレーブマニピュレータ１は、ユーザが保持またはユーザの腕に装着し操作を行うマスタ２と、遠隔操作によって作業を行うスレーブ３を有する。

マスタ２及びスレーブ３は、アームに７軸以上の自由度を有し、人体の自由度を模擬した、冗長自由度を持つマニピュレータである。典型的にはスレーブ３にはカメラが取り付けられており、マスタ２側に接地されたモニタにカメラからの画像を表示し、ユーザによる操作を補助する。また、マスタ２に提示される情報はモニタによる視覚提示だけでなく、音響提示装置などを用いても良い。以下では説明を簡略化するために、それぞれマスタ２、スレーブ３のそれぞれにアームが１つ設けられているものとして説明するが双腕であってもよい。

マスタ２は、本体部１１と、本体部に連結するアーム１２を備える。

本体部１１は、マスタ２のボディである。本体部１１は、演算制御部２１を有する。以下では、演算制御部２１をマスタ演算制御部２１と記載する場合がある。マスタ演算制御部２１については後に詳述する。

アーム１２は、上腕部、下腕部、手首部を有する。なお、本体部１１に最も近い方向から上腕部、下腕部、手首部とする。アーム１２は複数の軸を有し、上腕部、下腕部、手首部を回動ないし旋回可能に連結している。アーム１２は７つの各関節２２〜２８の７自由度を有する。すなわちアーム１２は、各関節として、肩ピッチ２２と、肩ロール２３と、肩ヨー２４と、肘ピッチ２５と、手首ヨー２６と、手首ピッチ２７と、手首ロール２８を有する。またアーム１２は、エンドエフェクタ操作装置２９を有する。以下では、上腕部、下腕部、手首部について、それぞれ先端に近づく方向を長軸方向とする。また長手方向と直交し、かつ互いに直交する方向を、上下軸方向、左右軸方向として説明する。

肩ピッチ２２は、上腕部をピッチ方向に動作させる軸である。肩ピッチ２２がアクチュエータ等の動力によって動作することにより、上腕部は左右軸まわりの運動を行う。肩ピッチ２２は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

肩ロール２３は、上腕部をロール方向に動作させる軸である。例えば肩ロール２３がアクチュエータ等の動力によって動作することにより、上腕部は長手方向を軸として傾動する。肩ロール２３は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

肩ヨー２４は、上腕部をヨー方向に動作させる軸である。例えば肩ヨー２４がアクチュエータ等の動力によって動作することにより、上腕部の先端部の向きを変更する。肩ヨー２４は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

肘ピッチ２５は、下腕部をピッチ方向に動作させる軸である。肘ピッチ２５がアクチュエータ等の動力によって動作することにより、下腕部は左右軸まわりの運動を行う。肘ピッチ２５は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

手首ヨー２６は、手首部をヨー方向に動作させる軸である。例えば手首ヨー２６がアクチュエータ等の動力によって動作することにより、手首部の先端部の向きを変更する。手首ヨー２６は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

手首ピッチ２７は、手首部をピッチ方向に動作させる軸である。手首ピッチ２７がアクチュエータ等の動力によって動作することにより、手首部は左右軸まわりの運動を行う。手首ピッチ２７は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

手首ロール２８は、手首部をロール方向に動作させる軸である。例えば手首ロール２８が動作することにより、手首部は長手方向を軸として傾動する。肩ロール２３は、マスタ演算制御部２１から入力した制御信号に基づいて動作を行う。

アーム１２には、全軸に角度検出器が内蔵されており、全ての軸の角度が決定すれば各部位の位置も自動的に決定される。なお、必ずしも１つの軸に１つのセンサが設けられている必要はなく、１つのセンサで複数の軸を計測しても良い。

マスタ演算制御部２１は、アーム１２の位置制御および力制御を行う。例えばマスタ演算制御部２１は、準備動作中の初期では、アーム１２の位置制御ゲインをゼロにする。またマスタ演算制御部２１は、準備動作中には、力制御ゲインについて、重力補償、摩擦補償、慣性補償などのマスタアーム操作力を低減するための補助ゲインを発生させるよう制御する。

演算制御部２１は、準備動作中において、アーム１２の肘関節の位置が目標とする位置の近傍となるようユーザの操作によって位置合わせが実行されている最中に、肘位置の偏差があらかじめ定めた範囲内であれば、後述するスレーブ３の肩ピッチ４２、肩ロール４３の角度、位置に近づけるように、肩ピッチ２２、肩ロール２３を引き寄せるゲインをセットする。次に、演算制御部２１は、アーム１２の手首関節の位置が目標とする位置の近傍となるよう位置合わせがユーザの操作によって実行されている最中に、手首位置の偏差があらかじめ定めた範囲内であれば、肩ヨー２４と肘ピッチ２５について、手首を目標位置に引き寄せるようにゲインをセットする。さらに演算制御部２１は、アーム１２の手首の角度の偏差があらかじめ定めた範囲内であれば、手首ヨー２６と手首ピッチ２７と手首ロール２８について、手首の角度を目標角度に近づけるようにゲインをセットする。

スレーブ３は、本体部３１と、本体部３１に連結するアーム３２を備える。

本体部３１は、スレーブ３のボディである。本体部１１は、演算制御部４１を有する。以下では、演算制御部４１をスレーブ演算制御部４１と記載する場合がある。

アーム３２は、上腕部、下腕部、手首部を有する。またアーム３２は、アーム１２と同様に、各関節として、肩ピッチ４２と、肩ロール４３と、肩ヨー４４と、肘ピッチ４５と、手首ヨー４６と、手首ピッチ４７と、手首ロール４８を有する。各関節４２〜４８は、スレーブ演算制御部４１から入力された制御信号に基づいて動作する。またアーム３２は、エンドエフェクタ４９を有する。

アーム３２には、全軸に角度検出器が内蔵されており、全ての軸の角度が決定すれば各部位の位置も自動的に決定される。なお、必ずしも１つの軸に１つのセンサが設けられている必要はなく、１つのセンサで複数の軸を計測しても良い。

スレーブ演算制御部４１は、通常の動作中では、マスタ２のアーム１２が動作するのに応じて、アーム３２を動作させるよう、各関節４２〜４８に制御信号を出力する。またスレーブ演算制御部４１は、準備動作の初期状態では、位置制御ゲインは現在位置を維持、力制御ゲインはゼロにする。

なお、マスタ演算制御部２１およびスレーブ演算制御部４１は、準備動作によるマスタ２とスレーブ３の位置、姿勢合わせが完了したら、マスタ２とスレーブ３の位置制御ゲインと力制御ゲインが同一になるように、動作制御を行う。これにより、マスタ２とスレーブ３の間が接続され、バイラテラルによる動作が開始する。

図３は、本実施の形態で利用される、４ｃｈ型のバイラテラルのマスタスレーブマニピュレータによる制御方式を記載した図である。４ｃｈ型による制御方式は、マスタ・スレーブ双方の位置、姿勢をそれぞれ独立にフィードバック制御することが可能であり、各関節に設けられたアクチュエータの減速機の摩擦補償や、電動機のロータコアの回転に伴う慣性をキャンセルする慣性補償、マニピュレータにかかる重力をキャンセルする重力補償などの制御アルゴリズムを加えた際の動作の滑らかさや、壁などの固い物質への衝突時のリアルな感覚の再現などに効果がある。

次に、マスタスレーブマニピュレータ１の動作について説明する。図４は、マスタスレーブマニピュレータ１の動作フローを示したフローチャートである。

最初に、準備動作を開始する。準備動作中は、マスタ２とスレーブ３の動作は切り離されている。

スレーブ３の演算制御部４１は、アーム３２の位置固定制御を開始する（ステップＳ１１）。すなわち、スレーブ３の位置制御ゲインは現在位置を維持、力制御ゲインはゼロにする。

マスタ２の演算制御部２１は、操作力補助制御を開始する（ステップＳ１２）。すなわち演算制御部２１は、アーム１２の位置制御ゲインをゼロにし、力制御ゲインについて、重力補償、摩擦補償、慣性補償などの操作力を低減するための補助ゲインを発生させるよう制御する。ここでユーザは、マスタ２のアーム１２の肘の位置を、スレーブ３のアーム３２の位置に合致させるように操作する。

演算制御部２１は、肘の位置の偏差が指定範囲内であるか否かを判定する（ステップＳ１３）。すなわち演算制御部２１は、スレーブ３が取得した肘の位置と、マスタ２が取得した肘の位置との偏差を算出する。偏差があらかじめ定めた指定範囲内であれば（ステップＳ１３でＹｅｓ）、ステップＳ１４に進む。偏差が指定範囲内でなければ（ステップＳ１３でＮｏ）、ユーザがアーム１２を動作させるとともに、ステップＳ１３を繰り返す。

ここで、肘関節の位置偏差が一定範囲内にあるか否かの判定を行うことは、肩ピッチ２２と肩ロール２３の角度の偏差が一定範囲内であるか否かの判定を行うことと同意である。演算制御部２１は、肩ピッチ２２と肩ロール２３の判定については、それぞれの偏差の自乗和が特定の範囲以下になっていることを判定する。これにより肘位置の引き寄せが始まる範囲が矩形ではなく円形になるため、ユーザはなめらかに操作することができる。

演算制御部２１は、スレーブ３が取得した肘の位置に合致させるように、肩ピッチ２２と肩ロール２３に与えるトルクのゲイン設定を行う（ステップＳ１４）。これにより、肩ピッチ２２と肩ロール２３は引き寄せ動作を行う。

演算制御部２１は、手首部の位置の偏差が指定範囲内であるか否かを判定する（ステップＳ１５）。すなわち演算制御部２１は、スレーブ３が取得した手首の位置と、マスタ２が取得した手首の位置との偏差を算出する。偏差があらかじめ定めた指定範囲内であれば（ステップＳ１５でＹｅｓ）、ステップＳ１６に進む。偏差が指定範囲内でなければ（ステップＳ１５でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

演算制御部２１は、スレーブ３が取得した手首の位置に合致させるように、肩ヨー２４と肘ピッチ２５に与えるトルクのゲイン設定を行う（ステップＳ１６）。これにより、肩ヨー２４と肘ピッチ２５は引き寄せ動作を行う。

演算制御部２１は、手首部の角度の偏差が指定範囲内であるか否かを判定する（ステップＳ１７）。すなわち演算制御部２１は、スレーブ３が取得した手首の向きと、マスタ２が取得した手首の向きとの偏差を算出する。偏差があらかじめ定めた指定範囲内であれば（ステップＳ１７でＹｅｓ）、ステップＳ１８に進む。偏差が指定範囲内でなければ（ステップＳ１７でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

演算制御部２１は、スレーブ３が取得した手首の向きに合致させるように、手首ヨー２６と手首ピッチ２７と手首ロール２８に与えるトルクのゲイン設定を行う（ステップＳ１８）。これにより、手首ヨー２６と手首ピッチ２７と手首ロール２８は動作を行う。

演算制御部２１は、全ての角度の偏差が指定範囲内であるか否かを判定する（ステップＳ１９）。ここで角度の偏差とは、各関節に使用されているアクチュエータやモータの角度である。全ての角度の偏差が指定範囲内であれば、肘や手首の位置、角度が、目標位置や角度に合致している状態である。全ての角度の偏差があらかじめ定めた指定範囲内であれば（ステップＳ１９でＹｅｓ）、準備動作を終了し、マスタ２とスレーブ３の位置制御ゲイン、力制御ゲインを一致させることにより、マスタ２とスレーブ３を接続し、バイラテラル動作を開始する。全ての角度の偏差があらかじめ定めた指定範囲内でなければ（ステップＳ１９でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

ここで、ステップＳ１３およびステップＳ１４で示した肘位置の偏差と、肩ピッチ２２と肩ロール２３の引き寄せゲインの関係は、図５の点線に示すようなゲインカーブを設定する。これは、図５の実線で示すように、偏差に比例したトルクを発生させる、標準的な位置制御Ｐゲイン（比例ゲイン）を設定すると、偏差が指定範囲に入った場合に急激にトルクが立ち上がる上に、目標位置付近において引き寄せトルクが小さくなってしまうために、位置合わせに遊びが有るような感覚となるためである。点線で示したようなゲインカーブとすることにより、偏差が指定範囲に入った場合にトルクが急速に立ち上がることもなく、目標位置に近づいた場合であっても、引き寄せトルクが小さくならず、遊びが有るような感覚となるのを防止することができる。また点線で示すように、目標位置付近でのトルクを大きくし過ぎると発振が起こる場合がある。この場合には、引き寄せトルクを抑えるか、位置制御ゲインにＩゲイン（積分ゲイン）や、Ｄゲイン（微分ゲイン）を加えることにより調整することで、発振を抑えることができる。

なお、ステップＳ１５およびステップＳ１６における肩ピッチ２２と肩ロール２３や、ステップＳ１７およびステップＳ１８における手首ヨー２６と手首ピッチ２７と手首ロール２８の引き寄せゲインについても同様に、図５の点線に示すようなゲインカーブの設定を行う。

これにより、７軸以上である冗長自由度を有するマスタスレーブマニピュレータにおいて、マスタ側とスレーブ側のマニピュレータの位置合わせを、スレーブ側のマニピュレータを動作させること無く、マスタ側の簡単な動作で安全に実行することができる。すなわち、マスタ側マニピュレータの位置姿勢がスレーブ側マニピュレータの位置姿勢と一致するまで、マスタ側マニピュレータの部位ごとに引き寄せトルクを発生させることにより、位置合わせを実行することができる。

実施の形態２．
次に、マスタ側とスレーブ側のマニピュレータの位置合わせを行う他の動作フローについて説明する。なお、実施の形態２にかかるマスタスレーブマニピュレータ１の構成物品については、実施の形態１に示した構成物品と同一であるため、説明を省略する。

図６及び図７は、マスタスレーブマニピュレータ１の動作フローを示したフローチャートである。

最初に、準備動作を開始する。準備動作中は、マスタ２とスレーブ３の動作は切り離されている。

スレーブ３の演算制御部４１は、アーム３２の位置固定制御を開始する（ステップＳ２１）。すなわち、スレーブ３の位置制御ゲインは現在位置を維持、力制御ゲインはゼロにする。

マスタ２の演算制御部２１は、操作力補助制御を開始する（ステップＳ２２）。すなわち演算制御部２１は、アーム１２の位置制御ゲインをゼロにし、力制御ゲインについて、重力補償、摩擦補償、慣性補償などの操作力を低減するための補助ゲインを発生させるよう制御する。ここでユーザは、マスタ２のアーム１２の位置及び姿勢を、スレーブ３のアーム３２の位置及び姿勢に合致させるように操作する。

演算制御部２１は、ユーザの操作によって肘の位置の偏差が指定範囲内に入ったかどうかを判定する（ステップＳ２３）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ２３でＹｅｓ）、ステップＳ２４に進む。偏差が指定範囲内でなければ（ステップＳ２３でＮｏ）、ステップＳ３０に進む。

演算制御部２１は、ステップＳ２４ではスレーブ３が取得した肘の位置に合致させるように、肩ピッチ２２と肩ロール２３に与えるトルクのゲイン設定を行う（ステップＳ２４）。

演算制御部２１は、ユーザの操作によって手首部の位置の偏差が指定範囲内に入ったかどうかを判定する（ステップＳ２５）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ２５でＹｅｓ）、ステップＳ２６に進む。偏差が指定範囲内でなければ（ステップＳ２５でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ

演算制御部２１は、スレーブ３が取得した手首の位置に合致させるように、肩ヨー２４と肘ピッチ２５に与えるトルクのゲイン設定を行う（ステップＳ２６）。

演算制御部２１は、ユーザの操作によって手首部の角度の偏差が指定範囲内に入ったかどうかを判定する（ステップＳ２７）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ２７でＹｅｓ）、ステップＳ１８に進む。偏差が指定範囲内でなければ（ステップＳ２７でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

演算制御部２１は、スレーブ３が取得した手首の向きに合致させるように、手首ヨー２６と手首ピッチ２７と手首ロール２８に与えるトルクのゲイン設定を行う（ステップＳ２８）。これにより、手首ヨー２６と手首ピッチ２７と手首ロール２８は動作を行う。その後、ステップＳ２９に進む。

演算制御部２１は、全ての角度の偏差が指定範囲内であるか否かを判定する（ステップＳ２９）。全ての角度の偏差があらかじめ定めた指定範囲内であれば（ステップＳ２９でＹｅｓ）、準備動作を終了し、マスタ２とスレーブ３の位置制御ゲイン、力制御ゲインを一致させることにより、マスタ２とスレーブ３を接続し、バイラテラル動作を開始する。全ての角度の偏差があらかじめ定めた指定範囲内でなければ（ステップＳ２９でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

ステップＳ２３において肘の位置の偏差が指定範囲内で無ければ、演算制御部２１は、手首部の位置の偏差が指定範囲内であるか否かを判定する（ステップＳ３０）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ３０でＹｅｓ）、ステップＳ３１に進む。偏差が指定範囲内でなければ（ステップＳ３０でＮｏ）、ステップＳ２３に戻る。このときユーザは、アーム１２を移動させ、肘または手首の位置が指定範囲に入るように姿勢を調整する。

演算制御部２１は、スレーブ３が取得した肘の位置に合致させるように、肘ピッチ２５に与えるトルクのゲイン設定を行う（ステップＳ３１）。

演算制御部２１は、肘の位置の偏差が指定範囲内であるか否かを判定する（ステップＳ３２）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ３２でＹｅｓ）、ステップＳ３３に進む。偏差が指定範囲内でなければ（ステップＳ３２でＮｏ）、ステップＳ３４に進む。

演算制御部２１は、スレーブ３が取得した肘の位置に合致させるように、肩ピッチ２２と肩ロール２３と肩ヨー２４に与えるトルクのゲイン設定を行う（ステップＳ３３）。その後、ステップＳ２７に進む。

ステップＳ３２において肘の位置の偏差が指定範囲内でなければ、演算制御部２１は、手首部の角度の偏差が指定範囲内であるか否かを判定する（ステップＳ３４）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ３４でＹｅｓ）、ステップＳ３５に進む。偏差が指定範囲内でなければ（ステップＳ３４でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

演算制御部２１は、スレーブ３が取得した手首の向きに合致させるように、手首ヨー２６と手首ピッチ２７と手首ロール２８に与えるトルクのゲイン設定を行う（ステップＳ３５）。

演算制御部２１は、肘の位置の偏差が指定範囲内であるか否かを判定する（ステップＳ３６）。偏差があらかじめ定めた指定範囲内であれば（ステップＳ３６でＹｅｓ）、ステップＳ３７に進む。偏差が指定範囲内でなければ（ステップＳ３６でＮｏ）、ユーザの操作によって指定範囲内に入るのを待つ。

演算制御部２１は、スレーブ３が取得した肘の位置に合致させるように、肩ピッチ２２と肩ロール２３と肩ヨー２４に与えるトルクのゲイン設定を行う（ステップＳ３７）。その後、ステップＳ２９に進み、全ての位置、角度の偏差が指定範囲内であるかを判定する。

これにより、肩側から先に位置を合わせるユーザと、手首側から先に位置を合わせるユーザのいずれにも対応可能な位置姿勢合わせ方法を提供することができる。位置の調整を肩側から行うか、手首側から行うかのいずれが好ましいかはユーザによって異なるが、実施の形態２にかかる動作フローによれば、いずれの場合にも対応することができる。すなわち、マスタ２をスレーブ３の正確な位置に引き寄せるトルクを発生させる際に、肩側関節から手首側関節の順に引き寄せることや、手首側関節から肩側関節から手首側関節の順に引き寄せることができ、粗い位置合わせが完了した関節軸から、順次引き寄せトルクを発生させることができる。

肘位置の偏差より先に、手首位置の偏差があらかじめ定めた指定範囲内に入るステップＳ３０のフローでは、肘ピッチ２５のゲインを制御することに加えて、手首の位置が変わらないように、他の各関節２１〜２４および２６〜２８を協調動作させることができると良い。この場合、ユーザは肘の位置を変更する自由度が１となり、肘関節の位置合わせが非常に容易となる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。またマスタ２において、手首ヨー２６を第１の手首ヨー、手首ロール２８を第２の手首ヨーに置き換え、スレーブ３において手首ヨー４６を第３の手首ヨー、手首ロール４８を第４の手首ヨーに置き換えても良い。これにより、例えばマスタ２の手首では、第１の手首ヨーと、手首ピッチと、第２の手首ヨーからなる軸構成とし、スレーブ３も同様の構成として、準備動作を実行できる。

１ マスタスレーブマニピュレータ
２ マスタ
３ スレーブ
１１ 本体部
１２ アーム
２１ 演算制御部
２２ 肩ピッチ
２３ 肩ロール
２４ 肩ヨー
２５ 肘ピッチ
２６ 手首ヨー
２７ 手首ピッチ
２８ 手首ロール
２９ エンドエフェクタ操作装置
３１ 本体部
３２ アーム
４１ 演算制御部
４２ 肩ピッチ
４３ 肩ロール
４４ 肩ヨー
４５ 肘ピッチ
４６ 手首ヨー
４７ 手首ピッチ
４８ 手首ロール
４９ エンドエフェクタ

Claims (1)

1. 冗長な自由度を有し、バイラテラルで動作するマスタスレーブマニピュレータの位置姿勢合わせ方法であって、
   マスタ側マニピュレータの肩位置、肩角度、肘位置、手首位置、手首角度の各パラメータについて、スレーブ側マニピュレータの位置、角度との偏差を算出し、
   前記偏差が指定範囲内になったときに、前記マスタ側マニピュレータの対応する部位が、スレーブ側マニピュレータの位置姿勢と一致するように、前記肩と前記肘と前記手首のうち、選択した任意の関節から順にトルクを印加して位置と角度の調整を行い、
   前記マスタ側マニピュレータの各部位の位置姿勢が、前記スレーブ側マニピュレータの位置姿勢と略一致した場合に、バイラテラル動作を開始する、
   マスタスレーブマニピュレータの位置姿勢合わせ方法。

Images