JP2005329476A - 操作部材の制御方法および装置 - Google Patents

Abstract

【課題】 制御的に支点を設定し、設定された制御支点の位置を操作対象物の変化に応じて動的に変化させ、制御支点を適切な位置に保つことを可能とすること。
【解決手段】 鉗子や電気メス等の操作部材１上に制御支点Ｐを設定し、この制御支点Ｐが、操作対象物２の表面の開口部の位置にあるように制御する。これにより、操作部材１を機械的に拘束することなく、設定された制御支点Ｐを軸として操作部材１を操作することができ、人体などの柔らかい物であっても開口部を損傷させることなく、体内の組織などに対して操作を加えることができる。また、操作部材１をスラスト方向に前後できるようにすることで、複雑な操作も可能となる。また、上記制御をバイラテラル制御に適用し、マスタ側の制御支点を固定し、スレーブ側の制御支点を操作対象物２の表面の開口部の位置にあるように制御してもよい。
【選択図】 図２

Description

本発明は、操作部材の支点となる位置を機械的に拘束することなく、制御系により支点を設定し、制御的に設定された支点を軸として操作部材を回動させ、操作部材の一方端を操作し、他方端で被操作体に対して操作を加える操作部材の制御方法および装置に関し、例えば低侵襲性外科手術などに適用するに好適な操作部材の制御方法および装置に関するものである。

操作部材を支点回りに動作させるような作業を行う際、例えば図１２（ａ）に示すように操作対象物に穴を開けるなど、操作対象物２をうまく利用することで機械的拘束点を実現することが可能である。しかし、機械的拘束による支点はブレが大きく、支点で生じる摩擦の影響を大きく受けることになる。
また操作対象物２が柔らかい物や壊れやすい物の場合、操作対象物２を利用して機械的拘束を作り出すのは困難となる。それは操作者４が大きな力で操作するときには、操作対象物２を破壊しないように操作者４が気を配る必要があるからである。これを克服する手段として、例えば図１２（ｂ）に示すように支点にしたい点の直上に丈夫な部材で機械的支点Ｐを設けることが考えられる。
しかしこの手法では、支点Ｐの設定位置は一点に限定され、機械的支点Ｐをセッティングするのに時間がかかるなどといった問題点がある。当然、摩擦も取り除くことはできない。

そこで、本発明者らは、先に、制御系により支点を作り出し（以下では、この支点を制御支点という）、棒状の部材を、上記制御的に拘束された制御支点を通るように動かすことで、機械的な拘束点を設けることなく操作部材を操作できるようにした制御系を提案した（非特許文献１参照）。
上記制御系を用いると、摩擦の影響を受けずに支点を作り出すことができ、また、操作対象物の硬さに関係なく、ロバストな位置制御により高精度な制御支点を実現することができる。さらにプログラムにより操作者が操作可能な範囲を容易に限定することができるので、より安全な作業が可能となる。
また機械的拘束の際に問題となったセッティング時間についても、短時間で、しかも簡単に支点のセッティングを実現することができる。
松本雄一、桂誠一郎、大西公平："低侵襲性医療における仮想支点を用いたバイラテラル制御"、電気学会産業計測制御研究会、ＩＩＣ−０３−３３，ｐｐ．９３−９７，２００３

上記非特許文献に記載される手法を用いることにより、機械的な支点を用いることなく、操作部材が常に制御支点を通るように制御することができ、この手法を６自由度マニピュレータに適用することで、装置構成を簡易化することができるだけでなく、支点に生ずる機械的摩擦を除去することができる。
しかし、上記非特許文献には、制御支点により、機械的支点を用いることなく操作部材の一点を拘束できるという技術思想は開示されているものの、実際の装置を如何に構成するかについては十分に示されていなかった。
特に、上記非特許文献では、制御支点を固定点として利用することが考慮されているにすぎず、例えば、人体のような物体の表面形状が時間とともに変化する対象物に適用するに際し、対象物の変化に応じて制御支点位置を如何に動的に変化させるかについては示されていなかった。

特に、バイラテラル制御系においては、マスタ側の操作部材の制御支点位置と、スレーブ側の制御支点位置を、それぞれ独立して制御できるようにすることで、操作性の向上を図ることができるものと期待されるが、上記非特許文献は、このような実際の装置を構成する上での操作性等を考慮した構成は開示されていなかった。
本発明は上記事情に鑑みなされたものであって、本発明は、制御支点の位置を操作対象物の変化に応じて動的に変化させることで、例えば表面が常に変化する人体などの対象物であっても、制御支点を適切な位置に保つことが可能な、実用性が高い操作部材の制御方法および装置を提供することを目的とする。

