JPH0820894B2 - 産業用ロボツトの動作制御方法 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は工業用ロボツトの動作制御方法に係り、特に
関節形ロボツトなど複雑な機構構造を有する工業用ロボ
ツトの手先作業点の位置、姿勢を制御しながら動作を行
わせる動作制御方法に関する。

〔従来の技術〕

従来、工業用ロボツトに所望の動作を行わせる場合の
方法として、ロボツトの手先あるいは手先に取付けた作
業工具のたどるべき経路上の点を順次教示（これらの点
の位置にロボツトを誘導するなどの方法で）し、これら
の教示点間を空間内で直線、円弧等で補間することによ
り動作を実現させる方法が広く行われている。あるい
は、ロボツト手先の作業工具のたどるべき経路を、作業
経路の記述に適した一般化座標系によつて表現された演
算式として与えておき、これを用いることにより教示を
行わずにロボツトを動作させる方法なども考えられる。

いずれにしても、これらの方法によつてロボツトを動
作させるためには、作業経路の記述された一般化座標系
（通常はロボツトの基台上に固定された直交座標系など
が用いられる）から、ロボツトの機構を構成する各自由
度の変位を要素とするロボツト座標系への交換演算が不
可欠となる。即ち、一般化座標系で表現されたロボツト
の手先作業工具の位置及び姿勢を実現するための、ロボ
ツトの各自由度の変位、例えば関節の角度、を求める変
換演算である。しかも、この演算は原則的にはロボツト
の動作に同期して実時間で行い、ロボツトを制御する必
要がある。

この変換演算、即ち一般化座標系からロボツト自由度
の座標系への変換の手法としては、解析的に厳密解を求
める方法が常識的な方法である。その具体例は、例えば
リチヤード、ポール著「ロボツト・マニプレータ」（R.
Paul,“Robot Manipulators “MIT Press,1981）70頁
〜79頁に示されている。ここに示されている方法は、ロ
ボツト自由度の座標系から一般化座標系への変換演算式
を求め、これから代数学的に逆の変換、即ち一般座標系
からロボツト自由度座標系への変換演算式を求める方法
であるが、この他にも幾何学的関係を用いて所要の変換
演算式を得る方法等もあることはもちろんである。

ここに例示した方法では、一般化座標系で表わされた
ロボツト手先の作業工具の位置及び姿勢にもとづいて、
これを満たすロボツトの各自由度の変位（例えば回転形
関節の角度）を求めている。一方、工業用ロボツトとは
類似の分野であるマスタースレープ・マニプレータの分
野などでは、マニプレータの動きをその手先の作業工具
の並進速度、回転速度として一般化座標で表わし、これ
を与えるマニプレータの各自由度の速度（例えば回転形
関節の場合は角速度）を求めることによつて動作を制御
するという方法が知られている。その具体例は、例えば
ダニエル・ホイツトニーがアイ・イー・イー・イー・ト
ランザクシヨン・オン・マン・マシン・システムズ，エ
ム・エム・エスー10,2,47頁〜53頁（IEEE Transactions
Man−Machine Systems,MMS−10（２）,P47〜53,1969
年）等に発表した「分解速度法（Resolved Motion Rate
Control）」に示されている。この方法は、ロボツト自
由度座標系での自由度変位と一般化座標系における位
置、姿勢との関係を与える座標変換行列のヤコビ行列と
呼ばれるものを利用した方法である。この場合ヤコビ行
列とは、一般化座標系における位置、姿勢を表わす各変
数を、ロボツト自由度座標系の各変数の各各で偏微分し
たものを要素とする行列である。この行列の逆行列を用
いることにより、手先工具の一般化座標系における位
置、姿勢の時間微分、即ち速度、角速度に対応するロボ
ツトあるいはマニプレータの各自由度の変位の時間微分
である速度ないしは角速度を求めることができ、これを
用いて動作の制御を行うことができるわけである。

ロボツトの各自由度の変位と、手先作業工具の位置、
姿勢との関係、あるいは、ロボツト自由度座標系と一般
化座標系との間の変換式は明らかにロボツトの自由度配
置、構造によつて異るが、同様にヤコビ行列の形もロボ
ツトの機構構造によつて異つたものとなる。このヤコビ
行列に関しては、一般化座標系のとり方、導出の方法な
どに関して、オリーンらが、インターナシヨナル・ジヤ
ーナル・オブ・ロボテイクス・リサーチ第３巻４号（Or
in,Schrader:Efficient Computation of the Jacobian
for Robot Manipulators,International Journal of Ro
botics Research,Vol3,No.4,1984）にまとめているごと
く、いくつかの方法が提案されている。

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

上記した２種の従来技術には、それぞれ異つた点での
問題が残されている。以下、順次その内容について述べ
る。

まず、第１の方法、即ち一般化座標系からロボツト自
由度座標系への変換演算式を代数学的、幾何学的などの
方法により、いわば解析的厳密解として求める方法は、
指定された手先作業点の位置及び作業工具の姿勢を正確
に満たすロボツト機構の各自由度の変位を得ることがで
きる長所を有している。しかし、この反面、ロボツト機
構の自由度配置の形態によつては、上記の変換演算式を
求めることが困難となり、実質的に変換演算を行うこと
ができない場合が生ずる。これについて、もう少し詳細
に説明する。

まず、ロボツト自由度座標系から、一般化座標系への
変換演算式は、例えばエイ・エス・エム・イー・トラン
ズアクシヨン、ジヤーナル・オブ・アプライド・メカニ
クス、1955年６月号215頁〜221頁に示されているデナビ
ツト及びハルテンベルグの記法（Denavit,Hartenberg:A
kinematic notation for lower−pair mechanisms bas
ed on matrices,ASME Journal Applied Mechanics,Vol
23,P215〜221,1955）を用いて座標変換行列を順次計算
することにより、ロボツトの機構の自由度配置がどのよ
うなものであつても求めることが可能である。なお、以
下ではこの、ロボツト自由度座標系から一般化座標系へ
の変換演算を「座標正変換」、このための演算式を「座
標正変換式」と呼ぶことにする。一方、一般化座標系か
らロボツト自由度座標系への変換演算式、即ち、一般化
座標系で表現されたロボツトの手先作業点位置及び作業
姿勢から、これを満たすロボツトの各自由度の変位を求
めるための変換演算式、（以下、これを「座標逆変換
式」と呼ぶことにする。）は、上記の座標正変換式を逆
に解くことによつて与えられる。

