(57)【特許請求の範囲】
【請求項１】
対象物の位置を検出する位置検出手段と、
上記対象物を駆動する駆動手段と、
上記位置検出手段が出力する位置信号と、上記駆動手段への加速度指令信号である駆動信号に基づき上記対象物が受ける反力を求める反力検出手段と、
上記対象物が受ける反力と目標となる力信号とから第１の加速度信号を求めるとともに、上記位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
ことを特徴とする位置・力制御装置。
【請求項２】
対象物の位置及び該対象物に作用する力を位置指令信号、力指令信号に応じて制御する位置・力制御装置であって、
上記対象物を駆動する駆動手段と、
上記対象物の位置を検出する位置検出手段と、
上記位置検出手段が出力する位置信号と、上記駆動手段への加速度指令信号である駆動信号から、該対象物が受ける反力を求める反力検出手段と、
位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第１の加速度信号に変換する第１の演算手段と、
上記反力検出手段により検出された上記反力と、力指令信号との和を求め、該和を第２の加速度信号に変換する第２の演算手段と、
上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
ことを特徴とする位置・力制御装置。
【請求項３】
マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝える位置・力制御装置であって、
マスタ側の操作部を駆動する第１の駆動手段と、
マスタ側の操作部の位置を検出する第１の位置検出手段と、
上記第１の位置検出手段が出力する位置信号と、上記第１の駆動手段への加速度指令信号である駆動信号から、上記操作部に作用する反力を求める第１の反力検出手段と、
スレーブ側の対象物を駆動する第２の駆動手段と、
スレーブ側の対象物の位置を検出する第２の位置検出手段と、
上記第２の位置検出手段が出力する位置信号と、上記第２の駆動手段への加速度指令信号である駆動信号から、上記対象物が受ける反力を求める第２の反力検出手段と、
上記第１の位置検出手段が出力する位置信号と、第２の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第１、第２の加速度信号に変換する第１の演算手段と、
上記第１の反力検出手段と、上記第２の反力検出手段の出力の和を求め、該和をマスタ側およびスレーブ側を制御するための第３、第４の加速度信号に変換する第２の演算手段と、
上記第１，第３の加速度信号を加算する第１の加算手段と、
上記第２，第４の加速度信号を加算する第２の加算手段と、
上記第１の加算手段の出力に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力する第１の制御手段と、
上記第２の加算手段の出力に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する第２の制御手段とを備えた
ことを特徴とする位置・力制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】  本発明は、力センサを用いることなく、対象物の位置と対象物に作用する力を応答性よく制御することができる位置・力制御装置に関する。
【背景技術】
【０００２】  人と機械が接触するヒューマンインタラクションの分野において、人間の操作に応じて遠隔に配置された対象物の位置や対象物に作用する力を応答性よく制御したいという要望がある。
例えば、人間が立ち入ることができない作業現場で用いられる遠隔操作装置や、遠隔地の患者に対する遠隔医療等の分野では、操作者によるマスタ側の操作に応じて遠隔地に配置されたスレーブ側の装置を作動させるようにしたマスタ・スレーブ制御装置が用いられている。この種のマスタ・スレーブ制御装置では、対象物の位置と対象物に作用する力を応答性よく制御し、繊細な作業を実現したいという要望がある。
上記マスタ・スレーブ装置としては、操作者から、マスタ側装置を介して、スレーブ側装置に指令を送ることができるが、スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができないユニラテラル制御方式や、操作者からマスタ側装置を介してスレーブ側装置に指令を送ることができ、かつ、上記スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができるバイラテラル制御方式が知られている。【０００３】  バイラテラル制御方式はスレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックしているので、この制御方式によれば、遠隔地における触覚を、現実と同様にリアルタイムで得ることができる。
バイラテラル制御方式の制御装置は、マスタ・スレーブ型ロボットで構成されており、遠隔地のスレーブにかかる力を、マスタを通じて人間に返す。また人間はマスタを動かすことにより、スレーブにマスタと同一の動きをさせることができる。こうすることで遠隔地の物体を、あたかも手元で触っている感覚を得ることができる。
上記マスタ・スレーブ制御装置としては、例えば特許文献１に記載のものが提案されている。
上記特許文献１に記載のものは、操作者によるマスタの操作に応じてスレーブが動作するマスタ・スレーブ装置において、スレーブが物体と接触している状態では、操作者の操作によりマスタに加わる力ｆ１を求め、スレーブにかかる力ｆ３が、マスタに加わる力ｆに応じた力ｆ２に追従するようにスレーブを制御するように構成したものであり、引用文献１に記載のものによれば、バイラテラル制御方式による長所を維持したまま、制御系の複雑化や高コスト化等を避けることができる。【０００４】  上記バイラテラル制御方式としては、上記特許文献１にも記載されるように図１４に示すものや、図１５に示すものが知られている。
図１４に示すものは、マスタ２０１の位置とスレーブ２０２の位置との偏差を求め、この偏差に応じて位置制御部２０３によりスレーブ側の位置を制御するとともに、マスタ２０１とスレーブ２０２に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部２０４によりマスタ側の力を制御するようにしたものである。
また、図１５に示すものは、マスタ２０１の位置とスレーブ２０２の位置との偏差を求め、この偏差に応じて位置制御部２０３によりスレーブ側の位置を制御するとともに、位置制御部２０３’によりマスタ側の位置を制御し、さらに、マスタ２０１とスレーブ２０２に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部２０４によりマスタ側の力を制御するとともに、力制御部２０４’によりスレーブ側を制御するようにしたものである。
さらに、上記のような力検出器と力制御部を備え、固い制御対象物でも安定して高精度な力制御を実現できるすると同時に、アームのどの部分に接触しても柔軟な特性を持つことができるロボットの制御装置が提案されている（特許文献２参照）
【特許文献１】特開２００２−３０７３３６号公報
【特許文献２】特開２００１−１９８８７０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】  上記従来の制御装置において、力または加速度情報の検出をする場合、歪みゲージなどの力センサあるいは加速度センサを用いて直接的に測定するのが一般的であった。
しかし、上記力センサや加速度センサを用いた直接的な力または加速度情報の検出では、以下のような問題点があった。
（ｉ）信号ノイズの問題
力センサや加速度センサによる力の検出では微小なアナログ信号を増幅して検出しているが、信号に含まれるノイズも増幅してしまう。高周波ノイズに関しては低周波域通過フィルタを通すことによりその影響を除去することができるが、その場合には必要としている信号の周波数帯域までも狭めてしまう。
（ｉｉ）センサの固有周波数の問題
力センサを用いた力検出では、環境を直接力センサに接触させて測定する。力センサには物理的な固有周波数が存在し、それ以上高い周波数帯域での力測定は不可能である。
（ｉｉｉ）システムモデルの変化
力センサはシステムに直接設置するため、センサ慣性がそのままシステムモデルに加わってくる。また力センサでは加わった力に比例した歪みを利用して力を測定するが、このため力センサには物理的に剛性の低い材料が用いられている。しかし環境との接触を伴うシステムでは、この低剛性の力センサによりシステムモデルが変化してしまう。モデルの変化はシステムの制御においては致命的な問題である。
（ｉｖ）センサ価格の問題
一般的に力センサや加速度センサは高価であり、このため、装置全体も高価なものとなってしまう。【０００６】  また、位置および力等のように複数の変量を制御する場合、例えば前記図１４、図１５に示したように、位置検出器と力検出器を設け、位置制御系と力制御系により位置と力を制御するのが一般的である。
しかし、上記構成の制御系では、位置制御系と力制御系のそれぞれにより位置と力を制御しているので、位置制御系と力制御系が互いに干渉して、ゲインを上げることができず、誤差が残ってしまうことがある。
さらに、上記制御系において、歪みゲージなどの力センサを用いて力検出を行うと、前記したように力センサのノイズや固有周波数等の影響により、測定可能周波数帯域が狭い範囲に限定されてしまうとともに、力センサの慣性や低い剛性が、システムモデルに影響を与えてしまい、高い周波数帯域での応答性能を向上させることができない。
このため、従来のバイラテラル制御系においては、硬い環境に触ると不安定になったり、瞬間的な衝撃力が得られない、また、操作感が重いといった問題があり、繊細な感覚情報を伝えることができず、例えば遠隔医療等の分野などにおいて、繊細な作業を実現することは困難であった。
また、従来の制御系では、位置センサに加え、比較的高価な力センサを使用しており、装置価格が高くなるといった問題もあった。
本発明は上記従来技術の問題点を解決するためになされたものであって、本発明の目的は、高い周波数帯域での応答性能を向上させることができ、繊細な作業を実現することが可能な位置・力制御装置を提供することである。
【課題を解決するための手段】
【０００７】
従来の歪みゲージを用いた力センサや、加速度センサのアナログ情報には信号ノイズが不可避であるが、位置情報はデジタル情報として検出可能なのでノイズの影響を受けにくい。
そこで、本発明においては、力センサに代えて反力検出手段を設け、位置情報より対象物に作用する反力を推定する。
これにより、ノイズを含まない反力を推定することができる。また、位置検出器はモータ軸とは非接触に位置情報を得ることができるので、力センサのような固有周波数による測定可能周波数帯域の制限がなく、非接触なのでセンサ設置による慣性および剛性などのシステムモデルの変化は起こらない。また、位置センサのみを用いているので、システムを安価に構築することができる。
また、従来のように位置制御系と力制御系をそれぞれ設けて位置および力を制御すると、位置制御系と力制御系が互いに干渉して、ゲインを上げることができない。
そこで、本発明においては、位置および力を加速度信号に変換し、両者を合成して、対象物を駆動する加速度指令信号である駆動信号を得るようにした。
これにより、位置制御と力制御が干渉することなく、それぞれのゲインを独立して設定することができる。
以上に基づき、本発明においては、次のようにして前記課題を解決する。
（１）対象物の位置を検出する位置検出手段を設け、位置情報と対象物を駆動する駆動手段への加速度指令信号である駆動信号に基づき、応力検出手段により対象物が受ける反力を推定する。
そして、上記対象物が受ける反力と目標となる力信号とから第１の加速度信号を求め、また、上記位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
（２）上記（１）の制御を、位置指令信号、力指令信号に応じて対象物の位置、対象物が受ける力を制御する制御装置に適用する。
すなわち、対象物の位置を検出する位置検出手段を設け、上記のように、反力検出手段により対象物が受ける反力を推定する。
そして、位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第１の加速度信号に変換し、また、上記反力検出手段により検出された上記反力と、力指令信号との和を求め、該和を第２の加速度信号に変換し、第１、第２の加速度信号を加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
（３）上記（１）の制御を、マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝えるバイラテラル制御方式に適用する。
すなわち、マスタ側の操作部とスレーブ側の対象物の位置を検出する第１、第２の位置検出器をそれぞれ設け、また、上記のように第１、第２の反力検出手段によりマスタ側の操作部が受ける反力および、対象物が受ける反力を推定する。
そして、上記第１の位置検出手段が出力する位置信号と、第２の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第１、第２の加速度信号に変換する。また、上記第１の反力検出手段と、第２の反力検出手段の出力の和を求め、該和を、マスタ側およびスレーブ側を制御するための第３、第４の加速度信号に変換する。
上記第１，第３の加速度制御信号を加算するとともに、第２，第４の加速度制御信号を加算し、上記第１，第３の加速度制御信号の加算結果に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力するとともに、上記第第２，第４の加速度制御信号の加算結果に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する。
【発明の効果】
【０００８】  本発明においては、以下の効果を得ることができる。
（１）反力検出手段を設け、位置検出手段の出力に基づき対象物に加わる力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、対象物の加わる力を求めることができる。
したがって、位置検出手段として高精度のセンサを用い、位置検出結果を短い周期でサンプリングすれば、対象物の位置、対象物に加わる力を応答性よく制御することができる。
また、センサとしては位置検出手段を設けるだけでよく、比較的高価な力センサを使用しないので、システムを安価に構築することができる。
（２）対象物が受ける反力と目標となる力信号とから第１の加速度信号を求め、また、上記位置検出手段により検出された位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号を合成し、対象物を制御する駆動手段への駆動信号を出力するようにしたので、従来の制御装置のように位置、力制御が互いに干渉するといった問題が生ずることがなく、位置制御のゲインと力制御のゲインを独立に設定することが可能となる。このため、位置制御のゲインを大きくすることにより、位置誤差を生じさせることなく、リアルタイムで制御することができる。
（３）本発明をバイラテラル制御に適用し、マスタ側とスレーブ側に設けた位置検出手段の出力から位置偏差を求めて、加速度信号に変換し、また、マスタ側とスレーブ側に設けた反力検出手段により推定されたマスタ側とスレーブ側の反力を加算して加速度信号に変換し、上記位置偏差から求めた加速度信号と、反力の加算結果から求めた加速度信号を合成してマスタ側とスレーブ側を制御するようにしたので、上記のように位置制御のゲインと力制御のゲインを独立に設定することが可能となり、例えば、マスタ側とスレーブ側の位置誤差が零になるように、かつ、力については両者の和に倣うようにマスタ側とスレーブ側を制御することができる。
このため、マスタ側とスレーブ側の位置誤差を生じさせることなく、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。
【図面の簡単な説明】
【０００９】［図１］本発明の第１の実施例の位置・力制御装置の概略構成を示す図である。
［図２］外科医が手術等に使用される鉗子の構成例を示す図である。
［図３］本発明の第１の実施例におけるマスタ側の装置構成を示す図である。
［図４］本発明の第１の実施例におけるスレーブ側の装置構成を示す図である。
［図５］本発明の第１の実施例の制御系のブロック図である。
［図６］本発明で使用される反力オブザーバのブロック図である。
［図７］従来の力センサと反力推定オブザーバの周波数特性を示す図である。
［図８］指令値を正弦波状に変化させた場合における従来の力センサと反力オブザーバの出力を示す図である。
［図９］反力オブザーバの他の構成例を示す図である。
［図１０］本実施例の制御装置の応答特性（位置）を示す図である。
［図１１］本実施例の制御装置の応答特性（力）を示す図である。
［図１２］本発明の第２の実施例の位置・力制御装置の概略構成を示す図である。
［図１３］本発明の第２の実施例の制御系のブロック図である。
［図１４］従来のバイラテラル制御方式の構成例（１）を示す図である。
［図１５］従来のバイラテラル制御方式の構成例（２）を示す図である。
【符号の説明】
【００１０】  １      マスタ
１ａ    リニアモータ
１ｂ    位置検出器
１ｃ    操作部
２，４  反力オブザーバ
３      スレーブ
３ａ    リニアモータ
３ｂ    位置検出器
３ｆ    把持部
５      位置制御部
６      操作力制御部
７      加速度合成部
１１    位置／力指令発生部
１３    スレーブ
１４    反力オブザーバ
１５    位置制御部
１６    操作力制御部
１７    加速度合成部
【発明を実施するための最良の形態】
【００１１】  図１は本発明の第１の実施例の位置・力制御装置の概略構成を示す図であり、同図は本発明をバイラテラル制御に適用した場合の概略構成を示している。
同図において、１は作業者により操作されるマスタであり、例えば作業者により操作される操作部、該操作部を駆動するマスタ側モータ、該モータの位置を検出するマスタ側位置検出器から構成される。２は反力オブザーバであり、後述するように上記マスタ側位置検出器の出力と、上記マスタ側への駆動信号から上記操作部に加わる力を求める。
３はマスタ１の操作に応じて動作するスレーブであり、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンドや、後述する鉗子であればその把持部等、遠隔地において実際に作業を行う操作部である（ここではこれらを含めて対象物と呼ぶこととする）。
４は反力オブザーバであり、上記マスタ側反力オブザーバと同様な構成を備え、後述するように上記スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記マスタ側位置検出器により検出された操作部の位置、スレーブ側位置検出器により検出された対象物の位置信号ｘｍ，ｘｓは、それぞれ位置制御部５に送られ、位置制御部５は上記位置信号を加速度参照値ａｐｍ，ａｐｓに変換する。
また、マスタ側の反力オブザーバ２により検出されたマスタ側の力信号Ｆｍ、スレーブ側の反力オブザーバ２により検出されたスレーブ側の力信号Ｆｓは操作力制御部６に送られ、操作力制御部６は、上記力信号Ｆｍ，Ｆｓを加速度参照値ａｆｍ，ａｆｓに変換する。
加速度合成部７は、上記加速度参照値ａｐｍ，ａｐｓ、加速度参照値ａｆｍ，ａｆｓを合成し、マスタ側モータへの駆動信号となる加速度指令信号ａｍ、スレーブ側モータへの駆動信号となる加速度指令信号ａｓを出力する。
本実施例の制御装置の反力オブザーバ２，４、位置制御部５、操作力制御部６、加速度合成部７は、コンピュータにより実現することができ、本実施例の制御装置をコンピュータで構成する場合には、上記位置検出器の出力を所定のサンプリング周期でコンピュータに取り込み、ソフトウェアにより上記機能を実現するための演算処理を行って、マスタ側、スレーブ側のモータを制御する。【００１２】  次に、医療用の鉗子を遠隔制御する場合を例として、バイラテラル制御方式に適用した本発明の実施例の位置・力制御装置について説明する。
鉗子は、外科医が手術等に使用する医療器具であり、図２（ａ）に示すように、ハンドル部１０１（以下操作部１０１という）と鉗子部１０２（以下把持部１０２という）から構成される。
鉗子の軸部１０３内を操作部材（図示せず）が貫通しており、操作部１０１のハンドル１ｅを手で開閉することにより、上記操作部材が同図の左右方向に移動する。
把持部１０２は図２（ｂ）に示すように、把持部１０２がリンク機構１０４に連結され、リンク機構１０４の他端側に上記操作部材１０５が連結されており、上記操作部１０１のハンドル１ｅを操作して、上記操作部材を同図の矢印方向に動かすことにより、把持部１０１を開閉させることができる。
本実施例では、上記鉗子を上記操作部１０１と把持部１０２の２つに分けて、操作部１０１をマスタ側、把持部１０２をスレーブ側とし、それぞれにリニアモータを連結し、該リニアモータを前記図１に示した制御系を用いて制御することにより、操作部１０１の操作に応じて、遠隔地に設置された把持部１０２を制御するようにした。
なお、以下では、操作部１０１の操作に応じて把持部を開閉させる１軸制御について説明するが、本実施例で説明する制御系を複数設けることにより操作部１０１の操作に応じて把持部全体を回転させたり、把持部全体を揺動させる等の多軸制御を行うこともできる。【００１３】  図３、図４は本実施例におけるマスタ側とスレーブ側の装置構成を示す図である。
図３（ａ）はマスタ側装置の上面図、（ｂ）は側面図、（ｃ）は同図（ａ）をＡ方向から見た図である。
同図において、１ａはリニアモータであり、リニアモータ１ａの可動軸１ｄに操作部１ｃを構成するハンドル１ｅの一方が連結され、ハンドル１ｅの他方はリニアモータ１ａのケースに固定されている。従って、手で操作部１ｃのハンドル１ｅを操作することにより、リニアモータ１ａの可動軸１ｄが同図（ａ）（ｂ）の左右方向に動く。１ｂはマスタ側位置検出器でありリニアモータ１ａの可動軸１ｄの位置を検出する。
図４（ａ）はスレーブ側装置の上面図、（ｂ）は側面図、（ｃ）は同図（ａ）をＡ方向から見た図である。
同図において、３ａはリニアモータであり、リニアモータ３ａの可動軸３ｄには、取付け金具３ｃを介して鉗子の軸部３ｄ内を貫通する前記操作部材３ｅが連結され、操作部材３ｅの先端側には前記したようにリンク機構を介して把持部３ｆが取付けられている。
従って、リニアモータ３ａの可動軸３ｄが同図（ａ）（ｂ）の左右方向に動くことにより、操作部材３ｅが左右方向に動き把持部３ｆが開閉する。３ｂはスレーブ側位置検出器であり、リニアモータ３ａの可動軸３ｄの位置を検出する。
なお、上記モータとしては、摩擦力の少ないモータを用いるのが望ましく、上記のようなリニアモータを使用する外、回転運動の場合にはダイレクトドライブモータ等を使用することができる。
また、上記位置検出器としては、高精度の検出器を用いるのが望ましく、また、応答性を向上させるには、位置検出器の検出結果を取り込むサンプリング周期を十分短くすることが必要である。【００１４】  図５は本実施例の制御系のブロック図である。
同図において、前記図１の示したものと同一のものには同一の符号が付されており、１はマスタであり、マスタ１は図３に示したように、リニアモータ１ａと位置検出器１ｂと操作部１ｃから構成される。マスタ側の反力オブザーバ２は、上記マスタ１のリニアモータ１ａに供給される電流信号Ｉａｍｒｅｆと、位置検出器１ｂにより検出されるリニアモータ１ａの可動軸１ｄの位置（即ち操作部１ｃの位置）に応じた位置検出信号ｘｍからマスタ側に加わる力の推定値Ｆｍを求める。
３はスレーブであり、スレーブ３は図４に示したように、リニアモータ３ａと位置検出器３ｂと前記機構により動作する把持部３ｆから構成される。
スレーブ側の反力オブザーバ４は、上記スレーブ２のリニアモータ３ａに供給される電流信号Ｉａｓｒｅｆと、位置検出器３ｂにより検出されるリニアモータ３ａの可動軸３ｄの位置（即ち把持部１ｆの位置）に応じた位置検出信号ｘｓからスレーブ側に加わる力の推定値Ｆｓを求める。【００１５】  ５は位置制御部であり、上記マスタ側の位置検出器１ｂにより検出されたマスタ側の操作部１ｃの位置信号ｘｍと、スレーブ側の位置検出器３ｂにより検出されたスレーブ側の把持部１ｆの位置信号ｘｓとの差を求める減算部５ａと、減算部５ａの出力に（Ｋｖｓ＋Ｋｐ）の演算を施す制御部５ｂと、制御部５ｂの出力に、〔Ｍｓ／（Ｍｍ＋Ｍｓ）〕，〔−Ｍｓ／（Ｍｍ＋Ｍｓ）〕を乗じて加速度参照値ａｐｍ，ａｐｓを生成する変換部５ｃ，５ｄから構成される。なお、ここで、Ｋｐは位置ゲイン、Ｋｖは速度ゲイン、Ｍｍはマスタ側の慣性、Ｍｓはスレーブ側の慣性である。
６は操作力制御部であり、マスタ側の反力オブザーバ２が出力するマスタ側の操作部１ｃに加わる力の推定値Ｆｍとスレーブ側の反力オブザーバ４が出力するスレーブ側の把持部３ｆに加わる力の推定値Ｆｓとの和を出力する加算部６ａと、加算部６ａの出力に力ゲインＫｆを乗算し、加速度参照値ａｆｍ，ａｆｓを生成する変換部６ｂとから構成される。上記力ゲインＫｆは仮想慣性の逆数であり任意の値に設定することができ、Ｋｆを適切に設定することにより、等価質量を見かけ上小さくすることができる。
７は加速度合成部であり、上記加速度参照値ａｐｍ，ａｆｍを加算してマスタ側の加速度参照値（ｘｍｒｅｆ）″を出力する加算部７ａと、加算部７ａの出力に〔Ｍｍ／Ｋｔｎｍ〕を乗じて、マスタ側のリニアモータ１ａを駆動する電流参照値Ｉａｍｒｅｆを生成する変換部７ｂと、上記加速度参照値ａｐｓ，ａｆｓを加算してスレーブ側の加速度参照値（ｘｓｒｅｆ）″を出力する加算部７ｃと、加算部７ｃの出力に〔Ｍｓ／Ｋｔｎｓ〕を乗じて、スレーブ側のリニアモータ３ａを駆動する電流参照値Ｉａｓｒｅｆを生成する変換部７ｄとから構成される。ここで、Ｍｍ，Ｍｓは前記したマスタ側の慣性、スレーブ側の慣性であり、Ｋｔｎｍ，Ｋｔｎｓはそれぞれマスタトルク定数、スレーブトルク定数である。
なお、図面上では、一次微分、２次微分を符号の上にドットを１または２付けて示すが、明細書中では一次微分を「’」を付して示し、２次微分を上記（ｘｍｒｅｆ）″のように「”」を付けて示す。【００１６】  図６は、反力オブザーバ２，４のブロック図である。
同図において、１，３はマスタ側、スレーブ側のブロック図であり、Ｉａｒｅｆはリニアモータ１ａ，３ａに供給される電流参照値、Ｋｔは推力定数であり、Ｉａｒｅｆ×Ｋｔはリニアモータの駆動力に相当する。Ｆｌはリニアモータの負荷、Ｍはマスタ側、スレーブ側の慣性であり、リニアモータ１ａ，３ａに電流Ｉａｒｅｆが供給され、負荷がＦｌのとき、マスタ側、スレーブ側は、同図に示す速度ｘ’で動き、その位置ｘは速度ｘ’を積分した値となる。
反力オブザーバ２，４には上記電流参照値Ｉａｒｅｆ、位置ｘが入力され、反力オブザーバ２，４は以下の（１）式によりマスタ側、スレーブ側の推定反力Ｆｍ，Ｆｓ（＝Ｆ）を求める。
Ｆ＝〔ｇ／（ｓ＋ｇ）〕×〔ＩａｒｅｆＫｔｎ＋ｘＭｎｓｇ−Ｆｉｎｉｔ〕−Ｍｎｓｇ…（１）ここで、Ｆｉｎｉｔ＝Ｆｄｉｓ＋Ｄｘ’＋（Ｍ−Ｍｎ）ｘ’である。また、ｇ／（ｓ＋ｇ）は一次の周波数選択フィルタである。
上記式および図６中の各記号は以下の値を表している。
Ｍ：慣性、Ｍｎ：慣性公称値、Ｋｔｎ：推力定数公称値、ｘ：位置、ｘ’：速度、Ｆｄｉｓ：クーロン摩擦、Ｄ：粘性摩擦係数。
なお、Ｆｄｉｓ，Ｄ，Ｍ，Ｍｎ等は予備実験により求めた既知の値であるとする。
上記反力オブザーバを用いて反力を推定することにより、力センサを用いる場合に比べ、前記したように、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にマスタ側、スレーブ側の反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。【００１７】  間接的センシングの有効性を確認するため、従来の歪みゲージを用いた力センサを用いて直接力を測定した場合と、反力推定オブザーバを用いて間接的に力を測定した場合を比較した。
図７に実験結果を示す。同図の横軸はω（ｒａｄ／ｓｅｃ）、縦軸はｇａｉｎ（ｄＢ）であり、同図（ａ）は従来の力センサの周波数特性、（ｂ）は反力オブザーバの周波数特性を示す。
同図から明らかなように、従来の力センサでは固有周波数の存在により、測定可能周波数帯域が約５００ｒａｄ／ｓｅｃ以下に限定されてしまうが、反力推定オブザーバでは設定したゲイン１５００ｒａｄ／ｓｅｃまでの広帯域で測定できる。
図８は指令値（印加する力）を正弦波状に変化させて、従来の歪みゲージを用いた力センサと上記反力オブザーバの出力を比較した図である。同図の横軸は時間（秒）、縦軸は力（ニュートン）であり、同図の点線は指令値（与えた力）、破線は上記反力オブザーバによる推定結果、実線は従来の力センサによる検出結果を示す（指令値と反力オブザーバによる推定結果はほぼ重なっている）。
同図の示すように、反力オブザーバの出力は、指令値とほぼ一致しているが、従来の力センサの出力は、上記指令値よりやや遅れており、また、固有周波数や慣性の影響により検出結果が一部振動的に変化している。【００１８】  図７、図８から明らかなように、反力オブザーバを用いれば、従来の力センサを用いた場合に比べ、広帯域での測定が可能であり、また、殆ど遅れなくマスタ側、スレーブ側に加わる力を推定することができ、これによりマスタ側、スレーブ側を応答よく制御することができる。
上記反力オブザーバは、例えば、「大西，”外乱オブザーバによるロバスト・モーションコントロール”日本ロボット学会誌ｖｏｌ．１１，ｎｏ．４，ｐｐ．４８６〜４９３，１９９３」等に開示されている。
図９は、反力オブザーバの他の構成例を示す図であり、同図は、周波数選択フィルタとして、図６に示した〔ｇ／（ｓ＋ｇ）〕に替え、〔ｋ１／（ｓ２＋ｋ２ｓ＋ｋ１）〕の伝達関数で表される２次フィルタを用いた場合を示している。
上記〔ｋ１／（ｓ２＋ｋ２ｓ＋ｋ１）〕の極は、ｓ２＋ｋ２ｓ＋ｋ１＝０から得られ、この極をα，βとすると、α＋β＝−ｋ２、αβ＝ｋ１となる。
なお、上記図９に示した反力オブザーバについては、例えば「ｋ．Ｏｈｎｉｓｈｉ，Ｍ．Ｓｈｉｂａｔａ，Ｔ．Ｍｕｒａｋａｍｉ，″Ｍｏｔｉｏｎ  Ｃｏｎｔｒｏｌ  ｆｏｒ  Ａｄｖａｎｃｅｄ  Ｍｅｃｈａｔｏｒｏｎｉｃｓ″，ＩＥＥＥ／ＡＳＭＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  Ｍｅｃｈａｔｒｏｎｉｃｓ」等を参照されたい。【００１９】  次に、図３、図４、図５に示す本実施例の制御装置の動作について説明する。
マスタ側の操作部１ｃを操作すると、リニアモータ１ａの可動軸１ｄが移動し、位置検出器１ａにより可動軸１ｄの位置が検出される。
この位置検出器１ａの出力と、スレーブ側の位置検出器３ａの出力が位置制御部５に送られ、位置制御部５はその偏差に基づき加速度参照値ａｐｍ，ａｐｓを生成し出力する。
一方、マスタ側の反力オブザーバ２はマスタ側に加わる力の推定値Ｆｍを出力する。
操作力制御部６は、上記マスタ側に加わる力の推定値Ｆｍと、スレーブ側の反力オブザーバ４が出力するスレーブ側に加わる力Ｆｓの和を求め、この和から加速度参照値ａｆｍ，ａｆｓを生成し出力する。
上記加速度参照値ａｐｓと加速度参照値ａｆｓは、加速度合成部７に送られ、加速度合成部７は上記加速度参照値ａｐｓと加速度参照値ａｆｓの和から、電流参照値Ｉａｓｒｅｆを生成する。また、上記加速度参照値ａｐｍと加速度参照値ａｆｍの和から、電流参照値Ｉａｍｒｅｆを生成する。
上記電流生成値Ｉａｓｒｅｆはスレーブ側のリニアモータ３ａに与えられ、リニアモータ３ａが駆動される。同様に、上記電流生成値Ｉａｍｒｅｆはマスタ側のリニアモータ１ａに与えられ、リニアモータ１ａが駆動される。【００２０】  マスタ側の操作部１ｃを操作することにより、スレーブ側のリニアモータ３ａの可動軸とマスタ側のリニアモータ１ａの可動軸に位置誤差が生ずると、上記加速度合成部７から、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａに上記位置誤差を修正するような駆動信号が与えられ、マスタ側のリニアモータ１ａ、スレーブ側のリニアモータ３ａの可動軸が駆動される。
これにより、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａの可動軸の位置が一致するように制御される。すなわち、マスタ側の操作部１ｃのハンドル１ｅの位置に応じて、スレーブ側の把持部３ｆの開度が制御される。
また、これと同時に、把持部３ｆに加わる力Ｆｓが反力オブザーバ４で推定されるとともに、操作部１ｃに加わる力Ｆｍが反力オブザーバ２で推定され、ＦｓとＦｍとの和に応じた加速度参照値ａｆｍ、ａｆｓが加速度合成部７に与えられる。
加速度合成部７は、把持部３ｆに加わる力Ｆｓと操作部１ｃに加わる力Ｆｍの和に応じて、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａを駆動する。
これにより、マスタ側の操作部１ｃに加わる力とスレーブ側の把持部３ｆに加わる力との和に倣うように、マスタ側の操作部１ｃとスレーブ側の把持部３ｆが制御される。
すなわち、マスタ側の操作部１ｃを操作して、スレーブ側の把持部３ｆで物体を把持すると、把持部３ｆに加わる反力に応じた力が操作部１ｃに伝わり、これにより、作業者は把持部３ｆで把持した物体からの反力等を操作部１ｃで感じ取ることができる。
以上の説明では、マスタ側を操作した場合について説明したが、スレーブ側を操作した場合でも上記と同様にマスタ側が制御され、スレーブ側にマスタ側の力が伝えられる。【００２１】  本実施例のバイラテラル制御装置は、上記のように、マスタ側の操作部、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、マスタ側の操作部、スレーブ側の対象物が受ける反力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、マスタ側とスレーブ側の位置誤差を生じさせることなく、位置および操作力を応答性よく制御することができる。
また、位置制御部５と操作力制御部６を設けて、加速度合成部で位置制御部５と操作力制御部６で生成した加速度参照値を合成して、リニアモータを駆動しているので、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。
このため、マスタ側とスレーブ側の位置誤差が無くなるように位置制御部５を設定することができ、また、マスタ側とスレーブ側の反力の和に倣うように両者を制御することができる。さらに、操作力制御部６の力ゲインＫｆの設定により、マスタ側の操作力を自由に設定することができる。【００２２】  本実施例のバイラテラル制御装置により前記鉗子を遠隔制御して、本発明の有効性を確認した。
実験では、制御装置をコンピュータで構成し、位置検出器として、分解能が１μｍ程度のリニアエンコーダを用い、１００μｓ程度のサンプリング周期でコンピュータで構成された制御装置に取り込んで、鉗子をバイラテラル制御した。これにより、１６０Ｈｚ程度の応答性を得ることができた。
図１０、図１１に上記実験結果を示す。
なお、図１０、図１１の横軸は時間（ｓｅｃ）、図１０の縦軸は位置、図１１の縦軸は力（ニュートン）であり、位置、力をステップ状に変化させたときの応答特性を示す。
前記した従来のバイラテラル制御系では環境接触時において位置偏差が生じてしまい位置再現性が悪かったが、本発明においては、計算機のサンプリング時間を十分に短して周波数帯域を十分広く確保し、位置制御部５の制御ゲインを上げることにより、図１０に示すように位置偏差を生じることなくリアルタイムに環境を再現できた。【００２３】  以上説明した実施例では、本発明をバイラテラル制御に適用する場合について説明したが、本発明は、上記バイラテラル制御に限らず、位置指令値、力指令値に応じてスレーブ側を制御する制御装置に適用することもできる。
図１２に上記のように位置指令値、力指令値に応じてスレーブ側を制御する本発明の第２の実施例の位置・力制御装置の概略構成を示す。
同図において、１１は位置指令値と力指令値を発生する位置／力指令発生部であり、位置／力指令発生部１１が出力する位置指令値ｘｒｅｆは位置制御部１５に与えられ、力指令値ｆｒｅｆは、操作力制御部１６に与えられる。
１３は上記位置指令、力指令に応じて動作するスレーブであり、前記したように、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンド等、遠隔地において実際に作業を行う操作部である。
１４は反力オブザーバであり、前記したように、スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記位置指令値ｘｒｅｆとスレーブ側に設けられた位置検出器により検出された対象物の位置信号ｘｓは、それぞれ位置制御部１５に送られ、位置制御部１５は上記位置信号を加速度参照値ａｐｓに変換する。
また、力指令値ｆｒｅｆとスレーブ側の反力オブザーバ１２により検出されたスレーブ側の力信号ｆｓは操作力制御部１６に送られ、操作力制御部１６は、上記力信号ｆｒｅｆ，ｆｓを加速度参照値ａｆｓに変換する。
加速度合成部１７は、上記加速度参照値ａｐｓ、加速度参照値ａｆｓを合成し、スレーブ側モータへの駆動信号となる加速度指令信号ａｓを出力する。【００２４】  上記位置／力指令発生部１１が発生する位置指令値、力指令値は、予めプログラミングされた値であり、例えば、熟練した作業者等による操作等を記憶することにより得ることができる。
例えば、前記実施例に示したバイラテラル制御装置を用いて熟練した作業者により所望の作業をしてもらい、マスタ側（もしくはスレーブ側）の位置ｘｍ（またはｘｓ）、応力オブザーバの出力ｆｍ（またはｆｓ）を、それぞれ位置指令値、力指令値として記憶しておく。そして、この記憶された位置指令値、力指令値を上記位置／力指令発生部１１から出力し、スレーブ側を制御することにより、熟練した作業者による作業を再現することができる。【００２５】
図１３は、本実施例の制御系のブロック図である。
同図において、１１は上記位置／力指令発生部、１３はスレーブであり、スレーブ１３は前記したように、リニアモータ等のモータと、位置検出器を備える。
１４は反力オブザーバであり、前記図６、図９に示した構成を備え、スレーブ１３のモータに供給される電流信号Ｉasref と、位置検出器により検出される位置に応じた位置検出信号ｘs からスレーブ側に加わる力の推定値Ｆs を求める。
１５は位置制御部であり、上記位置／力指令発生部１１が出力する位置指令値ｘref と、とスレーブ側の位置検出器により検出されたスレーブ側の位置信号ｘs との差を求める減算部１５ａと、減算部１５ａの出力に（Ｋv  ｓ＋Ｋp ）の演算を施す制御部１５ｂと、加速度参照値ａpsを生成する変換部１５ｃから構成される。なお、ここで、Ｋp は位置ゲイン、Ｋv は速度ゲインである。
１６は操作力制御部であり、力指令値ｆref と反力オブザーバ１４が出力するスレーブ側の対象物に加わる力の推定値Ｆs との和を出力する加算部１６ａと、加算部１６ａの出力に力ゲインＫf を乗算し、加速度参照値ａfsを生成する変換部１６ｂとから構成される。上記力ゲインＫf は前記したように仮想慣性の逆数であり任意の値に設定することができ、Ｋf を適切に設定することにより等価質量を見かけ上小さくすることができる。
７は加速度合成部であり、上記加速度参照値ａps，ａfs  を加算してスレーブ側の加速度参照値（ｘs ref ）" を出力する加算部１７ａと、加算部１７ａの出力に〔Ｍs ／Ｋtns 〕を乗じて、スレーブ側のモータを駆動する電流参照値Ｉasref   を生成する変換部１７ｂとから構成される。ここで、Ｍs はスレーブ側の慣性であり、Ｋtns はスレーブトルク定数である。
【００２６】
次に、図１３に示す本実施例の制御装置の動作について説明する。
位置／力指令発生部１１が出力する位置指令値ｘref とスレーブ側の位置検出器３ａの出力が位置制御部１５に送られ、位置制御部１５はその偏差に基づき加速度参照値ａpsを生成し出力する。
一方、操作力制御部１６は、上記位置／力指令発生部１１が出力する力指令値ｆref と反力オブザーバ４が出力するスレーブ側に加わる力Ｆs の和を求め、この和から加速度参照値ａfsを生成し出力する。
上記加速度参照値ａpsと加速度参照値ａfSは、加速度合成部１７に送られ、加速度合成部１７は上記加速度参照値ａpsと加速度参照値ａfsの和から、電流参照値Ｉasref を生成する。上記電流生成値Ｉasref はスレーブ側のモータに与えられ、モータが駆動される。
位置指令値ｘref が変化すると、スレーブ側のモータの位置との位置誤差が生じ、加速度合成部１７から、スレーブ側のモータに上記位置誤差を修正するような駆動信号が与えられ、スレーブ側のモータが駆動される。
これにより、上記位置指令値にスレーブ側の位置が一致するように制御される。
また、これと同時に、スレーブ側に加わる力Ｆs が反力オブザーバ４で推定され、力指令値ｆref とＦs との偏差に応じた加速度参照値ａfsが加速度合成部１７に与えられる。
加速度合成部１７は、力指令値ｆref とスレーブ側の力Ｆs の大きさが異なると、この大きさが一致するような駆動信号をスレーブ側のモータに与える。これにより、スレーブ側に加わる力が力指令値ｆref  に一致するように制御される。
【００２７】  本実施例においては、上記のように、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、対象物が受ける反力を推定しているので、第１の実施例と同様、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、位置誤差を生じさせることなく、スレーブ側の位置、操作力を応答性よく制御するこができる。
また、位置制御部１５と操作力制御部１６を設けて、加速度合成部１７で位置制御部１５と操作力制御部１６で生成した加速度参照値を合成して、スレーブ側の対象物を駆動しているので、第１の実施例と同様、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。
【産業上の利用可能性】
【００２８】  本発明は、医療分野、建築用作業ロボット、食品加工や食品のハンドリング、アッセッブル、楽器の操作等の各種分野に適用することができ、バイラテラル制御系に適用すれば、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。
例えば、本発明を医療分野で使用される鉗子等の遠隔制御に適用すれば、従来の遠隔手術では実現することができなかった繊細な感覚情報を伝えることができ、鉗子を直接手で操作する場合と同様な感覚で使用することが可能となる、また、繊細な感覚情報を伝えることが必要な触診等を遠隔から行うことも可能となる。
また、建築用作業ロボットに適用することにより、セメント塗り等の作業を熟練した作業者が直接手で塗ったのと同様に行うことができ、さらに、食品のハンドリング作業やアッセンブル、楽器の操作等においても、人手による作業や操作と同様に行うことが可能となる。
【図１】
0004696307.tif 000002
【図２】
0004696307.tif 000003
【図３】
0004696307.tif 000004
【図４】
0004696307.tif 000005
【図５】
0004696307.tif 000006
【図６】
0004696307.tif 000007
【図７】
0004696307.tif 000008
【図８】
0004696307.tif 000009
【図９】
0004696307.tif 000010
【図１０】
0004696307.tif 000011
【図１１】
0004696307.tif 000012
【図１２】
0004696307.tif 000013
【図１３】
0004696307.tif 000014
【図１４】
0004696307.tif 000015
【図１５】
0004696307.tif 000016