WO1985005198A1

WO1985005198A1 - Method of controlling multi-joint robot

Info

Publication number: WO1985005198A1
Application number: PCT/JP1985/000234
Authority: WO
Inventors: Kazuo Soji; Hiroyuki Nagano
Original assignee: Matsushita Electric Industrial Co., Ltd.
Priority date: 1984-04-27
Filing date: 1985-04-25
Publication date: 1985-11-21
Also published as: EP0191103A1; EP0191103B1; DE3587288D1; JPS60230206A; JP2604698B2; US4705999A; JPH0799486B2; JPS60233715A; EP0191103A4; DE3587288T2

Description

明細書

発明の名称

多関節型ロボッ卜の制御方法

技術分野

5 本発明は 2本のアームを有する多関節型ロボットにおける第

1 アーム及び第 2アームの制御方法に関する ₀

背景技術

近年、産業用ロボットの制御は、マイクロプロセッサの活用によ ]?、ディジタル化，高機能化へと推移してお i9 、アームの t o 速度制御も、カム曲線をディジタル化して記憶し、これをもとに速度指令を行う方法が一般的である o

具体的に説明すると、第 1 図に示すよう ¾時間と移動量の関係を有するカム曲線（ S カーブ）をディジタル化し、これを記憶装置にあらかじめ記憶しておき、動作時にはこれをもとに逐

1 5 時目標移動量を導き出し、現在位置との位置懾差を速度指令として各アームを駆動するモータに与えてお )、通常この記憶内容は sテーブルと呼ばれている ₀ この際、第 ¹ アーム及び第² アームが同時に始動し、また同時に停止するようにするため、実際に出力される目標移動量は、 s テーブルよ ]?導き出された 0 指令値に第 1 のモータの移動パルス量と第²のモータの移動パルス量の比が乗ぜられている ₀

各モータに速度指令を与える他の方法として、第 2図，第³図に示すよう ¾時間と速度の関係を有するカム曲線をディジタル化し、これを記憶装置にあらかじめ記憶しておき、動作時には 5 これをもとに逐時速度を導き出し、移動量の制御は、出力した速度指令を積分した値をもとに別途制御するハイブリッド制御方法、あるいは、第 4図，第 5図に示すような移動量と速度の関係を有する力ム曲線を用いる方法等がある o

元来、多関節型ロボットは、第 2 アームの角度によ慣性モ

5 — メントが大きく変化し、また、遠心力，コリオリカも第 2ァームの角度、第 1 アーム及び第 2アームの動きによって大きく変化する性質を有している o したがって、モータや減速機の負荷トルクは、これらによ ]？大きぐ変化する o

しかしがら、上記従来の方法では、負荷の変化に特別の考 l O 慮を行なわず一様の加減速を行なうため、最もきびしい条件下での負荷トルクをそれらの許容値以下におさえる必要があ、換言すれば、余裕のある動作に対しても一様の加減速を行なつているために、移動時間短縮の大きな障害となっていた o 発明の開示

1 5 そこで本発明の第 1 の目的は、モータや駆動回路の能力を最大限に生かし、また减速機の負荷トルクが許容限度を越えない範囲で常に最短時間で移動するように各アームの動きを制御する多関節型ロボットの制御方法を提供することにある。

本発明の他の目的は.、加減速時間を固定する従来の方法に比

20 ベ、移動時間を大幅に、しかも無理なく短縮でき ^)多関節型口ボットの制御方法を提供することにある o

上記本発明の目的は、第 1 の関節を介して本体に旋回可能に支持された第 ¹ アームと、第²の関節を介して前記第 ¹ アームに旋回可能に支持された第 2アームと、前記各関節に設けられ、 25 前記第 ¹ アーム及び第 2アームを旋回させる第 1 モータ及び第 2 モータもしくは、第 ¹ モータ及び第 ¹ 滅速機，第² モータ及び第 2減速機を備え、前記各モータに角速度指令を与えて前記第 ¹ アーム及び第 ²アームの動きを制御する多関節型ロボットの制御方法において、ある点 Pi から次なる点 P_i+1 へ移動する際、加速，減速の角速度曲線を、 ²点 Pi ， P_i+1 の位置決めデータに基づい τ、モータおよび減速機に掛るトルクが、いずれも許容限度以内で、最も短か加速時間，減速時間とるように決定することによ！）達成される ο

具体的には、本発明は点 Pi から次る点 P_i+1 に移動する際、

① 標準加速曲線，標準減速曲線をあらかじめ記憶装置に記憶しておき、

(2) 2点 Pi， P_i+1 の位置データを読込、

③ 第 ¹ モータ及び第² モータもしくは、第¹ モータ及び第 1 減速機、第² モータ及び第 ²減速機の負荷トルクがいずれもそれぞれに定まつた許容トルク以下で、少¾ くとも ¹ っ以上が許容範囲内で最大となるよう ¾加速時間もしくは加速時の移動量、減速時間もしくは減速時の移動量、最高回転数を求め、

@ 求められた加速時間もしくは加速時の移動量、減速時間もしくは減速時の移動量、最高回転数でもつて、標準加速曲線，標準減速曲線を拡大，圧縮し、実際に出力される加速曲線，減速曲線を求め、これによ i?加減速指令を行ない、よつて移動時間を短縮する o

