JPS59106007A - 多関節型ロボツトの軌跡制御装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 速軌跡発生装置に関する。

一般に１０ボツトなど多自由度を有するリンク系の軌跡
制御において、位置について直線移動させる並進と姿勢
について回転主軸まわシに回転させる動作を同時に行う
場合、例えば並進速度、？ターンを与えれば並進動作と
回転動作の同時性から回転速度パターンは一義的に決定
されてしまうため、各ジヨイント変数を制御するサーば
か追従できない程大きな回転速度パターンとなることが
しばしばある。また逆に、回転速度パターンを与えても
、並進速度・母ターンが過大となる場合がち９サーデ制
御上大きな問題であった。

本発明はこの問題を克服するため、ロボット手先に装着
した工具端の姿勢変化を回転主軸まわシの回転として定
義し、この回転主軸と直交し、その主軸から一定距離に
ある仮想点が回転に伴い描く円弧と工具端が並進するこ
とによυ描く直線とを合成した螺旋軌跡に対して速度・
ぐターンを与え、これから並進速度パター、ンと回転速
度・ぞターンとをその分速度として演算することによシ
、各ジヨイント変数を制御するサーボが滑らかに追従で
きるような突変のない目標位置・姿勢軌跡を出力するこ
とができる。また、本発明は並進運動だけならば、仮想
点上で与えた速度パターンが並進速度・母ターンそのも
のに、他方、回転主軸まわりの回転運動だけならば、仮
想点上で与えた速度Ａ？ターンを回転主軸と仮想点間゛
の距離で割ったもの力；回転速度パターンに、それぞｈ
自動設定できる。また組立作業では、工具端の激しい姿
勢変化を伴う並進動作が要求されるので、本発明を導入
して、前記回転主軸と仮想点間の距離をｉＰラメータと
して適当に設定することによシ、姿勢変化の全くない教
育点間では指定した通りの並進速度を保ち、姿勢変化の
激しい教育点間では回転速度が上昇するにしたがい並進
速度が滑らかに降下し、組立作業に好適な軌跡を発生す
るとともに組立作業時間短縮に関しても最適化を計るこ
とができる。

以下、第１図ないし第３図によシ本発明の基本原理を説
明する。第１図（ａ）は制御しようとするロボットの基
座標系又Ｏ・マ０．ｚＯ力、ら計ったロボット手先１に
装着された工具端２の教育点ｉ。

ｉ＋１　、　ｉ＋２　Ｆ）位置ヘクトｆｉｖ　Ｐｉ　ｒ
　Ｐｉ＋１　＋　”ｉ＋２ト姿勢行列（マｉ　＊　７ｉ
　＋”ｉ　）　＋　（マ、＋１＋Ｙｉヤ１，７１．１１
）、（マ、や２゜）’ｉ＋２＋　ｚｉ＋２）’ｅ示し、
これらを教育点データとして例えばメモリなどに記憶す
る。その際の行列データ形式は次のように表わす。

ここで、７０，１、＋１１ｉｉ＋２はロボット手先の方
向、ワｉｔフ１→、ｊ＋　Ｖｉ＋２は指の開閉方向およ
びマｉ　ｌマｉ＋１１Ｘｉ＋２はそれぞれ前記２方向と
直交する方向を示し、これらＩｔとめて姿勢行列Ｃ１，
Ｃ１＋１．Ｃ１＋２とする。

第１図（ｂ）は、教育点１１　ｌ＋　１１１　＋２の前
記位置・姿勢行列データテ１．〒ｉ＋１．テ１＋２から
求めた教育点１と１＋１およびｉ＋１と１＋２、間のそ
れぞれ並進ベクトルｔｉ　、ｔｉ＋ｊおよびその方向の
並進速度ｖ１．ｖ１＋１、回転主軸ベクトルｌ’ｌｉ　
、　、Ｑｉ＋１およびその軸まわシの回転速度Ｗｉ、Ｗ
ｉ刊を示し、また教育点１とｉ＋１およびｉ＋１と１−
１−２間でそれぞれ並進ベクトル７：１＋　Ｚｉ＋ｊに
沿って並進速度ｖｉ、ｖｉ＋１で並進移動する距離Ｌ０
．Ｌｉ＋１回転主軸ベクトルａｉｒ”ｉ＋１まわりに回
転速度Ｗ１．Ｗｉ＋１で回転する角度ξ１・Ｓｉ＋１、
および回転に伴い前記回転主軸ベクトルΩ１．Ωｉ＋１
と直交し、距離ｒｈだけ離れた仮想点の描く円弧長ｒｈ
・Ｃ１゜ｒｈ・Ｃ１＋１と前記仮想点が並進し、回転す
ることにより描く軌跡長とをベクトル合成した遷移距離
Ｓｉ。

Ｓ１＋１を示す・ そして第２図（ａ）は、前記仮想点上で指定された速度
■および加減速度αより作られ、前記遷移距離Ｓｉ　＋
　Ｓｉ＋１がＶ２／αよシ大きい場合の台形状速度パタ
ーン６を示す。第２図（ｂ）　、　（ｃ）はそれぞれ前
記仮想点の台形状速度・ぐターン内からベクトル成分と
して求められた並進速度・ぐターンＶと回転速度パター
ンｗｉ示す。ここでτｉ、τ１＋１は加減速度時間、Ｔ
ｉ、Ｔ１＋１はそれぞれ教育点ｉとｉ＋１およびｉ　＋
１とｉ　＋　２間の遷移時間を示す。また図中のＳｉ・
Ｓｉ＋１　＋　Ｌ１＋　Ｌｉ＋４　ｒξ１．Ｃ１＋１は
それぞれ各台形の面積を示す。

