JP2008006517A - 外力検知方法およびマニピュレータ制御方法 - Google Patents

Abstract

【課題】マニピュレータの表面全体をセンサ部で覆うことなく、マニピュレータに物体が干渉することでアーム部に作用する外力を検知可能なマニピュレータの外力検知方法等を提供する。
【解決手段】固定リンク部材を含む複数のリンク部材から構成されるアーム部と、アーム部に固定されたハンド部を備えるマニピュレータにおいて、固定リンク部材に作用する第１の作用力の大きさとハンド部に作用する第２の作用力の大きさを検出し、作用力の検出箇所の位置姿勢を算出し、固定リンク部材・ハンド部の質量と動作により生じる慣性力から作用力の推測値を求め、この作用力から推測値を減算し作用力の補正値を求め、作用力の検出時の位置姿勢に基づきアーム部に外力が作用していない場合の固定リンク部材・ハンド部における作用力の関係から補正値が外れている場合にアーム部に外力が作用したと判断する。
【選択図】図１

Description

本発明は、主として産業用に用いられるマニピュレータが作業を行う最中に、該マニピュレータに作用する外力の大きさなどを算出する外力検知方法および該外力検知方法を用いたマニピュレータ制御方法、およびマニピュレータ制御システムに関する。

一般的に、工場などの生産現場においては、ハンド部とロボットアームとからなるマニピュレータが用いられている。このようなマニピュレータは、物体を把持し、該物体をある場所から他の場所へ移動させたり、把持した物体を載置する方向を変化させたりするために用いられるものであり、ＰＣ等の制御部に記憶されたプログラムにより、定められたパターンに従った動作を行う。

前述のマニピュレータにおいては、通常の使用に際しては、定められた動作を行うことが求められるが、異常が生じた場合には、定められたパターンに従った動作と異なる動作を行う必要が生じることがある。例えば、マニピュレータの動作中において、マニピュレータを構成するアーム部に対して、アーム部の動作経路上に干渉する物体が存在すると、アーム部はその動きを停止するか、該物体との干渉を避ける動きをしなければならない。

そのため、このような物体との干渉を避けるために、マニピュレータを構成するアーム部の表面全体に、接触センサなどの検知手段を設けるとともに、力センサなどの力検出手段を設けることが考えられる（例えば特許文献１）。このように構成されたマニピュレータは、アーム部表面に干渉する物体が存在したことを検知し、さらに、その干渉した物体によりアーム部に作用した外力を算出することができる。
特開２００１−３８６６４号

前述のように、マニピュレータに物体の干渉が生じたか否かを検知するためには、アーム部の表面全体を、力検出手段を備えるカバー部で覆う等の構造を構成する必要がある。しかしながら、マニピュレータを構成するアーム部は、一般的には多関節の駆動部を含む場合が多く、このような関節部を含む構造物の表面全体を覆うようにカバー部を設ける構造を構成することは困難である場合が多い。

本発明は、このような問題を解決するためになされたものであり、マニピュレータの表面全体をセンサ部で覆うことなく、マニピュレータに物体が干渉することでアーム部に作用する外力を検知可能なマニピュレータの外力検知方法、および該外力検知方法を用いたマニピュレータの制御方法、およびマニピュレータ制御システムを提供することを目的とする。

本発明にかかる外力検知方法は、固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部とを備えるマニピュレータにおいて、前記固定リンク部材に取り付けられ、その取り付け箇所に作用する第１の作用力の大きさを検出する第１の検出部と、前記ハンド部に取り付けられ、その取り付け箇所に作用する第２の作用力の大きさを検出する第２の検出部と、前記第１の検出部と、第２の検出部との位置姿勢を算出する位置姿勢算出部とを設け、前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記第１の検出部の取り付け箇所と第２の検出部の取り付け箇所とに作用する作用力の推測値を各々求め、前記第１の検出部および第２の検出部により検出される検出値から、前記推測値を減算し、前記第１の検出部の取り付け箇所に作用する第１の作用力と、第２の検出部の取り付け箇所に作用する第２の作用力の補正値を各々求め、これらの作用力を検出した時の、前記第１の検出部および第２の検出部の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記取り付け箇所に作用する作用力と、ハンド部における前記取り付け箇所に作用する作用力との所定の関係を、前記求められた補正値が満たしているか否かを判定し、これらの補正値が前記所定の関係から外れている場合に、アーム部に外力が作用したと判断することを特徴としている。

