JP2017205835A - マニピュレータ装置の制御装置、マニピュレータ装置の制御方法及びマニピュレータ装置の制御プログラム - Google Patents

Abstract

【課題】操作性を向上させてマニピュレータ装置の直接教示を行う。【解決方法】連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御方法であって、前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出し、前記移動部を移動させるための駆動部を前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて制御し、前記交差箇所において前記駆動部の動力に従動して駆動する従動部に、前記駆動部の動力を伝達し、前記従動部を、前記推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動させる。【選択図】図１

Description

本発明は、マニピュレータ装置の制御装置、マニピュレータ装置の制御方法及びマニピュレータ装置の制御プログラムに関する。

近年、製造工程を自動化させるファクトリーオートメーションにおいては、本来、人が行う動作や作業をロボットアーム等のマニピュレータ装置に行わせることで製造品質の向上や製造工程を容易に切り替える分散プロセス化が行われている。

このようなマニピュレータ装置は、ティーチングプレイバック（教示作業）という方法で動作する。このティーチングプレイバックには、操作盤やタブレット端末等の入力装置を用いて、マニピュレータ装置の動作を指定する間接教示と、マニピュレータ装置の先端を模した装置やハンドルをユーザが操作して動作を指定する直接教示とがある。

直接教示においては、マニピュレータ装置を小さな力で直接教示できるようにモータを駆動させるアシスト制御が行われる。マニピュレータ装置の駆動方法のひとつとして、マニピュレータ装置の関節部分から離れた位置に設置したモータを駆動させ、プーリとベル
トもしくはワイヤによって駆動力を伝達させてマニピュレータ装置を関節ごとに駆動させる方法がある（例えば、特許文献１参照）。

しかし、特許文献１に開示された技術は、マニピュレータ装置を動作させるために、加えられた力（外力）を検知したことをトリガとして、アシスト制御が開始される。従って、直接教示を行うためにマニピュレータ装置を動かす際に、装置の重量や大きさによっては、大きな力を加える必要がある。

本発明は、上記実情を考慮してなされたものであり、操作性を向上させてマニピュレータ装置の直接教示を行うことを目的とする。

上記課題を解決するために、本発明の一態様は、連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出する外力算出部と、前記移動部を移動させるための駆動部を制御する駆動制御部と、前記交差箇所において前記駆動部の動力に従動して駆動する従動部と、前記駆動部の動力を前記従動部に伝達する駆動伝達部と、を含み、前記駆動制御部は、前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて前記駆動部を制御し、前記従動部は、前記外力の推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動することを特徴とする。

本発明によれば、直接教示を行う際に、操作性を向上させてマニピュレータ装置の直接教示を行うことが出来る。

本発明の実施形態に係るマニピュレータ装置の機械構成図。 本発明の実施形態に係るワイヤ駆動部の基礎構成図。 本発明の実施形態に係るワイヤ駆動部の基礎構成分解図。 本発明の実施形態に係る制御装置のハードウェア構成を示すブロック図。 本発明の実施形態に係る制御装置の機能構成を示すブロック図。 本発明の実施形態に係る外力算出部の内部構成を示す機能ブロック図。 本発明の実施形態に係る駆動制御部の機能構成を示すブロック図。 本発明の実施形態に係るマニピュレータ装置において発生する力について説明する図。 本発明の実施形態に係るマニピュレータ装置の動作を直接教示する際の動作を例示した図。 本発明の実施形態に係るマニピュレータ装置のアシスト制御の説明図。 本発明の実施形態に係るマニピュレータ装置のアシスト制御の流れを示すフローチャート。 本発明の実施形態に係るマニピュレータ装置に設定される移動力目標値を示す図。 本発明の実施形態に係るマニピュレータ装置のアシスト制御の説明図。 本発明の実施形態に係るひずみゲージを示す図。 本発明の実施形態に係る張力調整部を示す図。 本発明の実施形態に係る回転ダンパの構成を示す図。 本発明の実施形態に係る移動方向検知部の構成を示す図。

以下、図面を参照して、本発明の実施形態を詳細に説明する。本実施形態においては、自由度が「２」である垂直多関節型のロボットアームを例として、マニピュレータ装置の関節にあたる駆動部での動作を補助するアシスト制御について説明する。

図１は、本実施形態に係るマニピュレータ装置１の機械構成図である。図１に示すように、本実施形態に係るマニピュレータ装置１は、第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４、第一関節１８１、第二関節１８２、第一リンク１８３、第二リンク１８４、第三リンク１８５、第一ワイヤ１８６、第二ワイヤ１８７、第一モータ１８８、第一モータエンコーダ１８９、第一モータ減速装置１９０、第二モータ１９１、第二モータエンコーダ１９２、第二モータ減速装置１９３、第一駆動プーリ１９４、第一従動プーリ１９５、第二駆動プーリ１９６、第二中継プーリ１９７、第二従動プーリ１９８、ベース１９９を含む。

第一リンク１８３、第二リンク１８４、第三リンク１８５は、その交差箇所を、夫々第一関節１８１、第二関節１８２を回転軸とした回転可能な機構によって支持され、第一モータ１８８、第二モータ１９１によって移動させられる移動部として機能する。また、ベース１９９は、第一リンク１８３、第二リンク１８４、第三リンク１８５を支持する支持部として機能する。第二関節１８２によって支持されていない方の第三リンク１８５の端部には、把持動作を行うピッキングハンドやエアーチャック等のリンク機構が備えられている。

また、リンク機構と第三リンク１８５との接続部分には、マニピュレータ装置１が移動したことを検知する移動検知センサ１１２が含まれる構成であってもよい。この移動検知センサ１１２が含まれる場合、後述する外力検知部２１８と連動して機能する。

尚、移動検知センサ１１２として、六軸力覚センサ等を用いる構成であってもよい。本実施形態に係るマニピュレータ装置１は、図４で説明する制御装置２が出力する制御信号に基づいて動作を制御される。

