JPH07271420A - ロボットの制御装置 - Google Patents

Abstract

(57)【要約】 【目的】オフセット手首５軸多関節ロボットの正確、高
速な補間演算を行う。 【構成】教示点の間における第４、５軸角度差、動作経
路を分割して補間点の第４、５軸の角度、位置を、それ
ぞれ、演算する第１補間手段と、第２補間手段と、補間
点のジョイント座標を逐次演算により求め、既に、ワー
ルド座標と対応するジョイント座標とが正確に分かって
いる点を現特定点とし、現特定点に対する補間点の位置
偏差と、１次関数とから、現特定点に対する補間点の角
度偏差を演算し、角度偏差と現特定点のジョイント座標
とから次の新特定点のジョイント座標を演算し、その座
標に対応する正確な位置を演算した新特定点を現特定点
として、上記の演算を繰り返し実行して補間点に漸近す
る新特定点を新補間点として決定して教示点間の新補間
点を順次決定する演算手段とを有し、第４、５軸の角度
偏差は、補間点と現特定点の角度偏差に固定した。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、作業空間のワールド座
標（直交座標）系からロボットのジョイント座標系への
逆変換の解析解が存在しないオフセット手首を有する５
軸多関節ロボットの制御装置に関する。

【０００２】

【従来技術】従来、４軸（通常アームの中心軸）とアー
ムの先端に取り付けられたハンドの中心軸とが一致して
おらず、その２つの軸がオフセットした関係にあるロボ
ットがある。このようなロボットは４軸の回転によっ
て、ハンドの中心軸の位置が変化するため、ハンドのワ
ールド座標から４軸の先端位置の特定が困難となり、制
御しずらい。さらに、オフセット手首を有する５軸多関
節ロボットは、ハンド先端の取り得る姿勢はハンド先端
のワールド座標に依存するが、いつも任意の姿勢がとれ
る訳けではない。よって、回転主軸法により位置及び姿
勢の補間を行う場合には、その補間点で必ずしも姿勢が
とれるとは限らないので、回転主軸法により得られた補
間点に対応するジョイント座標を解析的に求めることは
できない。

【０００３】しかし、この様なオフセット手首を有する
５軸多関節ロボットであっても、レーザ加工機の様に
１、２、３軸が直動軸で構成された５軸多関節ロボット
においては、特開昭62-154006 号や特開昭50-124357 号
に示される方法によって解を求めることができる。これ
は、1 、2 、3 軸が直動軸である場合には、４、５軸に
よって決定されたハンドの姿勢に対して１、２、３軸が
影響しないためである。しかし、１、２、３軸のすくな
くとも１つが回転軸からなるものは、４、５軸によって
姿勢を決定しても、１、２、３軸の変化によって姿勢が
変化してしまい、上記公知例の方法は用いることができ
ない。このため１、２、３軸に回転軸を有し、オフセッ
ト手首を有する５軸多関節ロボットの補間に関して、本
願発明者は、次のように行うことを試みた。先ず、２つ
の教示点における位置及び姿勢（４×４の同次座標行
列）から回転主軸を演算して、この回転主軸の回りの回
転角を等分割して補間点における位置及び姿勢を演算す
る。この位置及び姿勢において、姿勢を用いて近似値的
に１軸から５軸のジョイント座標を決定する。しかし、
このようにして得られたジョイント座標は近似を用いて
いるため、補間点において姿勢が実現できない場合に
は、上記の回転主軸法による補間点の位置及び姿勢に対
応するものではない。そこで、得られたジョイント座標
をワールド座標での位置及び姿勢に変換し、その位置及
び姿勢を求める。この後、姿勢はこの値を用いて位置の
み回転軸によって求めた位置に置き換えて、再度、上記
のようにジョイント座標を求めるという操作を繰り返し
実行する。これによって位置は回転手軸によるものが得
られ、姿勢は順次近似される。そして、この繰り返し演
算により、回転主軸法による補間点での位置及び姿勢に
近似した取り得るワールド座標での位置及び姿勢をもと
め、その点を最終的に補間点の位置及び姿勢とするもの
である。

【０００４】

【発明が解決しようとする課題】ところが、上記の繰り
返し演算による方法は、とり得ない位置及び姿勢から無
理にジョイント座標を演算していることから、回転主軸
法による補間点への収束性が悪く、多数回の繰り返し演
算を必要とし時間がかかるだけでなく、又、回転主軸法
による補間点とはかけ離れた姿勢となることがある。

