JPH02262981A - ロボット制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明は、ロボットアームを目標位置へ移動させたり、
指定された複数の経由位置を経て目標位置まで移動させ
るロボット制御装置にかかるもので、特に動作速度を向
上させるための改良に関するものである。

〈従来の技術〉 本出願人は、特願昭６３−１５８８７７号でロボット制
御装置を出願している。

この発明は、ロボットアームが経由位置を経る場合、経
由位置の前後で、最も長い移動時間を要する関節（以下
、代表関節とする）、速度、加減速時間が異なるときは
、経由前の速度から停止するまでの減速停止距離と、停
止状態から経由後の速度に到達するまでの加速距離とを
比較して短い方を経由動作距離とし、減速停止距離が経
由動作距離よりも長い場合は、経由動作時−に到達する
まではロボットアームを等速パターンで動作させ、その
後、経由動作に移行させる制御を行い、経由動作におけ
る速度を滑らかに変化させるとともに、移動時間を短縮
したものである。

〈発明が解決しようとする課題〉 しかし、このロボット制御装置では、経由位置の前後に
おける代表関節の速度が著しく異なる場合には、大きな
加速度を必要とすることになり、滑らかな動作を行えな
いという問題点があった。

本発明はこのような問題点を解決するためになされたも
のであり、経由位置の前後で代表関節の速度が著しく異
なっていても、許容加速度以上の加速度を必要とせずに
滑らかに経由動作を行うことができるロボット制御装置
を実現することを目的とする。

く課題を解決するための手段〉 本発明は、 ロボットアームが移動する軌道を制御し、最も長い移動
時間を要する代表関節が出し得る最大の移動速度で動作
させるロボット制御装置において、ロボットアームの現
在位置を記憶する手段と、外部より経由位置を受信し記
憶する手段と、外部より目標位置を受信し記憶する手段
と、ロボットアームの各関節について、現在位置または
経由位置から経由位置または目標位置までの移動時間を
算出する手段と、 算出した各関節の移動時間から代表関節を選択する手段
と、 ロボットアームを加速動作させる加速パターンの軌道を
発生ずる手段と、 ロボットアームを等速動作させる等速パターンの軌道を
発生する手段と、 ロボットアームを減速動作させる減速パターンの軌道を
発生する手段と、 ロボットアームを経由位置で停止させることなく経由位
置近傍を滑らかに経由動作させる経由パターンの軌道を
発生する手段と、 動作開始後の経過時間、代表関節の移ａ距離および移動
速度から前記４つの軌道発生手段のいずれかを選択する
パターン選択手段と、 代表関節の移動距離から各関節の軌道を発生する手段と
を具備し、 前記パターン選択手段は、 経由前の移動速度から停止するまでに要する時間と、停
止状態から経由後の移動速度まで加速するまでに要する
時間を比較する比較手段と、比較した時間のうち長い方
を経由時間として選択する選択手段と、 前記経由点近傍の軌道発生手段を選択し°、選択した経
由時間内で減速パターンと加速パターンを組み合わせて
経由動作のための軌道を生成する軌道生成手段、 を有することを特徴とするロボット制御装置である。

く作用〉 このような本発明では、経由前の移動速度から停止する
までに要する時間と、停止状態から経由後の移動速度ま
で加速するのに要する時間を比較して長い方を経由時間
とし、この経由時間内で減速パターンと加速パターンを
組み合わせて、経由位置の前後で代表関節の速度が著し
く異なっていても滑らかに経由動作を行なわせる。

〈実施例〉 以下、図面を用いて本発明を説明する。

第１図は本発明の一実施例を示す構成ブロック図である
。

図で、１０はホストコンピュータ等外部からの通信を受
信するデータ受信手段、２０は多関節のロボットアーム
で、例えば６関節のものでは肩、肘、手首及びハンドの
上下回転、左右回転、捩りの６動作をするものである。