また第３図（ａ）は前記仮想点上で指定された速度Ｖお
よび加・減速度αより作られ、前記遷移距離Ｓ１ｒ、　
Ｓｉ＋１がｖ２／αよシ小さい場合の三角形状速度・ぐ
ターン内を示す。この場合、達成される最高速度がＶで
な（、／”：石−およびｉであシ、遷移距離の大きさに
よりそれぞれ違うことを除けば第２図の速度パターンと
同じであるから、以下の詳細説明は省略する。

第１図（Ｃ）は、ロボット手先１に装着した工具端、　
　２が第１図（ｂ）の並進ベクトルＺｉ＋Ｚｉ＋ｊに沿
って、第２図（ｂ）または第３図（ｂ）に示す速度パタ
ーンに従って並進した場合に描く位置ベクトルＰ　（ｔ
）の軌跡３を示し、途中の通過点ｉ’＋１附近では内接
する楕円弧となる。同時にまた、第１図（Ｃ）は第１図
（ｂ）に示す回転主軸ベクトルＱｉｒ　五ｉ　＋　１ま
わシに、第２図（ｃ）！、たけ第３図（Ｃ）に示す速度
パターンに従って回転した場合の工具端２の姿勢行列；
：（ｔ）　、　ｙ（ｔ）　、　７（ｔ）と回転主軸ベク
トルの先端が描く包絡線４を示し、前記位置ベクトルｐ
　（ｔ）と姿勢行列Ｍ（ｔ）、　ｙ（ｔ）　、　’Ｏｔ
）とをまとめて位置姿勢行列Ｔ（ｔ）とする。

この位置・姿勢行列〒（１）は次のように表わす。

本発明による軌跡発生装置の入力信号は例えばメモリに
記憶されていた位置・姿勢行列データ〒ｉ。

Ｔｉ＋４１　Ｔｉ＋２であシ、出力信号は前記位置・姿
勢行列軌跡Ｔ’（ｔ）である。第１図（ｂ）と第２図ま
たは第３図によれば、例えば並進距離Ｌｉ、Ｌｉ＋１が
極めて小さく、回転角ξ１．ξｉ＋１が大きい場合、工
具端２の速度・ぐターンＶを設定するのでなく、仮想点
の速度パターン内を設定すれば並進・回転の同時性によ
る回転速度・ぐターンＷの発散を避けることができ、ま
た逆に、回転へ々ｉ、ξ、＋１が小さく、並進距離”ｉ
＋　Ｌｉ＋ｌが大きくて、回転速度パターンＷを設定し
た場合の並進速度パターンＶの発散も、仮想点上で速度
・ぐターンｓ２設定することによシ避けることができる
。

かくして、本発明の軌跡発生装置から構成される装置・
姿勢行列軌跡〒（１）は姿勢が激しく変化しても自動的
に並進速度・ぐターンと回転速度・やターンとを調節し
て、制御せんとするロボット・サーボ目標値に突変を与
えない様にすることができる。

以下、図面に基づいて本発明の実施例を詳細に説明する
。

第４図は本発明の一実施例の全体ブロック１００を示す
ものである。

制御・管理ＣＰＵ　５は本軌跡発生装置の制御および管
理を行なうものである。メモリ６は教育点・１１＋＋＋
＋＋１．＋・・　の位置・姿勢行列データ・・Ｔ１−１
．Ｔ１．Ｔ１＋１．・°・　を記憶する領域７と、回転
主軸から仮想点までの距離Ｑ、　＋仮想点上で指令され
た速度■、加・減速度α等の軌跡情報データに）を記憶
する領域８を有している。教育点ｉと１＋１間の軌跡を
発生させる場合、、ＣＰＵ５からのメモリ読出し制御信
号ＲＥＡＤによシ、メモリ６に記憶されている位置・姿
勢行列データ〒１−１．〒ｊ・Ｔ１＋１．Ｔ１→２およ
び軌跡情報データｒｈ、Ｖ、αが読出され、遷移変数演
算器１２へ入力される。教育点選択信号発生器１０はＣ
ＰＵ　５からの制御信号に基いて、演算区間を選択する
教育点選択信号ＴＰＳを発生する。この教育点選択信号
ＴＰＳの制御に従って、遷移変数演算器１２ば、それぞ
れ教育点ｉ−１とｉ、ｉとｉ　＋１およびｉ　＋　１と
ｉ＋２区間に於ける仮想点の遷移距離信号５ｊ（ｊ−１
−１，ｌ、ｌ＋１）。

並進速度信号Ｖ’　（ｊ＝ｉ　−１＋　ｉ　、　ｌ＋１
　）　ｒ並進ベクトル信号ｔｊ（ｊ＝ｉ　、　ｉ　、　
ｉ＋ｌ　）　、回転速度信号Ｗｊ（ｊ　”　’　　Ｉ、
１＋　＋　＋　１　）および回転主軸ベクトル信号Ｑ、
Ｈ（ｊ＝ｉ　−１ｒ　ｉ　、　ｌ＋１　）全演算し出力
する。

教育点滅性制御信号発生器９はＣＰＵ　５からの制御信
号によシ、教育点が軌跡のスタート点、・ぐス点および
エンド点のいずれにあるかを示す教育点属性制御信号Ｓ
ＰＥを発生する。遷移所要時間演算器１３では、前記仮
想点上で指定した速度信号Ｖ、加・減速度信号αおよび
遷移距離信号Ｓｊ（ｊ＝ｉ−１゜ｉ、ｌ＋１）’ｉ大入
力し、教育点属性制御信号ＳＰＥの指示する軌跡のスタ
ート点、パス点およびエンド点にそれぞれ対応する教育
点間・遷移時間信号ＴＪ　（Ｊ−”　１　ｒ　１１１　
＋１　）および加・減速時間信号τ３（ｊ−１−１，ｌ
、ｌ＋１）を演算し出力する。