前記外力検知方法は、固定リンク部材に対して作用力が作用する箇所と、ハンド部に対して作用力が作用する箇所とにおいて、アーム部に外力が作用していない場合に、これらの箇所において作用する作用力の所定の関係を満たすことを利用するものである。すなわち、このような所定の関係を満たしていない場合は、アーム部全体をセンサ部で覆うことなく、アーム部に外力が作用していることを検知することができる。

また、このような外力検知方法において、ハンド部が物体を把持しており、ハンド部によって把持される物体の把持位置（重心位置）が既知の場合には、把持される物体と、ハンド部とを合わせた質量および慣性力を算出するとよい。

さらに本発明は、前述の外力検知方法を用いて、マニピュレータの動作を制御することも提案するものである。すなわち、固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部と、前記アーム部およびハンド部を駆動する駆動部と、を備えるマニピュレータを制御するためのマニピュレータ制御方法であって、前記固定リンク部材の所定箇所に作用する第１の作用力の大きさと、前記ハンド部の所定箇所に作用する第２の作用力の大きさとを検出する作用力検出ステップと、前記固定リンク部材およびハンド部における、前記第１の作用力および第２の作用力が作用した所定箇所の位置姿勢を各々算出する位置姿勢算出ステップと、前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記固定リンク部材およびハンド部の、前記所定箇所に作用する作用力の大きさを各々推測する作用力推測ステップと、前記作用力検出ステップで検出された作用力の大きさから、前記作用力推測ステップにより得られた作用力の大きさを減算し、前記固定リンク部材およびハンド部における前記所定箇所に作用する作用力の大きさを補正する作用力補正ステップと、前記作用力を検出した、固定リンク部材およびハンド部材の所定箇所の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記所定箇所に作用する作用力と、ハンド部における前記所定箇所に作用する作用力との所定の関係を、前記作用力補正ステップにより得られた補正後の作用力の値が満たしているか否かを判定する判定ステップと、を備えており、前記判定ステップにより、前記補正後の作用力の値が、前記所定の関係から外れていると判定された場合に、アーム部に外力が作用していると判断し、前記駆動部によるアーム部の動きを停止することを特徴としている。

このようなマニピュレータ制御方法によると、アーム部に外力が検知された場合に、マニピュレータの動きを停止させるように制御することができるため、マニピュレータの干渉による物体の破損等を回避することができる。

なお、このようなマニピュレータ制御方法においては、前述の作用力が所定の関係を満たしていない度合いを求め、その度合いに基づいてアーム部の動きを停止するか否かを定めるようにしてもよい。

また、本発明は、前述のようなマニピュレータ制御方法を実行するためのマニピュレータ制御システムをも提供するものである。すなわち、前述のような、固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部と、前記アーム部およびハンド部を駆動する駆動部とを有するマニピュレータと、該マニピュレータを制御するための制御部とを備えるマニピュレータ制御システムであって、前記固定リンク部材の所定箇所に作用する第１の作用力の大きさと、前記ハンド部の所定箇所に作用する第２の作用力の大きさとを検出する作用力検出部と、前記固定リンク部材およびハンド部における、作用力が作用した箇所の位置姿勢を各々算出する位置姿勢算出部と、前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記固定リンク部材の所定箇所に作用する作用力と、前記ハンド部に作用する作用力を推測する作用力推測部と、前記作用力検出部で検出された作用力の大きさから、前記作用力推測部により得られた作用力の大きさを減算し、前記固定リンク部材およびハンド部における前記所定箇所に作用する第１の作用力および第２の作用力の大きさを補正する作用力補正部と、前記作用力を検出した、固定リンク部材およびハンド部材の所定箇所の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記所定箇所に作用する作用力と、ハンド部における前記所定箇所に作用する作用力との所定の関係を、前記作用力補正ステップにより得られた補正後の作用力の値が満たしているか否かを判定する判定部と、を備えており、前記判定部により、前記補正後の作用力の値が、前記所定の関係を満たしていないと判定された場合に、前記制御部により、駆動部によるアーム部の動きを停止することを特徴としている。

このようなマニピュレータ制御システムにおいても、アーム部に外力が検知された場合に、マニピュレータの動きを停止させるように制御することができるため、マニピュレータの干渉による物体の破損等を回避することができる。

