JPH0254566B2 - - Google Patents

Description

【発明の詳細な説明】 この発明は産業用ロボツトの制御装置に係り、
主として高速移動制御における移動軌跡をより正
確ならしめるように改良したものに関する。

従来産業用ロボツトにおいて、エンドエフエク
タの位置を自動制御するに際し、補間を実行する
ことは周知である。しかしながら、産業用ロボツ
トの各制御軸の出しうる最大速度は、それに取り
付けられた動力によつて定まつてしまう。そのた
め、指令速度が大なる場合は、ある制御軸の速度
が追いつかず、エンドエフエクタの軌跡が所望ど
おりとならぬ場合がおこる。これではエンドエフ
エクタが予期せぬ軌跡を移動し、他と干渉するな
どの不都合が生じる。

そこでこの発明においては、各制御軸のうち、
指令分速度を出し得ない制御軸があつた場合は、
他の制御軸の分速度を減縮して、エンドエフエク
タの移動速度が多少低下しても、移動軌跡をでき
るかぎり正確ならしめ、もつて前述問題点を解決
しうる制御装置を提供することを主たる目的とす
る。この発明の他の目的や特徴は、以下のさらに
詳細な説明によつて逐次明確とされる。

以下、この発明実施例につき、図面を参照しつ
つ詳述する。なおこの実施例は、産業用ロボツト
の制御軸を多関節系としたものであり、そのエン
ドエフエクタは、溶接トーチとしたものである
が、この発明をこの実施の形態に限定するもので
はない。なお、この明細書において、産業用ロボ
ツトの一般的な制御系をＸ系、直角座標系をｘ
系、さらに多関節系をα系と称することとする。

第１図はこのような多関節形産業用ロボツトを
用いた自動溶接装置の一例と示す図解図である。
固定部材１がたとえば地面に固定され、その固定
部材１には、高さの比較的低い円筒形ベース３が
取付けられる。この円筒形ベース３の上端には、
比較的高さの高い円筒形回転体４が回転軸（図示
せず）によつて回転自在に設けられる。この円筒
形回転体４の中空部には、図示しないが、垂直回
転軸が設けられ、この円筒形回転体４の上方には
それと一体的に回転するように支持された回転体
５が設けられる。この円筒形回転体４かつしたが
つて回転体５は、図示しないが、後述のモータに
よつて回動角α₁について回動駆動される。回転体
５は、その上面からほぼ垂直にかつ平行に延びる
支持部材７，７を有する。この支持部材７，７に
は、水平方向の軸９によつて、回動腕１１が、回
動角α₂について回動自在に支持される。なお、こ
の回動腕１１は、図示しないが、後述のモータに
よつて回動駆動される。また、回動腕１１に関連
して、この回動腕の回動の状態に対してバランス
をとるためのバランス機構１３が設けられる。こ
のバランス機構１３は、図示しないが、たとえば
引つぱりばねなどを含み、円筒形回転体４と一体
に回転するように構成されている。

回動腕１１の先端には、軸９に平行な軸１５に
よつて、回動腕１７が、この腕１１に対してすな
わち回動角α₃について回動自在に支持される。そ
して、この回動腕１７は、図示しないが、後述の
モータによつて回動駆動される。回動腕１７の一
方の自由端には軸９に平行な軸１９によつて、回
動軸２１が、この回動腕１７に対してすなわち回
動角α₄について回動自在に支持される。この軸２
１は、図示しないが、後述のモータによつて、軸
１９周りに回動駆動される。軸２１には、トーチ
取付具２５を支持する回転軸２３が、軸２１に対
して同軸に回動自在に支持される。この軸２３
は、図示しないが、後述のモータによつて、軸２
１に対してすなわち回動角α₅について回動駆動さ
れる。トーチ取付具２５には、エンドエフエクタ
としての溶接トーチ２７が取付けられる。このよ
うにして、この自動溶接装置は、回転体５、回動
腕１１、回動腕１７、軸２１および回転軸２３の
それぞれの回動角α₁、α₂、α₃、α₄およびα₅を制御
することによつて、溶接トーチ２７の姿勢および
その先端すなわち溶接点Ｐの位置を制御する。す
なわちこの自動溶接装置は５自由度のα系産業用
ロボツトＲである。

