WO2008004487A1

WO2008004487A1 - Apparatus and method for controlling robot arm, robot, and robot arm control program

Info

Publication number: WO2008004487A1
Application number: PCT/JP2007/062984
Authority: WO
Inventors: Yasunao Okazaki
Original assignee: Panasonic Corp; Matsushita Electric Industrial Co Ltd
Current assignee: Panasonic Corp; Panasonic Holdings Corp
Priority date: 2006-07-04
Filing date: 2007-06-28
Publication date: 2008-01-10
Anticipated expiration: 2009-01-04
Also published as: JPWO2008004487A1; CN101432103B; CN101432103A; JP4243309B2; US8676379B2; JP2008302496A; US20090171505A1

Description

明細書

ロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラム

技術分野

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

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

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

[0004] また、特許文献 2には、図 11Aに示すように、障害物が存在する環境とロボット 513 との接触部およびその接触力をそれぞれ検知するセンサ機構 515と、移動機構 511 およびマニピュレータ 512の位置姿勢をそれぞれ計測する位置姿勢計測装置 514と、前記接触力と位置姿勢とからロボット 513の接触力回避動作を計算する計算機 51 7と、この計算機 517の計算結果によって移動機構 511とマニピュレータ 512とを駆動させるァクチユエータ 516とを有する制御装置が開示されており、障害物が存在する環境とロボット 513との接触時には、移動機構 511とマニピュレータ 512との両者の連携動作により回避動作を行うようにして、る。

[0005] また、特許文献 3には、図 12に示すように、エンドエフヱクタ 411を有するロボット 40 1に作用したことをカ覚センサ 412で検出した外部環境からの力に対して、リアルタイムに外部環境の剛性係数を推定して、モータ 413aの制御によりロボット 401のァーム 410を駆動制御するインピーダンス制御装置が開示されている。 [0006] 特許文献 1 :特開平 10— 329071号公報

特許文献 2：特開 2005— 59161号公報

特許文献 3：特開 2004 - 223663号公報

発明の開示

発明が解決しょうとする課題

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

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

[0009] また、特許文献 2では、図 11B及び図 11Cに示すように、ロボット 513における、障害物が存在する環境との接触部 532を、接触力 531の方向に接触回避経路 526沿いに移動させることで、回避動作 527を行うことで、環境力もの接触力 531を小さくし、最終的に接触がなくなるようにしている。また、図 11D及び図 11Eに示すように、口ボット 513における、障害物が存在する環境との接触部 532の接触面を、接触力 53 1により発生するモーメント 533の方向に傾けるとともに、前記接触部を、任意の大きさの円 526を描くように移動させることで、回避動作 527を行うことで、環境からの接触力 531を小さくし、最終的に接触がなくなるようにしている。しかしながら、それぞれの回避動作 527の具体的な動作制御にっ、ては、何ら開示はされてヽな、。

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

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

課題を解決するための手段

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

[0013] 本発明の第 1態様によれば、多関節型ロボットアームの制御装置であって、

人または移動体と上記多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、

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

[0014] 本発明の第 9態様によれば、多関節型ロボットアームの制御方法であって、

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

[0015] 本発明の第 10態様によれば、上記多関節型ロボットアームと、

上記ロボットアームを制御する第 1態様〜第 8態様のいずれか 1つに記載のロボットアームの制御装置とを有して、

上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。 [0016] 本発明の第 11態様によれば、コンピューターを、

人または移動体と多関節型ロボットアームの衝突位置を取得する衝突位置取得手段と、

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

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

図面の簡単な説明

[0018] 本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、

[図 1]図 1は、本発明の第 1実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す説明図であり、

[図 2]図 2は、本発明の上記第 1実施形態における上記ロボットアームの制御装置でのロボットアームの衝突警戒領域を説明する図であり、

[図 3A]図 3Aは、上記第 1実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、

[図 3B]図 3Bは、上記第 1実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、

[図 3C]図 3Cは、上記第 1実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、

[図 3D]図 3Dは、上記第 1実施形態における上記制御装置の人運動検出手段及び衝突位置推定手段の機能を説明する平面図であり、

[図 4]図 4は、上記第 1実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図であり、

[図 5]図 5は、上記第 1実施形態における上記制御装置の動作制御手段での制御プログラムの動作ステップを表すフローチャートであり、

[図 6]図 6は、上記第 1実施形態における上記制御装置の全体的な動作ステップを表すフローチャートであり、

[図 7]図 7は、本発明の上記第 1実施形態における上記ロボットアームの制御装置の目標軌道を説明する図であり、

圆 8A]図 8Aは、本発明の第 2実施形態におけるロボットアームの制御装置の構成及び制御対象であるロボットアームの構成を示す図であり、

[図 8B]図 8Bは、上記第 2実施形態における上記制御装置の動作制御手段の詳細構成を示すブロック線図であり、

[図 9A]図 9Aは、本発明の上記第 2実施形態における上記ロボットアームの制御装置の動作を説明する図であり、

[図 9B]図 9Bは、本発明の上記第 2実施形態における上記ロボットアームの制御装置の動作を説明する図であり、

[図 9C]図 9Cは、本発明の上記第 2実施形態における上記ロボットアームの制御装置の動作を説明する図であり、

[図 9D]図 9Dは、本発明の上記第 2実施形態における上記ロボットアームの制御装置の動作を説明する図であり、

