JP3204042B2 - ロボットの軌道の生成装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、入力された教示点の間
を補間して目標軌道を生成するロボットの軌道の生成装
置に関する。

【０００２】

【従来の技術】近年、ロボットがいろいろな作業現場で
活用されているが、その動作の円滑さと高速性が重要で
ある。

【０００３】以下、従来のロボットの軌道の生成装置に
ついて説明する。ロボットが指定された作業を実行する
ために通過する軌道は、通過するべき複数個の教示点が
操作者によってあらかじめ登録され、その教示点の間の
通過点を直線や円弧、放物線で補間することにより決定
される。この教示点の情報を与える教示点データは、教
示点の位置、姿勢の座標と、その教示点間を通過する速
度などが指定される。

【０００４】１つの教示点からつぎの教示点までの間を
単純に直線運動をさせる場合には、教示点を結ぶ直線軌
道を教示点間の補間演算により求める。図９はあらかじ
め与えられた教示点データにしたがい、全ての教示点間
を直線で移動する場合の移動軌道を示している。教示点
ＴＰｋと教示点ＴＰｋ＋１間の距離を教示点間距離ｌｋ
とし、教示点ＴＰｋと教示点ＴＰｋ＋１間の指定移動速
度をＶｋとする。またθｋを教示点ＴＰｋにおける折れ
線の折れ角とする。このような折れ線軌道でロボットを
動作させると、折れ角θｋが大きい場合に折れ線の節点
で先端部の速度が大きく変化するため、先端振動発生の
原因となる。また、節点を通過する時のロボットの関節
軸には大きな加速度が作用するため、減速機などロボッ
トの構成部品の寿命の短縮や破壊などの原因となる。

【０００５】このような問題を回避するため、折れ線の
節点でロボットを一旦停止させる手段や、特開平１−２
７４４３号公報や特開平４−１１１００６号公報が開示
しているように、折れ線の節点を経由せず、折れ線を構
成する２つの線分を滑らかな曲線で接続して節点を内回
りする軌道を生成する手段などがある。

【０００６】

【発明が解決しようとする課題】このような従来のロボ
ットの軌道の生成装置において、折れ線の各節点で一旦
停止するように動作させる上記の手段では、一定の速度
で連続的に動作させる場合のおよそ２倍の動作時間を必
要とする。

【０００７】また、折れ線の節点を内回り刷る軌道で動
作させる上記の手段でも、教示点にはさまれた補間区間
毎に移動速度が定められているため、教示点の前後で指
定速度が異なる場合には速度に不連続変化が発生し振動
発生の原因となる。

【０００８】本発明は上記の課題を解決するもので、教
示点での速度を連続にでき、加速度許容値以下で最短に
動作させ、先端の振動を発生させないロボットの軌道の
生成装置を提供することを目的とする。

【０００９】

【課題を解決するための手段】本発明は上記の目的を達
成するため、請求項１にかかわる本発明のロボットの軌
道の生成装置は、ロボットの先端部が移動する軌道の基
準を与える教示点と、その教示点に基づく軌道の形状を
指定する移動命令と、軌道上を先端部が移動する速度を
指定する移動速度と、教示点と軌道との間の誤差の最大
値を指定する許容経路誤差とを備えた教示点データを入
力とし、連続して存在する３点以上の教示点データによ
り指定された折れ線形状の経路の前記折れ線の節点を経
由せずに内回りする軌道の内回りを開始する点である内
回り開始点の位置と姿勢と、終了する点である内回り終
了点の位置と姿勢をロボットの動作前にあらかじめ計算
し、教示点データに登録する内回り開始終了点設定手段
を有し、放物線補間する際の先端移動速度として、単位
時間あたりのロボットの各関節軸の回転速度の変化量が
各関節軸毎に定められた最大加速度から定まる許容変化
量を超えることのない上限移動速度を、ロボットの動作
前にあらかじめ計算し、教示点データに登録する上限移
動速度設定手段を有し、内回りする区間が２つ以上連続
して存在する場合には、ある放物線軌道の終了点での先
端の到達速度から、次の放物線軌道の開始点での先端移
動速度まで、その間の直線移動区間を関節軸の許容角加
速度値以下で加速または減速移動すれば加速または減速
できるようそれぞれの放物線軌道の上限移動速度をあら
かじめ調整し教示点データへ登録する速度連続化手段を
有し、前記内回り開始終了点設定手段と前記上限移動速
度設定手段と前記速度連続化手段によって新たに登録し
た教示点データにしたがって折れ線形状の経路を内回り
する軌道を放物線軌道によって生成する放物線補間演算
手段がある軌道演算手段を有する構成とする。

【００１０】また請求項２にかかわる本発明のロボット
の軌道生成装置の内回り開始終了点設定手段は、内回り
開始点の位置と内回り終了点の位置を計算し、教示点デ
ータに登録する位置設定手段と、内回り開始点と内回り
終了点の手首姿勢を計算し、教示点データに登録する姿
勢設定手段とで構成する。

【００１１】また請求項３にかかわる本発明のロボット
の軌道生成装置の上限移動速度設定手段は、前記軌道演
算手段の構成要素である放物線補間演算手段を模擬演算
する放物線補間模擬演算手段と、各関節軸の回転速度の
単位時間あたりの変化量が内回り区間の全区間にわたり
各関節軸の最大加速度から定まる前記許容変化量を超え
ないように教示点データ内の移動速度をあらかじめ計算
し教示点データに登録する速度決定手段とで構成する。

