JPS63305407A - ロボットの軌跡制御方式 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 ［概要］ 本発明は多関節型ロボットの腕先端部を電子計算機によ
り移動させるとき、動作上の特異点及びその近傍を通過
させると、動作に不具合を生じるから、直線補間により
遅い動作または変位量の少ない方を選択させてスムーズ
に動作させる軌跡制御方式である。

［産業上の利用分野］ 本発明は多関節型ロボットの腕先端部を移動させて所要
の動作を行わせるときの軌跡制御方式に関する。

多関節型ロボットの腕先端部を移動させて所要の動作を
行わせるとき、途中に「特異点」があるとその前後にお
いて移動が不連続になるから、若干の時間を要しても特
異点の通過をスムーズにできるような試みが研究されて
いる。

［従来の技術］ 多関節型ロボットにおいては、直線機構を用いることな
く、回転・旋回のみにより腕（アーム）の動きを制御し
ているため、何の関節をどれだけ動かしたとき、先端が
何処へ動くかについて直観的に知ることは容易ではない
。そこで各関節の回転量を知り計算して求めることが行
われている。

多関節型ロボットにおいて関節座標系で操作させるため
には、各関節の座標を知る必要があり、当初位置から所
定の位置まで移動させるとき、関節座標系のみで、処理
することは複雑であるため、直交座標系により操作する
ことが研究された。即ち関節座標系で示すベクトルに対
し直交座標系への変換マトリックスを掛は合わせる演算
を行えば良いので、変換マトリックスを求めることが必
要である。これはロボットの移動制御においてその移動
範囲が広い場合に必要である。また逆変換マトリックス
により直交座標系で示すベクトルを関節座標系へ変換す
ることも可能である。

全姿勢ベクトルが成る状態のロボットに関節角度ベクト
ルを与えて所定位置へ移動させるとき、６自由度多関節
型ロボットでは８通りの解があり、移動のとき適宜な解
を選定して行うと、途中で「特異点」を通って他の解に
よる移動を行う必要が生じる。その「特異点」またはそ
の近傍を通過するときの処理については、特開昭５８−
１１４８８号公報に制御例が示されている。

６自由度多関節型ロボットの移動制御において、直交座
標系から関節座標系に変換する場合、特異点またはその
近傍通過時の、０１〜θ６の各軸の動きについて検討し
た所、特異点は （１）　　θ２＋θ３／２＝ｎπ （２）　θ３＝０ （３）　　θ５＝０ の場合に起こっている。しかしく１）の場合は腕先端を
直線運動させるときは対象外となり、（２）の場合とな
るケースは稀のため、（３）の場合について説明する。

第７図は特異点通過時の各軸の位置変化を、第８図は特
異点近傍通過時における各軸の位置変化を示す図である
０両者共に、θ４．θ６軸は特異点またはその近傍にお
いて殆どπの大きさの変化となる。従来の装置では軸の
起動のとき関節にリミット機構を設けているから、特異
点においてはそれに突き当たって軸が動けない、そして
他の解に対応する位置に軸が急に動かされ、そこから所
定位置へ向かう制御が再開されるから、πの大きさの移
動となる。０５軸は速度の符号が反転する。

次に関節座標系から直交座標系に変換する場合はティー
チングプレイバック式の多関節型ロボットにおいて問題
となる。直交座標系について現在位置（Ｘｓ、Ｙｓ、Ｚ
ｓ、　αｓ、　７？ｓ、ｒｓ）（Ｘ、Ｙ、Ｚは位置座標
、α、β、γはオイラ角〕から目標位Ｗ　（ＸＥ　、Ｙ
ｔ　、Ｚｔ　、α１．β、。

ＴＥ）まで成る軌道に沿って動作させる場合に、変位量
（Ｘえ−ＸＳ、Ｙ、−Ｙ９−・・γ、−γＳ）だけその
軌道に沿って変位させることとなる。この際、まず各軸
のエンコーダより各関節角を得て、直交座標系における
位置を計算する必要がある。

ここで従来は−π≦α、Ｔ≦π、０≦β≦πにα。

β、Ｔを限定しているため、第９図Ａ、　Ｂに示すよう
に特異点において変位していく方向が変わって仕舞うこ
とになる。特異点はオイラ角β＝ｎπにおいて存在する
。即ちβを１７５度から１８５度に変化させるとき、β
の値は増加−減少になり、α。

γの値はπだけ大きく変化する。そのためθ４軸。

θ６軸がπだけ大きく変化することになり、その可動範
囲を超え移動不可能になってしまっていた。

