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

Abstract

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

Description

【発明の詳細な説明】 〈産業上の利用分野〉 本発明は、ロボットマニピュレータを［１標点へ移動さ
せたり、指定された経由点を経て目標点まで移動させる
ロボット制御装置にかかるもので、特に動作性能の向上
に関する。

〈従来の技術〉 本件出願人・がロボットマニピュレータを開始点から複
数の経由点を経て終了点まで移動させるロボット制御装
置として出願しているものの一つに、第β図のように、
経由区間前の指定速度の減速距離と経由区間後の加速距
離を比較して短い方を経由距離とすることにより経由開
始点Ｐ。と経由終了点Ｐ２を算出し、これら経由開始点
Ｐ。と経由終了点Ｐ２及び経由位置Ｐ１の３点で決定さ
れる曲線上を移動させるようにしたものがある。

〈発明が解決しようとする課題〉 しかし、この従来のロボット制御装置では、ロボットマ
ニピュレータの移動速度との指定によってロボットマニ
ピュレータの経由動作時の軌跡が異なってしまうという
問題がある。

すなわち、低速で経由軌跡を教示したものを高速で動作
させると経由軌跡がより内側になって教示した軌跡が使
えなくなったり、障害物との接触を生じる危険性があっ
た。

本発明はこのような点に着目してなされたものであり、
その目的は、速度に依存することなく軌道か決定できる
ロボット制御装置を提供することにある。

〈課題を解決するための手段〉 上記課題を解決する本発明は、 ロボットマニピュレータの現在位置を記憶し、外部より
指定された経由点を経由して外部より指定された目標点
に前記ロボットマニピュレータを移動させるロボット制
御装置において、ロボットマニピュレータの現在位置を
記憶する手段と、 外部より指定される経由点の位置を受信し記憶する手段
と、 外部より指定される目標点の位置を受信し記憶する手段
と、 ロボットマニピュレータの移動に関連した移動距離、速
度等のパラメータを算出する手段と、ロボットマニピュ
レータを加速動作させる加速パターンの軌道を発生する
手段と、 ロボットマニピュレータを等速動作させる等速パターン
の軌道を発生する手段と、 ロボットマニピュレータを減速動作させる加速パターン
の軌道を発生する手段と、 ロボットマニピュレータを経由動作させる経由パターン
の軌道を発生する手段と、 動作開始後の経過時間、ロボットマニピュレータの移動
距離及び移動速度から前記４つの軌道発生手段のいずれ
かを選択するパターン選択手段と、ロボットマニピュレ
ータの移動距離からロボットマニピュレータの位置を算
出する手段と、ロボットマニピュレータの位置から各関
節の位置を算出する手段とを具備し、 前記パターン選択手段は最高動作速度時の減速停止距離
と経由後の加速距離の短い方を経由距離として演算する
手段を有し、 前記経由パターン軌道発生手段は経由点より経由距離前
の点と経由点と経由点より経由距離後の点の３点に基づ
いて４次のベジェ曲線を生成すると共に該ベジェ曲線上
を時間の４次式で補間する手段を有し、 経由開始点と経由終了点が指定された動作速度に依存す
ることなく決定され、経由軌道上で経由前の動作速度か
ら経由後の動作速度へ加速度の連続性を保ったままで変
化させることを特徴とするものである。

く作用〉 本発明のロボット制御装置において、ロボットマニピュ
レータの経由パターン軌道は経由点より経由距離前の点
と経由点、と経由点より経由距離後の点の３点に基づく
４次のベジェ曲線で決定され、該ベジェ曲線は時間の４
次式で補間される。

これにより、ロボットマニピュレータの経由パターン軌
道の経由開始点と経由終了点は指定動作速度に依存する
ことなく決定され、経由軌道上での速度は経由前の動作
速度から経由後の動作速度へ加速度の連続性を保ったま
まで変化し、移動速度の指定が異なる場合にも常に同じ
軌道上を経由することになる。

〈実施例〉 以下、図面を参照して本発明の実施例を詳細に説明する
。

