JPS62162109A - 多軸ロボツトの速度制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、多軸ロボットの速度制御方法に関し、更に詳
しくは、安全性を確保しつつ高速で多軸ロボットを動作
させるための速度ｖＩ御方法に関する。

従来技術 多軸ロボットにおける手先部の移動速度は、各軸速度を
合成した速度である。

しかし、どのように合成が行われるかは、移動する空間
によって異なるため、移９Ｊノ速度が与えられζもこれ
から各軸速度を知ることは困難である。

特に、多関節型ロボットにおいては、ｒｉ雑に合成され
るため、手先部の移動速度から各軸速度を知ることは大
変困趙である。

ところで、各軸にはそれぞれ許容される最セＪ速度Ｗ−
，が定められＣおり、各軸速度がその！ｉｋ高速度Ｗｉ
を越えることはできない。

もし一つの軸にその軸の許容最高速度Ｗｉつを越える軸
速度が与えられた場合には、目標値に追従不能の異常を
発生したり１、所定の作業線を逸脱して危険性を増ずな
どの問題を生しる。

しかし、前述のように、移動速度から各軸速度を得るこ
とは困難であるから、成る移動速度を与えたときに、各
軸速度が各々の最高速度ＷＬ、、、を越えないことを容
易にｇ！認することができず、実際に勤かしてみて異常
が発生するか否か試してみる外なかった。

そこで、一般的には、安全を見込んで移動速度をかなり
低い速度に設定することが行われている。

ｉ足来子支？、１にテのトｎ題点 安全を見込んで移動速度をかなり低めに設定するという
ことは、多軸ロボットの持つ能力を十分活用出来ないと
いうことであり、タクトタイムが増加するなどのロスを
生しさせることとなる。

発明の目的 本発明の目的とするところは、各軸速度が許容される最
高速度Ｗニーを越えない範囲で動作可能な最高速度を的
確にＩ）るごとか出来る多軸ロボットの速度制御方法を
提供することＧこある。

発明の構成 本発明の多軸ロポノＦ・の速度側１ｆｆ１１方法は、２
点間を移動するときの多軸ロボ−／　トの第１軸（１−
１，２，・・・）の位置変化量Δαｉを第１軸に許容さ
れた最高速度Ｗｉ−で除算した商τ□の最高値τ−で前
記位置変化量Δαｉを除算した値Ｗよを第１軸の動作可
能最高速度とすることを構成上の特徴とするものである
。

なお、上記発明は、結果として上記Ｗ□を第１軸の動作
可能最高速度とすれば足り、そのＷ□の算出の過程を限
定するものではない。

そこで、そのＷ□の算出の過程を限定した多軸ロボット
の速度制御方法として、教示された２点での位置姿勢と
移動速度とから１制御周期を毎に移動する補間点を得、
次いで各補間点間を移動するときの多軸ロボットの第１
軸（ｉ＝１．２．・・・）の速度Ｗ工を得、その速度Ｗ
Ｌがすべて第１軸に許容された最高速度ＷＬｍｘより小
ならその速度Ｗ工を多軸ロボットへの指示速度とし、一
方、速度Ｗ□の中にＷｉ−より大なる速度があれば、そ
の大なる速度のうち最も小なる速度を選び出し、それが
第ｍ軸の速度ＷｉならＷｉ、ＸＷｉ／Ｗ。

＝ｗ１を求め、この値Ｗ工を第１軸の動作可能最高速度
とすることを特徴とする発明がさらに提供される。

後者の発明は、結果として前者の発明と合致する。

作用 説明の都合上、３軸ロボツトをセ、定し、その第１軸、
第２軸、第３軸について許容される最高速度をそれぞれ
ｗ、、、ｗ、ｍ＋　Ｗ：１．ｘとする。

また、２Ｊｉｌｉ間点間を３軸ロボツトの手先ｒａ＋が
移動するときの第１軸、第２軸、第３軸の位置変化量を
それぞれΔαｉ．Δαつ、Δα〕とする。

すると、τ・＝Δαｉ　／　ｗ　ｌ□８．τり＝Δα２
／Ｗ２　、、　　τ３−Δα３／Ｗ］−となる。