いま、一般化座標系によるロボツトの手先作業点の位
置及び姿勢の各パラメータを要素とする多次元ベクトル
を考え、これを、また、ロボツト自由度座標系による
各自由度の変位を要素とする多次元ベクトルをと記す
ものとすれば、正座標変換式は、 ＝ｆ（） ……（１） なる関数として定義できるが、このとき、逆座標変換式
は ＝f^-1（） ……（２） より、（１）式の逆関数として示される。

数学的に考えても、関数ｆの形が与えられた場合、常
にその逆関数f^-1の形が求められるとは限らず、ロボツ
ト機構の場合でも上記した「正座標変換式」が与えられ
ても、その逆の「逆座標変換式」が常に容易に求められ
るとは限らない。具体的には、正座標変換式の中から、
ロボツト自由度座標系の各パラメータを順次代入，消去
して行くなどの代数的な解法で逆座標変換式を求める場
合、計算の段階において方程式の次数が高次になつた
り、多重の超越関数を含んだ形となつたりする場合が生
じうるのであり、この場合には、いわゆる解析的な厳密
解を与える逆座標変換式の形が明確に求められなくな
る。なお、このことの幾何学的な意味については、本発
明の実施例のところでも説明を補足する。

ロボツトの動作制御においては、例えばロボツトの手
先作業点を空間内での直線の上を動作させる場合などに
おいて、ロボツトの動作中に実時間で上記の逆座標変換
演算を行わなければならない。与えられた手先作業点位
置、姿勢に対応するロボツト各自由度の変位の各々を直
接与える演算式が明らかであり、容易に計算が可能な場
合には、動作制御上何ら問題はないが、逆座標変換式が
求められないもの、あるいは複雑で容易に計算すること
が困難な場合には、数値解析などの手法を用いて解を収
束演算等で求めなければならず、演算量等の面からも実
時間での動作制御に適用することは困難であり、事実
上、一般化座標系で表わされた手先作業点の位置、姿勢
を制御しつつロボツトを動作制御することは不可能とな
る。

次に、従来技術のうちの第２の方法について述べる。
この方法は前記した第１の方法中に示した（１）式の関
数ｆのヤコビ行列を用いる方法である。いま、（１）式
におけるロボツトの手先作業点の位置及び姿勢の各パラ
メータを要素とする多次元ベクトルを、 ＝（X₁,X₂,…,X_n）^Ｔ ……（３） （ここに、Ｔは転置を示す。） また、同様にロボツト各自由度の変位を要素とする多
次元ベクトルを ＝（θ₁,θ₂,…，θ_ｍ）^Ｔ ……（４） とおく。なお、ここで通常はｎ≦６である。またｍ＝ｎ
のときロボツトは冗長自由度を持たないと言う。

さて、ここで、 を第ｉ行ｊ列要素とする行列を考える。これはヤコビ行
列と呼ばれるものであり、ロボツトの各自由度の変位を
変化する速度（以下ロボツト各自由度の速度、ないしは
角速度と呼ぶ）と、ロボツト手先作業点の速度及びこの
点における作業姿勢変化の角速度（これをまとめて、以
下作業点の速度、角速度と呼ぶ）との関係を与える重要
な行列である。一般にこのヤコビ行列はロボツト各自由
度の変位の関数となるから、これをＪ（）と書くこと
にする。いま、上記のロボツト各自由度の速度、即ちロ
ボツト各自由度の変位の微分を また、手先作業点の速度、角速度、即ち手先作業点の
位置、姿勢の微分を と書けば、 であり、また が成り立つ。ここで、J^-1（）は、Ｊ（）が正則で
あれば常に存在する。このときJ^-1（）は、Ｊ（）
から常に解析的に求めることができる。従つて、ロボツ
トの手先作業点の速度、角速度が与えられれば、これに
対応するロボツトの各自由度の速度が、Ｊ（）が正則
である限り、厳密解として得られる。なお、Ｊ（）が
正則であるとき、ヤコビ行列の値、即ちヤコビ行列式
（ないしはヤコビアンとも言う）は０とはならない。即
ち、det J（）≠０が成立する。また、det J（）＝
０となるロボツトの姿勢（手先作業点の姿勢とは異り、
いわゆるロボツトのコンフイギユレーシヨン）は、特異
姿勢と呼ばれ、ロボツトの制御上重要な意味を持つてい
る。

さて、このヤコビ行列を用いる方法、即ち従来技術の
第２の方法は、（６）式により、与えられた手先作業点
の速度から、これを満たすロボツト各自由度の速度を求
める方法であるが、ロボツトの位置制御という点から見
ると問題を含んでいる。即ち、この方法を用いると、ロ
ボツトの手先作業点の位置、姿勢は（５）式の両辺を積
分した結果として得られるわけであるが、このときＪ
（）がの関数であることに意味しなければならな
い。言いかえれば、ヤコビ行列Ｊ（）は、関数ｆ
（）の微分であり、関数ｆ（）をなる値において
線形化したものに他ならず、＝の近傍において近似
値しか与えない。

このことを別の視点から更に説明する。一般に、
（５）式、あるいは（６）式を用いてロボツトの動作を
演算制御しようとする場合、マイクロコンピユータなど
のデイジタル計算機を用いるわけであるが、この演算に
は有限の時間が必要である。いま、この時間をΔｔとす
れば、 Δｔ≠０であるから、（６）式の表現形態は の代りに を用いたものとなる。