まず、第一モータ１８８による第一リンク１８３の駆動態様について説明する。第一リンク１８３には駆動部である第一モータ１８８が取り付けられており、第一モータ１８８の回転動作を制御する機構としてエンコーダ１８９が同軸上に取り付けられている。また、第一リンク１８３には、第一モータ１８８と同軸上に第一モータ減速装置１９０が取り付けられ、第一モータ減速装置１９０に第一駆動プーリ１９４が取り付けられている。

一方で、第一リンク１８３は第一関節１８１を回転軸として、第一従動プーリ１９５によって回動可能に支持されている。第一駆動プーリ１９４及び第一従動プーリ１９５には駆動伝達部である第一ワイヤ１８６が巻きつけられている。

また、第一関節１８１には一定のトルクを保持可能にするために、トルクリミッタ１７１が取り付けられている。そのため、トルクリミッタ１７１は、第二リンク１８４が第一関節１８１を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。

尚、図１の構成では出力軸である第一関節１８１にトルクリミッタ１７１が取り付けられているが、第一モータ１８８にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。

また、第一関節１８１にはさらに光学式の第一関節軸エンコーダ１７３が取り付けられている。第一関節軸エンコーダ１７３は、回転検知部として機能する。第一関節軸エンコーダ１７３は、ワイヤの伸び等を考慮して第一関節１８１に取り付けられる。尚、第一ワイヤ１８６を十分な張力で張り、ワイヤの伸びが発生しなければ、第一モータエンコーダ１８９と第一関節軸エンコーダ１７３とは線形性を有するため、第一関節軸エンコーダ１７３は不要である。

以上説明したように、第一モータ１８８による駆動力は、第一駆動プーリ１９４から第一ワイヤ１８６に伝達され、第一ワイヤ１８６が第一関節１８１を水平回転軸として第一従動プーリ１９５を回転駆動させる。そして、第一従動プーリ１９５は、第一従動プーリ１９５に固定されている第二リンク１８４を、第一関節１８１を水平回転軸として垂直に回転駆動させる。

次に第二モータ１９１による第一リンク１８３の駆動態様について説明する。第一リンク１８３には駆動部である第二モータ１９１が取り付けられており、第二モータ１９１の回転動作を制御する機構としてエンコーダ１９２が同軸上に取り付けられている。また、第一リンク１８３には、第二モータ１９１と同軸上に第二モータ減速装置１９３が取り付けられ、第二モータ減速装置１９３に第二駆動プーリ１９６が取り付けられている。

一方で、第一リンク１８３は第一関節１８１を回転軸として、第二中継プーリ１９７によっても回転可能に支持されている。また、第二リンク１８４は第二関節１８２を回転軸として、第二従動プーリ１９８によって回転可能に支持されている。第二駆動プーリ１９６、第二中継プーリ１９７、第二従動プーリ１９８には、夫々駆動伝達部である第二ワイヤ１８７が巻きつけられている。

また、第二関節１８２には一定のトルクを保持可能にするためにトルクリミッタ１７２が取り付けられている。そのため、トルクリミッタ１７２は、第三リンク１８５が第二関節１８２を回転軸として回転駆動する際に負荷として作用する、落下防止ブレーキとしても機能する。

尚、図１の構成では出力軸である第二関節１８２にトルクリミッタ１７２が取り付けられているが、第二モータ１９１にトルクリミッタあるいは電磁ブレーキが取り付けられる構成であってもよい。

また、第二関節１８２にはさらに光学式の第二関節軸エンコーダ１７４が取り付けられている。第二関節軸エンコーダ１７４は、回転検知部として機能する。第二関節軸エンコーダ１７４は、ワイヤの伸び等を考慮して第二関節１８２に取り付けられる。尚、第二ワイヤ１８７を十分な張力で張り、ワイヤの伸びが発生しなければ、第二モータエンコーダ１９２と第二関節軸エンコーダ１７４は線形性を有するため、第二関節軸エンコーダ１７４は不要である。

以上説明したように、第二モータ１９１による駆動力は、第二駆動プーリ１９６から第二ワイヤ１８７に伝達され、第二ワイヤ１８７が第二関節１８２を水平回転軸として第二従動プーリ１９８を回転駆動させる。そして、第二従動プーリ１９８は、第二従動プーリ１９８に固定されている第三リンク１８５を、第二関節１８２を水平回転軸として垂直に回転駆動させる。

次に、図２及び図３を参照して、本実施形態に係るワイヤ駆動部の構成について説明する。図２は、本実施形態に係るワイヤ駆動部の基礎構成図、図３は、本実施形態に係るワイヤ駆動部の基礎構成分解図である。

図２に示すように、第一駆動プーリ１９４及び第一従動プーリ１９５には駆動伝達部として第一ワイヤ１８６が巻きつけられている。第一ワイヤ１８６は図２において時計回りの回転を伝達する１８６Ａ部と、反時計回りの回転を伝達する１８６Ｂ部とに分かれて巻き付けられている。

また、第一駆動プーリ１９４は第一ワイヤ１８６の１８６Ａ部と１８６Ｂ部を夫々受ける１９４Ａ部と１９４Ｂ部とで構成されている。さらに、第一従動プーリ１９５は第一ワイヤ１８６の１８６Ａ部と１８６Ｂ部を夫々受ける１９５Ａ部と１９５Ｂ部とで構成されている。

図３に示すように、第一ワイヤ１８６の端部には、固定部材２０１Ａおよび２０１Ｂが取り付けられている。この固定部材２０１Ａおよび２０１Ｂを第一駆動プーリの溝１９４Ａ、１９４Ｂに夫々組み込むことで第一ワイヤ１８６は第一駆動プーリ１９４に固定される。以上の構成は第二駆動プーリ１９６と、その関連部についても同様であるため、重複する説明を省略する。

