JP3030565B2 - ロボットの駆動制御装置 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 この発明は、産業用ロボット、特に第５軸にオフセッ
トのある６軸多関節ロボットの駆動制御装置に関するも
のである。

〔発明の背景〕

産業用ロボットに所望の動作を行わせる方法として、
ロボットのエンドエフェクタの先端がたどるべき経路上
の点を順次ティーチングし、これらのティーチング点を
滑らかに結ぶ曲線又は直線で補間する方法が広く知られ
ている（ティーチングプレイバック）。従って、この方
法を用いてロボットの駆動を制御するには、ロボットの
エンドエフェクタの先端の位置と姿勢を表現するXYZ絶
対座標系（Ｘ系）からロボットの関節角を表現する関節
座標系（θ系）への座標変換、即ち逆変換が必要とな
る。

一般にロボットの座標変換には、θ系からＸ系への順
変換と、前述の逆変換がある。前者は、ロボットの軸構
成がどのようなものであっても解析的に厳密解を求める
ことができることが知られている。同様に、後者に関し
ても解析的に厳密解を求める方法が試みられており、例
えばR.C.Paul,“Robot Manipulators;Mathematics,Prog
ramming and Control"MIT Press（1981）にその具体例
が示されている。即ち、順変換の変換式から代数学的に
逆変換の変換式を求めるわけである。

しかし、ロボットの軸構成いかんによっては厳密な解
析解の求まる場合と求まらない場合があり、特にオフセ
ットのある多関節型ロボットにおいては、どの軸にオフ
セットがあるかによっても解析解が求まる場合と求まら
ない場合とが生じる。

〔従来の技術〕

第５軸にオフセットのある６軸関節ロボットは逆変換
の厳密解を求めることができないケースとして知られて
おり、その様な６軸関節ロボットのアームの各駆動量を
制御する研究が従来より活発に行われてきた。例えば、
特開昭62−193786号公報や特開昭64−8407号公報等に示
された従来技術が知られている。

前者は、まずオフセットのある手首部は動作しないも
の（第４〜第６軸の関節角θ_４〜θ_６が一定値）として
ロボットのエンドエフェクタの先端の位置データより、
第１〜第３軸の関節角θ_１〜θ_３の値を近似的に求め
る。

次に、これらの近似解（θ_１〜θ_３）を用いてエンド
エフェクタの先端の姿勢データより（この際位置データ
は０として扱う）、第４〜第６軸の関節角θ_４〜θ_６の
値を近似的に求めた後、位置データを補正する。更に、
補正後の位置データを用いて最初に関節角θ_４〜θ_６を
求めた近似的方法により、再度関節角θ_４〜θ_６を求め
る。そして、以上の演算を所望の精度に応じて繰り返し
行なうこととしたものである。

一方、後者はロボットの６個の関節角の中から少なく
とも一つの関節角について、次式に示す様にヤコビ行列
（ヤコビアン）を用いてその近似解を求める。即ち、ロ
ボットのエンドエフェクタ先端の現在の位置と姿勢及び
次の移動すべき位置とその位置での姿勢を表わすベクト
ルをそれぞれ_０及び_１とし、ベクトル_０及び_１
に対応するロボットの関節角を示すベクトルをそれぞれ
_０及び_１、ヤコビアンをＪ（）として表わすとす
れば、上述の近似解_１は、_１ ≒_０＋J^-1（）・（_１−_０） …（１） として求められる。そして、この様にして求めた関節角
の一部の近似解_１を用いて、残りの関節角の解を求め
るものである。この場合、逆変換の変換式から近似解と
して求めた関節角を除外することによりその分の数だけ
逆変換の変換式の未知数を減少させることができ、多く
の場合残りの関節角について解析的に解を求めることが
できるようになる。

〔発明が解決しようとする課題〕

従来の技術は以上の様に構成されているので、次の様
な問題点があった。

特開昭62−193786号公報で開示される従来技術は、全
ての関節角θ_１〜θ_６について収束演算を行わなければ
ならず且つ位置データの補正が必要とされるので、計算
が複雑でありアームの駆動量決定に時間がかかるという
問題点があった。

一方、特開昭64−8407号公報で開示される従来技術は
収束演算を用いないという点では演算速度を早める利点
を有するが、関節角θ_１〜θ_６の連立方程式の少なくと
も１つの関節角を近似計算し連立方程式の未知数を減少
させる方法であるため、関節角θ_３がゼロに近い領域あ
るいは関節角θ_５がゼロに近い領域等のいわゆる臨界領
域においては十分な精度で計算できないという問題点が
あった。

〔発明の目的〕

この発明は以上の様な問題点に鑑みなされたもので、
第５軸にオフセットのある６軸多関節ロボットに関し、
アームの駆動量決定の手順が簡単であって、且ついわゆ
る臨界領域においてもアームの駆動量の精度を一定のレ
ベル以上に保持できる駆動制御装置を提供することを目
的としている。

〔課題を解決するための手段〕