ここに、 ただし、 ΔX_i≒_ｉ・Δｔ Δθ_ｊ≒_ｊ・Δｔ であり、このとき（６）式は、 となる。従つて、両辺にΔｔを乗じて Δ≒J^-1（）Δ ……（10） ただし、 Δ＝（Δθ₁,Δθ₂,…，Δθ_ｍ）^Ｔ ……（11） Δ＝（ΔX₁,ΔX₂,…，ΔX_m）^Ｔ ……（12） が得られる。これより，ある時点におけるロボツトの手
先作業点の位置、姿勢を_Ａ、これに対応するロボツト
各自由度の変位を_Ａとし、このときの次の目標となる
ロボツトの手先作業点の位置、姿勢を_Ｂとすれば、 Δ＝_Ｂ−_Ａ ……（13） とすることにより、（10）式によつてΔが得られ、
_Ｂに対応すべきロボツト各自由度変位_Ｂが、_Ｂ ≒_Ａ＋Δ ……（14） として求められる。

即ち、この式は近似式であり、本質的に誤差を持つ。
このことは、時間Δｔの間に動作制御の結果としてロボ
ツトが移動することによつて、ロボツト各自由度の変位
が変化してしまい、の関数である逆ヤコビ行列J^-1
（）の値も変化してしまうことを考えれば明らかであ
る。

例えば、マスク・スレーブ方式のマニプレータ等の場
合には、このような誤差が生じても、人間の操作するマ
スタ・マニプレータにおいて、人間を介してスレーブ・
マニプレータの位置、姿勢に対するフイードバツクが働
き、スレーブ・マニプレータを本来動作させた経路に収
束させる作業を持つことから、この誤差は実用上問題と
ならないことが多いが、本質的に誤差を持つことをまぬ
がれない方法であることは、この方法の問題点であると
言わざるを得ず、特に工業用ロボツトにおけるテイーチ
ング・プレイバツク形の動作制御方式としては問題が残
されている。

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

上記２種の従来技術における問題点、即ち、第１の方
法における「逆座標変換式」の導出がロボツト機構の自
由度配置によつては困難である点、また第２の方法にお
ける近似誤差をまぬがれない点を解決するための新しい
方法として、本発明は位置付けられる。

即ち、ロボツトの手先作業点の位置及び作業姿勢を示
す複数のパラメータを、正確に制御することを必要とす
るパラメータ（群）と、ある程度の誤差を許容しうるパ
ラメータ（群）とに分離し、上記の正確に制御すること
を必要とするパラメータに対しての影響を与えない、も
しくは影響の少ないロボツト各自由度のうち少くとも一
つの自由度については、その変位をヤコビ行列を用いた
近似値として演算により求め、このゆにして求められた
ロボツトの自由度の変位を用いて、上記の正確に制御す
ることを必要とする手先作業点のパラメータの値を満た
すように、ロボツトの残りの自由度の変位の厳密解を求
めるのである。

例えば、ロボツトによつてアーク溶接作業を行う場合
について考えてみると、作業工具、即ち溶接トーチの位
置、及び作業工具主軸の姿勢については正確に制御する
ことが必要となるが、作業工具の主軸線まわりの回転量
については作業に影響を及ぼさないため、ある程度の誤
差は許容しうる。さらに、例えば通常のアーク溶接作業
の場合で考えると、溶接トーチの位置に比較して、その
姿勢の方が、作業品質に及ぼす影響は小であると考えら
れる。あるいは、スポツト溶接作業の場合においても、
指定された位置に正確に溶接を行うことが重要であり、
このときスポツト溶接ガンの姿勢にはある程度の誤差を
許容しうる。

また、例えば挿入力を検出しながら作業完了を判断す
るようなはめ合い組立作業の場合には、挿入方向の位置
（成分）よりも、むしろ作業姿勢を正確に制御すること
が重要となる。

このように、ロボツトに作業を行わせる場合、作業品
質に及ぼす影響の少ないパラメータを少くとも一つ選定
することは一般的に可能であり、従つてロボツトの手先
作業点の位置、姿勢を示す複数のパラメータを、正確に
制御すべきものと、ある程度の誤差を許容しうるものと
に分離することは充分に現実的な手法であると考えられ
る。

〔作用〕

いま、（10）式の関係を６自由度のロボツトの場合に
ついて示すと、 Δθ_１≒J₁ ^-1（）Δ ……（15） （ｉ＝1,2,…,6） となり、６組の式が得られる。このとき、_Ａをロボツ
トの手先作業点のある時点における位置、姿勢を示すベ
クトル（これは、この場合６個の独立したパラメータに
よつて表現できる）とし、_Ｂを目標のロボツト手先作
業点位置、姿勢ベクトルとすれば、 Δθ_ｉ≒J^-1（_Ａ ^＊）（_Ｂ−_Ａ） ……（16） （ｉ＝1,2,…,6） ここに、_Ａ ^＊は、_Ａもしくは、_Ａ近傍のであ
り、_Ａは_Ａに対応するロボツト自由度変位ベクトル
である。

である。いま、例えば、（16）式の６組の式のうち、Δ
θ_１のみを演算により求めれば、_Ｂに対応したロボツ
ト自由度変位ベクトル_Ｂにおいて、 θ_B1≒θ_A1＋Δθ_１ ……（17） ただし、_Ａ＝（θ_A1,θ_A2,…，θ_A6）^Ｔ _Ｂ ＝（θ_B1,θ_B2,…，θ_B6）^Ｔ としてθ_B1が近似的に得られる。

