JPWO2008004487A1 - ロボットアームの制御装置 - Google Patents

Abstract

ロボットアーム（８）の制御装置（１）であって、人接近検知手段（３）が人の接近を検知した時に、人運動検出手段（２）の検出した人の運動に基づいてロボットアーム（８）の各関節部ごとに個別に機械インピーダンスを設定することで人とロボットアーム（８）の衝突に対応する衝突対応動作制御手段（４）で制御する。

Description

本発明は、家庭用ロボット等、人との物理的な接触の可能性のあるロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムに関する。

近年、ペットロボットなどの家庭用ロボットの開発が盛んに行われており、将来は家事支援ロボット等、より実用的な家庭用ロボットが実用化されるものと期待されている。家庭用ロボットは、家庭内に入り人と共生する必要があるため、人との物理的な接触が不可欠であり、安全性の面から、柔軟であることが必要とされる。

こうした課題に対し、従来技術としては、特許文献１（特開平１０−３２９０７１号公報）において、ロボットアームに加わった人との接触力を検知し、アームに大きな力が加わった時には復元力を小さくし安全性を高め、アームに微少な力が加わっている時には復元力を大きくし動作精度を確保する制御装置を開示している。

また、特許文献２には、図１１Ａに示すように、障害物が存在する環境とロボット５１３との接触部およびその接触力をそれぞれ検知するセンサ機構５１５と、移動機構５１１およびマニピュレータ５１２の位置姿勢をそれぞれ計測する位置姿勢計測装置５１４と、前記接触力と位置姿勢とからロボット５１３の接触力回避動作を計算する計算機５１７と、この計算機５１７の計算結果によって移動機構５１１とマニピュレータ５１２とを駆動させるアクチュエータ５１６とを有する制御装置が開示されており、障害物が存在する環境とロボット５１３との接触時には、移動機構５１１とマニピュレータ５１２との両者の連携動作により回避動作を行うようにしている。

また、特許文献３には、図１２に示すように、エンドエフェクタ４１１を有するロボット４０１に作用したことを力覚センサ４１２で検出した外部環境からの力に対して、リアルタイムに外部環境の剛性係数を推定して、モータ４１３ａの制御によりロボット４０１のアーム４１０を駆動制御するインピーダンス制御装置が開示されている。

特開平１０−３２９０７１号公報 特開２００５−５９１６１号公報 特開２００４−２２３６６３号公報

しかしながら、上記従来制御装置では、多関節型のロボットアームへの適用を考慮しておらず、多関節型のロボットアームの場合において安全性が保証された制御装置となっていない。

また、上記従来制御装置では、接触する人の動きを考慮しておらず、人の動きに合わせた最適な制御になっていない。

また、特許文献２では、図１１Ｂ及び図１１Ｃに示すように、ロボット５１３における、障害物が存在する環境との接触部５３２を、接触力５３１の方向に接触回避経路５２６沿いに移動させることで、回避動作５２７を行うことで、環境からの接触力５３１を小さくし、最終的に接触がなくなるようにしている。また、図１１Ｄ及び図１１Ｅに示すように、ロボット５１３における、障害物が存在する環境との接触部５３２の接触面を、接触力５３１により発生するモーメント５３３の方向に傾けるとともに、前記接触部を、任意の大きさの円５２６を描くように移動させることで、回避動作５２７を行うことで、環境からの接触力５３１を小さくし、最終的に接触がなくなるようにしている。しかしながら、それぞれの回避動作５２７の具体的な動作制御については、何ら開示はされていない。

また、特許文献３については、図１２に示すように、算出された外部環境の剛性係数と相関関係をもってロボット側のインピーダンス特性を変化させ、これらの値を用いてエンドエフェクタ４１１の位置を算出するものであって、エンドエフェクタ４１１の位置にしか着目しておらず、人へ接触のダメージを与えないようにするためのアーム４１０の具体的な動作制御については全く開示されていない。

本発明の目的は、上記従来の制御装置の課題を解決し、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できるロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムを提供することにある。

上記目的を達成するために、本発明は以下のように構成する。

本発明の第１態様によれば、多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置を提供する。

本発明の第９態様によれば、多関節型ロボットアームの制御方法であって、
上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを接近検知手段で検知し、
上記接近検知手段により上記人または移動体の接近を検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を行なうように制御するロボットアームの制御方法を提供する。

本発明の第１０態様によれば、上記多関節型ロボットアームと、
上記ロボットアームを制御する第１態様〜第８態様のいずれか１つに記載のロボットアームの制御装置とを有して、
上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。

本発明の第１１態様によれば、コンピューターを、
人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラムを提供する。

本発明のロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムによれば、衝突位置取得手段と衝突対応動作制御手段とを備えるか、又は、それらの機能を備えるようにしている。このため、ロボットアームに人が接近したときに、人の運動に応じた適切な姿勢で動作したり又は適切な関節部の機械インピーダンスで動作することが可能となる。言い換えれば、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて、上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることにより、根元部に近い側の関節部を動きやすくして、当該関節部で衝突時にかかる力を逃がすことができ、人に与える衝撃力が小さくなり、安全性を高くすることができる。よって、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できる。

本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、図１は、本発明の第１実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す説明図であり、 図２は、本発明の上記第１実施形態における上記ロボットアームの制御装置でのロボットアームの衝突警戒領域を説明する図であり、 図３Ａは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、 図３Ｂは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、 図３Ｃは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、 図３Ｄは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、 図４は、上記第１実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図であり、 図５は、上記第１実施形態における上記制御装置の動作制御手段での制御プログラムの動作ステップを表すフローチャートであり、 図６は、上記第１実施形態における上記制御装置の全体的な動作ステップを表すフローチャートであり、 図７は、本発明の上記第１実施形態における上記ロボットアームの制御装置の目標軌道を説明する図であり、 図８Ａは、本発明の第２実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す図であり、 図８Ｂは、上記第２実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図であり、 図９Ａは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図であり、 図９Ｂは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図であり、 図９Ｃは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図であり、 図９Ｄは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図であり、 図１０は、本発明の第２実施形態におけるロボットアームの制御装置の制御対象であるロボットアームの他の構成を示す図であり、 図１１Ａは、特許文献２にかかる従来技術のロボット制御装置の概略構成図であり、 図１１Ｂは、図１１Ａのロボット制御装置における接触回避動作の具体的な例を示す図であり、 図１１Ｃは、図１１Ａのロボット制御装置における接触回避動作の上記具体的な例を示す図であり、 図１１Ｄは、図１１Ａのロボット制御装置における接触回避動作の別の具体的な例を示す図であり、 図１１Ｅは、図１１Ａのロボット制御装置における接触回避動作の上記別の具体的な例を示す図であり、 図１２は、特許文献３にかかる従来技術のロボット制御装置のブロック線図である。

本発明の記述を続ける前に、添付図面において同じ部品については同じ参照符号を付している。

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

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置を提供する。

本発明の第２態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクの上記根元部に最も近い側の関節部の剛性を他の関節部の剛性よりも低下させる上記衝突対応動作を行なうように制御する第１態様に記載のロボットアームの制御装置を提供する。

本発明の第３態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第４態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームの上記リンクより手首部に近い側の各関節部ごとに個別に関節角度を制御することで、上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第５態様によれば、上記人または移動体の運動を検出する運動検出手段をさらに備え、
上記衝突位置取得手段は、上記運動検出手段の検出結果に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を取得し、
上記衝突対応動作制御手段は、上記衝突位置取得手段で推定した上記衝突位置に基づいて上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第６態様によれば、上記運動検出手段の検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを検知する接近検知手段をさらに備え、
上記衝突対応動作制御手段は、上記接近検知手段が上記人または移動体の接近を検知したときに、上記衝突対応動作を行なうように制御する第５態様に記載のロボットアームの制御装置を提供する。

本発明の第７態様によれば、上記衝突対応動作制御手段は、上記運動検出手段の検出した上記人または移動体の運動の速度成分に基づいて、上記衝突対応動作を行なうように制御する第５態様又は第６態様に記載のロボットアームの制御装置を提供する。

本発明の第８態様によれば、上記運動検出手段は、上記人または移動体の位置と移動速度を検出して上記人の運動を検出する第５〜７態様のいずれか１つに記載のロボットアームの制御装置を提供する。

本発明の第９態様によれば、多関節型ロボットアームの制御方法であって、
上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを接近検知手段で検知し、
上記接近検知手段により上記人または移動体の接近を検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を行なうように制御するロボットアームの制御方法を提供する。

本発明の第１０態様によれば、上記多関節型ロボットアームと、
上記ロボットアームを制御する第１態様〜第８態様のいずれか１つに記載のロボットアームの制御装置とを有して、
上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。
本発明の第１１態様によれば、コンピューターを、
人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラムを提供する。

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

（第１実施形態）
図１は本発明の第１実施形態における多関節型のロボットアームの制御装置１の構成及びその制御対象である多関節型のロボットアーム８の構成を示す図である。このロボットアームの制御装置１は、上記第１実施形態のロボットアームの制御方法を実施するものであって、多関節型のロボットアーム８を備えたロボットに使用可能であり、後述するように、多関節型のロボットアーム８の制御プログラムとしても使用可能なものである。

図１において、２は運動検出手段の一例としての人運動検出手段であり、カメラなどの画像撮像装置３６で撮像された画像データにより画像認識処理を人運動検出手段２で行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を人運動検出手段２で検出する。

３は接近検知手段の一例としての人接近検知手段であり、人運動検出手段２による検出結果である人位置の情報と、後述する動作制御手段７から得られるロボットアーム８の関節角度情報より求められるロボットアーム８の姿勢の情報との関係より、ロボットアーム８に人３８が接近していることを検知する。

５は衝突位置取得手段の一例としての衝突位置推定手段であり、人運動検出手段２で検出した、接近する人３８の位置の情報、並びに、人３８の移動速度及び移動方向すなわち移動速度ベクトル情報より、ロボットアーム８と人３８との衝突予定位置（衝突位置として取り扱い可能な位置）を推定し、推定結果を、後述する衝突対応動作生成手段６に出力する。

４は衝突対応動作制御手段であり、ロボットアーム８で作業を遂行する際のロボットアーム８の動作の制御（具体的には、上記ロボットアーム８の機械インピーダンスの値の制御）を行い、人接近検知手段３がロボットアーム８への人３８の接近を検知した際には、人運動検出手段２の検出結果に基づき、人３８とロボットアーム８の衝突時の衝突対応動作を制御する。

衝突対応動作制御手段４は、衝突対応動作生成手段６、及び動作制御手段７とより構成される。

衝突対応動作生成手段６は、衝突位置推定手段５が推定した衝突予定位置において、ロボットアーム８と人３８が接触するときのロボットアーム８の関節目標軌道及び関節目標の機械インピーダンス値を計算して、計算結果を動作制御手段７に出力する。

動作制御手段７は、ロボットアーム８の手先の位置姿勢を制御し、目標とする作業をロボットアーム８の手先で実現すると共に、人３８が接近した場合には、衝突対応動作生成手段６が生成した関節目標軌道に追従するように制御を切り換えると同時に、衝突対応動作生成手段６が生成した関節目標の機械インピーダンス値になるようにロボットアーム８の各関節部の機械インピーダンスの値を制御するインピーダンス制御を行う。ここで、ロボットアーム８の各関節部は、後述するように、Ｄ／Ａボード２１及びＡ／Ｄボード２２に接続されたモータドライバ１９を介して、各関節部の駆動装置の一例としてのモータ３４がインピーダンス制御の下に駆動されてそれぞれ屈曲動作が行われるようになっている。

制御装置１は、ハードウェア的には一般的なパーソナルコンピュータにより構成されており、人運動検出手段２、人接近検知手段３、衝突位置推定手段５、衝突対応動作生成手段６、及び、動作制御手段７のうちの入出力ＩＦ（入出力インターフェース）２０を除く部分は、パーソナルコンピュータで実行可能な制御プログラム１８としてソフトウェア的に実現される。

入出力ＩＦ２０は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２１、Ａ／Ｄボード２２、カウンタボード２３により構成される。

ロボットアーム８の動作を制御するための制御プログラム１８が実行されることにより制御装置１が機能しており、ロボットアーム８の各関節部のモータ３４に連結されたエンコーダ３５より出力される関節角度情報がカウンタボード２３を通じて制御装置１に取り込まれ、制御装置１によって各関節部の回転動作での制御指令値が算出される。算出された各制御指令値は、Ｄ／Ａボード２１を通じてモータドライバ１９に与えられ、モータドライバ１９から送られた各制御指令値に従って、ロボットアーム８の各関節部のモータ３４が駆動される。