第２図はこの発明の制御装置Ｃの一実施例を示
す概略ブロツク図である。この実施例では、操作
パネル１００が設けられる。この操作バネル１０
０には、自動溶接装置をどの動作モードで動かす
かを指示するモード切換スイツチ１０１が設けら
れる。このモード切換スイツチ１０１は、マニユ
アルモードＭ、テイーチングモードＴおよびオー
トモードＡのいずれかのモードを選択的に切換え
て設定できる。操作バネル１００には、さらに、
押ボタンスイツチ１０３，１０５，１０７および
１０９が設けられる。押ボタンスイツチ１０３
は、オートモードのスタート指令を与えると共
に、テイーチングモードにおけるテイーチング指
令を与えるために操作される。押ボタンスイツチ
１０５，１０７および１０９は、それぞれ、直線
補間動作、ウイービング動作および円弧補間動作
を、自動溶接装置に指令する場合に、操作され
る。さらに、速度設定器１１１が設けられる。こ
の速度設定器１１１は、溶接トーチ２７かつした
がつて溶接点Ｐの移動すべき速度を指令するため
のものである。操作バネル１００には、２つのグ
ループのマニユアルスイツチ１１９，１２１，１
２３，１２５および１２７ならびに１２９，１３
１，１３３，１３５および１３７が設けられる。
スイツチ１１９ないし１２７は、溶接装置を直角
座標系すなわちｘ系で位置制御するために操作さ
れる。一方、スイツチ１２９ないし１３７は、各
回動角度α₁ないしα₅を直接に制御するために用い
られる。そのために、この操作バネル１００には
スイツチ１１９ないし１２７のグループかまたは
スイツチ１２９ないし１３７のグループのいずれ
を有効にするかを選択する、系切換スイツチ１１
７が設けられる。したがつて、この系切換スイツ
チ１１７を左方（第２図において）に切換えれ
ば、マニユアルスイツチ１１９ないし１２７のグ
ループが有効化され、スイツチ１１７を右方に切
換えることによつてマニユアルスイツチ１２９な
いし１３７のグループが有効化される。

これらマニユアルスイツチ１１９ないし１２７
および１２９ないし１３７は、それぞれ３つの位
置をとることができ、この第２図において実線で
示す位置がニユートラル位置である。そして、ス
イツチ１１９，１２１，１２３は、それぞれｘ
軸、ｙ軸およびｚ軸を制御するために用いられ、
その直角座標の原点から遠ざかる方向がアツプ方
向Ｕとして、その原点に近づく方向がダウン方向
Ｄとして、規定されている。また、スイツチ１２
５は溶接トーチ２７の配向角φを制御するために
用いられ、スイツチ１２７は溶接トーチ２７の姿
勢角θを制御するために用いられる。そして、こ
れらスイツチ１２５，１２７は、溶接トーチに関
連する角度φおよびθを、それぞれ、時計方向Ｃ
または反時計方向CCに制御することができる。
同様に、α系のマニユアルスイツチ１２９ないし
１３７も、各腕ないし軸の回動角α₁ないしα₅を、
それぞれ、時計方向Ｃまたは反時計方向CCに制
御することができる。これら各コンポーネントを
含む操作パネル１００とデータバス５５との間
で、インタフエース（図示せず）を介してデータ
のやりとりが行なわれる。

このデータバス５５には、さらに、制御手段を
構成するCPU３１と、メモリ３３とが連結され
る。メモリ３３には、CPU３１のシステムプロ
グラムやユーザプログラムを記憶しておくための
ROMや、CPU３１における演算その他の処理に
必要な記憶領域やフラグ領域を有するRAMが含
まれる。データバス５５には、複数の（この実施
例では５つの回動角α₁ないしα₅に対応して５つ
の）駆動回路３５１，３５２，３５３，３５４お
よび３５５と、これらに対応する複数の（この実
施例では５つの）インクリメンタルエンコーダ５
３１，５３２，５３３，５３４および５３５が、
それぞれ連結される。この第２図においては、駆
動回路３５１が、他を代表してより詳細に描かれ
ている。ここで、この駆動回路３５１の構成につ
いて説明するが、残りの駆動回路３５２ないし３
５５についても、同様の構成をとり得ることを予
め指摘しておく。

