JP4696307B2 - 位置・力制御装置 - Google Patents

Description

本発明は、力センサを用いることなく、対象物の位置と対象物に作用する力を応答性よく制御することができる位置・力制御装置に関する。

人と機械が接触するヒューマンインタラクションの分野において、人間の操作に応じて遠隔に配置された対象物の位置や対象物に作用する力を応答性よく制御したいという要望がある。
例えば、人間が立ち入ることができない作業現場で用いられる遠隔操作装置や、遠隔地の患者に対する遠隔医療等の分野では、操作者によるマスタ側の操作に応じて遠隔地に配置されたスレーブ側の装置を作動させるようにしたマスタ・スレーブ制御装置が用いられている。この種のマスタ・スレーブ制御装置では、対象物の位置と対象物に作用する力を応答性よく制御し、繊細な作業を実現したいという要望がある。
上記マスタ・スレーブ装置としては、操作者から、マスタ側装置を介して、スレーブ側装置に指令を送ることができるが、スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができないユニラテラル制御方式や、操作者からマスタ側装置を介してスレーブ側装置に指令を送ることができ、かつ、上記スレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックすることができるバイラテラル制御方式が知られている。

バイラテラル制御方式はスレーブ側が作業対象物等から受ける作業反力を操作者にフィードバックしているので、この制御方式によれば、遠隔地における触覚を、現実と同様にリアルタイムで得ることができる。
バイラテラル制御方式の制御装置は、マスタ・スレーブ型ロボットで構成されており、遠隔地のスレーブにかかる力を、マスタを通じて人間に返す。また人間はマスタを動かすことにより、スレーブにマスタと同一の動きをさせることができる。こうすることで遠隔地の物体を、あたかも手元で触っている感覚を得ることができる。
上記マスタ・スレーブ制御装置としては、例えば特許文献１に記載のものが提案されている。
上記特許文献１に記載のものは、操作者によるマスタの操作に応じてスレーブが動作するマスタ・スレーブ装置において、スレーブが物体と接触している状態では、操作者の操作によりマスタに加わる力ｆ１を求め、スレーブにかかる力ｆ３が、マスタに加わる力ｆに応じた力ｆ２に追従するようにスレーブを制御するように構成したものであり、引用文献１に記載のものによれば、バイラテラル制御方式による長所を維持したまま、制御系の複雑化や高コスト化等を避けることができる。

上記バイラテラル制御方式としては、上記特許文献１にも記載されるように図１４に示すものや、図１５に示すものが知られている。
図１４に示すものは、マスタ２０１の位置とスレーブ２０２の位置との偏差を求め、この偏差に応じて位置制御部２０３によりスレーブ側の位置を制御するとともに、マスタ２０１とスレーブ２０２に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部２０４によりマスタ側の力を制御するようにしたものである。
また、図１５に示すものは、マスタ２０１の位置とスレーブ２０２の位置との偏差を求め、この偏差に応じて位置制御部２０３によりスレーブ側の位置を制御するとともに、位置制御部２０３’によりマスタ側の位置を制御し、さらに、マスタ２０１とスレーブ２０２に作用する力を力検出器で検出し、その偏差をマスタ側にフィードバックして、力制御部２０４によりマスタ側の力を制御するとともに、力制御部２０４’によりスレーブ側を制御するようにしたものである。
さらに、上記のような力検出器と力制御部を備え、固い制御対象物でも安定して高精度な力制御を実現できるすると同時に、アームのどの部分に接触しても柔軟な特性を持つことができるロボットの制御装置が提案されている（特許文献２参照）
特開２００２−３０７３３６号公報 特開２００１−１９８８７０号公報

上記従来の制御装置において、力または加速度情報の検出をする場合、歪みゲージなどの力センサあるいは加速度センサを用いて直接的に測定するのが一般的であった。
しかし、上記力センサや加速度センサを用いた直接的な力または加速度情報の検出では、以下のような問題点があった。
（ｉ）信号ノイズの問題
力センサや加速度センサによる力の検出では微小なアナログ信号を増幅して検出しているが、信号に含まれるノイズも増幅してしまう。高周波ノイズに関しては低周波域通過フィルタを通すことによりその影響を除去することができるが、その場合には必要としている信号の周波数帯域までも狭めてしまう。
（ｉｉ）センサの固有周波数の問題
力センサを用いた力検出では、環境を直接力センサに接触させて測定する。力センサには物理的な固有周波数が存在し、それ以上高い周波数帯域での力測定は不可能である。
（ｉｉｉ）システムモデルの変化
力センサはシステムに直接設置するため、センサ慣性がそのままシステムモデルに加わってくる。また力センサでは加わった力に比例した歪みを利用して力を測定するが、このため力センサには物理的に剛性の低い材料が用いられている。しかし環境との接触を伴うシステムでは、この低剛性の力センサによりシステムモデルが変化してしまう。モデルの変化はシステムの制御においては致命的な問題である。
（ｉｖ）センサ価格の問題
一般的に力センサや加速度センサは高価であり、このため、装置全体も高価なものとなってしまう。