ロボットアーム８は、４自由度の多リンクマニピュレータであり、ハンド９と、ハンド９が取付けられる手首部１０を先端に有する第２リンク１１と、第２リンク１１が回転可能に連結される第１リンク１２と、第１リンク１２が回転可能に連結支持される台部１３とを有している。手首部１０は、第３関節部１６及び第４関節部１７の２つの回転軸（上下方向沿いの回転軸と横方向沿いの回転軸）を有しており、第２リンク１１に対するハンド９の相対的な姿勢（向き）を変化させることができる。第２リンク１１の他端は第１リンク１２の先端に対して第２関節部１５の上下方向沿いの回転軸周りに回転可能とし、第１リンク１２の他端は台部１３に対して第１関節部１４の上下方向沿いの回転軸周りに回転可能として、合計４個の軸周りに回転可能として上記４自由度の多リンクマニピュレータを構成している。

各軸の回転部分を構成する各関節部には、各関節部で連結される２つの部材のうちの一方の部材に備えられかつ後述するモータドライバ１９により駆動制御される回転駆動装置の一例としてのモータ３４（実際には、ロボットアーム８の各関節部の内部に配設されている）と、モータ３４の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ３５（実際には、ロボットアーム８の各関節部の内部に配設されている）とを備えて、モータ３４の回転軸が、各関節部で連結される２つの部材のうちの他方の部材に連結されて上記回転軸を正逆回転させることにより他方の部材を一方の部材に対して各軸周りに回転可能とするように構成している。

３２は、台部１３の下側固定部に対して相対的な位置関係が固定された絶対座標系であり、その原点をＯ_０とする。３３は、ハンド９に対して相対的な位置関係が固定された手先座標系である。絶対座標系３２から見た手先座標系３３の原点位置Ｏｅ（ｘ、ｙ）をロボットアーム８の手先位置、絶対座標系３２から見た手先座標系３３の姿勢（φ、θ）をロボットアーム８の手先姿勢とし、手先位置・姿勢ベクトルｒをｒ＝［ｘ、ｙ、φ、θ］^Ｔと定義する。ロボットアーム８の手先位置及び姿勢を制御する場合には、手先位置・姿勢ベクトルｒを目標手先位置・姿勢ベクトルｒ_ｗｄに追従させるように制御装置１で制御することになる。

次に、制御装置１を構成する各機能ブロックについて、詳細構成と動作について説明する。

人運動検出手段２には、画像撮像装置３６で撮影した画像データが入力され、人運動検出手段２での画像認識処理により、ロボットアーム８が配置された部屋の天井から床を見下ろしたときの人３８の２次元位置Ｘ_ｐ（ｘ_ｐ，ｙ_ｐ）及び人３８の移動速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）が検出され、２次元位置Ｘ_ｐ（ｘ_ｐ，ｙ_ｐ）及び移動速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の情報が人運動検出手段２から人接近検知手段３及び衝突位置推定手段５へ出力される。

人接近検知手段３は、図２に示す衝突警戒領域１００を定義し、人３８がこの領域１００に入ったかどうかで、人３８がロボットアーム８に接近したかどうかの検知判断を行なう。

衝突警戒領域１００の基端側の扇形領域１００Ａは、第１リンク１２の長さＬ_１のα_１倍、すなわちα_１Ｌ_１を半径とし、第１関節軸（第１関節部１４の回転軸）を要に、第１関節部１４の関節角ｑ_１の、例えば、第１リンク１２の中心軸を中心とした±β_１°の角度の領域とする。また、衝突警戒領域１００の先端側の扇形領域１００Ｂは、第２リンク１１の長さＬ_２とハンド９の長さＬ_Ｈのα_２倍、すなわちα_２（Ｌ_２＋Ｌ_Ｈ）を半径とし、第２関節軸（第２関節部１５の回転軸）を要に、第２関節部１５の関節角ｑ_２の、例えば、第２リンク１１の中心軸を中心とした±β_２°の角度の領域とする。このようにすれば、速度に比例して扇形領域の範囲を変化させても、ハンド９の先が、先端側の扇形領域１００Ｂの外に出るのを確実に防止できる。

衝突警戒領域１００を定義するα_１、α_２、β_１、β_２の値は、任意の値であるが、例えばロボットアーム８を動作させる手先速度Ｖ_ｒ＝｛（ｄｘ／ｄｔ）^２＋（ｄｙ／ｄｔ）^２｝^１／２を基にこれらの値を決めることができ、手先速度Ｖ_ｒに比例してα_１、α_２、β_１、β_２の値を大きくするように決めてやれば、ロボットアーム８の動作速度が大きい場合には衝突警戒領域１００の面積が大きくなり、安全性を向上させることができる。

この第１実施形態では、例えば、手先速度Ｖ_ｒの単位をｍ／ｓとするとともに、Ｖ_ＫはＶ_ｒの大きさによってＶ_ｒに比例する値又は定数となるものとして、α_１＝１．２Ｖ_Ｋ、α_２＝１．２Ｖ_Ｋ、β_１＝２４Ｖ_Ｋ、β_２＝２４Ｖ_Ｋとする。ただし、Ｖ_Ｋは、

の場合、Ｖ_Ｋ＝１．９Ｖ_ｒで定義される値（変数）であり、Ｖ_ｒ＜０．５［ｍ／ｓ］の場合、Ｖ_Ｋ＝０．９５と定数として定義される値（変数）である。

このようにすれば手先速度が０．５［ｍ／ｓ］以上の時には手先速度の大きさに比例して衝突警戒領域１００の大きさが決まり、また、０．５［ｍ／ｓ］未満の時にはα_１＝１．１４、α_２＝１．１４、β_１＝２２．８、β_２＝２２．８と固定値になり、手先速度が小さい場合でも、ある程度の大きさの衝突警戒領域１００が確保される。

人接近検知手段３は、人運動検出手段２の検出した人３８の位置の情報と、動作制御手段７から得られるロボットアーム８の関節角度情報より、人３８とロボットアーム８の相対関係を算出し、人３８が衝突警戒領域１００に入れば、衝突対応動作制御手段４に人侵入を通知する。

衝突位置推定手段５は、人運動検出手段２の検出した人３８の位置の情報及び移動速度ベクトルの情報から人３８との衝突予定位置

を推定して衝突対応動作生成手段６に出力する。

例えば、図３Ａに示すように、速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の延長線と垂直に交わり、ロボットアーム８の第１関節部１４の回転軸を通る直線Ａを考える。直線Ａと絶対座標系３２のｘ軸がなす角度をｑ_１ｉとし、速度ベク卜ルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の延長線と直線Ａとの交点を、衝突予定位置

とすると、角度ｑ_１ｉは以下の式で与えられ、

は以下の式（１）〜（３）で与えられる。

次に、図３Ａ〜図３Ｃを使い、衝突対応動作生成手段６の詳細について説明する。

衝突対応動作生成手段６は、図３Ａの通常作業時の状態（現在状態）から図３Ｂの衝突受け止め姿勢への関節目標軌道を生成する。

ロボットアーム８の第１リンク１２の中心軸の線が衝突予定位置

に重なるよう第１関節部１４の角度ｑ_１ｉを衝突対応動作生成手段６により決定する。すなわち、ｑ_１ｉ＝ａｔａｎ２（ｙ_ｉ，ｘ_ｉ）である。ただし、ａｔａｎ２（ｙ_ｉ，ｘ_ｉ）＝ａｒｇ（ｙ_ｉ＋ｊｘ_ｉ）であり、ｊは虚数、ａｒｇは複素数の偏角である。また、第２関節部１５の角度ｑ_２ｉは、人３８をロボットアーム８が囲うようにするために、例えば６０°という固定値とする。

衝突対応姿勢時のロボットアーム８の第１関節部１４の角度はｑ_１ｉとする。また、第２関節部１５の角度ｑ_２ｉに関しては、衝突予定位置

と絶対座標系３２の原点Ｏ_０間の距離Ｌ_ｘｉに応じて、以下の接触パターン（１）〜（３）の３パターンで変化させるとする。

図３Ｂに示す接触パターン（１）：

の場合、ｑ_２ｉ＝９０°とする。

図３Ｃに示す接触パターン（２）：

の場合、ｑ_２ｉ＝｛（Ｌ_ｘｉ−Ｌ_１）／０．４×Ｌ_１｝×９０°とする。

図３Ｄに示す接触パターン（３）：Ｌ_１×１．２＜Ｌ_ｘｉの場合、ｑ_２ｉ＝０°とする。

このようにすれば、ロボットアーム８は、以下のような姿勢を伴うインピーダンス制御動作を行なうことができる。

上記接触パターン（１）の場合、図３Ｂに示すように、人３８との接触は第１リンク１２で発生し、第２リンク１１は第１リンク１２に対して９０°折れ曲がるような姿勢になるため、ロボットアーム８は人３８を囲うような姿勢となる。

上記接触パターン（２）の場合、図３Ｃに示すように、人３８との接触は第２関節部１５付近で発生し、人３８との接触位置に応じて、第２関節部１５の開き角度ｑ_２ｉが変化する姿勢になる。

上記接触パターン（３）の場合、図３Ｄに示すように、人３８との接触は第２リンク１１で発生し、第２関節部１５の角度ｑ_２ｉは０°となり、ロボットアーム８は第１リンク１２と第２リンク１１が一直線上にある姿勢となる。

このようなインピーダンス制御動作において、ロボットアーム８の関節角度の現在値（ｑ_１、ｑ_２）から、接触動作時の関節角度（ｑ_１ｉ、ｑ_２ｉ）への目標軌道である関節角度目標値ｑ_ｎｄ（ｔ）は、以下の５次多項式補間により衝突対応動作生成手段６で計算して求める。ここで、ｔは時間である。

ただし，

である。また、ｔ_ｆはインピーダンス制御動作の動作終了時間である。

衝突対応動作生成手段６は、関節角度目標値として、式（４）で計算される関節角度目標値ｑ_ｎｄ（ｔ）を動作制御手段７へと出力する。

また、衝突対応動作生成手段６は、接触パターン（１）〜（３）及び衝突時の人３８の速度成分に応じて、図３Ｂ〜図３Ｄの衝突対応姿勢時の各関節部の機械インピーダンス値を決定する。機械インピーダンス値の設定パラメータとしては、慣性Ｉ、粘性Ｄ、剛性Ｋがある。

接触パターン（１）及び（２）の場合：接触時の人３８の速度の第１及び第２リンク１２，１１に垂直な成分ｖ_ｉ１、ｖ_ｉ２に応じて、各関節部の機械インピーダンスパラメータを、Ｉ＝Ｋ_Ｉ／ｖ_ｉｎ、Ｄ_ｎ＝Ｋ_Ｄ／Ｖ_ｉｎ、Ｋ_ｎ＝Ｋ_Ｋ／Ｖ_ｉｎにより決定する。ただし、ｎ＝１、２である。また、Ｋ_Ｉ、Ｋ_Ｄ、Ｋ_Ｋは定数ゲインであり、具体的数値は安全な動作となるように実験的に求める。また、各パラメータの上限値Ｉｍａｘ_ｎ、Ｄｍａｘ_ｎ、Ｋｍａｘ_ｎを定め、計算値がこれらの上限値を超える場合、各パラメータは上限値に等しいとする。

接触パターン（３）の場合：第１関節部１４に関してはＩ_１＝Ｉｍａｘ_１、Ｄ_１＝Ｄｍａｘ_１、Ｋ_１＝Ｋｍａｘ_１とし、第２関節部１５に関しては、接触時の人３８の速度の第２リンク１１に垂直な成分ｖ_ｉ２に応じて、第２関節部１５の機械インピーダンスパラメータを、Ｉ_２＝Ｋ_Ｉ／ｖ_ｉ２、Ｄ_２＝Ｋ_Ｄ／Ｖ_ｉ２、Ｋ_２＝Ｋ_Ｋ／Ｖ_ｉ２により決定する。

手首部１０の機械インピーダンスに関しては、常に、上限値Ｉ_ｎ＝Ｉｍａｘ_ｎ、Ｄ_ｎ＝Ｄｍａｘ_ｎ、Ｋ_ｎ＝Ｋｍａｘ_ｎ、ただし、（ｎ＝３，４）とする。

上限値は十分大きな値とし、例えば、Ｉｍａｘ_ｎ＝Ｄｍａｘ_ｎ＝Ｋｍａｘ_ｎ＝１００００、ただし、（ｎ＝１，２，３，４）とする。