【００１２】また請求項４にかかわる本発明のロボット
の軌道生成装置の速度連続化手段は、ロボットが停止し
ている最初の教示点から内回り開始終了点設定手段によ
って登録された最初の内回り開始点までの起動区間を、
各関節軸のうち少なくとも一つの軸が各関節軸毎に定め
られた最大加速度で加速しながら移動した場合に実現可
能な速度を計算し、前記上限移動速度設定手段によって
登録された上限移動速度とを比較し、どちらか小さい値
の速度を最初の内回りの区間の上限移動速度として、教
示点データに再登録する起動区間速度設定手段とで構成
する。

【００１３】また、請求項５にかかわる本発明のロボッ
トの軌道生成装置の速度連続化手段は、内回り開始終了
点設定手段によって登録された最後の内回り区間の内回
り終了点から、教示点データの最後の教示点までの停止
区間を、各関節軸のうち少なくとも一つの軸が各関節軸
毎に定められた負の最大加速度で減速しながら移動した
場合に、前記最後の教示点においてロボットの先端移動
速度が０となるようなロボットの先端移動速度を計算
し、前記上限移動速度設定手段によって登録された最後
の内回り区間の上限移動速度と比較し、どちらか小さな
値の速度を最後の内回り区間の上限移動速度として教示
点データに再登録する停止区間速度設定手段とで構成す
る。

【００１４】また請求項６にかかわる本発明のロボット
の軌道生成装置の速度連続化手段は、ある内回り区間を
第１の内回り区間とし、前記第１の内回り区間の次の内
回り区間を第２の内回り区間とし、第１の内回り区間の
上限移動速度を初速度として、第１の内回り区間の内回
り終了点と第２の内回り区間の内回り開始点にはさまれ
た接続区間を、各関節軸のうち少なくとも一つの軸が関
節軸毎に定められた最大加速度で加速しながら移動、ま
たは一定速度で移動、または減速しながら移動すること
により第２の内回り区間の上限移動速度が達成できるよ
うに、前記第１の内回り区間の上限移動速度と前記第２
の内回り区間の上限移動速度を再計算し、前記第１の内
回り区間の終了点と前記第２の内回り区間の開始点の距
離が０であるとき、前記第１の内回り区間の上限移動速
度と前記第２の内回り区間の上限移動速度の内、小さな
値の方を、両者の速度として設定する接続区間速度設定
手段とで構成する。

【００１５】また請求項７にかかわる本発明のロボット
の軌道生成装置の軌道演算手段は、２つの教示点間を直
線補間する直線補間演算手段と、折れ線を構成する２つ
の線分を放物線により滑らかに接続して節点を内回りす
るロボットの先端軌道を生成し、折れ線の節点にあたる
教示点の前後において、手首関節軸の回転速度の単位時
間あたりの変化量が許容変化量以下となるように手首関
節軸の回転速度の単位時間あたりの変化量を自動調整す
る放物線補間演算手段とで構成する。

【００１６】

【作用】ロボットの先端部が移動する折れ線軌道の基準
位置、形状、移動速度、教示点からの許容経路誤差を備
えた教示点データに対し、許容経路誤差の範囲内を内回
りする放物線軌道の開始点と終了点の位置を登録し、放
物線軌道を移動する際の関節軸角加速度が許容値以下と
なる最大の移動速度を登録し、内回りする区間が２つ以
上連続して存在する場合、ある放物線軌道の終了点から
次の放物線軌道の開始点までの直線移動区間を関節軸の
許容角加速度値以下で加速または減速移動できるよう放
物線軌道の最大移動速度を再計算し登録し、ロボットの
運転時にはこれら新たに登録された教示点データにした
がって軌道の生成計算をすることで、折れ線の節点を放
物線軌道で内回りし、直線部と放物線部の接続点での先
端移動速度および関節軸角速度の不連続変化をなくし、
ロボットの先端振動を抑制すると共に、ロボット運転時
の関節軸の角加速度を許容値以下に制限する条件下で最
短の作業時間を実現する。

【００１７】また、内回り開始終了点設定手段により、
教示点の位置と許容経路誤差の値より内回り開始点の位
置と手首の姿勢、内回り終了点の位置と手首姿勢を決定
し、教示点データに登録する。

【００１８】また、上限移動速度設定手段は、放物線補
間演算を、模擬演算し、各関節軸の回転速度の単位時間
あたりの変化量が内回り区間の全区間にわたり各関節軸
の最大加速度から定まる前記許容変化量を超えないよう
に教示点データ内の移動速度をあらかじめ計算し教示点
データに登録する。

【００１９】また、速度連続化手段は、ロボットが停止
している最初の教示点から最初の内回り開始点までの起
動区間を、各関節軸のうち少なくとも一つの軸が各関節
軸毎に定められた最大加速度で加速しながら移動した場
合に実現可能な速度を計算し、上限移動速度と比較し、
どちらか小さい値の速度を最初の内回り区間の上限移動
速度として、教示点データに再登録する。

【００２０】また、速度連続化手段は、最後の内回り区
間の内回り終了点から、教示点データの最後の教示点ま
での停止区間を、各関節軸のうち少なくとも一つの軸が
各関節軸毎に定められた負の最大加速度で減速しながら
移動した場合に、前記最後の教示点においてロボットの
先端移動速度が０となるようなロボットの先端移動速度
を計算し、最後の内回り区間の上限移動速度と比較し、
どちらか小さな値の速度を最後の内回り区間の上限移動
速度として教示点データに再登録する。