上記課題を解決するため、本発明においては、３次元計測装置等を用いて、操作対象物の表面の予め設定された位置を常に計測し、その点を制御支点生成装置の位置指令値とし、制御支点を制御する。
すなわち、以下のようにして前記課題を解決する。
（１）一方端が操作端である操作部材を多軸の駆動手段により駆動し、該操作部材の位置／方向を制御する。そして、操作対象となる対象物の予め設定された部分の位置を計測し、上記駆動手段の各軸の位置／角度を制御して、上記操作部材が、上記計測された位置を支点として動くように上記操作部材の位置／角度を制御する。
（２）上記（１）において、操作部材に作用する力を求める力検出手段を設け、この力検出手段により検出された操作部材に作用する力に応じて、上記駆動手段の各軸に作用する力を制御する。
（３）上記（１）（２）において、操作部材に対して上記支点を通る軸方向に作用する力に応じて、上記支点から操作部材の端部までの距離を変化させる。
（４）上記（１）−（３）をバイラテラル制御に適用し、マスタ側の制御支点の位置を基準位置になるように制御し、スレーブ側の制御支点の位置を、計測された操作対象物の予め設定された部分の位置になるように制御する。
（５）上記（１）−（３）をバイラテラル制御に適用し、マスタ側、スレーブ側の制御支点の位置を、計測された操作対象物の予め設定された部分の位置になるように制御する。（６）上記（４）（５）において、上記（３）と同様に、操作部材に対して上記支点を通る軸方向に作用する力に応じて、上記支点から操作部材の端部までの距離を変化させる。

本発明においては、以下の効果を得ることができる。
（１）支点を制御的に生成し、操作部材が常にこの制御支点を軸に動くようにを制御しているので、機械的な支点を用いる必要がない。このため、機械的な支点を設けることが難しい状況においても、操作部材に支点を設定することができる。
また、操作対象物表面の予め設定された位置を計測し、この位置に一致するように制御支点の位置を制御しているので、操作対象物の表面位置が変動している場合でも、制御対象物の特定の点を基準として制御支点の位置を設定することができる。
このため、例えば低侵襲性外科手術などにおいて、機械的支点を設けにくい人体等の対象物に対して、鉗子や電気メス等の操作部材を開口部を支点として操作することができ、従来に比べ操作性を著しく向上させることができる。
さらに、操作部材に対して上記支点を通る軸方向に作用する力に応じて、上記支点から操作部材の端部までの距離を変化させるようにすることで、一層、複雑な操作が可能となる。
（２）本発明をバイラテラル制御系に適用し、マスタ側の制御支点を基準位置になるように制御し、スレーブ側の制御支点を、計測された操作対象物表面の予め設定された位置になるように制御することで、スレーブ側で起こる位置の変動の影響を受けることなく、マスタ側での操作が可能となり、操作性が格段に向上する。
一方、マスタ側、スレーブ側の制御支点を共に、計測された操作対象物表面の予め設定された位置になるように制御することで、操作者は、操作対象物の表面変化の情報を得ることができる。

図１は、本発明における制御支点の概念を説明する図である。
本発明においては、図１（ａ）に示すように、操作部材１の駆動端１ａ（駆動手段側）と、操作端１ｂ（操作端側）を結ぶ線上に制御的な支点Ｐを設定する。
そして、駆動手段１ｃの各軸の位置角度から変換式により、制御支点Ｐの位置座標を求め、その位置が設定値（動的に変化する値でもよいし、固定点でもよい）に一致するように駆動手段の位置角度を制御する。
例えば、図１（ｂ）に示すように２次元平面で考えると、以下のように駆動端１ａの位置を制御すれば、制御支点Ｐが移動しないように制御することができる。
制御支点Ｐを座標原点として、操作部材１の制御支点Ｐから駆動端１ａまでの距離をｌｓとしたとき、操作部材１の水平軸に対する角度が同図の実線に示すように角度θ１のときの駆動端１ａの位置座標（ｘ１，ｙ１）は、同図に示すように、ｘ１＝ｌｓcos θ１、ｙ１＝ｌｓsin θ１となる。

この状態から、操作部材１を回転させ、水平軸に対する角度をθ２にしたときに、駆動端１ａの位置座標（ｘ２，ｙ２）をｘ２＝ｌｓcos θ２、ｙ２＝ｌｓsin θ２になるように、駆動端１ａを移動させれば、制御支点Ｐの位置を一定に保つことができる。
すなわち、操作部材１が平面上で移動する場合には、少なくとも３軸以上の駆動装置により操作部材１を駆動することで、制御支点Ｐの位置を一定に制御することができる。
なお、図１（ｂ）では駆動端が一方に設けられている場合について示しているが、駆動端が操作部材の両端に設けられている場合には、操作部材の両端に少なくととも２軸以上の駆動装置を設ければ、制御支点Ｐの位置が一定になるように制御することができる。 また、操作部材１が３次元空間で移動する場合であって、一方端に駆動端１ａが設けられている場合には、少なくとも６軸以上の駆動装置を用いればよい。