また、位置および力等のように複数の変量を制御する場合、例えば前記図１４、図１５に示したように、位置検出器と力検出器を設け、位置制御系と力制御系により位置と力を制御するのが一般的である。
しかし、上記構成の制御系では、位置制御系と力制御系のそれぞれにより位置と力を制御しているので、位置制御系と力制御系が互いに干渉して、ゲインを上げることができず、誤差が残ってしまうことがある。
さらに、上記制御系において、歪みゲージなどの力センサを用いて力検出を行うと、前記したように力センサのノイズや固有周波数等の影響により、測定可能周波数帯域が狭い範囲に限定されてしまうとともに、力センサの慣性や低い剛性が、システムモデルに影響を与えてしまい、高い周波数帯域での応答性能を向上させることができない。
このため、従来のバイラテラル制御系においては、硬い環境に触ると不安定になったり、瞬間的な衝撃力が得られない、また、操作感が重いといった問題があり、繊細な感覚情報を伝えることができず、例えば遠隔医療等の分野などにおいて、繊細な作業を実現することは困難であった。
また、従来の制御系では、位置センサに加え、比較的高価な力センサを使用しており、装置価格が高くなるといった問題もあった。
本発明は上記従来技術の問題点を解決するためになされたものであって、本発明の目的は、高い周波数帯域での応答性能を向上させることができ、繊細な作業を実現することが可能な位置・力制御装置を提供することである。

従来の歪みゲージを用いた力センサや、加速度センサのアナログ情報には信号ノイズが不可避であるが、位置情報はデジタル情報として検出可能なのでノイズの影響を受けにくい。
そこで、本発明においては、力センサに代えて反力検出手段を設け、位置情報より対象物に作用する反力を推定する。
これにより、ノイズを含まない反力を推定することができる。また、位置検出器はモータ軸とは非接触に位置情報を得ることができるので、力センサのような固有周波数による測定可能周波数帯域の制限がなく、非接触なのでセンサ設置による慣性および剛性などのシステムモデルの変化は起こらない。また、位置センサのみを用いているので、システムを安価に構築することができる。
また、従来のように位置制御系と力制御系をそれぞれ設けて位置および力を制御すると、位置制御系と力制御系が互いに干渉して、ゲインを上げることができない。
そこで、本発明においては、位置および力を加速度信号に変換し、両者を合成して、対象物を駆動する加速度指令信号である駆動信号を得るようにした。
これにより、位置制御と力制御が干渉することなく、それぞれのゲインを独立して設定することができる。
以上に基づき、本発明においては、次のようにして前記課題を解決する。
（１）対象物の位置を検出する位置検出手段を設け、位置情報と対象物を駆動する駆動手段への加速度指令信号である駆動信号に基づき、応力検出手段により対象物が受ける反力を推定する。
そして、上記対象物が受ける反力と目標となる力信号とから第１の加速度信号を求め、また、上記位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
（２）上記（１）の制御を、位置指令信号、力指令信号に応じて対象物の位置、対象物が受ける力を制御する制御装置に適用する。
すなわち、対象物の位置を検出する位置検出手段を設け、上記のように、反力検出手段により対象物が受ける反力を推定する。
そして、位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第１の加速度信号に変換し、また、上記反力検出手段により検出された上記反力と、力指令信号との和を求め、該和を第２の加速度信号に変換し、第１、第２の加速度信号を加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する。
（３）上記（１）の制御を、マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝えるバイラテラル制御方式に適用する。
すなわち、マスタ側の操作部とスレーブ側の対象物の位置を検出する第１、第２の位置検出器をそれぞれ設け、また、上記のように第１、第２の反力検出手段によりマスタ側の操作部が受ける反力および、対象物が受ける反力を推定する。
そして、上記第１の位置検出手段が出力する位置信号と、第２の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第１、第２の加速度信号に変換する。また、上記第１の反力検出手段と、第２の反力検出手段の出力の和を求め、該和を、マスタ側およびスレーブ側を制御するための第３、第４の加速度信号に変換する。
上記第１，第３の加速度制御信号を加算するとともに、第２，第４の加速度制御信号を加算し、上記第１，第３の加速度制御信号の加算結果に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力するとともに、上記第第２，第４の加速度制御信号の加算結果に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する。

本発明においては、以下の効果を得ることができる。
（１）反力検出手段を設け、位置検出手段の出力に基づき対象物に加わる力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、対象物の加わる力を求めることができる。
したがって、位置検出手段として高精度のセンサを用い、位置検出結果を短い周期でサンプリングすれば、対象物の位置、対象物に加わる力を応答性よく制御することができる。
また、センサとしては位置検出手段を設けるだけでよく、比較的高価な力センサを使用しないので、システムを安価に構築することができる。
（２）対象物が受ける反力と目標となる力信号とから第１の加速度信号を求め、また、上記位置検出手段により検出された位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号を合成し、対象物を制御する駆動手段への駆動信号を出力するようにしたので、従来の制御装置のように位置、力制御が互いに干渉するといった問題が生ずることがなく、位置制御のゲインと力制御のゲインを独立に設定することが可能となる。このため、位置制御のゲインを大きくすることにより、位置誤差を生じさせることなく、リアルタイムで制御することができる。
（３）本発明をバイラテラル制御に適用し、マスタ側とスレーブ側に設けた位置検出手段の出力から位置偏差を求めて、加速度信号に変換し、また、マスタ側とスレーブ側に設けた反力検出手段により推定されたマスタ側とスレーブ側の反力を加算して加速度信号に変換し、上記位置偏差から求めた加速度信号と、反力の加算結果から求めた加速度信号を合成してマスタ側とスレーブ側を制御するようにしたので、上記のように位置制御のゲインと力制御のゲインを独立に設定することが可能となり、例えば、マスタ側とスレーブ側の位置誤差が零になるように、かつ、力については両者の和に倣うようにマスタ側とスレーブ側を制御することができる。
このため、マスタ側とスレーブ側の位置誤差を生じさせることなく、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。

