JP3596682B2

JP3596682B2 - Robot acceleration / deceleration time determination method

Info

Publication number: JP3596682B2
Application number: JP7249094A
Authority: JP
Inventors: 洋和仮屋崎
Original assignee: Yaskawa Electric Corp
Current assignee: Yaskawa Electric Corp
Priority date: 1994-03-16
Filing date: 1994-03-16
Publication date: 2004-12-02
Anticipated expiration: 2019-12-02
Also published as: JPH07261822A

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として決定する。
【００２０】
こうして求められた加速時間ｔ_ａｃｃ、減速時間ｔ_ｄｅｃを補間演算部１５へ与え、補間演算部１５において、動作開始ではｔ_ａｃｃに従って加速し、終了時はｔ_ｄｅｖに従って減速し動作を終了するような指令を生成し、駆動部１６へ送る。この加速時間ｔ_ａｃｃ、減速時間ｔ_ｄｅｃにしたがって加減速制御を行なった場合、各軸に発生する負荷トルクはいずれも許容ピークトルクを越えない。加速終了時から減速開始時までは教示データ格納エリア１２から与えられた教示速度より補間演算部１５にて演算された補間速度でロボットは駆動される。
なお、ここではロボット先端部の質点のみを考慮してラグランジェの運動方程式を解いているが、ロボットアーム自身の質量などについては、集中マスのモデルを考え、各マスにより発生する負荷トルクを求め、最後に個別の負荷トルクを合計したものが、各軸にかかる負荷トルクとなる。従って、（５）式でのα_１、ｄ_１の項についても、同様に各マス毎の値を演算して合計をとればよい。すなわち下記（８）式に示すとおりである。
【００２１】
【数１】