上記のように関節部の機械インピーダンスを決定すれば、接触パターン（１）及び（２）では、例えば、人３８が第１リンク１２にほぼ垂直に衝突する場合、ｖ_ｉ２≒０であるので、第１関節部１４では、インピーダンス制御動作前よりも機械インピーダンス値が小さく設定されて第１関節部１４が柔軟になり、第２関節部１５及び第３関節部１６及び第４関節部１７では、インピーダンス制御動作前よりも機械インピーダンス値が上限値に設定されて第２関節部１５及び第３関節部１６及び第４関節部１７が固くなる。すなわち、第１リンク１２の衝突位置の属する第１リンク１２よりも根元部（台部１３）に近い側である第１関節部１４の剛性を、他の関節部例えば第２関節部１５及び第３関節部１６及び第４関節部１７の剛性よりも低下させる衝突対応動作を衝突対応動作制御手段４により行なうことができる。したがって、第１関節部１４の柔軟性により、衝突の衝撃が緩和されると共に、第２関節部１５が固いことで、衝突の反動で第２リンク１１が不用意に動いて第２リンク１１が人３８に衝突してしまうこともなく、安全性が確保される。また、第１リンク１２と第２リンク１１でＬ字形を構成して人３８を囲うようにすることにより、人３８の前側及び横側の２方向での支えとして働くことができて、人３８が転倒するのを防ぎ（人３８が転倒しかけたときに第１リンク１２又は第２リンク１１に人３８が捉まることにより、人３８が転倒するのを防ぐことができて）、より安全性を高めることができる。

また、接触パターン（２）において、第２リンク１１に垂直な速度成分ｖ_ｉ２がある場合でも、速度成分ｖ_ｉ２に応じて第２関節部１５の剛性が低く設定されて第２関節部１５が柔らかくなることになるので、安全性を確保することができる。

また、接触パターン（３）では、インピーダンス制御動作前よりも第１関節部１４が固く、人３８の移動速度の第２リンク１１に垂直な成分ｖｉ２に応じて、衝突予定位置の属する第２リンク１１の衝突予定位置よりも根元部（台部１３）に最も近い側の関節部である第２関節部１５がインピーダンス制御動作前よりも柔らかく設定されるため、人３８が第２リンク１１に衝突する衝撃を、第２関節部１５の柔軟性で吸収できるため、安全性を確保することができる。また、上記とは逆に、第１関節部１４を柔らかく、第２関節部１５を固く設定する場合に比べて、衝突時にかかる慣性は第２リンク１１の慣性が主であり、第１リンク１２の慣性はかからないため、衝突時の衝撃を小さくすることができる。

なお、接触パターン（３）では、第１関節部１４を柔らかく、第２関節部１５を固く設定する場合の他に、第１関節部１４と第２関節部１５を共に、他の第３関節部１６及び第４関節部１７よりも柔らかく、すなわち、衝突予定位置の属する第２リンク１１より根元部（台部１３）に近い側の全ての関節部を、それ以外の関節部よりも柔らかくする方法も考えられる。

この方法では、関節部に可動範囲がある場合に、より安全にすることが可能である。各関節部に可動範囲がある場合、衝突により関節部が動作し、可動範囲を越えるとそれ以上は関節部を動かすことができなくなるため、柔軟性を発揮することができなくなる。しかし、根元部（台部１３）に近い側の関節部を、それ以外の関節部よりも、すべて柔らかくする場合、第１関節部１４と第２関節部１５の可動範囲が加算され、より大きな可動範囲が取れるため、柔軟性を発揮できる範囲が大きくなり安全性を高くすることができる。

次に、図４を使い、動作制御手段７の詳細について説明する。

図４において、８は制御対象である図１に示したロボットアームである。ロボットアーム８からは、それぞれの関節軸のエンコーダ３５により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ_１，ｑ_２，ｑ_３，ｑ_４］^Ｔが出力され、カウンタボード２３により制御装置に取り込まれる。ただし、ｑ_１，ｑ_２，ｑ_３，ｑ_４は、それぞれ、第１関節部１４、第２関節部１５、第３関節部１６、第４関節部１７の関節角度である。

２４は作業目標軌道生成手段であり、目標とするロボットアーム８の作業を実現するための手先位置・姿勢目標ベクトルｒ_ｗｄが出力される。図７に示すように、目標とするロボットアーム８の動作は、目的とする作業に応じて、ティーチングによるポイントごとの位置（ｒ_ｗｄ０、ｒ_ｗｄ１、ｒ_ｗｄ２・・・）が事前に与えられており、目標軌道生成手段２４は多項式補間を使用し、各ポイントごとの位置間の軌道を補完し、手先位置・姿勢目標ベクトルｒ_ｗｄを生成する。ただし、上記位置ｒ_ｗｄ０は時間ｔ＝０のときの位置、ｒ_ｗｄ１は時間ｔ＝ｔ_１のときの位置、ｒ_ｗｄ２は時間ｔ＝ｔ_２のときの位置と仮定する。

目標軌道生成手段２４から逆運動学計算手段２５に入力された手先位置・姿勢目標ベクトルｒ_ｗｄは、逆運動学計算手段２５により関節目標ベクトルｑ_ｗｄへと変換される。

２６は目標軌道切換手段であり、目標軌道切換手段２６には、逆運動学計算手段２５から関節目標ベクトルｑ_ｗｄが、衝突対応動作生成手段６から衝突対応動作関節目標ベクトルｑ_ｉｄが、人接近検知手段３から衝突対応動作切換指令がそれぞれ入力されている。通常は、作業目標軌道生成手段２４により生成された手先位置・姿勢目標ベクトルｒ_ｗｄが逆運動学計算手段２５により変換された関節目標ベクトルｑ_ｗｄを目標軌道切換手段２６は選択し、関節目標ベクトルｑ_ｄとして目標軌道切換手段２６から出力する。しかしながら、人接近時に人接近検知手段３から衝突対応動作切換指令を目標軌道切換手段２６が受け取ると、衝突対応動作生成手段６から入力された接触動作関節目標ベクトルｑ_ｉｄを目標軌道切換手段２６は選択し、関節目標ベクトルｑ_ｄとして目標軌道切換手段２６から出力する。

２７はトルク推定手段であり、人３８とロボットアーム８の接触によってロボットアーム８の各関節部に加わる外力トルクを推定する。トルク推定手段２７には、モータドライバ１９の電流センサで計測された、ロボットアーム８の各関節部を駆動するモータ３４を流れる電流値ｉ＝［ｉ_１，ｉ_２，ｉ_３，ｉ_４］^ＴがＡ／Ｄボード２２を介して取り込まれ、また、関節角の現在値ｑ及び、後述する関節角度誤差補償出力ｕ_ｑｅが関節誤差補償手段３０から取り込まれる。トルク推定手段２７は、オブザーバーとして機能し、以上の電流値ｉ、関節角の現在値ｑ、関節角度誤差補償出力ｕ_ｑｅよりロボットアーム８に加わる外力により、各関節部に発生するトルクτ_ｅｘｔ＝［τ_１ｅｘ、τ_２ｅｘｔ、τ_３ｅｘｔ、τ_４ｅｘｔ］^Ｔを算出する。

関節インピーダンス計算手段２８は、ロボットアーム８に機械インピーダンスを実現する機能を果たす部分であり、人３８が接近していない通常動作の際は０を関節インピーダンス計算手段２８から位置制御系３１の入力側に出力する。一方、人３８が接近し、衝突対応動作生成手段６から接触動作切換指令を受け取ると、衝突対応動作生成手段６で設定された機械インピーダンスパラメータＩ、Ｄ、Ｋと、関節角の現在値ｑと、トルク推定手段２７が推定した外力トルクτ_ｅｘｔより、ロボットアーム８の各関節部ごとに機械インピーダンスを実現するための関節目標補正出力ｑ_ｄΔを以下の式（１２）により関節インピーダンス計算手段２８で計算し、位置制御系３１の入力側に出力する。関節目標補正出力ｑ_ｄΔは、目標軌道切換手段２６の出力する関節目標ｑ_ｄに位置制御系３１の入力側で加算され、補正関節目標ベクトルｑ_ｄｍが生成されて位置制御系３１に入力される。

ただし、

であり、ｓはラプラス演算子である。

３０は位置制御系３１内の関節誤差補償手段であり、ロボットアーム８において計測される関節角度ベクトルの現在値ｑと、関節補正目標ベクトルｑ_ｄｍとの誤差ｑ_ｅが関節誤差補償手段３０に入力され、関節誤差補償手段３０から関節誤差補償出力ｕ_ｑｅがロボットアーム８に向けて出力される。

関節誤差補償出力ｕ_ｑｅは、Ｄ／Ａボード２１を介してモータドライバ１９に電圧指令値として与えられ、各関節軸が正逆回転駆動されてロボットアーム８が動作する。

なお、上記動作制御手段７は、上記したように、目標軌道生成手段２４と逆運動学計算手段２５と目標軌道切換手段２６とトルク推定手段２７と関節インピーダンス計算手段２８と位置制御系３１とより構成されている。

以上のように構成される動作制御手段７に関して動作の原理について説明する。

動作の基本は、関節誤差補償手段３０による関節ベクトル（関節角の現在値）ｑのフィードバック制御（位置制御）であり、図４の点線で囲まれた部分が位置制御系３１になっている。関節誤差補償手段３０として、例えば、ＰＩＤ補償器を使用すれば、関節誤差ｑ_ｅが０に収束するように制御が働き、目標とするロボットアーム８の動作が実現する。

人３８の衝突警戒領域１００への侵入を人接近検知手段３により検知した場合、上記説明した位置制御系３１に対し、関節インピーダンス計算手段２８により関節目標補正出力ｑ_ｄΔが、目標軌道切換手段２６の出力する関節目標ｑ_ｄに位置制御系３１の入力側で加算され、関節部の目標値の補正が行われる。このために、上記した位置制御系３１は、関節部の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。関節目標補正出力ｑ_ｄΔは式（１２）により関節インピーダンス計算手段２８で算出されるため、慣性Ｉ、粘性Ｄ、剛性Ｋの機械インピーダンスが実現される。

以上の原理に基づく動作制御手段７の動作を実現する制御プログラムの実際の動作ステップについて、図５のフローチャートに基づいて説明する。

ステップ１では、エンコーダ３５により計測された関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）が制御装置１に取り込まれる。

次いで、ステップ２では、制御装置１のメモリ（図示せず）に予め記憶されていたロボットアーム８の動作制御プログラム１８に基づき、動作制御手段７の作業目標軌道生成手段２４によりロボットアーム８の手先位置・姿勢目標ベクトルｒ_ｗｄが計算される。

次いで、ステップ３では、手先位置・姿勢目標ベクトルｒ_ｗｄが、逆運動学計算手段２５において関節目標ｑ_ｗｄに変換される。

次いで、ステップ４では、目標軌道切換手段２６により目標軌道の切換が行われる。人３８の衝突警戒領域１００への侵入がない場合には、作業動作を実行するため、ステップ５Ａへ進む。一方、人３８の衝突警戒領域１００への侵入があった場合には、衝突対応動作を実行するためステップ５Ｂへ処理を進める（目標軌道切換手段２６での処理）。

ステップ５Ｂでは、関節目標ベクトルｑ_ｄを、衝突対応動作生成手段６が生成した接触動作関節目標ベクトルｑ_ｉｄとする（目標軌道切換手段２６での処理）。その後、ステップ６に進む。

ステップ５Ａでは、関節目標ベクトルｑ_ｄを関節目標ｑ_ｗｄとする。（目標軌道切換手段２６での処理）。その後、ステップ８に進む。

ステップ６では、それぞれのモータ３４の駆動電流値ｉと、関節角度データ（関節角度ベクトル（関節角の現在値）ｑ）と、関節角度誤差補償出力ｕ_ｑｅから、ロボットアーム８の関節部における外力トルクτ_ｅｘｔが、トルク推定手段２７により計算される（トルク推定手段２７での処理）。

次いで、ステップ７では、衝突対応動作生成手段６において設定された機械インピーダンスパラメータＩ、Ｄ、Ｋと、関節角度データ（関節角度ベクトルｑ）と、トルク推定手段２７により計算されたロボットアーム８に加わる外力トルクτ_ｅｘｔから、関節目標補正出力ｑ_ｄΔが、関節インピーダンス計算手段２８により計算される（関節インピーダンス計算手段２８での処理）。

次いで、ステップ８では、関節目標ベクトルｑ_ｄと関節目標補正出力ｑ_ｄΔの和として補正関節目標ベクトルｑ_ｄｍが計算されて位置制御系３１に入力される。

次いで、ステップ９では、補正関節目標ベクトルｑ_ｄｍと現在の関節ベクトルｑとの差である関節部の誤差ｑ_ｅが関節誤差補償手段３０に入力され、関節角度誤差保証出力ｕ_ｑｅが関節誤差補償手段３０で計算される（関節誤差補償手段３０での処理）。関節誤差補償手段３０の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、関節誤差が０に収束するように制御が働く。

次いで、ステップ１０では、関節角度誤差補償出力ｕ_ｑｅがＤ／Ａボード２１を通じ、モータドライバ１９に与えられ、それぞれの関節部のモータ３４を流れる電流量を変化させることによりロボットアーム８のそれぞれの関節軸の回転運動が発生する。

以上のステップ１〜ステップ１０が制御の計算ループとして繰り返し実行されることにより、ロボットアーム８の動作の制御が実現する。