以上、説明したように、本発明によると、マニピュレータの表面全体をセンサ部で覆うことなく、マニピュレータに物体が干渉することでアーム部に作用する外力を検知することができる。

発明の実施の形態１．
以下に、図１から図３を参照しつつ本発明の実施の形態１にかかるマニピュレータ制御システムについて説明する。この実施の形態においては、固定面としての床面にマニピュレータが固定されており、このマニピュレータを駆動部により駆動する例を示すものとする。

図１は、床面Ｐに固定されたマニピュレータ２０及び該マニピュレータを制御するための制御部１０を含むマニピュレータ制御システム１を概略的に示す概略図である。このマニピュレータ制御システム１において、マニピュレータ２０は床面Ｐに一端が固定されるアーム部２２と、このアーム部２２の他端にとりつけられるハンド部としてのロボットハンド３０とを備えている。

マニピュレータ２は、床面Ｐに固定される固定部２１１と、この固定部２１１に対して回動自在に回転駆動する回動部２１２とからなる固定リンク部材２１と、第１リンク部材２２１および第２リンク部材２２２とからなるアーム部２２と、ロボットハンド２３とを備えている。固定部２１１は床面Pに埋め込まれた固定軸であり、この固定軸に対して回動自在に接続された回動部２１２は、図示しない駆動部としてのサーボモータによって固定部２１１に対して１自由度を有する動きをとるように駆動する。

アーム部２２の第１リンク部材２２１と第２リンク部材２２２は、互いに１自由度を有するように回動自在に接続されており、回動部２１２と同じく図示しない駆動部としてのサーボモータによって駆動される。また、回動部２１１と第１リンク部材２２１とは、軸部材２４を介して接続されることで一体的に動作する。すなわち、回動部２１２が固定部２１１に対して回動することで、第１リンク部材２２１も回動部２１２に併せて回動する。このように、マニピュレータ２に含まれる回動部分は、マニピュレータ２内の関節部として作用する。

ロボットハンド２３は、1対の板状の把持部材が図示しないモータにより相対的に近接離間することで、物体を把持する構成を備えており、第２リンク部材に対して軸部材２５を介して固定されている。なお、ロボットハンド２３の物体を把持する動きは、アーム部２２の動きから独立して制御されているが、アーム部２２の動きに合わせて制御されてもよい。

さらに、軸部材２４、２５には、例えば自身の歪み量により、受けた力の大きさを表す検出信号を送信可能な第１の検出部、第２の検出部としての第１センサ３１、第２センサ３２が各々取り付けられており、各々の軸部材の取り付け箇所に作用する力の大きさを検出する。そして、この第１センサ３１および第２センサ３２は、軸部材に作用する力の大きさを検出するだけでなく、取り付けられている各リンク部材の長さおよびその駆動した量等から、後述する位置姿勢算出部により、自身の相対的な位置姿勢を算出される。

制御部１０は、いわゆるＣＰＵ等の演算処理部を有するコンピュータであり、マニピュレータ２０の動作を所定のプログラムに従って制御するものである。具体的には、制御部１０は、マニピュレータに備えられるサーボモータの駆動を制御するモータ制御部１１、固定リンク部材２１およびロボットハンド２３に作用する作用力を検出する作用力検出部１２、作用力を検出したときの固定リンク部材２１およびロボットハンド２３に取り付けられた第１センサ３１、第２センサ３２の位置姿勢を検出する位置姿勢算出部１３、位置姿勢算出部１３により検出された位置姿勢等の情報から作用力を推測する作用力推測部１４、検出された作用力の値を補正する作用力補正部１５、および補正後の作用力が所定の関係を満たしているか否かを判定する判定部１６と、を備えている。以下、図２を用いて詳細に説明する。

制御部１０に備えられたモータ制御部１１は、マニピュレータ２０に設けられた各リンク部材、アーム部およびハンド部等を駆動するためのサーボモータの駆動を制御するためのプログラムを備えたサーボコントローラである。モータ駆動を制御するためのプログラムは、制御部１０内の図示しない記憶領域中に記憶されており、マニピュレータ２０に対して所望の動作を行わせるものである。