図４は、本実施形態に係るマニピュレータ装置１を制御する制御装置２のハードウェア構成を示すブロック図である。図４に示すように、本実施形態に係る制御装置２は、一般的なサーバやＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）等の情報処理端末と同様の構成に加えて、モータ等の駆動機構を制御するエンジンを有する。即ち、本実施形態に係る制御装置２は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２１、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２２、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２３、エンジン２４、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）２５及びＩ／Ｆ２６がバス２９を介して接続されている。Ｉ／Ｆ２６には、センサ２７やスイッチ２８が接続され、スイッチ２８は、マニピュレータ装置１の電源を制御するものである。

また、センサ２７は、図１の移動検知センサ１１２を含み、マニピュレータ装置１の状態を検知するものである。さらに、Ｉ／Ｆ２６にはタブレット端末等の外部装置がネットワークを介して接続され、マニピュレータ装置１の動作に関する設定を入力することが出来る。

ＣＰＵ２１は演算手段であり、マニピュレータ装置１全体の動作を制御する。ＲＡＭ２２は、情報の高速な読み書きが可能な揮発性の記憶媒体であり、ＣＰＵ２１が情報を処理する際の作業領域として用いられる。ＲＯＭ２３は、読み出し専用の不揮発性記憶媒体であり、ファームウェア等のプログラムが格納されている。エンジン２４は、マニピュレータ装置１において実際にモータ駆動を実行する機構である。

ＨＤＤ２５は、情報の読み書きが可能な不揮発性の記憶媒体であり、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）や各種の制御プログラム、アプリケーション・プログラム等が格納されている。Ｉ／Ｆ２６は、バス２９と各種のハードウェアやネットワーク等を接続し制御する。

このようなハードウェア構成において、ＲＯＭ２３に格納されたプログラムや、ＨＤＤ２５若しくは図示しない光学ディスク等の記録媒体からＲＡＭ２２に読み出されたプログラムに従ってＣＰＵ２１が演算を行うことにより、ソフトウェア制御部が構成される。このようにして構成されたソフトウェア制御部と、ハードウェアとの組み合わせによって、本実施形態に係る制御装置２の機能を実現する機能ブロックが構成される。

図５は、本実施形態に係るマニピュレータ装置１の動作機構を示す図である。図１に示したようなマニピュレータ装置１は、制御装置２によって動作を制御される。制御装置２はＰＣ等の演算機能を備えた端末であるため、制御装置２を操作して、マニピュレータ装置１の位置や速度をユーザが任意に設定することが出来る。また、外部装置から制御装置２に入力される命令に基づいてマニピュレータ装置１の動作を制御する構成であってもよい。

第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、回転速度や回転位置及び回転方向を検知するためのマーカーが付加された円板を含み、そのマーカーを光学的に読み取ることによって検知信号を出力する。従って、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、移動方向検知部としても機能する。尚、検知信号の出力態様について詳細は後述する。

第一モータ減速装置１９０、第二モータ減速装置１９３は、それぞれ、第一モータ１８８、第二モータ１９１の回転数を減少させ、高いトルクを得るための装置である。また、第一モータ１８８、第二モータ１９１の回転数とマニピュレータ装置１が必要とする回転数が異なる場合に第一モータ１８８、第二モータ１９１の回転数を調節する。

第一モータ１８８、第二モータ１９１は、マニピュレータ装置１の動力装置であり、マニピュレータ装置１は第一モータ１８８、第二モータ１９１の回転駆動により発生した動力を第一ワイヤ１８６、第二ワイヤ１８７によって伝達させ、第一従動プーリ１９５、第二従動プーリ１９８を駆動させる。従って、第一モータ１８８、第二モータ１９１は第一従動プーリ１９５、第二従動プーリ１９８を回転駆動させる駆動部１０、１１として機能する。

また、第一ワイヤ１８６、第二ワイヤ１８７は、第一モータ１８８、第二モータ１９１の動力を伝達する駆動伝達部として機能する。さらに、第一駆動プーリ１９４、第二駆動プーリ１９６は、第一モータ１８８、第二モータ１９１の動力に基づいて従動する従動部として機能する。

図５は、本実施形態に係る制御装置２の機能構成を示すブロック図である。図４に示すように、制御装置２は、駆動制御部２１１、回転駆動部２１２、ＦＢ（フィードバック）取得部２１５、位置情報取得部２１６、外力算出部２１７を含む。尚、マニピュレータ装置１に移動検知センサ１１２が含まれる構成である場合、制御装置２は、図６に示すように、外力算出部２１７の内部機能として外力検知部２１８を含む。本実施形態にかかる制御装置２は、フィードバック制御部として機能し、マニピュレータ装置１は制御装置２から出力された信号によって動作を制御される。

駆動制御部２１１は、Ｉ／Ｆを介して外部装置から入力される駆動部１０、１１の回転速度の目標値、ＦＢ取得部２１５から入力されるフィードバック信号、外力算出部２１７から入力される外力の推定値に基づき、駆動部１０、１１を回転させるための制御値を回転駆動部２１２に対して出力する。回転駆動部２１２は、駆動制御部２１１から入力される制御値に基づき、駆動部１０、１１を回転させるためのＰＷＭ信号を生成して出力する。

駆動部１０、１１は、回転駆動部２１２から入力されるＰＷＭ信号に応じて回転する第一モータ１８８、第二モータ１９１である。この駆動部１０、１１の回転駆動によって、第一関節１８１、第二関節１８２を回転軸としてマニピュレータ装置１が移動される。駆動部１０、１１としては、ＤＣ（Ｄｉｒｅｃｔ Ｃｕｒｒｅｎｔ）モータを用いることが可能であり、インナーロータ型ＤＣモータであるブラシレスＤＣモータやブラシ付きＤＣモータを用いることが可能である。

本実施形態に係る駆動部１０、１１は、回転速度や回転位置を検知するためのマーカーが付加された円板を回転させる。また、第一関節１８１、第二関節１８２が、駆動部１０、１１の回転駆動に従動して回転駆動することによって、回転速度や回転位置を検知するためのマーカーが付加された円盤を回転させる。