３０はロボットアームの各関節に取付けられた位置検出
手段で、例えば関節の回転角を測定するエンコーダや各
関節に付されたマークの読取り装置が用いられる。

４０はロボットアームの現在位置を記憶する現状認識部
、５０はデータ受信手段１０を介して外部から指令され
た経由位置及び目標位置を記憶する指令認識部、６０は
各区間おける関節の移動時間を算出して、移動時間の最
も長い関節（以下、代表関節とする）を求める代表関節
選択部である。

前述した区間は、現在位置から経由位置まで、経由位置
から経由位置まで（経由位置が複数個指定された場合）
、経由位置から目標位置まで等からなる。

７０は現在位置、目標位置、経由位置の各区間の移動に
必要な加速、等速、減速及び経由動作の軌道パターンを
演算する軌道発生部、８０は軌道発生部７０の軌道パタ
ーンを各区間に適するように選択するパターン選択部、
９０はパターン選択に必要な各種のデータを記憶する各
種データ記憶部である。

１００は軌道発生部７０の出力に対応して代表関節を含
めた各関節の軌道パターンを発生する関節軌道発生手段
、１１０は関節軌道発生手段１００からの信号をもとに
ロボットアーム２０を駆動する駆動回路である。

次に、各部の詳細な構成を説明する。

現状認識部４０で、４１は位置検出手段３０の信号から
現在のロボットアーム２０の位置を記憶する現在位置レ
ジスタで、例えば各関節の回転角を記憶している。４２
は前回の目標位置の値を記憶する前回目標位置レジスタ
である。

指令認識部５０で、５１はデータ受信手段１゜で指定さ
れた目標位置を記憶する目標位置レジスタ、５２はデー
タ受信手段１０で指定された経由位置を記憶する経由位
置レジスタで、経由位置は複数個指定されることがある
。

代表関節選択部６０で、６１はロボットアーム２０の各
関節について各区間における移動所要時間を演算する移
動時間演算手段で、例えば移動量を予め各関節毎に定め
られている最大速度で除算して求めている。６２は代表
関節選択手段で、移動時間演算手段６１の値から最大値
を呈示した関節を代表関節として選択する。

軌道発生部７０で、７１は現在速度から代表関節の最大
速度まで加速動作する軌道を発生する加速パターン発生
手段、７２は最大速度で等速動作する軌道を発生する等
速パターン発生手段、７３は目標位置で滑らかに停止す
るための軌道を発生する減速パターン発生手段、７４は
経由位置近傍で経由前における代表関節の減速動作と経
由後における代表関節の加速動作を組み合わせることに
より経由動作をするための軌道を発生する経由パターン
発生手段である。

このように構成された装置の動作を説明する。

第２図は経由位置を１つ指定した場合の軌道を示した図
である。

図中、予め与えられている位置は、現在位置Ｐｓ、経由
位置ＩＰ１、目標位置ＩＰＥである。これら以外の最大
速度到達位置Ｐｂｔ、１Ｐｂ２、減速開始位置ｌＰｃ　
１．　ＩＰｃ　２は演算により求められたものである。

この場合の符号ＩＰはｎ次元（ｎは整数）の関節角度ベ
タ１−ルである０区間１〜■はこれらの位置を境界とし
ている。また、Ｐａ＋Ｐ　ｂ　１　＋　Ｐ　ＣＩ　＋　
Ｐ　ｄ　Ｉ　＊　Ｐ　ｂ　２　＊　Ｐ　Ｃ２＋Ｐｄ２は
、それぞれＩＰｓ　、　ＩＰｂ　＋　、　ＩＰｃ　ｔ　
、　ＩＰ＋ｌＰｂ　２　、　Ｐｃ　２　、　ＩＰＥの位
置に到達するまでにロボットアームが移動する距離であ
る。また、ｖ１■２は代表関節の最大移動速度である。