【００２２】
なお、上記では直列駆動型のロボットについて述べたが、並列駆動型（平行リンク型）のロボットについても、ラグランジェの運動方程式から導出される負荷トルクの演算式の形が異なるだけで、同様の手法が適用できる。
また、上記で、質点の質量はパラメータとしているが、実際のロボットの使用に際しては、ワークの質量が常に一定とは限らない。そこで、ロボット言語にてその時の実際の質量に見合ったデータを設定して教示データ格納エリア１２に格納しておき、そのデータを与えて演算を行なうと、より正確なトルク成分が求められ、それに基づいた加減速時間を求められるので、さらに動作性能の向上が図られる。
【００２３】
【発明の効果】
以上述べたように本発明によれば、ロボットの各軸において発生する負荷トルクが各軸の許容ピークトルクを越えない範囲での最短の加減速時間を求められるので、これを用いて加減速制御を行えば、低トルクでの動作時間の短縮、及び高トルクでの寿命向上を得られるという効果がある。
【図面の簡単な説明】
【図１】本発明の一実施例を示すブロック図
【図２】本実施例を説明するための教示位置とロボットのスケルトン図
【図３】本実施例で使用したロボットの構成を示すスケルトン図
【図４】従来例の問題を説明する図
【符号の説明】
１…第１軸
２…第１腕
３…第２軸
４…第２腕
５…第３軸
６…第３腕
１１…教示部
１２…教示データ格納エリア
１３…パラメータ格納エリア
１４…加減速時間演算部
１５…補間演算部
１６…駆動部[0001]
[Industrial applications]
The present invention relates to a method for determining the acceleration / deceleration time of a drive shaft that is affected by the inertia due to load inertia or acceleration, the interference torque due to the operation of another axis, etc., depending on the position and posture of the teaching point of the robot, the operation direction and operation speed of each axis. about the.
[0002]
[Prior art]
In general, when a robot performing a path operation operates at a high speed, it performs acceleration / deceleration control to obtain a smooth motion. As a method of determining the acceleration / deceleration time when performing the acceleration / deceleration control, a method has been adopted in which the load inertia is calculated from the position and the attitude, and the acceleration / deceleration time constant is determined based on the load inertia (Japanese Patent Laid-Open No. Hei 5-5-2). No. 46234).
[0003]
[Problems to be solved by the invention]
However, in the prior art, the interference torque, centrifugal force, Coriolis force, and the like generated by the operation of the other axis are not considered as the components of the torque applied to the drive shaft during the actual operation of the robot. Therefore, there is a problem that the same acceleration / deceleration time is determined if the posture is the same even when the torque generated by the operation is different.
On the other hand, Japanese Patent Laying-Open No. 4-282705 discloses an apparatus having a means for evaluating a load only on an axis largely involved in movement and determining a variable acceleration / deceleration.
[0004]
However, actually, there is a case where there is an axis whose load is increased due to an interference torque from another axis, and if this is not evaluated, there is a possibility that a torque shortage may occur. For example, as shown in FIG. 4, when the L-axis and the U-axis are simultaneously swung up and the amount of movement of the B-axis is small, if the load torque generated on the B-axis is not to be evaluated, The actuator operates at the acceleration / deceleration determined from the torque generated on the U-axis, and there is a possibility that torque shortage may occur on the B-axis due to interference torque from another axis.
Therefore, the present invention provides a method for evaluating an acceleration / deceleration time and an acceleration / deceleration control method by evaluating a load torque generated by an operation of a robot, including an interference torque from another axis, for all axes. With the goal.
[0005]
[Means for Solving the Problems]
In order to solve the above problems, the present invention provides a drive shaft which is affected by the gravitational moment, inertia due to acceleration, and interference torque due to the operation of other axes, depending on the position and posture of the teaching point of the robot, the operation direction and operation speed of each axis. The method for determining the acceleration / deceleration time of a robot having
The mass, the mass, the center of gravity, and the allowable peak torque of each drive shaft of the robot are stored in advance as parameters for the mass of the mass, and the distance , mass, center of gravity, and the distance between the mass axis and the pivot axis and the other arm pivot axis of the tip or the other arm. Aside,
An adjustment derived from a motion equation based on the robot model using i as the i-th drive axis for each axis from the teaching data including the angle , the movement direction, and the movement speed of each arm taught at each teaching point and the parameters. The load torque component (T _i ) affected by the speed is calculated, and the load not affected by the acceleration / deceleration from the allowable peak torque value of the drive shaft is calculated from the value of the load torque component (T _i ) affected by the acceleration / deceleration. and wherein computing the acceleration and deceleration time than the limit acceleration time calculation formula at the time of acceleration is a value obtained by dividing the value obtained by subtracting the value of the torque component d _i (a) the limit deceleration time calculation formula for deceleration (b) Is what you do.
t _ai = α _i / (T _i −d _i ) If α _i > 0, T _i = τ _i , if α _i <0, T _i = −τ _i (a)
t _di = −α _i / (T _i −d _i ) If α _i > 0, T _i = −τ _i , if α _i <0, T _i = τ _i (b)
Where t _ai is the acceleration time of the i-th drive shaft, t _di is the deceleration time of the i-th drive shaft, α _i is the acceleration torque component of the i-th drive shaft, and is affected by the acceleration / deceleration derived from the equation of motion. the parameters and values obtained by substituting the angle of each arm to the calculation formula obtained by modifying the equation for calculating the load torque component, tau _i is the absolute value of the permissible peak torque of the i drive shaft, d _i is the i drive The parameters and the angle of each arm are calculated in a calculation formula obtained by modifying a calculation formula of a load torque component which is a load torque component which is not affected by the acceleration / deceleration of the shaft and which is affected by the acceleration / deceleration derived from the equation of motion. The value is obtained by substituting the condition specification consisting of the operation direction and the operation speed.
The acceleration / deceleration time is obtained by calculating the acceleration / deceleration time for each axis, and performing acceleration / deceleration control on each axis of the robot using the maximum value of the time as the acceleration / deceleration time.
[0006]
[Action]
By the above means, the acceleration / deceleration time is determined so that the generated load torque becomes the allowable peak torque of the drive shaft. Therefore, the obtained acceleration / deceleration time is the shortest acceleration / deceleration within the range where the load torque does not exceed the allowable peak torque. Time.
[0007]
【Example】
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
FIG. 1 is a block diagram showing an example of a system for carrying out the present method. In the figure, reference numeral 11 denotes a teaching section, 12 denotes a teaching data storage area, 13 denotes a parameter storage area, 14 denotes an acceleration / deceleration time calculation section, and 15 denotes an acceleration / deceleration time calculation section. Denotes an interpolation operation unit, and 16 denotes a driving unit.
FIG. 2 is a skeleton diagram showing the teaching points and the form of the robot. The teaching point (S) is the operation start point, and the teaching point (E) is the operation end point.
FIG. 3 is a skeleton diagram showing the mechanism of the robot used in the present embodiment, and is composed of three axes. 1 is a first axis having a degree of freedom about an axis parallel to the ground, 2 is a first arm driven around the