図２は、鉗子等の操作部材を開口部から体内に挿入して手術を行なう低侵襲性外科手術に本発明を適用したときの概念図である。
同図では、制御支点Ｐを原点として、鉗子や電気メス等の操作部材１の長手方向（スラスト方向という）にＺ軸が重なるように、制御支点座標系を設定し、駆動端１ａを制御して、制御支点Ｐが所望の位置に保たれるように制御している。
同図に示すように、鉗子や電気メス等の操作部材１上に制御支点Ｐを設定し、この制御支点Ｐが、操作対象物２の表面の開口部（切開部）の位置にあるように制御する。これにより、操作部材１を機械的に拘束することなく、設定された制御支点Ｐを軸として、鉗子等の操作部材１を操作することができ、操作対象物２が人体などの柔らかい物であっても、開口部を損傷させることなく、体内の組織などに対して操作を加えることができる。

また、上記駆動端１ａと制御支点Ｐの距離ｌｓをコンプライアンス的に変化させ、操作部材１をスラスト方向に前後できるようにすることで、複雑な操作も可能となる。
さらに、同図に示すように、三次元計測装置３を設け、人体等の操作対象物２の表面位置（開口部位置）を計測し、計測された表面位置に追従させて制御支点の位置を制御することにより、操作対象物の表面位置に合わせて、制御支点を生成することができ、物体の表面形状が変化する対象物に対しても、安全に適用することができる。
上記三次元計測装置３による表面位置の計測は、例えば、操作対象物の表面の被計測点の周囲に、被計測点が中心となるように三角形状にＬＥＤを配置して、三次元計測装置２により各ＬＥＤの位置を計測して、計測された各ＬＥＤの三次元位置の中心を求めることで、被計測点の三次元位置を精度良く求めることができる。

図３〜図５は、本発明の制御支点の有効性を調べるために行った実験結果を示す図である。図３は、制御支点ｙ軸周りに正弦波状のトルク入力を行った様子を示す図である。
また、図４は、スラスト方向に正弦波状の力入力を行った様子を示す図である。図３、図４において、ｔｉｐは操作部材の先端（操作端）、ｃｓｐは制御支点、ｒｏｏｔは操作部材の根元（駆動端）である。
上記結果より、制御支点周りにおいて、２つの動作が干渉することなく、所望の動作が実現されていることが確認された。
続いて、人間が操作を行う実験を行った。その結果を図５に示す。同図において、図３、図４と同様、ｔｉｐは操作部材の先端（操作端）、ｃｓｐは制御支点、ｒｏｏｔは操作部材の根元（駆動端）である。
同図に示すように、人間の入力に対しても、制御支点が維持される、所望の動作を実現できることが確認された。

以下、本発明の実施例の制御装置の構成について説明する。
図６は、本発明を姿勢制御に適用した場合の概念図である。
同図において、１は操作部材であり、操作部材１の駆動端１ａ側は６自由度のマニピュレータ５で駆動され、操作部材１の操作端１ｂ側は、操作対象物２の表面に形成された開口部を介して操作対象物２の内部に挿入されている。
操作者４は、上記操作部材１の駆動端側を操作して、操作部材１の操作端１ｂにより操作対象物２内部の組織などに対して操作を行う。
その際、三次元計測装置３により操作対象物２の開口部の表面位置を計測する。そして、この点を位置指令値としてマニピュレータ５の制御装置に与え、操作部材１の制御支点Ｐの位置が常に上記位置指令値に一致するように制御する。
これにより、操作者４が操作部材１の駆動端１ａ側を操作したとき、同図の点線に示すように開口部が形成された操作対象物３の表面位置を支点として、操作部材１が動き、表面形状が変化する操作対象物２に対しても安全に操作を加えることができる。
さらに、前記したように、操作部材１をスラスト方向に前後できるようにすることで、複雑な操作も可能となる。

