JPS60230206A - 角加速度制御方法 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 産業上の利用分野 本発明は多関節型ロボットにおける第１アームと第２ア
ームの角加速度制御方法に関するものである。

従来例の構成とその問題点 水平多関節型ロボットのスローアツフ゛時、スローダウ
ン時におけるアームのモータに掛るトルクはその時の第
２アームの姿勢、移動に必要な第１アームと第２アーム
のモータ回転数比、第１アームと第２アームの回転方向
等によって異なってくるが、従来の水平多関節型ロボッ
トにおいては、全ての動作に渡ってスローアップ、スロ
ーダウフ時の角加速度制御は固定で、通常、最大トルク
カ；°　掛る動作に合せてスローアップ時間とスローダ
ウフ時間を決定していた。

しかし々から上記のような手法では低トルりのスローア
ップ、スローダウン動作に対しても最大トルクの掛るス
ローアップ、スローダウン動作と同じだけの時間を要す
ることになり、移動時間を短縮する際、大きな障害とな
っていた。

発明の目的 本発明は、モータや駆動回路の能力を最大限に生し常に
最短時間で移動するよう角加速度制御することによって
上記欠点を解消するものである。

発明の構成 本発明は、点Ｐｉから点Ｐｉ＋１へ移動する際、スロー
アップ、スローダウンの角速度曲線を、２点大にする最
適スローアップ、スローダウン時間をめることによって
決定するものである。

本発明は一般に移動時間を短縮したい場合、有利になる
。

実施例の説明 以下に本発明の一実施例を第１図にもとづいて説明する
。

図において１はロボット本体の支柱、１１は支柱１に接
続された第１アーム、１２は第２アームで１３と１４は
第１アーム１１と第２アーム１２を駆動するモータであ
る。２は上記モータ１３゜１４を駆動するだめの駆動回
路部である。３は数値制御装置で、演算装置３１．記憶
装置３２．速度指令装置３３から構成される装置 納された位置決めデータからスローアップ，スロ−ダウ
ン時のモータの角加速度を動作毎に決定し、速度指令装
置３３を通じて速度指令信号を前記駆動回路部２に入力
するものである。

次に加速度制御の方法を第２図にもとづいて説明する。

スローアップ時間が同じでもスローアップ時のトルクは
第２アーム１２の姿勢（θ２）、移動に必要な第１アー
ム１１と第２アーム１２のモータ回転数比と、回転方向
によって大きく異なってくる。第２アーム先端がある点
Ｐ、からある点Ｐ、＋１へ移動する場合第１アーム１１
と第２アーム１２の移動角がΔθ１．Δθ２で第１アー
ム１１と第２アーム１２が同時スタート、同時停止動作
をすると。

すれば、それぞれのモータの定常状態での角速度；１・
；２は ■　Δθ１・ｍｌ＜Δθ２・ｍ２の時 ■　７０１０ｍ１〉Δθ２・ｍ２の時 と表わされる。ここで である。

ところで力学の法則より第１アーム１１及び第２アーム
１２のモータに掛るトルク’ｒ　１　（ｔ）ｔ　”　２
（りは例えば次のようにして表わされる。

−ａｓ　・（２ω１・ω２＋ω２２）・ＳＩｎθ２＋ｂ
８ｅωニーｓｉｎθ２ ここでω１は第１モータの角速度、ω２は第２モータの
角速度、またａｌ　ｔ　ａ２　、・・・・・・、　ａｓ
及びｂｌ、ｂ２゜・・・・・・、ｂ８はいずれも定数で
ロボットのアームの長さ、重量、摩擦抵抗等によって異
なってくる。

従ってスローアップ曲線を第３図のような滑らかな曲線 ω（ｔ）＝　ｆｕ（ｔ）Ｔ　ｏ　＜ｔ　＜１と定め第１
モータ、第２モータの定常時における角速度ヲｃｃ＋１
．ａ＋、とし、スローアップ時間をｔ、動作間始時の第
２アーム１２の姿勢角を０２８とするとスローアップ中
、各モータに掛るトルクは次のように０２８１”１＋”
２＋’ｕの関数として書き表わすことができる。