［発明が解決しようとする問題点］ 第７図・第８図の場合は、特異点においてθ４゜θ６軸
は急激に変化し、０５軸は速度の符号が反転するから、
軸の動きに振動を起こすことがあった。中央処理装置が
サンプリングクロックにより処理動作を行うとき、移動
量に対しクロック周期が短か過ぎて対応できないことと
なる。

第９図の場合は、特異点を超えた点をティーチングし、
その点までの直線動作でプレイバンクさせると、α、γ
が大きく変位しているため移動に長時間を要する。また
姿勢が大きく変わっているため、移動軌跡はティーチン
グ時とは異なる姿勢をとりながら動いて仕舞う。そのた
め腕が環境と衝突する危険があった。

本発明の目的は前述の欠点を改善し、特異点またはその
近傍をロボットの移動アーム先端が移動するときは、計
算により得られる解に基づく動きを見て、スムーズな動
作をさせるようにしたロボットの軌跡制御方式を提供す
ることにある。

［問題点を解決するための手段］ 第１図は本発明の原理構成を示す図である。第１図にお
いて、■は中央処理装置、２は多関節型ロボット、２１
はロボットの腕先端部、３は関節移動速度の急激な変化
を検出する装置、４は角度の推定手段、５は直線補間す
る手段を示す。

多関節型ロボット２の腕先端部２１を中央処理装置１に
より移動制御させ、動作上の特異点またはその近傍を通
過するときの軌跡制御方式において、本発明は下記の構
成としている。

中央処理装置１には、関節移動速度の急激な変化を検出
する装Ｗ３と、該検出装置３の出力により軌跡の解を切
換える角度を推測する手段４と、該推測手段４の出力に
より関節移動を直線補間する手段５とを具備することで
ある。

［作用］ 第２図は第１図の動作説明用の図である。関節の移動を
開始してから、その移動速度をサンプリング周期の都度
見ていて、急激な変化を与えるときはそれを装置３によ
り検出する。第２図において◎印の点が急激な変化の点
とすれば、サンプリング周期の１周期前の点Ｐｓが解の
切換開始の点となる。また装置３の出力により、解を切
換えて次に動作する位置Ｐｃを、中央処理装置ｌの手段
４により予測する。次に手段５により、Ｐｓ−ＰＣ間を
直線補間し、中央処理装置１のサンプリング周期のＸ印
毎に移動する。このようにして、関節は特異点またはそ
の近傍であっても、スムーズに通過できる。

［実施例コ 中央処理装置１はサンプリング周期毎に軸移動の速度を
与えているから、例えばθ４軸について第３図に示すよ
うに動作させる。第３図において横軸は時間ｔ、縦軸は
θ４軸の一方の変位挙動θ４と他方の変位挙動θ４とを
採る。今、ｉ番目のサンプリング時点における０４軸の
速度θ′４１が１サンプリング周期前の時点における速
度θ′４（ｉ−１）と比較して例えば１．３倍以上の速
度となるように指定されたとき、そのことを第１図の速
度変化検出手段３が検出し、θ″４　（ｉ−１）から補
間動作に入る。この１．３は実験値である。他の解θ″
４に乗り換える時点Ｐｃについては、θ４との関連式（
１１によって判断する。

１θｉｈ　ｌ　＞　Ｉ　ｋｃ　ｔ　ｃ、ｖ＋１ｒｅ、；
≧Ｏｑ−、θ、ｊ＜ｃｌグ＋）−−−（１１ここでθ゛
４にはに＝ｉ、ｉ＋１・−・番目のサンプリング時点に
おける他の解を示し、また θ”４　ｃ　＝−θ’４（ｉ−１）　とする。

その結果下記（２）式によって直線補間を行い、θ゛′
４Ｃにおいて他の解に乗り換える。

詠＝１’ハ枯−−・ノー−−−（２） ただしＬ１＝２θ″４ｃ Ｌ２＝θ゛４Ｃ±ｒｃ　　Ｃ０ｔｃ＜０プ斗θ、”、＞
０でり次に関節座標系から直交座標系に変換する場合β
＝ｎπ（ｎは整数）において特異点となるが、このとき
実はβに２種類の解が存在している。ロボットの腕先端
部の位置を表す４×４マトリツクス（Ｈ）を各関節角（
０１〜θ６）より求める。

このマトリックス（Ｈ）をオイラ角α、β、γで表すと
（３）式で与えられる。

ただしＳα＝ｓｉｎα、Ｃα＝ｃｏｓαなどと示す。