【００２１】また、速度連続化手段は、ある内回り区間
を第１の内回り区間とし、前記第１の内回り区間の次の
内回り区間を第２の内回り区間とし、第１の内回り区間
の上限移動速度を初速度として、第１の内回り区間の内
回り終了点と第２の内回り区間の内回り開始点にはさま
れた接続区間を、各関節軸のうち少なくとも一つの軸が
関節軸毎に定められた最大加速度で加速しながら移動、
または一定速度で移動、または減速しながら移動するこ
とにより第２の内回り区間の上限移動速度が達成できる
ように、前記第１の内回り区間の上限移動速度と前記第
２の内回り区間の上限移動速度を再計算し、前記第１の
内回り区間の終了点と前記第２の内回り区間の開始点の
距離が０であるとき、前記第１の内回り区間の上限移動
速度と前記第２の内回り区間の上限移動速度の内、小さ
な値の方を両者の速度として設定する。

【００２２】また、起動演算手段は、２つの教示点間を
直接補間し、３つの教示点で構成される折れ線の節点を
経由せず、折れ線を構成する２つの線分を放物線により
滑らかに接続して節点を内回りするロボットの先端軌道
を生成し、折れ線の節点にあたる教示点の前後におい
て、手首関節軸の回転速度の単位時間あたりの変化量が
前記許容変化量以下となるように自動調整する。

【００２３】

【実施例】以下、本発明のロボットの軌道の生成装置の
一実施例について図面を参照しながら説明する。

【００２４】本発明のロボットの軌道の生成装置を図１
を用いて説明する。教示点データに対し本発明のロボッ
トの軌道の生成装置を用いて教示点データを変換し、変
換した教示点データにしたがってロボットを動作させた
場合の移動軌道が図１（ａ）である。教示点ＴＰ1と教
示点ＴＰNにはさまれたＮ−２個の教示点を内回り対象
点とし、内回り動作を行う区間の開始点であるＴＰk,s
とし、内回り動作を行う区間の終了点をＴＰk,eとし、
内回り動作時の移動速度をＶkとし、教示点ＴＰkでの許
容経路誤差をＤkとする。内回りの対象点から内回り開
始点ＴＰk,sまでの距離を開始点距離Ｌkとし、内回り対
象点から内回り終了点ＴＰk,eまでの距離は開始点距離
と同一の値となる。図では比較のため、実際の移動軌道
である放物線軌道による内回り区間の軌道に加えて、内
回りが行われる以前の折れ線形状の軌道も記している。

【００２５】また図１（ｂ）はロボットの移動している
位置を横軸とし、動作時の先端速度を縦軸としたグラフ
を示している。停止点ＴＰ1で０であるロボットの先端
速度は、まずＴＰlでの指定移動速度Ｖlまで加速し、そ
の後一定速度で動作し、内回り開始点ＴＰ2,sで指定さ
れた内回り区間の上限移動速度であるＶ2に向かって減
速する。内回り開始点ＴＰ2,sより内回り終了点ＴＰ2,e
までの内回り区間を放物線軌道で移動する場合、ＴＰ2,
sにおいてＶ2の値であった先端速度は徐々に減速し、区
間の中点において最小値を取り、ＴＰ2,eにおいて再び
Ｖ2の値となる。内回り終了点ＴＰ2,eよりつぎの内回り
区間開始点ＴＰ3,sまでの接続区間では、速度Ｖ2からつ
ぎの上限移動速度Ｖ3への加速または減速が行われる。
以降の内回り区間も同様の速度変化が行われ、最後の内
回り区間の終了点ＴＰN-l,eから停止点ＴＰNまでの停止
区間で速度が０まで減速される。これらの速度変化は全
てロボットの許容する最大加速以下での動作である必要
がある。そこで、もしある上限移動速度からつぎの区間
の上限移動速度までの加速または減速するための接続区
間の距離が十分でなければ大である側の上限移動速度を
加速または減速が可能な速度にまで下げ、また接続距離
が０である場合にはとなりあう内回り区間の上限移動速
度は同一にする必要がある。これらの変換処理を全教示
点データに対して行うことで、図に示したような滑らか
な軌道での動作が実現されることになる。

【００２６】つぎに、本実施例の具体的な処理内容につ
いて図２を参照しながら説明する。図２は本実施例の構
成を示すブロック図である。図２において、１は内回り
開始終了点設定手段、２は位置設定手段、３は姿勢設定
手段、４は上限移動速度設定手段、５は放物線補間模擬
演算手段、６は速度設定手段、７は速度連続手段、８は
起動区間速度設定手段、９は停止区間速度設定手段、１
０は接続区間速度設定手段、１１は軌道演算手段、１２
は直線補間演算手段、１３は放物線補間演算手段、１４
はロボットを駆動するアクチュエータの駆動部である。

【００２７】上記構成要素の相互関係と動作について説
明する。教示点データｐは内回り開始終了点設定手段１
に入力し、位置設定手段２において内回り開始点の位置
と内回り終了点の位置が計算され、教示点データに新た
に登録される。またその位置での手首姿勢が姿勢設定手
段３において計算され、教示点データに新たに登録され
る。ここで新たに登録された教示点データを変換教示点
データｐｒと称するものとする。

【００２８】上限移動速度設定手段４は変換教示点デー
タｐｒを入力とし、放物線補間模擬演算手段５におい
て、内回り開始点と内回り終了点にはさまれた内回り区
間道に対して放物線補間演算が行われる。その時の単位
時間あたりの関節角の変化量より、速度設定手段６にお
いてそのうち内回り区間における上限移動速度が計算さ
れ、変換教示点データｐｒに登録される。

