JPS5924313A - 高速軌跡発生装置 - Google Patents

Abstract

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

Description

【発明の詳細な説明】 の軌跡制御に於いて用いられるリアルタイムの高速軌跡
発生装置に関する。

一般的に、ロボット手先の位置は基座標原点からロボッ
ト手先捷での位置ベクトルで表わし、ロボット手先の姿
勢は指先の長手方向ならびに指の開く方向おまひ両者と
直交する方向の３成分を基座標系で計り、単位化した３
行３列の姿勢行列で表わし、ロボット手先の教育点は位
置ベクトルと姿勢行列を１とめて４行４列の位置・姿勢
行列で表わす。

従来、ロホ゛ノトの軌跡制御では、例えは、手先の移動
速度ベクトルと回転速度ベクトルとを入力とし、各関節
のジヨイント角速度を演算し制御する方式があったが、
前記手先の各ベクトルと、各ジヨイント角速度との関係
を表わす６行６列ヤコービアンの逆行列を求めねばなら
ず、多大な時間を要しだ。また演算時間の短縮を計るた
め、基準点のヤコービアン逆行列をあらかじめ求め、実
際のロボット制御には、前記基準点ヤコービアン逆行列
の内挿補間値を用いる方式もあるが、充分々軌跡精度は
得られなかった。まだ従来、ロボット手先の教育点での
位置・姿勢データから、教育点間の位置・姿勢軌跡を発
生させる方式では、完全なオフライン計算方式か、ある
いはリアルタイムで演算する方式であっても軌跡の位置
ベクトル成分や姿勢行列成分を時間の関数として演算し
ているため演算周期が長くなり、結果的に軌跡精度も悪
くなる欠点があった。

本発明はこのような従来の欠点を改良し、ロボット手先
の位置・姿勢を前記４行４列の位置・姿勢行列で表現し
、教育点間の軌跡発生に関しては内挿補間などの簡略演
算方式でなく、完全に、位置について定速並進し、姿勢
について主軸廻りに定速回転できる演算方式で、且つオ
フライン軌跡発生でなく、リアルタイムの高速演算軌跡
を発生できる装置を提供することを目的とする。

本発明の最も特徴とするところは教育点間の軌跡を発生
させるだめの演算方式であり、この演算方式は２つの教
育点の位置・姿勢行列から、その教育点間の並進ベクト
ルと主軸まわりの回転行列をまとめて表わした遷移行列
を演算し、さらにこの遷移行夕１ｌのＮ乗根を指定した
並進速度と回転速度とから演算することにより、教育点
の位置・姿勢行列に定数である遷移行列のＮ．乗根を一
定の時間毎に掛は込んで行くだけで厳密な定速並進・主
軸廻りの定速回転軌跡を発生させることができる。

本発明は軌跡の位置成分や姿勢成分を逐次時間の関数と
して演算するのではなく、単に定数の積・和をとること
によって演算できるので、従来方式に比べ１０〜１００
倍の高速化が期待でき、まだ、結果的に演算周期が極め
て短くなることから軌跡精度も向上する。

ロボットのリアル・タイム軌跡制御では、位置・姿勢軌
跡を発生させた後、これをロボ・ノト各関節のジヨイン
ト変数に変換し、最終的にこれらのジ・ｊイント変数を
目標値としてロボットの各サーボを駆動する際、従来方
式であれば、位置・姿勢軌跡の演算周期が長いので、サ
ーボ偏差の突変をさけ、滑らかな軌跡を作るだめにジヨ
イント変数レベルでの内挿補間を必要としたが、本発明
の装置を用いれば位置・姿勢軌跡の演算周期が短いので
、ジヨイント変数レベルでの内挿補間をしなくてもサー
ボ偏差の突変が起らず、充分滑らか且つ正確な軌跡を作
ることができる。

まだロボットにより描くことのできる軌跡は、従来方式
では直線軌跡および直線と直線とを円弧補間なとで結ぶ
近似曲線軌跡に限られていたが、本発明の装置を応用す
れば直線軌跡は勿論、近似でない完全な円軌跡を描くこ
とができる。すなわち、ロボット手先に付いている指の
開閉方向に、長さａの仮想的な工具が装着されていると
仮定し、この仮想工具の先端の位置・姿勢行列データを
本発明の軌跡発生装置の教育点データとして入力すると
、実際の口ｙｌｅ　ノ）手先の描く軌跡は並進のみの場
合には直線軌跡、回転のみの場合には円軌跡となる。こ
の円軌跡は空間に固定した任意面に精度良く描くことが
できるので、例えばアーク溶接作業、ホントメルトのり
付は作業など正確なロボット軌跡制御が必要とされる用
途に好適である。

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