【０００５】本発明は上記の課題を解決するために成さ
れたものであり、その目的は、オフセット手首を有し、
１、２、３軸に回転軸を有する５軸多関節ロボットにお
ける正確で且つ高速な補間演算を可能とすることであ
る。

【０００６】

【課題を解決するための手段】上記課題を解決するため
の発明の構成は、オフセット手首を有する５軸多関節ロ
ボットにおける補間点を演算する装置において、２つの
教示点の間における第４軸角度差、第５軸角度差を分割
して補間点の第４軸及び第５軸のジョイント座標を演算
する第１補間手段と、２つの教示点の間の動作経路を分
割して補間点のワールド座標を演算する第２補間手段
と、ジョイント座標をワールド座標へ変換するための関
数から得られる１次近似式であって、任意点におけるワ
ールド座標での微小変位とジョイント座標での微小変位
を関係付ける１次近似式を記憶した近似式記憶手段と、
補間点のジョイント座標を逐次演算により求める手段で
あって、既に、ワールド座標とそれに対応するジョイン
ト座標とが正確に分かっている点を現特定点として、そ
の現特定点に対する補間点のワールド座標の位置偏差
と、現特定点における１次関数とから、現特定点に対す
る補間点のジョイント座標の角度偏差を演算し、その角
度偏差と現特定点のジョイント座標とから次の新特定点
のジョイント座標を演算し、そのジョイント座標に対応
するワールド座標を演算し、その新特定点を現特定点と
して、上記の演算を繰り返し実行することで、補間点に
漸近する新特定点を新補間点として決定することによ
り、教示点間の新補間点を順次決定する演算手段とを有
し、演算手段において、第４軸及び第５軸の角度偏差
は、第１補間手段により演算された補間点の第４軸及び
第５軸のジョイント座標の現特定点のジョイント座標に
対する偏差として演算されることを特徴としている。

【０００７】

【作用及び発明の効果】本発明では、補間点は次のよう
に演算される。図１に示すように、教示点Ｗ１、Ｗ２、
Ｗ３、…が与えられている。この教示点は実際にロボッ
トの位置及び姿勢を変化させて教示させた点であるの
で、ハンド先端のワールド座標での位置及び姿勢を表し
た同次座標行列に対応する第１軸〜第５軸のジョイント
座標が分かっている。この教示点Ｗ１、Ｗ２、Ｗ３、…
における位置ベクトルをＶ１，Ｖ２，Ｖ３，…、ジョイ
ント座標行列をＵ１，Ｕ２，Ｕ３，…、特に、第４軸の
角度をα１，α２，α３，…、第５軸の角度をβ１，β
２，β３，…とする。

【０００８】本発明は、これらの教示点間を直線又は曲
線で補間した補間点Ｗ１_0,Ｗ１_1,Ｗ１_2,…におけるジョ
イント座標を演算するものである。尚、Ｗ１₀ は教示点
Ｗ１である。補間点Ｗ１_0,Ｗ１_1,Ｗ１_2,…における位置
ベクトル（Ｘ，Ｙ，Ｚ）をＰ（０），Ｐ（１），Ｐ
（２），…、ジョイント座標行列（θ_1,θ_2,θ_3,θ_4,θ
₅）をΘ（０），Θ（１），Θ（２），…とする。又、
現特定点Ｒの位置ベクトルをＰ_R 、ジョイント座標行列
をΘ_R 、新特定点Ｎの位置ベクトルをＰ_N 、ジョイント
座標行列をΘ_N とする。現特定点Ｒと新特定点Ｎとは、
逐次演算に使用される点であり、現特定点Ｒの位置ベク
トルＰ_R とジョイント座標行列Θ_R とから新特定点Ｎの
位置ベクトルＰ_N とジョイント座標行列Θ_N とが演算さ
れる。この新特定点Ｎは次の逐次演算における現特定点
Ｒとなる。そして、最終的に求められた新特定点Ｎが補
間点となる。

【０００９】先ず、補間点Ｗ１₁ のジョイント座標を求
める手順を説明する。一般に、第ｋ補間点Ｗ１_k の位置
ベクトルＰ（ｋ）は教示点Ｗ１、Ｗ２の位置ベクトルＶ
１、Ｖ２とから次式で求められる。

【００１０】