【００２９】速度連続化手段７は変換教示点データｐｒ
を入力とし、起動区間速度設定手段８において、変換教
示点データｐｒの最初の内回り区間の上限移動速度が、
停止区間速度設定手段９において変換教示点データｐｒ
の最後の内回り区間の上限移動速度が、接続区間速度設
定手段１０において変換教示点データｐｒ内に複数個の
内回り区間が存在する場合の各内回り区間の上限移動速
度が、それぞれ計算され、変換教示点データに登録され
る。

【００３０】軌道演算手段１１は、変換教示点データｐ
ｒを入力とし、教示点、移動命令、移動速度の情報にし
たがい、直線補間演算手段１２において直線補間演算を
行い、放物線補間演算手段１３において放物線補間演算
手段を行い、単位時間あたりの各関節角の変化量Ｊをア
クチュエータの駆動部１４へ出力する。

【００３１】図３は内回り開始終了点設定手段１の処理
動作を示すフローチャートである。図３において、ま
ず、ステップ３１において開始点距離Ｌkの許容最大距
離Ｌakを（１）式によって求める Ｌａ_k＝ｍｉｎ（ｌ_k-1／２，ｌ_k／２） ・・・（１） ただし、ｍｉｎ（ｘ，ｙ）はｘとｙの値の小さい方の値
を選択する関数である。（１）式はＴＰk-1と内回り点
ＴＰkの間の教示点間距離ｌ_K-1の半分の距離と、内回り
点ＴＰkとＴＰk+1の間の教示点距離ｌ_kの半分の距離を
超えて内回りしないことを表している。

【００３２】つぎに、ステップ３２においては内回り点
ＴＰｋにおける屈折角θｋが１８０度であるかを判定す
る。もし屈折角θｋが１８０度であれば、ステップ３５
へ移行し、（２）式により開始点距離Ｌｋを決定し、ス
テップ３６へ移行する。

【００３３】 Ｌk＝Ｌak ・・・（２） またステップ３２において屈折角θkが１８０度でなけ
れば、ステップ３３へ移行する。

【００３４】ステップ３３において、内回り点ＴＰkに
おける放物線補間で移動した際の経路誤差が許容経路誤
差Ｄk以内を実現する開始点距離Ｌkを（３）式より求め
る。

【００３５】 Ｌ_k＝２Ｄk／ｃｏｓ（θ_k／２） ・・・（３） つぎにステップ３４へ移行し、（３）式で求めた開始点
距離Ｌkと許容最大距離Ｌakを比較し、開始点距離Ｌkが
許容最大距離Ｌakよりも大であればステップ３５へ移行
し、（２）式により開始点距離Ｌkを決定し、ステップ
３６へ移行する。このときの経路誤差は許容経路誤差Ｄ
kよりも小となる。

【００３６】またステップ３４において開始点距離Ｌk
が許容最大距離Ｌak以下であれば開始点距離Ｌkは
（３）式で求めた値とし、ステップ６へ移行する。

【００３７】ステップ３６において、内回り開始点ＴＰ
k,sと、内回り終了点ＴＰk,eの位置を（４）式、（５）
式にしたがい決定し、教示点データに新たに登録され
る。

【００３８】 （ＴＰk,sの位置）＝（線分ＴＰk-lＴＰk上にＴＰkから距離Ｌk離れた点） ・・・（４） （ＴＰk,eの位置）＝（線分ＴＰkＴＰk+1上にＴＰkから距離Ｌk離れた点） ・・・（５） つぎにステップ３７において内回り開始点ＴＰk,sと、
内回り終了点ＴＰk,eの姿勢を（６）式、（７）式にし
たがい決定し、教示点データに登録される。

【００３９】 （ＴＰk,sの姿勢）＝（ＴＰk-1からＴＰkまで直線補間によって移動する際 にＴＰk,sの位置を通過する時の姿勢。） ・・・（６） （ＴＰk,sの姿勢）＝（ＴＰkからＴＰk+1まで直線補間によって移動する際 にＴＰk,eの位置を通過する時の姿勢。） （７） 最後にステップ３８において、内回り開始点と内回りの
対象点にあたる教示点の移動命令には放物線補間演算に
よる移動命令が登録され、内回り終了点にあたる教示点
の移動命令には、直線補間演算による移動命令が登録さ
れ、また内回り開始点と内回りの対象点にあたる教示点
の移動速度は移動速度Ｖk-1とＶkのどちらか小さい方が
登録され、内回り終了点にあたる教示点の移動速度は、
Ｖkと登録される。

【００４０】図４は上限移動速度設定手段４の処理動作
を示すフローチャートである。図４において、まずステ
ップ４１において変換教示点データｐｒより上限移動速
度を求める内回り区間の内回り開始点ＴＰk,s、内回り
対象点ＴＰk、内回り終了点ＴＰk,eを読み取る。

【００４１】ステップ４２において内回り区間の放物線
補間演算を放物線補間模擬演算手段５において実行す
る。この補間演算の結果求められた各関節軸角度の単位
時間あたりの変化量をＰnとする。ｎは補間点数を表す
カウンタであり、放物線補間演算で内回り開始点から内
回り終了点まで移動するのに要する補間数をＤＮとする
と補間数カウンタｎは１からＤＮの値をとる。また求め
た補間点間を移動するのに要する時間を補間時間とよび
ｔｃで表す。

【００４２】ステップ４３において各種変数の初期設定
を（８）式、（９）式により行う。ここでｔmaxは補間
点数を移動する際の最大所要時間であり、補間点数ｎ＝
１時点の関節軸の回転速度ベクトルをω１とする。