第１図は本発明の一実施例を示す構成ブロック図である
。図において、１０はホストコンピュータ等外部からの
通信による目標位置データ及び経由位置データを受信す
る指令位置データ受信器であり、受信した目標位置デー
タは［１標位置レジスタ２１に格納され、経由位置デー
タは経由位置レジスタ２２に格納される。なお、これら
目標位置レジスタ２１及び経由位置レジスタ２２は指令
認識部２０を構成している。３０は多関節のロボットマ
ニピュレータで、例えば６関節のものでは肩１１・ｌ１
手首、アームの上下回転及び捩りの６動作をする。４０
はロボットマニピュレータ３０の各関節に取り付けられ
た位置検出器で、例えば関節の回転角をｉ１Ｆ＋定する
エンコーダや各関節に付けられたマークを読み取るマー
ク読取装置が用いられる。

５０はロボットマニピュレータ３０の現在位置を把握す
るための現状認識部であり、現在位置レジスタ５．１及
び前回目標位置レジスタ５２で構成されている。現在位
置レジスタ５１には例えば各関節の回転角から算出され
た手先位置が格納される。

６０は目標位置レジスタ２１．経由位置レジスタ２２及
び前回目標位置レジスタ５２に格納されているデータ等
から移動時のパラメータを算出するパラメータ演算器で
ある。該パラメータ演算器６０は、移動時のパラメータ
として代表移動距離。

加減速度のパラメータ、最高動作速度時の加速距離や減
速距離等を算出して各種データ記憶装置７０に出力する
と共に発生パターン選択器８０にも出力する。該発生パ
ターン選択器８０の出力信号は軌道パターン発生部９０
に加えられる。該軌道パターン発生部９０には現在位置
、［目標位置及び経由位置の各区間の移動に必要な加速
パターン発生器９１１等速パターン発生器９２．減速パ
ターン発生器９３及び経由パターン発生器９４が設けら
れている。加速パターン発生器９１は現在速度から指定
された速度まで加速動作する軌道パターンを発生し、等
速パターン発生器９２は指定された速度で等速動作する
軌道パターンを発生し、減速パターン発生器９３は目標
位置で滑らかに停止する軌道パターンを発生し、経由パ
ターン発生器９４は経由位置近傍で経由前における減速
動作と経由後における加速動作を組み合わせることによ
り経由動作をするための軌道パターンを発生する。

これら各パターン発生器９１〜９４は終了信号を発生パ
ターン選択器８０に帰還して次のパターンの選択を促す
。このような軌道パターンの選択は目標位置に到着する
まで繰り返して実行される。

手先位置演算器１００は加速パターン発生器９１゜等速
パターン発生器９２及び減速パターン発生器９３から出
力される移動距離データに基づいてロボットマニピュレ
ータ３０の手先の位置を算出する。関節位置演算器１１
０には手先位置演算器１００の出力信号及び経由パター
ン発生器９４の出力信号が加えられていて、手先位置を
各関節の位置に変換するための演算を行う。該関節位置
演算器１１０で演算された各関節位置信号はロボットマ
ニピュレータ３０を駆動する駆動回路１２０にロボット
マニピュレータ３０の目標位置として与えられる。

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

第２図は経由位置を１つ指定した場合のロボットマニピ
ュレータの軌道を示した図である。図において、Ｐはｘ
、　　ｙ、　　ｚの３次元位置ベクトル、■１〜ｖ４は
指定速度、Ｐａ−Ｐｄ、Ｐａ’　〜Ｐｄ′は移動距離を
示している。予め与えられている位置は、現在位置Ｐｓ
、経由位置Ｐ３、目標位置ＰＥである。現在位置Ｐｓか
ら経由位置Ｐ、の近傍を経由して目標位置、Ｐ８へ至る
軌道パターンを発生する場合、区間Ｉ〜■において発生
バターン選択器８０を起動し、その時点での移動距離速
度等からいずれかのパターン発生器９１〜９４を選択し
てロボットマニピュレータ３０を動作させる。

■移動時のパラメータ演算器６０の動作現在位置Ｐ、及
び経由位置Ｐ１が与えられることにより、次のような５
種の演算を行う。

