(57)【特許請求の範囲】
【請求項１】
３つ以上の関節からなる基本軸と３つの関節からなる姿勢軸とを備えたロボットに対し空間上の直交座標系における位置および姿勢として与えられた目標制御点について、逆運動学変換を行い前記ロボットの各軸の関節角度を導出して前記ロボットの位置を制御する方法において、
前記目標制御点での前記ロボットの姿勢について、前記目標制御点を通るベクトルＶ回りに冗長性が与えられた場合に、
前記基本軸によって決定される前記ロボット上の所定の点を、前記目標制御点に対し前記所定の点が取り得る位置集合のうち前記ロボットの原点から最も近い点と一致させ、
前記姿勢軸によって前記ロボットの先端部の位置を前記目標制御点の位置と一致させることを特徴とする産業用ロボットの位置制御方法。
【請求項２】
マトリックスoＴcpとして与えられた前記目標制御点の位置および姿勢に対して前記基本軸および前記姿勢軸の関節角度を決定する手順として、
（１）前記ロボットが前記目標制御点oＴcpへ動作する際、前記基本軸により決定されるマトリックスo Ｔb の直交成分Ｐbを求めるステップと、
（２）前記Ｐbが取り得る位置の集合である円Ｃの中心Ｐvを原点とし、前記Ｖの単位ベクトルをＺ方向とし、前記Ｐvから前記Ｐbへの単位ベクトルをＸ方向とするマトリックスoＴvを求めるステップと、
（３）前記円Ｃ上の点のうち、前記ロボット原点から最も近い点をＰb’とし、前記Ｐvから前記Ｐb’ へ向かうベクトルと前記oＴvのＸ方向とのなす角度θを求めるステップと、
（４）前記oＴcpを前記Ｖ回りに前記θ回転させたマトリックスoＴcp’を求めるステップと、
（５）前記oＴcp’ に対して逆運動学変換を行って導出した前記ロボットの各軸の関節角度を動作目標とするステップと、
を備えることを特徴とする請求項１記載の産業用ロボットの位置制御方法。
【請求項３】
３つ以上の関節からなる基本軸と３つの関節からなる姿勢軸とを備えたロボットに対し空間上の直交座標系における位置および姿勢として与えられた目標制御点について、逆運動学変換を行い前記ロボットの各軸の関節角度を導出して前記ロボットの位置を制御する装置において、
前記目標制御点でのロボットの姿勢について、前記目標制御点を通るベクトルＶ回りに冗長性が与えられた場合に、
前記基本軸によって決定される前記ロボット上の所定の点を、前記目標制御点に対し前記所定の点が取り得る位置集合のうち前記ロボットの原点から最も近い点と一致させ、
前記姿勢軸によって前記ロボットの先端部の位置を前記目標制御点の位置と一致させることを特徴とする産業用ロボットの位置制御装置。
【請求項４】
マトリックスoＴcpとして与えられた前記目標制御点の位置および姿勢に対して前記基本軸および前記姿勢軸の関節角度を決定する手順として、
（１）前記ロボットが前記目標制御点oＴcpへ動作する際、前記基本軸により決定されるマトリックスo Ｔb の直交成分Ｐbを求めるステップと、
（２）前記Ｐbが取り得る位置の集合である円Ｃの中心Ｐvを原点とし、前記Ｖの単位ベクトルをＺ方向とし、前記Ｐvから前記Ｐbへの単位ベクトルをＸ方向とするマトリックスoＴvを求めるステップと、
（３）前記円Ｃ上の点のうち、前記ロボット原点から最も近い点をＰb’とし、前記Ｐvから前記Ｐb’ へ向かうベクトルと前記oＴvのＸ方向とのなす角度θを求めるステップと、
（４）前記oＴcpを前記Ｖ回りに前記θ回転させたマトリックスoＴcp’を求めるステップと、
（５）前記oＴcp’ に対して逆運動学変換を行って導出した前記ロボットの各軸の関節角度を動作目標とするステップと、
を実行することを特徴とする請求項３記載の産業用ロボットの位置制御装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、一般に産業用ロボットの位置制御に関するものであり、特に産業用ロボットにて作業する場合に対象物の円筒物体の把持方向や溶接トーチ方向など、その方向回りの姿勢に冗長性がある場合の産業用ロボットの位置制御に関するものである。
【０００２】
【従来の技術】
従来、産業用ロボットで作業を行う場合、目標の直交空間上の位置と姿勢は確定しておく必要がある。円筒物体の把持方向や溶接トーチ方向などの回りに冗長な軸がある場合は、有利な近接可能状態に基づいて６軸の座標を確定する。しかしそれでも作業対象物の位置、状態が他の作業対象物と変わっているとその作業対象物にはロボットの腕が届かなくなり、作業の中断を招くことがあった。このような事態をなくすため冗長軸回りに姿勢を調整して作業対象物に届くロボットの姿勢を有利に人手によって決定しようとすると、そのための教示作業は煩瑣となる。これを解決しようとして、コンピュータシュミレーションにより届く姿勢を捜す作業を自動化することが提案され、実施されている。これは冗長軸回りの全探索手法であって、時間（数秒から数十秒）を要するという不都合がある。
【０００３】
【発明が解決しようとする課題】
本発明の目的は、そのような冗長性を有する作業対象物に対してロボットの腕が届く範囲が最大限となるようにする産業用ロボットの位置制御方法および位置制御装置を提供することである。
【０００４】
【課題を解決するための手段】
この課題を解決するには３つの基本軸で定まるロボットの位置、姿勢を、それの腕を最大限に延ばせる状態に設定することによってなされる。
【０００５】
すなわち、３つ以上の関節からなる基本軸と３つの関節からなる姿勢軸とを備えたロボットに対し空間上の直交座標系における位置および姿勢として与えられた目標制御点について、逆運動学変換を行い前記ロボットの各軸の関節角度を導出して前記ロボットの位置を制御する方法において、前記目標制御点での前記ロボットの姿勢について、前記目標制御点を通るベクトルＶ回りに冗長性が与えられた場合に、前記基本軸によって決定される前記ロボット上の所定の点を、前記目標制御点に対し前記所定の点が取り得る位置集合のうち前記ロボットの原点から最も近い点と一致させ、前記姿勢軸によって前記ロボットの先端部の位置を前記目標制御点の位置と一致させる。
より具体的には、マトリックスoＴcpとして与えられた前記目標制御点の位置および姿勢に対して前記基本軸および前記姿勢軸の関節角度を決定する手順として、前記ロボットが前記目標制御点oＴcpへ動作する際、前記基本軸により決定されるマトリックスo Ｔb の直交成分Ｐbを求め、前記Ｐbが取り得る位置の集合である円Ｃの中心Ｐvを原点とし、前記Ｖの単位ベクトルをＺ方向とし、前記Ｐvから前記Ｐbへの単位ベクトルをＸ方向とするマトリックスoＴvを求め、前記円Ｃ上の点のうち、前記ロボット原点から最も近い点をＰb’とし、前記Ｐvから前記Ｐb’ へ向かうベクトルと前記oＴvのＸ方向とのなす角度θを求め、前記oＴcpを前記Ｖ回りに前記θ回転させたマトリックスoＴcp’を求め、前記oＴcp’ に対して逆運動学変換を行って導出した前記ロボットの各軸の関節角度を動作目標とする。
【０００６】
【作用】
本発明により初期設定されたロボットは、標準状態から外れた作業対象物を検知すると３つの基本軸の作動によって腕を最大限延ばせるようにロボットの基本姿勢を変更し、その残りの３つの軸の作動によってその作業対象物に対処できる。また、本発明により冗長軸回りの全探索は不要となって初期設定に要する時間は実質的に短縮される。
【０００７】
作業に当たっては作業中にロボットの腕が届かなくなって作業を中断するという事態は回避される。
【０００８】
【発明の実施の形態】
添付図を参照して本発明の実施の形態を説明する。
【０００９】
図１は本発明の位置制御方法を適用できる産業用ロボットの典型例を示し、図２はこの産業用ロボットの機構構成を示している。このロボットはＲ−Ｐ−Ｐ−Ｒ−Ｐ−Ｒの一般的ロボットであり、ｊ１からｊ６軸の自由度がある。図３はこの機構構成と座標系の関係を示す。作業対象物はベクトルＶの回りならどの方向から掴んでもよいとし、ベクトルＶを冗長軸方向とする。基本軸（ｊ１−ｊ３）により決定されるマトリックスをoＴb として oＴbの直交成分をＰbとする。姿勢軸（ｊ４−ｊ６）により決定されるマトリックス（位置要素は０）をbＴwとし、bＴw を平行移動して工具取り付けフランジを決定するマトリックスをwＴf とし、フランジから制御点へのエンドエフェクタ （ツール）マトリックスをfＴeとし、目標点マトリックスをoＴcpとする。
【００１０】
尚、図ではツールはグリッパとしている。本発明の処理を行うことによりoＴcpを求め直し、oＴcp’とし、oＴcp’を逆運動学変換して各軸の関節角度を求める。図４にoＴcpを制御点としたロボットの座標系と円Ｃの座標系関連図を、図５に新たな、oＴcp’を求める処理に関連した座標系説明図を、図６にoＴcp’を求める処理フローを示す。以下に、その処理を詳述する。尚、この処理の前に既知なデータはoＴcp、Ｖ、wＴf、 fＴeである。
【００１１】
▲１▼   指定されたoＴcpへ動作する場合のＰbを以下のように求める。
【００１２】
oＴcp＝oＴbbＴwwＴf fＴe である。またbＴw は末知であるが、直交位置要素は０であるため
oＴbbＴwの直交要素がＰbである。このため
oＴbbＴw＝oＴcp fＴe−１wＴf−１                          …１）
によりＰbを導出できる。
【００１３】
▲２▼   Ｐbの取り得る円Ｃのマトリックスとして直交要素が円の中心位置Ｐv姿勢行列のｘ方向がＰvＰbの単位ベクトル、ｚ方向がＶの単位ベクトルであるマトリックス０Ｔvを求める。
【００１４】
２−１）Ｐb[Ｐbx，Ｐby，Ｐbz]からベクトルＶへ垂線を降ろしＰv[Ｐｖx，Ｐｖy，Ｐｖz]とする。
【００１５】
２−２）ベクトルＰvＰbの単位ベクトルをＶxとする。
【００１６】
Ｖx＝ＰvＰb /  ｜ＰvＰb｜              …２）
２−３）Ｖの単位ベクトルをＶz とする。
【００１７】
Ｖz＝Ｖ/｜Ｖ｜                      …３）
２−４）ＶzとＶxの外積をＶyとする。    Ｖy＝Ｖz×Ｖx
２−５）Ｐvを原点として、Ｖx，Ｖy，Ｖzを姿勢行列要素としたマトリックスをoＴvとする。
【００１８】
▲３▼   ロボット原点から円Ｃへ最も近い点をＰb’とし、Ｐb’へ基本軸が動作できるoＴcp’を求める。oＴcp’はoＴcpと同じ直交位置で姿勢行列要素が異なる。
【００１９】
３−１）ロボット原点から円Ｃを含む平面へ垂線を降ろした点を求める。oＴｖから見たロボット原点Ｔoの座標vＴoを求める。
【００２０】
vＴo＝oＴv −１                            …４）
求めたvＴoのx、yの項はそのままでz方向成分が０の点が円Ｃを含む平面へ垂線を降ろした点のoＴv から見た座標値Ｐ１である。
【００２１】
Ｐ１[Ｐ１x，Ｐ１y，Ｐ１z]＝ [vＴoのx成分、vＴoのy成分、０]    …５）
３−２）oＴｖの直交位置点からＰ１へ向かう単位ベクトルをＶx’とする
Ｖx’＝Ｐ１/｜Ｐ１｜                  …６）
３−２）ＶxからＶx’への角度θを求める。
【００２２】
θ＝tan-1（Ｐ１y/Ｐ１x）                …７）
３−３）oＴcpをＶz回りにθだけ回転させたマトリックスをoＴcp’として求める。
【００２３】
oＴcp’＝oＴcpRot（Ｖx,θ）            …８）
以上がoＴcp’を求める処理である。
【００２４】
oＴcp’はoＴcpをＶz回りにθだけ回転させたマトリックスであるが、wＴf fＴe は固定値であり、bＴw は直交位置要素が０のマトリックスであるためoＴｂ’の直交位置要素Ｐb’は円Ｃ上にＰbをＶz回りにθだけ回転させた位置となりロボット原点から最も近く且つ、円Ｃ上の点となる。
【００２５】
oＴcp’を求めた後は oＴcp’に対して従来の逆運動学変換を行い、各軸の関節角度を導出し、その位置への位置制御を行う。
【００２６】
【発明の効果】
本発明により冗長軸回りの全探索は不要となって初期設定に要する時間は実質的に短縮される。また、作業中にロボットの腕が届かなくなって作業を中断するという事態は回避される。
【図面の簡単な説明】
【図１】産業用ロボットの斜視図。
【図２】図１の産業用ロボットの機構構成図。
【図３】図２の機構構成と座標系の関係図。
【図４】oＴcpを制御点としたロボットの座標系と円Ｃの座標関連図。
【図５】新たなoＴcp’を求める処理に関連した座標系説明図。
【図６】実施例としての逆運動学処理フロー。
【符号の説明】
oＴcp ：目標制御点
oＴb：基本軸３軸で決定される位置マトリックス
bＴw：手首軸３軸でとる姿勢マトリックス
wＴf ：工具の取付けフランジマトリックス
fＴe ：工具のエンドエフェクタマトリックス
Ｖ：冗長軸方向
【図１】
0004600796.tif 000002
【図２】
0004600796.tif 000003
【図３】
0004600796.tif 000004
【図４】
0004600796.tif 000005
【図５】
0004600796.tif 000006
【図６】
0004600796.tif 000007