（１） 請求項１に係る発明は、アーム先端にエンドエ
フェクタが取付けられてなり、第５軸にオフセットのあ
る第６軸関節ロボットの駆動制御装置であって、（ａ）
前記ロボットの設置面に平行な面をXY平面とするXYZ絶
対座標系において、所定の移動経路上の各ティーチング
点に前記エンドエフェクタの先端を順次に移動してその
位置及び姿勢決めを行うことにより得られる各ティーチ
ング点における各軸の関節角の値のデータを電気的に順
変換することにより各ティーチング点のティーチングデ
ータ信号を求めると共に、互いに隣り合うティーチング
点間を両ティーチング点に関するティーチングデータ信
号より補間して、補間点に於ける前記エンドエフェクタ
の先端の位置及び姿勢に関する補間点データ信号を電気
的に求める手段と、（ｂ）前記移動経路上の前記補間点
における前記エンドエフェクタの先端の位置及び姿勢の
補間点データ信号から、前記補間点における前記ロボッ
トの各軸の関節角を与える信号を、順変換方程式を変形
することにより与えられる、第１軸ないし第４軸の各関
節角を変数とする第１逆変換方程式を電気的に解くこと
によって決定する手段とを備え、前記手段（ｂ）は、
（ｂ−１）第４軸の関節角の値に初期値を電気的に与え
る手段と、（ｂ−２）前記手段（ｂ−１）により定めら
れた前記第４軸の関節角の値の信号を用いて前記第１逆
変換方程式より第１軸の関節角から第３軸の関節角まで
の値の信号を順次に電気的に求める手段と、（ｂ−３）
前記手段（ｂ−２）により求められた前記第１軸の関節
角から前記第３軸の関節角までの値の信号を用いて、前
記順変換方程式を変形することにより得られる、前記第
１軸ないし第３軸の各関節角を変数とする前記第４軸の
関節角を与える第２逆変換方程式より改めて第４軸の関
節角の値の信号を電気的に求める手段と、（ｂ−４）前
記手段（ｂ−３）により求められた第４軸の関節角の値
の信号と前記手段（ｂ−２）に与えられた前記初期値の
信号との差分データ信号を求め、当該差分データ信号が
許容範囲内か否かを判定する手段と、（ｂ−５）前記手
段（ｂ−４）が許容範囲内にないと判定したときには、
前記手段（ｂ−３）が求めた前記第４軸の関節角の値の
信号を電気的に補正して、新たな初期値を電気的に決定
する手段と、（ｂ−６）前記手段（ｂ−５）によって求
められた補正後の初期値の信号を前記第４軸の関節角の
値の信号として前記手段（ｂ−２）に入力して前記手段
（ｂ−２）を再実行させる手段と、（ｂ−７）前記手段
（ｂ−４）が、前記差分データ信号が前記許容範囲内に
なったと判定したときには、そのときの第１軸の関節角
の値から第４軸の関節角の値までの信号を前記第１逆変
換方程式の解として電気的に決定する手段と、（ｂ−
８）前記手段（ｂ−７）により決定された前記第１軸の
関節角から前記第４軸の関節角までの値の信号を用い
て、前記順変換方程式を変形して得られる第３及び第４
逆変換方程式よりそれぞれ第５軸及び第６軸の関節角の
値の信号を電気的に求める手段とを有し、前記駆動制御
装置は、（ｃ）前記手段（ｂ）で求めた各軸の関節角の
値の信号に応じた駆動出力を前記アームの対応する駆動
機構に与えて、前記６軸関節ロボットの駆動制御を行う
手段を更に備えることを特徴とする。

（２） 請求項２に係る発明は、請求項１記載の６軸関
節ロボットの駆動制御装置であって、前記移動経路上の
任意のｋ番目の点をP_kとして表すと、（ｋ＋１）番目の
補間点P_k+1における前記第４軸の関節角の初期値とし
て、（ｉ）前記ｋ番目の点P_k及び前記点P_k+1における前
記エンドエフェクタの先端のそれぞれの位置及び姿勢
と、（ii）前記点P_kにおける前記エンドエフェクタの先
端の位置及び姿勢のヤコビアンの値と、（iii）前記点P
_kにおいて既に決定されている前記第４軸の関節角の値
とを用いて近似計算により電気的に推定される前記補間
点P_k+1における第４軸の関節角の値を用いることを特徴
とする。

（３） 請求項３に係る発明は、請求項１記載の６軸関
節ロボットの駆動制御装置であって、前記移動経路上の
ｋ番目の点である補間点P_kにおける前記第４軸の関節角
の初期値として、（ｋ−）番目の点Ｐ_ｋ−〔１≦
≦（ｋ−１）〕から（ｋ−１）番目の点P_k-1それぞれに
おいて既に決定されている第４軸の関節角の値の線形結
合によって与えられる値を用いることを特徴とする。

〔作用〕

この発明の第１の構成では、第４軸の関節角の値に初
期値を与え、順変換の方程式を変換して得られる第１軸
の関節角から第４軸の関節角を変数とする第１逆変換方
程式より、第１軸の関節角から第３軸の関節角の値を求
める。そして、得られた第１軸の関節角から第３軸の関
節角の値を順変換の方程式を変換して新たに得られる第
２逆変換方程式へ代入して改めて第４軸の関節角の値を
求めた後、得られた第４軸の関節角の値と初期値との差
が許容範囲内であるかを比較する。許容範囲外である場
合には、前記得られた第４軸の関節角の値を新たな初期
値として以上の演算処理が前記差が許容範囲内に収束す
るまで繰り返されるので、精度の高い第１軸の関節角か
ら第４軸の関節角の値が求められる。その後、第５軸及
び第６軸の関節角を求めるとともに、以上求められた各
関節角の値に応じた駆動出力がアームの各軸の駆動機構
に与えられ、エンドエフェクタの先端が所定の位置へ正
確に移動し、所定の姿勢が正確に保たれることになる。

又、この発明の第２の構成では、第４軸の関節角の初
期値が、移動経路上の補間点P_k+1と点P_k+1の一つ前の点
P_kにおけるエンドエフェクタの先端の位置及び姿勢のデ
ータ、点P_kにおいてすでに求められた第４軸の関節角の
値及び点P_kにおけるエンドエフェクタの先端の位置・姿
勢より求まるヤコビアンの値より推定されるので、第４
軸の関節角の初期値として適切な値を与える。

更に、この発明の第３の構成においても、第４軸の関
節角の初期値が、エンドエフェクタの先端が移動すべき
補間点P_kよりも以前にエンドエフェクタの先端が移動し
た点P_l〔１≦ｌ≦（ｋ−１）〕において求められた第４
軸の関節角の値の線形結合により与えられるので、第４
軸の関節角の初期値として適切な値を与える。

〔実施例〕

A.装置の機械的構成 第１図は、この発明の一実施例である第５軸にオフセ
ットを有する６軸多関節ロボットRBの機械的構成を模式
的に示す側面図であり、第１軸から第６軸それぞれの軸
における関節変位（回転方向）もまた、第１図に模式的
に示されている。

