WO1996015480A1

WO1996015480A1 - Method of setting time constant in planning path of robot

Info

Publication number: WO1996015480A1
Application number: PCT/JP1995/002276
Authority: WO
Inventors: Ryuichi Hara; Atsuo Nagayama
Original assignee: Fanuc Corp
Current assignee: Fanuc Corp
Priority date: 1994-11-09
Filing date: 1995-11-07
Publication date: 1996-05-23
Anticipated expiration: 1997-05-09
Also published as: JPH08137524A; KR960018817A; EP0744677A1; US5751130A; EP0744677A4

Description

明細香

ロボットの軌道計画時における時定数の設定方法

技術分野

本発明は、産業用ロボットの軌道計画時に決定すべき、ロボットの各軸を駆動するサーボモ一夕の加減速時定数の決定方法に関する。

背景技術

現在、産業用ロボット（以下、単に「ロボット」と言う）の軌道計画時には、一つの動作毎に一定の時定数を設定し、その時定数のフィルタを掛けて滑らかな加減速動作をロボット各軸を駆動するサーボモー夕（以下、単に「モー夕」と言う。）に実現させる方式が一般的に用いられている。時定数を設定するに際しては、ロボット作業のサイクルタイムを短縮するために、最大加速度制御の考え方が適用されることが通常である。

従来の最大加速度制御では、動作中の各時点において各軸のモ一夕にかかるモーメント、イナ一シャを各軸個別のものとして計算して最大加速度が算出され、その結果に基づき、サーポモータが最大トルクを発生するように時定数が設定される。

しかし、ロボットは一般に複数の軸が同時に動作して目標軌道上の運動を行なうものであるから、口ポットの各軸には、他の軸の動作に起因したトルク（干渉トルク）が作用する。従って、従来のやり方では、各軸のモー夕が計算上最大トルクを発生するように時定数が設定されていても、干渉トルクのためにトルクの飽和現象が生じたり、逆に、小さなトルクを発生した状態でロボットが動作したりすることが起こる。

トルクの飽和現象が発生するとロボッ卜の軌跡精度が悪化する。また、使用トルクが小さい状態でロボットを動作させることは、ロボットの性能を十分に発揮させてサイクルタイムの短縮化が十分に行なわれていないことを意味する。このように、従来のロボットの軌道計画方法においては、干渉トルク分を考慮に入れて時定数を定めることが行なわれていないことに起因した問題の発生が避けられなかった。

発明の開示

本発明の目的は、それぞれサーボモー夕によって駆動される複数の軸を有するロボットの軌道計画時に設定されるべき加減速時定数を、他の軸が動作するために生じる干渉トルクを考慮にいれて設定することにより、ロボットの軌精度の向上とサイクルタイムの短縮を図ることめ。

上記目的を達成するため、本発明の 1 態様は、各々サーボモータによって駆動される複数の軸を有するロボットの軌道計画時における時定数の設定方法において、始点及び終点における各軸の速度が 0 とみなすことが出来る動作区間の始点及び終点において、前記軸の内少なくとも 2つの軸の間の干渉トルクの影 «を入れて発生トルクの飽和を起こさない条件で最短の加減速制御の時定数を前記少なくとも 2つの軸について前記ロボットを制御するロボット制御装置を用いたオンラインのソフトゥェァ処理によって求め、前記ロボットを制御するロボット制御装置に設定する。

また、本発明の別の態様は、各々サーボモー夕によつて駆動される複数の軸を有するロボットの軌道計画時における時定数の設定方法において、始点及び終点における各軸の速度が 0 とみなすことが出来る動作区間の始点及び終点において、前記軸の内少なくとも 2つの軸の間の干渉トルクの影 Sを入れて発生トルクの飽和を起こさない条件で最短の加減速制御の時定数を前記少なくとも 2つの軸について前記ロボットを制御する口ボット制御装置を用いたオンラインのソフトウ Xァ処理によって求め、その内の最長の時定数を全軸共通の時定数として前記ロボットを制御するロボット制御装置に設定する。

望ましくは、上記ソフトゥ -ァ処理が、ロボットの運動方程式に基づいて決定される干渉トルクの影 «を評価した条件で、目搮軌道を実現するための各軸の拘束条件と、ロボットの最大トルクによる拘束条件とを同時に満たす時定数の条件を求める処理を含む。

本発明は上記構成を有することによって、軌道計画時に短すぎる時定数が設定されることによって生じるモー夕発生トルクの飽和現象と、長すぎる時定数が設定されることによって生じる性能発揮不足の現象のいずれをも抑制することが出来る。従って、ロボットの軌跡精度の向上とサイクルタイムの短縮が図られる。