図７は本発明の第１の実施例の制御系の構成を示す図であり、同図は図６に示した姿勢制御系の構成を示す図である。なお、以下の明細書中では、変数の一次微分、二次微分をそれぞれ変数に「’」、「”」を付して示すが、図面中では、それぞれ変数の上に「ドット」、「２ドット」を付して示す。
図７において、５は上記６自由度のマニピュレータ、１１は力検出手段であり、力検出手段１１によりマニピュレータ５の各軸の関接が受ける力（トルク）を検出する。力検出手段１１としては、例えば、ひずみゲージを用いて外力より生ずる構造体のひずみを直接検出し、力に変換を行う力センサや、反力オブザーバを用いることができる（反力オブザーバにつては、例えば、村上俊之, 中村亮, 郁方銘, 大西公平: " 反作用力推定オブザーバに基づいた多自由度ロボットの力センサレスコンプライアンス制御" 日本ロボット学会誌, Vol.11, No. 5, pp.765-768, (1993) 等参照）。
また、本出願人らが先に提案した環境クオリアを力検出手段として用いることもできる（特願２００３−２７８９１９号参照）。上記環境クオリアは、マニピュレータを２台用意し、一方のマニピュレータには外力を加えず、上記外乱オブザーバにより、外力を加えないマニピュレータに働く重力や摩擦力などを同定し、同定した重力や摩擦力を作業を行っているロボットから引くことで、純粋な力の検出を行うものである。
環境クオリアは、外乱オブザーバにより上記したように重力や摩擦力を同定しているのでノイズにも強く、広帯域のセンシングが可能であり、また、煩雑な同定試験などを必要とせず、また多自由度マニピュレータへの適用も非常に容易である。

マニピュレータ５の各軸の角度θ^res は、図示しないセンサにより検出され、座標変換部１２に与えられる。座標変換部１２は、以下の式（１）−（２）式によりマニピュレータ５の各軸の角度ベクトルθ^res を作業空間の座標系（基準座標系Ｘ，Ｙ，Ｚ）における制御支点の位置座標ベクトルｘ^res に変換する。ここで、ヤコビ行列Ｊ_aco はマニピュレータの関節空間の座標θから作業空間の座標ｘに変換する行列、制御支点ｘは、以下の（３）（４）で表されるベクトル、ｎ，ｓ，ａは、基準座標系から制御支点座標系への回転行列のそれぞれの成分を表したものである。

３は前記図２で説明した三次元計測装置であり、三次元計測装置３により操作対象物の表面位置Ｘ^cmd 、速度Ｘ' ^cmd を検出し、上記座標変換部１２により得られた制御支点の位置ｘ^res 、速度ｘ' ^res と比較し、その偏差を位置制御部１４に与える。位置制御部１４は、例えば、ＰＤコントローラであり、以下の式（５）に示すように、偏差にＰＤ（比例、微分）演算を施し出力する。

一方、マニピュレータ５の各軸の関節に加わる力を力検出手段１１により検出する。ここで、マニピュレータ５の先端が物体に触れると、それぞれの関節が力を受け、基準座標系におけるトルクベクトルＮ_ext が求められる。
上記トルクベクトルＮ_ext は、コンプライアンス制御部１３に与えられる。コンプライアンス制御部１３は、上記トルクベクトルＮ_ext を以下の式（６）に代入し、コンプライアンス指令値を求め、式（７）に示す姿勢制御ゲインを求める。これを式（１１）に代入することで、姿勢の指令値を求める。ここで、Ｉは対角成分が全て１で残りが全て０の単位行列である。
ここで、対角行列であるＭ_c ，Ｄ_c ，Ｋ_c ，Ｋ_f の値を選ぶことによりコンプライアンス特性を任意に設定することができる。

本発明では、方向余弦誤差ベクトルを用いることにより、マニピュレータの姿勢制御を行った。方向余弦誤差ベクトルｅ^cmd は、式（１２）で計算される。
さらに、上記方向余弦誤差ベクトルと、前記コンプライアンス指令値と、応答値に基づき、以下の式（１３）でＰＤ演算を行う。

前記位置制御部１４と上記コンプライアンス制御部１３の出力は、演算部１５，１６で以下の式（１４）で示されるハイブリッド行列Ｈ，Ｉ−Ｈと演算され、その出力が加算されて加速度参照値Ｘ" ^ref が求められる。加速度参照値Ｘ" ^ref は、演算部１７で、前記ヤコビ行列Ｊ_aco の逆行列Ｊ_aco ^-1（作業空間ｘの座標系をマニピュレータの関節空間θの座標に変換する行列）と演算され、その出力によりマニピュレータ５が駆動される。

以上のような制御系において、操作者が、図６に示した操作部材１の駆動端１ａ側を操作すると、そのときの操作力が力検出手段１１により検出され、操作部材１は、コンプライアンス制御部１３により、操作者の操作に対してゆっくりとした変化で応答し、操作者の操作に応じた姿勢に制御される。この応答特性（コンプライアンス特性）は、前記したようにコンプライアンス制御部のＭ_c ，Ｄ_c ，Ｋ_c ，Ｋ_f の値を選ぶことにより任意に設定することができる。
一方、操作部材１の制御支点Ｐは、上記位置制御部１４により、常に操作対象物表面の設定された位置に制御され、操作対象物２の表面位置を支点として、操作者の操作に応じて操作部材１が動く。