図において、ロボットRBの設置面10上に固定された絶
対座標系としてＸ系を定義するとともに、ロボットRBの
基台11よりＺ軸に沿って延びた支柱20の先端部に当る第
１の関節部21の略中心（設置面10より高さd₁の位置）に
固定された座標系（x₁,y₁,z₁）を、第１軸の座標系とし
て定義する。即ち、z₁軸はＺ軸に平行な座標軸、x₁,y₁
軸はXY平面に平行な平面内であって、且つz₁軸に直交す
る座標軸である。そして第１の関節部21は、第１の関節
部21の内部に備えつけられた第１の駆動機構によって、
z₁軸の周りを第１の関節角θ_１方向に回転することがで
きる。

又、第１のアーム30が図に示す様に支柱20の第１の関
節部21に取付けられており、その接合部に当る第２の関
節部31の中心の固定された座標系（x₂,y₂,z₂）を、第２
軸の座標系として定義する。即ち、第１のアーム30の軸
に平行にz₂軸を、y₁軸に平行にy₂軸を、y₂軸,z₂軸に直
交する方向にx₂軸をとるものとする。そして第１のアー
ム30は、第２の関節部31の内部に備えつけられた第２の
駆動機構によって、y₂軸の周りを第２の関節角θ_２方向
に回転することができる。

更に、第１のアーム30の先端部に当る第３の関節部32
（第２の関節部31の中心と第３の関節部32の中心との距
離をd₂とする。）にはオフセットa₂を介して第２のアー
ム40が取り付けられており、第３の関節部32の中心に固
定された座標系（x₃,y₃,z₃）を第３軸の座標系として定
義する。即ち、y₂軸に平行にy₃軸を、第２のアーム40の
軸に平行にz₃軸を、y₃軸,z₃軸に直交する方向にx₃軸を
とるものとする。そして第２のアーム40は、第３の関節
部32の内部に備えつけられた第３の駆動機構によって、
y₃軸の周りを第３の関節角θ_３方向に回転することがで
きる。

尚、一般的には第２軸及び第３軸にオフセットがある
ケースが多く、第１図の構成においても実際には第３軸
のみならず第２軸にもオフセットが生じているが、その
ような第２軸及び第３軸にオフセットがある場合の逆変
換は容易に厳密解を解析的に求めることができる場合で
あるので、第２軸及び第３軸にオフセットがあるか否か
はこの発明においては本質的事項ではない。従って、第
１図においては、便宜上第２軸にオフセットがないもの
として取り扱っている。

又、第２のアーム40の先端部に当る第４の関節部41
（第３の関節部32の中心と第４の関節部41の中心との距
離をd₃とする。）にはオフセット部を介して第３のアー
ム50が取り付けられているが（尚、第４の関節部41は第
５の関節部を兼ね備えている）、この第２のアーム40と
第３のアーム50との接続関係については、第２図をも参
照しながら説明する。ここで第２図は、第２のアーム40
と第３のアーム50の接続部分の平面図を示している。

第１図及び第２図において、第４の関節部41にはオフ
セットa₃のオフセット部42が取り付けられており、オフ
セット部42の先端部に当る第６の関節部43には第３のア
ーム50が取り付けられている。更に、第３のアーム50の
先端にはＬ字状のエンドエフェクタ60が取り付けられて
いる。

そして、第４の関節部41の中心に固定された２つの座
標系（x₄,y₄,z₄）及び（x₅,y₅,z₅）を、それぞれ第４軸
の座標系，第５軸の座標系として定義する。即ち、第４
軸についてはオフセット部42の軸に平行にy₄軸を、第２
のアーム40の軸に平行にz₄軸を、y₄軸及びz₄軸に直交す
る方向にx₄軸をとるとともに、第５軸に関してはオフセ
ット部42の軸に平行にy₅軸を、第３のアーム50の軸に平
行にz₅軸を、y₅軸及びz₅軸に直交する方向にx₅軸をと
る。

又、第３のアーム50はオフセット部42とともに、第４
の関節部41の内部に備えつけられた第４の駆動機構によ
って、z₄軸の周りを第４の関節角θ_４方向に回転するこ
とができる。

更に、第３のアーム50は第４の関節部41の内部に備え
つけられた第５の駆動機構によって、y₅軸の周りを第５
の関節角θ_５方向に回転することもできる。

そして、第６の関節部43の中心に固定された座標系
（x₆,y₆,z₆）を、第６軸の座標系として定義する。即
ち、オフセット部42の軸に平行にy₆軸を、第３のアーム
50の軸に平行にz₆軸を、y₆軸及びz₆軸に直交する方向に
x₆軸をとる。

又、エンドエフェクタ60は第６の関節部43の内部に備
えつけられた第６の駆動機構によって、z₆軸の周りを第
６の関節角θ_６方向に回転することができる。

以上説明したロボットRBの各軸の関節角の可変範囲
を、表１に例示する。

B.装置の電気的構成 第３図は、第１図に示したロボットRBの制御系の電気
的構成を示すブロック図である。

この制御系はCPU71やメモリ72等からなるコントロー
ラ70を有しており、ソフト的に制御される。又、ティー
チングボックス80を操作することによって、ティーチン
グデータの取込みが行われ、ティーチングデータがメモ
リ72に書き込まれる。

一方、第１の駆動機構91を構成するモータM₁に駆動信
号D₁が送信され、モータM₁に取付けられたエンコーダE₁
が、第１軸の関節角θ_１の値を検出する。同様に、第２
〜第６の駆動機構92〜96にそれぞれ駆動信号D₂〜D₅が送
信され、各モータM₂〜M₆が動作し、第２軸の関節角θ２
から第６軸の関節角θ_６までがそれぞれエンコーダE₂〜
E₆によって検出される。

C.駆動制御方法 第４図は、ロボットRBを駆動制御するためのステップ
を示すフローチャートである。

ステップS1はティーチング工程であり、ティーチング
データ_ｉの作成と記憶を行う。即ち、オペレータはマ
ニュアルでエンドエフェクタ60の先端の位置決めを行
い、エンコーダE₁〜E₆により各軸の関節角の値（θ_１〜
θ_６）を検出し、メモリ72に格納する。そしてCPU71に
おいて、検出された各軸の関節角の値を順変換すること
により各ティーチング点のティーチングデータ_ｉが計
算され、ｉ＝1,2,…についてのティーチングデータ_ｉ
がメモリ72に格納される。