［図１］本発明の第１の実施例の位置・力制御装置の概略構成を示す図である。
［図２］外科医が手術等に使用される鉗子の構成例を示す図である。
［図３］本発明の第１の実施例におけるマスタ側の装置構成を示す図である。
［図４］本発明の第１の実施例におけるスレーブ側の装置構成を示す図である。
［図５］本発明の第１の実施例の制御系のブロック図である。
［図６］本発明で使用される反力オブザーバのブロック図である。
［図７］従来の力センサと反力推定オブザーバの周波数特性を示す図である。
［図８］指令値を正弦波状に変化させた場合における従来の力センサと反力オブザーバの出力を示す図である。
［図９］反力オブザーバの他の構成例を示す図である。
［図１０］本実施例の制御装置の応答特性（位置）を示す図である。
［図１１］本実施例の制御装置の応答特性（力）を示す図である。
［図１２］本発明の第２の実施例の位置・力制御装置の概略構成を示す図である。
［図１３］本発明の第２の実施例の制御系のブロック図である。
［図１４］従来のバイラテラル制御方式の構成例（１）を示す図である。
［図１５］従来のバイラテラル制御方式の構成例（２）を示す図である。

符号の説明

１ マスタ
１ａ リニアモータ
１ｂ 位置検出器
１ｃ 操作部
２，４ 反力オブザーバ
３ スレーブ
３ａ リニアモータ
３ｂ 位置検出器
３ｆ 把持部
５ 位置制御部
６ 操作力制御部
７ 加速度合成部
１１ 位置／力指令発生部
１３ スレーブ
１４ 反力オブザーバ
１５ 位置制御部
１６ 操作力制御部
１７ 加速度合成部

図１は本発明の第１の実施例の位置・力制御装置の概略構成を示す図であり、同図は本発明をバイラテラル制御に適用した場合の概略構成を示している。
同図において、１は作業者により操作されるマスタであり、例えば作業者により操作される操作部、該操作部を駆動するマスタ側モータ、該モータの位置を検出するマスタ側位置検出器から構成される。２は反力オブザーバであり、後述するように上記マスタ側位置検出器の出力と、上記マスタ側への駆動信号から上記操作部に加わる力を求める。
３はマスタ１の操作に応じて動作するスレーブであり、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンドや、後述する鉗子であればその把持部等、遠隔地において実際に作業を行う操作部である（ここではこれらを含めて対象物と呼ぶこととする）。
４は反力オブザーバであり、上記マスタ側反力オブザーバと同様な構成を備え、後述するように上記スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記マスタ側位置検出器により検出された操作部の位置、スレーブ側位置検出器により検出された対象物の位置信号ｘ_ｍ，ｘ_ｓは、それぞれ位置制御部５に送られ、位置制御部５は上記位置信号を加速度参照値ａ_ｐｍ，ａ_ｐｓに変換する。
また、マスタ側の反力オブザーバ２により検出されたマスタ側の力信号Ｆ_ｍ、スレーブ側の反力オブザーバ２により検出されたスレーブ側の力信号Ｆ_ｓは操作力制御部６に送られ、操作力制御部６は、上記力信号Ｆ_ｍ，Ｆ_ｓを加速度参照値ａ_ｆｍ，ａ_ｆｓに変換する。
加速度合成部７は、上記加速度参照値ａ_ｐｍ，ａ_ｐｓ、加速度参照値ａ_ｆｍ，ａ_ｆｓを合成し、マスタ側モータへの駆動信号となる加速度指令信号ａ_ｍ、スレーブ側モータへの駆動信号となる加速度指令信号ａ_ｓを出力する。
本実施例の制御装置の反力オブザーバ２，４、位置制御部５、操作力制御部６、加速度合成部７は、コンピュータにより実現することができ、本実施例の制御装置をコンピュータで構成する場合には、上記位置検出器の出力を所定のサンプリング周期でコンピュータに取り込み、ソフトウェアにより上記機能を実現するための演算処理を行って、マスタ側、スレーブ側のモータを制御する。

次に、医療用の鉗子を遠隔制御する場合を例として、バイラテラル制御方式に適用した本発明の実施例の位置・力制御装置について説明する。
鉗子は、外科医が手術等に使用する医療器具であり、図２（ａ）に示すように、ハンドル部１０１（以下操作部１０１という）と鉗子部１０２（以下把持部１０２という）から構成される。
鉗子の軸部１０３内を操作部材（図示せず）が貫通しており、操作部１０１のハンドル１ｅを手で開閉することにより、上記操作部材が同図の左右方向に移動する。
把持部１０２は図２（ｂ）に示すように、把持部１０２がリンク機構１０４に連結され、リンク機構１０４の他端側に上記操作部材１０５が連結されており、上記操作部１０１のハンドル１ｅを操作して、上記操作部材を同図の矢印方向に動かすことにより、把持部１０１を開閉させることができる。
本実施例では、上記鉗子を上記操作部１０１と把持部１０２の２つに分けて、操作部１０１をマスタ側、把持部１０２をスレーブ側とし、それぞれにリニアモータを連結し、該リニアモータを前記図１に示した制御系を用いて制御することにより、操作部１０１の操作に応じて、遠隔地に設置された把持部１０２を制御するようにした。
なお、以下では、操作部１０１の操作に応じて把持部を開閉させる１軸制御について説明するが、本実施例で説明する制御系を複数設けることにより操作部１０１の操作に応じて把持部全体を回転させたり、把持部全体を揺動させる等の多軸制御を行うこともできる。