更に本発明は上記制御方法において、時間もしくは移動量を基準に速度指令値もしくは目標移動位置指令値を創出する標準加速曲線と標準減速曲線をあらかじめ記憶装置に記憶しておき、また、モータの負荷トルクに影響を与え、移動開始点と移動終了点の位置データよ ]3定まる要素をそれぞれ離散値化して記憶 5 装置に記憶させ、前記離散'値パラメータのすべての組合せについて、第 ¹ モータ及び第² モータもしくは、第¹ モータ及び第 1 減速機，第 2 モータ及び第 2減速機の負荷トルクがいずれもそれぞれに定まつた許容トルク以下で、少なくとも 1 つ以上が前記許容範囲内で最大ならしめる加速時間もしくは加速時の移 l O 動量、滅速時間もしくは減速時の移動量を前もつて計算し、それらを前記離散値パラメ一タの組合せに対応して記憶装置に記憶しておく方法を含む o

図面の箇単な説明

第 1 図は時間と移動量の関係を示したカム曲線図、第 2図は時間と速度の関係を示した加速カム曲線図、第 3図は同減速ガム曲線図、第 4図は移動量と速度の関係を示した加速カム曲線図、第⁵図は同減速カム曲線図、第 6図は多関！^ロボットのシステム構成を示す図、第 7図は本発明の方法に用いられる標準加速曲線を示す図、第 S図は同標準減速曲線を示す図、第 9図 0 は第 1 モータ-及び第 2 モータのいずれもが最高回転数に達しない場.合の角速度曲線を示す図、第 1 O図は第 9図の加減速時間決定の際に用いる仮定の角速度曲線を示す図、第 1 1 図は本発明の第 1 の実施例のフローチャート、第 1 2図は本発明の第 2 の実施例のフ口一チヤ一ト、第 ¹ 3図及び第 ¹ 4図は多関^ ²⁵ ボットの動作を説明する図、第 1 5図は本発明の第³の実施例の部分フロ一チヤ一トである。

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

第 6図は 2本のアームを有する多関節型ロボ .:；トのシステム構成を示す図である。

5 図において 1 はロボット本体の支柱、 1 1 は支柱 1 に旋回可能に接続された第 1 アーム、 ^{1 2}は第 ¹ アーム ^{1 1} に旋回可能に接続された第 2アーム、 1 3 . 1 4は第 1 アーム 1 1 ，第 2 アーム 1 2を駆動するモータである。 1 5 ， 1 6は、モータ 1 3 ， 1 4の回転を減速し、第 1 アーム 1 1 ，第 2 アーム 1 2 i o に伝える減速機である。

2は上記モータ 1 ³ , 1 4を駆動するための駆動回路部である。 3は数値制御装置で、演算装置 3 1 、記憶装置³ 2、速度指令装置 3 3から構成され、記憶装置 3 2に格納された位置決めデータから速度指令信号を創出し、速度指令装置 3 3を通じ

1 5 て速度指令信号を前記駆動回路部 2に入力するものである。

次に本発明の第 1 の実施例における制御方法について説明す多関節型ロボットの場合、モータおよび減速機に掛るトルクは、第 2アーム 1 2の第 1 アーム 1 1 に対する姿勢（ ₂ ) ，第 0 1 アーム 1 1 ，第 2アーム 1 2の移動速度 θ θ₂ ，第 1 ァーム 1 1 と第 2アーム 1 2の回転方向（ J ， J ₂の正負）によて大きく異るってくる。

力学の法則よ ]?、本実施例の構成においてモータ 1 3に掛るトルク（，モータ ^{1 4}に掛るトルク T₂(t)，減速機 1 5に掛 5 るトルク H^t) , 減速機 1 6に掛るトルク (t) は例えば、 T₁ (t)= a₁a>₁ + a₂H₁(t) + ₃cy ₁ + a₄ (1)

^(t)= ( a + a₆cos っ ) ω a _T + a _Q cos (? ₂ ) ^ωつ

— ( a₉i« ₁ iy₂ + a _{1 0}iy₂ ) ²sin5₂ (3)

で表わされる o

ここに、， ω₂ は、モータ 1 3 ， 1 4の角速度、 ω , ώ₂はそれらの時間微分、 θ₂は第²アーム ^{1 2}が第 1 アーム 1 ¹ に対して成す角度、 ^， a₂ ,…… ， a₁₀および、 b ， b₂ , , b₈ はいずれも各ロボット固有の定数で、ロボットのアーム長, 重量，減速比，摩擦抵抗等によって定まるものである。

本発明の目的は、モータおよび減速機に掛る最大トルクを制御することにある。一般に、多闋節型ロボットの場合、加速時間，減速時間は 0.2〜 0.⁵秒程度で、したがつて (1)〜(4>式において、 , ₂ よ ]? も、， ωゥの方力 S大きい値となるため、 —般に、 ί^ -Ο , ο-Ο の状態、すわち、ロボットの移動速度が一定状態にるったときよ ]? も、加減速時の方が、モータおよび減速機に掛るトルクは大きい。よって、以後、加減速時のトルク制御に限定して説明する。