第１図は軌跡発生装置３の全体ブロックと位置・姿勢デ
ータ領域９および軌跡情報データ領域１゜を持つメモリ
２、そして前記軌跡発生装置３およびメモリ２を制御・
管理する中央処理装置（ＣＰＵ　）１の構成を示す。メ
モリ２の領域９には、例えば第２図に示すロボットの手
先】１や手先に装着された工具】２の位置・姿勢が教育
点ｉ、ト１に対応して、位置ベクトル信号成分Ｐｉ”ｉ
＋１と姿勢行列で記憶されている。

まだ、メモリ２の領域１０には第２図のロボット手先１
１や工具１・２の先端で指定された並進速度信号Ｖや回
転主軸Ω、まわシの回転速度信号ωが記憶されている。

前記教育点１＋１＋１に対応する姿勢行列Ｃｉ、Ｃ１＋
、の具体的内容は第２図に示す如く、第３列ベクトル成
分”ｉ　’　ｚｉ＋１がロボット手先１１の指先の方向
を示し、第２列ベクトル成分ワ、。

ｙｉ＋１　　がロボット手先１１の相開閉方向を示し、
す。

ここでＣＰＵ　１から指令されだＲＥＡＤ信号によって
メモリ２から、位置・姿勢行列信号Ｔ　ｒ　＋　Ｔ　１
　＋　１および並進速度信号Ｖ２回転速度ωが軌跡発生
装置３へ出力されると、遷移変数演算器４では、第３図
に示す如く、並進距離信号Ｌ３＋並進ベクｌ−／し信号
７０．遷移行列板の次数（軌跡分割数）Ｎ１２回転主軸
ベクトル信号Ω、およびそのまわり回転角信号ξ１を演
算する。即ち、この遷移変数演算器４は第４図に列挙さ
れている基本演算回路のスカラー和回路１０１．スカラ
ー差演算回路１０２．スカラー積回路＋０３．スカラー
四回路１０４．２乗根回路１０５　、逆余弦回路１０７
．ベクトル和回路１０８、ベクトル差回路１０９　、ベ
クトル内積回路１１０　、ベクトル外積回路１１１およ
びベクトル絶対値回路１１２が第３図に示すように接続
されて、次の各演算を行なうよう構成されている。

”　”　ＩＰｉ＋１−Ｐｉ　’ 次に、この遷移変数演算器４の出力信号の内のＮ、、　
Ｌ、、およびｔ、は第４図に示す基本演算回路のスカラ
ー差回路１０２およびベクトル定数積回路１１／ｌで構
成する並進位置ベクトル演算器５に入力され、第５図に
示す如く遷移行列Ｎ５５乗根並進Ｎ・、ξ・、Ω・は第
４図に示す基本演算回路中の、ス１１ カラー和回路１０１．スカラー差回路１０２．スカラー
積回路１０３．スカラー商回路１０４　、２乗根回路１
０５および余弦回路１０６で構成する回転姿勢行列演算
器６によって、第６図に示す如く遷移行列Ｎ１乗根の回
転姿勢行列成分信号ΔＣ１に変換される。

このΔで、と前記ΔＦ、とけ第４図（ｌｊｌ）に示す基
本演１ 算回路の位置・姿勢行列４行４列／＜ラフアレジスター
１６で構成する遷移行列Ｎ、乗根／％ノファφレジスタ
７によって、第７図に示す如く遷移行列のＮ１乗根ΔＴ
、とじて、即ち １ として一時的にストアされる。

