JPS5936811A - ロボツトハンドの経路補間方法 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 本発明は、経路補間方法に関する。

ロボットの移動経路の操作方法の１つに、ＰＴＰ（ｐｏ
ｉｎｔ ｔｏ ｐｏｉｎｔ）動作がある。ＰＴＰ動作は、
経路上の有限個の通過点を指定し、この指定通過点に従
って経路移動をさせる動作である。

従来、ＰＴＰ動作を連続して行う場合に、各ポイント（
位置）毎に停止しないで通過するやり方がとられる。そ
の際、ポイントからポイントへの折線部分は円弧で補間
させる。例えば、２本の相隣り合う折線との間では、該
２本の折線を指定された半径を持つ円弧で連続化させる
。

かかる従来例では、折線をもとにした円弧の計算が複雑
であること、その結果、ロボットの経路移動前の計算時
間に多大の時間を要し、ロボットの経路移動を実時間で
行い得ないとの欠点を持つ。更に、関節形ロボットの各
関節の動きを座標にとった関節角動作でのＰＴＰ動作高
速化への応用ができにくい欠点を持つ。

本発明の目的は、ＰＴＰ動作の高速化を達成してなる経
路補間方法を提供するものである。

本発明の要旨は、ロボットの経路を決定するために３点
以上の点をＰＴＰ動作で結ぶ場合に、中間点となる折点
の手前で折点方向へは速度を単調減少させると同時に、
折点から次の点方向へは速度を単調増加させる補間演算
を用い、ロボットのハンドが折点近傍で速度不連続とな
らないように経路を円滑化させた点にある。

以下、図面により本発明を詳述する。

第１図は３点Ａ、Ｂ、Ｃを与えて、Ａ→Ｂ→Ｃなる経路
に従ってロボットのハンドを移動させる事例の説明図で
ある。この３点Ａ、Ｂ、Ｃはティーチングによって教示
された値であり、メモリにティーチングデータとして格
納されている。

今、８間を速度υ、、ＢＣ間を速度υｂとで移動させ、
且つこの移動はサンプリング時間Ｔ単位に行うものとす
る。従って、ＡＢ間のサンプリング点数をｎａ、ＢＣ間
のサンプリング点数をｎｂとすると、 の関係となる。ここに〔〕なるカッコの意味は、例えば
（１）式では、ｎ、ａは、 の範囲の自然数であることを示す。従って、各々のサン
プリング点に卦ける目標点は、それぞれＡＢをｎａ等分
、ＢＣをｎｂ等分する点となる。

しかし、この方法でロボットのハンドを駆動すると、点
Ｂで速度が不連続となり、駆動不能となる。

そこで、本発明では、点Ｂに到達する前に点Ａから点Ｂ
に到達する前に、点Ａから点Ｂ方向へは点Ｂに到達した
時に速度ゼロになるように速度を単調減少し、点Ｂから
点Ｃ方向へは速度をゼロから単調増加するように経路を
補間する。

第２図と第３図で本発明の基本的な考え方を説明する。

第２図に示すように、点Ａから点Ｂへの方向を単位ベク
トル６５１点Ｂから点Ｃへの方向を単位ベクトルｅ、と
し、線分ＡＢ間の定速域の速度をυ６．線分ＢＣ間の定
速域の速度をυｂとする。更にベクトルｅ、方向の減速
時間とベクトルｅｂ方向の加速時間を同一とし、その値
をｔ４とする。更に１ベクトルｅａ方向の速度単調減少
を等減速度α、ベクトルＪ方向の速度単調増加を等加速
度すとする。かかる関係のもとでは、ａ，ｂは、となる
。

点Ｂを位置ベクトルＰｂ１線分ＡＢ上の減速開始点を点
Ｄ、その位置ベクトルをＰｄ、線分ＥＣ上の加速終了点
を点Ｅ１その位置ベクトルをＰ。

とすると、位置ベクトルＦｄ、Ｐ、はそれぞれ以下とな
る。

以上の如き経路補間によれば、Ａ→Ｄの経路では定速度
υａの移動を行い、Ｄ→Ｈの経路では定減速度αに従っ
て減速し、点Ｂでは速度ゼロとなる。次いで、Ｂ→Ｈの
経路では定加速度ｂに従って速度ゼロから連続的に速度
を増してゆき、点Ｅでは定速度υとなる。次いで、Ｅ→
Ｃの経路では、定速度υｂに従って移動する。