【００４３】 ｔmax＝ｔｃ ・・・（８） ωｌ＝Ｐｌ／ｔｃ ・・・（９） ステップ４４では補間点数カウンタｎがＤＮ以上であれ
ばステップ１１へ移行し上限速度の設定を行う。またス
テップ４で補間点数カウンタｎがＤＮ未満であればステ
ップ４５へ移行する。

【００４４】ステップ４５においては補間点数ｎ時点の
関節軸の回転速度ベクトルωｎを（１０）式により求め
る。

【００４５】 ωn＝Ｐn／ｔｃ ・・・（１０） つぎにステップ４６においては補間点数ｎと補間点数ｎ−１の間の回転速度の 変化量ｄωn＝ωn−ωn-1 ・・・（１１） つぎにステップ４７において回転速度の変化量ｄωnの
絶対値が各関節軸の最大加速度を表すベクトルであるａ
maxに補間時間ｔｃを乗じた許容変化量よりも大である
かどうかを判断する。この場合の判断は各関節軸毎に行
い、ｄωnが大となる軸が１軸でも存在すれば、ステッ
プ４８へ移行する。全軸において、許容変化量以下であ
れば、ステップ５０へ移行する。

【００４６】ステップ４８では回転速度の変化量ｄωn
が許容変化量以下となるための変化に要する時間ｔdmy
を（１２）式により求め、ステップ４９に移行する。

【００４７】 ｔdmy＝｛（Ｐn−Ｐn-1)／ａmax ｝^1/2 ・・・（１２） ステップ４９では最大所要時間ｔmaxを（１３）式によ
り更新しステップ５０へ移行する。

【００４８】 ｔmax＝ｍａｘ（ｔdmy，ｔmax） ・・・（１３） ここでｍａｘ（ｘ，ｙ）はｘとｙの大きい値を選択する
関数を表している。

【００４９】ステップ５０においては、補間点数カウン
タｎを（１４）式により更新し、ステップ４４へ移行す
る。

【００５０】 ｎ＝ｎ＋１ ・・・（１４） 最後にステップ５１においては上限移動速度Ｖkを（１
５）式により計算し、変換教示点データｐｒの対応箇所
に再登録する。

【００５１】 Ｖk＝Ｌk／｛ｔmax・ＤＮ｝ ・・・（１５） 図５は速度連続化手段７の構成要素である起動区間速度
設定手段の処理動作を示すフローチャートである。図５
において、まずステップ６１では変換教示点データｐｒ
の最初の教示点ＴＰlと最初の内回り開始点ＴＰ2,sを読
み取る。

【００５２】つぎにステップ６２では最初の教示点ＴＰ
lから最初の内回り開始点ＴＰ2,sまでの起動区間を直線
補間により補間演算を行う。

【００５３】つぎにステップ６３では、ステップ６２で
求められた各関節軸角度の補間時間あたりの変化量に対
して、各関節軸のうち少なくとも一つの軸が各関節軸毎
に定められた最大加速度で加速するように加速処理を行
う。その結果最初の内回り開始点ＴＰ2,sでロボットの
先端が実現する移動速度ベクトルをｖ2,sとする。

【００５４】ステップ６４では（１６）式にしたがい、
最初の内回り開始点ＴＰ2,sで到達する到達可能線速度
Ｖa2を求める。

【００５５】 Ｖa2＝｜ｖ2,s｜ ・・・（１６） ステップ６５では上限移動速度設定手段２で求めた最初
の内回り区間の上限移動速度Ｖ2が到達可能線速度Ｖa2
よりも大であるかを判定し、もし大であるならばステッ
プ６６へ移行し、（１７）式にしたがい、最初の内回り
区間の上限移動速度Ｖ2を更新し、ステップ６７へ移行
する。

【００５６】 Ｖ2＝Ｖa2 ・・・（１７） ステップ６７において上限移動速度Ｖ2を変換教示点デ
ータｐｒに再登録する。

【００５７】また、ステップ６５において、最初の内回
り区間の上限移動速度Ｖ2が到達可能線速度Ｖa2以下な
らば、ステップ６７へ移行し、そのままの上限移動速度
Ｖ2を変換教示点データｐｒに再登録する。

【００５８】速度連続化手段７の構成要素である停止区
間速度設定手段９の処理動作は起動区間速度設定手段８
における最初の教示点ＴＰlを最後の教示点ＴＰNに、最
初の内回り開始点ＴＰ2,sを最後の内回り終了点ＴＰN-
1,eとし、図５で示した処理と同様の処理動作を行うこ
とで最後の内回り区間の上限移動速度ＶN-1を更新し、
変換教示点データｐｒに再登録すればよい。

【００５９】図６は速度連続化手段７の構成要素である
接続区間速度設定手段１０の処理動作を示すフローチャ
ートである。図６において、ステップ７１では変換教示
点データｐｒより、ある内回り区間の内回り終了点ＴＰ
k,eと上限移動速度Ｖk、そのつぎの内回り区間の内回り
開始点ＴＰk+1,eと上限移動速度Ｖk+1を読み取る。

【００６０】つぎにステップ７２では内回りの対象点Ｔ
Ｐkの上限移動速度Ｖkと内回り対象点ＴＰk+1の上限移
動速度Ｖk+1を比較し、ＶkがＶk+1よりも小であればス
テップ７３へ移行し、ＶkがＶk+1以上であればステップ
７４へ移行する。

