JP4358979B2 - Robot system control method and control apparatus - Google Patents

Description

【００２４】
次いで、この実施形態で用いる各項目の単位及び個数を以下に示す。
【００２５】
【表２】
―――――――――――――――――――――――――――――――――――
記号 項目 単位 個数
―――――――――――――――――――――――――――――――――――
Ｔ 関節に発生するトルク Ｎ・ｍ ｎ×１行列
θ 関節位置（角度） ｒａｄ ｎ×１行列
θｄ 関節速度 ｒａｄ／ｓｅｃ ｎ×１行列
θｄｄ 関節加速度 ｒａｄ／ｓｅｃ² ｎ×１行列
Ｈ 慣性行列 ｋｇ・ｍ² ｎ×ｎ行列
Ｃ 粘性行列 Ｎ・ｍ ｎ×１行列
Ｇ 重力行列 ｋｇ・ｍ ｎ×３行列
ｇ 重力加速度ベクトル ｍ／ｓ² ３×１行列
Ｔｐｅａｋ 関節の許容ピークトルク Ｎ・ｍ ｎ×１行列
θｓ 開始位置 ｒａｄ ｎ×１
θｅ 目標位置 ｒａｄ ｎ×１
Ｖ 目標速度 ｒａｄ／ｓｅｃ ｎ×１
Ａｓｍｐ 仮の加速度 ｒａｄ／ｓｅｃ² ｎ×１
Ｄｓｍｐ 仮の減速度 ｒａｄ／ｓｅｃ² ｎ×１
Ｖｅｓｔ 加速度予測値 ｒａｄ／ｓｅｃ ｎ×１
Ｔｅｓｔ 動作時間予測値 ｓｅｃ ｎ×１
Ｖｒｅａ 速度予測値 ｒａｄ／ｓｅｃ ｎ×１
Ａｒｅａ 加速度予測値 ｒａｄ／ｓｅｃ² ｎ×１
Ｄｒｅａ 減速度予測値 ｒａｄ／ｓｅｃ² ｎ×１
Ｔｍａｘ 最長動作時間 ｓｅｃ １
―――――――――――――――――――――――――――――――――――
Ｉｘｉ，Ｉｘｙｉ，Ｉｘｚｉ リンクの慣性テンソル ｋｇ・ｍ² ３×３
Ｉｘｙｉ，Ｉｙｉ，Ｉｙｚｉ
Ｉｘｚｉ，Ｉｙｚｉ，Ｉｚｉ
―――――――――――――――――――――――――――――――――――
ｍｉ リンク質量 ｋｇ １×ｎ
ｍｘｉ，ｍｙｉ，ｍｚｉ リンク重心位置ベクトル ｍ ３×ｎ
記号なし 重力加速度 ｍ／ｓｅｃ² １
Ｃｍ 動粘性係数 Ｎ・ｍ・ｓｅｃ ｎ×１行列
θｄｍ モータ速度 ｒａｄ／ｓｅｃ ｎ×１行列
Ｆｃｏｕ クーロン摩擦トルク Ｎ・ｍ ｎ×１行列
―――――――――――――――――――――――――――――――――――
Ｉｍ 減速機を含むモータロータの ｋｇ・ｍ² １×ｎ
慣性モーメント
―――――――――――――――――――――――――――――――――――
Ｔａｄ 加減速で使用できるトルク Ｎ・ｍ ｎ×１行列
Ａｏｐｔ 最適な加速度 ｒａｄ／ｓｅｃ² ｎ×１行列
Ｄｏｐｔ 最適な減速度 ｒａｄ／ｓｅｃ² ｎ×１行列
Ｔｂａｓｅ 基準動作時間 ｓｅｃ １
Ｓａｉ 第ｉの関節の移動量 ｒａｄ １
Ｖｏｐｔｉ 第ｉの関節の最適な速度 ｒａｄ／ｓｅｃ １
Ｔａｉ 第ｉの関節の加速時間 ｓｅｃ １
Ｔｃｉ 第ｉの関節の定速時間 ｓｅｃ １
Ｔｄｉ 第ｉの関節の減速時間 ｓｅｃ １
ξｉ 第ｉの関節の減速比 無次元 １
―――――――――――――――――――――――――――――――――――
【００２６】
次いで、実施例（ｎ＝３）におけるロボットダイナミクスのベースパラメータについて説明する。質量ｍｉは、第ｉの関節（ここで、ｉは自然数であり、１から関節の最大個数までとる。）の動作により移動しかつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの質量を示す。また、重心位置ｍｘｉ、ｍｙｉ、ｍｚｉは、第ｉの関節の動作により移動し、かつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの重心位置を示す。また、第ｉの関節の慣性テンソルＩｘｉ乃至Ｉｚｉは、第ｉの関節の動作により移動しかつ第ｉ＋１の関節の動作によっては移動しない第ｉの関節に連結されるリンクの慣性テンソルを示し、例えば、従来技術文献１の節３．８に、演算手順が詳しく説明されており、最終的に慣性テンソルの演算式は、当該従来技術文献１の式（３．３８）となる。
【００２７】
ここで、重心位置及び慣性テンソルを演算する上での基準をリンク座標系とする。リンク座標系は、例えば、従来技術文献２「吉川恒夫訳，”ロボット・マニピュレータ”，株式会社コロナ社，初版，１９８９年１２月１５日」の節２．９に各関節毎に割り当てるリンク座標系の定義手順が詳細に説明されているが、ここでは、基本的なリンク座標系の定義について説明する。
【００２８】
まず、慣性テンソルＩは、物体の回転し難さを示すのに一般的に使用され、公知の通り次式の３×３の対称行列で表され、その対角成分はロボット機構部の各関節の慣性モーメントを、その他の成分は慣性乗積の負の値を表す。
【００２９】
【数２】

【００３０】
また、慣性モーメントは次式より求められ、物体（ロボットリンク）の回り難さ及び止め難さを表す。
【００３１】
【数３】
Ｉｘ＝∫（ｙ²＋ｚ²）ｄｍ
【数４】
Ｉｙ＝∫（ｚ²＋ｘ²）ｄｍ
【数５】
Ｉｚ＝∫（ｘ²＋ｙ²）ｄｍ
【００３２】
次いで、慣性乗積は、次式より求められ、物体（ロボットリンク）が回転運動を行っているとき、その運動を乱すモーメントを表す。
【００３３】
【数６】
Ｉｘｙ＝∫ｘｙｄｍ
【数７】
Ｉｙｚ＝∫ｙｚｄｍ
【数８】
Ｉｚｘ＝∫ｚｘｄｍ
【００３４】
ここで、式中のｘ，ｙ，ｚは、所定の慣性テンソルの基準座標で表され、ｄｍは微小体積の質量を示す。ここで、基準座標をリンク座標系とする。例えば、第２の関節から第３の関節に伸張するリンクＬ３において、第２の関節の回転中心を原点として、第２の関節に割り当てられたリンク座標が定められ、第２の関節のリンク座標におけるｘ方向をリンク長さの方向とし、ｚ方向を回転軸方向とし、ｙ方向を右手座標系の方向とする。ここで、第２の関節に連結されたリンクＬ３の慣性テンソルの基準座標は、第２の関節に割り当てられたリンク座標系をリンクＬ３の重心位置に移動したときの座標系とする。他の関節についても同様に定義する。
【００３５】
次いで、慣性、粘性及び重力の行列値の演算方法について説明する。慣性行列Ｈ、粘性行列Ｃ、及び重力行列Ｇは、次式を用いて数値計算して求める。なお、第１の関節の位置をθ₁とし、第２の関節の位置をθ₂とし、第３の関節の位置をθ₃とする。また、第１の関節の速度をθｄ₁とし、第２の関節の速度をθｄ₂とし、第３の関節の速度をθｄ₃とする。
【００３６】
【数９】

【００３７】
ここで、
【数１０】
H11
=mx1²・m1+[2・a1・mx2・cos(θ₂)+(1/2)・mx2²{1+cos(2・θ₂)}+a1²+mz2²]m2
+(a1²+2{cos(θ₂)・a2-my3・sin(θ₂+θ₃)+mx3・cos(θ₂+θ₃)}a1
+[-my3{sin(2・θ₂+θ₃)+sin(θ₃)}+mx3{cos(2・θ₂+θ₃)+cos(θ₃)}]a2
-mx3・my3・sin(2・θ₃+2・θ₂)+(1/2)a2²{1+cos(2・θ₂)}
-(1/2)my3²{cos(2・θ₃+2・θ₂)-1}
+(1/2)mx3²{cos(2・θ₃+2・θ₂)+1)}m3+(1/2)Ix2{1-cos(2・θ₂)}
+(1/2)Iy2{1+cos(2・θ₂)}
+(1/2)Ix3{1-cos(2・θ₃+2・θ₂)}+(1/2)Iy3{cos(2・θ₃+2・θ₂)+1}
-Ixy3・sin(2・θ₃+2・θ₂)+Iz1
【数１１】
H12=-m2・mz2・mx2・sin(θ₂)
【数１２】
H13=0
【００３８】
【数１３】
H21=H12
【数１４】
H22
=m2・mx2²+[a2²+2{mx3・cos(θ₃)-my3・sin(θ₃)}a2+my3²+mx3²]m3+Iz3+Iz2
【数１５】
H23=[{mx3・cos(θ₃)-my3・sin(θ₃)}a2+my3²+mx3²]m3+Iz3
【００３９】
【数１６】
H31=H13
【数１７】
H32=H23
【数１８】
H33=(my3²+mx3²)m3+Iz3
【００４０】
【数１９】
Ｃ＝［Ｃ１ Ｃ２ Ｃ３］^T
【００４１】
ここで、
【数２０】
C1
=[{-mx2・m2(mx2・sin(2・θ₂)+2・a1・sin(θ₂))
+(-2・a1(mx3・sin(θ₂+θ₃)+my3・cos(θ₂+θ₃)+a2・sin(θ₂))
-2・a2(mx3・sin(2・θ₂+θ₃)+my3・cos(2・θ₂+θ₃))-2・my3・mx3・cos(2・θ₃+2・θ₂)
+(my3²-mx3²)・sin(2・θ₃+2・θ₂)-a2²・sin(2・θ₂))m3
+(Ix3-Iy3)sin(2・θ₃+2・θ₂)
+(Ix2-Iy2)sin(2・θ₂)-2・Ixy3・cos(2・θ₃+2・θ₂)}θd₂
+{m3(-2・a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃))
-2・my3・mx3・cos(2・θ₃+2・θ₂)
+(my3²-mx3²)sin(2・θ₃+2・θ₂)
-a2(mx3(sin(2・θ₂+θ₃)+sin(θ₃))+my3(cos(2・θ₂+θ₃)+cos(θ₃))}]
-2・Ixy3・cos(2・θ₃+2・θ₂)-(Iy3-Ix3)sin(2・θ₃+2・θ₂))θd₃)θd₁
-m2・mz2・mx2・cos(θ₂)・θd₂ ²
【数２１】
C2
=[(1/2)mx2・m2(mx2・sin(2・θ₂)+2・a1・sin(θ₂))
+(1/2)(Iy3-Ix3)sin(2・θ₃+2・θ₂)
+(1/2)sin(2・θ₂)(Iy2-Ix2)+Ixy3・cos(2・θ₃+2・θ₂)
+{a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃))
+a2(mx3・sin(2・θ₂+θ₃)+my3・cos(2・θ₂+θ₃))
+(1/2)a2²・sin(2・θ₂)+(1/2)(mx3²-my3²)sin(2・θ₃+2・θ₂)
+my3・mx3・cos(2・θ₃+2・θ₂)
+a1・a2・sin(θ₂)}m3]θd₁ ²-2・m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₂・θd₃
-m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₃ ²
【数２２】
C3=[{a1(my3・cos(θ₂+θ₃)+mx3・sin(θ₂+θ₃))
+(1/2)a2{mx3(sin(2・θ₂+θ₃)+sin(θ₃))+my3(cos(2・θ₂+θ₃)+cos(θ₃))}
+(1/2)(mx3²-my3²)sin(2・θ₃+2・θ₂)+my3・mx3・cos(2・θ₃+2・θ₂)}m3
+(1/2)(Iy3-Ix3)sin(2・θ₃+2・θ₂)+Ixy3・cos(2・θ₃+2・θ₂)]θd₁ ²
+m3・a2(mx3・sin(θ₃)+my3・cos(θ₃))θd₂ ²
【００４２】
【数２３】

【００４３】
ここで、
【数２４】
G11
=-sin(θ₁)・mx1・m1
+{cos(θ₁)・mz2-sin(θ₁)・a1-(1/2)mx2(sin(θ₁+θ₂)-sin(-θ₁+θ₂))}m2
-sin(θ₁)・a1・m3-(1/2)m3((sin(θ₁+θ₂)-sin(-θ₁+θ₂))a2
+mx3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+θ₃))
+my3(cos(θ₁+θ₂+θ₃)-cos(-θ₁+θ₂+θ₃)))
【数２５】
G12
=cos(θ₁)・mx1・m1
+{sin(θ₁)・mz2+cos(θ₁)・a1+(1/2)mx2(cos(-θ₁+θ₂)+cos(θ₁+θ₂))}m2
+cos(θ₁)・a1・m3+(1/2)m3{(cos(-θ₁+θ₂)+cos(θ₁+θ₂))a2
+mx3(cos(-θ₁+θ₂+θ₃)+cos(θ₁+θ₂+θ₃))
+my3(-sin(-θ₁+θ₂+θ₃)-sin(θ₁+θ₂+θ₃))}
【数２６】
G13=0
【００４４】
【数２７】
G31
=(1/2)m3・{mx3(-sin(-θ₁+θ₂+θ₃)-sin(θ₁+θ₂+θ₃))
-my3(cos(-θ₁+θ₂+θ₃)+cos(θ₁+θ₂+θ₃))}
【数２８】
G32
=(1/2)m3・{mx3(cos(θ₁+θ₂+θ₃)-cos(-θ₁+θ₂+θ₃))
-my3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+θ₃))}
【数２９】
G33=m3(-my3・sin(θ₂+θ₃)+mx3・cos(θ₂+θ₃))
【００４５】
【数３０】
G21
=(1/2)mx2・m2(-sin(θ₁+θ₂)-sin(-θ₁+θ₂))
+(1/2)m3・a2(-sin(θ₁+θ₂)-sin(-θ₁+θ₂))
+(1/2)m3{mx3(-sin(-θ₁+θ₂+θ₃)-sin(θ₁+θ₂+θ₃))-my3(cos(-θ₁+θ₂+θ₃)
+cos(θ₁+θ₂+θ₃))}
【数３１】
G22
=(1/2)mx2・m2(-cos(-θ₁+θ₂)+cos(θ₁+θ₂))
+(1/2)m3・a2(-cos(-θ₁+θ₂)+cos(θ₁+θ₂))
+(1/2)m3(mx3(cos(θ₁+θ₂+θ₃)-cos(-θ₁+θ₂+θ₃))
-my3(-sin(-θ₁+θ₂+θ₃)+sin(θ₁+θ₂+θ₃)))
【数３２】
G23
=m2・mx2・cos(θ₂)+cos(θ₂)m3・a2+m3(-my3・sin(θ₂+θ₃)+mx3・cos(θ₂+θ₃))
【００４６】
上記数９乃至数３２は、表２に示すパラメータ、図２に示すリンク長さａ１、ａ２、並びに関節位置θ、関節速度θｄのみから構成され、ここで、θ、θｄ以外はすべて既知の値である。なお、ａ１はリンクＬ２のリンク長さであり、ａ２はリンクＬ３のリンク長さである。従って、開始位置θｓと速度予測値Ｖｒｅａを入力すれば、開始位置θｓにおけるロボットダイナミクスモデルを計算することができ、同様に、目標位置θｅと速度予測値Ｖｒｅａを入力すれば、目標位置におけるロボットダイナミクスモデルが計算できる。具体的には、以下の式に示す通りである。速度予測値Ｖｒｅａの演算方法については、後述するのでここでの説明は省略する。
【００４７】
【数３３】
開始位置の慣性行列：Ｈｓ＝Ｈ（θｓ）
【数３４】
開始位置の粘性行列：Ｃｓ＝Ｃ（θｓ，Ｖｒｅａ）
【数３５】
開始位置の重力行列：Ｇｓ＝Ｇ（θｓ）
【数３６】
目標位置の慣性行列：Ｈｅ＝Ｈ（θｅ）
【数３７】
目標位置の粘性行列：Ｃｅ＝Ｃ（θｅ，Ｖｒｅａ）
【数３８】
目標位置の重力行列：Ｇｅ＝Ｇ（θｅ）
【００４８】
ところで、ロボットの各関節に発生させることができるトルクは、モータや機械の設計仕様から制限される既知の値であり、これをＴｐｅａｋとすると、上述の数１の左辺を次式のように置き換えられる。
【００４９】
【数３９】
Ｔｐｅａｋ≧Ｈ（θ）・θｄｄ＋Ｃ（θ，θｄ）＋Ｇ（θ）・ｇ
【００５０】
ここで、Ｔｐｅａｋは、関節の許容ピークトルクｎ×１行列である。上記数３９を変形して次式の数４０の形式に変換する。数４０の左辺は、関節の許容ピークトルクＴｐｅａｋから、加減速度に関係の無い重力モーメントや遠心力、コリオリ力等の外乱トルクを差し引いた値であり、加減速に使用できるトルクを表す。数４０の右辺は、ロボット各関節が加減速度θｄｄで動作した場合に各関節で発生するトルクを表す。つまり、数４０は、第１の課題を満足させる加減速度を演算する上での条件式の１つとなる。
【００５１】
【数４０】
Ｔｐｅａｋ−（Ｃ(θ,θｄ)＋Ｇ(θ)・ｇ）≧Ｈ(θ)・θｄｄ
【００５２】
数４０において、Ｔｐｅａｋ及びｇは既知の固定値であり、また、Ｈ、Ｃ、Ｇは、ロボット各関節に連結されるリンクの重心位置、質量、慣性テンソル、リンク長さ、及び関節位置θ、関節速度θｄから演算されるロボットのダイナミクスモデルである。従って、上記数４０において、関節加速度θｄｄを除くと、未知の値は関節速度θｄのみとなる。なぜなら、ロボット各関節に連結されるリンクの重心位置、質量、慣性テンソル、リンク長さは、ロボットの機構やリンク形状及び材質等から予め求めておくことが可能な既知の固定値で、また関節位置θは、予め教示された開始位置あるいは目標位置を使用できるからである。
【００５３】
そのため、本実施形態では、数４０における関節速度θｄを予測する手段を持つ。ここでことわっておくが、θｄとして教示された目標速度を使用することはできない。すべての関節に対して最高速度での位置決め動作を要求しても、各関節ごとに移動量が違うことが一般的であり、目標位置へ到達する時間すなわち動作時間に差が発生する。そのため、すべての関節で動作時間を同じにすると、移動量の少ない関節は、目標速度より低い速度で動作する。同様の理由で、移動量の少ない関節は加減速度も抑えられるので、この考慮をしなければ、上述の第１の課題を解決する最適な値を演算することはできない。加減速度が抑えられる関節は、他の関節へ与える干渉力の影響を抑えることができる。そのため、本発明に係る実施形態では、関節毎の加減速度についても予測する手段を持つ。
【００５４】
関節毎の速度、加減速度の予測は、以下の手順で行う。まず、ロボットのいかなる姿勢でも正常に動作が可能な加速度及び減速度（それぞれ以下、仮の加速度、仮の減速度という。）を予め記憶しておく。次に、予め教示される開始位置θs、目標位置θe、及び目標速度Ｖから、上記した仮の加減速度が保持されるように、すべての関節の速度パターンを計算する。本発明では、この速度パターンの計算方法を、第１の速度パターン計算方法という。
【００５５】
この第１の速度パターン計算方法は、後述する図４の処理フローにおいて、ステップＳ２、Ｓ６及びＳ９において用いるものであり、その方法について以下に説明する。
【００５６】
ロボットの第ｉの関節における、第１の速度パターン計算方法は、第ｉの関節の教示される教示データの開始位置θｓｉ、目標位置θeｉ、目標速度Ｖｉから、目標加速度Ａｉ、目標減速度Ｄｉで動作するように、各関節の速度パターンを生成する公知の計算方法である。ここで、各関節の速度パターンとは、図７に示す台形状の速度パターン、又は図８に示す三角形状の速度パターンのことであり、第ｉの関節の加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作速度Ｖｒｉがこの第１の速度パターン計算方法により演算される。また、第ｉの関節の加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉを加算した値が速度パターンにおける動作時間Ｔｒｉとなる。第ｉの関節の場合、以下の式を使用することで、第ｉの関節の開始位置θsｉ、目標位置θeｉ、目標速度Ｖｉ、目標加速度Ａｉ、目標減速度Ｄｉから、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作時間Ｔｒｉ、動作速度Ｖｒｉを計算できる。
【００５７】
【数４１】
Ｔａｉ＝Ｖｉ／Ａｉ
【数４２】
Ｔｄｉ＝Ｖｉ／Ｄｉ
【数４３】
Ｔｃｉ＝｛θｅｉ−θｓｉ−Ｖｉ（Ｔａｉ＋Ｔｄｉ）／２｝／Ｖｉ
【数４４】
Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数４５】
Ｖｒｉ＝Ｖｉ
【００５８】
ここで、上記数４３により求められた定速時間Ｔｃｉが０未満となったときは、図８に示す三角形状の速度パターンとなるケースで、この場合は、以下の式により、加速時間Ｔａｉ、定速時間Ｔｃｉ、減速時間Ｔｄｉ、動作時間Ｔｒｉ、及び動作速度Ｖｒｉを計算できる。
【００５９】
【数４６】

