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

Abstract

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

Description

【発明の詳細な説明】 〔発明の利用分野〕 本発明は、ロボットハンドの姿勢につい【の経路補間方
法に関する。

（発明の背景〕 ロボットの移動経路の操作方法の１つに、ｐ　’ｒ　ｐ
（ｐｏｉｎｔ　ｔｏ　ｐｏｉｎｔ　）　ｇ）作がある。

ＰＴＰＩＩｔｌＩ作は、経路上の有限個の通過点を指定
し、この指定通過点に従って経路移動をさせる動作であ
る０ 従来、ＰＴＰ鯛作を連続して行う場合に、各ポイント（
位置）毎に停止しないで通過するやシ方がとられる。そ
の際、ポイントからポイントへの折ｉｔＭｓ分の補間は
並進成分だけについて行なわれ、回転成分については考
慮されていなかった。特に、速度制御方式で軌道制御す
る場合にはこの回転成分にクーて全く考慮されていなか
った。

（発明の目的〕 本発明の目的は、回転成分につい【考慮したロボットハ
ンドの姿勢にりいての経路補間方法を提供することにあ
る◇ 〔発明のａ袂〕 本発明の要旨は、６点以上の点を与え、中間となる折点
までは第１の回転軸まわりに回転移動し、前記折点から
次の点までは第２の回転軸まわシに回転移動するＰＴＰ
ｗＪ作に必要な経路を補間に７：シ次定するにあた９、
前記第１の回転軸が前記折点までまわすべき内閣だけ前
記第１の回転軸まわシに前記ｗＪ２の回転軸を逆回転さ
ぜ、前記折点の手Ｉσで前記第１の回転軸まわりの回転
速度を単画減少させると共に、前記逆回転してできた新
しい回転軸まわシの回転速度を単調増加させる補間演算
をする。

しかして、ロボットハンドが折点近傍で速度不連続とな
らな−ように姿勢についての経路を円滑化させ、しかも
ＦＴＰ動作の高速化を図る。

〔発明の実施例〕 以下本発明の一実施例を図面を参照して説明する。

先ず、本発明の詳細な説明する前に、第１図乃至＠４図
に基づき、四ボットハンドのハンドを並進移動させる方
法を説明する。

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

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

しかし、この方法でロボットのハンドを駆動すると、点
Ｂで速度が不連続となり、駆動不能となる、 そこで、点Ａから点Ｂに到達する前に、点Ａから点Ｂ方
向へは点Ｂに到達した時に速度ゼロになるように速度を
単調減少し、点Ｂから点Ｃ方向へは速度をゼロから単調
増加するように経路を補間する。

第２図と第６図で斯かる補間方法の基本的な考え方を説
明する。

鴫２図に示すように、点Ａから点Ｂへの方向を単位ベク
トル０４１点Ｂから゛点Ｃへの方向を単位ベクトルちと
し、線分−ＡＢ間の定速域の速度ｋ　ｖａｏ庫ＢＣ間の
定速域の速度を５とする。

更にベクトルｅ、方向の減速時間とベクトル町方向の加
速時間を同一とし、その値をｔ７Ｌとする。史に、ベク
トル・４方向の速度単調減少を等減速度４、ベクトル町
方向の速度単調増加を等加速度すとする。かかる関係の
もとでは、α。

ｈは、 ？４ ４＝−・・・・・川・・・・・・・（４）１ − ｂ＝−・・・・・・・・・・・・・・（５）ｔ？Ｌ となる。

点Ｂを位置ベクトル内、線分ＡＢ上の減速開始点Ｄ１そ
の位置ベクトル１に’（１，１Ｍ分ＢＣ上の加速終了点
を点Ｅ１その位置ベクトルをＰ４とすると、位置ベクト
ルＰ、　、　Ｐ４はそれぞれ以下となる。

ＰＩ　＝町−７’　”ｚ　’ａ ＝Ｐｂ−７−を−４１０１１０１（６）Ｆ４＝Ｆ、　十
−；ｈｔ”−ｅ。

＝Ｐｈ　”　−？ｈ’ｎ＠４　・・・・・・・・・・・
・・・・＋７３以上の如き経路補間によれば、Ａ→Ｄの
経路では定速度ｙユの移動を行い、Ｄ→Ｂの経路では定
減速ｇａＫ従って減速し、点Ｂでは速度ゼロとなる。次
いで、Ｂ→Ｅの経路では定加速度りに従って速度ゼロか
ら連続的に速度を増してゆき、点Ｅでは定速度Ｖ、とな
る。次いで、ＥｉＣの経路では、定速度−に従って移動
する。

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

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