まず最初に、第 1 アーム 1 1 ，第 2アーム 1 2の少くとも —方がその最高回転数に達する場合の加速時のトルク制御について説明する o

標準加速曲線の一例として、第 7図に示すように

t (5) w = /_u ( ）

τ u ただし O ^ ^ ^ l

U

/_u ) = 0 , /_U(1) = 1 ただし t_uは加速に要する時間

とし、 , ω₂ ¾·

t u

ただし、 , ό₂は各ステップごとに定まる定数

と ¾るよう ¾制御を行 ¾う場合、加速開始時の第 2 アーム 1 2 の第 1 アーム 1 1 に対して成す角度を _2Sとすると、（⁶⁾ ，（⁷⁾式よ、 ω , ω₂は常に線型関係にあるため、， ₂ が一定速度にるったときの値を， ¾ とすると、第 ¹ アーム 1 1 と第 2アーム 1 2の加速状態は、 _2s , ω_Λ , ω₂, t_uによつて一義的に定ま、よつて (り， T₂(t), H^t), H₂(t) のピーク値を T_1p, Τ_2ρ ， Η_{1 ρ}, Η_2ρ とすると、

Τ·ι p = £fi ( _{2 S}， 1 , ^2 , t_u) ほ） T_2p = g₂ ( <?_{2 s} , ，， t _u ) (9)

^H1 _p=sr₃ ^ 2 s，， ω₂ ， t _u ) ttO) H2p ⁼ ST4 ( ^2 s ' ^ω1 ' ^ω2 » * u ) ^ ¾る関数で表わすことができる。

ここで、

Δ Θ 第 1 アーム 1 1 の移動角 Δ Θ 2 第 2アーム 1 2の移動角 m 減速機 1 5の減速比（ > 0 ) in 2 減速機 ¹ 6の減速比（ m₂ > 0 ) とすると、 " と " ₂はつねに線型関係にあること、第 ¹ アーム 1 1 と第²アーム ^{1 2}は同時にスタートし、同時に停止することよ

2m₂ ω ω

2 2

ω ω がたつ ο ここで、モータの最高回転速度を Μ， ^ω2Μ» (ω_{ι Μ} > Ο , ω_2Μ > 0 ) とすると、回転方向を考慮して、

ω 2Μ

) Δ Θ o のとき

Δ Θ ^ m. ω Μ ω = ω 1Μ (13

Δ θ

ω

A d _Λ να._Λ Μ

Δ θ ω 2Μ

ii ) Δ Θ <ο のとき

ω Μ ω ^— ― ω Μ (15) ω = ― ω .

Δ 0 ^ Μ as ω 2M

iii ) Δ Θ₂ > のと

Δ Q > ω 1Μ

(18)

2Μ

Δ θ ω 2Μ , „

IV ) > のとき

ω 1 Μ

Δ θ _Λ τη.

ω ^ = 2Μ …（19) ω ― ω (20)

2Μ

V ) Δ θ = Ο , Δθ ^ > 0 のとき ω ^ = Ο (21)

(18) ω2= ^ω2Μ

vi ) Δ θ =Ό _T 5 < Ο のとき α> = Ο (21)

(20) 2=一 2Μ

vii ) j ^₁ > o , J^ = 0 のとき

(13) ω = Ο (22) νίϋ ) J < ο , J <?₂ = ο のとき ω1 = - ^ωΐΜ (15)

(23 がな i?たつ。ここで、 Δ ^ ^m2 ^ω M

= •(23)

2M

Δ Θ ^ m. ω 2M

β = •(24) とすると

^ = 0のとき β = O

Δ θ 2= Οのとき α = Ο

となるので、先の (13)〜！ 22)式は、

^ 〉。 . ！ "！ ^ のとき ω一₂= C C _2U ··

ϋ ) Δθ _Λ < 0 , I a I≤ のとき

ω . M …■ (27) 2=一 ^α ω 2Μ " (28) iii ) Δ θ₂= 〉 Ο , \ β [ < 1 のとき

■ (29 ω2= ^ω2Μ ·· - (30)

IV ) ^<?2<°' I ^ K ¹ のとき

