JP3596682B2 - ロボットの加減速時間決定方法 - Google Patents

Description

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

【００２２】
なお、上記では直列駆動型のロボットについて述べたが、並列駆動型（平行リンク型）のロボットについても、ラグランジェの運動方程式から導出される負荷トルクの演算式の形が異なるだけで、同様の手法が適用できる。
また、上記で、質点の質量はパラメータとしているが、実際のロボットの使用に際しては、ワークの質量が常に一定とは限らない。そこで、ロボット言語にてその時の実際の質量に見合ったデータを設定して教示データ格納エリア１２に格納しておき、そのデータを与えて演算を行なうと、より正確なトルク成分が求められ、それに基づいた加減速時間を求められるので、さらに動作性能の向上が図られる。
【００２３】
【発明の効果】
以上述べたように本発明によれば、ロボットの各軸において発生する負荷トルクが各軸の許容ピークトルクを越えない範囲での最短の加減速時間を求められるので、これを用いて加減速制御を行えば、低トルクでの動作時間の短縮、及び高トルクでの寿命向上を得られるという効果がある。
【図面の簡単な説明】
【図１】本発明の一実施例を示すブロック図
【図２】本実施例を説明するための教示位置とロボットのスケルトン図
【図３】本実施例で使用したロボットの構成を示すスケルトン図
【図４】従来例の問題を説明する図
【符号の説明】
１…第１軸
２…第１腕
３…第２軸
４…第２腕
５…第３軸
６…第３腕
１１…教示部
１２…教示データ格納エリア
１３…パラメータ格納エリア
１４…加減速時間演算部
１５…補間演算部
１６…駆動部

Claims (2)

1. ロボットの教示点の位置、姿勢や各軸の動作方向、動作速度によって、重力モーメント、加速度による慣性、他軸の動作による干渉トルクの影響を受ける駆動軸を有するロボットの加減速時間決定方法において、
   質点質量と、前記ロボットの各腕について、回動軸と先端部の他腕回動軸あるいは前記質点との距離、質量、重心位置、前記ロボットの各駆動軸の許容ピークトルクをパラメータとして予め格納しておき、
   各教示点で教示された各腕の角度、動作方向、及び動作速度からなる教示データと前記パラメータとから各軸についてｉを第ｉ駆動軸として前記ロボットのモデルに基づき運動方程式から導出される加減速度の影響を受ける負荷トルク成分（Ｔ_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は第ｉ駆動軸の加減速度の影響を受けない負荷トルク成分であり前記運動方程式から導出される加減速度の影響を受ける負荷トルク成分の算出式を変形して得られる算出式に前記パラメータと各腕の角度と動作方向及び動作速度からなるからなる条件指定を代入して求まる値とする。）
2. 前記加減速時間は、各軸ごとに加減速時間を演算し、この中の最大値を加減速時間として前記ロボットの各軸を加減速制御することを特徴とする請求項１記載のロボットの加減速時間決定方法。

Images