JP3886842B2

JP3886842B2 - Robot control device

Info

Publication number: JP3886842B2
Application number: JP2002123472A
Authority: JP
Inventors: 敬仁酒井
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2002-04-25
Filing date: 2002-04-25
Publication date: 2007-02-28
Anticipated expiration: 2022-04-25
Also published as: JP2003311664A

Description

【０００１】
【発明の属する技術分野】
本発明は、多関節型のロボットが特異点近傍を通過するロボットの制御装置の改良に関する。
【０００２】
【従来の技術】
ロボットの制御点の軌跡が直線を描くように動作させる直線補間動作は、動作開始位置と目標位置の位置と姿勢の情報（以下、直交データという）をロボットの制御装置に予め記憶しておき、それら２つの位置の間を制御点が直線を描くようにロボットの制御周期毎に直交データを算出し、その直交データから座標変換により各関節軸の角度（以下、関節データという）を求め、該関節データに一致するように各軸のモータを制御することにより実現している。
【０００３】
多関節型のロボットにおいては、直交データから関節データに座標変換する際、一意に関節データを求めることのできない特異点と呼ばれる位置が存在し、特異点又は特異点の近傍を通過する場合には、特定の関節軸において非常に大きな速度で回転しなければならないことが知られている。
そこで、補間動作中において、常に関節軸の速度指令を算出し、速度指令の増加率からロボットの制御点が特異点に近付いたか否かを判定し、近付いたと判断した場合、特異点近傍を通過できるような補間手段に処理を切り替えたり、特異点からの距離に応じて速度指令を補正して特異点近傍を通過させたりする手段が提案されている(特開平５−３２４０４４号公報)。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記のような技術的な手段は、補間動作中において、常に特異点近傍に対する判断処理、速度の補正処理が実行されている。このため、元々、特異点近傍を通過しない補間動作に対しては無駄な処理を実行しているので、補間動作の処理に時間が占有されることにより演算時間が増加する。したがって、迅速な制御が制約されるという問題点があった。
【０００５】
本発明は、上記の課題を解決するためになされたもので、補間動作の実行前に特異点近傍を通過するか否かを判断し、通過する場合のみ補間動作において特異点近傍通過のための処理を実行するロボットの制御装置を提供することを目的とするものである。
【０００６】
【課題を解決するための手段】
第１の発明に係るロボットの制御装置は、複数の関節軸を有する水平多関節型のロボットの制御装置において、前記ロボットの制御点を教示することにより、前記制御点の開始位置と目標位置を記憶する記憶手段と、前記ロボットが動作する前に、前記記憶手段から前記開始位置及び前記目標位置を読み出して、前記開始位置と前記目標位置とを結ぶ直線を求める演算手段と、前記ロボットの特異点の周りに設定された速度調整領域を、前記直線が通過するか否かを判断する第１の判断手段と、前記速度調整領域を前記直線が通過する場合、前記制御点から前記特異点までの距離に基づいて速度調整領域の通過を可能とする調整速度指令としての第１の速度指令を生成する第１の速度指令生成手段とを備えたことを特徴とするものである。
かかるロボットの制御装置によれば、直線補間動作を行う前に、第１の判断手段によりロボットの制御点が速度調整領域を通過するかどうか判断し、通過する場合には、制御点から特異点までの距離に基づいて第１の速度指令によりロボットの制御点が補間動作する。したがって、特異点近傍の速度調整領域を元々制御点が通過しない場合、特異点近傍を通過するための無駄な処理がロボットの動作中に実行されないので、演算の処理が円滑になるという効果がある。
【０００７】
第２の発明に係るロボットの制御装置は、記憶手段から読み出した前記開始位置、目標位置に基づいて制御点を所定の速度で補間する第２の速度指令を生成する第２の速度指令生成手段と、第１の判断手段により直線が通過すると判断することにより、制御点の補間動作を実行しながら第１の速度指令と第２の速度指令を比較して低い方の速度指令を選択する選択手段とを備えたことを特徴とするものである。
かかるロボットの制御装置によれば、選択手段が第１の速度指令と第２の速度指令を比較して低い方の速度指令を選択して制御点を補間動作させる。したがって、ロボットの制御点がより円滑に速度調整領域を通過できるという効果がある。
【０００８】
第３の発明に係るロボットの制御装置における第１の速度指令生成手段は、前記特異点から前記制御点までの距離をＲとし、特異点の影響を受け速度調整の対象となる所定の関節軸の最高回転速度をΔθ_1maxとし、前記直線補間の動作方向と水平方向の成す角をφとすると、前記第１の速度指令Ｖrcが下式となる
Ｖrc＝Ｒ・Δθ_1max／cosφ
ことを特徴とするものである。
かかるロボットの制御装置によれば、水平方向と併せて垂直方向の移動量も考慮するので、制御点が速度調整領域で水平方向に対して垂直方向に大きく動作する場合、水平方向のみ調整速度指令に対して、制御点の速度を適切に低下して速度調整領域を円滑に動作できるという効果がある。
【０００９】
第４の発明に係るロボットの制御装置は、特異点の周りに設定されると共に、前記速度調整領域よりも狭い特異点領域を有し、前記ロボットが動作する前に、前記直線が該特異点領域を通過するか否かを判断する第２の判断手段と、前記第２の判断手段によって前記直線が特異点領域を通過すると判断された場合に、警報を発生する警報手段とを備えたことを特徴とするものである。
ロボットの制御点が特異点領域を通過すると判断した場合は、通常の直線補間では動作できないので、動作開始前に警告を発する。かかる警告に基づいて、例えば、制御点の開始位置、目標位置を変更したり、直線補間を関節補間に変更したりすることにより制御点が特異点を通過しないようにすることができるという効果がある。
【００１０】
【発明の実施の形態】
実施の形態１．
本発明の一実施の形態を図１乃至図９によって説明する。図１において、多関節型のロボット１は、基柱３に設けられた上下方向に移動可能な直同軸Ｊ０と、回転可能な第１の関節Ｊ1と、回転可能な第２の関節Ｊ2と、第１の関節Ｊ1と第２の関節Ｊ2を連結させるアーム５と、先端に制御点Ｃpを有すると共に、第２の関節Ｊ2に連結されたアーム５と長さが等しいアーム７とから成っている。
【００１１】
図１に示すような、アーム５とアーム７の長さが等しい３自由度の水平多関節型ロボットでは、図２のようにアーム５とアーム７が折り重なった位置が特異点一つとなることが知られており、図３に示すように、状態１から状態７の制御点Ｃpにおける動作速度を一定に保ち、図２の特異点を通過するように直線補間の動作を実行しようとすると、状態４から状態５に移動する際、第１の関節Ｊ１は瞬時に１８０度回転しなければならない。このように制御点Ｃpが特異点近傍を通過しようとすると不具合が生じるので、かかる不具合を除去する補間処理の実行手段を以下に説明する。
【００１２】
図４において、補間処理手段は、教示により補間種類等を記憶した指令部１０と、制御点Ｃpの補間動作が特異点近傍の特異点領域Ａs又は、特異点領域Ａsよりも広い特異点速度調整領域Ａcを通過するか否かを判断する速度調整判断部１４と、補間処理を成す補間処理部１２と、速度調整判断部１４が特異点領域Ａsを通過すると判断することにより警報を発生する警報部１７と、算出された補間位置を各関節Ｊ0〜Ｊ2の位置指令に変換する座標変換部２６と、座標変換部２６からの指令に基づいて各関節Ｊ0〜Ｊ2のモータの位置を制御するサーボ制御部２８とを備えている。
【００１３】
指令部１０は、補間指令を記憶した記憶手段としてのＲＡＭから成っており、補間の種類を記憶した補間種類領域１０ａと、制御点Ｃpの動作すべき開始位置を記憶した開始位置領域１０ｂと、制御点Ｃpの目標位置を記憶した目標位置領域１０ｃとを有している。
補間処理部１２には、指令部１０からの補間指令に基づいて所望の第２の速度指令Ｖcを生成する第２の速度指令生成手段としての速度指令部１３と、特異点領域Ａs又は速度調整領域Ａcを通過するか否かを判断する第１及び第２の判断手段としての速度調整判断部１４と、速度調整領域Ａc、特異点領域Ａs、調整速度指令Ｖrcを求める演算式を記憶した速度調整生成部１６と、速度調整領域Ａcを通過すると判断した場合、速度調整領域Ａcの通過を可能とする第１の速度指令としての調整速度指令Ｖrcを生成する第１の速度指令生成手段としての速度調整指令部１８と、速度調整領域Ａcを通過すると判断すると、制御点Ｃpの補間動作において、調整速度指令Ｖrcと速度指令Ｖcとを比較してＶrc＜Ｖcであれば、切換スイッチ２２をａ端子側に投入し、Ｖrc≧Ｖcであれば、切換スイッチ２２をｂ端子側に投入する信号を発生する速度指令決定部１９と、速度指令Ｖc又は調整速度指令Ｖrcに基づいて補間位置を生成する補間位置生成部２４とを備えている。なお、速度指令決定部１９及び切換スイッチ２２により選択手段を成している。
【００１４】
速度調整生成部１６は、特異点から円形の領域であると共に、特異点領域Ａsよりも広い速度調整領域Ａcを記憶した速度調整領域１６aと、特異点から円形の領域となる特異点領域Ａsを記憶した特異点領域１６bと、制御点Ｃpが速度調整領域Ａcを通過する場合に調整速度指令Ｖrcを求める後述する所定の演算式が記憶された速度調整演算式領域１６ｃとを有するＲＡＭから成っている。
ここで、速度調整領域Ａcは、図１に示すロボットをＸ軸に沿って直線補間で水平動作させた場合、ロボットの制御点Ｃpが特異点の１つである第１の関節Ｊ₁軸の位置に最も近付くのは、図５に示すように制御点ＣpがＹ軸上に達したときである。この達した瞬間における制御点Ｃpの速度を速度指令Ｖcとする。
ここで、図５に示すように第１の関節Ｊ_１を中心とした半径Rの円を考え、第１の関節Ｊ１軸から距離Ｒ離れた位置において、第１の関節Ｊ_１軸を回転することにより発生可能な最高速度Ｖrmax(ｍ／sec)は、第１の関節Ｊ_１軸の最高回転速度をΔθ_１ｍａｘ(rad／sec)とすると、下式となる。なお、Ｙ軸上では最高速度Ｖrmaxと速度指令Ｖcの方向が一致する。
Ｖrmax=Ｒ・Δθ_１ｍａｘ・・・・・・(１)
速度指令Ｖcに対する円形の速度調整領域Ａcを、特異点を中心に定め、その半径Ｒcは、(１)式を変形して下式となる。
Ｒc =Ｖc／Δθ_１ｍａｘ・・・・(２)
簡単のために、速度指令Ｖcを、ロボットの最高指令速度Ｖcmaxとした固定した値として上記(２)式に代入すると、半径Ｒcは下式となる。
Ｒc=Ｖcmax／Δθ_１ｍａｘ・・・・(３)
また、速度調整領域Ａcの内側に設定する特異点領域Ａsの半径Ｒsは、下記の関係がある。
0＜Ｒs＜Ｒc ・・・・(４)
この(４)式を満足する範囲で、半径Ｒsを大きな値にすると、ロボットの動作範囲が制限されるので、所定の範囲に設定される。半径Ｒsの値は、例えば２ｍｍである。
【００１５】
速度調整指令部１８は、制御点Ｃpの軌跡として開始位置と目標位置の軌跡が速度調整領域Ａcを通過する場合、調整速度指令Ｖrcを発生する。
また、図６に示すように制御点Ｃpを斜めに直線補間動作する場合は、制御点Ｃpの軌跡に沿って速度指令Ｖcが生成される。速度指令Ｖcは、水平方向の動作速度Ｖhに対するものであるので、直線補間の動作方向と水平方向のなす角をφとすると、下式となる。
Ｖh =Ｖc・cosφ ・・・・(５)
この(５)式より、調整速度指令Ｖrcとすると、調整速度指令Ｖrcは、下式となり特異点からの制御点Ｃpまでの半径Ｒに比例する。
Ｖrc＝Ｒ・Δθ_１ｍａｘ／cosφ ・・・・(６)
【００１６】
速度調整判断部１４は、特異点領域Ａs、速度調整領域Ａcを通過する補間か否かを判断するもので、図７に示すように開始位置Ｐ1s〜Ｐ5sと目標位置Ｐ1t〜Ｐ5tをそれぞれ結んだ直線Ｌ1〜Ｌ5を求め(演算手段）、直線Ｌ1〜Ｌ5が速度調整領域Ａcを通過するか否かを判断し、速度調整領域Ａcを通過しないと判断した場合、切換スイッチ２２をｂ端子側に投入する。
【００１７】
教示された制御点Ｃpの開始位置と目標位置のＸ座標とＹ座標は任意の値を取るため、開始位置と目標位置を結んだ直線が速度調整領域Ａcを通過するか否か判断しにくい。したがって、図７に示すようにＸ座標方向の移動量ΔＸとＹ座標方向の移動量ΔＹとの傾きαを求め、傾きαを用い、次の式により直線Ｌ1，〜Ｌ5を原点周りに−α回転させたＸ´，Ｙ´を下式により求めて特異点領域Ａs、速度調整領域Ａcを通過するか否かを判断する。
【００１８】
【数１】

【００１９】
このような座標変換することにより動作開始位置Ｐ1s´，Ｐ2s´，Ｐ3s´，Ｐ4s´，Ｐ5s´と目標位置Ｐ1t´，Ｐ2t´，Ｐ3t´，Ｐ4t´，Ｐ5t´をそれぞれ結んだ直線Ｌ1´，Ｌ2´，Ｌ3´，Ｌ4´，Ｌ5´が図８のようになる。
【００２０】
速度調整判断部１４は、直線Ｌ1´〜Ｌ5´が速度調整領域Ａc、特異点領域Ａsを通過するか否かを以下のようにして判断する。
▲１▼．直線Ｌ1´のＸ座標値Ｘ1S，Ｘ1tが速度調整領域Ａcの半径Ｒcよりも大きい場合、すなわち、Ｘ1S，Ｘ1t＞Ｒcの場合には、速度調整領域Ａcを通過しないと判断する。
▲２▼．直線Ｌ2´のＹ座標値Ｙ2s，Ｙ2tの積、すなわち、Ｙ2s・Ｙ2t≦0であれば、直線Ｌ2´がＸ軸を横切ると判断し、Ｘ座標値Ｘ2s，Ｘ2t＜Ｒcの場合には、速度調整領域Ａcを通過すると判断する。
同様に、直線Ｌ3´のＹ座標値Ｙ3s，Ｙ3tの積、すなわち、Ｙ3s・Ｙ3t≦0であれば、直線Ｌ3´がＸ軸を横切りと判断し、Ｘ座標値Ｘ3s，Ｘ3t＜Ｒｓの場合には、特異点領域Ａsを通過すると判断する。
▲３▼．直線Ｌｘ´の座標値Ｘxs´，Ｙｘs´，Ｘｘt´，Ｙｘt´が速度調整領域Ａcに存在する場合、すなわち、Ｌc1≦Ｒc 又はＬc2≦Ｒcの場合には、直線Ｌｘ´が速度調整領域Ａcを通過すると判断する。例えば、直線Ｌ4´が該当する。
ここに、Ｌc1＝(Ｘxs´^２＋Ｙxs´^２)^１ ^/ ^２
Ｌc2＝(Ｘxt´^２＋Ｙｘt´^２)^１ ^/ ^２
また、直線Ｌｘ´の座標値Ｘxs´，Ｙｘs´，Ｘｘt´，Ｙｘt´が特異領域Ａsに存在する場合、すなわち、Ｌc1≦Ｒs 又はＬc2≦Ｒsの場合には、直線Ｌｘ´が特異点領域Ａsを通過すると判断する。例えば、直線Ｌ5´が該当する。
【００２１】
上記のように構成されたロボットの制御装置の動作を図１乃至図９によって説明する。まず、作業者は、補間種類として直線補間を補間種類領域１０ａに記憶し、制御点Ｃpを教示して開始位置Ｐ1s〜Ｐ5sを開始位置領域１０ｂに記憶し、目標位置Ｐ1t〜Ｐ5tを目標位置領域１０ｃに記憶する。速度生成部１６の速度調整領域１６ａに速度調整領域Ａcを記憶し、特異点領域１６ｂに特異点領域Ａｓを記憶し、上記(６)式の速度調整の演算式を速度調整演算式領域１６ｃに記憶する。
【００２２】
速度調整判断部１４は、指令部１０の補間種類領域１０ａから直線補間を読み出し、開始位置領域１０ｂ、目標位置領域１０ｃからそれぞれ開始位置Ｐ1s、目標位置Ｐ1tを読み出してＸ，Ｙ座標の移動量ΔＸ，ΔＹにより傾きαを求めることにより上記(７)式の演算により直線Ｌ1を原点周りに−α回転させた直線Ｌ1´を求め、直線Ｌ1´の位置Ｐ1s´，Ｐ1t´のＸ，Ｙ座標値Ｘ1s´，Ｘ1t´，Ｙ1s´，Ｙ1t´を求める。速度調整判断部１４は、Ｘ座標値Ｘ1s´＞速度調整領域Ａcの半径Ｒcであるので、速度調整領域Ａｃを通過しないと判断し、切換スイッチ２２をｂ端子側に投入する。ロボットの動作を開始して、速度指令部１３からの速度指令Ｖcにより補間位置生成部２４、座標変換部２６、サーボ制御部２８を介して制御点Ｃpが直線補間動作する。
【００２３】
また、速度調整判断部１４は、上記のようにして開始位置Ｐ2s、目標位置Ｐ2tを読みして直線Ｌ2´を求め、直線Ｌ2´の位置Ｐ2s´，Ｐ2t´のＸ，Ｙ座標値Ｘ2s´，Ｘ2t´，Ｙ2s´，Ｙ2t´を求める。速度調整判断部１４は、Ｙ座標値Ｙ2s´とＹ2t´との積を求め、該積の値が負となるので、直線Ｌ2´がＸ軸を横切ると判断し、Ｘ座標値Ｘ1s´＜速度調整領域Ａcになることを判断する。すなわち、直線Ｌ2´が速度調整領域Ａcを通過すると判断する。
ロボットの動作を開始し、補間動作において速度指令決定部１９は、速度指令部１３からの速度指令Ｖcと速度調整指令部１８からの調整速度指令Ｖrcとを比較し、Ｖrc＜Ｖcであれば、切換スイッチ２２をａ端子側に投入し、Ｖrc≧Ｖcであれば、切換スイッチ２２をｂ端子側に投入することにより速度指令Ｖc、調整速度指令Ｖrcを適宜選択しながら補間位置生成部２４、座標変換部２６、サーボ制御部２８を介して制御点Ｃpが直線補間する。
【００２４】
また、速度調整判断部１４は、上記のようにして開始位置Ｐ3s、目標位置Ｐ3tを読み出して直線Ｌ3´を求め、直線Ｌ3´の位置Ｐ3s´，Ｐ3t´のＸ，Ｙ座標値Ｘ3s´，Ｙ3s´，値Ｘ3t´，Ｙ3t´を求める。速度調整判断部１４は、Ｙ座標値Ｙ3s´とＹ3t´との積を求め、該積の値が負となるので、直線Ｌ2´がＸ軸を横切ると判断し、Ｘ座標値Ｘ3s´＜特異点領域Ａsと判断する。すなわち、速度調整判断部１４は、直線Ｌ3´が特異点領域Ａsを通過すると判断するので、警報部１７から警報を発する。かかる警告に基づいて、例えば、制御点Ｃpの開始位置、目標位置を変更したり、直線補間を関節補間に変更したりすることにより制御点Ｃpが特異点を通過しないようにする。
【００２５】
また、速度調整判断部１４は、上記のようにして開始位置Ｐ4s、目標位置Ｐ4tを読み出して直線Ｌ4´を求め、直線Ｌ4´の位置Ｐ4s´，Ｐ4t´のＸ，Ｙ座標値Ｘ4s´，Ｙ4s´，値Ｘ4t´，Ｙ4t´を求め、直線Ｌ4´の座標値Ｘ4s´，Ｙ4s´が速度調整領域Ａcの半径Ｒcの範囲内となる。ロボットの動作を開始すると、上記の直線Ｌ2´のように制御点Ｃpが補間動作する。
【００２６】
また、速度調整判断部１４は、上記のようにして開始位置Ｐ5s、目標位置Ｐ5tを読みして直線Ｌ5´を求め、直線Ｌ5´の位置Ｐ5s´，Ｐ5t´のＸ，Ｙ座標値Ｘ5s´，Ｙ5s´，Ｘ5t´，Ｙ5t´を求める。直線Ｌ5´の座標値Ｘ5s´，Ｙ5s´が特異点領域Ａsの半径Ｒｓの範囲内であるので、警報部１７から警報を発する。
【００２７】
上記実施の形態では、速度調整判断部１４は、直線Ｌ1´〜Ｌ5´が速度調整領域Ａcを通過すると判断することにより、制御点Ｃpの補間動作を実行しながら速度指令決定部１９が速度指令Ｖcと調整速度指令Ｖrcを比較して低い方の速度指令を選択して制御点Ｃpの補間動作を実行した。しかしながら、速度調整判断部１４は、直線Ｌ1´〜Ｌ5´が速度調整領域Ａcを通過すると判断することにより、調整速度指令Ｖrcのみによって制御点Ｃpの補間動作を実行しても良い。
【図面の簡単な説明】
【図１】本発明の一実施の形態を示す３自由度の水平多関節型ロボットの全体概念構成図である。
【図２】図１に示すロボットのアームどうしが折り重なった状態を示す平面図である。
【図３】図１に示すロボットが図２の特異点を直線補間で通過する際の水平アーム部の動きを表す図である。
【図４】図１に示すロボットの制御点を補間処理させるブロック図である。
【図５】図１に示すロボットの制御点が特異点に最も近付いたときの速度指令と、その位置においてＪ１軸の回転により生成される速度を表す図である。
【図６】図１に示すロボットの制御点が斜めに直線補間動作する場合の水平方向と垂直方向の速度成分を示す図である。
【図７】図１に示すロボットの制御点の開始位置と目標位置を結ぶ直線と速度調整領域との関係を表す図である。
【図８】図７に示す直線Ｌ1〜Ｌ4を原点周りに回転させてＸ軸の値を一定とした例を表した図である。
【図９】図１に示すロボットの制御点の開始位置と目標位置を結ぶ直線が速度調整領域を通過するか否かを判断するために用いる図である。
【符号の説明】
１０指令部(ＲＡＭ，記憶手段)、１４速度調整判断部(演算手段，第１及び第２の判断手段)、１７警報部、１８速度調整指令部(速度指令調整手段)。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to an improvement in a robot control apparatus in which an articulated robot passes near a singular point.
[0002]
[Prior art]
In the linear interpolation operation for making the locus of the control point of the robot draw a straight line, information on the operation start position, the position of the target position and the posture (hereinafter referred to as orthogonal data) is stored in advance in the robot control device, Orthogonal data is calculated for each robot control cycle so that the control point draws a straight line between these two positions, and the angle of each joint axis (hereinafter referred to as joint data) is obtained by coordinate transformation from the orthogonal data. This is realized by controlling the motor of each axis so as to match the joint data.
[0003]
In a multi-joint type robot, when coordinates are converted from orthogonal data to joint data, there is a position called a singular point where joint data cannot be obtained uniquely, and when passing through a singular point or the vicinity of a singular point It is known that a particular joint axis must rotate at a very high speed.
Therefore, during the interpolation operation, the joint axis speed command is always calculated, and it is determined whether the control point of the robot has approached the singular point from the rate of increase of the speed command. Means have been proposed for switching the processing to such an interpolation means, or for correcting the speed command according to the distance from the singular point and passing the vicinity of the singular point (Japanese Patent Laid-Open No. 5-324044).
[0004]
[Problems to be solved by the invention]
However, in the technical means as described above, determination processing and speed correction processing for the vicinity of the singular point are always executed during the interpolation operation. For this reason, useless processing is originally executed for the interpolation operation that does not pass through the vicinity of the singular point, so that the computation time increases due to the time occupied by the interpolation operation processing. Therefore, there is a problem that quick control is restricted.
[0005]
The present invention has been made to solve the above-described problem, and determines whether or not to pass near the singular point before the execution of the interpolation operation. It is an object of the present invention to provide a control device for a robot that executes processing.
[0006]
[Means for Solving the Problems]
Control apparatus for a robot according to a first aspect of the present invention is the control device of the horizontal articulated robot having a plurality of joint axes, Ri by to teach the control point of the robot, the start position of the previous SL control points Storage means for storing a target position; calculation means for reading the start position and the target position from the storage means before the robot operates, and obtaining a straight line connecting the start position and the target position; First determination means for determining whether or not the straight line passes through a speed adjustment region set around a singular point of the robot, and when the straight line passes through the speed adjustment region, And a first speed command generating means for generating a first speed command as an adjusted speed command that allows passage through the speed adjustment region based on the distance to the singular point.
According to such a robot control device, before the linear interpolation operation is performed, the first determination means determines whether or not the robot control point passes the speed adjustment region. Based on the distance up to, the control point of the robot is interpolated by the first speed command. Therefore, if the control point does not originally pass through the speed adjustment region in the vicinity of the singular point, useless processing for passing through the vicinity of the singular point is not executed during the operation of the robot, so that the calculation process is smooth. .
[0007]
According to a second aspect of the present invention, there is provided a control apparatus for a robot, wherein a second speed command generating means for generating a second speed command for interpolating a control point at a predetermined speed based on the start position and the target position read from a storage means. And selecting the lower speed command by comparing the first speed command and the second speed command while executing the control point interpolation operation by determining that the straight line passes by the first determination means. Means.
According to such a robot control apparatus, the selection means compares the first speed command and the second speed command, selects the lower speed command, and interpolates the control points. Therefore, there is an effect that the control point of the robot can pass through the speed adjustment region more smoothly.
[0008]
The first speed command generating means in the robot control apparatus according to the third invention is such that the distance from the singular point to the control point is R, and a predetermined joint axis that is subject to speed adjustment under the influence of the singular point _Is the maximum rotational speed of Δθ _1max and the angle between the linear interpolation operation direction and the horizontal direction is φ, the first speed command Vrc is _expressed by the following equation: Vrc = R · Δθ _1max / cosφ
It is characterized by this.
According to such a robot control device, since the amount of movement in the vertical direction is also taken into account in the horizontal direction, when the control point moves largely in the vertical direction relative to the horizontal direction in the speed adjustment region, the adjustment speed command only in the horizontal direction. On the other hand, there is an effect that the speed adjustment region can be smoothly operated by appropriately reducing the speed of the control point.
[0009]
A robot control device according to a fourth aspect of the present invention is set around a singular point and has a singular point region narrower than the speed adjustment region, and before the robot operates, the straight line is the singular point. A second determination unit that determines whether or not the region passes; and an alarm unit that generates an alarm when the second determination unit determines that the straight line passes through the singular point region. It is characterized by.
If it is determined that the control point of the robot passes through the singular point region, it cannot be operated by normal linear interpolation, so a warning is issued before the operation starts. Based on such warning, for example, the control point can be prevented from passing through the singular point by changing the start position and the target position of the control point or changing the linear interpolation to the joint interpolation. is there.
[0010]
DETAILED DESCRIPTION OF THE INVENTION
Embodiment 1 FIG.
An embodiment of the present invention will be described with reference to FIGS. In FIG. 1, an articulated robot 1 includes a base coaxial 3, a direct coaxial J 0 that is movable in the vertical direction, a rotatable first joint J 1, and a rotatable second joint J 2. The arm 5 connects the first joint J1 and the second joint J2, and the arm 5 has a control point Cp at the tip and is connected to the second joint J2 and has the same length. .
[0011]
In a three-degree-of-freedom horizontal articulated robot having the same lengths of the arm 5 and the arm 7 as shown in FIG. 1, the position where the arm 5 and the arm 7 overlap as shown in FIG. As shown in FIG. 3, when the operation speed at the control point Cp in the state 1 to the state 7 is kept constant and the linear interpolation operation is performed so as to pass the singular point in FIG. When moving from 4 to state 5, the first joint J1 must rotate 180 degrees instantaneously. As described above, since the trouble occurs when the control point Cp tries to pass the vicinity of the singular point, an interpolation processing execution means for removing the trouble will be described below.
[0012]
In FIG. 4, the interpolation processing means includes a command unit 10 that stores the interpolation type and the like by teaching, and the singularity region As near the singularity or the singularity velocity adjustment in which the interpolation operation of the control point Cp is wider than the singularity region As. A speed adjustment determination unit 14 that determines whether or not to pass through the region Ac, an interpolation processing unit 12 that performs interpolation processing, and an alarm that generates an alarm by determining that the speed adjustment determination unit 14 passes through the singular point region As Unit 17, a coordinate conversion unit 26 that converts the calculated interpolation position into a position command for each joint J0 to J2, and a servo that controls the position of the motor for each joint J0 to J2 based on the command from coordinate conversion unit 26 And a control unit 28.
[0013]
The command unit 10 includes a RAM as a storage unit that stores an interpolation command, an interpolation type region 10a that stores a type of interpolation, and a start position region 10b that stores a start position at which the control point Cp is to be operated. And a target position area 10c that stores the target position of the control point Cp.
The interpolation processing unit 12 includes a speed command unit 13 as second speed command generation means for generating a desired second speed command Vc based on the interpolation command from the command unit 10, and a singularity region As or speed adjustment. A speed adjustment determination unit 14 as first and second determination means for determining whether or not to pass through the area Ac, and a speed storing an arithmetic expression for determining the speed adjustment area Ac, the singular point area As, and the adjustment speed command Vrc As a first speed command generating means for generating an adjustment speed command Vrc as a first speed command that allows passage through the speed adjustment region Ac when it is determined that the adjustment generation unit 16 and the speed adjustment region Ac pass. If it is determined that the speed adjustment commanding section 18 and the speed adjustment area Ac are passed, in the interpolation operation of the control point Cp, if the adjustment speed command Vrc and the speed command Vc are compared and Vrc <Vc, the changeover switch 22 is set to a Throw in the terminal side If Vrc ≧ Vc, a speed command determination unit 19 that generates a signal for turning on the changeover switch 22 to the b terminal side, and an interpolation position generation unit that generates an interpolation position based on the speed command Vc or the adjustment speed command Vrc. 24. The speed command determination unit 19 and the changeover switch 22 constitute selection means.
[0014]
The speed adjustment generation unit 16 includes a speed adjustment area 16a that is a circular area from the singular point and stores a speed adjustment area Ac wider than the singular point area As, and a singular point area As that becomes a circular area from the singular point. It comprises a RAM having a memorized singularity area 16b and a speed adjustment arithmetic expression area 16c in which a predetermined arithmetic expression to be described later for obtaining an adjustment speed command Vrc when the control point Cp passes through the speed adjustment area Ac is stored. Yes.
Here, when the robot shown in FIG. 1 is horizontally operated by linear interpolation along the X axis, the speed adjustment area Ac is the first joint J ₁ axis whose robot control point Cp is one of the singular points. The position closest to the position is when the control point Cp reaches the Y axis as shown in FIG. The speed of the control point Cp at the moment when this is reached is defined as a speed command Vc.
Here, consider a circle of radius R around the first joint J ₁ as shown in FIG. 5, at a position away distance R from the first joint J1 axis, for rotating the first joint J ₁ axis maximum speed Vrmax (m / sec) can be generated by, when a maximum rotational speed of the first joint J ₁ axis and Δθ _{1max (rad} / sec), the following equation. Note that the direction of the maximum speed Vrmax and the speed command Vc coincide on the Y axis.
Vrmax = R · Δθ _1max (1)
A circular speed adjustment region Ac with respect to the speed command Vc is determined with the singular point as the center, and the radius Rc is obtained by modifying the formula (1) as follows.
Rc = Vc / Δθ _1max (2)
For the sake of simplicity, if the speed command Vc is substituted into the above equation (2) as a fixed value with the maximum command velocity Vcmax of the robot, the radius Rc becomes the following equation.
Rc = Vcmax / Δθ _1max (3)
Further, the radius Rs of the singular point region As set inside the speed adjustment region Ac has the following relationship.
0 <Rs <Rc (4)
If the radius Rs is set to a large value within a range satisfying the expression (4), the robot operation range is limited, and thus the predetermined range is set. The value of the radius Rs is 2 mm, for example.
[0015]
The speed adjustment command unit 18 generates an adjustment speed command Vrc when the trajectory of the start position and the target position passes through the speed adjustment region Ac as the trajectory of the control point Cp.
Further, when the control point Cp is linearly interpolated obliquely as shown in FIG. 6, the speed command Vc is generated along the locus of the control point Cp. Since the speed command Vc is for the horizontal operation speed Vh, if the angle formed by the linear interpolation operation direction and the horizontal direction is φ, the following equation is obtained.
Vh = Vc · cosφ (5)
From this equation (5), if the adjustment speed command Vrc is used, the adjustment speed command Vrc is expressed by the following equation and is proportional to the radius R from the singular point to the control point Cp.
Vrc = R · Δθ _1max / cosφ (6)
[0016]
The speed adjustment determination unit 14 determines whether or not the interpolation passes through the singular point area As and the speed adjustment area Ac, and connects the start positions P1s to P5s and the target positions P1t to P5t as shown in FIG. The straight lines L1 to L5 are obtained (calculation means), it is determined whether or not the straight lines L1 to L5 pass through the speed adjustment area Ac, and if it is determined that they do not pass through the speed adjustment area Ac, the changeover switch 22 is moved to the b terminal side. throw into.
[0017]
Since the start position of the taught control point Cp and the X and Y coordinates of the target position take arbitrary values, it is difficult to determine whether or not a straight line connecting the start position and the target position passes through the speed adjustment region Ac. Accordingly, as shown in FIG. 7, the inclination α between the movement amount ΔX in the X coordinate direction and the movement amount ΔY in the Y coordinate direction is obtained, and the straight line L1,. The rotated X ′ and Y ′ are obtained by the following equation to determine whether or not the singular point region As and the speed adjustment region Ac are passed.
[0018]
[Expression 1]

[0019]
By such coordinate conversion, straight lines L1 'connecting the operation start positions P1s', P2s', P3s', P4s', P5s' and the target positions P1t', P2t ', P3t', P4t ', P5t', respectively. L2 ', L3', L4 ', and L5' are as shown in FIG.
[0020]
The speed adjustment determination unit 14 determines whether or not the straight lines L1 ′ to L5 ′ pass through the speed adjustment region Ac and the singular point region As as follows.
(1). When the X coordinate values X1S, X1t of the straight line L1 ′ are larger than the radius Rc of the speed adjustment area Ac, that is, when X1S, X1t> Rc, it is determined that the speed adjustment area Ac is not passed.
(2). If the product of the Y coordinate values Y2s and Y2t of the straight line L2 ′, that is, Y2s · Y2t ≦ 0, it is determined that the straight line L2 ′ crosses the X axis, and if the X coordinate values X2s and X2t <Rc, the speed It is determined that it passes through the adjustment area Ac.
Similarly, if the product of the Y coordinate values Y3s and Y3t of the straight line L3 ′, that is, Y3s · Y3t ≦ 0, it is determined that the straight line L3 ′ crosses the X axis, and the X coordinate values X3s and X3t <Rs. Is determined to pass through the singularity region As.
(3). When the coordinate values Xxs ′, Yxs ′, Xxt ′, and Yxt ′ of the straight line Lx ′ exist in the speed adjustment area Ac, that is, when Lc1 ≦ Rc or Lc2 ≦ Rc, the straight line Lx ′ has the speed adjustment area Ac. Judge that it will pass. For example, the straight line L4 ′ is applicable.
^{Here, Lc1 = (Xxs' 2 + Yxs'} 2) 1/2
^{^{Lc2 = (Xxt' 2 + Yxt' 2}} ) 1/2
Further, when the coordinate values Xxs ′, Yxs ′, Xxt ′, and Yxt ′ of the straight line Lx ′ are present in the singular region As, that is, when Lc1 ≦ Rs or Lc2 ≦ Rs, the straight line Lx ′ is the singular point region As. Judging to pass. For example, the straight line L5 ′ is applicable.
[0021]
The operation of the robot control apparatus configured as described above will be described with reference to FIGS. First, the operator stores linear interpolation as the interpolation type in the interpolation type area 10a, teaches the control point Cp, stores the start positions P1s to P5s in the start position area 10b, and sets the target positions P1t to P5t in the target position area. Store in 10c. The speed adjustment area Ac is stored in the speed adjustment area 16a of the speed generation unit 16, the singular point area As is stored in the singular point area 16b, and the speed adjustment calculation formula of the above equation (6) is stored in the speed adjustment calculation formula area 16c. Remember.
[0022]
The speed adjustment determination unit 14 reads linear interpolation from the interpolation type area 10a of the command unit 10, reads the start position P1s and the target position P1t from the start position area 10b and the target position area 10c, respectively, and moves the movement amount ΔX of the X and Y coordinates. , ΔY to obtain the inclination α, the straight line L1 ′ obtained by rotating the straight line L1 around the origin by −α is obtained by the calculation of the above equation (7), and the X, Y coordinate values of the positions P1s ′, P1t ′ of the straight line L1 ′ X1s ', X1t', Y1s ', Y1t' are obtained. Since the X coordinate value X1s ′> the radius Rc of the speed adjustment area Ac, the speed adjustment determination unit 14 determines that the speed adjustment area Ac does not pass, and inputs the changeover switch 22 to the b terminal side. The operation of the robot is started, and the control point Cp is linearly interpolated through the interpolation position generation unit 24, the coordinate conversion unit 26, and the servo control unit 28 according to the speed command Vc from the speed command unit 13.
[0023]
Further, the speed adjustment determination unit 14 reads the start position P2s and the target position P2t as described above to obtain the straight line L2 ′, and the X and Y coordinate values X2s ′, P2s ′ and P2t ′ of the straight line L2 ′. X2t ', Y2s', Y2t' are obtained. The speed adjustment determination unit 14 obtains the product of the Y coordinate values Y2s 'and Y2t', and determines that the straight line L2 'crosses the X axis because the value of the product is negative. The X coordinate value X1s'<speed It is determined that the adjustment area Ac is entered. That is, it is determined that the straight line L2 ′ passes through the speed adjustment region Ac.
In the interpolation operation, the speed command determination unit 19 compares the speed command Vc from the speed command unit 13 with the adjustment speed command Vrc from the speed adjustment command unit 18, and if Vrc <Vc, If the changeover switch 22 is turned on to the a terminal side and Vrc ≧ Vc, the changeover switch 22 is turned on to the b terminal side to appropriately select the speed command Vc and the adjustment speed command Vrc, and the interpolation position generator 24, coordinates The control point Cp is linearly interpolated via the conversion unit 26 and the servo control unit 28.
[0024]
Further, the speed adjustment determination unit 14 reads the start position P3s and the target position P3t as described above to obtain a straight line L3 ′, and the X and Y coordinate values X3s ′ and Y3s of the positions P3s ′ and P3t ′ of the straight line L3 ′. ', Values X3t' and Y3t 'are obtained. The speed adjustment determination unit 14 calculates the product of the Y coordinate values Y3s ′ and Y3t ′, and determines that the straight line L2 ′ crosses the X axis because the value of the product is negative. The X coordinate value X3s ′ <singular It is determined as a point area As. That is, the speed adjustment determination unit 14 determines that the straight line L3 ′ passes through the singular point region As, and thus issues an alarm from the alarm unit 17. Based on the warning, for example, the control point Cp does not pass the singular point by changing the start position and the target position of the control point Cp or changing the linear interpolation to the joint interpolation.
[0025]
Further, the speed adjustment determination unit 14 reads out the start position P4s and the target position P4t as described above to obtain the straight line L4 ′, and the X and Y coordinate values X4s ′ and Y4s of the positions P4s ′ and P4t ′ of the straight line L4 ′. ', Values X4t' and Y4t 'are obtained, and the coordinate values X4s' and Y4s 'of the straight line L4' fall within the radius Rc of the speed adjustment area Ac. When the operation of the robot is started, the control point Cp is interpolated as shown by the straight line L2 ′.
[0026]
Further, the speed adjustment determination unit 14 reads the start position P5s and the target position P5t as described above to obtain a straight line L5 ′, and the X and Y coordinate values X5s ′, P5s ′ and P5t ′ of the straight line L5 ′. Y5s', X5t ', and Y5t' are obtained. Since the coordinate values X5s' and Y5s' of the straight line L5 'are within the radius Rs of the singular point region As, an alarm is issued from the alarm unit 17.
[0027]
In the above embodiment, the speed adjustment determination unit 14 determines that the straight lines L1 ′ to L5 ′ pass through the speed adjustment region Ac, so that the speed command determination unit 19 performs the interpolation operation of the control point Cp while the speed command determination unit 19 performs the speed command. The lower speed command is selected by comparing Vc and the adjustment speed command Vrc, and the interpolation operation of the control point Cp is executed. However, the speed adjustment determination unit 14 may execute the interpolation operation of the control point Cp only by the adjustment speed command Vrc by determining that the straight lines L1 ′ to L5 ′ pass through the speed adjustment region Ac.
[Brief description of the drawings]
FIG. 1 is an overall conceptual configuration diagram of a three-degree-of-freedom horizontal articulated robot showing an embodiment of the present invention.
FIG. 2 is a plan view showing a state where arms of the robot shown in FIG. 1 are folded.
3 is a diagram showing the movement of the horizontal arm unit when the robot shown in FIG. 1 passes through the singular point of FIG. 2 by linear interpolation.
4 is a block diagram for interpolating control points of the robot shown in FIG.
FIG. 5 is a diagram showing a speed command when the control point of the robot shown in FIG. 1 is closest to a singular point and a speed generated by rotation of the J1 axis at that position.
6 is a diagram showing velocity components in the horizontal direction and the vertical direction when the control point of the robot shown in FIG. 1 performs a linear interpolation operation obliquely. FIG.
7 is a diagram showing a relationship between a straight line connecting a start position of a control point and a target position of the robot shown in FIG. 1 and a speed adjustment region.
8 is a diagram illustrating an example in which the value of the X axis is made constant by rotating the straight lines L1 to L4 shown in FIG. 7 around the origin.
9 is a diagram used to determine whether or not a straight line connecting a start position of a control point and a target position of the robot shown in FIG. 1 passes through a speed adjustment region.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 10 Command part (RAM, memory | storage means), 14 Speed adjustment judgment part (Calculation means, 1st and 2nd judgment means), 17 Alarm part, 18 Speed adjustment command part (Speed command adjustment means).

Claims

複数の関節軸を有する水平多関節型のロボットの制御装置において、
前記ロボットの制御点を教示することにより、前記制御点の開始位置と目標位置を記憶する記憶手段と、
前記ロボットが動作する前に、前記記憶手段から前記開始位置及び前記目標位置を読み出して、前記開始位置と前記目標位置とを結ぶ直線を求める演算手段と、
前記ロボットの特異点の周りに設定された速度調整領域を、前記直線が通過するか否かを判断する第１の判断手段と、
前記速度調整領域を前記直線が通過する場合、前記制御点から前記特異点までの距離に基づいて速度調整領域の通過を可能とする調整速度指令としての第１の速度指令を生成する第１の速度指令生成手段と、
を備えたことを特徴とするロボットの制御装置。 In a control apparatus for a horizontal articulated robot having a plurality of joint axes,
Ri by to teach the control points of the robot, storage means for storing the starting position and the target position of the previous SL control point,
Before the robot operates, a calculation unit that reads the start position and the target position from the storage unit and obtains a straight line connecting the start position and the target position;
First determination means for determining whether or not the straight line passes through a speed adjustment region set around a singular point of the robot;
When the straight line passes through the speed adjustment region, a first speed command is generated as an adjustment speed command that allows passage through the speed adjustment region based on a distance from the control point to the singular point. Speed command generation means;
A robot control device comprising:

前記記憶手段から読み出した前記開始位置、前記目標位置に基づいて前記制御点を所定の速度で補間する第２の速度指令を生成する第２の速度指令生成手段と、
前記第１の判断手段により前記直線が通過すると判断することにより、前記制御点の補間動作を実行しながら前記第１の速度指令と前記第２の速度指令を比較して低い方の速度指令を選択する選択手段と、
を備えたことを特徴とする請求項１に記載のロボットの制御装置。 Read before Symbol start position from the storage means, a second speed command generating means for generating a second speed command for interpolating the control point at a predetermined speed based on the target position,
By determining that the straight line passes by the first determining means, the first speed command and the second speed command are compared while executing the interpolation operation of the control point, and the lower speed command is determined. A selection means to select;
The robot control device according to claim 1, further comprising:

前記第１の速度指令生成手段は、前記特異点から前記制御点までの距離をＲとし、特異点の影響を受け速度調整の対象となる所定の関節軸の最高回転速度をΔθ_1maxとし、前記直線補間の動作方向と水平方向の成す角をφとすると、前記第１の速度指令Ｖrcが下式となる
Ｖrc＝Ｒ・Δθ_1max／cosφ
ことを特徴とする請求項１又は２に記載のロボットの制御装置。The first speed command generation means sets R as the distance from the singular point to the control point, and _sets the maximum rotation speed of a predetermined joint axis that is subject to speed adjustment under the influence of the singular point as Δθ _1max. When the angle formed by the linear interpolation operation direction and the horizontal direction is φ, the first speed command Vrc is _expressed by the following equation: Vrc = R · Δθ _1max / cosφ
The robot control device according to claim 1, wherein the control device is a robot.

前記特異点の周りに設定されると共に、前記速度調整領域よりも狭い特異点領域を有し、前記ロボットが動作する前に、前記直線が該特異点領域を通過するか否かを判断する第２の判断手段と、
前記第２の判断手段によって前記直線が特異点領域を通過すると判断された場合に、警報を発生する警報手段と、
を備えたことを特徴とする請求項１に記載のロボットの制御装置。A singularity area that is set around the singularity and that is narrower than the speed adjustment area, and determines whether or not the straight line passes through the singularity area before the robot operates. 2 judgment means;
Alarm means for generating an alarm when the second determination means determines that the straight line passes through the singularity area ;
The robot control device according to claim 1, further comprising: