JP5044011B2 - マニピュレータおよびその制御方法 - Google Patents

Description

本発明は、マニピュレータおよびその制御方法に関するものである。

障害物への衝突を回避する技術に関して、さまざまな提案がなされている。

例えば、特許文献１に開示された従来のマニピュレータでは、多関節構造体の先端にカメラが設けられ、このカメラでマニピュレータの先端付近の画像を撮影すると共に、障害物を検知した場合には、障害物を避けるように姿勢を制御する。

特開平９−２０７０８９号公報

従来のマニピュレータでは、姿勢制御するときに障害物が検知されると、可動部材を駆動している駆動部のモータに流す電流の向きが反転されて、アームに逆向きのトルクを与える。そして、この逆向きのトルクによって、衝突を回避している。一般に、マニピュレータにおいては、駆動トルクを稼ぐためにモータ出力部に減速機構が組み込まれ、モータが作動しているときの回転数は比較的高くなっている。そのため、モータの減速から反転に至る時間が大きくなる傾向がある。すなわち、従来のマニピュレータでは、可動部材の反転に至るまでの時間が大きくなるため、衝突回避又は衝突時の反転動作の応答性が不十分であるという課題がある。

そこで本発明は、可動部材の反転動作を迅速に行うことを可能にするマニピュレータおよびその制御方法を提供することを目的とする。

本発明の一局面に従うマニピュレータは、第１可動部材と、第２可動部材と、保持部と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、前記第２可動部材と前記保持部とを回動可能に連結する第２関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第２関節部を駆動可能な第２関節駆動部と、前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定部と、前記部材特定部によって特定された前記一方の可動部材を前記障害物から離れる方向に回動させると共に、他方の可動部材を前記障害物に近づく方向に回動させるように、前記第１関節駆動部及び前記第２関節駆動部を制御する制御装置と、を備えている。

また本発明の他の局面に従うマニピュレータは、第１可動部材と、第２可動部材と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、移動駆動部を有する移動部と、前記第２可動部材と前記移動部とを回動可能に連結する第３関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第３関節部を駆動可能な第３関節駆動部と、前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定部と、前記部材特定部によって特定された一方の可動部材を前記障害物から離れる方向に回動させると共に、前記一方の可動部材が前記第１可動部材の場合には、前記移動部を前記障害物から離れる方向に移動させ、前記一方の可動部材が前記第２可動部材の場合には、前記移動部を前記障害物に近づく方向に移動させるように、前記第１関節駆動部、前記第３関節駆動部及び前記移動駆動部を制御する制御装置と、を備えている。

また本発明の他の局面に従うマニピュレータの制御方法は、第１可動部材と、第２可動部材と、保持部と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、前記第２可動部材と前記保持部とを回動可能に連結する第２関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第２関節部を駆動可能な第２関節駆動部と、を備えたマニピュレータの制御方法であって、前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定ステップと、前記部材特定ステップにおいて特定された一方の可動部材を前記障害物から離れる方向に回動させると共に、他方の可動部材を前記障害物に近づく方向に回動させるように、前記第１関節駆動部及び前記第２関節駆動部を制御する制御ステップと、が含まれている。

また本発明の他の局面に従うマニピュレータの制御方法は、第１可動部材と、第２可動部材と、移動駆動部を有する移動部と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、前記第２可動部材と前記移動部とを回動可能に連結する第３関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第３関節部を駆動可能な第３関節駆動部と、を備えたマニピュレータの制御方法であって、前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定ステップと、前記部材特定ステップにおいて特定された一方の可動部材を障害物から離れる方向に回動させると共に、前記一方の可動部材が前記第１可動部材の場合には、前記移動部を前記障害物に近づく方向に移動させるように前記第１関節駆動部、前記第３関節駆動部及び前記移動駆動部を制御する制御ステップと、が含まれている。

本発明よれば、可動部材の反転動作を迅速に行うことが可能である。

本発明の実施の形態１におけるマニピュレータの概略構成図である。 同マニピュレータの構成を示すブロック図である。 同マニピュレータの衝突回避動作の第１の例を示す概略正面図である。 同マニピュレータの衝突回避動作の第２の例を示す概略正面図である。 同マニピュレータの加速判断部の加速判断条件を説明するための図である。 同マニピュレータの可動部材の回転に寄与する反力成分の方向を説明するための図である。 同マニピュレータの障害物回避動作を説明するフローチャートである。 本発明の実施の形態２におけるマニピュレータの概略構成図である。 同マニピュレータの構成を示すブロック図である。 同マニピュレータの退避動作の第１の例を示す概略正面図である。 同マニピュレータの退避動作の第２の例を示す概略正面図である。

以下、本発明の実施の形態について、図面を用いて詳細に説明する。

（実施の形態１）
まず、図１を参照しながら、本発明の実施の形態１におけるマニピュレータ１の構成について説明する。図１は、本発明の実施の形態１におけるマニピュレータ１の概略構成図である。

マニピュレータ１は、可動部９１と、この可動部９１を保持する保持部８とを備えている。可動部９１は、可動部材としてのハンド２と、可動部材としてのアーム４と、可動部材としてのアーム６とを有する。なお、可動部９１は、少なくとも２つの可動部材（第１の可動部材と第２の可動部材）を有すればよく、例えば、可動部９１は、ハンド２とアーム４のみを有する構成や、アーム４とアーム６のみを有する構成や、あるいは、さらに別の可動部材を連結した構成としてもよい。

ハンド２は、物を把持する機能を有し、関節部３を介してアーム４の先端部に連結されている。アーム４の基端部は、関節部５を介してアーム６の先端部に連結されている。アーム６の基端部は、関節部７を介して保持部８に連結されている。ハンド２およびアーム４、６は、それぞれが互いに回動可能に連結されている。関節部５の回動軸と関節部７の回動軸とは図１の紙面垂直方向において互いに平行となっている。

保持部８は、機構部８１と駆動部８２とを備える。機構部８１は、可動部９１を撮影可能な撮影部１１、１２を備えている。撮影部１１，１２は、可動部９１に衝突する障害物９も撮影可能である。駆動部８２は、操作指令を入力するための入力部２１と、障害物９との衝突を監視するための衝突監視部２３と、ハンド２およびアーム４、６を駆動する関節駆動部（後述するアクチュエータ３１、５１、７１）の制御装置２２とを備えている。撮影部１１、１２は衝突監視部２３に撮影画像を出力する。制御装置２２は、入力部２１から得た操作指令情報と衝突監視部２３から得た監視情報とに基づいて、関節駆動部の制御部２２で制御する。