次に、本発明の第１実施形態における制御装置１の全体的な動作について、ロボットアーム８のハンド９で物体を把持し、運搬する作業を例に、図６のフローチャートに基づいて説明する。

ステップ２１では、人運動検出手段２において、画像撮像装置３６の画像データより画像認識処理を行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を検出する。

次いで、ステップ２２では、人３８の衝突警戒領域１００への侵入判断が人接近検知手段３により行なわれ、侵入無しと人接近検知手段３で判断された場合には、ステップ２３へ進む。

ステップ２３では、ロボットアーム８は動作制御手段７により動作制御されて、ロボットアーム８のハンド９で物体を把持し、運搬する作業動作を行なう。

ステップ２３の後はステップ２１に戻り、人接近検知手段３で人３８の侵入が確認されない限り、ステップ２１→ステップ２２→ステップ２３→ステップ２１・・・のループが繰り返され、ロボットアーム８は通常の上記物体把持及び運搬作業を実行する。

ステップ２２において人３８の侵入が確認された場合、ステップ２４へ進み、衝突位置推定手段５により衝突予定位置

が推定される。

次いで、ステップ２４．１では、衝突予定位置Ｘ_ｐと絶対座標系３２の原点Ｏ_０間の距離Ｌ_ｘｉを衝突対応動作生成手段６算出し、距離Ｌ_ｘｉと長さＬ_１を衝突対応動作生成手段６比較し、式（３Ａ）の不等式の場合を接触パターン（１）、式（３Ｂ）の不等式の場合を接触パターン（２）、その他の場合を接触パターン（３）であると、衝突対応動作生成手段６により判断する。

次いで、ステップ２５では、衝突予定位置

で衝突対応姿勢をとるための、衝突対応動作目標軌道ｑ_ｄ（ｔ）が衝突対応動作目標軌道生成手段６において式（４）により生成される。

次いで、ステップ２６では、衝突対応姿勢において設定する関節部の機械インピーダンス値が、接触パターン（１）〜（３）に応じて、また、接近する人３８の移動速度に応じて、先に、関節部の機械インピーダンスを決定の仕方として述べたように、衝突対応動作目標軌道生成手段６によって設定される。

次いで、ステップ２７では、動作制御手段７において、目標軌道切換手段２６が動作し、衝突対応動作目標軌道ｑ_ｄ（ｔ）が選択され、位置制御系によりロボットアーム８が動作し、衝突対応姿勢となる。また、関節インピーダンス計算手段２８も動作し、ロボットアーム８の各関節部は設定した機械インピーダンス値に個別に制御される。

次いで、ステップ２８では、人運動検出手段２において、画像撮像装置３６の画像データより画像認識処理を行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を検出する。

次いで、ステップ２９では、人３８の衝突警戒領域１００への侵入判断が人接近検知手段３で行なわれ、侵入有りと人接近検知手段３で判断された場合には、ステップ２７へ戻り、人３８の衝突警戒領域１００からの離脱が確認されない限り、ステップ２７→ステップ２８→ステップ２９→ステップ２７・・・のループが繰り返され、ロボットアーム８は衝突対応動作を継続する。

一方、ステップ２９において、人３８の衝突警戒領域１００への侵入無しと人接近検知手段３で判断された場合には、ステップ２１へと戻り、ロボットアーム８は通常の上記物体把持及び運搬作業動作へと復帰する。

以上の動作ステップ２１〜ステップ２９により、ロボットアーム８による上記物体把持及び運搬作業が実現し、人３８が接近した際には衝突対応動作への切換が実現する。

以上のように、人運動検出手段２、人接近検知手段３、衝突位置推定手段５、衝突対応動作制御手段４を備える事により、ロボットアーム８に人３８が接近したときに、人３８の運動に応じた適切な姿勢で動作し、適切な関節部の機械インピーダンスで、人３８と上記ロボットアーム８の衝突に対応する衝突対応動作を制御することが可能となる。

ロボットアーム８は根元部である台部１３にて床面に固定されているため、衝突予定位置より根元部（台部１３）側の関節部において、他の関節部よりも剛性が低く動きやすい状態の関節部が無いと、衝突に対する抗力が発生し、衝撃力が発生してしまうことになる。しかし、本発明の上記第１実施形態によれば、ロボットアーム８の各関節部ごとに個別に機械インピーダンスを設定するとき、他の関節部よりも根元（台部１３）側の関節部において剛性が低く動きやすい状態の関節部が設定されるため、その関節部で衝突時にかかる力を逃がすことができ、人に対する衝撃力は小さくなり、安全性を高くすることができる。

したがって、本発明の上記第１実施形態の制御装置１によれば、多関節型のロボットアーム８であっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存することができる安全なロボットアーム８の動作制御を実現できる。

なお、上記第１実施形態では、ロボットアーム８の関節部の数を３としたが、これに限られるわけではなく、２関節部以上のロボットアーム８において同様の効果を発揮する。

また、上記第１実施形態では、ロボットアーム８と人との衝突を例に説明を行ったが、これに限られるわけではなく、移動ロボットや移動中のワゴン等、その他の移動体とロボットアーム８との衝突でも同様の効果を発揮する。

なお、上記第１実施形態では、衝突警戒領域１００に人３８が進入した場合に、一度、衝突予定位置の推定を行うとしたが、これに限られるわけではなく、継続して衝突予定位置の推定を連続的に行う方法も可能である。この場合、衝突の推定精度が向上するため、より最適な姿勢、接触パターンで対応することができ、さらに安全性を向上することができる。一方、上記第１実施形態のように、一度、衝突予定位置の推定を行う場合は、計算量が少なくでき、ＣＰＵへの不可を軽減できるという利点がある。

（第２実施形態）
図８Ａ及び図８Ｂは本発明の第２実施形態におけるロボットアームの制御装置１の構成及び制御対象であるロボットアーム８の構成を示す図並びに上記第２実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図である。本発明の第２実施形態における制御装置１の基本的な構成は、図１に示した第１実施形態の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。

本発明の第２実施形態では、画像認識処理ではなく、ロボットアーム８と人３８が接触することにより発生する力を検出することにより、人運動検出及び人接近検知を行う。そのため、人運動検出手段２には、動作制御手段７のトルク推定手段２７から外力トルク情報が入力される構造となっている。従って、第２実施形態における制御装置１では画像撮像装置３６は不要となっている。

人運動検出手段２は、トルク推定手段２７が推定した外力トルクτ_ｅｘｔ＝［τ_１ｅｘ、τ_２ｅｘｔ、τ_３ｅｘｔ、τ_４ｅｘｔ］^Ｔを基に、以下のパターンにて、ロボットアーム８と人３８の相対的な位置関係を推定する。ただし、図９Ａ〜図９Ｄにおいて（ロボットアーム８を上方側から見下ろして）関節部を左回りに回そうとするトルクを正のトルクと定義する。

（１）第１関節部１４の外力トルクτ_１ｅｘｔ＞０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の場合：図９Ａのように、人３８の第１リンク１２での接触と、人運動検出手段２で推定する。

（２）第１関節部１４の外力トルクτ_１ｅｘｔ＜０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の場合：図９Ｂのように、人３８の第１リンク１２での接触と、人運動検出手段２で推定する。

（３）第１関節部１４の外力トルクτ_１ｅｘｔ＞０、第２関節部１５の外力トルクτ_２ｅｘｔ＞０の場合：図９Ｃのように、人３８の第２リンク１１での接触と、人運動検出手段２で推定する。

（４）第１関節部１４の外力トルクτ_１ｅｘｔ＜０、第２関節部１５の外力トルクτ_２ｅｘｔ＜０の場合：図９Ｄのように、人３８の第２リンク１１での接触と、人運動検出手段２で推定する。

以上の４パターンによる分類を行えば、ロボットアーム８と人３８の大まかな位置関係を知ることができる。また、人接近検知手段３は、第１関節部１４の外力トルクτ_１ｅｘｔ≒０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の状態から上記（１）〜（４）のパターンのいづれかに外力トルクが変化するのを検知することで、人３８の接近を検知することになる。

以上の方法により、人３８の概略位置を推定することで、衝突対応動作制御手段４で、位置関係に応じた姿勢、関節トルクを制御することにより、人３８と共存することができる安全なロボットアーム８の動作制御が可能となる。

なお、上記第１実施形態では、人運動検出手段２を画像認識処理によるとしたが、これに限られるわけではなく、その他のセンシング手段、例えば、レーザーレーダーセンサや、超音波センサ等、人３８の位置と速度が検出できるものであれば、同様の効果を発揮することができる。

また、上記第２実施形態では、トルク推定手段２７の推定した外力トルクを基に人検知を行うとしたが、これに限られるわけではなく、図１０に示すように、ロボットアーム８のリンク１２，１１など、人３８が接触する可能性のある部分に接触センサ３９を衝突位置取得手段の一例として設けて、接触センサ３９に人３８が接触することにより人検知を行うことでも、同様の効果を発揮することができる。また、接触センサと外力トルクを利用した人検知を併用すれば推定精度が向上し、さらに安全なロボットアーム８の制御を行なうことができる。

なお、上記第２実施形態では、ロボットアーム８の関節の数を３としたが、これに限られるわけではなく、２関節以上のロボットアーム８において同様の効果を発揮する。

また、上記第２実施形態では、ロボットアーム８と人との衝突を例に説明を行ったが、これに限られるわけではなく、移動ロボットや移動中のワゴン等、その他の移動体とロボットアーム８との衝突でも同様の効果を発揮する。

また、上記第１及び第２実施形態ではロボットアーム８を例に説明したが、アームに限らず、車輪により動く移動ロボットや、２足歩行ロボット、多足歩行ロボットなどの移動体でも、本体と人との接触に置いて本発明を適用することにより、本発明と同様の効果を発揮することができる。

なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明のロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムは、家庭用ロボットなど人と接する可能性があるロボットアームの動作の制御を行うロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムとして有用である。また、家庭用ロボットに限らず、産業用ロボットや、生産設備等における可動機構の制御装置としても適用が可能である。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

本発明は、家庭用ロボット等、人との物理的な接触の可能性のあるロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムに関する。

近年、ペットロボットなどの家庭用ロボットの開発が盛んに行われており、将来は家事支援ロボット等、より実用的な家庭用ロボットが実用化されるものと期待されている。家庭用ロボットは、家庭内に入り人と共生する必要があるため、人との物理的な接触が不可欠であり、安全性の面から、柔軟であることが必要とされる。

こうした課題に対し、従来技術としては、特許文献１（特開平１０−３２９０７１号公報）において、ロボットアームに加わった人との接触力を検知し、アームに大きな力が加わった時には復元力を小さくし安全性を高め、アームに微少な力が加わっている時には復元力を大きくし動作精度を確保する制御装置を開示している。

また、特許文献２には、図１１Ａに示すように、障害物が存在する環境とロボット５１３との接触部およびその接触力をそれぞれ検知するセンサ機構５１５と、移動機構５１１およびマニピュレータ５１２の位置姿勢をそれぞれ計測する位置姿勢計測装置５１４と、前記接触力と位置姿勢とからロボット５１３の接触力回避動作を計算する計算機５１７と、この計算機５１７の計算結果によって移動機構５１１とマニピュレータ５１２とを駆動させるアクチュエータ５１６とを有する制御装置が開示されており、障害物が存在する環境とロボット５１３との接触時には、移動機構５１１とマニピュレータ５１２との両者の連携動作により回避動作を行うようにしている。

また、特許文献３には、図１２に示すように、エンドエフェクタ４１１を有するロボット４０１に作用したことを力覚センサ４１２で検出した外部環境からの力に対して、リアルタイムに外部環境の剛性係数を推定して、モータ４１３ａの制御によりロボット４０１のアーム４１０を駆動制御するインピーダンス制御装置が開示されている。

特開平１０−３２９０７１号公報 特開２００５−５９１６１号公報 特開２００４−２２３６６３号公報

しかしながら、上記従来制御装置では、多関節型のロボットアームへの適用を考慮しておらず、多関節型のロボットアームの場合において安全性が保証された制御装置となっていない。

また、上記従来制御装置では、接触する人の動きを考慮しておらず、人の動きに合わせた最適な制御になっていない。

また、特許文献２では、図１１Ｂ及び図１１Ｃに示すように、ロボット５１３における、障害物が存在する環境との接触部５３２を、接触力５３１の方向に接触回避経路５２６沿いに移動させることで、回避動作５２７を行うことで、環境からの接触力５３１を小さくし、最終的に接触がなくなるようにしている。また、図１１Ｄ及び図１１Ｅに示すように、ロボット５１３における、障害物が存在する環境との接触部５３２の接触面を、接触力５３１により発生するモーメント５３３の方向に傾けるとともに、前記接触部を、任意の大きさの円５２６を描くように移動させることで、回避動作５２７を行うことで、環境からの接触力５３１を小さくし、最終的に接触がなくなるようにしている。しかしながら、それぞれの回避動作５２７の具体的な動作制御については、何ら開示はされていない。