first axis

1, and 3 is provided at the tip of the first arm 2 and is parallel to the first axis 1. The second axis 4 having a degree of freedom around the second axis 4 is a second arm driven around the second axis 3, and the fifth arm 5 is provided at the tip of the second arm 4, and has a degree of freedom about an axis parallel to the second axis. A third arm 6 having a third arm 6 is driven around the third axis 5 and has a mass at its tip. Each axis has one degree of freedom, and has a total of three degrees of freedom, and is a robot operating in the XY plane. Each axis is affected by the gravitational moment, inertia due to acceleration, interference torque due to movement of other axes, and the like.
[0008]
With respect to the model shown in FIG. 3, it is known that when the load torque applied to the first shaft 1 during operation is derived from Lagrange's equation of motion, the load torque is expressed in the form of equation (1).
_{T 1 = m {{2cos (} θ 2 + θ 3) r 1 r 3 + 2cos θ 2 r 1 r 2 + 2cos θ 3 r 2 r 3 + r 1 2 + r 2 2 + r 3 2} θ 1 "+ {cos (θ 2 _{_{_{+ θ 3) r 1 r 3}}} + cos θ 2 r 1 r 2 + 2cos θ 3 r 2 r 3 + r 2 2 + r 3 2} θ 2 "+ {cos (θ 2 + θ 3) r 1 r 3 + cos θ 3 r 2 r _{_{^{_{3 + r 3 2} θ 3}}}} "-2r 1 {sin (θ 2 + θ 3) r 3 + sin θ 2 r 2} θ 1 'θ 2' -2r 3 {sin (θ 2 + θ 3) r 1 + sin θ 3 r ₂ ｝ θ ₁ 'θ ₃ ' -2r ₃ ｛sin (θ ₂ + θ ₃ ) r ₁ + sin θ ₃ r ₂ ｝ θ ₂ 'θ ₃ ' -r ₁ ｛sin (θ ₂ + θ ₃ ) r ₃ + sin θ ₂ r ₂ ｝ θ ₂ ′ ² -r ₃ ｛sin (θ ₂ + θ ₃ ) r ₁ + sin θ ₃ r ₂ {θ ₃ ' ² -g} cos (θ ₁ + θ ₂ + θ ₃ ) r ₃ + cos (θ ₁ + θ ₂ ) r ₂ + cos θ ₁ r ₁ } (1)
[0009]
Here, T ₁ is the load torque generated on the first axis 1, θ ₁ ′, θ ₂ ′, θ ₃ ′ are the angular velocities of each axis, θ ₁ ″, θ ₂ ″, θ ₃ ″ are the angular acceleration, g Indicates the gravitational acceleration, the arm distances r ₁ , r ₂ , r ₃ and the mass m are known, given the angular velocities θ ₁ ′, θ ₂ ′, θ ₃ ′ and the angles θ ₁ , θ ₂ , θ _3. If so, the above equation (1) can be rewritten as an equation using only the angular accelerations θ ₁ ”, θ ₂ ”, and θ ₃ ”as shown in the equation (2).
[0010]
T ₁ = a ₁ θ ₁ ″ + a ₂ θ ₂ ″ + a ₃ θ ₃ ″ + d ₁ (2)
here,
[0011]
_{a 1 = m {2cos (θ} 2 + θ 3) r 1 r 3 + 2cos θ 2 r 1 r 2 + 2cos θ 3 r 2 r 3 + r 1 2 + r 2 2 + r 3 2}
[0012]
_{a 2 = m {cos (θ} 2 + θ 3) r 1 r 3 + cos θ 2 r 1 r 2 + 2cos θ 3 r 2 r 3 + r 2 2 + r 3 2}
[0013]
_{a 3 = m {cos (θ} 2 + θ 3) r 1 r 3 + cosθ 3 r 2 r 3 + r 3 2}
[0014]
d ₁ = m ｛−2r ₁ ｛sin (θ ₂ + θ ₃ ) r ₃ + sin θ ₂ r ₂ ｝ θ ₁ 'θ ₂ ' -2r ₃ ｛sin (θ ₂ + θ ₃ ) r ₁ + sin θ ₃ r ₂ ｝ θ _{_{_{1 'θ 3' -2r 3 {}}} sin (θ 2 + θ 3) r 1 + sin θ 3 r 2} θ 2 'θ 3' -r 1 {sin (θ 2 + θ 3) r 3 + sin θ 2 r 2} θ _{^{_{2 '2 -r 3 {sin (}}} θ 2 + θ 3) r 1 + sin θ 3 r 2} θ 3' 2 -g {cos (θ 1 + θ 2 + θ 3) r 3 + cos (θ 1 + θ 2) r 2 + cos θ ₁ r ₁ ｝｝
[0015]
In the acceleration / deceleration processing in the robot, the acceleration / deceleration times of all axes are unified (in order to maintain the interpolation accuracy by equalizing the time until reaching the steady speed). That is, what is desired here is the acceleration / deceleration time common to all axes.
Since acceleration = speed / time, if the acceleration time is t,
θ ^* ″ = θ ^* '/ t
Then, the equation can be transformed as follows.
T ₁ = {a ₁ θ ₁ ′ + a ₂ θ ₂ ′ + a ₃ θ ₃ ′} / t + d ₁ (3)
[0016]
Since the angular velocity θ ^* ′ is known, the inside of ｛｝ can be regarded as a coefficient. Therefore, equation (3) can be modified as follows.
T ₁ = α ₁ / t + d ₁ (4)
here,
_{_{_{α 1 = a 1 θ 1 '}}} + a 2 θ 2' + a 3 θ 3 '
The alpha ₁ is referred to as the acceleration torque component.
In this way, an expression for calculating the load torque using only the acceleration time as a parameter can be derived. Solving this equation for the acceleration time t gives:
t = α ₁ / (T ₁ −d ₁ ) (5)
Here, by substituting the allowable peak torque of the first shaft ₁ into T ₁ and solving the equation, an acceleration time (referred to as a limit acceleration time) at which the generated load torque becomes equal to the allowable peak torque is obtained for t. .
The load torque is represented by a positive or negative value depending on the direction. Therefore, depending on the situation, it is necessary to substitute the allowable peak torque with a positive or negative value. At the time of acceleration, it is necessary to employ an allowable peak torque in the direction in which the acceleration torque component acts. Therefore, if the sign of the allowable peak torque is determined in accordance with the sign of the value of α, a correct solution can be obtained.
At the time of deceleration, an acceleration torque component acts in a direction opposite to that at the time of acceleration. Therefore, the sign of the allowable peak torque is determined so that the sign is opposite to the sign of α. Since the deceleration time is a negative acceleration time, a deceleration time (referred to as a critical deceleration time) in which the generated load torque becomes equal to the allowable peak torque is obtained by using the following equation with respect to the equation (5). ).
t = −α ₁ / (T ₁ −d ₁ ) (5 ′)
With respect to the second axis 3 and the third axis 5 as well, only equations for obtaining the load torque derived from the Lagrange's equation of motion are different, and equations corresponding to equation (5) are obtained by the same method as described above. Can be done. Therefore, the following equations (6a), (6b), (6c), (7a), (7b), and (7c) are obtained.
[0017]
(When accelerating)
t _a1 = α ₁ / (T ₁ −d ₁ ) (If α ₁ > 0, T ₁ = τ ₁ , if α ₁ <0, T ₁ = −τ ₁ ) (6a)
_{_{_{t a2 = α 2 / (T}}} 2 -d 2) (α 2> 0 if _{_{_{T 2 = τ 2, α 2}}} <0 if _{_{T 2 = -τ 2) ... (}} 6b)
_{_{_{t a3 = α 3 / (T}}} 3 -d 3) (α 3> 0 if _{_{_{T 3 = τ 3, α 3}}} <0 if _{_{T 3 = -τ 3) ... (}} 6c)
[0018]
(During deceleration)
t _d1 = −α ₁ / (T ₁ −d ₁ ) (T ₁ = −τ ₁ if α ₁ > 0, T ₁ = τ ₁ if α ₁ <0) (7a)
t _d2 = −α ₂ / (T ₂ −d ₂ ) (T ₂ = −τ ₂ if α ₂ > 0, T ₂ = τ ₂ if α ₂ <0) (7b)
t _d3 = −α ₃ / (T ₃ −d ₃ ) (T ₃ = −τ ₃ if α ₃ > 0, T ₃ = τ ₃ if α ₃ <0) (7c)
Here, τ ₁ , τ ₂ , τ ₃ represent the absolute value of the allowable peak torque of each axis, t _a1 , t _a2 , and ta ₃ are the limit acceleration times for each axis, t _d1 , t _d2 , t _d3. Represents a limit deceleration time in each axis.
[0019]
The determination of the acceleration / deceleration time using the equations (6a) to (7c) is performed in the following procedure. First, the teaching position, command speed, and the like taught by the teaching unit 11 are stored in the teaching data storage area 12, and from the teaching data storage area 12, the respective axis angles θ _1s , θ _{2s at the} operation start point (S) are _read. , θ _3s , and the speeds θ _1s ′, θ _2s ′, θ _3s ′ of the respective axes when operating according to the instructed command speed, are given to the acceleration / deceleration time calculation unit 14. The acceleration / deceleration time calculation unit 14 substitutes these data into θ ₁ , θ ₂ , θ ₃ , θ ₁ ′, θ ₂ ′, and θ ₃ ′ to calculate the equations (6a) to (6c). At this time, the allowable peak torque of each axis is a value that differs depending on the performance of the speed reducer and the like, and its absolute value is stored in the parameter storage area 13 in advance and given. The mass m of the mass point and the distances r ₁ , r ₂ , r _{3 of the} arms are also stored in the parameter storage area 13 in advance. Solution t _a1 obtained as a result of the above calculation, t _a2, t _a3, since the limit acceleration time for each axis, selects the maximum value from among these, by rounding the value appropriately, acceleration time t _acc To be determined. Subsequently, similarly to the above, from the teaching data storage area 12, the axis angles θ _1e , θ _2e , θ _{3e at} the operation end point (E), and the speeds θ _1e ′, θ _2e of the respective axes when operating according to the command speed. ', θ _3e ' are given to the acceleration / deceleration time calculation unit 14 and substituted into θ ₁ , θ ₂ , θ ₃ , θ ₁ ', θ ₂ ', θ ₃ 'to calculate the equations (7a) to (7c). The maximum value is rounded out of the obtained limit deceleration times t _d1 , t _d2 , and t _d3 of each axis and determined as the deceleration time t _dec .
[0020]
The acceleration time t _acc and the deceleration time t _dec obtained in this way are given to the interpolation calculation unit 15, and the interpolation calculation unit 15 accelerates according to t _{acc at the} start of the operation, decelerates according to t _dev at the end of the operation, and ends the operation. A command is generated and sent to the drive unit 16. The acceleration time t _acc, when subjected to acceleration and deceleration control according to the deceleration time t _dec, any load torque generated each axis does not exceed the permissible peak torque. From the end of acceleration to the start of deceleration, the robot is driven at the interpolation speed calculated by the interpolation calculation unit 15 from the teaching speed given from the teaching data storage area 12.
Here, Lagrange's equation of motion is solved taking into account only the mass of the robot tip, but for the mass of the robot arm itself, consider the model of concentrated mass and calculate the load torque generated by each mass. Finally, the sum of the individual load torques is the load torque applied to each axis. Therefore, for the terms of α ₁ and d _{1 in} the equation (5), the values of the respective cells may be similarly calculated and summed. That is, it is as shown in the following equation (8).
[0021]
(Equation 1)

[0022]
Although the series-driven robot has been described above, the same applies to the parallel-driven (parallel link) robot, except that the form of the load torque calculation formula derived from Lagrange's equation of motion is different. Techniques can be applied.
In the above description, the mass of the mass point is used as a parameter. However, when an actual robot is used, the mass of the workpiece is not always constant. Therefore, if data corresponding to the actual mass at that time is set in the robot language and stored in the teaching data storage area 12 and the calculation is performed by giving the data, a more accurate torque component is obtained. Since an acceleration / deceleration time based on the acceleration / deceleration time can be obtained, the operation performance can be further improved.
[0023]
【The invention's effect】
As described above, according to the present invention, since the shortest acceleration / deceleration time within a range in which the load torque generated in each axis of the robot does not exceed the allowable peak torque of each axis can be obtained, acceleration / deceleration control is performed using this. Is effective in shortening the operation time at low torque and improving the life at high torque.
[Brief description of the drawings]
FIG. 1 is a block diagram showing an embodiment of the present invention; FIG. 2 is a skeleton diagram of a teaching position and a robot for explaining the embodiment; FIG. 3 is a skeleton diagram showing a configuration of a robot used in the embodiment; FIG. 4 is a diagram for explaining a problem of a conventional example.
1 1st axis 2 1st arm 3 2nd axis 4 2nd arm 5 3rd axis 6 3rd arm 11 teaching unit 12 teaching data storage area 13 parameter storage area 14 acceleration / deceleration time Arithmetic unit 15 ... Interpolation arithmetic unit 16 ... Drive unit

Claims

In the method of determining the acceleration / deceleration time of a robot having a drive axis that is affected by the gravitational moment, inertia due to acceleration, and interference torque due to the movement of another axis, depending on the position, posture, and operation direction of each axis, and the operation speed of each teaching point of the robot,
The mass, the mass, the center of gravity, and the allowable peak torque of each drive shaft of the robot are stored in advance as parameters for the mass of the mass, and the distance , mass, center of gravity, and the distance between the mass axis and the pivot axis and the other arm pivot axis of the tip or the other arm. Aside,
An adjustment derived from a motion equation based on the robot model using i as the i-th drive axis for each axis from the teaching data including the angle , the movement direction, and the movement speed of each arm taught at each teaching point and the parameters. The load torque component (T _i ) affected by the speed is calculated, and the load not affected by the acceleration / deceleration from the allowable peak torque value of the drive shaft is calculated from the value of the load torque component (T _i ) affected by the acceleration / deceleration. and wherein computing the acceleration and deceleration time than the limit acceleration time calculation formula at the time of acceleration is a value obtained by dividing the value obtained by subtracting the value of the torque component d _i (a) the limit deceleration time calculation formula for deceleration (b) Method for determining the acceleration / deceleration time of a running robot.
t _ai = α _i / (T _i −d _i ) If α _i > 0, T _i = τ _i , if α _i <0, T _i = −τ _i (a)
t _di = −α _i / (T _i −d _i ) If α _i > 0, T _i = −τ _i , if α _i <0, T _i = τ _i (b)
(Where t _ai is the acceleration time of the i-th drive shaft, t _di is the deceleration time of the i-th drive shaft, α _i is the acceleration torque component of the i-th drive shaft, and the influence of the acceleration / deceleration derived from the equation of motion is shown below. receiving load torque the parameters and values obtained by substituting the angle of each arm to the calculation formula obtained by modifying the equation for calculating component, tau _i is the absolute value of the permissible peak torque of the i drive shaft, d _i is the i The parameter and the angle of each arm are obtained by modifying the calculation formula of the load torque component which is not affected by the acceleration / deceleration of the drive shaft and is affected by the acceleration / deceleration derived from the equation of motion. The value is obtained by substituting the condition specification consisting of the following and the operation direction and operation speed.)

The robot according to claim 1, wherein the acceleration / deceleration time is calculated for each axis, and acceleration / deceleration control of each axis of the robot is performed using the maximum value of the acceleration / deceleration time as the acceleration / deceleration time. Deceleration time determination method.