なお、ここでは、撮影部１１、１２を機構部８１に設ける構成としているが、撮影部１１，１２を機構部８１とは別に設ける構成（例えば、マニピュレータ１全体を俯瞰できる周辺環境位置に設ける構成）であっても良い。

次に、図２を参照しながら、マニピュレータ１の構成および動作について説明する。図２は本発明の実施の形態１におけるマニピュレータ１の構成を示すブロック図である。

関節部３、５、７は、関節駆動部としてのアクチュエータ３１、５１、７１をそれぞれ備えている。アクチュエータ３１は減速機３８を介してアーム４に対してハンド２を回転駆動させる。同様に、アクチュエータ５１は減速機３８を介してアーム６に対してアーム４を回転駆動させる。アクチュエータ７１は減速機３８を介して保持部８に対してアーム６を回転駆動させる。減速機３８は、後述するモータ３４の回転をギア等により減速させると共に、アクチュエータ３１、５１、７１の駆動トルクを高める。また、アクチュエータ３１、５１、７１の駆動力を、それぞれハンド２、アーム４、６に伝達する動力伝達機構としての役割も有する。減速機３８には、例えば、遊星歯車減速機、平歯車減速機やベルト減速機構などを使用することができる。

アクチュエータ３１、５１、７１は、駆動用のモータ３４と、このモータ３４を駆動するモータ駆動部３５と、エンコーダ３６とを備えている。

モータ駆動部３５は、例えばＨブリッジ駆動回路等の駆動回路を有し、この駆動回路によりモータに電力を供給して、モータ３４の正逆の回転を行う。

エンコーダ３６は、モータ３４の軸（図示せず）に連結するように設けられ、モータ３４の回動情報を検出する。エンコータ３６には、例えば、符号板とフォトセンサから構成される光学式エンコーダや、ホール素子や磁気抵抗素子とＮＳ２極着磁の回転磁石とを用いた磁気式エンコーダなどを使用することができる。

入力部２１は、図略の入力デバイスを備える。入力部２１は、この入力デバイスによって入力された操作指令を制御装置２２に出力する。入力デバイスとしては、例えば、キー入力装置、ジョイステック、タッチパネルなどを使用することができる。なお、入力部２１は、図１に示す例では保持部８に収納した構成としているが、保持部８と着脱自在な構成や別のユニットとして構成してもよい。また、マニピュレータ１が自律移動可能に構成されている場合には、入力部２１は、外部に構成される上位制御装置（図示せず）からの操作指令を入力するようにしてもよい。

衝突監視部２３は、各撮影部１１、１２によって得られた２つの撮影画像を分析し、ステレオ視により検出した距離方向を含む障害物９の動き情報、障害物９と可動部９１との相互の位置関係などの位置関係情報を監視する。そして、衝突監視部２３は、障害物９と可動部９１とが衝突の可能性があるか否かを判断する。そして、衝突監視部２３は、障害物９と可動部９１とが衝突する可能性があると判断した場合には、その衝突する可能性のある可動部９１の可動部材（例えばアーム４）を特定し、障害物９の情報（動き情報、位置情報）、衝突する可能性がある可動部材の位置情報を監視情報として制御装置２２に出力する。なお、衝突監視部２３の機能を制御装置２２の機能として設けるようにしてもよい。また、ここでは、距離方向を含む障害物９の動き、障害物９と可動部９１との相互の位置関係等を監視する手段として、撮影部１１、１２の視差を利用したステレオカメラ法を用いているが、この手法に限定されない。例えば、発光ダイオードから照射した光が測定対象で反射してＣＣＤ（Charge Coupled Device）上に戻ってくる到達時間（Time of Flight）を計測し、画像情報を重ねることで、対象の画像と対象までの距離を画素ごとに出力する距離画像センサなどを使用することができる。

制御装置２２は、プログラムに基づいて各種機能を実行する中央制御装置（ＣＰＵ）と、各種プログラムなどを記憶している読み出し専用リードオンリーメモリ（ＲＯＭ）と、データを一時格納する書き換え可能なランダムアクセスメモリ（ＲＡＭ）と、外部とのデータの入出力を行う入出力部とを備えている。これにより、制御装置２２は、入力部２１から得た操作指令と衝突監視部２３から得た監視情報とに基づいて各種プログラムを実行することにより、アクチュエータ３１、５１、７１を駆動し、可動部９１と障害物９とが衝突する可能性がある場合には、回避動作をするように制御する。また、制御装置２２は、エンコーダ３６から得たモータ３４の回転情報に基づいて、ハンド２、アーム４およびアーム６のそれぞれの位置および姿勢、可動部材間の角度を検知することができる。さらに、エンコーダ３６から得たモータ３４の回転方向の情報に基づき、制御装置２２は、衝突監視部２３によって特定された可動部材（例えばアーム４）と異なる可動部材（例えばアーム６）の回動方向に関する情報（回動方向情報）を取得することができる。

また、制御装置２２は、加速判断部２５を備えている。加速判断部２５は、障害物９と可動部９１との衝突回避動作を行うときの加速判断を行う。この加速判断部２５については後述する。

次に、図２、図３および図４を参照しながら、マニピュレータ１における障害物９との衝突回避動作について説明する。図３は本実施の形態１におけるマニピュレータ１の衝突回避動作の第１の例を示す概略正面図であり、図４は同マニピュレータ１の衝突回避動作の第２の例を示す概略正面図である。

まず、図２および図３を参照しながら、マニピュレータ１における衝突を回避する第１の例について説明する。

制御装置２２は、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１を駆動させると共に、衝突監視部２３から監視情報を取得する。制御装置２２は、この監視情報から障害物９と可動部９１とが衝突の可能性があることを検知した場合には、衝突の可能性のある可動部９１の可動部材（ハンド２またはアーム４、６）の位置・姿勢に関する情報、他の可動部材の位置・姿勢に関する情報、関節部３、５、７の駆動情報（回動方向情報を含む）などに基づき、障害物９から可動部材を回避させる動作を開始する。すなわち、障害物９から可動部材を回避するように制御情報を生成し、この制御情報によりアクチュエータ３１、５１、７１を駆動させ、関節部３、５、７を回動制御させる。

マニピュレータ１は、例えば、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１を駆動させる。例えば、マニピュレータ１は、操作指令に応じて、アクチュエータ３１を停止させ、アクチュエータ５１、７１を駆動させる。

これにより、マニピュレータ１は、例えば、図３（ａ）に示すように、アーム４を関節部５の回転中心に対してＡ方向に回動し、アーム６を関節部７の回転中心に対してＡ方向に回動する。すなわち、関節部７を中心として、アーム６を回動させる方向とアーム４を回動させる方向とが同じ方向となっている。