Ｔ１（す＝９１（０２ｓｌ”１　、＝、、ｔｕ）Ｔ２（
ｔ）−（７２（θ２ｓ＋”ｌ”２＋會Ｕ）Ｉａｌ＜：ｉ
のとき冨、−、、＋２＝α・０Ｍ１α１〉１のときω１
＝ｉ・ωＭ、ω２＝ωＭであるから、θ２８．αをパラ
メータとして、それぞれの０２ｓ＋　αを有する動作に
対して要する最小のスローアップ時間ｔｕｔｈを第４図
のフローチャートに従って計算することができる。

次にフローチャート（第４図）のアルゴリズムを説明す
る。

■　スローアップ時間ｔ、−１適当な初期値ｔｕ。

とする。

ここで ■　Ｔ１＝ｍａｘ　’ｒ１（ｔ）、　Ｔ２＝ｍａｘ　’
ｒ２（ｔ）としてＩＴｌ−ＴＭＩＫε１またはｌ　Ｔ２
−７Ｍ２　’　〈ε２ならば現在のｔｕが実行中のパラ
メータθ２８．αに対して最適スローアップ時間ｔｕｉ
となる。

ここで ■　さもなければ をめる。

■　ｔｕ＝ｍａｘ（ｔｕｌ、　ｆｕ２）として　へ戻る
。

以下これを繰返すことによって任意のθ２ｓとαに対し
て最適スローアップ時間ｔｕｉをめることができる。

このようにしてまったｔｕｉに対して最適スローアップ
曲線は ■　ω１ｉ”ｕｉ）＝ωＭのとき ω２．（１）＝ωＭ−甲婢一）、ω１．（１）＝÷・ω
２．（１）但しｏくｔ＜ｔ　。

ｌ としてまる。

スローダウンの場合もθ２Ｅを動作の終点Ｐ、＋１にお
ける第２アームの姿勢角とすると、θ２Ｅｌαをパラメ
ータとして、スローアップの場合と同様としてまる。

以　下　余　白 このようにしてまった角速度データは、速度指令装置３
３へ入力され、速度信号となって駆動回路部２に出力さ
れる。さらに駆動回路部２で増巾されて、第１モータ１
３と第２モータ１４が駆動される構成になっている。

以上の実施例では第１モータと第２モータの最大角速度
が同じであるとして説明しだがその必要はない。以上の
アルゴリズムをより一般的にスローチャートを用いて説
明すると次のようになる（第５図参照）。

■　標準のスローアップ、スローダウン曲線但しｆ、１
ｏ）＝ｏ、　ｆｕ（１）−１ｆｄ（Ｔ−）　、　ｏ＜ｔ
　＜ｔｄ 但しｆｄゆ）−１，ｆｄ（１）−〇 を入力。

■　２点”ｉ’ｉ＋１　のデータを入力。

■　点Ｐｉから点Ｐｉ＋１への移動におけるスローアッ
プ中、第１モータ、第２モータに掛るトルクＴ　（ｔ）
、Ｔ　（ｔ）が Ｖｔ６（ｏ、　ｔｕ）　Ｋ対して ＩＴｕｌ（を月くＴｕｌ、１Ｔｕ２（ｔ）ｌ　く〒ｕ２
であり、かつ ヨｔＥｃｏ、ｔ）に対して ＩＴｕｌ（ｔ）１−Ｙｕｌまだは１Ｔｕ２（を月−ｔｕ
２とならしめるｔｕ　を第４図の最適化手法によってめ
る。ここでＴｕｌ、ｔｕ２はスローアップ時、第１モー
タ、第２モータに許される最大トルク。

■　ステップ■でめた最適スローアップ時間をｔｕｉと
おく。

