JPS5968014A - ロボツトの経路補間装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、連続したポイントツウポイント（Ｐ。

ＴＰ）動作の高速化に好適なロボットの経路轡１゜開方
式に間するものである。

〔従来技術〕

従来、ｐ７ｐ（ポイントッーポイント）動作を連続して
行なう場合に各ポイント毎に停止しないで通過する方法
として、各ポイントを結ぶ経路同志を円弧で結び、この
円弧に沿って各ポイントの近傍を通過する方法が用いら
れていた。

しかし、この方法では接続のための円弧をもとめる計算
に時間がかかるという欠点があり、ロボットのリアルタ
イム制御には不向きであった。

〔発明の目的〕

本発明の目的は、上記従来技術の欠点をなく・し、簡単
な演算により連続したＦＴＰ動作の高・連化をリアルタ
イムで行なえるようにした。ｏ。

ポットの経路補間装置を提供することにある。−１〔発
明の概要〕 本発明は、ロボットのシーケンスを制御する。

ロボットシーケンス制御装置と、このシーケン。

スを記憶する動作シーケンスレジスタと、ロボ。

ヅトの移動に必要なパラメータを演算する経路１゜補間
手段と、このパラメータを記憶する加減速。

パラメータレジスタと、このパラメータに基づ。

きロボットの移動を制御するロボット移動制御。

手段とより構成されたロボ・ノド制御装置において、上
記経路補間手段が上記ロボ９トシーケン５ ス制御装置より与えられる２ケ所の移動目標値間におけ
る加減速パラメータを演算する２点間パラメータ演算手
段と、この加減速パラメータと上記Ｏボ・ノトンーケン
ス制御装置より与えられる位置データおよび速度データ
とに基づいて０ ３　・ 減速パラメータを演算する連続移動パラメータ演算手段
と、この連続移動パラメータ演算手段。

で演算されるパラメータに基づき加速度を判定。

する加速判定手段と、この加速度判定手段の判。

定に基づき上記連続移動パラメータ演算手段で。

算出されたパラメータの値を修正するパラメー。

夕修正手段とから構成されることを特徴とする。

ロボ・ソトの経路補間装置である。

〔発明の実施例）　　　　　　　　　　　　　　ｌｆ１
以下本発明を図に示す実施例にもとづいて具体的に説明
する。

まず連続したｐｒｐ動作を実行する一般的手順を第１図
に従い述べる。ロボ・リドの現在地点をＰＯ１目標地点
をＰｌ、　Ｐ２とする。ＰＯからＰｌを結ぶ経路上の２
点をＰ（３１，／’０２またＰｌからＰ２を結ぶ経路上
の２点をＰｌｌ、Ｐ１２とする。まず、ロボ・ノドはＰ
Ｏから加速を開始し、ＰｏｌからＰＯ２までは定速で移
動し、Ｐ１近傍のＰＯ２に達した時。

Ｐ１１方向へ定まった曲線上を加減速しながら移ＩＣ） ４　・ 動し、ＰｌｌからＰ１２までは定速で移動し、　／’１
２゜かう減速を開始しＰ２へ到達する。

ここで、　ＰＱからＰｏｌまでの加速区間、ＰＯ２か。

らＰｌｌまでの加減速区間、Ｐ１２からＰ２までの減速
。

区間におけるロボットの移動目標位置をもとめする演算
は、同一のアルコリズムで、しかモ簡単２な演算で行な
える事が必要である。よって１本。

特許においては、この移動目標位置を算出する。

方法として、移動目標位置を時刻ｔに関する多。

環式として表わす方法を用いた。第２図に示すような１
時刻ｔ＝０で速度ν１　ｅ　’　＝ｉａで速度ｖ２を満
足するような位置を表わす多項式Ｐはｔ＝０での位置を
ＰＯとおけば１式（１）のように表わされる。

ここで、ｔ＝０で加速度α＝ａ、ｔ＝ｔ、でα＝０なる
条件が付けば１式（２）のように表わされる一般に、ロ
ボットの位置を示すパラメータとしては、直交座標にお
ける手先の位置と姿勢、特に関節形ロボットでは各関節
角度などが用いら。

れる。そこで、上記１式（１）１式（２）における、速
。

度および位置を、ベクトル表示にあらためる。。

次に、第１図に示す／’０２からＰｌｌへの移動を。

行なうには、　ＰＯ２およびＰｌｌの位置かあらかじ。

