(57)【要約】【課題】把持力等の作用力を安定に精度よく制御可能なマニピュレータシステムおよびその制御方法を提供する。【解決手段】マニピュレータシステムの制御装置２は、把持開始推定オブザーバ５１と、把持角度決定部５２と、逆キネマティック行列演算部５３と、ＰＩＤ制御部５４と、モータ駆動部５５と、目標把持角度設定部５６と、最閉角度決定部５７と、操作量更新部５８とを有する。把持開始推定オブザーバ５１は把持開始角度を推定し、推定した把持開始角度に基づいてグリッパ４の最閉角度を設定して、最閉角度に達するまでグリッパ軸角度を制御する。これにより、モータのトルク制御で把持力を制御する場合よりも、精度よくグリッパ４の把持力制御が可能となる。【選択図】図８
2010076012.tif 000002
【特許請求の範囲】
【請求項１】
アクチュエータと、
前記アクチュエータの動力に基づいて対象物に対して力を作用する作用部と、
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、
前記アクチュエータを制御する制御部と、を備え、
前記制御部は、
角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するオブザーバ信号生成手段と、
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステム。
【請求項２】
複数のアクチュエータと、
前記複数のアクチュエータの動力に基づいて対象物に対して力を作用する姿勢軸と、前記対象物をそれぞれ異なる方向に動作させる複数の動作軸と、を有する作用部と、
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、
前記アクチュエータを制御する制御部と、を備え、
前記複数のアクチュエータは、前記姿勢軸および前記複数の動作軸の駆動に影響を与える干渉系アクチュエータであり、
前記制御部は、
前記複数のアクチュエータのそれぞれごとに設けられ、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号候補をそれぞれ生成する複数のオブザーバ信号候補生成手段と、
前記複数のアクチュエータの各駆動軸の動作速度に応じて、前記複数の作用力開始角度推定手段から出力されたオブザーバ信号のいずれか一つを選択するオブザーバ信号選択手段と、
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で選択されたオブザーバ信号に対応する作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステム。
【請求項３】
前記オブザーバ信号選択手段は、前記複数のアクチュエータのうち、前記姿勢軸の駆動に直接関与するアクチュエータの駆動軸の動作速度が正であれば、このアクチュエータに対応するオブザーバ信号を選択し、前記姿勢軸の駆動に直接関与するアクチュエータの駆動軸の動作速度が負であれば、他のアクチュエータの中で駆動軸の動作速度が正のアクチュエータに対応するオブザーバ信号を選択することを特徴とする請求項２に記載のマニピュレータシステム。
【請求項４】
前記制御部は、
前記オブザーバ信号選択手段で選択されたオブザーバ信号と予め設定された動作軸補正基準値とに基づいて、前記複数の動作軸の動作量を補正するための動作軸補正開始角度を推定する補正開始角度推定手段と、
前記動作軸補正開始角度が推定された後に、前記作用部の姿勢軸角度に基づいて前記複数の動作軸の偏差角度を算出する偏差角度算出手段と、
前記偏差角度に基づいて前記複数の動作軸の動作量を補正する動作軸補正手段と、を有することを特徴とする請求項２または３に記載のマニピュレータシステム。
【請求項５】
前記動作軸補正基準値は、前記作用部の作用力開始角度を推定するための基準値以下に設定されることを特徴とする請求項４に記載のマニピュレータシステム。
【請求項６】
前記作用力開始角度推定手段は、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに前記作用部の作用力開始角度を推定するか、または前記項を考慮に入れて把持力を推定するかを切替える第１の切替手段を有することを特徴とする請求項１乃至５のいずれかに記載のマニピュレータシステム。
【請求項７】
前記第１の切替手段は、前記作用部が対象物を作用方向に動作させるときに、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに前記作用部の作用力開始角度を推定し、前記作用部が対象物の作用動作を解除したときに、前記項を考慮に入れて作用力を推定することを特徴とする請求項６に記載のマニピュレータシステム。
【請求項８】
前記第１の切替手段は、前記対象物の姿勢軸の目標速度が負の場合に、前記作用部が対象物を作用方向に動作させていると判断し、前記対象物の姿勢軸の目標速度が正かゼロの場合に、前記作用部が前記対象物の作用動作を解除したと判断することを特徴とする請求項７に記載のマニピュレータシステム。
【請求項９】
前記制御部は、前記作用部が前記対象物を作用方向に動作させ、かつ前記オブザーバ信号が所定の基準値以上である場合に、作用開始を示す作用フラグ情報をセットし、前記作用部の姿勢軸角度が前記作用力開始角度よりも大きくなった場合に、前記作用部が前記対象物への作用を終了したとみなして前記作用フラグ情報をリセットする作用フラグ決定手段を有することを特徴とする請求項１乃至８のいずれかに記載のマニピュレータシステム。
【請求項１０】
前記作用部の作用力を操作指令する操作指令部を備え、
前記制御部は、前記作用力開始角度推定手段により推定された作用力開始角度に対応する前記操作指令部の操作量を基点として、前記操作指令部の操作量と前記作用部の姿勢軸角度との相関関係に基づいて、前記作用部の作用力を調整することを特徴とする請求項１乃至８のいずれかに記載のマニピュレータシステム。
【請求項１１】
前記制御部は、前記操作指令部の操作量が最大のときに最大把持力となるように前記相関関係を設定することを特徴とする請求項１０に記載のマニピュレータシステム。
【請求項１２】
前記制御部は、
前記作用部の姿勢軸角度が作用力開始角度と姿勢軸限界角度との間にある場合に、前記作用部の姿勢軸の駆動に直接関係するアクチュエータの駆動軸角度と、該アクチュエータに対応する前記作用力開始角度推定手段から出力されたオブザーバ信号とを記録する記録手段と、
前記作用部の姿勢軸角度が前記姿勢軸限界角度に達したときに、前記記録手段で記録された駆動軸角度とオブザーバ信号とを用いて前記対象物の硬さを推定する硬さ推定手段と、
予め設定された基準の硬さと前記硬さ推定手段で推定された硬さとに基づいて、前記姿勢軸限界角度を更新する姿勢軸限界角度更新手段と、を有することを特徴とする請求項１乃至１１のいずれかに記載のマニピュレータシステム。
【請求項１３】
前記作用力開始角度推定手段により推定された作用力開始角度とこの作用力開始角度の最小値とのずれ量に基づいて、異常検知を行う異常検知手段を備えることを特徴とする請求項１乃至１２のいずれかに記載のマニピュレータシステム。
【請求項１４】
対象物の姿勢軸を駆動するアクチュエータと、
前記アクチュエータの動力に基づいて前記対象物に対して力を作用する作用部と、
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、
前記アクチュエータを制御する制御部と、を備えたマニピュレータシステムの制御方法であって、
前記制御部は、
角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するステップと、
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定するステップと、
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出するステップと、
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御するステップと、を有することを特徴とするマニピュレータシステムの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、アクチュエータの動力を動力伝達機構を介して作用部に伝達するマニピュレータシステムおよびその制御方法に関する。
【背景技術】
【０００２】
マニピュレータの把持力を把持機構部にセンサを設けることなく制御することは従来からの課題である。例えば特許文献１に記載されている医療用マニピュレータの場合、マニピュレータ先端部は体腔内部に侵入されるため、細径小型化が求められるとともに、十分な力の伝達が必要とされる。また、医療用機器であるため、洗浄滅菌処理が可能で、通電部分がないことが好ましい。このような理由から、先端部には把持力検出用のセンサ等を配置しない方が望ましい。医療用マニピュレータに限らず、配管作業ロボット、ホームロボット等でも小型化、軽量化、低コスト化の面から、少ないセンサ数で制御可能であることが望まれている。
【０００３】
このような課題を解決するために、特許文献２には、ハンドを駆動するモータ電流から物体を把持したことを認識し、モータ電流値によってトルク制御を行う技術が記載されている。また、特許文献３には、把持機構を駆動するモータ電流と角速度からオブザーバを構築して、把持力用の出力トルクを推定し、この出力トルクを制御する技術が記載されている。
【特許文献１】特開２００２−１０２２４８号公報
【特許文献２】特開平１１−３３３７７６号公報
【特許文献３】特開２００２−１７８２８１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、モータ電流値と実際の把持力とはヒステリシスな関係を持っており、必ずしも１対１に対応していない。このため、モータ電流値によるトルク制御では十分とはいえない。上述した特許文献３には、オブザーバを構築して摩擦や慣性の影響を考慮することが記載されているが、ここで求められたトルク推定量と実際の把持力は必ずしも１対１に対応するものではない。
【０００５】
また、上述した特許文献２、３のいずれも、把持機構とそれを駆動するモータが１対１に対応しており、比較的良好な制御性能を示すことが考えられるが、把持機構以外にも自由度を持つマニピュレータであって、把持動作に関与するモータが他の自由度の動作にも寄与しているような干渉系機構の場合であると、よりいっそう電流値と実際の作用力との相関は複雑となる。そういったことから、電流値によるトルク制御で把持力を制御すると、実際の把持力が必ずしも所望の値になるとは限らないこと、力制御の挙動が位置（角度）の偏差挙動を引き起こすことなどといった問題がある。
【０００６】
本発明は、把持力等の作用力を安定に精度よく制御可能なマニピュレータシステムおよびその制御方法を提供するものである。
【課題を解決するための手段】
【０００７】
本発明の一態様によれば、アクチュエータと、
前記アクチュエータの動力に基づいて対象物に対して力を作用する作用部と、
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、
前記アクチュエータを制御する制御部と、を備え、
前記制御部は、
角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号を生成するオブザーバ信号生成手段と、
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で推定された作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステムが提供される。
【０００８】
また、本発明の一態様によれば、複数のアクチュエータと、
前記複数のアクチュエータの動力に基づいて対象物に対して力を作用する姿勢軸と、前記対象物をそれぞれ異なる方向に動作させる複数の動作軸と、を有する作用部と、
可撓性部材を有し、前記アクチュエータの動力を前記作用部に伝達する動力伝達機構と、
前記アクチュエータを制御する制御部と、を備え、
前記複数のアクチュエータは、前記姿勢軸および前記複数の動作軸の駆動に影響を与える干渉系アクチュエータであり、
前記制御部は、
前記複数のアクチュエータのそれぞれごとに設けられ、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れずに、前記作用部の作用力開始角度を推定するためのオブザーバ信号候補をそれぞれ生成する複数のオブザーバ信号候補生成手段と、
前記複数のアクチュエータの各駆動軸の動作速度に応じて、前記複数の作用力開始角度推定手段から出力されたオブザーバ信号のいずれか一つを選択するオブザーバ信号選択手段と、
前記オブザーバ信号の信号レベルを所定の基準値と比較することにより、作用力開始角度を推定する作用力開始角度推定手段と、
予め設定された前記作用部の作用力に対応する姿勢軸角度と前記作用力開始角度推定手段で選択されたオブザーバ信号に対応する作用力開始角度とに基づいて、姿勢軸限界角度を検出する限界角度検出手段と、
前記作用部の姿勢軸角度を前記姿勢軸限界角度まで角度制御する角度制御手段と、を有することを特徴とするマニピュレータシステムが提供される。
【発明の効果】
【０００９】
本発明によれば、把持力等の作用力を安定に精度よく制御可能なマニピュレータシステムおよびその制御方法を提供できる。
【発明を実施するための最良の形態】
【００１０】
以下、図面を参照しながら、本発明の一実施形態について説明する。以下では、主に医療用マニピュレータの例を説明する。
【００１１】
図１は医療用マニピュレータシステムの外観図である。図１のシステムは、マスタスレーブ方式で動作するマニピュレータ本体１と、マニピュレータ本体１の駆動を制御する制御装置２とを備えている。この他、図１では省略しているが、マニピュレータ本体１の動作状態を表示する動作状態表示器を図１のシステムに設けてもよい。
【００１２】
マニピュレータ本体１は、操作者（術者）が操作を指令する操作指令部３と、その操作指令に応じて対象物に所定量の力を作用する作用部４と、操作指令部３と作用部４とを一体的に接続する連結部５と、作用部４の駆動力を発生する駆動部６とを有する。
【００１３】
操作指令部３は、作用部４の作用量と作用方向を指示する少なくとも一つの指令デバイス（ダイヤル等）を有し、各指令デバイスには、その指令値を読み取るセンサが取り付けられている。操作者が操作指令部３を操作すると、その指令情報は逐次、制御装置２に送信される。
【００１４】
作用部４は、例えば、人体の患部に種々の処置を施すものである。作用部４は、患部に対して処置を施すための少なくとも一つの自由度を有し、先端の姿勢を可変させたり、先端に取り付けられたグリッパの開閉角度を可変させることが可能である。作用部４の作用力の変更は操作指令部３の指令によって行われる。
【００１５】
マニピュレータ本体１の作用部４と連結部５は作業部７を構成し、操作指令部３と駆動部６は操作部８を構成しており、作業部７と操作部８は互いに着脱可能な構造になっている。
【００１６】
図２は作業部７と操作部８を分離した状態を示す図である。図２に示すように、作業部７内の連結部５の一端側には、複数の出力軸プーリ１１ａ，１１ｂ，１１ｃが設けられており、各プーリにはそれぞれワイヤ１２ａ，１２ｂ，１２ｃが巻掛けられている。連結部５は中空であり、作用部４側が開口になっているため、ワイヤ１２ａ，１２ｂ，１２ｃは、連結部５の内部を通過して作用部４側まで延在している。なお、ワイヤ１２ａ，１２ｂ，１２ｃの一部にロッドリンク等を用いて動力を伝達してもよい。
【００１７】
作用部４には、図２では省略しているが、ワイヤ１２ａ，１２ｂ，１２ｃを巻掛けるプーリ、歯車、リンク等が設けられている。これにより、動力が作業部７内の出力軸プーリ１１ａ，１１ｂ，１１ｃとワイヤ１２ａ，１２ｂ，１２ｃを介して作用部４まで伝達される。各出力軸プーリ１１ａ，１１ｂ，１１ｃには、駆動部６からの動力を受け取るための動力受取部１３ａ，１３ｂ，１３ｃが接続されている。
【００１８】
駆動部６は、複数の（図２では３つの）モータ１４ａ，１４ｂ，１４ｃを有する。これらモータ１４ａ，１４ｂ，１４ｃの動力は、それぞれ不図示の減速機を介して、カップリングからなる動力伝達部１５ａ，１５ｂ，１５ｃに伝達される。各モータ１４ａ，１４ｂ，１４ｃには、不図示のエンコーダが装備されており、モータ１４ａ，１４ｂ，１４ｃの回転角度を計測することができる。各エンコーダの計測信号は制御装置２に伝送される。
【００１９】
駆動部６内の動力伝達部１５ａ，１５ｂ，１５ｃの端部には凹部が形成され、連結部５内の動力受取部１３ａ，１３ｂ，１３ｃの端部には凸部が形成されている。これら凹部と凸部は互いに係合され、これにより動力伝達部１５ａ，１５ｂ，１５ｃの動力が動力受取部１３ａ，１３ｂ，１３ｃに伝達される。動力伝達部１５ａ，１５ｂ，１５ｃと動力受取部１３ａ，１３ｂ，１３ｃの位相が合わないと、凹部と凸部が係合されないようにすることで、原点姿勢以外では操作部８と作業部７との結合を防止している。
【００２０】
図２では、モータやスイッチ、センサなどの電子機器を操作部８側に配置し、作業部７側には機構部品を配置している。これにより、作業部７全体を洗浄でき、洗浄性と保守性の向上が図れる。すなわち、図２では、汚染度合いと洗浄方法の違いを考慮に入れて、作業部７と操作部８に分離している。
【００２１】
操作部８は、連結部５の基端側に接続されたブラケット８１と、このブラケット８１に対して回動自在に取り付けられた円柱状の操作桿８２と、この操作桿８２に取り付けられた操作デバイスとを有する。操作桿８２の操作デバイスは作用部４の姿勢決め等を指示する機能を有する。
【００２２】
操作デバイスは、図２に示すように、横方向ダイヤル８３と、縦方向ダイヤル８４と、トリガ８５と、操作モード切替スイッチ８６とを有する。図２の操作デバイスを用いて図３のような把持動作を行う作業部７を操作する例について説明する。図３において、横方向ダイヤル８３がヨー軸２９の動作を指令し、縦方向ダイヤル８４がロール軸４６の動作を指令し、トリガ８５がグリッパ軸θｇの動作を指令する。操作モード切替スイッチ８６は、マスタスレーブ動作の開始・終了、初期姿勢復帰動作等の操作モードの切り替えを行うスイッチである。
【００２３】
横方向ダイヤル８３、縦方向ダイヤル８４及びトリガ８５には、その動作量を検出するセンサがそれぞれ組み込まれており、センサ信号は制御装置２に送られ、その内部の演算部で処理される。横方向ダイヤル８３と縦方向ダイヤル８４に組み込まれるセンサは、操作桿８２との相対的な回動量を検出するものであり、例えばポテンショメータやエンコーダを適用することができる。
【００２４】
トリガ８５は、操作桿８２に対して並進動作が可能であり、その動作量により、作業部７のグリッパ角度を指令する。トリガ８５には並進動作位置を検出するセンサが設けられている。
【００２５】
図３はマニピュレータ本体１の作用部４の構造を示す斜視図、図４は作用部４の詳細構造を示す分解斜視図である。図３および図４に示すように、作用部４は、処置対象に対して処置を行う第１の作用部材３２５と第２の作用部材３２６を先端に有する連結部材４１と、連結部材４１の長手方向に沿った方向に対して直交する第１の回動軸部材１６と、第１の回動軸部材１６の回動軸２９回りの方向に回動自在に支持されて第１の回動軸部材１６に対して直交する主軸部３２０ｂを有する主軸部材３２０と、第１の回動軸部材１６の回動軸２９回りの方向に回動自在に支持される第１の歯車１１７と、第１の回動軸部材１６の回動軸２９回りの方向に回動自在に支持される第２の歯車２１８とを有する。
【００２６】
主軸部材３２０にはプーリ３２０ａが結合され、第１の歯車１１７にはプーリ１１７ａが結合され、第２の歯車２１８にはプーリ２１８ａが結合されている。プーリ３２０ａ、プーリ１１７ａ、プーリ２１８ａにはそれぞれ第１ワイヤ５０ａ、第２ワイヤ５０ｂ、第３ワイヤ５０ｃが巻掛けられている。
【００２７】
また、作用部４は、第１の歯車１１７と直交して噛み合い、主軸部３２０ｂ回りの方向に回動自在に支持される第３の歯車３２１ａと、第２の歯車２１８と直交して噛み合い、主軸部３２０ｂ回りの方向に回動自在に支持される第４の歯車３２２ａと、第３の歯車３２１ａとともに同軸に回動する第２の回動軸部材３２１ｂと、第４の歯車３２２ａとともに同軸に回動する第５の歯車３２２ｂとを有する。
【００２８】
また、作用部４は、主軸４６周りに回動する第２の回動軸部材３２１ｂに直交して第２の回動軸部材３２１ｂの回動とともに主軸４６回りに回動し、第１の回動軸部材１６の回動軸２９に対して捩れの位置から平行な位置に回動自在に配置される第３の回動軸部材３２３ａを有する。
【００２９】
また、作用部４は、第３の回動軸３２３ａ回りの方向に回動自在に支持され、第５の歯車３２２ｂと直交して噛み合う第６の歯車３２３と、第２の回動軸３２１ｂ 回りの方向に回動するとともに、第６の歯車３２３とともに第３の回動軸３２３ａ回りの方向に回動する第１の作用部材３２５と、第３の歯車３２１ａおよび第２の回動軸３２１ｂとともに回動する第２の作用部材３２６とを有する。第１の作用部材３２５と第２の作用部材３２６は、一対の作用部材（グリッパ）を構成している。第１の作用部材３２５が第３の回動軸３２３ａ回りの方向に回動することで、第２の作用部材３２６に対して、相対的に開閉可能な把持機構を構成している。
【００３０】
このように、本実施形態の作用部４は、回動軸２９（回動軸部材１６）と主軸４６（主軸部材３２１ｂ）を回動中心とした２自由度で姿勢変更が可能な開閉機能（把持機構：グリッパ）を構成する第１および第２の作用部材３２５、３２６を有する。
【００３１】
カバー３２７は、歯車の露出を最小限にとどめるためのものであり、カバー固定ピン３２８により、第２の作用部材３２６の下部にて固定されている。カバー３２７は、第２の作用部材３２６とともに、第２の回動軸部材３２１ｂ（主軸４６）回りの方向に回動する。固定ナット３２９は、主軸部材３２０の先端部のねじ部に固定されており、第３の歯車３２１ａおよび第２の回動軸部材３２１ｂの主軸部３２０ｂの軸方向の位置を拘束するために設けられる。固定ナット３３０は、第３の回動軸３２３ａおよび第６の歯車３２３の第３の回動軸３２３ａの軸方向の位置を拘束するために設けられる。
【００３２】
図５は制御装置２とその周辺の制御系の詳細構成の一例を示すブロック図である。制御装置２は、操作指令部３による操作指令に応じた力を作用部４が対象物に作用するようにモータ１４ａ，１４ｂ，１４ｃを動作させる制御（第１の制御手段）と、この制御の後に、作用部４による作用力が低下しない範囲で作用部４が対象物に対して所定量よりも小さい力を逆に作用するようにモータ１４ａ，１４ｂ，１４ｃを動作させる制御（第２の制御手段）とを行う。
【００３３】
より具体的には、図５の制御装置２は、電源部２１と、演算部２２と、モータ駆動回路部２３と、安全保護装置２４と、動作状態提示器２５と、指令入力部２６とを有する。
【００３４】
電源部２１は、外部電源２７から不図示の変圧器と整流器を介して、演算部２２とモータ駆動回路部２３に必要な電力を供給する。
【００３５】
マニピュレータ本体１内の操作部８は、作業部７を駆動するモータ１４ａ，１４ｂ，１４ｃと、モータ１４ａ，１４ｂ，１４ｃの回転角度を検出する角度検出器１６と、操作指令部３の姿勢角度（操作量）を検出する角度検出器１７とを有する。
【００３６】
演算部２２は、不図示のＣＰＵと、記憶装置と、論理回路と、ＩＯインターフェイスとを有し、角度検出器１７で検出した操作指令部３の操作量に基づいてモータ１４ａ，１４ｂ，１４ｃの制御目標値を生成する機能と、角度検出器１６で検出したモータ１４ａ，１４ｂ，１４ｃの回転角度とモータ１４ａ，１４ｂ，１４ｃの制御目標値との偏差を検出して、その偏差を解消するためのモータ指令入力を算出する機能とを有する。
【００３７】
また、演算部２２は、医療用マニピュレータに装備された不図示の認識手段の信号を読み取るとともに、各種スイッチ２８の信号入力を監視して、予め決められたプログラムに従って制御演算を行う。
【００３８】
モータ駆動回路部２３は、演算部２２からの指令入力に従ってモータ１４ａ，１４ｂ，１４ｃに電力を供給する回路であり、指令入力に比例した電流をモータ１４ａ，１４ｂ，１４ｃに出力する電流指令制御を行う。また、モータ駆動回路部２３は、各モータ１４ａ，１４ｂ，１４ｃの電流値を計測する。
【００３９】
安全保護装置２４は、演算部２２の演算周期異常、モータ駆動回路異常、緊急停止指令などの異常事態に備えて、マニピュレータ本体１への電力を遮断し、その動作を直ちに停止させる機能を有する。
【００４０】
動作状態提示器２５は、演算部２２の指示に応じて、マニピュレータ本体１の動作状態を文字情報や画像情報にて提示する。指令入力部２６は、各種のスイッチ等の操作部材からなり、指令入力部２６の操作情報は演算部２２に送られる。指令入力部２６は、マニピュレータ本体１の動作状態や電源の切換などを指令する。
【００４１】
図６は図５の演算部２２が実行するプログラムの一例を示すフローチャートである。演算部２２は、図６のフローチャートを予め決められた制御周期に従って繰り返し実行する。
【００４２】
まず、操作指令部３の角度検出器１７の出力と駆動部６内のモータ１４ａ，１４ｂ，１４ｃの角度検出器１６の出力を読み取る（ステップＳ１１）。また、操作指令部３や制御装置２に装備されたスイッチ等の操作情報を演算部２２で認識し（ステップＳ１２）、その認識結果をもとにマニピュレータ本体１の動作モードを決定する（ステップＳ１３）。ここで、動作モードとは、予め決められた動作を自動で実行する自動モードや、操作指令部３の操作に従って作業部７を動作させるマスタスレーブ動作モードを指す。動作方式とは、両動作モードの切り換えにおいて安全に動作をつなげるための、加速動作、減速動作、等速動作、停止動作などの動作方式を指す。
【００４３】
次に、判定された動作モードに従って動作方式の判別とモータ１４ａ，１４ｂ，１４ｃの目標値を生成する（ステップＳ１４）。生成された制御目標値と先に読み取った角度検出器１６の検出値とからＰＩＤ制御などの制御演算を行ってモータ出力を算出し、モータ駆動回路部２３に出力する（ステップＳ１５）。
【００４４】
なお、上記ステップＳ１４では、後述する図９に示すように、操作デバイスから得られた値をもとに算出されたヨー軸目標角度θｙ、ロール軸目標角度θｒ、グリッパ軸目標角度θｇを第１モータ軸角度θ１、第２モータ軸角度θ２、モータ１４ｃ軸角度θ３に変換するための逆キネマティック演算を行って目標値を生成する。
【００４５】
次に、定義された種々の条件とセンサなどで読み取った状態とを比較し、状態判別を行う（ステップＳ１６）。判別された結果に基づき、制御装置２に装備されたランプや動作状態提示器２５に出力を行う（ステップＳ１７）。
【００４６】
以下、作用部４のグリッパによる把持動作について説明する。説明の簡略化のために、図３のマニピュレータ本体１の先端部分の歯車の歯数がすべて等しいとすると、モータ１４ａ，１４ｂ，１４ｃの出力軸角度θ１、θ２、θ３と先端部の姿勢軸角度θｙ、θｒ、θｇとの関係は、（１）式で表すことができる。
【数１】
2010076012.tif 000003
【００４７】
上述した（１）式における３×３行列は、逆キネマティック行列Ｋ-1と呼ばれるものである。
【００４８】
グリッパ軸の把持動作のみについて考えると、他の姿勢軸を動作させない場合は、θｙ＝θｒ＝０であるから、θ３＝−θｇとなる。つまり、モータ１４ａ、１４ｂ、１４ｃをθｄ＝［０、０、−θｇ］Ｔと制御することでグリッパ軸をθｇだけ動作させることができる。グリッパ軸角度は、ちょうど閉じた状態を０度、開く方向をプラス方向とし、グリッパ軸をマイナス方向に動作させるということは、より閉じる方向に力をかけるようにモータ１４ｃを制御することを意味する。
【００４９】
さて、実際に把持力がどのように変化するかを評価するために、グリッパの片側である第２の作用部材３２６の把持面に感圧センサを貼り付け、その出力を測定するシステムを構築した。上述したように、モータ駆動回路部５５は電流制御方式を採用している。また、グリッパ軸の角度制御はＰＤ制御を基本として構築している。すなわち、図５の制御装置２は、速度のＰＩ制御、位置のＰＤ制御および速度フィードフォワード制御を組み合わせた制御を行う。
【００５０】
図７は制御装置２によって制御される把持角度と実際の把持力との関係を示す図である。図示のように、把持角度と把持力とは１次相関関係になる。位置のＩ制御が含まれると、把持角度と把持力との関係は１次相関とはならないため、把持力調整には採用しない。
【００５１】
図７において、領域１は、把持角度が小さくて、ワイヤ１５ｃの伸び量と把持力が比例する領域である。この領域１では、モータ１４ｃは所望の目標角度に適度に制御される。領域２は、ワイヤ１５ｃが伸びきってモータ１４ｃのトルクと把持力が比例する領域であり、モータ１４ｃは所望の目標角度からずれた角度に制御された状態を維持する。
【００５２】
このように、領域１ではワイヤ１５ｃの伸びと把持力は比例し、領域２では位置のＩ制御を含まない位置偏差が把持力と比例する。どちらの領域であっても、把持角度と把持力は１次相関の関係にある。制御状態が領域１と２のどちらであるかは、モータ１４ｃの制御偏差の大きさにより確認することができる。
【００５３】
上述したように、グリッパ軸角度は、閉じた角度を０度と定義している。例えば、何も物体を把持せずにグリッパ４を開き角４５度から閉じ角３５度すなわちθｇを４５度から−３５度まで動作するようにモータ１４ｃを制御した場合、実際のグリッパは４５度から０度まで動作し、その後グリッパ角度が幾何的拘束を受けて実際にはそれ以上閉じる方向には動かないが、モータ１４ｃはよりいっそう閉じる方向にグリッパ軸換算で３５度だけ制御される。このとき、把持角度は３５度となる。
【００５４】
さて、図７に示すように、把持角度と実際の把持力とが１次相関関係にあるシステムを構築できた場合には、把持角度を目標に制御すれば、把持力を制御することができるといえる。
【００５５】
ここで、把持角度について説明を加えておく。把持角度とは把持する方向に動作させたときの最大動作量である。先の説明で、４５度から−３５度まで動作させた場合は、把持角度が３５度であると述べたが、４５度から−４５度まで動作させた後に−３５度まで戻した場合に、把持角度が３５度であるとは言わない。
【００５６】
把持角度で把持力を制御する場合、すなわち角度制御にて把持力を制御する場合には、実際に物体を把持し始めたグリッパ角度を認識し、その角度を基点に把持角度分だけ制御する必要がある。グリッパ４に力センサがあれば直接物体を把持したことを認識でき、角度センサがあれば実際のグリッパ角度が停止あるいは目標値との偏差の発生で物体を把持したことを推定できるが、本実施形態は、センサレスで把持力制御を行うことを念頭に置いており、センサを用いた制御は想定外である。
【００５７】
そこで、本実施形態では、把持開始推定オブザーバを用いて把持開始角度を推定する。この把持開始推定オブザーバは、図５の制御装置２の内部、例えば演算部２２の内部に設けられる。
【００５８】
図８は把持開始推定オブザーバを有する制御装置２が行う処理動作の一例を示す制御システム図である。図８に示すように、制御装置２は、把持開始推定オブザーバ（オブザーバ信号生成手段）５１と、把持角度決定部５２と、逆キネマティック行列演算部５３と、ＰＩＤ制御部５４と、モータ駆動部５５と、目標把持角度設定部５６と、最閉角度決定部５７と、操作量更新部５８とを有する。
【００５９】
把持開始推定オブザーバ５１は、後で詳述するように、把持開始角度を推定するための把持開始角度推定信号（把持開始角度推定信号）ｆａを生成する。
【００６０】
把持角度決定部５２は、把持開始角度推定信号ｆａと操作デバイスの操作量に基づいて、ヨー軸およびロール軸に関する参照目標値θａ（ｙ，ｒ）と、グリッパ軸に関する参照目標値θａ（ｇ）を生成する。
【００６１】
逆キネマティック行列演算部５３は、上述した（１）式に基づいて、モータ１４ａ，１４ｂ，１４ｃの出力軸角度θｄを算出する。上述した（１）式を簡易的に表すと、以下の（２）式で表される。θｄ＝［θ１，θ２，θ３］Ｔ、θａ＝［θｙ，θｒ，θｇ］Ｔである。
【００６２】
θｄ＝Ｋ-1θａ    …（２）
【００６３】
図８のＰＩＤ制御部５４は、逆キネマティック行列演算部５３の出力値（モータの目標値）θｄとモータの実測値θｍとの偏差をフィードバック制御する。より具体的には、ＰＩＤ制御部５４は、偏差が小さくなるように、モータ駆動部５５に供給する電圧Ｖinを制御する。
【００６４】
目標把持角度設定部５６は、例えば操作デバイスとは別個に設けられるボリューム等の値を読み取って、図７に示した把持角度と把持力の１次相関関係に基づいて、所望の把持力を得るための設定把持角度θｈを設定する。
【００６５】
最閉角度決定部５７は、以下の（３）式に基づいて、把持開始角度θｔから設定把持角度θｈを減じることにより、グリッパ軸の最閉角度θｃを算出する。
【００６６】
θｃ＝θｔ−θｈ    …（３）
【００６７】
操作量更新部５８は、最閉角度決定部５７からの決定結果に応じて、グリッパ軸目標値θｇを逆キネマティック演算部５３に送り、グリッパ軸目標値θｇを更新する。
【００６８】
把持開始推定オブザーバ５１は、モータ角速度ωｍを検出する微分回路６１と、モータ加速度αｍを検出する微分回路６２と、クーロン摩擦項算出部６３と、粘性摩擦項算出部６４と、慣性項算出部６５と、粘性摩擦項と慣性項を加算する加算器６６と、クーロン摩擦項を考慮に入れるか否かを切替える第１の切替部ＳＷ１と、モータ電流から加算器６６の出力と第１の切替部ＳＷ１の出力とを減じる減算器６７と、この減算器６７の出力に含まれる高周波ノイズを除去するローパスフィルタ（ＬＰＦ）６８と、各モータに応じたＬＰＦ６８の出力のうちゼロでないものを一つ選択する第２の切替部ＳＷ２とを有する。
【００６９】
把持開始推定オブザーバ５１の内部構成のうち、第２の切替部ＳＷ２を除く部分は、各モータ１４ａ，１４ｂ，１４ｃのそれぞれごとに設けられている。図９は把持開始推定オブザーバ５１のより詳細な内部構成の一例を示すブロック図である。図示のように、モータ１４ａ，１４ｂ，１４ｃのそれぞれごとに、微分回路６１からローパスフィルタ６８までの部分（オブザーバ信号候補生成手段）５１ａ，５１ｂ，５１ｃが設けられており、各ローパスフィルタ６８の出力のうちいずれか一つが第２の切替部ＳＷ２で選択されて、把持開始角度推定信号ｆａとして出力される。この第２の切替部ＳＷ２はオブザーバ信号選択手段に対応する。
【００７０】
なお、１軸方向しか制御を行わないマニピュレータシステムの場合は、第２の切替部ＳＷ２は不要となる。
【００７１】
以下、把持開始推定オブザーバ５１の処理動作について説明する。電動モータの場合、モータ電流はモータ出力トルクに比例するため、モータ出力軸からグリッパ軸までの機構パラメータに基づいて、モータ出力軸とグリッパ軸の軸間の関係が求まり、ひいてはモータ電流とグリッパ軸のトルクの関係が求まる。把持力は、グリッパ軸のトルクと、グリッパ軸と把持点との距離とで算出できる。
【００７２】
モータ電流やモータ角度等の情報を用いた外乱オブザーバを利用することで、把持力を推定することができる。ここで、外乱とは、モータ電流から機構パラメータを換算して差し引いた残りの値である。把持機構を備えている場合は、物体を把持することにより生じる外力が把持力であると想定している。
【００７３】
図８の把持開始推定オブザーバ５１内の慣性項算出部６５は、モータ加速度αに係数Ｊｎを乗じた項を算出する。粘性摩擦項算出部６４は、モータ速度ωに粘性摩擦係数ｆｖを乗じた項を算出する。クーロン摩擦項算出部６３は、モータ速度ωの符号関数にクーロン摩擦係数ｆｃを乗じた項を算出する。これら３つの算出部６３〜６５をモータ電流ｉから減じることにより、グリッパ軸のトルク、すなわち把持力に変換した出力を得ることができる。
【００７４】
なお、回転については、回転速度を角速度、回転加速度を角加速度と呼ぶのが正式であるが、機構系においては、回転や並進を複合したものである場合を踏まえて、ここでは説明上区別していない。同じ理由で、位置制御と角度制御は同義である。
【００７５】
図８の慣性係数Ｊｎ、粘性摩擦係数ｆｖ、クーロン摩擦係数ｆｃは設計値や実験により求められる値である。慣性項を算出するにあたり、加速度情報を得るために離散的な角度情報の差分処理を二度行うとノイズが増幅されることがある。そこで、特許文献３の図２に示す等価的なブロック図のようなものを用いて慣性項を求めてもよい。
【００７６】
図１０は図８の把持開始推定オブザーバ５１の特性を示す図である。図１０（ａ）はグリッパ４の開動作→ヨー軸動作→ロール軸動作→グリッパ４の閉動作→グリッパ４の開動作を連続的に行った場合のグリッパ軸角度θｇ、ヨー軸角度θｙ、ロール軸角度θｒ、モータ１４ｃの軸角度θ３が変化する様子を示している。
【００７７】
図１０（ｂ）は図８の把持開始推定オブザーバ５１内に第１および第２の切替部ＳＷ１，ＳＷ２を設けない場合に生成されたオブザーバ信号ｆｅの変化する様子を示す図である。図１０（ｃ）は把持力センサの出力を示す図である。
【００７８】
オブザーバ信号ｆｅは、モータ電流から、慣性係数Ｊｎ、粘性摩擦係数ｆｖおよびクーロン摩擦係数ｆｃの３つの項を減じることにより生成されるが、このオブザーバ信号ｆｅを用いて推定した把持力を、実際に測定した把持力と比較すると、図１０（ｂ）に示すように、オブザーバ信号ｆｅを用いて推定した把持力（電流換算値）は振動性の特徴を持ち、動作速度変化点付近にはスパイク状の信号も現れる。これは、伝達機構系において発生するクーロン摩擦の非線形性や、動作角度や位置による摩擦係数の微小変化によるものであり、実測した把持力との差異が大きくなる。
【００７９】
このように、クーロン摩擦等を考慮に入れて生成したオブザーバ信号ｆｅでは、作業部７が微細な構造で、把持力が相当な大きさの場合には、ノイズ成分と小さな把持力とを区別するのは困難であり、把持開始点を正確に検出することができない。例えば、ノイズの大きさを考慮に入れて、判定既定値をプラス側（例えばｆ０）に設定しておき、判定既定値より推定把持力が大きくなった点を把持開始と推定することも可能だが、この場合、小さな把持力を認識できない。カットオフ周波数がより低いフィルタを使用してノイズ成分を除去することもできるが、生成された信号に位相遅れが生じてしまい、把持開始時点Ｔ２を認識するのに時間的な遅れが生じ、その結果、実際の把持開始角度と推定角度に大きなずれが生じてしまう。
【００８０】
このような事情で、本実施形態では、把持開始の認識に遅れが生じずにノイズ成分と区別できるように、把持開始推定オブザーバ５１内に第１の切替部ＳＷ１を設けている。この第１の切替部ＳＷ１は、グリッパ軸動作時にはオフして、クーロン摩擦項を考慮に入れずに把持開始推定信号ｆａを生成し、グリッパ軸動作時以外はオンして、クーロン摩擦項を考慮に入れて同信号ｆａを生成する。クーロン摩擦項を考慮に入れないとは、角速度および加速度の少なくとも一方の大きさには依存しないが方向には依存する項を考慮に入れないことと等価である。第１の切替部ＳＷ１のように、クーロン摩擦項を計算した後にその出力を遮断する代わりに、グリッパ軸動作時には、クーロン摩擦係数ｆｃをゼロとして、クーロン摩擦項の計算結果がゼロになるようにしてもよい。
【００８１】
図８の把持開始推定オブザーバ５１を正常に動作させるには、把持開始のタイミングを精度よく認識しなければならない。把持開始の認識は、グリッパ４を閉じるとき、すなわち、グリッパ軸速度ωｇが負の場合に起こるため、第１の切替部ＳＷ１は、グリッパ軸速度ωｇが負になったときにオフするように切替制御される。なお、グリッパ軸速度は、図８の把持角度決定部５２の出力である参照目標値θａ（ｇ）を微分処理することにより得られる。
【００８２】
図１０（ｄ）は把持開始時に第１の切替部ＳＷ１をオフした状態で得られた把持開始推定信号ｆａの波形図である。図１０（ｄ）の把持開始推定信号ｆａの波形を見ればわかるように、クーロン摩擦項を考慮に入れないことから、グリッパ４を閉じ始めた時点Ｔ０以降での信号ｆａの信号レベルは図１０（ｂ）の信号ｆｅの信号レベルよりも大きい。そして、信号ｆａが所定のしきい値ｆ１を超えた時点Ｔ１で、把持開始角度θｔに達したと判断される。この時刻Ｔ１は、図１０（ｂ）で把持開始を認識する時刻Ｔ２よりも前であり、把持開始を迅速かつ正確に推定できることがわかる。
【００８３】
把持開始角度θｔが検出されると、上述した（３）式により、グリッパ軸の最閉角度θｃを求めることができる。
【００８４】
このように、本実施形態では、所望の把持力をまず設定して、角度（位置）制御でグリッパ軸を閉じる動作を行い、把持力開始推定オブザーバの出力にて把持開始角度を認識したら、次にグリッパ軸の最閉角度を設定し、次にグリッパ軸角度をグリッパ軸の最閉角度まで角度（位置）制御する。
【００８５】
なお、把持力の設定または把持角度の設定は、制御装置や操作指令部３に付属のボリューム等を用いて設定してもよいし、作業部７の種類に応じて予め決められた設定値を付与してもよい。把持中にグリッパ軸の最閉角度を調整すると、把持しながら把持力を微調整することが可能となる。
【００８６】
図８の把持開始推定オブザーバ５１では、慣性項や粘性項を考慮に入れているが、例えばマニピュレータが微細構造で慣性の影響を無視できる場合には、慣性係数Ｊｎをゼロにしてもよい。粘性摩擦項についても必要に応じて無視してもよい。このように、把持開始推定オブザーバ５１の具体的構成は、適宜変更可能である。
【００８７】
図８の最閉角度決定部５７は、例えば図１１のフローチャートに示す処理手順に従ってグリッパ軸の最閉角度を設定する。この図１１のフローチャートは例えば図６のステップＳ１４の中で行われるものであり、作用フラグ決定手段に対応する。
【００８８】
まず、図８の把持開始推定オブザーバ５１から出力される把持開始推定信号ｆａを取得する（ステップＳ２１）。次に、把持フラグ（作用フラグ情報）の状態を確認する（ステップＳ２２）。ここで、把持フラグとは、グリッパが対象物を把持しているか否かを示すフラグ情報であり、把持している場合にオンし、把持していない場合にオフする。
【００８９】
把持フラグがオフと判断された場合は、グリッパ軸の目標角度θｇがグリッパ軸の最大角度θｐより小さいか否かを判定する（ステップＳ２３）。θｇ＜θｐの場合は、グリッパ速度ωｇが負か否かを判定する（ステップＳ２４）。
【００９０】
ωｇが負であれば、グリッパ軸が閉じる方向に動作していると判断できる。そこで、次に、把持開始推定信号ｆａが所定のしきい値ｆ１以上か否かを判定する（ステップＳ２５）。ｆａ≧ｆ１であれば、図１０（ｄ）の時刻ｔ１と同様に把持を開始したと判断して、把持フラグをオンする（ステップＳ２６）。そして、この時点でのグリッパ軸の最閉角度θｇを把持開始角度θｔに設定する（ステップＳ２７）。次に、上述した（３）式に従って、把持開始角度θｔから設定把持角度θｈを減じてグリッパ最閉角度θｃを算出する（ステップＳ２８）。上述したステップＳ２５〜Ｓ２７の処理が作用力開始角度推定手段に対応し、ステップＳ２８の処理が限界角度検出手段に対応する。
【００９１】
上述したステップＳ２３〜Ｓ２５のいずれかの判定結果がＮＯの場合には、把持が開始されたとは判断されない。
【００９２】
一方、ステップＳ２２で把持フラグがオンであると判定された場合は、グリッパ軸の目標角度θｇがグリッパ軸の最大角度θｐに等しいか否かを判定する（ステップＳ２９）。等しくない場合には、目標角度θｇが把持開始角度θｔより大きいか否かを判定する（ステップＳ３０）。
【００９３】
ステップＳ２９でθｇがθｐに等しいと判定された場合、またはステップＳ３０でθｇ＞θｔと判定された場合は、把持動作は終了したと判定して、把持フラグをオフする（ステップＳ３１）。次に、グリッパ軸の最小角度θｎを把持開始角度θｔとして設定する（ステップＳ３２３）。ここで、把持開始角度θｔをグリッパ軸の最小角度に設定するのは、把持開始角度をリセットするためである。把持動作が終了したのに把持開始角度をリセットしないとすると、前回把持した対象物よりも厚さが薄いものを把持するまでグリッパが動作しないといった不具合が生じうるためである。このとき、把持開始角度をゼロに設定してもよいが、後述するように初期誤差が考えられるため、動作可能範囲での最小角度に設定している。
【００９４】
ステップＳ２９でθｇがθｐに等しくないと判定されるか、またはステップＳ３０でθｇ≦θｔと判定された場合には、まだ把持動作を行っていると判断して把持フラグをオンのまま維持する。この場合は、グリッパ軸の目標角度θｇが最閉角度θｔに達するまで角度制御が行われる。この角度制御は制御装置２が行うものであり、角度制御手段に対応する。
【００９５】
図３に示した作業部７を駆動する駆動部６は、干渉系の機構を有する。モータ１４ａ，１４ｂ，１４ｃの出力軸角度θｄ＝［θ１、θ２、θ３］Ｔと作業部７の先端部の姿勢軸θａ＝［θｙ、θｒ、θｇ］Ｔとの関係は、上述した（１）式および（２）式で表される。これらの式では、マニピュレータの先端部分の歯車の歯数がすべて等しいと仮定している。
【００９６】
グリッパ軸の把持動作のみについて考えると、他の姿勢軸を動作させない場合はθｙ＝θｒ＝０であるため、θ３＝−θｇとなる。すなわち、モータをθｄ＝［０、０、−θｇ］Ｔと制御することで、グリッパ軸をθｇだけ動作させることができる。
【００９７】
グリッパ軸角度は、閉じた状態を０度、開く方向をプラス方向としている。したがって、グリッパ軸をマイナス方向に動作させるということは、より閉じる方向に力をかけるようにモータを制御することを意味する。
【００９８】
グリッパ軸を動作させる場合、把持開始推定信号ｆａはモータ１４ｃの電流値だけで生成できる。モータ１４ｃの電流値の波形は図１０（ｂ）のようになる。モータ１４ｃの電流値だけでグリッパ軸の動作が決まるのは、１自由度しか持たないグリッパを備えたマニピュレータや、２以上の自由度を持っていてもグリッパ軸を駆動するアクチュエータが他の姿勢軸と干渉しない非干渉構造の場合でも同様である。
【００９９】
ところが、例えば図３の作業部７を備えたマニピュレータなどのように、他の姿勢軸と同時にグリッパ軸を動作させる場合は、モータ１４ｃの電流値だけで把持力が決まるとは限らない。
【０１００】
そこで、干渉系機構を備えたマニピュレータを念頭に置いて、トルク干渉を考慮に入れて、把持力とモータトルクの関係を求めてみる。
【０１０１】
仮想仕事の原理により、モータ１４ａ，１４ｂ，１４ｃのトルクτ１、τ２、τ３と作業部７の姿勢軸であるヨー軸回り、ロール軸回り、グリッパ軸回りのトルクτｙ、τｒ、τｇとの関係は、以下の（４）式で表される。
【数２】
2010076012.tif 000004
【０１０２】
把持力が出力されている状態とは、トルクτｇが出力されている状態であり、他の姿勢軸の出力をゼロにすれば、上記（４）式から、各モータ１４ａ，１４ｂ，１４ｃのトルクτ１、τ２、τ３は、以下の（５）式で求められる。
【数３】
2010076012.tif 000005
【０１０３】
（５）式を見ればわかるように、モータ１４ｃ以外のモータ１４ａ，１４ｂにも把持トルクの影響が現れることになり、各モータ軸に関するオブザーバ信号により把持開始を推定できることがわかる。
【０１０４】
端的な例として、マニピュレータのグリッパ軸とヨー軸の動作が同期して行われる場合を説明する。ヨー軸動作角度とグリッパ軸動作角度が同量だけ同期して動作するとき、モータ１４ａ、１４ｂの動作速度は等しく、モータ１４ｃの速度はゼロ、すなわち停止した状態で把持動作を行うことになる。
【０１０５】
図１２は、グリッパ軸を単独動作させた後、引き続いてグリッパ軸とヨー軸が同期して動作する場合の動作波形図である。図１２（ａ）はモータ角度の波形図、図１２（ｂ）はモータ電流の波形図、図１２（ｃ）は把持開始推定信号ｆａの波形図、図１２（ｄ）は把持力の波形図であり、これらの図の横軸はいずれも時間である。
【０１０６】
図１２（ａ）に示すように、グリッパ軸が単独で動作しているときは（時刻ｔ０〜ｔ２）、モータ１４ｃのみが動作しており、グリッパ軸とヨー軸が同期して動作しているときは（時刻ｔ２〜ｔ４）は、把持動作中であってもモータ１４ｃは停止し、モータ１４ｂが動作していることを示している。
【０１０７】
このように、時刻ｔ２〜ｔ４では、グリッパで把持動作をしている間、モータ１４ｃが停止中であるため、図１２（ｂ）に示すようにモータ１４ｃの電流値はほぼ一定となっており、角度の変化がないため、オブザーバ信号の変化も生じない。しかしながら、実際には、図１２（ｄ）に示すように、把持力は生じているため、上述のようにモータ１４ｃの軸にも反力が働いているはずである。
【０１０８】
本来ならば、モータ１４ｃが停止中であっても、反力が働くと電流値に変化が生じるが、動力伝達機構の摩擦により把持力とモータトルクとの間にヒステリシスが生じて、モータ軸まで把持力が伝わらなくなる。このようなときは、停止しているモータ１４ｃ以外で、トルク干渉の関係から反力が加わる軸のうち実際に駆動しているモータ、ここではモータ１４ｂの把持開始推定信号ｆａを用いて把持開始点を推定すればよい。
【０１０９】
図１２（ｃ）は、時刻ｔ０〜ｔ２の間はモータ１４ｃに対応する把持開始推定信号ｆａを利用し、時刻ｔ２〜ｔ４ではモータ１４ｂに対応する把持開始推定信号ｆａを利用して、それぞれ把持開始を推定する例を示している。
【０１１０】
把持動作時に、ヨー軸、ロール軸およびグリッパ軸が同期して動作し、モータ１４ｂとモータ１４ｃの速度がいずれもゼロになる場合もありうるが、この場合はモータ１４ａの把持開始推定信号ｆａに基づいて把持開始点を推定すればよい。
【０１１１】
すべてのモータ１４ａ，１４ｂ，１４ｃの速度が同時にゼロになった状態で把持動作を行うことはありえないため、いずれか一つのモータの把持開始推定信号ｆａで把持開始を推定することができる。
【０１１２】
このように、トルク干渉の関係から把持力のかかるモータ軸を特定し、特定したモータ軸に対応するモータのうち、モータ速度に応じて把持開始推定信号ｆａを切替えるため、作用部４の種々の動きによらず、把持開始点を精度よく推定することができる。より具体的には、図８に示すように、モータ軸速度ωｄに基づいて、第２の切替部ＳＷ２を切替えることにより、いずれか一つのモータに対応する把持開始推定信号ｆａを選択する。
【０１１３】
モータ軸速度がゼロとなるような特殊な場合でないときは、上述した（５）式より、把持トルクの反力が最も大きくなる軸はモータ１４ａの軸である。ところが、実際はモータと出力軸との間には減速機が介在しており、この減速比がモータ１４ｂ，１４ｃよりも２倍以上大きい（出力軸が２倍以上に減速される）と、モータ１４ａのモータ電流から算出される把持開始推定信号ｆａは、他の軸から算出される値よりも小さくなり、判定では扱いにくい。
【０１１４】
逆に、３軸とも減速比が同じであれば、モータ１４ａのモータ電流から算出される把持開始推定信号ｆａを積極的に使用するとよいことを上記（５）式は示している。
【０１１５】
このように、測定しているモータ電流に対応する軸（モータ入力軸）からグリッパ４に至るまでのすべての減速比を換算した値と把持トルクの反力の大きさとに基づいて、第２の切替部ＳＷ２を切替えてもよい。
【０１１６】
図８の第１および第２の切替部ＳＷ１，ＳＷ２の切替の条件をまとめると、以下のようになる。
【０１１７】
第１の切替部ＳＷ１は、グリッパ軸目標速度ωｇが負の場合にオフする。この場合、把持開始推定オブザーバ５１は、クーロン摩擦項を考慮に入れずに把持開始推定信号ｆａを生成する。
【０１１８】
第１の切替部ＳＷ１は、グリッパ軸目標速度ωｇが正かゼロの場合にオフする。この場合、把持開始推定オブザーバ５１は、クーロン摩擦項を考慮に入れて把持開始推定信号ｆａを生成する。
【０１１９】
第２の切替部ＳＷ２は、代表的にはグリッパ軸を駆動するモータ１４ｃに対応するオブザーバ信号を把持開始推定信号ｆａとして選択するが、トルク干渉があることを考慮して、他のモータ１４ａまたはモータ１４ｂに対応するオブザーバ信号を把持開始推定信号ｆａとして選択することもできる。より具体的には、把持動作中（グリッパ軸速度ωｇが負のとき）には、以下の３つの切替を行う。
【０１２０】
モータ１４ｃの速度が正の場合にはモータ１４ｃを選択する。モータ１４ｃの速度が正でなく、かつモータ１４ｂの速度が正の場合はモータ１４ｂを選択する。モータ１４ｂ、１４ｃの速度が正でない場合は、モータ１４ａを選択する。
【０１２１】
このように、第１の実施形態では、把持開始推定オブザーバ５１を設けて、把持開始角度を推定し、推定した把持開始角度に基づいてグリッパ４の最閉角度を設定して、最閉角度に達するまでグリッパ軸角度を制御する。これにより、モータのトルク制御で把持力を制御する場合よりも、精度よくグリッパ４の把持力制御が可能となる。
【０１２２】
また、本実施形態の把持開始推定オブザーバ５１では、クーロン摩擦項を考慮に入れずに把持開始角度を推定するため、把持開始角度を精度よく推定することができる。
【０１２３】
さらに、トルク干渉機構を備えたマニピュレータの場合には、各モータごとに把持開始推定オブザーバ５１ａ，５１ｂ，５１ｃを設けて、各モータのモータ速度によって、いずれか一つの把持開始推定オブザーバの出力を最終的な把持開始推定信号ｆａとして選択するため、トルク干渉機構の場合であっても、迅速かつ正確に把持開始角度を推定できる。
【０１２４】
（第２の実施形態）
第２の実施形態は、上述した把持開始推定オブザーバ５１を用いて、医療用マニピュレータのようなマスタスレーブ操作による把持動作を行うものである。
【０１２５】
図１３（ａ）および図１３（ｂ）は操作桿８２の内部構造の一例を示す図であり、図１３（ａ）はトリガ８５の復帰状態、図１３（ｂ）はトリガ８５を引いた状態を示している。これらの図に示すように、操作桿８２は、トリガ８５の端部に取り付けられたロック棒８７を保持するロック部材８８と、トリガ８５の引き量を検出するセンサ８９と、トリガ８５に形成されたガイド孔９０を通って挿脱されるガイド棒９１と、ガイド棒９１の周囲に取り付けられたばね９２とを有する。
【０１２６】
センサ８９は、トリガ８５の並進動作位置を検出するものであり、具体的には、スライドポテンションメータやリニアエンコーダ等で構成することができる。例えば、スライドポテンションメータでセンサ８９を構成する場合、トリガ８５の端部に取り付けられた突起部材９３の凹部がスライドポテンションメータのつまみ９４の凸部と係合される。これら凹部と凸部は、トリガ８５の並進動作に合わせて一体に移動する。そして、センサ８９は、凸部の位置によりトリガ８５の引き量を検出する。
【０１２７】
トリガ８５を最も引いた状態が作用部４のグリッパを最も閉じた状態（図１３（ａ））、すなわち開閉角度が最大閉角度となる状態となり、トリガ８５を最も戻した状態がグリッパを最も開いた状態となり、すなわち、グリッパを最も開いた状態となるように、センサ８９で検出した値を変換することで、グリッパ開閉目標値を得ることができる。
【０１２８】
トリガ８５の引き量を長時間維持することは操作者にとって負担が大きく、作用部４での細かい操作指示にも影響を与える可能性がある。このため、トリガ８５から手を離してもその引き量が維持されるように、操作桿８２にロック機構を設けることがあり、本実施形態の操作桿８２においても、ロック機構が設けられている。
【０１２９】
トリガ８５を最大限引くと、トリガ８５の端部に取り付けられたロック棒８７が操作桿８２のロック部材８８に収まる。その後、操作者がトリガ８５から手を離しても、ロック部材８８はロック棒８７を固定し、トリガ８５の引き量が一定に維持される。ロック部材８８には、トリガ８５の引き量に応じて、ロック棒８７がわずかに並進動作するためのあそび範囲が設けられている。このように、ロック棒８７とロック部材８８によりロック機構が実現されている。
【０１３０】
トリガ８５がロックされた状態でさらにトリガ８５を引くと、ロック部材８８はロック棒８７を開放する。トリガ８５には、操作桿８２のガイド棒９１を挿脱するためのガイド孔９０が設けられている。トリガ８５をロックした状態では、ガイド棒９１がガイド孔９０に挿入され、ばね９２がトリガ８５の端部により収縮される。このロック状態でトリガ８５をさらに引いてロック部材８８がロック棒８７を開放すると、ばね９２の復元力により、トリガ８５は押し出されて、元の位置に復帰する。このとき、図１３（ａ）のように、ガイド棒９１はガイド孔９０から抜き出される。
【０１３１】
図１３（ａ）および図１３（ｂ）に示すように、マスタースレーブ操作によりグリッパ軸を動作させて把持力を調整する場合を考えると、操作者は把持状態を観測しながら操作を行うため、トリガ８５を引く程度に応じてグリッパ４が閉じて把持力が増大し、設定した最大把持力が最大限トリガ８５を引いたときに得られるようにすると、操作感と適合して望ましい。すなわち、マスタスレーブ操作による把持力調整機能とは、最大把持力を設定する機能と言い換えることができる。
【０１３２】
ここで、センサ８９が検出する電圧範囲がトリガを最大に引いたときにＶ０、トリガが最も出ている状態でＶ２であるとする。トリガ８５が最も出ている状態では、グリッパ軸角度がθｐ（＞０）に設定されるものとする。
【０１３３】
図１４はセンサ８９の出力電圧とグリッパ軸目標値との関係を示す図である。センサ出力電圧Ｖｔは、把持開始推定オブザーバ５１によって検出更新された把持開始角度に対応するセンサ出力電圧であり、以下の（６）式で表される。
【数４】
2010076012.tif 000006
【０１３４】
ここで、センサ電圧Ｖ１は、グリッパ４が物体を把持していないときの、センサ出力とグリッパ軸目標値との関係を決める既定値で、Ｖ０とＶ２の間に設定されるものである。より具体的には、Ｖ１はグリッパ４が物体を把持せずに閉じた場合に、グリッパ軸目標値を０度とする電圧値である。
【０１３５】
トリガ８５の引き量に応じてグリッパ４の把持力が変化する感度をよくするためには、Ｖ０とＶ１の間隔を広めに取っておけばよい。また、硬くて小さいものを設定した最大力で掴むことを前提にしていれば、Ｖ０とＶ１との間隔を狭く取っておくと良い。
【０１３６】
物体を把持していない状態、すなわち、センサ出力電圧ＶｇがＶｔ以上の場合、グリッパ軸目標角度θｇ１と電圧Ｖｇとの関係は以下の（７）式で表される。
【数５】
2010076012.tif 000007
【０１３７】
物体を把持している状態、すなわち、ＶｇがＶｔ以下の場合、グリッパ軸目標角度θｇ２と電圧Ｖｇとの関係は以下の（８）式で表すことができる。
【数６】
2010076012.tif 000008
【０１３８】
上述した図８の把持開始推定オブザーバ５１で生成された把持開始推定信号ｆａに基づいて把持開始角度が推定されると、推定された把持開始角度でのセンサ８９の電圧を検出し、検出されたセンサ電圧を基点として、トリガ８５の引き量に応じたセンサ出力電圧とグリッパ軸角度との関係を求めて、グリッパ軸の角度目標値を設定する。これにより、設定した最大把持力が最大限トリガ８５を引いたときに得られるような把持力調整機能をグリッパ軸の位置（角度）制御で実現することができる。
【０１３９】
このように、第２の実施形態では、トリガ８５の引き量をセンサ８９のセンサ電圧で検出可能なマスタスレーブ操作のマニピュレータにおいて、把持開始角度が推定された時点を基点として、センサ出力電圧とグリッパ軸角度の関係から、トリガ８５の引き量に応じたグリッパ軸角度の制御を行うため、把持力の制御を精度よく行うことができる。
【０１４０】
（第３の実施形態）
第１の実施形態では、本発明がトルク干渉機構を備えたマニピュレータにも適用できることを説明した。以下に説明する第３の実施形態では、この種のトルク干渉機構におけるトルク干渉補正について説明する。
【０１４１】
上述したように、把持動作をすると、トルク干渉により各モータ軸に対して反力がかかる。より具体的には、作用部４の姿勢軸周りにトルクが作用すると、各姿勢軸を駆動するモータ出力軸に反力となるトルクが作用する。モータ出力軸のトルクは、上述した（４）式で表される。
【０１４２】
マニピュレータの場合、各モータ出力軸と先端の作用部４との間には、動力伝達機構の一部を構成するワイヤ（可とう性部材）１２ａ，１２ｂ，１２ｃが存在する。これらワイヤは力に比例して伸びるため、各モータ出力軸が上述した（２）式を満たすように角度制御されていても、実際の先端部の従属プーリはモータ軸側に対して角度がずれて姿勢が変化してしまう。
【０１４３】
そこで、そのずれ量を求めてみる。今、ワイヤの剛性をｗとすると、以下の（９）式が成り立つ。
【数７】
2010076012.tif 000009
【０１４４】
把持トルクτgとグリッパ軸角度は比例することから、比例定数αを用いると、以下の（１０）式の関係が成り立つ。
【０１４５】
τｇ＝αΔθｇ      …（１０）
【０１４６】
モータ出力軸と姿勢軸との関係を表した上記（５）式の両辺にそれぞれ（９）式と（１０）式を代入して整理すると、以下の（１１）式が得られる。
【数８】
2010076012.tif 000010
【０１４７】
このとき、Δθｄ＝ＫΔθａを用いることで、姿勢軸偏差角度Δθｄは、以下の（１２）式で表される。
【数９】
2010076012.tif 000011
【０１４８】
ここで、（１２）式において、Δθｇの項の等式が成り立つとすると、Δθｇ＝６（α／ｗ）Δθｇとなり、α／ｗ＝１／６となる。これにより、グリッパ軸偏差Δθｇ（グリッパ軸目標値と実際のグリッパ軸角度との偏差）と、ヨー軸、ロール軸の姿勢偏差Δθｙ、Δθｒとの関係は、以下の（１３）式および（１４）式で表される。
【数１０】
2010076012.tif 000012
【０１４９】
これまでの算出で用いられている姿勢軸偏差角度Δθｄは、モータ出力軸角度を上記（２）式により換算した値で、これはトルク損失がない場合であるが、実際はトルク損失が起こり、先端部の姿勢はこれと異なる。実際の先端部姿勢偏差は、モータ出力軸から先端部姿勢軸までのトルク効率ηを用いると、以下の（１５）式および（１６）式で表される。
【数１１】
2010076012.tif 000013
【０１５０】
把持開始点を認識したら、把持開始後のグリッパ軸角度Δθｇに応じて上述のように求めた偏差角度分だけ、姿勢軸補正を行うように制御すればよい。なお、ここでトルク効率ηは、モータ出力軸からグリッパ軸に至る動力伝達機構の軸数の関数として表すことができる。例えば、図２〜図４に示すマニピュレータにおいては、出力軸プーリ３９ｃ、回転軸１６、ギア３２２の回転軸、グリッパ軸３２３ａの４軸を通じてグリッパ軸動作が行われているため、通常の潤滑状態であれば１軸あたりの効率を０．９として、０．９の４乗がトルク効率ηになる。
【０１５１】
例えば、洗浄された直後の医療用マニピュレータの場合、洗浄後に潤滑処理していない無潤滑状態であれば、１軸あたりの効率を０．８とすればよい。もちろん軸の材質などでも変化するので適宜設定すればよい。
【０１５２】
なお、上述したように、（１２）式からα／ｗ＝１／６の関係が得られるため、この関係を（１１）式に代入して求めた各モータ出力軸のずれ角度を補正角度として、逆キネマティック算出後の各モータ出力軸目標値にこの補正角度を減算してはならない。このようにすると、新たな姿勢角度を定義することになり、グリッパ軸の目標値までも変化してしまい、目的とした把持力を出力できなくなってしまう。よって、上述した（１３）および（１４）式に示すように、グリッパ軸角度を基準とした補正角度を姿勢軸換算で求め、それを目標の各姿勢角度から減算した後、逆キネマティック演算をして、各モータ軸の目標値を算出するのが望ましい。
【０１５３】
ここで、把持力を調整するための把持力開始点を推定する既定値と、姿勢補正を行うための把持力開始点を推定する既定値とは同じである必要はない。把持力調整の場合、把持開始角度と把持角度から最大把持力を決めるが、その最大把持力の大きさは把持角度と絶対的な関係にあるわけではない。例えば、図１５において、把持力開始点を判定するための規定値ｆ１の代わりに、より大きな値ｆ２を規定値として設定してもよい。この場合、規定値ｆ２からの把持力と把持角度との関係は、規定値ｆ１からの把持力と値がオフセットされるだけで、把持力と把持角度についての１次的な関係には変化がないため、規定値が変化しても把持力の調整は可能である。
【０１５４】
このように、規定値を変更しても、設定したい把持力の大きさの基準またはスケールが変化したに過ぎない。規定値以下の把持力を調整することはできず、規定値を高くするほどオブザーバ信号に含まれるノイズの影響を受けにくくなることから、両者のトレードオフにより規定値のレベルは設定される。
【０１５５】
一方、姿勢補正を行う場合は、把持力の変化が姿勢変化につながることから、できるかぎり把持開始点を正確に推定する必要がある。このため、図１５に示すように、姿勢補正を行う場合の規定値は、把持開始時に変化が現れるｆ３以上で、かつｆ１以下に設定するのが望ましい。このｆ３は動作軸補正基準値に対応する。姿勢補正のための把持開始点を推定する処理は制御装置２が行うものであり、この処理は補正開始角度推定手段に対応する。また、制御装置２は、把持開始点を推定した後に上述した（１５）および（１６）式に基づいて偏差角度を算出する処理を行うが、この処理は偏差角度算出手段に対応する。その後、制御装置２は、偏差角度に基づいて各姿勢軸の姿勢補正を行うが、この処理は動作軸補正手段に対応する。
【０１５６】
このように、第３の実施形態では、トルク干渉機構を備えたマニピュレータの場合、把持動作の制御により、他の姿勢軸にも影響が及ぶことから、把持開始後のグリッパ軸角度に応じて姿勢軸補正を行う。これにより、把持力が変化しても姿勢補正を精度よく行うことができる。
【０１５７】
（第４の実施形態）
以下に説明する第４の実施形態は、把持開始推定信号ｆａを用いて、対象物の相対的な柔軟度を測定するものである。
【０１５８】
把持力を把持角度で制御することが従来行われていない理由の一つは、対象物が変形することにより、角度制御では実際の把持力が当初予定していた把持力よりも小さくなってしまうことがあるためである。第１および第２の実施形態においても、非常に柔らかい物体を把持するときと、変形しない硬い物体を把持するときでは、同じ把持角度を設定しても、同じ把持力にはならない。
【０１５９】
しかしながら、把持力を決定する把持角度は任意に調整できるため、対象物に応じて設定を調整することで実用的な対応が可能となる。未知の大きさの物体を、設定した大きさ以上の把持力で把持することはないことから、最大把持力を設定することと同等と考えることができる。把持力を必要以上に出さないシステムは、安全性の上でも望ましく、モータの発熱を抑えて劣化を防止し、また省エネで効率的なシステムを構築できる。例えば、干渉機構では、把持動作に寄与するモータが他の姿勢軸動作にも影響を与えるが、把持動作に必要以上の出力を割り当てないとすると、他の姿勢軸動作に十分な出力を割り当てることができる。
【０１６０】
本実施形態では、対象物の硬さによって生じる把持力の誤差を補正できるとともに、対象物の硬さを推定できることを特徴とする。
【０１６１】
図１６はマニピュレータが物体を把持するときのモータ１４ｃの出力軸角度θ３と把持開始推定信号ｆａとの関係を示すグラフである。上述したように、θ３＝−θｇであるため、θ３が増加するほど把持力が高くなる。
【０１６２】
把持開始推定信号ｆａが図１５に示した既定値ｆ１と一致するときの把持角度は、図１６では符号を反転させて−θｔとしている。仮に物体Ａを用いて関係付けられた把持角度と把持力の関係から把持力を調整する機能が初期設定されていたとする。この設定で、物体Ａよりも柔らかい物体Ｂを把持すると、算出された最閉角度θｃ（モータ１４ｃの出力軸では−θｃ）までグリッパ４を閉じても予定の把持力まで達しない。これは物体Ａの硬さ係数ｋａと物体Ｂの硬さ係数ｋｂとが異なり、同じ角度θｈだけ閉じても把持力は異なるためである。
【０１６３】
そこで、物体の硬さ係数を、把持動作中であって、グリッパ軸角度θｇが把持開始を認識した把持開始角度θｔからそのとき算出された最閉角度θｃまでの間の角度θｕにあるときのグリッパ軸動作に関わるモータ軸角度、ここではモータ１４ｃの出力軸角度と、把持開始推定オブザーバ５１の出力との関係を最小二乗法で算出する。算出された値はその物体ｂの硬さを表す指標ｋｂである。
【０１６４】
予め物体ａの硬さ係数ｋａを同様の手法で算出しておくとすると、物体ａを把持する把持角度θａで設定された把持力と同等の把持力で物体ｂを把持するときの把持角度θｂの関係は以下の（１７）式で表される。
【０１６５】
ｋａθａ＝ｋｂθｂ      …（１７）
この場合、ｋｂ＜ｋａであるから、θｂ＞θａである。よって、物体ａを把持角度θａで把持した場合の把持力と同等の把持力で物体ｂを把持するときの把持角度θｂとの差Δθｃは、以下の（１８）式で表すことができる。
【数１２】
2010076012.tif 000014
【０１６６】
上記（１８）式で算出された分だけ最閉角度を増やせばよいことになる。
【０１６７】
図１７は硬さ補正処理の処理手順の一例を示すフローチャートである。この処理は図６のフローチャート内のステップＳ１３の処理の中で行われ、より具体的には図１１の処理に引き続いて行われる。
【０１６８】
まず、図１１の処理によって求めた把持開始角度θｔとグリッパ軸最閉角度θｃを取得する。そして把持開始角度θｔとグリッパ軸最閉角度θｃとから、角度θｕを取得する。（ステップＳ４１）。次に、グリッパ軸角度θｇがθｔ＞θｇ＞θｕの範囲内にあるか否かを判定する（ステップＳ４２）。ステップＳ４２の判定がＹＥＳの場合には、その時点でのグリッパ軸動作に関係するモータ出力軸角度、具体的にはモータ１４ｃの出力軸角度θ３と、モータ１４ｃに対応する把持開始推定信号ｆａを取得して保存する（ステップＳ４３）。θ３とｆａを保存する理由は、後に硬さ係数ｋｂを算出する際に用いるためである。このステップＳ４３の処理は記録手段に対応する。
【０１６９】
ステップＳ４２の判定がＮＯの場合には、物体の違いによる補正を行ったか否かを示す補正フラグの状態を判定する（ステップＳ４４）。この補正フラグは初期状態ではリセットされており、補正前であることを示している。
【０１７０】
補正フラグがリセット状態と判断された場合は、ステップＳ４３で保存したモータ出力軸角度θ３と把持開始推定信号ｆａを用いて最小二乗法により硬さ係数を算出し（ステップＳ４５）、上述した（１８）式に基づいてグリッパ軸最閉角度を更新し（ステップＳ４６）、補正フラグをセットする（ステップＳ４７）。このステップＳ４５の処理は硬さ推定手段に対応し、ステップＳ４６の処理は姿勢軸限界角度更新手段に対応する。
【０１７１】
以降、グリッパ軸目標値θｇが更新された最閉角度になるまでの間、補正フラグはセット状態を維持するため、新たな最閉角度で制御される。なお、把持状態が解除されて把持フラグがリセットされるとともに、補正フラグもリセットされる。
【０１７２】
図１７のステップＳ４６では、対象物の硬さ係数を算出しているが、この係数を提示（表示）するようにすれば、対象物の硬さを操作者に知らせることができる。ただし、実際には、対象物の動力学モデルには速度に依存するダンパ効果があることや、マニピュレータ全体に作用する力と把持力との区別などが困難であることなどから、絶対的な物体の硬さを算出提示することは困難である。しかし、同様な把持動作を自動動作などで実現すれば、対象物の部分ごとに把持した結果から、相対的な硬さの違いを認識することが可能である。この機能により、周囲に対してある部分だけ硬さが違うとその部分を推定して候補として上げることができる。
【０１７３】
このように、第４の実施形態では、把持対象物が硬い場合と柔らかい場合では、同じ把持角度を設定しても、同じ把持力にはならないことから、把持開始角度を推定した後に対象物の硬さ係数を計算し直して、グリッパの最閉角度を更新するため、対象物の硬さに応じて把持力を調整できる。これにより、対象物の硬さに差異があっても、角度制御により精度よく把持力を調整できる。
【０１７４】
（第５の実施形態）
第５の実施形態は、初期姿勢誤差を推定して初期姿勢を補正するものである。
【０１７５】
上述した把持開始推定オブザーバ５１は、物体を把持していない場合、例えばマニピュレータの場合は、グリッパを構成する第１および第２の作用部材３２５，３２６が接触した角度を推定することになる。例えば、グリッパを閉じた状態を初期姿勢とすれば、グリッパ軸における初期姿勢を推定することになる。したがって、マニピュレータの製造、組み立て誤差によりグリッパ軸の初期姿勢にばらつきが生じても、グリッパ軸の試行動作中の把持開始推定信号ｆａに基づいて、初期姿勢の誤差を推定して補正することができる。
【０１７６】
もちろん、本実施形態の手法によれば、物体の大きさによらず把持開始点を推定できるため、初期姿勢に誤差があっても、その影響を受けることなく、把持力を調整することができるのは言うまでもない。
【０１７７】
初期姿勢の補正は、例えば以下の手順で行われる。マニピュレータを使用する前の準備動作等でグリッパに強く把持する動作を行わせて、初期姿勢誤差を推定して記録しておく。運用中に推定した把持開始点の最小値は、物体を把持せずにグリッパの第１および第２の作用部材３２５，３２６が接触した点である。この最小値が運用中あるいは運用再開時に低下していることが確認されたら、動力伝達機構に異常が発生していると推定でき、簡易的に異常検知手段を実現できる。例えば、動力伝達機構におけるワイヤの伸びやほつれ、ワイヤとプーリとの締結緩み、軸のずれなどにより、認識した把持開始点の低下が考えられる。
【０１７８】
このように、把持開始点の最小値を記録して、その変化を観測することで、マニピュレータの異常検知を行うことができる。同様に、準備動作等における物体を把持しない把持動作において、硬さ係数を算出して記録しておき、定期的な同等の動作でこの値を比較することで、ワイヤの硬さ変化すなわち、ワイヤの劣化を検知することができる。検知した結果は表示器等で使用者に知らせればよい。
【０１７９】
以上、作用部４の機構としてグリッパが物体を把持する場合について説明してきたが、本発明に係るマニピュレータシステムの作用部４においては、例えば、物体を二つに分ける作用を行う機構、物体を一定方向に押さえつける機構、物体を切断する機構などを有する作用部４に対して適用できるのは言うまでもない。例えば対象物を剥離する機構を有する作用部４を制御する場合は、図１７においてグリッパ軸目標値の向きを反対にすればよい。このようにシステム構成によって出力の向きが変わるが、それに応じて、出力信号の扱い方をプラス側をマイナス側、判定処理における「以上」を「以下」、「未満」を「より大きい」など、適宜変更すればよい。
【０１８０】
また、本発明に係るマニピュレータシステムは、医療用に限らず、例えば、配管内で異物除去を行う作業用ロボット、家庭やオフィス、店舗等で物品を運搬するロボットなどの種々の用途のシステムにも適用可能である。
【０１８１】
さらに、本発明に係るマニピュレータのアクチュエータは、電機モータに限らず、空圧アクチュエータ等でもよい。本発明は、電機モータの電流に相当する量をセンシングできるアクチュエータで駆動されるマニピュレータシステムに幅広く適用可能である。
【０１８２】
図５に示した制御装置２の内部構成の少なくとも一部は、ハードウェアで構成してもよいし、ソフトウェアで構成してもよい。ソフトウェアで構成する場合には、制御装置２の少なくとも一部の機能を実現するプログラムをフレキシブルディスクやＣＤ−ＲＯＭ等の記録媒体に収納し、コンピュータに読み込ませて実行させてもよい。記録媒体は、磁気ディスクや光ディスク等の着脱可能なものに限定されず、ハードディスク装置やメモリなどの固定型の記録媒体でもよい。
【０１８３】
また、制御装置２の少なくとも一部の機能を実現するプログラムを、インターネット等の通信回線（無線通信も含む）を介して頒布してもよい。さらに、同プログラムを暗号化したり、変調をかけたり、圧縮した状態で、インターネット等の有線回線や無線回線を介して、あるいは記録媒体に収納して頒布してもよい。
【０１８４】
この他、上記の記載に基づいて、当業者であれば、本発明の追加の効果を想到できたり、種々の変形が可能であるかもしれないが、これらも本発明の開示範囲に含まれるものと思料される。本発明の態様は、上述した個々の実施形態には限定されない。特許請求の範囲に規定された内容およびその均等物から定義される技術的範囲から逸脱しない範囲で種々の追加、変更および部分的削除が可能である。
【図面の簡単な説明】
【０１８５】
【図１】医療用マニピュレータシステムの外観図。
【図２】作業部と操作部を分離した状態を示す図。
【図３】マニピュレータ本体の作用部の構造を示す斜視図。
【図４】作用部の詳細構造を示す分解斜視図。
【図５】制御装置とその周辺の制御系の詳細構成の一例を示すブロック図。
【図６】図５の演算部が実行するプログラムの一例を示すフローチャート。
【図７】制御装置によって制御される把持角度と実際の把持力との関係を示す図。
【図８】把持開始推定オブザーバを有する制御装置が行う処理動作の一例を示す制御システム図。
【図９】把持開始推定オブザーバのより詳細な内部構成の一例を示すブロック図。
【図１０】（ａ）〜（ｄ）は図８の把持開始推定オブザーバの特定を示す図。
【図１１】最閉角度決定部の処理手順を示すフローチャート。
【図１２】グリッパ軸を単独動作させた後、引き続いてグリッパ軸とヨー軸が同期して動作する場合の動作波形図。
【図１３】（ａ）および（ｂ）は操作桿の内部構造の一例を示す図。
【図１４】センサの出力電圧とグリッパ軸目標値との関係を示す図。
【図１５】把持開始点の推定を説明する図。
【図１６】マニピュレータが物体を把持するときのモータの出力軸角度と把持開始推定信号との関係を示すグラフ。
【図１７】硬さ補正処理の処理手順の一例を示すフローチャート。
【符号の説明】
【０１８６】
１  マニピュレータ本体
２  制御装置
３  操作指令部
４  作用部
５  連結部
６  駆動部
７  作業部
８  操作部
５１  把持開始推定オブザーバ
５２  把持角度決定部
５３  逆キネマティック行列演算部
５４  ＰＩＤ制御部
５５  モータ駆動部
５６  目標把持角度設定部
５７  最閉角度決定部
５８  操作量更新部
【図１】
2010076012.tif 000015
【図２】
2010076012.tif 000016
【図３】
2010076012.tif 000017
【図４】
2010076012.tif 000018
【図５】
2010076012.tif 000019
【図６】
2010076012.tif 000020
【図７】
2010076012.tif 000021
【図８】
2010076012.tif 000022
【図９】
2010076012.tif 000023
【図１０】
2010076012.tif 000024
【図１１】
2010076012.tif 000025
【図１２】
2010076012.tif 000026
【図１３】
2010076012.tif 000027
【図１４】
2010076012.tif 000028
【図１５】
2010076012.tif 000029
【図１６】
2010076012.tif 000030
【図１７】
2010076012.tif 000031