[図 10]図 10は、本発明の第 2実施形態におけるロボットアームの制御装置の制御対象であるロボットアームの他の構成を示す図であり、

[図 11A]図 11Aは、特許文献 2にかかる従来技術のロボット制御装置の概略構成図であり、

[図 11B]図 11Bは、図 11Aのロボット制御装置における接触回避動作の具体的な例を示す図であり、

[図 11C]図 11Cは、図 11Aのロボット制御装置における接触回避動作の上記具体的な例を示す図であり、

[図 11D]図 11Dは、図 11Aのロボット制御装置における接触回避動作の別の具体的な例を示す図であり、

[図 11E]図 11Eは、図 11Aのロボット制御装置における接触回避動作の上記別の具体的な例を示す図であり、

[図 12]図 12は、特許文献 3にかかる従来技術のロボット制御装置のブロック線図である。

発明を実施するための最良の形態

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

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

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

[0022] 本発明の第 1態様によれば、多関節型ロボットアームの制御装置であって、

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

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

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

[0026] 本発明の第 5態様によれば、上記人または移動体の運動を検出する運動検出手段をさらに備え、

上記衝突位置取得手段は、上記運動検出手段の検出結果に基づ!、て上記人または移動体と上記多関節型ロボットアームの上記衝突位置を推定して上記衝突位置を取得し、

上記衝突対応動作制御手段は、上記衝突位置取得手段で推定した上記衝突位置に基づいて上記衝突対応動作を行なうように制御する第 1態様又は第 2態様に記載のロボットアームの制御装置を提供する。

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

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

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

[0030] 本発明の第 9態様によれば、多関節型ロボットアームの制御方法であって、

[0031] 本発明の第 10態様によれば、上記多関節型ロボットアームと、

上記人と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御することを特徴とするロボットを提供する。

本発明の第 11態様によれば、コンピューターを、

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

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

[0033] (第 1実施形態）

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

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

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

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

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

[0038] 衝突対応動作制御手段 4は、衝突対応動作生成手段 6、及び動作制御手段 7とより構成される。

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

[0040] 動作制御手段 7は、ロボットアーム 8の手先の位置姿勢を制御し、目標とする作業をロボットアーム 8の手先で実現すると共に、人 38が接近した場合には、衝突対応動作生成手段 6が生成した関節目標軌道に追従するように制御を切り換えると同時に、衝突対応動作生成手段 6が生成した関節目標の機械インピーダンス値になるように口ボットアーム 8の各関節部の機械インピーダンスの値を制御するインピーダンス制御を行う。ここで、ロボットアーム 8の各関節部は、後述するように、 DZ Aボード 21及び AZDボード 22に接続されたモータドライバ 19を介して、各関節部の駆動装置の一例としてのモータ 34力インピーダンス制御の下に駆動されてそれぞれ屈曲動作が行われるようになつている。

[0041] 制御装置 1は、ハードウェア的には一般的なパーソナルコンピュータにより構成されており、人運動検出手段 2、人接近検知手段 3、衝突位置推定手段 5、衝突対応動作生成手段 6、及び、動作制御手段 7のうちの入出力 IF (入出力インターフェース） 2 0を除く部分は、パーソナルコンピュータで実行可能な制御プログラム 18としてソフトゥア的に実現される。

[0042] 入出力 IF20は、パーソナルコンピュータの PCIバスなどの拡張スロットルに接続された、 DZAボード 21、 AZDボード 22、カウンタボード 23により構成される。

[0043] ロボットアーム 8の動作を制御するための制御プログラム 18が実行されることにより制御装置 1が機能しており、ロボットアーム 8の各関節部のモータ 34に連結されたェンコーダ 35より出力される関節角度情報がカウンタボード 23を通じて制御装置 1に取り込まれ、制御装置 1によって各関節部の回転動作での制御指令値が算出される。算出された各制御指令値は、 DZAボード 21を通じてモータドライバ 19に与えられ、モータドライバ 19から送られた各制御指令値に従って、ロボットアーム 8の各関節部のモータ 34が駆動される。

[0044] ロボットアーム 8は、 4自由度の多リンクマニピュレータであり、ハンド 9と、ハンド 9が取付けられる手首部 10を先端に有する第 2リンク 11と、第 2リンク 11が回転可能に連結される第 1リンク 12と、第 1リンク 12が回転可能に連結支持される台部 13とを有している。手首部 10は、第 3関節部 16及び第 4関節部 17の 2つの回転軸（上下方向沿 V、の回転軸と横方向沿、の回転軸）を有しており、第 2リンク 11に対するハンド 9の相対的な姿勢（向き)を変化させることができる。第 2リンク 11の他端は第 1リンク 12の先端に対して第 2関節部 15の上下方向沿いの回転軸周りに回転可能とし、第 1リンク 1 2の他端は台部 13に対して第 1関節部 14の上下方向沿いの回転軸周りに回転可能として、合計 4個の軸周りに回転可能として上記 4自由度の多リンクマニピュレータを構成している。

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

[0046] 32は、台部 13の下側固定部に対して相対的な位置関係が固定された絶対座標系であり、その原点を Oとする。 33は、ハンド 9に対して相対的な位置関係が固定され

0

た手先座標系である。絶対座標系 32から見た手先座標系 33の原点位置 Oe (x、 y) をロボットアーム 8の手先位置、絶対座標系 32から見た手先座標系 33の姿勢（ φ、 0 )をロボットアーム 8の手先姿勢とし、手先位置'姿勢ベクトル rを r= [x、 y、 φ、 0 ]^τ と定義する。ロボットアーム 8の手先位置及び姿勢を制御する場合には、手先位置' 姿勢ベクトル _rを目標手先位置 ·姿勢ベクトル r に追従させるように制御装置 1で制 wd