【数１】Ｐ（ｋ）＝｛ｋ・Ｖ２＋（ｍ−ｋ）Ｖ１｝／ｍ 但し、ｍは補間区間数である。ｍ−１が２つの教示点間
の補間点数となる。

【００１１】よって、第１補間点Ｗ１₁ の位置ベクトル
Ｐ（１）は次式となる。

【数２】Ｐ（１）＝｛Ｖ２＋（ｍ−１）Ｖ１｝／ｍ このようにして、補間点の位置ベクトルは決定される。

【００１２】次に、補間点のジョイント座標を求める
が、第４軸、第５軸の角度は次のようにして求められ
る。一般に、第ｋ補間点Ｗ１_k の第４軸角度θ₄(k)、第
５軸角度θ₅(k)は、教示点Ｗ１、Ｗ２の第４軸角度α
１、α２、第５軸角度β１、β２とから次式で求められ
る。

【数３】θ₄(k)＝｛ｋ・α２＋（ｍ−ｋ）α１｝／ｍ

【数４】θ₅(k)＝｛ｋ・β２＋（ｍ−ｋ）β１｝／ｍ よって、第１補間点Ｗ１₁ の第４軸角度θ₄(1)、第５軸
角度θ₅(1)は次式で与えられる。

【数５】θ₄(1)＝｛α２＋（ｍ−１）α１｝／ｍ

【数６】θ₅(1)＝｛β２＋（ｍ−１）β１｝／ｍ

【００１３】又、補間点Ｗ１₁ の現特定点Ｒに対する位
置偏差（ベクトル）をΔＰ＝（ΔＸ，ΔＹ，ΔＺ）、角
度偏差（行列）をΔΘ＝（Δθ_1,Δθ_2,Δθ_3,Δθ_4,Δ
θ₅）とする。

【数７】ΔＰ＝Ｐ（１）−Ｐ_R

【数８】ΔΘ＝Θ（１）−Θ_R

【００１４】又、ジョイント座標行列（θ_1,θ_2,θ_3,θ
_4,θ₅ ）からワールド座標の位置ベクトル（Ｘ，Ｙ，
Ｚ）への変換式はロボットの幾何学的構造により容易に
決定される。

【数９】Ｘ＝Ｘ（θ_1,θ_2,θ_3,θ_4,θ₅ ）

【数１０】Ｙ＝Ｙ（θ_1,θ_2,θ_3,θ_4,θ₅ ）

【数１１】Ｚ＝Ｚ（θ_1,θ_2,θ_3,θ_4,θ₅ ）

【００１５】よって、ΔＰとΔΘに関して次式が成立す
る。

【数１２】ΔＰ＝Ａ・ΔΘ 但し、ＡはΔΘからΔＰへ変換するための変換行列であ
り、現特定点Ｒにおける１次偏微分行列である。

【００１６】即ち、

【数１３】

【００１７】第１回演算は、次のように行われる。現特
定点Ｒは教示点Ｗ１（＝Ｗ１₀ ）に設定される。教示点
Ｗ１の位置ベクトルＰ（０）はＶ１で、ジョイント座標
行列Θ（０）はＵ１で既知である。即ち、

【数１４】Ｐ_R ＝Ｖ１

【数１５】Θ_R ＝Ｕ１

【００１８】よって、この教示点Ｗ１での数１３の変換
行列Ａを決定することができる。又、補間点Ｗ１₁ の位
置ベクトルＰ（１）と第４軸角度θ₄(1)と第５軸角度θ
₅(1)は、数１、数５、数６により決定されている。よっ
て、位置偏差ベクトルΔＰは数７で決定され、第４軸角
度偏差Δθ₄ 、第５軸角度偏差Δθ₅ は、数８により決
定される。これらの既知の値を用いて、数１２から角度
偏差行列ΔΘを演算することができる。但し、第４軸角
度偏差、第５軸角度偏差は、数５、数６の補間演算で決
定されている値である。

【００１９】次に現特定点Ｒでのジョイント座標行列Θ
_R と決定された角度偏差行列ΔΘとから数８と同様な関
係式を用いて、新特定点Ｎのジョイント座標行列Θ_N が
決定される。次に、数９、数１０、数１１を用いて、新
特定点Ｎのジョイント座標行列Θ_N を順変換して、新特
定点Ｎの位置ベクトルＰ_N が演算される。この位置ベク
トルＰ_N は、補間点Ｗ１₁ の位置ベクトルＰ（１）に近
いが異なった値である。

