JP4085208B2

JP4085208B2 - Robot control method

Info

Publication number: JP4085208B2
Application number: JP19878997A
Authority: JP
Inventors: 佳市高岡; 浩治冨田
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1997-07-24
Filing date: 1997-07-24
Publication date: 2008-05-14
Anticipated expiration: 2017-07-24
Also published as: JPH1139021A

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットの手首フランジに装着された工具の先端部の制御点の移動経路（軌道）として入力された教示点から、前記制御部の目標経路を生成するロボットの経路補間方法に関するもので、詳しくは、移動経路に屈曲したコーナー部分が存在する場合に、コーナー部分を加減速なしで高速に通過可能にするための改良に関するものである。
【０００２】
【従来の技術】
近年、ロボットがいろいろな作業現場で活用されている。このようなロボットは、例えば、多関節のアームの先端部の手首フランジに作業用の工具を装着し、工具の先端部を、作業内容に即して、所望の移動経路（軌道）で移動させる。
従って、作業の品質向上、信頼性向上を図るためには、工具の先端部の移動動作の精度と安定性の追求が重要課題とされている。
前記工具の先端部（制御点）の移動経路は、複数の教示点により予め与え、一つの教示点から次の教示点までの間を単純に直線運動させる場合には、教示点を結ぶ直線経路を教示点間の補間演算により求める。
しかし、複数の教示点で示した移動経路の途中に屈曲したコーナー部分が存在する場合、直線経路と同様の取り扱いでコーナー部分を通過させようとすると、アームの各関節部分等に過大な衝撃がかかるという問題が生じる。
そこで、従来では、複数の教示点で示した移動経路の途中に屈曲したコーナー部分が存在する場合、コーナー部分に近づくと減速してコーナー点で一旦停止させ、進行方向を変えた後に加速させて通常の移動速度に戻したり、あるいは、コーナー部分を形成している二つの直線経路上の加減速が必要となる範囲を滑らかな円弧等の曲線経路に置き換えることで、コーナー部分通過時における衝撃の作用を抑える経路補間方法が提案されている。
【０００３】
【発明が解決しようとする課題】
しかし、前述した従来の経路補間方法では、コーナー部分の通過時に加減速と一旦停止を実行する方法の場合は、制御点の移動速度がコーナー部分付近で大きく変化するため、例えば、ロボットによる作業がシール材を塗布するシーリングの場合、シール材の付着量がコーナー部分付近で増えるなど、作業品質にばらつきが生じ易いという問題が生じる。また、移動経路を曲線に置き換える方法の場合は、当初に要求された角張った教示経路を忠実に再現することができず、移動精度の低下等の問題が生じる。
そこで、本発明の目的は上記課題を解消することにあり、ロボットの制御点の移動経路に屈曲したコーナー部分が存在する場合に、ロボットの各関節軸への衝撃を抑えながら、かつ、制御点の動作は直線動作のままで、コーナー部分を加減速なしで高速に通過させることのできるロボットの経路補間方法を提供することである。
【０００４】
【課題を解決するための手段】
請求項１記載のロボットの制御方法の発明は、フランジ駆動軸によって回転する手首フランジと、前記フランジ駆動軸の中心軸線からオフセット距離を持つように前記手首フランジに設けられた工具と、を備えたロボットにおいて、前記工具先端部の制御点は、始点（Ｐ１）、角部（Ｐ２）、終点（Ｐ３）の教示点で表される角部を含む移動経路をたどるとともに、前記制御点が前記角部Ｐ２を通過する前後において、前記手首フランジが回転することにより、前記フランジ駆動軸の中心軸線と前記教示点がなす教示平面との交点は、前記角部（Ｐ２）を通り、かつ前記制御点の移動経路に対して外側に膨らんだ移動経路をたどることを特徴としている。
【０００５】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態について説明する。
図１乃至図７は本発明に係るロボットの経路補間方法の一実施形態を示したもので、図１は本発明に係る経路補間方法の一実施形態により作業するロボットの全体概略図、図２は図１に示したロボットの手首フランジの上面図、図３は図１に示したロボットの手首フランジの側面図、図４は本発明に係る経路補間方法の一実施形態における補間演算処理の手順を示すフローチャート、図５は図４に示した処理において、制御点がコーナー点に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図、図６は図４に示した処理において、制御点がコーナー点通過後のフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図、図７は図４に示した処理によって制御点が教示点Ｐ１から教示点Ｐ２を経て教示点Ｐ３に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
まず、本発明の一実施形態の経路補間方法により動作するロボット１について、図１乃至図３に基づいて説明する。
前記ロボット１は、いわゆる多関節ロボットで、工具３が装着される手首フランジ５のフランジ面６が、前記工具３の先端部の制御点８の移動経路９を示唆する教示点群のなす教示平面１１に対して平行に設定されている。また、このロボット１では、前記手首フランジ５がフランジ駆動軸１３によって回転可能に支持されており、前記フランジ駆動軸１３の中心軸線Ｃ１が前記教示平面１１に直交している。
また、前記工具３は、図３に示すように、前記制御点８が前記フランジ駆動軸１３の中心軸線Ｃ１に対してオフセット距離Ｒ１を持つように前記フランジ面６に装備されている。
そして、ロボット１には、前記制御点８の移動経路の補間演算処理や、補間演算処理結果に基づいて各関節部に動作信号を出力する制御装置（図示略）が備えられている。
前記制御装置は、前記制御点８を教示平面１１上へ投影した点を制御点Ｐｃ、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点（即ち、手首フランジ５の回転中心Ｐｆを教示平面１１上に投影した点）をＰｆ₀とするとき、交点Ｐｆ₀がほぼ教示点を結んだ線上に位置し、かつ、前記交点Ｐｆ₀から制御点Ｐｃへの方向がほぼ前記制御点Ｐｃの移動方向となるように、予め３点の教示点Ｐ１，Ｐ２，Ｐ３を与え、前記制御点Ｐｃが教示点Ｐ１，Ｐ２，Ｐ３で示した移動経路９を直線動作により進むように経路補間を行う。
３点の教示点Ｐ１，Ｐ２，Ｐ３の示す移動経路９が、図１に示すように、教示点Ｐ１を出発点として直線動作で教示点Ｐ３まで進む時、教示点Ｐ２をコーナー点とする直交２直線の軌道となる場合、前記制御装置は、図４に示す手順で、コーナー部分に対する経路補間処理を行う。なお、図４に示した処理は、基本制御クロック毎に実施される。
まず、制御点Ｐｃが教示点Ｐ１から教示点Ｐ２に向かっている途中なのか、あるいは既に教示点Ｐ２を通過して教示点Ｐ３に向かっている途中なのかを判別する（Ｓ１０１）。
そして、前記Ｓ１０１で、制御点Ｐｃが教示点Ｐ１から教示点Ｐ２に向かっている途中と判別された場合には、まず、通常の逆変換（＝逆運動学の解を求めて直交座標をロボットの各軸座標に変換すること）処理を行う直前まで、直線補間処理を行う（Ｓ２０１）。
次いで、次の（１）式に基づいて制御点Ｐｃから手首フランジ５の回転中心Ｐｆを求めると共に、求めた手首フランジ５の回転中心Ｐｆから直線Ｐ１Ｐ２へ下した垂線の交点Ｐｆ₀を求める（Ｓ２０２）。
Ｐｆ＝Ｐｃ＊Ｅ^-1 …（１）
前記（１）式において、Ｅはエンドエフェクタである。
次いで、既にコーナー部の補間処理中か否かを、制御フラグに基づいて判別する（Ｓ２０３）。
前記Ｓ２０３で、コーナー部の補間処理中でないと判断された場合は、前記交点Ｐｆ₀から教示点Ｐ２までの距離が基準距離Ｌ１よりも小さいか否かを判別し（Ｓ２１１）、交点Ｐｆ₀から教示点Ｐ２までの距離が基準距離Ｌ１よりも大きい場合には、先にＳ２０１で算出した制御点Ｐｃを補間した制御点位置Ｐｃ₀として所定の逆変換処理を行い（Ｓ４０１）、さらに算出したロボットの各軸角度をモータ現在パルスに変換してサーボ部に払い出して処理を終了する（Ｓ４０２）。
前述のＳ２１１で、交点Ｐｆ₀から教示点Ｐ２までの距離が基準距離Ｌ１よりも小さいと判断された場合は、図５に示すように、交点Ｐｆ₀を交点Ｐ４、そして該交点Ｐ４から教示点Ｐ２までの距離をＬｅ、直線Ｐ２Ｐ３上で教示点Ｐ２から距離Ｌｅの点を点Ｐ５とし、交点Ｐ４，教示点Ｐ２，点Ｐ５通る円Ｃ２の中心点Ｏｐと半径Ｒ２とを求め、コーナー部の補間処理中の制御フラグをセットして、前述したＳ４０１に移行する（Ｓ２１２）。
前述のＳ２０３で、コーナー部の補間処理中と判断された場合は、次の（Ａ）〜（Ｄ）の処理を順に行う（Ｓ２２１）。
（Ａ）まず、図５に示すように、制御点Ｐｃを中心とした半径Ｒ１の円Ｃ１を求める。図５では、制御点Ｐｃが教示点Ｐ２に重なった状態を示している。
（Ｂ）そして、円Ｃ１と前記円Ｃ２との交点が、手首フランジ５の回転中心Ｐｆの位置となるように、以下の演算を実行する。
まず、図５に示すように、制御点Ｐｃと教示点Ｐ１とを結んだ線分ＰｃＰ１と、制御点Ｐｃと円Ｃ２の中心点Ｏｐとを結んだ線分ＰｃＯｐのなす角度αを、次の（２）〜（４）式に基づいて求める（内積）。
ｌ＝Ｐ１−Ｐｃ／｜Ｐ１Ｐｃ｜ …（２）
ｍ＝Ｏｐ−Ｐｃ／｜ＯｐＰｃ｜ …（３）
α＝ｃｏｓ^-1（ｌ_xｍ_x＋ｌ_yｍ_y＋ｌ_zｍ_z） …（４）
さらに、制御点Ｐｃと中心点Ｏｐとの距離をＬ２として、それぞれの円Ｃ１，Ｃ２の半径Ｒ１，Ｒ２と前記距離Ｌ２とを次の（５）式に代入して、円Ｃ１，Ｃ２相互の交点Ｑと制御点Ｐｃとを結んだ線分ＱＰｃと、制御点Ｐｃと中心点Ｏｐとを結んだ線分ＯｐＰｃとの成す角度βを求める。
β＝ｃｏｓ^-1（Ｒ１²＋Ｌ２²−Ｒ２²／２＊Ｒ１＊Ｌ２） …（５）
（Ｃ）次いで、線分ＱＰｃと線分Ｐ１Ｐ２との成す角度γを次の（６）式により求める。
γ＝β−α …（６）
（Ｄ）次いで、前記（６）式で求めた角度γを次の（７）式に代入して、補間した制御点位置Ｐｃ₀を算出する。
Ｐｃ₀＝Ｐｃ＊ｒｏｔ（ｚ，γ） …（７）
上記の（７）式は、制御点ＰｃをＴｚ回りに角度γ（又は−γ）だけ回転させている。
以上のＳ２２１の処理が済んだら、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
以上のＳ２０１以降の処理は、制御点Ｐｃが教示点Ｐ２に到達するまで繰り返される。また、Ｓ２２１以降の処理は、最初にＳ２１２によってコーナー部処理中の制御フラグがセットされたら、それ以後、制御点Ｐｃが教示点Ｐ２に到達するまで繰り返される。
また、前記Ｓ１０１で、制御点Ｐｃが教示点Ｐ２から教示点Ｐ３に向かっている途中と判別された場合には、まず、通常の逆変換（＝逆運動学の解を求めて直交座標をロボットの各軸座標に変換すること）処理を行う直前まで、直線補間処理を行う（Ｓ３０１）。
次いで、前述の（１）式に基づいて制御点Ｐｃから手首フランジ５の回転中心Ｐｆを求めると共に、求めた手首フランジ５の回転中心Ｐｆから直線Ｐ２Ｐ３へ下した垂線の交点Ｐｆ₀を求める（Ｓ３０２）。
次いで、既にコーナー部の補間処理中か否かを、制御フラグに基づいて判別する（Ｓ３０３）。
前記Ｓ３０３で、コーナー部の補間処理中でないと判断された場合は、前述のＳ４０１に移行して、先にＳ３０１で算出した制御点Ｐｃを補間した制御点位置Ｐｃ₀として所定の逆変換処理を行い、さらに算出したロボットの各軸角度をモータ現在パルスに変換してサーボ部に払い出して処理を終了する（Ｓ４０２）。
前述のＳ３０３で、コーナー部の補間処理中と判断された場合は、次の（Ａ）〜（Ｄ）の処理を順に行う（Ｓ３２１）。
（Ａ）まず、図６に示すように、制御点Ｐｃを中心とした半径Ｒ１の円Ｃ１を求める。
（Ｂ）そして、円Ｃ１と前記円Ｃ２との交点Ｑが、手首フランジ５の回転中心Ｐｆの位置となるように、以下の演算を行う。
まず、図６に示すように、制御点Ｐｃと教示点Ｐ３とを結んだ線分ＰｃＰ３と、制御点Ｐｃと円Ｃ２の中心点Ｏｐとを結んだ線分ＰｃＯｐのなす角度αを、次の（８）〜（１０）式に基づいて求める（内積）。
ｌ＝Ｐ３−Ｐｃ／｜Ｐ３Ｐｃ｜ …（８）
ｍ＝Ｏｐ−Ｐｃ／｜ＯｐＰｃ｜ …（９）
α＝π−ｃｏｓ^-1（ｌ_xｍ_x＋ｌ_yｍ_y＋ｌ_zｍ_z） …（１０）
さらに、制御点Ｐｃと中心点Ｏｐとの距離をＬ２として、それぞれの円Ｃ１，Ｃ２の半径Ｒ１，Ｒ２と前記距離Ｌ２とを次の（１１）式に代入して、円Ｃ１，Ｃ２相互の交点Ｑと制御点Ｐｃとを結んだ線分ＱＰｃと、制御点Ｐｃと中心点Ｏｐとを結んだ線分ＯｐＰｃとの成す角度βを求める。
β＝ｃｏｓ^-1（Ｒ１²＋Ｌ２²−Ｒ２²／２＊Ｒ１＊Ｌ２） …（１１）
（Ｃ）次いで、線分ＱＰｃと線分Ｐ２Ｐ３との成す角度γを次の（１２）式により求める。
γ＝α−β …（１２）
（Ｄ）次いで、前記（６）式で求めた角度γを次の（１３）式に代入して、補間した制御点位置Ｐｃ₀を算出する。
Ｐｃ₀＝Ｐｃ＊ｒｏｔ（ｚ，γ） …（１３）
上記の（１３）式は、制御点ＰｃをＴｚ回りに角度γ（又は−γ）だけ回転させている。
以上のＳ３２１の処理が済んだら、交点Ｐｆ０から点Ｐ５までの距離が最短か否かを判断し（Ｓ３２２）、最短でない場合は、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
前記Ｓ３２２において、交点Ｐｆ０から点Ｐ５までの距離が最短であると判断された場合には、前述したコーナー部処理中の制御フラグをリセットし（Ｓ３２３）、その後に、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
以上のＳ３２１以降の処理は、交点Ｐｆ０から点Ｐ５までの距離が最短となってコーナー部処理中の制御フラグがリセットされるまで繰り返され、それ以後、教示点Ｐ３に向かう間は、再度、通常の直線補間制御が行われる。
以上の補間処理によって、前記制御点Ｐｃが教示点Ｐ１から教示点Ｐ３へ到達するまでの制御点Ｐｃの軌跡を経時的に、連続描画したのが、図７である。
以上の説明から明らかなように、本発明の一実施形態の経路補間方法は、３点の教示点Ｐ１，Ｐ２，Ｐ３を与え、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づいた場合に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点をＰ４、教示点Ｐ２から交点Ｐ４までの距離をＬｅ、教示点Ｐ２から教示点Ｐ３へ向かう直線上で教示点Ｐ２から距離Ｌｅの点をＰ５として、以後、制御点８が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまで、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジ５を回転駆動させて、制御点８が教示した経路上を直線動作するように経路補間するもので、３点の教示点Ｐ１，Ｐ２，Ｐ３で与えられたロボットの制御点８の移動経路９が、教示点Ｐ２がコーナー部分となる折れ線経路の場合、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づくと、以後、制御点８が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまでは、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジ５を回転駆動させて、制御点８は教示した経路上を直線動作させる。
そのため、接続点自体は、教示点によって与えられた直線経路を忠実にトレースすることができる。
そして、関節部である手首フランジ５は、滑らかな回転動作となるため、コーナー部分の通過時に、ロボットの基本軸（位置を司るベースの３軸）に対して急峻な速度変動が発生しない。そのため、ロボットの各関節軸等の大きな衝撃荷重が作用せず、衝撃荷重の作用に起因する振動の発生を抑えて、コーナー点を直線動作で通過させることができる。
【０００６】
【発明の効果】
本発明のロボットの経路補間方法によれば、上記構成によれば、３点の教示点Ｐ１，Ｐ２，Ｐ３で与えられたロボットの制御点の移動経路が、教示点Ｐ２がコーナー部分となる折れ線経路の場合、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸の中心軸線と教示平面との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づくと、以後、制御点が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまでは、前記フランジ駆動軸の中心軸線と教示平面との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジを回転駆動させて、制御点は教示した経路上を直線動作させる。
そのため、接続点自体は、教示点によって与えられた直線経路を忠実にトレースすることができる。
そして、関節部である手首フランジは、滑らかな回転動作となるため、コーナー部分の通過時に、ロボットの基本軸（位置を司るベースの３軸）に対して急峻な速度変動が発生しない。そのため、ロボットの各関節軸等の大きな衝撃荷重が作用せず、衝撃荷重の作用に起因する振動の発生を抑えて、コーナー点を直線動作で通過させることができる。
従って、ロボットの各関節軸への衝撃を抑えながら、かつ、制御点の動作は直線動作のままで、コーナー部分を加減速なしで高速に通過させることができる。
【図面の簡単な説明】
【図１】本発明に係る経路補間方法の一実施形態により作業するロボットの全体概略図である。
【図２】図１に示したロボットの手首フランジの上面図である。
【図３】図１に示したロボットの手首フランジの側面図である。
【図４】本発明に係る経路補間方法の一実施形態における補間演算処理の手順を示すフローチャートである。
【図５】図４に示した処理において、制御点がコーナー点に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【図６】図４に示した処理において、制御点がコーナー点通過後のフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【図７】図４に示した処理によって制御点が教示点Ｐ１から教示点Ｐ２を経て教示点Ｐ３に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【符号の説明】
１ロボット
３工具
５手首フランジ
６フランジ面
８，Ｐｃ制御点
９移動経路
１１教示平面
１３フランジ駆動軸[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a robot path interpolation method for generating a target path of a control unit from teaching points input as a movement path (trajectory) of a control point of a tip portion of a tool mounted on a wrist flange of the robot. Specifically, the present invention relates to an improvement for allowing a corner portion to pass at high speed without acceleration / deceleration when a bent corner portion exists in a moving path.
[0002]
[Prior art]
In recent years, robots are used in various work sites. Such a robot, for example, attaches a working tool to the wrist flange at the tip of an articulated arm, and moves the tip of the tool along a desired movement path (orbit) according to the work content. .
Therefore, in order to improve work quality and reliability, it is important to pursue the accuracy and stability of the moving operation of the tip of the tool.
The movement path of the tip (control point) of the tool is given in advance by a plurality of teaching points, and when the tool is simply linearly moved from one teaching point to the next teaching point, a linear path connecting the teaching points. Is obtained by interpolation between teaching points.
However, if there is a bent corner part in the middle of the movement path indicated by multiple teaching points, if you try to pass the corner part with the same handling as the straight line path, excessive impact will be applied to each joint part of the arm, etc. Such a problem arises.
Therefore, in the past, when there is a bent corner part in the middle of the movement path indicated by a plurality of teaching points, the vehicle decelerates when approaching the corner part, temporarily stops at the corner point, accelerates after changing the traveling direction. By returning to the normal movement speed or replacing the area where acceleration / deceleration on the two linear paths forming the corner part is necessary with a curved path such as a smooth circular arc, A path interpolation method that suppresses the action has been proposed.
[0003]
[Problems to be solved by the invention]
However, in the conventional path interpolation method described above, in the method of executing acceleration / deceleration and temporary stop when passing through the corner portion, the movement speed of the control point greatly changes near the corner portion. In the case of sealing with a sealing material applied, there is a problem in that work quality tends to vary, such as the amount of adhesion of the sealing material increasing near the corner portion. In addition, in the case of the method of replacing the movement route with a curve, the angular teaching route that was originally requested cannot be faithfully reproduced, causing problems such as a reduction in movement accuracy.
Accordingly, an object of the present invention is to eliminate the above-mentioned problem, and when there is a bent corner portion in the movement path of the robot control point, while suppressing the impact on each joint axis of the robot, the control point This is to provide a robot path interpolation method that allows a corner portion to pass at high speed without acceleration / deceleration while maintaining a linear motion.
[0004]
[Means for Solving the Problems]
The invention of the robot control method according to claim 1 includes a wrist flange that is rotated by a flange drive shaft, and a tool that is provided on the wrist flange so as to have an offset distance from a center axis of the flange drive shaft. In the robot, the control point of the tool tip follows a movement path including a corner represented by a teaching point of a start point (P1) , a corner (P2) , and an end point (P3) , and the control point is the angle. before and after passing through the section P2, by the wrist flange rotates, intersection of the teachings plane the teaching point to the center axis of the flange drive shaft forms the Ri through the angle portion (P2), and the control It is characterized by following a movement path bulging outward with respect to a point movement path.
[0005]
DETAILED DESCRIPTION OF THE INVENTION
Embodiments of the present invention will be described below with reference to the drawings.
1 to 7 show an embodiment of a robot path interpolation method according to the present invention. FIG. 1 is an overall schematic diagram of a robot working according to an embodiment of the path interpolation method according to the present invention. 1 is a top view of the wrist flange of the robot shown in FIG. 1, FIG. 3 is a side view of the wrist flange of the robot shown in FIG. 1, and FIG. 4 is a procedure of interpolation calculation processing in one embodiment of the path interpolation method according to the present invention. FIG. 5 is a flowchart illustrating the movement of the control point between the intersection of the center axis of the flange drive shaft and the teaching plane until the control point reaches the corner point in the processing shown in FIG. 4, and FIG. 4 is an explanatory diagram of the movement operation between the control point and the intersection of the center axis of the flange drive shaft after passing the corner point and the teaching plane in the process shown in FIG. 4, and FIG. Control point is taught from teaching point P1 An intersection between the center axis and teachings plane of the flange drive shaft until it reaches the teaching point P3 through the point P2, which is a moving operation explanatory diagram of the control points.
First, a robot 1 that operates according to a path interpolation method according to an embodiment of the present invention will be described with reference to FIGS.
The robot 1 is a so-called articulated robot, and a teaching plane formed by a teaching point group in which the flange surface 6 of the wrist flange 5 on which the tool 3 is mounted suggests the movement path 9 of the control point 8 at the tip of the tool 3. 11 is set in parallel. In the robot 1, the wrist flange 5 is rotatably supported by a flange drive shaft 13, and the center axis C <b> 1 of the flange drive shaft 13 is orthogonal to the teaching plane 11.
Further, as shown in FIG. 3, the tool 3 is mounted on the flange surface 6 so that the control point 8 has an offset distance R <b> 1 with respect to the central axis C <b> 1 of the flange drive shaft 13.
The robot 1 includes a control device (not shown) that outputs an operation signal to each joint unit based on the interpolation calculation process of the movement path of the control point 8 and the interpolation calculation process result.
The control device sets a point obtained by projecting the control point 8 onto the teaching plane 11 as a control point Pc, and an intersection between the center axis C1 of the flange drive shaft 13 and the teaching plane 11 (that is, a rotation center Pf of the wrist flange 5). When the point projected onto the teaching plane 11 is Pf ₀ , the intersection point Pf ₀ is located almost on the line connecting the teaching points, and the direction from the intersection point Pf ₀ to the control point Pc is substantially the control point Pc. The three teaching points P1, P2, and P3 are given in advance so that the moving direction becomes the control direction P, and the path interpolation is performed so that the control point Pc moves along the moving path 9 indicated by the teaching points P1, P2, and P3 by a linear motion. Do.
As shown in FIG. 1, when the moving path 9 indicated by the three teaching points P1, P2 and P3 proceeds to the teaching point P3 in a linear motion from the teaching point P1 as a starting point, it is orthogonal with the teaching point P2 as a corner point. When the trajectory has two straight lines, the control device performs a path interpolation process on the corner portion according to the procedure shown in FIG. The process shown in FIG. 4 is performed for each basic control clock.
First, it is determined whether the control point Pc is on the way from the teaching point P1 toward the teaching point P2 or is already on the way to the teaching point P3 after passing the teaching point P2 (S101).
If it is determined in S101 that the control point Pc is on the way from the teaching point P1 to the teaching point P2, first, a normal inverse transformation (= inverse kinematics solution is obtained to obtain the orthogonal coordinates from the robot. The linear interpolation process is performed until immediately before the process is performed (S201).
Next, the rotation center Pf of the wrist flange 5 is obtained from the control point Pc based on the following equation (1), and the intersecting point Pf _{0 of} the perpendicular drawn from the obtained rotation center Pf of the wrist flange 5 to the straight line P1P2 is obtained (S202). ).
Pf = Pc * E ⁻¹ (1)
In the formula (1), E is an end effector.
Next, it is determined based on the control flag whether or not the corner portion interpolation processing is already in progress (S203).
In the S203, if it is found not to be in the interpolation processing of the corner section, it is determined whether or not the distance from the intersection point Pf ₀ to the teaching point P2 is smaller than the reference distance L1 (S211), from the intersection point Pf ₀ When the distance to the teaching point P2 is larger than the reference distance L1, a predetermined inverse conversion process is performed as the control point position Pc ₀ obtained by interpolating the control point Pc previously calculated in S201 (S401), and the calculated robot Each shaft angle is converted into a motor current pulse and delivered to the servo unit, and the process is terminated (S402).
In S211 described above, if the distance from the intersection Pf ₀ to the teaching point P2 is determined to be smaller than the reference distance L1, as shown in FIG. 5, the teaching point intersection Pf ₀ from the intersection P4, and intersection point P4 The distance to P2 is Le, the point Le5 from the teaching point P2 on the straight line P2P3 is the point P5, the center point Op and the radius R2 of the circle C2 passing through the intersection point P4, the teaching point P2, and the point P5 are obtained. The control flag during the interpolation processing is set, and the process proceeds to S401 described above (S212).
If it is determined in S203 described above that the corner portion is being interpolated, the following processes (A) to (D) are performed in order (S221).
(A) First, as shown in FIG. 5, a circle C1 having a radius R1 around the control point Pc is obtained. FIG. 5 shows a state where the control point Pc overlaps with the teaching point P2.
(B) Then, the following calculation is executed so that the intersection of the circle C1 and the circle C2 is the position of the rotation center Pf of the wrist flange 5.
First, as shown in FIG. 5, an angle α formed by a line segment PcP1 connecting the control point Pc and the teaching point P1 and a line segment PcOp connecting the control point Pc and the center point Op of the circle C2 is expressed as follows. It calculates | requires based on (2)-(4) Formula (inner product).
l = P1-Pc / | P1Pc | (2)
m = Op-Pc / | OpPc | (3)
^{_{α = cos -1 (l x m}} x + l y m y + l z m z) ... (4)
Furthermore, the distance between the control point Pc and the center point Op is L2, and the radii R1, R2 of the respective circles C1, C2 and the distance L2 are substituted into the following equation (5), so that the mutual relation between the circles C1, C2 An angle β formed by a line segment QPc connecting the intersection point Q and the control point Pc and a line segment OpPc connecting the control point Pc and the center point Op is obtained.
^{^{β = cos -1 (R1 2 +}} L2 2 -R2 2/2 * R1 * L2) ... (5)
(C) Next, an angle γ formed by the line segment QPc and the line segment P1P2 is obtained by the following equation (6).
γ = β−α (6)
(D) Next, the interpolated control point position Pc ₀ is calculated by substituting the angle γ determined by the above equation (6) into the following equation (7).
Pc ₀ = Pc * rot (z, γ) (7)
In the above equation (7), the control point Pc is rotated around the Tz by the angle γ (or −γ).
When the above-described processing of S221 is completed, the above-described S401 and S402 are executed in sequence to end the processing.
The processes after S201 are repeated until the control point Pc reaches the teaching point P2. Further, the processing after S221 is repeated until the control point Pc reaches the teaching point P2 after the control flag during corner processing is first set in S212.
If it is determined in S101 that the control point Pc is in the middle of the teaching point P2 toward the teaching point P3, first, normal inverse transformation (= reverse kinematics solution is obtained to obtain the orthogonal coordinates from the robot. The linear interpolation process is performed until immediately before the process is performed (S301).
Next, the rotation center Pf of the wrist flange 5 is obtained from the control point Pc based on the above-described equation (1), and the intersection point Pf _{0 of} the perpendicular drawn from the obtained rotation center Pf of the wrist flange 5 to the straight line P2P3 is obtained (S302). ).
Next, it is determined based on the control flag whether or not the corner portion interpolation processing is already in progress (S303).
In the S303, if it is found not to be in the interpolation processing of the corner section, the process proceeds to S401 described above, the predetermined inverse transform control points Pc calculated in S301 above as a control point position Pc ₀ interpolated Then, each calculated robot shaft angle is converted into a motor current pulse and delivered to the servo unit, and the process is terminated (S402).
If it is determined in S303 described above that the corner portion interpolation process is being performed, the following processes (A) to (D) are sequentially performed (S321).
(A) First, as shown in FIG. 6, a circle C1 having a radius R1 around the control point Pc is obtained.
(B) Then, the following calculation is performed so that the intersection point Q between the circle C1 and the circle C2 is the position of the rotation center Pf of the wrist flange 5.
First, as shown in FIG. 6, an angle α formed by a line segment PcP3 connecting the control point Pc and the teaching point P3 and a line segment PcOp connecting the control point Pc and the center point Op of the circle C2 is expressed as follows. Obtained based on equations (8) to (10) (inner product).
l = P3-Pc / | P3Pc | (8)
m = Op-Pc / | OpPc | (9)
^{α = π-cos -1 (l} x m x + l y m y + l z m z) ... (10)
Further, the distance between the control point Pc and the center point Op is L2, and the radii R1, R2 of the respective circles C1, C2 and the distance L2 are substituted into the following equation (11), and the mutual relation between the circles C1, C2 is calculated. An angle β formed by a line segment QPc connecting the intersection point Q and the control point Pc and a line segment OpPc connecting the control point Pc and the center point Op is obtained.
^{^{β = cos -1 (R1 2 +}} L2 2 -R2 2/2 * R1 * L2) ... (11)
(C) Next, an angle γ formed by the line segment QPc and the line segment P2P3 is obtained by the following equation (12).
γ = α−β (12)
(D) Next, the interpolated control point position Pc ₀ is calculated by substituting the angle γ obtained by the equation (6) into the following equation (13).
Pc ₀ = Pc * rot (z, γ) (13)
In the above equation (13), the control point Pc is rotated around the Tz by an angle γ (or −γ).
After the process of S321 is completed, it is determined whether or not the distance from the intersection Pf0 to the point P5 is the shortest (S322). If not the shortest, the above-described S401 and S402 are executed in sequence to end the process.
If it is determined in S322 that the distance from the intersection Pf0 to the point P5 is the shortest, the control flag during the corner processing described above is reset (S323), and then the above-described S401 and S402 are sequentially performed. Execute to finish the process.
The above-described processing after S321 is repeated until the distance from the intersection Pf0 to the point P5 becomes the shortest and the control flag during corner processing is reset. The linear interpolation control is performed.
FIG. 7 shows the locus of the control point Pc continuously drawn over time until the control point Pc reaches the teaching point P3 from the teaching point P1 by the above interpolation processing.
As is apparent from the above description, the path interpolation method of one embodiment of the present invention provides three teaching points P1, P2, and P3, and the flange is moved during the movement operation from the teaching point P1 to the teaching point P2. When the intersection between the center axis C1 of the drive shaft 13 and the teaching plane 11 is closer to the teaching point P2 than the point of the reference distance L1 from the teaching point P2, the center axis C1 of the flange driving shaft 13 and the teaching plane 11 are The intersection point is P4, the distance from the teaching point P2 to the intersection point P4 is Le, and the point at the distance Le from the teaching point P2 on the straight line from the teaching point P2 to the teaching point P3 is P5. Thereafter, the control point 8 is changed from the teaching point P2. Until the point P5 on the straight line directed to the teaching point P3 passes, the intersection of the center axis C1 of the flange drive shaft 13 and the teaching plane 11 moves on an arc passing through the intersection P4, the teaching point P2, and the point P5. The wrist franc 5 is rotationally driven, and the path is interpolated so as to linearly move on the path taught by the control point 8, and the movement path 9 of the robot control point 8 given by the three teaching points P1, P2 and P3. However, in the case of a polygonal line path where the teaching point P2 is a corner portion, during the movement operation from the teaching point P1 to the teaching point P2, the intersection of the center axis C1 of the flange drive shaft 13 and the teaching plane 11 is from the teaching point P2. When approaching the teaching point P2 rather than the point of the reference distance L1, thereafter, until the control point 8 passes a point P5 on a straight line from the teaching point P2 to the teaching point P3, the center axis C1 of the flange drive shaft 13 The wrist flange 5 is rotationally driven so that the intersection point with the teaching plane 11 moves on an arc passing through the intersection point P4, the teaching point P2, and the point P5, and the control point 8 linearly moves on the taught path.
Therefore, the connection point itself can faithfully trace the straight path given by the teaching point.
And since the wrist flange 5 which is a joint part becomes a smooth rotation operation, when passing through the corner portion, a steep speed fluctuation does not occur with respect to the basic axis of the robot (the three axes of the base which controls the position). Therefore, a large impact load such as each joint axis of the robot does not act, and it is possible to suppress the generation of vibration due to the effect of the impact load and pass the corner point in a linear motion.
[0006]
【The invention's effect】
According to the robot path interpolation method of the present invention, according to the above configuration, the movement path of the robot control point given by the three teaching points P1, P2, and P3 is a polygonal line where the teaching point P2 is a corner portion. In the case of the path, during the movement operation from the teaching point P1 to the teaching point P2, when the intersection of the center axis of the flange drive shaft and the teaching plane approaches the teaching point P2 from the teaching point P2 than the point of the reference distance L1, Thereafter, until the control point passes the point P5 on the straight line from the teaching point P2 to the teaching point P3, the intersection of the center axis of the flange drive shaft and the teaching plane passes through the intersection P4, the teaching point P2, and the point P5. The wrist flange is rotationally driven so as to move on the arc, and the control point moves linearly on the taught path.
Therefore, the connection point itself can faithfully trace the straight path given by the teaching point.
Since the wrist flange, which is a joint portion, has a smooth rotational motion, no steep speed fluctuation occurs with respect to the basic axis of the robot (the three axes of the base that controls the position) when passing through the corner portion. Therefore, a large impact load such as each joint axis of the robot does not act, and it is possible to suppress the generation of vibration due to the effect of the impact load and pass the corner point in a linear motion.
Therefore, it is possible to pass the corner portion at high speed without acceleration / deceleration while suppressing the impact on each joint axis of the robot and maintaining the operation of the control point as a linear operation.
[Brief description of the drawings]
FIG. 1 is an overall schematic diagram of a robot working according to an embodiment of a path interpolation method according to the present invention.
2 is a top view of a wrist flange of the robot shown in FIG. 1. FIG.
3 is a side view of a wrist flange of the robot shown in FIG. 1. FIG.
FIG. 4 is a flowchart showing a procedure of an interpolation calculation process in an embodiment of a path interpolation method according to the present invention.
FIG. 5 is an explanatory diagram of the movement operation between the control point and the intersection between the center axis of the flange drive shaft and the teaching plane until the control point reaches the corner point in the process shown in FIG. 4;
6 is an explanatory view of the movement operation between the control point and the intersection of the center axis of the flange drive shaft and the teaching plane after the control point passes the corner point in the process shown in FIG. 4;
7 shows the movement of the control point between the intersection of the center axis of the flange drive shaft and the teaching plane until the control point reaches the teaching point P3 from the teaching point P1 through the teaching point P2 by the processing shown in FIG. It is operation | movement explanatory drawing.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Robot 3 Tool 5 Wrist flange 6 Flange surface 8, Pc Control point 9 Movement path 11 Teaching plane 13 Flange drive shaft

Claims

In a robot comprising a wrist flange rotated by a flange drive shaft, and a tool provided on the wrist flange so as to have an offset distance from a center axis of the flange drive shaft,
The control point of the tool tip follows a movement path including corners represented by teaching points of a start point (P1) , a corner (P2) , and an end point (P3) , and the control point follows the corner P2. As the wrist flange rotates before and after passing, the intersection between the center axis of the flange drive shaft and the teaching plane formed by the teaching point is
Ri through the angle portion (P2),
And following a movement path bulging outward with respect to the movement path of the control point,
A robot control method characterized by the above.