【数４７】
Ｔａｉ＝Ｖｉ／Ａｉ・ｓｆ
【数４８】
Ｔｄｉ＝Ｖｉ／Ｄｉ・ｓｆ
【数４９】
Ｔｃｉ＝０
【数５０】
Ｔｒｉ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数５１】
Ｖｒｉ＝Ｖｉ・ｓｆ
【００６０】
次に、すべての関節の速度パターンの中から、動作時間が最大となる関節（以下、最大動作関節という。）を抽出し、すべての関節において速度パターンの動作時間が、最大動作関節の動作時間と同じになるように速度パターンの再生成を行う。この処理を、終了時間の同期処理という。終了時間の同期処理については、公知の技術であるので説明は省略する。以上の処理の一例を図１０及び図１１に示す。図１０及び図１１からも判るように、第１の速度パターン計算を行い、終了時間の同期処理を行うことで、速度予測値Ｖｒｅａ、加速度予測値Ａｒｅａ、減速度予測値Ｄｒｅａを演算できる。
【００６１】
速度予測値Ｖｒｅａを上記数４０の関節速度θｄに代入することにより、上記数４０は関節の加速度又は減速度であるθｄｄ以外はすべて既知の値となる。また、他の関節の動作により干渉トルクを受ける駆動関節の許容ピークトルクを最適に配分するために、演算された加速度予測値Ａｒｅａ及び減速度予測値Ｄｒｅａの各関節間の比例関係を求め、この比例関係と上記数４０とを同時に満足する加減速度を求める。つまり、加速時においては、予測された加速度の各関節間の比例関係が保たれ、かつ上記数４０を満足させる加速度が、前述の第１の課題を解決する最適な加速度Ａｏｐｔとなり、減速時においては、予測された減速度の各関節間の比例関係が保たれ、かつ上記数４０を満足させる減速度が、前述の第１の課題を解決する最適な減速度Ｄｏｐｔとなる。
【００６２】
以上の結果、最適な加速度及び減速度が求まるがこれだけでは最適な速度パターンが計算できたとは言えない。なぜなら、最適な加速度Ａｏｐｔと最適な減速度Ｄｏｐｔを使用しても、速度パターンの生成手段が従来と同じであれば、関節毎に動作時間が変わり、図１０及び図１１に示すような従来と同様の終了時間の同期処理を施さねばならず、これにより最適な加減速度が保持されないためである。そのため、本発明に係る実施形態では、計算の結果求められた最適な加減速度が保持されるような、速度パターン生成処理を含む。以下に、その手順を説明する。
【００６３】
前述した終了時間の同期処理において、最長動作時間となった関節についてのみ、既に求めた最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを用いて、前述した第１の速度パターン計算方法により基準動作時間Ｔｂａｓｅを求める（図４のステップＳ６）。その他の関節については、最適な加速度Ａｏｐｔと最適な減速度Ｄｏｐｔを保持し、かつ動作時間が上記基準動作時間Ｔｂａｓｅとなるように、第２の速度パターン計算方法を用いた速度パターン生成処理を実行する（図４のステップＳ７）。これにより、すべての関節の動作時間が最長動作関節と同じになるので、終了時間の同期処理を施す必要が無くなり、先に計算された最適な加減速度が保持される。
【００６４】
先に求められた最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを保持し、かつ動作時間が上記基準動作時間Ｔｂａｓｅとなるような速度パターンが実現できない関節が存在した場合は（図４のステップＳ８でＮＯ）、その関節のみ第１の速度パターン計算を行い、すべての関節の速度パターン計算が終了した（図４のステップＳ９）後に、前述の終了時間の同期処理（図４のステップＳ１０）を施せばよい。このようなことが起こるのは、加減速度を最適化したことで、最適化前とは最大動作関節が変わってしまうケースである。終了時間の同期処理を施すことで、加減速度の最適度合いが若干低下するが、加減速に使用できるトルクを越えることは無い。
【００６５】
以上のように演算される速度パターンで各関節を動作させることにより、第１の課題を解決できる。第２の課題については、上記数４０の慣性行列Ｈ及び粘性行列Ｃが関節に連結されるリンクの慣性テンソルを含んだものになるため、必然的に達成可能となる。
【００６６】
また、別の実施形態において、各関節内のモータの軸受けや減速機の摩擦抵抗を考慮した場合について説明する。上記数１で表されるロボットダイナミクスモデルを、次式のように変更することにより、各関節内のモータの軸受けや減速機の摩擦抵抗を考慮できる。
【００６７】
【数５２】

【００６８】
ここで、Ｃｍはモータの軸受けや減速機の動粘性係数ｎ×１行列であり、動粘性係数とは、速度と動摩擦トルクとの比例関係を表す定数である。また、動摩擦トルクとは、速度に比例した大きさを持つトルク成分である。さらに、θｄｍはモータの速度ｎ×１行列であり、Ｆｃｏｕはクーロン摩擦トルクである。クーロン摩擦トルクとは、速度に無関係な大きさを持ち、常に運動と逆の方向に働くトルク成分である。上記数５２において、ｓｇｎ（θｄｍ）はモータの速度ｎ×１行列に基づいて次式のように決定される符号関数である。
【００６９】
【数５３】
ｓｇｎ（θｄｍ）
＝１；θｄｍ＞０のとき
＝０；θｄｍ＝０のとき
＝−１；θｄｍ＜０のとき
【００７０】
また、第ｉの関節の速度とモータの回転速度の関係はその減速比ξｉから次式のように表される。
【００７１】
【数５４】
θｄｍｉ＝θｄｉ・ξｉ
【００７２】
さらに、上記数５２のθｄｍは、次式で表される。
【００７３】
【数５５】
θｄｍ＝［θｄ₁・ξ₁，θｄ₂・ξ₂，θｄ₃・ξ₃］^T
【００７４】
ここで、ξｉは、第ｉの関節内の減速機の減速比である。ここで、パラメータＣｍ、Ｆｃｏｕ、ξも、予め設定可能な既知の値であるから、上記数５２であっても、上述と同様に最適な加減速度の演算が可能であるので、詳細については省略する。
【００７５】
さらに、そのほかの別の実施形態においては、上述の数９に示す慣性行列Ｈを以下のように補正することにより、各関節内における減速機を含むモータロータの慣性モーメントにより発生するトルクの考慮が可能になる。
【００７６】
【数５６】

【００７７】
ここで、Ｉｍｉは減速機を含むモータロータの慣性モーメントである。連動して動く回転体の慣性モーメントは、その減速比の２乗分の１倍されて入力軸の慣性モーメントに影響を与える。従って、入力軸の慣性モーメントが出力軸に与える影響はその逆であるため、減速比の２乗倍されなければならない。減速機を含むモータロータの慣性モーメントが減速比の２乗倍されて加算されているのはそのためである。
【００７８】
慣性行列Ｈの対角要素は自分自身に与えるトルクの影響を示し、他の要素は他の関節に与える干渉トルクの影響を示している。減速機を含むモータロータの慣性モーメントが他の関節に干渉することは無いので、対角要素のみ補正すればよい。従って、Ｉｍｉ、ξｉも、予め設定可能な既知の値であるから、上記数５６であっても、上述と同様に最適な加減速度の演算が可能であるので、詳細については省略する。
【００７９】
次いで、図１を参照して、本実施形態に係るロボットシステムの構成について説明する。図１に示すように、制御装置１０は、
（ａ）クロック発生器１１０によって発生されるクロック信号と当該クロック信号を所定の分周比で分周する分周器１１１によって発生される同期信号ＳＹＮＣに基づいてＲＯＭ１０１に格納されたシステムプログラムに従って、ロボット機構部１の動作を制御するＣＰＵ１００と、
（ｂ）ロボット機構部１の動作を制御するための位置決め動作処理を含むプログラムと、当該プログラムを実行するために必要なデータを格納するＲＯＭ１０１と、
（ｃ）ＣＰＵ１００のワーキングエリアとして用いられ、計算途中のデータを一時的に格納するＲＡＭ１０２と、
（ｄ）上記位置決め動作処理を実行するために必要なデータを予め格納する記憶装置であって、教示データメモリ１０６ａと、仮の加減速度情報メモリ１０６ｂと、ロボットダイナミクスベースパラメータメモリ１０６ｃとを含むハードディスクメモリ１０６と、
（ｅ）ティーチペンダント２０が接続されたインターフェース１０３と、
（ｆ）各モータＭＯ１乃至ＭＯ３に対する駆動制御値であるモータ指令位置のデータを格納するデュアルポートＲＡＭ１０４とを備える。
これらの各回路１００乃至１０３及び１０６とデュアルポートＲＡＭ１０４の第１のポートがバス１０９を介して接続される。なお、ティーチペンダント２０についての処理はＣＰＵ１００の割り込み処理によって実行される。
【００８０】
ハードディスクメモリ１０６内の教示データメモリ１０６ａは、ロボット機構部１を動作させるときのロボット機構部１の各関節の開始位置と、目標位置と、目標速度とを、教示データとして予めティーチペンダント２０を用いて入力して格納する。また、仮の加減速度情報メモリ１０６ｂは、速度パターンを予測演算するときに用いる各関節の仮の加速度と仮の減速度の情報を予めティーチペンダント２０を用いて入力して格納する。さらに、ロボットダイナミクスベースパラメータメモリ１０６ｃは、ロボット機構部１の各関節に連結されるリンクの慣性テンソルと、質量と、重心位置とを含むロボットダイナミクスベースパラメータをティーチペンダント２０を用いて入力して格納する。
【００８１】
デュアルポートＲＡＭ１０４の第２のポートはラッチ回路１０５に接続され、ラッチ回路１０５は同期信号ＳＹＮＣに同期して、デュアルポートＲＡＭ１０４に格納された各モータＭＯ１乃至ＭＯ３に対する３個のモータ指令位置のデータを同時にラッチした後、ロボット機構部１の関節制御に関するモータ指令位置のデータをサーボ制御回路１０７を介してモータＭＯ１乃至ＭＯ３に出力して駆動する。
【００８２】
次いで、ロボット機構部１の各関節ＲＪｋ（実施例では、ｋ＝１，２，３）の構造とサーボ制御回路１０７の構成について図３を参照して説明する。図３に示すように、モータＭＯｋの回転軸は減速機ＲＥｋの入力軸に連結され、減速機ＲＥｋはモータＭＯｋの回転速度を所定の減速比λｋだけ減速することによって、モータＭＯｋによって発生されるトルクを上記減速比の逆数倍だけ増大させて、減速機ＲＥｋの出力軸に連結されたリンクＬｋに出力する。ここで、モータＭＯｋの回転軸にはセンサＳＮｋの軸が連結され、当該センサＳＮｋからの出力信号がサーボ制御回路１０７内の加算器１２０の減算入力端子に入力される。一方、ラッチ回路１０５からのモータ指令位置ＤＰｋのデータが加算器１２０の加算入力端子に入力される。加算器１２０から出力されるデータは、Ｄ／Ａ変換器（図示せず。）によってアナログ電圧信号に変換された後、所定の増幅度を有する増幅器１２１を介してモータＭＯｋの駆動端子に印加される。以上のように構成されたサーボ制御のフィードバック系においては、回転角度がモータ指令位置ＤＰｋとなるように制御され、これによって、モータと減速機を介して連結されるリンクＬｋが、ＤＰｋを減速比で除した分である関節変数θｋだけ回転される。
【００８３】
図４は、図１のＣＰＵ１００によって実行される位置決め動作処理を示すフローチャートである。
【００８４】
図４において、まず、ステップＳ１において、教示データメモリ１０６ａ内の教示データに基づいて、ロボット機構部１の各関節の開始位置、目標位置及び目標速度を設定し、ステップＳ２において、上記ステップＳ１で設定されたデータと、仮の加減速度情報メモリ１０６ｂ内の仮の加減速度情報とに基づいて、第１の速度パターン計算を行い、ロボット機構部１の各関節の動作時間と加速度予測値とを演算する。この処理は、図１０の従来例のステップＳ１０３及びＳ１０４と同様の処理である。具体的には、第１の速度パターン計算方法における目標加速度Ａとして仮の加速度Ａｓｍｐを、目標減速度Ｄとして仮の減速度Ｄｓｍｐを使用する。また、第１の速度パターン計算方法により演算される動作速度Ｖｒを加速度予測値Ｖｅｓｔとし、動作時間Ｔｒを動作時間予測値Ｔｅｓｔとする。
【００８５】
さらに、ステップＳ３において、ステップＳ２で演算された全ての関節の速度パターンに対して終了時間の同期処理を実行する。この終了時間の同期処理は、図１０の従来例のステップＳ１０９乃至Ｓ１１１の処理（以下、従来例の速度パターン再計算処理という。）と同様の処理であり、上記動作時間予測値の中から、動作時間が最大となる関節を最大動作関節とし、その最大動作関節の動作時間を最長動作時間Ｔｍａｘとする。そして、各関節について、上記動作時間予測値が上記最長動作時間と同じになるように、動作速度及び加減速度を再計算した値をそれぞれ、速度予測値、加速度予測値及び減速度予測値としてＲＡＭ１０２に一時的に格納する。
【００８６】
そして、ステップＳ４においてロボットダイナミクスベースパラメータメモリ１０６ｃ内のロボットダイナミクスベースパラメータに基づいて、開始位置及び目標位置のロボットダイナミクスモデルの演算処理を実行する。この処理では、上記開始位置及び目標位置と上記速度予測値と上記ロボットダイナミクスベースパラメータから、数９乃至数３８を用いて、開始位置及び目標位置における慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇを含むロボットダイナミクスモデルを演算してロボットダイナミクスベースパラメータメモリ１０６ｃに格納する。さらに、ステップＳ５において最適な加減速度の演算を実行する。この処理では、上記演算されたロボットダイナミクスモデルに基づいて、上記数４０と、上記加速度予測及び減速度予測における各関節間の比例関係を同時に満足する加減速度を、最適な加減速度として求める。つまり、加速時においては、加速度予測値Ａｒｅａの各関節間の比例関係（後述する数７６）が保たれ、かつ数４０を満足するような最適な加速度Ａｏｐｔを演算する一方、減速時においては、減速度予測値Ｄｒｅａの各関節間の比例関係（後述する数８９）が保たれ、かつ数４０を満足するような最適な減速度Ｄｏｐｔを演算し、演算された最適な加速度Ａｏｐｔ及び最適な減速度ＤｏｐｔをＲＡＭ１０２内に格納する。
【００８７】
次いで、ステップＳ６では、ステップＳ３の処理結果に基づいて、最長動作時間となった関節を検索し、検索した関節である第ｋの関節について、ステップＳ５で演算された最適な加速度Ａｏｐｔｋと、最適な減速度Ｄｏｐｔｋと、第ｋ関節の教示データ（開始位置、目標位置、目標速度）とから、上述の第１の速度パターン計算方法を用いて第ｋ関節の速度パターンを演算し、その動作時間を基準動作時間Ｔｂａｓｅとし、演算結果をＲＡＭ１０２に格納する。さらに、ステップＳ７では、上記の第ｋの関節以外のすべての関節について、第２の速度パターン計算方法を用いて、基準動作時間Ｔｂａｓｅと、最適な加速度Ａｏｐｔと、最適な減速度Ｄｏｐｔと、教示データ（開始位置、目標位置）とから、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンを生成してＲＡＭ１０２に格納する。そして、ステップＳ８では、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンがすべての関節で存在したか否かが判断され、ＹＥＳのときは当該位置決め動作処理を終了するが、ＮＯのときはステップＳ９に進む。
【００８８】
ステップＳ９では、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンが存在しなかったすべての関節について、第１の速度パターン計算方法を用いて、最適な加速度Ａｏｐｔと、最適な減速度Ｄｏｐｔと、教示データ（開始位置、目標位置、目標速度）から速度パターンを演算してＲＡＭ１０２に格納する。最後に、ステップＳ１０では、ステップＳ６、Ｓ７及びＳ９で演算された速度パターンに基づいて、すべての関節の速度パターンの中から最長動作時間を抽出し、従来例の速度パターン再計算処理と同様に、すべての関節について、最長動作時間と一致するように、速度パターンを再計算して計算結果をＲＡＭ１０２に格納する。
【００８９】
この位置決め動作処理の後、生成された速度パターンに基づいて、公知の通り、モータ指令位置ＤＰｋのデータを計算してデュアルポートＲＡＭ１０４、ラッチ回路１０５及びサーボ制御回路１０７を介してロボット機構部１に出力することにより、最適に位置決め動作処理がなされたデータに基づいて、ロボット機構部１の動作を制御することができる。
【００９０】
なお、以上の実施形態において、上記数５２に示すように、加速度に関係しないトルク成分として、粘性行列Ｃ及び重力行列Ｇに加え、モータＭＯ１乃至ＭＯ３の軸受けや減速機ＲＥ１乃至ＲＥ３から発生する摩擦抵抗を考慮することで、加速に使用することのできるトルクの演算精度を向上させてもよい。また、上記数５６に示すように、慣性行列Ｈの対角成分に、減速機ＲＥ１乃至ＲＥ３を含むモータＭＯ１乃至ＭＯ３のロータの慣性モーメントを加算することで、自分自身の加減速で発生するトルク演算値を補正してもよい。
【００９１】
図４のステップＳ８において、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターン存在しない関節があったとき（ステップＳ８でＮＯ）、その関節を最長動作時間の関節の番号をｋとし、ステップＳ６からの処理を再度行ってもよい。
【００９２】
【実施例】
図２に示すロボット機構部１（ｎ＝３）における、位置決め動作のための最適な加減速度の演算例を図４のフローチャートを参照して説明する。
【００９３】
ステップＳ１では、ロボット機構部１の各関節について、位置決め動作の開始位置、目標位置及び目標速度を、教示データメモリ１０６ａ内の教示データから読み出して以下のようにセットする。
【００９４】
【数５７】
開始位置：θｓ＝［θｓ１，θｓ２，θｓ３］^T
【数５８】
目標位置：θｅ＝［θｅ１，θｅ２，θｅ３］^T
【数５９】
目標速度：Ｖ＝［Ｖ１，Ｖ２，Ｖ３］^T
【００９５】
一方、仮の加減速度情報メモリ１０６ｂには、ロボットのすべての姿勢で動作が可能な仮の加減速度情報の値が以下のように予めセットされる。
【００９６】
【数６０】
仮の加速度：Ａｓｍｐ＝［Ａｓｍｐ１，Ａｓｍｐ２，Ａｓｍｐ３］^T
【数６１】
仮の減速度：Ｄｓｍｐ＝［Ｄｓｍｐ１，Ｄｓｍｐ２，Ｄｓｍｐ３］^T
【００９７】
そして、ステップＳ２では、上記セットされた開始位置、目標位置、目標速度、仮の加速度及び仮の減速度に基づいて、各関節毎に前述の第１の速度パターン計算を行う。第１の速度パターンの計算は、従来例と同様である。第１の速度パターン計算により、以下の値が予測される。その概念を図５に示す。図５の例では、第１の関節及び第２の関節は、目標速度に達するに必要な移動量があったが、第３の関節は移動量が少なく、目標速度に達していない。
【００９８】
【数６２】
加速度予測値：Ｖｅｓｔ＝［Ｖｅｓｔ１，Ｖｅｓｔ２，Ｖｅｓｔ３］^T
【数６３】
動作時間予測値：Ｔｅｓｔ＝［Ｔｅｓｔ１，Ｔｅｓｔ２，Ｔｅｓｔ３］^T
【００９９】
さらに、ステップＳ３においては、前述の終了時間の同期処理後におけるロボット各関節の速度及び加減速度の値を以下のように再予測する。
【０１００】
【数６４】
速度予測値：Ｖｒｅａ＝［Ｖｒｅａ１，Ｖｒｅａ２，Ｖｒｅａ３］^T
【数６５】
加速度予測値：Ａｒｅａ＝［Ａｒｅａ１，Ａｒｅａ２，Ａｒｅａ３］^T
【数６６】
減速度予測値：Ｄｒｅａ＝［Ｄｒｅａ１，Ｄｒｅａ２，Ｄｒｅａ３］^T
【０１０１】
上記動作時間予測値における最長動作時間をＴｍａｘとおくと、上記の値は、以下の式から演算できる。終了時間の同期処理の概念を図６に示す。図６の予測例では、移動量の少ない第２の関節及び第３の関節の速度及び加減速度が低下しているのがわかる。
【０１０２】
【数６７】
Ｖｒｅａｉ＝Ｖｅｓｔｉ・Ｔｅｓｔｉ／Ｔｍａｘ
【数６８】
Ａｒｅａｉ＝Ａｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）²
【数６９】
Ｄｒｅａｉ＝Ｄｓｍｐｉ・（Ｔｅｓｔｉ／Ｔｍａｘ）²
【０１０３】
なお、ロボットダイナミクスベースパラメータメモリ１０６ｃは、上記数１で表されるロボットダイナミクスモデルを生成するための、ロボットダイナミクスベースパラメータを予め格納する。ロボットダイナミクスベースパラメータの一覧を以下の表に示す。
【０１０４】
【表３】