姿勢行列軌跡演算器８は第４図に示すベクトル和回路１
０８、３行３列行列積回路１１５および位置・姿勢行列
４行４列ノ＜ソファ・レジスター１６と、ベクトル・ケ
゛−ト１５、マトリックス・ケ゛ート１６、ベクトル・
セレクタ１７、およびマトリックス・セレクタ１８によ
って第８図に示すように構成されている。ベクトル・セ
レクタ１７およびマトリックス・セレクタ１８はＣＰＵ
　ＩからのＲＥＡＤ信号によって、教育点ｌおよび１刊
に対応する行列データＴ１をそれぞれ選び、その仙、の
場合には出力とする位置・蟻勢軌跡行列信号Ｔ（ｔ）の
成分である位置ベクトル信号Ｐ（ｔ）および蟻勢行列Ｃ
（ｔ）をそれぞれ選んで、ベクトル和演算回路１９の一
方の入力信号および３行３列行列積演算回路２０の一方
の入力信号として出力される。まだベクトル和演算回路
１９および行列積演算回路２０の他方の入力端子へはバ
ッファ・レジスタ７にストアよびΔＣがそれぞれ供給さ
れ、その結果、位置についてはベクトル和信号ΔＰ＋Ｐ
がベクトル・グー　ｌ−１５に、蟻勢については行列積
信号ΔＣｉ＊Ｃがマトリックス・ケ゛−ト１６に入力さ
れる。このベクトル・ケ”−）＋５およびマトリックス
・ゲート１６へはＣＰＵ　１から一定の周期でＲＴＣ信
号が供給されておシ、その結果、ＲＴＣ信号の一定周期
で決まる時間毎に、ベクトル・ケ゛−ト１５からは指定
した並進速度信号■を実現するように位置ベクトル信め
て位置ベクトル信号ｐ（ｔ）として出力され、また、マ
トリックス・ケ”　−ト１６からは回転主軸Ω１まわり
に指定した回転速度信号ωで回転するように姿値が改た
めて姿勢行列信号Ｃ（ｔ）として出力される。

最後にかくの如き演算回路を経た位置ベクトル信号ｐ（
ｔ）と姿勢行列信号Ｃ（ｔ）は位置・姿勢軌跡行列信号
Ｔ（ｔ）、即ち、 トシてバッファ愉レジスタ１４にストアされる。

このストアされた信号Ｔ　（ｔ）が軌跡発生装置３の出
力となる。

この出力信号直ｔ）をもとに、例えばロボットなど多自
由度リンク系を有する制御機械の実時間軌跡制御を行う
には、ロボット手先１１または工具１２の時々刻々の位
置信号ｐ（ｔ）および姿勢信号Ｃ（ｔ）から、各リンク
系のジヨイント変数を逆規し、各リンク系を制御してい
るサーボに入力することによシ簡単に実現できる。

第９図はその一例を示すもので、多関節型マニフ０レー
タの軌跡制御において本実施例の軌跡発生装置を用いた
ものである。

ロボットの手先１１または工具端１２の位置Ｐ。

および姿勢Ｘ　’　＋　ｙ’　＋　Ｚ　　を遷移せしめ
るには））教育モｌｌ −ドおよび１１）門生モードの２通りがある。

さらに教育モードではティーチ・被ンダント・ボックス
３１のスイッチによシ、ジヨイント角の目標値を指令す
るジヨイント角モード’　ＸＯ”ｏ　＋　Ｚ　。

方向への並進および回転を指令する基座標モードおよび
Ｘ、ｙ、ｚ　方向への並進および回転を指令する工具座
標モードがある。

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

次に、ティーチ・被ンダント・ボックス３１から基座標
モードおよび工具座標モードでスイッチ操作を行うと、
デコーダ３３は目標位置・姿勢指令手段３５を駆動し、
位置・姿勢行列レジスタ・バッファ３６に目標位置・姿
勢行列信号Ｔが入力される。この目標位置・姿勢行列信
号Ｔは工具逆変換手段３７およびジヨイント座標逆変換
手段３８を経て目標ジヨイント角θに変換され、さらに
前記６軸サーボ３９の入力となり、モータ４０を駆動せ
しめ、ロボット手先１１に装着された工具１２の位置・
姿勢がＴに一致するまで動く。

このようにして、教育モードではロボット手先１１に装
着した工具１２をジョイント角モード、基座標モードお
よび工具座標モードで目的の位置・蟻勢へ移動させるこ
とができる。この時、ティーチ・にンダント・ボックス
３１のレコードスイッチを押すか、またはコンソール３
２よりＲＥＣＯＤＥ指令をＣＰＵ　Ｉに与えると、モー
タ４ｏの回転軸に直結されたエンコーダ４１の出方であ
る現在ジヨイント角θ８がジヨイント座標順変換手段４
３および工具順変換手段４４を経て工具端■２の位置・
姿勢行列Ｔに変換され、位置・姿勢行列レジスタ・バッ
ファ３６にストアされさらに、メモリ２の位置・姿勢行
列データ領域９に教育点ｉの位置・姿勢行列データＴ１
としてストアされる。