このとき、（２）式の関係をもう一度ふりかえつて見
る。（２）式は、 θ_ｉ＝f^-1（X₁,X₂,…,X₆） ……（18） （ｉ＝1,2,…,6） であるが、これは具体的には θ_１＝f₁ ^-1（X₁,X₂,…,X₆） ……（19） θ_２＝f₂ ^-1（X₁,X₂,…,X₆） ……（20） θ_３＝f₃ ^-1（X₁,X₂,…,X₆） ……（21） θ_４＝f₄ ^-1（X₁,X₂,…,X₆） ……（22） θ_５＝f₅ ^-1（X₁,X₂,…,X₆） ……（23） θ_６＝f₆ ^-1（X₁,X₂,…,X₆） ……（24） と書き下せる。このf_i ^-1（ｉ＝1,2,…,6）の形がこのま
までは解析的な形で解くことが出来ない場合であつて
も、例えば上記（17）式によつてθ_１の値が確定してい
ると、未知数はθ₂,θ₃,θ₄,θ₅,θ_６の５つとなる。従
つて、X₁,X₂,X₃,X₄,X₅,X₆のうちの１つのパラメータを
取り除いた５次元ベクトル、例えば、X₂を取り除いた ＊＝（X₁,X₃,X₄,X₅,X₆）^Ｔ ……（25） に対して、これに対応する ＊＝（θ₂,θ₃,θ₄,θ₅,θ_６）^Ｔ ……（26） を求めることは、必ずしも困難ではなくなる。この具体
的な手法については後に実施例の中で説明するが、逆に
言えば、の中から少くとも１つの要素を取り除いて次
元を下げたベクトル＊と、の中から、同様に少くと
も１つの要素を取り除いて次元を下げたベクトル＊と
の関係、即ち、 ＊＝ｆ＊^-1（） ……（27） における関数ｆ＊^-1が解析的な厳密解として求められる
ように、それぞれ及びから取り除く要素を選べばよ
いわけである。なお、ベクトル＊の次元は、＊の次
元と一致するように選択する。また、ベクトルの次元が
１次元増加すると、「逆座標変換式」f^-1の算出に要す
る手間はほぼ等比級数的に増大するため、６自由度のロ
ボツト機構の場合、ベクトルの次数を１つ減少させ５次
元にするだけで、解析的厳密解を求め得るようにするこ
とが、ほとんどの場合可能である。

〔実施例〕

以下、本発明の一実施例を第１図乃至第４図を用いて
説明する。

第１図は、本発明の概念を示したものである。ある時
点（例えば現在）のロボツトの手先作業点の位置、姿勢
の一般化座標系表現１は、このときのロボツトの各自由
度変位２から正座標変換（（１）式）によつて得られ
る。また、このときのヤコビ行列３の要素は、同様にロ
ボツト各自由度変位を用いて計算できる。次に、ロボツ
ト手先作業点の位置、姿勢の一般化座標系による目標値
４が与えられたとする。この目標値は、例えば１秒間に
数回乃至数百回程度の周期で時々刻々と与えることによ
つてロボツトの動作制御を実現するための指令値とも言
うべきものである。さて、ロボツトの手先作業点の位
置、姿勢の目標値４が与えられても、これに対応するロ
ボツト各自由度の変位が正確に演算できない場合が、本
発明の主たる対象とするところである。このとき、手先
位置、姿勢の目標値４と、実際の手先位置、姿勢１との
偏差５を求める。ロボツトの各自由度を駆動するサーボ
系に対する変位指令の出力、更新は上記のように１秒間
に数回乃至数百回程度の周期毎に行われるから、上記の
偏差５（Δ）は概ね微小量と見なすことができ、従つ
てヤコビ行列３が既知であることから、（10）式あるい
は（16）式によつてロボツト自由度変位のうちの少くと
も１つについての変位の差６（Δθ_ｉ）が求められる。
これと、手先位置、姿勢１に対応する実際のロボツト各
自由度変位２とから、目標値４に対応すべきロボツト自
由度変位のうちの少くとも１つについての近似解７が得
られる。最後に、この近似解７及び手先位置、姿勢の目
標値４とから、代数学的、幾何学的などの方法によつ
て、残る未知のロボツト自由度の変位８を厳密解として
得る。なお、このとき目標値４を示す複数のパラメータ
のうちの少くとも１つ、即ち手先作業点位置、姿勢のう
ち誤差を許容するとしたもの、については無視して演算
を行えばよい。以上により、手先作業点目標値４に対応
する。すべてのロボツト各自由度変位が確定し、「逆座
標変換演算」と等価な機能を実際できる。

第２図に本発明の対象とするロボツト機構の一例を示
す。また、第３図に、このロボト機構の自由度配置が分
り易いように構成化したスケルトン図を示す。この例
は、いわゆる垂直多関節形ロボツトと呼ばれる機構10で
あり、旋回11、上腕12、前腕13の主軸３軸の先に、前腕
まわりの回転14、これと直交する方向の軸まわりの曲げ
15、更に曲げ軸15と直交する軸まわりのひねり16、この
手首３軸、計６自由度により構成されている。また、ひ
ねり軸16には手先作業工具17が取り付けられている。こ
こで特徴としては、回転軸14と曲げ軸15とが交点を持た
ない機構構造となつていることであり、このとき回転軸
14と曲げ軸15との空間距離はオフセツト量30と呼ばれ
る。

ロボツト各自由度変位は、このとき、旋回軸11の変位
をθ_１とし、以下上腕軸12をθ_２、前腕軸13をθ₃,回転
軸14をθ_４、曲げ軸15をθ_５、ひねり軸16の変位をθ_６
と定め、 ＝（θ₁,θ₂,θ₃,θ₄,θ₅,θ_６）^Ｔ ……（28） とおく。このとき、ロボツト機構の各リンク長、オフセ
ツト長、ジヨイント関節間ねじれ角などを前記したデナ
ビツト及びハルテンベルグの記法に述つて次のように表
わされるものとする。

このとき、ロボツト手先作業点18は、簡単のためひね
り軸16の主軸16A上に、ひねり軸16と曲げ軸15との交点1
7からl_hの距離の点18とする。また作業姿勢をひねり軸1
6の主軸16Aをx_hとし、ひねり軸16の主軸16Aと直交する
方向にひねり軸可動部材上にy_h軸をとり、x_h軸及びy_h軸
上の単位ベクトルの、基準座標系ΣＡに対する方向余弦
を用いて表わすことにする。ここで、基準座標系ΣＡ
は、ロボツトの基台上に固定された座標系であるとす
る。

（28）式及び上表１のパラメータを用いて、ロボツト
の手法作業点の位置及び姿勢を求めると、１つの自由度
θ_ｉに関する変換行列をA_iとして、 ここに、 S_i＝sinθ_ｉ C_i＝cosθ_ｉ であり、基準座標系ΣＡから、x_h軸、y_h軸を主軸のうち
の２つとする手先座標系ΣＢへの変換行列Ｔが Ｔ＝A₁A₂A₃A₄A₅A₆ ……（35） として求められる。いま、 とおけば、手先作業点の位置の基準座標系ΣＡによる成
分表現は（p_x,p_y,p_z）となる。