【０１０５】
次いで、ステップＳ４では、上記ロボットダイナミクスベースパラメータと、ロボット機構部の各関節の位置及び速度から、上記数１で表現されるロボットダイナミクスモデルを生成する。ロボットダイナミクスモデルは、慣性行列Ｈと、粘性行列Ｃと、重力行列Ｇから構成される。図２の機構を持つロボット機構部１におけるダイナミクスモデルを、ニュートン及びオイラー法に基づき計算したものが数９乃至数３８に示されている。これら数９乃至数３８に示されたモデル式において、未知の値は、関節の位置及び速度のみであり、これに開始位置θｓと、速度予測値Ｖｒｅａとを入力すれば、開始位置におけるロボットダイナミクスモデルを計算できる。同様に、目標位置θｅと、速度予測値Ｖｒｅａを入力すれば、目標位置におけるロボットダイナミクスモデルを計算できる。従って、ステップＳ４では、以下のロボットダイナミクスモデルが計算される。
【０１０６】
【表４】

【０１０７】
次いで、ステップＳ５では、最適な加速度Ａｏｐｔ及び最適な減速度Ｄｏｐｔを、以下の手順で演算する。最初は、最適な加速度Ａｏｐｔを求める。そして、Ｈｓ、Ｇｓ、Ｃｓをそれぞれ数４０のＨ、Ｇ、Ｃに代入し、最適な加速度Ａｏｐｔ（又は最適な減速度Ｄｏｐｔ）を数４０のθｄｄとする。また、簡略化のため数４０の左辺を１つにまとめると次式のように表すことができる。
【０１０８】
【数７０】
Ｔａｄ≧Ｈｓ（θｓ)・Ａｏｐｔ
【０１０９】
数７０の左辺のＴａｄは、次式で表され、加速時においては加速に使用できるトルクであり、減速時においては減速に使用できるトルクとなる。ここでは、加速度を求めようとしているので、Ｔａｄは加速に使用できるトルクを表す。加速時と減速時とでは、使用するトルクの符号が異なるので、前述した終了時間の同期処理後における各関節加速度の予測値Ａｒｅａの符号を考慮する。
【０１１０】
【数７１】
Ｔａｄ
＝ｓｇｎ２（Ａｒｅａ）・Ｔｐｅａｋ−(Ｃｓ(θｓ，Ｖｒｅａ)
＋Ｇ(θｓ)・ｇ）
【０１１１】
ここで、符号関数ｓｇｎ２は次式で表される。
【０１１２】
【数７２】
ｓｇｎ２（Ａｒｅａ）
＝１；Ａｒｅａ≧０のとき
＝−１；Ａｒｅａ＜０のとき
【０１１３】
上記数７０を要素別に展開すると、次式のように表される。Ｔａｄは、加速に使用できるトルクを表しているから、次式が加速に使用できるトルクを最大限利用するための加速度を求める条件式となる。
【０１１４】
【数７３】
Ｔａｄ１
≧Ｈｓ１１・Ａｏｐｔ１＋Ｈｓ１２・Ａｏｐｔ２＋Ｈｓ１３・Ａｏｐｔ３
【数７４】
Ｔａｄ２
≧Ｈｓ２１・Ａｏｐｔ１＋Ｈｓ２２・Ａｏｐｔ２＋Ｈｓ２３・Ａｏｐｔ３
【数７５】
Ｔａｄ３
≧Ｈｓ３１・Ａｏｐｔ１＋Ｈｓ３２・Ａｏｐｔ２＋Ｈｓ３３・Ａｏｐｔ３
【０１１５】
前述した終了時間の同期処理後における各関節加速度の予測値はＡｒｅａであった。各関節の最適な加速度Ａｏｐｔも、加速度予測値Ａｒｅａと同じ比例関係を保つものとすると、次式の関係を満たさなければならない。
【０１１６】
【数７６】
Ａｏｐｔ１：Ａｏｐｔ２：Ａｏｐｔ３＝Ａｒｅａ１：Ａｒｅａ２：Ａｒｅａ３
【０１１７】
上記数７３乃至７５及び数７６の関係を満たす加速度Ａｏｐｔが、加速度予測値Ａｒｅａの各関節間の比例関係を保ち、かつ加速に使用できるトルクを最大限利用できる加速度であり、本実施形態において、最適となる値となる。基準関節は、動作する関節であればどれでも良いが、本実施例では、Ａｒｅａの中で絶対値が最大となる関節を求め、これを基準関節ｊとする。基準関節ｊをベースに上記数７６を次式のように変形する。
【０１１８】
【数７７】
Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・Ａｏｐｔｊ
【数７８】
Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・Ａｏｐｔｊ
【数７９】
Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・Ａｏｐｔｊ
【０１１９】
ここで、Ａｒｅａｊは基準関節ｊの予測加速度であり、Ａｏｐｔｊは基準関節ｊの最適な加速度である。次に、上記数７７乃至７９を上記数７３乃至７５に代入して展開すると、次の３個の不等式になる。
【０１２０】
【数８０】
Tad1
≧H11・(Area1/Areaj)・Aoptj+H12・(Area2/Areaj)・Aoptj
+H13・(Area3/Areaj)・Aoptj
【数８１】
Tad2
≧H21・(Area1/Areaj)・Aoptj+H22・(Area2/Areaj)・Aoptj
+H23・(Area3/Areaj)・Aoptj
【数８２】
Tad3
≧H31・(Area1/Areaj)・Aoptj+H32・(Area2/Areaj)・Aoptj
+Hn3・(Area3/Areaj)・Aoptj
【０１２１】
上記数８０乃至８２の３個の不等式において、基準関節ｊにおける最適な加速度Ａｏｐｔｊ以外はすべて既知の値であるから、上記数８０乃至８２の３個の不等式から、基準関節ｊにおける最適な加速度が３通り計算できる。このときに計算される３個の加速度をそれぞれを、Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３とすると、次式を得る。
【０１２２】
【数８３】
Aoptj_1=Tad1・Areaj/(H11・Area1+H12・Area2+H13・Area3)
【数８４】
Aoptj_2=Tad2・Areaj/(H21・Area1+H22・Area2+H23・Area3)
【数８５】
Aoptj_3=Tad3・Areaj/(H31・Area1+H32・Area2+H33・Area3)
【０１２３】
上記数８３乃至８５に示す加速度Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３から、絶対値が最小の値を基準関節ｊの最適な加速度としなければならない。なぜなら、絶対値が最小の値のみが、上記数８０乃至数８２の３個の不等式を同時に満たすことができるからである。加速度Ａｏｐｔｊ＿１、Ａｏｐｔｊ＿２、Ａｏｐｔｊ＿３の中で最小の値を最小の加速度Ａｏｐｔｊ＿ｍｉｎとし、これを上記数７７乃至数７９に代入すると次式となり、これによりすべての関節の最適な加速度を求めることができる。
【０１２４】
【数８６】
Ａｏｐｔ１＝（Ａｒｅａ１／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【数８７】
Ａｏｐｔ２＝（Ａｒｅａ２／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【数８８】
Ａｏｐｔ３＝（Ａｒｅａ３／Ａｒｅａｊ）・Ａｏｐｔｊ＿ｍｉｎ
【０１２５】
このようにして求められた加速度が、第１の課題を満たす最適な加速度となる。最適な減速度Ｄｏｐｔについても、全く同様の手順で求めることができる。具体的には、上記数７０乃至数８８において、パラメータθｓ、Ｈｓ、Ｃｓ、Ｇｓ、Ａｏｐｔ、Ａｒｅａの代わりにそれぞれ、パラメータθｅ、Ｈｅ、Ｃｅ、Ｇｅ、Ｄｏｐｔ、Ｄｒｅａを使用すればよい。ここで、加速度に関する数７６に対応する減速度に関する式は次式となる。
【０１２６】
【数８９】
Ｄｏｐｔ１：Ｄｏｐｔ２：Ｄｏｐｔ３＝Ｄｒｅａ１：Ｄｒｅａ２：Ｄｒｅａ３
【０１２７】
次いで、ステップＳ６では、以上の計算の結果求められた最適な加減速度が保持されるような、速度パターンの演算処理を行う。以下に、その手順を説明する。ステップＳ３の終了時間の同期処理において、最長動作時間となった第ｋ関節についてのみ、最適な加速度Ａｏｐｔｋ、最適な減速度Ｄｏｐｔｋを用いて、前述した第１の速度パターン計算により基準動作時間Ｔｂａｓｅを求める。次いで、ステップＳ７では、第ｋ関節以外の関節について、加速度が最適な加速度Ａｏｐｔとなり、減速度が最適な減速度Ｄｏｐｔとなり、かつ動作時間が基準動作時間Ｔｂａｓｅとなるように、第２の速度パターン計算を行う。ステップＳ６、Ｓ７により、すべての関節の速度パターンにおける動作時間が基準動作時間Ｔｂａｓｅとなるので、終了時間の同期処理を施す必要が無くなり、先に計算された最適な加減速度が保持される。
【０１２８】
ロボットの第ｉの関節における、第２の速度パターン計算方法について図９を交えて具体的に説明する。図９から判るように、既知の値である基準動作時間Ｔｂａｓｅ、第ｉの関節の最適な加速度Ａｏｐｔｉ、第ｉの関節の最適な減速度Ｄｏｐｔｉ、第ｉの関節の移動量Ｓａｉは、第ｉの関節の最適な速度Ｖｏｐｔｉ、第ｉの関節の加速時間Ｔａｉ、第ｉの関節の定速度時間Ｔｃｉ、第ｉの関節の減速時間Ｔｄｉにより以下の式で表される。
【０１２９】
【数９０】
Ｔｂａｓｅ＝Ｔａｉ＋Ｔｃｉ＋Ｔｄｉ
【数９１】
Ｓａｉ＝Ｖｏｐｔｉ・（Ｔｃｉ＋（Ｔａｉ＋Ｔｄｉ）／２）
【数９２】
Ｖｏｐｔｉ＝Ｔａｉ・Ａｏｐｔｉ
【数９３】
Ｖｏｐｔｉ＝Ｔｄｉ・Ｄｏｐｔｉ
【０１３０】
上記数９０乃至９３の４つの式からなる連立方程式を解くと、時間Ｔｃｉ、Ｖｏｐｔｉ、Ｔａｉ、Ｔｄｉを、以下の式から演算できる。
【０１３１】
【数９４】

【０１３２】
【数９５】
Ｖｏｐｔｉ
＝(Ｔｂａｓｅ−Ｔｃｉ)・Ａｏｐｔｉ・Ｄｏｐｔｉ／(Ｄｏｐｔｉ＋Ａｏｐｔｉ)
【数９６】
Ｔａｉ＝Ｖｏｐｔｉ／Ａｏｐｔｉ
【数９７】
Ｔｄｉ＝Ｖｏｐｔｉ／Ｄｏｐｔｉ
【０１３３】
ただし、加速度が最適な加速度Ａｏｐｔｉとなり、減速度が最適な減速度Ｄｏｐｔｉとなり、かつ動作時間が基準動作時間Ｔｂａｓｅとなるような、速度パターン計算が求まらないケースがある。それは、次の２つのケースである。
（ａ）上記数９４において、定速時間Ｔｃｉが虚数として求まる場合、及び
（ｂ）上記数９４において、最適な速度Ｖｏｐｔｉが教示速度Ｖｉを越える値として求まる場合。
【０１３４】
ステップＳ８では、上記の２つのケースに該当する関節が存在した場合をＮＯと判定する。ステップＳ９では、ステップＳ８でＮＯと判定された全ての関節について、従来と同様の第１の速度パターン計算を実施する。次いでステップＳ１０では、すべての関節の速度パターンについて、前述の終了時間の同期処理を施す。ステップＳ８でＮＯと判定されるのは、加減速度を最適化したことで、最適化前とは最大動作関節が変わってしまうケースであるあるが、ステップＳ９、Ｓ１０の処理を実施することで、加減速度の最適度合いが若干低下するが、加減速に使用できるトルクを越えることは無い。
【０１３５】
最後に、上記数７１におけるｇは重力加速度ベクトルであり、重力の働く方向を定義するものである。一般的に、ロボット座標において重力が−ｚ方向に働くよう座標を定義するので、ｇは次式のようになる。なお、重力加速度は、９．８ｍ／ｓｅｃ²である。
【０１３６】
【数９８】
ｇ＝［０，０，−１×（重力加速度）］^T
【０１３７】
以上説明したように、本発明に係る実施形態によれば、開始位置及び目標位置と速度予測値とロボットダイナミクスベースパラメータから、上記数９乃至数３８を用いて、開始位置及び目標位置における慣性行列Ｈ、粘性行列Ｃ及び重力行列Ｇを含むロボットダイナミクスモデルを計算し（ステップＳ４）、計算されたロボットダイナミクスモデルに基づいて、加速度予測値の各関節間の比例関係と各関節の許容ピークトルクの条件を満足する最適な加速度及び減速度を演算した（ステップＳ５）後、すべての関節で動作時間を一致させる同期処理を実行する（ステップＳ６、Ｓ７）。従って、ロボット機構部１の各関節毎の移動量に大きな差がある位置決め動作を行う場合でも、駆動関節の許容トルクを最大限有効に使用した加減速度を演算し、動作時間の短縮を可能にし、関節に連結されるリンク自体の慣性による負荷トルク成分を考慮し、関節回転中心から関節に連結されるリンクの重心位置までの距離が短いロボットや、ロボットが特異な姿勢となっても、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。これにより、ロボット機構部１の各関節の許容ピークトルクを越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求めることができる。
【０１３８】
また、動作時間が基準動作時間Ｔｂａｓｅとなり、最適な加速度Ａｏｐｔで動作し、最適な減速度Ｄｏｐｔで動作する速度パターンがすべての関節で存在しないときは、ステップＳ９で第１の速度パターン計算方法を用いて速度パターンを演算した後、ステップＳ１０で従来例の速度パターン再計算処理で同期処理を実行しているので、すべての場合について、最適な速度パターンを演算できる。
【０１３９】
さらに、本実施形態によれば、モータの軸受けや減速機の摩擦抵抗を考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４０】
またさらに、本実施形態によれば、減速機を含むモータロータの慣性モーメントにより発生するトルクを考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４１】
＜変形例＞
以上の実施形態において、ロボット機構部１は、３個の関節ＲＪ１乃至ＲＪ３を備えているが、本発明はこれに限らず、少なくとも２個の関節を備えていればよい。
【０１４２】
以上の実施形態においては、各関節ＲＪ１乃至ＲＪ３は回転可能に構成されているが、本発明はこれに限らず、２つの関節以上では少なくとも１つは、並進運動を行う直動であってもよい。
【０１４３】
【発明の効果】
以上詳述したように本発明に係るロボットシステムの制御方法又は制御装置によれば、計算されたロボットダイナミクスモデルに基づいて、加速度予測値の各関節間の比例関係と各関節の許容ピークトルクの条件を満足する最適な加速度及び減速度を演算した後、すべての関節で動作時間を一致させる同期処理を実行する。従って、ロボット機構部の各関節毎の移動量に大きな差がある位置決め動作を行う場合でも、駆動関節の許容トルクを最大限有効に使用した加減速度を演算し、動作時間の短縮を可能にし、関節自体の慣性による負荷トルク成分を考慮し、関節回転中心から重心位置までの距離が短いロボットや、ロボットが特異な姿勢となっても、厳密な負荷トルク成分の計算を実現することで、許容値を越えるトルクを必要とするような加減速度の演算を防止する。これにより、ロボット機構部の各関節の許容ピークトルクを越えず、かつ最短時間で動作が終了できる最適な各関節の加減速度を求めることができる。
【０１４４】
また、上記ロボットシステムの制御方法又は制御装置において、好ましくは、上記加速度に関係しないトルク成分を、上記粘性行列及び重力行列に加算することにより、上記各関節で発生する摩擦抵抗を考慮する。従って、上記各関節の摩擦抵抗を考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【０１４５】
さらに、上記ロボットシステムの制御方法又は制御装置において、好ましくは、上記慣性行列の対角成分に、上記各関節の慣性モーメントを加算することにより、上記各関節自身の加減速で発生するトルク演算値を補正する。従って、上記各関節の慣性モーメントにより発生するトルクを考慮し、厳密な負荷トルク成分の計算を実現するので、許容値を越えるトルクを必要とするような加減速度の演算を防止することができる。
【図面の簡単な説明】
【図１】 本発明に係る一実施形態であるロボットシステムの構成を示すブロック図である。
【図２】 図１のロボット機構部１の構成を示すスケルトン図である。
【図３】 図１のロボット機構部１の各関節ＲＪｋ（ｋ＝１，２，３）の構成を示す斜視図、並びに図１の制御装置１０内のサーボ制御回路１０７の構成を示すブロック図である。
【図４】 図１のＣＰＵ１００によって実行される位置決め動作処理を示すフローチャートである。
【図５】 図４の速度パターンの予測演算処理（ステップＳ２）の概念を示すグラフであって、（ａ）は第１の関節の予想速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の予想速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の予想速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【図６】 図４の速度パターンの終了時間の同期処理（ステップＳ３）の概念を示すグラフであって、（ａ）は第１の関節の予想速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の予想速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の予想速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【図７】 台形状の速度パターンのときの本実施形態で用いる第１の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図８】 三角形状の速度パターンのときの本実施形態で用いる第１の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図９】 本実施形態で用いる第２の速度パターン計算方法を示す、時間対速度特性を示すグラフである。
【図１０】 第１の従来例の位置決め動作処理を示すフローチャートである。
【図１１】 図１０の終了時間の同期処理の一例を説明するための各関節の速度パターンを示すグラフであって、（ａ）は第１の関節の速度パターンを示す時間対第１の関節の速度特性を示すグラフであり、（ｂ）は第２の関節の速度パターンを示す時間対第２の関節の速度特性を示すグラフであり、（ｃ）は第３の関節の速度パターンを示す時間対第３の関節の速度特性を示すグラフである。
【符号の説明】
１…ロボット機構部、
１０…制御装置、
２０…ティーチペンダント、
１００…ＣＰＵ、
１０１…ＲＯＭ、
１０２…ＲＡＭ、
１０３…インターフェース、
１０４…デュアルポートＲＡＭ、
１０５…ラッチ回路、
１０６…ハードディスクメモリ、
１０６ａ…教示データメモリ、
１０６ｂ…仮の加減速度情報メモリ、
１０６ｃ…ロボットダイナミクスベースパラメータメモリ、
１０７…サーボ制御回路、
１１０…クロック発生器、
１１１…分周器、
ＲＪ１乃至ＲＪ３……関節、
Ｌ１乃至Ｌ４…リンク、
ＭＯ１乃至Ｍ３…モータ、
ＲＥ１乃至ＲＥ３…減速機、
ＳＮ１乃至ＳＮ３…センサ。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a control method and a control apparatus for a robot system.
[0002]
[Prior art]
Conventionally, a control device of a robot system having a plurality of joints is provided with an acceleration / deceleration (hereinafter, acceleration and deceleration are included, and acceleration / deceleration is included, and acceleration / deceleration is included) in an operation speed command. Operation is realized. Even if the joints of the robot are operated with the same speed pattern, the torque generated at the joints differs depending on the posture of the robot. This is because load torque such as gravity and interference force received from other joints changes depending on the joint position of the robot, that is, the robot posture. If the speed pattern is fixed in the robot posture in which the load torque is maximum, the operation time in the posture in which the influence of the load torque is small is extended more than necessary. On the other hand, if it is fixed in the minimum posture, a torque that is larger than the motor can tolerate in a posture with a large load torque is required, and an abnormality occurs in the motor control.
[0003]
FIG. 10 is a flowchart showing the positioning operation process of the first conventional example. In FIG. 10, first, after resetting the joint number i to 0 in step S101, the joint number i becomes the final joint i._maxSteps S102 to S104 are repeated for each joint number i. Here, in step S102, the start position, the target position, and the target speed are set. In step S103, the movement amount is calculated. In step S104, the speed pattern is calculated based on the acceleration and deceleration preset information. Do. Further, in step S107, the joint having the longest motion time is extracted, and after resetting the joint number i to 0 in step S108, the joint number i becomes the final joint i._maxUntil end time, the synchronization process of the end time, which is the recalculation process of the speed pattern in step S109, is repeated for each joint number i.
[0004]
FIG. 11 is a graph showing a velocity pattern of each joint for explaining an example of the end time synchronization process of FIG. In step S107 of FIG. 10, the second joint has the longest motion time t._maxThe velocity patterns of the first joint and the third joint are extracted as having the motion time t of the second joint is the longest motion time t_maxAnd
(A) The area Sa becomes equal to the area Sap,
(B) The area Sb is equal to the area Sbp,
(C) The area Sc is equal to the area Scp,
(D) Area Sd becomes equal to area Sdp
To be determined. In addition, the area which is the integral value of the graph of FIG. 10 represents distance.
[0005]
In the first conventional example, the acceleration and deceleration information used for generating the speed pattern in step S104 is preset for each joint, and the degree of influence of the load torque that varies according to the posture of the robot is Not considered. The fluctuating load torque includes gravity, load inertia moment, interference force received from other joints, and the like. For this reason, the preset acceleration and deceleration information are generally set to values that do not cause any abnormality in motor control in the robot posture where the load torque is maximum.
[0006]
[Problems to be solved by the invention]
However, the first conventional method has the following problems.
(A) In the positioning operation in the robot posture where the load torque is not maximum, the motor capacity is not used to the maximum extent.
(B) When designing the robot, the motor must be selected so that no abnormality occurs in the motor control in the robot posture with the maximum load torque, which inevitably results in overspec.
[0007]
In order to solve this problem, Japanese Patent Application Laid-Open No. 7-261822 (hereinafter referred to as a second conventional example) describes a driving joint that receives an interference torque due to a gravitational moment, an inertia due to acceleration, and an operation of another joint. The load torque component generated from the mass of each joint and the position of the center of gravity is calculated, and the load torque component affected by the acceleration is subtracted from the allowable peak torque of the drive joint, which is not affected by the acceleration / deceleration. The method of obtaining the acceleration / deceleration time was taken so as to be a value. However, this method also has the following problems.
[0008]
(A) Drive when the acceleration / deceleration time is calculated under the precondition that all the joints of the robot have the same acceleration / deceleration time, and the positioning operation has a large difference in the movement amount of each joint. The operating time cannot be shortened even though there is a margin in the allowable torque of the joint. Hereinafter, it is referred to as a first problem.
(B) Since the load torque component due to the inertia tensor of the robot link itself is not taken into account, a robot with a short distance from the rotation center of the joint to the position of the center of gravity cannot accurately calculate the load torque component. Hereinafter, it is referred to as a second problem.
[0009]
The object of the present invention is to solve the above-mentioned problems and to obtain the optimum acceleration / deceleration of each joint that can be completed in the shortest time without exceeding the allowable peak torque (allowable value) of each joint of the robot mechanism section. It is also possible to provide a robot system control method and control apparatus that can reduce the operation time.
[0010]
[Means for Solving the Problems]
  A control method of a robot system according to the present invention is a control method of a robot system that controls the operation of a robot mechanism unit having a plurality of joints,
  A first storage step of storing, as teaching data, a start position, a target position, and a target speed of each joint of the robot mechanism section when operating the robot mechanism section;
  A second storage step of storing, in a second storage means, information on acceleration and deceleration of each joint used when predicting and calculating a speed pattern of each joint of the robot mechanism section;
  Based on the teaching data stored in the first storage means and the acceleration and deceleration of each joint stored in the second storage means,A speed pattern with respect to time for each joint so as to operate with the acceleration and deceleration of each joint stored from the teaching data which is the start position, target position and target speed of each joint, and a constant acceleration Calculate a trapezoidal speed pattern that operates at a constant speed and operates at a constant deceleration, or a triangular speed pattern that operates at a constant acceleration and operates at a constant deceleration.Using the first velocity pattern calculation method, each joint of the robot mechanism unitAdditionA first calculation step of calculating a speed predicted value and an operating time predicted value, and calculating the maximum value as the longest operating time from the operating time predicted value;
  The joint with the maximum motion time among the predicted motion time values is the maximum motion joint, the motion time of the maximum motion joint is the longest motion time, and the motion time prediction value for each joint is the same as the longest motion time. The trapezoidal speed is the same when the acceleration is the same before and after the recalculation, and the same when the deceleration is before and after the recalculation, and operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. Recalculate the motion speed and acceleration / deceleration values so that they become patterns, and recalculate them as speed prediction values, acceleration prediction values, and deceleration prediction values, respectively.Using the speed pattern recalculation method, for each joint of the robot mechanism unit, the predicted operation time calculated by the first calculation step is the same as the longest operation time calculated by the first calculation step. In a control method for a robot system, including a second calculation step in which values recalculated as speed, acceleration, and deceleration are used as a speed prediction value, an acceleration prediction value, and a deceleration prediction value.
  A third storage step of storing, in a third storage means, robot dynamics base parameters including an inertia tensor, a mass and a gravity center position of each link connected to each joint of the robot mechanism section;
  A start position and a target position of each joint stored in the first storage means; a predicted speed value calculated in the second calculation step; and a robot dynamics base parameter stored in the third storage means; On the basis of the,
(A) For each joint of the robot mechanism section, an inertia matrix indicating torque generated in the joint itself by acceleration, and interference torque generated by acceleration of other joints;
(B) a viscosity matrix indicating a torque generated by centrifugal force and Coriolis force for each joint of the robot mechanism unit;
(C) a gravity matrix indicating torque generated by the gravitational moment of each joint of the robot mechanism section;
A third calculation step for calculating a robot dynamics model at the start position and the target position, including:
  Based on the robot dynamics model calculated by the third calculation step, and the predicted acceleration value and deceleration prediction value of each joint calculated by the second calculation step,A first proportional relationship in which a predicted acceleration value of each joint is proportional to each other and a proportional relationship between the joints of the acceleration predicted value is maintained during acceleration, and a predicted deceleration value of each joint is decelerated in proportion to each other Sometimes a second proportional relationship is maintained that the proportional relationship between each joint in the predicted deceleration value is retainedSatisfies the first condition, andThe value of the weight term obtained from the gravity matrix not related to acceleration and deceleration from the allowable peak torque of each joint, and the value of the viscosity term obtained from the viscosity matrix not related to acceleration and decelerationThe subtraction result value obtained by subtracting the optimal acceleration and the optimal condition satisfying the second condition that each joint is equal to or greater than the torque generated at each joint when the joint is operated with the predicted acceleration value and the predicted deceleration value. DecelerationFor each jointA fourth calculation step for calculating;
  The joint having the longest operation time in the first calculation step is searched, and the optimum acceleration and optimum deceleration calculated in the fourth calculation step are calculated for the k-th joint that is the searched joint, From the teaching data including the start position, target position and target speed of the kth joint stored in the first storage means, the speed pattern of the kth joint is calculated using the first speed pattern calculation method, A fifth calculation step using the operation time as a reference operation time;
  For all joints other than the kth joint, the reference operation time calculated by the fifth calculation step, the optimum acceleration and optimum deceleration calculated by the fourth calculation step, and the first Based on the teaching data including the start position and the target position stored in the storage means,Only for the joint that has reached the reference movement time that is the longest movement time, the reference movement time is obtained by the first speed pattern calculation method using the optimum acceleration and the optimum deceleration, and for other joints, Calculate the trapezoidal velocity pattern of each joint so that the optimal acceleration and the optimal deceleration are maintained and the operation time is the above-mentioned reference operation time.Using a second speed pattern calculation method, and a sixth calculation step of generating a speed pattern that has the operation time as the reference operation time and operates at the optimum acceleration and the optimum deceleration. And
[0011]
  In the above-described robot system control method, preferably,In the viscosity matrix and the gravity matrix, as torque not related to acceleration,By adding to the viscosity matrix and the gravity matrix, the frictional resistance generated at each joint is taken into consideration.
[0012]
  Furthermore, in the control method of the robot system, preferably,Consisting of moment of inertia of each jointFor each diagonal component of the inertia matrix,Motor rotor including reducerAnd a step of correcting a torque calculation value generated by acceleration / deceleration of each joint itself by adding the inertia moments.
[0013]
  A control device for a robot system according to the present invention is a control device for a robot system that controls the operation of a robot mechanism having a plurality of joints,
  First storage means for storing, as teaching data, the start position, target position, and target speed of each joint of the robot mechanism when operating the robot mechanism;
  Second storage means for storing information on acceleration and deceleration of each joint used when predicting and calculating a speed pattern of each joint of the robot mechanism section;
  Based on the teaching data stored in the first storage means and the acceleration and deceleration of each joint stored in the second storage means,A speed pattern with respect to time for each joint so as to operate with the acceleration and deceleration of each joint stored from the teaching data which is the start position, target position and target speed of each joint, and a constant acceleration Calculate a trapezoidal speed pattern that operates at a constant speed and operates at a constant deceleration, or a triangular speed pattern that operates at a constant acceleration and operates at a constant deceleration.First speed pattern calculationMethodUsing the above, each joint of the robot mechanismAdditionA first calculation means for calculating a speed prediction value and an operation time prediction value, and calculating the maximum value as the longest operation time from the operation time prediction values;
  The joint with the maximum motion time among the predicted motion time values is the maximum motion joint, the motion time of the maximum motion joint is the longest motion time, and the motion time prediction value for each joint is the same as the longest motion time. The trapezoidal speed is the same when the acceleration is the same before and after the recalculation, and the same when the deceleration is before and after the recalculation, and operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. Recalculate the motion speed and acceleration / deceleration values so that they become patterns, and recalculate them as speed prediction values, acceleration prediction values, and deceleration prediction values, respectively.Speed pattern recalculationMethodFor each joint of the robot mechanism unit, the speed of the predicted motion time calculated by the first calculation means is the same as the longest motion time calculated by the first calculation means. And a controller for a robot system comprising second calculation means for setting the values recalculated as acceleration and deceleration as speed prediction values, acceleration prediction values, and deceleration prediction values,
  Third storage means for storing robot dynamics base parameters including an inertia tensor, a mass and a gravity center position of each link connected to each joint of the robot mechanism section;
  A start position and a target position of each joint stored in the first storage means; a speed prediction value calculated by the second calculation means; and a robot dynamics base parameter stored in the third storage means; On the basis of the,
(A) For each joint of the robot mechanism section, an inertia matrix indicating torque generated in the joint itself by acceleration, and interference torque generated by acceleration of other joints;
(B) a viscosity matrix indicating a torque generated by centrifugal force and Coriolis force for each joint of the robot mechanism unit;
(C) a gravity matrix indicating torque generated by the gravitational moment of each joint of the robot mechanism section;
A third computing means for calculating a robot dynamics model at the start position and the target position, including:
  Based on the robot dynamics model calculated by the third calculation means and the predicted acceleration value and deceleration prediction value of each joint calculated by the second calculation means,A first proportional relationship in which a predicted acceleration value of each joint is proportional to each other and a proportional relationship between the joints of the acceleration predicted value is maintained during acceleration, and a predicted deceleration value of each joint is decelerated in proportion to each other Sometimes a second proportional relationship is maintained that the proportional relationship between each joint in the predicted deceleration value is retainedSatisfies the first condition, andThe value of the weight term obtained from the gravity matrix not related to acceleration and deceleration from the allowable peak torque of each joint, and the value of the viscosity term obtained from the viscosity matrix not related to acceleration and decelerationThe subtraction result value obtained by subtracting the optimal acceleration and the optimal condition satisfying the second condition that each joint is equal to or greater than the torque generated at each joint when the joint is operated with the predicted acceleration value and the predicted deceleration value. DecelerationFor each jointA fourth calculating means for calculating;
  A search is made for the joint having the longest operation time in the first calculation means, and for the k-th joint that is the searched joint, the optimum acceleration and optimum deceleration calculated by the fourth calculation means, From the teaching data including the start position, target position and target speed of the kth joint stored in the first storage means, the first speed pattern calculation is performed.MethodCalculating a speed pattern of the k-th joint using, and using the operation time as a reference operation time;
  For all joints other than the k-th joint, the reference operation time calculated by the fifth calculation means, the optimum acceleration and optimum deceleration calculated by the fourth calculation means, and the first Based on the teaching data including the start position and the target position stored in the storage means,Only for the joint that has reached the reference movement time that is the longest movement time, the reference movement time is obtained by the first speed pattern calculation method using the optimum acceleration and the optimum deceleration, and for other joints, Calculate the trapezoidal velocity pattern of each joint so that the optimal acceleration and the optimal deceleration are maintained and the operation time is the above-mentioned reference operation time.Second speed pattern calculationMethodAnd a sixth operation means for generating a speed pattern that operates with the optimum acceleration and the optimum deceleration.
[0014]
  In the control device for the robot system, preferably,In the viscosity matrix and the gravity matrix, as torque not related to acceleration,The frictional resistance generated at each joint is taken into consideration.
[0015]
  Furthermore, in the control device of the robot system, preferably,Consisting of moment of inertia of each jointFor each diagonal component of the inertia matrix,Motor rotor including reducerMeans for correcting torque calculation values generated by acceleration / deceleration of the joints by adding the inertia moments.
[0016]
DETAILED DESCRIPTION OF THE INVENTION
Embodiments according to the present invention will be described below with reference to the drawings.
[0017]
FIG. 1 is a block diagram showing a configuration of a robot system according to an embodiment of the present invention, and FIG. 2 is a skeleton diagram showing a configuration of the robot mechanism unit 1 of FIG. In FIG. 1, a robot system according to this embodiment includes, for example, a robot mechanism unit 1 having a plurality of joints RJ1 to RJ3, three, a control device 10 for controlling the operation of the robot mechanism unit 1, and teaching data. The teaching pendant 20 is an input means for inputting data such as. Here, as shown in FIG. 2, the robot mechanism unit 1 is connected to the joint RJ1 from the base BA1 via the link L1, and then the joint RJ1 is connected to the joint RJ2 via the link L2, and then the joint RJ2 Is connected to the joint RJ3 via the link L3, and the link R4 is connected to the joint RJ3. The end of the link L4 is an operation end LE for performing a predetermined operation such as a welding operation. Here, each joint RJ1 to RJ3 is configured to be rotatable.
[0018]
The control device 10 of the robot system according to this embodiment will be described in brief. By efficiently using the performance of the motor that drives the joints RJ1 to RJ3 of the robot mechanism unit 1, the tact time (process time) is shortened. In addition, the tolerance for motor selection at the time of manipulator development is improved. Specifically, an allowable peak torque that can be generated in the joints RJ1 to RJ3 is specified, and each joint RJ1 to RJ3 of the robot mechanism unit 1 is specified. A dynamics base comprising a starting position, a target position, a target speed, and a center of gravity position, a mass, and an inertia tensor of a robot link (also referred to as a link arm, but in this embodiment, links L1 to L4) connected to each joint. Based on the parameters, the optimal joint adjustments that allow the operation to be completed in the shortest time without exceeding the allowable peak torque of each joint. Determine the degree. In this case, even when a positioning operation with a large difference in the movement amount of each joint RJ1 to RJ3 of the robot mechanism unit 1 is performed, the acceleration / deceleration using the allowable peak torque of the driving joint is used to the maximum and the operation time is calculated. In addition to enabling shortening and considering the load torque component due to the inertia tensor of the link itself connected to the joint, the robot has a short distance from the joint rotation center to the link center of gravity, or the robot has a unique posture However, by implementing a strict calculation of the load torque component, calculation of acceleration / deceleration that requires torque exceeding the allowable value is prevented. Further, the control device 10 of the robot system according to this embodiment takes into account the frictional resistance of the motors MO1 to MO3 and the reduction gear, and realizes a strict calculation of the load torque component so that a torque exceeding the allowable value can be obtained. Prevents calculation of acceleration / deceleration as required. Furthermore, the control device 10 of the robot system according to this embodiment takes into account the torque generated by the moment of inertia of the rotors of the motors MO1 to MO3 including the speed reducer, and realizes a strict calculation of the load torque component to allow Acceleration / deceleration calculations that require torque exceeding the value are prevented.
[0019]
First, the operation principle of the control method according to this embodiment will be described in detail below. From the position of the center of gravity, the mass, the link length, the inertia tensor, and the position (angle), speed, and acceleration of each joint, the torque T generated at each joint is expressed by the following equation: It is known that it can be calculated by the robot dynamics model.
[0020]
[Expression 1]
T = H (θ) · θdd + C (θ, θd) + G (θ) · g
[0021]
This number 1 can be found in, for example, 11.3. Of Prior Art Document 1 “Shigeo Hirose,“ Robot Engineering—Vector Analysis of Mechanical Systems ”, Hanafusa, 2nd Edition, February 10, 1989”. In [4], the procedure of inverse dynamics calculation using Newton Euler's method, which is one of rigid body motion analysis methods, is explained in detail. 1 (11.28). Where J in formula (11.28)^TK is an external force and moment to be applied to the tip of the robot, and is a term that does not need to be considered in the embodiment according to the present invention. Here, the robot dynamics model is composed of an inertia matrix H, a viscosity matrix C, and a gravity matrix G. The inertia matrix H is generated by the torque generated in the joint itself by the acceleration of each joint of the robot mechanism and the acceleration of the other joint. The viscosity matrix C indicates the torque generated by the centrifugal force and the Coriolis force of each joint of the robot mechanism unit, and the gravity matrix G indicates the torque generated by the gravity moment of each joint of the robot mechanism unit.
[0022]
The inertia matrix H, the viscosity matrix C, and the gravity matrix G can be calculated from the center of gravity position, the mass, the inertia tensor, and the position, velocity, and acceleration of each joint connected to each joint of the robot mechanism unit 1. Here, the position of the center of gravity, the mass, the inertia tensor, and the link length of the link connected to each joint of the robot are known fixed values that can be obtained in advance from the mechanism and link shape and material of the robot. When the above equation 1 is used, the torque generated at each joint can be calculated from the position, velocity, and acceleration of each joint. Each item of Equation 1 has the following meaning. In the embodiment, the number of each joint is n = 3.
[0023]
[Table 1]

[0024]
Next, the unit and the number of each item used in this embodiment are shown below.
[0025]
[Table 2]
―――――――――――――――――――――――――――――――――――
Symbol Item Unit Quantity
―――――――――――――――――――――――――――――――――――
T Torque generated in the joint N · mn × 1 matrix
θ Joint position (angle) rad n × 1 matrix
θd Joint speed rad / sec n × 1 matrix
θdd Joint acceleration rad / sec²  n × 1 matrix
H Inertia matrix kg · m²        n × n matrix
C Viscosity matrix N · m n × 1 matrix
G Gravity matrix kg · m n × 3 matrix
g Gravity acceleration vector m / s²          3 × 1 matrix
Tpeak allowable joint torque N · m n × 1 matrix
θs start position rad n × 1
θe target position rad n × 1
V target speed rad / sec n × 1
Asmp provisional acceleration rad / sec²  n × 1
Dsmp Temporary deceleration rad / sec²  n × 1
VestAdditionSpeed prediction value rad / sec n × 1
Test operation time predicted value sec n × 1
Vrea speed prediction value rad / sec n × 1
Area Acceleration prediction value rad / sec²  n × 1
Dream deceleration prediction value rad / sec²  n × 1
Tmax Maximum operating time sec 1
―――――――――――――――――――――――――――――――――――
Ixi, Ixyi, Ixzi Link inertia tensor kg · m²  3x3
Ixyi, Iyi, Iyzi
Ixzi, Iyzi, Izi
―――――――――――――――――――――――――――――――――――
mi link mass kg 1 × n
mxi, myi, mzi Link centroid position vector m 3 × n
No symbol Gravity acceleration m / sec²      1
Cm Kinematic viscosity coefficient N ・ m ・ sec n × 1 matrix
θdm Motor speed rad / sec n × 1 matrix
Fcou Coulomb friction torque N · m n × 1 matrix
―――――――――――――――――――――――――――――――――――
Im kg / m of motor rotor including reduction gear²        1xn
            Moment of inertia
―――――――――――――――――――――――――――――――――――
Tad Torque that can be used for acceleration / deceleration N ・ mn × 1 matrix
Aopt Optimal acceleration rad / sec²    n × 1 matrix
Dopt Optimal deceleration rad / sec²    n × 1 matrix
Tbase reference operation time sec 1
Sai The movement amount of the i-th joint rad 1
Vopti Optimal speed of the i-th joint rad / sec 1
Tai Acceleration time of the i-th joint sec 1
Tci Constant speed time of the i-th joint sec 1
Tdi Deceleration time of the i-th joint sec 1
ξi Reduction ratio of i-th joint dimensionless 1
―――――――――――――――――――――――――――――――――――
[0026]
Next, base parameters for robot dynamics in the example (n = 3) will be described. The mass mi is connected to the i-th joint that moves by the operation of the i-th joint (where i is a natural number, taking from 1 to the maximum number of joints) and does not move by the operation of the i + 1-th joint. Indicates the mass of the link to be performed. The center-of-gravity positions mxi, myi, and mzi indicate the center-of-gravity positions of the links that are moved by the operation of the i-th joint and that are connected to the i-th joint that is not moved by the operation of the i + 1-th joint. Further, the inertia tensors Ixi to Izi of the i-th joint indicate the inertia tensors of the link that is connected to the i-th joint that moves by the motion of the i-th joint and does not move by the motion of the i + 1-th joint. In Section 3.8 of Prior Art Document 1, the calculation procedure is described in detail, and finally, the operation expression of the inertia tensor is the expression (3.38) of Prior Art Document 1.
[0027]
Here, a reference for calculating the position of the center of gravity and the inertia tensor is a link coordinate system. The link coordinate system is, for example, a link coordinate system assigned to each joint in section 2.9 of the prior art document 2 “Translation by Tsuneo Yoshikawa,“ Robot Manipulator ”, Corona, Inc., First Edition, December 15, 1989”. The definition procedure is described in detail. Here, the definition of the basic link coordinate system will be described.
[0028]
First, the inertia tensor I is generally used to indicate the difficulty of rotating an object, and is represented by a 3 × 3 symmetric matrix of the following formula as known in the art, and its diagonal components are the joints of the robot mechanism. The other moments represent the negative value of the product of inertia.
[0029]
[Expression 2]

[0030]
The moment of inertia is obtained from the following equation, and represents the difficulty in turning and stopping the object (robot link).
[0031]
[Equation 3]
Ix = ∫ (y²+ Z²) Dm
[Expression 4]
Iy = ∫ (z²+ X²) Dm
[Equation 5]
Iz = ∫ (x²+ Y²) Dm
[0032]
Next, the inertial product is obtained from the following equation, and represents the moment that disturbs the motion of the object (robot link) when it is performing the rotational motion.
[0033]
[Formula 6]
Ixy = ∫xydm
[Expression 7]
Iyz = ∫yzdm
[Equation 8]
Izx = ∫zxdm
[0034]
Here, x, y, and z in the formula are expressed by reference coordinates of a predetermined inertia tensor, and dm indicates a mass of a minute volume. Here, the reference coordinate is a link coordinate system. For example, in the link L3 extending from the second joint to the third joint, the link coordinates assigned to the second joint are determined with the rotation center of the second joint as the origin, and the link coordinates of the second joint are determined. The x direction in is the link length direction, the z direction is the rotation axis direction, and the y direction is the right-handed coordinate system direction. Here, the reference coordinate of the inertia tensor of the link L3 connected to the second joint is the coordinate system when the link coordinate system assigned to the second joint is moved to the center of gravity of the link L3. The same applies to other joints.
[0035]
Next, a method for calculating matrix values of inertia, viscosity, and gravity will be described. The inertia matrix H, the viscosity matrix C, and the gravity matrix G are obtained by numerical calculation using the following equations. Note that the position of the first joint is θ₁And the position of the second joint is θ₂And the position of the third joint is θ_ThreeAnd Also, the velocity of the first joint is θd₁And the velocity of the second joint is θd₂And the speed of the third joint is θd_ThreeAnd
[0036]
[Equation 9]

[0037]
here,
[Expression 10]
H11
= mx1²・ M1 + [2 ・ a1 ・ mx2 ・ cos (θ₂) + (1/2) ・ mx2²{1 + cos (2 ・ θ₂)} + a1²+ mz2²] m2
+ (a1²+2 {cos (θ₂) ・ A2-my3 ・ sin (θ₂+ θ_Three) + mx3 ・ cos (θ₂+ θ_Three)} a1
+ [-my3 {sin (2 ・ θ₂+ θ_Three) + sin (θ_Three)} + mx3 {cos (2 ・ θ₂+ θ_Three) + cos (θ_Three)}] a2
-mx3 ・ my3 ・ sin (2 ・ θ_Three+2 ・ θ₂) + (1/2) a2²{1 + cos (2 ・ θ₂)}
-(1/2) my3²{cos (2 ・ θ_Three+2 ・ θ₂) -1}
+ (1/2) mx3²{cos (2 ・ θ_Three+2 ・ θ₂) +1)} m3 + (1/2) Ix2 {1-cos (2 ・ θ₂)}
+ (1/2) Iy2 {1 + cos (2 ・ θ₂)}
+ (1/2) Ix3 {1-cos (2 ・ θ_Three+2 ・ θ₂)} + (1/2) Iy3 {cos (2 ・ θ_Three+2 ・ θ₂) +1}
-Ixy3 ・ sin (2 ・ θ_Three+2 ・ θ₂) + Iz1
## EQU11 ##
H12 = -m2 ・ mz2 ・ mx2 ・ sin (θ₂)
[Expression 12]
H13 = 0
[0038]
[Formula 13]
H21 = H12
[Expression 14]
H22
= m2 ・ mx2²+ [a2²+2 {mx3 ・ cos (θ_Three) -my3 ・ sin (θ_Three)} a2 + my3²+ mx3²] m3 + Iz3 + Iz2
[Expression 15]
H23 = [{mx3 ・ cos (θ_Three) -my3 ・ sin (θ_Three)} a2 + my3²+ mx3²] m3 + Iz3
[0039]
[Expression 16]
H31 = H13
[Expression 17]
H32 = H23
[Formula 18]
H33 = (my3²+ mx3²) m3 + Iz3
[0040]
[Equation 19]
C = [C1 C2 C3]^T
[0041]
here,
[Expression 20]
C1
= [{-mx2 ・ m2 (mx2 ・ sin (2 ・ θ₂) +2 ・ a1 ・ sin (θ₂))
+ (-2 ・ a1 (mx3 ・ sin (θ₂+ θ_Three) + my3 ・ cos (θ₂+ θ_Three) + a2 ・ sin (θ₂))
-2 ・ a2 (mx3 ・ sin (2 ・ θ₂+ θ_Three) + my3 ・ cos (2 ・ θ₂+ θ_Three))-2 ・ my3 ・ mx3 ・ cos (2 ・ θ_Three+2 ・ θ₂)
+ (my3²-mx3²) ・ Sin (2 ・ θ_Three+2 ・ θ₂) -a2²・ Sin (2 ・ θ₂)) m3
+ (Ix3-Iy3) sin (2 ・ θ_Three+2 ・ θ₂)
+ (Ix2-Iy2) sin (2 ・ θ₂) -2 ・ Ixy3 ・ cos (2 ・ θ_Three+2 ・ θ₂)} θd₂
+ {m3 (-2 ・ a1 (my3 ・ cos (θ₂+ θ_Three) + mx3 ・ sin (θ₂+ θ_Three))
-2 ・ my3 ・ mx3 ・ cos (2 ・ θ_Three+2 ・ θ₂)
+ (my3²-mx3²) sin (2 ・ θ_Three+2 ・ θ₂)
-a2 (mx3 (sin (2 ・ θ₂+ θ_Three) + sin (θ_Three)) + my3 (cos (2 ・ θ₂+ θ_Three) + cos (θ_Three))}]
-2 ・ Ixy3 ・ cos (2 ・ θ_Three+2 ・ θ₂)-(Iy3-Ix3) sin (2 ・ θ_Three+2 ・ θ₂)) θd_Three) θd₁
-m2, mz2, mx2, cos (θ₂) ・ Θd₂ ²
[Expression 21]
C2
= [(1/2) mx2 ・ m2 (mx2 ・ sin (2 ・ θ₂) +2 ・ a1 ・ sin (θ₂))
+ (1/2) (Iy3-Ix3) sin (2 ・ θ_Three+2 ・ θ₂)
+ (1/2) sin (2 ・ θ₂) (Iy2-Ix2) + Ixy3 ・ cos (2 ・ θ_Three+2 ・ θ₂)
+ {a1 (my3 ・ cos (θ₂+ θ_Three) + mx3 ・ sin (θ₂+ θ_Three))
+ a2 (mx3 ・ sin (2 ・ θ₂+ θ_Three) + my3 ・ cos (2 ・ θ₂+ θ_Three))
+ (1/2) a2²・ Sin (2 ・ θ₂) + (1/2) (mx3²-my3²) sin (2 ・ θ_Three+2 ・ θ₂)
+ my3 ・ mx3 ・ cos (2 ・ θ_Three+2 ・ θ₂)
+ a1 ・ a2 ・ sin (θ₂)} m3] θd₁ ²-2 ・ m3 ・ a2 (mx3 ・ sin (θ_Three) + my3 ・ cos (θ_Three)) θd₂・ Θd_Three
-m3 ・ a2 (mx3 ・ sin (θ_Three) + my3 ・ cos (θ_Three)) θd_Three ²
[Expression 22]
C3 = [{a1 (my3 ・ cos (θ₂+ θ_Three) + mx3 ・ sin (θ₂+ θ_Three))
+ (1/2) a2 {mx3 (sin (2 ・ θ₂+ θ_Three) + sin (θ_Three)) + my3 (cos (2 ・ θ₂+ θ_Three) + cos (θ_Three))}
+ (1/2) (mx3²-my3²) sin (2 ・ θ_Three+2 ・ θ₂) + my3 ・ mx3 ・ cos (2 ・ θ_Three+2 ・ θ₂)} m3
+ (1/2) (Iy3-Ix3) sin (2 ・ θ_Three+2 ・ θ₂) + Ixy3 ・ cos (2 ・ θ_Three+2 ・ θ₂)] θd₁ ²
+ m3 ・ a2 (mx3 ・ sin (θ_Three) + my3 ・ cos (θ_Three)) θd₂ ²
[0042]
[Expression 23]

[0043]
here,
[Expression 24]
G11
= -sin (θ₁) ・ Mx1 ・ m1
+ {cos (θ₁) ・ Mz2-sin (θ₁) ・ A1- (1/2) mx2 (sin (θ₁+ θ₂) -sin (-θ₁+ θ₂))} m2
-sin (θ₁) ・ A1 ・ m3- (1/2) m3 ((sin (θ₁+ θ₂) -sin (-θ₁+ θ₂)) a2
+ mx3 (-sin (-θ₁+ θ₂+ θ_Three) + sin (θ₁+ θ₂+ θ_Three))
+ my3 (cos (θ₁+ θ₂+ θ_Three) -cos (-θ₁+ θ₂+ θ_Three)))
[Expression 25]
G12
= cos (θ₁) ・ Mx1 ・ m1
+ {sin (θ₁) ・ Mz2 + cos (θ₁) ・ A1 + (1/2) mx2 (cos (-θ₁+ θ₂) + cos (θ₁+ θ₂))} m2
+ cos (θ₁) ・ A1 ・ m3 + (1/2) m3 {(cos (-θ₁+ θ₂) + cos (θ₁+ θ₂)) a2
+ mx3 (cos (-θ₁+ θ₂+ θ_Three) + cos (θ₁+ θ₂+ θ_Three))
+ my3 (-sin (-θ₁+ θ₂+ θ_Three) -sin (θ₁+ θ₂+ θ_Three))}
[Equation 26]
G13 = 0
[0044]
[Expression 27]
G31
= (1/2) m3 ・ {mx3 (-sin (-θ₁+ θ₂+ θ_Three) -sin (θ₁+ θ₂+ θ_Three))
-my3 (cos (-θ₁+ θ₂+ θ_Three) + cos (θ₁+ θ₂+ θ_Three))}
[Expression 28]
G32
= (1/2) m3 ・ {mx3 (cos (θ₁+ θ₂+ θ_Three) -cos (-θ₁+ θ₂+ θ_Three))
-my3 (-sin (-θ₁+ θ₂+ θ_Three) + sin (θ₁+ θ₂+ θ_Three))}
[Expression 29]
G33 = m3 (-my3 ・ sin (θ₂+ θ_Three) + mx3 ・ cos (θ₂+ θ_Three))
[0045]
[30]
G21
= (1/2) mx2 ・ m2 (-sin (θ₁+ θ₂) -sin (-θ₁+ θ₂))
+ (1/2) m3 ・ a2 (-sin (θ₁+ θ₂) -sin (-θ₁+ θ₂))
+ (1/2) m3 {mx3 (-sin (-θ₁+ θ₂+ θ_Three) -sin (θ₁+ θ₂+ θ_Three))-my3 (cos (-θ₁+ θ₂+ θ_Three)
+ cos (θ₁+ θ₂+ θ_Three))}
[31]
G22
= (1/2) mx2 ・ m2 (-cos (-θ₁+ θ₂) + cos (θ₁+ θ₂))
+ (1/2) m3 ・ a2 (-cos (-θ₁+ θ₂) + cos (θ₁+ θ₂))
+ (1/2) m3 (mx3 (cos (θ₁+ θ₂+ θ_Three) -cos (-θ₁+ θ₂+ θ_Three))
-my3 (-sin (-θ₁+ θ₂+ θ_Three) + sin (θ₁+ θ₂+ θ_Three)))
[Expression 32]
G23
= m2 ・ mx2 ・ cos (θ₂) + cos (θ₂) m3 ・ a2 + m3 (-my3 ・ sin (θ₂+ θ_Three) + mx3 ・ cos (θ₂+ θ_Three))
[0046]
The above formulas 9 to 32 are composed only of the parameters shown in Table 2, the link lengths a1 and a2 shown in FIG. 2, the joint position θ, and the joint velocity θd. Here, all values other than θ and θd are known values. It is. Note that a1 is the link length of the link L2, and a2 is the link length of the link L3. Therefore, if the start position θs and the predicted speed value Vrea are input, the robot dynamics model at the start position θs can be calculated. Similarly, if the target position θe and the predicted speed value Vrea are input, the robot dynamics at the target position is calculated. The model can be calculated. Specifically, it is as shown in the following formula. Since the calculation method of the predicted speed value Vrea will be described later, a description thereof is omitted here.
[0047]
[Expression 33]
Inertial matrix of start position: Hs = H (θs)
[Expression 34]
Viscosity matrix at the start position: Cs = C (θs, Vrea)
[Expression 35]
Gravity matrix at the start position: Gs = G (θs)
[Expression 36]
Inertial matrix of target position: He = H (θe)
[Expression 37]
Viscosity matrix of target position: Ce = C (θe, Vrea)
[Formula 38]
Gravity matrix of the target position: Ge = G (θe)
[0048]
By the way, the torque that can be generated in each joint of the robot is a known value that is limited by the design specifications of the motor and machine. If this is Tpeak, the left side of the above equation 1 is replaced by the following equation: It is done.
[0049]
[39]
Tpeak ≧ H (θ) · θdd + C (θ, θd) + G (θ) · g
[0050]
Here, Tpeak is a joint allowable peak torque n × 1 matrix. The above equation (39) is transformed into the following equation (40). The left side of Equation 40 is a value obtained by subtracting disturbance torque such as gravity moment, centrifugal force, and Coriolis force that is not related to acceleration / deceleration from the allowable peak torque Tpeak of the joint, and represents torque that can be used for acceleration / deceleration. The right side of Equation 40 represents the torque generated at each joint when each joint of the robot operates at the acceleration / deceleration θdd. That is, Equation 40 is one of conditional expressions for calculating the acceleration / deceleration that satisfies the first problem.
[0051]
[Formula 40]
Tpeak− (C (θ, θd) + G (θ) · g) ≧ H (θ) · θdd
[0052]
In Equation 40, Tpeak and g are known fixed values, and H, C, and G are the gravity center position, mass, inertia tensor, link length, and joint position θ of the link connected to each joint of the robot. It is a robot dynamics model calculated from the joint velocity θd. Therefore, in the above formula 40, except for the joint acceleration θdd, the unknown value is only the joint velocity θd. This is because the center of gravity, mass, inertia tensor, and link length of the link connected to each joint of the robot are known fixed values that can be obtained in advance from the robot mechanism, link shape, material, etc. This is because the start position or the target position taught in advance can be used as the position θ.
[0053]
For this reason, the present embodiment has means for predicting the joint velocity θd in Equation 40. Note that the target speed taught as θd cannot be used. Even if the positioning operation at the maximum speed is requested for all the joints, the movement amount is generally different for each joint, and there is a difference in the time to reach the target position, that is, the operation time. Therefore, if the operation time is the same for all joints, a joint with a small amount of movement operates at a speed lower than the target speed. For the same reason, since the acceleration / deceleration of a joint with a small amount of movement can be suppressed, the optimum value for solving the first problem cannot be calculated without taking this into consideration. A joint in which acceleration / deceleration can be suppressed can suppress the influence of interference forces on other joints. Therefore, the embodiment according to the present invention has means for predicting the acceleration / deceleration for each joint.
[0054]
The prediction of the speed and acceleration / deceleration for each joint is performed according to the following procedure. First, acceleration and deceleration (hereinafter referred to as temporary acceleration and temporary deceleration, respectively) that can be normally operated in any posture of the robot are stored in advance. Next, the speed patterns of all the joints are calculated from the start position θs, the target position θe, and the target speed V that are taught in advance so that the temporary acceleration / deceleration described above is maintained. In the present invention, this speed pattern calculation method is referred to as a first speed pattern calculation method.
[0055]
This first speed pattern calculation method is used in steps S2, S6 and S9 in the processing flow of FIG. 4 described later, and the method will be described below.
[0056]
The first speed pattern calculation method for the i-th joint of the robot is based on the target acceleration Ai and the target deceleration Di from the start position θsi, target position θei, and target speed Vi of the teaching data taught for the i-th joint. This is a known calculation method for generating a velocity pattern for each joint so as to operate. Here, the speed pattern of each joint is the trapezoidal speed pattern shown in FIG. 7 or the triangular speed pattern shown in FIG. 8, and the acceleration time Tai of the i-th joint, the constant speed time Tci, The deceleration time Tdi and the operation speed Vri are calculated by this first speed pattern calculation method. The value obtained by adding the acceleration time Tai, the constant speed time Tci, and the deceleration time Tdi of the i-th joint is the operation time Tri in the speed pattern. In the case of the i-th joint, by using the following expression, from the start position θsi, target position θei, target speed Vi, target acceleration Ai, target deceleration Di of the i-th joint, acceleration time Tai, constant speed time Tci, deceleration time Tdi, operation time Tri, and operation speed Vri can be calculated.
[0057]
[Expression 41]
Tai = Vi / Ai
[Expression 42]
Tdi = Vi / Di
[Expression 43]
Tci = {θei−θsi−Vi (Tai + Tdi) / 2} / Vi
(44)
Tri = Tai + Tci + Tdi
[Equation 45]
Vri = Vi
[0058]
Here, when the constant speed time Tci obtained by the above equation 43 is less than 0, it is a case where the triangular speed pattern shown in FIG. 8 is obtained. In this case, the acceleration time Tai, The constant speed time Tci, the deceleration time Tdi, the operation time Tri, and the operation speed Vri can be calculated.
[0059]
[Equation 46]

[Equation 47]
Tai = Vi / Ai · sf
[Formula 48]
Tdi = Vi / Di · sf
[Equation 49]
Tci = 0
[Equation 50]
Tri = Tai + Tci + Tdi
[Equation 51]
Vri = Vi · sf
[0060]
Next, the joint having the maximum motion time (hereinafter referred to as the maximum motion joint) is extracted from the speed patterns of all the joints, and the motion time of the speed pattern in all the joints is the motion time of the maximum motion joint. The speed pattern is regenerated to be the same as. This processing is called end time synchronization processing. Since the end time synchronization process is a known technique, a description thereof will be omitted. An example of the above processing is shown in FIGS. As can be seen from FIG. 10 and FIG. 11, by calculating the first speed pattern and performing the synchronization processing of the end time, the predicted speed value Vrea, predicted acceleration value Area, and predicted deceleration value Dream can be calculated.
[0061]
By substituting the predicted speed value Vrea into the joint speed θd of the above equation 40, the above equation 40 becomes a known value except for θdd which is the acceleration or deceleration of the joint. Further, in order to optimally distribute the allowable peak torque of the driving joint that receives the interference torque due to the operation of other joints, the proportional relationship between the calculated acceleration predicted value Area and deceleration predicted value Drea is obtained. An acceleration / deceleration that satisfies the proportional relationship and the above equation 40 at the same time is obtained. That is, at the time of acceleration, the proportional relationship between the joints of the predicted acceleration is maintained, and the acceleration that satisfies the above equation 40 is the optimal acceleration Aopt that solves the first problem described above, and at the time of deceleration. The deceleration that maintains the proportional relationship between the joints of the predicted deceleration and satisfies the above equation 40 is the optimum deceleration Dopt that solves the first problem described above.
[0062]
As a result, the optimum acceleration and deceleration can be obtained, but it cannot be said that the optimum velocity pattern can be calculated by this alone. This is because even if the optimum acceleration Aopt and the optimum deceleration Dopt are used, if the speed pattern generation means is the same as the conventional one, the operation time changes for each joint, and the conventional one as shown in FIGS. This is because the same end time synchronization process must be performed, and the optimum acceleration / deceleration cannot be maintained. Therefore, the embodiment according to the present invention includes a speed pattern generation process in which the optimum acceleration / deceleration obtained as a result of the calculation is maintained. The procedure will be described below.
[0063]
In the above-described synchronization processing of the end time, only the joint having the longest operation time is used for the reference operation time Tbase by the above-described first speed pattern calculation method using the optimum acceleration Aopt and the optimum deceleration Dopt already obtained. Is obtained (step S6 in FIG. 4). For the other joints, the speed pattern generation process using the second speed pattern calculation method is executed so that the optimal acceleration Aopt and the optimal deceleration Dopt are maintained and the operation time becomes the reference operation time Tbase. (Step S7 in FIG. 4). As a result, since the operation time of all the joints is the same as that of the longest operation joint, it is not necessary to perform the synchronization process of the end time, and the optimum acceleration / deceleration calculated previously is maintained.
[0064]
If there is a joint that has the optimum acceleration Aopt and the optimum deceleration Dopt determined earlier and cannot realize a speed pattern in which the operation time becomes the reference operation time Tbase (in step S8 in FIG. 4). NO), the first speed pattern calculation is performed only for the joint, and after the speed pattern calculation of all the joints is completed (step S9 in FIG. 4), the above-described end time synchronization processing (step S10 in FIG. 4) is performed. That's fine. This occurs when the acceleration / deceleration is optimized and the maximum motion joint changes from before the optimization. By performing the synchronization processing of the end time, the optimum degree of acceleration / deceleration is slightly reduced, but the torque that can be used for acceleration / deceleration is not exceeded.
[0065]
The first problem can be solved by operating each joint with the speed pattern calculated as described above. The second problem is inevitably achieved because the inertia matrix H and the viscosity matrix C of the formula 40 include the inertia tensor of the link connected to the joint.
[0066]
Moreover, in another embodiment, the case where the bearing of the motor in each joint and the frictional resistance of a reduction gear are considered is demonstrated. By changing the robot dynamics model expressed by Equation 1 as shown in the following equation, it is possible to consider the frictional resistance of the motor bearings and reduction gears in each joint.
[0067]
[Formula 52]