そして、第一モータエンコーダ１８９、第二モータエンコーダ１９２によって構成される回転検知部２１４は、そのマーカーを光学的に読み取ることによって検知信号を出力する。また、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４によって構成される位置変化量検知部２１９も、円盤に付加されたマーカーを光学的に読み取ることによって検知信号を外力算出部２１７に出力する。

具体的には、駆動部１０、１１の回転に伴って回転する円盤に付加されたマーカーが、回転検知部２１４が光学的に読み取る読み取り位置を通過する。これにより、回転検知部２１４による光学的な読み取り状態が変化し、回転検知部２１４がマーカーを検知して検知信号を出力する。尚、位置変化量検知部２１９においても同様に、第一関節１８１、第二関節１８２の回転に伴って回転する円盤に付加されたマーカーが、位置変化量検知部２１９が光学的に読み取る位置を通過する。

尚、本実施形態に係る回転検知部２１４及び位置変化量検知部２１９は２組のセンサを含む。この２組のセンサは、上述した円板に付加されたマーカーの位相差がπ／２（ｒａｄ）となるように配置されている。これにより、本実施形態に係る回転検知部２１４及び位置変化量検知部２１９は、π／２（ｒａｄ）の位相差を有する２つの検知信号を出力する。

ＦＢ取得部２１５は、回転検知部２１４の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、駆動部１０、１１の回転位置を計算し、その計算結果から駆動部１０、１１の回転速度を算出する。算出された駆動部１０、１１の回転速度は、フィードバック値として駆動制御部２１１に入力される。また、ＦＢ取得部２１５は、上述したように、π／２（ｒａｄ）の位相差を有する２つの検知信号の位相差を利用して、駆動部１０、１１の回転方向を検知する。

このようにして出力されるＦＢ取得部２１５のフィードバック値に基づき、駆動制御部２１１は、外部装置から入力される目標値と、ＦＢ取得部２１５から入力されるフィードバック値との差分に基づいて、駆動部１０、１１を回転させるための制御値を出力する。

位置情報取得部２１６は、上述したような制御装置２におけるフィードバック制御のサイクルにおいて、駆動部１０、１１の回転状態を示す値である特徴量を取得する。図５において破線で示しているように、駆動部１０、１１の回転状態を示す値としては、ＦＢ取得部２１５が取得した回転検知部２１４の検知信号や、ＦＢ取得部２１５が算出した駆動部１０、１１の回転速度、外力算出部２１７が算出した第一関節１８１、第二関節１８２の回転位置を用いることができる。

また駆動制御部２１１が回転駆動部２１２に入力する制御値を用いることができる。更に、回転駆動部２１２が出力するＰＷＭ信号を用いることができる。これらの値が、フィードバック制御において得られるフィードバック値である。位置情報取得部２１６は、取得したマニピュレータ装置１の位置情報を駆動制御部２１１に返信する。

外力算出部２１７は、位置変化量検知部２１９の検知信号を取得し、単位時間あたりにマーカーが検知された回数に基づいて、第一関節１８１、第二関節１８２の回転位置を算出する。尚、この計算結果から、第一関節１８１、第二関節１８２の回転速度を算出する構成であってもよい。

以上説明したように、外力算出部２１７は、第一従動プーリ１９５、第二従動プーリ１９８に設置された第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４から送信される信号に基づいて、マニピュレータ装置１に加えられている外力の推定値を算出する。

マニピュレータ装置１のユーザがマニピュレータ装置１を動かそうとした場合、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４は、それぞれ第一関節１８１、第二関節１８２における位置の変化量を検知する。この時、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４において検知される位置の変化量とは、第一関節１８１と第二関節１８２夫々の関節軸に対して第一リンク１８３及び第二リンク１８４、第二リンク１８４と第三リンク１８５が夫々形成する角度の変化量のことを示す。

外力算出部２１７は、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４が位置の変化量を検知したタイミングにおいて、その位置の変化量に基づいて、マニピュレータ装置１に加えられている外力の推定値を算出する。

尚、マニピュレータ装置１の先端部分に移動検知センサ１１２が設置されている場合に、制御装置２が外力検知部２１８を含む構成であってもよい。外力検知部２１８は、移動検知センサ１１２によって第二リンク１８４、第三リンク１８５に力が加えられたこと（外力）を検知すると、第一モータ１８８、第二モータ１９１への入力トルクやマニピュレータ装置１の位置及び速度等の情報を取得する。そして取得した情報に基づいて外力を検知する。

また、制御装置２に外力検知部２１８が含まれる構成である場合、外力算出部２１７が、外力検知部２１８で検知された外力の値に基づいてマニピュレータ装置１に加えられている外力の推定値を算出する構成であってもよい。

尚、本実施形態に係るフィードバック制御においては、第一関節１８１、第二関節１８２を回転駆動させるために、駆動制御部２１１が、駆動部１０、１１の回転速度及び第一関節１８１、第二関節１８２の位置の目標値とフィードバック値とに基づいて制御値を出力する。即ち、回転速度に基づいた制御（以降、「速度制御」とする）が行われる。また、第一関節１８１、第二関節１８２の回転位置に基づいた制御（以降、「位置制御」とする）が行われる。第二リンク１８４、第三リンク１８５が移動されることで加わる外力と、装置自体の回転速度及び位置情報に基づいて、マニピュレータ装置１の動作を制御することが本実施形態に係る要旨の１つである。

位置制御を行う場合、ＦＢ取得部２１５は、マーカーが検知された回数に基づいて駆動部１０、１１の回転位置を計算し、フィードバック値として駆動制御部２１１に入力する。また、駆動制御部２１１には、第一関節１８１、第二関節１８２の回転位置の目標値が入力される。これにより、駆動制御部２１１は、目標値とフィードバック値との差分に基づいて駆動部１０、１１の回転を制御するための制御値を生成して出力する。これにより、第一関節１８１、第二関節１８２の回転位置が、入力された目標値に保たれる。

このように、駆動部１０、１１に従動して回転駆動する第一関節１８１と第二関節１８２は、制御装置２によってその回転駆動を制御される。