また、手先作業姿勢、即ち前記x_h軸、y_h軸の方向余弦
は各々（a_x,a_y,a_z），（n_x,n_y,n_z）となる。これらを
（35）式と比較すれば P_x＝C₁［C₂l₁＋S₂₃l₂＊＋C₂₃l_s＊］＋S₁l_w＊……（37） P_y＝S₁［C₂l₁＋S₂l₂＊＋C₂₃l_s＊］−C₁l_w＊ ……（38） P_z＝S₂l₁−C₂₃l₂＊＋S₂₃l₅＊＋l₀ ……（39） ただし、 ここに C₂₃＝cos（θ_２＋θ_３） S₂₃＝sin（θ_２＋θ_３） l₂＊＝l₂＋C₂l_h l_s＊＝l_s−S₄l_w＋C₄S₅l_h l_w＊＝C₄l_w＋S₄S₅l_hである。また、 n_x＝C₁（C₂₃（C₄C₅C₆−S₄S₆）−S₂₃S₅C₆） ＋S₁（S₄C₅C₆＋C₄S₆） ……（40） n_y＝S₁（C₂₃（C₄C₅C₆−S₄S₆）−S₂₃S₅C₆） −C₁（S₄C₅C₆＋C₄S₆） ……（41） n_z＝S₂₃（C₄C₅C₆−S₄S₆）＋C₂₃S₅−C₆ ……（42） a_x＝C₁（C₂₃C₄S₅＋S₂₃C₅）＋S₁S₄S₅ ……（43） a_y＝S₁（C₂₃C₄S₅＋S₂₃C₅）−C₁S₄S₅ ……（44） a_z＝S₂₃C₄C₅−C₂₃C₅ ……（45） が得られる。また、＝（n_x,n_y,n_z）^T,＝（o_x,o_y,
o_z）^T,＝（a_x,a_y,a_z）^Ｔとすれば、＝×（外
積）である。（37）式から（45）式までが、ロボツトの
各自由度変位ベクトルから、手先作業点位置、姿勢へ
の「正座標変換」式の実際の形に相当している。なお、
ここでは式が９個示されているが、との直交条件、
単位ベクトルであることなどから、独立した変数は６個
である。このことは、ロボツト機構10の持つ６つの自由
度θ_１〜θ_６を用いて、３次元空間内の手先作業点18の
位置、姿勢を制御することが一般的に可能であることを
示している。なお、前述したように、ヤコビ行列式が０
となる特異姿勢においては、ロボツト10の自由度が退化
し、手先作業点18の位置、姿勢の制御は不可能となる。

さて、ロボツト10の手先作業点18の位置、姿勢を制御
しつつ動作を行わせるためには、（37）式〜（45）式で
与えられるp_x,p_y,p_z,n_x,n_y,n_z,a_x,a_y,a_zを満たすθ_１〜
θ_６を、ロボツト10の動作中に時々刻々と求め、各自由
度の変位をサーボ制御することが必要となる。従つて、
（37）式〜（45）式を逆に解いた式、いわゆる「逆座標
変換式」を求めなくてはならない。

しかし、本ロボツトの機構構成の場合、これは極めて
困難である。例えば代数的に求めようとするならば、方
程式の次数が高次となるなどの問題が生じる。また幾何
学的に求めようとすれば、まず手先作業点17の位置から
ベクトルの成分及び距離ｈに着目して、ひねり軸16と
曲げ軸15との交点18の位置座標値を求めることまでは容
易にできるが、これより更にさかのぼつて回転軸14の位
置、ないしは前腕13の先端の位置を求めることは困難で
ある。

なお、第４図に示す形のロボツト機構20のように、ロ
ボツト機構10とほとんど同じ機構構成であつても、回転
軸14と曲げ軸15の空間距離、即ちオフセツト量が０であ
れば、逆座標変換式は比較的容易に求められる。すなわ
ち、この場合、回転軸14の主軸14A、曲げ軸15の主軸15
A、ひねり軸16の主軸16Aは、１点19で常に交わるためで
ある。具体的に示すと、この場合（37）式（39）式は次
の形となる。

P_x＝C₁［C₂l₁＋S₂₃l₂＊＋C₂₃l_s＊＊］ ＋S₁S₄S₅l_h ……（37）′ P_y＝S₁［C₂l₁＋S₂₃l₂＊＋C₂₃l_s＊＊］ −C₁S₄S₅l_h ……（38）′ P_z＝S₂l₁−C₂₃l₂＊＋S₂₃l_s＊＊＋l₀ ……（39）′ ここに、 l_s＊＊＝l_s＋C₄S₅l_h また、（40）〜（45）式は同一である。このとき、ま
ず、点19の位置（Q_x,Q_y,Q_z）は、 Q_x＝P_x−a_xh ……（46） Q_y＝P_y−a_yh ……（47） Q_z＝P_z−a_zh ……（48） として求められる。

また、明らかに幾何学的関係から、 次に、上腕12（主軸12A）の回転中心12Bから、手首の
点19に引いた直線と、基準座標系ΣＡのxy平面とのなす
角度をθ_ｃとすれば、 であり、更に、上腕12、前腕13を含む平面と前腕軸13A
との交点を13Bとして、点12B,点19,点13Bとによつて形
成される三角形に余弦定理を適用することによつて また、同様に ただし、 あるいは、 として、θ₁,θ₂,θ_３が求められる。

さらに、θ_４は２つの解を持ち、 また、 となる。以上により、第４図に示すような機構構成のロ
ボツトに関しては、解析解として「逆座標変換式」が導
出できることがわかる。