[0068]
Here, Cm is a dynamic viscosity coefficient n × 1 matrix of a motor bearing or a speed reducer, and the dynamic viscosity coefficient is a constant representing a proportional relationship between speed and dynamic friction torque. The dynamic friction torque is a torque component having a magnitude proportional to the speed. Furthermore, θdm is the motor speed n × 1 matrix, and Fcou is the Coulomb friction torque. Coulomb friction torque is a torque component that has a magnitude independent of speed and always works in the opposite direction of motion. In the above equation 52, sgn (θdm) is a sign function determined as follows based on the motor speed n × 1 matrix.
[0069]
[Equation 53]
sgn (θdm)
= 1; When θdm> 0
= 0; when θdm = 0
= -1; When θdm <0
[0070]
Further, the relationship between the speed of the i-th joint and the rotational speed of the motor is expressed by the following equation from the reduction ratio ξi.
[0071]
[Formula 54]
θdmi = θdi · ξi
[0072]
Further, θdm in the above formula 52 is expressed by the following equation.
[0073]
[Expression 55]
θdm = [θd₁・ Ξ₁, Θd₂・ Ξ₂, Θd_Three・ Ξ_Three]^T
[0074]
Here, ξi is a reduction ratio of the reduction gear in the i-th joint. Here, since the parameters Cm, Fcou, and ξ are also known values that can be set in advance, even with the above formula 52, the optimum acceleration / deceleration can be calculated in the same manner as described above, and the details are omitted. To do.
[0075]
Furthermore, in another embodiment, it is possible to consider the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint by correcting the inertia matrix H shown in Equation 9 as follows. become.
[0076]
[56]