作用力検出部１２は、第１センサ３１および第２センサ３２からの検出信号を受けるとともに、受信した検出信号を解析して固定リンク部材２１１、ロボットハンド２３における第１センサ３１および第２センサ３２の取り付けられた箇所に作用する作用力Ｆ_１，Ｆ_２を算出する。

位置姿勢算出部１３は、前述の第１センサ３１および第２センサ３２が、作用力Ｆ_１，Ｆ_２を検出した時の第１センサ３１および第２センサ３２の位置姿勢Ｓ_１、Ｓ_２を算出する。これらの位置姿勢は、取り付けられている各リンク部材の長さおよびその駆動した量等から求められる。

作用力推測部１４は、作用力検出部１２によって算出される作用力とは別に、固定リンク部材２１１、ロボットハンド２３における第１センサ３１および第２センサ３２の取り付けられた箇所に作用する、自重や慣性力等の影響により作用する作用力を推測する。これらの作用力は、モータ制御部１１より得られる、回動する関節部が回動した角度、角速度、角加速度の３つのパラメータで特定される既知の関数を用いて得られる。

作用力補正部１５は、前記第１センサ３１および第２センサ３２から得られる検出値に基づいて算出された作用力から、前記推測により得られた作用力を減算し、固定リンク部材２１１、ロボットハンド２３に各々作用する、作用力の値を補正する。

判定部１６は、作用力補正部１５において補正されることで得られた固定リンク部材２１１、ロボットハンド２３に各々作用する作用力Ｆ_１'，Ｆ_２'が、所定の関係を満たしているか否かを判定する。このような所定の関係は、位置姿勢算出部１３により算出された位置姿勢Ｓ_１、Ｓ_２と、各センサ部の移動する移動速度とを関係づける、予め定められたヤコビ行列Ｊによって定められる。言い換えると、前述のヤコビ行列Ｊの転置行列Ｊ^Ｔは、外部から力を加えられることなくマニピュレータが動作している場合において、各センサ部の取り付け箇所に作用する作用力を所定の関係に対応づけるものである。したがって、前記補正後の各作用力Ｆ_１'，Ｆ_２'は、この転置行列Ｊ^Ｔを用いた既知の関係式で表される場合は、マニピュレータに外部からの力（外力）が作用していないと言える。逆に、既知の関係式で表せない場合は、マニピュレータに外力が作用していると言える。

このように構成されたマニピュレータ制御システム１において、アーム部２２に作用した外力を検知し、かつ、マニピュレータ２０の動きを制御する手順について、図３を用いて説明する。

マニピュレータ２０は、制御部１０に備えられたモータ制御部１１により、所定のプログラムに従ってその動作が制御されており、所望の作業等を行っている（ＳＴＥＰ１０１）。その際に、アーム部２２と固定リンク部材２１１とを接続する軸部材２４に取り付けられた第１センサ３１と、ロボットハンド２３と第２リンク部材２５とを接続する軸部材２５に取り付けられた第２センサ３２が検出した信号が、微小時間間隔で作用力検出部に送信されている（ＳＴＥＰ１０２）。

作用力検出部１２部は、このように微小時間間隔で送信される検出信号を受けとり、固定リンク部材２１１、ロボットハンド２３に各々作用する作用力Ｆ_１、Ｆ_２を算出する。一方、位置姿勢算出部１３は、検出信号を受信した際の第１センサ部３１、第２センサ部３２の位置姿勢Ｓ_１、Ｓ_２を算出する。そして、これらの算出値が作用力推測部１４に送信される（ＳＴＥＰ１０３）。

次に、作用力推測部１４においては、モータ制御部１１より各関節部の駆動した量（角度）や駆動した速度(角速度)等のパラメータを受け取り、これらのパラメータにより特定される既知の関数を用いて、固定リンク部材２１１、ロボットハンド２３における所定箇所（軸部材２４，２５）に作用した作用力ｇ_１，ｇ_２を求める（ＳＴＥＰ２０３）。そして、作用力補正部１５は、作用力検出部１２から送信された作用力Ｆ_１、Ｆ２の値から、これらの作用力ｇ_１、ｇ_２を減算し、固定リンク部材２１１、ロボットハンド２３に作用する作用力の補正値Ｆ_１'，Ｆ_２'を算出する（ＳＴＥＰ１０４）。