この回動動作により、例えば、障害物９とアーム４とが衝突する可能性がある場合には、衝突監視部２３が作動し、監視情報を出力する。監視情報は制御装置２２に入力される。

制御装置２２は、アーム４と障害物９との衝突可能性を検知した後、監視情報に基づいて、アクチュエータ５１及びアクチュエータ７１を駆動制御させる。具体的には、アクチュエータ５１は、アーム４をＡ方向とは逆方向の回転トルクで駆動させ、アクチュエータ７１は、回転トルクを上げてアーム６を回動方向を変えないで加速させる。すなわち、マニピュレータ１は、アーム６に対してアーム４を障害物９に対する回避方向（図３（ｂ）のＢ方向）に回動させながら、アーム６を障害物９に対して衝突方向に加速回動させる。

図３（ｂ）に示すように、アーム６を関節部７の回転中心に対してＡ方向に加速回動させると、アーム４の基端部がＡ方向に加速することになるので、アーム４の重心位置４ａには、慣性による反力（回転モーメント）が作用する。この反力はアーム４を関節部５の回転中心回りにＢ方向に回動させるように働く。すなわち、この反力によりアーム６の回動を加速させることによって、アーム４の回動を反転させる動きをアシストすることができる。したがって、アーム６の動きを障害物９に対して衝突方向に加速したにもかかわらず、アーム４には衝突方向に反対向きの回転モーメントが働くため、アーム４の姿勢がＢ方向に迅速に変化する。すなわち、アーム４は、障害物９から離れる方向に移動する。

この結果、マニピュレータ１では、アーム４をＢ方向に回動駆動させるときに、アクチュエータ５１の回転トルクに慣性による反力も加えて駆動させることができ、障害物９からアーム４を迅速に回避させることができる。

さらに、アーム６の姿勢によっては、可動部９１が保持部８に向けて折り畳まれるように駆動することになる。これにより、より確実にアーム４を障害物９から回避させることができる。

以上のように、マニピュレータ１は、アーム６の回動方向を変えることなく加速することにより、アクチュエータ５１の回転トルクに加え、反力も利用して、アーム４を回避方向へ回転駆動させることができる。それにより、アーム４を迅速に障害物９から回避させることができる。

なお、アクチュエータ５１の機械的・電気的時定数が大きい場合や駆動力が小さい場合であっても、本実施の形態では反力を活用しているため、上述の如く、アーム４を障害物９から迅速に回避させることができる。

次に、図４を参照しながら、衝突を回避する第２の例について説明する。

上記の第１の例では、アーム６の回動を図３のＡ方向に加速させることによって、アーム４を反転させる方向（図３のＢ方向）に反力を生じさせるようにした。これに対し、第２の例では、アーム６の回動中心となっている関節部７自体を移動させることによって反力を生じさせるものである。

マニピュレータ１の保持部８は、保持部本体８ａと、この保持部本体８ａに対して、可動部の位置を平行移動させるための移動部１３とを備えている。この移動部１３を用いて衝突を回避する。

移動部１３は、保持部８の保持部本体８ａに所定の方向に摺動可能に設けられた部材である。この移動部１３としては、例えば、直動スライダを用いる。移動部１３に関節部７が支持されているので、この移動部１３により可動部９１を図４（ｂ）のＤ方向に平行移動させることができる。移動部１３は移動駆動部１４を備えている。この移動駆動部１４は、モータ（図示せず）、減速機（図示せず）、モータを駆動するドライバ（図示せず）を有し、制御装置２２によって制御される。アーム６は関節部７を介して移動部１３に回動自在に連結されている。移動部１３の移動により反力を確実に発生させるため、移動部１３の移動方向は、関節部７の回動軸に垂直な方向となるのが望ましい。しかしながら、保持部８の構成等により、移動部１３の移動範囲が制限される場合は、移動部１３の移動方向をこれ以外の方向にすることもできる。

ここで、例えば図４（ａ）に示すように、アーム４の重心位置４ａが、関節部５に対して根元側（Ｘ−Ｘ軸より機構部８１の側）にある場合を例として、障害物９との衝突回避動作について説明する。この場合、マニピュレータ１は、第２の例における障害物９とアーム４との位置関係およびアーム４、６の動作状況に基づいて衝突回避動作を行う。

マニピュレータ１は、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１を駆動させる。例えば、マニピュレータ１は、アクチュエータ３１を停止させ、アクチュエータ５１、７１を駆動させる。具体的には、アーム４を、関節部５を中心としてＢ方向に回動させ、アーム６を関節部７を中心としてＡ方向に回動させる。

この動作時において、障害物９とアーム４とが衝突する可能性がある場合には、衝突監視部２３が作動し、制御装置２２に監視情報を出力する。制御装置２２は、この監視情報に基づいて、アーム４が障害物９から回避するように、アクチュエータ５１、７１を制御すると共に、移動駆動部１４を制御する。

具体的には、制御装置２２は、アーム４と障害物９との衝突可能性を検知した後、アクチュエータ５１ではアーム４をＢ方向とは逆方向（Ａ方向）の回転トルクで駆動させると共に、アクチュエータ７１ではアーム６をＡ方向とは逆方向（Ｂ方向）の回転トルクで駆動させる。これにより、関節部５回りのアーム４の回動方向が反転するとともに、関節部７回りのアーム６の回動方向も反転する。制御装置２２は、さらに、移動部１３を停止状態からＤ方向に加速移動するように、移動駆動部１４を駆動させる。すなわち、マニピュレータ１は、アーム４およびアーム６をそれぞれ逆方向に回動させながら、移動部１３を障害物９に対して衝突方向に加速移動させる。

図４（ｂ）に示すように、移動部１３をＤ方向に加速移動させると、アーム６の基端部がＤ方向に加速するので、アーム４の重心位置４ａには慣性による反力（回転モーメント）がＥ方向に作用する。この反力により、アーム４には、関節部５を回転中心としてアーム４をＡ方向に回動させる力が働く。同様に、アーム６の重心位置６ａには慣性による反力がＥ方向に作用する。このため、アーム６には、関節部７を回転中心としてアーム６をＢ方向に回動させる力が働く。このため、移動部１３を障害物９に対して衝突方向（Ｄ方向）に加速したにもかかわらず、アーム４の重心位置４ａおよびアーム６の重心位置６ａには、慣性による反力が両アーム４，６を反転させるアシスト力としてＥ方向に作用するので、障害物９と衝突する可能性があるとして特定されたアーム４を、障害物９から離れる方向に迅速に移動させることができる。