御すること〖こなる。 [0047] 次に、制御装置 1を構成する各機能ブロックについて、詳細構成と動作について説明する。

[0048] 人運動検出手段 2には、画像撮像装置 36で撮影した画像データが入力され、人運動検出手段 2での画像認識処理により、ロボットアーム 8が配置された部屋の天井から床を見下ろしたときの人 38の 2次元位置 X (X , y )及び人 38の移動速度ベクトル

P P P

V (V , V )が検出され、 2次元位置 X (X , y )及び移動速度ベクトル V (V , v )の情報が人運動検出手段 2から人接近検知手段 3及び衝突位置推定手段 5へ出力される。

[0049] 人接近検知手段 3は、図 2に示す衝突警戒領域 100を定義し、人 38がこの領域 10 0に入ったかどうかで、人 38がロボットアーム 8に接近したかどうかの検知判断を行な

[0050] 衝突警戒領域 100の基端側の扇形領域 100Aは、第 1リンク 12の長さ Lの oc倍、すなわち a Lを半径とし、第 1関節軸 (第 1関節部 14の回転軸)を要に、第 1関節部 14の関節角 qの、例えば、第 1リンク 12の中心軸を中心とした ± |8 ° の角度の領域とする。また、衝突警戒領域 100の先端側の扇形領域 100Bは、第 2リンク 11の長さ Lとハンド 9の長さ L (D o 倍、すなわち a (L +L )を半径とし、第 2関節軸 (第 2関

2 Η 2 2 2 Η

節部 15の回転軸）を要に、第 2関節部 15の関節角 qの、例えば、第 2リンク 11の中

2

心軸を中心とした ± j8

2 ° の角度の領域とする。このようにすれば、速度に比例して扇形領域の範囲を変化させても、ハンド 9の先が、先端側の扇形領域 100Bの外に出るのを確実に防止できる。

[0051] 衝突警戒領域 100を定義する α 、 a 、 β 、 β の値は、任意の値であるが、例え

1 2 1 2

ばロボットアーム 8を動作させる手先速度 V ={(dx/dt)²+(dy/dt)²}^1/2を基にこれらの値を決めることができ、手先速度 Vに比例して α 、 a 、 β 、 β の値を大きくするように決 r 1 2 1 2

めてやれば、ロボットアーム 8の動作速度が大きい場合には衝突警戒領域 100の面積が大きくなり、安全性を向上させることができる。

[0052] この第 1実施形態では、例えば、手先速度 Vの単位を m/sとするとともに、 Vは Vの r K r 大きさによって Vに比例する値又は定数となるものとして、 = 1. 2V 、ひ = 1. 2V r 1 K 2 ヽ β 二 24V 、 β = 24Vとする。ただし、 Vは、 [数 1]

V _r > 0 . 5 [m/ s ] の場合、 V = 1. 9Vで定義される値 (変数)であり、 V < 0. 5[mZs]の場合、 V = 0

. 95と定数として定義される値 (変数)である。

[0053] このようにすれば手先速度が 0. 5[mZs]以上の時には手先速度の大きさに比例して衝突警戒領域 100の大きさが決まり、また、 0. 5[mZs]未満の時には α = 1. 14 、 a = 1. 14、 β = 22. 8、 β = 22. 8と固定値になり、手先速度が小さい場合でも

、ある程度の大きさの衝突警戒領域 100が確保される。

[0054] 人接近検知手段 3は、人運動検出手段 2の検出した人 38の位置の情報と、動作制御手段 7から得られるロボットアーム 8の関節角度情報より、人 38とロボットアーム 8の相対関係を算出し、人 38が衝突警戒領域 100に入れば、衝突対応動作制御手段 4 に人侵入を通知する。

[0055] 衝突位置推定手段 5は、人運動検出手段 2の検出した人 38の位置の情報及び移動速度ベクトルの情報から人 38との衝突予定位置

[数 2]

Χ _μ ( Λ' ,. , y , )

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

[0056] 例えば、図 3Αに示すように、速度ベクトル V (V , V )の延長線と垂直に交わり、口ボットアーム 8の第 1関節部 14の回転軸を通る直線 Aを考える。直線 Aと絶対座標系 32の X軸がなす角度を q とし、速度ベクトル V (V , V )の延長線と直線 Aとの交点を

、衝突予定位置

[数 3]

X κ X i , y i )

とすると、角度 q は以下の式で与えられ、

画

X _P ( y , ) は以下の式（1)〜（3)で与えられる。

[0057] [数 5]

•式（ 1 )

[数 6]

{ l2-q )x_p+y_p

- - · ，式（2)

tan a -g_u +π/2

[数 7] y, =x, tan^, . . . , 式（ 3 )

[0058] 次に、図 3Α〜図 3Cを使い、衝突対応動作生成手段 6の詳細について説明する。