図７は、本実施形態に係る制御装置２の特徴的な機能を実現する駆動制御部２１１の機能構成を示す図である。図７に示すように、駆動制御部２１１は、移動力目標生成部２２１、位置速度指令生成部２２２を含む。移動力目標生成部２２１は、制御装置２に入力されるマニピュレータ装置１を移動させる移動力を設定する情報に基づいて、第二リンク１８４、第三リンク１８５を移動させる時の移動力目標値の設定を行う。また、マニピュレータ装置１の位置や外力の変化から、移動力目標値の設定を変更して第一関節１８１、第二関節１８２を回転させるために駆動部１０、１１の駆動を制御するための移動力目標情報を生成する。

位置速度指令生成部２２２は、制御装置２に入力されるマニピュレータ装置１の移動範囲や移動速度を設定する情報に基づいて、第二リンク１８４、第三リンク１８５を移動させる位置速度目標値の設定を行う。また、マニピュレータ装置１の位置や移動速度の変化と目標値との差分に基づいて、第一関節１８１、第二関節１８２を回転させるために駆動部１０、１１の駆動を制御しマニピュレータ装置１の位置・移動速度の設定を変更するための位置速度指令情報を生成する。

図８（ａ）及び（ｂ）は、本実施形態に係るマニピュレータ装置１において発生する力について説明する図である。本実施形態においては、第二リンク１８４、第三リンク１８５が第一関節１８１、第二関節１８２で接続された２関節マニピュレータの平面運動と第三リンク１８５の自由末端における力Ｆとの関係から、第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２に加わるトルクを算出する。

図８（ａ）は、本実施形態に係るマニピュレータ装置１のｘｙ平面での動作を模式的に示した図である。ここでは、第二関節１８２の第二関節トルクリミッタ１７２によって第二リンク１８４と連結されていない側の第三リンク１８５の末端が自由末端となる。実際のマニピュレータ装置１の動作においては、この自由末端には、移動検知センサ１１２が取り付けられ、外力の検知を行う構成であってもよい。

図８（ｂ）は、図８（ａ）にて示したマニピュレータ装置１に発生する物理量を説明する図である。図８（ｂ）においては、夫々、Ｉｉ：慣性モーメント、Ｌｉ：リンクの長さ、ｍｉ：質量、θｉ：角度、τｉ：トルク、ｒｉ：関節中心と質量中心との距離、ｇ：重力加速度（ｙ軸負方向）を示している。また、本実施形態においては、自由度２のマニピュレータ装置１を例として説明するので、ｉ＝１，２とする。

図８（ｂ）に示すように、マニピュレータ装置１がｘｙ平面で動作している時の運動方程式は、（数１）で与えられる。

数１において、ｑは一般化座標を示し、ｑは（数２）のようにあらわされる。
（数１）において、第一項は慣性力、第二項はコリオリ力、第三項は重力項を示す。

（数３）に示すように、本実施形態に係るマニピュレータ装置１に（数１）に示す運動方程式を適用すると、動作中に第一関節トルクリミッタ１７１、第二関節トルクリミッタ１７２に加わるトルクを推定することが出来る。

（数３）で推定したトルクは、マニピュレータ装置１の機械構成や減速装置、剛性、粘性摩擦等を考慮することで更に精度を高めることが出来る。また、数３で推定したトルクと、第三リンク１８５の自由末端における力Ｆとには、（数４）に示す関係がある。
Ｊはヤコビ行列を表す。従って、第三リンク１８５の自由末端における力Ｆは、
によって求めることが出来る。

また、本実施形態に係るマニピュレータ装置１のヤコビ行列は、
である。（数６）を（数５）に代入すると、
となる。このようにして、マニピュレータ装置１の動作から第三リンク１８５の自由末端における力Ｆを求めることが出来る。この時求めた力Ｆは、第三リンク１８５に加えられた外力に相当する。

次に、図９を参照して本実施形態に係るマニピュレータ装置１を直接教示する際の動作について説明する。図９は、本実施形態に係るマニピュレータ装置１の動作を直接教示する際にユーザが行う動作を例示した図である。本実施形態に係るマニピュレータ装置１のように、マニピュレータ装置の関節部およびその駆動方式として、関節の回転軸と同じ軸にモータおよび減速装置を配置しているものがある。

このようなマニピュレータ装置においては、減速装置の出力軸にリンク部などの次節移動部を直接接続することで、簡単な構造かつ軽量な構成で関節としての機能を実現している。また、この減速装置として、多段遊星歯車機構や波動歯車装置を用いたマニピュレータ装置があるが、直接教示を行う際に、装置自体の質量や減速装置のギヤ効率、内部摩擦などによってマニピュレータ装置が動き出すまでに大きな力を加える必要がある。

図９に示すように、マニピュレータ装置１を直接教示する際に、上述したような理由によってマニピュレータ装置１を移動させることが困難な場合がある。そこで、本実施形態に係るマニピュレータ装置１においては、外力が検知される前に、マニピュレータ装置１が教示される方向に働く力（移動量）を検知する。また、移動量が検知されたことをトリガとしてマニピュレータ装置１のアシスト制御を開始することにより、直接教示を行う方向にいち早く力が付与されるため、マニピュレータ装置１移動状況に応じて、操作性を向上させた直接教示を行うことが可能になる。

このように、ユーザによるマニピュレータ装置１の移動を補助するために、駆動部１０、１１に対して行う制御のことを以後、「アシスト制御」と記載する。図９及び図１０に示すように、ユーザが直接教示する方向にマニピュレータ装置１のアシスト制御を行い、操作性を高めて直接教示を行うことが本発明の要旨の１つである。

次に、図１１を参照して、本実施形態に係るマニピュレータ装置１のアシスト制御の流れについて説明する。図１１は、本実施形態に係るマニピュレータ装置１が移動される際にアシスト制御を行う処理の流れを示すフローチャートである。位置変化量検知部２１９において位置の変化量が検知される（Ｓ１１０１）と、外力算出部２１７は、第二リンク１８４、第三リンク１８５が移動されることで発生した位置の変化量であるか否か判定する（Ｓ１１０２）。