以上によりマニピュレータ１は、アーム４に逆向きの回転トルク（衝突回避方向）を与えると共に、反力も利用してアーム４を障害物９から回避することができるので、衝突を迅速に回避することができる。

なお、例えばアーム４の衝突回避動作時において、アーム６の加速動作及び移動部１３の加速動作を併用してもよく、あるいは何れか一方のみの動作を行ってもよい。

次に、図５および図６（ａ）〜（ｄ）を参照しながら、制御装置２２に設けられた加速判断部２５について説明する。図５は本実施の形態１におけるマニピュレータ１の加速判断部２５による加速判断条件を説明するための図であり、図６（ａ）〜（ｄ）はマニピュレータ１の可動部材２、４、６の回動に寄与する反力成分（以下、単に反力と呼ぶ）の方向を説明するための図である。

加速判断部２５は、図５に示すように、アーム６とアーム４とのなす角度θ、アーム４と障害物９との位置関係及びアーム６の回動方向によって決定される加速判断条件により、加速の適否を判断する。そして、アーム４の重心位置４ａに作用する反力の方向が衝突回避動作に寄与する関係にあると判断する場合には「加速回動させる」という判断結果を出力し、重心位置４ａに作用する反力が衝突回避動作に寄与しないと判断する場合には「加速回動させない」という判断結果を出力する。

加速可否の判断について、具体的に説明する。本実施の形態では、図５に示すように、互いに直交する絶対座標軸Ｘａ、Ｙａの原点に対してアーム６を時計回り（Ａ方向）に回動するとともに、互いに直交する相対座標軸Ｘｂ、Ｙｂの原点に対してアーム４を時計回り（Ａ方向）に回動させる場合を動作例として説明する。なお、図５に示すように、絶対座標軸Ｘａ、Ｙａの原点は、関節部７の回転中心に設定され、相対座標軸Ｘｂ、Ｙｂの原点は、関節部５の回転中心に設定されている。そして、相対座標軸Ｘｂは、アーム６の長手方向と直交する方向に設定され、相対座標軸Ｙｂは、アーム６の長手方向に設定されている。

ここで、相対座標軸Ｘｂ、Ｙｂの原点は、絶対座標軸Ｘａ、Ｙａの原点を中心とし、両原点間の距離を半径とする円周２０上を移動する。

加速判断部２５は、相対座標軸Ｘｂ、Ｙｂ系におけるアーム４の位置に基づいて、アーム４の重心位置４ａに作用する反力方向を判断し、この反力方向と、アーム４と障害物９との位置関係とによって加速判断を行う。すなわち、加速判断部２５は、まず、相対座標軸Ｘｂ、Ｙｂ系において、アーム４の重心位置が第１領域（Ｘｂ＜０、Ｙｂ＜０）、第２領域（Ｘｂ＜０、Ｙｂ＞０）、第３領域（Ｘｂ＞０、Ｙｂ＞０）、第４領域（Ｘｂ＞０、Ｙｂ＜０）にある場合の反力方向を判断する。そして、この反力方向と、アーム４と障害物９との位置関係によって加速判断を行う。なお、図５において、アーム６とアーム４とのなす角度θは、関節部５と関節部７の回動中心を結ぶ線分に一致するアーム６の中心線を基準とし、そこからアーム４の回動方向を正としたときの両アーム４，６のなす角度である。すなわち、アーム４がアーム６に重なり合うときの角度がゼロとなり、そこからアーム４を図５の時計回りに回動したときの角度がθである。

次に、図６（ａ）〜図６（ｄ）を参照しながら、各領域におけるアーム４の重心に作用する反力方向を説明する。

図６（ａ）は、アーム４の重心位置４ａが第１領域にある場合の例である。第１領域を前述の角度θで記述すると、０°＜θ＜９０°の領域となる。この時、アーム６を加速させることによってアーム４の重心位置４ａに生ずる反力はＦ方向に作用する。すなわち、Ａ方向のアーム４の回動によって重心位置４ａが移動する方向と同じ方向に反力が作用する。このため、加速判断部２５は、反力によってアーム４の回動が促進される方向と逆側（角度θが小さくなる方向）に障害物９がある場合には「加速回動させる」と判断する。アーム６を加速させることによってアーム４に作用する反力が衝突回避方向となるからである。一方、反力によってアーム４が回動する方向（角度θが大きくなる方向）に障害物９がある場合には「加速回動させない」と判断する。この場合には、反力が衝突方向に作用することになるからである。

図６（ｂ）は、アーム４の重心位置４ａが第２領域にある場合の例である。第２領域を前述の角度θで記述すると、９０°＜θ＜１８０°の領域となる。この時、アーム６を加速させることによってアーム４の重心位置４ａに生ずる反力はＧ方向に作用する。すなわち、Ａ方向のアーム４の回動によって重心位置４ａが移動する方向とは反対方向に反力が作用する。このため、加速判断部２５は、反力によってアーム４の回動が促進される方向と逆側（角度θが大きくなる方向）に障害物９がある場合には「加速回動させる」と判断する。アーム６を加速させることによってアーム４に作用する反力が衝突回避方向となるからである。一方、反力によってアーム４が回動する方向（角度θが小さくなる方向）に障害物９がある場合には「加速回動させない」と判断する。この場合には、反力が衝突方向に作用することになるからである。

図６（ｃ）は、アーム４の重心位置４ａが第３領域にある場合の例である。第３領域を前述の角度θで記述すると、１８０°＜θ＜２７０°の領域となる。この時、アーム４の重心位置４ａに加わる反力はＨ方向に作用する。すなわち、Ａ方向のアーム４の回動によって重心位置４ａが移動する方向とは反対方向に反力が作用する。よって、加速判断部２５は、反力によってアーム４が回動する方向と逆側（角度θが大きくなる方向）に障害物９がある場合には、「加速回動させる」と判断する。アーム６を加速させることによってアーム４に作用する反力が衝突回避方向となるからである。一方、反力によってアーム４が回動する方向（角度θが小さくなる方向）に障害物９がある場合には、「加速回動させない」と判断する。この場合には、反力が衝突方向に作用することになるからである。

図６（ｄ）は、アーム４の重心位置４ａが第４領域にある場合の例である。第４領域を前述の角度θで記述すると、２７０°＜θ４＜３６０°の領域となる。この時、アーム４の重心位置４ａに加わる反力はＩ方向に作用する。すなわち、Ａ方向のアーム４の回動によって重心位置４ａが移動する方向と同じ方向に反力が作用する。このため、加速判断部２５は、反力によってアーム４が回動する方向と逆側（角度θが小さくなる方向）に障害物９がある場合には、「加速回動させる」と判断する。アーム６を加速させることによってアーム４に作用する反力が衝突回避方向となるからである。一方、反力によってアーム４が回動する方向（角度θが大きくなる方向）に障害物９がある場合には「加速回動させない」と判断する。この場合には、反力が衝突方向に作用することになるからである。