第６図は速度補間と位置補間とを実現させてるｐ’ｒｐ
動作の説明図である。横軸に時間、縦軸１に速度をとっ
ている。ｑｔ性ハはＢ→Ｅ→Ｃでの速度臀性を示し、特
性１２はＡ　−＊　Ｄ→Ｂでの速度喘−＆を示１゜然る
に、特性ハとＪ−２とを同一時間軸でとらえ、図の時間
軸でＦＴＰ　動作を行わせる。即ち、ハンドに対して、
１間は定速度のベクトルｅａ　、　ｅｈを与える。この
ベクトルは時間ｔ７Ｌの間与える。この２つのベクトル
ｅａ、ｅｂによって、移動経路はＤ　−４Ｂ→Ｉｌｉ　
−＊　Ｃとｔａ　ｒｘ、らず、ｂｔ介さずに、ＤからＥ
方向にむけて、ベクトル響α、ｔｂとの合成ベクトルに
従った曲線を描きながら移動する。ｔｎ時間後、位置Ｂ
Ｖｃ到達し、ｉｃ間は定速度ｒ、で移動させる。

第４図に、点りで２方向ＤＢ　、　Ｂ１１１のベクトル
＠ａ　、　ａｈを与えた場合の点りから点Ｅへの＃動経
路を示す。点りから点Ｅへの移動経路は本来連続的でお
るが、サンプリング点に従って移動させているため、そ
の経路は不連続となっている。サンプリング点の考え方
は、ＤＢ間を例えば６点（Ｄ、　、Ｄ、、Ｄ、、Ｄ４．
Ｄ、、Ｂ　）サンダルしｉ間を６点（Ｅｉ、　、ｊｌ＋
、、ｌｌｉ、、１ｉｔ４．Ｅｊ、、Ｅ）サンプルし、各
サンプル点の合成点毎にベクトルｅｔＬａｈを与えて合
成ベクトルを得る。この合成ベクトルで形成される経路
（Ｄ　−ｅ　１”、→Ｐ２→Ｐ、→Ｐ４Ｐ、→Ｂ）が各
サンプル点の合成点対応位置となる。以上でのＤ→Ｅｏ
Ｍ路が位置補間された経路を示す。

以上の位置補間での位置ベクトルは以下となる。今、点
りでの時間１ｔ＝Ｑとする。ベクトルｅａ方向速度成分
は”Ｃ’ｎ”’−’）、ベクトル１方向速度成分はｂｔ
である０従って、を時間後の移ｍｌＪ成分は、ベクトル
ｃａに関してはｍ−４る０これによ９１点１１］１ｆ１
４の位置ベクトルＰ　ｉｔ）は、 ２ Ｆｉｔ）＝ｌＰＡ　−ａ　（ｔ？Ｌ−ｔ）’　＠、十Ｔ
　Ａｔ　ｅ、曲回−（８）となる。これよル、速度をめ
ると、 −’ｌ？Ｌｅａ＋　（ｂｔｂ−”ａ　）ｔｄ”　Ｉ’１
ｔ） ｄｔ２ｂ″ ？ｈＩｂ−ｖａ＠α ｔ？Ｌ ＝　−Ｍ　・・−・・・・・・・・・・・・（１０）と
なる。かかる１９）、（１０）　式より、ＤＩ間では速
度は連続でめ９、且つ補間域前後の速度は連続でめる。

以上が、ロボットハンドを並進＃鯛させる経路補間方法
の一例で必る。しかし、斯かる経路補間方法を姿勢を制
御するために回転成分に対してその′ｉま適用すると、
特殊な場合を除いて不都合が生じる。つまり、第５図に
示すようにＡ点からＢ点までは単位ベクトルｗ＋を回転
軸として回転速度θ１で回転移動し、Ｂ点から０点まで
は単位ベクトルＷ２を回転軸として回転速度θ２で回転
移動しＦＴＰ動作でロボットの姿勢を変化させる場合に
、上述の並進移動における補間方法をそのまま適用して
、Ｂ点に達する前に回転軸ＷＩ′ｆ、中心とする回転速
度θ１を単調減少すると共に回転軸Ｗ！を中心とする回
転速度θ２を単調増加するだけでは次に述べるような不
都合がおる。