検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定された場合（Ｓ１１０２／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。外力算出部２１７は、例えば、マニピュレータ装置１がベース１９９ごと動かされて生じた位置の変化量については、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定する。

検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化であると判定された場合（Ｓ１１０２／Ｙｅｓ）、駆動制御部２１１は、検知された位置の変化量に基づいて第二リンク１８４、第三リンク１８５が移動するように第一モータ１８８、第二モータ１９１に出力されるモータ出力値を算出する（Ｓ１１０３）。

この時、駆動制御部２１１は、検知された位置の変化量に基づいて駆動部１０、１１を駆動させるために、電流値と駆動部１０、１１のトルク定数を乗算した値（モータ出力値）を回転駆動部２１２に出力する。回転駆動部２１２は、モータ出力値に基づいて、駆動部１０、１１を駆動させる（Ｓ１１０４）。

駆動部１０、１１がモータ出力値に基づいて回転駆動することによって、第二リンク１８４、第三リンク１８５は移動される。

以上説明したように、本実施形態においては、第二リンク１８４、第三リンク１８５の移動状況に応じて、ユーザが移動させようとする方向にアシスト制御を行うため、マニピュレータ装置１の操作性を向上させることが出来る。従って、マニピュレータ装置１の質量や、第一モータ１８８、第二モータ１９１のトルクが大きく、ユーザが意図する操作が行いにくい場合でも、ユーザが操作しようとする方向に第二リンク１８４、第三リンク１８５が移動するようにアシスト制御を行う。そのため、ユーザは第二リンク１８４、第三リンク１８５を操作する際に、小さな力を加えるだけで、マニピュレータ装置１を動かすことが出来る。

また、ユーザの操作によっては、第二リンク１８４、第三リンク１８５が突然移動する等して、ユーザが意図しない方向に、マニピュレータ装置１が移動することも考えられる。これに対して、本実施形態に係る制御装置２においては、図１２に示すように、第二リンク１８４、第三リンク１８５を移動させるために必要な移動力目標値を設定し、外力が不感帯を持つように設定する。さらに、予めマニピュレータ装置１が移動可能な位置の範囲や移動速度の設定値である位置速度指令値を設定する。

このような移動力目標値、位置速度指令値を設定してマニピュレータ装置１の動作を制御する処理について以下、図１６を参照して説明する。位置速度指令生成部２２２は、制御装置２に入力されたマニピュレータ装置１が移動可能な範囲や移動速度の設定値の情報に基づいて、位置速度指令値を設定する（Ｓ１３０１）。さらに、移動力目標生成部２２１は、制御装置２に入力された第二リンク１８４、第三リンク１８５を移動させるために必要な移動力目標値の情報に基づいて、移動力目標値を設定する（Ｓ１３０２）。尚、位置速度指令値または移動力目標値は何れか一つを設定する構成であってもよい。

位置変化量検知部２１９において位置の変化量が検知される（Ｓ１３０３）と、外力算出部２１７は、第二リンク１８４、第三リンク１８５が移動されることで発生した位置の変化量であるか否か判定する（Ｓ１３０４）。

検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定された場合（Ｓ１３０４／Ｎｏ）、駆動制御部２１１は、本処理を終了させる。外力算出部２１７は、図１１と同様に、例えば、マニピュレータ装置１がベース１９９ごと動かされて生じた位置の変化量については、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化ではないと判定する。

検知された位置の変化量が、外力算出部２１７において、第二リンク１８４、第三リンク１８５が操作されて発生した位置の変化量であると判定された場合（Ｓ１３０４／Ｙｅｓ）、位置情報取得部２１６は、駆動部１０、１１の位置に基づいて、第二リンク１８４、第三リンク１８５の位置情報を出力する。

移動力目標生成部２２１は、位置情報及び外力算出部２１７において検知された位置の変化量に基づいて、Ｓ１３０２で設定した移動力目標値と外力の推定値とを比較する。そして、この比較結果に基づいて、移動力目標情報を生成し、駆動制御部２１１に送信する（Ｓ１３０５、移動力目標値誤差検知）。

駆動制御部２１１は、移動力目標情報に基づいて第二リンク１８４、第三リンク１８５が移動力で移動するように、駆動部１０、１１を制御する。また、位置速度指令生成部２２２は、Ｓ１６０１にて設定した位置速度指令値とマニピュレータ装置１の移動情報及び移動速度とを比較する。そして、この比較結果に基づいて、位置速度指令情報を生成し、駆動制御部２１１に送信する（Ｓ１３０６）。

駆動制御部２１１は、推定された外力と同じ方向にアシスト制御が行われるように第一モータ１８８、第二モータ１９１に出力されるモータ出力値を回転駆動部２１２に出力する（Ｓ１３０７）。この時、検知された位置の変化量に基づいて駆動部１０、１１を駆動させるために、電流値と駆動部１０、１１のトルク定数を乗算した値と、Ｓ１３０５で生成された新たな移動力目標情報とＳ１３０６で生成された位置・速度を指定する情報とを参照して算出される。回転駆動部２１２は、モータ出力値に基づいて駆動部１０、１１を駆動させる（Ｓ１３０８）。

駆動部１０、１１がモータ出力値に基づいて回転駆動することによって、第二リンク１８４、第三リンク１８５は、Ｓ１３０５で生成された新たな移動力目標情報とＳ１３０６で生成された位置・速度を指定する情報に基づいて移動される。

以上説明したように、制御装置２に入力される移動力目標値及び位置速度設定値との誤差をフィードバックし、第二リンク１８４、第三リンク１８５を移動させるために、駆動部１０、１１の回転駆動のアシスト制御を実行する。このようにすることで、マニピュレータ装置１が突然動作することや、意図しない位置に移動することを抑制できる。また、移動力目標値を設定する範囲によっては、マニピュレータ装置１を所定の移動力を加えることで移動させることが可能である。