以下、現在位置ＩＰｓ、経由位置ＩＰ１、目標位置ＩＰ
Ｅが与えられたとき、この装置が移動経路の途中の位置
をどのように演算しているかを説明する。

（１）移動時間演算手段の動作 移動時間演算手段６１は各関節ｉが所定の最大速度で等
速駆動するか否かを判別するための判別基準路ＪｌｆＤ
ｉｓｔ＜を次の式から求める。

Ｄｔｓｔｉ＝（１／２）Ｘ（Ｔｕｐ＋ＴｏＮ）ＸＶｔａ
χｉ 続いて、現在位置ＩＰｓ、経由位置ＩＰ１、目標位置Ｉ
ＰＥで定まる各区間の距Ｍ　Ｄｔと判別基準距離Ｔ）ｉ
ｓｔｉとを比較して、ＤｊＳｔｉ≧Ｄ、の場合は等速駆
動を含む駆動（以下、台形駆動とする）を行い、ＤＬｓ
ｔしくＤＬの場合は等速駆動を含まない駆動（以下、三
角駆動とする）を行う。

移動時間演算手段６１は、それぞれの場合に次式から移
動時間Ｔ、を求める。なお、ここでは現在位置ＩＰｓと
経由位Ｍ　ＩＰ　＋　との間のみ説明する。

（１）台形駆動の場合 （１１）三角駆動の場合 ここで、各式の各符号は次に定めるものである。

Ｔｕｐ：加速時間 ’！’ＯＮ：減速時間 Ｖｔ１Ｉａχ、：１番目の関節の指定された最大速度Ｄ
ｔ　：１番目の関節の移動距離 ＝　ｌ　ＩＰ　ｓ　ｔ　　ＩＰ　＋　ｔｌＰｓｉ　ｘｉ
番目の関節の現在位置 ｔＰ＋ｔ：ｉ番目の関節の経由位置 Ｔｉ：１番目の関節の移動時間 １＝１．２．３・・・ｎ ＩＰ　Ｉ　とＩＰＥが与えられた場合も、ＩＰ　ｓとＩ
Ｐ　＋が与えられた場合と同様にして経由後の各関節の
移動時間を算出する。

Ｃ）代表関節選択手段の動作 代表関節選択手段６２は、各関節の移動時間Ｔ□を比較
し、これらの中の最大ｆｆｆＴｈを選択する。すなわち
、 ’Ｉ’ｙ　＝ｍａｘ　（Ｔ、　、　Ｔ２　＊　＝Ｔｕ　
ｌの演算を行って代表関節ｋを選択し、 Ｄ＝Ｄｋ Ｄｊ　！３ｔ　＝Ｄｊ　Ｓｔ　ｋ Ｖｌｌ　ａ　Ｘ　””ＶＷｌ　ａ　Ｘ　ｋおよび、 Ｋｕ　ｐ　＝　（１／２）　・（Ｖｔａｘ／Ｔｕｐ　）
：加速時の係数 Ｋｏ　Ｎ＝　（１／２）　・（ＶｍａＩＸ／ＴｏＮ　）
：減速時の係数 Ｓ１ａ　ｘ　ｔ　ｔ　＝Ｉ）Ｌ／Ｄ ：池関節への配分係数 を算出して記憶する。

（３）加速パターン発生手段の動作 加速パターン発生手段７１は、発生パターン選択手段８
０から、現在までの代表関節の移動距離Ｐａ、現在の代
表関節の速度Ｖ０および加速終了時間Ｔｕが与えられる
と、次の式に従って代表関節の移動距離Ｐ（ｔ）を算出
する。

ｔ：時刻 ０≦ｔ≦Ｔｕ （４）等速パターン発生手段の動作 等速パターン発生手段７２は、発生パターン選択手段８
０から、現在までの代表関節の移動距離Ｐｈ−現在の代
表関節の速度ｖ０および等速終了時間′ｒＥが与えられ
ると、次の式に従って代表関節の移動距離Ｐ（ｔ）を算
出する。