ω ^ = — β ω^ ^ ·· ··… (3D に整理することができる ₀

したがつて (8> 〜(U)式は、

ί ) | α I≤ 1 のとき ^{Δ Θ}Λ ^ωΛ . ^{Δ Θ}Λ ^{α ω}2Μ 、

^Τ1ρ ⁼ 2s，，） ⁽³³⁾

\ Δ β. \ \ Δ Ο \

T_2n=g₂(½_S' , ~，） ⁽³⁾

\ Δ Ο \ \ Δθ _Λ \

^{Δ Θ}Λ ^ωιΜ ^{Δ Θ} Λ ^αω2Μ

Η_{1 Ρ} = _2S, … ，， … ，， ) ⁽³⁵⁾ 2ρ = 9₄(θ₂₅ , ~~―— ——― - - *u ∞

ij ) I I < 1 のとき

. ）

^H1p^=<?3^2s , ，） ⁽³⁹⁾

<?₂ i I J5₂ J

₂,βω Δ θ₂ω_2Μ

¾ρ = ?4(^23 . . ）

\ \ \ Δ β '

2 2

となる。したがって、

, Ηΐ ρ , ^Η2 ρ はいずれも、 θ₂₈ , a， β , _{ι Μ}， ω_2Μ, Δ θ の正負， J<?2の正負、 t_uによつて表わすことができる。 w_{l M}, ω_2Μ は定数、 θ_{2 3} , a ,

J の正負、の正負は、点 Pi から次る点 P _{i + 1}へ移動するステップにおいて、点？ i ，点 P_{i +1}の位置データによ i？、一義的に定まるものであるため、 T_{1 p} ， Τ_2ρ, Η_{1 ρ}， Η_2ρ の値を変えるには、 t_u を変える必要がある。

本発明の目的は、 p , Τ_2ρ , Η_{1 ρ} , Η_2ρがいずれも、各々に定まった許容限度以下で、しかもその条件下で t_u を最小にすることである。したがって t_uの値を変えがら、 τ_{1 ρ}, Τ_2ο , Η_{1 ρ} , Η_{2 ρ} の絶対値の最大値を求め、それが、各々に定まった許容限度以下で、しかもその条件下で最小の t_u に、 t_u を収束させる手法を用いることによって最適加速時間 t_u を求めることができる。

このようにして求まつた点 Piから点 P_{i +1}ヘステツブの最適加速時間 t_{u i}に対して、最適加速曲線は、 | α | ) i « I≤ のとき、

Δ θ

ω i (t) =

Δ θ

u： ii ) l < のとき

Δ θ

ω (t)= ω 2Μ fix ('八

Δ β

u とし乙^ K i ο

減速時の場合も、加速時の場合と同様、 e_2sのかわに、点

P_{i +1} における第アームの第アームに対する角度 ₂E，標準加速曲線のかわに、第 3図に示すよう ¾

標準減速曲線 _d (~:~)

t d

ただし /_d = 1 /_d(¹)= o

tのかわ i?に、 t — t_s ( t_s : 減速開始時間）を用い、最適減速時間を求めることができる。

また最適減速曲線も同様に

i ) | " | ≤ 1 のとき

J t-t _

ω (t) = ^ωΐΜ d

ά θ d

ii )

として求まる。

このようにして求まった角速度データは、速度指令装置 3 3 へ入力され、速度信号となって駆動回路部²に出力される。さらに駆動回路部 ² で増幅されて、モータ ^{1 3} ，モータ ^{1 4}が駆動される。

次に、第 9図に示すように、 Α θ ₂ が小さく、 ω ₂ が， ω₂に達しな場合について説明する

このとき、アームは、 Αよ ]?加速曲線

1 ω

2 =c_2/u (

u に沿つて加速し、 B ， B' よ定速とる j?、 C C' よ ]?、今度は減速曲線 ω = Di/d ( ) , ₂ = D₂ （ )

d d

に沿って減速し、 Dで停止する。

ここで問題となるのは、加速曲線における B— E ， Β'— Ε' 間，減速曲線における F— C , F'-C⁷ 間が実際の動作には用いられず、加速曲線，減速曲線の决定にあたつて、パラメータ t_u， t_d を用いられないことである。

しかしながら、本実施例では、次に説明する考え方で、先に説明した場合と全く同様のァルゴリズムで処理できる。

第 9図に示すように、第 1 O図にける 4 θ _Λ ， Δ Θ 、ある定数を掛け、， ^₂に達する部分が現われるような動作を仮定する。このとき、

T₁ ( Α〜Β )て Τ₁ ( Α〜Ε )

Τ₂ (Α〜： Β') こ Τ₂ (Α〜Ε')