【００２０】第２回演算は、次のように行われる。即
ち、新特定点Ｎを現特定点Ｒとして、上記の第１回演算
と同様な演算を実行して、次の新特定点Ｎを演算する。
この第２回演算では、第４軸角度、第５軸角度は、数
５、数６で表される値となっているので、第４軸角度偏
差Δθ₄ 、第５軸角度偏差Δθ₅ は、共に０とする。

【００２１】第３回以上の繰り返し演算が、第２回演算
と同様に実行される。そして、位置偏差ベクトルΔＰと
角度偏差行列ΔΘとが十分に小さくなった時に、逐次演
算が終了する。そして、その終了時点での新特定点Ｎの
位置ベクトルＰ_N とジョイント座標行列Θ_N とが求める
新補間点となる。

【００２２】次に、第２補間点Ｗ１₂ に対して同様な処
理が行われる。但し、現特定点Ｒの初期値は、上述の演
算で得られた新補間点の値が採用される。以下、教示点
Ｗ２まで、全ての補間点に対する処理が実行され、ｋ−
１個の新補間点のジョイント座標行列が決定される。こ
のようにして、教示点Ｗ１と教示点Ｗ２間の補間処理が
実行される。

【００２３】本発明装置による補間では、第４軸及び第
５軸に関しては、角度差を等分して得られる補間角度に
固定し、ジョイント座標行列からワールド座標の位置ベ
クトルへの変換式の一次近似式に基づいて、位置ベクト
ルの偏差から角度偏差を演算して、全軸の角度を求め、
順変換により対応するワールド座標の位置ベクトルを演
算するという演算を繰り返し実行して、補間点を求める
ようにした。このため、第１、２、３軸に回転軸を有す
るオフセット手首を有する５軸多関節ロボットであって
も、補間点を高速、且つ、的確に求めることができる。
即ち、第４軸及び第５軸の角度が既に決定されているの
で、第１回の演算から補間点に近い点が得られ、演算の
繰り返し回数も少なくてすむ。さらに、変換行列Ａは第
４軸角度及び第５軸角度が決定されているので、繰り返
し演算において、第４軸角度及び第５軸角度によって決
定される値を演算し直す必要がないため、補間点の演算
が高速で行える。また、ジョイント座標行列からワール
ド座標の位置ベクトルへの変換式の一次近似式を使用し
ているため、他のロボットへ応用する場合であっても、
ロボットの幾何学的構造によりこの式を求めることがで
きるため、簡単に流用でき、使用範囲が広い。

【００２４】

【実施例】以下、本発明を具体的な実施例に基づいて説
明する。図３は第２、３軸に回転軸を有し、オフセット
手首を有する５軸多関節レーザ加工ロボットの構成を示
した図であり、図４はロボットのアーム等の幾何学的な
結合関係を図示した機構図である。レーザ加工ロボット
５０は、自動車のボディＢ（被加工物）がその前後方向
に順次移動されてレーザ加工されるラインにおいて、そ
のボディＢの左右に１台ずつ設けられている。

【００２５】図３に示すように、床面Ｆに長方形状のベ
ッド５１が据え付けられている。ベッド５１の両側部分
の上部には、自動車のボディＢに対して進退する方向
（自動車の前後方向と直角方向）に、一対のラック５２
が設けられている。スライダ５３はテーブル５４とロー
テータ５５とからなっている。テーブル５４には、各ラ
ック５２に噛み合うピニオン５６が、各ラック５２に対
して２つずつ設けられている。テーブル５４には第１軸
モータＭ１が設けられており、第１軸モータＭ１の駆動
力に基づいて各ピニオン５６が回転し、テーブル５４が
ラック５２の方向である第１軸Ａ₁ （Ｘ軸、θ₁ 方向）
方向に移動するようにされている。従って、第１軸は直
動軸となっている。

【００２６】ローテータ５５は、ローテータ５５に設け
られた第２軸モータＭ２により、周知の減速機及びギヤ
機構（いずれも図示省略）を介して、鉛直方向の第２軸
Ａ₂（θ₂ 方向）回りに回転可能とされている。ローテ
ータ５５にはアーム取付部５７が形成されており、アー
ム取付部５７に対してアーム支持部５８が取り付けられ
ている。アーム支持部５８は第３軸モータＭ３により、
周知のベルト機構、減速機及びギア機構（いずれも図示
省略）を介して、第３軸Ａ₃ （θ₃ 方向）（図１におい
て紙面に直角方向）回りに揺動可能とされている。従っ
て、第２、３軸は回転軸となっている。

