JP2541245B2

JP2541245B2 - ロボット軌跡制御方法

Info

Publication number: JP2541245B2
Application number: JP62277768A
Authority: JP
Inventors: 晃福田
Original assignee: Daikin Industries Ltd
Current assignee: Daikin Industries Ltd
Priority date: 1987-11-02
Filing date: 1987-11-02
Publication date: 1996-10-09
Anticipated expiration: 2011-10-09
Also published as: JPH01119809A

Description

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

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

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

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

（問題点を解決するための手段）上記目的を達成するため本発明の解決手段は、ロボッ
トのアーム手先が速度計画に基づいて始点（Ａ）から終
点（Ｂ）までの間の経路（Ｃ）を移動するように制御す
るロボットの軌跡制御方法として、上記始点（Ａ）から
現在位置までの移動距離を始点（Ａ）から終点（Ｂ）ま
での距離で徐してなる媒介関数を導入し、上記経路
（Ｃ）における速度モードを、上記媒介関数の変化率が
単調増加する加速部と、上記媒介関数の変化率が上記加
速部の終点の値と同じ一定値となる定速部と、上記媒介
関数の変化率が上記低速部の値から単調減小する減速部
とに区画した速度計画を立案し、上記媒介関数に沿って
ロボットのアーム手先の位置の補間点を生成する一方、
上記経路（Ｃ）上の地点（Ｅ又はＦ）で外部から上記速
度計画にはない停止信号が出力されたときには、上記地
点（Ｅ又はＦ）に対応する媒介関数上の点を始点とし、
この新たな始点における変化率と同じ変化率を与える媒
介関数の減速部上の点から媒介関数の終点までの部分を
変更された始点に付加してなる新たな媒介関数を導入
し、この新たな媒介関数に沿って補間点を生成しながら
ロボットのアーム手先を停止させる方法である。

（作用）以上の方法により、本発明では、２つの地点（Ａ），
（Ｂ）間の経路（Ｃ）上で基本的な速度計画に基づいて
軌跡上の補間点が生成され、該補間点にアーム手先が一
致するようにロボットが制御される。そして、上記経路
（Ｃ）の途中の地点（Ｅ又はＦ）で外部から非常停止等
による停止信号が出力されると、その地点（Ｅ）に対応
する媒介関数上の点を出発点とし、そのときの変化率と
同じ変化率を与える媒介関数の減速部上の点から媒介関
数の終点までの部分を新たな始点の前方に付加した新た
な媒介関数が導入され、ロボットのアーム手先が停止す
るまでこの新たな媒介関数に沿って補間点の生成が行わ
れる。

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

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

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

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

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

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

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

次に、上記プランナ（２）における速度計画の設定方
法について説明する。

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

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

そして、上記２点（Ａ），（Ｂ）間の経路（Ｃ）に沿
った移動を表わす各点つまり補間点の時間に対する変化
を、それまでに進んだ距離を経路（Ｃ）全体の距離で徐
したいわゆる媒介変数pct（パーセント）（媒介関数）
で表わすと、該媒介変数pctは、第４図に示すように、
上記速度計画線図に基づいて出発点（ａ）における
「０」から終点（ｂ）における「１」まで変化するもの
となる。すなわち、第３図の加速部（−１）に対応す
る加速部ではその変化率を次第に増加させる（つまり単
調増加する）正の２次関数となり、第３図の定速部（
−２）に対応する低速部では、変化率が加速部の終点と
同じ値である直線となり、第３図の減速部（−３）に
対応する減速部では変化率が低速部における値から次第
に減少する（つまり単調減小する）負の２次関数とな
る。

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

I.ジョイントモードで行う場合この場合、始点（Ａ）の位置ベクトルとして始点
（Ａ）での各関節の角度J1（ｉ）を、始点（Ａ）から終
点（Ｂ）へのベクトルとして各関節の移動量Dj（ｉ）を
用いる。上記媒介変数pctを用いて、ロボットの各関節
の位置を表わすと、 Jn（ｉ）＝Jn1（ｉ）＋pct・Djn（ｉ）（４）（ただし、１≦ｉ≦軸数）となる。

II.直線モードで行う場合この場合には、上記J1（ｉ）の代わりに始点（Ａ）の
座標（x,y,z）および姿勢の各パラメータPs1（ｉ）を、
Djn（ｉ）の代わりに各パラメータの移動量Dps（ｉ）を
用いる。そして、上記媒介変数pctを用いて目標直線上
の内挿点を表わすと、 Ps（ｉ）＝Ps1（ｉ）＋pct・Dps（ｉ）（５）（ただし、１≦ｉ≦パラメータ数）となる。

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

そして、始点（Ａ）の位置ベクトルps（ｉ）と、目
標点（Ｂ）までの角度の移動量Ｄθを用いると、内挿点
Pps（ｉ）は Pps（ｉ）＝ps＋［ｉ・｛cos（pct・Ｄθ）−１｝ −ｉ・sin（pct・Ｄθ）］・ｒ（６）（ただし、ｉ＝x,y,z、は中心から始点（Ａ）へ向う
単位ベクトル、は円弧の面内でに直交する単位ベク
トルである）となる。

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

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

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

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