Η₁ ( Α〜Β ) Η₁ ( Α〜Ε ) Η₂ (Α〜Β,) C Η₂ ( Α〜Ε

Τ₁ ( C〜D ) C Τ_η ( F〜D )

Τ₂ ( C'〜D) C Τ₂ ( F,〜D) Η₁ ( C〜！） ) Z Η₁ ( F〜D ) H 2 ( 〜D ) C H₂ ( F,〜D)

であるからつねに、

T ( A

P 〜B ) I ≤ I T ( A

P 〜 E )

T_{2 p} ( A〜Bつ I ≤ I T_{2 p} ( A〜E,)

H_{1 p} ( A〜B ) I ≤ I H_{1 p} ( A〜E )

H_{2 p} ( Α〜Β' ) I ≤ I H_2p ( A〜E

T ( C〜D ) I ≤ I T₁ ( F〜D )

P

T_2p ( C,〜D) I ≤ I T_{2 p} ( F,〜D) I

H₁ p ( C〜D ) ! ≤ I p ( F〜D ) I

H_{2 p} ( Cノ〜 D) I ≤ I H_{2 p} ( F,〜D) I

が ¾ i)たつ。したがって、第 9図の動作を、第 1 o図の動作におきかえて、 t_u, t_d を求めても、モータおよび減速機に過負荷がかかることはるい。

さらに、本実施例では、 (23)〜式で示すように、ピークトルクの算出に β^ , ₂を用いず、 a ， β , ω_{Λ Μ}, ω_2Μ, Δ θ _Λ の正負， 4 θ ₂ の正負を用いてお j9、 a , 3は、（23)〜 (24)式に示すように Δ β_Λ と ₂ の比によって定まる数で、 , ₂の大小には左右されい。したがって、第 9図に示す動作を先に説明したアルゴリズムで処理すると、自動的に第 1 0図に示した動作に置き換えて処理したことに、よって、第 9図に示す動作に対して、特別 ¾処理をする必要はなく、全く同じアルゴリズムで処理することが可能である。

以上説明したアルゴリズムを第 1 1 図に示すフローチャートを用いて説明する。

① 2点、 Pi， P_i+1のデータを読込む、点 Pi から点 P_i+1への加速時における Τ

Ρ 2ρ

Η. ， ^Η2Ρ が

Ti p I T_{1 max} かつ

τ

2p 2ma∑ かつ 1 p I≤^H1 max かつ

H

2 2 max

であ

^Ti ma_X- I ^T1 p I l < ^ei または T2max~ I ^T2p M < ^e ₂ または

^H1 max - I "1 p M < 3 またはとるらしめる t_u を最適化手法をもって求める。

ここに、 T_{1 max}， T_{2 max}, H_{1 max}， H_2maxは、モータ，減速機の許容負荷トルクで、， e₂ , ε₃, ε₄ は十分に小さい数である。

また、減速時においても、同様にして最適減速時間、 t_dを求める。

③ ステップ②で求めた t_u， t_d を t_ui， t_di とする、

④ 加速曲線

ί ) I α I 1 のとき

ii ) I ^ I < 1 のとき

ΔΘ t

i Δβ

t 2

ω 6)

2 i(t) = 2 へ

I Δ β

2 t 減速曲線を、

i ) I « I≤ のとき

Δ θ ₁ t―

Δβ _Λ t d

)

ii ) I Iぐ 1 のとき

^{D D} 2. ₃

ωΐ ⁼ ^MAfd^— )

^2 I ^l d

J Θ ^

2 . t ¹ - t ^L s

ω 2一 t)= ⁶2M^d )

2 l とする。

以下、 P_i+1 が最終点に到達するまでステップ①から、ステップ @までを繰返す。

次に本発明の第 2の実施例について第 1 2図に基づて説明する。本実施例は、第 1 1 図に示すフローチャートの (g)のステップにおいて、各パラメータを離散値パラメ一タとし、各パラメータごとの最適加速時間，最適減速時間を前もつて計算，記憶し、点 Pi ，点 P_i+1 の位置デ一タよ！）求められたパラメータを離散値パラメータに照らし合わせることによ、最適加速時間，最適減速時間を近似的に求め、処理を大幅に簡略化することを特徵としている。

前記第 1 の実施例で説明したように、最適加速時間，最適減速時間を求めるパラメータとしては、

Θ 2 s ：加速開始時の第 2アーム 1 2が第 1 アーム 1 1 に対して成す角度

0_{2 E} :停止時の第 2アーム 1 2が第 1 アーム 1 1 に対して成す角度 a =

^ ⁰ 1 ^m 1 ^ω 2Μ

Δ θ m ω

2 Μ

β β =

Δ θ ゥ. m 2 ω

Μ

Δ θ _Λ の正負 2 の正負

があるが、の正負，の正負は 2つずつの値しかないため、 Θ _{2 s}， θ _{2 Ε} , α ， β , を離散値パラメータとすればよいことはあきらかである。

以下、第 1 2図のフローチャートにもとづいて、加速の場合のアルゴリズムを説明する。

① ²点 Pi , P_i+1 の位置データを読込む。

ラメ一タ _{2 s} ， 9 ₂E > ^a r ^ , , を求める。ロボットの動作パターンが、 4種類に分類されたパターンのいずれかを判定し、前もつて記憶しているテーブルにて、加速時間 t_u をサーチする。このとき、テーブルは、

j ) j a | ≤ 1 , J0₁ 〉 Oのとき、

2 s j k

ii ) a ≤ 1 , ^^ ぐ。のとき、 u i 2 s j k

iii ) β I < do oのと

八

- ( θ

U 1 、 2 s j )

iv ) | ^ | く 1 ， ^^2く 0のとき、

u i _{2 s j} ， 3_k )

ここに

j = 1 , 2 , 3 , …… ，

k = 1 , 2 , 3 , , β'

— max 2 sト 1、 3 j ≤ ⁵2 max

- 1 ≤ «_k _！ < « _k<1

ただし、 0₂ ma_X は第 2 ァムの第 1 アームに対して成し得る最大角度

β , ζ は自然数

て^?る。

@ 求められた t _u i に対して

i ) I α I 1 のとき、

Δθ _Λ

ω

1 i(t) = ^ω 1 Μ /u

Δ θ u Δ β t

ω_{2 i}(t)= ^{α ω}2Μ (~ "~" )

I _{u i}

ii ) Ί ^ [ぐ 1 のとき、

J t

ω

1 ⁼ ~ 1 M,_U (― ~~

I * u i 2

ω 2 i )⁼ ； )

I I

¾る加速曲線を決定する。