Ａ１代表移動距離 Ｄ−Ｉ　Ｐ、−ＰＳ Ｂ、指定速度Ｖに到達するための距離 Ｄ　＋−＋　−（１／２）（Ｔ−ｐ　＋　Ｔａ−）　Ｖ
Ｖ二指定速度 Ｔ、、：加速時間 Ｔｏ＝減速時間 Ｃ，ｘ、　　ｙ、　　ｚ方向への配分係数Ｓ１・・１−
買一（Ｐ　Ｌ−Ｐ　ｓ−）　／　ＤＳ　＋−１−−−（
Ｐ　＋−Ｐ　ｓ−）　／　ＤＳ　＋−０−−−（Ｐ　＋
−Ｐ　ｓ−）　／　ＤＤ、加減減速時の係数 Ｋ　、ｐ−（１／２）（Ｖ　／　Ｔ　ｕｐ）Ｋｄ、、−
（１／２）（Ｖ／Ｔ、、） Ｅ、最高速度Ｖ　Ｍ　１　Ｍで動作した場合の加速距離
及び減速距離 １）Ｄ≧（１／２）（Ｔ−ｐ＋Ｔ＋−）ｖ、、、・・・
台形駆動り、、−（１／２）Ｔ、、Ｖ、、、。

Ｄｄ、、−（１／２）　Ｔｄｆｉｖ−、ｔ２）Ｄ　＜　
（１／２）（Ｔ　ｕ、十Ｔａ−＞　ｖ−−−−三角駆動
り、、−Ｔ、、Ｄ／（Ｔ、、＋Ｔｄ、）Ｄａ、−Ｔｉ、
、Ｄ／（丁、、十Ｔ、ｎ）上記の各演算結果は各種デー
タ記憶装置７０に格納される。

■加速パターン発生器９１の動作 発生パターン選択器８０から現在までの移動距離Ｐ　ｂ
　、現在速度Ｖ。及び加速終了時間Ｔ。かり。

えられると、次式に従って移動距離ｐ　（ｔ）を算出す
る。

Ｐ（ｔ）＝Ｐ、−Ｋｕｐ　（ｔ’　／Ｔ、　２）＋２Ｋ
ｕｐ（ｔ３／Ｔ、、）　十Ｖｃｔ（０≦ｔ≦Ｔ、） ■等速パターン発生器９２の動作 発生パターン選択器８０から現在までの移動距離Ｐ１．
現在速度Ｖ、及び加速終了時間ＴＩ！が与えられると、
次式に従って移動距離ｐ　（ｔ）を算出する。

Ｐ（Ｌ）−Ｐ、＋Ｖ、ｔ （０≦ｔｓＴｇ） ■減速パターン発生器９３の動作 発生パターン選択器８０から現在までの移動距離Ｐ１．
現在速度Ｖ、及び加速終了時間ＴＯが与えられると、次
式に従って移動距ＭＰ（Ｌ）を算出する。

Ｐ　（ｔ）−Ｐ　Ｃ十ＫＯＮ　（ｔ　’　／　Ｔｏ　２
）２ＫＤＮ　（ｔ　’　／Ｔｏ　）　＋ｖｅｔ（０≦ｔ
≦Ｔｏ） ■経由パターン発生器９４の動作 発生パターン選択器８０から現在位置Ｐ。、経由位置Ｐ
１．経由終了位ｆｉｒ２．経由終了時間１日及び現在速
度ｖｅが与えられると、次式に従って手先位置Ｐ　（ｔ
）を算出する。

Ｐ（ｔ）−（Ｐｏ　　２Ｆ＋　＋Ｐ２　）ｔ””２　　
（Ｐｏ　　　２Ｐ＋　　＋Ｐ２）　　ｔ′　ｓ＋２　　
（Ｐ＋　　−Ｐａ　）ｔ’　　＋Ｐ。