判定部１６においては、アーム部２２に外力が作用していない場合に定められる、固定リンク部材に作用する作用力ｆ_１と、ハンド部に作用する作用力ｆ_２との関係を、前述のヤコビ行列Ｊの転置行列Ｊ^Ｔを用いて、例えば以下の式１に示すように定められている。
ｆ_２＝Ｊ^Ｔ・ｆ_１ ・・・ 式１

そこで、判定部１５においては、位置姿勢算出部１３で算出された、第１センサ３１の位置姿勢Ｓ_１および第２センサ３２の位置姿勢Ｓ_２、および作用力推測部１４より算出された、固定リンク部材２１１における所定箇所（軸部材２４）に作用した作用力Ｆ_１'を上記式１のｆ_１に代入し、外力が作用していない場合における、ロボットハンド２３における所定箇所（軸部材２５）に作用する作用力Ｆ_２"を算出する（ＳＴＥＰ１０５）。このようにして算出された、ロボットハンド２３の所定箇所に作用する作用力Ｆ_２"と、作用力補正部１５において算出された補正値Ｆ_２'と判定部１６においてを比較し（ＳＴＥＰ１０６）、その差が所定の閾値ΔＦよりも大きい場合は、アーム部２２に外力が作用していると判定（ＳＴＥＰ１０７）し、モータ制御部１１によりマニピュレータ２０の動きを停止する（ＳＴＥＰ１０８）。逆に、算出された作用力Ｆ_２"と補正値Ｆ_２'との差がΔＦよりも小さい場合においては、アーム部２２に外力が作用していない、または作用している外力が、考慮すべき程度ではないと判断し、通常のマニピュレータ２０の制御を継続する。そして、再度マニピュレータに取り付けられた第１センサ３１および第２センサ３２により検出される信号を受信するＳＴＥＰ１０３に戻り、同様の手順を繰り返す。

このような制御により、本実施形態におけるマニピュレータ制御システムによれば、
マニピュレータ２０のアーム部２２に一定の値以上の外力が作用した場合に、マニピュレータにおける、駆動する関節部を含むアーム部全体を覆うことなく、その外力を検知し、マニピュレータの動きを停止することができる。

なお、前述の実施形態においては、一定以上の外力がマニピュレータ（アーム部）に作用した場合に、マニピュレータの動きを停止しているが、本発明はこれに限られるものではない。すなわち、マニピュレータに外力が作用したことを検知した瞬間に、警告音等を出力することで、周囲の作業者にマニピュレータが外部の物体と干渉していることを知らせるようなものであってもよい。もしくは、作用する外力の大きさに比例するように、マニピュレータの動きの速度を変化させるなどといった、作用する外力の大きさに基づいたマニピュレータの駆動制御を行ってもよい。

さらに、前述の実施形態においては、固定された床面に取り付けられたマニピュレータに関する例を説明しているが、たとえば、移動可能な移動体の側面を固定面とすることで、移動可能なマニピュレータシステムに適用することも可能である。すなわち、ヒューマノイド型のロボットに用いられるアーム部分に、本発明にかかる外力検知方法やマニピュレータ制御方法等を適用することも可能である。

第１の実施の形態に係るマニピュレータ制御システムの全体構成を概略的に示す概略図である。 図１に示すマニピュレータ制御システムに含まれる、制御部の内部構成を概念的に示すブロック図である。 図１に示すマニピュレータ制御システムにおいて、マニピュレータに作用する外力を検知する手順を示すフローチャートである。

符号の説明

１・・・マニピュレータ制御システム
２０・・・マニピュレータ
１０・・・制御部
１２・・・作用力検出部
１３・・・位置姿勢算出部
１４・・・作用力推測部
１５・・・作用力補正部
１６・・・判定部
２１・・・固定リンク部材
２２・・・アーム部
２２１・・・第１リンク部材
２２２・・・第２リンク部材
２３・・・ハンド部（ロボットハンド）
３１・・・第１センサ（第１の検出部）
３２・・・第２センサ（第２の検出部）
Ｐ・・・固定面（床面）

Claims (4)