減速の場合も、前記③， ④と同様に処理する。

以下、点 P_i+1 が最終点に達するまで、.ステップ①から、ステッブ④までを繰返す。

以上のように本実施例では、加減速時間、 t _{u i} , t _{d i} を求める処理が大幅に筒略化されるため、小型の演算処理装置でも十分対応できるという特徵を有している。

次に本発明の第 3の実施例を第¹ 3図〜第¹ 5図に基づいて説明する。

前記第 2の実施例では、第 1 3図，第 1 4図に示すような対称形の動作は、別の動作とみなされ、それぞれに記憶テーブルが用意されるようになっている。第 1 3図，第¹ 4図の動作は、図よ ]?明らかであるように、

J ₁ (9) = - J Θ ₁ (S) Θ ₂ (9) = - ^ (？ 2 (8)

がなたつ。したがって、第 1 ³図，第 1 4図の動作において、モータ，減速機の負荷トルクは、正負は逆であるが絶対値が等し。本発明の目的は、ピークトルクの制御で、負荷トルクの絶対値が許容限度以内で、しかも最適な加滅速時間を求めることにあるから、前記の 2つの動作を同じ動作であると見なし、 5 それに対して 1 つの記憶テーブルを用意してもよく、またそうすることによって、記憶テ一ブルの量を前記第 2の実施例の半分に圧縮することが可能である。

以下、第 1 2図におけるステップにあたる部分を第¹ 5図に示す。

,Ο 前記第 2の実施例では、 | a | の判定のあと、， ₂ の正負を判定しているが、第³の実施例では、

· 2 S ， 2 · ^d2 s

の正負を判定している。なぜ ¾ら、第^{1 3}図，第^{1 4}図を例にとって説明すると、

T5 (Θ) . ^_{2 3}(9)= (-^ ^ (8)) . (-<? _{2 s}(8))

= Λ Θ _Λ {3) · θ _{2 8}(8)

とる。すなわち、 ^ ' s s の正負を判定することによ、これら 2つの動作は、同じ動作として取扱うことができる。また、 2 · についても同様である。

0 また記憶テ一ブルの離散値パラメ一タとして、 ⁰2 s j , のかわに I 2s 〗 i を用いる必要がある。

よつて整理すると

Ϊ ) | a | I '

のとき、 t _U i ( I ， ^a

5 ϋ ) | " | I ' e t ' e s sく oのとき、八

u ( I 9 a

2 s k

iii ) β \ < ■ Δ Θ

2 2 s ≥oのとき、

へ

u i 2 s > j ; ， ^ak)

iv ) β I < 1 ， 0₂ * 0_{2 s}<oのとき、

u l ¹ 2 s ¹ j ' fcノ

ここに

j = 1 , 2 , 3 , , β

k = 1 , 2 , 3 , …… , H'

\ e_{2 s} \ < \ Θ _{2 5} \ ^ e_{2 sma∑}

— な k-i k≤¹ ただし、 02 smax は、第 2アームの第 1 アームに対して成し得る最大角度

e , ' は自然数

とるる。

上記以外の処理については、第 2の実施例と同様に処理すればよい。

以上のように、本実施例では、記憶テーブルの量を第 2の実施例の半分にすることができる。

¾ぉ、前記各実施例では、標準加速曲線および標準減速曲線を時間を基準に速度指令を創出するものとしたが、これは一例であって、他のものを用てもよい。

また、前記各実施例では、最適化条件としてモータおよび減速機の負荷トルクを用いたが、これは一例であつて、例えば許容限度の大きい減速機を用いて、減速機の負荷トルクを考慮せずに最適化を行なってもよい。あるいは逆に、他の要素の負荷トルクを考慮に加えてもよ。さらには、振動を押えるために、負荷トルクを時間微分したものを考慮してもよい。要は、結果的に標準加減速曲線を基準に加減速時間，ピーク回転数を圧縮、拡大することによ、最適化を図るものであればよい。

また、第 2及び第 3の実施例では、離散値パラメータに 0_2s， a β ^ E を用いたが、これは一例であって、

Δ Θ - m ω

2 2 Μ

a = (23)

Δ θ m. ω 2Μ

m. ω 2Μ

β = (¾)

Δβ m ω

2 2 Μ

^Θ 2 Ε= ^Θ _{2 5} + ^d 2

であるから、 ⁰ _{2 s}， ^a ， ^ ， 0₂E を離散値パラメータとすることと、 _{2 S} , ^Jタ 1 ，を離散値パラメータとすることは全く同じことを意味してお）、これらを離散値パラメータとしても本発明に何ら変]?は。

産業上の利用可能性

以上のように本発明は、動作ごとに、入力された位置データにもとづいて、加減速時にモータもしくは、モータおよび減速機に掛るトルクが許容範囲内で最大と ¾るよう角加速度を制御するため、従来のように加減速時間を固定した場合に比べ、移動時間を大幅に、しかも無理なく短縮できる。

Claims

請求の範囲

1 . 第 1 の関節を介して本体に旋回可能に支持された第 1 ァ一ムと、第 2の関節を介して前記第 1 アームに旋回可能に支持された第 2アームと、前記各関節に設けられ、前記第 1 アーム及