以上のＡ→Ｄ→Ｂ→Ｅ→Ｃの経路移動では、速度は連続
的であり、且つ補間域前後の速度も連続することとなり
、円滑な補間を達成できる。

以上はＤ→Ｈ、Ｂ→Ｅの間で速度補間を行う事例である
が、速度補間と共に位置補間を行場合を次に説明する。

第３図は速度補間と位置補間とを実現させてるＰＴＰ動
作の説明図である。横軸に時間、縦軸に速度をとってい
る。特性ｌ１はＢ→Ｅ→Ｃでの速度特性を示し、特性ｌ
２はＡ→Ｄ→Ｂでの速度特性を示す。然るに、特性ｌ１
と４とを同一時間軸でとらえ、図の時間軸でＰＴＰ動作
を行わせる。

即ち、ハンドに対して、ＡＢ間は定速度υ、で移動させ
る。点Ｄで２つの方向Ｑ欣へのベクトルｅａ，ｅｂを与
える。このベクトルは時間ｔ、の間与える。この２つの
ベクトルｅａ、ｅｂによって、移動経路はＤ→Ｂ→Ｅ→
Ｃとはならず、Ｂを介さずに、ＤからＥ方向にむけて、
ベクトルｅ、、ｅｊとの合成ベクトルに従った曲線を描
きながら移動する。ｔ、時間後、位置Ｅに到達し、ＥＣ
間は定速度υｂで移動させる。

第４図に、点Ｄで２方向辿最のベクトルｅ。

ｅ６を与えた場合の点Ｄから点Ｅへの移動経路を示す。

点Ｄから点Ｅへの移動経路は本来連続的であるが、サン
プリング点に従って移動させているため、その経路は不
連続となっている。サンプリング点の考え方は、９３間
を例えば６点（Ｄ、、Ｄ、、Ｄ、、Ｄ、、Ｄ、、Ｂ）サ
ンプルし、ＤＥ間を６点（Ｅ１、Ｅ２、Ｅ３、Ｅ４、Ｅ
５、Ｅ）サンプルし、各サンプル点の合成点毎にベクト
ルｅａ、ｅｂを与えて合成ベクトルる得る。この合成ベ
クトルで形成される経路（Ｄ→Ｐ１→Ｐ２→Ｐ３→Ｐ４
→Ｐ５→Ｅ）が各サンプル点の合成点対応位置となる。

以上でのＤ→Ｅの経路が位置補間された経路を示す。

以上の位置補間での位置ベクトルは以下となる。今、点
Ｄでの時間をｔ＝０とする。ベクトルｅ４方向速度成分
はα（ｔｎーｔ）、ベクトルｅｂ方向速度成分はｂｔで
ある。従って、ｔ時間後の移動成分は、ベクトルｅａに
関しては−ｉα（ｔい−６）ｔ１ベクトルｅｈに関して
は、ｈｔ＊となる。これにより、点ＤＥ間の位置ベクト
ルＰ（ｔ）は、 となる。これより、速度を求めると、 となる。加速度は となる。かかる（９）、（１０）式より、ＤＥ間では速
度は連続であり、且つ補間域前後の速度は連続である。

次に、位置補間の他の例を第５図で説明する。

第５図は、３個の経路１，２．３を示す。経路１は、Ｆ
Ｇなる経路を持つ事例であり、経路２はＤＥなる経路を
持つ事例であり、経路３はＨｌなる経路を持つ事例であ
る。ここで、Ｆ点は、点ＤからＡよりに、等速度υ。で
時間ｔ６だけ移動した点とする。更に、Ｈ点は、点Ｂか
らＤよりに加速度αで時間（ｔ、−６Ｊ）だけ移動した
点とする。

点Ｄは第４図で示した点Ｄと同一点を示す。

（１）先ず、Ａ→Ｆ→Ｇ→Ｃなる経路１では、各区間Ａ
Ｆ、ＦＧ、ＧＣ毎に次の如き制御を行う。

（１−１）区間ＡＦの時、即ちｔく０の時。