駆動回路３５１には、CPU３１からの指令位
置情報がロードされる指令位置バツフア３７と、
対応のインクリメンタルエンコーダ５３１からの
パルス信号をカウントするためのフイードバツク
カウンタ３９が設けられる。指令位置バツフア３
７の内容が被減数として、またフイードバツクカ
ウンタ３９の内容が減数として、減算器４１の２
つの入力として与えられる。減算器４１の出力は
Ｄ／Ａ変換器４３に与えられる。したがつて、こ
のＤ／Ａ変換器４３からは、指令位置と現在位置
との差に応じた電圧信号が導出されることにな
る。Ｄ／Ａ変換器４３の出力は、サーボアンプ４
５、コマンドリミツタ４７を介して、サーボモー
タ４９の駆動信号として与えられる。なお、コマ
ンドリミツタ４７の出力は、さらに、Null信号
検知回路５１に与えられる。このNull信号は、
サーボ系によつて位置制御されたときにサーボア
ンプ４５から得られる零信号であり、目標位置の
ごく近い領域すなわちモータ４９がほとんど停止
するタイミングで導出される。したがつて、この
Null信号検知回路５１の出力は、サーボモータ
４９によつて制御される被制御体が、指令位置バ
ツフア３７にロードされた指令位置に達したこと
を示す信号として、データバス５５を介して
CPU３１に与えられる。

以上のような構成において、以下に、第３図に
示すフローダイヤグラムと第４図ないし第７図に
示す模式図を参照して、第２図かつしたがつて第
１図の実施例の操作ないし動作について説明す
る。

最初に、テイーチングのためのマニユアルモー
ドについて説明する。CPU３１は、その内部に
設けられたクロツクソースからのクロツクを受け
るタイマを有し、そのタイマはクロツクに応じて
或る一定時間ごとに出力を発生する。そして、
CPU３１では、そのタイマの出力があれば、そ
れによつてインタラプトがかかる。そして操作パ
ネル１００の各軸のマニユアルスイツチ１１９，
１２１，１２３，１２５および１２７が、すべ
て、ニユートラル位置に保たれているかどうかを
判断する。換言すれば、このステツプでは、これ
らマニユアルスイツチ１１９ないし１２７のいず
れかが操作されているかどうかを検出する。な
お、マニユアルモードにおいては、直角座標系
（ｘ系）で指令したほうが、ワークピースの溶接
線が相互に直角方向に延長されている場合が多い
ので、オペレータにとつてトーチ２７をこの溶接
線に沿つて移動させ易く、また補間演算等がやり
易いので、操作パネル１００に設けられる系切換
スイツチ１１７は、ｘ系に切換えておく。続い
て、CPU３１は、操作パネル１００からの信号
に基づいて、操作されたマニユアルスイツチの操
作方向（ＵもしくはＤまたはＣもしくはCC）と
速度設定器１１１で設定された速度に基づいて上
述のタイマ時間に対応した指令値（基本量）を増
減する。

かくしてトーチ２７はオペレータの操作によつ
て、そのθ角およびφ角が必要角度に遠隔操作さ
れる。なお角θは第１図２点鎖線図示のように垂
直迄変化させうる。さらにオペレータは溶接点Ｐ
の位置を、図示しないワークＷの溶接線WL上の
テイーチングすべき位置、例えば点P_K-1に移動さ
せ、さらにオートモードにおいてトーチＴを移動
させるべき指令速度Ｖに速度設定器１１１を設定
したうえで、スイツチ１０３を操作すれば、各回
路３５１ないし３５５のカウンタ３９の内容が、
設定器１１１の設定値と共にメモリ３３に取り込
まれる。

同様にして、テイーチング点P_K、P_K+1、およ
びP_K+2をテイーチングする。この際、点P_K+1をテ
イーチングするときに、スイツチ１０５も操作し
て、すなわち直線補間指令を与えておく。

かくして、一連の溶接線WLにつき、テイーチ
ングが完了したなれば、スイツチ１０１を操作し
てテストモードとし、スイツチ１０３を操作すれ
ば、トーチＴはテイーチングされたとおり、しか
しながらトーチ２７は作動せず（電極に電圧が印
加されず）、その位置が自動制御され、オペレー
タは記憶させたユーザプログラムをチエツクし、
不都合個所があれば修正しておく。

そのうえで、オペレータはスイツチ１０１を操
作して、オートモードとし、スイツチ１０３を操
作すれば、トーチ２７は作動しながら溶接線WL
を自動溶接する。

前述テストモードおよびオートモードにおける
トーチ２７の位置制御については、この発明の要
旨にかかわるところであるので、以下、特に詳述
する。

まずステツプS₁において、ｘ系で表現された教
示点P_K、P_K+1間の距離Ｄを演算する。

ステツプS₂においては、距離Ｄをあらかじめ指
令された指令速度Ｖで除して、点P_K、P_K+1間の
移動時間Ｔを演算する。