次に、遷移量積算器１４では、ＣＰＵ５からのクロック
・・？ルスＣＫを時刻演算カウンター１が積算し出力す
る時刻信号上と、前記教育点間遷移時間信号Ｔｊ（ｊ−
１’１１１　’ｌ　Ｉ　＋　Ｌ）と、加・減速時間信号
τｊ（ｊ−ｉ　　１．ｉ、ｌ＋１）とを入力とし、前記
教育点属性制御信号ＳＰＥの制御によシ、時刻ｔにおけ
る速度パターンの時間積分値即ち遷移量信号ｔ。

（、＋−＋　　１１１１１　＋１）を積算し出力する。

ここで、遷移量信号ｔｊ（ｊ＝ｉ−１、ｉ　、　ｌ＋１
）は、第２図の速度パターンにおいて、台形の高さを１
と仮定した場合の時刻を迄の台形の面積に対応している
。

以上の演算器に続く、並進位置ベクトル演算器１５と回
転（姿勢）行列演算器１６は同時並列演算を行なう。並
進位置ベクトル演算器１５は並進速度信号”３　（Ｊ−
’　　１１　］　＋１１＋　１　）と並進ベクトル信号
１３　（ｊ＝ｉ　−１＋　ｉ　＋　ｌ＋１　）および遷
移量信号ｔＪ（ｊ−ｉ　−１、ｉ　、　ｌ＋１）ヶいヵ
よ′い、＊ｌ］ｔゆおゆる並進位置ベクトル信号Ｐ、（
ｔ）（Ｊ−１−１，ｌ、１＋１）を出力する。

また後者の回転（姿勢）行列演算器１６は回転速度信号
Ｗ（ｊ＝ｉ　　１　＋　ｉ　、　ｉ＋ｌ）と回転主軸ベ
クトル、ｊＪ・ 信号Ｊ　（ｊ＝ｉ　−１、ｉ　、　ｌ＋１）および遷移
量信号ｔ。

（ｊ＝１−１．ｉ、１＋１）を入力とし、時刻ｔにおけ
る回転（姿勢）行列信号でｊ（ｊ）（ｊ＝ｉ−１＋ｉｙ
ｉ＋１）を出力する。

最後に、位置・姿勢行列軌跡演算器１７では、前記教育
点ｉの位置・姿勢行列データ〒＋と前記時刻ｔにおける
並進位置ベクトル信号Ｐ、Ｈ（ｔ）　（ｊ＝　＋　　１
　。

ｉ、ｌ＋１）および前記時刻ｔにおける回転（姿勢）行
列信号Ｃ；（ｔ）　（Ｊ＝ｉ　　１　、　ｉ　、　ｌ＋
１　）とから、位置成分についてはベクトル加算、姿勢
成分については行列積を作ることによシ時刻ｔにおける
位置・姿勢行列軌跡Ｔ（ｔ）、　ｋ出力する。これによ
シ、教育点ｉとｌ＋１間の本軌跡発生装置１００の一連
の動作の１サイクルを完了するが、次の教育点１＋１と
１−１−２間の軌跡発生はＣＰＵ　５からのメモリ読出
し制御信号ＲＥＡＤによって、メモリ６の位置・姿勢デ
ータ領域７に記憶されている行列データのアドレス・ポ
インタが＋１加算され読み出されることによシ開始され
る一連の動作は教育点ｌと１＋１間の軌跡発生と全く同
じであるから詳細説明は省略する。このサイクル動作は
ＣＰＵ　５の制御信号によシ教育点属性制御信号発生器
９が教育点属性制御信号ＳＰＥとして軌跡のエンド点を
指示する信号を発するまで続く。

以下、第４図の各演算器ブロックについて、その詳細を
説明する。

なお、第５図は各演算ブロックに用いる基本演算回路を
記号的に示すものである。用いる基本演算回路としては
、スカシ和回路１０１．スカラ差回路１０２．スカシ積
回路１０３．スカラ商回路］、　０４．　、２乗根回路
１０５．余弦回路１０６．逆余弦回路１０７．ベクトル
和回路１０８．ベクトル差回路１０９．ベクトル内積回
路１１０．ベクｊ・歩積回路１１１．ベクトル絶対値回
路１１２゜単位ベクトル回路１１３．ベクトル定数積回
路１１４、、　、３行３列行列積回路１１５２位置・姿
勢行列４行４列バッファレジスタ回路１１６等がある。

第６図は遷移変数演算器１２の詳細なハード・ウェア構
成の一例を示す。本演算器１２はベクトル・セレクタ２
１，２２、マトリックス・セレクタ２３，２４、スカシ
・セレクタ２６、比較器２５の外に、第：５図に示す基
本演算回路１０１．１０’２゜１０３．１０４，１０５
，１０６，１０７，１０８，１０９゜１１０．１１１，
１１２，１１３から成っている。メモリ６から読み出さ
れた教育点１１　＋　ｌｒ　ｌｒ　ｌ　ｌ　］　＋　２
における位置・姿勢行列信号データＴｉ、、−１＋　Ｔ
ｉ　＋〒１＋１．〒１＋２はその位置ベクトル成分信号
Ｐｉ−１＋Ｐｉ＋Ｐｉ＋Ｉ　Ｔ　Ｐｉ＋２がベクトル・
セレクタ２１．２２の入力となり、姿勢行列成分信号Ｃ
１−１ｐ　Ｃｉ、　＋　Ｃｌ＋１　＋でｌ＋２がマトリ
ックス・セレクタ２３．２４の入力となって教育点デー
タの区間選択を行う信号ＴＰＳにより、それぞれ位置ベ
クトル成分信号の一組Ｐ”　＋Ｐ＋１と姿勢行列成分信
号の一組Ｃｊ＋　弓＋１がＪ 選択される。