め計算されていなければならない。まず、この。

移動開始位置ＰＯ２に対する条件は、この移動中Ｉｌｌ
の加速度が強度上許容される最大加速度を起える場合に
は、この移動を行なえないので、ＰＯ２からＰｌまで、
最大加速度で減速して停止できる事である。よって、Ｐ
Ｏ２における速度なｒＶｌとすれば、ＰＯ２からＰｌま
での距離は、速度ｌｒ１から速度０まで最大加速度で加
速するのに要する距離である。一方、ｌ′１１の位置は
、アルゴリズムを簡略化するため、ＰＯ２をもとめるア
ルゴリズムと同一にして求める。即ち、Ｐｌｌでの速度
をＩＶ２とすれば、　ＰｌからＰｌｌまでの距離は、速
度０から速度Ｗ２まで最大加速度で加速するのに要す。

る距離である。

また　ＰＯ，Ｐ１間の加速終了点ｊ′０１は、速度０゜
からＨＶｌまで最大加速度で加速するσ）に要する距。

離から求め、　Ｐｌ、　Ｐ２間の減速開始点／’１２は
、速１度ＩＶ２から０まで最大加速度で減速するのに要
する距離から求める事が最短時間での移動に必要。

である。

よって、　ＰＯ、Ｐ１間での加速時間、減速時間を。

ｔａｌ、　Ｐｌ、　Ｐ２間での加速時間、減速時間を　
”　ｌｆ＋とすれば、第２図の加減速ノくクーンの場合
、　。

でもとめられる。ここでｖｌＬは速度ｌＶ１のｉ番目の
成分、ｖ２ｉは速度ＪＶ２のｉ番目の成分、αｍαｉは
、ｉ番目の速度に対する最大加速度である。

第３図の加減速パターンの場合。

ｖ１ｉ ｔ　ａ　１　＝　ｍＱ、：ｔ　（−、）　　　　　　　
　　・・・・・・・・・（ハ　。

２αｍａｓｔ ５ｖ２ｉ　　　　　　　　　、・川・・・・（８）・Ｐ
Ｏ２：（２αｍａｘｉ　） で求められる。各加減速の開始あるいは終了位。

置は、第２図、第６図のパターン共 ／ｌ’ｏ１＝−−，７＋’／’ｏ　　　　　　　−−−
−・・−、、（ｑｌ　　。

／Ｐｏ１＝／／’１−−１１’１−−−−−−−−・（
ｊｎ）　　：１／ｌ’１２＝Ｉｌ’２−とＩＰ２　　　
　　　・・・・・・・・・（１２）。

で算出できる。

次に、ＰＯ２からＰｌｌへ式（３）あるいは式（４）で
移。

動する場合だ要求される条件を示す。式（３ｊ　、　（
４）；。

テｔ　＝　ｔｔ　、７／’ｏ　＝ＩＰｏ２ｃＤ場合、　
ＩＰ　ハ’Ｐ１１テアリー１両式とも１式（１６）が成
立する。

ＩＰ＋　１−１Ｐａ２＝　−、ｌＶ＋　＋　”−ＩＰ２
　　・・・・・・・・・（１３）２ 一方１式００）　＊　（Ｈ）より （Ｐ＋　１−　／／’＋１２　＝　−、ＩＡ　＋−ＩＰ
２−−−−−−　・−（１４）２−　　２　　　　　　
　１コ であり１式（１３）　、　（１４）が恒等的に成り立つ
ためには。

ｔａ１＝ｔａ２−ｔ／、０．、、．０．、（１５）でな
ければならない。しかし、一般には、　ｔｕ１％ｔａ２
であるから　１ａ１）　ＰＯ２である時には。

第４図に示すように、ＰＯ２か′らＰｌに向かって。

７　。

（ｔ・１−ｔ・２）時間減速する。減速後の速度をＩＰ
１゜位置をＰＯ３とすれば。

ｙ、’　＝ｉａムｔＡ ｔ４．　　　　　　　　　　　　・・・・・・・・・（
１６）　　。

ｌ１０５＝ＩＰ＋−ルミＩＶ１′　　　　　　・・・・
・・・・・（１７）で算出できる。なお、この時のＰＯ
３からＰｌｌへ−の移動時間はＰＯ２である。一方　１
！ａ１（ｔ・２で。

ある時には、第５図に示すように、ＰＯ２からＰｌ３゜
へ向かって移動する。このＰｌ３はＰｌからＰ２方向。