なお、加速判断部２５は、各領域におけるアーム４の重心位置に加わる反力の作用方向を記憶部（図示せず）に記憶し、この記憶された反力の作用方向に基づいて判断してもよい。

このように、加速判断部２５は、加速判断条件に基づいて、慣性による反力が衝突回避動作に寄与すると判断される場合には「加速回動する」と判断する。これにより、制御装置２２は、アーム４の重心位置４ａに加わる反力の作用方向が障害物９との衝突回避動作に寄与するときにアーム６を加速回動することができる。すなわち、アーム４は、アクチュエータ５１による障害物回避方向の回転トルクに加え、障害物回避方向への反力も利用することができるので、障害物９との衝突を迅速に回避することができる。

次に、図７を参照しながら、マニピュレータ１の動作について説明する。図７は、本発明の実施の形態１におけるマニピュレータ１の障害物回避動作を説明するフローチャートである。

まず、マニピュレータ１は、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１により関節部３、５、７を駆動させる（ステップＳ１００）。

次に、マニピュレータ１は、障害物９と可動部材２，４，６との衝突を衝突監視部２３で監視する（ステップＳ１０２）。

次に、マニピュレータ１は、可動部材２，４，６が障害物９に衝突するか否かを衝突監視部２３から出力される監視情報に基づいて判断する（ステップＳ１０４）。すなわち、衝突監視部２３は、可動部材２，４，６と障害物９とが衝突する可能性がある場合に作動するので、衝突監視部２３は、衝突の可能性のある場合に制御装置２２に監視情報を出力する。一方、衝突監視部２３は、障害物９と可動部材２、４、６とが衝突する可能性がない場合には作動せず、制御装置２２に監視情報を出力しない。したがって、マニピュレータ１は、監視情報に基づいて障害物９と可動部材２、４、６との衝突の有無を判断することができる。すなわち、ステップＳ１０２及びステップＳ１０４は、各可動部材２、４、６において、衝突可能性の有無を判断する衝突監視ステップとなっている。

マニピュレータ１は、衝突監視部２３が作動したときには、監視情報に基づいて可動部材２，４，６が障害物９から回避するように、アクチュエータ３１、５１、７１または移動駆動部１４を制御する（ステップＳ１０６）。

このとき、マニピュレータ１の加速判断部２５は、衝突監視部２３によって特定された可動部材（例えばアーム４）の重心位置（例えば、図３（ａ）の姿勢において、アーム４の重心位置４ａ）が、関節部５に設定された相対座標系の４つの内のどの領域に存在するかを、アーム４とアーム６とがなす角度θにより特定する。そして、重心位置の存在する領域と、他方の可動部材（例えばアーム６）の回動方向情報と、可動部材（例えばアーム４）と障害物９との位置関係情報とから、他方の可動部材（例えばアーム６）を加速回動させた場合に可動部材（アーム４）の重心位置に作用する反力が衝突回避動作に寄与するか否かを判断する（ステップＳ１０８）。そして、加速判断部２５は、他方の可動部材（例えばアーム６）の加速に伴って生ずる反力が衝突回避動作に寄与する場合には「加速回動させる」と判断し、反力が衝突回避動作に寄与しない場合には「加速回動させない」と判断する。ここで、可動部材（例えばアーム４）の重心位置に作用する反力が衝突回避動作に寄与する場合とは、反力の作用方向が障害物９に対して衝突回避方向に作用する場合である。また、可動部材（例えばアーム４）の重心位置に作用する反力が衝突回避動作に寄与しない場合とは、反力の作用方向が障害物９に対して衝突方向に作用する場合である。すなわち、ステップＳ１０８は、加速判断ステップとなっている。

マニピュレータ１は、加速判断部２５で「加速回動させる」と判断された場合には、例えば、図３（ｂ）の姿勢において、アーム４を逆方向（Ｂ方向）に反転させて回動させ、これと共にアクチュエータ７１の回転トルクを上げて、アーム６の回動を加速させる（ステップＳ１１０）。これにより、アーム４は、その基端部において、アクチュエータ５１による回転力に加えて、アーム６の加速動作に伴って生ずる重心位置４ａ回りの回転モーメントを受ける。これにより、アーム４は、障害物９から離れる方向に迅速に回動する。すなわち、ステップＳ１１０は、制御ステップとなっている。そして、マニピュレータ１は、障害物９を回避したか否かを判断する（ステップＳ１１２）。マニピュレータ１は、障害物を回避していないと判断した場合は、ステップＳ１１０に戻り、引き続き加速回動させる。一方、回避したと判断した場合は、ステップＳ１１４に進む。

マニピュレータ１は、衝突回避動作をした後、例えば、アクチュエータ３１、５１、７１または移動駆動部１４を停止させ、可動部９１の動作を停止させる（Ｓ１１４）。なお、目標位置が設定されている場合は、再度、可動部９１の動作を開始して移動するようにしてもよい。

以上述べたように、本実施の形態１におけるマニピュレータ１によれば、衝突監視部２３によって特定された可動部材が障害物を回避する動作を行うに際し、アクチュエータ３１、５１、７１の駆動力に加え、当該可動部材の重心に作用する慣性による反力も利用することができる。したがって、ハンド２またはアーム４、６において、障害物９からの迅速な回避動作を実現することができる。

なお、本実施の形態１では、アーム４，６が２つの場合を例として説明したが、これに限定されない。３つ以上の可動部材を連結したマニピュレータとしても、同様の効果を得ることができる。

また、本実施の形態１では、移動部１３が、保持部本体８ａに対して相対的に移動可能な構成としたが、これに代え、保持部８自体が車輪等の駆動部及びその駆動部を有する自走式であるとし、衝突回避動作時に保持部８自体を加速移動させるようにしてもよい。この場合には、保持部８が可動部材として機能することになる。また、この場合には、保持部８が可動部材として機能するため、アームが１つのみ設けられる構成としてもよい。

また、各関節部３，５，７が後述する実施の形態２で示すようなクラッチ部及び切替部を有し、例えばアーム４の衝突回避動作時に関節部５のクラッチ部をフリー状態にする制御を行うようにしてもよい。

（実施の形態２）
次に、図８から図１０を用いて、本発明の実施の形態２におけるマニピュレータ１０について説明する。図８は本発明の実施の形態２におけるマニピュレータ１０の概略構成図であり、図９は同マニピュレータ１０の構成を示すブロック図であり、図１０は同マニピュレータ１０の退避動作を説明する概略正面図である。以下、実施の形態１と同様の構成要素については、同一符号を付して説明する。