図面の簡単な説明

図 1 は本発明の方法が適用される 4軸水平多関節ロボットの軸構成を表わした図、

図 2は本実施例における時定数の設定をオンラインで実施するために使用されるロボット制御装置の代表的な構成を要部ブロック図で示したもの、及び、

図 3は本実施例における時定数の設定のための処理の概要を記したフローチヤ一トである。

発明を実施するための最良の形態

まず、本発明による時定数の設定原理について説明する。

本発明は、従来の最大加速度制御の考え方を生かしつつ、他軸からの干渉トルクを考慮に入れた時定数の設定を行なおうとするものである。即ち、本発明においては, 軌道計画時に次の三つの条件に基づいて干渉トルクを考慮に入れた時定数が決定される。

( 1 ) ロボットの運動方程式：考慮される干渉トルクは, この条件を介して時定数の決定内容に反映される。

( 2 ) 目標軌道を実現するための各軸の拘束条件式：目搮軌道とロボットの構造パラメ一夕によって決まる条件であり、具体的には、各軸の空間座標（ X Y Z ) と軸空間座標（ 0 1 Θ 2 · · θ N ) の変換関係を規定するヤコビ行列で記述される（後述実施例参照）。

( 3 ) ロボットの最大トルクによる拘束条件：モータの性能の条件である。

以上、三つの条件の内、（ 2 ) ，（ 3 ) は従来の最大加速度制御における時定数算出にも当然使用されていた条件である。しかし、干渉トルクを考慮するために、他軸の運動状態の記述を含むロボットの運動方程式を時定数算出条件に取り入れることは、本発明においてはじめて行われたことである。

ところで、干涉トルクは他軸の運動状態に依存して决まる量であるから、厳密には、ロボットの運動に従って刻々と変化する。しかし、干渉トルクの影響が最も問題となるのは、加減速制御が行なわれる運動区間の始点付近及び終点付近であり、その主因は加速度にある（始点及び終点における速度は 0 または干涉トルクへの寄与が無視出来る程度に小さいとする）。

そこで、本発明では、軌道計画の中で上記三条件に基づいて時定数を決定する処理を合理的な処理負担の範囲内に抑えてオンラインで行なうために、運動区間の始点と終点の二点について、他の軸の加速度によって生じる干渉トルクのみを考慮して干渉トルクの計算処理を実行する。

本発明では、口ポットの軌道計画時に干渉トルクを考慮に入れて時定数が設定されるので、モータ発生トルクの飽和現象や性能発揮不足の現象が抑制され、ロボットの軌跡精度の向上とサイクルタイムの短縮が実現される c 次に本発明の時定数の設定方法について、ロボットが図 1 に示したような 4軸水平多関節ロボットの場合を例に説明する

図 1 において、符号 R Bで全体的に指示された 4軸水平多関節ロボットは、ベース B側から順に、第 1 軸 1 〜第 4軸 4 を有している。第 1 軸 1 は鉛直方向の直動軸、第 2軸 2、第 3軸 3は各々水平面内で動作するリンクを駆動する旋回軸である。また、第 4軸 4 は、水平面内でロボットの手先のエンドエフ X クタ 5 を回転させる軸である。符号 6はエンドエフクタ 5の先端位置に設定されたツールセンタポイントを表わしている。軸変数（第 2軸及び第 3軸については図示された角度）を J 1 〜 J 4 で表わすこととする。なお、 ∑ 0 はロボット R Bが動作する 3次元直交座標系を表わしている。

一般に、干渉トルクの影 «を及ぼし合い易いのは、ベース B側に近い旋回軸同士である。そこで、本実施例では、このようなロボットの軌道計画を立てるに際し、第 2軸 2及び第 3軸 3の干渉トルクを考慮に入れて時定数を定めることを考える。

前記条件（ 1 ) 〜（ 3 ) を具体的に記述すれば、次のようになる。

( 1 ) ロボットの運動方程式；

第 2軸及び第 3軸の運動方程式は一般に次の式 [ 1 ] ,

[ 2 ] で与えられる。

T 2= I 2* J 2- C 2* ( J 2+ J 3)

+ f ( J 2, J 3, J 2, J 3) [ 1 ] T 3= I 3* J 3- C 3* ( J 2+ J 3)

+ g ( J 2, J 3, J 2, J 3) [ 2 ] ここで、記号の意味は次の通りである（ i = 2 or 3) 。