ここて、 Ｌ’　　−（（ＶｃＴＢ　／２Ｌ）　　１１（Ｌ／ＴＲ
）　’−２（（ｖ、　Ｔｏ　／２Ｌ）−１ｔ（ｔ／Ｔ８
）　　’＋（ｖ　ｃ　Ｔｅ　　／２Ｌ）（ｔ／　　ＴＢ
　）Ｌ　””　ｌ　Ｐ　ｏ　　Ｐ　ｔ （０≦【≦ＴＢ） ■手先位置演算器１００の動作 加速パターン発生器９１２等速パターン発生器９２及び
減速パターン発生器９３から移動距離Ｐ（Ｏが与えられ
ると、次式に従って手先位置Ｐ（ｔ）を算出する。

■発生パターン選択器の動作 第２図において現在位置Ｐｓ、経由位置Ｐ。

目標位置ｔｐｉが指定され、た場合のＩＰｓからＰ２で
の動作を順を追って説明する。

ま すなわち、パラメータ演算器６０から各区間毎に次のよ
うなものが与えられる。

・区間　Ｐ、→Ｐ１ Ｄ　−ＮＩ　Ｖ−ＮＩ　Ｄｉａｌ−Ｎ　ｒ　Ｋ１１ｌｌ
−ＮＩ　Ｋ＋１＋−ＮＩ　　Ｓａｌ　−Ｎ＊　Ｄ　ｕｐ
−ＮＩ　Ｄｄ＋＋−Ｎ・区間　Ｐ、−Ｐ。

Ｄ　−Ｐｒ　Ｖ−Ｐ＋　Ｄｉａｌ−Ｆ　＋　ＫｕＰ−Ｐ
＋　Ｋｄ＊−Ｐ＋　　Ｓ　ｌ５１１１−１’ｌ　Ｄ　ｕ
＋＋−Ｐ＋　Ｄ　ｄａ−Ｐここで、 Ｄ　−ｏ　：移動距離 Ｖ　−Ｏ：指定速度 Ｄ　ｌ　＠　ｌ　−０”−０に到達するための移動距離
に−ｐ−０’加速時の係数 Ｋ　ａ　ｎ　−ｏ　’減速時の係数 Ｓ　ｌａ＋＋＋−０’　ｘ、　　５１１　　Ｚ方向への
配分係数Ｄｕｐ−Ｄ’最高速度■４１、時の加速距離Ｄ
ａ＋＋−ｏ：最高速度Ｖ□、８時の減速距離発生パター
ン選択器８０は、パラメータ演算器６０から与えられる
これらのデータに従って、以下に示すように各区間１〜
■毎に適切なパターン発生器９１〜９３を選択する。

ａ）区間Ｉ Ｐ、−〇 Ｔ、＝　　（Ｖｌ　　　Ｖｏ　）（Ｔ、−／Ｖ−Ｎ）Ｐ
、−Ｐ、＋（１／２）（Ｖｌ　　＋ｖｏ）Ｔ。

として、加速パターン発生器９１を選択する。

そして、加速パターンの発生が終了すると区間■へ移る
。

ｂ）区間■ 指定速度により経由開始点が異ならないように、最高速
度Ｖ１１．時の減速開始点まで等速動作を行う。

１）ＤＮ　　Ｐ　ｂ　＞　Ｄａ−−Ｎ（等速区間あり）
Ｖ、−Ｖ。

ＴＥ　−（ＤＮ　　　Ｐ　ｂ、　　Ｄａ−−Ｎ）　／　
ＶｌＰＣＩＩＭＰ、＋ｖＩ　ＩＩＴ６ として、等速パターン発生器９２を選択する。

そして、等速パターンの発生が終了すると区間■へ移る
。

２）ＤＮ　　Ｐｂ≦Ｄａ−ｓ（等速区間なし）Ｐｃ−Ｐ
。

として、区間■へ移る。

Ｃ）区間■ 指定速度により経由軌道が変化しないように、紅白時の
移動距離ＤＢを、 Ｄロー１１ｎ　　（Ｄ−Ｎ　　Ｐｃ　、　Ｄｕｅ−ｐ）
として、常に同じ経由開始位置Ｐ。、経由点Ｐ１゜経由
終了位置Ｐ２によって決定される曲線上を動作させる。