また、特許文献３については、図１２に示すように、算出された外部環境の剛性係数と相関関係をもってロボット側のインピーダンス特性を変化させ、これらの値を用いてエンドエフェクタ４１１の位置を算出するものであって、エンドエフェクタ４１１の位置にしか着目しておらず、人へ接触のダメージを与えないようにするためのアーム４１０の具体的な動作制御については全く開示されていない。

本発明の目的は、上記従来の制御装置の課題を解決し、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できるロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムを提供することにある。

上記目的を達成するために、本発明は以下のように構成する。

本発明の第１態様によれば、多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置を提供する。

本発明の第９態様によれば、多関節型ロボットアームの制御方法であって、
上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを接近検知手段で検知し、
上記接近検知手段により上記人または移動体の接近を検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を行なうように制御するロボットアームの制御方法を提供する。

本発明の第１０態様によれば、上記多関節型ロボットアームと、
上記ロボットアームを制御する第１態様〜第８態様のいずれか１つに記載のロボットアームの制御装置とを有して、
上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。

本発明の第１１態様によれば、コンピューターを、
人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラムを提供する。

本発明のロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムによれば、衝突位置取得手段と衝突対応動作制御手段とを備えるか、又は、それらの機能を備えるようにしている。このため、ロボットアームに人が接近したときに、人の運動に応じた適切な姿勢で動作したり又は適切な関節部の機械インピーダンスで動作することが可能となる。言い換えれば、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて、上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることにより、根元部に近い側の関節部を動きやすくして、当該関節部で衝突時にかかる力を逃がすことができ、人に与える衝撃力が小さくなり、安全性を高くすることができる。よって、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できる。

本発明の記述を続ける前に、添付図面において同じ部品については同じ参照符号を付している。

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

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置を提供する。

本発明の第２態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクの上記根元部に最も近い側の関節部の剛性を他の関節部の剛性よりも低下させる上記衝突対応動作を行なうように制御する第１態様に記載のロボットアームの制御装置を提供する。

本発明の第３態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第４態様によれば、上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームの上記リンクより手首部に近い側の各関節部ごとに個別に関節角度を制御することで、上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第５態様によれば、上記人または移動体の運動を検出する運動検出手段をさらに備え、
上記衝突位置取得手段は、上記運動検出手段の検出結果に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を取得し、
上記衝突対応動作制御手段は、上記衝突位置取得手段で推定した上記衝突位置に基づいて上記衝突対応動作を行なうように制御する第１態様又は第２態様に記載のロボットアームの制御装置を提供する。

本発明の第６態様によれば、上記運動検出手段の検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを検知する接近検知手段をさらに備え、
上記衝突対応動作制御手段は、上記接近検知手段が上記人または移動体の接近を検知したときに、上記衝突対応動作を行なうように制御する第５態様に記載のロボットアームの制御装置を提供する。

本発明の第７態様によれば、上記衝突対応動作制御手段は、上記運動検出手段の検出した上記人または移動体の運動の速度成分に基づいて、上記衝突対応動作を行なうように制御する第５態様又は第６態様に記載のロボットアームの制御装置を提供する。

本発明の第８態様によれば、上記運動検出手段は、上記人または移動体の位置と移動速度を検出して上記人の運動を検出する第５〜７態様のいずれか１つに記載のロボットアームの制御装置を提供する。

本発明の第９態様によれば、多関節型ロボットアームの制御方法であって、
上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを接近検知手段で検知し、
上記接近検知手段により上記人または移動体の接近を検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を行なうように制御するロボットアームの制御方法を提供する。

本発明の第１０態様によれば、上記多関節型ロボットアームと、
上記ロボットアームを制御する第１態様〜第８態様のいずれか１つに記載のロボットアームの制御装置とを有して、
上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。
本発明の第１１態様によれば、コンピューターを、
人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラムを提供する。

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

（第１実施形態）
図１は本発明の第１実施形態における多関節型のロボットアームの制御装置１の構成及びその制御対象である多関節型のロボットアーム８の構成を示す図である。このロボットアームの制御装置１は、上記第１実施形態のロボットアームの制御方法を実施するものであって、多関節型のロボットアーム８を備えたロボットに使用可能であり、後述するように、多関節型のロボットアーム８の制御プログラムとしても使用可能なものである。

図１において、２は運動検出手段の一例としての人運動検出手段であり、カメラなどの画像撮像装置３６で撮像された画像データにより画像認識処理を人運動検出手段２で行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を人運動検出手段２で検出する。

３は接近検知手段の一例としての人接近検知手段であり、人運動検出手段２による検出結果である人位置の情報と、後述する動作制御手段７から得られるロボットアーム８の関節角度情報より求められるロボットアーム８の姿勢の情報との関係より、ロボットアーム８に人３８が接近していることを検知する。

５は衝突位置取得手段の一例としての衝突位置推定手段であり、人運動検出手段２で検出した、接近する人３８の位置の情報、並びに、人３８の移動速度及び移動方向すなわち移動速度ベクトル情報より、ロボットアーム８と人３８との衝突予定位置（衝突位置として取り扱い可能な位置）を推定し、推定結果を、後述する衝突対応動作生成手段６に出力する。

４は衝突対応動作制御手段であり、ロボットアーム８で作業を遂行する際のロボットアーム８の動作の制御（具体的には、上記ロボットアーム８の機械インピーダンスの値の制御）を行い、人接近検知手段３がロボットアーム８への人３８の接近を検知した際には、人運動検出手段２の検出結果に基づき、人３８とロボットアーム８の衝突時の衝突対応動作を制御する。

衝突対応動作制御手段４は、衝突対応動作生成手段６、及び動作制御手段７とより構成される。

衝突対応動作生成手段６は、衝突位置推定手段５が推定した衝突予定位置において、ロボットアーム８と人３８が接触するときのロボットアーム８の関節目標軌道及び関節目標の機械インピーダンス値を計算して、計算結果を動作制御手段７に出力する。

動作制御手段７は、ロボットアーム８の手先の位置姿勢を制御し、目標とする作業をロボットアーム８の手先で実現すると共に、人３８が接近した場合には、衝突対応動作生成手段６が生成した関節目標軌道に追従するように制御を切り換えると同時に、衝突対応動作生成手段６が生成した関節目標の機械インピーダンス値になるようにロボットアーム８の各関節部の機械インピーダンスの値を制御するインピーダンス制御を行う。ここで、ロボットアーム８の各関節部は、後述するように、Ｄ／Ａボード２１及びＡ／Ｄボード２２に接続されたモータドライバ１９を介して、各関節部の駆動装置の一例としてのモータ３４がインピーダンス制御の下に駆動されてそれぞれ屈曲動作が行われるようになっている。

制御装置１は、ハードウェア的には一般的なパーソナルコンピュータにより構成されており、人運動検出手段２、人接近検知手段３、衝突位置推定手段５、衝突対応動作生成手段６、及び、動作制御手段７のうちの入出力ＩＦ（入出力インターフェース）２０を除く部分は、パーソナルコンピュータで実行可能な制御プログラム１８としてソフトウェア的に実現される。

入出力ＩＦ２０は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２１、Ａ／Ｄボード２２、カウンタボード２３により構成される。

ロボットアーム８の動作を制御するための制御プログラム１８が実行されることにより制御装置１が機能しており、ロボットアーム８の各関節部のモータ３４に連結されたエンコーダ３５より出力される関節角度情報がカウンタボード２３を通じて制御装置１に取り込まれ、制御装置１によって各関節部の回転動作での制御指令値が算出される。算出された各制御指令値は、Ｄ／Ａボード２１を通じてモータドライバ１９に与えられ、モータドライバ１９から送られた各制御指令値に従って、ロボットアーム８の各関節部のモータ３４が駆動される。

ロボットアーム８は、４自由度の多リンクマニピュレータであり、ハンド９と、ハンド９が取付けられる手首部１０を先端に有する第２リンク１１と、第２リンク１１が回転可能に連結される第１リンク１２と、第１リンク１２が回転可能に連結支持される台部１３とを有している。手首部１０は、第３関節部１６及び第４関節部１７の２つの回転軸（上下方向沿いの回転軸と横方向沿いの回転軸）を有しており、第２リンク１１に対するハンド９の相対的な姿勢（向き）を変化させることができる。第２リンク１１の他端は第１リンク１２の先端に対して第２関節部１５の上下方向沿いの回転軸周りに回転可能とし、第１リンク１２の他端は台部１３に対して第１関節部１４の上下方向沿いの回転軸周りに回転可能として、合計４個の軸周りに回転可能として上記４自由度の多リンクマニピュレータを構成している。

各軸の回転部分を構成する各関節部には、各関節部で連結される２つの部材のうちの一方の部材に備えられかつ後述するモータドライバ１９により駆動制御される回転駆動装置の一例としてのモータ３４（実際には、ロボットアーム８の各関節部の内部に配設されている）と、モータ３４の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ３５（実際には、ロボットアーム８の各関節部の内部に配設されている）とを備えて、モータ３４の回転軸が、各関節部で連結される２つの部材のうちの他方の部材に連結されて上記回転軸を正逆回転させることにより他方の部材を一方の部材に対して各軸周りに回転可能とするように構成している。

３２は、台部１３の下側固定部に対して相対的な位置関係が固定された絶対座標系であり、その原点をＯ_０とする。３３は、ハンド９に対して相対的な位置関係が固定された手先座標系である。絶対座標系３２から見た手先座標系３３の原点位置Ｏｅ（ｘ、ｙ）をロボットアーム８の手先位置、絶対座標系３２から見た手先座標系３３の姿勢（φ、θ）をロボットアーム８の手先姿勢とし、手先位置・姿勢ベクトルｒをｒ＝［ｘ、ｙ、φ、θ］^Ｔと定義する。ロボットアーム８の手先位置及び姿勢を制御する場合には、手先位置・姿勢ベクトルｒを目標手先位置・姿勢ベクトルｒ_ｗｄに追従させるように制御装置１で制御することになる。

次に、制御装置１を構成する各機能ブロックについて、詳細構成と動作について説明する。

人運動検出手段２には、画像撮像装置３６で撮影した画像データが入力され、人運動検出手段２での画像認識処理により、ロボットアーム８が配置された部屋の天井から床を見下ろしたときの人３８の２次元位置Ｘ_ｐ（ｘ_ｐ，ｙ_ｐ）及び人３８の移動速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）が検出され、２次元位置Ｘ_ｐ（ｘ_ｐ，ｙ_ｐ）及び移動速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の情報が人運動検出手段２から人接近検知手段３及び衝突位置推定手段５へ出力される。

人接近検知手段３は、図２に示す衝突警戒領域１００を定義し、人３８がこの領域１００に入ったかどうかで、人３８がロボットアーム８に接近したかどうかの検知判断を行なう。

衝突警戒領域１００の基端側の扇形領域１００Ａは、第１リンク１２の長さＬ_１のα_１倍、すなわちα_１Ｌ_１を半径とし、第１関節軸（第１関節部１４の回転軸）を要に、第１関節部１４の関節角ｑ_１の、例えば、第１リンク１２の中心軸を中心とした±β_１°の角度の領域とする。また、衝突警戒領域１００の先端側の扇形領域１００Ｂは、第２リンク１１の長さＬ_２とハンド９の長さＬ_Ｈのα_２倍、すなわちα_２（Ｌ_２+Ｌ_Ｈ）を半径とし、第２関節軸（第２関節部１５の回転軸）を要に、第２関節部１５の関節角ｑ_２の、例えば、第２リンク１１の中心軸を中心とした±β_２°の角度の領域とする。このようにすれば、速度に比例して扇形領域の範囲を変化させても、ハンド９の先が、先端側の扇形領域１００Ｂの外に出るのを確実に防止できる。

衝突警戒領域１００を定義するα_１、α_２、β_１、β_２の値は、任意の値であるが、例えばロボットアーム８を動作させる手先速度V_r={(dx/dt)²+(dy/dt)²}^1/2を基にこれらの値を決めることができ、手先速度V_rに比例してα_１、α_２、β_１、β_２の値を大きくするように決めてやれば、ロボットアーム８の動作速度が大きい場合には衝突警戒領域１００の面積が大きくなり、安全性を向上させることができる。

この第１実施形態では、例えば、手先速度V_rの単位をm/sとするとともに、V_ＫはV_rの大きさによってV_rに比例する値又は定数となるものとして、α_１＝１．２V_Ｋ、α_２＝１．２V_Ｋ、β_１＝２４V_Ｋ、β_２＝２４V_Ｋとする。ただし、V_Ｋは、

の場合、V_Ｋ＝１．９V_ｒで定義される値（変数）であり、V_ｒ＜０．５[ｍ／ｓ]の場合、V_Ｋ＝０．９５と定数として定義される値（変数）である。