T i ；第 i 軸の発生トルク

I i ；第 i 軸のイナーシャ

C i ；第 i 軸の干渉トルクの係数

f () ：第 2軸の速度による項

g () ；第 3軸の速度による項

J i ：第 i 軸の軸変数値（図 1.参照）

なお、記号の頂部にドットが 1 つつくと時間の 1 次微分を表し、 2つつくと時間の 2次微分を表す。

ここで、始点及び終点では速度項を無視出来るものとして、次式 [ 3 ] ， [ 4 ] を用いる。

T 2= I 2* J 2- C 2* ( J 2+ J 3) [ 3 ]

T 3= I 3* J 3- C 3* ( J 2+ J 3) [ 4 ]

( 2 ) 目標軌道を実現するための各軸の拘束条件式；ロボットが動作する 3次元直交座標系∑ 0 (以下、単に「直交座標系」と言う。）上で表わしたロボットのッール先端点の位置を P、 j 番目の軸の位置を j で表わすと、 p は j の関数となる。これを式 [ 5 ] とし、これを時間で微分して式 [ 6 ] を得る。

P = f ( J ) ···· [ 5 ] p = J ( j ) * j [ 6 ] ここで、 J ( j ) は、 p と j の関係を規定するヤコビ行列であり、具体的な内容はロボットの構造パラメ一夕に基づいて計算される。

式 [ 6 ] を更に時間で微分すると次式 [ 7 ] を得る。

P = J ( j ) * j + J ( j ) * j [ 7 ] そして、式 [ 7 ] において、動作の始点または終点であることを考慮して、 Jの時間微分 J = 0 とすると、式 C 8 ] を得る。

P = J ( j ) * j ···· [ 8] また、目標軌道を実現するための、各軸の拘束条件式として、式 [8 ] から式 [9 ] を得る。

j = J ( j ) P · ··· [ 9]

( 3 ) ロボットの最大トルクによる拘束条件；

第 i 軸の発生トルクと最大トルクとをそれぞれ T i 、 Tmax-i とし、第 2軸と第 3軸について、次式 [ 1 0] ，

[ 1 1 ] の条袢が存在するものと +る。なお、ここで言う最大トルクには静負荷トルク（Tv2， T w3) は含まれていない。

T 2 < T max-2 [ 1 0 ]

T 3 < Tiax-3 [ 1 1 ] 以上の条件式をまとめて、改めて害き出せば次の諸式

[ 1 2 ] 〜 [： 1 6 ] を得る。

T 2= I 2* J 2- C 2* ( J 2+ J 3) 1 2 ] T3= I 3* J 3- C3* ( J 2+ J 3) 1 3 ] j = J ( j ) - ¹ *· P 1 4 ] T 2 < T max-2 1 5 ] T 3 < T max - 3 1 6 ] ここで、式 [ 1 4 ] を次のように表現する。

J 2= A 2* A c c [ 1 4 A ]

J 3 = A 3 * A c c [ 1 4 B ] ここで、記号 A c cは目標軌道の加速度を表わし、 A i は目標軌道の加速度に対する係数を表わしている。

式 [ 1 2 ] , [ 1 4 A ] , [ 1 5 ] より、次式 [ 1 7 ] を得る。

T 2= { I 2* A 2- C 2* ( A 2+ A 3) }

* A c c≤ T max-2 · · [ 1 7 ] 更に式 [ 1 7 ] を変形して式 [ 1 8] を得る（

T max-2

A c c

I 2* A 2- C 2* ( A 2+ A 3)

···· [ 1 8 ]

—方、式 [ 1 3 ] , [ 1 4 B] , [ 1 6 ] より、次式

[ 1 9 ] を得る。

T 3= { I 3* A 3- C 3* ( A 2+ A 3) }

* A c c≤ T max-3 [ 1 9 ] 更に式 [ 1 9 ] を変形して式 [ 2 0 ] を得る。

T max-3

A c c

I 3* A 3- C 3* ( A 2+ A 3)

···· [ 2 0 ] 結局、式 [ 1 8 ] 及び式 ϋ 2 0 ] を満たす最大の A c 求め、これに基づいて J 2 , J 3 の加速度を前出の式 [ 1 4 A] , [ 1 4 B ] に従って求めれば、第 2軸及び第 3軸について干渉トルクを考慮した加速度が定められる。求める時定数ては、その動作区間における教示速度に対応した J 2 ， J 3 の速度 V t2, V t3を計算し、次式 [ 2 1 ] , [ 2 2 ] で τ 2 ，て 3 を求め、第 2軸及び第 3軸の時定数に設定する。なお、高精度の軌跡を望む場合には、 τ , て 3 の内の小さくない方の値を第 2軸と第 3軸共通の時定数に設定しても良い。