5 び第 2アームを旋回させる第 1 モータ及び第 2 モータを備え、前記各モータに角速度指令を与えて前記第 1 アーム及び第 2ァームの動きを制御する多関節型 π ボットの制御方法におて、時間もしくは移動量を基準に前記各モータの速度指令値もしくは目標移動位置指令値を創出する標準加速曲線と標準減速曲線 i o をあらかじめ記憶装置に記憶しておくと共に、以下の各ステツブを実行することによ ^前記第 2 ァ一ムの先端が点 P i から点

P に移動する際の加減速の角速度指令を前記各モータに与える多関節ボットの制御方法。

① ²点 P i ， P _{i + 1} の位置データを読込むステップ、

15 ② 点 P i から点 P _{i + 1}への移動の加速時において、前記標準加速曲線の基準である加速時間もしくは加速時の移動量を圧縮または拡大するステツプ、

<D 前記加速時間もしくは加速時移動量と 2点 P i ， p _{i + 1} の位置デ一タによつて定まる加速状態の全てにわたって前記第 0 ¹ モータ及び第 2モータの負荷トルクを計算するステツブ、

@ 前記負荷トルクの最大値が、いずれもそれぞれに定まつた許容トルク以下で、かつ少くとも 1 つが前記許容範囲内で最大であるかどうかを判定し、前記条件が満たされ ¾ 場合は、再びステップ②に戻])、満たされる場合は求めるべき

25 加速時間もしくは加速時の移動量を前記値のものとするステッブ、

( 点 P i から点 P _{i + 1} への移動の減速時において、前記標準減速曲線の基準である減速時間もしくは減速時の移動量を圧縮または拡大するステップ、

5 ⑥ 前記減速時間もしくは減速時移動量と 2点 P ₄， P _{i + 1} の位置データによって定まる減速状態の全てにわたつて前記第

1 モータ及び第 2 モータの負荷トルクを計算するステッブ、 ® 前記負荷トルクの最大値が、いずれもそれぞれに定まつた許容トルク以下で、かつ少くとも 1 つが前記許容範囲内 _{l o} で最大であるかどうかを判定し、前記条件が満たされい場合は、再びステップ ©に戻、満たされる場合は求めるべき減速時間もしくは減速時の移動量を前記値のものとするステップ、

⑧ 前記手法によつて求められた加速時間もしくは加速時の

1 5 移動量を前記標準加速曲線の基準として用い、加速時の速度指令値もしくは目標移動位置指令値を出力するステツブ、

# 前記手法によって求められた減速時間もしくは減速時の移動量を前記標準減速曲線の基準として用い、減速時の速度指令値もしくは、目標移動位置指令値を出力するステップ。

0 2 . 第 1 の関節を介して本体に旋回可能に支持された第 1 ァ— ムと、第 2の関節を介して前記第 1 アームに旋回可能に支持された第 2 アームと、前記各関節に設けられ、前記第 1 アーム及び第²アームを旋回させる第¹ モータ及び第 2 モータを備え、前記各モータに角速度指令を与えて前記第 1 アーム及び第 2ァ 5 —ムの動きを制御する多関節型ロボットの制御方法において、時間もしくは移動量を基準に速度指令値もしくは目標移動位置指令値を創出する標準加速 ffi線と標準減速曲線をあらかじめ記憶装置に記憶しておき、また、モータの負荷トルクに影響を与え、移動開始点と移動終了点の位置データよ定まる要素をそ

5 れぞれ離散値化して記憶装置に記憶させ、前記離散値パラメ一'.

タのすべての組合せについて、前記第 1 モータ及び第 2 モータの負荷トルクがいずれもそれぞれに定まつた許容トルク以下で、かつ少なくとも 1つが前記許容範囲内で最大らしめる加速時間もしくは加速時の移動量、減速時間もしくは減速時の移動量 l O を前もつて計算し、それらを前記離散値パラメ一タの組合せに対応して記憶装置に記憶しておくと共に、以下の各ステツブを実行することによ ]?前記第 2アームの先端が点 P i から点

P _{i +1} に移動する際.の加減速の角速度指令を前記各モータに与える多関^ロボットの制御方法 o

1 5 ① 前記 P i , P _{i +1}の位置データを読込むステップ、

② 前記 P i , P _{i +1} の位置データよ ]?前記離散値パラメータに用られている要素を算出するステッブ、

③ 前記要素算出値を前記離散値パラメータに照合し、最も近組合せに対応する加速時間もしくは加速時の移動量、滅

20 速時間もしくは減速時の移動量を求めるべき値-とするステツブ、