[0077]
Here, Imi is the moment of inertia of the motor rotor including the speed reducer. The moment of inertia of the rotating body that moves in conjunction is multiplied by the square of its reduction ratio to affect the moment of inertia of the input shaft. Therefore, the influence of the moment of inertia of the input shaft on the output shaft is the opposite, and must be multiplied by the square of the reduction ratio. This is why the moment of inertia of the motor rotor including the speed reducer is multiplied by the square of the speed reduction ratio and added.
[0078]
The diagonal element of the inertia matrix H indicates the influence of torque applied to itself, and the other elements indicate the influence of interference torque applied to other joints. Since the moment of inertia of the motor rotor including the speed reducer does not interfere with other joints, only the diagonal elements need be corrected. Accordingly, since Imi and ξi are also known values that can be set in advance, even if the number is 56, the optimum acceleration / deceleration can be calculated in the same manner as described above, and the details are omitted.
[0079]
Next, the configuration of the robot system according to the present embodiment will be described with reference to FIG. As shown in FIG.
(A) According to the system program stored in the ROM 101 based on the clock signal generated by the clock generator 110 and the synchronization signal SYNC generated by the frequency divider 111 that divides the clock signal by a predetermined frequency dividing ratio. A CPU 100 for controlling the operation of the robot mechanism unit 1;
(B) a program including a positioning operation process for controlling the operation of the robot mechanism unit 1, a ROM 101 for storing data necessary for executing the program,
(C) a RAM 102 that is used as a working area of the CPU 100 and temporarily stores data being calculated;
(D) A storage device that stores data necessary for executing the positioning operation process in advance, and includes a teaching data memory 106a, a temporary acceleration / deceleration information memory 106b, and a robot dynamics base parameter memory 106c. Memory 106;
(E) an interface 103 to which the teach pendant 20 is connected;
(F) a dual-port RAM 104 that stores data of motor command positions, which are drive control values for the motors MO1 to MO3.
These circuits 100 to 103 and 106 and the first port of the dual port RAM 104 are connected via a bus 109. The process for the teach pendant 20 is executed by the interrupt process of the CPU 100.
[0080]
The teaching data memory 106a in the hard disk memory 106 uses the teach pendant 20 in advance as teaching data for the start position, the target position, and the target speed of each joint of the robot mechanism unit 1 when the robot mechanism unit 1 is operated. Enter and store. The temporary acceleration / deceleration information memory 106b inputs and stores information on temporary acceleration and temporary deceleration of each joint used for predicting the speed pattern using the teach pendant 20 in advance. Further, the robot dynamics base parameter memory 106c receives and stores the robot dynamics base parameters including the inertia tensor, the mass, and the position of the center of gravity of the link connected to each joint of the robot mechanism unit 1 using the teach pendant 20. To do.
[0081]
The second port of the dual port RAM 104 is connected to the latch circuit 105, and the latch circuit 105 receives the data of the three motor command positions for each of the motors MO1 to MO3 stored in the dual port RAM 104 in synchronization with the synchronization signal SYNC. After latching at the same time, the motor command position data relating to the joint control of the robot mechanism unit 1 is output to the motors MO1 to MO3 via the servo control circuit 107 and driven.
[0082]
Next, the structure of each joint RJk (k = 1, 2, 3 in the embodiment) of the robot mechanism unit 1 and the configuration of the servo control circuit 107 will be described with reference to FIG. As shown in FIG. 3, the rotational axis of the motor MOk is connected to the input shaft of the reduction gear REk, and the reduction gear REk is generated by the motor MOk by reducing the rotational speed of the motor MOk by a predetermined reduction ratio λk. The torque is increased by a reciprocal number times the reduction ratio and output to the link Lk connected to the output shaft of the reduction gear REk. Here, the shaft of the sensor SNk is connected to the rotation shaft of the motor MOk, and an output signal from the sensor SNk is input to the subtraction input terminal of the adder 120 in the servo control circuit 107. On the other hand, the data of the motor command position DPk from the latch circuit 105 is input to the addition input terminal of the adder 120. Data output from the adder 120 is converted into an analog voltage signal by a D / A converter (not shown), and then applied to a drive terminal of the motor MOk via an amplifier 121 having a predetermined amplification degree. The In the servo control feedback system configured as described above, the rotation angle is controlled to be the motor command position DPk, whereby the link Lk connected to the motor via the reduction gear reduces the DPk to the reduction ratio. Is rotated by the joint variable θk, which is the amount divided by.
[0083]
FIG. 4 is a flowchart showing a positioning operation process executed by the CPU 100 of FIG.
[0084]
  In FIG. 4, first, in step S1, the start position, target position, and target speed of each joint of the robot mechanism unit 1 are set based on the teaching data in the teaching data memory 106a. In step S2, the above-described step S1 Based on the set data and the temporary acceleration / deceleration information in the temporary acceleration / deceleration information memory 106b, the first speed pattern calculation is performed, and the operation time of each joint of the robot mechanism unit 1 is calculated.AdditionCalculate the predicted speed value. This process is the same as steps S103 and S104 in the conventional example of FIG. Specifically, the temporary acceleration Asmp is used as the target acceleration A and the temporary deceleration Dsmp is used as the target deceleration D in the first speed pattern calculation method. In addition, the operation speed Vr calculated by the first speed pattern calculation method isAdditionThe speed predicted value Vest is set, and the operating time Tr is set as the operating time predicted value Test.