[0059] 衝突対応動作生成手段 6は、図 3Aの通常作業時の状態 (現在状態)力図 3Bの衝突受け止め姿勢への関節目標軌道を生成する。

[0060] ロボットアーム 8の第 1リンク 12の中心軸の線が衝突予定位置

[数 8]

Χ_μ ( Λ' ,. , y , )

に重なるよう第 1関節部 14の角度 q を衝突対応動作生成手段 6により決定する。す li

なわち、 q =atan2(y, x)である。ただし、 atan2(y, x) =arg(y +jx)であり、 jは虚数、 argは複素数の偏角である。また、第 2関節部 15の角度 q は、人 38をロボット

2i

アーム 8が囲うようにするために、例えば 60° という固定値とする。

[0061] 衝突対応姿勢時のロボットアーム 8の第 1関節部 14の角度は qとする。また、第 2関

li

節部 15の角度 q に関しては、衝突予定位置

2i

[数 9]

X X y

と絶対座標系 32の原点 O間の距離 L に応じて、以下の接触パターン（1)〜（3)の 3

0 xi

パターンで変化させるとする。

[0062] 図 3Bに示す接触パターン（1):

[数 10] L_x i < L ! X 0 . 8 式（ 3 A ) の場合、 q =90° とする。

2i

[0063] 図 3Cに示す接触パターン（2):

[数 11]

L_XX 0. 8く

LjX l. 2 式（ 3 B ) の場合、 q ={(L — L )ZO.4XL }X90° とする。

2i xi 1 1

[0064] 図 3Dに示す接触パターン（3): L XI.2<L の場合、 q =0° とする。

1 xi 2i

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

[0066] 上記接触パターン（1)の場合、図 3Bに示すように、人 38との接触は第 1リンク 12で発生し、第 2リンク 11は第 1リンク 12に対して 90° 折れ曲がるような姿勢になるため、ロボットアーム 8は人 38を囲うような姿勢となる。

[0067] 上記接触パターン（2)の場合、図 3Cに示すように、人 38との接触は第 2関節部 15 付近で発生し、人 38との接触位置に応じて、第 2関節部 15の開き角度 q が変化す

2i

る姿勢になる。

[0068] 上記接触パターン（3)の場合、図 3Dに示すように、人 38との接触は第 2リンク 11で発生し、第 2関節部 15の角度 q は 0° となり、ロボットアーム 8は第 1リンク 12と第 2リ

2i

ンク 11がー直線上にある姿勢となる。

[0069] このようなインピーダンス制御動作において、ロボットアーム 8の関節角度の現在値

(q、q ) (q 、q )

1 2から、接触動作時の関節角度 li 2iへの目標軌道である関節角度目標値 q (t)は、以下の 5次多項式補間により衝突対応動作生成手段 6で計算して求め nd

る。ここで、 tは時間である。

[0070] [数 12]

^nd (0 = ^αϋ ^{+ a}x^l + a,t² + a + ⁴ + a_s · ■ ' '式（4) ただし,

[数 13] ^ao - Ί,, 式（ 5 )

[数 14]

α = q_n . . . . 式 ( 6 ) [数 15] a₂ =—a

² · · · · 式 m

[数 16] a^{? =} It^^"' ~^20q" " ¹²^/ "³¾²) · ■ · ' 式（8 ) [数 17] a₄ . . . . 式（9 )

[数 18]

"_{5 =} 12¾— ό¾ .—^/, ) . . . · 式（ 1 0) [数 19] η = I , 2 , · · · ·式（ 1 1 )

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

f

[0071] 衝突対応動作生成手段 6は、関節角度目標値として、式 (4)で計算される関節角度目標値 q (t)を動作制御手段 7へと出力する。

nd

[0072] また、衝突対応動作生成手段 6は、接触パターン（1)〜（3)及び衝突時の人 38の速度成分に応じて、図 3B〜図 3Dの衝突対応姿勢時の各関節部の機械インピーダンス値を決定する。機械インピーダンス値の設定パラメータとしては、慣性 I、粘性 D、岡 IJ'性 Kがある。

[0073] 接触パターン（1)及び（2)の場合:接触時の人 38の速度の第 1及び第 2リンク 12, 11に垂直な成分 V 、v に応じて、各関節部の機械インピーダンスパラメータを、 I = K /v 、 D =K ZV 、 K =K ZV により決定する。ただし、 n= l、 2である。また

I in n D in n K in

、K、K、Kは定数ゲインであり、具体的数値は安全な動作となるように実験的に求

1 D K

める。また、各パラメータの上限値 Imax、 Dmax、 Kmaxを定め、計算値がこれらの上限値を超える場合、各パラメータは上限値に等しいとする。

[0074] 接触パターン（3)の場合:第 1関節部 14に関しては I =Imax、 D =Dmax、 K

=Kmaxとし、第 2関節部 15に関しては、接触時の人 38の速度の第 2リンク 11に垂直な成分 V に応じて、第 2関節部 15の機械インピーダンスパラメータを、 I =K /v

i2 2 I i2

、 D

2 =κ D Zv 、

i2 κ 2 =κ Κ Ζν により決定する。

i2

[0075] 手首部 10の機械インピーダンスに関しては、常に、上限値 I =Imax、 D =Dmax

、 K =Kmax、ただし、 (n= 3, 4)とする。