ｒｌ、τ２．τ〕のうちの最大値をｒ、とすると、各軸
の動作可能最高速度は、ｗｌ　−Δαｉ／τｍ、Ｗ２　
−Δα２　／τ＋ａｘ、Ｗ３　　＝Δαｉ／τｉとなる
。

仮に、τｉ　〉τ２〉τ〕が成立したとするならば、τ
ｉが最大値で１となるから、Ｗｉ−Δαｉ／τｉ．Ｗ２
＝Δα２／τＩ　、　Ｗ３　＝Δα３／τｉとなる。

ここで、 ■Ｗｉ＝＝Δαｉ／τオ＝Ｗｉ１であり、第１軸に許容
される最高速度である。

０ｗ２　＝Δα２／ｒ、＜Δα２　／　τ２　＝　ｗ２
１であり、第２軸に許容される最高速度を越えない。

■Ｗ〕−Δα３　／　ｒ　＋　＜Δα３　／　τ３　＝
　ｗ３＋＋ｗであり、第３軸に許容される最高速度を越
えない。

■各軸は共にｒ、時間でそれぞれΔαｉ．Δαり１　Δ
α３だけ位置変化する。

したがって、Ｗｌ、Ｗ２．Ｗ３は、各軸について許容さ
れる最高速度ｗ、　、ｌ　ｗ、　ＩＩＩＸ＋　ｗ３１ａ
ｌ＋をどの軸についても越えないで且つ２点間を逸脱し
ないで移動することがごきる最高の動作可能速度である
。

以上は２７ｆｔ間点間についての説明であるが、これを
２教示点間に適用する場合は、例えば２＠示点間におけ
る各補間点間についての（ｗ、、ｗ２、Ｗｉ）の築合の
中で最小の値（ｗｌ、ｗり、Ｗｌ）１．Ｉを採用すれば
よい。また、これらの各軸速度から、合成した教示点間
動作可能最高速度が求められる。

奥方Ｓ１イタリ 以下、図に示す実施例に基づいて本発明を更に詳しく説
明する。ここに第１図は本発明の速度制御方法を実施す
る多軸ロボット装置の構成外観図、第２図は第１図に示
す多軸ロボット装置による速度算出処理の要部フローチ
ャート、第３図は第１図に示す多軸ロボント装ヱによる
再補間処理の要部フローチャート、第４図は速度算出処
理の他の例の第２図相当図である。なお、以下の実施例
が本発明を限定するものではない。

第１図に示す多軸ロボット装置１０は、多関節型ロボッ
ト１を制御装置２により駆動するもので、制御の内容は
制御盤２３もしくはリモートコン上ローラ３からオペレ
ータが教示する。

説明の都合上、多関節型ロボットｌは、α、〜α１の５
つの自由度を有し、制御周期を毎の定時間制御をなされ
るものとする。

始点Ａから終点Ｂまで一定速度■で多関節型ロボットｌ
を駆動したい場合は、オペレータは、始点Ａでの位置姿
勢（Ｘ、、Ｙ、、Ｚ、、　　θ８．φ。

）および終点Ｂでの位置姿勢（Ｘ、、ＹＩ、、ｚ、。

、θｂ、φＩ、）および希望の移動速度■を入力する。

上記入力が行われると、制御装置２は、第２図に示すよ
うに処理（速度算出Ｉ）を開始する。

まず、教示された移動速度■と制御周期ｔとから仮移動
量Δｌを求める。すなわち、速度Ｖで移動すると仮定し
たときの１制御周期当たりの移動量ΔｉをΔｇ−ｖｔに
より求める（Ｓｔ）。

次に、現点Ｐ、ｌよりΔｌだけ進んだ仮次点ｃｉｒ１．
。

の位置姿勢を求める（Ｓ２）。

最初の現点Ｐ、は始点Ａであり、仮次点Ｑ、の位置姿勢
は次のよ）になる。

Ｘｑ　　”Ｘｉ　　”　　（Ｘｂ　　−Ｘ＠　）　　Δ
Ｉ／まただし、ｌは始点Ａから終点Ｂまでの長さである
。