次のステップ群10は再生動作に対応し、そのうちのス
テップS2ではひとつのティーチング点についてのティー
チングデータ_ｉをメモリ72より読出す。

そして、ステップS3ではCPU71においてそのティーチ
ング点とそれに隣接するティーチング点との間を補間
し、補間点のデータ▲▼を計算する。補間点のデ
ータ▲▼はＸ系による値であるため、エンドエフ
ェクタ60を補間点へ移動するための適切な各軸の関節角
の値を決定する必要がある。

そこで、ステップS4ではＸ系からθ系への逆変換が行
われる。ここで、第５図はステップS4を詳細に説明する
フローチャートであり、以下第５図に基づいてステップ
S4を説明することとする。尚、以下の解析による逆変換
アルゴリズムの流れを第６図に示す。この第６図におい
て、かっこ付きの数字は以下の説明中における数式の番
号を示しており、（＊）印はティーチングデータから直
ちに得られるＸ系の量を、（＊＊）印は逆変換によって
求めるべきθ系の量を示している。また、ティーチング
点▲▼および補間点▲▼のすべてを順次に配
列した連鎖を点P_k（ｋ＝1,2,…）で表現する。以下で
は、特に、任意のｋの値に対し、ｋ番目の補間点P_kにつ
いての逆変換を考える。

まずステップS41では、各補間点P_kごとに第４軸の関
節角θ_４の初期値θ₄₀を設定する。そして設定された初
期値θ₄₀は、メモリ72に格納される。この初期値θ₄₀の
設定という手順は、後述する様に逆変換式の内、エンド
エフェクタ60の先端の位置ベクトルに関する３元連立方
程式が第１軸の関節角θ_１から第４軸の関節角θ_４によ
り表わされることから、第４軸の関節角θ_４を既知の量
として扱えば、第１軸の関節角θ_１から第３軸の関節角
θ_３の解を求めることができる点に着眼したことによる
ものである。

ここで初期値θ₄₀として、以下のものを用いることも
できる。

その一つは、現在逆変換を行うとしている補間点P_kに
対して、既に逆変換を行った（ｋ−）番目の点Ｐ
_ｋ−から（ｋ−１）番目の点P_k-1までのｌ個（１≦
≦（ｋ−１））の点（これらの点は補間点又はティーチ
ング点（ティーチング点では逆変換は不要）である）の
それぞれにおける第４軸の関節角の値（θ_４）_ｋ−〜
（θ_４）_k-1の線形結合によって与えられる量を、初期
値θ₄₀として用いるものである。即ち、初期値θ₄₀は、
係数c_mを用いて（２）式により表わされる。

従って、上記（２）式の例として例えば、以下の
（３）式〜（５）式により表わされるものを初期値θ₄₀
として用いてもよい。

θ₄₀＝（θ_４）_k-1 …（３） θ₄₀＝２（θ_４）_k-1−（θ_４）_k-2 …（４） θ₄₀＝３（θ_４）_k-1−３（θ_４）_k-2＋（θ_４）_k-3 …（５） （３）式〜（５）式により与えられる初期値θ₄₀は、
補間点P_kに対して前回計算した点P_k-1,前々回計算した
点P_k-2等において既に求めた第４軸の関節角θ_４の値を
用いるものであるから、補間点P_kにおける第４軸の関節
角の真値（θ_４）_ｋに対して比較的良い近似値となりえ
るものである。特に、再生動作において補間点（▲
▼）を細かくとる場合には、十分良い近似値となりえ
る。

尚、（２）式等により初期値θ₄₀を設定する場合に
は、あらかじめ所定のプログラムを作成しておけば、後
はCPU71がメモリ72に格納された各点における第４軸の
関節角θ_４のデータを読み出して自動的に初期値θ₄₀を
決定してしまうので、初期値設定の自動化を図れる利点
もある。

更に補間点P_kにおける初期値θ₄₀の他の設定法として
は、前回の点P_k-1（この点は補間点又はティーチング点
である）におけるエンドエフェクタ60の先端の位置及び
姿勢のヤコビアンを求めることにより、補間点P_kにおけ
る第４軸の関節角（θ_４）_ｋを電気的に推定することも
できる。その方法を、以下に詳述する。

今、点P_k-1及び点P_kにおけるエンドエフェクタ60の先
端の位置及び姿勢を表わすベクトルをそれぞれベクトル 点P_k-1及び点P_kにおける各軸の関節角を表わすベクトル
をそれぞれ とする。

＝（X₁,X₂,…,X₆） …（６） ＝（θ₁,θ₂,…，θ_６） …（７） そして、∂X_i/∂θ_ｊを第ｉ行第ｊ列の行列要素とす
るヤコビアンをＪ（）とし、ヤコビアン（Ｊ）の逆
行列J^-1（）を用いて、点P_kにおける関節角ベクトル
内を（８）式〜（10）式によって近似的に表わすこと
ができる。

従って、（８）式〜（10）式を用いて補間点P_kにおけ
る第４軸の関節角θ_４の値を電気的に推定することがで
き、（８）式によって計算した第４軸の関節角θ_４の値
を補間点P_kにおける第４軸の関節角（θ_４）_ｋの初期値
θ₄₀に用いることができる。この方法による初期値θ₄₀
の設定もまた良い近似値を与えるものであり、前述した
設定例と同様に、メモリ72より点P_k-1及び補間点P_kにお
けるエンドエフェクタ60の先端の位置及び姿勢データを
読み出して（８）式から（10）式の一連の演算をCPU71
に行わせることにより、初期値θ₄₀の設定をソフト的に
実行できる利点をも合わせもつものである。

以上の様にして初期値θ₄₀の設定を行なった後、ステ
ップS42に示す一連の演算処理を行うこととなる。

ステップS42は、補間点P_kにおける第４軸の関節角
（θ_４）_ｋについての収束演算を行うステップであり、
５つのステップS42A〜S42Eより構成される。以下、補間
点P_kにおける第４軸の関節角（θ_４）_ｋを単に第４軸の
関節角θ_４と記すこととして、各ステップ（S42A〜S42
E）について詳述する。