ステツプS₃において、点P_K+1のテイーチングに
おいて直線補間指令があることに応じて、時間Ｔ
を、ｘ系でのきざみ時間△Ｔで除して補間きざみ
数Ｎを演算する。この実施例では時間△Ｔは
200msecに定めるものとする。なおＮの値は小数
点以下は切り上げる。

ステツプS₄において、補間きざみ点のｘ系の位
置情報をP_Mとし、 P_M＝〔（Ｎ−Ｍ）×P_K＋Ｍ×P_K+1〕／Ｎ を演算する。

ステツプS₅において、補間きざみ各点のｘ系の
位置情報P_M（ｘ）をα系の位置情報p_M（α）に関
数変換する。このような関数変換については、特
に第５図ないし第１２図を参照しつつ詳述する。
なお、この実施例では、CPU３１として、たと
えばマイクロコンピユータを用いるわけである
が、最近のマイクロコンピユータは性能が向上
し、高速演算が可能になつてきた。そこで、この
実施例では、従来近似計算で処理していたもの
を、正規の計算を行ない精度良く処理するように
している。

第５図は、第１図の自動溶接装置の模式図を示
す。この第５図におけるα₁ないしα₅は、第１図に
おけるそれと対応する。そして、この第５図にお
いて、a₁は固定部材１の下端から軸９までの長さ
成分を示し、a₂は軸９から軸１５までの長さを示
し、a₃は軸１５から軸１９までの長さ成分を示
し、a₄は軸１９から溶接トーチ２７の延長線と軸
２１の延長線との交わる点Ｂまでの長さ成分を示
す。そして、点Ｆは軸１５の位置を示し、点Ｑは
軸１９の位置を示し、点Ｐは溶接トーチ２７の先
端すなわち溶接点を示す。そして、角度α₁は、回
転体５の回転角度を示し、α₂は回動腕１１の回動
角度を示し、α₃は回動腕１７の回動角度を示す。
また、角度α₄は軸２１の回動角度を示し、垂直軸
に対する角度を示す。角度α₅は、垂直軸方向で０
度になるように設定された、軸２３の回動角度を
示す。

第６図は第１図すなわち第５図の装置を上から
見た状態を模式的に示す図である。この第６図に
おいて、「絶対系」とは、固定部材１（第１図）
下端のほぼ中心をその座標軸の原点とする直角座
標系をいう。また、「ロボツト系」とは、装置の
或る点を原点とした直角座標系を示す。さらに、
「トーチ系」とは、第７図の点Ｂを原点とした直
角座標系を示す。なお、第７図は点ＱとＢとの間
をより詳細に示す図であり、この第７図における
角度成分α₆は一定である。ここで、直角座標系は
各併進軸成分および角度成分すなわち（ｘ，ｙ，
ｚ，φ，θ）で表わされ、多関節座標系は角度成
分すなわち（α₁，α₂，α₃，α₄，α₅）で表わされ
る。

第５図において各角度α₁ないしα₅が与えられた
ときの各点Ｆ，Ｑ，Ｐの直角座標系の位置を求め
ると、点Ｆのｘ軸成分、ｙ軸成分、ｚ軸成分、
F_x、F_y、F_zは、それぞれ次のようになる。

F_x＝−e₁sinα₁＋a₂sinα₂・cosα₁ F_y＝e₁cosα₁＋a₂sinα₂・sinα₁ F_z＝a₁＋a₂cosα₂ 同様に、点Ｑの各軸成分Q_x、Q_y、Q_zおよび点
Ｂの各軸成分B_x、B_y、B_zはそれぞれ次のように
なる。

Q_x＝F_x＋a₃sin（α₂＋α₃）・cosα₁ Q_y＝F_y＋a₃sin（α₂＋α₃）・sinα₁ Q_z＝F_z＋a₃cos（α₂＋α₃） B_x＝Q_x＋a₄sinα₄・cosα₁ B_y＝Q_y＋a₄sinα₄・sinα₁ B_z＝Q_z＋a₄cosα₄ ただし、α₄＝α₂＋α₃＋α_4Eで表わされる。そし
て、点Ｐの各軸成分と溶接トーチ２７の配向角φ
および姿勢角θを求めるために、点Ｑでのロボツ
ト系からトーチ系への座標変更を行なう。なお、
以下の各式において、「C_i」は「cosα_i」を意味
し、「s_i」は「sinα_i」を意味するものとする。上
述の座標変換のために、次式(1)が与えられる。

そして、溶接点Ｐの位置を求めるために、（α
β ｒ １）＝（０ ０ −a₅ ０）を与える。

そうすると、点Ｐの各軸成分P_x、P_y、P_zは、
それぞれ、次のようになる。