このようにすれば手先速度が０．５[ｍ／ｓ]以上の時には手先速度の大きさに比例して衝突警戒領域１００の大きさが決まり、また、０．５[ｍ／ｓ]未満の時にはα_１＝１．１４、α_２＝１．１４、β_１＝２２．８、β_２＝２２．８と固定値になり、手先速度が小さい場合でも、ある程度の大きさの衝突警戒領域１００が確保される。

人接近検知手段３は、人運動検出手段２の検出した人３８の位置の情報と、動作制御手段７から得られるロボットアーム８の関節角度情報より、人３８とロボットアーム８の相対関係を算出し、人３８が衝突警戒領域１００に入れば、衝突対応動作制御手段４に人侵入を通知する。

衝突位置推定手段５は、人運動検出手段２の検出した人３８の位置の情報及び移動速度ベクトルの情報から人３８との衝突予定位置

を推定して衝突対応動作生成手段６に出力する。

例えば、図３Ａに示すように、速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の延長線と垂直に交わり、ロボットアーム８の第１関節部１４の回転軸を通る直線Ａを考える。直線Ａと絶対座標系３２のｘ軸がなす角度をｑ_１ｉとし、速度ベクトルＶ_ｐ（ｖ_ｘ，ｖ_ｙ）の延長線と直線Ａとの交点を、衝突予定位置

とすると、角度ｑ_１ｉは以下の式で与えられ、

は以下の式（１）〜（３）で与えられる。

次に、図３Ａ〜図３Ｃを使い、衝突対応動作生成手段６の詳細について説明する。

衝突対応動作生成手段６は、図３Ａの通常作業時の状態（現在状態）から図３Ｂの衝突受け止め姿勢への関節目標軌道を生成する。

ロボットアーム８の第１リンク１２の中心軸の線が衝突予定位置

に重なるよう第１関節部１４の角度ｑ_１ｉを衝突対応動作生成手段６により決定する。すなわち、ｑ_１ｉ＝ａｔａｎ２（ｙ_ｉ，ｘ_ｉ）である。ただし、ａｔａｎ２（ｙ_ｉ，ｘ_ｉ）＝ａｒｇ（ｙ_ｉ＋ｊｘ_ｉ）であり、ｊは虚数、ａｒｇは複素数の偏角である。また、第２関節部１５の角度ｑ_２ｉは、人３８をロボットアーム８が囲うようにするために、例えば６０°という固定値とする。

衝突対応姿勢時のロボットアーム８の第１関節部１４の角度はｑ_ｌｉとする。また、第２関節部１５の角度ｑ_２ｉに関しては、衝突予定位置

と絶対座標系３２の原点Ｏ_０間の距離Ｌ_ｘｉに応じて、以下の接触パターン（１）〜（３）の３パターンで変化させるとする。

図３Ｂに示す接触パターン（１）：

の場合、ｑ_２ｉ＝９０°とする。

図３Ｃに示す接触パターン（２）：

の場合、ｑ_２ｉ＝｛（Ｌ_ｘｉ−Ｌ_１）／０．４×Ｌ_１｝×９０°とする。

図３Ｄに示す接触パターン（３）：Ｌ_１×１．２＜Ｌ_ｘｉの場合、ｑ_２ｉ＝０°とする。

このようにすれば、ロボットアーム８は、以下のような姿勢を伴うインピーダンス制御動作を行なうことができる。

上記接触パターン（１）の場合、図３Ｂに示すように、人３８との接触は第１リンク１２で発生し、第２リンク１１は第１リンク１２に対して９０°折れ曲がるような姿勢になるため、ロボットアーム８は人３８を囲うような姿勢となる。

上記接触パターン（２）の場合、図３Ｃに示すように、人３８との接触は第２関節部１５付近で発生し、人３８との接触位置に応じて、第２関節部１５の開き角度ｑ_２ｉが変化する姿勢になる。

上記接触パターン（３）の場合、図３Ｄに示すように、人３８との接触は第２リンク１１で発生し、第２関節部１５の角度ｑ_２ｉは０°となり、ロボットアーム８は第１リンク１２と第２リンク１１が一直線上にある姿勢となる。

このようなインピーダンス制御動作において、ロボットアーム８の関節角度の現在値（ｑ_１、ｑ_２）から、接触動作時の関節角度（ｑ_１ｉ、ｑ_２ｉ）への目標軌道である関節角度目標値ｑ_ｎｄ（ｔ）は、以下の５次多項式補間により衝突対応動作生成手段６で計算して求める。ここで、ｔは時間である。

ただし，

である。また、ｔ_ｆはインピーダンス制御動作の動作終了時間である。

衝突対応動作生成手段６は、関節角度目標値として、式（４）で計算される関節角度目標値ｑ_ｎｄ（ｔ）を動作制御手段７へと出力する。

また、衝突対応動作生成手段６は、接触パターン（１）〜（３）及び衝突時の人３８の速度成分に応じて、図３Ｂ〜図３Ｄの衝突対応姿勢時の各関節部の機械インピーダンス値を決定する。機械インピーダンス値の設定パラメータとしては、慣性Ｉ、粘性Ｄ、剛性Ｋがある。

接触パターン（１）及び（２）の場合：接触時の人３８の速度の第１及び第２リンク１２，１１に垂直な成分ｖ_ｉ１、ｖ_ｉ２に応じて、各関節部の機械インピーダンスパラメータを、Ｉ_ｎ＝Ｋ_Ｉ／ｖ_ｉｎ、Ｄ_ｎ＝Ｋ_Ｄ／Ｖ_ｉｎ、Ｋ_ｎ＝Ｋ_Ｋ／Ｖ_ｉｎにより決定する。ただし、ｎ＝１、２である。また、Ｋ_Ｉ、Ｋ_Ｄ、Ｋ_Ｋは定数ゲインであり、具体的数値は安全な動作となるように実験的に求める。また、各パラメータの上限値Ｉｍａｘ_ｎ、Ｄｍａｘ_ｎ、Ｋｍａｘ_ｎを定め、計算値がこれらの上限値を超える場合、各パラメータは上限値に等しいとする。

接触パターン（３）の場合：第１関節部１４に関してはＩ_１＝Ｉｍａｘ_１、Ｄ_１＝Ｄｍａｘ_１、Ｋ_１＝Ｋｍａｘ_１とし、第２関節部１５に関しては、接触時の人３８の速度の第２リンク１１に垂直な成分ｖ_ｉ２に応じて、第２関節部１５の機械インピーダンスパラメータを、Ｉ_２＝Ｋ_Ｉ／ｖ_ｉ２、Ｄ_２＝Ｋ_Ｄ／Ｖ_ｉ２、Ｋ_２＝Ｋ_Ｋ／Ｖ_ｉ２により決定する。

手首部１０の機械インピーダンスに関しては、常に、上限値Ｉ_ｎ＝Ｉｍａｘ_ｎ、Ｄ_ｎ＝Ｄｍａｘ_ｎ、Ｋ_ｎ＝Ｋｍａｘ_ｎ、ただし、（ｎ＝３，４）とする。

上限値は十分大きな値とし、例えば、Ｉｍａｘ_ｎ＝Ｄｍａｘ_ｎ＝Ｋｍａｘ_ｎ＝１００００、ただし、（ｎ＝１，２，３，４）とする。

上記のように関節部の機械インピーダンスを決定すれば、接触パターン（１）及び（２）では、例えば、人３８が第１リンク１２にほぼ垂直に衝突する場合、ｖ_ｉ２≒０であるので、第１関節部１４では、インピーダンス制御動作前よりも機械インピーダンス値が小さく設定されて第１関節部１４が柔軟になり、第２関節部１５及び第３関節部１６及び第４関節部１７では、インピーダンス制御動作前よりも機械インピーダンス値が上限値に設定されて第２関節部１５及び第３関節部１６及び第４関節部１７が固くなる。すなわち、第１リンク１２の衝突位置の属する第１リンク１２よりも根元部（台部１３）に近い側である第１関節部１４の剛性を、他の関節部例えば第２関節部１５及び第３関節部１６及び第４関節部１７の剛性よりも低下させる衝突対応動作を衝突対応動作制御手段４により行なうことができる。したがって、第１関節部１４の柔軟性により、衝突の衝撃が緩和されると共に、第２関節部１５が固いことで、衝突の反動で第２リンク１１が不用意に動いて第２リンク１１が人３８に衝突してしまうこともなく、安全性が確保される。また、第１リンク１２と第２リンク１１でＬ字形を構成して人３８を囲うようにすることにより、人３８の前側及び横側の２方向での支えとして働くことができて、人３８が転倒するのを防ぎ（人３８が転倒しかけたときに第１リンク１２又は第２リンク１１に人３８が捉まることにより、人３８が転倒するのを防ぐことができて）、より安全性を高めることができる。

また、接触パターン（２）において、第２リンク１１に垂直な速度成分ｖ_ｉ２がある場合でも、速度成分ｖ_ｉ２に応じて第２関節部１５の剛性が低く設定されて第２関節部１５が柔らかくなることになるので、安全性を確保することができる。

また、接触パターン（３）では、インピーダンス制御動作前よりも第１関節部１４が固く、人３８の移動速度の第２リンク１１に垂直な成分ｖｉ２に応じて、衝突予定位置の属する第２リンク１１の衝突予定位置よりも根元部（台部１３）に最も近い側の関節部である第２関節部１５がインピーダンス制御動作前よりも柔らかく設定されるため、人３８が第２リンク１１に衝突する衝撃を、第２関節部１５の柔軟性で吸収できるため、安全性を確保することができる。また、上記とは逆に、第１関節部１４を柔らかく、第２関節部１５を固く設定する場合に比べて、衝突時にかかる慣性は第２リンク１１の慣性が主であり、第１リンク１２の慣性はかからないため、衝突時の衝撃を小さくすることができる。

なお、接触パターン（３）では、第１関節部１４を柔らかく、第２関節部１５を固く設定する場合の他に、第１関節部１４と第２関節部１５を共に、他の第３関節部１６及び第４関節部１７よりも柔らかく、すなわち、衝突予定位置の属する第２リンク１１より根元部（台部１３）に近い側の全ての関節部を、それ以外の関節部よりも柔らかくする方法も考えられる。

この方法では、関節部に可動範囲がある場合に、より安全にすることが可能である。各関節部に可動範囲がある場合、衝突により関節部が動作し、可動範囲を越えるとそれ以上は関節部を動かすことができなくなるため、柔軟性を発揮することができなくなる。しかし、根元部（台部１３）に近い側の関節部を、それ以外の関節部よりも、すべて柔らかくする場合、第１関節部１４と第２関節部１５の可動範囲が加算され、より大きな可動範囲が取れるため、柔軟性を発揮できる範囲が大きくなり安全性を高くすることができる。

次に、図４を使い、動作制御手段７の詳細について説明する。

図４において、８は制御対象である図１に示したロボットアームである。ロボットアーム８からは、それぞれの関節軸のエンコーダ３５により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ_１，ｑ_２，ｑ_３，ｑ_４］^Ｔが出力され、カウンタボード２３により制御装置に取り込まれる。ただし、ｑ_１，ｑ_２，ｑ_３，ｑ_４は、それぞれ、第１関節部１４、第２関節部１５、第３関節部１６、第４関節部１７の関節角度である。

２４は作業目標軌道生成手段であり、目標とするロボットアーム８の作業を実現するための手先位置・姿勢目標ベクトルｒ_ｗｄが出力される。図７に示すように、目標とするロボットアーム８の動作は、目的とする作業に応じて、ティーチングによるポイントごとの位置（ｒ_ｗｄ０、ｒ_ｗｄ１、ｒ_ｗｄ２・・・）が事前に与えられており、目標軌道生成手段２４は多項式補間を使用し、各ポイントごとの位置間の軌道を補完し、手先位置・姿勢目標ベクトルｒ_ｗｄを生成する。ただし、上記位置ｒ_ｗｄ０は時間ｔ＝０のときの位置、ｒ_ｗｄ１は時間ｔ＝ｔ_１のときの位置、ｒ_ｗｄ２は時間ｔ＝ｔ_２のときの位置と仮定する。

目標軌道生成手段２４から逆運動学計算手段２５に入力された手先位置・姿勢目標ベクトルｒ_ｗｄは、逆運動学計算手段２５により関節目標ベクトルｑ_ｗｄへと変換される。