[0076] 上限値は十分大きな値とし、例えば、 Imax =Dmax =Kmax = 10000、ただし

、 (n= l, 2, 3, 4)とする。

[0077] 上記のように関節部の機械インピーダンスを決定すれば、接触パターン（1)及び（2

)では、例えば、人 38が第 1リンク 12にほぼ垂直に衝突する場合、 V =0であるので

i2

、第 1関節部 14では、インピーダンス制御動作前よりも機械インピーダンス値力、さく設定されて第 1関節部 14が柔軟になり、第 2関節部 15及び第 3関節部 16及び第 4関節部 17では、インピーダンス制御動作前よりも機械インピーダンス値が上限値に設定されて第 2関節部 15及び第 3関節部 16及び第 4関節部 17が固くなる。すなわち、第 1リンク 12の衝突位置の属する第 1リンク 12よりも根元部（台部 13)に近い側である第 1関節部 14の剛性を、他の関節部例えば第 2関節部 15及び第 3関節部 16及び第 4関節部 17の剛性よりも低下させる衝突対応動作を衝突対応動作制御手段 4により行なうことができる。したがって、第 1関節部 14の柔軟性により、衝突の衝撃が緩和されると共に、第 2関節部 15が固いことで、衝突の反動で第 2リンク 11が不用意に動いて第 2リンク 11が人 38に衝突してしまうこともなぐ安全性が確保される。また、第 1リンク 12と第 2リンク 11で L字形を構成して人 38を囲うようにすることにより、人 38の前側及び横側の 2方向での支えとして働くことができて、人 38が転倒するのを防ぎ (人 3 8が転倒しかけたときに第 1リンク 12又は第 2リンク 11に人 38が捉まることにより、人 3 8が転倒するのを防ぐことができて）、より安全性を高めることができる。 [0078] また、接触パターン（2)において、第 2リンク 11に垂直な速度成分 Vがある場合で i2

も、速度成分 V に応じて第 2関節部 15の剛性が低く設定されて第 2関節部 15が柔ら i2

力べなることになるので、安全性を確保することができる。

[0079] また、接触パターン（3)では、インピーダンス制御動作前よりも第 1関節部 14が固く、人 38の移動速度の第 2リンク 11に垂直な成分 vi2に応じて、衝突予定位置の属する第 2リンク 11の衝突予定位置よりも根元部（台部 13)に最も近い側の関節部である第 2関節部 15がインピーダンス制御動作前よりも柔らかく設定されるため、人 38が第 2リンク 11に衝突する衝撃を、第 2関節部 15の柔軟性で吸収できるため、安全性を確保することができる。また、上記とは逆に、第 1関節部 14を柔らかぐ第 2関節部 15 を固く設定する場合に比べて、衝突時にかかる慣性は第 2リンク 11の慣性が主であり、第 1リンク 12の慣性は力からないため、衝突時の衝撃を小さくすることができる。

[0080] なお、接触パターン (3)では、第 1関節部 14を柔らかぐ第 2関節部 15を固く設定する場合の他に、第 1関節部 14と第 2関節部 15を共に、他の第 3関節部 16及び第 4 関節部 17よりも柔らかぐすなわち、衝突予定位置の属する第 2リンク 11より根元部（台部 13)に近い側の全ての関節部を、それ以外の関節部よりも柔ら力べする方法も考えられる。

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

[0082] 次に、図 4を使い、動作制御手段 7の詳細について説明する。

[0083] 図 4において、 8は制御対象である図 1に示したロボットアームである。ロボットァーム 8からは、それぞれの関節軸のエンコーダ 35により計測された関節角の現在値（関節角度ベクトル) q= [q , q , q , q ]^Tが出力され、カウンタボード 23により制御装置

1 2 3 4

に取り込まれる。ただし、 q , q , q , qは、それぞれ、第 1関節部 14、第 2関節部 15 、第 3関節部 16、第 4関節部 17の関節角度である。

[0084] 24は作業目標軌道生成手段であり、目標とするロボットアーム 8の作業を実現するための手先位置'姿勢目標ベクトル r が出力される。図 7に示すように、目標とする口 wd

ボットアーム 8の動作は、目的とする作業に応じて、ティーチングによるポイントごとの位置 (r 、r 、r · · ·)が事前に与えられており、目標軌道生成手段 24は多項式 wdO wdl wd2

補間を使用し、各ポイントごとの位置間の軌道を補完し、手先位置'姿勢目標べタトル r を生成する。ただし、上記位置 r は時間 t=0のときの位置、 r は時間 t=t wd wdO wdl 1 のときの位置、 r は時間 t=tのときの位置と仮定する。

wd2 2

[0085] 目標軌道生成手段 24から逆運動学計算手段 25に入力された手先位置 ·姿勢目標ベクトル r は、逆運動学計算手段 25により関節目標ベクトル q へと変換される。

wd wa

[0086] 26は目標軌道切換手段であり、目標軌道切換手段 26には、逆運動学計算手段 2 5から関節目標ベクトル q 力衝突対応動作生成手段 6から衝突対応動作関節目標 wd

ベクトル q 力人接近検知手段 3から衝突対応動作切換指令がそれぞれ入力されて id

いる。通常は、作業目標軌道生成手段 24により生成された手先位置 ·姿勢目標べクトル r が逆運動学計算手段 25により変換された関節目標ベクトル q を目標軌道切 wd wd 換手段 26は選択し、関節目標ベクトル qとして目標軌道切換手段 26から出力する。