従って、ＤａＭ−ＮＩ　Ｄｕｅ　Ｐの関係により以下の
ような処理を行う。

１）Ｄ、−Ｎ−Ｐ、≦Ｄ　ｕｐ−Ｐ Ｖ２−Ｖ。

Ｐ、−Ｐ。

として、区間■へ移る。

２）１１．−Ｐｃ＞Ｄｕ、−ｐ はじめに、経由開始時の速度ｖ２は、 とする。従って、 Ｖ１≦ｖＰの時は、 Ｐ、−Ｐｃ Ｖ、−Ｖ。

ＴＥ　−（Ｄ−Ｎ　　Ｐ、−Ｄａ　）／Ｖ＋Ｐ、−Ｐ、
＋Ｖ、　　・Ｔ９ として、等速パターン発生器９２を選択する。そして、
等速パターンの発生が終了すると区間■へ移る。

Ｖ、＞ｖ、の時は、 ｄ−（１／４）（Ｖｌ　’　−Ｖ、　’　／Ｋｍ、−Ｎ
）＋Ｄ［ｌ とし、さらに、 Ｄ−、−Ｐｃ−ｄ＞０では、 Ｐｂ””Ｐ。

ｅＩｗｖＩ Ｔ！！　＝　（Ｄ−Ｎ　　Ｐ　ｔ　　ｄ）／ＶＩＰｃ−
Ｐｂ　＋ｖ、　　・ＴＩ！ として、等速パターン発生器９２を選択する。そして、
等速パターンの発生が終了すると再び区間■へ移る。一
方、 ＩＩＮ−Ｐｃ−ｄ≦０では、 Ｖ、−Ｖ。

■２″′ｖＰ Ｔａ　−（ＶＩ　　Ｖ２　／Ｖ−Ｎ）　　（Ｔ、１ｆｉ
）Ｐｄ−Ｐｃ＋（１／２）（ＶＩ　＋Ｖ２）Ｔ。

として、減速パターン発生器９３を選択する。そして、
減速パターンの発生が終了すると区間■へ移る。

ｄ）区間■ まず、経由後区間の速度ｖ４を算出する。

そして、算出された速度ｖ４を用いて経由終了とし、 ＶＢ　　−ｍｉｎ（Ｖｚ　、ＶＢ　） とする。

さらに、 Ｐ　ｏ　＝　Ｐ　３　＋　Ｄ　Ｂ　・＄　ｌ＊＋＋＋−
ＮＰ　２　＝　Ｐ　１　＋　Ｄ　Ｂ　・＄　ｌｓｎ＋−
ＰＴｏ　””４　・ＤＢ　／　（Ｖ２　＋Ｖ３　）Ｐ、
　　−Ｄｌｌ として、経由パターン発生器９４を選択する。

そして、経由パターンの発生が終了すると区間Ｖへ移る
。

ｅ）区間Ｖ 経由後の動作を行うために、目標位置をＩＰ、からＰｓ
に、Ｐ５からＰ１ヘコピーする。さらに、各種の係数０
−Ｐを０−Ｎにコピーする。

１）Ｖ、　−Ｖ４ Ｐ、−Ｐ。

として、区間■へ移る。

２）Ｖ、＜Ｖ４ ｃ−ＶＢ Ｔ−＝　（Ｖ４　　ＶＢ　）（Ｔ、、／Ｖ−Ｎ）Ｐｂ　
　　−Ｐ、　’　　＋（１／２）（Ｖ４　　＋Ｖ３　）
　　Ｔｕとして、加速パターン発生器９１を選択する。

そして、加速パターンの発生が終了すると区間■へ移る
。

ｆ）区間■ 区間■と同様な処理を行い、処理後区間■へ移る。

ｇ）区間■ １）Ｄ−、−Ｐ、’　　＞（１／４）（Ｖ４　’　／Ｋ
ＤＮ−Ｎ）ｐ　、　’　ｍ　ｐｃ ■ゎ−■４ Ｔ、−ＩＤ−Ｎ−Ｐｃ （１／４）（Ｖ４　’　／　ＫＤＮ−Ｎ）ｌ／　Ｖ４Ｐ
、　　＝Ｐｈ’　＋Ｖ４　　・Ｔｐ として、等速パターン発生器９２を選択する。