ｔ＜０としたのは、Ｆ点を時間ｔ＝０としているために
よる。この区間では、ベクトルｅ６方向へは定速度移動
、ベクトルｅＡ方向へは移動ベクトルを与えない。この
結果、移動はＡＰの方向となる。この区間での位置ベク
トルｔｐ（ｔ）は、となる。速度は、 となる。

（１−１１）区間ＦＤに相当するＦＧでの相当区間、即
ち、０≦１＜１．の時。

この区間では、ベクトルｅ６方向へは定速度υ６移動、
ベクトル町方向へは定加速度運動を与える。この結果、
ＦＧの方向にむけて移動点は曲線変化を起す。この区間
での位置ベクトルＦ（ｔ）は、又は となる。速度は、 となる。

（１−１１１）区間ＤＢに相当するＦＧでの相当区間、
即ち、ｔ６≦１＜１．の時。

この区間では、ベクトルｅ６方向へは定減速度移動、ベ
クトル１．方向へは定加速度移動の運動を与える。この
結果、（１−１１）で形成された経路に引き続く経路と
して経路１上にベクトルｅ１とＪとで合成された合成ベ
クトルを得る。

この区間での位置ベクトルｐ（ｔ）は、又は、 となる。速度は、 となる。

（１−１υ）ｔ、、≦ｔ＜ｔｎ＋ｔ、の時。

この区間では、ベクトルｅ１方向へは定減速度移動、ベ
クトル１に方向へは定速度移動の運動を与える。位置ベ
クトルＦ（ｔ）は 又は となる。速度は、 となる。

（１−υ）ｔ≧ｔ、＋ｔａの時。

この区間では、ベクトルｅａ方向へは移動なしで、ベク
トルｅ１方向へは定速度移動を行わせる。

この時の位置ベクトルｐ＜ｔ）は、 又は、 となる。速度は、 となる。

以上の（１−１）〜（１−Ｉυ）で、各区間内における
速度の連続性は明らかであるから、次に各区間の境界点
での連続性を示す。今、速度ベクトル、（ｔ）を与える
と、 となる。微小時間をＯとすると、 となる。これにより、速度は全区間の境界点でも連続性
を示す。

（２）次に、Ａ→Ｈ→Ｉ→Ｅ→Ｇ→Ｃなる経路３では、
以下となる。但し、点Ｈは、点Ｄから点Ｂの方向へ時間
１．だけ寄った時間での位置である。

今、点Ｈでの時間をｔ＝０とする。この時、点Ｈの位置
ベクトルＰＡ及び位置Ｉの位置ベクトルＰ、は、 となる。経路３上での位置ベクトルＰ（ｔ）は、各区間
で次のようになる。

以上の各区間内における速度の連続性は明らかである。

各区間の境界点でも連続性を有する。

即ち、 となるためによる。

以上の説明から次のことが明らかとなる。Ａ→Ｂ→Ｃな
る経路の他に、届の経路上のどの時点からでも曲線（上
記実施例では、サンプルしている故に、折点による曲線
となる。但しサンプル区間を小さくしてゆけば連続的な
曲線と同じとなる）に従った補間が可能とガる。且つＡ
Ｂの経路上のどの時点からによる補間であっても、経路
上のすべての点に沿って速度の連続性を持つことができ
る。これにより、ロボットハンドの駆動が容易となる。

次に、各経路１，２，３．４での点Ｆから点Ｇまでの移
塀１時間を比較する。

（１）経路１での移動時間。

ベクトルｅａ方向成分では最初の時間ｔ６の間は定速度
移動、次の時間ｔ４の間は等減速度移動である。ベクト
ルＣ１方向成分では、最初の時間ｔ。

の間は、等加速度移動、次の時間ｔ、の間は定速度移動
である。従って、移動時間１ｔはとなる。

（２）経路２での移動時間。

経路をＦＤ、ＤＥ、ＥＧと３分割して考えると、区間Ｆ
Ｄでは、時間ｔａの間、ベクトル上。方向成分のみ定速
度でＳ動し、区間ＤＥでは、ｔ、、の間は、ベクトルｅ
ａ方向成分では等減速度移動であり、同時にベクトルＣ
１方向成分では等加速度移動である。