ステップS42Aでは、初期値θ₄₀を用いて逆変換の方程
式より第１軸の関節角θ_１の仮値θ₁₁から第３軸の関節
角θ_３の仮値θ₃₁までを求める。ここで、一般化して以
下の手順を説明すべく、第１軸の関節角θ_１の仮値θ₁₁
から第３軸の関節角θ_３の仮値θ₃₁までを、（11）式〜
（13）式で示すように整数ｉ（ｉ≧１）をもって表現す
る。

θ₁₁→θ_1i …（11） θ₂₁→θ_2i …（12） θ₃₁→θ_3i …（13） ここで、仮値θ_1i〜θ_3iの導出式を導き出すために、
順変換及び逆変換の方程式の導出について説明する。

今、エンドエフェクタ60の先端の位置ベクトルをｐ、
姿勢ベクトルを（，，）で表わすとすれば、θ系
からＸ系への座標変換マトリックスＴ、即ち順変換の方
程式は、次式（14）式及び（15）式により表わされる。

Ｔ＝A₁A₂A₃A₄A₅A₆ …（15） ここで、マトリックスA₁〜A₆はロボットRBの各関節部
の回転に伴う座標変換マトリックスであり、（16）式〜
（21）式により表わされる。

尚、（16）式〜（21）式においては、余弦及び正弦の
記載方法として（22）式〜（24）式により示される簡略
形を用いており、以後の説明においても同様に（22）式
〜（24）式の簡略形を用いることとする。

C_i＝cosθ_ｉ …（22） S_i＝sinθ_ｉ …（23） i＝１〜６ …（24） そこで、（16）式〜（21）式を用いて（15）式の順変
換方程式を展開すれば、座標変換マトリックスＴの各要
素を求めることができるが、そのうちベクトルのｚ成
分n_z,ベクトルのｚ成分a_z及びベクトルの各成分に
ついて成立する順変換の方程式のみを（25）式〜（29）
式へ示す。

n_z＝−（S₂₃C₄C₅＋C₂₃S₅）C₆＋S₂₃S₄S₆ …（25） a_z＝−S₂₃C₄S₅＋C₂₃C₅ …（26） p_x＝d₃C₁S₂₃−a₂S₁＋d₂C₁S₂−a₃（C₁C₂₃S₄＋S₁C₄） …（27） p_y＝d₃S₁S₂₃＋a₂C₁＋d₂S₁S₂−a₃（S₁C₂₃S₄−C₁C₄） …（28） p_z＝d₃C₂₃＋d₂C₂＋d₁＋a₃S₂₃S₄ …（29） 尚、（25）式〜（29）式においては、（30）式及び
（31）式により定義される簡略形C₂₃及びS₂₃を用いてい
る。

C₂₃＝C₂C₃−S₂S₃ …（30） S₂₃＝C₂S₃＋S₂C₃ …（31） 以後、以上の様にして得られた順変換の方程式（15）
式及び（25）式〜（29）式により、逆変換の解θ_１〜θ
_６を求めることになる。

そこで、（15）式をマトリックスA₁〜A₃の逆マトリッ
クスA₁ ^-1〜A₃ ^-1を用いて以下の変換を行う。

まず（15）式の両辺に逆マトリックスA^-1を乗ずれ
ば、 A^-1T＝A₂A₃A₄A₅A₆ …（32） となる。この（32）式の両辺を展開し比較すると、第１
軸の関節角θ_１から第４軸の関節角θ_４を変数とする３
元連立方程式が得られる。即ち、 d₃S₂₃−a₃S₄C₂₃＋d₂S₂＝p_xC₁＋p_yS₁ …（33） a₂＋a₃C₄＝−p_xS₁＋p_yC₁ …（34） d₃C₂₃−a₃S₄S₂₃＋d₂C₂＝p_z−d₁ …（35） となる。これらの（33）式〜（35）式は、第１軸の関節
角θ_１ないし第４軸の関節角θ_４を変数とする、順変換
方程式（15）式を変形して得られる「第１逆変換方程
式」である。

更に、（15）式の両辺に逆マトリックスの積A₃ ^-1A₂ ^-1
A₁ ^-1を乗ずれば、 A₃ ^-1A₂ ^-1A₁ ^-1T＝A₄A₅A₆ …（36） となる。この（36）式の両辺を展開して比較すると、 S₅S₄＝−a_xS₁＋a_yC₁ …（37） S₅C₄＝C₂₃（a_xC₁＋a_yS₁）−a_zS₂₃ …（38） C₅＝S₂₃（a_xC₁＋a_yS₁）＋a_zC₂₃ …（38−１） S₅S₆＝S₂₃（o_xC₁＋o_yS₁）＋o_zC₂₃ …（38−２） −S₅C₆＝S₂₃（n_xC₁＋n_yS₁）＋n_zC₂₃ …（38−３） となる。従って、第４軸の関節角θ_４について、（37）
式及び（38）式より、 が得られる。この（39）式は、第１軸の関節角θ_１ない
し第３軸の関節角θ_３を変数として与えられる、順変換
方程式（15）式を変形して得られる「第２逆変換方程
式」である。

ここで、（33）式〜（35）式及び（39）式を概観すれ
ば、第４軸の関節角θ_４を定数として（33）式〜（35）
式を取扱うとするならば、（34）式より第１軸の関節角
θ_１を求めることができ、後述する様に（33）式及び
（35）式を変形して第２軸の関節角θ_２及び第３軸の関
節角θ_３を求めることができることが判る。従って、第
４軸の関節角θ_４に初期値を設定して（33）式〜（35）
式へ代入し、得られた第１軸から第３軸の関節角θ_１〜
θ_３を（39）式へ代入すれば、第４軸の関節角θ_４を求
めることができる。そして、この第４軸の関節角θ_４の
値と初期値とを比較し、両者の差（差分データ信号に相
当）が許容できる範囲内にない場合は、初期値を修正し
たうえで以上の手順を繰り返し続行すれば、最終的には
十分精度の高い第１軸から第４軸の関節角θ_１〜θ_４の
値が得られるであろうと考えることができる。