図８は本発明の第２の実施例の制御系の構成を示す図であり、本実施例は、前記図７に示した制御系にスラスト制御を付加したものである。
図８においては、前記図７に示した制御系に制御支点座標系変換部１８と、コンプライアンスモデル１９を設け、操作者が前記操作部材１をスラスト方向（操作部材１の長手方向）に操作力を加えたとき、前記図２に示したように、駆動端１ａと制御支点Ｐの距離ｌｓをコンプライアンス的に変化させ、操作部材１をスラスト方向に前後できるようにしたものである。
図８において、力検出手段１１により検出された操作力は、制御支点座標系変換部１８に与えられ、制御支点座標系変換部１８は、以下の式（１５）により、マニピュレータ先端に加わる力Ｆ^ext を制御支点周りの外力Ｆ^csp に変換する。なお、ｘ_R は前記した基準座標系から制御支点座標系への回転行列（姿勢）である。

ついで、上記制御支点座系に変換された力ベクトルＦ^csp を次の（１６）式に示すコンプライアンスモデルを用いて、コンプライアンス指令値に変換する。
そして、マニピュレータの駆動端１ａから制御支点までの距離ｌｓを次の（１７）式により、コンプライアンス指令値により変化させる。すなわち、（１６）式により求まったコンプライアンス指令値ｘ_c ^cmd を上記距離ｌｓの初期値であるｌｓ ^intから引き算し、制御支点までの距離ｌｓを変化させる。これに応じ前記式（１）におけるｆ（θ）行列の成分及び逆ヤコビ行列Ｊ_aco ^-1の成分を変化させる。
マニピュレータ５は、最終リンク長にかかわらず、制御支点をある一点に保つように制御しているのでマニピュレータはｌｓが短くなると、押し動作をし、ｌｓが長くなると引き動作をすることになる。

次に本発明をバイラテラル制御に適用した場合について説明する。
バイラテラル制御に制御支点を用いると、同様に操作対象物の表面形状が変化する対象操作対象物に有効になる。
しかし、操作対象物の表面形状変化が大きいときには、マスタとスレーブシステムの２つの制御支点を同じように動かしてしまうと、操作者は支点が変化する影響により、安定した操作の実現は困難なものとなる。
そこで、本発明では、バイラテラル制御においてマスタのみ固定支点にする。つまり、スレーブだけが操作対象物の表面形状にあわせて制御支点を可変に制御し、マスタに対してはある一点のみを制御支点に設定する。これにより操作者はスレーブ側で起こる位置の変動の影響を受けることなくマスタ側での操作が可能となり、操作性が格段に向上することが考えられる。特に、心臓外科などの分野ではスレーブ側は心臓の拍動とともに制御支点をフィードバックして制御を行い、操作者は固定の制御支点を用いることで心臓の拍動を意識せずにマスタ側の操作を行うことが可能となる。
一方、操作者が対象物の表面変化の情報を得たいときは、マスタ、スレーブそれぞれを動的制御支点に設定する。

図９は、本発明をバイラテラル制御に適用した場合の概念図である。
同図において、Ｍはマスタ側制御系、Ｓはスレーブ側制御系であり、それぞれの制御系には、６自由度のマニピュレータ５ｍ、５ｓが設けられており、マスタ側のマニピュレータ５ｍを操作者４が操作することで、スレーブ側のマニピュレータ５ｓが追従して動作し、また、スレーブ側のマニピュレータ５ｓに作用する力が、マスタ側にフィードバックされる。
スレーブ側の制御系において、１は操作部材であり、操作部材１の駆動端１ａ側は６自由度のマニピュレータ５ｓで駆動され、操作部材１の操作端１ｂ側は、操作対象物２の表面に形成された開口部を介して操作対象物２の内部に挿入されている。
操作者４は、マスタ側のマニピュレータ５ｍの駆動端を操作する。これに応じて、スレーブ側の操作部材１の操作端１ｂが駆動され、操作対象物２内部の組織などに対して操作が行われる。
その際、三次元計測装置３により操作対象物２の開口部の表面位置を計測する。そして、この点を位置指令値としてマニピュレータ５ｓの制御装置に与え、操作部材１の制御支点Ｐｓの位置が常に上記位置指令値に一致するように制御する。
一方、マスタ側の制御支点Ｐｍの位置は、上述したように、固定支点にしたり、スレーブと同様、動的制御支点に設定する。