区間ＥＧでは、時間ｔ、の間は、ベクトルＣ１方向成分
のみ定速度移動であるから、全区間の移動時間は、区間
ＦＤとＤＢとＥＧでの総移動時間となる。

従って、移動時間１ｔは、 （３）経路３での移動時間。

経路３を、区間Ｆｌ）、ＤＨ，Ｅｌ、ＩへＥＧと５分割
してそれぞれの区間での移動時間は、 区間ＦＤの移動時間＝ｔ゜ 区間ＤＥの移動時間＝ｔｈ 区間Ｈ１の移動時間＝ｔ．−ｔ゜ 区間ＩＥの移動時間＝ｔ易 区間ＥＧの移動時間＝ｔ゜ 従って、経路３での移動時間ｔｌは、 となる。

（４）経路４での移動時間。

経路を区間ＰＲと区間ＢＧと２分割する。区間ＦＥでは
、ベクトル上。方向成分のみの定速度移動であり、区間
ＦＢの距離は、−＞υ、ｔ、＋υ、ｔ、であるから、こ
の距離を定速度υ、で移動すると、移動時間は、となる
。同様に区間胎では、距離は、Ｕ４であり、定速度で移
動してかかる時間は、となる。従って、経路４の移動時
間１ｔはとなる。

以上の４つの経路の比較をすると、線分ＡＢから線分Ｂ
Ｃへの移行をＡ→Ｂ→Ｃなる経路ではなく、減速開始点
Ｄより点Ａの方向へ寄ったＦで開始すると、Ａ→Ｂ→Ｃ
なる経路４の場合よりも移動時間を短縮することができ
る。

第６図は、本発明の処理システムの実施例図を示す。プ
ロセッサ１はバス２を介してメモリ３、テーブル４、乗
除算器５．出力ポート６。

入力ポート７、ティーチングボックス８とインターフェ
ースする。出力ポート６、入力ポート７はロボット本体
９とバス２との間の入出力部を形成する。

プロセッサーは各種の演算制御、及びバスを介したメモ
リ３．テーブル４９乗除算器５．出力ポート６、入力ポ
ート７、ティーチングボックス８の管理を行う。メモリ
２は、演算制御用のプログラム及び各種のデータを格納
する。テーブル４は、三角関数、逆三角関数等を格納す
るデータテーブルでおる。

乗除算器５は乗除算専用のハードウェアである。ティー
チングボックス８は、ティーチング時のマン−マシンイ
ンターフェイス用のコンソールである。このティーチン
グボックス８によって指示されてロボットの駆動系或い
はセンサから得られるティーチングデータはメモリ３に
格納される。

第５図に示した経路の中のどれを補間用として選択する
かは、動作モード及び各ティーチングデータによって事
前に設定しておく。この選択の指示はプロセッサ１が行
う。

動作を説明する。

先ず、ティーチングボックス８からの指示によりロボッ
ト本体９の所定の部位を所定の位置まで駆動し、この位
置におけるアクチュエータの変位検出器（図示せず）の
値を読み取り、この値からハンドの空間内における位置
を計算しティーチングデータとしてメモリ３に格納する
。

実際の操作時には、先ずメモリ３から動作モードに従っ
てティーチングデータを読出す。次にティーチングボッ
クス８による指示（例えば速度の指示）に従って、ティ
ーチングデータの点間の補間処理を行う。

点間の補間処理を第５図の事例で説明する。

先ず、定速度υａ、υｂ、及び加速度α、ｂ、選択経路
を指定する。この指定はプロセッサ１が行う。

プロセッサ１は上記各指定値に基づき選択経路に従った
補間処理を行う。この補間処理とは、上記選択経路の細
分化、細分化した区間内での方向ベクル及びその速度等
の指定を云う。乗除算器５け各区間毎に補間演算を行う
。補間演算は前述した各種の演算式であり、乗算、除算
、加算、減算の組合せより成る単純な演算に係る。

乗除算器５の出力は、テーブル４の内容を利用してハン
ドの対偶変位に座標変換を受け、出力ポート６を通じて
ロボット本体７に送られアクチュエータの作動を行う。