P_x＝−a₅（c₁c₄c₅c₆ ^-s₁s₅s₆ ⁺c₁s₄c₆）＋B_x P_y＝−a₅（s₁c₄c₅s₆ ⁺c₁s₅s₆ ⁺s₁s₄c₆）＋B_y P_z＝−a₅（−s₄c₅s₆＋c₄＋c₆）＋B_z これをまとめると、 P_x＝a₂s₂c₁ ^-e₁s₁＋a₃s₂₊₃c₁＋a₄s₄c₁ −（c₁c₄c₅s₆ ^-s₁s₅s₆＋c₁s₄c₆）a₅ ＝c₁（a₂s₂＋a₃s₂₊₃＋a₄s₄−c₄c₅e₄−s₄a₇） ＋s₁（e₄s₅ ^-e₁） ＝c₁｛a₂s₂＋a₃s₂₊₃＋（a₄−a₇）s₄−e₄c₄c₅｝ ＋s₁（e₄s₅ ^-e₁） P_y＝e₁c₁＋a₂s₂s₁＋a₃s₂₊₃s₁＋a₄s₄s₁−s₁c₄c₅e₄ −e₄c₁s₅ ^-a₇s₁s₄ ＝c₁（e₁−e₄s₅）＋s₁｛a₂s₂＋a₃s₂₊₃ ＋（a₄−a₇）s₁s₄−e₄c₄c₅｝ P_z＝a₁＋a₂c₂＋a₃＋c₂₊₃＋a₄c₄＋e₄s₄c₅−a₇c₄ ＝a₁＋a₂c₂＋a₃c₂₊₃＋（a₄−a₇）c₄＋e₄s₇c₅ そして、溶接トーチ２７の角度φおよびθを求
めるために、（α β ｒ １）＝（０ ０ １
０）を、上記(1)式に代入して、各軸への方向余弦
（tx、ty、tz）を求める。

t_x＝c₁c₄c₅c₆−s₁s₅s₆＋c₁s₁c₆ t_y＝s₁c₄c₅c₆−c₁s₅s₆＋s₁s₁c₆ t_z＝c₄c₆−s₄c₅s₆ txy＝（tx²＋ty²）１／２ 溶接トーチ２７の角度φおよびθは、それぞ
れ、 φ＝tan^-1（ty／tx） θ＝tan^-1（txy／tz） となる。ここで、点ＦおよびＱについてもまとめ
ると、 F_x＝a₂s₂c₁−e₁s₁ F_y＝a₂s₂s₁＋e₁c₁ F_z＝a₂c₂＋a₁ Q_x＝a₃s₂₊₃c₁＋a₂s₂c₁−e₁s₁ Q_y＝a₃s₂₊₃s₁＋a₂s₂s₁＋e₁c₁ Q_z＝a₃s₂₊₃＋a₂s₂＋a₁ となる。このようにして、各点の直角座標系（ロ
ボツト系）での位置情報が求められる。次に、ス
テツプS₅のような直角座標系（ロボツト系）から
多関節座標系への座標変換を第８図ないし第１２
図を参照して、考えてみる。テイーチング時や補
間制御などの目標位置は、点Ｐのそれとして与え
られる。したがつて、この直角座標系に基づい
て、各回動軸への駆動量を計算する必要がある。

（P_x、P_y、P_z、φ、θ）から、その溶接トー
チ２７の延長点Ｂを座標を求めると、Ｂ点の各軸
成分B_x、B_y、B_zはそれぞれ次のようになる。

B_x＝P_x＋a₅sinθcosφ B_y＝P_y＋a₅sinθsinφ B_z＝P_z＋a₅cosθ このB_x、B_y、B_zに基づいて、第８図および第
１２図の各角度成分W1，W2とW3が求められる。

W1＝tan^-1By／Bx W2＝（By²＋Bx²−e² ₁）^1/2 W3＝tan^-1e₁／W2 したがつて、角度α₁は、 α₁＝W1−W2 で与えられる。

次に、与えられるトーチ角度φ、θとα₁から、
点Ｑを求める。第９図参照。この場合には、（（α
β ｒ）系から（x′y′z′）系への変換を行な
う。

そして、α²＋β²＝e² ₃、ｒ＝−a₈の円とy′＝０の
平面の交点を求める。なお、以下の式において
「θ_c」は「cosθ」を示し、「θ_s」は「sinθ」を示す
。
同様に「φ_c」は「cosφ」を示し、「φ_s」は
「sinφ」を示す。