さて、問題をもとにもどして、第２図及び第３図のロ
ボツト機構10については、上記（46）〜（58）式のよう
に、「逆座標変換式」を求めることができない。そこ
で、前述のようにヤコビ行列を利用した方法を用いるこ
とが必要となる。本ロボツト機構10のヤコビ行列は、次
の（62）式のようになる。ただし、v_x,v_y,v_zはそれぞれ
手先座標系ΣＢのx,y,z軸方向の手先作業点の並進の速
度成分であり、またw_x,w_y,w_zは同様にそれぞれ手先座標
系ΣＢのx,y,z軸まわりの手先作業姿勢（手先座標系Σ
Ｂ自体）の回転速度成分である。

なお、明らかに であり、w_x,w_y,w_z、即ち回転速度成分の基準座標系ΣＡ
による表現と手先座標系ΣＢによる表現との関係につい
ても同様である。

また、さらに である。

ここに、 J₁₁＝−Ｒ（S₄C₅C₆＋C₄S₆） ＋C₄l_w（C₂₃（C₄C₅C₆−S₄S₆）−S₂₃S₅C₆） J₂₁＝−Ｒ（−S₄C₅C₆＋C₄S₆） ＋C₄l_w（C₂₃（−C₄C₅C₆−S₄S₆）＋S₂₃S₅C₆） J₃₁＝−RS₄S₅＋C₄l_w（C₂₃C₄S₅＋S₂₃C₅） J₄₁＝S₂₃（C₄C₅C₆−S₄S₆）＋S₂₃S₅C₆ J₅₁＝S₂₃（−C₄C₅C₆−S₄S₆）−S₂₃S₅C₆ J₆₁＝S₂₃C₄C₅−C₂₃C₅ J₁₂＝（S₃（C₄C₅C₆−S₄S₆）＋C₃S₅C₆）l₁ ＋（C₄C₅C₆−S₄S₆）l₂＋S₅C₆（l_s−S₄l_w） J₂₂＝（S₃（−C₄C₅C₆−S₄S₆）−C₃S₅S₆）l₁ −（C₄C₅S₆＋S₄C₆）l₂−S₅S₆（l_s−S₄l_w） J₃₂＝（S₃C₄S₅−C₃C₅）l₁＋C₄S₅l₂−C₅（l_s−S₄l_w） J₄₂＝S₄C₅C₆＋C₄S₆ J₅₂＝−S₄C₅S₆＋C₄C₆ J₁₃＝（C₄C₅C₆−S₄S₆）l₂ ＋S₅C₆（l_s−S₄l_w） J₂₃＝（−C₄C₅S₆−S₄C₆）l₂ −S₅S₆（l_s−S₄l_w） J₃₃＝C₄S₅l₂−C₅（l_s−S₄l_w） ただし、Ｒ＝C₂l₁＋S₂₃l₂＋C₂₃（l_s−S₄l_w） また、簡単のため、l₀＝l_h＝０とした。（このように
しても問題の本質は不変である。即ち、l₀により、一般
化座標系の原点が平行移動されるのみであり、またl_hの
影響に対しては、ちようど（46）〜（48）式においてｈ
＝l_hとおくことにより、手先位置、姿勢から直ちにひね
り軸16と曲げ軸15の交点位置Ｑ＊を求めることが可能で
あるからであり、実際の手先作業点18における速度、角
速度を上記Ｑ＊点における速度、角速度に変換すれば、
上記（62）式がそのまま適用できるのである。なお、第
２図，第３図の例では手先作業点18をひねり軸16上にと
つているが、これも点Ｑ＊との相対的関係が既知であれ
ば任意の点に設定しうることは、上記の論議と同様であ
ることを付記しておく。） さて、このヤコビ行列Ｊ（）は ＝（λ₁,λ₂,λ₃,λ₄,λ₅,λ_６）^Ｔ ……（63） ＝（v_x,v_y,v_z,w_x,w_y,w_z）^Ｔ ……（64） として、 ＝Ｊ（）・ ……（65） であるから、 ＝J^-1（）・ ……（66） としてが求められる。しかし、これをこのままの形で
解くことはやや煩雑であり、かつ本発明の方法ではの
要素をすべて求まる必要がないので、次のような手法を
用いる。即ち、ヤコビ行列の要素に着目し、必要とする
の要素以外の要素に関する項に、なるべく０が多くな
るような形に変形する。ここで、必要とするの要素と
は、ヤコビ行列を用いて求めるべきの要素である。

前述したように、本発明はロボツトの手法作業工具17
の作業点18における位置及び姿勢を規定する６パラメー
タのうち正確に制御する必要のないパラメータ、従つて
誤差を許容するパラメータを少くとも１つ選び出し、こ
れを考慮してヤコビ行列から求めた近似解を用いて制御
を行う少くともロボツトの自由度と、この近似解を利用
して正確に制御すべきパラメータを満足するように求め
た厳密解を用いて制御を行う残りのロボツトの自由度と
を区別することを特徴としている。

ここで、例えばロボツトの適用作業として、作業工具
17の作業点18のたどるべき経路に対して要求される精度
に比して、作業姿勢に対して要求される精度が低くて良
い場合を考える。これは例えばアーク溶接作業、シーリ
ング作業、塗装作業、スポツト溶接作業など、通常産業
用ロボツトの対象とする作業の多くの場合に対してあて
はまると言つて良い。このとき、ロボツトの手先作業点
18の位置は正確に、また姿勢についてはある程度ラフ
に、言いかえれば近似誤差を許すものとして制御するこ
とになる。一方、このことをロボツトの自由度にあては
めて考えると、旋回θ_１、上腕θ_２、前腕θ_３は主とし
て手先作業点18の位置を、また手首の回転θ_４、曲げθ
_５、ひねりθ_６は主として手先作業姿勢を、それぞれ制
御するための機能をはたす自由度であるととらえること
ができるものと考えれば、上記の作業仕様に照らし合わ
せて、θ₄,θ₅,θ_６の３つを近似解として求めるべき自
由度、またθ₁,θ₂,θ_３の３つの自由度変位を（手先作
業点の位置に対する）厳密解として求めるべきものと分
類することが一つの方法としてありうる。