上式よりα、β、γは次のように求められる。

ｃｏｓβ≠±１のとき（ｓｉｎβ≠０である）部ちβに
はβ１とβ２の２種類の解が存在し、そのときはｓｉｎ
βの符号が異なり、α、γはπだけずれた解がそれぞれ
存在することとなる。ここでβの範囲を０≦β≦２πに
限定して、（４）式から２つの解を求める。１動作前の
オイラ角と比較し変位量の少ない方の解を選択すれば良
い。この場合のオイラ角の変化は第５図のようになる。

したがって特異点の通過において、オイラ角α、β、Ｔ
の値に急変が起こらず、スムーズな動作が出来る。

次に直線軌道でロボットの腕先端部を動作させる場合、
サンプリング周期毎に関節の速度を指令しているが、特
異点の近傍を通過するとき速度の符号が変化すると、求
める速度の値に加速度の影響が無視できなくなる。関節
の速度は従来、（５）式％式％ ただしＤＴはサンプリング周期を示す。

このとき第６図に示すように加速する時間と等速制御の
時間とがあり、斜線部分の面積部分だけ目標位置θｉに
近づかないこととなり、加速時間が長いとそれだけ斜線
部の面積が太き（なる。したがって加速度を考慮して速
度を求める必要がある。

○サンプリング周期ＤＴで動作可能な場合ｔ、　　＋ｔ
ｚ　＝ＤＴ　　　　　　　　　　　（６１δＩ’−１ｔ
、弓メ１：　＋（臥汁ＣＩｔ’）ｔｚ＝ひ１−１１＞？
−１ｊ　　ｉ　　：　　７．、−、−ｔｃｉｔ、＝σ；
−１十〆（Ｉ　０丁−／”−’Ｅ（Ｏｚ＄−ｒ−ρ’ｔ
−ｒ　’　１７（７）Ｏサンプリング周期ＤＴでは動作
不可能な場合このときは、サンプリング周期を引き伸ば
すことによる。まず速度を（５）式によって決定すると
、加速時間ｔ１は、ｔ、＝（β１・　−θ、、、　）　
／αしたがってサンプリング周期を上記二つの式から得
られた（ｔ＋＋ｔｚ）に設定する。

［発明の効果］ このようにして本発明によると、ロボットの腕先端部の
移動操作に際し、特異点及びその近傍を通過するときに
、スムーズに通過可能としたから、直交座標系による操
作のとき従来より大きな範囲に移動可能となった。また
特異点近傍でのティーチングを行い、それをプレイバン
クしてもティーチングのとおりに動作させることが可能
となって、ロボットの操作にきわめて有効である。

【図面の簡単な説明】

第１図は本発明の原理構成を示す図、 第２図は第１図の動作説明用の図、 第３図はβ４軸についての動作を説明する図、第４図は
オイラ角の解について説明する図、第５図はオイラ角の
変化を示す図、 第６図は加速度の影響を説明する図、 第７図は従来の特異点通過時の各軸の位置変化を示す図
、 第８図は特異点近傍通過時における各軸の位置変化を示
す図、 第９図は特異点通過時のオイラ角変化を示す図である。 ■−・中央処理装置 ２・−ロボット ２１−腕先端部 ３−関節移動速度変化の検出部 ４−・切換える角度を推測する手段 ５−・−直線補間する手段 特許出願人　　　　富士通株式会社 代　理　人　　弁理士　　鈴木栄祐 第２図 ｅ４胸貧雅例 第３図 βの解 第４図 第５図 第７図 第８図

Claims (1)

1. 【特許請求の範囲】 I 、多関節型ロボット（２）の腕先端部（２１）を中
   央処理装置（１）により移動制御させ、動作上の特異点
   またはその近傍を通過するときの軌跡制御方式において
   、 中央処理装置（１）には、関節移動速度の急激な変化を
   検出する装置（３）と、該検出装置（３）の出力により
   軌跡の解を切換える角度を推測する手段（４）と、該推
   測手段（４）の出力により関節移動を直線補間する手段
   （５）と、 を具備することを特徴とするロボットの軌跡制御方式。 II、ロボット（２）の各関節角から直交座標系における
   腕先端部（２１）の位置姿勢を計算し、オイラー角につ
   いて特異点及びその近傍での動作上２つの異なるオイラ
   ー角を得たとき、最小変位量を選択する手段を具備する
   ことを特徴とする特許請求の範囲第１項記載のロボット
   の軌跡制御方式。