したがつて、α，βおよびｒは、それぞれ次式
で与えられる。

α＝θ_cφ_cx′−θ_cφ_sy′−θ_sz′ β＝φ_sx′＋φ_cy′ ｒ＝θ_sφ_cx′−θ_sφ_sy′＋θ_cz′ そして、y′＝０から、α，β，ｒは、さらに次
のようになる。

α＝θ_cφ_cx′−θ_sz′ β＝φ_sx′ ｒ＝θ_sφ_cx′＋θ_cz′ また、ｒ＝−a₈から、次式(2)が与えられ、α²＋
β²＝e₃ ²から、次式(3)が与えられる。

θ_sφ_cx′＋θ_cz′＋a₈＝０ ……(2) θ² _Cφ² _cx′²−2θ_cφ_cθ_sx′z′ ＋θ² _sz′²＋φ² _s′x²＝e² ₃ ……(3) 上記(3)式に、z′＝−（θ_sφ_cx′＋a₈）／θ_cを代入
する と、次式が得られる。

（φ² _c＋θ² _cφ² _s）x′²＋2a₈θ_sφ_cX′ ＋（a² ₈θ² _s−e² ₃θ² _c）＝０ 上記式における判別式WDを求めると、判別式
WDは次のようになる。

WD＝a² ₈θ² _sφ² _c−（φ² _c＋θ² _cφ² _s）（a² ₈θ² _s−e² ₃
θ² _c） ＝a² ₈θ² _sφ² _c−a² ₈θ² _sφ² _c＋e² ₃θ² _cφ² _c −a² ₈θ² _sθ² _cφ² _S＋e² ₃θ⁴ _cφ² _s ＝e² ₃θ² _c（φ² _c＋θ² _cφ² _s）−a² ₈θ² _sθ² _cφ² _s ＝θ² _c｛e² ₃（φ² _c＋φ² _sθ² _c）−a² ₈θ² _sφ² _s｝ したがつて、この判別式WDの値が負になれ
ば、点Ｑは存在し得ないことになる。また、 WA＝φ² _c＋θ² _cφ² _s WB＝a₈θ_sφ_c WC＝a² ₈θ² _s−e² ₃θ² _c とすれば、点Ｑのx′座標は x′＝−WB±（WB²−WA×WC）^1/2／WA で与えられる。これに対応するz′の値は z′＝−（θ_sφ_cx′＋a₈）／θ_c で決定される。

一方、θ≦45゜の領域では、x′を求め、０＞45゜
の領域ではz′を求めるとすれば、上記(2)式から
x′＝−（θ_cz′＋a₈）／θ_sφ_cを得て、これを上記(3)
式
に代入する。そうすると、次式が得られる。

（φ² _c＋θ² _cφ² _s）z′²＋2a₈θ_cz′ ＋φ² _c（a² ₈θ² _c−e² ₃θ² _s）＋a² ₈φ² _s＝０ そして、x′を求めたのと同様にして、 W_a＝φ² _c＋θ² _cφ² _s W_b＝a₈θ_c W_c＝φ² _c（a² ₈θ² _c−e² ₃θ² _s）＋a² ₈φ² _s とおけば、判別式WDは次式で与えられる。

WD＝W_b ²−W_a×W_c したがつて、点Ｑのｘ軸成分Q_x、Ｚ軸成分Q_z
は、それぞれ次式で与えられる。

Q_z＝z′＝−Wb±√WD／Wa Q_x＝x′＝−（θ_cz′＋a₈）／θ_sφ_c で示される。いずれの場合も、点Ｑの座標は、
Q₁（x′₁、z′₂）、Q₂（x′₂，z′₂）の２点が存在する
。
この実施例においては、そのいずれか１点に限定
するものと理解されたい。

原点を点Ｂとして、ロボツト系の座標（x′，
y′，z′）と点Ｑの座標（Q_x′，Q_y′，Q_z）から、角
度α₄とα₅を決定する。まず、Q_y′＝０であるの
で、α₄は次式で与えられる。

α₄＝tan^-1−Qx／−Qz そして、角度α₅を求めるために、先のロボツト
座標系（x′，y′，z′）からさらに各（x″，y″，z″
）
を求める。そして、この（x″，y″，z″）系で（α
β ｒ）＝（０ ０ −１）とする。

したがつて、x″およびy″はそれぞれ次式で与
えられる。

x″＝c₄φ_cθ_s＋s₄θ_c y″＝φ_sθ_s そして、角度α₅の基準線を、第１０図のよう
に、垂直線からの角度とすれば、この角度α₅は次
式で与えられる。

