(57)【特許請求の範囲】
【請求項１】
ロボットの教示点の位置、姿勢や各軸の動作方向、動作速度によって、重力モーメント、加速度による慣性、他軸の動作による干渉トルクの影響を受ける駆動軸を有するロボットの加減速時間決定方法において、
質点質量と、前記ロボットの各腕について、回動軸と先端部の他腕回動軸あるいは前記質点との距離、質量、重心位置、前記ロボットの各駆動軸の許容ピークトルクをパラメータとして予め格納しておき、
各教示点で教示された各腕の角度、動作方向、及び動作速度からなる教示データと前記パラメータとから各軸についてｉを第ｉ駆動軸として前記ロボットのモデルに基づき運動方程式から導出される加減速度の影響を受ける負荷トルク成分（Ｔi）を演算して、加減速度の影響を受ける負荷トルク成分（Ｔi）の値から、駆動軸の許容ピークトルク値から加減速度の影響を受けない負荷トルク成分ｄiの値を差し引いた値で除した値である加速時の限界加速時間計算式（ａ）と減速時の限界減速時間計算式（ｂ）より加減速時間を演算することを特徴とするロボットの加減速時間決定方法。
ｔai＝αi／（Ｔi−ｄi）  αi＞０ならばＴi＝τi，αi＜０ならばＴi＝−τi  …（ａ）
ｔdi＝−αi／（Ｔi−ｄi）  αi＞０ならばＴi＝−τi，αi＜０ならばＴi＝τi  …（ｂ）
（ただし、ｔaiは第ｉ駆動軸の加速時間、ｔdiは第ｉ駆動軸の減速時間、αiは第ｉ駆動軸の加速度トルク成分であり前記運動方程式から導出される加減速度の影響を受ける負荷トルク成分の算出式を変形して得られる算出式に前記パラメータと各腕の角度を代入して求まる値、τiは第ｉ駆動軸の許容ピークトルクの絶対値、ｄiは第ｉ駆動軸の加減速度の影響を受けない負荷トルク成分であり前記運動方程式から導出される加減速度の影響を受ける負荷トルク成分の算出式を変形して得られる算出式に前記パラメータと各腕の角度と動作方向及び動作速度からなるからなる条件指定を代入して求まる値とする。）
【請求項２】
前記加減速時間は、各軸ごとに加減速時間を演算し、この中の最大値を加減速時間として前記ロボットの各軸を加減速制御することを特徴とする請求項１記載のロボットの加減速時間決定方法。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、ロボットの教示点の位置、姿勢や各軸の動作方向、動作速度によって、負荷イナーシャや加速度による慣性、他軸の動作による干渉トルク等の影響を受ける駆動軸の加減速時間決定方法に関する。
【０００２】
【従来の技術】
一般に経路作業を行なうロボットは、高速で動作する場合、加減速制御を行い、滑らかな動きを得ている。この加減速制御を行なうときの加減速時間の決定方法として、位置、姿勢から負荷イナーシャを演算し、この負荷イナーシャに基づいた加減速時定数を決定する方法がとられていた（特開平５−４６２３４号公報参照）。
【０００３】
【発明が解決しようとする課題】
ところが従来技術では、実際のロボットの動作時に駆動軸が受けるトルクの成分として、他軸の動作によって生じる干渉トルクや遠心力、コリオリ力などが考慮されていない。そのため、実際には動作によって発生するトルクが異なる場合でも、姿勢が同じならば、同じ加減速時間が決定されるという問題があった。
一方、特開平４−２８２７０５号公報では、移動に大きく関与する軸についてのみ負荷を評価し、可変な加減速度を決定する手段を備えるものが開示されている。
【０００４】
しかし、実際は他軸からの干渉トルクにより、負荷が大きくなる軸があるケースがあり、これを評価しないままでいると、トルク不足が生ずる可能性がある。例えば、図４に示すように、Ｌ軸、Ｕ軸を同時に振り上げるときに、Ｂ軸の移動量が少ない場合に、Ｂ軸に発生する負荷トルクが評価の対象にならないとすると、Ｌ軸、Ｕ軸に発生するトルクから求められた加減速度で動作することになり、他軸からの干渉トルクによりＢ軸でトルク不足が発生する可能性がある。
そこで本発明は、ロボットの動作によって発生する負荷トルクを、他軸からの干渉トルクも含めて、全軸について評価して、最適な加減速時間を決定する方法および加減速制御方法を提供することを目的とする。
【０００５】
【課題を解決するための手段】
上記問題を解決するため、本発明は、ロボットの教示点の位置、姿勢や各軸の動作方向、動作速度によって、重力モーメント、加速度による慣性、他軸の動作による干渉トルクの影響を受ける駆動軸を有するロボットの加減速時間決定方法において、
質点質量と、前記ロボットの各腕について、回動軸と先端部の他腕回動軸あるいは前記質点との距離、質量、重心位置、前記ロボットの各駆動軸の許容ピークトルクをパラメータとして予め格納しておき、
各教示点で教示された各腕の角度、動作方向、及び動作速度からなる教示データと前記パラメータとから各軸についてｉを第ｉ駆動軸として前記ロボットのモデルに基づき運動方程式から導出される加減速度の影響を受ける負荷トルク成分（Ｔi）を演算して、加減速度の影響を受ける負荷トルク成分（Ｔi）の値から、駆動軸の許容ピークトルク値から加減速度の影響を受けない負荷トルク成分ｄiの値を差し引いた値で除した値である加速時の限界加速時間計算式（ａ）と減速時の限界減速時間計算式（ｂ）より加減速時間を演算することを特徴とするものである。
ｔai＝αi／（Ｔi−ｄi）  αi＞０ならばＴi＝τi，αi＜０ならばＴi＝−τi  …（ａ）
ｔdi＝−αi／（Ｔi−ｄi）  αi＞０ならばＴi＝−τi，αi＜０ならばＴi＝τi  …（ｂ）
ただし、ｔaiは第ｉ駆動軸の加速時間、ｔdiは第ｉ駆動軸の減速時間、αiは第ｉ駆動軸の加速度トルク成分であり前記運動方程式から導出される加減速度の影響を受ける負荷トルク成分の算出式を変形して得られる算出式に前記パラメータと各腕の角度を代入して求まる値、τiは第ｉ駆動軸の許容ピークトルクの絶対値、ｄiは第ｉ駆動軸の加減速度の影響を受けない負荷トルク成分であり前記運動方程式から導出される加減速度の影響を受ける負荷トルク成分の算出式を変形して得られる算出式に前記パラメータと各腕の角度と動作方向及び動作速度からなるからなる条件指定を代入して求まる値とする。
また、前記加減速時間は、各軸ごとに加減速時間を演算し、この中の最大値を加減速時間として前記ロボットの各軸を加減速制御することを特徴とするものである。
【０００６】
【作用】
上記手段により、発生する負荷トルクが駆動軸の許容ピークトルクとなるような加減速時間を求められるので、得られる加減速時間は、負荷トルクが許容ピークトルクを越えない範囲での最短の加減速時間となる。
【０００７】
【実施例】
以下、本発明の実施例を図に基づいて説明する。
図１は、本方法を実施するためのシステムの例を示すブロック図であり、図中１１は教示部、１２は教示データ格納エリア、１３はパラメータ格納エリア、１４は加減速時間演算部、１５は補間演算部、１６は駆動部である。
図２は、教示点とロボットの形態を示したスケルトン図で、教示点（Ｓ）を動作開始点、教示点（Ｅ）を動作終了点とする。
図３は、本実施例で使用するロボットの機構を表したスケルトン図で、３軸で構成される。１は大地に平行な軸回りに自由度を持つ第１軸、２は第１軸１回りに駆動する第１腕、３は第１腕２の先端部に設けられ、第１軸１に平行な軸回りに自由度を持つ第２軸、４は第２軸３回りに駆動する第２腕、５は第２腕４の先端部に設けられ、第２軸に平行な軸回りに自由度を持つ第３軸、６は第３軸５回りに駆動し、先端部に質点を有する第３腕である。各軸は１自由度ずつ有し、合計３自由度を有する、ＸＹ平面内を動作するロボットである。そして、各軸がそれぞれ、重力モーメント、加速度による慣性、他軸の動作による干渉トルク等の影響を受ける。
【０００８】
図３に示すモデルについて、ラグランジェの運動方程式から、動作時に第１軸１にかかる負荷トルクを導出すると、（１）式の形で表されることが知られている。
Ｔ１  ＝ｍ｛｛２ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋２ｃｏｓ  θ２  ｒ１  ｒ２  ＋２ｃｏｓ  θ３  ｒ２  ｒ３  ＋ｒ１  ２  ＋ｒ２  ２  ＋ｒ３  ２  ｝θ１”＋｛ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋ｃｏｓ  θ２  ｒ１  ｒ２  ＋２ｃｏｓ  θ３  ｒ２  ｒ３  ＋ｒ２  ２  ＋ｒ３  ２  ｝θ２  ”  ＋｛  ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋ｃｏｓ  θ３  ｒ２  ｒ３  ＋ｒ３  ２  ｝θ３  ”  −２ｒ１  ｛ｓｉｎ（θ２  ＋θ３  ）  ｒ３  ＋ｓｉｎ  θ２  ｒ２  ｝θ１  ’  θ２  ’  −２ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ１’θ３’−２ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ２’θ３’−ｒ１  ｛  ｓｉｎ（  θ２  ＋θ３）ｒ３  ＋ｓｉｎ  θ２  ｒ２  ｝θ２’２  −ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３）ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ３’２  −ｇ｛  ｃｏｓ（  θ１  ＋θ２  ＋θ３）ｒ３  ＋ｃｏｓ（θ１  ＋θ２）ｒ２  ＋ｃｏｓ  θ１  ｒ１  ｝｝    ……（１）
【０００９】
ここで、Ｔ1 は、第１軸１に発生する負荷トルク、θ1', θ2', θ3'は各軸の角速度、θ1", θ2", θ3"は角加速度、ｇは重力加速度を示す。腕の距離ｒ1,ｒ2,ｒ3 、質量ｍは、既知である。角速度θ1', θ2', θ3'、角度θ1,θ2,θ3 が与えられるとすれば、上記（１）式は、（２）式に示すように角加速度θ1", θ2", θ3"のみをパラメータとする式へと書き換えられる。
【００１０】
Ｔ１    ＝ａ１  θ１”＋ａ２  θ２”＋ａ３  θ３”＋ｄ１  ……（２）
ここで、
【００１１】
ａ１  ＝ｍ｛２ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋２ｃｏｓ  θ２  ｒ１  ｒ２  ＋２ｃｏｓ  θ３  ｒ２  ｒ３  ＋ｒ１  ２  ＋ｒ２  ２  ＋ｒ３  ２  ｝
【００１２】
ａ２  ＝ｍ｛ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋ｃｏｓ  θ２  ｒ１  ｒ２  ＋２ｃｏｓ  θ３  ｒ２  ｒ３  ＋ｒ２  ２  ＋ｒ３  ２  ｝
【００１３】
ａ３  ＝ｍ｛ｃｏｓ（θ２  ＋θ３）ｒ１  ｒ３  ＋  ｃｏｓθ３  ｒ２  ｒ３  ＋ｒ３  ２  ｝
【００１４】
ｄ１  ＝ｍ｛−２ｒ１  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ３  ＋ｓｉｎ  θ２  ｒ２  ｝θ１’θ２’−２ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ１’θ３’−２ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ２’θ３’−ｒ１  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ３  ＋ｓｉｎ  θ２  ｒ２  ｝θ２’２  −ｒ３  ｛  ｓｉｎ（  θ２  ＋θ３  ）  ｒ１  ＋ｓｉｎ  θ３  ｒ２  ｝θ３’２  −ｇ｛  ｃｏｓ（  θ１  ＋θ２  ＋θ３  ）  ｒ３  ＋ｃｏｓ（θ１  ＋θ２  ）  ｒ２  ＋ｃｏｓ  θ１  ｒ１  ｝｝
【００１５】
ロボットにおける加減速処理では、全軸の加減速時間を統一する（定常速度到達までの時間を等しくして、補間精度を保つため）。すなわち、ここで求めたいのは全軸共通の加減速時間である。
加速度＝速度／時間であるから、加速時間をｔとおくと、
θ＊  ”＝  θ＊  ’／  ｔ
とおけるので、次のように式が変形できる。
Ｔ１  ＝｛ａ１  θ１  ’＋ａ２  θ２  ’＋ａ３  θ３  ’｝／ｔ＋ｄ１  ……（３）
【００１６】
角速度θ＊  ’が既知であることから、｛｝内も係数と見ることが出来る。従って（３）式は次のように変形できる。
Ｔ１  ＝  α１  ／ｔ＋ｄ１              ……（４）
ここで、
α１  ＝  ａ１  θ１’＋ａ２  θ２’＋ａ３  θ３’
このα１  を加速度トルク成分と称す。
こうして、加速時間のみをパラメータとする負荷トルクを演算する式が導出できる。この式を加速時間ｔについて解くと、
ｔ＝α１  ／（Ｔ１  −ｄ１  ）        ……（５）
ここで、Ｔ１  に第１軸１の許容ピークトルクを代入して式を解くと、発生する負荷トルクが許容ピークトルクに等しくなるような加速時間（限界加速時間と称す）がｔに求められる。
負荷トルクはそのかかる方向により正または負の値で表される。そこで、状況により、正または負の値で許容ピークトルクを代入する必要がある。加速時には、加速度トルク成分が作用する方向の許容ピークトルクを採用する必要がある。従って、αの値の符号に合わせて許容ピークトルクの正負を決めれば、正しく解を得られる。
減速時には、加速時とは反対方向へ加速度トルク成分が作用する。従って、αの符号と反対の符号となるように許容ピークトルクの正負を決める。減速時間とは、負の加速時間であるから、（５）式に対して次のような式を用いて、発生する負荷トルクが許容ピークトルクに等しくなるような減速時間（限界減速時間と称す）を求める。
ｔ＝−α１  ／（Ｔ１  −ｄ１  ）    ……（５’）
第２軸３、第３軸５についても、ラグランジェの運動方程式から導出される負荷トルクを求める式が異なるだけで、上記と同様の手法により、それぞれ（５）式に相当する式を得ることが出来る。よって次の（６ａ）式、（６ｂ）式、（６ｃ）式、（７ａ）式、（７ｂ）式、（７ｃ）式が得られる。
【００１７】
（加速時）
ｔａ１＝α１  ／（Ｔ１  −ｄ１  ）    （α１  ＞０ならばＴ１  ＝τ１  ，α１  ＜０ならばＴ１  ＝−τ１  ）    …（６ａ）
ｔａ２＝α２  ／（Ｔ２  −ｄ２  ）    （α２  ＞０ならばＴ２  ＝τ２  ，α２  ＜０ならばＴ２  ＝−τ２  ）    …（６ｂ）
ｔａ３＝α３  ／（Ｔ３  −ｄ３  ）    （α３  ＞０ならばＴ３  ＝τ３  ，α３  ＜０ならばＴ３  ＝−τ３  ）    …（６ｃ）
【００１８】
（減速時）
ｔｄ１＝−α１  ／（Ｔ１  −ｄ１  ）  （α１  ＞０ならばＴ１  ＝−τ１  ，α１  ＜０ならばＴ１  ＝τ１  ）  …（７ａ）
ｔｄ２＝−α２  ／（Ｔ２  −ｄ２  ）    （α２  ＞０ならばＴ２  ＝−τ２  ，α２  ＜０ならばＴ２  ＝τ２  ）  …（７ｂ）
ｔｄ３＝−α３  ／（Ｔ３  −ｄ３  ）  （α３  ＞０ならばＴ３  ＝−τ３  ，α３  ＜０ならばＴ３  ＝τ３  ）  …（７ｃ）
ここで、τ１，τ２，τ３  は、各軸の許容ピークトルクの絶対値を表し、ｔａ１，  ｔａ２，  ｔａ３は、各軸における限界加速時間、ｔｄ１，  ｔｄ２，  ｔｄ３は、各軸における限界減速時間を表わす。
【００１９】
この（６ａ）〜（７ｃ）式を用いた加減速時間決定は、次の手順で行なう。まず、教示部１１において教示された教示位置、指令速度などは教示データ格納エリア１２に格納されており、この教示データ格納エリア１２から、動作開始点（Ｓ）における各軸角度θ1s, θ2s, θ3s、及び教示された指令速度によって動作するときの各軸の速度θ1s',θ2s',θ3s' を、加減速時間演算部１４に与える。加減速時間演算部１４では、これらのデータをそれぞれθ1,θ2,θ3 、θ1', θ2', θ3'に代入して、（６ａ）〜（６ｃ）式を演算する。この際、各軸の許容ピークトルクは、減速機の性能などによって異なる値であり、パラメータ格納エリア１３にその絶対値を予め格納しておき、それを与える。質点の質量ｍ、腕の距離ｒ1,ｒ2,ｒ3 も、パラメータ格納エリア１３に予め格納されているものを用いる。上記演算の結果得られた解ｔa1, ｔa2, ｔa3は、各軸ごとの限界加速時間であるから、この中から最大値を選択し、その値を適当にまるめて、加速時間ｔaccとして決定する。 続いて、上記と同様に教示データ格納エリア１２から、動作終了点（Ｅ）における各軸角度θ1e, θ2e, θ3e、指令速度によって動作するときの各軸の速度θ1e',θ2e',θ3e' を加減速時間演算部１４に与え、θ1,θ2,θ3 、θ1', θ2', θ3'に代入して（７ａ）〜（７ｃ）式を演算し、得られた各軸の限界減速時間ｔd1, ｔd2 ,ｔd3の中から、最大値をまるめて、減速時間ｔdecとして決定する。
【００２０】
こうして求められた加速時間ｔａｃｃ  、減速時間ｔｄｅｃ  を補間演算部１５へ与え、補間演算部１５において、動作開始ではｔａｃｃ  に従って加速し、終了時はｔｄｅｖ  に従って減速し動作を終了するような指令を生成し、駆動部１６へ送る。この加速時間ｔａｃｃ  、減速時間ｔｄｅｃ  にしたがって加減速制御を行なった場合、各軸に発生する負荷トルクはいずれも許容ピークトルクを越えない。加速終了時から減速開始時までは教示データ格納エリア１２から与えられた教示速度より補間演算部１５にて演算された補間速度でロボットは駆動される。
なお、ここではロボット先端部の質点のみを考慮してラグランジェの運動方程式を解いているが、ロボットアーム自身の質量などについては、集中マスのモデルを考え、各マスにより発生する負荷トルクを求め、最後に個別の負荷トルクを合計したものが、各軸にかかる負荷トルクとなる。従って、（５）式でのα１  、ｄ１  の項についても、同様に各マス毎の値を演算して合計をとればよい。すなわち下記（８）式に示すとおりである。
【００２１】
【数１】
0003596682.tif 000002
【００２２】
なお、上記では直列駆動型のロボットについて述べたが、並列駆動型（平行リンク型）のロボットについても、ラグランジェの運動方程式から導出される負荷トルクの演算式の形が異なるだけで、同様の手法が適用できる。
また、上記で、質点の質量はパラメータとしているが、実際のロボットの使用に際しては、ワークの質量が常に一定とは限らない。そこで、ロボット言語にてその時の実際の質量に見合ったデータを設定して教示データ格納エリア１２に格納しておき、そのデータを与えて演算を行なうと、より正確なトルク成分が求められ、それに基づいた加減速時間を求められるので、さらに動作性能の向上が図られる。
【００２３】
【発明の効果】
以上述べたように本発明によれば、ロボットの各軸において発生する負荷トルクが各軸の許容ピークトルクを越えない範囲での最短の加減速時間を求められるので、これを用いて加減速制御を行えば、低トルクでの動作時間の短縮、及び高トルクでの寿命向上を得られるという効果がある。
【図面の簡単な説明】
【図１】本発明の一実施例を示すブロック図
【図２】本実施例を説明するための教示位置とロボットのスケルトン図
【図３】本実施例で使用したロボットの構成を示すスケルトン図
【図４】従来例の問題を説明する図
【符号の説明】
１…第１軸
２…第１腕
３…第２軸
４…第２腕
５…第３軸
６…第３腕
１１…教示部
１２…教示データ格納エリア
１３…パラメータ格納エリア
１４…加減速時間演算部
１５…補間演算部
１６…駆動部
【図１】
0003596682.tif 000003
【図２】
0003596682.tif 000004
【図３】
0003596682.tif 000005
【図４】
0003596682.tif 000006