実施の形態１におけるマニピュレータ１では、衝突監視部２３が設けられており、この衝突監視部２３の監視情報に基づいて衝突する可能性のある可動部材（例えばアーム４）を特定した。このとき特定された可動部材（例えばアーム４）と障害物９との衝突を、可動部材に加える障害物回避方向の回転トルクと、他方の可動部材（例えばアーム６）の加速動作に伴って可動部材に作用する慣性による反力とを併用して回避するようにした。

これに対し、本実施の形態２におけるマニピュレータ１０は、後述する衝突検知部２４を備え、この衝突検知部２４の検知情報に基づいて障害物９と衝突した可動部材を特定し、衝突検知部２４によって特定された可動部材において、慣性による反力を利用して衝撃力を緩和させるものである。これにより、マニピュレータ１０が障害物９に衝突した場合であっても、その衝突による衝撃力を緩和することができる。

まず、図８を参照しながら、本実施の形態２におけるマニピュレータ１０の構成について説明する。

マニピュレータ１０は、ハンド２とアーム４とアーム６とを有する可動部と、関節部３０、５０、７０と、可動部を保持する保持部８と、を備えている。

ハンド２は、関節部３０を介してアーム４に連結されている。アーム４は、関節部５０を介してアーム６に連結されている。アーム６は、関節部７０を介してアーム６を保持する保持部８に連結されている。ハンド２、アーム４、６および保持部８はそれぞれが互いに回動可能に連結されている。

アーム４、６の表面には、障害物９との接触を検出する接触センサ４１、６１がそれぞれ設けられている。

接触センサ４１、６１には、例えば、導電性感圧インクが縦横パターンに印刷された感圧シートなどを使用することができる。これにより、アーム４に障害物９が接触して、感圧シートに圧力が加わった際の感圧シートの抵抗の変化に基づいて、接触センサ４１で接触を感知することができる。接触センサ６１は、同様に、アーム６に障害物９が接触して、感圧シートに圧力が加わると、接触を感知することができる。

保持部８は、機構部８１と駆動部８２とを備える。駆動部８２は、操作指令を入力するための入力部２１と、障害物９との衝突を検知するための衝突検知部２４と、制御装置２２と、を備えている。制御装置２２は、入力部２１から得た操作指令情報と衝突検知部２４から得た検知情報とに基づいてハンド２およびアーム４、６の動きを制御する。衝突検知部２４には、接触センサ４１、６１が接続されている。

次に、図９を参照しながら、マニピュレータ１０の構成および動作について説明する。図９は、本発明の実施の形態２におけるマニピュレータ１０の構成を示すブロック図である。

制御装置２２は、入力部２１からの操作指令に応じてハンド２およびアーム４、６の動作を制御する。また、制御装置２２は、衝突検知部２４の検知情報に基づいて障害物９と衝突した可動部材を、後述するアクチュエータ３１、５１、７１を駆動させて、退避させる。さらに、制御装置２２は加速判断部２５を備え、この加速判断部２５による判断に従って障害物９から可動部材を退避させるときに、可動部材の重心に作用する慣性による反力を利用するか否かを判断する。

衝突検知部２４は、接触センサ４１、６１から取得した接触感知情報によって、障害物９と衝突した可動部材を特定し、その特定した可動部材の検知情報を制御装置２２に出力する。例えば、衝突検知部２４は、接触センサ４１が接触を感知したときには、アーム４と障害物９とが衝突したことを検知する。同様に、接触センサ６１が接触を感知したときには、アーム６と障害物９とが衝突したことを検知する。

関節部３０、５０、７０は、関節駆動部としてのアクチュエータ３１、５１、７１と、関節駆動部の動力を伝える動力伝達部としてのクラッチ部３２、５２、７２と、動力伝達切替部としての切替部３３、５３、７３と、角度センサ３７、５７、７７と、を備えている。

クラッチ部３２は、切替部３３の作動または非作動により、アクチュエータ３１の動力を、減速機３８を介してハンド２に連結させる連結状態、またはアクチュエータ３１の動力をハンド２から遮断する非連結状態にする。クラッチ部３２としては、例えば一対のクラッチ板を用い、切替部３３としては、例えば、電流のＯＮ／ＯＦＦにより前記一対のクラッチ板を接触／非接触状態にする電磁石を用いる。なお、切替部３３の「作動」状態とは、電流ＯＦＦにより前記一対のクラッチ板が接触状態になっている状態であり、「非作動」状態とは、電流ＯＮにより前記一対のクラッチ板が非接触状態になっている状態である。クラッチ部３２は、一対のクラッチ板を接触状態にすることで、アクチュエータ３１の動力を、減速機３８を介してハンド２に連結させる。一方、クラッチ部３２は、一対のクラッチ板を非接触状態にすることでアクチュエータ３１の動力をハンド２に非連結にする。

クラッチ部５２は、同様に、切替部５３の作動または非作動により、アクチュエータ５１の動力を、減速機３８を介してアーム４に連結状態または非連結状態にする。また、クラッチ部７２は、切替部７３の作動または非作動により、アクチュエータ７１の動力を、減速機３８を介してアーム６に連結状態または非連結状態にする。

したがって、クラッチ部３２が非連結状態のときには、関節部３０が外力に対して回動自在な（フリー）状態となる。同様に、クラッチ部５２が非連結状態のときには、関節部５０が外力に対して回動自在な（フリー）状態となり、クラッチ部７２が非連結状態のときには、関節部７０が外力に対して回動自在な（フリー）状態となる。

エンコーダ３６は、モータ３４の回転情報を検出する。制御装置２２は、この回転情報と減速機３８の減速比とに基づいて、ハンド２、アーム４およびアーム６の位置および姿勢を検出するとともに、ハンド２、アーム４，６の動作を制御する。

角度センサ３７は、絶対関節角度情報に基づいてハンド２の姿勢及び動きを検出する。クラッチ部３２を非連結状態にしたときにはアクチュエータ３１の動力がハンド２に伝達されなくなるため、エンコーダ３６によってハンド２の動きを検出できなくなるが、この場合でも、角度センサ３７によってハンド２の動きを検出することができる。また、再びクラッチ部３２を連結状態にした場合には、角度センサ３７の絶対関節角度情報を用いて、エンコーダ３６の回転情報を補正することができる。角度センサ５７は、同様に、絶対関節角度情報に基づいてアーム４の姿勢及び動きを検出する。角度センサ７７は、絶対関節角度情報に基づいてアーム６の姿勢及び動きを検出する。