て 2 = V 11/ ( A 2 * A c c ) [ 2 1 ] て 3 = V t3Z (A 3 * A c c ) [ 2 2 ] 以上説明した計算に基づく時定数の設定は、オンラインで実施される。図 2は、そのために使用されるロボット制御装置の代表的な構成を要部ブロック図で示したものである。

図 2 において、符号 3 0で表示されたロボット制御装置はプロセッサボード 3 1 が装備されており、このプロセッサボード 3 1 はマイクロプロセッサからなる中央演算処理装置（以下、 C P Uと言う。） 3 1 a、 R 0 M 3 1 b並びに R AM 3 1 c を備えている。

C P U 3 1 aは R OM 3 1 b に格納されたシステムプログラムに従ってロボット制御装置全体を制御する。 R A M 3 1 cの相当部分は不揮発性メモリ領域を構成しており、動作プログラム、関連設定値の他に前出の式 [ 1 8 ] , [ 2 0 ] の計算を行なうための諸データ等が格納されている。これには、ヤコビ行列ゃ静負荷トルク Twi を算出するための構造パラメ一夕、各軸の加速度による干渉トルク係数 C i 、イナーシャ I i 、式 [ 1 4 A] , [ 1 4 B ] における係数 A2 , A3 を定めるためのデー夕等が含まれる。また、 R AM 3 1 cの一部は C P U 3 1 aの実行する計算処理等のための一時的なデータ記憶に使用される。

プロセッサボード 3 1 はパス 3 9に結合されており、このバス結合を介してロボット制御装置内の他の部分と指令やデータの授受が行なわれるようになつている。デジタルサーボ制御回路 3 2がプロセッサボード 3 1 に接統されており、 C P U 3 1 aからの指令を受けて、サーボアンブ 3 3を経由してサーボモー夕 5 1〜 5 6を駆動する。各サーボモータ 5 1〜 54はロボット R Bに内蔵されており、各軸を動作させる。

シリアルポート 3 4はバス 3 9に結合され、液晶表示部付の教示操作盤 5 7、 R S 2 3 2 C機器（通信用インターフェイス）及び C RT 3 6 aに接铳されている。教示操作盤 5 7は教示プログラム等のプログラムや教示デ一夕、その他必要な設定値等を入力するために使用される。この他、パス 3 9には、デジタル信号用の入出力装置（デジタル 1ノ0) 3 5、アナログ信号用の入出力装置（アナログ I ZO) 3 7及び大容量メモリ 3 8が結合されている。

デジタル I Z 03 5には、 C R T 3 6 aの画面を見ながら動作条件の設定、変更等を行なうための操作パネル 3 6 bが接铳されている。また、大容量メモリ 3 8には、教示データ、位置データ、各種設定値、動作プログラム等が格納される。

干渉トルクを考慮した加減速制御を実行するためのプログラムやトルクカーブを表わすデータについても、不使用時にはこの大容量メモリ 3 8 に格納されており、システムの立ち上げ時に R O M 3 1 b に格納されている起動プログラムが作動して、所要の関連データと共に適宜プロセッサボード 3 1 内の R A M 3 1 c にダウンロードされる方式とすることが出来る。

以下、上記したロボット制御装置 3 0の構成と機能、並びにプログラム及びデータ等の格納状況を前提に、図 3 のフローチャートを参照して、本実施例における時定数設定方法を実行するための処理について、各処理ステップ毎に処理内容の概略を記す形式で説明する。なお、ここで対象とする動作区間はロングモーションであるものとする。また、説明は第 1 軸〜第 4軸のすべてに共通した時定数を設定した加減速制御が行なわれる場合について説明する。

先ず、軸番号指標 nを 1 に初期設定した状態で処理を開始する。ステップ S 1 では、教示プログラムデータに基づき、その移動ブロックにおける移動目標位置までの当該軸 J n の移動距離から、その軸についての教示速度 V tnを求める。

铳くステップ S 2では、当該軸に関するトルクカーブデータ（対象とするサーボモータについて速度に対する最大トルク T max の依存関係を表現したデータ）に基づき、教示速度 V tnに対応する名目最大トルク Traax(n)を求める。ここで求められる名目最大トルク T max(n)には、静負荷トルク T wn が含まれている。