■　第１モータ、第２モータの角速度をω１ｉ（１）。

ω２ｉ（ｔ）とすると、最適スローアップ曲線は次のよ
うにしてまる。

の　ω１１６□）＝ω１Ｍのとき ■　ω２１６□）＝ω２Ｍのとき 以下、Ｐ、＋１が最終点に到来するまでステップ■から
ステップ■を繰返す。

又、最適スローダウン曲線も全く同様に考えることがで
きる。

次に、もう１つの実施例について述べる。

実施例１の説明により、点Ｐｉ、Ｐｉ＋１のデータから
点Ｐｉでの第２アームの姿勢角θ２８とモータの回転数
比αとモータ（アーム）の回転方向が分ればその時の最
適スローアップ時間ｔｕｉをめることが可能である。

そこで０２８とαを離散値パラメータとして、それらの
最適スローアップ時間を前もって計算し、次のように表
わすことができる（ここでは簡単のため第１モータと第
２モータの最大回転数を同じとし、ωＭで表わす）。

ここでｌ、５は　ｉ＝１．２．・・・・・・、ｌｊ　”
”　＋　２＋・・・・・・　ｌｌであり、又θ１．αｉ
、β１は である。

これらの１ｕ、１ｕは全て、記憶装置３２に前もって記
憶させておくことができるので、プログラムされた任意
の位置決めデータに対して移動開始時のスローアップ時
間ｔｕｋは第６図のフローチャートに従って近似的に決
定することができる。

スローアップ時間’ｕｋがまれば実施例１と同様に、そ
の時の最適なスローアップ曲線をめることができる。

以上のことをフローチャートに表わすと第７図のように
なる。

又、スローダウンについても全く同様の考え方ができる
のは明らかである。

さらに第１モータと第２モータの最大角速度が異なる場
合も実施例１で示したごとく、同様の考え方で拡張でき
る。

この実施例ではモータのトルクに影響を与える要素とし
てθ２とαとモータの回転方向を用いたが、これに換る
別の要素を用いてもよい・要は表現はどうであれＰｌ、
Ｐ１＋１に応じてモータのトルクに影響を与える全ての
要素が含まれていればよい。

以上、２つの実施例において、移動距離によっては第８
図のようにスローアップ中にスローダウンを開始する場
合も発生するが、この場合でも本手法は全く有効である
。

は十分大きいと仮定すれば前に完全なスローアップ曲線
とスローダウン曲線がまり、実際のスローアップ、スロ
ーダウン指令にはその曲線の必要な区間だけを例えば第
８図のようにして用いればよいからである。

モータに掛るトルクＴ　１（ｔ）　、　Ｔ　２（ｔ）の
式はこの実施例で用いた表現にこだわる必要はない。

要は各モータに掛るトルク表現できるものであればよい
。

実施例１においては最適スローアップ、スローダウン曲
線をめることが可能である。実施例２においではパラメ
ータθ２　、　（Ｌを離散値化してめるという意味にお
いて準最適なスローアップ、スローダウン曲線となるが
一般に点ＰｉとＰ、４１のデータを読込んでからスロー
アップ、スローダウン曲線をめるまでの処理時間は前者
に比べて大巾に短縮できるという特徴がある。

発明の効果 このように本発明は動作毎に、入力された位置決めデー
タに基づいて、スローアップ、スローダ大となるように
角加速度制御をしている。従って従来のようにスローア
ップ、スローダウン時間を固定した場合に比べると ■　はとんど全ての動作に対して移動時間が短縮できる
、 ■　等距離移動における平均速度が均一化される、 といった効果がある。

【図面の簡単な説明】