図３、図４は本実施例におけるマスタ側とスレーブ側の装置構成を示す図である。
図３（ａ）はマスタ側装置の上面図、（ｂ）は側面図、（ｃ）は同図（ａ）をＡ方向から見た図である。
同図において、１ａはリニアモータであり、リニアモータ１ａの可動軸１ｄに操作部１ｃを構成するハンドル１ｅの一方が連結され、ハンドル１ｅの他方はリニアモータ１ａのケースに固定されている。従って、手で操作部１ｃのハンドル１ｅを操作することにより、リニアモータ１ａの可動軸１ｄが同図（ａ）（ｂ）の左右方向に動く。１ｂはマスタ側位置検出器でありリニアモータ１ａの可動軸１ｄの位置を検出する。
図４（ａ）はスレーブ側装置の上面図、（ｂ）は側面図、（ｃ）は同図（ａ）をＡ方向から見た図である。
同図において、３ａはリニアモータであり、リニアモータ３ａの可動軸３ｄには、取付け金具３ｃを介して鉗子の軸部３ｄ内を貫通する前記操作部材３ｅが連結され、操作部材３ｅの先端側には前記したようにリンク機構を介して把持部３ｆが取付けられている。
従って、リニアモータ３ａの可動軸３ｄが同図（ａ）（ｂ）の左右方向に動くことにより、操作部材３ｅが左右方向に動き把持部３ｆが開閉する。３ｂはスレーブ側位置検出器であり、リニアモータ３ａの可動軸３ｄの位置を検出する。
なお、上記モータとしては、摩擦力の少ないモータを用いるのが望ましく、上記のようなリニアモータを使用する外、回転運動の場合にはダイレクトドライブモータ等を使用することができる。
また、上記位置検出器としては、高精度の検出器を用いるのが望ましく、また、応答性を向上させるには、位置検出器の検出結果を取り込むサンプリング周期を十分短くすることが必要である。

図５は本実施例の制御系のブロック図である。
同図において、前記図１の示したものと同一のものには同一の符号が付されており、１はマスタであり、マスタ１は図３に示したように、リニアモータ１ａと位置検出器１ｂと操作部１ｃから構成される。マスタ側の反力オブザーバ２は、上記マスタ１のリニアモータ１ａに供給される電流信号Ｉ_ａｍ ^ｒｅｆと、位置検出器１ｂにより検出されるリニアモータ１ａの可動軸１ｄの位置（即ち操作部１ｃの位置）に応じた位置検出信号ｘ_ｍからマスタ側に加わる力の推定値Ｆ_ｍを求める。
３はスレーブであり、スレーブ３は図４に示したように、リニアモータ３ａと位置検出器３ｂと前記機構により動作する把持部３ｆから構成される。
スレーブ側の反力オブザーバ４は、上記スレーブ２のリニアモータ３ａに供給される電流信号Ｉ_ａｓ ^ｒｅｆと、位置検出器３ｂにより検出されるリニアモータ３ａの可動軸３ｄの位置（即ち把持部１ｆの位置）に応じた位置検出信号ｘ_ｓからスレーブ側に加わる力の推定値Ｆ_ｓを求める。

５は位置制御部であり、上記マスタ側の位置検出器１ｂにより検出されたマスタ側の操作部１ｃの位置信号ｘ_ｍと、スレーブ側の位置検出器３ｂにより検出されたスレーブ側の把持部１ｆの位置信号ｘ_ｓとの差を求める減算部５ａと、減算部５ａの出力に（Ｋ_ｖｓ＋Ｋ_ｐ）の演算を施す制御部５ｂと、制御部５ｂの出力に、〔Ｍ_ｓ／（Ｍ_ｍ＋Ｍ_ｓ）〕，〔−Ｍ_ｓ／（Ｍ_ｍ＋Ｍ_ｓ）〕を乗じて加速度参照値ａ_ｐｍ，ａ_ｐｓを生成する変換部５ｃ，５ｄから構成される。なお、ここで、Ｋ_ｐは位置ゲイン、Ｋ_ｖは速度ゲイン、Ｍ_ｍはマスタ側の慣性、Ｍ_ｓはスレーブ側の慣性である。
６は操作力制御部であり、マスタ側の反力オブザーバ２が出力するマスタ側の操作部１ｃに加わる力の推定値Ｆ_ｍとスレーブ側の反力オブザーバ４が出力するスレーブ側の把持部３ｆに加わる力の推定値Ｆ_ｓとの和を出力する加算部６ａと、加算部６ａの出力に力ゲインＫｆを乗算し、加速度参照値ａ_ｆｍ，ａ_ｆｓを生成する変換部６ｂとから構成される。上記力ゲインＫ_ｆは仮想慣性の逆数であり任意の値に設定することができ、Ｋ_ｆを適切に設定することにより、等価質量を見かけ上小さくすることができる。
７は加速度合成部であり、上記加速度参照値ａ_ｐｍ，ａ_ｆｍを加算してマスタ側の加速度参照値（ｘ_ｍ ^ｒｅｆ）^″を出力する加算部７ａと、加算部７ａの出力に〔Ｍ_ｍ／Ｋ_ｔｎｍ〕を乗じて、マスタ側のリニアモータ１ａを駆動する電流参照値Ｉ_ａｍ ^ｒｅｆを生成する変換部７ｂと、上記加速度参照値ａ_ｐｓ，ａ_ｆｓを加算してスレーブ側の加速度参照値（ｘ_ｓ ^ｒｅｆ）^″を出力する加算部７ｃと、加算部７ｃの出力に〔Ｍ_ｓ／Ｋ_ｔｎｓ〕を乗じて、スレーブ側のリニアモータ３ａを駆動する電流参照値Ｉ_ａｓ ^ｒｅｆを生成する変換部７ｄとから構成される。ここで、Ｍ_ｍ，Ｍ_ｓは前記したマスタ側の慣性、スレーブ側の慣性であり、Ｋ_ｔｎｍ，Ｋ_ｔｎｓはそれぞれマスタトルク定数、スレーブトルク定数である。
なお、図面上では、一次微分、２次微分を符号の上にドットを１または２付けて示すが、明細書中では一次微分を「’」を付して示し、２次微分を上記（ｘ_ｍ ^ｒｅｆ）^″のように「”」を付けて示す。

