JPH0256682B2 - - Google Patents
Info
- Publication number
- JPH0256682B2 JPH0256682B2 JP13296682A JP13296682A JPH0256682B2 JP H0256682 B2 JPH0256682 B2 JP H0256682B2 JP 13296682 A JP13296682 A JP 13296682A JP 13296682 A JP13296682 A JP 13296682A JP H0256682 B2 JPH0256682 B2 JP H0256682B2
- Authority
- JP
- Japan
- Prior art keywords
- signal
- matrix
- trajectory
- vector
- transition
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired
Links
- 239000011159 matrix material Substances 0.000 claims description 79
- 230000007704 transition Effects 0.000 claims description 30
- 238000004364 calculation method Methods 0.000 claims description 21
- 238000013519 translation Methods 0.000 claims description 18
- 230000036544 posture Effects 0.000 description 17
- 238000006243 chemical reaction Methods 0.000 description 8
- 229940050561 matrix product Drugs 0.000 description 6
- 238000000034 method Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000007796 conventional method Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000012549 training Methods 0.000 description 2
- 238000004026 adhesive bonding Methods 0.000 description 1
- 239000012943 hotmelt Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003466 welding Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B19/00—Programme-control systems
- G05B19/02—Programme-control systems electric
- G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/41—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/45—Nc applications
- G05B2219/45083—Manipulators, robot
Description
【発明の詳細な説明】
本発明は例えばロボツト手先に装着した工具端
の軌跡制御に於いて用いられるリアルタイムの高
速軌跡発生装置に関する。
の軌跡制御に於いて用いられるリアルタイムの高
速軌跡発生装置に関する。
一般的に、ロボツト手先の位置は基座標原点か
らロボツト手先までの位置ベクトルで表わし、ロ
ボツト手先の姿勢は指先の長手方向ならびに指の
開く方向および両者と直交する方向の３成分を基
座標系で計り、単位化した３行３列の姿勢行列で
表わし、ロボツト手先の教育点は位置ベクトルと
姿勢行列をまとめて４行４列の位置・姿勢行列で
表わす。
らロボツト手先までの位置ベクトルで表わし、ロ
ボツト手先の姿勢は指先の長手方向ならびに指の
開く方向および両者と直交する方向の３成分を基
座標系で計り、単位化した３行３列の姿勢行列で
表わし、ロボツト手先の教育点は位置ベクトルと
姿勢行列をまとめて４行４列の位置・姿勢行列で
表わす。
従来、ロボツトの軌跡制御では、例えば、手先
の移動速度ベクトルと回転速度ベクトルとを入力
とし、各関節のジヨイント角速度を演算し制御す
る方式があつたが、前記手先の各ベクトルと、各
ジヨイント角速度との関係を表わす６行６列ヤコ
ービアンの逆行列を求めねばならず、多大な時間
を要した。また演算時間の短縮を計るため、基準
点のヤコービアン逆行列をあらかじめ求め、実際
のロボツト制御には、前記基準点ヤコービアン逆
行列の内挿補間値を用いる方式もあるが、充分な
軌跡精度は得られなかつた。また従来、ロボツト
手先の教育点での位置・姿勢データから、教育点
間の位置・姿勢軌跡を発生させる方式では、完全
なオフライン計算方式か、あるいはリアルタイム
で演算する方式であつても軌跡の位置ベクトル成
分や姿勢行列成分を時間の関数として演算してい
るため演算周期が長くなり、結果的に軌跡精度も
悪くなる欠点があつた。
の移動速度ベクトルと回転速度ベクトルとを入力
とし、各関節のジヨイント角速度を演算し制御す
る方式があつたが、前記手先の各ベクトルと、各
ジヨイント角速度との関係を表わす６行６列ヤコ
ービアンの逆行列を求めねばならず、多大な時間
を要した。また演算時間の短縮を計るため、基準
点のヤコービアン逆行列をあらかじめ求め、実際
のロボツト制御には、前記基準点ヤコービアン逆
行列の内挿補間値を用いる方式もあるが、充分な
軌跡精度は得られなかつた。また従来、ロボツト
手先の教育点での位置・姿勢データから、教育点
間の位置・姿勢軌跡を発生させる方式では、完全
なオフライン計算方式か、あるいはリアルタイム
で演算する方式であつても軌跡の位置ベクトル成
分や姿勢行列成分を時間の関数として演算してい
るため演算周期が長くなり、結果的に軌跡精度も
悪くなる欠点があつた。
本発明はこのような従来の欠点を改良し、ロボ
ツト手先の位置・姿勢を前記４行４列の位置・姿
勢行列で表現し、教育点間の軌跡発生に関しては
内挿補間などの簡略演算方式でなく、完全に、位
置について定速並進し、姿勢について主軸廻りに
定速回転できる演算方式で、且つオフライン軌跡
発生でなく、リアルタイムの高速演算軌跡を発生
できる装置を提供することを目的とする。
ツト手先の位置・姿勢を前記４行４列の位置・姿
勢行列で表現し、教育点間の軌跡発生に関しては
内挿補間などの簡略演算方式でなく、完全に、位
置について定速並進し、姿勢について主軸廻りに
定速回転できる演算方式で、且つオフライン軌跡
発生でなく、リアルタイムの高速演算軌跡を発生
できる装置を提供することを目的とする。
本発明の最も特徴とするところは教育点間の軌
跡を発生させるための演算方式であり、この演算
方式は２つの教育点の位置・姿勢行列から、その
教育点間の並進ベクトルと主軸まわりの回転行列
をまとめて表わした遷移行列を演算し、さらにこ
の遷移行列のNi乗根を指定した並進速度と回転
速度とから演算することにより、教育点の位置・
姿勢行列に定数である遷移行列のNi乗根を一定
の時間毎に掛け込んで行くだけで厳密な定速並
進・主軸廻りの定速回転軌跡を発生させることが
できる。本発明は軌跡の位置成分や姿勢成分を逐
次時間の関数として演算するのではなく、単に定
数の積・和をとることによつて演算できるので、
従来方式に比べ10〜100倍の高速化が期待でき、
また、結果的に演算周期が極めて短くなることか
ら軌跡精度も向上する。
跡を発生させるための演算方式であり、この演算
方式は２つの教育点の位置・姿勢行列から、その
教育点間の並進ベクトルと主軸まわりの回転行列
をまとめて表わした遷移行列を演算し、さらにこ
の遷移行列のNi乗根を指定した並進速度と回転
速度とから演算することにより、教育点の位置・
姿勢行列に定数である遷移行列のNi乗根を一定
の時間毎に掛け込んで行くだけで厳密な定速並
進・主軸廻りの定速回転軌跡を発生させることが
できる。本発明は軌跡の位置成分や姿勢成分を逐
次時間の関数として演算するのではなく、単に定
数の積・和をとることによつて演算できるので、
従来方式に比べ10〜100倍の高速化が期待でき、
また、結果的に演算周期が極めて短くなることか
ら軌跡精度も向上する。
ロボツトのリアル・タイム軌跡制御では、位
置・姿勢軌跡を発生させた後、これをロボツト各
関節のジヨイント変数に変換し、最終的にこれら
のジヨイント変数を目標値としてロボツトの各サ
ーボを駆動する際、従来方式であれば、位置・姿
勢軌跡の演算周期が長いので、サーボ偏差の突変
をさけ、滑らかな軌跡を作るためにジヨイント変
数レベルでの内挿補間を必要としたが、本発明の
装置を用いれば位置・姿勢軌跡の演算周期が短い
ので、ジヨイント変数レベルでの内挿補間をしな
くてもサーボ偏差の突変が起らず、充分滑らか且
つ正確な軌跡を作ることができる。
置・姿勢軌跡を発生させた後、これをロボツト各
関節のジヨイント変数に変換し、最終的にこれら
のジヨイント変数を目標値としてロボツトの各サ
ーボを駆動する際、従来方式であれば、位置・姿
勢軌跡の演算周期が長いので、サーボ偏差の突変
をさけ、滑らかな軌跡を作るためにジヨイント変
数レベルでの内挿補間を必要としたが、本発明の
装置を用いれば位置・姿勢軌跡の演算周期が短い
ので、ジヨイント変数レベルでの内挿補間をしな
くてもサーボ偏差の突変が起らず、充分滑らか且
つ正確な軌跡を作ることができる。
またロボツトにより描くことのできる軌跡は、
従来方式では直線軌跡および直線と直線とを円弧
補間などで結ぶ近似曲線軌跡に限られていたが、
本発明の装置を応用すれば直線軌跡は勿論、近似
でない完全な円軌跡を描くことができる。すなわ
ち、ロボツト手先に付いている指の開閉方向に、
長さａの仮想的な工具が装着されていると仮定
し、この仮想工具の先端の位置・姿勢行列データ
を本発明の軌跡発生装置の教育点データとして入
力すると、実際のロボツト手先の描く軌跡は並進
のみの場合には直線軌跡、回転のみの場合には円
軌跡となる。この円軌跡は空間に固定した任意面
に精度良く描くことができるので、例えばアーク
溶接作業、ホツトメルトのり付け作業など正確な
ロボツト軌跡制御が必要とされる用途に好適であ
る。
従来方式では直線軌跡および直線と直線とを円弧
補間などで結ぶ近似曲線軌跡に限られていたが、
本発明の装置を応用すれば直線軌跡は勿論、近似
でない完全な円軌跡を描くことができる。すなわ
ち、ロボツト手先に付いている指の開閉方向に、
長さａの仮想的な工具が装着されていると仮定
し、この仮想工具の先端の位置・姿勢行列データ
を本発明の軌跡発生装置の教育点データとして入
力すると、実際のロボツト手先の描く軌跡は並進
のみの場合には直線軌跡、回転のみの場合には円
軌跡となる。この円軌跡は空間に固定した任意面
に精度良く描くことができるので、例えばアーク
溶接作業、ホツトメルトのり付け作業など正確な
ロボツト軌跡制御が必要とされる用途に好適であ
る。
以下、図面に基づいて本発明の実施例を詳細に
説明する。
説明する。
第１図は軌跡発生装置３の全体ブロツクと位
置・姿勢データ領域９および軌跡情報データ領域
１０を持つメモリ２、そして前記軌跡発生装置３
およびメモリ２を制御・管理する中央処理装置
（CPU）１の構成を示す。メモリ２の領域９に
は、例えば第２図に示すロボツトの手先１１や手
先に装着された工具１２の位置・姿勢が教育点
ｉ，ｉ＋１に対応して位置ベクトル信号成分i，
Ｐi+1と姿勢行列成分信号C〓i，C〓i+1を含むT〓i，T〓
i+1
の形式、即ち で記憶されている。
置・姿勢データ領域９および軌跡情報データ領域
１０を持つメモリ２、そして前記軌跡発生装置３
およびメモリ２を制御・管理する中央処理装置
（CPU）１の構成を示す。メモリ２の領域９に
は、例えば第２図に示すロボツトの手先１１や手
先に装着された工具１２の位置・姿勢が教育点
ｉ，ｉ＋１に対応して位置ベクトル信号成分i，
Ｐi+1と姿勢行列成分信号C〓i，C〓i+1を含むT〓i，T〓
i+1
の形式、即ち で記憶されている。
また、メモリ２の領域１０には第２図のロボツ
ト手先１１や工具１２の先端で指定された並進速
度信号Ｖや回転主軸iまわりの回転速度信号ω
が記憶されている。前記教育点ｉ，ｉ＋１に対応
する姿勢行列C〓i，C〓i+1の具体的内容は第２図に示
す如く、第３列ベクトル成分i，i+1がロボツ
ト手先１１の指先の方向を示し、第２列ベクトル
成分i，i+1がロボツト手先１１の指開閉方向
を示し、第１列ベクトル成分i，i+1はそれぞ
れ前記i，とiおよびi+1とi+1との直交する
方向を示す。
ト手先１１や工具１２の先端で指定された並進速
度信号Ｖや回転主軸iまわりの回転速度信号ω
が記憶されている。前記教育点ｉ，ｉ＋１に対応
する姿勢行列C〓i，C〓i+1の具体的内容は第２図に示
す如く、第３列ベクトル成分i，i+1がロボツ
ト手先１１の指先の方向を示し、第２列ベクトル
成分i，i+1がロボツト手先１１の指開閉方向
を示し、第１列ベクトル成分i，i+1はそれぞ
れ前記i，とiおよびi+1とi+1との直交する
方向を示す。
ここでCPU１から指令されたFEAD信号によ
つてメモリ２から、位置・姿勢行列信号T〓i，T〓i+1
および並進速度信号Ｖ、回転速度ωが軌跡発生装
置３へ出力されると、遷移変数演算器４では、第
３図に示す如く、並進距離信号Li、並進ベクトル
信号i、遷移行列根の次数（軌跡分割数）Ni、
回転主軸ベクトル信号iおよびそのまわり回転
角信号ξiを演算する。即ち、この遷移変数演算器
４は第４図に列挙されている基本演算回路のスカ
ラー和回路１０１、スカラー差演算回路１０２、
スカラー積回路１０３、スカラー商回路１０４、
２乗根回路１０５、逆余弦回路１０７、ベクトル
和回路１０８、ベクトル差回路１０９、ベクトル
内積回路１１０、ベクトル外積回路１１１および
ベクトル絶対値回路１１２が第３図に示すように
接続されて、次の各演算を行なうよう構成されて
いる。
つてメモリ２から、位置・姿勢行列信号T〓i，T〓i+1
および並進速度信号Ｖ、回転速度ωが軌跡発生装
置３へ出力されると、遷移変数演算器４では、第
３図に示す如く、並進距離信号Li、並進ベクトル
信号i、遷移行列根の次数（軌跡分割数）Ni、
回転主軸ベクトル信号iおよびそのまわり回転
角信号ξiを演算する。即ち、この遷移変数演算器
４は第４図に列挙されている基本演算回路のスカ
ラー和回路１０１、スカラー差演算回路１０２、
スカラー積回路１０３、スカラー商回路１０４、
２乗根回路１０５、逆余弦回路１０７、ベクトル
和回路１０８、ベクトル差回路１０９、ベクトル
内積回路１１０、ベクトル外積回路１１１および
ベクトル絶対値回路１１２が第３図に示すように
接続されて、次の各演算を行なうよう構成されて
いる。
Li＝｜i+1−i｜
i＝（i+1−i）／Li
Ni＝√（i）2＋（i8）2
ξi＝cos-1〔１／２｛（i，i+1）＋（i，i+1
）＋（i，i+1）−１｝〕 Ωi＝（i×i+1＋i×i+1＋i×i+1）／｜
i×i+1＋i×i+1＋i×i+1｜ 次に、この遷移変数演算器４の出力信号の内の
Ni，Li、およびiは第４図に示す基本演算回路
のスカラー差回路１０２およびベクトル定数積回
路１１４で構成する並進位置ベクトル演算器５に
入力され、第５図に示す如く遷移行列Ni乗根の
並進位置ベクトル成分信号Δi＝１／Ni・Li・iが 出力されると同時に、遷移変数演算器４の出力信
の一部Ni，ξi，iは第４図に示す基本演算回路中
の、スカラー和回路１０１、スカラー差回路１０
２、スカラー積回路１０３、スカラー商回路１０
４，２乗根回路１０５および余弦回路１０６で構
成する回転姿勢行列演算器６によつて、第６図に
示す如く遷移行列Ni乗根の回転姿勢行列成分信
号ΔC〓iに変換される。
）＋（i，i+1）−１｝〕 Ωi＝（i×i+1＋i×i+1＋i×i+1）／｜
i×i+1＋i×i+1＋i×i+1｜ 次に、この遷移変数演算器４の出力信号の内の
Ni，Li、およびiは第４図に示す基本演算回路
のスカラー差回路１０２およびベクトル定数積回
路１１４で構成する並進位置ベクトル演算器５に
入力され、第５図に示す如く遷移行列Ni乗根の
並進位置ベクトル成分信号Δi＝１／Ni・Li・iが 出力されると同時に、遷移変数演算器４の出力信
の一部Ni，ξi，iは第４図に示す基本演算回路中
の、スカラー和回路１０１、スカラー差回路１０
２、スカラー積回路１０３、スカラー商回路１０
４，２乗根回路１０５および余弦回路１０６で構
成する回転姿勢行列演算器６によつて、第６図に
示す如く遷移行列Ni乗根の回転姿勢行列成分信
号ΔC〓iに変換される。
このΔC〓iと前記Δiとは第４図タに示す基本演
算回路の位置・姿勢行列４行４列バツフアレジス
タ１１６で構成する遷移行列Ni乗根バツフア・
レジスタ７によつて、第７図に示す如く遷移行列
のNi乗根ΔTiとして、即ち として一時的にストアされる。
算回路の位置・姿勢行列４行４列バツフアレジス
タ１１６で構成する遷移行列Ni乗根バツフア・
レジスタ７によつて、第７図に示す如く遷移行列
のNi乗根ΔTiとして、即ち として一時的にストアされる。
姿勢行列軌跡演算器８は第４図に示すベクトル
和回路１０８、３行３列行列積回路１１５および
位置・姿勢行列４行４列バツフア・レジスタ１１
６と、ベクトル・ゲート１５、マトリツクス・ゲ
ート１６、ベクトル・セレクタ１７、およびマト
リツクス・セレクタ１８によつて第８図に示すよ
うに構成されている。ベクトル・セレクタ１７お
よびマトリツクス・セレクタ１８はCPU１から
のREAD信号によつて、教育点ｉおよびｉ＋１
に対応する行列データT〓iおよびT〓i+1がメモリ２か
ら呼び出された時にT〓iの成分である位置ベクトル
信号iおよび姿勢行列信号C〓iをそれぞれ選び、
その他の場合には出力とする位置・姿勢軌跡行列
信号Ｔ（ｔ）の成分である位置ベクトル信号
（ｔ）および姿勢行列C〓（ｔ）をそれぞれ選んで、
ベクトル和演算回路１９の一方の入力信号および
３行３列行列積演算回路２０の一方の入力信号と
して出力される。またベクトル和演算回路１９お
よび行列積演算回路２０の他方の入力端子へはバ
ツフア・レジスタ７にストアされていた遷移行列
Ni乗根ΔT〓iの成分信号ΔiおよびΔC〓iがそれぞれ
供給され、その結果、位置についてはベクトル和
信号Δi＋がベクトル・ゲート１５に、姿勢
については行列積信号ΔC〓i＊C〓がマトリツクス・
ゲート１６に入力される。このベクトル・ゲート
１５およびマトリツクス・ゲート１６へはCPU
１から一定の周期でRTC信号が供給されており、
その結果、RTC信号の一定周期で決まる時間毎
に、ベクトル・ゲート１５からは指定した並進速
度信号Ｖを実現するように位置ベクトル信号
（ｔ）と並進位置ベクトル信号Δiとの加算値が
改めて位置ベクトル信号（ｔ）として出力さ
れ、また、マトリツクス・ゲート１６からは回転
主軸iまわりに指定した回転速度信号ωで回転
するように姿勢行列信号C〓（ｔ）と回転姿勢行列
信号ΔC〓iとの行列積値が改ためて姿勢行列信号C〓
（ｔ）として出力される。最後にかくの如き演算
回路を経た位置ベクトル信号（ｔ）と姿勢行列
信号C〓（ｔ）は位置・姿勢軌跡行列信号Ｔ（ｔ）、
即ち、 としてバツフア・レジスタ１４にストアされる。
このストアされた信号T〓（ｔ）が軌跡発生装置３
の出力となる。
和回路１０８、３行３列行列積回路１１５および
位置・姿勢行列４行４列バツフア・レジスタ１１
６と、ベクトル・ゲート１５、マトリツクス・ゲ
ート１６、ベクトル・セレクタ１７、およびマト
リツクス・セレクタ１８によつて第８図に示すよ
うに構成されている。ベクトル・セレクタ１７お
よびマトリツクス・セレクタ１８はCPU１から
のREAD信号によつて、教育点ｉおよびｉ＋１
に対応する行列データT〓iおよびT〓i+1がメモリ２か
ら呼び出された時にT〓iの成分である位置ベクトル
信号iおよび姿勢行列信号C〓iをそれぞれ選び、
その他の場合には出力とする位置・姿勢軌跡行列
信号Ｔ（ｔ）の成分である位置ベクトル信号
（ｔ）および姿勢行列C〓（ｔ）をそれぞれ選んで、
ベクトル和演算回路１９の一方の入力信号および
３行３列行列積演算回路２０の一方の入力信号と
して出力される。またベクトル和演算回路１９お
よび行列積演算回路２０の他方の入力端子へはバ
ツフア・レジスタ７にストアされていた遷移行列
Ni乗根ΔT〓iの成分信号ΔiおよびΔC〓iがそれぞれ
供給され、その結果、位置についてはベクトル和
信号Δi＋がベクトル・ゲート１５に、姿勢
については行列積信号ΔC〓i＊C〓がマトリツクス・
ゲート１６に入力される。このベクトル・ゲート
１５およびマトリツクス・ゲート１６へはCPU
１から一定の周期でRTC信号が供給されており、
その結果、RTC信号の一定周期で決まる時間毎
に、ベクトル・ゲート１５からは指定した並進速
度信号Ｖを実現するように位置ベクトル信号
（ｔ）と並進位置ベクトル信号Δiとの加算値が
改めて位置ベクトル信号（ｔ）として出力さ
れ、また、マトリツクス・ゲート１６からは回転
主軸iまわりに指定した回転速度信号ωで回転
するように姿勢行列信号C〓（ｔ）と回転姿勢行列
信号ΔC〓iとの行列積値が改ためて姿勢行列信号C〓
（ｔ）として出力される。最後にかくの如き演算
回路を経た位置ベクトル信号（ｔ）と姿勢行列
信号C〓（ｔ）は位置・姿勢軌跡行列信号Ｔ（ｔ）、
即ち、 としてバツフア・レジスタ１４にストアされる。
このストアされた信号T〓（ｔ）が軌跡発生装置３
の出力となる。
この出力信号T〓（ｔ）をもとに、例えばロボツ
トなど多自由度リンク系を有する制御機械の実時
間軌跡制御を行うには、ロボツト手先１１または
工具１２の時々刻々の位置信号（ｔ）および姿
勢信号C〓（ｔ）から、各リンク系のジヨイント変
数を逆算し、各リンク系を制御しているサーボに
入力することにより簡単に実現できる。
トなど多自由度リンク系を有する制御機械の実時
間軌跡制御を行うには、ロボツト手先１１または
工具１２の時々刻々の位置信号（ｔ）および姿
勢信号C〓（ｔ）から、各リンク系のジヨイント変
数を逆算し、各リンク系を制御しているサーボに
入力することにより簡単に実現できる。
第９図はその一例を示すもので、多関節型マニ
プレータの軌跡制御において本実施例の軌跡発生
装置を用いたものである。
プレータの軌跡制御において本実施例の軌跡発生
装置を用いたものである。
ロボツトの手先１１または工具端１２の位置
ｉおよび姿勢i，i，iを遷移せしめるには）
教育モードおよび）再生モードの２通りがあ
る。
ｉおよび姿勢i，i，iを遷移せしめるには）
教育モードおよび）再生モードの２通りがあ
る。
さらに教育モードではテイーチ・ペンダント・
ボツクス３１のスイツチにより、ジヨイント角の
目標値を指令するジヨイント角モード、p，
ｐ，p方向への並進および回転を指令する基座標
モードおよびi，i，i方向への並進および回
転を指令する工具座標モードがある。
ボツクス３１のスイツチにより、ジヨイント角の
目標値を指令するジヨイント角モード、p，
ｐ，p方向への並進および回転を指令する基座標
モードおよびi，i，i方向への並進および回
転を指令する工具座標モードがある。
テイーチ・ペンダント・ボツクス３１からジヨ
イント角モードでスイツチ操作を行うと、デコー
ダ３３は目標ジヨイント角指令手段３４を駆動
し、目標ジヨイント角が直接６軸サーボ手段３
９に入力される。この時、６軸サーボ手段３９に
は同時に、エンコーダ４１から現在ジヨイント角
θ＊、微分器４２からジヨイント角およびモー
タ４０から電流が入力されそれぞれ位置フイー
ド・バツク、速度フイード・バツクおよび電流フ
イード・バツクがかけられ、その偏差出力によつ
てモータ４０が駆動され、最終的に目標ジヨイン
ト角に一致するまで動く。
イント角モードでスイツチ操作を行うと、デコー
ダ３３は目標ジヨイント角指令手段３４を駆動
し、目標ジヨイント角が直接６軸サーボ手段３
９に入力される。この時、６軸サーボ手段３９に
は同時に、エンコーダ４１から現在ジヨイント角
θ＊、微分器４２からジヨイント角およびモー
タ４０から電流が入力されそれぞれ位置フイー
ド・バツク、速度フイード・バツクおよび電流フ
イード・バツクがかけられ、その偏差出力によつ
てモータ４０が駆動され、最終的に目標ジヨイン
ト角に一致するまで動く。
次に、テイーチ・ペンダント・ボツクス３１か
ら基座標モードおよび工具座標モードでスイツチ
操作を行うと、デコーダ３３は目標位置・姿勢指
令手段３５を駆動し、位置・姿勢行列レジスタ・
バツフア３６に目標位置・姿勢行列信号T〓が入力
される。この目標位置・姿勢行列信号Ｔは工具逆
変換手段３７およびジヨイント座標逆変換手段３
８を経て目標ジヨイント角に変換され、さらに
前記６軸サーボ３９の入力となり、モータ４０を
駆動せしめ、ロボツト手先１１に装着された工具
１２の位置・姿勢がT〓に一致するまで動く。
ら基座標モードおよび工具座標モードでスイツチ
操作を行うと、デコーダ３３は目標位置・姿勢指
令手段３５を駆動し、位置・姿勢行列レジスタ・
バツフア３６に目標位置・姿勢行列信号T〓が入力
される。この目標位置・姿勢行列信号Ｔは工具逆
変換手段３７およびジヨイント座標逆変換手段３
８を経て目標ジヨイント角に変換され、さらに
前記６軸サーボ３９の入力となり、モータ４０を
駆動せしめ、ロボツト手先１１に装着された工具
１２の位置・姿勢がT〓に一致するまで動く。
このようにして、教育モードではロボツト手先
１１に装着した工具１２をジヨイント角モード、
基座標モードおよび工具座標モードで目的の位
置・姿勢へ移動させることができる。この時、テ
イーチ・ペンダント・ボツクス３１のレコードス
イツチを押すか、またはコンソール３２より
RECODE指令をCPU１に与えると、モータ４０
の回転軸に直結されたエンコーダ４１の出力であ
る現在ジヨイント角＊がジヨイント座標順変換
手段４３および工具順変換手段４４を経て工具端
１２の位置・姿勢行列Ｔに変換され、位置・姿勢
行列レジスタ・バツフア３６にストアされさら
に、メモリ２の位置・姿勢行列データ領域９に教
育点ｉの位置・姿勢行列データT〓iとしてストアさ
れる。
１１に装着した工具１２をジヨイント角モード、
基座標モードおよび工具座標モードで目的の位
置・姿勢へ移動させることができる。この時、テ
イーチ・ペンダント・ボツクス３１のレコードス
イツチを押すか、またはコンソール３２より
RECODE指令をCPU１に与えると、モータ４０
の回転軸に直結されたエンコーダ４１の出力であ
る現在ジヨイント角＊がジヨイント座標順変換
手段４３および工具順変換手段４４を経て工具端
１２の位置・姿勢行列Ｔに変換され、位置・姿勢
行列レジスタ・バツフア３６にストアされさら
に、メモリ２の位置・姿勢行列データ領域９に教
育点ｉの位置・姿勢行列データT〓iとしてストアさ
れる。
次に再生モードでは、あらかじめ採集した教育
点…ｉ−１，ｉ，ｉ＋１，…の位置・姿勢行列デ
ータ…T〓i-1，T〓i，Ti+1，…とコンソール３２から
指令されCPU１によりメモリ２の軌跡情報デー
タ領域１０にストアされた並進速度Ｖおよび回転
速度ωとが前記第１図にて説明した軌跡発生装置
３へ読み出され、前述した方法により、教育点間
の位置・姿勢行列データT〓をCPU１からのリア
ル・タイム・クロツク信号RTCに同期して刻々
と発生せしめ、位置・姿勢行列レジスタ・バツフ
ア３６、工具逆変換手段３７およびジヨイント座
標逆変換手段３８を経て、目標ジヨイント角に
変換され、前記６軸サーボ手段３９によりモータ
４０を目標ジヨイント角に一致させるように駆動
する。結果的にロボツトが目標ジヨイント角に一
致するまで動けば、ロボツト手先１１に装置した
工具１２の位置・姿勢が目標位置・姿勢行列T〓と
一致するので、多関節型ロボツトを直交座標空間
で、指定速度Ｖに従い並進し、回転主軸まわりに
指定回転速度ωに従い回転移動させることができ
る。
点…ｉ−１，ｉ，ｉ＋１，…の位置・姿勢行列デ
ータ…T〓i-1，T〓i，Ti+1，…とコンソール３２から
指令されCPU１によりメモリ２の軌跡情報デー
タ領域１０にストアされた並進速度Ｖおよび回転
速度ωとが前記第１図にて説明した軌跡発生装置
３へ読み出され、前述した方法により、教育点間
の位置・姿勢行列データT〓をCPU１からのリア
ル・タイム・クロツク信号RTCに同期して刻々
と発生せしめ、位置・姿勢行列レジスタ・バツフ
ア３６、工具逆変換手段３７およびジヨイント座
標逆変換手段３８を経て、目標ジヨイント角に
変換され、前記６軸サーボ手段３９によりモータ
４０を目標ジヨイント角に一致させるように駆動
する。結果的にロボツトが目標ジヨイント角に一
致するまで動けば、ロボツト手先１１に装置した
工具１２の位置・姿勢が目標位置・姿勢行列T〓と
一致するので、多関節型ロボツトを直交座標空間
で、指定速度Ｖに従い並進し、回転主軸まわりに
指定回転速度ωに従い回転移動させることができ
る。
第１０図は本発明をコンピユーター・ソフトウ
エアーで実施した場合のフロー・チヤートであ
る。
エアーで実施した場合のフロー・チヤートであ
る。
同図手順５０の教育点数Nendおよび時間きざ
みτの読み込みは、第１図ハードウエア実施例に
於けるCPU１からREAD信号およびRTC信号に
対応する。以下順次第１０図フロー・チヤートに
沿つて説明する。
みτの読み込みは、第１図ハードウエア実施例に
於けるCPU１からREAD信号およびRTC信号に
対応する。以下順次第１０図フロー・チヤートに
沿つて説明する。
手順５１によつて軌跡情報データの並進速度Ｖ
および回転主軸ベクトルiまわりの回転速度ω
を読み込んだ後、手順５２によつて第１教育点を
ｉ≒１に設定し、手順５３によつて教育点ｉおよ
びｉ＋１に対応する位置・姿勢行列データT〓iおよ
びT〓i+1を読み込み、しかる後に手順５４により遷
移変数である軌跡分割数（遷移行列根次数）Ni、
並進距離Li、並進ベクトルi、回転主軸ベクト
ルiおよびそのまわりの回転角ξiを計算する。こ
れらの計算方法の説明は第３図ハードウエア実施
例と全く同じであるから省略する。次に、手順５
５では並進位置ベクトルΔiが、手順５６で回
転姿勢行列ΔC〓iが、および、手順５７では遷移行
列Li乗根ΔT〓iが順次計算されるが、これらの計算
方法の説明も、第５図、第６図および第７図のハ
ードウエア実施例と全く同じであるから省略す
る。
および回転主軸ベクトルiまわりの回転速度ω
を読み込んだ後、手順５２によつて第１教育点を
ｉ≒１に設定し、手順５３によつて教育点ｉおよ
びｉ＋１に対応する位置・姿勢行列データT〓iおよ
びT〓i+1を読み込み、しかる後に手順５４により遷
移変数である軌跡分割数（遷移行列根次数）Ni、
並進距離Li、並進ベクトルi、回転主軸ベクト
ルiおよびそのまわりの回転角ξiを計算する。こ
れらの計算方法の説明は第３図ハードウエア実施
例と全く同じであるから省略する。次に、手順５
５では並進位置ベクトルΔiが、手順５６で回
転姿勢行列ΔC〓iが、および、手順５７では遷移行
列Li乗根ΔT〓iが順次計算されるが、これらの計算
方法の説明も、第５図、第６図および第７図のハ
ードウエア実施例と全く同じであるから省略す
る。
ここで、手順５８では教育点ｉとｉ＋１間の時
刻の初期値ti＝０を設定し、手順５９により、第
７図のハードウエア実施例において説明した計算
方法に従つて、位置・姿勢行列軌跡T〓（ｔ）を計
算するとともに、手順６０により、例えばロボツ
ト等の多リンク機構を制御する場合ならば、各関
節のジヨイント変数を解析する座標変換プログラ
ムなどに、前記位置・姿勢行列軌跡T〓（ｔ）を時
間きざみτのタイミングで出力する。
刻の初期値ti＝０を設定し、手順５９により、第
７図のハードウエア実施例において説明した計算
方法に従つて、位置・姿勢行列軌跡T〓（ｔ）を計
算するとともに、手順６０により、例えばロボツ
ト等の多リンク機構を制御する場合ならば、各関
節のジヨイント変数を解析する座標変換プログラ
ムなどに、前記位置・姿勢行列軌跡T〓（ｔ）を時
間きざみτのタイミングで出力する。
手順６１では、教育点ｉとｉ＋１間の時刻ti
が、前記教育点間の並進・回転に要する時間、す
なわち、遷移所要時間Ni・τを越えたかどうか
比較され、NOならば手順６３により、教育点間
時刻tiにきざみτを加算した後、ステツプ６５に
還り、手順５９，６０をくり返す。YESならば、
手順６２へ進み、現在の教育点が最後の１つ手前
かどうかを判定し、NOならば手順６４で教育点
ｉを１つ加算した後、ステツプ６６に環り、手順
５３，５４，５５を順次くり返す。YESならば、
一連の軌跡発生が終了したので、ただちに動作を
停止する。
が、前記教育点間の並進・回転に要する時間、す
なわち、遷移所要時間Ni・τを越えたかどうか
比較され、NOならば手順６３により、教育点間
時刻tiにきざみτを加算した後、ステツプ６５に
還り、手順５９，６０をくり返す。YESならば、
手順６２へ進み、現在の教育点が最後の１つ手前
かどうかを判定し、NOならば手順６４で教育点
ｉを１つ加算した後、ステツプ６６に環り、手順
５３，５４，５５を順次くり返す。YESならば、
一連の軌跡発生が終了したので、ただちに動作を
停止する。
第１図は本発明の軌跡発生装置の一実施例のブ
ロツク図を示すものである。第２図は教育点ｉお
よびｉ＋１におけるロボツト手先とそれに装着さ
れた工具の位置・姿勢を例示する図である。第３
図は第１図における遷移変数演算器４の構成の一
例を詳細に示すものである。第４図イ〜タはそれ
ぞれ本発明の軌跡発生装置に用いる基本演算回路
を機能的に示すものである。第５図は第１図にお
ける並進ベクトル演算器５の構成の一例を示すも
のである。第６図は第１図における回転行列演算
器６の構成の一例を示すものである。第７図は第
１図における遷移行列Ｎ乗根バツフア・レジスタ
７の機能を示す図である。第８図は第１図におけ
る位置・姿勢軌跡演算器８の構成の一例を示す図
である。第９図は本発明の軌跡発生装置を含む多
関節型マニプレータの軌跡制御方式の一例を示す
図である。第１０図は本発明の軌跡発生装置にお
ける各部手段の機能を実現するのにコンピユータ
を用いる場合のソフトウエアの手順を示すフロー
図である。 １……制御・管理用CPU、２……メモリ、３
……軌跡発生装置、４……遷移変数演算器、５…
…並進位置ベクトル演算器、６……回転姿勢行列
演算器、７……遷移行列Ｎ乗根バツフア・レジス
タ、８……位置・姿勢行列軌跡演算器、１１……
ロボツト手先、１２……工具、１３……基座標、
１４……バツフア・レジスタ、１５……ベクト
ル・ゲート、１６……マトリツクス・ゲート、１
７……ベクトル・セレクタ、１８……マトリツク
ス・セレクタ、３４……目標ジヨイント角指令手
段、３５……目標位置・姿勢指令手段、３６……
位置・姿勢行列レジスタ・バツフア、３７……工
具逆変換手段、３８……ジヨイント座標逆変換手
段、３９……６軸サーボ手段、４０……モータ、
４１……エンコーダ、４２……微分器、４３……
ジヨイント座標順変換手段、４４……工具順変換
手段、１０１……スカラー和回路、１０２……ス
カラー差回路、１０３……スカラー積回路、１０
４……スカラー商回路、１０５……２乗根回路、
１０６……余弦回路、１０７……逆余弦回路、１
０８……ベクトル和回路、１０９……ベクトル差
回路、１１０……ベクトル内積回路、１１１……
ベクトル外積回路、１１２……ベクトル絶対値回
路、１１３……単位ベクトル回路、１１４……ベ
クトル定数積回路、１１５……３行３列行列積回
路、１１６……位置・姿勢行列４行４列バツフ
ア・レジスタ。
ロツク図を示すものである。第２図は教育点ｉお
よびｉ＋１におけるロボツト手先とそれに装着さ
れた工具の位置・姿勢を例示する図である。第３
図は第１図における遷移変数演算器４の構成の一
例を詳細に示すものである。第４図イ〜タはそれ
ぞれ本発明の軌跡発生装置に用いる基本演算回路
を機能的に示すものである。第５図は第１図にお
ける並進ベクトル演算器５の構成の一例を示すも
のである。第６図は第１図における回転行列演算
器６の構成の一例を示すものである。第７図は第
１図における遷移行列Ｎ乗根バツフア・レジスタ
７の機能を示す図である。第８図は第１図におけ
る位置・姿勢軌跡演算器８の構成の一例を示す図
である。第９図は本発明の軌跡発生装置を含む多
関節型マニプレータの軌跡制御方式の一例を示す
図である。第１０図は本発明の軌跡発生装置にお
ける各部手段の機能を実現するのにコンピユータ
を用いる場合のソフトウエアの手順を示すフロー
図である。 １……制御・管理用CPU、２……メモリ、３
……軌跡発生装置、４……遷移変数演算器、５…
…並進位置ベクトル演算器、６……回転姿勢行列
演算器、７……遷移行列Ｎ乗根バツフア・レジス
タ、８……位置・姿勢行列軌跡演算器、１１……
ロボツト手先、１２……工具、１３……基座標、
１４……バツフア・レジスタ、１５……ベクト
ル・ゲート、１６……マトリツクス・ゲート、１
７……ベクトル・セレクタ、１８……マトリツク
ス・セレクタ、３４……目標ジヨイント角指令手
段、３５……目標位置・姿勢指令手段、３６……
位置・姿勢行列レジスタ・バツフア、３７……工
具逆変換手段、３８……ジヨイント座標逆変換手
段、３９……６軸サーボ手段、４０……モータ、
４１……エンコーダ、４２……微分器、４３……
ジヨイント座標順変換手段、４４……工具順変換
手段、１０１……スカラー和回路、１０２……ス
カラー差回路、１０３……スカラー積回路、１０
４……スカラー商回路、１０５……２乗根回路、
１０６……余弦回路、１０７……逆余弦回路、１
０８……ベクトル和回路、１０９……ベクトル差
回路、１１０……ベクトル内積回路、１１１……
ベクトル外積回路、１１２……ベクトル絶対値回
路、１１３……単位ベクトル回路、１１４……ベ
クトル定数積回路、１１５……３行３列行列積回
路、１１６……位置・姿勢行列４行４列バツフ
ア・レジスタ。
Claims (1)
- 【特許請求の範囲】 １ 位置ベクトル成分信号i，i+1と姿勢行列
成分信号C〓i，C〓i+1を含む位置・姿勢行列データ信
号T〓i，T〓i+1、および工具端で指定された並進速度
信号Ｖと回転速度信号ωを含む軌跡情報データを
記憶する記憶手段と、 その記憶手段から読出した位置ベクトル成分信
号i，i+1から並進ベクトル信号iおよびその
全並進距離信号Liを演算し、前記記憶手段から読
出した姿勢行列成分信号C〓i，C〓i+1から回転主軸ベ
クトル信号iおよびその軸のまわりの全回転角
信号ξiを演算し、この全並進距離信号Liおよび全
回転角信号ξiと前記記憶手段から読出した並進速
度信号Ｖおよび回転速度信号ωから軌跡分割数信
号Niを演算する遷移変数演算手段と、 この遷移変数演算手段の出力である軌跡分割数
信号Niおよび全並進距離信号Liおよび並進ベクト
ル信号iに基いて、教育点ｉから教育点ｉ＋１
への並進・回転遷移を表わす遷移行列のNi乗根
の並進位置ベクトル成分信号Δiを演算する並
進位置ベクトル演算手段と、 前記遷移変数演算手段の出力である軌跡分割数
信号Niおよび全回転角信号ξiおよび回転主軸ベク
トル信号iに基いて、前記遷移行列のNi乗根の
回転姿勢行列成分信号ΔC〓iを演算する回転姿勢行
列演算手段と、 前記並進位置ベクトル成分信号Δiと前記回
転姿勢行列成分信号ΔC〓iとを合わせて遷移行列の
Ni乗根信号ΔTiとして一時的に記憶する一時記憶
手段と、 一定のタイミングRTCで教育点の位置・姿勢
行列データ信号T〓iに前記遷移行列のNi乗根信号
ΔT〓iを位置成分についてベクトル加算、姿勢成分
について行列乗算することにより、教育点間を位
置について一定速度で並進し、姿勢について一定
速度で回転主軸廻りに回転する位置・姿勢行列軌
跡信号T〓を発生せしめる位置・姿勢行列軌跡演算
手段とを具備することを特徴とするリアルタイム
の高速軌跡発生装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP13296682A JPS5924313A (ja) | 1982-07-31 | 1982-07-31 | 高速軌跡発生装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP13296682A JPS5924313A (ja) | 1982-07-31 | 1982-07-31 | 高速軌跡発生装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS5924313A JPS5924313A (ja) | 1984-02-08 |
| JPH0256682B2 true JPH0256682B2 (ja) | 1990-11-30 |
Family
ID=15093661
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP13296682A Granted JPS5924313A (ja) | 1982-07-31 | 1982-07-31 | 高速軌跡発生装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPS5924313A (ja) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60193015A (ja) * | 1984-03-14 | 1985-10-01 | Hitachi Ltd | ロボツトハンドの経路補間方法 |
| CN105252548B (zh) * | 2015-11-03 | 2017-03-08 | 葛洲坝易普力股份有限公司 | 不规则rpr、rp和pr型机械臂连杆坐标系统的运动学性能分析方法 |
| DE102016004841B4 (de) * | 2016-04-24 | 2018-01-04 | Kastanienbaum GmbH | Verfahren und Vorrichtung zum Festlegen eines Bewegungsablaufs für einen Roboter |
-
1982
- 1982-07-31 JP JP13296682A patent/JPS5924313A/ja active Granted
Also Published As
| Publication number | Publication date |
|---|---|
| JPS5924313A (ja) | 1984-02-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US4529921A (en) | Tracking control system of multijoint robot | |
| US5353386A (en) | Apparatus for controlling industrial robot system to perform coordinated operation using teaching playback method and method thereof | |
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| US4823279A (en) | Coordinate conversion system and method for controlling an industrial robot | |
| JPH079606B2 (ja) | ロボット制御装置 | |
| JPS6051121B2 (ja) | ロボツトア−ム用のプログラムを発生する方法 | |
| JPH0310782A (ja) | ロボットの動作制御方法 | |
| JP2874238B2 (ja) | 多関節形ロボットの制御方法 | |
| CN115122325A (zh) | 一种具有视场约束的拟人化机械手鲁棒视觉伺服控制方法 | |
| Anderson | Autonomous, teleoperated, and shared control of robot systems | |
| Géza | Position Singularities and Ambiguities of the KUKA KR5 Robot | |
| JPH0256682B2 (ja) | ||
| JPS62251901A (ja) | 多軸ロボツトの経路制御装置 | |
| JPH11191005A (ja) | ロボット制御装置 | |
| JP2610996B2 (ja) | 多関節ロボット制御装置 | |
| JPH0630012B2 (ja) | 産業用ロボツトの制御方法 | |
| JPH10244481A (ja) | ロボットの動作プログラム作成方法および装置 | |
| JPH07200018A (ja) | ロボットの制御装置 | |
| JP2787207B2 (ja) | 多軸位置サーボ装置 | |
| Ikeura et al. | Motion planning for computer controlled automata | |
| JP2005230952A (ja) | 制御方法および制御装置 | |
| JPH02271405A (ja) | ロボットの制御方法 | |
| JP2576176B2 (ja) | ロボットの制御方法 | |
| Kohno et al. | Intelligent assembly robot | |
| JP2909074B2 (ja) | ロボットアームの動作シミュレータ装置 |