@ 前記手法によつて求められた加速時間もしくは加速時の移動量を前記標準加速曲線の基準として用い、加速時の速度指令値もしくは、目標移動位置指令値を出力するステップ、 25 © 前記手法によって求められた減速時間もしくは減速時の移動量を前記標準減速曲線の基準として用い、減速時の速度指令値もしくは目標移動位置指令値を出力するステップ0 3。第 1 の関節を介して本体に旋回可能に支持された第 1 ァームと、第 2の関節を介して前記第¹ アームに旋回可能に支持された第 2アームと、前記各関節に設けられ、前記第¹ アーム及び第 2アームを旋回させる第 1 モータ及び第¹ 減速機、第² モータ及び第 2減速機を備え、前記各モータに角速度指令を与えて前記第 1 アーム及び第²アームの動きを制御する多闋節型口ボットの制御方法におて、時間もしくは移動量を基準に速度指令値もしくは目標移動位置指令値を創出する標準加速曲線と標準減速曲線をあらかじめ記憶装置に記憶しておくと共に、以下の各ステップを実行することによ ]?前記第 2 ァ一ムの先端が点から点 P _{i +1}に移動する際の加減速の角速度指令を前記各モータに与える多闋節、 π ボットの制御方法。

① 2点 P i , P _{i +1}の位置データを読込むステップ、

② 点 P i から点 P _{i + 1} への移動の加速時において、前記標準加速曲線の基準である加速時間もしくは加速時の移動量を圧縮または拡大するステツブ、

前記加速時間もしくは加速時移動量と 2点 P i , P _{i + 1}の位置データによって定まる加速状態の全てにわたつて第 1 モータ及び第 1 減速機、第 2 モータ及び第 2減速機の負荷トルクを計算するステッブ、

④ 前記負荷トルクの最大値が、いずれもそれぞれに定まつた許容トルク以下で、かつ少なくとも 1 つが前記許容範囲内で最大であるかどうかを判定し、前記条件が満たされ ¾い場 '合は、再びステップ②に戻 1)、満たされる場合は求めるべき加速時間もしくは加速時の移動量を前記値のものとするステップ、

⑥ 点 P iから点 P _{i +1}への移動の減速時において、前記標準

5 減速曲線の基準である減速時間もしくは減速時の移動量を圧縮または拡大するステ ' ·/ プ、

(D 前記減速時間もしくは減速時移動量と 2点 P i， P _{i +1} の位置データによつて定まる減速状態の全てにわたつて第 1 モータ及び第 1 減速機、第 2 モータ及び第 2減速機の負荷トルクを計算するステップ、

⑦ 前記負荷トルクの最大値が、いずれもそれぞれに定まつた許容トルク以下で、かつ少なくとも 1 つが前記許容範囲内で最大であるかどうかを判定し、前記条件が満たされない場合は、再びステ ' ブ⑤に戻！）、満たされる場合は求めるべき5 減速時間もしくは減速時の移動量を前記値のものとするステツブ、

® 前記手法によつて求められた加速時間もしくは加速時の移動量を前記標準加速曲線の基準として用い、加速時の速度指令値もしくは、目標移動位置指令値を出力するステップ、0 ⑨ 前記手法によって求められた減速時間もしくは減速時の移動量を前記標準減速曲線の基準として用い、減速時の速度指令値もしくは、目標移動位置指令値を出力するステップ。 4 - 第¹ の関節を介して本体に旋回可能に支持された第 1 ァームと、第²の関節を介して前記第 1 アームに旋回可能に支持さS れた第² アームと、前記各関節に設けられ、前記第 1 アーム及び第 2アームを旋回させる第 1 モータ及び第 1 減速機、第² モ —タ及び第 2滅速機を備え、前記各モータに角速度指令を与えて前記第 1 アーム及び第² アームの動きを制御する多関節型口ボットの制御方法において、時間もしくは移動量を基準に速度指令値もしくは目標移動位置指令値を創出する標準加速曲と標準減速曲線をあらかじめ記憶装置に記憶しておき、また、第 1 モータ及び第 1減速機、第² モータ及び第 2減速機の負荷トルクに影響を与え、移動開始点と移動終了点の位置データよ定まる要素をそれぞれ離散値化して記憶装置に記憶させ、前記離散値パラメータのすべての組合せについて、第¹ モータ及び第 1 減速機、第² モータ及び第²減速機の負荷トルクがいずれもそれぞれに定まつた許容トルク以下で、かつ少なくとも ¹ つが前記許容範囲内で最大ならしめる加速時間もしくは加逑時の移動量、減速時間もしくは減速時の移動量を前もつて計算し、それらを前記離散値パラメ一タの組合せに対応して記憶装置に記憶しておくと共に、以下の各ステツブを実行することによ前記第 2 アームの先端が点 P i から点 P _{i + 1}に移動する際の加減速の角速度指令を前記各モータに与える多関節ロボッ卜の制御方法。

① 前記 P i , P _{i + 1} の位置データを読込むステップ、

② 前記 P i ， p _{i + 1} の位置データよ前記離散値パラメータに用いられている要素を算出するステツブ、

(D 前記要素算出値を前記離散値パラメータに照合し、最も近い組合せに対応する加速時間もしくは加速時の移動量，滅速時間もしくは減速時の移動量を求めるべき値とするステツブ、

④ 前記手法によつて求められた加速時間もしくは加速時の移動量を前記標準加速曲線の基準として用い、加速時の速度指令値もしくは、目標移動位置指令値を出力するステップ、 ⑤ 前記手法'によって求められた減速時間もしくは減速時の移動量を前記標準減速曲線の基準として.用い、減速時の速度指令値もしぐは目標移動位置指令値を出力するステツプ。