[0085]
Further, in step S3, end time synchronization processing is executed for the speed patterns of all joints calculated in step S2. This synchronization processing of the end time is the same processing as the processing of steps S109 to S111 of the conventional example of FIG. 10 (hereinafter referred to as the speed pattern recalculation processing of the conventional example). The joint having the maximum motion time is defined as the maximum motion joint, and the motion time of the maximum motion joint is defined as the longest motion time Tmax. Then, for each joint, the values obtained by recalculating the motion speed and the acceleration / deceleration so that the motion time prediction value is the same as the longest motion time are respectively used as the speed prediction value, the acceleration prediction value, and the deceleration prediction value. Temporarily store.
[0086]
In step S4, based on the robot dynamics base parameter in the robot dynamics base parameter memory 106c, the calculation processing of the robot dynamics model of the start position and the target position is executed. In this process, the inertia matrix H, the viscosity matrix C, and the gravity matrix G at the start position and the target position are calculated from the start position, the target position, the predicted speed value, and the robot dynamics base parameter using Equations 9 to 38. The robot dynamics model is calculated and stored in the robot dynamics base parameter memory 106c. In step S5, the optimum acceleration / deceleration is calculated. In this processing, based on the calculated robot dynamics model, the acceleration / deceleration that satisfies the above equation 40 and the proportional relationship between the joints in the acceleration prediction and the deceleration prediction at the same time is obtained as the optimum acceleration / deceleration. That is, during acceleration, the optimal acceleration Aopt that maintains the proportional relationship between the joints of the predicted acceleration area Area (Equation 76 described later) and satisfies Equation 40 is calculated, while during deceleration, The optimum deceleration Aopt and the optimum reduction are calculated by calculating the optimum deceleration Dopt that maintains the proportional relationship between the joints of the predicted deceleration value Dream (Equation 89 described later) and satisfies Equation 40. The speed Dopt is stored in the RAM 102.
[0087]
Next, in step S6, based on the processing result in step S3, the joint having the longest operation time is searched, and the optimal acceleration Aoptk calculated in step S5 and the optimal acceleration for the k-th joint that is the searched joint are calculated. The k-th joint speed pattern is calculated using the above-described first speed pattern calculation method from the actual deceleration Doptk and the k-th joint teaching data (start position, target position, target speed), and the operation time is calculated. Is the reference operation time Tbase, and the calculation result is stored in the RAM 102. Further, in step S7, for all joints other than the kth joint described above, using the second speed pattern calculation method, the reference operation time Tbase, the optimum acceleration Aopt, the optimum deceleration Dopt, and the teaching Based on the data (start position, target position), the operation time becomes the reference operation time Tbase, and a speed pattern that operates at the optimum acceleration Aopt and operates at the optimum deceleration Dopt is generated and stored in the RAM 102. In step S8, the operation time becomes the reference operation time Tbase, and it is determined whether or not the speed pattern operating at the optimal acceleration Aopt and operating at the optimal deceleration Dopt exists in all the joints. Ends the positioning operation process, but proceeds to step S9 if NO.
[0088]
In step S9, the first speed pattern calculation method is used for all joints in which the operation time becomes the reference operation time Tbase, the operation is performed at the optimum acceleration Aopt, and the velocity pattern operated at the optimum deceleration Dopt does not exist. Thus, a speed pattern is calculated from the optimum acceleration Aopt, optimum deceleration Dopt, and teaching data (start position, target position, target speed) and stored in the RAM 102. Finally, in step S10, the longest operation time is extracted from the speed patterns of all joints based on the speed patterns calculated in steps S6, S7, and S9, and similar to the speed pattern recalculation process of the conventional example. For all the joints, the speed pattern is recalculated so as to coincide with the longest operation time, and the calculation result is stored in the RAM 102.
[0089]
After this positioning operation processing, as is well known, based on the generated speed pattern, the data of the motor command position DPk is calculated and transmitted to the robot mechanism unit 1 via the dual port RAM 104, the latch circuit 105 and the servo control circuit 107. By outputting, the operation of the robot mechanism unit 1 can be controlled based on the data that has been optimally subjected to the positioning operation process.
[0090]
In the above embodiment, as shown in Equation 52, in addition to the viscosity matrix C and the gravity matrix G, friction generated from the bearings of the motors MO1 to MO3 and the reducers RE1 to RE3 as torque components not related to acceleration. By considering the resistance, the calculation accuracy of the torque that can be used for acceleration may be improved. Further, as shown in the above formula 56, by adding the inertia moment of the rotors of the motors MO1 to MO3 including the speed reducers RE1 to RE3 to the diagonal component of the inertia matrix H, the torque generated by its own acceleration / deceleration The calculated value may be corrected.
[0091]
In step S8 of FIG. 4, when the operation time becomes the reference operation time Tbase, there is a joint that operates at the optimum acceleration Aopt and operates at the optimum deceleration Dopt (NO in step S8), the joint The number of the joint with the longest operation time may be k, and the processing from step S6 may be performed again.
[0092]
【Example】
A calculation example of the optimum acceleration / deceleration for the positioning operation in the robot mechanism unit 1 (n = 3) shown in FIG. 2 will be described with reference to the flowchart of FIG.
[0093]
In step S1, the start position, target position, and target speed of the positioning operation for each joint of the robot mechanism unit 1 are read from the teaching data in the teaching data memory 106a and set as follows.
[0094]
[Equation 57]
Start position: θs = [θs1, θs2, θs3]^T
[Formula 58]
Target position: θe = [θe1, θe2, θe3]^T
[Formula 59]
Target speed: V = [V1, V2, V3]^T
[0095]
On the other hand, in the temporary acceleration / deceleration information memory 106b, values of temporary acceleration / deceleration information that can be operated in all postures of the robot are set in advance as follows.
[0096]
[Expression 60]
Temporary acceleration: Asmp = [Asmp1, Asmp2, Asmp3]^T
[Equation 61]
Temporary deceleration: Dsmp = [Dsmp1, Dsmp2, Dsmp3]^T
[0097]
In step S2, the first speed pattern calculation described above is performed for each joint based on the set start position, target position, target speed, temporary acceleration, and temporary deceleration. The calculation of the first speed pattern is the same as in the conventional example. The following values are predicted by the first speed pattern calculation. The concept is shown in FIG. In the example of FIG. 5, the first joint and the second joint have a moving amount necessary to reach the target speed, but the third joint has a small moving amount and does not reach the target speed.
[0098]
[62]
AdditionSpeed prediction value: Vest = [Vest1, Vest2, Vest3]^T
[Equation 63]
Expected operating time: Test = [Test1, Test2, Test3]^T
[0099]
Furthermore, in step S3, the speed and acceleration / deceleration values of each joint of the robot after the synchronization processing of the end time are re-predicted as follows.
[0100]
[Expression 64]
Speed predicted value: Vrea = [Vrea1, Vrea2, Vrea3]^T
[Equation 65]
Acceleration prediction value: Area = [Area1, Area2, Area3]^T
[Equation 66]
Deceleration prediction value: Dream = [Drea1, Dream2, Dream3]^T
[0101]
If the maximum operating time in the predicted operating time is Tmax, the above value can be calculated from the following equation. The concept of the end time synchronization process is shown in FIG. In the prediction example of FIG. 6, it can be seen that the speed and acceleration / deceleration of the second joint and the third joint with a small amount of movement are reduced.
[0102]
[Expression 67]
Vreai = Vesti · Testi / Tmax
[Equation 68]
Areai = Asmpi · (Testi / Tmax)²
[Equation 69]
Dream = Dsmpi · (Testi / Tmax)²
[0103]
Note that the robot dynamics base parameter memory 106c stores in advance robot dynamics base parameters for generating the robot dynamics model represented by the above equation (1). The following table lists the robot dynamics base parameters.
[0104]
[Table 3]