２６は目標軌道切換手段であり、目標軌道切換手段２６には、逆運動学計算手段２５から関節目標ベクトルｑ_ｗｄが、衝突対応動作生成手段６から衝突対応動作関節目標ベクトルｑ_ｉｄが、人接近検知手段３から衝突対応動作切換指令がそれぞれ入力されている。通常は、作業目標軌道生成手段２４により生成された手先位置・姿勢目標ベクトルｒ_ｗｄが逆運動学計算手段２５により変換された関節目標ベクトルｑ_ｗｄを目標軌道切換手段２６は選択し、関節目標ベクトルｑ_ｄとして目標軌道切換手段２６から出力する。しかしながら、人接近時に人接近検知手段３から衝突対応動作切換指令を目標軌道切換手段２６が受け取ると、衝突対応動作生成手段６から入力された接触動作関節目標ベクトルｑ_ｉｄを目標軌道切換手段２６は選択し、関節目標ベクトルｑ_ｄとして目標軌道切換手段２６から出力する。

２７はトルク推定手段であり、人３８とロボットアーム８の接触によってロボットアーム８の各関節部に加わる外力トルクを推定する。トルク推定手段２７には、モータドライバ１９の電流センサで計測された、ロボットアーム８の各関節部を駆動するモータ３４を流れる電流値ｉ＝［ｉ_１，ｉ_２，ｉ_３，ｉ_４］^ＴがＡ／Ｄボード２２を介して取り込まれ、また、関節角の現在値ｑ及び、後述する関節角度誤差補償出力ｕ_ｑｅが関節誤差補償手段３０から取り込まれる。トルク推定手段２７は、オブザーバーとして機能し、以上の電流値ｉ、関節角の現在値ｑ、関節角度誤差補償出力ｕ_ｑｅよりロボットアーム８に加わる外力により、各関節部に発生するトルクτ_ｅｘｔ＝［τ_１ｅｘ、τ_２ｅｘｔ、τ_３ｅｘｔ、τ_４ｅｘｔ］^Ｔを算出する。

関節インピーダンス計算手段２８は、ロボットアーム８に機械インピーダンスを実現する機能を果たす部分であり、人３８が接近していない通常動作の際は０を関節インピーダンス計算手段２８から位置制御系３１の入力側に出力する。一方、人３８が接近し、衝突対応動作生成手段６から接触動作切換指令を受け取ると、衝突対応動作生成手段６で設定された機械インピーダンスパラメータＩ、Ｄ、Ｋと、関節角の現在値ｑと、トルク推定手段２７が推定した外力トルクτ_ｅｘｔより、ロボットアーム８の各関節部ごとに機械インピーダンスを実現するための関節目標補正出力ｑ_ｄΔを以下の式（１２）により関節インピーダンス計算手段２８で計算し、位置制御系３１の入力側に出力する。関節目標補正出力ｑ_ｄΔは、目標軌道切換手段２６の出力する関節目標ｑ_ｄに位置制御系３１の入力側で加算され、補正関節目標ベクトルｑ_ｄｍが生成されて位置制御系３１に入力される。

ただし、

であり、ｓはラプラス演算子である。

３０は位置制御系３１内の関節誤差補償手段であり、ロボットアーム８において計測される関節角度ベクトルの現在値ｑと、関節補正目標ベクトルｑ_ｄｍとの誤差ｑ_ｅが関節誤差補償手段３０に入力され、関節誤差補償手段３０から関節誤差補償出力ｕ_ｑｅがロボットアーム８に向けて出力される。

関節誤差補償出力ｕ_ｑｅは、Ｄ／Ａボード２１を介してモータドライバ１９に電圧指令値として与えられ、各関節軸が正逆回転駆動されてロボットアーム８が動作する。

なお、上記動作制御手段７は、上記したように、目標軌道生成手段２４と逆運動学計算手段２５と目標軌道切換手段２６とトルク推定手段２７と関節インピーダンス計算手段２８と位置制御系３１とより構成されている。

以上のように構成される動作制御手段７に関して動作の原理について説明する。

動作の基本は、関節誤差補償手段３０による関節ベクトル（関節角の現在値）ｑのフィードバック制御（位置制御）であり、図４の点線で囲まれた部分が位置制御系３１になっている。関節誤差補償手段３０として、例えば、ＰＩＤ補償器を使用すれば、関節誤差ｑ_ｅが０に収束するように制御が働き、目標とするロボットアーム８の動作が実現する。

人３８の衝突警戒領域１００への侵入を人接近検知手段３により検知した場合、上記説明した位置制御系３１に対し、関節インピーダンス計算手段２８により関節目標補正出力ｑ_ｄΔが、目標軌道切換手段２６の出力する関節目標ｑ_ｄに位置制御系３１の入力側で加算され、関節部の目標値の補正が行われる。このために、上記した位置制御系３１は、関節部の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。関節目標補正出力ｑ_ｄΔは式（１２）により関節インピーダンス計算手段２８で算出されるため、慣性Ｉ、粘性Ｄ、剛性Ｋの機械インピーダンスが実現される。

以上の原理に基づく動作制御手段７の動作を実現する制御プログラムの実際の動作ステップについて、図５のフローチャートに基づいて説明する。

ステップ１では、エンコーダ３５により計測された関節角度データ（関節変数ベクトル又は関節角度ベクトルｑ）が制御装置１に取り込まれる。

次いで、ステップ２では、制御装置１のメモリ（図示せず）に予め記憶されていたロボットアーム８の動作制御プログラム１８に基づき、動作制御手段７の作業目標軌道生成手段２４によりロボットアーム８の手先位置・姿勢目標ベクトルｒ_ｗｄが計算される。

次いで、ステップ３では、手先位置・姿勢目標ベクトルｒ_ｗｄが、逆運動学計算手段２５において関節目標ｑ_ｗｄに変換される。

次いで、ステップ４では、目標軌道切換手段２６により目標軌道の切換が行われる。人３８の衝突警戒領域１００への侵入がない場合には、作業動作を実行するため、ステップ５Ａへ進む。一方、人３８の衝突警戒領域１００への侵入があった場合には、衝突対応動作を実行するためステップ５Ｂへ処理を進める（目標軌道切換手段２６での処理）。

ステップ５Ｂでは、関節目標ベクトルｑ_ｄを、衝突対応動作生成手段６が生成した接触動作関節目標ベクトルｑ_ｉｄとする（目標軌道切換手段２６での処理）。その後、ステップ６に進む。

ステップ５Ａでは、関節目標ベクトルｑ_ｄを関節目標ｑ_ｗｄとする。（目標軌道切換手段２６での処理）。その後、ステップ８に進む。

ステップ６では、それぞれのモータ３４の駆動電流値ｉと、関節角度データ（関節角度ベクトル（関節角の現在値）ｑ）と、関節角度誤差補償出力ｕ_ｑｅから、ロボットアーム８の関節部における外力トルクτ_ｅｘｔが、トルク推定手段２７により計算される（トルク推定手段２７での処理）。

次いで、ステップ７では、衝突対応動作生成手段６において設定された機械インピーダンスパラメータＩ、Ｄ、Ｋと、関節角度データ（関節角度ベクトルｑ）と、トルク推定手段２７により計算されたロボットアーム８に加わる外力トルクτ_ｅｘｔから、関節目標補正出力ｑ_ｄΔが、関節インピーダンス計算手段２８により計算される（関節インピーダンス計算手段２８での処理）。

次いで、ステップ８では、関節目標ベクトルｑ_ｄと関節目標補正出力ｑ_ｄΔの和として補正関節目標ベクトルｑ_ｄｍが計算されて位置制御系３１に入力される。

次いで、ステップ９では、補正関節目標ベクトルｑ_ｄｍと現在の関節ベクトルｑとの差である関節部の誤差ｑ_ｅが関節誤差補償手段３０に入力され、関節角度誤差保証出力ｕ_ｑｅが関節誤差補償手段３０で計算される（関節誤差補償手段３０での処理）。関節誤差補償手段３０の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、関節誤差が０に収束するように制御が働く。

次いで、ステップ１０では、関節角度誤差補償出力ｕ_ｑｅがＤ／Ａボード２１を通じ、モータドライバ１９に与えられ、それぞれの関節部のモータ３４を流れる電流量を変化させることによりロボットアーム８のそれぞれの関節軸の回転運動が発生する。

以上のステップ１〜ステップ１０が制御の計算ループとして繰り返し実行されることにより、ロボットアーム８の動作の制御が実現する。

次に、本発明の第１実施形態における制御装置１の全体的な動作について、ロボットアーム８のハンド９で物体を把持し、運搬する作業を例に、図６のフローチャートに基づいて説明する。

ステップ２１では、人運動検出手段２において、画像撮像装置３６の画像データより画像認識処理を行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を検出する。

次いで、ステップ２２では、人３８の衝突警戒領域１００への侵入判断が人接近検知手段３により行なわれ、侵入無しと人接近検知手段３で判断された場合には、ステップ２３へ進む。

ステップ２３では、ロボットアーム８は動作制御手段７により動作制御されて、ロボットアーム８のハンド９で物体を把持し、運搬する作業動作を行なう。

ステップ２３の後はステップ２１に戻り、人接近検知手段３で人３８の侵入が確認されない限り、ステップ２１→ステップ２２→ステップ２３→ステップ２１・・・のループが繰り返され、ロボットアーム８は通常の上記物体把持及び運搬作業を実行する。

ステップ２２において人３８の侵入が確認された場合、ステップ２４へ進み、衝突位置推定手段５により衝突予定位置

が推定される。

次いで、ステップ２４．１では、衝突予定位置Ｘ_ｐと絶対座標系３２の原点Ｏ_０間の距離Ｌ_ｘｉを衝突対応動作生成手段６算出し、距離Ｌ_ｘｉと長さＬ_１を衝突対応動作生成手段６比較し、式（３Ａ）の不等式の場合を接触パターン（１）、式（３Ｂ）の不等式の場合を接触パターン（２）、その他の場合を接触パターン（３）であると、衝突対応動作生成手段６により判断する。

次いで、ステップ２５では、衝突予定位置

で衝突対応姿勢をとるための、衝突対応動作目標軌道ｑ_ｄ（ｔ）が衝突対応動作目標軌道生成手段６において式（４）により生成される。

次いで、ステップ２６では、衝突対応姿勢において設定する関節部の機械インピーダンス値が、接触パターン（１）〜（３）に応じて、また、接近する人３８の移動速度に応じて、先に、関節部の機械インピーダンスを決定の仕方として述べたように、衝突対応動作目標軌道生成手段６によって設定される。

次いで、ステップ２７では、動作制御手段７において、目標軌道切換手段２６が動作し、衝突対応動作目標軌道ｑ_ｄ（ｔ）が選択され、位置制御系によりロボットアーム８が動作し、衝突対応姿勢となる。また、関節インピーダンス計算手段２８も動作し、ロボットアーム８の各関節部は設定した機械インピーダンス値に個別に制御される。

次いで、ステップ２８では、人運動検出手段２において、画像撮像装置３６の画像データより画像認識処理を行い、ロボットアーム８に接近する人３８の位置並びに移動速度及び移動方向の情報を検出する。

次いで、ステップ２９では、人３８の衝突警戒領域１００への侵入判断が人接近検知手段３で行なわれ、侵入有りと人接近検知手段３で判断された場合には、ステップ２７へ戻り、人３８の衝突警戒領域１００からの離脱が確認されない限り、ステップ２７→ステップ２８→ステップ２９→ステップ２７・・・のループが繰り返され、ロボットアーム８は衝突対応動作を継続する。

一方、ステップ２９において、人３８の衝突警戒領域１００への侵入無しと人接近検知手段３で判断された場合には、ステップ２１へと戻り、ロボットアーム８は通常の上記物体把持及び運搬作業動作へと復帰する。

以上の動作ステップ２１〜ステップ２９により、ロボットアーム８による上記物体把持及び運搬作業が実現し、人３８が接近した際には衝突対応動作への切換が実現する。

以上のように、人運動検出手段２、人接近検知手段３、衝突位置推定手段５、衝突対応動作制御手段４を備える事により、ロボットアーム８に人３８が接近したときに、人３８の運動に応じた適切な姿勢で動作し、適切な関節部の機械インピーダンスで、人３８と上記ロボットアーム８の衝突に対応する衝突対応動作を制御することが可能となる。

ロボットアーム８は根元部である台部１３にて床面に固定されているため、衝突予定位置より根元部（台部１３）側の関節部において、他の関節部よりも剛性が低く動きやすい状態の関節部が無いと、衝突に対する抗力が発生し、衝撃力が発生してしまうことになる。しかし、本発明の上記第１実施形態によれば、ロボットアーム８の各関節部ごとに個別に機械インピーダンスを設定するとき、他の関節部よりも根元（台部１３）側の関節部において剛性が低く動きやすい状態の関節部が設定されるため、その関節部で衝突時にかかる力を逃がすことができ、人に対する衝撃力は小さくなり、安全性を高くすることができる。

したがって、本発明の上記第１実施形態の制御装置１によれば、多関節型のロボットアーム８であっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存することができる安全なロボットアーム８の動作制御を実現できる。

なお、上記第１実施形態では、ロボットアーム８の関節部の数を３としたが、これに限られるわけではなく、２関節部以上のロボットアーム８において同様の効果を発揮する。