【００２７】アーム支持部５８は円筒部５９を有し、円
筒部５９には円筒状のアーム６０が挿入されている。ア
ーム６０はアーム支持部５８に設けられた第４軸モータ
Ｍ４（図５）（図３図示省略）により、周知のギヤ機構
（図示省略）を介して、アーム６０の中心軸線である第
４軸Ａ₄ （θ₄ 方向）回りに回転可能とされている。ア
ーム６０の先端部には、手首部材６１が、次のように第
５軸Ａ₅ （θ₅ 方向）回りに回転可能に設けられてい
る。アーム６０の内部は、円筒状の手首回転部材６２が
アーム６０と同心的に挿入されており、手首回転部材６
２の先端部に形成されたかさ歯歯車６３と、手首部材６
１の上端部に形成されたかさ歯歯車６４とが噛み合って
いる。そして、アーム支持部５８に設けられた第５軸モ
ータＭ５（図５、図３図示省略）により、周知の減速機
（図示省略）を介して手首回転部材６２が自身の軸線回
りに回転し、手首部材６１が第５軸Ａ₅ 回りに回転す
る。手首部材６１の先端部にはレーザ光ノズル部６５が
形成されている。そして、このレーザ光ノズル部６５の
軸線方向は、図３に示されるように、第４軸Ａ₄ に対し
て、平行でオフセットした位置となっている。

【００２８】次に、このレーザ加工ロボット５０のレー
ザ光の伝送路について説明する。図３に示すように、第
１軸Ａ₁ （スライダ５３の移動方向）に沿ったスライダ
５３の後方には、レーザ光発振装置６６が床面Ｆに据え
付けられており、伸縮自在の第１導光路６７を介してス
ライダ５３と連結されている。テーブル５４の内部には
第１ミラー６８が設けられており、レーザ光発振装置６
６から投入されたレーザ光Ｌが、ローテータ５５の側部
に設けられた第２導光路６９に向けて、鉛直上方向に伝
送される。

【００２９】第２導光路６９の内部には紙面に垂直に反
射させる第２ミラー（図示略）が設けられており、その
第２ミラーによりそのレーザ光Ｌがアーム６０の基端部
に向けて水平方向（紙面に垂直方向）に伝送される。ア
ーム６０の基端部の内部には第３ミラー７０が設けられ
ており、その第３ミラー７０により水平方向に反射され
て、そのレーザ光Ｌがアーム６０内（手首回転部材６２
内）を、アーム６０の軸方向に沿ってアーム６０の先端
部に向けて伝送される。アーム６０の先端部には第４ミ
ラー７１が設けられており、そのレーザ光Ｌが手首部材
６１に向けて伝送される。

【００３０】手首部材６１の内部には放物面鏡を有する
集光部７２が設けられており、集光部７２によりそのレ
ーザ光Ｌが集光され、レーザ光ノズル部６５からボデー
Ｂの被加工物に対して投射されて、被加工部のレーザ加
工が成される。

【００３１】又、図５は、本ロボットの姿勢制御装置の
電気的構成を示したブロックダイヤグラムである。２０
はマイクロコンピュータ等から成る中央処理装置であ
る。この中央処理装置２０には、メモリ２５、サーボモ
ータを駆動するためのサーボＣＰＵ２２ａ〜２２ｅ、ジ
ョグ運転の指令、教示点の指示等を行う操作盤２６が接
続されている。ロボットに取付けられた各軸Ａ₁ 〜Ａ₅
駆動用のサーボモータＭ１〜Ｍ５は、それぞれサーボＣ
ＰＵ２２ａ〜２２ｅによって駆動される。

【００３２】サーボＣＰＵ２２ａ〜２２ｅのそれぞれ
は、中央処理装置２０から出力される出力角度データθ
₁ 〜θ₅ と、サーボモータＭ１〜Ｍ５に連結されたエン
コーダＥ１〜Ｅ５の出力α₁ 〜α₅ との間の偏差を演算
し、この演算された偏差の大きさに応じた速度で各サー
ボモータＭ１〜Ｍ５を回転させるように作動する。