この位置ベクトル成分信号の一組Ｐｊ　ｒ　Ｐｊ＋１か
らは並進距離信号Ｌｊ＝ｌ礼＋１−百ｊｌ　（ｊ−＋　
　１　＋　＋　、＋＋１）と並進ベクトル信号ｔｊ−（
Ｐｊ＋＋　　ｒ’、）／ｌ、、　（ｊ＝ｉ　−１。

ｉ　ｒ　ｌ＋１　）が演算される。

また姿勢行列成分信号の一組Ｃ５ｒ　Ｃ５＋１からは回
転角信号ξｊ−ｃｏＳ　”　（ｖｊ）　（ｊ＝ｉ　−１
＋　ｉ　＋　ｌ＋１　）と回転主軸ベクトル信号勇−イ
ｊ／ｌｑ、ｌ　（ｊ＝ｉ−１，ｉ　、　ｌ＋１）が−】 演算される。なお、ｖｊはｖ４−７Ｒ５Ｆｊ、ｘｊ＋ｔ
）＋（５’ｊ、３’、Ｈ＋＋）＋（”ｊ　、’ｊ＋＋）
　１）の演算によって算出され、１３ばｑｊ＝ｗｊＸ札
刊十夕、×〒ｊ　＋１　＋”　ｊ　×”ｊ　＋　１の演
算によって算出される。

そして、前記並進距離信号Ｌｊ（ｊ＝ｉ　−１＋　Ｌｉ
＋１）と回転角信号ξ、（ｊ”ｌ　　１＋ｉ＋ｉ＋１）
および回転主軸から仮想点迄の距離ｒｈとからは遷移（
並進・回転）距離信号Ｓ３−５−票π石ア（ｊ＝ｉ　−
１、ｉ　、　ｌ＋１　）が演算される。

この遷移（並進・回転）距離信号Ｓｊと指定速度２ 信号Ｖの２乗と指定加・減速度信号αの比７・とが１比
較器２５で比較され、比較出力信号ＬＡＳが得られる。

この信号ＬＡＳ　ｉｄ速度ツクターンが第２図に示す如
く台形状になるか、第３図に示す如く三角形状になるか
を表わし、スカシセレクタ２６を制御する。

スカシセレクタ２６は、加・減速度信号αと距離信号Ｓ
、とから演算した信号儒可と指定速度信号ｖを入力とし
、仮想点上における速度信号ＶＶｊ（ｊ−４＝１　＋　
ｉ　ｒ　ｌ＋１　）　ｆ出力する。この信号ＶＶｊに、
前記並進距離信号Ｌｊ（ｊ””ｌ’　１＋　Ｉ＋１＋１
）と遷移距離信号８４　（ｊ＝ｉ　−１、ｉ　、　ｌ＋
１　）の比を掛けて分速度である並進速度信号Ｖ５　（
ｊ＝ｉ　−１＋　ｉ　＋　ｌ＋１　）を演算するととも
に、前記回転信号ξ５　（ｊ＝ｉ　−］　＋　ｉ　＋　
ｌ＋１）と遷移距離信号Ｓ４　（ｊ＝ｉ　−１、ｉ　、
　ｌ＋１　）の比を掛けて分速度である回転速度信号Ｗ
ｊ（ｊ＝ｉ　−１＋　ｉ　＋　ｌ＋１　）全演算する。

即ち、■、＝（Ｌｊ／Ｓｊ）・■ｖｊおよびＷ、＝（ξ
、Ｈ／ｓ　ｊ）　Ｖ　ｖｊ　’演算する。

最終的に、遷移変数演算器１２から出力されるのは、並
進ベクトル信号１！−ｊ（３−＋　　１．　＋　：　＋
　１＋　１）％遷移距離信号５ｊ（Ｊ−＋、　　１　＋
　ｌｒ　ｌ＋１　）　、並進速度信号Ｖｊ（ｊ二ｉ−１
、ｉ　、　ｌ＋１　）、回転速度”Ｊ（ｊ＝＋　　１．
　＋　１゜１＋１）および回転主軸ベクトル信号δ３（
ｊ＝ｉ−１゜ｉ、ｌ＋１）である。

第７図は遷移所要時間演算器１３の詳細な・・−ド・ウ
ェア構成を示す。本演算器はスカシ・、セレクタ２７，
２８，２９，３０，３１．３２および比較器２５の外に
、第５図に示す基本演算回路１０１．１０３，１０４，
１０５で構成されていて、前記遷移距離信号Ｓｊ（ｊ＝
ｉ　−１、ｉ　、　ｌ＋１　）そして指定された速度信
号Ｖおよび加・減速度信号αから、各教育点間訪ρ遷移
時間信号Ｔｊ（ｊ＝ｉ−１＋　ｉ　ｒ　ｉ＋１　）と加
・減速時間信号τ、Ｈ（ｊ＝ｉ−ｉ、］、ｉ＋ｉ）を演
算する。

比較器２５は、前記遷移距離信号Ｓｊ　（ｊ＝ｉ−１、
１゜ｉ＋１）と指定速度信号Ｖの２乗に対する指定加・
減速度信号αの比から、第２図および第３図に示す台形
状および三角形状速度パターンに対応してスカラセレク
タの切換制御をする信号ＬＡＳを作る。

この制御信号によりスカラセレクタ３１は前記加・減速
時間τｊ（ｊ＝ｓ−１＋　ｉ　＋　ｉ＋１　）を選択し
て出力し、スカラセレクタ２７は教育点がスタートとエ
ンドである場合の、スカラセレクタ２８はスタートとノ
ヤスである場合の、セレクタ２９は・ぐスとエンドであ
る場合の、そしてセレクタ３０はノ１°スとパスである
場合の遷移時間信号を選択し、同じくスカラセレクタ３
２へ出力する。

スカラセレクタ３２では前記教育魚屑性制御信号発生器
９からの出力である制御信号ＳＰＥによって選択制御が
なされ、最終的に、前記４つの遷移時間信号の内から只
一つが選ばれて教育点間遷移時間信号Ｔｊ（コ＝ｉ−１
、ｉ　ｒ　ｉ＋１　）が出力される。

