JPH01119809A - ロボット軌跡制御方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、速度計画に基づいたロボットの軌跡制御方法
に係り、特にロボットのアーム手先の非常停止時におけ
る軌跡の確保対策に関する。

（従来の技術） 従来より、ロボットの軌跡制御方法において、アーム手
先が基本計画に基づく軌跡上の補間点の生成を行いなが
ら経路を移動中に、例えば装置の故障、教示等のために
外部からの停止指令信号により停止する必要が生じた場
合、直ちに軌跡」二の補間点の生成を中止し、その後の
アーム手先の減速・停止はモータ等駆動機構の制御系に
委ねられるものが一般的な技術として知られている。

（発明が解決しようとする問題点） しかしながら、上記従来の方法による場合、ロボットの
各関節の惰性等があるため、そのまま停止すると、オー
バーシュートを生じて軌跡から外れた場所で停止するこ
とになる。そして、停止した位置が軌跡上から外れてい
るために、故障等が修復されて再スタートする際、その
ままでは制御のための指令を入力できず、いったん軌跡
上又は出発地点に戻ってから再度速度計画を設定しなけ
ればならないという問題があった。

本発明は斯かる点に鑑みてなされたものであり、その目
的は、ロボットのアーム手先が移動中に外部から停止信
号を受けた場合、本来の速度計画に沿った軌跡上で停止
させることにより、移動再開時の軌跡を確保することに
ある。

（問題点を解決するための手段） 上記目的を達成するため本発明の解決手段は、ロボット
のアーム手先が速度計画に基づいた軌跡上の補間点を生
成しながら２点（Ａ）、　　（Ｂ）間の経路（Ｃ）を移
動するように制御するロボットの軌跡制御方法を前提と
する。

そして、上記経路（Ｃ）上の地点（Ｅ）で外部から上記
速度計画にはない停止信号が出力されたときに、その停
止信号を検出すると、そのときの速度計画を減速時の速
度計画に切換え、この切換えられた再計画に応じて軌跡
上の補間点の生成を続行しながら上記アーム手先を停止
させるようにする。

（作用） 以上の方法により、本発明では、２つの地点（Ａ）、（
Ｂ）間の経路（Ｃ）上を基本計画に基づく軌跡上の補間
点が生成され、該補間点にアーム手先が一致するように
ロボットが制御される。

そして、上記経路（Ｃ）の途中の地点（Ｅ）で外部から
非常停止等による停止信号が出力されると、基本計画が
減速時の速度計画に切換えられ、ロボットが停止するま
でこの切換えられた速度計画に基づく軌跡上の補間点の
生成が行われる。

よって、オーバーシュートを抑制して軌跡」二でロボッ
トのアーム手先を停止させることができ、移動再開時の
軌跡を確保することができる。

（実施例） 以下、本発明の実施例について、図面に基づき説明する
。

第２図は、本発明を適用したロボット制御装置の信号系
を示し、（１）は入力装置（図示せず）からの命令を翻
訳してその解釈どおりに実行するインタプリタ、（２）
は該インタプリタ（１）からの命令に応じて最適なロボ
ットの運動の計画としての速度計画を立案するプランナ
、（６）は該プランナ（２）で立案された速度計画に基
づきロボットのアーム手先（図示せず）の移動すべき軌
跡上の補間点を逐次算出するインタボレータ、（３）は
該インタボレータ（６）で算出された補間点についての
目標位置信号とロボットのアーム手先の実際の位置とを
比較してロボットの運動をフィードバック制御するため
のフィードバックループ部、（４）は該フィードバック
ループ部（３）からの制御信号を受けてロボットのアー
ムを駆動するためのパルス信号を出力するドライバ部、
（５）は該ドライバ部（４）の出力に応じて運動するロ
ボットの機構部である。