この場合、ヤコビ行列の並行列を求めることは得策で
はなく、必要な部分のみを用いて、θ₄,θ₅,θ_６に対応
するλ₄,λ₅,λ_６のみを求めればよい。このことに着目
すれば、例えば次のような方策が有効である。まず、
（62）式のヤコビ行列を のように３×３の小行列に分割した時、何らかの形でJ
_AL及びJ_BLの部分の要素に０が多くなるように変形し、
行列J_AL,J_BLの計６つの行（各３要素）のうちの３つ
が、０のみの要素で構成されるように変形できれば、結
局λ₄,λ₅,λ_６に関する３つの方程式が得られ、これら
を解くことによつてλ₄,λ₅,λ_６が得られる。この手順
については本発明の本質的内容ではないので詳述しない
が、例えば、λ_２＊＝λ_２＋λ_３とし、かつ とおけば、（６）式は、 ここに⁵ J₁₁＝−RS₄C₅＋C₄l_w（C₂₃C₄C₅−S₂₃S₅）⁵ J₂₁＝−RC₄−C₄l_wC₂₃S₄ ⁵ J₃₁＝−RS₄C₅＋C₄l_w（C₂₃C₄C₅＋S₂₃S₅）⁵ J₄₁＝S₂₃C₄C₅＋C₂₃S₅ ⁵ J₆₁＝S₂₃C₄C₅−C₂₃C₅ ⁵ J₁₂＝（S₃C₄C₅＋C₃S₅）l₁ ⁵ J₃₂＝（S₃C₄C₅−C₃S₅）l₁ ⁵ J₁₃＝C₄C₅l₂＋S₅（l_s−S₄l_w）⁵ J₃₃＝C₄C₅l₂−S₅（l_s−S₄l_w） のように変形される。即ち、手先の位置、姿勢の変化速
度を、ひねり軸16の変位θ_６＝０のときの状態に固定さ
れた座標系、言いかえれば、曲げ軸15上に対して固定さ
れた座標系に変換して表わしたことに相当する。主とし
てこのような手法を用いてヤコビ行列を変換して行け
ば、最終的に次のような手順によりλ₄,λ₅,λ_６が得ら
れる。⁴ v_z＝⁵v_zS₅−v_zC₅＋l_w（⁵w_xC₅＋w_zS₅） ……（69）⁴ w_z＝⁵w_xS₅−w_zC₅ ……（70）³ w_z＝v_x（C₄C₅C₆−S₄S₆）−v_y（C₄C₅C₆＋S₄C₆） ＋v_zC₄S₅−l_wC₄ ⁴w_z ……（71）³ v_y＝v_x（S₄C₅C₆＋C₄S₆）−v_y（S₄C₅S₆−C₄C₆） ＋v_zS₄S₅＋l_wS₄（⁵w_xC₅＋w_zS₅） ……（72）³ w_x＝w_x（C₄C₅C₆−S₄S₆） −w_y（C₄C₅C₆＋S₄S₆）＋w_zC₄S₅ ……（73）³ w_y＝w_x（S₄C₅C₆＋C₄S₆） −w_y（S₄C₅C₆−C₄S₆）＋w_zS₄S₅ ……（74） さらに、 A₁₁＝³w_y（l₂C₃−l_sS₃）−（³v_xC₃−⁴v_zS³）……（75） A₂₁＝（C₂l₁＋S₂₃l₂＋C₂₃l_s） ・³w_x＋S₂₃・³v_y ……（76） A₂₂＝−（C₂l₁＋S₂₃l₂＋C₂₃l_s） ・⁴w_z−C₂₃・³v_y ……（77） D₁₁＝−S₄（C₂l₁＋S₂₃l₂＋C₂₃l_s） ……（78） D₁₂＝C₄S₅（C₂l₁＋S₂₃l₂＋C₂₃l_s） ＋S₂₃S₄C₅l_w ……（79） D₂₁＝C₄（l₂C₃−l_sS₃） ……（80） D₂₂＝S₄S₅（l₂C₃−l_sS₃）−（C₃C₄C₅−S₃S₅）l_w……（8
1） Ｂ＝D₁₁D₂₂−D₁₂D₂₁ ……（82） これらを用いて、 λ_６＝（D₁₁A₁₁−D₂₁A₂₁）/B ……（83） λ_５＝（D₂₂A₂₁−D₁₂A₁₁）/B ……（84） また、これより となる。即ち、λ₄,λ₅,λ_６が求められた。

なお、ここで示した（67）〜（85）式は、解法の一例
を示しているにすきず、種々の変形した形でも表現しう
るものであることは言うまでもない。なお、ここで示し
た解法の中では、³v_x,³v_yなどの表記を用いたが、これ
らの値は手先速度をリンク13の上に固定した座標系で表
現したものに対応している（今の場合必ずしも変形手順
その他により符号を含めて完全に同一ではない。）この
ことから、ヤコビ行列を解いてロボツト機構の自由度の
変位速度を求めるには、必ずしも上述したような形でな
く、例えばロボツト機構10の前腕13上に固定された座標
形ΣＣなどを用いて、表わされた手先作業点の速度、角
速度とロボツト各自由度変位速度との関係を与えるヤコ
ビ行列（別の表現によるヤコビ行列）を直接求め、これ
をもとに多少の変形によつて所要のロボツト自由度変位
速度を求めれば良いことがわかる。ただし、この詳細に
ついては本発明の目的とやや異る内容となるのでここで
は詳述することはさしひかえる。

ロボツト10の手先作業点18のある時刻t₀における位
置、姿勢と、これより時間Δｔだけ後の手先作業点18の
位置、姿勢とから、これらの差を上記したようなヤコビ
行列を表わすのに適当な座標系の成分として並進、回転
移動量の形で求め、これら速度成分とみなして（ ）〜
（ ）式のようにしてλ₄,λ₅,λ_６を求める。時刻t₀に
おけるロボツト10の対応する自由度変位と、これらか
ら、時刻t₀＋Δｔにおける自由度変位θ₄,θ₅,θ_６の近
似解が求められる。このとき、時刻ｔ＋Δt₀における手
先作業点18の目標位置をX_r,Y_r,Z_rと基準座標系で表わす
ものとすれば、（46）〜（53）式と同様の考え方を用い
て、これら目標位置X_r,Y_r,Z_rを正確に満たすための自由
度変位θ₁,θ₂,θ_３が、下記のように求められる。