角度センサ３７、５７、７７としては、例えば、ホール素子や磁気抵抗素子とＮＳ２極着磁の回転磁石とを用いた磁気式センサや、可変抵抗器の原理を用いたポテンショメータなどを使用することができる。

次に、図９および図１０（ａ）（ｂ）を参照しながら、マニピュレータ１０の退避動作について説明する。

制御装置２２は、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１を駆動させると共に、障害物９と接触した可動部材（ハンド２またはアーム４、６）の検知情報を衝突検知部２４から取得する。制御装置２２は、この検知情報から障害物９と可動部材が衝突したことを検知した場合には、衝突した可動部材の検知情報、関節部５０、７０の駆動情報などに基づき、障害物９から可動部材を退避させる動作を開始する。すなわち、障害物９から可動部材を退避させ、衝突による衝撃力を緩和するための制御情報を生成し、この制御情報によりアクチュエータ３１、５１、７１を駆動させ、関節部３０、５０、７０を回動制御させる。

図１０（ａ）に示すように、マニピュレータ１０は、入力部２１からの操作指令に応じて、アクチュエータ３１、５１、７１を駆動させる。例えば、マニピュレータ１０は、アクチュエータ３１を停止させ、切替部３３、５３、７３を作動させると共に、アクチュエータ５１、７１を駆動させる。これにより、例えば、アーム４は関節部５０を回転中心としてＡ方向に回動し、アーム６は関節部７０を回転中心としてＡ方向に回動する。

そして、障害物９とアーム４とが接触した場合には、接触センサ４１が作動し、衝突検知部２４は、アーム４と障害物９との衝突を検知して制御装置２２に検知情報を出力する。制御装置２２は、この検知情報に基づいて、アーム４が障害物９から退避するように、切替部５３、７３を設定すると共に、アクチュエータ５１、７１を制御する。

具体的には、制御装置２２は、アーム４と障害物９との衝突を検知した後、切替部５３を「作動」から「非作動」に切り替えると共に、アクチュエータ７１の駆動力を上げ、アーム６をＡ方向に加速回動させる。すなわち、マニピュレータ１０は、関節部５０を外力に対して回動自在（フリー）にした状態で、アーム６を障害物９に対して衝突方向（Ａ方向）に加速回動させる。

図１０（ｂ）に示すように、関節部７０を回転中心としてアーム６をＡ方向に加速回動させると、関節部５０が外力に対して回動自在（フリー）な状態になっているため、アーム６の加速に伴ってアーム４に作用する慣性力が反力となって作用する。これにより、アーム４は、関節部５０を回転中心として障害物９から離れる方向（Ｂ方向）に回動する。したがって、アーム６を障害物９に対して衝突方向に加速回動させたにもかかわらず、アーム４の重心には慣性による障害物退避方向への反力が作用する為、衝突したときのアーム４の姿勢がＣ方向に変化する。

この結果、マニピュレータ１０は、障害物９からアーム４を退避させることができる。同様に、制御装置２２は、接触センサ６１が作動し、衝突検知部２４がアーム６と障害物９との衝突を検知した場合には、障害物９からアーム６が退避するようにアクチュエータ５１、７１を駆動させる。

以上述べたように、本実施形態２におけるマニピュレータ１０によれば、実施形態１同様に、慣性による反力を利用してアーム４を障害物９から退避させることができるので、アームが障害物に衝突した場合であっても、衝突による衝撃力を緩和することができる。

なお、接触センサ４１、６１が接触を感知していない状態（すなわち、アーム４およびアーム６がいずれも障害物９と衝突していない状態）は、衝突検知部２４は、制御装置２２に対して検知情報を出力しない。このときには、制御装置２２は、切替部５３、７３を作動させ、クラッチ部５２、７２を連結させる。これにより、マニピュレータ１０は、入力部２１からの操作指令に応じて、アーム４およびアーム６を所定の位置に移動することができる。

また、衝突検知部２４から検知情報が出力されると、ＣＰＵは、ハードウェア割り込みにより検知情報を受け付け、回避動作を行うためのプログラムを実行する。これにより、制御装置２２は、衝突があった場合の応答速度を高めることができる。

また、マニピュレータ１０は、衝突したときにモータ３４の回動方向を逆方向に変更させる。したがって、例えば、衝突した障害物９からアーム４を回避させる場合でも、慣性による反力を併用利用してアーム４を障害物９から回避させることができるため、より迅速に障害物から回避させることができる。

なお、マニピュレータ１０は、衝突監視部２３と衝突検知部２４とを併用し、衝突監視部２３から得られる監視情報と衝突検知部２４から得られる検知情報とを利用するようにしてもよい。これにより、衝突した場合であっても、衝突してからの衝撃力を緩和する動作に加え、衝突前に、監視情報を使って衝突回避方向への動作を行うことができる。

また、図１１（ａ）に示すように、可動部材と障害物９との衝突が検知されたときに、移動部１３を加速して移動させるようにしてもよい。すなわち、アーム４の重心位置４ａが、関節部５０に対して根元側（Ｘ−Ｘ軸より機構部８１の側）にある状況において、衝突検知部２４がアーム４と障害物９との衝突を検知した場合、マニピュレータ１０は、切替部５３、７３を非作動にし、２つの関節部５０、７０を外力に対して回動自在（フリー）な状態にすると共に、障害物９に対して移動部１３を衝突方向に加速させる。これにより、図１１（ｂ）に示すように、アーム４を障害物９から退避させることができるので、衝突による衝撃力を緩和することができる。

また、制御装置２２が、複数の関節部３０、５０、７０の動作を協調させる協調プログラムを備え、このプログラムを実行することにより、関節部３０、５０、７０を協調して動作させるようにしてもよい。これにより、衝突回避動作、衝撃力緩和動作の効果を高めることができる。

なお、本実施の形態２においては、制御装置２２は、エンコーダ３６の回転情報と減速機３８の減速比とに基づいて、ハンド２、アーム４およびアーム６の位置および姿勢を検出し、これらの動作を制御するようにしたが、エンコーダ３６を設けずに、角度センサ３７、５７、７７の絶対関節角度情報のみを用いて、ハンド２、アーム４及およびアーム６の位置及び姿勢を検出して、ハンド２等の動作を制御するようにしてもよい。この構成によれば、クラッチ部３２、５２、７２を非連結にした後、再度クラッチ部３２、５２、７２を連結した場合も、角度情報を補正せずに制御を行うことができる。

本発明によれば、マニピュレータを制御することができ、特に、障害物との衝突の恐れのある状況で使用されるマニピュレータおよび制御方法などに有用である。