Ｐ（ｔ）＝Ｐｂ＋Ｖ０・ｔ Ｏ≦ｔ≦’ｒＥ （５）減速パターン発生手段の動作 減速パターン発生手段７３は、発生パターン選択手段８
０から、現在までの代表関節の移動距離ＰＣ２現在の代
表関節の速度ＶＯおよび減速終了時間Ｆｌ゛、が与えら
れると、次の式に従って代表関節の移動距離ｐ（ｔ）を
算出する。

０≦ｔ≦Ｔ。

（６）経由パターン発生手段の動作 経由パターン発生器７４は発生パターン選択手段８０か
ら次のものが与えられる。

ＴＤＮＰ：経由前速度から停止するまでに要する時間 ’ｒＤＮ８：経由前速度から停止する時刻’Ｔ’ｕｐｐ
：停止状態から経由後速度に到達するまでに要する時間 Ｔｕｐｓ：停止状態から経由後速度へ加速を開始する時
刻 ’Ｉ’Ｂ：経由終了時刻 経由パターン発生器７４はこれらから、次式を用いて以
下のものを算出する。

■経由前における代表関節の移動距離Ｐｏ５（ｔ）（θ
含曳−≦τ）ＩＪ、す【１） Ｐｐｎ（ｋ’３　＝　ｔ’　ｔ　＋　　Ｖｏ　Ｔ）ＩＮ
Ｐ２゜ （ＴＤ囁く＋−五τｂｆ＞い） ■経由後における代表関節の移動距ＵＰｕｐ（ｔ）Ｐｕ
、（モ）１も （Ｔ町（ｔへＴＢｑ鳴 ただし、ｔ”＝ｔ−Ｔｕｐａ ここで、次式に従って関節位置Ｐｉ　　（ｔ）を算出す
る。

Ｆ　Ｘ’＆’）　＝　ＰＤＩ４　（、％’）　・５−ｉ
ｎ　十Ｐ　ｕｐ　（、％）・５−ｉｔ（θを曳含Ｔ８　
ｔ＞ｔ＝’） （７）関節軌道発生手段の動作 関節軌道発生手段１００は、経由パターン発生手段７４
以外の各パターン発生手段から代表関節の杉動距ｉｆ！
ＩＰ（ｔ）が与えられると、次の式に従って代表関節を
含む各関節の位置Ｐｉ　（ｔ）を算出する。

Ｐ＜（ｔ）＝Ｐｓｔ 十ｐ（ｔ）・５ｊａｕｔｌ 上式で、代表関節の位置を求める場合は５ｆａｖＬｔＬ
＝１とする・ （８）発生パターン選択手段の動作 第２図の位置ＩＰｓ　、　ＩＰ＋　、　ＩＰＥが指定さ
れた場合、ＩＰ　ｓからＩＰＥまでの動作を順を追って
説明する。

移動時間演算手段６１および代表・関節選択手段６２か
ら次のデータが与えられる。

■ＩＰ　ｓからＩＰ　＋までの区間においてＤＮ、Ｄｉ
　Ｓｔ　Ｎ、ＶｔａＸＮ。

Ｋｕｐ　　Ｎ、Ｋｏ　　ＮＮＩ　　５１ａｕｔ　　Ｎ■
ＩＰ　＋からＩＰＥまでの区間においてＤＰ　ＩＤｉ　
Ｓ　ｔ　Ｐ　＋　■ＩＬａＸＰ　＋ＫＵＰＰ　　、Ｋｎ
　　ＮＰ　　Ｉ　　５ｎａｕｔｐＶｔａｘロ二代表関節
の指定された最大速度 りロ：代表関節の移動距離 Ｄｉｅｔ口：Ｖ＠、χ口に到達するための移動距離 ＫＬＩＰ口：加速時の係数 ＫＤＮ口：減速時の係数 口はＮまたはＰ 以下、各区間の動作について説明する。