第８図は遷移量積算器１４の詳細なハードウェア構成を
示す。本積算器はスカラセレクタ３７゜３８．３９，４
０，４１，４２，４３．４４および４５と比較器３’３
，３４．３５および３６の外に、第５図に示す基本演算
回路１０１、．１０２．１０３゜１０４で構成されてい
る。そして、前記教育点間遷移時間信号Ｔ、（ｊ＝ｉ　
−１、ｉ　、　ｉ＋１　）と加・減速時間信号τｊ（Ｊ
＝ｉ−１，ｉＩｉ＋１）および前記カウンタ１１からの
出力である時刻信号上とから、第２図または第３図に示
す速度パターンに従って、速度・ぐターンの最高値を１
とした場合の時刻ｔまでの台形又は三角形の面積を意味
する遷移量ｔｊ（ｊ−ｉ−１゜ｉ、ｉ＋１）を積算する
。

比較器３３は現在の演算に使われている教育点がパス点
でしかも現在発生している軌跡は速度・ぞターンの減速
期間にあるかどうかを選定制御する信号ＰＤを出力し、
比較器３４は同じく・ソス点で加速期間にあるかどうか
を選定制御する信号ＰＡを出力し、比較器３５は教育点
がエンド点で速度パターンの減速期間にあるかどうかを
選定制御する信号ＥＤを出力し、そして比較器３６は教
育点がスタート点で速度パターンの加速期間にあるかど
うか全選定制御する信号ｓＡｌ出力する。

スカラセレクタ３７および４１は前記信号ＰＤおよびＳ
Ａによシそれぞれ制御されて、スタート→・やス教育点
間における遷移量信号を選択する。

スカラセレクタ３８および４２は前記信号ＥＤおよびＳ
Ａによシそれぞれ制御されて、スタート→エンド教育点
間における遷移量信号を選択する。

スカラセレクタ３９および４３は前記信号ＥＤおよびＰ
Ａによシそれぞれ制御されて・ぐス→エンド教育点間に
おける遷移量を選ぶ。同様にしてスカラセレクタ４０お
よび４４は前記信号ＰＤおよびＰＡによりそれぞれ制御
されて・やス→・ぐス教育点間における遷移量を選んで
出力する。

最後にスカラセレクタ４５は、前記信号ＳＰＥの制御に
よって前記スタート→・ぐス、スタート−エンド、パス
→エンドおよヒノクス→ｉＥスの各教育点間の遷移量信
号の内から、現在の軌跡発生に関与するものを只一つ選
び時刻ｔの遷移量信号ｔ３　（ｊ＝ｉｌ　ｒ　ｌｒ　ｉ
＋１　）を出力する。

第９図は並進位置ベクトル演算器１５の詳細なハードウ
ェア構成を示す。本演算器は第５図に示す基本演算回路
１０３および１１４で構成されてイテ、前記並進ベクト
ル信号ｚｊ（ｊ＝ｉ−１，ｌ、ｉ＋１）に分速度である
並進速度信号Ｖｊ　（コニ１１＋ｌ＋１＋１）および、
時刻ｔにおける遷移量信号ｔｊ（ｊ＝ｉ−１゜ｉ　、　
ｉ＋１　）を掛けて並進位置ベクトル信号Ｐ、Ｈ（ｔ）
　（ｊ＝ｉ−１＋ｉ、ｉ＋１）を出力する。

第１０図は回転（姿勢）行列演算器１６の詳細なハード
ウェア構成を示す。本演算器は第５図に示す基本演算回
路１０１．１’０２，１０３，１０５および１０６から
構成されていて、前記回転角速度信号Ｗ’　（ｊ＝ｉ−
１ｒ　ｉ　ｒ　ｉ＋１　）も時刻ｔにおける遷移貴信号
ｔｊ（コ＝１−１ｒ　１　＋　１　＋　１　）を掛け、
時刻を迄に回転主軸まわりに回転すべき回転角を求め、
これと前記回転主軸ベクトル信号角（ｊ＝ｉ　−１ｒ　
ｉ　＋　ｉ＋１　）の各３成分０３０）、Ωｊ（２）　
、Ω、（３）から回転行列の３行３列要素を氷めて回転
（姿勢）行′列信号Ｃｊ（ｔ）　（コニ１　１＋１＋ｉ
＋１）を出力する。

第１１図は位置・姿勢行列軌跡演算器１７の詳細なハー
ドウェア構成を示す。本演算器は第５図に示す基本演算
回路１０８，１１５および１１６から構成されていて、
教育点ｉに対応する位置・姿勢行列データ〒１の位置ベ
クトル成分、ｐｌについて、前記並進位置ベクトル信号
”ｉ−＋（ｔ）　、Ｐｉ（ｔ）およびＰｉ＋＋（ｔ）と
ベクトル加算をし時刻Ｅに於ける工具端２の位置ベクト
ル信号ｐ（ｔ）そしてバッファ・レジスタ］１６に一時
スドアし、まだ、位置・姿勢行列データ〒１の姿勢行列
成分ｄ１については、前記回転（姿勢）行列信号で１−
１（ｔ）　、　Ｃ″、（１）　、および６ｉ＋１　　（
！：行列積をとシ、時刻ｔにおける工具端２の姿勢行列
信号Ｃ（ｔ）としてパッファレノスタ１１６へ一時スド
アする。即ち、バ、ファレノスタ１１６には第１図（ｃ
）に示す信号成分ｐ（ｔ）　、マ（ｔ）　、　ｙ（ｔ）
　、　；（ｔ）が一時記憶される。最後に、これらの各
信号成分は行列形式に１とめられ、位置・姿勢行列軌跡
信号Ｔ　（ｔ）として出力される。