【００６１】ステップ７３では直線補間を行う際の始点
Ｐｓと終点Ｐｅと加速処理を行う際の始点での初速度Ｖ
ｓ、終点での終速度Ｖｅを（１８）式、（１９）式、
（２０）式、（２１）式で示すように決定し、ステップ
７５へ移行する。

【００６２】 Ｐｓ＝ＴＰk,e ・・・（１８） Ｐｅ＝ＴＰk+1,s ・・・（１９） Ｖｓ＝Ｖk ・・・（２０） Ｖｅ＝Ｖk+1 ・・・（２１） ステップ７４では直線補間を行う際の始点Ｐｓと終点Ｐ
ｅと加速処理を行う際の始点での初速度Ｖｓ、終点での
終速度Ｖｅを（２２）式、（２３）式、（２４）式、
（２５）式で示すように決定し、ステップ７５へ移行す
る。

【００６３】 Ｐｓ＝ＴＰk+1,s ・・・（１８） Ｐｅ＝ＴＰk,e ・・・（１９） Ｖｓ＝Ｖk+1 ・・・（２０） Ｖｅ＝Ｖk ・・・（２１） ステップ７５では始点Ｐｓと終点Ｐｅの距離ｄｌを求め
る。

【００６４】ステップ７６では距離ｄｌが０かどうかを
判定する。もしｄｌが０ならばステップ７７へ移行す
る。０でなければステップ７８へ移行する。

【００６５】ステップ７７では接続距離が０であるため
加速、減速ができないのでＶｅを（２６）式にしたがっ
て設定し、ステップ８３へ移行する。

【００６６】 Ｖｅ＝Ｖｓ ・・・（２６） ステップ７８では、始点Ｐｓから終点Ｐｅの間を始点Ｐ
ｓにあたる教示点データに登録されている移動速度で直
線補間演算を行う。

【００６７】つぎにステップ７９では、ステップ７８で
行った直線補間演算で求められた各関節軸角度の補間時
間あたりの変化量に対して、各関節軸のうち少なくとも
一つの軸が各関節軸毎に定められた最大加速度で加速す
るように初速度Ｖｓから加速処理を行う。その結果、終
点Ｐｅでロボットの先端が実現する移動速度ベクトルを
ｖｅとする。

【００６８】ステップ８０では（２７）式にしたがい、
終点Ｐｅで到達可能な線速度Ｖaeを求める。

【００６９】 Ｖae＝｜ｖｅ｜ ・・・（２７） ステップ８１では終速度Ｖｅが到達可能線速度Ｖaeより
も大であるかを判定し、もし大であるならばステップ８
２へ移行する。ステップ８０では（２８）式にしたがい
終点にあたる教示点Ｐｅの上限移動速度Ｖｅを更新し、
ステップ８３へ移行する。

【００７０】 Ｖｅ＝Ｖae ・・・（２８） ステップ８３では変換教示点データｐｒに以上で求めた
上限移動速度を再登録する。

【００７１】図７は教示点Ａ，Ｂ，Ｃで規定される折れ
線上の軌道を放物線補間演算手段により補間する方法を
示す説明図である。本実施例における放物線補間演算は
連続する３つの教示点で規定される折れ線の節点から許
容経路誤差以内の範囲を通過し折れ線に接する放物線軌
道を決定する。図７において放物線補間演算が行われる
のは前記の内回りの開始点にあたる点Ｍより内回り対象
点である点Ｂ、内回り終了点である点Ｎまでの区間とな
る。ここで点Ａ，Ｂ間の教示移動速度がＶであったとす
ると、ロボットの先端が点Ｍを速度Ｖで通過し放物線状
の経路に沿って点Ｂ’を通過し点Ｎに速度Ｖで到達する
為に必要な先端加速度αはＢＢ’の方向に（２９）式で
表される。

【００７２】 α＝｛Ｖ・ｃｏｓ（θ／２）｝²／２ ・・・（２９） （２９）式で求められる先端加速度を用いて点Ｍ、点
Ｂ’、点Ｎを結ぶ補間点の位置を求める。

【００７３】図８は放物線補間演算手段の処理動作を示
すフローチャートである。まずステップ９１において点
Ｍから点Ｂまでの直線を延長した位置に仮目標点Ｎ’を
設定する。

【００７４】ステップ９２ではステップ９１で設定した
直線ＭＮ’間の補間点座標を速度Ｖでの直線補間で求め
る。

【００７５】ステップ９３においては（２９）式に示す
加速度αによる位置の変化量をステップ９２で求めた補
間点座標に加算することで放物線経路上の補間点座標を
求め、その位置を実現する関節軸の角度を求める。この
ときの位置変換量ΔＰｋは（３０）式で表される。

【００７６】 ΔＰk＝｛α（ｋ・ｔｃ）²／２｝・ｎ ・・・（３０） ここでｔｃは点ＭＮ’間の単位補間時間であり、ｎはＢ
Ｂ’方向の単位ベクトル、ｋは補間回数を示すカウンタ
を表している。

【００７７】ステップ９４では内回り区間を放物線軌道
で移動する際の手首姿勢の補間演算を、点Ｍから点Ｂま
での区間を直線補間で移動した場合と同じ手首姿勢の補
間演算を行う。

【００７８】ステップ９５では点Ｂから点Ｎまでの区間
を直線補間で移動した場合と同じ手首姿勢の補間演算を
行う。

【００７９】つぎにステップ９６では、点Ｂの前後にお
いて、補間演算後の手首関節軸の回転速度の単位時間当
りの変化量が各手首関節軸の最大加速度から定まる許容
変化量を超える値となる場合には、手首関節軸の回転速
度の単位時間あたりの変化量が許容変化量以下となるよ
うに前記手首関節軸の回転速度の単位時間あたりの変化
量に対して、移動平均処理や一次遅れフィルタ処理、も
しくはそれらの処理に類似した処理による連続化処理を
行う。