へｔｇ１時間加速した場合の位置であり、この位置での
速度をｒＶ２’とすれば。

で算出でき、ＰＯ２からＰｌ３への移動時間はｔａｌで
ある。また、Ｐｌ３からＰｌｌへは（ＰＯ２−ｔａｌ）
時間加速すればよい。

５ 以上、移動を行なうのに必要なパラメータはすべてもと
まったが、第１図における／’０２からＰｌｌへの移動
あるいは、第４図におけるＰＯ３からＰｌｌへの移動あ
るいは、第５図におけるＰＯ２から／’１５への移動は
無条件に行なえるわけでは８　・ なく、この区間における加速度を吟味する必要。

がある。この区間の移動開始速度を（Ｖｕ・（例え。

ばＰＯ５における速度ＩＪ’１’）、移動終了速度なｌ
Ｖｇ　　。

（例えば、Ｐｌｌにおける速度ＩＶ２）とし、それら。

の第を番目の成分なυむ、　ｖｌｌｉと表し、これに対
する許容最大加速度をαｍａｘｉ、移動時間なｉｆとす
れば１式（６）にもとづく移動の場合。

ｖｇｉ−ＰＯ１ １７１＜ｔｔｍａｘｉ　　　　　　”・・・・−（２０
）式（４）にもとづく移動の場合 を、各々満足する必要がある。即ち１式（２０）あるい
は１式（２１）を満足する場合には、この移動が行なえ
るが、満足しない場合には、第１図、第４図、第５図の
いずれの場合においてもＰＯ２からＰｌまで減速して停
止し、ＰｌからＰｌｌまで加速するようにしなければな
らない。

以上の原理にもとづく１本発明の一実施例を第６図によ
り説明する。

上記したアルゴリズムにおける各パラメータのうち、第
１図におけるＰＯからＰＯ２への移動中に必要なパラメ
ータは１位置ベクトルＩＰｏ　１．／ＰＯ２，。

速度ベクトルＩＶ１．　ＰａからＰｏｌまでの加速時間
ｔσ１であり、残りのパラメータは移動中に算出すれ。

ばよい。そこで、まず、ロポ・ノドの移動中にお。

ける処理手順について説明する。

移動中に行なう演算で必要な初期情報は１位装置ベクト
ルＩＰ［１２、ＩＰｌ　、　Ｉｒ２　、速度ベクトル＋
１／１゜Ｆ２．　／’０２からＰｌへの減速時間ｔσ１
であり、こ。

れらの値は、ロボットシーケンス制御装置１よ。

り加減速パラメータレジスフ４にストアされる１、１に
こで、ロボットシーケンス制御装置１は、動。

作シーケンスレジスタ２に記憶されたシーケン８スデー
タおよび位置データにもとづき、ロホ−ｙ。

トのシーケンスをコントロールするものである。

また、加減速パラメータレジスタ４には、上記５ した初期情報ならびに演算結果がストアされる。

この加減速パラメータレジスタ４にストアされるデータ
を次に示す。

１Ｐ０２＋Ｐ０２の位置ベクトル Ｉ′°３“ｐｏｓ　ａ＋　　　　　　　　　　　　　　
　２ｎ１１　・ ＩＰｌｌＰｌ　　の位置ベクトル Ｉｒ１３ＩＰ１３の １Ｐ１１＋　Ｐｌｌの　１ （Ｆ１２　ｔ　／’１２の　　１ Ｉｒ２：Ｆ２の　Ｉ ＩＶ１１Ｆ０２における速度ベクトル Ｐｔ’　　＋　／’（＋５ １Ｖ２　１　Ｆ１３ Ｐ２＋　Ｐｌｌ ｔａ１雪Ｐ０２からＰｌへの減速時間　　　　　　１、
（ｔａ２ＩＰ１からＰｌｌへの加速時間 ｔ１１Ｐ０２からｉ’ｏ３’＼の減速時間ｔ２１ＰＯ！
ｒからＦ１３への加減速時間ｔｓ　　Ｉｒ１３からＰｌ
ｌへの加速時間欠に、ロボットシーケンス制御装置１は
、２５ 点間加減速パラメータ演算器５に演算開始指令を与える
。２点間加減速パラメータ演算器５はこの指令を受け１
式（６）あるいは式（８）よりｔａ’ｌを。