[0105]
Next, in step S4, a robot dynamics model expressed by Equation 1 is generated from the robot dynamics base parameter and the position and speed of each joint of the robot mechanism. The robot dynamics model includes an inertia matrix H, a viscosity matrix C, and a gravity matrix G. Equations 9 to 38 show the dynamics model in the robot mechanism unit 1 having the mechanism shown in FIG. 2 calculated based on the Newton and Euler methods. In the model equations shown in Equations 9 to 38, the unknown values are only the joint position and velocity, and if the start position θs and the predicted velocity value Vrea are input thereto, the robot dynamics at the start position is obtained. The model can be calculated. Similarly, if the target position θe and the predicted speed value Vrea are input, the robot dynamics model at the target position can be calculated. Accordingly, in step S4, the following robot dynamics model is calculated.
[0106]
[Table 4]

[0107]
Next, in step S5, the optimum acceleration Aopt and the optimum deceleration Dopt are calculated according to the following procedure. First, an optimum acceleration Aopt is obtained. Then, Hs, Gs, and Cs are substituted into H, G, and C in Formula 40, respectively, and the optimal acceleration Aopt (or optimal deceleration Dopt) is set as θdd in Formula 40. For simplification, when the left side of Formula 40 is combined into one, it can be expressed as the following equation.
[0108]
[Equation 70]
Tad ≧ Hs (θs) · Aopt
[0109]
Tad on the left side of Equation 70 is expressed by the following equation, and is torque that can be used for acceleration during acceleration, and torque that can be used for deceleration during deceleration. Here, since acceleration is to be obtained, Tad represents torque that can be used for acceleration. Since the sign of the torque to be used is different between the time of acceleration and the time of deceleration, the sign of the predicted value Area of each joint acceleration after the end time synchronization processing described above is considered.
[0110]
[Equation 71]
Tad
= Sgn2 (Area) · Tpeak− (Cs (θs, Vrea)
+ G (θs) · g)
[0111]
Here, the sign function sgn2 is expressed by the following equation.
[0112]
[Equation 72]
sgn2 (Area)
= 1; When Area ≧ 0
= -1; When Area <0
[0113]
When the above formula 70 is expanded for each element, it is expressed as follows. Since Tad represents the torque that can be used for acceleration, the following expression is a conditional expression for obtaining an acceleration for making maximum use of the torque that can be used for acceleration.
[0114]
[Equation 73]
Tad1
≧ Hs11 · Aopt1 + Hs12 · Aopt2 + Hs13 · Aopt3
[Equation 74]
Tad2
≧ Hs21 · Aopt1 + Hs22 · Aopt2 + Hs23 · Aopt3
[Expression 75]
Tad3
≧ Hs31 · Aopt1 + Hs32 · Aopt2 + Hs33 · Aopt3
[0115]
The predicted value of each joint acceleration after the end time synchronization processing described above was Area. Assuming that the optimum acceleration Aopt of each joint also maintains the same proportional relationship as the predicted acceleration value Area, the relationship of the following equation must be satisfied.
[0116]
[76]
Aopt1: Aopt2: Aopt3 = Area1: Area2: Area3
[0117]
The acceleration Aopt satisfying the relations of the above formulas 73 to 75 and formula 76 is an acceleration that maintains the proportional relationship between the joints of the predicted acceleration value Area and can use the maximum torque that can be used for acceleration. It will be the optimum value. The reference joint may be any joint that operates, but in the present embodiment, the joint having the maximum absolute value is obtained from the Area, and this is set as the reference joint j. Based on the reference joint j, the above equation 76 is transformed as follows.
[0118]
[77]
Aopt1 = (Area1 / Areaj) · Aoptj
[Formula 78]
Aopt2 = (Area2 / Areaj) · Aoptj
[79]
Aopt3 = (Area3 / Areaj) · Aoptj
[0119]
Here, Areaj is the predicted acceleration of the reference joint j, and Aoptj is the optimum acceleration of the reference joint j. Next, when the above formulas 77 to 79 are substituted into the above formulas 73 to 75 and expanded, the following three inequalities are obtained.
[0120]
[80]
Tad1
≧ H11 ・ (Area1 / Areaj) ・ Aoptj + H12 ・ (Area2 / Areaj) ・ Aoptj
+ H13 ・ (Area3 / Areaj) ・ Aoptj
[Formula 81]
Tad2
≧ H21 ・ (Area1 / Areaj) ・ Aoptj + H22 ・ (Area2 / Areaj) ・ Aoptj
+ H23 ・ (Area3 / Areaj) ・ Aoptj
[Formula 82]
Tad3
≧ H31 ・ (Area1 / Areaj) ・ Aoptj + H32 ・ (Area2 / Areaj) ・ Aoptj
+ Hn3 ・ (Area3 / Areaj) ・ Aoptj
[0121]
In the three inequalities of the above formulas 80 to 82, all except for the optimum acceleration Aoptj at the reference joint j are known values, and therefore the optimum acceleration at the reference joint j is obtained from the three inequalities of the above formulas 80 to 82. You can calculate three ways. Assuming that the three accelerations calculated at this time are Aoptj_1, Aoptj_2, and Aoptj_3, the following equations are obtained.
[0122]
[Formula 83]
Aoptj_1 = Tad1 ・ Areaj / (H11 ・ Area1 + H12 ・ Area2 + H13 ・ Area3)
[Expression 84]
Aoptj_2 = Tad2 ・ Areaj / (H21 ・ Area1 + H22 ・ Area2 + H23 ・ Area3)
[Expression 85]
Aoptj_3 = Tad3 ・ Areaj / (H31 ・ Area1 + H32 ・ Area2 + H33 ・ Area3)
[0123]
From the accelerations Aoptj_1, Aoptj_2, and Aoptj_3 shown in the above equations 83 to 85, the value having the smallest absolute value must be the optimum acceleration of the reference joint j. This is because only the value having the smallest absolute value can simultaneously satisfy the three inequalities of the above formulas 80 to 82. By substituting the minimum value among the accelerations Aoptj_1, Aoptj_2, and Aoptj_3 as the minimum acceleration Aoptj_min and substituting it into the above equations 77 to 79, the optimum accelerations of all joints can be obtained.
[0124]
[86]
Aopt1 = (Area1 / Areaj) · Aoptj_min
[Expression 87]
Aopt2 = (Area2 / Areaj) · Aoptj_min
[Equation 88]
Aopt3 = (Area3 / Areaj) · Aoptj_min
[0125]
The acceleration thus obtained is the optimum acceleration that satisfies the first problem. The optimum deceleration Dopt can also be obtained by the same procedure. Specifically, in the above formulas 70 to 88, the parameters θe, He, Ce, Ge, Dopt, and Dream may be used instead of the parameters θs, Hs, Cs, Gs, Aopt, and Area, respectively. Here, the equation relating to the deceleration corresponding to the equation 76 relating to acceleration is as follows.
[0126]
[Equation 89]
Dopt1: Dopt2: Dopt3 = Drea1: Drea2: Drea3
[0127]
Next, in step S6, a speed pattern calculation process is performed so that the optimum acceleration / deceleration obtained as a result of the above calculation is maintained. The procedure will be described below. In the synchronization process of the end time in step S3, only the kth joint having the longest operation time is used to calculate the reference operation time Tbase by the above-described first speed pattern calculation using the optimal acceleration Aoptk and the optimal deceleration Doptk. Ask. Next, in step S7, the second speed pattern is set so that the acceleration is the optimal acceleration Aopt, the deceleration is the optimal deceleration Dopt, and the operation time is the reference operation time Tbase for joints other than the k-th joint. Perform the calculation. By the steps S6 and S7, the operation time in the speed pattern of all the joints becomes the reference operation time Tbase, so that it is not necessary to perform the synchronization process of the end time, and the optimum acceleration / deceleration calculated previously is held.
[0128]
A second speed pattern calculation method for the i-th joint of the robot will be specifically described with reference to FIG. As can be seen from FIG. 9, the reference motion time Tbase, the optimal acceleration Aopt of the i-th joint, the optimal deceleration Dopt of the i-th joint, and the movement amount Sai of the i-th joint are known values. The optimal velocity Vopti, the i-th joint acceleration time Tai, the i-th joint constant speed time Tci, and the i-th joint deceleration time Tdi are expressed by the following equations.
[0129]
[90]
Tbase = Tai + Tci + Tdi
[91]
Sai = Vopti · (Tci + (Tai + Tdi) / 2)
[Equation 92]
Vopti = Tai Aopti
[Equation 93]
Vopti = Tdi ・ Dopti
[0130]
Solving the simultaneous equations consisting of the above four equations 90 to 93, the time Tci, Vopti, Tai, Tdi can be calculated from the following equations.
[0131]
[Equation 94]