したがって、上記実施例では、例えば第７図に示すよ
うに、速度計画における定速部（図中−２）途中の点
（Ｅ）で外部からの停止信号を受けた場合、直ちに停止
するための速度計画を減速時のものに切換えるように再
計画する。つまり、点（Ｅ）を始点とし、破線で示す最
初の速度計画における減速部の減速線Ｄ−Ｂ（第３図
−３）と同じ傾斜を持った減速線Ｅ−Ｂ′（第７図上図
−３）に切換え、該減速線Ｅ−Ｂ′に沿って、つまり
上記一般式（３）に基づく内挿処理によって補間点の生
成を持続しながら停止する。このことは、第７図下図に
示すように、停止信号を受けた地点（Ｅ）に対応する媒
介変数上の点（ｅ）を始点とし、点（ｅ）における変化
率と同じ変化率を与える減速部上の点（ｄ）から終点
（ｂ）までの部分を点（ｅ）の前方に付加してなる新た
な媒介変数を導入し、この新たな媒介変数に沿って補間
点を生成することに等しい。つまり、第７図下図におい
て破線で示される当初の媒介変数の点（ｄ）から点
（ｂ）の部分が点（ｅ）の前方に付加され、新たな終点
は点（ｄ′）となる。このように、媒介変数pctは、そ
の変化率が次第に減少する二次関数（減速部）に沿って
変化し、ロボットのアーム手先は軌跡上で停止すること
になる。

また、第８図上図に示すように、加速部（図中−
１）途中の点（Ｆ）で停止信号を受けた場合にも、点
（Ｆ）を始点とし、上記点（Ｆ）における速度と同じ速
度を与える減速部上の点（Ｇ）から先の減速線Ｇ−Ｂと
同じ傾斜を持った減速線Ｆ−Ｂ′（第８図上図−３）
に切換え、該再計画に基づく内挿処理によって補間点の
生成を行いながら停止する。このことは、第８図下図に
示すように、停止信号を受けた地点（Ｆ）に対応する媒
介変数上の点（ｆ）を始点とし、点（ｆ）における変化
率と同じ変化率を与える減速部上の点（ｇ）から終点
（ｂ）までの部分を点（ｆ）の前方に付加してなる新た
な媒介変数を導入し、この新たな媒介変数に沿って補間
点を生成することに等しい。つまり、第８図下図におい
て破線で示される当初の媒介変数の点（ｄ）から点
（ｂ）の部分が点（ｆ）の前方に付加され、新たな終点
は点（ｄ′）となる。このように、媒介変数pctは、そ
の変化率が次第に減少する二次関数（減速部）に沿って
変化し、ロボットのアーム手先は軌跡上で停止すること
になる。

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

（発明の効果）以上説明したように、本発明の軌跡制御方法によれ
ば、ロボットのアーム手先が基本速度計画に基づき所定
の経路を移動する軌跡の制御において、外部からの停止
指令信号が入力されたときには、当該停止指令信号が出
力された点を新たな始点とし、停止指令信号が出力され
た点における速度と同じ速度を与える減速部と点から終
点までを新たな始点に付加してなる新たな媒介関数を導
入し、アーム手先が停止するまでこの新たな媒介関数に
沿って軌跡の内挿処理を行うようにしたので、オーバー
シュートを抑制して軌跡上でアーム手先を停止させるこ
とができ、移動再開時の軌跡を確保することができる。

【図面の簡単な説明】

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

Claims

(57)【特許請求の範囲】

【請求項１】ロボットのアーム手先が速度計画に基づい
て始点（Ａ）から終点（Ｂ）までの間の経路（Ｃ）を移
動するように制御するロボットの軌跡制御方法であっ
て、上記始点（Ａ）から現在位置までの移動距離を始点
（Ａ）から終点（Ｂ）までの距離で徐してなる媒介関数
を導入し、上記経路（Ｃ）における速度モードを、上記
媒介関数の変化率が単調増加する加速部と、上記媒介関
数の変化率が上記加速部の終点の値と同じ一定値となる
定速部と、上記媒介関数の変化率が上記低速部の値から
単調減小する減速部とに区画した速度計画を立案し、上記媒介関数に沿ってロボットのアーム手先の位置の補
間点を生成する一方、上記経路（Ｃ）の上の地点（Ｅ又はＦ）は外部から上記
速度計画にはない停止信号が出力されたときには、上記
地点（Ｅ又はＦ）に対応する媒介関数上の点を始点と
し、この新たな始点における変化率と同じ変化率を与え
る媒介関数の減速部上の点から媒介関数の終点までの部
分を変更された始点に付加してなる新たな媒介関数を導
入し、この新たな媒介関数に沿って補間点を生成しなが
らロボットのアーム手先を停止させることを特徴とする
ロボットの軌跡制御方法。