また１式（１１）および式（１２）より１Ｐ１１および
Ｉｒ１２を算出する。２点間加減速パラメータ演算器５
は演０ １２　。

算終了後、演算結果を加減速ノくラメ−タレジス。

り４にストアし、連続移動ノくラメータ演算器６゜に演
算開始指令を与える。連続移動ノくラメータ。

演算器６は、この演算開始指令を受け　ｔａｌと。

ｔｓ２の大小を判定する。ｔａｌ　）　ｔｓ２であれ（
工、５式（１６）および式（１７）に従って％ＩＶ１′
および／／’ｏ３を算出。

し、且つ次のようにノくラメータの値を設定する。。

１Ｐ１３　＝ｌＰ１１ Ｆ２’　＝　Ｆ２ １ｊ＝１・１−ｔ・２ｔ。

＋２−＝ｆ、ａ２ ＋５　　＝　０ また　ｔａ１＝　ｔｓ２であれば１次のようにノくラメ
。

−タの値を設定する。

ｔｅａｓ二ｌＰＯ２ ５ １Ｐ１５−ＩＰｌｌ Ｆ１’　＝　Ｆ　ｌ Ｉｒ２’　＝１７／’２ ｔｌ　　＝　０ １２＝ｔｘ１ ＋３　−０ また、　ｔａｌ＜：　ｔｓ２であれば１式（１８）およ
び式（１９）に従って、　ｌＦ２′およびＦ１５を算出
し、且つ次のようにパラメータの値を設定する。

ＪＰＯ５：Ｉ／’０２ １Ｖ１’　＝ｌＪ’１ ｔｌ　　＝　０ ＋２＝ｔａ１ ｔｓ　　　＝　　ｔｓ２−　　ｔ１１１連続移動パラメ
ータ演算器６は演算終了後、慎。

算結果を加減速パラメータレジスタ４にストア。

し、加速度判定器７に演算開始指令を与える。。

加速度判定器７は、この指令を受け１式（２０）ある。

いは１式（２１）によって加速度を判定し、この条件。

を満足する場合には、演算終了信号をロボ・・トシーケ
ンス制御装置１に送り１条件を満足しない場合には、パ
ラメータ修正器８に開始指令を与える。パラメータ修正
器８はこの指令を受けると１次のようにパラメータの値
を修正する。

ＪＰａ５＝ＨＰ１ １Ｐ１５　＝ＩＰ＋ １ｒ１′：。

ｌｊｌ’２’＝。

ｔｌ　　　＝　　ｔａｌ ｔ２　　＝　０ ｔ５＝ｔａ２ パラメータ修正器８は、修正終了後、演算終了。

信号をロボットシーケンス制御装置１に送る。。

ロボ・ソトシーケンス制御装置１ば、上記演算。

終了信号を受けつけるとロボット移動制御装置６に開始
指令を与える。０ボ・ノド移動制御装置３は、加減速パ
ラメータレジスタ４の内容に従い１次の順にロボットの
移動制御を行なう。

（ｌｌ　　／’０２からＰａ３まで２１時間１式（６）
または式（４）によって、各サンプリングタイム毎の位
置目標値を演算し、この位置目標値をサーボ装置９へ送
る。なお、ｔ１＝Ｑの時、このステ・ツブは実行しない
。

（Ｉｌｌ　　Ｐａ３からＰ１３ずでｔ２時間、式（３）
あるいは式（４）によって、各サンプリングタイム毎の
位置目標値を演算し、この位置目標値をす。

−水装置９へ送る。なお、ｔｏ＝０の時、このステップ
は実行しない。

（ｌｌｌ）Ｐ１５からＰｌｌまで２３時間１式（３）あ
るいは。

式（４）によって、各サンプリングタイム毎の。

位置目標値を演算し、この位置目標値をす。

−水装置９へ送る。なお、　ｔＳ　＝　Ｏの時、こ。

のステップは実行しない。

（θ　Ｐｌｌから／’１２まで下記式によって各サン。

プリングタイム毎の位置目標値７Ｆを演算し。

この位置目標値をサーボ装置９へ送る。なお、Ｐ１１＝
Ｐ１２の時、このステップは実行しない。

１ｐ　＝　ＩＶ２ｔ　＋　／／’１１ ここで、ｔはＰｌｌからの経過時間 なお、ここで、サーボ装置９は５位置目標値と。