マスタ側の制御支点Ｐｍを固定支点にすることにより、前記したように、操作対象物２の表面位置の変動に係わらず、制御支点Ｐｍの位置は変わらないので、操作者４は、操作対象物２の変動にかかわらず、安定した操作を行うことができる。一方、スレーブ側では、開口部が形成された操作対象物３の表面位置を支点として、操作部材１が動き、表面形状が変化する操作対象物２に対しても安全に操作を加えることができる。
また、マスタ側の制御支点Ｐｍをスレーブと同様、動的制御支点に設定することで、操作者は、対象物の表面変化を把握することができるようになる。
さらに、前記したように、操作部材１をスラスト方向に前後できるようにすることで、複雑な操作も可能となる。

図１０は、本発明の第３の実施例の制御系の構成を示す図であり、本実施例は、バイラテラル制御において、上述したように、マスタのみ固定支点にし、スレーブだけが操作対象物の表面形状にあわせて制御支点を可変にする場合の構成を示している。
図１０において、５ｍ，５ｓはそれぞれマスタ側、スレーブ側の６自由度のマニピュレータ、１１ｓ，１１ｍはそれぞれマスタ側、スレーブ側の力検出手段であり、力検出手段１１ｍ，１１ｓによりマニピュレータ５ｍ，５ｓの各軸の関節が受ける力（トルク）を検出する。
マニピュレータ５ｍ，５ｓの各軸の角度θ_m ^res , θ_s ^res は、図示しないセンサにより検出され、それぞれ座標変換部１２ｍ，１２ｓに与えられる。座標変換部１２ｍ，１２ｓは、前記したように、マニピュレータ５ｍ，５ｓの各軸の角度ベクトルθ_m ^res , θ_s ^res を制御支点の位置座標ベクトルｘ^res に変換する。
３は三次元計測装置であり、三次元計測装置３により操作対象物の表面位置Ｘ^cmd 、速度Ｘ' ^cmd を検出し、上記スレーブ側の座標変換部１２ｍにより得られた制御支点の位置と比較し、その偏差を位置制御部１４ｍに与える。位置制御部１４ｍは、偏差にＰＤ（比例、微分）演算を施し出力する。
また、マスタ側においては、予め設定された制御支点の指令値Ｘ^cmd と、上記マスタ側の座標変換部１２ｍにより得られた制御支点の位置とを比較し、その偏差を位置制御部１４ｍに与える。位置制御部１４ｍは、偏差にＰＤ（比例、微分）演算を施し出力する。

一方、マニピュレータ５ｍ，５ｓの各軸の関節に加わる力を力検出手段１１ｍ，１１ｓにより検出し、それぞれトルクベクトルＮ_mext，Ｎ_sextを求める。
上記マスタ側、スレーブ側のトルクベクトルＮ_mext，Ｎ_sextは加算され、その加算結果であるトルクベクトルＮ_ext は、コンプライアンス制御部１３ｍ，１３ｓに与えられる。 コンプライアンス制御部１３ｍ，１３ｓは、上記トルクベクトルＮ_ext に基づき、前記したように、姿勢の指令値を求め、方向余弦誤差ベクトルと、前記コンプライアンス指令値と、応答値に基づき、前記したように、ＰＤ演算を行う。
ここで、前記したようにコンプライアンス制御部１３ｍ，１３ｓのＭ_c ，Ｄ_c ，Ｋ_c ，Ｋ_f の値を選ぶことによりコンプライアンス特性を任意に設定することができる。
前記位置制御部１４ｍ，１４ｓと上記コンプライアンス制御部１３ｍ，１３ｓの出力は、演算部１５ｍ，１５ｓ，１６ｍ，１６ｓでハイブリッド行列Ｈ，Ｉ−Ｈと演算され、その出力が加算されて、マスタ側、スレーブ側の加速度参照値Ｘ_m " ^ref ，Ｘ_s " ^ref が求められる。加速度参照値Ｘ_m " ^ref ，Ｘ_s " ^ref は、演算部１７ｍ，１７ｓで、仮想支点に関するヤコビ行列Ｊ_aco の逆行列Ｊ_aco ^-1と演算され、その出力によりマスタ側、スレーブ側のマニピュレータ５ｍ，５ｓが駆動される。

以上のような制御系において、操作者が、マスタ側のマニピュレータ５ｍの駆動端を操作すると、そのときの操作力が力検出手段１１ｍにより検出され、マニピュレータ５ｍの駆動端は、コンプライアンス制御部１３ｍにより、操作者の操作に対してゆっくりとした変化で応答し、操作者の操作に応じた姿勢に制御される。その際、マスタ側の制御支点の位置は、予め設定された指令値Ｘ^cmd になるように制御される。
また、上記力検出手段１１ｍにより検出されマスタ側のマニピュレータ５ｍの各軸に加わる力と、スレーブ側のマニピュレータ５ｓの各軸に加わる力が加算され、この和がマスタ側、スレーブ側のコンプライアンス制御部１３ｍ，１３ｓにフィードバックされ、マスタ側のマニピュレータ５ｍの操作端に加わる力に応じて、スレーブ側のマニピュレータ５ｓが制御される。
一方、操作対象物２の表面位置が三次元計測装置３により計測され、スレーブ側の制御支点の位置は、常に操作対象物表面の設定された位置に制御され、操作対象物３の表面位置を支点として、操作者の操作に応じて、スレーブ側の操作部材１が動く。