そして、上記フィードバックループ部（３）において、
（７）は上記プランナ（２）からの速度計画に基づく目
標位置信号と後述の補償器（１５）からのロボットのア
ーム手先の位置信号との代数差を演算する第１加え合せ
点、（８）は該第１加え合せ点（７）の演算結果に位置
ゲインに１を乗じる第１ループゲイン器、（９）は該第
１ループゲイン器（８）の乗算結果と補償器（１５）か
らのロボットのアーム手先の速度信号との代数差を演算
する第２加え合せ点、（１０）は該第２加え合せ点（９
）の演算結果に速度ゲインに２を乗じてロボットのアー
ムを駆動するためのトルク信号を出力する第２ループゲ
イン器である。また、（１２）はロボットのアーム手先
の位置信号を受けて実際の速度を演算する第１微分要素
、（１３）は該第１微分要素で演算されたロボットのア
ーム手先の速度をさらに微分して加速度を演算する第２
微分要素、（１４）は」−２ロボットのアーム手先の位
置信号と、第１微分要素（１２）で演算された速度と、
第２微分要素（１３）で演算された加速度とを加算して
いわゆる速度のＰＩＤ成分を演算する第４加え合せ点、
（１５）は該第４加え合せ点（１４）からのＰＩＤ信号
のゲインを補償するための補償器である。

次に、」二記ドライバ部（４）において、（１７）は」
−記第２加え合せ点（１０）からのトルク信号と、」二
記機構部（５）を駆動するための電流のフィードバック
信号との代数差を演算して位相遅れを補償する第３加え
合せ点、（１８）は該第３加え合せ点（１７）からのト
ルク信号をパルス信号に変換していわゆるＰＷＭ信号を
出力する信号変換器である。

そして、上記機構部（５）において、（１９）は上記信
号変換器（１８）からのＰＷＭ信号を増幅するためのパ
ワーアンプ、（２０）は該パワーアンプ（１９）で増幅
されたＰＷＭ信号に応じて回転するサーボモータ、（２
１）は該サーボモータ（２０）により駆動される関節ダ
イナミクスとしての積分要素である。

すなわち、ロボットのアーム手先の移動経路等の指令は
上記インタプリタ（１）で翻訳され、その翻訳された指
令に基づいてプランナ（２）で最適な速度計画が立案さ
れると、該速度計画に応じてインタボレータ（６）で軌
跡上の補間点つまり目標位置信号が逐次算出され、該目
標位置信号に基づきフィードバックループ部（３）で制
御信号が出力され、次に、ドライバ部（４）でＰＷＭ信
号に変換されて機構部（５）が駆動され、ロボットのア
ーム手先の運動が制御されるようになされている。

次に、上記プランナ（２）における速度計画の設定方法
について説明する。　　　□ 第１図に示すように、ロボットのアーム手先が、地点（
Ａ）を出発地点とし地点（Ｂ）を目標地点とする曲線状
の経路（Ｃ）上を移動する場合を例にとる。上記経路（
Ｃ）における速度計画は、第３図に示すように、速度が
リニアに増大する加速部（Ｃ＋）（図中■−１の部分）
と、速度が一定となる定速部（Ｃ２）（図中■−２の部
分）と、速度がリニアに減少する減速部（Ｃａ）（図中
■−３の部分）とからなる台形状の速度線図で一般的に
表される。

上記目標軌跡である経路（Ｃ）上における内挿処理につ
いて説明する。上記のような速度計画に基づく各部（Ｃ
１）〜（Ｃ３）における補間点は、一般的には、順に Ｐ＝ｆ＋　　（ｔ）＋ｓｌ）　　　　　　　　　　（１
）Ｐ−ｆ２　（ｔ）＋ｓｐ　　　　　　　　　　（２）
Ｐ−１３（ｔ）＋Ｓｐ　　　　　　　　　　（３）（た
だし、ｆ＋　　（ｔ）〜ｆ３　（ｔ）は区間固有の関数
、ｓｐは始点の座標である）のように表わされる。