【００３３】メモリ２５にはロボットを教示点データに
従って動作させるためのプログラムが記憶されたＰＤＡ
領域２５１とロボットの位置と姿勢を表す教示点データ
を記憶するＰＡ領域２５２が設けられており、教示モー
ドにおいて、複数の教示点における位置データと姿勢デ
ータが記憶される。又、教示点ではジョイント座標も記
憶される。

【００３４】次に、ＣＰＵ２０の処理手順を示した図
６、図７のフローチャートを参照して本装置の作用を説
明する。ステップ１００において、教示点番号ｉが初期
値２に設定され、教示点データＶi-1 、Ｕi-1 、Ｖi 、
Ｕi がＲＡＭ２５のＰＤＡ領域２５１から読み込まれ
る。次に、ステップ１０２において、補間点番号ｋが初
期値１に設定され、ステップ１０４において、第ｋ補間
点における位置ベクトルＰ（ｋ）が隣接する２つの教示
点データのワールド座標から数１式の教示点番号をｉに
一般化した式を用いて演算される。次に、ステップ１０
６において、第ｋ補間点における第４軸角度θ₄(k)、第
５軸角度θ₅(k)が数３式、数４式の教示点番号をｉに一
般化した式を用いて演算される。

【００３５】次に、ステップ１０８において、現特定点
Ｒを第ｉ−１教示点とする。即ち、現特定点Ｒの位置ベ
クトルＰ_R は第ｉ−１教示点の位置ベクトルＶi-1 に、
現特定点Ｒのジョイント座標行列Θ_R は第ｉ−１教示点
のジョイント座標行列Ｕi-1に設定される。次に、ステ
ップ１１０において、現特定点Ｒの第ｋ補間点に対する
位置偏差（ベクトル）ΔＰと、角度偏差（行列）ΔΘが
数７式、数８式の補間点番号をｋに一般化した式を用い
て演算される。

【００３６】次に、ステップ１１２において、数１３式
で定義される１次偏微分行列式Ａの現特定点Ｒにおける
値が演算される。１次偏微分行列式Ａは図４に示す具体
的な幾何学的構成においては、次のように演算される。
床に固定された座標系Ｏ−ＸＹＺ，各軸Ａ１〜Ａ５に固
定された座標系Ｏ−Ｘ₁ Ｙ₁ Ｚ₁ ，〜Ｏ−Ｘ₅ Ｙ₅ Ｚ₅
間の座標変換行列Ａ１〜Ａ５は、図８に示すようにな
る。但し、Ｃｉ＝cos(θ_i ) 、Ｓｉ＝sin(θ_i ) であ
る。この座標変換行列の積Ａ１Ａ２Ａ３Ａ４Ａ５を演算
することで、ジョイント座標をワールド座標における位
置ベクトル及び姿勢行列へ変換する変換式が求められ
る。

【００３７】この関係式は図９に示すようになる。ただ
し、この式においては、ツール長ｌ（レーザ光ノズル部
６５の長さ）は考慮していない。よって、数９式、数１
０式、数１１式のＸ，Ｙ，Ｚ関数は次式のようになる。

【００３８】

【数１４】Ｘ（θ_i )=d5cos(θ₂)cos(θ₃)cos(θ₄)+d4c
os( θ₂)sin(θ₃)-d5sin( θ₂)sin(θ₄)+d3sin( θ₂)+
θ₁

【数１５】Ｙ（θ_i )=d5sin(θ₂)cos(θ₃)cos(θ₄)+d4s
in( θ₂)sin(θ₃)+d5cos( θ₂)sin(θ₄)-d3cos( θ₂)

【数１６】 Ｚ（θ_i )=-d5sin( θ₃)cos(θ₄)+d4cos( θ₃)+d2

【００３９】よって、数１３式で定義される１次偏微分
行列Ａは次のようになる。

【数１７】δＸ／δθ₁ ＝1

【数１８】δＸ／δθ₂ ＝-d5sin( θ₂)cos(θ₃)cos(θ
₄)-d4sin( θ₂)sin(θ₃)-d5cos( θ₂)sin(θ₄)+d3cos(
θ₂)

【数１９】δＸ／δθ₃ ＝-d5cos( θ₂)sin(θ₃)cos(θ
₄)+d4cos( θ₂)cos(θ₃)

【数２０】δＸ／δθ₄ ＝-d5cos( θ₂)cos(θ₃)sin(θ
₄)-d5sin( θ₂)cos(θ₄)

【数２１】δＸ／δθ₅ ＝0

【数２２】δＹ／δθ₁ ＝0