図１１は、本発明の第４の実施例の制御系の構成を示す図であり、本実施例は、バイラテラル制御において、前述したように、マスタ側とスレーブ側の制御支点の位置を操作対象物の表面形状にあわせて可変にする場合の構成を示している。
図１１の構成は図１０に示したものと同様であり、図１１においては、三次元計測装置３により検出され操作対象物の表面位置をそれぞれマスタ側、スレーブ側に与え、マスタ側、スレーブ側の制御支点位置が、三次元計測装置による検出値と等しくなるように制御している。
その他の動作は、図１０に示したものと同様であり、操作者４がマスタ側のマニピュレータ５ｍの駆動端を操作すると、それに追従してスレーブ側のマニピュレータ５ｍが動き、スレーブ側の操作部材が操作者の操作に応じて動作する。

本発明における制御支点の概念を説明する図である。 鉗子等の操作部材を開口部から体内に挿入して手術を行なう低侵襲性外科手術に本発明を適用したときの概念図である。 制御支点ｙ軸周りに正弦波状のトルク入力を行った場合の動きを示す図である。 制御支点に対してスラスト方向に正弦波状の力入力を行った場合の動きを示す図である。 人間が操作を行った場合の制御支点周りの動きを示す図である。 本発明を姿勢制御に適用した場合の概念図である。 本発明の第１の実施例の制御系の構成を示す図である。 本発明の第２の実施例の制御系の構成を示す図である。 本発明をバイラテラル制御に適用した場合の概念図である。 本発明の第３の実施例の制御系の構成を示す図である。 本発明の第４の実施例の制御系の構成を示す図である。 機械的拘束点を作り出す方法を説明する図である。

符号の説明

１ 操作部材
２ 操作対象物
３ 三次元計測装置
４ 操作者
５，５ｍ，５ｓ マニピュレータ
１１，１１ｍ，１１ｓ 力検出手段
１２，１２ｍ，１２ｓ 座標変換部
１３，１３ｍ，１３ｓ コンプライアンス制御部
１４，１４ｍ，１４ｓ 位置制御部
１５，１５ｍ，１５ｓ 演算部
１６，１６ｍ，１６ｓ 演算部
１７，１７ｍ，１７ｓ 演算部
１８，１８ｍ，１８ｓ 制御支点座標系変換部
１９，１９ｍ，１９ｓ コンプライアンスモデル

Claims (7)

1. 少なくとも一方端が操作端である操作部材を多軸の駆動手段により駆動し、該操作部材の位置／方向を制御し、該操作部材により、対象物に対して物理的・化学的な操作を行う操作部材の制御方法であって、
   上記対象物の予め設定された部分の位置を計測し、
   上記駆動手段の各軸の位置／角度を制御して、上記操作部材が、上記計測された位置を支点として動くように上記操作部材の位置／角度を制御する
   ことを特徴とする操作部材の制御方法。
2. 少なくとも一方端が操作端である操作部材を多軸の駆動手段により駆動し、該操作部材の位置／方向を制御し、該操作部材により、対象物に対して物理的・化学的な操作を行う操作部材の制御装置であって、
   上記対象物の予め定められた部分の位置を計測する計測手段と、
   上記駆動手段の各軸の位置／角度を検出する検出手段と、
   上記検出された位置／角度から、上記操作部材の支点となるべき位置を算出する手段と、
   上記算出された支点位置と、上記計測手段により計測された位置との偏差を求め、その偏差に基づき上記駆動手段の各軸の位置／角度を制御して、上記操作部材が上記計測手段により計測された位置を支点として動くように上記駆動手段により操作部材の位置／角度を制御する制御手段を備えた
   ことを特徴とする操作部材の制御装置。
3. 上記操作部材に作用する力を求める力検出手段を備え、上記制御手段は、上記力検出手段により検出された操作部材に作用する力に応じて、上記駆動手段の各軸に作用する力を制御する
   ことを特徴とする請求項３の操作部材の制御装置。
4. 上記制御手段は、上記操作部材に対して上記支点を通る軸方向に力が作用したとき、この力に応じて、上記支点から操作部材の端部までの距離を変化させる
   ことを特徴とする請求項２または請求項３の操作部材の制御装置。