そして、上記２点（Ａ）、（Ｂ）間の経路（Ｃ）に沿っ
た移動を表わす各点つまり補間点の時間に対する変化を
それまでに進んだ距離を経路（Ｃ）全体の距離で除した
いわゆる媒介変数ｐｅｔ（パーセント）で表わすと、該
媒介変数１）ｅｔは、第４図に示すように、上記速度計
画線図に基づいて零から１まで変化するものとなる。す
なわち、加速部（Ｃ１）に対応する部分ではその変化率
を次第に増加させる正の２次関数となり、定速部（Ｃ２
）に対応する部分では、変化率を一定とする直線となり
、減速部（Ｃ３）に対応する部分では変化率が次第に減
少する負の２次関数となる。

また、ロボットの制御モードの種類により、上記補間点
を生成するための内挿処理時に使用する関数は異なり、
以下の３通りについて切換可能になされている。

■、ジヨイントモードで行う場合 この場合、始点（Ａ）の位置ベクトルとして始点（Ａ）
での各関節の角度Ｊｌ　　（ｉ）を、始点（Ａ）から終
点（Ｂ）へのペクト４ルとして各関節の移動量Ｄｊ　　
（ｉ）を用いる。上記媒介変数１）ｅｔを用いて、ロボ
ットの各関節の位置を表わすと、 Ｊｎ　　（ｉ）　＝Ｊｎｌ　（ｉ）　＋ｐｃｔ−Ｄｊｎ
　（ｉ）　（４）（ただし、１≦ｉ≦軸数）となる。

■、直線モードで行う場合 この場合には、上記Ｊｌ　　（ｉ）の代わりに始点（Ａ
）の座標（Ｘ、ｙ、ｚ）および姿勢の各パラメータＰｓ
ｉ（ｉ）を、Ｄｊｎ（ｉ）（７）代わりに各パラメータ
の移動量Ｄｐｓ（ｉ）を用いる。

そして、上記媒介変数ｐｅｔを用いて目標直線上の内挿
点を表わすと、 Ｐｓ　　（ｉ）−Ｐｓｉ　（ｉ）＋ｐｃｔ−Ｄｐｓ　（
ｉ）（５）（ただし、１≦ｉ≦パラメータ数）となる。

■０円円弧間モードの場合 この場合、第５図に示すように、中心と始点（Ａ）とを
結ぶ線分と、中心と内挿点とを結ぶ線分との間の角度θ
をｐｅｔに合わせて変化させることにより、円弧軌跡上
で滑らかな加減速を行う。

そして、始点（Ａ）の位置ベクトル１ｐｓ（ｉ）と、目
標点（Ｂ）までの角度の移動ｉＤθを用いると、内挿点
ＰＩ）Ｓ（ｉ）は Ｐｐｓ（ｉ）　− ぼｐｓ＋［６ｉ　ＩＩ　（ｃｏｓ　（ｐｃｔＩＩＤθ）
　−１１−ｃｉ　　−５ｉｎ　　　（ｐｃｔ　−Ｄ　θ
）　コ　−ｒ　　　　　　　　　（６）（ただし、ｉ＝
ｘ、ｙ、ｚ、５は中心から始点（Ａ）へ向う単位ベクト
ル、σは円弧の面内で５に直交する単位ベクトルである
）となる。

また、姿勢については、直線モードと同様の（５）式で
内挿を行う。

すなわち、各制御モードに応じて上記（４）〜（６）式
を選択し、上記（１）〜（３）式に基づき経路（Ｃ）の
各部（Ｃ１）〜（Ｃ３）における軌跡上の内挿点を算出
することにより、補間点を生成してロボットのアーム手
先が基本計画に基づく軌跡上を移動するようになされて
いる。

次に、上記のような速度計画の再設定に基づいたロボッ
トのアーム手先の軌跡制御について、第６図のフローチ
ャートに基づき説明する。

