(57)【要約】【課題】  重力や摩擦力などの影響を受けることなく対象物に作用する純粋な外力の検出を可能とするとともに、検出した純粋な外力により対象物を制御すること。【解決手段】  作業用のロボット１ａに加え、ロボット１ａと同一構造を有するダミーのロボット１ｂを用意し、作業用ロボット１ａのみに外力を作用させ、第１の外乱推定手段により、作業用ロボット１ａに作用する外乱力を同定する。また、第２の外乱推定手段により、外力を作用させないダミーロボット１ｂに加わる外乱力を同定し（この場合外力が作用しないので重力や摩擦力等だけとなる）、第１の外乱推定手段の出力から第２の外乱推定手段の出力を引くことで、純粋な外力の検出を可能とする。また、このようにして検出した外力を用いて上記ロボットを制御する。この方法は同定試験などを必要とせず、また多自由度マニピュレータへの適用も容易である。【選択図】          図１
2005212054.tif 000002
【特許請求の範囲】
【請求項１】
第１の制御対象物に作用する外力を検出する力検出方法であって、
上記第１の制御対象物と同一構造を有し、外力が作用しない第２の制御対象物を設け、  上記第１の制御対象物へ与えられる駆動力と、第１の制御対象物の位置／角度に対応する値に基づき、第１の制御対象物に作用する外乱力を検出し、
上記第２の制御対象物に与えられる駆動力と、第２の制御対象物の位置／角度に対応する値に基づき、第２の制御対象物に作用する外乱力を検出し、
上記検出された第１の制御対象物に作用する外乱力と、上記第２の制御対象物に作用する外乱力の差を求めることにより、上記第１の制御対象物の作用する外力を検出する
ことを特徴とする力検出方法。
【請求項２】
外力が作用する第１の制御対象物と、
上記第１の制御対象物の位置／角度を検出する第１の位置／角度検出手段と、
上記第１の制御対象物へ与えられる駆動力と第１の制御対象物の位置／角度に対応した値に基づき、上記第１の制御対象物に作用する外乱力を検出する第１の外乱推定手段と、  上記第１の制御対象物と同一の構造を有し、外力が作用しない第２の制御対象物と、  上記第２の制御対象物の位置／角度を検出する第２の位置／角度検出手段と、
上記第２の制御対象物へ与えられる駆動力と第２の制御対象物の位置／角度に対応した値に基づき、第２の制御対象物に作用する外乱力を検出する第２の外乱推定手段と、
上記第１の外乱推定手段により検出された外乱力と、上記第２の外乱推定手段により検出された外乱力との差を求めることにより、上記第１の制御対象物に作用する外力を求める力検出手段を備えた
ことを特徴とする力検出装置。
【請求項３】
外力が作用する第１の制御対象物と、
上記第１の制御対象物の位置／角度を検出する第１の位置／角度検出手段と、
上記第１の制御対象物へ与えられる駆動力と第１の制御対象物の位置／角度に対応した値に基づき、上記第１の制御対象物に作用する外乱力を検出する第１の外乱推定手段と、  上記第１の制御対象物と同一の構造を有し、外力が作用しない第２の制御対象物と、  上記第２の制御対象物の位置／角度を検出する第２の位置／角度検出手段と、
上記第２の制御対象物へ与えられる駆動力と第２の制御対象物の位置／角度に対応した値に基づき、第２の制御対象物に作用する外乱力を検出する第２の外乱推定手段と、
上記第１の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにより、上記第１の制御対象物に作用する外力を求める外力検出手段と、
上記第１、第２の位置／角度検出手段が出力する位置／角度検出信号の和に基づき位置／角度指令信号を発生する位置／角度指令信号発生手段と、
上記位置／角度指令信号と上記第１の対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき、上記第１の制御対象物を制御する第１の制御手段と、
上記位置／角度指令信号と上記第２の対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき、上記第２の制御対象物を制御する第２の制御手段とを備えた
ことを特徴とする力検出機能を備えた制御装置。
【請求項４】
スレーブ側に設けられ、外力が作用する第１の制御対象物と、
上記第１の制御対象物の位置／角度を検出する第１の位置／角度検出手段と、
上記第１の制御対象物へ与えられる駆動力と第１の制御対象物の位置／角度に対応した値に基づき、上記第１の制御対象物に作用する外乱力を検出する第１の外乱推定手段と、  マスタ側に設けられ、外力が作用する第２の制御対象物と、
上記第２の制御対象物の位置／角度を検出する第２の位置／角度検出手段と、
上記第２の制御対象物へ与えられる駆動力と第２の制御対象物の位置／角度に対応した値に基づき、第２の制御対象物に作用する外乱力を検出する第２の外乱推定手段と、
スレーブ側もしくはマスタ側に設けられ、上記第１、第２の制御対象物と同一の構造を有し、外力が作用しない第３の制御対象物と、
上記第３の制御対象物の位置／角度を検出する第３の位置／角度検出手段と、
上記第３の制御対象物へ与えられる駆動力と第３の制御対象物の位置／角度に対応した値に基づき、第３の制御対象物に作用する外乱力を検出する第３の外乱推定手段と、
上記第１の外乱推定手段により検出された外乱力と上記第３の外乱推定手段により検出された外乱力との差を求めることにより、上記第１の制御対象物に作用する外力を求める第１の外力検出手段と、
上記第２の外乱推定手段により検出された外乱力と上記第３の外乱推定手段により検出された外乱力との差を求めることにより、上記第２の制御対象物に作用する外力を求める第２の外力検出手段と、
上記第１、第２、第３の位置／角度検出手段が出力する位置／角度検出信号の和に基づき位置／角度指令信号を発生する位置／角度指令信号発生手段と、
上記第１、第２の外力検出手段が出力する外力の和を求める手段と、
上記位置／角度指令値と第１の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第１の制御対象物を制御する第１の制御手段と、
上記位置／角度指令値と第２の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第２の制御対象物を制御する第２の制御手段と、
上記位置／角度指令値と第３の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第３の制御対象物を制御する第３制御手段とを備えた
ことを特徴とする力検出機能を備えた制御装置。
【請求項５】
スレーブ側に設けられ、外力が作用する第１の制御対象物と、
上記第１の制御対象物の位置／角度を検出する第１の位置／角度検出手段と、
上記第１の制御対象物へ与えられる駆動力と第１の制御対象物の位置／角度に対応した値に基づき、上記第１の制御対象物に作用する外乱力を検出する第１の外乱推定手段と、  スレーブ側に設けられ、上記第１の制御対象物と同一の構造を有し、外力が作用しない第２の制御対象物と、
上記第２の制御対象物の位置／角度を検出する第２の位置／角度検出手段と、
上記第２の制御対象物へ与えられる駆動力と第２の制御対象物の位置／角度に対応した値に基づき、第２の制御対象物に作用する外乱力を検出する第２の外乱推定手段と、
マスタ側に設けられ、外力が作用する第３の制御対象物と、
上記第３の制御対象物の位置／角度を検出する第３の位置／角度検出手段と、
上記第３の制御対象物へ与えられる駆動力と第３の制御対象物の位置／角度に対応した値に基づき、第３の制御対象物に作用する外乱力を検出する第３の外乱推定手段と、
マスタ側に設けられ、上記第３の制御対象物と同一の構造を有し、外力が作用しない第４の制御対象物と、
上記第４の制御対象物の位置／角度を検出する第４の位置／角度検出手段と、
上記第４の制御対象物へ与えられる駆動力と第４の制御対象物の位置／角度に対応した値に基づき、第４の制御対象物に作用する外乱力を検出する第４の外乱推定手段と、
上記第１の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにより、上記第１の制御対象物に作用する外力を求める第１の外力検出手段と、
上記第３の外乱推定手段により検出された外乱力と上記第４の外乱推定手段により検出された外乱力との差を求めることにより、上記第３の制御対象物に作用する外力を求める第２の外力検出手段と、
上記第１、第２、第３、第４の位置／角度検出手段が出力する位置／角度検出信号の和に基づき位置／角度指令信号を発生する位置／角度指令信号発生手段と、
上記第１、第２の外力検出手段が出力する外力の和を求める手段と、
上記位置／角度指令値と第１の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第１の制御対象物を制御する第１の制御手段と、
上記位置／角度指令値と第２の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第２の制御対象物を制御する第２の制御手段と、
上記位置／角度指令値と第３の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第３の制御対象物を制御する第３制御手段と、
上記位置／角度指令値と第４の制御対象物の位置／角度検出信号との偏差信号と、上記外力との和に基づき、上記第４の制御対象物を制御する第４制御手段とを備えた
ことを特徴とする力検出機能を備えた制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、重力や摩擦力などの影響を受けることなく対象物に作用する力を精度よく検出し、また、対象物に作用する力を精度よく制御することができる力検出方法および装置並びに力検出機能を備えた制御装置に関する。
【背景技術】
【０００２】
ロボットがどのような環境に対しても所望の力を安定して発生させることのできる力制御の実現は、産業応用上非常に重要な課題となっている。また、近年、様々なハプティックデバイスが研究、開発されている。ハプティックデバィスは遠隔操作、バーチャルリアリティなどへの応用が期待されており、将来私たちの生活に多大な貢献をすることが予想される。しかし、高精度なバイラテラル制御系の実現が困難であったのは高精度な力制御系が実現できていないにもかかわらず、バイラテラル制御において安定性と透明性を追求した点である。
従来の研究では新しい力制御理論を構築することに重点が置かれ、ほとんどのものが、力センサにより力を検出していた。しかし、力センサにより検出された値のフィードバックが、力制御の実現を困難なものにしていたことの最大の理由である。言い換えれば、力制御の実現はいかに正確な力の検出ができるかによって決まるといっても過言ではない。
【０００３】
力センサは、ひずみゲージを用いて外力より生ずる構造体のひずみを直接検出し、力に変換を行っている。このため、環境自体が柔らかく接触動作時にひずむ場合において正確な反作用力の計測が困難となっている。また、当然のことながら力センサを設置した部分のみでしか力を計測することができない。よって、外力の加わる部分をあらかじめ想定しておかなければならないので、複雑な環境に対応することができないということも問題となるといえる。さらに、力センサによる検出の周波数帯域が狭いことが力制御系に大きな影響を及ぼしている。
すなわち、従来の力センサによる力検出には、以下の問題がある。
(i) 信号ノイズの問題
力センサや加速度センサによる力の検出では微小なアナログ信号を増幅して検出しているが、信号に含まれるノイズも増幅してしまう。高周波ノイズに関しては低周波域通過フィルタを通すことによりその影響を除去することができるが、その場合には必要としている信号の周波数帯域までも狭めてしまう。
(ii)センサの固有周波数の問題
力センサを用いた力検出では、環境を直接力センサに接触させて測定する。力センサには物理的な固有周波数が存在し、それ以上高い周波数帯域での力測定は不可能である。
(iii) システムモデルの変化
力センサはシステムに直接設置するため、センサ慣性がそのままシステムモデルに加わってくる。また力センサでは加わった力に比例した歪みを利用して力を測定するが、このため力センサには物理的に剛性の低い材料が用いられている。しかし環境との接触を伴うシステムでは、この低剛性の力センサによりシステムモデルが変化してしまう。モデルの変化はシステムの制御においては致命的な問題である。
【０００４】
一方、上記力センサに代えて非特許文献１に記載される反力推定オブザーバを用い、対象物の位置／速度情報と、対象物を駆動する駆動手段への駆動信号に基づき、対象物が受ける力を推定すれば、ノイズの影響を受けることなく、また、センサの固有周波数の問題や、システムモデルが変化してしまうといった問題が生ずることなく、対象物に作用する力を推定することができる。
図１２に上記反力推定オブザーバのブロック図を示す。
反力推定オブザーバは、アクチュエータで構成される駆動系における反作用トルク（反作用力) を推定するものであり、図１２に示すように、駆動系の入力電流Ｉref と、回転速度θ’を取り込み、重力項、摩擦項等を引くことにより、駆動系に作用する反作用トルクτ＾reacを推定する。なお、図中では速度、加速度等の微分信号を、上にドット１つあるいはドット２つを付して示すが、文中では、上記のように「’」，「”」を付して示す。推定された外乱は次の（１）式のようになる。
【０００５】
【数１】
2005212054.tif 000003
【０００６】
ここで、上記（１）の各パラメータは次の値を表す。
τ＾reac  ：反作用トルク
τint   ：干渉トルク
τg     ：重力トルク
Ｆ      ：クローン摩擦
Ｄθ’  ：粘性摩擦
ｇreac  ：反力推定オブザーバゲイン
Ｉa ref ：電流参照値
Ｋtn    ：ノミナルトルク定数
Ｊn     ：ノミナル慣性
上記反力推定オブザーバに基づく反力検出においては、サンプリング周波数及びオブザーバゲインの設定により、力センサをはるかに凌駕したセンシングの周波数帯域の確保が可能となる。
【０００７】
図１２に示した反力推定オブサーバにより力を検出しフィードバックさせたときの様子を図１３（ａ）（ｂ）に、また、上記反力推定オブサーバに代えて、従来のひずみゲージ等を用いた力センサにより、力情報をフィードバックさせたときの様子を図１４（ａ），（ｂ）に示す。なお、図１３（ａ）、図１４（ａ）の横軸は時間（ｓ）、縦軸は力（Ｎ）、図１３（ｂ）、図１４（ｂ）の横軸は時間（ｓ）、縦軸は位置（ｍ）であり、図１３、図１４におけるｃｍｄは指令値である。
図１３、図１４より、力制御に力センサ情報を用いると安定な環境との接触が保つことができないが、反力推定オブサーバを用いた場合は環境との安定な接触を維持することが分かる。
【０００８】
特に、図１３（ａ）、図１４（ａ）において力センサの値と反力推定オブザーバの値に違いが見られる。
一般的に、センサには比例型と平衡型のものがあり、平衡型の方が精度がよいということが知られている。
比例型センサとは、従来型の歪ゲージなどを用いたセンサであり、対象物に作用する力そのものを検出するセンサであり、上記力センサのように、その構造上、位置の偏差を伴わないと力を計測することができない。
一方、平衡型センサとは、作用した力に対する反力を求めるセンサであり、例えば上記反力推定オブサーバのようなセンサを言う。反力推定オブザーバはモータに加わる反力を計算で求めるため、位置によらず測定が可能である。
【０００９】
反力推定オブサーバを用いた場合、外力はオブザーバゲインｇreac（図１２参照）で決まる一次のローパスフイールタを通って推定される。外乱推定手段を用いた力検出においては、サンプリング周波数及び上記オブザーバゲインの設定により帯域を任意に設定できる。
前記力センサと反力推定オブザーバの力検出情報をそれぞれフィードバックしたときの周波数応答を図１５に示す。同図の白四角は、外乱推定手段による力検出信号をフィードバックした場合を示し、黒菱形は力センサによる力検出信号をフィードバックした場合を示す。同図に示すように、反力推定オブザーバを用いた場合、従来力センサを用いた場合に比べ、高い周波数帯域まで速い応答特性が得られることが分かる。
【非特許文献１】村上俊之, 中村亮, 郁方銘, 大西公平: " 反作用力推定オブザーバに基づいた多自由度ロボットの力センサレスコンプライアンス制御" 日本ロボット学会誌, Vol.11, No. 5, pp.765-768, (1993)
【発明の開示】
【発明が解決しようとする課題】
【００１０】
力センサはひずみゲージを用いて僅かな変位を増幅させて力情報へと変換をおこなっている。よって、力センサの内部に柔らかい機構を持っているため、環境自体がひずむ場合において正確な反作用力を検出することができない。これは力センサが比例型のセンサであることを意味している。また、ノイズも多く周波数帯域を広く取ることができない。
一方、前記したように、反力推定オブザーバによりセンサレスで力検出を行う方法が提案されている。反力推定オブザーバはあらかじめ重力項、摩擦項などを同定試験により求め、総外乱から引くことにより環境からの反作用力を得ようとするものであり、反力推定オブザーバは平衡型のセンサとみなすことができるため、環境によらず反作用力の測定が可能である。
しかし、摩擦項は図１６に示すようにロボットの速度によって変化する非線形な外乱であり、非常に非線形性が強いために同定を行うことが非常に困難であり、同定如何によっては接触動作自体が保てなくなってしまう。そのため多自由度マニピュレータへ適用することができなかった。
【００１１】
このように、力のセンシングの精度が低いゆえに生じていた力制御の不安定性を、従来においては、制御で粘性を付加することによって安定化してきた。しかし、これによってますます力の周波数帯域は狭くなり接触動作の実現を困難なものにしてしまっていた。
重力は定常的に加わる外乱であるため、重力項の補償が接触動作に与える影響はそれほど大きなものではないものの、外力が加わっていない状態にロボットが動いてしまうため、操作感に影響を与えることになる。また、多自由度マニピュレータでは姿勢の変動により根元の関節ほど同定が困難となる。
本発明は上記従来技術の問題点を解決するためになされたものであって、本発明の目的は、重力や摩擦力などの影響を受けることなく対象物に作用する純粋な外力の検出を可能とするとともに、検出した純粋な外力により対象物を制御することである。
【課題を解決するための手段】
【００１２】
本発明では、ツインロボットシステムを用いて力（トルクも含む) 検出を行う新しい力検出手法およびこの力検出を用いた制御手法を提案する。
すなわち、本発明においては、以下のようにして、力の検出および力制御を行う。
（１）第１の制御対象物（例えばロボット）と同一構造を有し、常に外力を加えない第２の制御対象物（例えばロボット）をもう１台用意し、前記平衡型センサである第１、第２の外乱推定手段により、上記第１、第２の制御対象物に加わる外力と、重力、摩擦力、内部干渉力等の総和である外乱力を検出する。そして、第１、第２の外乱推定手段により推定された外乱力の差を求めることにより、第１の制御対象物に作用する外力を検出する。これにより純粋な外力の検出が可能となる。
（２）上記力検出手法を用いて、以下のようにして力制御を行う。
外力が作用する第１の制御対象物と、上記第１の制御対象物と同一の構造を有し、外力が作用しない第２の制御対象物と、第１の制御対象物に作用する外乱力を検出する第１の外乱推定手段と、上記第２の制御対象物に作用する外乱力を検出する第２の外乱推定手段を設け、上記第１の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにより、上記第１の制御対象物に作用する外力を求める。
そして、上記第１、第２の制御対象物の位置／角度を検出し、その和に基づき位置／角度指令信号を発生し、上記位置／角度指令信号と上記第１の対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき上記第１の制御対象物を制御するとともに、上記位置／角度指令信号と上記第２の対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき、上記第２の制御対象物を制御する。
上記のように構成することにより、第１の制御対象物に作用する純粋な外力を検出し、制御することができる。
（３）上記力制御をバイラテラル制御に適用し、マスタ側の制御対象物とスレーブ側の制御対象物を以下のように制御する。
(i) 外力が作用するマスタ側の第１の制御対象物と、外力が作用するスレーブ側の第３の制御対象物と、上記第１、第３の制御対象物と同一の構造を有し、外力が作用しない第２の制御対象物と、第１、第２、第３の制御対象物に作用する外乱力を検出する第１、第２、第３の外乱推定手段を設け、上記第１の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにマスタ側の第１の制御対象物に作用する外力を求め、また、上記第３の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにスレーブ側の第３の制御対象物に作用する外力を求める。
そして、上記第１、第２、第３の制御対象物の位置／角度を検出し、その和に基づき位置／角度指令信号を発生し、上記位置／角度指令信号と上記第１の制御対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき上記第１の制御対象物を制御するとともに、上記位置／角度指令信号と上記第２の対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき、上記第２の制御対象物を制御し、さらに、上記位置／角度指令信号と上記第３の制御対象物の位置／角度との偏差と、上記外力検出手段により検出された外力とに基づき上記第３の制御対象物を制御する。
(ii)外力が作用するマスタ側の第１の制御対象物と、第１の制御対象物と同一の構造を有し外力が作用しないマスタ側の第２の制御対象物と、外力が作用するスレーブ側の第３の制御対象物と、第３の制御対象物と同一の構造を有し、外力が作用しないスレーブ側の第３の制御対象物と、第１、第２、第３、第４の制御対象物に作用する外乱力を検出する第１、第２、第３、第４の外乱推定手段とを設け、上記第１の外乱推定手段により検出された外乱力と上記第２の外乱推定手段により検出された外乱力との差を求めることにマスタ側の第１の制御対象物に作用する外力を求め、また、上記第３の外乱推定手段により検出された外乱力と上記第４の外乱推定手段により検出された外乱力との差を求めることにスレーブ側の第３の制御対象物に作用する外力を求める。そして、上記(i) と同様に第１、第２、第３の制御対象物を制御する。
【発明の効果】
【００１３】
本発明においては、以下の効果を得ることができる。
（１）平衡型センサである外乱推定手段をツインロボットシステムに適用して、純粋な外力のみを検出するようにしたので、ノイズにも強く、広帯域のセンシングが可能となる。このため、今までの力制御において常識であった粘性項の付加を必要とせず、また、煩雑な同定試験などを必要とせず、また多自由度マニピュレータへの適用も非常に容易である。
（２）環境からの忠実な加速度情報を得ることができるので、力制御のみならずバイラテラル制御に適用することで、より効果を得ることができ、広帯域でノイズの少ない力覚センシングにより、高精度の再現性、操作性、安定性の確保を実現することができる。
【発明を実施するための最良の形態】
【００１４】
以下、本発明の実施の形態について説明する。
まず、本発明で使用される外乱オブザーバの構成とその特性について説明する。ここでは、まず簡単な駆動モデルを使って外乱トルクとその計算法について説明する。
モータの発生トルクをτm ，負荷トルクをτl 、モータ軸慣性をＪm ，角速度をθとすると、モータの動力学関係式は次の（２）式のように表すことができる。
Ｊm θ”＝τm −τl …（２）
（２）式をブロック線図で表したものを図２（ａ）に示す。
モータが発生するトルクは電流Ｉa にトルク係数Ｋt をかけることで表現されるので（３）式のように表される。
τm ＝Ｋt Ｉa ＝Ｋt Ｉa ref …（３）
ここで、Ｉa ref は電流参照値を表す。
また、図２において負荷トルクτl の中身を考えると以下の３つが考えられる。
・内部干渉力τint 
コリオリ力、遠心力など接触作業をしなくてもモータ軸が受ける負荷
・外力τext 
ロボットが接触作業によって対象物から受ける反作用力による負荷
・摩擦力Ｆ＋Ｄθ’
クーロン摩擦と粘性摩擦係数による摩擦
したがって上記負荷トルクτl は次の（４）式で表される。
τl ＝τint ＋τext ＋（Ｆ＋Ｄθ’）…（４）
従って、運動方程式は以下の（５）式と書き直すことができる。
Ｊｍθ”＝Ｋt Ｉa −（τint ＋τext ＋Ｆ＋Ｄθ’）…（５）
【００１５】
ここでのパラメータは慣性モーメントJ とトルク係数Ｋt の２つである。このうち、慣性モーメントＪはロボットによって大きく変動し、その公称値から以下の（６）式のようにΔJ だけずれる。
Ｊ＝Ｊn ＋ΔＪ…（６）
このずれによって変動慣性による慣性トルクはΔＪθ”となる。またトルク係数も以下の（７）式のような変化をする。
Ｋt ＝Ｋtn＋ΔＫt …（７）
Ｋt の変動はモータにトルクリプル−ΔＫt Ｉa ref を生じる。
この変動トルクと負荷トルクの全合計を外乱トルクτdis と呼ぶ。すなわち外乱トルクは（８）式のようになる。
τdis ＝τl ＋ΔＪθ”−ΔＫt Ｉa ref 
＝τint ＋τext ＋Ｆ＋Ｄθ’＋（Ｊ−Ｊn ）θ”＋（Ｋtn−Ｋt ）Ｉa ref             …（８）
本発明で使用される外乱オブザーバ（以下、外乱推定手段ともいう）は、電流指令値と角速度からモータに加わる外乱トルクτ＾dis （外乱力）を推定する機構であり、推定外乱τ＾dis は以下の（９）式のように算出される。ここで、ｇdis は高域のノイズ低減のためのＬＰＦの遮断周波数である。
【００１６】
【数２】
2005212054.tif 000004
【００１７】
外乱オブザーバ（外乱推定手段）のブロック線図を図２（ｂ）に示す。同図に示すように、アクチュエータの駆動電流Ｉa ref と、アクチュエータの角速度ωから推定外乱τ＾dis を得ることができる。ここで、計算された外乱トルクをフィードフォワードすると外乱トルクを補償し、加速度次元でのロバスト性を確保することができる。
前記図１２に示した反力推定オブザーバは、上記外乱オブサーバにおいて、外乱推定値から重力項、摩擦項などを引くことによってアクチュエータに加えられた外力を求めるようにしたものである。
【００１８】
次に、本発明の力検出手法について説明する。本発明では、図１に示すように、作業用の第１駆動機構（第１のロボット１ａという）に加え、第１駆動機構と同一構造を有する第２の駆動機構（第２のロボット１ｂという）を用意し、第２の駆動機構自体をセンサとして用いる。なお、以下では、この同一構造を有する第１、第２のロボットから構成される機構をツインロボットといい、第１のロボットを作業用ロボット、第２のロボットをダミーロボットという。
すなわち、作業用ロボット１ａのみに外力を作用させ、第１の外乱推定手段により、作業用ロボット１ａに作用する外乱力を同定する。また、第２の外乱推定手段により、外力を作用させないダミーロボット１ｂに加わる外乱力を同定し（この場合外力が作用しないので重力や摩擦力等だけとなる）、第１の外乱推定手段の出力から第２の外乱推定手段の出力を引くことで、純粋な外力の検出を可能とする。この方法は同定試験などを必要とせず、また多自由度マニピュレータへの適用も容易である。
【００１９】
図１により、上記力検出について説明する。前記図２（ｂ）に示した第１、第２の外乱推定手段により推定されるロボット１ａ、１ｂの外乱力はそれぞれ以下の（１０）（１１）式のようになる。
Ｆdis1＝Ｆenv ＋Ｆg1＋Ｆf1＋Ｆint1…（１０）
Ｆdis2＝０＋Ｆg2＋Ｆf2＋Ｆint2…（１１）
ここで、Ｆdis は外乱力、Ｆg は重力、Ｆf は摩擦力、Ｆint は干渉力、Ｆenv は外力である。なお、ここでは、力をＦで表すが、このＦはトルクも含むこととし、以下の実施例の説明では、Ｆをトルクとして説明する。
ロボット１ａとロボット１ｂが同構造であり、同じ姿勢に制御できる場合、以下の（１２）〜（１４）式のように重力項、摩擦項、干渉項は全て同じ値となる。
Ｆg1＝Ｆg2  …（１２）
Ｆf1＝Ｆf2  …（１３）
Ｆint1＝Ｆint2  …（１４）
よって、操作するロボットの外乱トルクから常に外力を加えないロボットの外乱トルクを引くことによって外力のみを抽出することができる。
Ｆenv ＝Ｆdis1−Ｆdis2…（１５）
外乱推定手段は環境から加わる外乱トルクをシステムの内部変数として取り込むことができる。すなわち、加速度情報によって環境情報を取り込むことが可能である。よって本発明の力検出装置により、純粋な環境からの加速度情報を得ることができる。なお、以下では、上記ツインロボットを用いて外力を検出する手段を環境クオリアと呼ぶ。
【００２０】
以下、上記環境クオリアを用いた本発明の実施例の力検出装置およびこの力検出機能を備えた制御装置について説明する。
（１）実施例１
図３は、本発明の第１の実施例の力検出装置の構成例を示すブロック図である。
同図において、１ａは外力が作用する作業用ロボット、１ｂはロボット１ａと同一構造を有し、重力、摩擦力などのみが作用するダミーロボットである。
２ａ，２ｂは作業用ロボット１ａとダミーロボット１ｂの各軸の位置／角度を検出する検出手段であり、検出手段に２ａ，２ｂにより検出された各軸の位置／角度ｘ1res  ，ｘ2res  はフィードバックされて比較器３ａ， 3ｂでそれぞれ位置指令値ｘcmd ( ｘはベクトル）と比較され、その偏差が制御手段４ａ，４ｂに与えられる。
制御手段４ａ，４ｂは、例えばＰＤ制御を行う制御部４１と変換部４２から構成され、変換部４２は、制御部４１が出力する位置偏差の加速度に対応した信号を電流指令信号Ｉ1ref，Ｉ2refに変換する。
【００２１】
一方、上記位置／角度検出手段２ａ，２ｂの出力は変換手段７ａ，７ｂにより速度信号ｘ’1res，ｘ’2resに変換され、この速度信号ｘ’1res，ｘ’2resが第１および第２の外乱推定手段６ａ，６ｂに与えられる。また、第１および第２の外乱推定手段６ａ，６ｂには、駆動手段５ａ，５ｂに与えられる電流指令値Ｉ1a，Ｉ2aが与えられる。
第１および第２の外乱推定手段６ａ，６ｂは、前記図２（ｂ）に示した構成を備え、上記速度信号ｘ’1res，ｘ’2res，および電流値Ｉ1a，Ｉ2a  に基づき、上記作業用およびダミーロボット１ａ，１ｂに作用する外乱力（トルク）を検出する。
外乱推定手段６ａ，６ｂで検出したトルクτ＾dis1，τ＾dis2に対応した信号は、比較器８ａ，８ｂで前記電流指令信号Ｉ1ref，Ｉ2refと比較され、その差が電流指令値Ｉ1a，Ｉ2a  として駆動手段５ａ，５ｂに与えられる。
そして、駆動手段５ａ，５ｂにより作業用およびダミーロボット１ａ， 1ｂの各軸のアクチュエータが駆動され、作業用およびダミーロボット１ａ，１ａの各軸の位置／角度が位置指令値ｘcmd に一致するように制御される。なお、前記図２（ｂ）に示した外乱推定手段６ａ，６ｂが出力するトルクτ＾dis1，τ＾dis2の符号は負であるので、図３以降の図では、比較器８ａ，８ｂの一方の入力を”−”で示している。
上記第１および第２の外乱推定手段６ａ，６ｂが出力するトルクτ＾dis1，τ＾dis2は減算器９に与えられ、τ＾dis1−τ＾dis2がトルク検出（力検出）信号τ＾env として出力される。
【００２２】
上記構成の力検出装置においては、作業用ロボット１ａ，ダミーロボット１ｂの各軸の位置／角度を検出して位置指令値ｘcmd と比較し、その位置偏差により作業用ロボット１ａ，ダミーロボット１ｂを制御しているので、作業用ロボット１ａ，ダミーロボット１ｂは、常に同じ姿勢に制御される。このため、２台のロボットに働く重力、摩擦力の影響は等しくなる。
また、前記したように、作業用ロボット１ａとダミーロボット１ｂは同一構造を有し、作業用ロボット１ａには外力が働き、ダミーロボット１ｂには重力、摩擦力などの外力以外の力のみが作用するので、上記第１、第２の外乱推定手段６ａ，６ｂの出力の差は、摩擦、重力等の影響が除去された作業用ロボットに作用する外力のみとなる。
したがって上記差を求めることにより、作業用ロボット１ａに作用する外力を検出することができる。
【００２３】
（２）実施例２
次に本発明の環境クオリアを用いた本発明の第２の実施例の力制御装置について説明する。前記力覚検出手段を用いた力制御装置における位置指令値、力指令値は以下のように表せる。
【００２４】
【数３】
2005212054.tif 000005
【００２５】
すなわち、位置に関しては２台のロボットのコモンモードであり、力に関してはディファレンシャルモードで表現することができる。これを２次のアダマール行列Ｈ2 を用いて表すと、（１８）式のように表せる。
【００２６】
【数４】
2005212054.tif 000006
【００２７】
図４は、本発明の環境クオリアを力制御装置に適用した本発明の第２の実施例の構成例を示すブロック図である。
同図において、図３と同様、ロボット１ａは外力が作用する作業用のロボット、ロボット１ｂは、同一構造を有する重力、摩擦力などのみが作用するダミーロボットである。
２ａ，２ｂは作業用ロボット１ａとダミーロボット１ｂの各軸の位置／角度を検出する検出手段であり、検出手段に２ａ，２ｂにより検出された各軸の位置／角度ｘ1res  ，ｘ2res  はフードバックされて比較器３ａ， 3ｂでそれぞれ位置指令値ｘcmd と比較され、その偏差が制御手段４ａ，４ｂに与えられる。なお、この場合、位置指令信号ｘcmd は、検出手段２ａ，２ｂにより検出された２台のロボットの位置／角度を加算器１１で加算し、演算器１２で１／２した値として与えられる（前記（１６）式参照）。
制御手段４ａ，４ｂは、例えばＰＤ制御を行う制御部４１と減算器４０と変換部４２から構成される。そして、上記減算器４０において、上記制御部４１の出力から、トルク検出信号τ＾env と力指令値τcmd との偏差に応じた信号を出力する変換器１３の出力が減算される。また、変換部４２は、上記減算器制御部４１が出力する位置偏差の加速度に対応した信号を電流指令信号Ｉ1ref，Ｉ2refに変換する。
【００２８】
一方、上記位置／角度検出手段２ａ，２ｂの出力は変換手段７ａ，７ｂにより速度信号ｘ’1res，ｘ’2resに変換され、この速度信号ｘ’1res，ｘ’2resが第１および第２の外乱推定手段６ａ，６ｂに与えられる。また、第１および第２の外乱推定手段６ａ，６ｂには、駆動手段５ａ，５ｂに与えられる電流指令値Ｉ1a，Ｉ2aが与えられる。
第１および第２の外乱推定手段６ａ，６ｂは、前記したように、上記速度信号ｘ’1res，ｘ’2res，および電流値Ｉ1a，Ｉ2a  に基づき、上記作業用およびダミーロボット１ａ，１ｂに作用する外乱力（トルク）を検出する。
外乱推定手段６ａ，６ｂで検出したトルクτ＾dis1，τ＾dis2に対応した信号は、比較器８ａ，８ｂで前記電流指令信号Ｉ1ref，Ｉ2refと比較され、その差が電流指令値Ｉ1a，Ｉ2aとして駆動手段５ａ，５ｂに与えられる。
また、上記第１および第２の外乱推定手段６ａ，６ｂが出力するトルクτ＾dis1，τ＾dis2は減算器９に与えられ、τ＾dis1−τ＾dis2がトルク検出（力検出）信号τ＾env として、比較器１０で、力指令値τcmd と比較され、その偏差が変換手段１３に与えられる。
変換手段１３は、上記トルク信号の偏差τcmd −τ＾env を加速度信号ｘ”に変換し、このトルク信号の偏差に対応した信号が、前記制御部４ａ，４ｂの減算器４０の一方の入力端に入力される。
そして、駆動手段５ａ，５ｂにより作業用およびダミーロボット１ａ， 1ｂの各軸のアクチュエータが駆動され、作業用およびダミーロボット１ａ，１ｂの各軸の位置／角度が位置指令値ｘcmd に一致するように制御されるとともに、ロボット１ａ，１ｂに作用する力が力指令値τcmd に一致するように制御される。
【００２９】
本実施例においては、前記したように位置／角度検出手段２ａ，２ｂにより検出された２台のロボットの位置／角度を加算器１１で加算し、演算器１２で（１／２）した値が位置指令信号ｘcmd として、２台のロボット１ａ，１ｂに与えられるので、作業用ロボット１ａに追従して、ダミーロボット２ｂの位置／角度が制御される。このため、作業用ロボット１ａ，ダミーロボット１ｂは、常に同じ姿勢に制御され、２台のロボットに働く重力、摩擦力の影響は等しくなる。
また、上記第１および第２の外乱推定手段６ａ，６ｂが出力するトルクの差τ＾dis1−τ＾dis2であるトルク検出（力検出）信号τ＾env と力指令値τcmd との偏差を制御手段４ａ，４ｂに入力して、作業用ロボット１ａ，ダミーロボット１ｂを制御しているので、ロボット１ａ，１ｂが出力する力（トルク）は、力指令値τcmd 応じた値になるように制御される。
【００３０】
図５は、図４に示したブロック図の各ブロックの具体例を示す図である。
図５において、図４と同一のものには同一の符号が付されており、図５では、図４に示した位置／角度検出手段２ａ，２ｂが省略されている。
同図に示すように、制御手段４ａ，４ｂは、例えば位置偏差にｋｐ＋ｋｖｓ（ｋｐは比例ゲイン、ｋｖは微分ゲイン、ｓはラプラス演算子）の演算を施す制御部４１と、減算部４０と、減算部４０の出力x "1ref ，x"2ref，に伝達関数（Ｊ／ｋt G r ）を掛けてI1ref ，I2ref に変換する変換部４２から構成することができる。ここで、Ｊは駆動手段５ａ，５ｂに設けられたモータの慣性、ｋt はトルク定数、G r は上記モータのギヤ比である。
また、トルク信号の偏差τcmd −τ＾env を加速度信号ｘ”に変換する変換手段１３は、同図に示すように伝達関数Ｍc -1で構成することができる。ここで、Ｍc は、力制御ゲインである。
さらに、位置／角度検出手段２ａ，２ｂの出力を速度信号ｘ’1res，ｘ’2resに変換する変換手段７ａ，７ｂは、伝達関数ｓｇpd／（ｓ＋ｇpd）で構成することができる。ここで、ｇpdは、位置信号を速度信号に変換する擬似微分のためのローパスフィルタゲインである。                                  
【００３１】
前記したように、反力推定オブザーバは平衡型センサであり、図１３、図１４に示したように反力推定オブザーバを力制御に用いることで、環境との安定な接触を維持することができる。また、反力推定オブザーバは、サンプリング周波数及び上記オブザーバゲインの設定により帯域を任意に設定できるので、前記図１５に示したように高い周波数帯域で速い応答特性を得ることができる。
本実施例においては、上記平衡型センサである反力推定オブザーバと同様の構成を有する外力推定手段を用いて力検出を行い、ツインロボットシステムを採用し、前記した重力Ｆg 、摩擦力Ｆf 、干渉力Ｆint 等の影響を除去しているので、ノイズにも強く、広帯域のセンシングが可能で、さらに今までの力制御において常識であった粘性項の付加を必要とせず、高い周波数帯域での速い応答特性を得ることができる力制御装置を実現できる。
【００３２】
図６、図７に、力センサ、反力推定オブザーバ、および、本発明の力検出による制御系のステップ応答を示す。
図６（ａ）は、力センサを用いた場合のステップ応答特性を示し、図６（ｂ）に反力推定オブザーバを用いた場合のステップ応答を示し、図７に本発明の制御系のステップ応答を示す。同図の横軸は時間（ｓｅｃ）、縦軸は力（Ｎ）であり、同図のｃｍｄは指令値である。
同図に示すように、本発明の制御系では、力センサ、反力推定オブザーバを用いた制御系に比べ、速い応答特性を得ることが分かる。
なお、図６（ｂ）の反力推定オブザーバを用いた制御系では応答が振動的になっているが、これは離散値を用いているため、値の少しの変動が大きく出力の変動に現れているためと考えられる。
反力推定オブザーバでは、応答を速くするために力制御ゲインを大きくすると、摩擦の補償の影響により安定な接触動作を行うことが困難になる。本発明の力検出によれば、ロボット２台を用いてノイズの無い広帯域のセンシングが可能であるため、ゲインを大きく設定することも可能である。
【００３３】
図８、図９は、本発明の環境クオリアを用いた制御系と反力推定オブザーバを用いた制御系のフリーモーション時の力応答、位置応答を示す図である。
図８（ａ）は本発明の力応答を示し、図８（ｂ）は反力推定オブザーバを用いた場合の力応答を示し、図９は本発明の制御系の位置応答を示す。なお、図８の横軸は時間（ｓｅｃ）、縦軸は力（Ｎ）、図９の横軸は時間、縦軸は位置（ｍ）である。
反力推定オブザーバは、あらかじめ重力項、摩擦項などを同定試験により求めておき、反力推定オブザーバで得られた総外乱から引くことにより反作用力を得ようとするものであり、上記摩擦項等など影響により、図８（ｂ）に示すように力が振動的に変化する。
本発明においては、図８（ａ）に示すように、上記影響を受けることない力応答、位置応答を得ることができる。
【００３４】
次に、本発明をバイラテラル制御系に適用した場合の実施例について説明する。バイラテラル制御系は、操作者が、マスタ側ロボットを介してスレーブ側ロボットに指令を送ることができ、かつ、上記スレーブ側ロボットが作業対象物等から受ける作業反力を操作者にフィードバックすることができる制御方式である。
本発明においては、ロボット自体をセンサとして用いるため、マスタ、スレーブそれぞれ２台ずつロボットが必要となる。
また、マスタ・スレーブが同構造であった場合は、マスタ、スレーブ側ロボットとダミーロボットの３台のロボットを用いてバイラテラル制御を行うことが可能である。
本発明の力検出により、純粋な人間や環境からの加速度を抽出することができるため、バイラテラル制御により高精度の位置・力再現性を得ることが可能である。また、センシングした力情報にノイズがのってしまうという力センサや反力推定オブザーバの欠点を解決しているため、ノイズに埋もれる事の無い力提示が可能であり、操作性の向上も期待できる。
以下、本発明を上記バイラテラル制御に適用した場合に実施例について説明する。
（３）実施例３
図１０は、本発明の第３の実施例の構成例を示すブロック図であり、本実施例は本発明をマスタースレーブ型の力制御装置に適用した場合の構成例を示しており、本実施例はマスター側およびスレーブ側ロボットと、前記外力が作用しないダミーロボットを一台用いた場合の構成例を示している。
同図において、ロボット１ａは作業者により操作されるマスタ側の作業用の作業用ロボット（以下マスタ側ロボットという）、ロボット１ｃはスレーブ側の作業用ロボット（以下スレーブ側ロボットという）、ロボット１ｂは、ロボット１ａ，１ｃと同一構造を有し重力、摩擦力などのみが作用するダミーロボットである。なお、この例では、ダミーロボット１ｂがマスタ側に配置されている場合を示しているが、ダミーロボット１ｂをスレーブ側に配置してもよい。
２ａ，２ｂ，２ｃはマスタ側ロボット１ａ、ダミーロボット１ｂ、スレーブ側ロボット１ｃの各軸の位置／角度を検出する検出手段であり、検出手段２ａ，２ｂ，２ｃにより検出された各軸の位置／角度ｘ1res，ｘ2res，ｘ3resはフィードバックされて比較器３ａ， 3ｂ， 3ｃでそれぞれ位置指令値ｘcmd と比較され、その偏差が制御手段４ａ，４ｂ ,４ｃに与えられる。なお、この場合、位置指令信号ｘcmd は、検出手段２ａ，２ｂ，２ｃにより検出された３台のロボットの位置／角度を加算器１１で加算し、演算器１２で１／３した値として与えられる。
制御手段４ａ，４ｂ，４ｃは、前記したように例えばＰＤ制御を行う制御部４１と減算器４０と変換部４２から構成される。
そして、上記減算器４０において、上記制御部４１の出力から、トルク検出信号τ＾env に応じた信号を出力する変換手段１３の出力が減算される。また、変換部４２は、上記減算器制御部４１が出力する位置偏差の加速度に対応した信号を電流指令信号Ｉ1ref，Ｉ2ref，Ｉ3refに変換する。
【００３５】
一方、上記位置／角度検出手段２ａ，２ｂ，２ｃの出力は変換手段７ａ，７ｂ，７ｃにより速度信号ｘ’1res，ｘ’2res，ｘ’3resに変換され、この速度信号ｘ’1res，ｘ’2res，ｘ’3resが第１，２および第３の外乱推定手段６ａ，６ｂ，６ｃに与えられる。また、第１，２および第３の外乱推定手段６ａ，６ｂ，６ｃには、駆動手段５ａ，５ｂ，５ｃに与えられる電流指令値Ｉ1a，Ｉ2a，Ｉ3aが与えられる。
第１，２および第３の外乱推定手段６ａ，６ｂ，６ｃは、前記したように、上記速度信号ｘ’1res，ｘ’2res，，ｘ’3resおよび電流値Ｉ1a，Ｉ2a，Ｉ3aに基づき、上記マスタ側ロボット１ａ、ダミーロボット１ｂおよびスレーブ側ロボット１ｃに作用する外乱力（トルク）を検出する。
外乱推定手段６ａ，６ｂ，６ｃで検出したトルクτ＾dis1，τ＾dis2，τ＾dis3に対応した信号は、比較器８ａ，８ｂ，８ｃで前記電流指令信号Ｉ1ref，Ｉ2ref，Ｉ3refと比較され、その差が電流指令値Ｉ1a，Ｉ2a，Ｉ3aとして駆動手段５ａ，５ｂ，５ｃに与えられる。
また、上記第１および第２の外乱推定手段６ａ，６ｂが出力するトルクτ＾dis1，τ＾dis2は減算器９ａに与えられ、減算器９ａはその差であるτ＾env1を出力する。また、上記第２および第３の外乱推定手段６ｂ，６ｃが出力するトルクτ＾dis2，τ＾dis3は減算器９ｂに与えられ、減算器９ｂはその差であるτ＾env2を出力する。このτ＾env1，τ＾env2はそれぞれ、重力、摩擦などの影響を除去したマスタ側ロボット１ａ、スレーブ側ロボット１ｃに作用する力信号に相当する。  
上記τ＾env1，τ＾env2は、加算器１４で加算され、演算器１５で（１／２）とされ、変換手段１３に与えられる。
変換手段１３は、上記トルク信号１／２（τ＾env1＋τ＾env2）を加速度信号ｘ”に変換し、このトルク信号の偏差に対応した信号が、前記制御部４ａ，４ｂ，４ｃの減算器４０の一方の入力端に入力される。そして、駆動手段５ａ，５ｂ，５ｃによりマスタ側、スレーブ側およびダミーロボット１ａ， 1ｂ，１ｃの各軸のアクチュエータが駆動され、マスタ側、スレーブ側およびダミーロボット１ａ，１ｂ，１ｃの各軸の位置／角度が位置指令値ｘcmd に一致するように制御されるとともに、ロボット１ａ，１ｂ，１ｃの各軸に作用する力が等しくなるように制御される。
【００３６】
本実施例においては、前記したように位置／角度検出手段２ａ，２ｂ，２ｃにより検出された３台のロボットの位置／角度を加算器１１で加算し、演算器１２で（１／３）した値が位置指令信号ｘcmd として、３台のロボット１ａ，１ｂ，１ｃに与えられるので、マスタ側ロボット１ａに追従して、ダミーロボット２ｂ、スレーブ側ロボット１ｃの各軸の位置／角度が制御され、スレーブ側ロボット１ｃ，ダミーロボット１ｂは、マスタ側ロボット１ａに追従して動作する。
また、この３台のロボットが常に同じ姿勢に制御されるので、マスタ側ロボット１ａ、スレーブ側ロボット１ｃ、ダミーロボット１ｂに働く重力、摩擦力の影響は等しくなる。  また、第１および第２の外乱推定手段６ａ，６ｂの出力の差であるマスタ側トルク検出（力検出）信号τ＾env1と、第２および第３の外乱推定手段６ｂ，６ｃの出力の差であるスレーブ側トルク検出（力検出）信号τ＾env2を加算し、その１／２をトルク検出信号の平均値として各ロボット１ａ，１ｂ，１ｃの制御手段４ａ，４ｂ，４ｃにフィードバックし、マスタ側ロボット１ａ、スレーブ側ロボット１ｃ、ダミーロボット１ｂを制御しているので、マスタ側ロボット１ａ，スレーブ側ロボット１ｃ、ダミーロボット１ｂが出力する力（トルク）は等しくなる。
このため、マスタ側ロボット１ａを操作することで、スレーブ側ロボット１ｂを追従させて動作させることができるとともに、マスタ側ロボットの操作力をスレーブ側のロボットに伝えることができる。また、スレーブ側ロボット１ｂが受ける操作反力をマスタ側ロボットにフィードバックすることができる。
【００３７】
（４）実施例４
図１１は、本発明の第４の実施例の構成例を示すブロック図であり、本実施例はマスタ・スレーブが同構造でなく、マスター側およびスレーブ側に、ダミーロボットをそれぞれ設けた場合の構成例を示している。なお、同図では、図１０に示した位置／角度検出手段が省略されている。
同図において、ロボット１ａはマスタ側ロボット、ロボット１ｃはスレーブ側ロボットである。また、ロボット１ｂは、ロボット１ａと同一構造を有し重力、摩擦力などのみが作用するマスタ側のダミーロボット、ロボット１ｄは、ロボット１ｃと同一構造を有し重力、摩擦力などのみが作用するスレーブ側のダミーロボットである。なお、この例では、マスタ側とスレーブ側にそれぞれダミーロボット１ｂ，１ｃを設けているので、マスタ側ロボット１ａと、スレーブ側ロボット１ｃが同一構造である必要はない。
本実施例の制御系の構成は、ダミーロボットを２台設けた点を除き、前記図１０に示したものと同様であり、上記各ロボット１ａ，１ｂ，１ｃ，１ｄの各軸の位置／角度ｘ1res，ｘ2res，ｘ3res，ｘ4resはフードバックされて比較器３ａ， 3ｂ， 3ｃ， 3ｄでそれぞれ位置指令値ｘcmd と比較され、その偏差が制御手段４ａ，４ｂ ,４ｃ，４ｄに与えられる。なお、この場合、位置指令信号ｘcmd は、検出手段２ａ，２ｂ，２ｃ，２ｄにより検出された４台のロボットの位置／角度を加算器１１ａ，１１ｂで加算し、演算器１２で１／４した値として与えられる。
制御手段４ａ，４ｂ，４ｃ，４ｄでは、前記したように位置偏差信号に対応した信号から変換手段１３の出力が減算され、電流指令信号Ｉ1ref，Ｉ2ref，Ｉ3ref，Ｉ4refに変換される。
【００３８】
一方、上記各ロボット１ａ，１ｂ，１ｃ，１ｄの各軸の位置／角度は速度信号ｘ’1res，ｘ’2res，ｘ’3res，ｘ’4resに変換され、この速度信号ｘ’1res，ｘ’2res，ｘ’3res，ｘ’4res、および電流指令値Ｉ1a，Ｉ2a，Ｉ3a，Ｉ4aが第１，２，３および第４の外乱推定手段６ａ，６ｂ，６ｃ，６ｄに与えられ、第１，２，３および第４の外乱推定手段６ａ，６ｂ，６ｃ，６ｄは、前記したように、上記マスタ側ロボット１ａ、ダミーロボット１ｂおよびスレーブ側ロボット１ｃ、ダミーロボット１ｄに作用する外乱力（トルク）を検出する。
外乱推定手段６ａ，６ｂ，６ｃ，６ｄで検出したトルクτ＾dis1，τ＾dis2，τ＾dis3，τ＾dis4に対応した信号は、比較器８ａ，８ｂ，８ｃ，８ｄで前記電流指令信号Ｉ1ref，Ｉ2ref，Ｉ3ref，Ｉ4refと比較され、その差が電流指令値Ｉ1a，Ｉ2a，Ｉ3a，Ｉ4aとして駆動手段５ａ，５ｂ，５ｃ，５ｄに与えられる。
また、上記第１および第２の外乱推定手段６ａ，６ｂが出力するトルクτ＾dis1，τ＾dis2は減算器９ａに与えられ、減算器９ａはその差であるτ＾env1を出力する。また、上記第３および第４の外乱推定手段６ｃ，６ｄが出力するトルクτ＾dis3，τ＾dis4は減算器９ｂに与えられ、減算器９ｂはその差であるτ＾env2を出力する。このτ＾env1，τ＾env2はそれぞれ、重力、摩擦などの影響を除去したマスタ側ロボット１ａ、スレーブ側ロボット１ｃに作用する力信号に相当する。  
上記τ＾env1，τ＾env2は、加算器１４で加算され、演算器１５で（１／２）とされ、変換手段１３に与えられる。
変換手段１３は、上記信号を加速度信号ｘ”に変換し、この信号が、前記制御部４ａ，４ｂ，４ｃ，４ｄに入力される。そして、駆動手段５ａ，５ｂ，５ｃ，５ｄによりマスタ側、スレーブ側およびダミーロボット１ａ， 1ｂ，１ｃ，１ｄの各軸のアクチュエータが駆動され、マスタ側、スレーブ側およびダミーロボット１ａ，１ｂ，１ｃ，１ｄの各軸の位置／角度が位置指令値ｘcmd に一致するように制御されるとともに、ロボット１ａ，１ｂ，１ｃ，１ｄの各軸に作用する力が等しくなるように制御される。
【００３９】
本実施例においても、前記第３の実施例と同様、マスタ側ロボット１ａを操作することで、スレーブ側ロボット１ｂを追従させて動作させることができるとともに、マスタ側ロボットの操作力をスレーブ側のロボットに伝えることができる。また、スレーブ側ロボット１ｂが受ける操作反力をマスタ側ロボットにフィードバックすることができる。
また、本実施例では、マスタ側とスレーブ側にそれぞれダミーロボットを設けたので、マスタ側ロボット１ａと、スレーブ側ロボット１ｃが同一構造とする必要はない。
【産業上の利用可能性】
【００４０】
本発明は、ツインロボットシステムに平衡型センサである外乱推定手段を適用し、純粋な力検出を可能としたものであり、これまでの常識を打ち破るブレークスルーであり、学術界、産業界、医療分野へ与えるインパクトも大きいと考えられ、上記実施例で説明したように、力センサ、力制御、バイラテル制御等に適用できるほか、例えば、モーションコントロール、ロボティクス、ハプティクス、メカトロニクス、バーチャルリアリティ等の各種分野に適用することが可能である。
【図面の簡単な説明】
【００４１】
【図１】本発明における力検出の考え方を説明する図である。
【図２】外乱オブザーバ（外乱推定手段）を説明する図である。
【図３】本発明の第１の実施例の力検出装置の構成例を示すブロック図である。
【図４】本発明の第２の実施例の構成例を示すブロック図である。
【図５】図４の各ブロックの具体例を示す図である。
【図６】力センサ、反力推定オブザーバのステップ応答特性を示す図である。
【図７】本発明の力検出を用いた制御系のステップ応答を示す図である。
【図８】本発明の力検出を用いた制御系と反力推定オブザーバを用いた制御系の力応答特性を示す図である。
【図９】本発明の力検出を用いた制御系の位置応答を示す図である。
【図１０】本発明の第３の実施例の構成例を示すブロック図である。
【図１１】本発明の第４の実施例の構成例を示すブロック図である。
【図１２】反力推定オブザーバの構成を示すブロック図である。
【図１３】反力推定オブザーバを用いた制御系の応答特性を示す図である。
【図１４】力センサを用いた制御系の応答特性を示す図である。
【図１５】力センサと反力推定オブザーバの力検出情報をフィードバックしたときの周波数応答を示す図である。
【図１６】速度と摩擦力の関係を示す図である。
【符号の説明】
【００４２】
１ａ〜１ｄ      ロボット                      
２ａ〜２ｄ      位置／角度検出手段
３ａ〜３ｄ      比較器
４ａ〜４ｄ      制御手段
５ａ〜５ｄ      駆動手段
６ａ〜６ｄ      応力推定手段（応力オブザーバ）
７ａ〜７ｄ      変換手段
８ａ〜８ｄ      比較器
9，９ａ〜９ｂ  減算器
１０            比較器
１１            加算器
１１ａ〜１１ｂ  加算器
１２            演算器
１３            変換手段              
１４            加算器
１５            演算手段
【図１】
2005212054.tif 000007
【図２】
2005212054.tif 000008
【図３】
2005212054.tif 000009
【図４】
2005212054.tif 000010
【図５】
2005212054.tif 000011
【図６】
2005212054.tif 000012
【図７】
2005212054.tif 000013
【図８】
2005212054.tif 000014
【図９】
2005212054.tif 000015
【図１０】
2005212054.tif 000016
【図１１】
2005212054.tif 000017
【図１２】
2005212054.tif 000018
【図１３】
2005212054.tif 000019
【図１４】
2005212054.tif 000020
【図１５】
2005212054.tif 000021
【図１６】
2005212054.tif 000022