他の位Ｅ座標ｙｑ、ｚｑおよび姿勢θ１．ｙ９について
も同様に求める。

現点ＰＴｌおよび仮次点ＱＴｌヤ、の位置姿勢が得られ
たら、その位置姿勢を逆変換し、各点に対応する関節角
αＬｎおよびα□ｎ＋１を求める（３３）。

多関節型ロボット１が５つの自由度を有しているから、
　（αＩｎ＋　　α２ｎ、α３Ｔ１＋　　α４ｎ＋　　
αち、）および（ｙｌ　ｎｉｌ　＝　α２　＋’ｌ＋１
　、α、ｎや１．α、ｎ＋。

、α、ｎや、）を得ることになる。

次に、各関節角の仮角変位量Δαｉ□を、Δαｉ。

−α、ｎ＋１−α、。により求める（Ｓ４）。

得られた仮角変位量Δαｉｎは、教示された移動速度Ｖ
で１制御周期ｔだけ移動した場合の各関節角の変位量で
ある。従って、仮関節速度ＷＬｒｌは、ＷＬｎ＝Δαｔ
　ｎ　／　Ｌにより求められる（Ｓ５）。

次に、仮関節速度Ｗ□。が、各関節に許容された最高速
度Ｗｉ−より小さいか否かを比較する（３６）、すなわ
ち、Ｗｌ　ｎ　＜Ｗｌ　−＋　Ｗ２　ｎ　＜ｗ、　ＩＩ
ＩＸ＋　　Ｗｌ　ｙｌ　＜　Ｗｌ　１＋　Ｗｉ　ｙｌ　
＜　Ｗｉユ、Ｗｉ。＜Ｗｉ□のそれぞれを比較演算する
。

全ての仮関節速度Ｗｉ。が対応する許容最高速度Ｗｉよ
り小さければ、教示された速度Ｖで多関節型ロボット１
を駆動しても何ら問題がない。

そこでステップＳ７からステップＳ８へ分岐し、仮次点
Ｑｎ＋１を次点Ｐｎ＋１　とする（Ｓ８）。また、教示
された移動速度Ｖを実際に多関節型ロボット１に指示す
る移動速度ｖｎとする（Ｓ９）。

一方、仮関節速度Ｗｉ、のいずれか１つでも対応する許
容最高速度Ｗｉ−を越えておれば、教示された移動速度
Ｖで多関節型ロボット１を駆動するのは好ましくないか
ら、実際に動作させ得る最高の速度を演算により算出し
なければならない。

そこで、ステップＳ７よりステップＳＩＯへ分岐する。

ステップＳＩＯでは、各関節をその関節に許容された最
高速度Ｗｉで仮角変位量Δαｉｏだけ駆動したときの所
要時間すなわち最短角変位時間τ、。を算出する。すな
わち、τ□、＝Δαｔｎ／Ｗｉ．を演算する。そして、
各関節について得られたτＩｎ・　τ２ｎ・　τ３ｎ・
　τ４ｎ・　τ５ｎのうち最大の値をτ−６とする（Ｓ
ＩＯ）。

これは、現点Ｐｎから仮次点Ｑｎヤ、へ移動するのに、
τｓｕｎ以上の時間を与えなければ、いずれかの関節で
その許容最高速度Ｗｉ−を越えてしまうことを意味して
いる。

次に、動作可能最高角変位速度Ｗｉ．を、ＷＬｎ−Δα
□、ｌ／τ、、ｌにより算出する（Ｓｉｔ）。

これにより得られたＷｌｙ、、Ｗ２ｎ１ｗ３ｎ＋Ｗ◆ｎ
＋　Ｗｆｉｎのいずれかは、その軸の許容最高速度であ
り、その他は仮関節速度ＷＬｎの比率に応じて減速した
関節速度となる。

次に、動作可能最高角変位速度ｗｉｎで１制御周期ｔの
間に変位する各変位量δαｌｎをδαＬｎ＝ｗ、ｎｔに
より算出する（Ｓ１２）。

また、その各変位量δα、。たけ現関節角αＬｎより進
んだ次関節角αＬｎ＋＋をαＬＴｌ＋１＝α、。

＋δαｔｎより算出する（Ｓ１３）。