d

しかしながら、人接近時に人接近検知手段 3から衝突対応動作切換指令を目標軌道切換手段 26が受け取ると、衝突対応動作生成手段 6から入力された接触動作関節目標ベクトル qを目標軌道切換手段 26は選択し、関節目標ベクトル qとして目標軌 id d

道切換手段 26から出力する。

[0087] 27はトルク推定手段であり、人 38とロボットアーム 8の接触によってロボットアーム 8 の各関節部に加わる外力トルクを推定する。トルク推定手段 27には、モータドライバ 19の電流センサで計測された、ロボットアーム 8の各関節部を駆動するモータ 34を流れる電流値 i= [i , i , i , i ]^Tが AZDボード 22を介して取り込まれ、また、関節角

1 2 3 4

の現在値 q及び、後述する関節角度誤差補償出力 u が関節誤差補償手段 30から qe

取り込まれる。トルク推定手段 27は、オブザーバ一として機能し、以上の電流値 i、関節角の現在値 q、関節角度誤差補償出力 u よりロボットアーム 8に加わる外力により qe

、各関節部に発生するトルク τ = [ τ 、 τ 、 τ 、 τ ]^Τを算出する。

ext lex 2ext 3ext [0088] 関節インピーダンス計算手段 28は、ロボットアーム 8に機械インピーダンスを実現する機能を果たす部分であり、人 38が接近していない通常動作の際は 0を関節インピ一ダンス計算手段 28から位置制御系 31の入力側に出力する。一方、人 38が接近し、衝突対応動作生成手段 6から接触動作切換指令を受け取ると、衝突対応動作生成手段 6で設定された機械インピーダンスパラメータ I、 D、 Kと、関節角の現在値 qと、トルク推定手段 27が推定した外力トルク τ より、ロボットアーム 8の各関節部ごとに機

ext

械インピーダンスを実現するための関節目標補正出力 q を以下の式（12)により関節インピーダンス計算手段 28で計算し、位置制御系 31の入力側に出力する。関節目標補正出力 q は、目標軌道切換手段 26の出力する関節目標 qに位置制御系 3

1の入力側で加算され、補正関節目標ベクトル q が生成されて位置制御系 31に入力される。

[0089] [数 20] q = (5²I + 5D + K) 式（ 1 2 ) ただし、

[数 21]

0 0 0

0 1₂ 0 0

式（ 1 3

0 0 0

0 0 0 h

[数 22]

1 0 0

0 D, 0

D 式（ 1 4 )

0 0

[数 23] κ、 0 0 0

0 κ₂ 0 0

0 0 0

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

[0090] 30は位置制御系 31内の関節誤差補償手段であり、ロボットアーム 8において計測される関節角度ベクトルの現在値 qと、関節補正目標ベクトル q との

dm 誤差 q

eが関節誤差補償手段 30に入力され、関節誤差補償手段 30から関節誤差補償出力 u がロボ qe ットアーム 8に向けて出力される。

[0091] 関節誤差補償出力 u は、 DZAボード 21を介してモータドライバ 19に電圧指令値 qe

として与えられ、各関節軸が正逆回転駆動されてロボットアーム 8が動作する。

[0092] なお、上記動作制御手段 7は、上記したように、目標軌道生成手段 24と逆運動学計算手段 25と目標軌道切換手段 26とトルク推定手段 27と関節インピーダンス計算手段 28と位置制御系 31とより構成されて、る。

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

[0094] 動作の基本は、関節誤差補償手段 30による関節ベクトル (関節角の現在値) qのフイードバック制御 (位置制御）であり、図 4の点線で囲まれた部分が位置制御系 31になっている。関節誤差補償手段 30として、例えば、 PID補償器を使用すれば、関節誤差 q力 SOに収束するように制御が働き、目標とするロボットアーム 8の動作が実現す e

る。

[0095] 人 38の衝突警戒領域 100への侵入を人接近検知手段 3により検知した場合、上記説明した位置制御系 31に対し、関節インピーダンス計算手段 28により関節目標補正出力 q 1S 目標軌道切換手段 26の出力する関節目標 qに位置制御系 31の入 ά Δ d

力側で加算され、関節部の目標値の補正が行われる。このために、上記した位置制御系 31は、関節部の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。関節目標補正出力 q は式（12)により関節インピーダンス計算手段 28で算出されるため、慣性 I、粘性 D、剛性 Kの機械インピーダンスが実現される。 [0096] 以上の原理に基づく動作制御手段 7の動作を実現する制御プログラムの実際の動作ステップについて、図 5のフローチャートに基づいて説明する。

[0097] ステップ 1では、エンコーダ 35により計測された関節角度データ（関節変数ベクトル又は関節角度ベクトル q)が制御装置 1に取り込まれる。

[0098] 次いで、ステップ 2では、制御装置 1のメモリ（図示せず）に予め記憶されていたロボットアーム 8の動作制御プログラム 18に基づき、動作制御手段 7の作業目標軌道生成手段 24によりロボットアーム 8の手先位置'姿勢目標ベクトル r が計算される。

wd

[0099] 次いで、ステップ 3では、手先位置 ·姿勢目標ベクトル r 1S 逆運動学計算手段 25

wd