【数２３】δＹ／δθ₂ ＝d5cos(θ₂)cos(θ₃)cos(θ₄)
+d4cos( θ₂)sin(θ₃)-d5sin( θ₂)sin(θ₄)+d3sin( θ
₂)

【数２４】δＹ／δθ₃ ＝-d5sin( θ₂)sin(θ₃)cos(θ
₄)+d4sin( θ₂)cos(θ₃)

【数２５】δＹ／δθ₄ ＝-d5sin( θ₂)cos(θ₃)sin(θ
₄)+d5cos( θ₂)cos(θ₄)

【数２６】δＹ／δθ₅ ＝0

【数２７】δＺ／δθ₁ ＝0

【数２８】δＺ／δθ₂ ＝0

【数２９】 δＺ／δθ₃ ＝-d5cos( θ₃)cos(θ₄)-d4sin( θ₃)

【数３０】δＺ／δθ₄ ＝d5sin(θ₃)sin(θ₄)

【数３１】δＺ／δθ₅ ＝0

【００４０】但し、上記式のθ₁ 〜θ₅ は、現特定点Ｒ
における値である。

【００４１】次に、ステップ１１４において、数１２式
をΔΘについて解いて解く。但し、Δθ₄ 、Δθ₅ は、
第ｋ補間点の角度に対する現特定点Ｒの角度の偏差であ
り、ステップ１１０で既に求められている値に固定す
る。この結果、数１２より、未知変数Δθ₁ 〜Δθ₃ の
３変数に対して３つの独立した式が存在するので、３変
数Δθ₁ 〜Δθ₃ を解くことができる。よって、角度偏
差（行列）ΔΘが求められる。

【００４２】次に、ステップ１１６で新特定点Ｎのジョ
イント座標Θ_N が現特定点Ｎのジョイント座標Θ_R と角
度偏差ΔΘとを用いて演算される。次に、ステップ１１
８において、数１４式〜数１６式を用いて、現特定点Ｎ
のジョイント座標Θ_R からワールド座標における位置ベ
クトルＰ_N が演算される。

【００４３】次に、ステップ１２０において、位置偏差
ΔＰと角度偏差ΔΘが共に十分に小さい値（Ｔ_A,Ｔ_B ）
か否かが判定される。小さくなければ、新補間点Ｎは第
ｋ補間点に未だ収束していないので、ステップ１２２に
おいて、ステップ１１６、１１８で演算されたジョイン
ト座標と位置ベクトルを新補間点Ｒのジョイント座標と
位置ベクトルと置き換えて、ステップ１１０に戻り、上
記の処理が繰り返し実行される。この時、θ₄ 、θ
₅ は、第ｋ補間での値θ₄ (k) 、θ₅(k)に固定される。

【００４４】このようにして、ステップ１１８におい
て、位置偏差ΔＰと角度偏差ΔΘが共に十分小さくなる
まで、上記の演算が繰り返し実行される。そして、それ
らの値が十分に小さくなったと判定されると、ステップ
１２４において、ステップ１１８で演算された新特定点
Ｎのジョイント座標が位置決めするための第ｋ補間点の
ジョイント座標と決定される。次に、ステップ１２６で
ステップ１２２と同様に新補間点Ｎが現特定点Ｒに変更
された後、ステップ１２８で補間点番号ｋが最終か否か
が判定されて、最終でなければ、ステップ１３０におい
て、補間点番号ｋが１だけ加算され、ステップ１０４に
戻り、次の第ｋ補間点を演算する処理が上記のようにし
て演算される。又、ステップ１２８で第ｋ補間点番号が
最終であれば、ステップ１３２において、教示点番号ｉ
が最終か否かが判定され、最終でなければ、ステップ１
３４に移行して教示点番号ｉが１だけ加算されて、ステ
ップ１００に移行して、次の教示点に対する補間演算が
実行される。

【００４５】上記実施例において、教示データ記憶手段
はＲＡＭ２５で、第１補間手段はＣＰＵ２０、ステップ
１０６で、第２補間手段はＣＰＵ２０、ステップ１０４
で、近似式記憶手段はＲＡＭ２５（数１３式、数１７〜
３１式）で、演算手段はＣＰＵ２０、ステップ１１０〜
１２０で、それぞれ、具体化されている。