この次関節角α、ｎや、をｊｉＪ］変換すれば動作可能
最高角変位速度Ｗｉ．ｌで各関節を駆動したときの制御
周Ｑｔ後の位置姿勢が分かる。すなわち次点Ｐｙ１＋１
の位置姿勢を算出できる（Ｓ１４）。

現点Ｐ。と次点Ｐｎ＋＋が分かれば、その間の移動時間
は１制御周期ｔだから、移動速度Ｖ。を算出することが
できる（Ｓ１５）。

第１回目の上記ステップ３１〜５１５の処理により最初
の補間点Ｐ２および始点Ａからその補間点Ｐ２までの移
動速度■１が得られることになる。

Ｐ２が求まれば、これを現点として上記ステップ３１〜
Ｓ１５を行うことによりＰ〕およびＰ２からＰ３への移
動速度■２を求めることができる。

以下同様にして、補間点Ｐ、、Ｐ５．・・・および移動
速度ｖ〕２　ｖ＋、・・・が得られるのである。

移動速度ｖＩ、ｖ２．・・・は、教示された移動速度■
が各関節の許容最高速度Ｗえ−に適合するものであれば
、その教示された速度■となり、教示された速度Ｖが適
合しなければ動作させ得る最高の速度まで減速された値
となる。

さて、上記により得られた補間点間の移動速度ｖ、、ｖ
２．・・・が一定値になるとは限らない、しかし、始点
Ａから終点Ｂまで一定の速度を保ちたいならば、第３図
に示す如く再補間処理を行って新補間点を算出すればよ
い。

すなわち、第３図に示すように、前記ステップ８１〜３
１５を繰り返すことによって得られた移動速度Ｖｌ、Ｖ
ｌ、・・・を相互に比較し、その最小の値をＶｌ、、と
する、（Ｓ２１）。

どの補間点間においても各関節の速度が許容最高速度Ｗ
□−を越えないようにするためにはこの最小の値Ｖ１１
．を採用すればよい。

そこで、この最小の値ｖ１１．を採用したときの１制御
周期を当たりの移動量δｅを、δ６＝ｖ、、、ｔにより
算出しく５２２）、その移動量δｌ毎の区間に始点Ａか
ら終点Ｂまでを分割し、新補間点を求めればよい（Ｓ２
３）、言うまでもな（、移動速度はｖ、１．一定である
。

移動速度Ｖの教示を行わずに、はじめから多関節型ロボ
ット１の動作可能な最高速度で駆動しようとする場合、
オペレータは始点Ａと終点Ｂでの位置姿勢のみを教示し
、制御装置２に第２図の破線の如き処理（速度算出■）
を行わせる。

ただし、Δｉとして、たとえば、Ｉｉ！線的に移動する
とみなされる最も長い２点間距離や１制御周期ｔで移動
しうると考えられる距離より少し大きめの距離を設定す
る。

さて、以上の実施例は、始点ＡからΔβづつ進みながら
速度算出を行う処理であったが、第４図に示すものは、
予め始点Ａから終点Ｂまでを分割し、各々について移動
速度を算出する実施例である。

まず、教示された速度■と制御周期ｔとから仮移動量Δ
ｌを求める（Ｓ３１）。

次に始点へから終点ＢまでのΔβ毎の区間に分割して補
間点Ｐｎを求める（Ｓ３２）。

次に、点Ｐｎと点Ｐ　ｎ＋１での位置姿勢を逆変換して
それぞれの関節角αＬｒｌ及びαｌｎｉを求める（Ｓ３
３）。

次に、仮角変位量Δαｔｎを求め（Ｓ　３４）、仮関節
速度ＷＬｎを求め（Ｓ３５）、仮関節速度Ｗ□。が各関
節に許容された最高速度Ｗｉより小さいか否か比較する
（Ｓ３６）。これらのステップＳ３４．３５．３６は、
前記ステップＳ４゜５．６と同じである。

全ての仮関節速度Ｗｉｒｌが対応する許容速度ＷＬｗｍ
より小さければ、教示速度Ｖを移動速度ｖｎとする（Ｓ
３９）。