α＝tan^-1（−φ_sθ_s／c₄φ_cθ₅−s₄θ_c） 次に、角度α₂およびα₃を求めるために、まず、
絶対座標系で点Ｑの位置を求めると、このＱの各
軸成分Qx、Qy、Qzは、それぞれ次式で与えられ
る。

Qx＝Bx＋x′cosα₁ Qy＝By＋x′sinα₁ Qz＝Bz＋z′ 一方、点Ｇ（第１２図参照）は、（−e₁sin₁、
e₁cosα₁、a₁）であるので、長さ成分l₁、l₂および
角度成分W1は、それぞれ、次式で表わされる。

l₁＝｛（Qx＋e₁s₁）²＋（Qy−e₁c₁）²｝１／２ l₂＝｛l² ₁₂＋（Qz−a₁）²｝１／２ W1＝tan^-1（Qz−a₁／l₁） そして、WA＝（a² ₂−l² ₂−a² ₃）／2a₂l₂とすれば、
角度成分W2は次式で与えられる。

W2＝cos^-1（WA）＝tan^-1｛（１−Wa²）／WA^1/2｝ また、WB＝（a² ₂＋a² ₃−l² ₂）／2a₂a₃とすれば、
角度成分W3は次式で与えられる。

W3＝tan^-1｛（１−WB²）^1/2／WB｝ したがつて、角度α₂およびα₃は、それぞれ次式
で与えられる。

α₂＝ｘ／２−W1±W2 α₃＝±（ｘ−W3） このようにして、直角座標から多関節座標への
変換が行なわれる。

次にステツプS₆において、α系の第ｉ番目の軸
について、P_MP_M+1をその第ｉ番目の軸の出しう
る最大速度Vmax(i)で動いたときの、所要時間を
ｔ(i)とし、 ｔ(i)＝〔α_M＋１(i)−α_M(i)〕／Vmax(i)を演算す
る。

ステツプS₇において、α系でのきざみ時間を△
ｔとし、第ｉ番目の軸でのきざみ数ｎ(i)を、 ｎ(i)＝ｔ(i)／△ｔ と演算する。この実施例においては、△ｔと
10msecとする。

ステツプS₈において、α系でのきざみ数ｎを、 ｎ＝max〔ｎ(i)、no〕 できめる。但し、ｎ＝△Ｔ／△ｔで、この実施例
の場合no＝20となる。

ステツプS₉において、α系でのきざみ点の位置
をp_nとし、 pm〔（ｎ−ｍ）×p_M＋（ｍ×p_M+1）〕／ｎを演算。

ステツプS₁₀において、かくして求めたp_nの各
軸の位置情報を指令値として出力する。

ステツプS₁₁において、メモリ３３における
「ｍ」の値に「１」を加える。

ステツプS₁₂において、ｍ＞ｎを判断する。そ
うでなければステツプS₉に戻る。そうであればス
テツプS₁₃を実行する。

ステツプS₁₃では△Ｔ時間毎に、メモリ３３に
おける「Ｍ」の値に「１」を加える。

ステツプS₁₄で、Ｍ＞Ｎを判断する。もうそう
でなければステツプS₃に戻る。もしそうであれば
次のステツプに進む。

ステツプS₁₅ではメモリ３３における「Ｋ」の
値に「１」を加える。

ステツプS₁₆では「Ｋ」の値が最終点の値か否
か判断する。そうでなければステツプS₁に戻る。
もしそうであればこの一連のステツプＳは終了す
る。

なお前述ステツプS₁₀において、p_nの各制御軸
毎の位置情報を、指令位置として出力したことに
伴ない、各制御軸に対応する駆動回路３５１ない
し３５５におけるサーボモータ４９が駆動され、
各制御軸毎に移動し、指令点に達したなれば、各
駆動回路３５１ないし３５５における検知回路５
１からのNull信号により、CPU３１は、次の
p_n+1の位置情報を指令するものである。但し、第
２図に示すように、点Ｐ間の軌跡は、正確な直線
とはならず、ロボツトのＸ系特有の曲線をえが
く。しかしながら、点Ｐ間の時間が短いから、点
P_Kと点P_K+1間は実質的には直線補間がなされる
ものと理解されたい。

この発明は前述実施例の他、下記する種々の変
形も可能である。

() ロボツトの機構上のＸ制御系は、この実施
例のような多関節系にかぎることはなく、例え
ば直角座標系であつてもよく、また例えば極座
標系であつても、円筒座標系であつても同様実
施しうる。Ｘ制御系が直角座標系の場合は、制
御のために他の座標系に座標変換する手段は通
常不要となろう。