1. 固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部とを備えるマニピュレータにおいて、
   前記固定リンク部材に取り付けられ、その取り付け箇所に作用する第１の作用力の大きさを検出する第１の検出部と、
   前記ハンド部に取り付けられ、その取り付け箇所に作用する第２の作用力の大きさを検出する第２の検出部と、
   前記第１の検出部と、第２の検出部との位置姿勢を算出する位置姿勢算出部とを設け、
   前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記第１の検出部の取り付け箇所と第２の検出部の取り付け箇所とに作用する作用力の推測値を各々求め、
   前記第１の検出部および第２の検出部により検出される検出値から、前記推測値を減算し、前記第１の検出部の取り付け箇所に作用する第１の作用力と、第２の検出部の取り付け箇所に作用する第２の作用力の補正値を各々求め、
   これらの作用力を検出した時の、前記第１の検出部および第２の検出部の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記取り付け箇所に作用する作用力と、ハンド部における前記取り付け箇所に作用する作用力との所定の関係を、前記求められた補正値が満たしているか否かを判定し、これらの補正値が前記所定の関係から外れている場合に、アーム部に外力が作用したと判断することを特徴とする外力検知方法。
2. 固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部と、前記アーム部およびハンド部を駆動する駆動部と、を備えるマニピュレータを制御するためのマニピュレータ制御方法であって、
   前記固定リンク部材の所定箇所に作用する第１の作用力の大きさと、前記ハンド部の所定箇所に作用する第２の作用力の大きさとを検出する作用力検出ステップと、
   前記固定リンク部材およびハンド部における、前記第１の作用力および第２の作用力が作用した所定箇所の位置姿勢を各々算出する位置姿勢算出ステップと、
   前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記固定リンク部材およびハンド部の、前記所定箇所に作用する作用力の大きさを各々推測する作用力推測ステップと、
   前記作用力検出ステップで検出された作用力の大きさから、前記作用力推測ステップにより得られた作用力の大きさを減算し、前記固定リンク部材およびハンド部における前記所定箇所に作用する作用力の大きさを補正する作用力補正ステップと、
   前記作用力を検出した、固定リンク部材およびハンド部材の所定箇所の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記所定箇所に作用する作用力と、ハンド部における前記所定箇所に作用する作用力との所定の関係を、前記作用力補正ステップにより得られた補正後の作用力の値が満たしているか否かを判定する判定ステップと、を備えており、
   前記判定ステップにより、前記補正後の作用力の値が、前記所定の関係から外れていると判定された場合に、アーム部に外力が作用していると判断し、前記駆動部によるアーム部の動きを停止することを特徴とするマニピュレータ制御方法。
3. 前記補正後の作用力の値が、所定の関係から外れている度合いを算出し、この度合いが所定の閾値を超えた場合にのみ、アーム部の動きを停止することを特徴とする請求項２に記載のマニピュレータ制御方法。
4. 固定面にとりつけられた固定リンク部材を含む複数のリンク部材から構成されるアーム部と、該アーム部に固定されたハンド部と、前記アーム部およびハンド部を駆動する駆動部とを有するマニピュレータと、該マニピュレータを制御するための制御部とを備えるマニピュレータ制御システムであって、
   前記固定リンク部材の所定箇所に作用する第１の作用力の大きさと、前記ハンド部の所定箇所に作用する第２の作用力の大きさとを検出する作用力検出部と、
   前記固定リンク部材およびハンド部における、作用力が作用した箇所の位置姿勢を各々算出する位置姿勢算出部と、
   前記固定リンク部材およびハンド部の各々の質量と、各々の動作により生じる慣性力とから、前記固定リンク部材の所定箇所に作用する作用力と、前記ハンド部に作用する作用力を推測する作用力推測部と、
   前記作用力検出部で検出された作用力の大きさから、前記作用力推測部により得られた作用力の大きさを減算し、前記固定リンク部材およびハンド部における前記所定箇所に作用する第１の作用力および第２の作用力の大きさを補正する作用力補正部と、
   前記作用力を検出した、固定リンク部材およびハンド部材の所定箇所の位置姿勢に基づいて、アーム部に外力が作用していない場合に定められる、固定リンク部材における前記所定箇所に作用する作用力と、ハンド部における前記所定箇所に作用する作用力との所定の関係を、前記作用力補正ステップにより得られた補正後の作用力の値が満たしているか否かを判定する判定部と、を備えており、
   前記判定部により、前記補正後の作用力の値が、前記所定の関係を満たしていないと判定された場合に、前記制御部により、駆動部によるアーム部の動きを停止することを特徴とするマニピュレータ制御システム。

Images