次に再生モードでは、あらかじめ採集した教育点ｌ−１
，１＋　１−＋−＋　、・・の位置・姿勢行列データ・
・・〒−１１ Ｔ、ｉ’、、、　　と−７：／　７−　／ｌ／　３２か
ら指令されＣＰＵ　１によりメモリ２の軌跡情報データ
領域１０にストアされ／ζ並進速速度および回転速度ω
とが前記第１図にて説明した軌跡発生装置３へ読み出さ
れ、前述した方法によシ、教育点間の位置・姿勢行列デ
ータＴ　Ｑ　ＣＰＵ　１からのリアル・タイム・クロッ
ク信号ＲＴＣに同期して刻々と発生せしめ、位置・姿勢
行列レジスタ・バッファ３６、工具逆変換手段３７およ
びジヨイント座標逆変換手段３８を経て、目標ジヨイン
ト角θに変換され、前記６軸サ一ボ手段３９によりモー
タ４ｏを目標ジヨイント角に一致させるように駆動する
。結果的にロボットが目標ジヨイント角に一致するまで
動けば、ロボット手先１１に装置した工具１２の位置・
姿勢が目標位置・姿勢行列Ｔと一致するので、多関節型
ロボットを直交座標空間で、指定速度Ｖに従い廉進し、
回転主軸塘わシに指定回転速度ωに従い回転移動させる
ことができる。

第１０図は本発明をコンピー−ター−ソフトウェア−で
実施した場合のフロー・チャートである。

同図手順５０の教育点数Ｎｅｎｄおよび時間きざみτの
読み込みは、第１図ハードウェア実施例に於けるＣＰＵ
　１からのＲＥＡＤ信号およびＲＴＣ信号に対応する。

以下順次第１０図フロー・チャートに沿って説明する。

手順５１によって軌跡情報データの並進速度Ｖおよび回
転主軸ベクトルΩ、まわりの回転速度ωを読み込んだ後
、手順５２によって第１教育点をＩ；１に設定し、手順
５３によって教育点ｌおよびＩ＋１に対応する位置・姿
勢行列データＴ　およびＴ１＋１を読み込み、しかる後
に手順５４によシ遷移変数である軌跡分割数（遷移行列
根次数）Ｎｏ、並およびそのまわりの回転角ξ、を割算
する。これらの計算方法の説明は第３図・・−ドウエア
実施例と全く同じであるから省略する。次に、手順５５
では並進位置ベクトルΔＰ　が、手順５６では回転姿勢
省列ΔＣか、および、手Ｊ［５７では遷移行列当乗根Δ
Ｔ　が順次割算されるが、これらの計算方法の説明も、
第５図、第６図および第７図の）・−ドウ１ア実施例と
全く同じであるから省略する。

ここで、手順５８では教育点ｌとｉ＋１間の時刻の初期
値１．＝０を設定し、手順５９により、第７図の・・−
ドウ１ア実施例において説明した計算方法に従って、位
置・姿勢行列軌跡Ｔ”（ｔ）を計算するとともに、手順
６０によシ、例えばロボット等の多リンク機構を制御す
る場合々らば、各関節のジヨイント変数を解析する座標
変換プログラムなどに、前記位置・姿勢行列軌跡Ｔ　（
ｔ）を時間きざみてのタイミングで出力する。

手順６１では、教育点ｉと１＋１間の時刻ｔ、が、前記
教育点間の並進・回転に要する時間、すなわち、遷移所
要時間Ｎ、・τを越えたかどうか比較され、ＮＯならば
手順６３によシ、教育点間時刻ｔｉにきざみτを加算し
た後、ステツプ６５に還り、手順５９．６０をくシ返す
。ＹＥＳならば、手順６２へ進み、現在の教育点が最後
の１つ手前かどうかを判定し、Ｎｏならば手順６４で教
育点ｉを１つ加算した後、ステツｆ６６に還り、手順５
３．５４　。

５５を順次くシ返す。ＹＥＳならば、一連の軌跡発生が
終了したので、ただちに動作を停止する。

【図面の簡単な説明】