以上説明したアシスト制御の処理においては、外力と同じ方向のベクトルにマニピュレータ装置１を直接教示する場合を例として説明を行った。これらの処理においては、推定された外力と逆方向（逆ベクトル）にマニピュレータ装置１が移動するように機能を切り替える制御を行うことも可能である。尚、マニピュレータ装置１がアシスト制御される方向を切り替える制御は、スイッチ２８をＯＮ／ＯＦＦ操作して切り替える構成であってもよい。また、推定された外力が所定以上である場合や回転速度を検知した場合に、アシスト制御を実行しない構成であってもよい。

尚、マニピュレータ装置１を軽量化または小型化するために、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４の代わりに、第一ワイヤ１８６の伸縮状況に基づいて第二リンク１８４、第三リンク１８５の移動量を検知してもよい。図１４は、本実施形態に係る第一ワイヤ１８６に取り付けられるひずみゲージ１８６Ｂを示す図である。

図１４に示すように、ひずみゲージ１８６Ｂは、第一ワイヤ１８６の直線部分と接続されたプレート１８６Ａの上に設置されており、第一ワイヤ１８６の伸縮を電気的に検知することが出来る。従って、ひずみゲージ１８６Ｂは、ワイヤの伸縮量を検知する伸縮検知部として機能する。マニピュレータ装置１のユーザが、第二リンク１８４、第三リンク１８５を移動させた場合に、制御装置２とひずみゲージ１８６Ｂとに接続されている電気回路および伝達経路によって第一ワイヤ１８６の伸縮量が、外力算出部２１７に伝達される。

外力算出部２１７は、ひずみゲージ１８６Ｂから伝達された第一ワイヤ１８６の伸縮量に基づいて、マニピュレータ装置１に加えられた外力の値を推定する。尚、第二ワイヤ１８７についても同様の構成にすることによって、第二ワイヤ１８７の伸縮量を外力算出部２１７に伝達することが出来る。

また、マニピュレータ装置１を軽量化または小型化するために、第一関節軸エンコーダ１７３、第二関節軸エンコーダ１７４の代わりに、第一ワイヤ１８６の張力調整部１３５によって第二リンク１８４、第三リンク１８５の移動量を検知することも出来る。図１５は、本実施形態に係る第一ワイヤ１８６に取り付けられる張力調整部１３５の構造を示す図である。

図１５に示すように、張力調整部１３５は、張力検知部１３１、アイドラプーリ１３２、１３３、１３４を含む。張力検知部１３１は、図１４で説明したひずみゲージ１８６Ｂを含んで構成され、第二リンク１８４、第三リンク１８５が移動させられて第一ワイヤ１８６に生じる張力の変化を検知する。また、アイドラプーリ１３２、１３３、１３４は、第一ワイヤ１８６を這いまわすように配置される。

外力算出部２１７は、張力検知部１３１から伝達された第一ワイヤ１８６の伸縮量に基づいて、マニピュレータ装置１に加えられた外力の値を推定する。尚、第二ワイヤ１８７についても同様の構成にすることによって、第二ワイヤ１８７の伸縮量を外力算出部２１７に伝達することが出来る。

さらに、第二リンク１８４、第三リンク１８５の移動方向を検知するために、図１６及び図１７に示すような移動方向検知部材５００を用いてもよい。図１６は、移動方向検知部材５００に用いられる回転ダンパ５０１の構成を示す図、図１７は、本実施形態に係る移動方向検知部材５００である。

図１６に示すように、回転ダンパ５０１は、入力軸５０１Ａ、出力軸５０１Ｂ、シャッタ部５０１Ｃを含む。シャッタ部５０１Ｃは、出力軸５０１Ｂに付加され構成されている。また、図１７に示すように、移動方向検知部材５００は、第二リンク１８４、第三リンク１８５にそれぞれ装着されており、回転ダンパ５０１、ブラケット５０２、フォトインタラプタ５０３、スプリングプランジャ５０４を含む。

回転ダンパ５０１は、第一従動プーリ１９５、第二従動プーリ１９８にそれぞれ装着されている。移動方向検知部材５００は、ブラケット５０２によって第二リンク１８４、第三リンク１８５に固定されている。また、フォトインタラプタ５０３およびスプリングプランジャ５０４はブラケット５０２上に配置されている。

フォトインタラプタ５０３は、対向する発光部と受光部とから構成され、発光部から照射される光を受光部で検出する際の検出状況に応じて物体の有無あるいは位置を判定するセンサである。スプリングプランジャ５０４は、対向する弾性体によってシャッタ部５０１Ｃを保持する保持部材である。

図１６に示す例においては、２つのフォトインタラプタ５０３がどちらもＯＦＦである中立位置を表している。この状態からマニピュレータ装置１が操作された場合、入力軸５０１Ａの回転が出力軸５０１Ｂに伝わり、シャッタ部５０１Ｃが回転する際の正または負の回転方向に応じて２つのフォトインタラプタ５０３のうち、いずれかをＯＮにする。

出力軸５０１Ｂの位相は、シャッタ部５０１Ｃが両側をスプリングプランジャ５０４によって押圧されているため、フォトインタラプタ５０３の検知位置からシャッタ部５０１Ｃが抜去されない範囲で動作する。入力軸５０１Ａの回転が停止すると、弾性体の作用で出力軸５０１Ｂは中立位置に復帰し、次回入力軸５０１Ａが回転するまで停止する。

このように、従動プーリに伝達される駆動力の伝達方向に応じて制御されるフォトインタラプタ５０３のＯＮ信号に基づいてアシスト制御を開始することによって、簡単な回路構成およびアルゴリズムを実装したマニピュレータ装置１の制御装置２を実現することが出来る。以上説明したように、移動方向検知部材５００は、従動プーリに伝達される駆動力の伝達方向に応じて第二リンク１８４及び第三リンク１８５の移動方向をそれぞれ検知する移動方向検知部として機能する。