いずれかの仮関節速度Ｗｌｎが対応する許容最高速度Ｗ
ｉ−より大きければ、最短角変位時間τＬｎを求め、各
関節についてのτ、。のうち最大の値をτ−１とじ（５
４０）、そのτ−１で仮角変位量Δαｔｎを除算して動
作可能最高角変位速度ＷＬ１を算出する（Ｓ４１）、こ
れらのステップ８４０．４１は、前記ステップＳＩ０，
１１と同じである。

求められた動作可能最高角変位速度ＷＬ□に制御時間ｔ
を乗じて角変位量δαｔｎを求め（Ｓ４２）、現関節角
αＬｎにその角変位量δα、ｎを加えて次関節角αＬｎ
＋１を求める（Ｓ４３）。これらのステップＳ４２゜４
３は前記ステップＳ１２、Ｓ１３と同じである。

次閏節角αｔｎや１を順変換すれば、ｌ制御周期ｔで進
め得る点Ｅ、ｌの座標が求められる（Ｓ４４）。

次に点Ｐｎと点Ｅ、ｌの座標から移動速度■。を算出す
る（Ｓ４５）。

上記ステップ５３１−３４５を各補間点間について繰り
返せば、各補間点間の移動速度Ｖ。が求められる。そこ
で、第３図に示す再補間処理を行えば、始点Ａから終点
Ｂまでほぼ一定の高速度で多関節型ロボット１を駆動す
るためのデータが得られる。

発明の効果 本発明によれば、２点間を移Ωノするときの多軸ロボッ
トの第１軸（＋＝１．２．・・・）の位置変化量Δαｉ
を第１軸に認められた最高速度Ｗｉ−で除算した商τ、
の最高値τ−で前記位置変化量Δαえを除算した値〜１
．を第１軸の動作可能最高速度とすることを特徴とする
多軸ロボットの速度制御方法が提供され、これにより多
軸ロボットを動作させ得る最高速度が自動的に得られる
ので、多軸ロボットを高効率で駆動することができる。

【図面の簡単な説明】

第１図は本発明の速度制御方法を実施する多軸ロボット
装置の構成外観図、第２図は第１図に示す多軸ロボット
装置による速度算出処理の要部フローチャート、第３図
は第１図に示す多軸ロボット装置による再補間処理の要
部フローチャート、第４図は速度算出処理の他の例の第
２図相当図である。 （符号の説明） ■・・・多関節型ロボット　　　２・・・制御装置２８
・・・制御盤 ３・・・リモートコントローラ １０・・多軸ロボット装置。

Claims (1)

1. 【特許請求の範囲】 １、２点間を移動するときの多軸ロボットの第ｉ軸（ｉ
   ＝１、２、・・・）の位置変化量Δα＿ｉを第ｉ軸に許
   容された最高速度Ｗ＿ｉ＿ｍ＿ａ＿ｘで除算した商τ＿
   ｉの最高値τ＿ｍ＿ａ＿ｘで前記位置変化量Δα＿ｉを
   除算した値ｗ＿ｉを第ｉ軸の動作可能最高速度とするこ
   とを特徴とする多軸ロボットの速度制御方法。 ２、教示された２点での位置姿勢と移動速度とからＩ制
   御周期ｔ毎に移動する補間点を得、次いで各補間点間を
   移動するときの多軸ロボットの第ｉ軸（ｉ＝１、２、・
   ・・）の速度Ｗ＿ｉを得、その速度Ｗ＿ｉがすべて第ｉ
   軸に許容された最高速度Ｗ＿ｉより小ならその速度Ｗ＿
   ｉを多軸ロボットへの指示速度とし、一方、速度Ｗ＿ｉ
   の中にＷ＿ｉ＿ｍ＿ａ＿ｘより大なる速度があれば、そ
   の大なる速度のうち最も小なる速度を選び出し、それが
   第ｍ軸の速度Ｗ＿ｍならＷ＿ｍ＿ｍ＿ａ＿ｘ×Ｗ＿ｉ／
   Ｗ＿ｍ＝ｗ＿ｉを求め、この値ｗ＿ｉを第ｉ軸の動作可
   能最高速度とすることを特徴とする多軸ロボットの速度
   制御方法。