図６は、反力オブザーバ２，４のブロック図である。
同図において、１，３はマスタ側、スレーブ側のブロック図であり、Ｉ_ａ ^ｒｅｆはリニアモータ１ａ，３ａに供給される電流参照値、Ｋｔは推力定数であり、Ｉ_ａ ^ｒｅｆ×Ｋ_ｔはリニアモータの駆動力に相当する。Ｆ_ｌはリニアモータの負荷、Ｍはマスタ側、スレーブ側の慣性であり、リニアモータ１ａ，３ａに電流Ｉ_ａ ^ｒｅｆが供給され、負荷がＦ_ｌのとき、マスタ側、スレーブ側は、同図に示す速度ｘ’で動き、その位置ｘは速度ｘ’を積分した値となる。
反力オブザーバ２，４には上記電流参照値Ｉ_ａ ^ｒｅｆ、位置ｘが入力され、反力オブザーバ２，４は以下の（１）式によりマスタ側、スレーブ側の推定反力Ｆ_ｍ，Ｆ_ｓ（＝Ｆ）を求める。
Ｆ＝〔ｇ／（ｓ＋ｇ）〕×〔Ｉ_ａ ^ｒｅｆＫ_ｔｎ＋ｘＭ_ｎｓｇ−Ｆ_ｉｎｉｔ〕−Ｍ_ｎｓｇ…（１）ここで、Ｆ_ｉｎｉｔ＝Ｆ_ｄｉｓ＋Ｄｘ’＋（Ｍ−Ｍ_ｎ）ｘ’である。また、ｇ／（ｓ＋ｇ）は一次の周波数選択フィルタである。
上記式および図６中の各記号は以下の値を表している。
Ｍ：慣性、Ｍ_ｎ：慣性公称値、Ｋ_ｔｎ：推力定数公称値、ｘ：位置、ｘ’：速度、Ｆ_ｄｉｓ：クーロン摩擦、Ｄ：粘性摩擦係数。
なお、Ｆ_ｄｉｓ，Ｄ，Ｍ，Ｍ_ｎ等は予備実験により求めた既知の値であるとする。
上記反力オブザーバを用いて反力を推定することにより、力センサを用いる場合に比べ、前記したように、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にマスタ側、スレーブ側の反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。

間接的センシングの有効性を確認するため、従来の歪みゲージを用いた力センサを用いて直接力を測定した場合と、反力推定オブザーバを用いて間接的に力を測定した場合を比較した。
図７に実験結果を示す。同図の横軸はω（ｒａｄ／ｓｅｃ）、縦軸はｇａｉｎ（ｄＢ）であり、同図（ａ）は従来の力センサの周波数特性、（ｂ）は反力オブザーバの周波数特性を示す。
同図から明らかなように、従来の力センサでは固有周波数の存在により、測定可能周波数帯域が約５００ｒａｄ／ｓｅｃ以下に限定されてしまうが、反力推定オブザーバでは設定したゲイン１５００ｒａｄ／ｓｅｃまでの広帯域で測定できる。
図８は指令値（印加する力）を正弦波状に変化させて、従来の歪みゲージを用いた力センサと上記反力オブザーバの出力を比較した図である。同図の横軸は時間（秒）、縦軸は力（ニュートン）であり、同図の点線は指令値（与えた力）、破線は上記反力オブザーバによる推定結果、実線は従来の力センサによる検出結果を示す（指令値と反力オブザーバによる推定結果はほぼ重なっている）。
同図の示すように、反力オブザーバの出力は、指令値とほぼ一致しているが、従来の力センサの出力は、上記指令値よりやや遅れており、また、固有周波数や慣性の影響により検出結果が一部振動的に変化している。

図７、図８から明らかなように、反力オブザーバを用いれば、従来の力センサを用いた場合に比べ、広帯域での測定が可能であり、また、殆ど遅れなくマスタ側、スレーブ側に加わる力を推定することができ、これによりマスタ側、スレーブ側を応答よく制御することができる。
上記反力オブザーバは、例えば、「大西，”外乱オブザーバによるロバスト・モーションコントロール”日本ロボット学会誌ｖｏｌ．１１，ｎｏ．４，ｐｐ．４８６〜４９３，１９９３」等に開示されている。
図９は、反力オブザーバの他の構成例を示す図であり、同図は、周波数選択フィルタとして、図６に示した〔ｇ／（ｓ＋ｇ）〕に替え、〔ｋ_１／（ｓ^２＋ｋ_２ｓ＋ｋ_１）〕の伝達関数で表される２次フィルタを用いた場合を示している。
上記〔ｋ_１／（ｓ^２＋ｋ_２ｓ＋ｋ_１）〕の極は、ｓ^２＋ｋ_２ｓ＋ｋ_１＝０から得られ、この極をα，βとすると、α＋β＝−ｋ_２、αβ＝ｋ_１となる。
なお、上記図９に示した反力オブザーバについては、例えば「ｋ．Ｏｈｎｉｓｈｉ，Ｍ．Ｓｈｉｂａｔａ，Ｔ．Ｍｕｒａｋａｍｉ，″Ｍｏｔｉｏｎ Ｃｏｎｔｒｏｌ ｆｏｒ Ａｄｖａｎｃｅｄ Ｍｅｃｈａｔｏｒｏｎｉｃｓ″，ＩＥＥＥ／ＡＳＭＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｍｅｃｈａｔｒｏｎｉｃｓ」等を参照されたい。