[0132]
[95]
Vopti
= (Tbase-Tci) / Aopti / Dopti / (Dopti + Aopti)
[Equation 96]
Tai = Vopti / Aopti
[Equation 97]
Tdi = Vopti / Dopti
[0133]
However, there is a case where the speed pattern calculation is not obtained such that the acceleration becomes the optimum acceleration Aopt, the deceleration becomes the optimum deceleration Dopti, and the operation time becomes the reference operation time Tbase. There are two cases:
(A) In the above formula 94, when the constant speed time Tci is obtained as an imaginary number, and
(B) When the optimum speed Vopti is obtained as a value exceeding the teaching speed Vi in the above-mentioned formula 94.
[0134]
In step S8, it is determined as NO when there is a joint corresponding to the above two cases. In step S9, the first speed pattern calculation similar to the conventional one is performed for all the joints determined to be NO in step S8. Next, in step S10, the above-described end time synchronization processing is performed on the speed patterns of all joints. In step S8, NO is determined because the acceleration / deceleration has been optimized, and the case where the maximum motion joint is changed is the case before optimization, but by performing the processing of steps S9 and S10, Although the optimum degree of acceleration / deceleration is slightly reduced, the torque that can be used for acceleration / deceleration is not exceeded.
[0135]
Finally, g in the above formula 71 is a gravitational acceleration vector, which defines the direction in which gravity works. In general, since the coordinates are defined so that gravity acts in the −z direction in the robot coordinates, g is as follows. The gravitational acceleration is 9.8 m / sec.²It is.
[0136]
[Equation 98]
g = [0, 0, −1 × (gravity acceleration)]^T
[0137]
As described above, according to the embodiment of the present invention, the inertia matrix at the start position and the target position is calculated from the start position, the target position, the predicted speed value, and the robot dynamics base parameter using the above formulas 9 to 38. A robot dynamics model including H, viscosity matrix C, and gravity matrix G is calculated (step S4). Based on the calculated robot dynamics model, the proportional relationship between the joints of the predicted acceleration value and the allowable peak torque of each joint are calculated. After calculating the optimum acceleration and deceleration satisfying the conditions (step S5), a synchronization process for matching the operation time is performed for all the joints (steps S6 and S7). Therefore, even when performing a positioning operation in which the movement amount of each joint of the robot mechanism unit 1 has a large difference, an acceleration / deceleration using the maximum allowable torque of the driving joint is calculated and the operation time can be shortened. Considering the load torque component due to the inertia of the link itself connected to the joint, even if the distance from the center of joint rotation to the center of gravity of the link connected to the joint is short, or even if the robot has a unique posture By implementing a simple calculation of the load torque component, it is possible to prevent the calculation of acceleration / deceleration that requires a torque exceeding the allowable value. Thereby, the optimal acceleration / deceleration of each joint that can be completed in the shortest time without exceeding the allowable peak torque of each joint of the robot mechanism unit 1 can be obtained.
[0138]
Further, when the operation time becomes the reference operation time Tbase, the operation is performed with the optimum acceleration Aopt, and the velocity pattern that operates with the optimum deceleration Dopt does not exist in all the joints, the first velocity pattern calculation method is performed in step S9. After the speed pattern is calculated using the above, the synchronization process is executed in the speed pattern recalculation process of the conventional example in step S10. Therefore, the optimal speed pattern can be calculated in all cases.
[0139]
Furthermore, according to the present embodiment, the calculation of the exact load torque component is realized in consideration of the friction of the motor bearing and the reduction gear, so that the acceleration / deceleration calculation that requires a torque exceeding the allowable value is performed. Can be prevented.
[0140]
In addition, according to the present embodiment, the torque generated by the moment of inertia of the motor rotor including the speed reducer is taken into consideration, and a precise calculation of the load torque component is realized. Speed computation can be prevented.
[0141]
<Modification>
In the above embodiment, the robot mechanism unit 1 includes the three joints RJ1 to RJ3. However, the present invention is not limited to this, and it is only necessary to include at least two joints.
[0142]
In the above embodiment, each of the joints RJ1 to RJ3 is configured to be rotatable. However, the present invention is not limited to this, and at least one of the two joints or more may be a linear motion that performs a translational motion. Good.
[0143]
【The invention's effect】
As described above in detail, according to the control method or control device of the robot system according to the present invention, based on the calculated robot dynamics model, the proportional relationship between the joints of the predicted acceleration value and the allowable peak torque of each joint. After calculating the optimum acceleration and deceleration satisfying the conditions, a synchronization process for matching the operation time is executed for all the joints. Therefore, even when performing a positioning operation with a large difference in the amount of movement for each joint of the robot mechanism, it calculates the acceleration / deceleration using the maximum allowable torque of the driving joint as much as possible, and shortens the operation time. Considering the load torque component due to the inertia of the joint itself, even if the robot has a short distance from the center of rotation of the joint to the center of gravity position, or even if the robot is in a unique posture, it is acceptable by calculating the exact load torque component Acceleration / deceleration calculations that require torque exceeding the value are prevented. As a result, it is possible to obtain the optimum acceleration / deceleration speed of each joint that does not exceed the allowable peak torque of each joint of the robot mechanism unit and that can complete the operation in the shortest time.
[0144]
In the control method or control apparatus of the robot system, preferably, the frictional resistance generated at each joint is taken into consideration by adding a torque component not related to the acceleration to the viscosity matrix and the gravity matrix. Accordingly, the calculation of the exact load torque component is realized in consideration of the frictional resistance of each joint, so that the acceleration / deceleration calculation that requires a torque exceeding the allowable value can be prevented.
[0145]
Further, in the control method or control apparatus of the robot system, preferably, a torque calculation value generated by acceleration / deceleration of each joint itself by adding the inertia moment of each joint to the diagonal component of the inertia matrix. Correct. Accordingly, the calculation of the load torque component is realized in consideration of the torque generated by the inertia moment of each joint described above, so that the acceleration / deceleration calculation that requires a torque exceeding the allowable value can be prevented.
[Brief description of the drawings]
FIG. 1 is a block diagram showing a configuration of a robot system according to an embodiment of the present invention.
FIG. 2 is a skeleton diagram showing the configuration of the robot mechanism unit 1 of FIG.
3 is a perspective view showing a configuration of each joint RJk (k = 1, 2, 3) of the robot mechanism unit 1 in FIG. 1, and a block diagram showing a configuration of a servo control circuit 107 in the control device 10 in FIG. It is.
4 is a flowchart showing a positioning operation process executed by a CPU 100 of FIG.
FIG. 5 is a graph showing the concept of the speed pattern prediction calculation process (step S2) of FIG. 4, wherein (a) shows the speed characteristic of the first joint versus time indicating the predicted speed pattern of the first joint; (B) is a graph showing the speed characteristic of the second joint versus time showing the expected speed pattern of the second joint, and (c) is a graph showing the speed characteristic of the third joint vs. time. It is a graph which shows the speed characteristic of the 3rd joint.
6 is a graph showing the concept of the speed pattern end time synchronization process (step S3) in FIG. 4, wherein (a) shows the time indicating the expected speed pattern of the first joint versus the speed of the first joint. FIG. 4B is a graph showing characteristics of the second joint with respect to time indicating the predicted speed pattern of the second joint, and FIG. 4C is a graph showing the expected speed pattern of the third joint. It is a graph which shows the speed characteristic of 3rd joint with respect to time.
FIG. 7 is a graph showing a time-speed characteristic showing a first speed pattern calculation method used in the present embodiment when a trapezoidal speed pattern is used.
FIG. 8 is a graph showing speed versus speed characteristics, illustrating a first speed pattern calculation method used in the present embodiment when a triangular speed pattern is used.
FIG. 9 is a graph showing a speed vs. speed characteristic showing a second speed pattern calculation method used in the present embodiment.
FIG. 10 is a flowchart showing a positioning operation process of a first conventional example.
11 is a graph showing a speed pattern of each joint for explaining an example of the end time synchronization process of FIG. 10, where (a) is a time vs. first joint showing a speed pattern of the first joint; (B) is a graph showing the speed characteristic of the second joint versus time showing the speed pattern of the second joint, and (c) shows the speed pattern of the third joint. It is a graph which shows the speed characteristic of 3rd joint with respect to time.
[Explanation of symbols]
1 ... Robot mechanism part,
10 ... Control device,
20 ... Teach pendant,
100 ... CPU,
101 ... ROM,
102 ... RAM,
103 ... interface,
104: Dual port RAM,
105 ... Latch circuit,
106: Hard disk memory,
106a ... Teaching data memory,
106b ... Temporary acceleration / deceleration information memory,
106c ... Robot dynamics based parameter memory,
107: Servo control circuit,
110: Clock generator,
111 ... frequency divider,
RJ1 to RJ3 …… Joint,
L1 to L4 ... link,
MO1 to M3 ... motor,
RE1 to RE3 ... reducer,
SN1 to SN3 ... sensors.

Claims (6)

A control method of a robot system for controlling the operation of a robot mechanism having a plurality of joints,
A first storage step of storing, as teaching data, a start position, a target position, and a target speed of each joint of the robot mechanism section when operating the robot mechanism section;
A second storage step of storing, in a second storage means, information on acceleration and deceleration of each joint used when predicting and calculating a speed pattern of each joint of the robot mechanism section;
Based on the teaching data stored in the first storage means and the acceleration and deceleration of each joint stored in the second storage means, the start position, target position, and target speed of each joint. From the teaching data, it is a speed pattern with respect to time for each joint so as to operate at the stored acceleration and deceleration of each joint, which operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. By using a first speed pattern calculation method for calculating a trapezoidal speed pattern that operates or a triangular speed pattern that operates at a constant acceleration and operates at a constant deceleration, the joint mechanism of the robot mechanism section is subjected to the addition. A first calculation step of calculating a speed predicted value and an operating time predicted value, and calculating the maximum value as the longest operating time from the operating time predicted value;
The joint with the maximum motion time among the predicted motion time values is the maximum motion joint, the motion time of the maximum motion joint is the longest motion time, and the motion time prediction value for each joint is the same as the longest motion time. The trapezoidal speed is the same when the acceleration is the same before and after the recalculation, and the same when the deceleration is before and after the recalculation, and operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. Using the speed pattern recalculation method that recalculates the recalculated values of motion speed and acceleration / deceleration as patterns, respectively, as a speed prediction value, acceleration prediction value, and deceleration prediction value, For the speed, acceleration and so that the predicted motion time calculated by the first calculation step is the same as the longest motion time calculated by the first calculation step. The recalculated values as speed, velocity prediction value, a control method of a robot system and a second calculation step of the acceleration estimated value and the deceleration predicted value,
A third storage step of storing, in a third storage means, robot dynamics base parameters including an inertia tensor, a mass and a gravity center position of each link connected to each joint of the robot mechanism section;
A start position and a target position of each joint stored in the first storage means; a predicted speed value calculated in the second calculation step; and a robot dynamics base parameter stored in the third storage means; On the basis of the,
(A) For each joint of the robot mechanism section, an inertia matrix indicating torque generated in the joint itself by acceleration, and interference torque generated by acceleration of other joints;
(B) a viscosity matrix indicating a torque generated by centrifugal force and Coriolis force for each joint of the robot mechanism unit;
(C) a third calculation step of calculating a robot dynamics model at the start position and the target position including a gravity matrix indicating a torque generated by a gravity moment of each joint of the robot mechanism section;
Based on the robot dynamics model calculated in the third calculation step and the predicted acceleration and deceleration predicted values of the joints calculated in the second calculation step, the predicted acceleration values of the joints are mutually determined. The proportional relationship between the joints of the acceleration predicted value is proportionally maintained during acceleration, and the predicted deceleration value of each joint is proportional to each other. The weight term obtained from the gravity matrix not satisfying the acceleration and the deceleration is satisfied from the allowable peak torque of each joint , satisfying the first condition that the second proportional relationship is maintained. value, and the subtraction result value obtained by subtracting the value of the viscosity term obtained from the relationship without viscous matrix in acceleration and deceleration, each when each joint is operated by the acceleration estimated value and the deceleration predicted value A fourth computing step of computing an optimal acceleration and optimal deceleration that satisfies the second condition that the above torque generated in the section for each joint,
The joint having the longest operation time in the first calculation step is searched, and the optimum acceleration and optimum deceleration calculated in the fourth calculation step are calculated for the k-th joint that is the searched joint, From the teaching data including the start position, target position and target speed of the kth joint stored in the first storage means, the speed pattern of the kth joint is calculated using the first speed pattern calculation method, A fifth calculation step using the operation time as a reference operation time;
For all joints other than the kth joint, the reference operation time calculated by the fifth calculation step, the optimum acceleration and optimum deceleration calculated by the fourth calculation step, and the first Based on the teaching data including the start position and the target position stored in the storage means, only the joint that has reached the reference operation time, which is the longest operation time, using the optimum acceleration and the optimum deceleration described above. The reference motion time is obtained by the first speed pattern calculation method. For the other joints, the base of each joint is maintained so that the optimum acceleration and the optimum deceleration are maintained and the motion time becomes the reference motion time. using the second speed pattern calculation method for calculating the velocity pattern of the shape, operation time becomes the reference operation time, operating in the optimal acceleration and optimal deceleration Sixth control method of a robot system which comprises a calculating step of generating a speed pattern Una. 2. The robot system control method according to claim 1 , wherein in the viscosity matrix and the gravity matrix, a frictional resistance generated at each joint is considered as a torque not related to acceleration . The torque calculation value generated by the acceleration / deceleration of each joint itself is corrected by adding the inertia moment of the rotor of the motor including the reducer of each joint to the diagonal component of the inertia matrix consisting of the inertia moment of each joint. The robot system control method according to claim 1, further comprising the step of: A control device for a robot system that controls the operation of a robot mechanism having a plurality of joints,
First storage means for storing, as teaching data, the start position, target position, and target speed of each joint of the robot mechanism when operating the robot mechanism;
Second storage means for storing information on acceleration and deceleration of each joint used when predicting and calculating a speed pattern of each joint of the robot mechanism section;
Based on the teaching data stored in the first storage means and the acceleration and deceleration of each joint stored in the second storage means, the start position, target position, and target speed of each joint. From the teaching data, it is a speed pattern with respect to time for each joint so as to operate at the stored acceleration and deceleration of each joint, which operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. By using a first speed pattern calculation method for calculating a trapezoidal speed pattern that operates or a triangular speed pattern that operates at a constant acceleration and operates at a constant deceleration, the joint mechanism of the robot mechanism section is subjected to the addition. A first calculating means for calculating a speed predicted value and an operating time predicted value, and calculating the maximum value as the longest operating time from the operating time predicted value;
The joint with the maximum motion time among the predicted motion time values is the maximum motion joint, the motion time of the maximum motion joint is the longest motion time, and the motion time prediction value for each joint is the same as the longest motion time. The trapezoidal speed is the same when the acceleration is the same before and after the recalculation, and the same when the deceleration is before and after the recalculation, and operates at a constant acceleration, operates at a constant speed, and operates at a constant deceleration. Using the speed pattern recalculation method that recalculates the recalculated values of motion speed and acceleration / deceleration as patterns, respectively, as a speed prediction value, acceleration prediction value, and deceleration prediction value, With respect to the speed, acceleration, and deceleration so that the predicted operating time calculated by the first calculating means is the same as the longest operating time calculated by the first calculating means. The recalculated values Te, the speed predictive value, in the control apparatus for a robot system having a second calculating means for the acceleration estimated value and the deceleration predicted value,
Third storage means for storing robot dynamics base parameters including an inertia tensor, a mass and a gravity center position of each link connected to each joint of the robot mechanism section;
A start position and a target position of each joint stored in the first storage means; a speed prediction value calculated by the second calculation means; and a robot dynamics base parameter stored in the third storage means; On the basis of the,
(A) For each joint of the robot mechanism section, an inertia matrix indicating torque generated in the joint itself by acceleration, and interference torque generated by acceleration of other joints;
(B) a viscosity matrix indicating a torque generated by centrifugal force and Coriolis force for each joint of the robot mechanism unit;
(C) third computing means for calculating a robot dynamics model at the start position and the target position, including a gravity matrix indicating a torque generated by the gravitational moment of each joint of the robot mechanism section;
Based on the robot dynamics model calculated by the third calculating means and the predicted acceleration and deceleration predicted values of the joints calculated by the second calculating means, the predicted acceleration values of the joints are mutually determined. The proportional relationship between the joints of the acceleration predicted value is proportionally maintained during acceleration, and the predicted deceleration value of each joint is proportional to each other. The weight term obtained from the gravity matrix not satisfying the acceleration and the deceleration is satisfied from the allowable peak torque of each joint , satisfying the first condition that the second proportional relationship is maintained. originating value, and the subtraction result value obtained by subtracting the value of the viscosity term obtained from the relationship without viscous matrix in acceleration and deceleration, in each joint when the joint is operating at the acceleration estimated value and the deceleration predicted value A fourth computing means for optimal acceleration and optimal deceleration that satisfies the second condition that the above torque calculating for each joint that,
A search is made for the joint having the longest operation time in the first calculation means, and for the k-th joint that is the searched joint, the optimum acceleration and optimum deceleration calculated by the fourth calculation means, From the teaching data including the start position, target position and target speed of the kth joint stored in the first storage means, the speed pattern of the kth joint is calculated using the first speed pattern calculation method , Fifth operation means having the operation time as a reference operation time;
For all joints other than the k-th joint, the reference operation time calculated by the fifth calculation means, the optimum acceleration and optimum deceleration calculated by the fourth calculation means, and the first Based on the teaching data including the start position and the target position stored in the storage means, only the joint that has reached the reference operation time, which is the longest operation time, using the optimum acceleration and the optimum deceleration described above. The reference motion time is obtained by the first speed pattern calculation method. For the other joints, the base of each joint is maintained so that the optimum acceleration and the optimum deceleration are maintained and the motion time becomes the reference motion time. using the second speed pattern calculation method for calculating the velocity pattern of the shape, operation time becomes the reference operation time, speed so as to operate at the optimal acceleration and optimal deceleration Control apparatus for a robot system, characterized in that a sixth calculating means for generating a pattern. 5. The robot system control device according to claim 4 , wherein in the viscosity matrix and the gravity matrix, a frictional resistance generated at each joint is considered as a torque not related to acceleration . The torque calculation value generated by the acceleration / deceleration of each joint itself is corrected by adding the inertia moment of the rotor of the motor including the reducer of each joint to the diagonal component of the inertia matrix consisting of the inertia moment of each joint. The robot system control device according to claim 4, further comprising:

Images