逆変換の方程式については以上述べた通りであり、再
びステップS42Aへ話を戻すこととする。

そこで、第４軸の関節角θ_４の初期値θ₄₀及び（11）
式〜（13）式を（33）式〜（35）式へ代入すれば、仮値
θ_1i,θ_2i,θ_3iに関して d₂S_2i＋d₃S_2i3i−a₃′C_2i3i＝pxC_1i＋p_yS_1i …（40） d₂C_2i＋d₃C_2i3i＋a₃′C_2i3i＝p_z−d₁ …（41） −p_xS_1i＋p_yC_1i＝a₂′ …（42） が成立する（第１軸ないし第３軸の各関節角の仮値及び
第４軸の関節角の初期値に関する第１逆変換方程式に該
当）。

ここで、定数a₂′及びa₃′は a₂′＝a₂＋a₃cosθ₄₀ …（42A） a₃′＝a₃sinθ₄₀ …（40A） により定義される量である。

従って、（42）式より仮値θ_1iは、（43）式〜（45）
式によって求められる。

Ф_１＝atan2（p_y,p_x） …（44） ここで、関数atan2（X,Y）は、 Ｘ＝Ｓ_φ,Y＝Ｃ_φ …（46） を満足する角度φを一意に与える関数である。

次に、仮値θ_2i及びθ_3iについては次の様にして求め
られる。（40）式及び（41）式をそれぞれ辺々自乗して
加算すると、 2d₂（αS₂＋βC₂）＝α^２＋β^２＋d₂ ²−d₃ ²−a₃′^２
…（47） が得られる。ここで、α，βは α＝p_xC_1i＋p_yS_1i …（48） β＝p_z−d₁ …（49） によって与えられる定数である。

従って、仮値θ_2iは（50）式〜（52）式によって求め
られる。

Φ_２＝atan2（β，α） …（51） 又、仮値θ_3iは（50）式で求められた仮値θ_2iを（4
0）式及び（41）式へ代入して、（53）式により求めら
れる。