そして、等速パターンの発生が終了すると再び区間■へ
移る。

２）Ｄ−、−Ｐｃ’　　＞（１／４）（Ｖ４　’　／Ｋ
ＤＮ−Ｎ）■、−ｖ４ Ｔａ　”＝　（Ｖ４　／　ＶＮ　）　Ｔａ。

として、減速パターン発生器９２を選択する。

そして、パターン発生終了後停止となる。

なお、パターン発生器の選択順序は必ずしも第２図のよ
うになるものではなく、現在位置ＰＳ。

経由位置ＰＩ、目標位置ＰＥの関係、指定速度、加減速
時間等に応じて変化することもある。

また、以上の説明では直交座標系（ｘ、ｙ、ｚ）の補間
について述べたが、ｎ個の関節を持つロボットマニピュ
レータの関節補間についても同様である。

また、経由点は１つに限るものではなく、複数の経由点
を指定することもてきる。

〈発明の効果〉 以上詳細に説明したように、本発明によれば、速度に依
存することなく軌道が決定できるロボット制御装置を提
供することができ、ロボットマニピュレータに対する教
示が容易になる。

これにより、例えば低速で経由軌跡を教示したものを高
速で動作させて、も従来のように経由軌跡がより内側に
なって教示した軌跡が使えなくなったり、障害物との接
触を生じることはない。

【図面の簡単な説明】

第１図は本発明の一実施例を示す構成ブロック図、 第２図は第１図の動作説明図、 第３図は従来の装置の動作説明図である。 １０・・・指令−データ受信器 ２０・・・指令認識部 ３０・・・ロボットマニピュレータ ４０・・・位置検出器　　　　５０・・・現状認識部６
０・・・パラメータ演算器 ７０・・・各種データ記憶装置 ８０・・・発生パターン選択器 ９０・・・軌道パターン発生部 ９１・・・加速パターン発生器 ９２・・・等速パターン発生器 ９３・・・減速パターン発生器 ９４・・・経由パターン発生器 １００・・・手先位置演算器

Claims (1)

1. 【特許請求の範囲】 ロボットマニピュレータの現在位置を記憶し、外部より
   指定された経由点を経由して外部より指定された目標点
   に前記ロボットマニピュレータを移動させるロボット制
   御装置において、 ロボットマニピュレータの現在位置を記憶する手段と、 外部より指定される経由点の位置を受信し記憶する手段
   と、 外部より指定される目標点の位置を受信し記憶する手段
   と、 ロボットマニピュレータの移動に関連した移動距離、速
   度等のパラメータを算出する手段と、ロボットマニピュ
   レータを加速動作させる加速パターンの軌道を発生する
   手段と、 ロボットマニピュレータを等速動作させる等速パターン
   の軌道を発生する手段と、 ロボットマニピュレータを減速動作させる加速パターン
   の軌道を発生する手段と、 ロボットマニピュレータを経由動作させる経由パターン
   の軌道を発生する手段と、 動作開始後の経過時間、ロボットマニピュレータの移動
   距離及び移動速度から前記４つの軌道発生手段のいずれ
   かを選択するパターン選択手段と、ロボットマニピュレ
   ータの移動距離からロボットマニピュレータの位置を算
   出する手段と、ロボットマニピュレータの位置から各関
   節の位置を算出する手段とを具備し、 前記パターン選択手段は最高動作速度時の減速停止距離
   と経由後の加速距離の短い方を経由距離として演算する
   手段を有し、 前記経由パターン軌道発生手段は経由点より経由距離前
   の点と経由点と経由点より経由距離後の点の３点に基づ
   いて４次のベジェ曲線を生成すると共に該ベジェ曲線上
   を時間の４次式で補間する手段を有し、 経由開始点と経由終了点が指定された動作速度に依存す
   ることなく決定され、経由軌道上で経由前の動作速度か
   ら経由後の動作速度へ加速度の連続性を保ったままで変
   化させることを特徴とするロボット制御装置。