第１１図に示すように３つの並進位置ベクトル信号”ｉ
−＋（ｔ）、百ｉ（ｔ、）およびゃ１＋１（ｔ）をベク
トル加算するので、１１＋　ｌ＋　ｌ＋１の３点が同一
直線上にない場合にはパス点ｌの近傍の前後の一定時間
内、即ちＴｉ−ＰｉからＴ工＋τ１において、互に方向
の異なるベクトルを合成することになジ、第１図（ｃ）
に示すように、工具端２の描く軌跡３が教育パス点ｌ（
なお、第１図（ｃ）では教育点ｉ＋１がこれに対応）の
前後のＴ１−ＰｉからＴ１＋τ１の間で内接楕円弧にな
る。また第１１図に示すように３つの回転（姿勢）行列
信号Ｃ′１−１（ｔ）、　Ｃ１（ｔ）およびで、や、（
ｔ）の行列積をとる（７）ｆ、／ｅス点ｉ近傍の前後の
Ｔ１−Ｔ１からＴ１＋τｉの区間では互に回転主軸の異
なる回転行列を掛は合わせるととになるので、第１図（
ｃ）に示すように、工具端２の回転主軸の描く包絡線４
が教育パス点ｌ（第１図（ｃ）では教育点ｉ　＋　１が
これに対応）の前後のＴ１−ＰｉからＴ１＋τ１の区間
で滑らかに繋がり、回転主軸の方向が突変しない。

以上において、本発明の軌跡発生装置のハード構成の一
実施例について詳細に説明したが、論理演ｘｉ　他部分
はコンピュータ・ソフトウェアで実現することができる
。第１２図はその一実施例の動作のフロー・チャートで
ある。この動作手順について以下に詳細に説明する。

本軌跡発生プログラムがスタートすると、手順２００に
よシ教育点数Ｎｅｎｄおよび時間きざみＵが読み込まれ
る。続いて、手順２０１によシ軌跡情報データである速
度ｖ１加・減速度αおよび回転主軸から仮想点までの距
離ｒｈｆ読み込みさらに手順２０２によって第１教育点
を１−１に設定した後、手順２０４で教育点＋　　１＋
］＋１＋１＋＋＋２に対応する位置・姿勢行列データ〒
１−１．〒ｉ、〒ｉ＋１゜Ｔ’ｉ＋２　ｋ読み込む。尚
、教育点がスタート点であればＴ１−１は読まれず、ま
た教育点がスタート点とエンド点の２点だけであるなら
ばＴｉとＴｉ＋＋だけが読まれる。

次に、これらの教育点データと軌峻傳壬データを基に、
手順２０５において、並進ベクトルＡｊ・並進速度Ｖ１
１回転主軸ベクトル机（３４）、回転速度Ｗｊ（３３）
および遷移距離Ｓｊを計算し、手順２０６において、遷
移時間Ｔ３および加・減速時間τｊを計算する。これら
の計算手順は、第４図ハードウェア実施例において説明
した動作手順と同じであるから説明は省略する。

そして、手順２０７では、時刻ｔｉＱに設定し、手順２
０９で、時刻ｔにおける遷移量ｔｊ−ｉ５計算し、手順
２１０で、時刻ｔに５おける並進位置ベクトルＰｊ（ｔ
）、手順、２１１で、時刻りにおける回転（姿勢）行列
Ｃｊ（ｔ）’を計算し、最終的に、手順２１２で、時刻
ｔにおける位置・姿勢行列軌跡Ｔ（ｔ）　を計算する。

これらの計算方法も、第４図ハードウェア実施例の動作
と全く同じであるから説明は省略する。

手順２１２で計算された後、位置・姿勢行列軌跡Ｔ（ｔ
）は、手順２１３によシ、例えば口がット等の多リンク
機構を制御する場合ならば、各関節のノヨイント変数を
解析する座標変換プログラムなどに、一定の時間Ｕ毎に
出力される。

続いて、次の計算サイクルを確立するために、手順２１
４で、時刻ｔが教育点１とｉ＋１間の遷移時間Ｔ１を越
えていないかを判断し、ＹＥＳならば手順２１５によシ
、時刻ｔに時間きざみＵを加えたものを薊７：ｌ：時刻
ｔとし、手順２１６によシ、前記遷移時間Ｔ１を越えた
かどうか判定し、ＹＥＳならば手順２１７で時刻ｔ’ｌ
遷移時間Ｔｉとなし、ＮＯならば手順２１８へ飛んだ後
、手順２０８へ還シ、再び手順２０９以降の計算がくシ
返され、次々と位置・姿勢行列軌跡〒（１）が出力され
る。これは手順２１４でＮｏの判定が下されるまで続く
。

手順２１４でＮｏならば、手順２１９へ進み教育点１が
Ｎ。ｎｄよシ１だけ少ない数すなわち軌跡計算終了点に
なったかどうか判断し、ＮＯならば手順２２０によシ教
育点１に１を加算して手順、２０３へ還９、手順２０４
のデータ読み込み以降教育点ｉ＋１に対する軌跡発生が
くり返される。この動作は手順２１９でＹＥＳの判定が
なされる迄続き、ＹＥＳならばただちに終了する。

第１３図は以上に説明した本発明による軌跡発生装置を
多関節型ロボットに適用してなる軌跡制御方式のブロッ
ク図を示すものである。

ロボット手先１−！たは工具端２の位置Ｐｉおよび姿勢
Ｘｉ　ｒ　７ｉ　’　”ｊの移動モードには１）教育モ
ードおよび１１）再生モードがある。そして、教育モー
ドには、ティーチ・ペンダント・ボックス４００に装着
されたスイッチにより、目標ジヨイント角Ｉを指令する
ジョイント角モードとマθ、ｙθ、ｉθ方向への並進お
よび回転を与える位置・姿勢行列目標値４゜を指令する
基座標モードおよびＸｉ＋’７ｉ＋Ｚ□方向への並進・
回転を与える位置・姿勢行列目標値４ｏを指令する工具
座標モードがある。