次に、図３、図４、図５に示す本実施例の制御装置の動作について説明する。
マスタ側の操作部１ｃを操作すると、リニアモータ１ａの可動軸１ｄが移動し、位置検出器１ａにより可動軸１ｄの位置が検出される。
この位置検出器１ａの出力と、スレーブ側の位置検出器３ａの出力が位置制御部５に送られ、位置制御部５はその偏差に基づき加速度参照値ａ_ｐｍ，ａ_ｐｓを生成し出力する。
一方、マスタ側の反力オブザーバ２はマスタ側に加わる力の推定値Ｆ_ｍを出力する。
操作力制御部６は、上記マスタ側に加わる力の推定値Ｆ_ｍと、スレーブ側の反力オブザーバ４が出力するスレーブ側に加わる力Ｆ_ｓの和を求め、この和から加速度参照値ａ_ｆｍ，ａ_ｆｓを生成し出力する。
上記加速度参照値ａ_ｐｓと加速度参照値ａ_ｆｓは、加速度合成部７に送られ、加速度合成部７は上記加速度参照値ａ_ｐｓと加速度参照値ａ_ｆｓの和から、電流参照値Ｉ_ａｓ ^ｒｅｆを生成する。また、上記加速度参照値ａ_ｐｍと加速度参照値ａ_ｆｍの和から、電流参照値Ｉ_ａｍ ^ｒｅｆを生成する。
上記電流生成値Ｉ_ａｓ ^ｒｅｆはスレーブ側のリニアモータ３ａに与えられ、リニアモータ３ａが駆動される。同様に、上記電流生成値Ｉ_ａｍ ^ｒｅｆはマスタ側のリニアモータ１ａに与えられ、リニアモータ１ａが駆動される。

マスタ側の操作部１ｃを操作することにより、スレーブ側のリニアモータ３ａの可動軸とマスタ側のリニアモータ１ａの可動軸に位置誤差が生ずると、上記加速度合成部７から、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａに上記位置誤差を修正するような駆動信号が与えられ、マスタ側のリニアモータ１ａ、スレーブ側のリニアモータ３ａの可動軸が駆動される。
これにより、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａの可動軸の位置が一致するように制御される。すなわち、マスタ側の操作部１ｃのハンドル１ｅの位置に応じて、スレーブ側の把持部３ｆの開度が制御される。
また、これと同時に、把持部３ｆに加わる力Ｆ_ｓが反力オブザーバ４で推定されるとともに、操作部１ｃに加わる力Ｆ_ｍが反力オブザーバ２で推定され、Ｆ_ｓとＦ_ｍとの和に応じた加速度参照値ａ_ｆｍ、ａ_ｆｓが加速度合成部７に与えられる。
加速度合成部７は、把持部３ｆに加わる力Ｆ_ｓと操作部１ｃに加わる力Ｆ_ｍの和に応じて、マスタ側のリニアモータ１ａとスレーブ側のリニアモータ３ａを駆動する。
これにより、マスタ側の操作部１ｃに加わる力とスレーブ側の把持部３ｆに加わる力との和に倣うように、マスタ側の操作部１ｃとスレーブ側の把持部３ｆが制御される。
すなわち、マスタ側の操作部１ｃを操作して、スレーブ側の把持部３ｆで物体を把持すると、把持部３ｆに加わる反力に応じた力が操作部１ｃに伝わり、これにより、作業者は把持部３ｆで把持した物体からの反力等を操作部１ｃで感じ取ることができる。
以上の説明では、マスタ側を操作した場合について説明したが、スレーブ側を操作した場合でも上記と同様にマスタ側が制御され、スレーブ側にマスタ側の力が伝えられる。

本実施例のバイラテラル制御装置は、上記のように、マスタ側の操作部、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、マスタ側の操作部、スレーブ側の対象物が受ける反力を推定しているので、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、マスタ側とスレーブ側の位置誤差を生じさせることなく、位置および操作力を応答性よく制御することができる。
また、位置制御部５と操作力制御部６を設けて、加速度合成部で位置制御部５と操作力制御部６で生成した加速度参照値を合成して、リニアモータを駆動しているので、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。
このため、マスタ側とスレーブ側の位置誤差が無くなるように位置制御部５を設定することができ、また、マスタ側とスレーブ側の反力の和に倣うように両者を制御することができる。さらに、操作力制御部６の力ゲインＫｆの設定により、マスタ側の操作力を自由に設定することができる。

本実施例のバイラテラル制御装置により前記鉗子を遠隔制御して、本発明の有効性を確認した。
実験では、制御装置をコンピュータで構成し、位置検出器として、分解能が１μｍ程度のリニアエンコーダを用い、１００μｓ程度のサンプリング周期でコンピュータで構成された制御装置に取り込んで、鉗子をバイラテラル制御した。これにより、１６０Ｈｚ程度の応答性を得ることができた。
図１０、図１１に上記実験結果を示す。
なお、図１０、図１１の横軸は時間（ｓｅｃ）、図１０の縦軸は位置、図１１の縦軸は力（ニュートン）であり、位置、力をステップ状に変化させたときの応答特性を示す。
前記した従来のバイラテラル制御系では環境接触時において位置偏差が生じてしまい位置再現性が悪かったが、本発明においては、計算機のサンプリング時間を十分に短して周波数帯域を十分広く確保し、位置制御部５の制御ゲインを上げることにより、図１０に示すように位置偏差を生じることなくリアルタイムに環境を再現できた。