Claims (12)

1. 第１可動部材と、
   第２可動部材と、
   保持部と、
   前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、
   前記第２可動部材と前記保持部とを回動可能に連結する第２関節部と、
   前記第１関節部を駆動可能な第１関節駆動部と、
   前記第２関節部を駆動可能な第２関節駆動部と、
   前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定部と、
   前記部材特定部によって特定された前記一方の可動部材を前記障害物から離れる方向に回動させると共に、他方の可動部材を前記障害物に近づく方向に回動させるように、前記第１関節駆動部及び前記第２関節駆動部を制御する制御装置と、を備えているマニピュレータ。
2. 前記制御装置は、前記部材特定部によって特定された前記一方の可動部材と前記障害物との位置関係を表す位置関係情報と、前記第１可動部材と前記第２可動部材とがなす角度を表す角度情報と、前記他方の可動部材の回動方向を表す回動方向情報とに基づいて、前記他方の可動部材の回動を加速させる否かの判断をする加速判断部を備える請求項１に記載のマニピュレータ。
3. 前記加速判断部は、前記位置関係情報、前記角度情報及び前記回動方向情報に基づき、前記他方の可動部材の回動の加速によって前記一方の可動部材に生ずる反力の方向とは逆の方向に前記障害物がある場合には前記他方の可動部材の回動を加速させると判断し、前記反力の方向に前記障害物がある場合には前記他方の可動部材の回動を加速させないと判断する請求項２に記載のマニピュレータ。
4. 前記第１関節部の回動中心回りに前記他方の可動部材の回動する方向を正として、前記第２可動部材に対して前記第１可動部材がなす角度をθとし、
   前記一方の可動部材が前記第１可動部材であり、前記他方の可動部材が前記回動方向に回動する場合において、
   前記加速判断部は、
   ０°＜θ＜９０°又は２７０°＜θ＜３６０°の条件が成立し、前記角度θが小さくなる方向に前記障害物がある場合には、前記他方の可動部材の回動を加速すると判断し、
   ０°＜θ＜９０°又は２７０°＜θ＜３６０°の条件が成立し、前記角度θが大きくなる方向に前記障害物がある場合には、前記他方の可動部材の回動を加速しないと判断し、
   ９０°＜θ＜１８０°又は１８０°＜θ＜２７０°の条件が成立し、前記角度θが大きくなる方向に前記障害物がある場合には、前記他方の可動部材の回動を加速すると判断し、
   ９０°＜θ＜１８０°又は１８０°＜θ＜２７０°の条件が成立し、前記角度θが小さくなる方向に前記障害物がある場合には、前記他方の可動部材の回動を加速しないと判断する請求項３に記載のマニピュレータ。
5. 前記保持部に備えられた移動部は、前記第２可動部材を保持すると共に移動駆動部によって移動可能に構成されており、
   前記制御装置は、
   前記一方の駆動部材が前記第１可動部材の場合には、前記移動部を前記障害物に近づく方向に移動させるように、前記移動駆動部を制御する請求項１から請求項４のいずれか１項に記載のマニピュレータ。
6. 前記部材特定部は、少なくとも２つの撮影部によって撮影して得た撮影画像から前記障害物の位置を導出し、前記第１可動部材および前記第２可動部材のうち、前記障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する請求項１から請求項５のいずれか１項に記載のマニピュレータ。
7. 第１可動部材と、
   第２可動部材と、
   前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、
   移動駆動部を有する移動部と、
   前記第２可動部材と前記移動部とを回動可能に連結する第３関節部と、
   前記第１関節部を駆動可能な第１関節駆動部と、
   前記第３関節部を駆動可能な第３関節駆動部と、
   前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定部と、
   前記部材特定部によって特定された一方の可動部材を前記障害物から離れる方向に回動させると共に、前記一方の可動部材が前記第１可動部材の場合には、前記移動部を前記障害物に近づく方向に移動させるように、前記第１関節駆動部、前記第３関節駆動部及び前記移動駆動部を制御する制御装置と、を備えているマニピュレータ。
8. 前記制御装置は、前記部材特定部によって特定された前記一方の可動部材と前記障害物との位置関係を表す位置関係情報と、前記第１可動部材と前記第２可動部材とがなす角度を表す角度情報とに基づいて、前記移動部を加速させるか否かの判断をする加速判断部を備える請求項７に記載のマニピュレータ。
9. 前記加速判断部は、前記位置関係情報及び前記角度情報に基づき、前記移動部の加速によって前記一方の可動部材の重心に生ずる反力の方向とは逆の方向に前記障害物がある場合には前記移動部を加速させると判断し、前記反力の方向に前記障害物がある場合には前記移動部を加速させないと判断する請求項８に記載のマニピュレータ。
10. 第１可動部材と、第２可動部材と、保持部と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、前記第２可動部材と前記保持部とを回動可能に連結する第２関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第２関節部を駆動可能な第２関節駆動部と、を備えたマニピュレータの制御方法であって、
    前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定ステップと、
    前記部材特定ステップにおいて特定された一方の可動部材を前記障害物から離れる方向に回動させると共に、他方の可動部材を前記障害物に近づく方向に回動させるように、前記第１関節駆動部及び前記第２関節駆動部を制御する制御ステップと、が含まれているマニピュレータの制御方法。
11. 前記制御ステップでは、前記他方の可動部材の回動の加速によって前記一方の可動部材に生ずる反力の方向とは逆の方向に前記障害物がある場合には、前記他方の可動部材を加速すると判断し、前記反力の方向に前記障害物がある場合には、前記他方の可動部材の回動を加速しないと判断する請求項１０に記載のマニピュレータの制御方法。
12. 第１可動部材と、第２可動部材と、移動駆動部を有する移動部と、前記第１可動部材と前記第２可動部材とを回動可能に連結する第１関節部と、前記第２可動部材と前記移動部とを回動可能に連結する第３関節部と、前記第１関節部を駆動可能な第１関節駆動部と、前記第３関節部を駆動可能な第３関節駆動部と、を備えたマニピュレータの制御方法であって、
    前記第１可動部材および前記第２可動部材のうち、障害物との衝突可能性又は衝突が検知された一方の可動部材を特定する部材特定ステップと、
    前記部材特定ステップにおいて特定された一方の可動部材を障害物から離れる方向に回動させると共に、前記一方の可動部材が前記第１可動部材の場合には、前記移動部を前記障害物に近づく方向に移動させるように前記第１関節駆動部、前記第３関節駆動部及び前記移動駆動部を制御する制御ステップと、が含まれているマニピュレータの制御方法。

Images