【００４６】又、ステップ１０８は現特定点の位置ベク
トル及びジョイント座標を教示点での値に設定する初期
値設定手段であり、ステップ１１０は、それぞれ、現特
定点の位置ベクトル、ジョイント座標の第ｋ補間点の位
置ベクトル、ジョイント座標に対する偏差を演算する偏
差演算手段であり、ステップ１１２は現特定点における
ワールド座標での微小変位とジョイント座標での微小変
位とを関係付ける１次近似式を演算する近似式演算手段
であり、ステップ１１４は位置ベクトルの微小変位から
対応するジョイント座標の微小変位を演算する逆演算手
段であり、ステップ１１６は次の新特定点のジョイント
座標を演算する更新手段であり、ステップ１１８はジョ
イント座標からワールド座標の位置ベクトルを演算する
順変換手段である。

【００４７】又、ステップ１２４、１２６は、次の教示
区間に対する現特定点の初期値を設定する初期値設定手
段であり、ステップ１２８は教示データの最終を判定す
る演算最終判定手段である。以上述べた実施例では、ス
テップ１２０の位置偏差ΔＰと角度偏差ΔΘが共に十分
小さくなったかを設定値（Ｔ_A,Ｔ_B ）と比較することに
よって判断しているが、ある程度の回数（例えば５回）
を繰り返し演算すれば十分小さくなることが分かってい
れば、繰り返し演算の回数を予め設定するようにしても
良い。

【図面の簡単な説明】

【図１】本発明の補間の原理を説明した説明図。

【図２】本発明の補間の原理を説明した説明図。

【図３】本発明の具体的な実施例にかかるロボットの機
械的構成を示した機構図。

【図４】本発明の具体的な実施例にかかるロボットの幾
何学的構成を示した構成図。

【図５】同実施例装置の電気的構成を示したブロック
図。

【図６】補間手順を示したフローチャート。

【図７】補間手順を示した図６に続くフローチャート。

【図８】各軸間の変換行列を示した説明図。

【図９】ジョイント座標からワールド座標への変換式を
示した説明図。

【符号の説明】

５４…テーブル ５５…ローテータ ５８…アーム支持部 ６０…アーム

Claims (2)

【特許請求の範囲】
1. 【請求項１】オフセット手首を有する５軸多関節ロボッ
   トにおいて、２つの教示点の間の動作軌跡上にある補間
   点のワールド座標を決定して、その補間点のワールド座
   標をロボットの各軸の角度で表したジョイント座標に変
   換し、その補間点のジョイント座標を用いて、順次、補
   間点に位置決め制御する制御装置において、 前記教示点の座標データを記憶した教示データ記憶手段
   と、 ２つの前記教示点の間における第４軸角度差、第５軸角
   度差を分割して補間点の第４軸及び第５軸のジョイント
   座標を演算する第１補間手段と、 ２つの前記教示点の間の動作経路を分割して補間点のワ
   ールド座標を演算する第２補間手段と、 ジョイント座標をワールド座標へ変換するための関数か
   ら得られる１次近似式であって、任意点におけるワール
   ド座標での微小変位とジョイント座標での微小変位を関
   係付ける１次近似式を記憶した近似式記憶手段と、 前記補間点のジョイント座標を逐次演算により求める手
   段であって、既に、ワールド座標とそれに対応するジョ
   イント座標とが正確に分かっている点を現特定点とし
   て、その現特定点に対する前記補間点のワールド座標の
   位置偏差と、前記現特定点における前記１次関数とか
   ら、前記現特定点に対する前記補間点のジョイント座標
   の角度偏差を演算し、その角度偏差と前記現特定点のジ
   ョイント座標とから次の新特定点のジョイント座標を演
   算し、そのジョイント座標に対応するワールド座標を演
   算し、その新特定点を現特定点として、上記の演算を繰
   り返し実行することで、前記補間点に漸近する前記新特
   定点を新補間点として決定することにより、前記教示点
   間の新補間点を順次決定する演算手段とを有し、 前記
   演算手段において、第４軸及び第５軸の前記角度偏差
   は、前記第１補間手段により演算された前記補間点の第
   ４軸及び第５軸のジョイント座標の前記現特定点のジョ
   イント座標に対する偏差として演算されるロボットの制
   御装置。
2. 【請求項２】前記請求項１のロボット制御装置におい
   て、前記オフセット手首を有する５軸多関節ロボット
   は、第１軸、第２軸および第３軸の少なくとも１つが回
   転軸であることを特徴とするロボット制御装置。