第１図は本発明の一実施例における水平多関節型ロボッ
トの概略構成図、第２図は第１アームと第２アームの角
速度曲線およびその時モータに掛るトルク曲線の関係を
示す図、第３図は標準のスローアップ曲線図、第４図は
最適スローアップ時間をめるだめのフローチャート図、
第５図は第１実施例の最適スローアップ、スローダウン
曲線をめるフローチャート図、第６図はプログラムデー
タに基づいて最適スローアップ時間をテーブルからサー
チするだめのフローチャート図、第７図は第２実施例の
最適スローアップ曲線をめるフローチャート図、第８図
はスローアップ中にスローダウンする場合の角速度曲線
図である。 １・・・・・・ロボット本体の支柱、１１・・・・・・
第１アーム、１２・・・・・・第２アーム、１３・旧・
第１モータ、１４・・・・・・第２モータ、２・・・・
・・モータ駆動回路部、３・・・・・・数値制御装置、
３１・・・・・・演算装置、３２・・・・・・記憶装置
、３３・・・・・・速度指令装置。 代理人の氏名　弁理士　中　尾　敏　男　はが１名第１
図 第　２　図 第３図 ω 第４図 第　８　図 ０ノ

Claims (1)

1. 【特許請求の範囲】 ０）モータに速度指令を与え、回転可能な第１アームと
   連結した第２アームを作動させる角加速度制御方法であ
   −て前記第２アームの先端が、点Ｐ。 から点Ｐｉや、へ移動する際、スローアップ、スローダ
   ウンの角速度指令を与える方法として、標準スローアッ
   プ曲線 と標準スローダウン曲線 但しｆｕｐ）＝　ｆｄ（１）＝。 を記憶し、 ■　２点Ｐ１とＰｉ＋１　のデータを読込むステップ ■　点Ｐｌから点Ｐ１＋１への移動時に用いるスローア
   ップ曲線において、第１アームまたは第２アームのモー
   タにかかるトルクがいずれも、それぞれに定まった許容
   範囲内で、前記トルクの少なくともどちらか一方が許容
   範囲内で最大ならしめる最適スローアップ時間ｔｕ１を
   めるステップ、 ■　第１アーム、第２アームのモータ角速度をＣ１ｉ（
   １）、Ｃ２ｉ（ｔ）として最適スローアップ曲線を但し
   Ｃ１ｉ、Ｃ２ｉは定数 としてめるステップ、 ■　点Ｐｉから点Ｐｉ＋１への移動時に用いるスロちら
   か一方を許容範囲内で最大ならしめる最適スローダウン
   時間ｔｄｉをめるステップ、■　最適スローダウン曲線
   を としてめるステップ、 を繰返して構成された角加速度制御方法。 （２）モータに速度指令を与え、回転可能な第１アーム
   およびこの第１アームと連結した第２アームを作動させ
   る角加速度制御方法であって前記第２アームの先端が、
   点Ｐ１から点Ｐ、＋１へ移動する際、スローアップ、ス
   ローダウンの角速度指令を与える方法として、モータの
   トルクに影響を与え、Ｐｉ。 Ｐｉ＋１　のデータから決まる要素をａｌ、・・・・・
   ・、ａｎ但しｊおよびに１．に２．・・・・・・、ｋｎ
   は整数として表わし、これらａｌ（ｔ）　＋　ａ２０）
   　＋・・・・・・、　ａｎ（ｊ）を組み合せて出来る全
   てのスローアップ曲線と、容範囲内で最大にする最適ス
   ローアップ時間と最適スローダウン時間を前もってめ、
   これらをメモリに記憶しておき、任意のＰｉ、！：Ｐｉ
   ＋１が与えられるとＰ、とＰｉ＋１のデータから要素ａ
   １．　ａ２　、・・・・・・。 〜を計算し、それに最も近いａｌ　（］）　ｌ　ａ２　
   （］）　ｌ・・・・・・、　ａｎＯ）をめ、それに対す
   るスローアップ時間とスローダウン時間をメモリから呼
   び出すことによってＰｉ＋Ｐｉ＋１　の位置決めデータ
   を読込んでから、最適スローアップ曲線と最適スローダ
   ウン曲線をめるまでの時間を短縮する角加速度制御方法
   。