において関節目標 q に変換される。

wd

[0100] 次いで、ステップ 4では、目標軌道切換手段 26により目標軌道の切換が行われる。

人 38の衝突警戒領域 100への侵入がない場合には、作業動作を実行するため、ステツプ 5Aへ進む。一方、人 38の衝突警戒領域 100への侵入があった場合には、衝突対応動作を実行するためステップ 5Bへ処理を進める（目標軌道切換手段 26での処理)。

[0101] ステップ 5Bでは、関節目標ベクトル qを、衝突対応動作生成手段 6が生成した接

d

触動作関節目標ベクトル q とする（目標軌道切換手段 26での処理)。その後、ステツ

id

プ 6に進む。

[0102] ステップ 5Aでは、関節目標ベクトル qを関節目標 q とする。 (目標軌道切換手段 2

d wd

6での処理)。その後、ステップ 8に進む。

[0103] ステップ 6では、それぞれのモータ 34の駆動電流値 iと、関節角度データ（関節角度ベクトル（関節角の現在値) q)と、関節角度誤差補償出力 u から、ロボットアーム 8の

qe

関節部における外力トルクて力トルク推定手段 27により計算される（トルク推定手

ext

段 27での処理)。

[0104] 次、で、ステップ 7では、衝突対応動作生成手段 6にお、て設定された機械インピ一ダンスパラメータ I、 D、 Kと、関節角度データ（関節角度ベクトル q)と、トルク推定手段 27により計算されたロボットアーム 8に加わる外力トルク τ から、関節目標補正出

ext

力 q 力関節インピーダンス計算手段 28により計算される（関節インピーダンス計算手段 28での処理)。 [0105] 次いで、ステップ 8では、関節目標ベクトル qと関節目標補正出力 q の和として補 d ά Δ

正関節目標ベクトル q が計算されて位置制御系 31に入力される。

dm

[0106] 次いで、ステップ 9では、補正関節目標ベクトル q と現在の関節ベクトル qとの差で dm

ある関節部の誤差 qが関節誤差補償手段 30に入力され、関節角度誤差保証出力 u

e

が関節誤差補償手段 30で計算される（関節誤差補償手段 30での処理)。関節誤 qe

差補償手段 30の具体例としては PID補償器が考えられる。定数の対角行列である比例、微分、積分の 3つのゲインを適切に調整することにより、関節誤差が 0に収束するように制御が働く。

[0107] 次いで、ステップ 10では、関節角度誤差補償出力 u 力 ¾ZAボード 21を通じ、モ qe

ータドライバ 19に与えられ、それぞれの関節部のモータ 34を流れる電流量を変化させることによりロボットアーム 8のそれぞれの関節軸の回転運動が発生する。

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

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

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

[0111] 次いで、ステップ 22では、人 38の衝突警戒領域 100への侵入判断が人接近検知手段 3により行なわれ、侵入無しと人接近検知手段 3で判断された場合には、ステツプ 23へ進む。

[0112] ステップ 23では、ロボットアーム 8は動作制御手段 7により動作制御されて、ロボットアーム 8のハンド 9で物体を把持し、運搬する作業動作を行なう。

[0113] ステップ 23の後はステップ 21に戻り、人接近検知手段 3で人 38の侵入が確認されない限り、ステップ 21→ステップ 22→ステップ 23→ステップ 21 · · ·のループが繰り返され、ロボットアーム 8は通常の上記物体把持及び運搬作業を実行する。

[0114] ステップ 22において人 38の侵入が確認された場合、ステップ 24へ進み、衝突位置推定手段 5により衝突予定位置

[数 24]

X _P "'い y , )

が推定される。

[0115] 次いで、ステップ 24. 1では、衝突予定位置 Xと絶対座標系 32の原点 O間の距離

P 0

L を衝突対応動作生成手段 6算出し、距離 L と長さ Lを衝突対応動作生成手段 6

1

比較し、式 (3A)の不等式の場合を接触パターン（1)、式 (3B)の不等式の場合を接触パターン (2)、その他の場合を接触パターン (3)であると、衝突対応動作生成手段 6により判断する。

[0116] 次!、で、ステップ 25では、衝突予定位置

[数 25]

X _P { X i , y , ) で衝突対応姿勢をとるための、衝突対応動作目標軌道 q (t)が衝突対応動作目標軌道生成手段 6において式 (4)により生成される。

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

[0118] 次いで、ステップ 27では、動作制御手段 7において、目標軌道切換手段 26が動作し、衝突対応動作目標軌道 q (t)が選択され、位置制御系によりロボットアーム 8が動作し、衝突対応姿勢となる。また、関節インピーダンス計算手段 28も動作し、ロボットアーム 8の各関節部は設定した機械インピーダンス値に個別に制御される。

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

[0120] 次ヽで、ステップ 29では、人 38の衝突警戒領域 100への侵入判断が人接近検知手段 3で行なわれ、侵入有りと人接近検知手段 3で判断された場合には、ステップ 27 へ戻り、人 38の衝突警戒領域 100からの離脱が確認されない限り、ステップ 27→ステツプ 28→ステップ 29→ステップ 27· · ·のループが繰り返され、ロボットアーム 8は衝突対応動作を継続する。

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

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

[0123] 以上のように、人運動検出手段 2、人接近検知手段 3、衝突位置推定手段 5、衝突対応動作制御手段 4を備える事により、ロボットアーム 8に人 38が接近したときに、人