以上説明した実施例では、本発明をバイラテラル制御に適用する場合について説明したが、本発明は、上記バイラテラル制御に限らず、位置指令値、力指令値に応じてスレーブ側を制御する制御装置に適用することもできる。
図１２に上記のように位置指令値、力指令値に応じてスレーブ側を制御する本発明の第２の実施例の位置・力制御装置の概略構成を示す。
同図において、１１は位置指令値と力指令値を発生する位置／力指令発生部であり、位置／力指令発生部１１が出力する位置指令値ｘ_ｒｅｆは位置制御部１５に与えられ、力指令値ｆ_ｒｅｆは、操作力制御部１６に与えられる。
１３は上記位置指令、力指令に応じて動作するスレーブであり、前記したように、対象物を駆動するスレーブ側モータと、該モータの位置を検出するスレーブ側位置検出器から構成される。上記対象物とは、例えば、各種作業を行うロボットのハンド等、遠隔地において実際に作業を行う操作部である。
１４は反力オブザーバであり、前記したように、スレーブ側位置検出器の出力と、上記スレーブ側への駆動信号から上記対象物に加わる力を求める。
上記位置指令値ｘ_ｒｅｆとスレーブ側に設けられた位置検出器により検出された対象物の位置信号ｘ_ｓは、それぞれ位置制御部１５に送られ、位置制御部１５は上記位置信号を加速度参照値ａ_ｐｓに変換する。
また、力指令値ｆ_ｒｅｆとスレーブ側の反力オブザーバ１２により検出されたスレーブ側の力信号ｆ_ｓは操作力制御部１６に送られ、操作力制御部１６は、上記力信号ｆ_ｒｅｆ，ｆ_ｓを加速度参照値ａ_ｆｓに変換する。
加速度合成部１７は、上記加速度参照値ａ_ｐｓ、加速度参照値ａ_ｆｓを合成し、スレーブ側モータへの駆動信号となる加速度指令信号ａ_ｓを出力する。

上記位置／力指令発生部１１が発生する位置指令値、力指令値は、予めプログラミングされた値であり、例えば、熟練した作業者等による操作等を記憶することにより得ることができる。
例えば、前記実施例に示したバイラテラル制御装置を用いて熟練した作業者により所望の作業をしてもらい、マスタ側（もしくはスレーブ側）の位置ｘ_ｍ（またはｘ_ｓ）、応力オブザーバの出力ｆ_ｍ（またはｆ_ｓ）を、それぞれ位置指令値、力指令値として記憶しておく。そして、この記憶された位置指令値、力指令値を上記位置／力指令発生部１１から出力し、スレーブ側を制御することにより、熟練した作業者による作業を再現することができる。

図１３は、本実施例の制御系のブロック図である。
同図において、１１は上記位置／力指令発生部、１３はスレーブであり、スレーブ１３は前記したように、リニアモータ等のモータと、位置検出器を備える。
１４は反力オブザーバであり、前記図６、図９に示した構成を備え、スレーブ１３のモータに供給される電流信号Ｉ_as ^ref と、位置検出器により検出される位置に応じた位置検出信号ｘ_s からスレーブ側に加わる力の推定値Ｆ_s を求める。
１５は位置制御部であり、上記位置／力指令発生部１１が出力する位置指令値ｘ_ref と、とスレーブ側の位置検出器により検出されたスレーブ側の位置信号ｘ_s との差を求める減算部１５ａと、減算部１５ａの出力に（Ｋ_v ｓ＋Ｋ_p ）の演算を施す制御部１５ｂと、加速度参照値ａ_psを生成する変換部１５ｃから構成される。なお、ここで、Ｋ_p は位置ゲイン、Ｋ_v は速度ゲインである。
１６は操作力制御部であり、力指令値ｆ_ref と反力オブザーバ１４が出力するスレーブ側の対象物に加わる力の推定値Ｆ_s との和を出力する加算部１６ａと、加算部１６ａの出力に力ゲインＫ_f を乗算し、加速度参照値ａ_fsを生成する変換部１６ｂとから構成される。上記力ゲインＫ_f は前記したように仮想慣性の逆数であり任意の値に設定することができ、Ｋ_f を適切に設定することにより等価質量を見かけ上小さくすることができる。
７は加速度合成部であり、上記加速度参照値ａ_ps，ａ_fs を加算してスレーブ側の加速度参照値（ｘ_s ^ref ）^" を出力する加算部１７ａと、加算部１７ａの出力に〔Ｍs ／Ｋ_tns 〕を乗じて、スレーブ側のモータを駆動する電流参照値Ｉ_as ^ref を生成する変換部１７ｂとから構成される。ここで、Ｍ_s はスレーブ側の慣性であり、Ｋ_tns はスレーブトルク定数である。

次に、図１３に示す本実施例の制御装置の動作について説明する。
位置／力指令発生部１１が出力する位置指令値ｘ_ref とスレーブ側の位置検出器３ａの出力が位置制御部１５に送られ、位置制御部１５はその偏差に基づき加速度参照値ａ_psを生成し出力する。
一方、操作力制御部１６は、上記位置／力指令発生部１１が出力する力指令値ｆ_ref と反力オブザーバ４が出力するスレーブ側に加わる力Ｆ_s の和を求め、この和から加速度参照値ａ_fsを生成し出力する。
上記加速度参照値ａ_psと加速度参照値ａ_fSは、加速度合成部１７に送られ、加速度合成部１７は上記加速度参照値ａ_psと加速度参照値ａ_fsの和から、電流参照値Ｉ_as ^ref を生成する。上記電流生成値Ｉ_as ^ref はスレーブ側のモータに与えられ、モータが駆動される。
位置指令値ｘ_ref が変化すると、スレーブ側のモータの位置との位置誤差が生じ、加速度合成部１７から、スレーブ側のモータに上記位置誤差を修正するような駆動信号が与えられ、スレーブ側のモータが駆動される。
これにより、上記位置指令値にスレーブ側の位置が一致するように制御される。
また、これと同時に、スレーブ側に加わる力Ｆ_s が反力オブザーバ４で推定され、力指令値ｆ_ref とＦ_s との偏差に応じた加速度参照値ａ_fsが加速度合成部１７に与えられる。
加速度合成部１７は、力指令値ｆ_ref とスレーブ側の力Ｆ_s の大きさが異なると、この大きさが一致するような駆動信号をスレーブ側のモータに与える。これにより、スレーブ側に加わる力が力指令値ｆ_ref に一致するように制御される。