5. 多軸の第１の駆動手段により駆動されるマスタ側の操作部材と、
   多軸の第２の駆動手段により駆動される少なくとも一方端が操作端であるスレーブ側の操作部材を備え、
   マスタ側の操作部材に作用する力に応じてマスタ側の操作部材を制御するとともに、スレーブ側の操作部材に作用する力を制御し、また、スレーブ側の操作部材が対象物から受ける力をマスタ側の操作部材に伝え、スレーブ側の操作部材により、対象物に対して物理的・化学的な操作を行う操作部材の制御装置であって、
   上記対象物の予め定められた部分の位置を計測する計測手段と、
   上記マスタ側の操作部材に作用する力を求める第１の力検出手段と、
   上記スレーブ側の操作部材に作用する力を求める第２の力検出手段と、
   上記マスタ側の第１の駆動手段の各軸の位置／角度を検出する検出手段と、
   上記検出された位置／角度から、上記マスタ側の操作部材の支点となるべき位置を算出する手段と、
   上記算出された支点位置と、予め設定された基準位置との偏差を求め、その偏差に基づき上記第１の駆動手段の各軸の位置／角度を制御して、上記マスタ側の操作部材が上記基準位置を支点として動くように上記駆動手段により操作部材の位置／角度を制御するとともに、上記第１の力検出手段と第２の力検出手段により検出された力の和に基づき、上記第１の駆動手段の各軸に作用する力を制御する第１の制御手段と、
   上記スレーブ側の第２の駆動手段の各軸の位置／角度を検出する検出手段と、
   上記検出された位置／角度から、上記スレーブ側の操作部材の支点となるべき位置を算出する手段と、
   上記算出された支点位置と、上記計測手段により計測された位置との偏差を求め、その偏差に基づき上記第２の駆動手段の各軸の位置／角度を制御して、上記スレーブ側の操作部材が上記計測手段により計測された位置を支点として動くように上記駆動手段により操作部材の位置／角度を制御するとともに、上記第１の力検出手段と第２の力検出手段により検出された力の和に基づき、上記第２の駆動手段の各軸に作用する力を制御する第２の制御手段とを備えた
   ことを特徴とする操作部材の制御装置。
6. 多軸の第１の駆動手段により駆動されるマスタ側の操作部材と、
   多軸の第２の駆動手段により駆動される少なくとも一方端が操作端であるスレーブ側の操作部材を備え、
   マスタ側の操作部材に作用する力に応じてマスタ側の操作部材を制御するとともに、スレーブ側の操作部材に作用する力を制御し、また、スレーブ側の操作部材が対象物から受ける力をマスタ側の操作部材に伝え、スレーブ側の操作部材により、対象物に対して物理的・化学的な操作を行う操作部材の制御装置であって、
   上記対象物の予め定められた部分の位置を計測する計測手段と、
   上記マスタ側の操作部材に作用する力を求める第１の力検出手段と、
   上記スレーブ側の操作部材に作用する力を求める第２の力検出手段と、
   上記マスタ側の第１の駆動手段の各軸の位置／角度を検出する検出手段と、
   上記検出された位置／角度から、上記マスタ側の操作部材の支点となるべき位置を算出する手段と、
   上記算出された支点位置と、上記計測手段により計測された位置との偏差を求め、その偏差に基づき上記第１の駆動手段の各軸の位置／角度を制御して、上記マスタ側の操作部材が上記計測手段により計測された位置を支点として動くように上記駆動手段により操作部材の位置／角度を制御するとともに、上記第１の力検出手段と第２の力検出手段により検出された力の和に基づき、上記第１の駆動手段の各軸に作用する力を制御する第１の制御手段と、
   上記スレーブ側の第２の駆動手段の各軸の位置／角度を検出する検出手段と、
   上記検出された位置／角度から、上記スレーブ側の操作部材の支点となるべき位置を算出する手段と、
   上記算出された支点位置と、上記計測手段により計測された位置との偏差を求め、その偏差に基づき上記第２の駆動手段の各軸の位置／角度を制御して、上記スレーブ側の操作部材が上記計測手段により計測された位置を支点として動くように上記駆動手段により操作部材の位置／角度を制御するとともに、上記第１の力検出手段と第２の力検出手段により検出された力の和に基づき、上記第２の駆動手段の各軸に作用する力を制御する第２の制御手段とを備えた
   ことを特徴とする操作部材の制御装置。
7. 上記第１、第２の制御手段は、上記マスタ側およびスレーブ側の操作部材に対して上記支点を通る軸方向に力が作用したとき、この力に応じて上記支点から操作部材の端部までの距離を変化させる
   ことを特徴とする請求項５または請求項６の操作部材の制御装置。

Images