（１）区間Ｉ 上記のデータが与えられると、発生パターン選択手段８
０では、 Ｐａ＝Ｏ Ｐｂ　＝Ｐａ　＋（１／２　）　　（Ｖｔ　十Ｖｏ　）
　ＴｕＶｏ＝Ｏ Ｔｕ＝（Ｖｔ　　Ｖｏ）（Ｔｕｐ／ＶｍａｘＮ）ここで
、 加速パターンの発生が終了すると、区間■へ移る。

（１１）区間■ まず、等速区間が存在するかどうかを調べる。

■Ｄ　ＮＰ　ａ　＞　（１／　４　） ｘ（Ｖ＋　２／ＫＤＮＮ）の ときは、等速区間が存在し、 Ｐｃ　−？４　＋Ｖｔ・Ｔ。

として等速パターン発生手段７２を選択する６等速パタ
ーンの発生が終了すると、区間■へ移る。

■ＤＮ−Ｐ、≦＜１／４） Ｘ　（Ｖ＋　２／ＫＤ　ＮＮ）の ときは、等速区間が存在せず、 ＰＣ＝ｐｂ として、区間■へ移る。

（資）区間■ ＰｓからＰｌまでの区間と、ＩＰ　＋からＩＰＥまでの
区間では、代表関節が異なることは十分にあり得る。ま
た、Ｐｓから２１に至る区間における速度ｖ１とＰｌか
らＰＥに至る区間における速度■２が異なることもあり
得る。従って、経由位置の前後で減速または加速するこ
となく、さらに大きな加速度が加わらないように、経由
前動作の代表関節の減速停止と経由後動作の代表関節の
加速を組み合わせることによって経由動作を行なわせる
。

初めに、経由後の速度ｖ２を次式から求める。

”　＝　”３’Ｊ？　　　（”？　”、Ｉ）’＋Ｇｔ？
　’）　）：ｊ）■経由前速度から停止するまでに要す
る時間ＤＮＦ Ｔｏ　ＮＰ　＝Ｔｏ　Ｎ　・（Ｖ＋　／Ｖｔａ　Ｘ　Ｎ
　）■停止状態から経由後速度に到達するのに必要な時
間’ｒｕ　Ｐ　Ｐ ’ｒ’ｕ　Ｐ　Ｐ″Ｔｕ　ｐ　′（Ｖ２　／Ｖｔａχｐ
　）ここで、’ｒＯＮＰとＴ’ｕｐｐを比叙して次式か
ら経由終了時刻ＴＢを決定する。

ＴＢ　＝ｍａｘ　（ＴＯＮＰ　、　Ｔｕ　ｐ　ｐ　）こ
の時刻は経由動作開始時を基準にした時刻である。

最後に、 ’ｒｏ　ＮＢ　＝Ｔ（、ＮＰ Ｔｕ　ｐ　　ａ　　＝Ｔａ　　−Ｔｕ　ｐ　　ｐＰｂ　
２　　＝　　（Ｖ２　／　２　）　　・’ｒｕｐｐＶ、
＝Ｖ。

として経由パターン発生器７４を選択し、経由パターン
を生成し発生する。

経由パターンの速度プロファイルを第３図に示す。

第３図で、経由パターン発生器７４により減速パターン
と加速パターンを破線のように組み合わせて経由パター
ンにする。

経由パターンの発生が終わると、経由後動作を行うため
に、位置Ｐ１をＩＰ　ｓに、位置ＩＰＥをＩＰ　＋にそ
れぞれコピーする。さらに、各種の係数口Ｐを口Ｎにコ
ピーする（口は係数を表わす部分）。

その後、区間■へ移る。

（転）区間■ この区間では区間■と同様な処理を行って区間Ｖへ移る
。