本実施例においては、上記のように、スレーブ側の対象物の位置を検出する位置検出手段を設け、位置検出手段の出力に基づき、反力オブザーバにより、対象物が受ける反力を推定しているので、第１の実施例と同様、信号ノイズやセンサ自体の固有周波数やセンサ慣性に影響されることなく、かつ安価にスレーブ側の対象物に加わる反力を推定することができる。またクーロン摩擦、粘性摩擦係数による影響を最小限にすることもできる。
このため、位置検出手段として高精度のセンサを用い、位置検出結果を比較的短い周期でサンプリングし、位置制御部の制御ゲインを大きく設定すれば、位置誤差を生じさせることなく、スレーブ側の位置、操作力を応答性よく制御するこができる。
また、位置制御部１５と操作力制御部１６を設けて、加速度合成部１７で位置制御部１５と操作力制御部１６で生成した加速度参照値を合成して、スレーブ側の対象物を駆動しているので、第１の実施例と同様、位置制御のゲインと操作力制御のゲインを独立に設定することが可能となる。

本発明は、医療分野、建築用作業ロボット、食品加工や食品のハンドリング、アッセッブル、楽器の操作等の各種分野に適用することができ、バイラテラル制御系に適用すれば、マスタ側の操作力をスレーブ側に応答性よく伝えることができるとともに、スレーブ側に加わる力をマスタ側に応答性よく伝えることができ、繊細な作業を実現することが可能となる。
例えば、本発明を医療分野で使用される鉗子等の遠隔制御に適用すれば、従来の遠隔手術では実現することができなかった繊細な感覚情報を伝えることができ、鉗子を直接手で操作する場合と同様な感覚で使用することが可能となる、また、繊細な感覚情報を伝えることが必要な触診等を遠隔から行うことも可能となる。
また、建築用作業ロボットに適用することにより、セメント塗り等の作業を熟練した作業者が直接手で塗ったのと同様に行うことができ、さらに、食品のハンドリング作業やアッセンブル、楽器の操作等においても、人手による作業や操作と同様に行うことが可能となる。

Claims (3)

1. 対象物の位置を検出する位置検出手段と、
   上記対象物を駆動する駆動手段と、
   上記位置検出手段が出力する位置信号と、上記駆動手段への加速度指令信号である駆動信号に基づき上記対象物が受ける反力を求める反力検出手段と、
   上記対象物が受ける反力と目標となる力信号とから第１の加速度信号を求めるとともに、上記位置信号と目標位置とから第２の加速度信号を求め、上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
   ことを特徴とする位置・力制御装置。
2. 対象物の位置及び該対象物に作用する力を位置指令信号、力指令信号に応じて制御する位置・力制御装置であって、
   上記対象物を駆動する駆動手段と、
   上記対象物の位置を検出する位置検出手段と、
   上記位置検出手段が出力する位置信号と、上記駆動手段への加速度指令信号である駆動信号から、該対象物が受ける反力を求める反力検出手段と、
   位置指令信号と上記位置検出手段が出力する位置信号との偏差を求め、該偏差信号を第１の加速度信号に変換する第１の演算手段と、
   上記反力検出手段により検出された上記反力と、力指令信号との和を求め、該和を第２の加速度信号に変換する第２の演算手段と、
   上記第１、第２の加速度信号とを加算して、上記駆動手段への駆動信号となる加速度指令信号を出力する制御手段を備えた
   ことを特徴とする位置・力制御装置。
3. マスタ側の操作部とスレーブ側の対象物の位置偏差に応じて、スレーブ側の対象物とマスタ側の操作部の位置を制御し、マスタ側の操作力に応じた駆動力で対象物を駆動するとともに、対象物が受ける力をマスタ側に伝える位置・力制御装置であって、
   マスタ側の操作部を駆動する第１の駆動手段と、
   マスタ側の操作部の位置を検出する第１の位置検出手段と、
   上記第１の位置検出手段が出力する位置信号と、上記第１の駆動手段への加速度指令信号である駆動信号から、上記操作部に作用する反力を求める第１の反力検出手段と、
   スレーブ側の対象物を駆動する第２の駆動手段と、
   スレーブ側の対象物の位置を検出する第２の位置検出手段と、
   上記第２の位置検出手段が出力する位置信号と、上記第２の駆動手段への加速度指令信号である駆動信号から、上記対象物が受ける反力を求める第２の反力検出手段と、
   上記第１の位置検出手段が出力する位置信号と、第２の位置検出手段が出力する位置信号との偏差を求め、該偏差をマスタ側およびスレーブ側を制御するための第１、第２の加速度信号に変換する第１の演算手段と、
   上記第１の反力検出手段と、上記第２の反力検出手段の出力の和を求め、該和をマスタ側およびスレーブ側を制御するための第３、第４の加速度信号に変換する第２の演算手段と、
   上記第１，第３の加速度信号を加算する第１の加算手段と、
   上記第２，第４の加速度信号を加算する第２の加算手段と、
   上記第１の加算手段の出力に基づき、上記マスタ側の操作部への駆動信号となる加速度指令信号を出力する第１の制御手段と、
   上記第２の加算手段の出力に基づき、上記スレーブ側の対象物への駆動信号となる加速度指令信号を出力する第２の制御手段とを備えた
   ことを特徴とする位置・力制御装置。

Images