38の運動に応じた適切な姿勢で動作し、適切な関節部の機械インピーダンスで、人

38と上記ロボットアーム 8の衝突に対応する衝突対応動作を制御することが可能となる。

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

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

[0126] なお、上記第 1実施形態では、ロボットアーム 8の関節部の数を 3とした力これに限られるわけではなぐ 2関節部以上のロボットアーム 8において同様の効果を発揮する [0127] また、上記第 1実施形態では、ロボットアーム 8と人との衝突を例に説明を行ったが、これに限られるわけではなぐ移動ロボットや移動中のワゴン等、その他の移動体とロボットアーム 8との衝突でも同様の効果を発揮する。

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

[0129] (第 2実施形態）

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

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

[0131] 人運動検出手段 2は、トルク推定手段 27が推定した外力トルク τ = [ τ 、 τ ext lex 2ext

、 τ 、 τ ]^Tを基に、以下のパターンにて、ロボットアーム 8と人 38の相対的な位

3ext 4ext

置関係を推定する。ただし、図 9A〜図 9Dにおいて（ロボットアーム 8を上方側から見下ろして）関節部を左回りに回そうとするトルクを正のトルクと定義する。

[0132] (1)第 1関節部 14の外力トルク τ >0、第 2関節部 15の外力トルク τ Oの場 lext 2ext 合：図 9Aのように、人 38の第 1リンク 12での接触と、人運動検出手段 2で推定する。

[0133] (2)第 1関節部 14の外力トルク τ く 0、第 2関節部 15の外力トルク τ Oの場合：図 9Bのように、人 38の第 1リンク 12での接触と、人運動検出手段 2で推定する。

[0134] (3)第 1関節部 14の外力トルク τ >0、第 2関節部 15の外力トルク τ >0の場

lext 2ext 合：図 9Cのように、人 38の第 2リンク 11での接触と、人運動検出手段 2で推定する。

[0135] (4)第 1関節部 14の外力トルク τ く 0、第 2関節部 15の外力トルク τ く 0の場

lext 2ext 合：図 9Dのように、人 38の第 2リンク 11での接触と、人運動検出手段 2で推定する。

[0136] 以上の 4パターンによる分類を行えば、ロボットアーム 8と人 38の大まかな位置関係を知ることができる。また、人接近検知手段 3は、第 1関節部 14の外力トルク τ

lext

、第 2関節部 15の外力トルク τ 0の状態から上記（1)〜 (4)のパターンのいづ

2ext

れかに外力トルクが変化するのを検知することで、人 38の接近を検知することになる

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

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

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

[0140] なお、上記第 2実施形態では、ロボットアーム 8の関節の数を 3とした力これに限られるわけではなぐ 2関節以上のロボットアーム 8において同様の効果を発揮する。

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

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

[0143] なお、上記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより

、それぞれの有する効果を奏するようにすることができる。

産業上の利用可能性

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

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

Claims

請求の範囲

[1] 多関節型ロボットアームの制御装置であって、

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

[2] 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットァームのリンクの上記根元部に最も近い側の関節部の剛性を他の関節部の剛性よりも低下させる上記衝突対応動作を行なうように制御する請求項 1に記載のロボットアームの制御装置。

[3] 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットァームのリンクより手首部に近い側の関節部の剛性を維持またはさらなる剛性を与えることで、上記衝突対応動作を行なうように制御する請求項 1または請求項 2に記載の口ボットアームの制御装置。

[4] 上記衝突対応動作制御手段は、上記衝突位置の属する上記多関節型ロボットァームの上記リンクより手首部に近い側の各関節部ごとに個別に関節角度を制御することで、上記衝突対応動作を行なうように制御する請求項 1又は 2に記載のロボットァームの制御装置。

[5] 上記人または移動体の運動を検出する運動検出手段をさらに備え、

上記衝突対応動作制御手段は、上記衝突位置取得手段で推定した上記衝突位置に基づ!/、て上記衝突対応動作を行なうように制御する請求項 1又は 2に記載のロボットアームの制御装置。

[6] 上記運動検出手段の検出結果に基づいて上記ロボットアームに上記人または移動体が接近したことを検知する接近検知手段をさらに備え、

上記衝突対応動作制御手段は、上記接近検知手段が上記人または移動体の接近を検知したときに、上記衝突対応動作を行なうように制御する請求項 5に記載のロボットアームの制御装置。

[7] 上記衝突対応動作制御手段は、上記運動検出手段の検出した上記人または移動体の運動の速度成分に基づ、て、上記衝突対応動作を行なうように制御する請求項

5または請求項 6に記載のロボットアームの制御装置。

[8] 上記運動検出手段は、上記人または移動体の位置と移動速度を検出して上記人の運動を検出する請求項 5〜請求項 7のいずれか 1つに記載のロボットアームの制御装置。

[9] 多関節型ロボットアームの制御方法であって、

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

[10] 上記多関節型ロボットアームと、

上記ロボットアームを制御する請求項 1〜請求項 8のいずれか 1つに記載のロボットアームの制御装置とを有して、

上記人または移動体と上記ロボットアームの衝突に対応する上記衝突対応動作を上記衝突対応動作制御手段により行なうように制御するロボット。コンピューターを、

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