(57)【特許請求の範囲】
【請求項１】
フランジ駆動軸によって回転する手首フランジと、前記フランジ駆動軸の中心軸線からオフセット距離を持つように前記手首フランジに設けられた工具と、を備えたロボットにおいて、
前記工具先端部の制御点は、始点（Ｐ１）、角部（Ｐ２）、終点（Ｐ３）の教示点で表される角部を含む移動経路をたどるとともに、前記制御点が前記角部Ｐ２を通過する前後において、前記手首フランジが回転することにより、前記フランジ駆動軸の中心軸線と前記教示点がなす教示平面との交点は、
前記角部（Ｐ２）を通り、
かつ前記制御点の移動経路に対して外側に膨らんだ移動経路をたどる、
ことを特徴するロボットの制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットの手首フランジに装着された工具の先端部の制御点の移動経路（軌道）として入力された教示点から、前記制御部の目標経路を生成するロボットの経路補間方法に関するもので、詳しくは、移動経路に屈曲したコーナー部分が存在する場合に、コーナー部分を加減速なしで高速に通過可能にするための改良に関するものである。
【０００２】
【従来の技術】
近年、ロボットがいろいろな作業現場で活用されている。このようなロボットは、例えば、多関節のアームの先端部の手首フランジに作業用の工具を装着し、工具の先端部を、作業内容に即して、所望の移動経路（軌道）で移動させる。
従って、作業の品質向上、信頼性向上を図るためには、工具の先端部の移動動作の精度と安定性の追求が重要課題とされている。
前記工具の先端部（制御点）の移動経路は、複数の教示点により予め与え、一つの教示点から次の教示点までの間を単純に直線運動させる場合には、教示点を結ぶ直線経路を教示点間の補間演算により求める。
しかし、複数の教示点で示した移動経路の途中に屈曲したコーナー部分が存在する場合、直線経路と同様の取り扱いでコーナー部分を通過させようとすると、アームの各関節部分等に過大な衝撃がかかるという問題が生じる。
そこで、従来では、複数の教示点で示した移動経路の途中に屈曲したコーナー部分が存在する場合、コーナー部分に近づくと減速してコーナー点で一旦停止させ、進行方向を変えた後に加速させて通常の移動速度に戻したり、あるいは、コーナー部分を形成している二つの直線経路上の加減速が必要となる範囲を滑らかな円弧等の曲線経路に置き換えることで、コーナー部分通過時における衝撃の作用を抑える経路補間方法が提案されている。
【０００３】
【発明が解決しようとする課題】
しかし、前述した従来の経路補間方法では、コーナー部分の通過時に加減速と一旦停止を実行する方法の場合は、制御点の移動速度がコーナー部分付近で大きく変化するため、例えば、ロボットによる作業がシール材を塗布するシーリングの場合、シール材の付着量がコーナー部分付近で増えるなど、作業品質にばらつきが生じ易いという問題が生じる。また、移動経路を曲線に置き換える方法の場合は、当初に要求された角張った教示経路を忠実に再現することができず、移動精度の低下等の問題が生じる。
そこで、本発明の目的は上記課題を解消することにあり、ロボットの制御点の移動経路に屈曲したコーナー部分が存在する場合に、ロボットの各関節軸への衝撃を抑えながら、かつ、制御点の動作は直線動作のままで、コーナー部分を加減速なしで高速に通過させることのできるロボットの経路補間方法を提供することである。
【０００４】
【課題を解決するための手段】
請求項１記載のロボットの制御方法の発明は、フランジ駆動軸によって回転する手首フランジと、前記フランジ駆動軸の中心軸線からオフセット距離を持つように前記手首フランジに設けられた工具と、を備えたロボットにおいて、前記工具先端部の制御点は、始点（Ｐ１）、角部（Ｐ２）、終点（Ｐ３）の教示点で表される角部を含む移動経路をたどるとともに、前記制御点が前記角部Ｐ２を通過する前後において、前記手首フランジが回転することにより、前記フランジ駆動軸の中心軸線と前記教示点がなす教示平面との交点は、前記角部（Ｐ２）を通り、かつ前記制御点の移動経路に対して外側に膨らんだ移動経路をたどることを特徴としている。
【０００５】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態について説明する。
図１乃至図７は本発明に係るロボットの経路補間方法の一実施形態を示したもので、図１は本発明に係る経路補間方法の一実施形態により作業するロボットの全体概略図、図２は図１に示したロボットの手首フランジの上面図、図３は図１に示したロボットの手首フランジの側面図、図４は本発明に係る経路補間方法の一実施形態における補間演算処理の手順を示すフローチャート、図５は図４に示した処理において、制御点がコーナー点に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図、図６は図４に示した処理において、制御点がコーナー点通過後のフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図、図７は図４に示した処理によって制御点が教示点Ｐ１から教示点Ｐ２を経て教示点Ｐ３に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
まず、本発明の一実施形態の経路補間方法により動作するロボット１について、図１乃至図３に基づいて説明する。
前記ロボット１は、いわゆる多関節ロボットで、工具３が装着される手首フランジ５のフランジ面６が、前記工具３の先端部の制御点８の移動経路９を示唆する教示点群のなす教示平面１１に対して平行に設定されている。また、このロボット１では、前記手首フランジ５がフランジ駆動軸１３によって回転可能に支持されており、前記フランジ駆動軸１３の中心軸線Ｃ１が前記教示平面１１に直交している。
また、前記工具３は、図３に示すように、前記制御点８が前記フランジ駆動軸１３の中心軸線Ｃ１に対してオフセット距離Ｒ１を持つように前記フランジ面６に装備されている。
そして、ロボット１には、前記制御点８の移動経路の補間演算処理や、補間演算処理結果に基づいて各関節部に動作信号を出力する制御装置（図示略）が備えられている。
前記制御装置は、前記制御点８を教示平面１１上へ投影した点を制御点Ｐｃ、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点（即ち、手首フランジ５の回転中心Ｐｆを教示平面１１上に投影した点）をＰｆ0とするとき、交点Ｐｆ0がほぼ教示点を結んだ線上に位置し、かつ、前記交点Ｐｆ0から制御点Ｐｃへの方向がほぼ前記制御点Ｐｃの移動方向となるように、予め３点の教示点Ｐ１，Ｐ２，Ｐ３を与え、前記制御点Ｐｃが教示点Ｐ１，Ｐ２，Ｐ３で示した移動経路９を直線動作により進むように経路補間を行う。
３点の教示点Ｐ１，Ｐ２，Ｐ３の示す移動経路９が、図１に示すように、教示点Ｐ１を出発点として直線動作で教示点Ｐ３まで進む時、教示点Ｐ２をコーナー点とする直交２直線の軌道となる場合、前記制御装置は、図４に示す手順で、コーナー部分に対する経路補間処理を行う。なお、図４に示した処理は、基本制御クロック毎に実施される。
まず、制御点Ｐｃが教示点Ｐ１から教示点Ｐ２に向かっている途中なのか、あるいは既に教示点Ｐ２を通過して教示点Ｐ３に向かっている途中なのかを判別する（Ｓ１０１）。
そして、前記Ｓ１０１で、制御点Ｐｃが教示点Ｐ１から教示点Ｐ２に向かっている途中と判別された場合には、まず、通常の逆変換（＝逆運動学の解を求めて直交座標をロボットの各軸座標に変換すること）処理を行う直前まで、直線補間処理を行う（Ｓ２０１）。
次いで、次の（１）式に基づいて制御点Ｐｃから手首フランジ５の回転中心Ｐｆを求めると共に、求めた手首フランジ５の回転中心Ｐｆから直線Ｐ１Ｐ２へ下した垂線の交点Ｐｆ0を求める（Ｓ２０２）。
Ｐｆ＝Ｐｃ＊Ｅ-1                        …（１）
前記（１）式において、Ｅはエンドエフェクタである。
次いで、既にコーナー部の補間処理中か否かを、制御フラグに基づいて判別する（Ｓ２０３）。
前記Ｓ２０３で、コーナー部の補間処理中でないと判断された場合は、前記交点Ｐｆ0から教示点Ｐ２までの距離が基準距離Ｌ１よりも小さいか否かを判別し（Ｓ２１１）、交点Ｐｆ0から教示点Ｐ２までの距離が基準距離Ｌ１よりも大きい場合には、先にＳ２０１で算出した制御点Ｐｃを補間した制御点位置Ｐｃ0として所定の逆変換処理を行い（Ｓ４０１）、さらに算出したロボットの各軸角度をモータ現在パルスに変換してサーボ部に払い出して処理を終了する（Ｓ４０２）。
前述のＳ２１１で、交点Ｐｆ0から教示点Ｐ２までの距離が基準距離Ｌ１よりも小さいと判断された場合は、図５に示すように、交点Ｐｆ0を交点Ｐ４、そして該交点Ｐ４から教示点Ｐ２までの距離をＬｅ、直線Ｐ２Ｐ３上で教示点Ｐ２から距離Ｌｅの点を点Ｐ５とし、交点Ｐ４，教示点Ｐ２，点Ｐ５通る円Ｃ２の中心点Ｏｐと半径Ｒ２とを求め、コーナー部の補間処理中の制御フラグをセットして、前述したＳ４０１に移行する（Ｓ２１２）。
前述のＳ２０３で、コーナー部の補間処理中と判断された場合は、次の（Ａ）〜（Ｄ）の処理を順に行う（Ｓ２２１）。
（Ａ）  まず、図５に示すように、制御点Ｐｃを中心とした半径Ｒ１の円Ｃ１を求める。図５では、制御点Ｐｃが教示点Ｐ２に重なった状態を示している。
（Ｂ）  そして、円Ｃ１と前記円Ｃ２との交点が、手首フランジ５の回転中心Ｐｆの位置となるように、以下の演算を実行する。
まず、図５に示すように、制御点Ｐｃと教示点Ｐ１とを結んだ線分ＰｃＰ１と、制御点Ｐｃと円Ｃ２の中心点Ｏｐとを結んだ線分ＰｃＯｐのなす角度αを、次の（２）〜（４）式に基づいて求める（内積）。
ｌ＝Ｐ１−Ｐｃ／｜Ｐ１Ｐｃ｜                      …（２）
ｍ＝Ｏｐ−Ｐｃ／｜ＯｐＰｃ｜                      …（３）
α＝ｃｏｓ-1（ｌxｍx＋ｌyｍy＋ｌzｍz）      …（４）
さらに、制御点Ｐｃと中心点Ｏｐとの距離をＬ２として、それぞれの円Ｃ１，Ｃ２の半径Ｒ１，Ｒ２と前記距離Ｌ２とを次の（５）式に代入して、円Ｃ１，Ｃ２相互の交点Ｑと制御点Ｐｃとを結んだ線分ＱＰｃと、制御点Ｐｃと中心点Ｏｐとを結んだ線分ＯｐＰｃとの成す角度βを求める。
β＝ｃｏｓ-1（Ｒ１2＋Ｌ２2−Ｒ２2／２＊Ｒ１＊Ｌ２）  …（５）
（Ｃ）  次いで、線分ＱＰｃと線分Ｐ１Ｐ２との成す角度γを次の（６）式により求める。
γ＝β−α                  …（６）
（Ｄ）次いで、前記（６）式で求めた角度γを次の（７）式に代入して、補間した制御点位置Ｐｃ0を算出する。
Ｐｃ0＝Ｐｃ＊ｒｏｔ（ｚ，γ）            …（７）
上記の（７）式は、制御点ＰｃをＴｚ回りに角度γ（又は−γ）だけ回転させている。
以上のＳ２２１の処理が済んだら、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
以上のＳ２０１以降の処理は、制御点Ｐｃが教示点Ｐ２に到達するまで繰り返される。また、Ｓ２２１以降の処理は、最初にＳ２１２によってコーナー部処理中の制御フラグがセットされたら、それ以後、制御点Ｐｃが教示点Ｐ２に到達するまで繰り返される。
また、前記Ｓ１０１で、制御点Ｐｃが教示点Ｐ２から教示点Ｐ３に向かっている途中と判別された場合には、まず、通常の逆変換（＝逆運動学の解を求めて直交座標をロボットの各軸座標に変換すること）処理を行う直前まで、直線補間処理を行う（Ｓ３０１）。
次いで、前述の（１）式に基づいて制御点Ｐｃから手首フランジ５の回転中心Ｐｆを求めると共に、求めた手首フランジ５の回転中心Ｐｆから直線Ｐ２Ｐ３へ下した垂線の交点Ｐｆ0を求める（Ｓ３０２）。
次いで、既にコーナー部の補間処理中か否かを、制御フラグに基づいて判別する（Ｓ３０３）。
前記Ｓ３０３で、コーナー部の補間処理中でないと判断された場合は、前述のＳ４０１に移行して、先にＳ３０１で算出した制御点Ｐｃを補間した制御点位置Ｐｃ0として所定の逆変換処理を行い、さらに算出したロボットの各軸角度をモータ現在パルスに変換してサーボ部に払い出して処理を終了する（Ｓ４０２）。
前述のＳ３０３で、コーナー部の補間処理中と判断された場合は、次の（Ａ）〜（Ｄ）の処理を順に行う（Ｓ３２１）。
（Ａ）  まず、図６に示すように、制御点Ｐｃを中心とした半径Ｒ１の円Ｃ１を求める。
（Ｂ）  そして、円Ｃ１と前記円Ｃ２との交点Ｑが、手首フランジ５の回転中心Ｐｆの位置となるように、以下の演算を行う。
まず、図６に示すように、制御点Ｐｃと教示点Ｐ３とを結んだ線分ＰｃＰ３と、制御点Ｐｃと円Ｃ２の中心点Ｏｐとを結んだ線分ＰｃＯｐのなす角度αを、次の（８）〜（１０）式に基づいて求める（内積）。
ｌ＝Ｐ３−Ｐｃ／｜Ｐ３Ｐｃ｜                      …（８）
ｍ＝Ｏｐ−Ｐｃ／｜ＯｐＰｃ｜                      …（９）
α＝π−ｃｏｓ-1（ｌxｍx＋ｌyｍy＋ｌzｍz）  …（１０）
さらに、制御点Ｐｃと中心点Ｏｐとの距離をＬ２として、それぞれの円Ｃ１，Ｃ２の半径Ｒ１，Ｒ２と前記距離Ｌ２とを次の（１１）式に代入して、円Ｃ１，Ｃ２相互の交点Ｑと制御点Ｐｃとを結んだ線分ＱＰｃと、制御点Ｐｃと中心点Ｏｐとを結んだ線分ＯｐＰｃとの成す角度βを求める。
β＝ｃｏｓ-1（Ｒ１2＋Ｌ２2−Ｒ２2／２＊Ｒ１＊Ｌ２）  …（１１）
（Ｃ）  次いで、線分ＱＰｃと線分Ｐ２Ｐ３との成す角度γを次の（１２）式により求める。
γ＝α−β                    …（１２）
（Ｄ）次いで、前記（６）式で求めた角度γを次の（１３）式に代入して、補間した制御点位置Ｐｃ0を算出する。
Ｐｃ0＝Ｐｃ＊ｒｏｔ（ｚ，γ）          …（１３）
上記の（１３）式は、制御点ＰｃをＴｚ回りに角度γ（又は−γ）だけ回転させている。
以上のＳ３２１の処理が済んだら、交点Ｐｆ０から点Ｐ５までの距離が最短か否かを判断し（Ｓ３２２）、最短でない場合は、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
前記Ｓ３２２において、交点Ｐｆ０から点Ｐ５までの距離が最短であると判断された場合には、前述したコーナー部処理中の制御フラグをリセットし（Ｓ３２３）、その後に、前述したＳ４０１及びＳ４０２を順に実行して処理を終了する。
以上のＳ３２１以降の処理は、交点Ｐｆ０から点Ｐ５までの距離が最短となってコーナー部処理中の制御フラグがリセットされるまで繰り返され、それ以後、教示点Ｐ３に向かう間は、再度、通常の直線補間制御が行われる。
以上の補間処理によって、前記制御点Ｐｃが教示点Ｐ１から教示点Ｐ３へ到達するまでの制御点Ｐｃの軌跡を経時的に、連続描画したのが、図７である。
以上の説明から明らかなように、本発明の一実施形態の経路補間方法は、３点の教示点Ｐ１，Ｐ２，Ｐ３を与え、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づいた場合に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点をＰ４、教示点Ｐ２から交点Ｐ４までの距離をＬｅ、教示点Ｐ２から教示点Ｐ３へ向かう直線上で教示点Ｐ２から距離Ｌｅの点をＰ５として、以後、制御点８が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまで、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジ５を回転駆動させて、制御点８が教示した経路上を直線動作するように経路補間するもので、３点の教示点Ｐ１，Ｐ２，Ｐ３で与えられたロボットの制御点８の移動経路９が、教示点Ｐ２がコーナー部分となる折れ線経路の場合、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づくと、以後、制御点８が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまでは、前記フランジ駆動軸１３の中心軸線Ｃ１と教示平面１１との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジ５を回転駆動させて、制御点８は教示した経路上を直線動作させる。
そのため、接続点自体は、教示点によって与えられた直線経路を忠実にトレースすることができる。
そして、関節部である手首フランジ５は、滑らかな回転動作となるため、コーナー部分の通過時に、ロボットの基本軸（位置を司るベースの３軸）に対して急峻な速度変動が発生しない。そのため、ロボットの各関節軸等の大きな衝撃荷重が作用せず、衝撃荷重の作用に起因する振動の発生を抑えて、コーナー点を直線動作で通過させることができる。
【０００６】
【発明の効果】
本発明のロボットの経路補間方法によれば、上記構成によれば、３点の教示点Ｐ１，Ｐ２，Ｐ３で与えられたロボットの制御点の移動経路が、教示点Ｐ２がコーナー部分となる折れ線経路の場合、教示点Ｐ１から教示点Ｐ２への移動動作中に、前記フランジ駆動軸の中心軸線と教示平面との交点が教示点Ｐ２から基準距離Ｌ１の点よりも教示点Ｐ２に近づくと、以後、制御点が教示点Ｐ２から教示点Ｐ３へ向かう直線上の点Ｐ５を通過するまでは、前記フランジ駆動軸の中心軸線と教示平面との交点が前記交点Ｐ４，教示点Ｐ２，点Ｐ５通る円弧上を移動するように、前記手首フランジを回転駆動させて、制御点は教示した経路上を直線動作させる。
そのため、接続点自体は、教示点によって与えられた直線経路を忠実にトレースすることができる。
そして、関節部である手首フランジは、滑らかな回転動作となるため、コーナー部分の通過時に、ロボットの基本軸（位置を司るベースの３軸）に対して急峻な速度変動が発生しない。そのため、ロボットの各関節軸等の大きな衝撃荷重が作用せず、衝撃荷重の作用に起因する振動の発生を抑えて、コーナー点を直線動作で通過させることができる。
従って、ロボットの各関節軸への衝撃を抑えながら、かつ、制御点の動作は直線動作のままで、コーナー部分を加減速なしで高速に通過させることができる。
【図面の簡単な説明】
【図１】本発明に係る経路補間方法の一実施形態により作業するロボットの全体概略図である。
【図２】図１に示したロボットの手首フランジの上面図である。
【図３】図１に示したロボットの手首フランジの側面図である。
【図４】本発明に係る経路補間方法の一実施形態における補間演算処理の手順を示すフローチャートである。
【図５】図４に示した処理において、制御点がコーナー点に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【図６】図４に示した処理において、制御点がコーナー点通過後のフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【図７】図４に示した処理によって制御点が教示点Ｐ１から教示点Ｐ２を経て教示点Ｐ３に到達するまでのフランジ駆動軸の中心軸線と教示平面との交点と、制御点との移動動作説明図である。
【符号の説明】
１  ロボット
３  工具
５  手首フランジ
６  フランジ面
８，Ｐｃ  制御点
９  移動経路
１１  教示平面
１３  フランジ駆動軸
【図１】
0004085208.tif 000002
【図２】
0004085208.tif 000003
【図３】
0004085208.tif 000004
【図４】
0004085208.tif 000005
【図５】
0004085208.tif 000006
【図６】
0004085208.tif 000007
【図７】
0004085208.tif 000008