第１図は本発明の軌跡発生装置の一実施例のブロック図
を示すものである。 第２図は教育点１およびｉ＋１におけるロゲット手先と
それに装着された工具の位置・姿勢を例示する図である
。 第３図は第１図における遷移変数演算器４の構成の一例
を詳細に示すものである。 第４図（イ）〜し）はそれぞれ本発明の軌跡発生装置に
用いる基本演算回路を機能的に示すものである。 第５図は第１図における並進ベクトル演算器５の構成の
一例を示すものである。 第６図は第１図における回転行列演算器６の構成の一例
を示すものである。 第７図は第１図における遷移行列Ｎ乗根バッファ・レノ
スタフの機能を示す図である。 第８図は第１図における位置・姿勢軌跡演算器８の構成
の一例を示す図である。 第９図は本発明の軌跡発生装置を含む多関節型マニプレ
ータの軌跡制御方式の一例を示す図である。 第１０図は本発明の軌跡発生装置における各部子段の機
能を実現するのにコンビーータを用いる場合のソフトウ
ェアの手ノーを示すフロー図である。 ｌ・・・制御・管理用ＣＰＵ、２・・・メモリ、３・・
軌跡発生装置、４・・・遷移変数演算器、５・・−並進
位置ベクトル演算器、６・・・回転姿勢行列演算器、７
・・・遷移行列Ｎ乗根バッファ・レジスタ、８・・・位
置・姿勢行列軌跡演算器、１１′・・・ロボノ）手先、
１２・・工具、１３・・・基座標、１４・・・バッファ
・レジスタ、１５・・・ベクトル・＋”−）、１６・・
マトリックスＱゲート、１７・・・ベクトル・セレクタ
、１８・・・マトリックス・セレクタ、３４・・・目標
ジヨイント角指令手段、３５・・・目標位置・姿勢指令
手段、３６・・・位置・姿勢行列レジスタ・バッファ、
３７・・工具逆変換手段、３８・・・ジヨイント座標逆
変換手段、３９・・・６軸サ一ボ手段、４０・・・モー
タ、４１・・・エンコーダ、４２・・・微分器、４３・
・・ジヨイント座標順変換手段、４４・・・工具順変換
手段、１０１・・スカラー和回路、１０２・・・スカラ
ー差回路、１０３・・・スカラー積回路、１０４・・・
スカラー商回路、１０５・・・２乗根回路、１０６・・
・余弦回路、１０７・・・逆余弦回路、１０８・・・ベ
クトル和回路、１０９・・・ベクトル差回路、１１０・
・ベクトル内積回路、１１１・・・ヘクトル外積回路、
１１２・・・ベクトル絶対イ直回路、＋１３・・単位ベ
クトル回路、１１４・・・ベクトル定数積回路、＋１５
・・・３行３列行列積回路、１１６・位置・姿勢行列４
行４列ノくノファ・レジスタ。 特許出願人　　株式会社　豊田中央研究所層　　　上　
　引　　　−７−へ’ｊｌ：？”・１、’１

Claims (1)

1. 【特許請求の範囲】 位置ベクトル成分信号Ｐｉ、Ｐｉ−Ｈと姿勢行列成分子
   １＋１、および工具端で指定された並進速度信号Ｖと回
   転速度信号ωを含む軌跡情報データを記憶する記憶手段
   と、 その記憶手段から読出した位置ベクトル成分信号Ｐｉ、
   Ｐｉ＋１から並進ベクトル信号ｔ１およびその全並進距
   離信号Ｌ１を演算し、前記記憶手段から読出しだ姿勢行
   列成分信号Ｃ，，Ｃ，，がら回転主軸ベクトル信号Ω１
   およびその軸のまわりの全回転角信号ξ、を演算し、こ
   の全並進距離信号Ｌ１および全回転角信号ξ１と前記記
   憶手段から読出した並進速度信号■および回転速度信号
   ωから軌跡分割数信号Ｎ。 を演算する遷移変数演算手段と、 この遷移変数演算手段の出力である軌跡分割数信号Ｎ１
   おまひ全並進距離信号し、および並進ベクトル信号ｔ、
   に基いて、教育点ｉから教育点ｉ＋１への並進・回転遷
   移を表わす遷移行列のＮ１乗根の並進位装 置ベクトル成分信号　ΔＰ、を演算する並進位置ベクト
   ル演算手段と、 前記遷移変数演算手段の出力である軌跡分割数信号Ｎ、
   および全回転角信号ξ、および回転主軸ペク１　　　　
   　　　　　　　　　　　　　　　　　　　　　　　　１
   トル信号Ω５に基いて、前記遷移行列のＮ１乗根の回転
   姿勢行列成分信号ΔＣ８を演算する回転姿勢行列！ 演算手段と、 前記並進位置ベクトル成分信号ΔＰ　と前記回転姿勢行
   列成分信号ΔＣ，とを合わせて遷移行列のＮ。 乗根信号ΔＴ　として一時的に記憶する一時記憶手■ 段と、 一定のタイミングＲＴＣで教育点の位置・姿勢行列デー
   タ信号Ｔに前記遷移行列のＮ１乗根信号ΔＴ。 を位置成分についてベクトル加算、姿勢成分について行
   列乗算することにより、教育点間を位置について一定速
   度で並進し、姿勢について一定速度で回転主軸廻りに回
   転する位置・姿勢行列軌跡信号Ｔを発生せしめる位置・
   姿勢行列軌跡演算手段とを具備することを特徴とするリ
   アルタイムの高速軌跡発生装置。