まず、ステップＳ１で最初に立案された速度計画に基づ
いて内挿点を算出しく補間点の生成）、ロボットのアー
ム手先の移動する軌跡が上記算出された内挿点に一致す
るように上記サーボモータ（２０）を駆動してアーム手
先の制御を行う。そして、ステップＳ２で外部からの停
止信号が入力されたか否かを判別し、その結果がＮｏで
あれば、通常の制御状態であると判断して上記ステップ
Ｓ１を繰り返す。一方、外部からの停止信号が出力され
、ステップＳ２における判別がＹＥＳになる（地点（Ｅ
））と、非常停止時であると判断してステップＳ３に進
み、速度計画を減速時の速度計画に切換えるように再計
画する。そして、ステップＳ４で、アーム手先が停止す
るまで、上記再計画に基づく内挿点の算出（補間点の生
成）とアーム手先の制御とを行う。

したがって、上記実施例では、例えば第７図に示すよう
に、速度計画における定速部（Ｃ２）（図中■−２）途
中の点（Ｅ）で外部からの停止信号を受けた場合、直ち
に停止するための速度計画を減速時のものに切換えるよ
うに再計画し、最初の速度計画における減速部（Ｃ３）
の減速線（第３図■−３）と同じ傾斜を持った減速線（
第７図」二図■−３）に切換え、該減速線に沿って、つ
まり上記一般式（３）に基づく内挿処理によって補間点
の生成を持続しながら停止する。そのとき、媒介変数ｐ
ｅｔは同下図に示すように、その変化率が次第に減少す
る二次関数に沿って変化しながら軌跡」二で停止するこ
とになる。

また、第８図上図に示すように、加速部（図中■−１）
途中の点（Ｆ）で停止信号を受けた場合にも、速度計画
を基本速度計画における減速部（Ｃ３）の減速線（第３
図■−３）と同じ傾斜を持った減速線（第８図上図■−
３）に切換え、該再計画に基づく内挿処理によって補間
点の生成を行いながら停止し、媒介変数ｐｅｔは同下図
に示すように、その変化率が次第に減少する二次関数に
沿って変化しながら軌跡上で停止することになる。

よって、従来のように停止点が軌跡上からずれることが
なく、オーバルシュートを抑制して軌跡上でアーム手先
を停止させることができ、移動再開時の軌跡を有効に確
保することができるのである。

（発明の効果） 以上説明したように、本発明の軌跡制御方法によれば、
ロボットのアーム手先が基本速度計画に基づき所定の経
路を移動する軌跡の制御において、外部からの停止指令
信号が入力されたときにも、アーム手先が停止するまで
再計画に基づいた軌跡の内挿処理を行うようにしたので
、オーバーシュートを抑制して軌跡上でアーム手先を停
止させることができ、移動再開時の軌跡を確保すること
ができる。

【図面の簡単な説明】

図面は本発明の実施例を示し、第１図はロボットのアー
ム手先の移動する経路図、第２図はロボットの制御装置
の構成を示すブロック線図、第３図は基本速度計画に基
づく経路上の速度線図、第４図は第３図の速度線図に対
応する媒介変数の変化特性図、第５図は円弧補間モード
におけるベクトルの説明図、第６図は軌跡制御方法を示
すフローチャート図、第７図上下図はそれぞれ定速部に
おける非常停止時の速度および媒介変数の変化を示す図
、第８図上下図は加速部における上記第７図上下図に相
当する図である。 （Ａ）・・・出発地点、（Ｂ）・・・到達地点、（Ｃ）
・・・経路、（Ｅ）・・・非常停止開始地点。 特　許　出　願　人　　ダイキン工業株式会社代　　理
　　人　　　　弁理士　前　１）　弘第３図 ○ 時間を 第４図 峙面ｔ

Claims (1)

【特許請求の範囲】
1. （１）ロボットのアーム手先が速度計画に基づいた軌跡
   上の補間点を生成しながら２点（Ａ）、（Ｂ）間の経路
   （Ｃ）を移動するように制御するロボットの軌跡制御方
   法において、上記経路（Ｃ）上の地点（Ｅ）で外部から
   上記速度計画にはない停止信号が出力されたときに、そ
   の停止信号を検出すると、そのときの速度計画を減速時
   の速度計画に切換え、この切換えられた再計画に応じて
   軌跡上の補間点の生成を続行しながら上記アーム手先を
   停止させることを特徴とするロボットの軌跡制御方法。