θ_3i＝atan2（S_2i3i,C_2i3i）−θ_2i …（53） S_2i3i=〔d_３(α-d_２S_２)+a_３′(β-d_２c_２)〕/(d₃ ²+
a_３′^２) …（54） C_2i3i=〔d_３(β-d_２c_２)-a_３′(α-d_２S_２)〕/(d₃ ²+
a_３′^２ …（55） 以上の結果より、CPU71は（43）式，（50）式及び（5
3）式を用いて仮値θ_1i〜θ_3iを算出する。

次にステップS42Bでは、ステップS42Aで算出した仮値
θ_1i〜θ_3iを用いて、第４軸の関節角θ_４の仮値θ_4iを
算出する。即ち、（39）式より仮値θ_4iは、（56）式又
は（57）式（仮値θ_4iを与える第２逆変換方程式に該
当）によって求められる。

θ_4i＝atan2（−a_xS_1i＋a_yC_1i,a_xC_1iC_2i3i ＋a_yS_1iC_2i3i−a_zS_2i3i）（θ_５＞０のとき） …（56） θ_4i＝atan2（a_xS_1i−a_yC_1i,−a_xC_1iC_2i3i −a_yS_1iC_2i3i＋a_zS_2i3i）（θ_５＜０のとき） …（57） ステップS42Cでは、仮値θ_4iと初期値θ₄₀との比較を
行う。即ち、許容値δをあらかじめ設定しておきメモリ
72内に蓄えておく。そしてCPU71はメモリ72により許容
値δを読み出し、仮値θ_4iと初期値θ₄₀との差（θ_4i−
θ₄₀）と許容値δとを比較する。

もし、差（θ_4i−θ₄₀）が許容値δより大きい場合、
即ち （θ_4i−θ₄₀）＞δ …（58） の場合には、ステップS42Dへ移る。

ステップS42D及びS42Eは、初期値θ₄₀の修正を行うス
テップである。

即ち、ステップS42Dでは、（60）式〜（62）式で与え
られる量θ_4i′を新たな初期値θ₄₀として用いる。

θ_4i′→θ₄₀ …（59） ｆ＝θ_4i−θ₄₀ …（61） ここで、関数f_B及び記号θ_40Bは、それぞれ関数ｆ及び
初期値θ₄₀の１つ前の計算段階における値である。尚、
（62）式の≒で結ばれた関数式を用いると、微分式df/d
θ₄₀を直接計算しなくても前回の計算段階で求めた値を
そのまま使用できる利点がある。

そして、（59）（60）式で与えられる初期値θ₄₀を用
いて、ステップS42E（ステップS42Eは、新たな仮値θ
_1(i+1),θ_2(i+1),θ_3(i+1)を求める意である。）を介し
て、再び一連のステップS42A〜S42Cが繰り返される。

一方、差（θ_4i−θ₄₀）が許容値δの範囲内となる場
合には、一連のステップS42A〜S42Eの繰り返しは終了
し、ステップS43へ移る。

ステップS43では、ステップS42で求められた仮値、即
ち （θ_4i−θ₄₀）≦δ …（63） を満足させる仮値θ_1i〜θ_4iをそれぞれ当該補間点P_kに
関して求めるべき第１軸の関節角θ_１から第４軸の関節
角θ_４の値として決定する。

θ_１＝θ_1i …（64） θ_２＝θ_2i …（65） θ_３＝θ_3i …（66） θ_４＝θ_4i …（67） ステップS44では、（64）式〜（67）式で与えられる
第１軸から第４軸の関節角の値θ_１〜θ_４を用いて、当
該補間点P_kにおける第５軸及び第６軸の関節角の値θ₅,
θ_６を求める。即ち、（37）式と（38−１）式より、第
５軸の関節角θ_５は（68）〜（70）式により決定される
（これらの式は「第３逆変換方程式」に相当する）。

θ_５＝atan2（ξ₁,ξ_２） …（68） ξ_１＝（−a_xS₁＋a_yC₁）/S₄ …（69） ξ_２＝S₂₃（a_xC₁＋a_yS₁）＋a_zC₂₃ …（70） 又、第６軸の関節角θ_６は（38−２）式と（38−３）式
より、 θ_６＝atan2（ε₁,ε_２） …（71） ε_１＝（o_xC₁S₂₃＋o_yS₁S₂₃＋o_zC₂₃）/S₅ …（72） ε_２＝−（n_xC₁S₂₃＋n_yS₁S₂₃＋n_zC₂₃）/S₅ …（73） によって決定される（これらの式は「第４逆変換方程
式」に相当する）。

以上の手順によってステップS4は終了する。

再び第４図に戻り、次のステップS5では、ステップS4
で求められた各軸の関節角（θ_１〜θ_６）がそれぞれ駆
動信号D₁〜D₆に変換された後、各駆動機構91〜96に伝達
される（第３図参照）。そして、各駆動機構91〜96は、
各駆動信号D₁〜D₆に応じた量だけ各アームを駆動し、エ
ンドエフェクタ60の先端は点P_k、つまりステップS3にて
計算により求めた補間点（▲▼）へ向けて移動す
る。そして、補間点P_kにて所定の処理が行われる。

次に、ステップS6では、その補間点（▲▼）が
最後の補間点か否かを判断する。尚、その判断はソフト
的にコントローラ70によって行われる。そして、補間点
（▲▼）が最後の補間点でないと判断したときに
はステップS3へ戻って次の補間点 が計算され、一連のステップS4〜S5が同様に行われる。

それに対して、補間点（▲▼）が最後の補間点
であると判断したときには、ステップS7へ移る。

ステップS7では、ティーチング点（▲▼）が最後
のティーチング点か否かを判断する。この判断も又、コ
ントローラ70によって行われる。そして、ティーチング
点（▲▼）が最後のティーチング点でないと判断し
たときには、ステップS2へ戻り次のティーチング点のデ
ータ がメモリ72より読出され、一連のステップS3〜S6即ちテ
ィーチングプレイバックが再び行われる。

それに対して、ティーチング点（▲▼）が最後の
ティーチング点であると判断したときには、ロボットRB
の全ての動作が終了する。

D.エンドエフェクタ先端の位置・姿勢の制御精度 この発明を用いてエンドエフェクタ60の先端の位置・
姿勢を決定した場合の位置・姿勢の精度について、シミ
ュレーションを行なった結果について、簡単に説明す
る。

位置・姿勢の精度の評価は、次の様な手順で行われ
る。

まず、エンドエフェクタ60の先端の位置・姿勢の目
標値のデータを定める。

次に、目標値のデータにこの発明を適用して第１
軸〜第６軸の関節角θ_１〜θ_６の各値を求める。即ち、
逆変換を行う。

そして、得られた第１軸〜第６軸の関節角θ_１〜θ
_６の値を順変換して、エンドエフェクタ60の先端の位置
・姿勢を求める。このときの位置・姿勢のデータは＋
Δとなり、データΔが求める位置・姿勢の誤差とな
る。

以上の手順によりエンドエフェクタ60の先端の位置・
姿勢の誤差を求めた結果は、次の通りである。

例えば、エンドエフェクタ60の先端が10mm〜40mmの間
隔で２点間（点P_kと点P_k+1）を移動するものとすれば、
（59）式〜（61）式で与えられる収束演算を１〜２回繰
返す程度でも、誤差Δはエンドエフェクタ60の先端の
作動範囲の10^-6程度（位置誤差±0.001mm程度）内に抑
制することができる。この誤差値は殆ど無視できうるも
のであり、エンドエフェクタ60の先端の位置・姿勢の精
度は十分良好であるといえる。

又、第５軸の関節角θ_５がゼロに十分近い領域（θ_５
≦数deg.）では逆変換の変換式の解は一般に求まりにく
いと考えられるが、この発明によれば、（56）式と（5
7）式間の変更を行うことにより容易に解を求めること
ができる。更に収束演算の回数を増やせばエンドエフェ
クタ60の先端の位置・姿勢の精度を高めることができ、
シミュレーション結果によれば、上述の誤差と同程度の
誤差範囲内でエンドエフェクタ60の先端の位置・姿勢を
決定できることが確認されている。

又、第４軸の関節角θ_４の初期値θ₄₀として、（８）
式や（４）式又は（５）式で与えられるものを用いて逆
変換の変換式の解を求める場合には、上述初期値θ₄₀と
して（３）式で与えられるものを用いて逆変換の変換式
を求める場合よりも、収束演算の繰返し数が同一である
にもかかわらず、エンドエフェクタ60の先端の位置・姿
勢の精度が大巾に向上されることも、シミュレーション
により確認されている。

E.その他 この発明は以上説明した第５軸にオフセットのある６
軸多関節ロボットの駆動制御に限られるものでなく、自
由度が６のロボットであってそのうちの１の自由度に関
してオフセットを有するため、逆変換の変換式を解析的
に解くことができない構成のロボットの駆動制御方法全
般について適用できるものである。

〔発明の効果〕

請求項１の発明によれば、６つの関節角θ_１〜θ_６の
内第４軸の関節角θ_４のみ収束演算を行なえば他の関節
角については一意的に求められるため、アームの各駆動
量決定のプロセスを簡単化できる効果があるとともに、
第５軸の関節角θ_５がゼロに近い様な臨界領域において
も高精度にアームの各駆動量を決定できるので、常にエ
ンドエフェクタの先端の位置及び姿勢を高精度で制御で
きる効果がある。

又、請求項２の発明によれば、第４軸の関節角θ_４の
収束演算の繰返し回数を少なくしても高精度にアームの
各駆動量を決定できるので、エンドエフェクタの先端の
位置及び姿勢を高精度で且つ短時間で制御できる効果が
ある。

更に、請求項３の発明においても、請求項２の発明と
同様の効果を有するものである。

【図面の簡単な説明】

第１図はこの発明の一実施例である６軸多関節ロボット
の機械的構成を示す側面図、 第２図は第１図の６軸多関節ロボットの第２のアーム，
オフセット部及び第３のアームの機械的構成を拡大して
示す平面図、 第３図は６軸多関節ロボットの制御系の電気的構成を示
すブロック図、 第４図は６軸多関節ロボットを駆動制御するためのステ
ップを示すフローチャート、 第５図は第４図におけるステップS4を詳細に説明するフ
ローチャート、第６図は逆変換アルゴリズムの流れを示
す説明図である。 RB……６軸多関節ロボット 10……ロボットの設置面 20……ロボットの支柱 21……第１の関節部、30……第１のアーム 31……第２の関節部、32……第３の関節部 40……第２のアーム、41……第４の関節部 42……オフセット部、43……第６の関節部 50……第３のアーム、60……エンドエフェクタ 91……第１の駆動機構 92……第２の駆動機構 93……第３の駆動機構 94……第４の駆動機構 95……第５の駆動機構 96……第６の駆動機構 θ_１……第１軸の関節角 θ_２……第２軸の関節角 θ_３……第３軸の関節角 θ_４……第４軸の関節角 θ_５……第５軸の関節角 θ_６……第６軸の関節角

Claims (3)

(57)【特許請求の範囲】
1. 【請求項１】アーム先端にエンドエフェクタが取付けら
   れてなり、第５軸にオフセットのある６軸関節ロボット
   の駆動制御装置であって、 （ａ）前記ロボットの設置面に平行な面をXY平面とする
   XYZ絶対座標系において、所定の移動経路上の各ティー
   チング点に前記エンドエフェクタの先端を順次に移動し
   てその位置及び姿勢決めを行うことにより得られる各テ
   ィーチング点における各軸の関節角の値のデータを電気
   的に順変換することにより各ティーチング点のティーチ
   ングデータ信号を求めると共に、 互いに隣り合うティーチング点間を両ティーチング点に
   関するティーチングデータ信号より補間して、補間点に
   於ける前記エンドエフェクタの先端の位置及び姿勢に関
   する補間点データ信号を電気的に求める手段と、 （ｂ）前記移動経路上の前記補間点における前記エンド
   エフェクタの先端の位置及び姿勢の補間点データ信号か
   ら、前記補間点における前記ロボットの各軸の関節角を
   与える信号を、順変換方程式を変形することにより与え
   られる、第１軸ないし第４軸の各関節角を変数とする第
   １を変換方程式を電気的に解くことによって決定する手
   段とを備え、 前記手段（ｂ）は、 （ｂ−１）第４軸の関節角の値に初期値を電気的に与え
   る手段と、 （ｂ−２）前記手段（ｂ−１）により定められた前記第
   ４軸の関節角の値の信号を用いて前記第１逆変換方程式
   より第１軸の関節角から第３軸の関節角までの値の信号
   を順次に電気的に求める手段と、 （ｂ−３）前記手段（ｂ−２）により求められた前記第
   １軸の関節角から前記第３軸の関節角までの値の信号を
   用いて、前記順変換方程式を変形することにより得られ
   る、前記第１軸ないし第３軸の各関節角を変数とする前
   記第４軸の関節角を与える第２逆変換方程式より改めて
   第４軸の関節角の値の信号を電気的に求める手段と、 （ｂ−４）前記手段（ｂ−３）により求められた第４軸
   の関節角の値の信号と前記手段（ｂ−２）に与えられた
   前記初期値の信号との差分データ信号を求め、当該差分
   データ信号が許容範囲内か否かを判定する手段と、 （ｂ−５）前記手段（ｂ−４）が許容範囲内にないと判
   定したときには、前記手段（ｂ−３）が求めた前記第４
   軸の関節角の値の信号を電気的に補正して、新たな初期
   値を電気的に決定する手段と、 （ｂ−６）前記手段（ｂ−５）によって求められた補正
   後の初期値の信号を前記第４軸の関節角の値の信号とし
   て前記手段（ｂ−２）に入力して前記手段（ｂ−２）を
   再実行させる手段と、 （ｂ−７）前記手段（ｂ−４）が、前記差分データ信号
   が前記許容範囲内になったと判定したときには、そのと
   きの第１軸の関節角の値から第４軸の関節角の値までの
   信号を前記第１逆変換方程式の解として電気的に決定す
   る手段と、 （ｂ−８）前記手段（ｂ−７）により決定された前記第
   １軸の関節角から前記第４軸の関節角までの値の信号を
   用いて、前記順変換方程式を変形して得られる第３及び
   第４逆変換方程式よりそれぞれ第５軸及び第６軸の関節
   角の値の信号を電気的に求める手段とを有し、 前記駆動制御装置は、 （ｃ）前記手段（ｂ）で求めた各軸の関節角の値の信号
   に応じた駆動出力を前記アームの対応する駆動機構に与
   えて、前記６軸関節ロボットの駆動制御を行う手段を更
   に備えることを特徴とする、 ６軸関節ロボットの駆動制御装置。
2. 【請求項２】請求項１記載の６軸関節ロボットの駆動制
   御装置であって、 前記移動経路上の任意のｋ番目の点をP_kとして表すと、 （ｋ＋１）番目の補間点P_k+1における前記第４軸の関節
   角の初期値として、 （ｉ）前記ｋ番目の点P_k及び前記点P_k+1における前記エ
   ンドエフェクタの先端のそれぞれの位置及び姿勢と、 （ii）前記点P_kにおける前記エンドエフェクタの先端の
   位置及び姿勢のヤコビアンの値と、 （iii）前記点P_kにおいて既に決定されている前記第４
   軸の関節角の値 とを用いて近似計算により電気的に推定される前記補間
   点P_k+1における第４軸の関節角の値を用いることを特徴
   とする、 ６軸関節ロボットの駆動制御装置。
3. 【請求項３】請求項１記載の６軸関節ロボットの駆動制
   御装置であって、 前記移動経路上のｋ番目の点である補間点P_kにおける前
   記第４軸の関節角の初期値として、 （ｋ−）番目の点Ｐ_ｋ−〔１≦≦（ｋ−１）〕か
   ら（ｋ−１）番目の点P_k-1それぞれにおいて既に決定さ
   れている第４軸の関節角の値の線形結合によって与えら
   れる値を用いることを特徴とする、 ６軸関節ロボットの駆動制御装置。