() 教示点間の補間は、直線補間のみならず、
例えば３点を教示しての円補間を行なう場合
も、さらに教示点間のピツチを小として、補間
を行なわせない場合も、いずれもこの発明を実
施して有効である。

() その他この発明の技術的思想の範囲内にお
ける、各構成の均等物との置換えも、またこの
発明の技術的範囲に含まれるものである。

この発明は前述したとおりであるから、ロボツ
トのエンドエフエクタを高速移動させるときなど
に、予期せぬ軌跡をえがくことを、自動的に防止
しうるものである。

【図面の簡単な説明】

図面はいずれもこの発明一実施例を示し、第１
図は、この発明に実施されうる多関節形産業用ロ
ボツトの斜視図、第２図は制御装置のブロツク
図、第３図はフローダイアグラム、第４図ないし
第１２図はいずれも作用説明模式図である。 Ｒ……産業用ロボツト、Ｃ……制御装置、S₁…
…(イ)および(ト)手段、S₂……(チ)手段、S₃……(ロ)お
よ
び(リ)手段、S₄……(ヌ)手段、S₅……（ル）手段、S₆
……(ハ)および（ヲ）手段、S₇……(ニ)および（ワ）
手段、S₈……(ホ)、(カ)および（ヨ）手段、Ｓ……(ヘ)
および（タ）手段。

Claims (1)

1. 【特許請求の範囲】 １ ある座標系の産業用ロボツトの制御装置にお
   いて、下記する各手段をそなえてなる、制御装
   置。 (イ) 通過点Ｐと次の通過点Ｐ間の移動距離Ｄを演
   算する手段。 (ロ) 指令速度Ｖ、およびあらかじめ定めたきざみ
   時間ΔTから、通常のきざみ数N₀＝Ｄ／Ｖ×
   ΔT、を演算する手段。 (ハ) 前記点P_Kと点P_K+1との間を、前記座標系の
   第ｉ番目の制御軸の出しうる最大速度Vmax(i)
   で動いたときの所要時間Ｔ(i)＝〔P_K+1(i)−P_K
   (i)〕／Vmax(i)、を演算する手段。 (ニ) 前記第ｉ番目の制御軸でのきざみ数Ｎ(i)＝Ｔ
   (i)／ΔT、を演算する手段。 (ホ) 決定きざみ数Ｎ＝max〔Ｎ(i)、N₀〕、を演算
   する手段。 (ヘ) 指令きざみ位置PM＝〔（Ｎ−Ｍ）×P_K＋Ｍ×
   P_K+1〕／Ｎ、を演算する手段。 ２ ある座標系の産業用ロボツトの制御装置であ
   り、かつ直角座標系との座標交換手段を含んでい
   るものにおいて、下記する各手段をそなえてな
   る、制御装置。 (ト) 直角座標系で表現された教示点P_Kおよび
   P_K+1間の距離Ｄを演算する手段。 (チ) 移動時間Ｔ＝距離Ｄ／指令速度Ｖ、を演算す
   る手段。 (リ) 補間きざみ数Ｎ＝時間Ｔ／直角座標系でのき
   ざみ時間ΔT、を演算する手段。 (ヌ) 補間きざみ点の直角座標系での位置PM＝
   〔（Ｎ−Ｍ）×P_K＋Ｍ×P_K+1〕／Ｎ、を演算する
   手段。 (ル) 前記各位置PMを前記ある座標系の位置PM
   に変換する手段。 (ヲ) 前記ある座標系の第ｉ番目の制御軸につ
   き、PM、PM＋１点間を、前記第ｉ番目の制
   御軸の出しうる最大速度Vmax(i)で動いたとき
   の所要時間ｔ(i)＝〔α_M+1(i)−α_M(i)〕／Vmax(i)、
   を演算する手段。 (ワ) 前記ある座標系でのきざみ時間をΔtとし、
   前記第ｉ番目の制御軸でのきざみ数ｎ(i)＝ｔ
   (i)／Δt、を演算する手段。 (o) n₀＝ΔT／Δt、を演算する手段。 (ヨ) 前記ある座標系でのきざみ数ｎ＝max（ｎ
   (i)、n₀〕、を演算する手段。 (タ) 前記ある座標系での指令きざみ点の位置
   pm＝〔（ｎ−ｍ）×pM＋ｍ×p_M+1〕／ｎ、を演
   算する手段。