尚、入力軸５０１Ａと接続することが可能であれば、例えば、第一駆動プーリ１９４、第二駆動プーリ１９６や第一モータ減速装置１９０、第二モータ減速装置１９３もしくはモータ軸等に移動方向検知部材５００を装着する構成であってもよい。

また、移動方向を検知するために、マグネット式もしくはコイルスプリング式もしくはフリクション式のトルクリミッタを移動方向検知部材５００として使用してもよい。さらに、ストローク型ダンパを回転ダンパ５０１の代わりに使用することも出来る。ストローク型ダンパを用いる場合には、マニピュレータ装置１における回転部材の他に、直線運動を行う部材にも適用することができるため、例えば、第一ワイヤ１８６、第二ワイヤ１８７や張力調整部１３５に対して適用することが可能である。

尚、Ｓ１１０１及びＳ１３０３の処理においては、マニピュレータ装置１が停止状態から移動されたタイミングにおいて位置の変化量を検知してもよい。このようにすると、マニピュレータ装置１が動き出す際にアシスト制御を行うことが出来るため、ユーザが第二リンク１８４、第三リンク１８５を操作するために必要な力を小さくすることが出来る。

１ マニピュレータ装置
１０、１１ 駆動部
２１ ＣＰＵ
２２ ＲＡＭ
２３ ＲＯＭ
２４ エンジン
２５ ＨＤＤ
２６ Ｉ／Ｆ
２７ センサ
２８ スイッチ
２９ バス
１３５ 張力調整部
１７１ 第一関節トルクリミッタ
１７２ 第二関節トルクリミッタ
１７３ 第一関節軸エンコーダ
１７４ 第二関節軸エンコーダ
１８１ 第一関節
１８２ 第二関節
１８３ 第一リンク
１８４ 第二リンク
１８５ 第三リンク
１８６ 第一ワイヤ
１８７ 第二ワイヤ
１８８ 第一モータ
１８９ 第一モータエンコーダ
１９０ 第一モータ減速装置
１９１ 第二モータ
１９２ 第二モータエンコーダ
１９３ 第二モータ減速装置
１９４ 第一駆動プーリ
１９５ 第一従動プーリ
１９６ 第二駆動プーリ
１９７ 第二中継プーリ
１９８ 第二従動プーリ
１９９ ベース
１８６Ｂ ひずみゲージ
２０１ 回転駆動制御部
２１１ 駆動制御部
２１２ 回転駆動部
２１４ 回転検知部
２１５ ＦＢ取得部
２１６ 位置情報取得部
２１７ 外力算出部
２１８ 外力検知部
２１９ 位置変化量検知部
２２１ 移動力目標生成部
２２２ 位置速度指令生成部
５００ 移動方向検知部

国際公開第２０１３／０２７２５０号公報

Claims (10)

1. 連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御装置であって、
   前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出する外力算出部と、
   前記移動部を移動させるための駆動部を制御する駆動制御部と、
   前記交差箇所において前記駆動部の動力に従動して駆動する従動部と、
   前記駆動部の動力を前記従動部に伝達する駆動伝達部と、
   を含み、
   前記駆動制御部は、
   前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて前記駆動部を制御し、
   前記従動部は、
   前記外力の推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動することを特徴とするマニピュレータ装置の制御装置。
2. 前記駆動制御部は、
   前記移動状況が変化したタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項１に記載のマニピュレータ装置の制御装置。
3. 前記駆動制御部は、
   前記移動部が停止状態から移動させられたタイミングに前記移動部が移動させられた方向に前記移動部が移動するように前記駆動部を制御することを特徴とする請求項１または請求項２に記載のマニピュレータ装置の制御装置。
4. 前記駆動伝達部の伸縮量を検知する伸縮検知部を含み、
   前記外力算出部は、
   前記駆動伝達部の伸縮状況に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。
5. 前記移動部の移動量を光学的に検知する位置変化量検知部を含み、
   前記外力算出部は、
   前記交差箇所における前記移動状況の光学的な変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。
6. 前記駆動伝達部の伸縮量を検知する伸縮検知部を含み、
   前記外力算出部は、
   検知された前記伸縮量に基づいて前記交差箇所における前記移動状況の変化を検知し、検知した前記移動状況の変化に基づいて前記推定値を算出することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。
7. 前記交差箇所における前記移動部の移動方向を検知する移動方向検知部を含み、
   前記移動方向検知部は、
   前記従動部に前記駆動部の動力を伝達する際の伝達方向に基づいて前記移動部の移動方向を検知することを特徴とする請求項１ないし請求項３いずれか一項に記載のマニピュレータ装置の制御装置。
8. 前記交差箇所における前記移動部の移動方向を検知する移動方向検知部を含み、
   前記移動方向検知部は、
   前記伸縮量に基づいて前記移動部の移動方向を検知することを特徴とする請求項６に記載のマニピュレータ装置の制御装置。
9. 連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御方法であって、
   前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出し、
   前記移動部を移動させるための駆動部を前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて制御し、
   前記交差箇所において前記駆動部の動力に従動して駆動する従動部に、前記駆動部の動力を伝達し、
   前記従動部を、前記推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動させることを特徴とするマニピュレータ装置の制御方法。
10. 連結された複数の移動部の交差箇所及び前記移動部を支持する支持部と前記移動部との交差箇所が夫々駆動するマニピュレータ装置の制御プログラムであって、
    前記交差箇所における前記移動部の移動状況の変化に基づいて前記移動部に加わる外力の推定値を算出するステップと、
    前記移動部を移動させるための駆動部を前記移動状況が変化したタイミングに算出された前記外力の推定値に基づいて制御するステップと、
    前記交差箇所において前記駆動部の動力に従動して駆動する従動部に、前記駆動部の動力を伝達するステップと、
    前記従動部を、前記推定値に基づいて制御される前記駆動部の動力に従動して前記移動部が移動するように駆動させるステップと、
    を実行させることを特徴とするマニピュレータ装置の制御プログラム。