【００８０】以上のように本実施例のロボットの軌道の
生成方法によれば、あらかじめ与えられた教示点データ
に新たな教示点の追加や移動速度、移動命令の変更など
の変換処理を行い、ロボットの動作時にはその変換が行
われた教示点データにしたがって動作させることによ
り、停止が指定された点からつぎの停止が指定された点
まで滑らかな動作を実現することができる。

【００８１】なお、本実施例の各構成要素と動作はマイ
クロコンピュータのプログラム制御により実現できるこ
とはいうまでもない。

【００８２】

【発明の効果】以上の実施例から明らかなように、本発
明はロボットの先端部が移動する折れ線軌道の基準位
置、形状、移動速度、教示点からの許容経路誤差を備え
た教示点データに対して許容経路誤差の範囲内を内回り
する放物線軌道の開始点と終了点の位置と、放物線軌道
を移動する際の関節軸角加速度が許容値以下となる最大
の移動速度を新たに登録し、内回りする区間が２つ以上
連続して存在する場合には、ある放物線軌道の終了点で
の先端の到達速度から次の放物線軌道の開始点での先端
移動速度まで、その間の直線移動区間を関節軸の許容角
加速度値以下で加速または減速移動すれば加速または減
速できるようにそれぞれの放物線軌道の最大移動速度を
あらかじめ調整し教示点データへ再登録し、ロボットの
運転時にはこれら新たに登録された教示点データにした
がって軌道の生成計算をすることで、折れ線の節点では
放物線軌道で内回りし滑らかに動作を実現する効果と、
直線部と放物線部の接続点において先端移動速度および
関節軸角速度が不連続な変化をすることがなくなるとい
う効果と、ロボットの先端振動を抑制する効果と、ロボ
ット運転時の関節軸の角加速度を許容値以下に制限され
る条件下での最短の作業時間を実現するという優れた効
果を奏するものである。

【図面の簡単な説明】

【図１】（ａ）本発明のロボットの軌道の生成装置によ
るロボット先端の移動軌道を示す図 （ｂ）同ロボットの移動している位置に対するロボット
先端の速度を示すグラフ

【図２】本発明のロボットの軌道の生成装置の一実施例
の構成を示すブロック図

【図３】本発明の内回り開始終了点設定手段の処理動作
を示すフローチャート

【図４】本発明の上限移動速度設定手段の処理動作を示
すフローチャート

【図５】本発明の速度連続化手段の構成要素である起動
区間速度設定手段の処理動作を示すフローチャート

【図６】本発明の速度連続化手段の構成要素である接続
区間速度設定手段の処理動作を示すフローチャート

【図７】本発明の放物線補間演算手段の処理内容を示す
説明図

【図８】放物線補間演算手段の処理動作を示すフローチ
ャート

【図９】従来のロボットの軌道の生成装置によるロボッ
ト先端の移動軌道を示す図

【符号の説明】

１ 内回り開始終了点設定手段 ２ 位置設定手段 ３ 姿勢設定手段 ４ 上限移動速度設定手段 ５ 放物線補間模擬演算手段 ６ 速度設定手段 ７ 速度連続化手段 ８ 起動区間速度設定手段 ９ 停止区間速度設定手段 １０ 接続区間速度設定手段 １１ 軌道演算手段 １２ 直線補間演算手段 １３ 放物線補間演算手段

───────────────────────────────────────────────────── フロントページの続き (56)参考文献 特開 昭64−84309（ＪＰ，Ａ) 特開 平７−64621（ＪＰ，Ａ) 特開 平７−64622（ＪＰ，Ａ) 特開 昭57−211605（ＪＰ，Ａ) 特開 昭63−205708（ＪＰ，Ａ) 特開 平３−52003（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05B 19/18 - 19/46 B25J 3/00 - 3/10 B25J 9/10 - 9/22 B25J 13/00 - 13/08 B25J 19/02 - 19/06

Claims (7)

(57)【特許請求の範囲】
1. 【請求項１】 ロボットの先端部が移動する軌道の基準
   を与える教示点と、その教示点に基づく軌道の形状を指
   定する移動命令と、軌道上を先端部が移動する速度を指
   定する移動速度と、教示点と軌道との間の誤差の最大値
   を指定する許容経路誤差とを備えた教示点データを入力
   とし、連続して存在する３点以上の教示点データにより
   指定された折れ線形状の経路の前記折れ線の節点を経由
   せずに内回りする軌道の内回りを開始する点である内回
   り開始点の位置と姿勢と、終了する点である内回り終了
   点の位置と姿勢をロボットの動作前にあらかじめ計算
   し、教示点データに登録する内回り開始終了点設定手段
   を有し、放物線補間する際の先端移動速度として、単位
   時間あたりのロボットの各関節軸の回転速度の変化量が
   各関節軸毎に定められた最大加速度から定まる許容変化
   量を超えることのない上限移動速度を、ロボットの動作
   前にあらかじめ計算し、教示点データに登録する上限移
   動速度設定手段を有し、内回りする区間が２つ以上連続
   して存在する場合には、ある放物線軌道の終了点での先
   端の到達速度から、次の放物線軌道の開始点での先端移
   動速度まで、その間の直線移動区間を関節軸の許容角加
   速度値以下で加速または減速移動すれば加速または減速
   できるようそれぞれの放物線軌道の上限移動速度をあら
   かじめ調整し教示点データへ登録する速度連続化手段を
   有し、前記内回り開始終了点設定手段と前記上限移動速
   度設定手段と前記速度連続化手段によって新たに登録し
   た教示点データにしたがって折れ線形状の経路を内回り
   する軌道を放物線軌道によって生成する放物線補間演算
   手段がある軌道演算手段を有することを特徴とするロボ
   ットの軌道の生成装置。