Ｍ区間Ｖ この区間では、 ■Ｏ：ｖ２ Ｔｄ＝（Ｖｏ／Ｖｍａｘ　Ｎ　）　　・Ｔｏ　　ＮＰａ
ｗ　＝Ｐｃ２＋（１／２）　・Ｖｏ　・Ｔａとして減速
パターン発生器７３を選択し、減速パタπンを生成し発
生する６発生終了後にアームを停止する。

なお、ＩＰｓ　、　ＩＰ＋　、　ＩＰＥの位置関係、各
関節の最高速度および加減速時開の関係により、必ずし
も第２図の順番でパターン発生手段が選択されるとは限
らない０例えば、ＩＰ　ｓとＩＰ　、の位置が接近して
いる場合は、■の等速パターンが選択されずに加速区間
Ｉからすぐに減速区間■に移る。

また、実施例ではｎ個の関節を有するロボットの関節位
置を補間する場合について説明したが、直交座標系で表
わされる位置を補間する場合に適用してもよい。

また、実施例では経由位置が１個である場合について説
明したが、経由位置は複数個指定することも可能である
。

［効果］ 本発明によれば、経由を行うときは、経由前速度から停
止するまでに要する時間と、停止状態から経由後速度に
到達するまでに要する時間を比教し、長い方を経由時間
とし、経由時間内で経由パターン発生器を選択して減速
パターンと加速パターンを組み合わせて経由パターンを
生成している。

このなめ、経由位置の前後で代表関節の速度が著しく異
なっていても、許容加速度以上の加速度を必要とせず、
滑らかに経由動作を行うことができる。

【図面の簡単な説明】

第１図は本発明の一実施例の構成を示した図、第２図は
第１図の装置の動作説明図、第３図は経由動作時の速度
プロファイル図である。 １０・・・指令位置データ受信手段、２０・・・ロボッ
トアーム、３０・・・位置検出手段、４０・・・現状認
識部、５０・・・指令認識部、６０・・・代表関節選択
部、７０・・・軌道発生部、７１・・・加速パターン発
生手段、７２・・・等速パターン発生手段、７３・・・
減速パターン発生手段、７４・・・経由パターン発生手
段、８０・・・発生パターン選択手段、１００・・・関
節軌道発生手段、１１０・・・駆動回路。

Claims (1)

1. 【特許請求の範囲】 ロボットアームが移動する軌道を制御し、最も長い移動
   時間を要する代表関節が出し得る最大の移動速度で動作
   させるロボット制御装置において、ロボットアームの現
   在位置を記憶する手段と、外部より経由位置を受信し記
   憶する手段と、外部より目標位置を受信し記憶する手段
   と、ロボットアームの各関節について、現在位置または
   経由位置から経由位置または目標位置までの移動時間を
   算出する手段と、 算出した各関節の移動時間から代表関節を選択する手段
   と、 ロボットアームを加速動作させる加速パターンの軌道を
   発生する手段と、 ロボットアームを等速動作させる等速パターンの軌道を
   発生する手段と、 ロボットアームを減速動作させる減速パターンの軌道を
   発生する手段と、 ロボットアームを経由位置で停止させることなく経由位
   置近傍を滑らかに経由動作させる経由パターンの軌道を
   発生する手段と、 動作開始後の経過時間、代表関節の移動距離および移動
   速度から前記４つの軌道発生手段のいずれかを選択する
   パターン選択手段と、 代表関節の移動距離から各関節の軌道を発生する手段と
   を具備し、 前記パターン選択手段は、 経由前の移動速度から停止するまでに要する時間と、停
   止状態から経由後の移動速度まで加速するまでに要する
   時間を比較する比較手段と、比較した時間のうち長い方
   を経由時間として選択する選択手段と、 前記経由点近傍の軌道発生手段を選択し、選択した経由
   時間内で減速パターンと加速パターンを組み合わせて経
   由動作のための軌道を生成する軌道生成手段、 を有することを特徴とするロボット制御装置。