第６図にお−て、位置ベクトルＰを単位ベクトル・１の
まわりに角度ψ量だけ回転させたものを位置ベクトルＰ
とし、次にこの位置ベクトルＰを単位ベクトルｅ！のま
わりに角度ψ２だけ回転させたものを位置ベクトルＰ　
とする。この位置ベクトルＰ　は、回転の順−ｔを逆に
して、位置ベクトルＰを先ず単位ベクトルｅ２のまわシ
に角度９２だけ回転し次にこれを単位ベクトル１のまわ
りに角度ψ菅だけ回転して得らｎる位置ベクトル（図示
せず）と、哲殊な場合を除き、一般的に一致しない。

従って、第５図に示す単位ベクトルＷ＋［わりの回転移
Ｊ＠が終了する削に単位ベクトルＷ２まわυの回転を始
めると、単位ベクトルｙ＋ｉわシＶｃｊＡりていた分の
回転を終了したときの姿勢は、単位ベクトルｗ＋まわシ
の回転ｔ−終った後に単位ベクトルＷ２まわシに回転さ
せたときのどの姿勢とも一致しない。即ち、回転軌道か
らずれたものとなってしまう。

従って、本発明では次のように回転成分にっ−て補間金
し、上述の不都合を回避する。

第７図において、第２の回転軸としての単位ベクトル＠
２を第１の回転軸としての単位ベクトル［Ｆ］１の動き
にリンクする。即ち、単位ベクトルｅ１まわシに回転角
αだけ回転移動し、単位ベクトルｅ２まわシに回転角β
だけ回転移動する場合には、単位ベクトルｅｚを単位ベ
クトルｅｌ′！ワｐに角度αだけ逆回転させた単位ベク
トルｔ１２′をめる。そして、単位ベクトル＠言まゎシ
の回転移動に対しては、第１図に示す折点Ｂで回転速度
（角速度）がゼロとなるように速度を単調減少させ、単
位ベクトルｅ２まわシの回転移動に対しては前述の単位
ベクトル　、Ｉまわシの回転移動における角速度を単調
増加させるように、ロボットハンドの姿勢についての経
路の補間演算を、第１図乃至第４図を参照して説明した
並進移動の場合と同様に行なう。

このように、単位ベクトル＠２′をめ、これを基準とし
て並進移動の場合と同様に補間演算を行なうときに、第
６図で説明した不都合が生じないのは次の理由による。

第７図において、位置ベクトルＰを単位ベクトル命２　
まわりに回転角βだけ回転したものが位置ベクトルＰ′
であ夛、これを単位ベクトルＣＩまわシに回転角αだけ
回転したものが位置ベクトル２　である。一方、単位ベ
クトル命Ｉまわシに回転角αだけ回転したものが位置ベ
クトルＰ′であり、これを単位ベクトルｅ宏まわシに回
転角βだけ回転して得られる位置ベクトルＰ″″は前ｌ
己ベクトル１１！：全く一致する。

これを数式で示すと次のようになる。

一般に、単位ベクトルの回転軸Ｗのまわシ罠ベクトルＰ
を回転角θだけ回転して得られるベクトルＰは次式でめ
られる。

ＩＰ　−Ｆｃｏｓθ＋（１−ｔｍθバｗ　、　Ｐ　）　
ｗ十ｗｘ　Ｐｗθ・−・・−（１１）これを単に ＩＰ　ｗ　ｒｏｔ　（ｗ　、θ）　ＩＰ−・−（１２）
と表わすと、とのｍ　（１２）式によシ、第７図のベク
トルａ２．　Ｆ　、　Ｆ　の関係は次のように示すこと
ができる。即ち、 ｅ、′＝　ｒｏｔ　（ｅ＋　、−α）ｃｉ　・・・・・
・（１６）［２＝　ｒｏｔ（ｅ＋、α　）　（ｒｏｔ（
６ｇ’　、β）（Ｐ　）−（１４）としてめた位置ベク
トル〆′と ［Ｐ　ｇ＝　ｒｏｔ（ｅ＊、／）（ｒｏｔ（ｃ＋、ａ）
［Ｐ）−・−（１５）としてめた位置ベクトルＰ　は全
く等しい。

従って、第５図に示す回転１１１Ｗ＝を、回転軸ｗ＋の
回転とりンクした単位ベクトルｗ２（＝ｒｏｔ（Ｗｌ、
−〇＋　）　ｗ−）で表わすと、並進移動のときの場合
と同様に回転速度θ１と回転速度θ２を定めることがで
きる。すなわち、第８図に示すように回転軸が単位ベク
トルｅｌで示される回転対偶に、回転軸が単位ベクトル
Ｃ２で示される回転対偶が連鎖しているのと等向の処理
方法となシ、この回転対偶に回転速度の指令値を出して
いることＫなシ、扱いは並進移動と同じになる。

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