また、上記第１実施形態では、ロボットアーム８と人との衝突を例に説明を行ったが、これに限られるわけではなく、移動ロボットや移動中のワゴン等、その他の移動体とロボットアーム８との衝突でも同様の効果を発揮する。

なお、上記第１実施形態では、衝突警戒領域１００に人３８が進入した場合に、一度、衝突予定位置の推定を行うとしたが、これに限られるわけではなく、継続して衝突予定位置の推定を連続的に行う方法も可能である。この場合、衝突の推定精度が向上するため、より最適な姿勢、接触パターンで対応することができ、さらに安全性を向上することができる。一方、上記第１実施形態のように、一度、衝突予定位置の推定を行う場合は、計算量が少なくでき、ＣＰＵへの不可を軽減できるという利点がある。

（第２実施形態）
図８Ａ及び図８Ｂは本発明の第２実施形態におけるロボットアームの制御装置１の構成及び制御対象であるロボットアーム８の構成を示す図並びに上記第２実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図である。本発明の第２実施形態における制御装置１の基本的な構成は、図１に示した第１実施形態の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。

本発明の第２実施形態では、画像認識処理ではなく、ロボットアーム８と人３８が接触することにより発生する力を検出することにより、人運動検出及び人接近検知を行う。そのため、人運動検出手段２には、動作制御手段７のトルク推定手段２７から外力トルク情報が入力される構造となっている。従って、第２実施形態における制御装置１では画像撮像装置３６は不要となっている。

人運動検出手段２は、トルク推定手段２７が推定した外力トルクτ_ｅｘｔ＝［τ_１ｅｘ、τ_２ｅｘｔ、τ_３ｅｘｔ、τ_４ｅｘｔ］^Ｔを基に、以下のパターンにて、ロボットアーム８と人３８の相対的な位置関係を推定する。ただし、図９Ａ〜図９Ｄにおいて（ロボットアーム８を上方側から見下ろして）関節部を左回りに回そうとするトルクを正のトルクと定義する。

（１）第１関節部１４の外力トルクτ_１ｅｘｔ＞０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の場合：図９Ａのように、人３８の第１リンク１２での接触と、人運動検出手段２で推定する。

（２）第１関節部１４の外力トルクτ_１ｅｘｔ＜０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の場合：図９Ｂのように、人３８の第１リンク１２での接触と、人運動検出手段２で推定する。

（３）第１関節部１４の外力トルクτ_１ｅｘｔ＞０、第２関節部１５の外力トルクτ_２ｅｘｔ＞０の場合：図９Ｃのように、人３８の第２リンク１１での接触と、人運動検出手段２で推定する。

（４）第１関節部１４の外力トルクτ_１ｅｘｔ＜０、第２関節部１５の外力トルクτ_２ｅｘｔ＜０の場合：図９Ｄのように、人３８の第２リンク１１での接触と、人運動検出手段２で推定する。

以上の４パターンによる分類を行えば、ロボットアーム８と人３８の大まかな位置関係を知ることができる。また、人接近検知手段３は、第１関節部１４の外力トルクτ_１ｅｘｔ≒０、第２関節部１５の外力トルクτ_２ｅｘｔ≒０の状態から上記（１）〜（４）のパターンのいづれかに外力トルクが変化するのを検知することで、人３８の接近を検知することになる。

以上の方法により、人３８の概略位置を推定することで、衝突対応動作制御手段４で、位置関係に応じた姿勢、関節トルクを制御することにより、人３８と共存することができる安全なロボットアーム８の動作制御が可能となる。

なお、上記第１実施形態では、人運動検出手段２を画像認識処理によるとしたが、これに限られるわけではなく、その他のセンシング手段、例えば、レーザーレーダーセンサや、超音波センサ等、人３８の位置と速度が検出できるものであれば、同様の効果を発揮することができる。

また、上記第２実施形態では、トルク推定手段２７の推定した外力トルクを基に人検知を行うとしたが、これに限られるわけではなく、図１０に示すように、ロボットアーム８のリンク１２，１１など、人３８が接触する可能性のある部分に接触センサ３９を衝突位置取得手段の一例として設けて、接触センサ３９に人３８が接触することにより人検知を行うことでも、同様の効果を発揮することができる。また、接触センサと外力トルクを利用した人検知を併用すれば推定精度が向上し、さらに安全なロボットアーム８の制御を行なうことができる。

なお、上記第２実施形態では、ロボットアーム８の関節の数を３としたが、これに限られるわけではなく、２関節以上のロボットアーム８において同様の効果を発揮する。

また、上記第２実施形態では、ロボットアーム８と人との衝突を例に説明を行ったが、これに限られるわけではなく、移動ロボットや移動中のワゴン等、その他の移動体とロボットアーム８との衝突でも同様の効果を発揮する。

また、上記第１及び第２実施形態ではロボットアーム８を例に説明したが、アームに限らず、車輪により動く移動ロボットや、２足歩行ロボット、多足歩行ロボットなどの移動体でも、本体と人との接触に置いて本発明を適用することにより、本発明と同様の効果を発揮することができる。

なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明のロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムは、家庭用ロボットなど人と接する可能性があるロボットアームの動作の制御を行うロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラムとして有用である。また、家庭用ロボットに限らず、産業用ロボットや、生産設備等における可動機構の制御装置としても適用が可能である。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形や修正は明白である。そのような変形や修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。
図１は、本発明の第１実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す説明図である。 図２は、本発明の上記第１実施形態における上記ロボットアームの制御装置でのロボットアームの衝突警戒領域を説明する図である。 図３Ａは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図である。 図３Ｂは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図である。 図３Ｃは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図である。 図３Ｄは、上記第１実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図である。 図４は、上記第１実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図である。 図５は、上記第１実施形態における上記制御装置の動作制御手段での制御プログラムの動作ステップを表すフローチャートである。 図６は、上記第１実施形態における上記制御装置の全体的な動作ステップを表すフローチャートである。 図７は、本発明の上記第１実施形態における上記ロボットアームの制御装置の目標軌道を説明する図である。 図８Ａは、本発明の第２実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す図である。 図８Ｂは、上記第２実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図である。 図９Ａは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図である。 図９Ｂは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図である。 図９Ｃは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図である。 図９Ｄは、本発明の上記第２実施形態における上記ロボットアームの制御装置の動作を説明する図である。 図１０は、本発明の第２実施形態におけるロボットアームの制御装置の制御対象であるロボットアームの他の構成を示す図である。 図１１Ａは、特許文献２にかかる従来技術のロボット制御装置の概略構成図である。 図１１Ｂは、図１１Ａのロボット制御装置における接触回避動作の具体的な例を示す図である。 図１１Ｃは、図１１Ａのロボット制御装置における接触回避動作の上記具体的な例を示す図である。 図１１Ｄは、図１１Ａのロボット制御装置における接触回避動作の別の具体的な例を示す図である。 図１１Ｅは、図１１Ａのロボット制御装置における接触回避動作の上記別の具体的な例を示す図である。 図１２は、特許文献３にかかる従来技術のロボット制御装置のブロック線図である。

本発明の第１態様によれば、少なくとも２つのリンクと少なくとも２つの関節部とを有する多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突予定位置を取得する衝突位置推定手段として機能する衝突位置取得手段と、
上記衝突位置取得手段が推定した、上記人または移動体と上記多関節型ロボットアームの上記衝突予定位置に基づいて上記衝突予定位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を衝突前に行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置を提供する。

本発明の第１０態様によれば、少なくとも２つのリンクと少なくとも２つの関節部とを有する多関節型ロボットアームの制御方法であって、
上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近して上記ロボットアームの衝突警戒領域内に入ったか否かを接近検知手段で検知し、
上記接近検知手段により上記人または移動体が接近して上記ロボットアームの衝突警戒領域内に入ったことを検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの衝突予定位置を推定して上記衝突予定位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突予定位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を衝突前に行なうように制御するロボットアームの制御方法を提供する。

本発明の第１１態様によれば、上記多関節型ロボットアームと、
上記ロボットアームを制御する第１態様〜第９態様のいずれか１つに記載のロボットアームの制御装置とを有して、
上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により衝突前に行なうように制御することを特徴とするロボットを提供する。

本発明の第１２態様によれば、コンピューターを、
人または移動体と、少なくとも２つのリンクと少なくとも２つの関節部とを有する多関節型ロボットアームの衝突予定位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突予定位置に基づいて上記衝突予定位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を衝突前に行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラムを提供する。

本発明は、家庭用ロボット等、人との物理的な接触の可能性のあるロボットアームの制御装置に関する。

本発明の目的は、上記従来の制御装置の課題を解決し、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できるロボットアームの制御装置を提供することにある。

本発明の第１態様によれば、少なくとも２つのリンクと少なくとも２つの関節部とを有する多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有し、
上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御するロボットアームの制御装置を提供する。

本発明の第２態様によれば、少なくとも２つのリンクと少なくとも２つの関節部とを有する多関節型ロボットアームの制御装置であって、
人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有し、
上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクの上記根元部に最も近い側の関節部の剛性を他の関節部の剛性よりも低下させ、かつ、上記衝突位置の属する上記多関節型ロボットアームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御するロボットアームの制御装置を提供する。

本発明のロボットアームの制御装置によれば、衝突位置取得手段と衝突対応動作制御手段とを備えるか、又は、それらの機能を備えるようにしている。このため、ロボットアームに人が接近したときに、人の運動に応じた適切な姿勢で動作したり又は適切な関節部の機械インピーダンスで動作することが可能となる。言い換えれば、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて、上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることにより、根元部に近い側の関節部を動きやすくして、当該関節部で衝突時にかかる力を逃がすことができ、人に与える衝撃力が小さくなり、安全性を高くすることができる。よって、多関節型のロボットアームであっても安全に人と接触することができ、さらに、人の動きに応じた最適な接触動作が可能で、人へ接触のダメージを与えることなく人と共存できる安全なロボットアームの動作制御を実現できる。

Claims (11)

1. 多関節型ロボットアームの制御装置であって、
   人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
   上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段とを有するロボットアームの制御装置。
2. 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクの上記根元部に最も近い側の関節部の剛性を他の関節部の剛性よりも低下させる上記衝突対応動作を行なうように制御する請求項１に記載のロボットアームの制御装置。
3. 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御する請求項１または請求項２に記載のロボットアームの制御装置。
4. 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットアームの上記リンクより手首部に近い側の各関節部ごとに個別に関節角度を制御することで、上記衝突対応動作を行なうように制御する請求項１又は２に記載のロボットアームの制御装置。
5. 上記人または移動体の運動を検出する運動検出手段をさらに備え、
   上記衝突位置取得手段は、上記運動検出手段の検出結果に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を取得し、
   上記衝突対応動作制御手段は、上記衝突位置取得手段で推定した上記衝突位置に基づいて上記衝突対応動作を行なうように制御する請求項１又は２に記載のロボットアームの制御装置。
6. 上記運動検出手段の検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを検知する接近検知手段をさらに備え、
   上記衝突対応動作制御手段は、上記接近検知手段が上記人または移動体の接近を検知したときに、上記衝突対応動作を行なうように制御する請求項５に記載のロボットアームの制御装置。
7. 上記衝突対応動作制御手段は、上記運動検出手段の検出した上記人または移動体の運動の速度成分に基づいて、上記衝突対応動作を行なうように制御する請求項５または請求項６に記載のロボットアームの制御装置。
8. 上記運動検出手段は、上記人または移動体の位置と移動速度を検出して上記人の運動を検出する請求項５〜請求項７のいずれか１つに記載のロボットアームの制御装置。
9. 多関節型ロボットアームの制御方法であって、
   上記ロボットアームに接近する人または移動体の運動を運動検出手段で検出し、
   上記運動検出手段で検出された上記人または移動体の運動検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを接近検知手段で検知し、
   上記接近検知手段により上記人または移動体の接近を検知したときに、上記運動検出手段で検出した上記人または移動体の上記運動に基づいて上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を衝突位置取得手段で取得するとともに、衝突対応動作制御手段により、上記衝突位置取得手段で取得した上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させることで、上記人と上記ロボットアームの衝突に対応する衝突対応動作を行なうように制御するロボットアームの制御方法。
10. 上記多関節型ロボットアームと、
    上記ロボットアームを制御する請求項１〜請求項８のいずれか１つに記載のロボットアームの制御装置とを有して、
    上記人または移動体と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御するロボット。
11. コンピューターを、
    人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、
    上記衝突位置取得手段が取得した、上記人または移動体と上記多関節型ロボットアームの上記衝突位置に基づいて上記衝突位置の属する上記多関節型ロボットアームのリンクより根元部に近い側の関節部の剛性を他の関節部の剛性よりも低下させる衝突対応動作を行なうように制御する衝突対応動作制御手段として機能させるためのロボットアームの制御プログラム。

Images