そこで続くステップ S 3では、当該軸について、最大トルク Traax(n)から静負荷トルク Twnを減じて（正味の）最大トルク T max-n を求める。

また、干渉トルクを考慮しない軸（本実施例では n = 1 及び n = 4 ) については、ここで最大トルク T max-n を满たす加速度 A c c -n を

A c c -n = T max-n / I n

で求める（ステップ S 4、ステップ S 5 ) 。そして、その時の時定数 τ n を

て n = V tn/ A c c -n

で求める（ステップ S 6 ) 。

以上の処理プロセスを軸番号指標を 1 づっカウントァップしながら、計 4 回繰り返した上で（ステップ S 7 , S 8 ) 、式 [ 1 8 ] 及び [ 2 0 ] を満たす最大の A c c を計算する。求められた A c c を A c c (cal) とする (ステップ S 9 ) 。

次いで、て 2 とて 3 を

て 2 = V \1/ A 2 * A c c (cal) 、

て 3 = V t3/ A 3 * A c c (cal)

で計算する（ステップ S 1 0、 S l l ) 。

計算された τ η ( η = 1 , , 3, 4 ) を比較し、そのうちの最も大きい値を各軸の時定数てとして設定する (ステップ S 1 2 ) 。

以上の処理によって、全軸共通の時定数の設定が完了する。

なお、ロボットを各軸移動形式で移動させる場合（移動目標位置へ移動する際の軌道が教示軌道からはずれることが許容される）には、全軸に共通した時定数を指定する必要はなく、各軸毎に、必要に応じて干渉トルクを考慮に入れた時定数を定めれば良い。

以上の説明した実施例では、干渉トルクの考慮は特定の軸間（第 2軸と第 3軸間の干渉トルク）のみを考えているが、複数の軸間乃至全軸間の干渉トルク考慮を行なつても良いことは勿論である。しかし、全軸共通の時定数を定める条件（教示軌道を遵守）では、最も干渉トルクの影 Sが大きいと考えられる軸間について干渉トルクを考慮して時定数を決定すれば、他の軸間については、結果的にトルクの飽和や性能発揮不十分を起こさない時定数が設定されると期待されるから、干渉トルクを考慮した時定数の計算をどの軸間について行なうかは、ケ一スに応じて定めることが好ましい。

また、本実施例ではロボットとして 4軸水平多関節口ボットを想定したが、他の型式のロボットであっても、本発明の技術思想が適用可能であることは、これまでの説明に照らして明らかであろう。

Claims

請求の範囲

各々サーボモータによって駆動される複数の軸を有するロボットの軌道計画時における時定数の設定方法であって、

始点及び終点における各軸の速度が 0 とみなすことが出来る動作区間の始点及び終点において、前記軸の内少なくとも 2つの軸の間の干渉トルクの影 «を入れて、発生トルクの飽和を起こさない条件で最短の加減速制御の時定数を、前記少なくとも 2つの軸について前記ロボットを制御するロボット制御装置を用いたォンラインのソフトウエア処理によって求め、

かく上記軸について求めた時定数を前記ロボットを制御するロボット制御装置に設定する、

前記の方法。

各々サーボモー夕によって駆動される複数の軸を有するロボットの軌道計画時における時定数の設定方法であって、

始点及び終点における各軸の速度が 0 とみなすことが出来る動作区間の始点及び終点において、前記軸の内少なくとも 2つの軸の間の干涉トルクの影 Sを入れて発生トルクの飽和を起こさない条件で最短の加減速制御の時定数を、前記少なくとも 2つの軸について前記ロボットを制御するロボット制御装置を用いたオンラインのソフトウエア処理によって求め、

その内の最長の時定数を全軸共通の時定数として前 - 16 - 記ロボットを制御する口ボット制御装置に設定する、前記の方法。

前記ソフトゥァ処理が、ロボットの運動方程式に基づいて決定される干渉トルクの影響を評価した条件で、目標軌道を実現するための各軸の拘束条件と、口ボットの最大トルクによる拘束条件とを同時に満たす時定数の条件を求める処理を含んでいる請求の範囲第 1 項または第 2 に記載の、ロボットの軌道計画時における時定数の設定方法。

ロボットの全軸のうち干渉トルクの影 «が大きいと予め想定される軸についてのみ、その軸間の干渉トルクの影 «を入れて発生トルクの飽和を起こさない条件で最短の加減速制御の時定数を求める、請求の範囲第 2項に記載のロボットの軌道計画時における時定数の設定方法。