ロボ・ソトの状態量（例えば、各モータの回転角度１回
転角速度など）とからロボー・トのアクチュエータのサ
ーボを行なう装置である。

０ボーノド移動制御装置３は、加減速パラメータレジス
タ４の情報から得られる最終位置目標。

値をサーボ装置９に与えた後、ただちに、制御。

終了信号をロボットシーケンス制御装置１に送、る。ロ
ボ−Ｉ）シーケンス制御装置１は、この信。

号を受けると、移動を続行するために加減速パ、。

ラメータレジスタ４の内容の初期値を設定スる。。

即ち、まず１次に示すようにパラメータの変更。

を行なう。

／／’０２←１Ｐ１２ １Ｐ１　　←ｔｐ２ ０ １Ｖ１４−１ｒ２ ｔａｌ　←　Ｐａ２ 次に、シーケンスレジスタ２より新たな目標位置および
速度を取り出し、これらをｌＰ２および＋１７２にスト
アする。よって１以上の手順をくり返５ すことにより、移動を連続して行なうことができる。

移動開始時には、ロボ−）シーケンス制御装置１により
、下記のように加減速パラメータレジスタ４の内容の初
期値を設定すればよい。ここで、移動開始時の位置をｔ
ＰＬ＋目標位置をＰｄ、。

速度をＩＶａとする。

ｌＰＯ２：ＪＰｉ １Ｐ１　　＝ＪＰｉ ｒｐ２．、、ｒｐｄ Ｆ１＝（１ １Ｊ’２　　＝　ＩＶｄ ｔａに〇 移動終了時には、ロボ・ソトシーケンス制御装。

置１により、下記のように加減速パラメータレジスタ４
の内容の初期値を設定すればよい。ここで、移動終了時
の位置をＩＰｇとする。

／／’０２←ＩＰ１２ １Ｐ１　　←／／’２　＝ｌＰｒｔ １ｒ１←ＩＶ２ ｔａｌ　←　Ｉｌｌ２ ７／’２　　＝／Ｐε ＩＶ２＝Ｏ 〔発明の効果〕 以上説明したように１本発明によれは、各ポインド毎に
停止しないでＰＴＰ動作を連続して。

行なうための演算を容易に行なうことができる。

ので、ロボットのリアルタイム制御に適し、口。

ポットの動作時間を短縮する効果がある。

【図面の簡単な説明】

第１図は連続動作を行った場合のロボット手。 先の軌跡を示す図、第２図及び第６図は速度パ。 ターンを示す図、第４図及び第５図は連続動作。 を行った場合のロボ＝）手先の軌跡を示す図、。 第６図は本発明の一実施例を示す概略構成図である。 １・・・ロボットシーケンス制御装置、２・・・動作シ
ーケンスレジスタ、３・・・ロボ・ソト移動制御装置、
４・・・加減速パラメータレジスタ、５・・・２点間加
減速パラメータ演算器、６・・・連続移動パラメータ演
算器、７・・・加速度判定器、８・・・パラメータ修正
器、９・・・サーボ装置。 １９　。 茅　２　図 第　３図 第　２　図 ｒ。 第　　　乙　　　図 ／ ７５−

Claims (1)

1. 【特許請求の範囲】 ロボーノトのシーケンスを制御するロボ・ソトシ。 −ケンス制御装置と、このシーケンスを記憶−ｒ。 る動作シーケンスレジスタと、ロボ・ノドの移動。 に必要なパラメータを演算する経路補間手段と、。 このパラメータを記憶する加減速パラメータレ。 ジスタと、このパラメータに基づきロボットの。 移動を制御するロボット移動制御手段とより構成された
   ロボット制御装置において、上記経路補間手段が上記ロ
   ボットシーケンス制御装置より与えられる２ケ所の移動
   目標値間における加減速パラメータを演算する２点間パ
   ラメータ演算手段と、この加減速パラメータと上記ロボ
   ・・・トシーケンス制御装置より与えられる位置データ
   および速度データとに基づいてロボ・ソトを連続させて
   動作させるのに必要な加減速パラメータを演算する連続
   移動パラメータ演算手段と。 この連続移動パラメータ演算手段で算出されるパラメー
   タに基づき加速度を判定する加速度列。 定手段と、この加速度判定手段の判定に基づき。 上記連続移動パラメータ演算手段で算出された。 パラメータの値を修正するパラメータ修正手段。 とから構成されることを特徴とするロボ・ノドの。 経路補間装置。