2. 【請求項２】 内回り開始終了点設定手段は、教示点デ
   ータを入力とし、教示点の位置と許容経路誤差の値より
   内回り開始点の位置と内回り終了点の位置を計算し、教
   示点データに登録する位置設定手段と、教示点データを
   入力とし折れ線を構成する２つの線分のうち前半の線分
   を直線補間で補間演算した際に前記位置設定手段で登録
   した内回り開始点の位置座標を通過するときの手首姿勢
   を内回り開始点における手首姿勢として教示点データに
   登録し、折れ線を構成する２つの線分のうち後半の線分
   を直接補間で補間演算した際に前記位置設定手段で登録
   した内回り終了点の位置座標を通過するときの手首姿勢
   を内回り終了点における手首姿勢として教示点データに
   登録する姿勢設定手段とを具備した請求項１記載のロボ
   ットの軌道の生成装置。
3. 【請求項３】 上限移動速度設定手段は、前記内回り開
   始終了点設定手段によって新たに登録された教示点デー
   タを入力し、前記軌道演算手段の構成要素である放物線
   補間演算手段を模擬演算する放物線補間模擬演算手段
   と、前記放物線補間模擬演算手段の結果、各関節軸の回
   転速度の単位時間あたりの変化量が内回り区間の全区間
   にわたり各関節軸の最大加速度から定まる前記許容変化
   量を超えないように教示点データ内の移動速度をあらか
   じめ計算し教示点データに登録する速度決定手段を具備
   した請求項１記載のロボットの軌道の生成装置。
4. 【請求項４】 速度連続化手段は、ロボットが停止して
   いる最初の教示点から内回り開始終了点設定手段によっ
   て登録された最初の内回り開始点までの起動区間を、各
   関節軸のうち少なくとも一つの軸が各関節軸毎に定めら
   れた最大加速度で加速しながら移動した場合に実現可能
   な速度を計算し、前記上限移動速度設定手段によって登
   録された上限移動速度とを比較し、どちらか小さい値の
   速度を最初の内回りの区間の上限移動速度として、教示
   点データに再登録する起動区間速度設定手段を具備する
   請求項１記載のロボットの軌道の生成装置。
5. 【請求項５】 速度連続化手段は、内回り開始終了点設
   定手段によって登録された最後の内回り区間の内回り終
   了点から、教示点データの最後の教示点までの停止区間
   を、各関節軸のうち少なくとも一つの軸が各関節軸毎に
   定められた負の最大加速度で減速しながら移動した場合
   に、前記最後の教示点においてロボットの先端移動速度
   が０となるようなロボットの先端移動速度を計算し、前
   記上限移動速度設定手段によって登録された最後の内回
   り区間の上限移動速度と比較し、どちらか小さな値に速
   度を最後の内回り区間の上限移動速度として教示点デー
   タに再登録する停止区間速度設定手段を具備した請求項
   １記載のロボットの軌道の生成装置。
6. 【請求項６】 速度連続化手段は、ある内回り区間を第
   １の内回り区間とし、前記第１の内回り区間の次の内回
   り区間を第２の内回り区間とし、第１の内回り区間の上
   限移動速度を初速度として、第１の内回り区間の内回り
   終了点と第２の内回り区間の内回り開始点にはさまれた
   接続区間を、各関節軸のうち少なくとも一つの軸が関節
   軸毎に定められた最大加速度で加速しながら移動、また
   は一定速度で移動、または減速しながら移動することに
   より第２の内回り区間の上限移動速度が達成できるよう
   に、前記第１の内回り区間の上限移動速度と前記第２の
   内回り区間の上限移動速度を再計算し、前記第１の内回
   り区間の終了点と前記第２の内回り区間の開始点の距離
   が０であるとき、前記第１の内回り区間の上限移動速度
   と前記第２の内回り区間の上限移動速度の内、小さな値
   の方を、両者の速度として設定する接続区間速度設定手
   段を具備した請求項１記載のロボット軌道の生成装置。
7. 【請求項７】 軌道演算手段は、教示点データを入力と
   し、２つの教示点間を直線補間する直線補間演算手段
   と、３つの教示点で構成される折れ線の節点を経由せ
   ず、折れ線を構成する２つの線分を放物線により滑らか
   に接続して節点を内回りするロボットの先端軌道を生成
   し、内回り区間を放物線軌道で移動する際の手首姿勢の
   補間演算を、内回り開始点から折れ線の節点までの区間
   を直線補間で移動した場合と同じ手首姿勢の補間演算を
   行い、折れ線の節点から内回り終了点までの区間を直線
   補間で移動した場合と同じ手首姿勢の補間演算を行い、
   折れ線の節点にあたる教示点の前後において、補間演算
   後の手首関節軸の回転速度の単位時間あたりの変化量が
   各手首関節軸の最大加速度から定まる許容変化量を超え
   る値となる場合には、手首関節軸の回転速度の単位時間
   あたりの変化量が前記許容変化量以下となるように前記
   手首関節軸の回転速度の単位あたりの変化量を自動調整
   する放物線補間演算手段を具備した請求項１記載のロボ
   ットの軌道の生成装置。