ティーチ・ペンダント・ボックス４００がらジョイント
角モードでスイッチ操作を行うと、デコーダ４０２は、
目標ジヨイント角指令手段４０３を駆動し、その出力で
ある目標ジヨイント角Ｉを直接６軸サ一ボ手段４１０に
入力する。この時、６軸サ一ボ手段４１０には、同時に
、エンコーダ４１３から現在ジヨイント角Ｉ＊微分器４
１１からジヨイント角速度１＊およびモータ４１２がら
偏差電流子が入力され、それぞれ位置フィード・バック
、速度フィード・パックおよび電流フィード・パンクが
かけられ、その偏差出力によってモータ４１２が駆動さ
れ、最終的に目標ジョイイト角１に一致するまで動く。

次に、ティーチ・ペンダント・ボックス４００から基座
標モードおよび工具座標モードで゛スイッチ操作を行う
と、デコーダ４０２は目標位置・姿勢指令手段４０４を
駆動し、位置・姿勢行列レジスタ・バッファ４０５に目
標位置・姿勢行列信号Ｔを入力する。この目標位置・姿
勢行列信号〒は工具逆変換手段４０６およびジヨイント
座標逆変換手段４０８を経て目標ジヨイント角１に変換
され、前記６軸サ一ボ手段４１０の入力となりそのサー
ボ偏差出力がモータ４１２を、最終的に目標ジヨイント
角１に一致するまで駆動することによって、ロボット手
先１に装着された工具２が目標位置・姿勢行列信号〒に
一致するまで動く。このようにして、教育モードではロ
ボット手先１に装着した工具２をジョイント角モード、
基座標モードおよび工具座標モードで目標とする位置・
姿勢へ移動させることができる。その際、ティーチ・ペ
ンダント・ボックス４００に装着されているレコートス
イソチを押すか、またはコンソール４０１から’ＲＥＣ
ＯＤＥ　’　ノ如くＫＥＹＩＮすることによッテｃＰＵ
５にレコード指令を与えると、モータ４１２の回転軸に
直結されたエンコーダ４１３の出力である現在ジヨイン
ト角Ｔ＊がジヨイント座標順変換手段４０９および工具
順変換手段４ｏ７を経て工具端２の位置・姿勢行列信号
〒に変換され、位置・姿勢行列レジスタ・バッファ４０
５にストアされ、さらに教育点ｉの位置・姿勢行列デー
タ〒１としてメモリ６の位置・姿勢行列データ領域７に
ストア（ＲＥＣＯＤＥ　）される。

次に再生モードでは、あらかじめ採集され前記位置・姿
勢行列データ領域７にストアされた教育点・・・ｉ　　
１＋ｉ＋ｉ＋１・・・の位置・姿勢行列データ・・・。

ＴＨ−１’＋　Ｔｌｒ　Ｔ４１１　ｒ　”’と’：１７
７−／Ｉ／４０１から指令されＣＰＵ　５によってメモ
リ６の軌跡情報データ領域８ヘスドアされた回転主軸か
ら仮想点までの距離ｒｈ、その仮想点上で与えられた並
進速度■および加減速度αがＣＰＵ　５からのＲＥＡＤ
信号のタイミングによって本発明になる軌跡発生装置１
００へ読み出される。第４図に前述した如く本軌跡発生
装置１００では、教育点間の任意時刻における位置姿勢
行列信号データ〒（１）を刻々と発生せしめ、ＣＰＵ　
５からのリアル・タイム・クロック信号ＲＴＣに同期し
て位置・姿勢行列レノスタ・バッファ４０５へ出力する
。前記位置・姿勢行列信号データ〒（１）は工具逆変換
手段４０６およびジヨイント座標逆変換手段４０８を経
て、目標ジヨイント角１に変換され、前記６軸サ一ボ手
段４１０によりモータ４１２を目標ジヨイント角に一致
させるように、駆動する。ロボットが目標ジヨイント角
に一致するように動けば、結果的に、ロボット手先１に
装着した工具端２の位置・姿勢が目標位置・姿勢行列〒
（１）と一致するように動くので、多関節型ロボットを
直交座標空間で移動させることができる。

従来、組立用ロボットなどにしばしば要求される急激な
姿勢変化を伴った並進軌跡制御では、速度・加速度が突
変し、高速軌跡制御ができず、充分な軌跡精度も得られ
なかったが、本発明になる軌跡発生装置、１００を第１
２図に示す多関節型ロボットの軌跡制御方式に組み入れ
ることにより、位置・姿勢のいかなる変化にも高速（速
度１ｍ／ｓ。

加減速度１ｇ）追従し、軌跡精度もＯ，］、　ｔｔｒｍ
以内の高精度を達成することができる。

また、本発明の一態様によれば、各教育・ぐス点ｌに関
１−て、３つの並進位置ベク：・ル信号ｐ　、　−１（
ｔ）　ｒｐ、（ｔ）および’Ｉ＋１（ｔ）をベクトル加
算するので、工具端２の描く軌跡を内接楕円弧とするこ
とができ、寸だ、３つの回転（姿勢）行列信号ｃ、−，
（ｔ）　、　ｃｌおよび”　ｉ　＋　１（ｔ）の行列積
をとるので、工具端２の回転主軸の描く包絡線が各教育
パス点の近傍で滑らかに繋がり、回転主軸の方向が突変
しないようにできる。

【図面の簡単な説明】