入カポ−）７．７”イーチングボックス８とインターフ
ェースする。出力ポートロ、入力ポードアはロボット本
体９とパス２との間の入出力部を形成する。

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

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

動作を説明する。

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

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

先ず、定角速度？、、？Ａ、及び加速度α、ｈ選択経路
を指定する。この指定はプロセッサ１が行う。プロセッ
サ１は上記各指定値に基づき選択経路に従った補間処理
を行う。この補間処理とは、上記選択経路の細分化、細
分化した区間内での方向ベクル及び七〇角速度等の指定
を云う。乗除界器５は谷区間毎に補間演算を行う０袖間
演算はｍ述した各種の演算式でおり、乗算除算、加算、
減算の組合せより成る単純な演算に係る。乗除鼻器５の
出力は、テーブル４の内容を利用してハンドの対偶変位
に座標変換を受け、出力ポートロを通じてロボット本体
９に送られアクチュエータの作動を行う。

この際の座標変換の意味は以下となる。ティーチングデ
ータはアクチュエータの変位検出器の検出位置でのデー
タであり、実際のアクチュエータの位置とは異なる。ま
た、ハンドは対偶変位として操作入力を受けて作動する
。かかる対偶変位を出力するに必要な処理が座標変換処
理でおる。この処理はプロセッサーが行う。かかる実施
例によれば演算内容は簡単でめ９且り速度の連続性を保
持できる。実時間でハンド補間制御が可ｔｔｅとなった
。

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

減速に賛する時間を【、とすると、等減速度−Ｆａ’、
Ｌ−Ｃのゐ・　ｇＥ′人 〔区間Ｇ＋Ｂを等角速度？、ＬでＳ鯛するに要する時間
〕十〔区間ＧｚＢを等角速度）で移動するＩｃ費また、
〔区間Ｇ＋０２を等加速度（Ａ−ａ）で移動するに賛す
る時間〕＝〔区間Ｇ＋Ｇｚの距離〕／＝ｔｎ となる。

従って本発明の経路補間方法を用いると、移一時間は点
Ａ、Ｂ間を一定角速度Ｖ４１点Ｂ、Ｃ闇を一定角速度Ｖ
、で移動させた場合と等しくすることができる。

２本の平行な直ｍＡＢｅＢｃがめりて、両者の中ｇ経路
１に補間する場合にあっても本発明μ過用できる。丈に
、ｋ’ＴＰｉｌＥＩＪ作としてはデカルト座標糸におけ
るＩ［＃Ｉだけでなく、ロボットの１クチ凰エータの変
位を座標軸にとったａ襟系での直一部製の場合にも適用
できる。

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

また本発明は速度制御方式の軌道制御にも適用でき、従
来の位置制御方式のみのものに比べて適用範囲が広い。

【図面の簡単な説明】

第１図はＦＴＰ　！Ｅｌ１作祝明図、第２図は本発明の
原理説明図、第３図は経路説明図、第４図は経路の補間
の説明図、第５図は回転移励説明図第６図は回転の説明
図、第７図は本発明の回転後動に関する原理説明図、第
８図は本発明の詳細な説明する機構等価図、第９図は本
発明の実施例図、第１０図は経路補間の説明図である。 １・・・プロセッサ　２・・・バス ５・・・メモリ　４・・・テーブル ５・・・乗除算器　６・・・出力ポードア・・・人力ボ
ート ８・・・ティーチングボックス ９・・・ロボット本体 代理人弁理士　高　橋　明　夫 第　１　図 Ｖ′久 第　２　図 第　４　図 Ｃ 第　５　図 第　６　閉 第　７　図 第　３　図 笥　ｑ　詔

Claims (1)

1. 【特許請求の範囲】 １６点以上の点を与え、中間となる折点までは第１の回
   転軸まわりに回転移動し、前記折点から次の点までは第
   ２の回転軸まわシに回転移動するｐ’ｒｐ　Ｎｈ作に必
   要な経路を補間によシ決定し、該決定した補間経路に従
   り【ロボットハンドの操作を指示するロボットハンドの
   経路補間方法において、前記折点の手前で、前記第１の
   回転軸まわシの回転速度を単調減少させると共に、前記
   １ｉ１１の回転軸が前記折点までまわすべき角度だけ前
   記第１の回転軸まわシに前記ｍ２０回転軸を逆回転させ
   、この逆回転させてできた第２の回転軸まわシにおける
   回転速に’に単調増加させることによシ得られる経路を
   補間経路とすることを脣徴とするロボットハンドの経路
   補間方法。