l_2D＝l₂＋C₅l_h ……（86） l_SD＝l_s−S₄l_w＋S₄S₅l_h ……（87） l_WD＝C₄l_w＋S₄S₅l_h ……（88） ただし、ここでは、θ₂,θ_３は前腕軸13の位置が、上
腕軸12及び手先作業点18を結ぶ直線よりも上方にある場
合の解を示した。

以上により、手先作業点18の目標位置、姿勢及びロボ
ツト10の状態（自由度変位、手先作業位置、姿勢）とか
ら、目標位置を正確に与え、姿勢に関しては近似誤差を
持つ形でのロボツトの各自由度変位目標値を決定する、
即ちいわゆる「逆座標変換」の機能を果たすための、ヤ
コビ行列を応用した演算制御方式、本発明の一つの実施
例が示されたことになる。

なお、本実施例においては、手先作業点18の位置及び
姿勢のうち、姿勢についてはある程度の誤差を許容しう
るものとして、ロボツトの自由度変位のうちθ₄,θ₅,θ
_６についてはヤコビ行列を用いた近似解を、またθ₁,θ
₂,θ_３については手先作業点18の位置を正確に満足する
ための厳密解を求め、これらによりロボツトを制御する
方式について例示したが、この他にも、例えばθ_６のみ
をヤコビ行列により近似解として求め、θ_１〜θ_５につ
いては厳密解を求める方法など、近似解を用いる自由度
及びその数は任意に選定しうることは言うまでもない。
また、ロボツトの使用目的とは別に、例えば手先作業点
18の姿勢のみを正確に制御し、位置については近似誤差
を許容する場合、あるいは手先作業点18の位置、姿勢を
与える６つのパラメータのうち５つ、あるいは４つ、更
には２つ、１つというように任意の個数のもののみを正
確に制御し、残りのパラメータには近似誤差を許容する
といつた変形例もありうることは言うまでもない。

これらに加えて、本実施例では近似解を求めるロボツ
トの自由度をθ₄,θ₅,θ_６と常に定めていたが、例えば
正確に制御すべき手先作業点18の位置、姿勢パラメータ
を満たし、かつ残りの誤差を許容するパラメータに及ぼ
す影響が最小となる、言いかえれば誤差を許容するパラ
メータに対してもその誤差を極力小さくしうるロボツト
の自由度をロボツトの状態に応じてその都度選択して、
それらの自由度に対しては近似解を用いる、などの方法
も考えられる。また、近似解の求め方として、ロボツト
の現在の状態、即ち動作目標点へ向かう直前のロボツト
の各自由度変位を用いたヤコビ行列により近似解を得る
方法について実施例を示したが、より誤差の少ない近似
解を与えるロボツトの各自由度変位の組を推定して用い
る方法、あるいは、その都度ヤコビ行列を計算する代り
に、ある程度の分割数でロボツトの動作領域の代表点に
おけるヤコビ行列各要素の値などを予め求めたテーブル
を持たせ、この表を線形補間するなどの方法で近似解の
求め方を簡単化するなどの変形例も考えられる。

また、以上の説明においてはロボツトの機構構成とし
て第２図並びに第３図に示すロボツト機構10を例にとり
あげて説明したが、この例に限らず本発明の方式は任意
の機構構成のロボツトに対して適用可能であることは論
を待たない。これは、例えばロボツトの持つ自由度の数
が本実施例に示した６自由度の場合以外においても言え
ることである。

〔発明の効果〕

以上述べたごとく、本発明によれば、ロボツト機構の
構成、自由度配分が複雑であり、手先作業点の位置及び
手先作業点における姿勢を与えるための、ロボツトの各
自由度の変位に対する厳密解を求める演算式、即ち、い
わゆる逆座標変換式を求めることが困難な場合において
も、制御すべき手先作業点の位置、姿勢のパラメータの
すべてに近似誤差を含むようなことなく、正確に制御す
ることの必要なパラメータ（群）を正確に満足すること
の可能なロボツトの動作制御方法を実現することができ
る。また、実施例において例示した式からも明らかな通
り、本発明の制御方式にかかる演算処理量は従来方式と
比較してほとんど同等であり、マイクロコンピユータ等
を用いて実時間制御する場合における演算処理負荷上
も、従来方式と比較して特に問題となることはないとい
う効果をも併せ持つている。

【図面の簡単な説明】

第１図は本発明の概念を示す制御処理流れ図、第２図は
本発明の対象となるロボツト機構の一例を示す外観図、
第３図は第２図のロボツト機構の自由度配置を示す機構
構成図、第４図は従来方法の適用が可能なロボツト機構
の一例を示す機構構成図である。 １……_Ａ、２……、３……J^-1（_Ａ）、４……
_Ｂ、５……Δ、６……Δ_０、７……_B0、８……
_BR、10……ロボツト機構、11……旋回、12……上腕、13
……前腕、14……手首回転、15……手首曲げ、16……手
首ひねり。

Claims (1)

【特許請求の範囲】
1. 【請求項１】複数個の自由度を有し、これら自由度の協
   調動作により手先もしくは手先に取付けられた作業工具
   に所要の動作を行わせてなる産業用ロボツトを用いて、
   該手先もしくは該作業工具に対して相対的に定められる
   手先作業点の空間内における位置及び姿勢を、作業を記
   述するために好適な一般化座標系により表現し、該位置
   及び姿勢情報に対応する上記複数個の自由度の各々の変
   位を演算等の手段により求めることにより、上記手先作
   業点に所要の動作を行わせてなる産業用ロボツトの動作
   制御方法において、上記複数個の自由度のうちの少くと
   も一個の自由度に対しては近似解法を用いて近似的に変
   位を求め、該自由度の近似解を用いて上記手先作業点の
   位置あるいは位置及び姿勢のうち、特に正確に制御を行
   う必要のあるパラメータないしはパラメータ群を満たす
   上記複数個の自由度のうちの残りの自由度に関する厳密
   解を求め、以つて、手先作業点の動作を制御することを
   特徴とする産業用ロボツトの動作制御方法。