第１図ないし第３図は本発明の基本原理を説明するだめ
の図である。 第４図は本発明の軌跡発生装置の一実施例を示すブロッ
ク図である。 第５図は第４図に示す軌跡発生装置における各演算プロ
、りに用いる基本演算回路を示す図である。 第６図は第４図の実施例における遷移変数演算器の回路
構成の一例を示す図である。 第７図は第４図の実施例における遷移所要時間演算器の
回路構成の一例を示す図である。 第８図は第４図の実施例における遷移量積算器の回路構
成の一例を示す図である。 第９図は第４図の実施例における並進位置ベク行列軌跡
演算器の回路構成の一例を示す図である。 第１２図は本発明の軌跡発生装置の機能の一部をコンピ
ュータソフトウェアを用いて実現する実施例の動作のフ
ローチャートである。 第１３図は本発明の軌跡発生装置を多関節型ロボ、１・
に適用した軌跡制御方式のブロック図である。 １・・・ロボット手先、２・・工具端、３・・・位置ベ
クトルπ（１）の軌跡、４・・・回転主軸ベクトルの先
端が描く包絡線、５・・・制御・管理ＣＰＵ、６・・・
メモリ、７・・・位置・姿勢データ領域、８・・・軌跡
情報データ領域、９・・・教育点層性制御信号発生器、
１０・・・教育点選択信号発生器、１１・・・時刻演算
カウンタ、１２・・・遷移変数演算器、１３・・・遷移
所要時間演算器、１４・・・遷移量積算器、１５・・・
並進位置ベクトル演算器、１６・・・回転（姿勢）行列
演算器、１７・・・位置・姿勢行列軌跡演算器、２］、
、２２・・・ベクトルセレクタ、２３．２４・・・マト
リックスセレクタ、２５．３３〜３６・・・比較器、２
６〜３２゜３７〜４５・・スカラセレクタ、１００・・
軌跡発生装置。 第２図 第３図 第５図

Claims (2)

【特許請求の範囲】
1. （１）　　複数の教育点の位置・姿勢行列データおよび
   教育点間の軌跡情報データを設定し、これらのデータを
   基にロアｉ’ノド手先に装着した工具端の軌跡を制御す
   る多関節型ロボットの軌跡制御方式において、 前記ロボット手先に装着した工具端の回転主軸から直交
   した方向に任意に設定した一定距離ｒｈにある仮想点が
   回転に伴い描く円弧と前記工具端が並進することによシ
   描く直線とを合成した螺旋軌跡上の速度■および加・減
   速度αとして前記軌跡情報データを設定する手段と、 前記螺旋軌跡」二の速度■および加・減速度αに基いて
   定まる螺旋軌跡上の速度・やターンからその分速度とし
   ２て工具端の並進速度パターンと回転車軸回シの回転速
   度パターンとを演算する手段と、これらの並進速度・ぐ
   ターンおよび回転速度・ぐターンに従い、位置について
   並進し、姿勢について回転主軸まわシに回転する位置・
   姿勢行列信号を発生させる手段と、 を備えたことを特徴とする前記方式。
2. （２）少くとも１個のパス教育点ｉｉ含む複数の教育点
   ・・・、ｉ−１，ｉ、ｉ＋１．・・・の位置・姿勢行列
   データ・・・〒ｉ−１ｒ　Ｔｉ　＋〒ｉ＋１．〒１＋２
   ．・・・および教育点間の軌跡情報データを設定し、こ
   れらのデータを基にロボット手先に装着した工具端の軌
   跡全制御する多関節型ロボットの軌跡制御方式において
   、前記ロボット手先に装着した工具端の回転主軸から直
   交した方向に任意に設定した一定距離ｒｈにある仮想点
   が回転に伴い描く円弧と前記工具端が並進することによ
   シ描く直線とを合成した教育点間の螺旋軌跡上の速度Ｖ
   および加・減速度αとして前記軌跡情報データを設定す
   る手段と、前記螺旋軌跡上の速度ｖ１加・減速度αおよ
   び位置・姿勢データ〒１−１．〒ｉ　＋　？ｉ＋１１〒
   １＋２から、各教育点間の螺旋軌跡上の時刻ｔにおける
   速度・ぐターンの時間積分値を表わす量ｔ３　（但じｊ
   ＝ｌＬｉ　、　ｉ＋１　）、および前記各教育点間の螺
   旋軌跡上の速度に対する分速度として各教育点間の並進
   速度Ｖ　（但しｊ　＝　ｉ−１’＋　１１１　＋　１　
   ）および回転主軸まわＤの回転速度Ｗｊ（但し、ｊ＝ｉ
   −１ｒ　ｉ　、　ｉ＋１）　’ｆｒ：算出する手段と、 前記時刻ｔにおける速度パターンの時間積分値を表わす
   量ｔｊ１前記並進速度■１およびその並進ベクトルｔｊ
   から、並進位置ベクトルｐｉ−１（ｔＬπ、（１）　。 Ｐ、＋１（ｔ）を算出する手段と、 前記時刻ｔにおける速度・ぐターンの時間積分値を表わ
   す量ｔｊ１前記回転速度Ｗｊおよび回転主軸ベクトル五
   から、回転姿勢行列”１−１（ｔ）　、５１（ｔ）　。 ”ｉ＋１（ｔ）を算出する手段と、 教育点１に対応する位置・姿勢行列データＴｉの位置、
   ベクトル成分Ｐ、につ、いて、前記並進位置ベクトル’
   １−１（ｔ）、Ｐｉ（ｔ）および百ｔ　’＋　１＜”）
   　とベクトル加算をして工具端の位置ベクトル信号Ｐ（
   ｔ）　’ｅ得る手段と、 位置姿勢行列データ〒ｉの姿勢行列成分Ｃｉについて、
   前記回転姿勢行列信号６ｌ−１（ｔ）、ｄｉ（ｔ）およ
   び”　ｉ　＋　１（ｔ）と行列積をとシ工具端の姿勢行
   列信号ｄ（ｔ）を得る手段と 全備えたことを特徴とする前記方式。