JPH0319964B2 - - Google Patents

Info

Publication number
JPH0319964B2
JPH0319964B2 JP57215589A JP21558982A JPH0319964B2 JP H0319964 B2 JPH0319964 B2 JP H0319964B2 JP 57215589 A JP57215589 A JP 57215589A JP 21558982 A JP21558982 A JP 21558982A JP H0319964 B2 JPH0319964 B2 JP H0319964B2
Authority
JP
Japan
Prior art keywords
trajectory
signal
rotational
velocity
education
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
Application number
JP57215589A
Other languages
English (en)
Other versions
JPS59106007A (ja
Inventor
Hiroshi Moribe
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Central R&D Labs Inc
Original Assignee
Toyota Central R&D Labs Inc
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Toyota Central R&D Labs Inc filed Critical Toyota Central R&D Labs Inc
Priority to JP57215589A priority Critical patent/JPS59106007A/ja
Priority to US06/560,012 priority patent/US4529921A/en
Publication of JPS59106007A publication Critical patent/JPS59106007A/ja
Publication of JPH0319964B2 publication Critical patent/JPH0319964B2/ja
Granted legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00Programme-control systems
    • G05B19/02Programme-control systems electric
    • G05B19/42Recording and playback systems, i.e. in which the programme is recorded from a cycle of operations, e.g. the cycle of operations being manually controlled, after which this record is played back on the same machine
    • G05B19/425Teaching successive positions by numerical control, i.e. commands being entered to control the positioning servo of the tool head or end effector
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00Program-control systems
    • G05B2219/30Nc systems
    • G05B2219/45Nc applications
    • G05B2219/45083Manipulators, robot

Description

【発明の詳細な説明】 本発明は例えばロボツト手先に装着した工具端
の軌跡制御を行なう多関節型ロボツトの軌跡制御
装置に関し、とくに同装置において用いられるリ
アルタイムの高速軌跡発生装置に関する。
一般に、ロボツトなど多自由度を有するリンク
系の軌跡制御において、位置について直線移動さ
せる並進と姿勢について回転主軸まわりに回転さ
せる動作を同時に行う場合、例えば並進速度パタ
ーンを与えれば並進動作と回転動作の同時性から
回転速度パターンは一義的に決定されてしまうた
め、各ジヨイント変数を制御するサーボが追従で
きない程大きな回転速度パターンとなることがし
ばしばある。また逆に、回転速度パターンを与え
ても、並進速度パターンが過大となる場合があり
サーボ制御上大きな問題であつた。
特公昭63−27723号公報記載の従来技術は、ロ
ボツトハンドの姿勢を補間することを目的とし、
ハンドの姿勢を表わすハンドに固定された2つの
単位ベクトルf→、g→を用い、ハンドの2つの姿勢
(f→0、f→0)と(f→o、f→o)との間の回転運動

e→=(f→p−f→o)×(f→p−f→o)軸回りに回
転する様
に、軸回りの回転角を時間的に補間するものであ
る。すなわち、この従来技術は、2ベクトルf→,
g→より回転ベクトルおよび回転角を求めているの
で、ベクトルf→まわり、又はベクトルg→まわりの
回転についてはともにベクトルe→(=(f→p−f→o

×(g→p−g→o))=0となり定義できないため、回
転角の補間ができないという問題があつた。ま
た、この従来技術はロボツトハンドの姿勢のみを
補間するものであり、ロボツトの位置と姿勢を同
時に発生する軌跡制御に関するものではない。
本発明はこの問題を克服するため、ロボツト手
先に装着した工具端の姿勢変化を回転主軸まわり
の回転として定義し、この回転主軸と直交し、そ
の主軸から一定距離にある仮想点が回転に伴い描
く円弧と工具端が並進することにより描く直線と
を合成した螺旋軌跡に対して速度パターンを与
え、これから並進速度パターンと回転速度パター
ンとをその分速度として演算することにより、各
ジヨイント変数を制御するサーボが滑らかに追従
できるような突変のない目標位置・姿勢軌跡を出
力することができる。また、本発明は並進運動だ
けならば、仮想点上で与えた速度パターンが並進
速度パターンそのものに、他方、回転主軸まわり
の回転運動だけならば、仮想点上で与えた速度パ
ターンを回転主軸と仮想点間の距離で割つたもの
が回転速度パターンに、それぞれ自動設定でき
る。また組立作業では、工具端の激しい姿勢変化
を伴う並進動作が要求されるので、本発明を導入
して、前記回転主軸と仮想点間の距離をパラメー
タとして適当に設定することにより、姿勢変化の
全くない教育点間では指定した通りの並進速度を
保ち、姿勢変化の激しい教育点間では回転速度が
上昇するにしたがい並進速度が滑らかに降下し、
組立作業に好適な軌跡を発生するとともに組立作
業時間短縮に関しても最適化を計ることができ
る。
本発明は、ロボツト手先の工具端の回転主軸よ
り直交方向に距離roにある仮想点が回転に伴い描
く円弧と、工具端が並進することにより描く直線
とを合成した螺旋軌跡に対して速度パターンを与
え、さらにその分速度パターンとして並進速度パ
ターンと回転速度パターンとを演算し、これらに
基づき位置について並進し、姿勢について回転主
軸まわりに回転させる様に制御するので、ロボツ
トの位置と姿勢を同時に、高速かつ高精度に出力
して好適な軌跡を発生するという、従来技術にな
い効果を奏する。
以下、第1図ないし第3図により本発明の基本
原理を説明する。第1図aは制御しようとするロ
ボツトの基座標系pppから計つたロボツ
ト手先1に装着された工具端2の教育点i,i+
1+i+2の位置ベクトルii+1i+2と姿
勢行列(iii)、(i+1i+1i+1)、
i+2i+2i+2)を示し、これらを教育点デ
ータとして例えばメモリなどに記憶する。その際
の行列データ形式は次のように表わす。
ここで、ii+1i+2はロボツト手先の方
向、ii+1i+2は指の開閉方向およびi
i+1i+2はそれぞれ前記2方向と直交する方
向を示し、これらをまとめて姿勢行列C〜i、C〜i+1
C〜i+2とする。
第1図bは、教育点i、i+1、i+2の前記
位置・姿勢行列データT〜i、T〜i+1、T〜i+2から求

た教育点iとi+1およびi+1とi+2間のそ
れぞれ並進ベクトルii+1およびその方向の
並進速度vi、vi+1、回転主軸ベクトルii+1
よびその軸まわりの回転速度wi、wi+1を示し、ま
た教育点iとi+1およびi+1とi+2間でそ
れぞれ並進ベクトルii+1に沿つて並進速度
vi、vi+1で並進移動する距離Li、Li+1、回転主軸ベ
クトルii+1まわりに回転速度wi、wi+1で回
転する角度ξi、ξi+1、および回転に伴い前記回転
主軸ベクトルii+1と直交して、距離rhだけ
離れた仮想点の描く円弧長rh・ξi、rh・ξi+1と前記
仮想点が並進し、回転することにより描く軌跡長
とをベクトル合成した遷移距離Si、Si+1を示す。
そして第2図aは、前記仮想点上で指定された
速度Vおよび加減速度αより作られ、前記遷移距
離Si、Si+1がV2/αより大きい場合の台形状速度
パターンS〓を示す。第2図b,cはそれぞれ前記
仮想点の台形状速度パターンS〓からベクトル成分
として求められた並進速度パターンvと回転速度
パターンwを示す。ここでτi、τi+1は加減速度時
間、Ti、Ti+1はそれぞれ教育点iとi+1および
i+1とi+2間の遷移時間を示す。また図中の
Si、Si+1、Li、Li+1、ξi、ξi+1はそれぞれ各台形の
面積を示す。
また第3図aは前記仮想点上で指定された速度
Vおよび加・減速度αより作られ、前記遷移距離
Si、Si+1がV2/αより小さい場合の三角形状速度
パターンS〓を示す。この場合、達成される最高速
度がVでなく√i・および√i+1・であり、
遷移距離の大きさによりそれぞれ違うことを除け
ば第2図の速度パターンと同じであるから、以下
の詳細説明は省略する。
第1図cは、ロボツト手先1に装着した工具端
2が第1図bの並進ベクトルii+1に沿つて、
第2図bまたは第3図bに示す速度パターンに従
つて並進した場合に描く位置ベクトル(t)の
軌跡3を示し、途中の通過点i+1附近では内接
する楕円弧となる。同時にまた、第1図cは第1
図bに示す回転主軸ベクトルii+1まわりに、
第2図cまたは第3図cに示す速度パターンに従
つて回転した場合の工具端2の姿勢行列(t)、
y(t)、(t)と回転主軸ベクトルの先端が
描く包絡線4を示し、前記位置ベクトル(t)
と姿勢行列(t)、(t)、(t)とをまと
めて位置姿勢行列T〜(t)とする。
この位置・姿勢行列T〜(t)は次のように表わ
す。
本発明による軌跡発生装置の入力信号は例えば
メモリに記憶されていた位置・姿勢行列データT〜
、T〜i+1、T〜i+2であり、出力信号は前記位置・姿
勢行列軌跡T〜(t)である。第1図bと第2図ま
たは第3図によれば、例えば並進距離Li、Li+1
極めて小さく、回転角ξi、ξi+1が大きい場合、工
具端2の速度パターンvを設定するのでなく、仮
想点の速度パターンS〓を設定すれば並進・回転の
同時性による回転速度パターンwの発散を避ける
ことができ、また逆に、回転角ξi、ξi+1が小さく、
並進距離Li、Li+1が大きくて、回転速度パターン
wを設定した場合の並進速度パターンvの発散
も、仮想点上で速度パターンS〓を設定することに
より避けることができる。
かくして、本発明の軌跡発生装置から出力され
る位置・姿勢行列軌跡T〜(t)は姿勢が激しく変
化しても自動的に並進速度パターンと回転速度パ
ターンとを調節して、制御せんとするロボツト・
サーボ目標値に突変を与えない様にすることがで
きる。
以下、図面に基づいて本発明の実施例を詳細に
説明する。
第4図は本発明の一実施例の全体ブロツク100
を示すものである。
制御・管理CPU5は本軌跡発生装置の制御お
よび管理を行なうものである。メモリ6は教育点
……i−1,i,i+1,……の位置・姿勢行列
データ……T〜i-1,T〜i,T〜i+1,……を記憶する

域7と、回転主軸から仮想点までの距離rh、仮想
点上で指令された速度V、加・減速度α等の軌跡
情報データを記憶する領域8を有している。教育
点iとi+1間の軌跡を発生させる場合、CPU
5からのメモリ読出し制御信号READにより、
メモリ6に記憶されている位置・姿勢行列データ
T〜i-1、T〜i、T〜i+1、T〜i+2および軌跡情報デー
タrh
V、αが読出され、遷移変数演算器12へ入力さ
れる。教育点選択信号発生器10はCPU5から
の制御信号に基いて、演算区間を選択する教育点
選択信号TPSを発生する。この教育点選択信号
TPSの制御に従つて、遷移変数演算器12は、
それぞれ教育点i−1とi、iとi+1およびi
+1とi+2区間に於ける仮想点の遷移距離信号
Sj(j=i−1、i、i+1)、並進速度信号Vj
(j=i−1、i、i+1)、並進ベクトル信号
(j=i、i、i+1)、回転速度信号wj(j=
i−1、i、i+1)および回転主軸ベクトル信
j(j=i−1、i、i+1)を演算し出力
する。
教育点属性制御信号発生器9はCPU5からの
制御信号により、教育点が軌跡のスタート点、パ
ス点およびエンド点のいずれにあるかを示す教育
点属性制御信号SPEを発生する。遷移所要時間演
算器13では、前記仮想点上で指定した速度信号
V、加・減速度信号αおよび遷移距離信号Sj(j
=i−1、i、i+1)を入力とし、教育点属性
制御信号SPEの指示する軌跡のスタート点、パス
点およびエンド点にそれぞれ対応する教育点間・
遷移時間信号Tj(j=i−1、i、i+1)およ
び加・減速時間信号τj(j=i−1、i、i+1)
を演算し出力する。
次に、遷移量積算器14では、CPU5からの
クロツクパルスCKを時刻演算カウンタ11が積
算し出力する時刻信号tと、前記教育点間遷移時
間信号Tj(j=i−1、i、i+1)と、加・減
速時間信号τj(j=i−1、i、i+1)とを入
力とし、前記教育点属性制御信号SPEの制御によ
り、時刻tにおける速度パターンの時間積分値即
ち遷移量信号tj(j=i−1、i、i+1)を積
算し出力する。ここで、遷移量信号tj(j=i−
1、i、i+1)は、第2図の速度パターンにお
いて、台形の高さを1と仮定した場合の時刻t迄
の台形の面積に対応している。
以上の演算器に続く、並進位置ベクトル演算器
15と回転(姿勢)行列演算器16は同時並列演
算を行なう。並進位置ベクトル演算器15は並進
速度信号Vj(j=i−1、i、i+1)と並進ベ
クトル信号j(j=i−1、i、i+1)およ
び遷移量信号tj(j=i−1、i、i+1)を入
力とし、時刻tにおける並進位置ベクトル信号
j(t)(j=i−1、i、i+1)を出力する。
また後者の回転(姿勢)行列演算器16は回転
速度信号wj(j=i−1、i、i+1)と回転主
軸ベクトル信号j(j=i−1、i、i+1)
および遷移量信号tj(j=i−1、i、i+1)
を入力とし、時刻tにおける回転(姿勢)行列信
号C〜j(t)(j=i−1、i、i+1)を出力す
る。
最後に、位置・姿勢行列軌跡演算器17では、
前記教育点iの位置・姿勢行列データT〜iと前記
時刻tにおける並進位置ベクトル信号j(t)
(j=i−1、i、i+1)および前記時刻tに
おける回転(姿勢)行列信号C〜j(t)(j=i−
1、i、i+1)とから、位置成分についてはベ
クトル加算、姿勢成分については行列積を作るこ
とにより時刻tにおける位置・姿勢行列軌跡T〜
(t)を出力する。これにより、教育点iとi+
1間の本軌跡発生装置100の一連の動作の1サ
イクルを完了するが、次の教育点i+1とi+2
間の軌跡発生はCPU5からのメモリ読出し制御
信号READによつて、メモリ6の位置・姿勢デ
ータ領域7に記憶されている行列データのアドレ
ス・ポインタが+1加算され読み出されることに
より開始される一連の動作は教育点iとi+1間
の軌跡発生と全く同じであるから詳細説明は省略
する。このサイクル動作はCPU5の制御信号に
より教育点属性制御信号発生器9が教育点属性制
御信号SPEとして軌跡のエンド点を指示する信号
を発するまで続く。
以下、第4図の各演算器ブロツクについて、そ
の詳細を説明する。
なお、第5図は各演算ブロツクに用いる基本演
算回路を記号的に示すものである。用いる基本演
算回路としては、スカラ和回路101、スカラ差
回路102、スカラ積回路103、スカラ商回路
104、2乗根回路105、余弦回路106、逆
余弦回路107、ベクトル和回路108、ベクト
ル差回路109、ベクトル内積回路110、ベク
トル積回路111、ベクトル絶対値回路112、
単位ベクトル回路113、ベクトル定数積回路1
14、3行3列行列積回路115、位置・姿勢行
列4行4列バツフアレジスタ回路116等があ
る。
第6図は遷移変数演算器12の詳細なハード・
ウエア構成の一例を示す。本演算器12はベクト
ル・セレクタ21,22、マトリツクス・セレク
タ23,24、スカラ・セレクタ26、比較器2
5の外に、第5図に示す基本演算回路101,1
02,103,104,105,106,10
7,108,109,110,111,112,
113から成つている。メモリ6から読み出され
た教育点i−1、i、i+1、i+2における位
置・姿勢行列信号データT〜i-1、T〜i、T〜i+1、C
i+2
はその位置ベクトル成分信号i-1ii+1
i+2がベクトル・セレクタ21,22の入力と
なり、姿勢行列成分信号C〜i-1、C〜i、C〜i+1、C
i+2
がマトリツクス・セレクタ23,24の入力とな
つて教育点データの区間選択を行う信号TPSに
より、それぞれ位置ベクトル成分信号の一組j
Pj+1と姿勢行列成分信号の一組C〜j、C〜j+1
選択される。
この位置ベクトル成分信号の一組jj+1
らは並進距離信号Lj=|j+1j|(j=i−
1、i、i+1)と並進ベクトル信号j=(j+1
j)/Lj(j=i−1、i、i+1)が演算さ
れる。
また姿勢行列成分信号の一組C〜j、C〜j+1からは
回転角信号ξj=cos-1(vj)(j=i−1、i、i
+1)と回転主軸ベクトル信号jj/|qj|
(j=i−1、i、i+1)が演算される。なお、
vjはvj=1/2{(jj+1)+(jj+1)+(
j、 zj+1)−1}の演算によつて算出され、jj
j×j+1j×j+1j×j+1の演算によ
つて算出される。ここで、(jj+1)および
×j+1はそれぞれベクトルjj+1との内積お
よび外積を表わす。
そして、前記並進距離信号Lj(j=i−1、i、
i+1)と回転角信号ξj(j=i−1、i、i+
1)および回転主軸から仮想点迄の距離rhとから
は遷移(並進・回転)距離信号Sj=√2 j+(h j
2(j=i−1、i、i+1)が演算される。
この遷移(並進・回転)距離信号Sjと指定速度
信号Vの2乗と指定加・減速度信号αの比V2/αと が比較器25で比較され、比較出力信号LASが
得られる。この信号LASは速度パターンが第2
図に示す如く台形状になるか、第3図に示す如く
三角形状になるかを表わし、スカラセレクタ26
を制御する。
スカラセレクタ26は、加・減速度信号αと距
離信号Sjとから演算した信号√・jと指定速度
信号Vを入力とし、仮想点上における速度信号
VVj(j=i−1、i、i+1)を出力する。こ
の信号VVjに、前記並進距離信号Lj(j=i−1、
i、i+1)と遷移距離信号Sj(j=i−1、i、
i+1)の比を掛けて分速度である並進速度信号
Vj(j=i−1、i、i+1)を演算するととも
に、前記回転信号ξj(j=i−1、i、i+1)
と遷移距離信号Sj(j=i−1、i、i+1)の
比を掛けて分速度である回転速度信号wj(j=i
−1、i、i+1)を演算する。即ち、Vj
(Lj/Sj)・VVjおよびwj=(ξj/Sj)VVjを演算す
る。
最終的に、遷移変数演算器12から出力される
のは、並進ベクトル信号j(j=i−1、i、
i+1)、遷移距離信号Sj(j=i−1、i、i+
1)、並進速度信号Vj(j=i−1、i、i+
1)、回転速度wj(j=i−1、i、i+1)お
よび回転主軸ベクトル信号j(j=i−1、i、
i+1)である。
第7図は遷移所要時間演算器13の詳細なハー
ド・ウエア構成を示す。本演算器はスカラ・セレ
クタ27,28,29,30,31,32および
比較器25の外に、第5図に示す基本演算回路1
01,103,104,105で構成されてい
て、前記遷移距離信号Sj(j=i−1、i、i+
1)そして指定された速度信号Vおよび加・減速
度信号αから、各教育点間毎の遷移時間信号Tj
(j=i−1、i、i+1)と加・減速時間信号
τj(j=i−1、i、i+1)を演算する。
比較器25は、前記遷移距離信号Sj(j=i−
1、i、i+1)と指定速度信号Vの2乗に対す
る指定加・減速度信号αの比から、第2図および
第3図に示す台形状および三角形状速度パターン
に対応してスカラセレクタの切換制御をする信号
LASを作る。この制御信号によりスカラセレク
タ31は前記加・減速時間τj(j=i−1、i、
i+1)を選択して出力し、スカラセレクタ27
は教育点がスタートとエンドである場合の、スカ
ラセレクタ28はスタートとパスである場合の、
セレクタ29はパスとエンドである場合の、そし
てセレクタ30はパスとパスである場合の遷移時
間信号を選択し、同じくスカラセレクタ32へ出
力する。
スカラセレクタ32では前記教育点属性制御信
号発生器9からの出力である制御信号SPEによつ
て選択制御がなされ、最終的に、前記4つの遷移
時間信号の内から只一つが選ばれて教育点間遷移
時間信号Tj(j=i−1、i、i+1)が出力さ
れる。
第8図は遷移量積算器14の詳細なハードウエ
ア構成を示す。本積算器はスカラセレクタ37,
38,39,40,41,42,43,44およ
び45と比較器33,34,35および36の外
に、第5図に示す基本演算回路101,102,
103,104で構成されている。そして、前記
教育点間遷移時間信号Tj(j=i−1、i、i+
1)と加・減速時間信号τj(j=i−1、i、i
+1)および前記カウンタ11からの出力である
時刻信号tとから、第2図または第3図に示す速
度パターンに従つて、速度パターンの最高値を1
とした場合の時刻tまでの台形又は三角形の面積
を意味する遷移量tj(j=i−1、i、i+1)
を積算する。
比較器33は現在の演算に使われている教育点
がパス点でしかも現在発生している軌跡は速度パ
ターンの減速期間にあるかどうかを選定制御する
信号PDを出力し、比較器34は同じくパス点で
加速期間にあるかどうかを選定制御する信号PA
を出力し、比較器35は教育点がエンド点で速度
パターンの減速期間にあるかどうかを選定制御す
る信号EDを出力し、そして比較器36は教育点
がスタート点で速度パターンの加速期間にあるか
どうかを選定制御する信号SAを出力する。
スカラセレクタ37および41は前記信号PD
およびSAによりそれぞれ制御されて、スタート
→パス教育点間における遷移量信号を選択する。
スカラセレクタ38および42は前記信号EDお
よびSAによりそれぞれ制御されて、スタート→
エンド教育点間における遷移量信号を選択する。
スカラセレクタ39および43は前記信号EDお
よびPAによりそれぞれ制御されてパス→エンド
教育点間における遷移量を選ぶ。同様にしてスカ
ラセレクタ40および44は前記信号PDおよび
PAによりそれぞれ制御されてパス→パス教育点
間における遷移量を選んで出力する。
最後にスカラセレクタ45は、前記信号SPEの
制御によつて前記スタート→パス、スタート→エ
ンド、パス→エンドおよびパス→パスの各教育点
間の遷移量信号の内から、現在の軌跡発生に関与
するものを只一つ選び時刻tの遷移量信号tj(j
=i−1、i、i+1)を出力する。
第9図は並進位置ベクトル演算器15の詳細な
ハードウエア構成を示す。本演算器は第5図で示
す基本演算回路103および114で構成されて
いて、前記並進ベクトル信号j(j=i−1、
i、i+1)に分速度である並進速度信号Vj(j
=i−1、i、i+1)および、時刻tにおける
遷移量信号tj(j=i−1、i、i+1)を掛け
て並進位置ベクトル信号j(t)(j=i−1、
i、i+1)を出力する。
第10図は回転(姿勢)行列演算器16の詳細
なハードウエア構成を示す。本演算器は第5図に
示す基本演算回路101,102,103,10
5および106から構成されていて、前記回転角
速度信号wj(j=i−1、i、i+1)も時刻t
における遷移量信号tj(j=i−1、i、i+1)
を掛け、時刻t迄に回転主軸まわりに回転すべき
回転角を求め、これと前記回転主軸ベクトル信号
Ωj(j=i−1、i、i+1)の各3成分Ωj
(1)、Ωj(2)、Ωj(3)から回転行列の3行3列要素を
求めて回転(姿勢)行列信号C〜j(t)(j=i−
1、i、i+1)を出力する。
第11図は位置・姿勢行列軌跡演算器17の詳
細なハードウエア構成を示す。本演算器は第5図
に示す基本演算回路108,115および116
から構成されていて、教育点iに対応する位置・
姿勢行列データT〜iの位置ベクトル成分iについ
て、前記並進位置ベクトル信号i-1(t)、i
(t)およびi+1(t)とベクトル加算をし時刻
tに於ける工具端2の位置ベクトル信号(t)
そしてバツフア・レジスタ116に一時ストア
し、また、位置・姿勢行列データT〜iの姿勢行列
成分C〜iについては、前記回転(姿勢)行列信号
C〜i-1(t)、C〜i(t)、およびC〜i+1と行列積
をと
り、時刻tにおける工具端2の姿勢行列信号C〜
(t)としてバツフアレジスタ116へ一時スト
アする。即ち、バツフアレジスタ116には第1
図cに示す信号成分(t)、(t)、(t)、
z(t)が一時記憶される。最後に、これらの各
信号成分は行列形式にまとめられ、位置・姿勢行
列軌跡信号T〜(t)として出力される。
第11図に示すように3つの並進位置ベクトル
信号i-1(t)、i(t)およびi+1(t)をベ
クトル加算するので、i−1、i、i+1の3点
が同一直線上にない場合にはパス点iの近傍の前
後の一定時間内、即ちTi−τiからTi+τiにおいて、
互に方向の異なるベクトルを合成することにな
り、第1図cに示すように、工具端2の描く軌跡
3が教育パス点i(なお、第1図cでは教育点i
+1がこれに対応)の前後のTi−τiからTi+τi
間で内接楕円弧になる。また第11図に示すよう
に3つの回転(姿勢)行列信号C〜i-1(t)、C〜i
(t)およびC〜i+1(t)の行列積をとるので、
パス点i近傍の前後のTi−τiからTi+τiの区間で
は互に回転主軸の異なる回転行列を掛け合わせる
ことになるので、第1図cに示すように、工具端
2の回転主軸の描く包絡線4が教育パス点i(第
1図cでは教育点i+1がこれに対応)の前後の
Ti−τiからTi+τiの区間で滑らかに繋がり、回転
主軸の方向が突変しない。
以上において、本発明の軌跡発生装置のハード
構成の一実施例について詳細に説明したが、論理
演算機能部分はコンピユータ・ソフトウエアで実
現することができる。第12図はその一実施例の
動作のフロー・チヤートである。この動作手順に
ついて以下に詳細に説明する。
本軌跡発生プログラムがスタートすると、手順
200により教育点数Neodおよび時間きざみuが読
み込まれる。続いて、手順201により軌跡情報デ
ータである速度V、加・減速度αおよび回転主軸
から仮想点までの距離rhを読み込みさらに手順
202によつて第1教育点をi=1に設定した後、
手順204で教育点i−1、i、i+1、i+2に
対応する位置・姿勢行列データT〜i-1、T〜i、T〜i+
1

T〜i+2を読み込む。尚、教育点がスタート点であ
ればT〜i-1は読まれず、また教育点がスタート点
とエンド点の2点だけであるならばT〜iとT〜i+1
けが読まれる。
次に、これらの教育点データと軌跡情報データ
を基に、手順205において、並進ベクトル信号
、並進速度Vj、回転主軸ベクトルj(34)、回
転速度wj(33)および遷移距離Sjを計算し、手順
206において、遷移時間Tjおよび加・減速時間τj
を計算する。これらの計算手順は、第4図ハード
ウエア実施例において説明した動作手順と同じで
あるから説明は省略する。
そして、手順207では、時刻tを0に設定
し、手順209で、時刻tにおける遷移量tjを計算
し、手順210で、時刻tにおける並進位置ベクト
j(t)、手順211で、時刻tにおける回転
(姿勢)行列C〜j(t)を計算し、最終的に、手順
212で、時刻tにおける位置・姿勢行列軌跡T〜
(t)を計算する。これらの計算方法も、第4図
ハードウエア実施例の動作と全く同じであるから
説明は省略する。
手順212で計算された後、位置・姿勢行列軌跡
C〜(t)は、手順213により、例えばロボツト等
の多リンク機構を制御する場合ならば、各関節の
ジヨイント変数を解析する座標変換プログラムな
どに、一定の時間u毎に出力される。
続いて、次の計算サイクルを確立するために、
手順214で、時刻tが教育点iとi+1間の遷移
時間Tiを越えていないかを判断し、YESならば
手順215により、時刻tに時間きざみuを加え
たものを新たに時刻tとし、手順216により、前
記遷移時間Tiを越えたかどうか判定し、YESな
らば手順217で時刻tを遷移時間Tiとなし、NO
ならば手順218へ飛んだ後、手順208へ還り、再び
手順209以降の計算がくり返され、次々と位置・
姿勢行列軌跡T〜(t)が出力される。これは手順
214でNOの判定が下されるまで続く。
手順214でNOならば、手順219へ進み教育点i
がNeodより1だけ少ない数すなわち軌跡計算終
了点になつたかどうか判断し、NOならば手順
220により教育点iに1を加算して手順203へ還
り、手順204のデータ読み込み以降教育点i+1
に対する軌跡発生がくり返される。この動作は手
順219でYESの判定がなされる迄続き、YESなら
ばただちに終了する。
第13図は以上に説明した本発明による軌跡発
生装置を多関節型ロボツトに適用してなる軌跡制
御方式のブロツク図を示すものである。
ロボツト手先1または工具端2の位置iおよ
び姿勢iiiの移動モードには(i)教育モー
ドおよび(ii)再生モードがある。そして、教育モー
ドには、テイーチ・ペンダント・ボツクス400
に装着されたスイツチにより、目標ジヨイント角
θを指令するジヨイント角モードと〓、〓、〓
方向への並進および回転を与える位置・姿勢行列
目標値40を指令する基座標モードおよびi
ii方向への並進・回転を与える位置・姿勢
行列目標値40を指令する工具座標モードがあ
る。
テイーチ・ペンダント・ボツクス400からジ
ヨイント角モードでスイツチ操作を行うと、デコ
ーダ402は、目標ジヨイント角指令手段403
を駆動し、その出力である目標ジヨイント角を
直接6軸サーボ手段410に入力する。この時、
6軸サーボ手段410には、同時に、エンコーダ
413から現在ジヨイント角*微分器411か
らジヨイント角速度*およびモータ412から
偏差電流が入力され、それぞれ位置フイード・
バツク、速度フイード・バツクおよび電流フイー
ド・バツクがかけられ、その偏差出力によつてモ
ータ412が駆動され、最終的に目標ジヨイント
角に一致するまで動く。
次に、テイーチ・ペンダント・ボツクス400
から基座標モードおよび工具座標モードでスイツ
チ操作を行うと、デコーダ402は目標位置・姿
勢指令手段404を駆動し、位置・姿勢行列レジ
スタ・バツフア405に目標位置・姿勢行列信号
T〜を入力する。この目標位置・姿勢行列信号T〜は
工具逆変換手段406およびジヨイント座標逆変
換手段408を経て目標ジヨイント角に変換さ
れ、前記6軸サーボ手段410の入力となりその
サーボ偏差出力がモータ412を、最終的に目標
ジヨイント角に一致するまで駆動することによ
つて、ロボツト手先1に装着された工具2が目標
位置・姿勢行列信号T〜に一致するまで動く。この
ようにして、教育モードではロボツト手先1に装
着した工具2をジヨイント角モード、基座標モー
ドおよび工具座標モードで目標とする位置・姿勢
へ移動させることができる。その際、テイーチ・
ペンダント・ボツクス400に装着されているレ
コードスイツチを押すか、またはコンソール40
1から‘RECODE'の如くKEY INすることによ
つてCPU5にレコード指令を与えると、モータ
412の回転軸に直結されたエンコーダ413の
出力である現在ジヨイント角*がジヨイント座
標順変換手段409および工具順変換手段407
を経て工具端2の位置・姿勢行列信号T〜に変換さ
れ、位置・姿勢行列レジスタ・バツフア405に
ストアされ、さらに教育点iの位置・姿勢行列デ
ータT〜iとしてメモリ6の位置・姿勢行列データ
領域7にストア(RECODE)される。
次に再生モードでは、あらかじめ採集され前記
位置・姿勢行列データ領域7にストアされた教育
点……i−1、i、i+1……の位置・姿勢行列
データ……T〜i-1、T〜i、T〜i+1、……とコンソー

401から指令されCPU5によつてメモリ6の
軌跡情報データ領域8へストアされた回転主軸か
ら仮想点までの距離rh、その仮想点上で与えられ
た並進速度Vおよび加減速度αがCPU5からの
READ信号のタイミングによつて本発明になる
軌跡発生装置100へ読み出される。第4図に前
述した如く本軌跡発生装置100では、教育点間
の任意時刻における位置・姿勢行列信号データC〜
(t)を刻々と発生せしめ、CPU5からのリア
ル・タイム・クロツク信号RTCに同期して位
置・姿勢行列レジスタ・バツフア405へ出力す
る。前記位置・姿勢行列信号データT〜(t)は工
具逆変換手段406およびジヨイント座標逆変換
手段408を経て、目標ジヨイント角に変換さ
れ、前記6軸サーボ手段410によりモータ41
2を目標ジヨイント角に一致させるように駆動す
る。ロボツトが目標ジヨイント角に一致するよう
に動けば、結果的に、ロボツト手先1に装着した
工具端2の位置・姿勢が目標位置・姿勢行列T〜
(t)と一致するように動くので、多関節型ロボ
ツトを直交座標空間で移動させることができる。
従来、組立用ロボツトなどにしばしば要求され
る急激な姿勢変化を伴つた並進軌跡制御では、速
度・加速度が突変し、高速軌跡制御ができず、充
分な軌跡精度も得られなかつたが、本発明になる
軌跡発生装置100を第12図に示す多関節型ロ
ボツトの軌跡制御方式に組み入れることにより、
位置・姿勢のいかなる変化にも高速(速度1m/
s、加減速度1g)追従し、軌跡精度も0.1mm以内
の高精度を達成することができる。
また、本発明の一態様によれば、各教育パス点
iに関して、3つの並進位置ベクトル信号i-1
(t)、i(t)およびi+1(t)をベクトル加
算するので、工具端2の描く軌跡を内接楕円弧と
することができ、また、3つの回転(姿勢)行列
信号C〜i-1(t)、C〜iおよびC〜i+1(t)の行列
積を
とるので、工具端2の回転主軸の描く包絡線が各
教育パス点の近傍で滑らかに繋がり、回転主軸の
方向が突変しないようにできる。
【図面の簡単な説明】
第1図ないし第3図は本発明の基本原理を説明
するための図である。第4図は本発明の軌跡発生
装置の一実施例を示すブロツク図である。第5図
は第4図に示す軌跡発生装置における各演算ブロ
ツクに用いる基本演算回路を示す図である。第6
図は第4図の実施例における遷移変数演算器の回
路構成の一例を示す図である。第7図は第4図の
実施例における遷移所要時間演算器の回路構成の
一例を示す図である。第8図は第4図の実施例に
おける遷移量積算器の回路構成の一例を示す図で
ある。第9図は第4図の実施例における並進位置
ベクトル演算器の回路構成の一例を示す図であ
る。第10図は回転(姿勢)行列演算器の回路構
成の一例を示す図である。第11図は第4図の実
施例における位置・姿勢行列軌跡演算器の回路構
成の一例を示す図である。第12図は本発明の軌
跡発生装置の機能の一部をコンピユータソフトウ
エアを用いて実現する実施例の動作のフローチヤ
ートである。第13図は本発明の軌跡発生装置を
多関節型ロボツトに適用した軌跡制御方式のブロ
ツク図である。 1……ロボツト手先、2……工具端、3……位
置ベクトル(t)の軌跡、4……回転主軸ベク
トルの先端が描く包絡線、5……制御・管理
CPU、6……メモリ、7……位置・姿勢データ
領域、8……軌跡情報データ領域、9……教育点
属性制御信号発生器、10……教育点選択信号発
生器、11……時刻演算カウンタ、12……遷移
変数演算器、13……遷移所要時間演算器、14
……遷移量積算器、15……並進位置ベクトル演
算器、16……回転(姿勢)行列演算器、17…
…位置・姿勢行列軌跡演算器、21,22……ベ
クトルセレクタ、23,24……マトリツクスセ
レクタ、25,33〜36……比較器、26〜3
2,37〜45……スカラセレクタ、100……
軌跡発生装置。

Claims (1)

  1. 【特許請求の範囲】 1 複数の教育点の位置・姿勢行列データおよび
    教育点間の軌跡情報データを設定し、これらのデ
    ータを基にロボツト手先に装着した工具端の軌跡
    を制御する多関節型ロボツトの軌跡制御装置にお
    いて、 前記ロボツト手先に装着した工具端の回転主軸
    と直交した方向に前記回転主軸より距離rhにある
    仮想点が回転に伴い描く円弧と前記工具端が並進
    することにより描く直線とを合成した螺旋軌跡上
    の速度Vおよび加・減速度αとして前記軌跡情報
    データを設定する手段と、 前記螺旋軌跡上の速度Vおよび加・減速度αに
    基いて定まる螺旋軌跡上の速度パターンからその
    分速度として工具端の並進速度パターンと回転主
    軸回りの回転速度パターンを演算する手段と、 これらの並進速度パターンおよび回転速度パタ
    ーンに従い、位置について並進し、姿勢について
    回転主軸まわりに回転する位置・姿勢行列信号を
    発生させる手段と、 を備えたことを特徴とする前記装置。 2 少くとも1個のパス教育点iを含む複数の教
    育点……、i−1,i,i+1,……の位置・姿
    勢行列データ……T〜i-1,T〜i、T〜i+1、T〜i+2
    ……
    および教育点間の軌跡情報データを設定し、これ
    らのデータを基にロボツト手先に装着した工具端
    の軌跡を制御する多関節型ロボツトの軌跡制御装
    置において、 前記ロボツト手先に装着した工具端の回転主軸
    と直交した方向に前記回転主軸より距離rhにある
    仮想点が回転に伴い描く円弧と前記工具端が並進
    することにより描く直線とを合成した教育点間の
    螺旋軌跡上の速度Vおよび加・減速度αとして前
    記軌跡情報データを設定する手段と、 前記螺旋軌跡上の速度V、加・減速度αおよび
    位置・姿勢データT〜i-1,T〜i、T〜i+1、T〜i+2
    ら、
    各教育点間の螺旋軌跡上の時刻tにおける速度パ
    ターンの時間積分値を表わす量tj(但しj=i−
    1、i、i+1)、および前記各教育点間の螺旋
    軌跡上の速度に対する分速度として各教育点間の
    並進速度Vj(但しj=i−1、i、i+1)およ
    び回転主軸まわりの回転速度Wj(但しj=i−
    1、i、i+1)を算出する手段と、 前記時刻tにおける速度パターンの時間積分値
    を表わす量tj、前記並進速度Vjおよびその並進ベ
    クトルjから、並進位置ベクトルi-1(t)、i
    (t)、i+1(t)を算出する手段と、 前記時刻tにおける速度パターンの時間積分値
    を表わす量tj、前記回転速度wjおよび回転主軸ベ
    クトルjから、回転姿勢行列信号C〜i-1(t)、C〜
    i
    (t)、C〜i+1(t)を算出する手段と、 教育点iに対応する位置・姿勢行列データT〜i
    の位置ベクトル成分iについて、前記並進位置
    ベクトルi-1(t)、i(t)およびi+1(t)
    とベクトル加算をして工具端の位置ベクトル信号
    P(t)を得る手段と、 位置姿勢行列データT〜iの姿勢行列成分C〜i
    ついて、前記回転姿勢行列信号C〜i-1(t)、C〜i
    (t)およびC〜i+1(t)と行例積をとり工具端の
    姿勢行列信号C〜(t)を得る手段と を備えたことを特徴とする前記装置。
JP57215589A 1982-12-10 1982-12-10 多関節型ロボツトの軌跡制御装置 Granted JPS59106007A (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP57215589A JPS59106007A (ja) 1982-12-10 1982-12-10 多関節型ロボツトの軌跡制御装置
US06/560,012 US4529921A (en) 1982-12-10 1983-12-09 Tracking control system of multijoint robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP57215589A JPS59106007A (ja) 1982-12-10 1982-12-10 多関節型ロボツトの軌跡制御装置

Publications (2)

Publication Number Publication Date
JPS59106007A JPS59106007A (ja) 1984-06-19
JPH0319964B2 true JPH0319964B2 (ja) 1991-03-18

Family

ID=16674929

Family Applications (1)

Application Number Title Priority Date Filing Date
JP57215589A Granted JPS59106007A (ja) 1982-12-10 1982-12-10 多関節型ロボツトの軌跡制御装置

Country Status (2)

Country Link
US (1) US4529921A (ja)
JP (1) JPS59106007A (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022225042A1 (ja) 2021-04-23 2022-10-27 王子ホールディングス株式会社 ヒートシール紙および包装袋

Families Citing this family (33)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2684359B2 (ja) * 1985-02-22 1997-12-03 ファナック 株式会社 ロボットのワーク直交座標系設定装置
US4718078A (en) * 1985-08-19 1988-01-05 Siemens Aktiengesellschaft System for controlling motion of a robot
US4698572A (en) * 1986-04-04 1987-10-06 Westinghouse Electric Corp. Kinematic parameter identification for robotic manipulators
US4772831A (en) * 1986-11-20 1988-09-20 Unimation, Inc. Multiaxis robot control having improved continuous path operation
US4786847A (en) * 1986-11-20 1988-11-22 Unimation Inc. Digital control for multiaxis robots
US4864206A (en) * 1986-11-20 1989-09-05 Westinghouse Electric Corp. Multiaxis robot control having improved energy monitoring system for protecting robots against joint motor overload
US4774445A (en) * 1986-11-20 1988-09-27 Unimation, Inc. Multiaxis robot control having capability for executing timed moves
US4882527A (en) * 1987-10-16 1989-11-21 Nissan Motor Co., Ltd. Three-dimensional measuring robot
JPH02113305A (ja) * 1988-10-24 1990-04-25 Fanuc Ltd スプライン補間方法
US5250886A (en) * 1988-12-27 1993-10-05 Canon Kabushiki Kaisha Method of controlling robot and robot control apparatus
JP2786225B2 (ja) * 1989-02-01 1998-08-13 株式会社日立製作所 工業用ロボットの制御方法及び装置
DE58909509D1 (de) * 1989-02-28 1996-01-04 Siemens Ag Steuerungsverfahren bei einer numerischen Werkzeugmaschine oder einem Roboter.
EP0398704B1 (en) * 1989-05-17 1996-08-14 Fujitsu Limited Profile control system for robots
JP2874238B2 (ja) * 1990-01-23 1999-03-24 株式会社安川電機 多関節形ロボットの制御方法
DE4014647C2 (de) * 1990-05-08 1995-03-30 Voith Gmbh J M Einrichtung zum Auftragen einer Beschichtung auf eine laufende Warenbahn
JPH04111006A (ja) * 1990-08-30 1992-04-13 Kobe Steel Ltd ロボットの経路補間方法
EP0530401B1 (de) * 1991-09-06 1996-07-24 Siemens Aktiengesellschaft Verfahren zum Auslösen von positionsbezogenen Schaltvorgängen während eines von einem Roboter oder einer Werkzeugmaschine ausgeführten Bearbeitungsvorganges
FR2682905B1 (fr) * 1991-10-28 1995-12-01 Commissariat Energie Atomique Procede de generation de trajectoire pour un systeme robotise.
JPH05185387A (ja) * 1992-01-14 1993-07-27 Mitsubishi Electric Corp ロボットの制御装置
KR0160998B1 (ko) * 1992-09-18 1998-12-15 윤종용 로보트의 구동경로 계획방법
US5373221A (en) * 1993-07-30 1994-12-13 Fanuc Robotics North America, Inc. Method and system for estimating robot tool center point speed
US5602968A (en) * 1994-05-02 1997-02-11 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Task space angular velocity blending for real-time trajectory generation
US5740327A (en) * 1994-12-27 1998-04-14 Nec Corporation Method of and apparatus for robot tip trajectory control
KR100342255B1 (ko) 1999-06-25 2002-06-27 윤종용 로봇 구동용 모터의 속도제어방법
JP3681972B2 (ja) * 2000-10-18 2005-08-10 ファナック株式会社 加減速制御方法
US20040236453A1 (en) * 2003-05-22 2004-11-25 Gabor Szoboszlay Method and apparatus for combining and generating trajectories
JP5323280B1 (ja) * 2012-07-26 2013-10-23 三菱電機株式会社 数値制御装置
EP2845696B1 (de) * 2013-09-10 2017-05-17 Siemens Aktiengesellschaft Bearbeitungsmaschine mit redundanten Achsen und Auflösung der Redundanz in Echtzeit
JP6360301B2 (ja) * 2013-12-25 2018-07-18 川崎重工業株式会社 動作プログラム作成方法およびロボットの制御方法
US11141855B2 (en) * 2018-01-15 2021-10-12 Canon Kabushiki Kaisha Robot system, method of controlling robot arm, recording medium, and method of manufacturing an article
CN110888422B (zh) * 2018-08-21 2022-05-27 广东大冶摩托车技术有限公司 一种二维跟踪的机器人喷涂编程方法
CN113125463B (zh) * 2021-04-25 2023-03-10 济南大学 一种用于汽车轮毂焊缝缺陷检测的示教方法及装置
CN113894796B (zh) * 2021-11-24 2022-12-13 伯朗特机器人股份有限公司 一种基于梯形规划的机械臂多关节轨迹时间同步方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4163183A (en) * 1975-10-28 1979-07-31 Unimation, Inc. Programmable automatic assembly system

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022225042A1 (ja) 2021-04-23 2022-10-27 王子ホールディングス株式会社 ヒートシール紙および包装袋

Also Published As

Publication number Publication date
US4529921A (en) 1985-07-16
JPS59106007A (ja) 1984-06-19

Similar Documents

Publication Publication Date Title
JPH0319964B2 (ja)
US4308584A (en) Apparatus for control of manipulator
US4528632A (en) Industrial articulated robot linear interpolation control device
US4449196A (en) Data processing system for multi-precision arithmetic
EP0523889A2 (en) Apparatus for controlling industrial robot to perform coordinated operation using teaching playback method and method thereof
JPS626246B2 (ja)
JPH079606B2 (ja) ロボット制御装置
JPH01230105A (ja) ロボットのツール姿勢制御方法
JP2874238B2 (ja) 多関節形ロボットの制御方法
US4803640A (en) Control arrangement for an industrial robot
US4677276A (en) Method of welding robot control which controls the three basic axes of a robot to provide a repetition of a weaving pattern along a robot locus
JP3493765B2 (ja) 産業用ロボットの制御方法とその装置
JPH0256682B2 (ja)
JPH0420203B2 (ja)
JPS62154006A (ja) ロボツト制御装置
JPS5823186B2 (ja) ヨウセツヨウロボツトノ セイギヨソウチ
JPH10244481A (ja) ロボットの動作プログラム作成方法および装置
JP2787207B2 (ja) 多軸位置サーボ装置
JPH02144705A (ja) ロボットアームの制御方法
JPS60220408A (ja) 関節形ロボツト用制御装置
JPH1148091A (ja) 軌跡制御装置
JPH0428471B2 (ja)
JP3457693B2 (ja) ロボットの軌跡制御方法
JPH1020910A (ja) ロボットコントローラおよびロボットの制御方法
JP2601433B2 (ja) 工業用ロボットの補間制御方法および装置