この際の座標変換の意味は以下となる。ティーチングデ
ータはアクチェエータの変位検出器の検出位置でのデー
タであり、実際のアクチュエータの位置とは異なる。ま
た、ハンドは対偶変位として操作入力を受けて作動する
。かかる対偶変位を出力するに必要な処理が座標変換処
理である。この処理はプロセッサ１が行う。かかる実施
例によれば演算内容は簡単であり且つ速度の連続性を保
持できる。実時間でハンド補間制御が可能と浸った。２
本の線分ＡＢ、ＢＣが第９図に示す如く一直線上にある
場合の経路補間を説明する。第７図、第９図に示すごと
く、まず線分ＡＢ上を定速度υ４で移動中時刻Ｔ、で等
減速度αで減速し時刻Ｔ２で速度ゼロとなるとともに点
Ｂに達するように等減速度α及び減速開始点Ｇ１を決定
する。この減速に要した時間と同じ時間点Ｂから点Ｃ方
向へ加速し時刻Ｔ３で速度υｂとなるように等加速度す
及び加速終了点Ｇ２を決定する。

次に第９図点Ｇ１と点Ｇ２の区間を点Ｇｈ時刻Ｔ。

に等減速度αで減速を開始すると同時に等加速度ｂで加
速を開始すると、第８図に示す速度曲線となり、時刻Ｔ
、’において第９図に示す点Ｇ、に達する。

次に移動時間について示す。

減速に要する時間をｔ、、とすると、等減速度ａ＝−υ
、７ｔ、、等加速度ｈ＝ｔｌυｔ、であり、区間Ｇ、Ｂ
の距離は−υ＾、区間Ｇ、Ｂの距離はｉυ＾である。従
って 〔区間Ｇ、Ｂを等速度υ。で移動するに要する時間〕＋
〔区間ＧｔＢを等速度υｂで移動するに要する時間］ また、〔区間ＧＩＧ、を等加速度（ｂ−α）で移動する
に要する時間〕＝〔区間ＧＩＧ、の距離〕／（ｉ＝ｔ％ となる。

従って本発明の経路補間方法を用いると、移動時間は線
分初上を一定速度υａｔ線分に上を一定速度ｕ４で移動
させた場合と等しくすることができる。

２本の平行な直線ＡＢ、ＢＣがあって、両者の中継経路
を補間する場合にあっても本発明は適用できる。更に、
ＰＴＰ動作としてはデカルト座標系における直線だけで
なく、ロボットのアクチュエータの変位を座標軸にとっ
た座標系での直線駆動の場合にも適用できる。

本発明によれば、速度の連続性を維持したままの補間制
御を達成できた。且つその補間処理内容は簡単であるた
め、補間処理時間が短縮でき、補間制御を実時間で行え
た。

【図面の簡単な説明】

第１図はＰＴＰ動作説明図、第２図は本発明の原理説明
図、第３図は経路説明図、第４図は経路の補間の説明図
、第５図は多数の経路補間の説明図、第６図は本発明の
実施例図、第７図。 第８図、第９図は一直線上のＰＴＰ動作における補間の
説明図である。 １・・・プロセッサ２・・・バス ３・・・メモリ４・・・テーブル ５・・・乗除算器６・・・出力ポート ア・・・入力ポート ８・・・ティーチングボックス ９・・・ロボット本体

Claims (1)

1. 【特許請求の範囲】 １、３点以上の点を与えて、ＰＴＰ動作に必要な経路を
   補間により決定し、該決定した補間経路に従ってロボッ
   トハンドの操作を指示するロボットハンドの経路補間方
   法に於いて、中間点となる折点の手前で折点方向へは折
   点て速度をゼロになるように単調減少させ、折点から次
   の点の方向へは速度ゼロから速度を単調増加させるべく
   補間経路を決定してなるロボットハンドの経路補間方法
   。 ２、３点以上の点を与えて、ＰＴＰ動作に動作に必要な
   経路を補間により決定し、該決定した補間経路に従って
   ロボットハンドの操作を指示するロボットハンドの経路
   補間方法に於いて、中間点となる折点の手前で折点方向
   へは折点て速度をゼロになるように単調減少させる方向
   のベクトルを与え、折点から次の点の方向成分として速
   度ゼロから速度を単調増加させる方向のベクトルを与え
   、該２つの方向のベクトルを合成させこの合成により得
   られる経路を補間経路として決定してなるロボットハン
   ドの経路補間方法。