JP2972278B2 - 多関節形マニピュレータの関節角制御方法 - Google Patents

Description

【発明の詳細な説明】 （産業上の利用分野） 本発明は、６関節形マニピュレータの指先を作業空間
内の指定された軌道に沿って移動させた場合に必要な各
関節の回転量を精確かつ迅速に制御することができる多
関節形マニピュレータの関節角制御方法の改良に関する
ものである。

（従来の技術） ３次元空間内での任意の位置で任意の方向にマニピュ
レータの先端を向けるために、マニピュレータは最低６
自由度をもつ必要がある。即ち、位置に対して３つ、方
向に対して３つの独立変数を指定することになる。６つ
の関節で構成された６関節形マニピュレータは最も頻繁
に作用されていて、各関節の独立な動きによって、指先
を希望する位置・姿勢に到達させることができる。この
各関節の独立な動きと指先の位置・姿勢との間には、運
動学方程式で既定された非線形の関数関係が存在する。
各関節角を与えると、マニピュレータの指先位置・姿勢
は座標変換行列の概念を用いて簡単に導きだすことがで
きる。（これを順問題と言う。）一方、マニピュレータ
の与えられた位置・姿勢を実現させる関節角の決定は、
（これを逆問題と言う。）連立非線形方程式の根を求め
ることに対応し、概して解析表示に頼ることは不可能な
ことが多い。解法の主流は現在のところ、運動学方程式
を各関節角の近傍で線形化してそのヤコビ逆行列から逐
次反復法で関節角を導き出している。

（発明が解決しようとする課題） しかし、上述の如き従来の方法では、まずヤコビ行列
（通常６×６）の計算が36個の行列要素をもつため、相
当複雑な計算となり、またその行列式が零となるいわゆ
る特異点発生の場合に解を求めることができない。さら
に、線形化の基本特性質から、厳密解に対する初期推定
値をある程度正しく与えておかないと繰り返しの回数が
増えるとともに、解そのものの信憑性が疑わしくなると
言う問題点がある。加えて、逆行列を相当効率よく計算
しても、６次以上の行列計算ではその演算時間が大きな
問題となる。

本発明の目的は上述の如き従来技術の欠点を改善した
多関節形マニピュレータの関節角制御方法を提供するこ
とにある。

（課題を解決するための手段） 本発明では、駆動部に回転ジョイントが使用されたあ
らゆるタイプの６関節形マニピュレータについて、そ逆
問題の計算方法を提起する。６自由度のマニピュレータ
のなかには、６個の関節変数θ_１〜θ_６について、最初
の３つの変数がマニピュレータの指先（手首）位置を決
定し、残りの３つの関数変数が指先の方向を定める目的
で設計されたものがある。これは、手首の位置とその姿
勢が独立に決定できる機構を意味する。６つの関節変数
が複雑に関係しあいながら指先位置と姿勢を決定する機
構に比べれば、構造もさることながら、特に、関節解を
導く操作は極めて単純なものとなる。しかしながら、実
際にはこの種の機構少なく、このことがマニピュレータ
逆問題の計算方法上の困難さにもつながっている。

本発明では、こうした６つの関節が単純に位置決定機
構と姿勢決定機構とに分離できない一般的構造に対し
て、数値解法上便宜的に６つの関節変数のうち３つの独
立変数にのみ着目して、３つの関係式に整理しなおす。
ヤコブ法が６つの変数を同時に扱ったことに比べれば、
３つの関節変数で記述されるこの関係式は遥かに取扱い
が容易である。この関係式に対して、その計算値と実際
に取るべき値との偏差を最小化し、零にもっていくこと
により関節解を導き出す。つまり、６変数体系を３変数
の偏差二乗和で構成される評価関数に置き換えた後は、
この関数を線形近似した最適操作により、関節変位を連
立方程式の解として決定する。ただ、このレベルの計算
では、線形化を一部適用しているために、初期推定値か
らの解の探索範囲は極めて微小な範囲に限られる。その
ために、本発明では、毎回の繰返し計算でこの繰返し巾
（関節角の変位）を直接補正・改善することで通常の線
形理論に基づく計算では望めない初期推定値からの大き
な変位に対しても安定に解を得る効果的方法を与えた。
以前に公表した最小化手法（佐々木忍、「関節変数の分
離に基づく逆運動学の解法」、計測自動制御学会論文
集、Vol.26,No.6,685/691（1990）参照）は根元の構造
に着目して４変数形として定式化を行ったが、本手法は
より少ない変数系で処理できるモデルとして一般的な６
関節形マニピュレータに適用可能な特徴を備えている。

（実施例） 本発明の制御方法を６関節形マニピュレータに適用し
た場合の実施例について詳細に説明する。

先ず、第１図を参照すると、本発明を適用した６関節
形マニピュレータが示されている。このマニピュレータ
はベース10に連結され、６つのリンク11、12、13、14、
15、16と６つの関節21、22、23、24、25、26とを備えて
いる。これら関節は回転又は旋回可能である。

次に、以下で使用する記号について下記の約束をす
る。二重添字で表記した記号の内、偏導関数については
（∂F_j/∂θ_ｋ）＝F_jk,（∂θ_j/∂θ_ｋ）＝θ_jk,（∂P_x
/∂θ_ｊ）＝P_xj等と略記する。その他、s_i＝sinθ_i,c_i
＝cosθ_i,s_ij＝sin（θ_ｉ＋θ_ｊ）,c_ij＝cos（θ_ｉ＋θ
_ｊ）:n＝（n_x,n_y,n_z）^T,o＝（o_x,o_y,o_z）^T,a＝（a_x,a_y,
a_z）^T;マニピュレータの指先姿勢を決定する３方向単位
ベクトル（Ｔは転置記号）である。:P_x,P_y,P_z;基準座標
系における指先の位置座標を表す。

さて、６自由度を前提とする一般的なマニピュレータ
運動学はつぎの方程式系で表される。

F_i＝F_i（θ₁,θ₂,……，θ_６）（ｉ＝1,……,6）
（１） この内、６個の独立変数θ_１〜θ_６に対して３つの方
程式F₁,F₂,F₃が指先の姿勢を、また残り３つが指先の位
置を定める。いま、（１）の３式に含まれた独立変数の
なかから、３つの変数を取りだし残りを従属関係とみな
したうえで、これを（１）の残り３式と関連づけて評価
関数の最小値から関節解を決定する。計算過程では、第
1,第2,第３関節変数θ₁,θ₂,θ_３（以下基準変数と呼
ぶ。）を固定して残りの関節変数をこの３つで表現する
ことにする。すなわち、（１）の左辺を既知として姿勢
についての３式F₁,F₂,F₃を整理して陰関数θ₄,θ₅,θ_６
を θ_４＝H₁（θ₁,θ₂,θ_３） θ_５＝H₂（θ₁,θ₂,θ_３） （２） θ_６＝H₃（θ₁,θ₂,θ_３） なる陽的な表示を行う。（陰関数の存在を仮定する。）
これを（１）のF₄,F₅,F₆と関係づけると、指先位置が基
準変数のみで表せる。この３変数から構成される位置方
程式の解を、現在位置と目標値との偏差に着目した下記
の評価関数の極値探索から誘導する。

（ただし、F_j は指先での目標値とする。） まず、（３）の偏差２乗和を最小すなわち零にする関
節解は、各因子Φ_ｊ（θ₁,θ₂,θ_３）を与えられた指先
位置に十分近いと推定される関節変数の初期値θ_１゜，
θ_２゜，θ_３゜の近傍でつぎのように線形近似して引出
す。

F_j＝（θ_１゜＋Δθ₁,……θ_６゜＋Δθ_６）−F_j゜ ＝F_j1Δθ_１＋F_j2Δθ_２＋F_j3Δθ_３＋ F_j4｛θ₄₁Δθ_１＋θ₄₂Δθ_２＋θ₄₃Δθ_３｝＋ F_j5｛θ₅₁Δθ_１＋θ₅₂Δθ_２＋θ₅₃Δθ_３｝＋ F_j6｛θ₆₁Δθ_１＋θ₆₂Δθ_２＋θ₆₃Δθ_３｝ ＝｛F_j1＋F_j4θ₄₁＋F_j5θ₅₁＋F_j6θ₆₁｝Δθ_１＋ ｛F_j2＋F_j4θ₄₂＋F_j5θ₅₂＋F_j6θ₆₂｝Δθ_２＋ ｛F_j3＋F_j4θ₄₃＋F_j5θ₅₃＋F_j6θ₆₃｝Δθ_３ ＝Φ_ｊ（θ_１゜＋Δθ₁,θ_２゜＋Δθ₂,θ_３゜＋Δθ_３）−Φ_ｊ゜ ＝Φ_j1Δθ_１＋Φ_j2Δθ_２＋Φ_j3Δθ_３（ｊ＝4,5,6） （４） 但し、F_j゜＝F_j（θ_１゜，…，θ_６゜）＝Φ_ｊ（θ_１
゜，θ_２゜，θ_３゜）＝Φ_ｊ（θ_０）＝Φ_ｊ゜（ｊ＝4,
5,6）したがって、（４）と目標値との偏差２乗和を最
小にするΔθ_ｉ（ｉ＝1,2,3）が連立方程式の解として
反復操作により決りその結果関節解が求まる。

以下、第１図に示したマニピュレータについて解法を
説明する。この運動学方程式を（５）〜（16）に記載す
る。

n_x＝−（s₁s₂₃c₅＋s₄s₅c₁＋s₁s₅c₂₃c₄）c₆＋s₆（c₁c₄−s₁s₄c₂₃） （５） n_y＝（s₂₃c₁c₆−s₁s₄s₅＋s₅c₁c₂₃c₄）c₆＋s₆（c₁c₄−s₄c₁c₂₃） （６） n_z＝（c₂₃c₅−s₂₃s₅c₄）c₆−s₂₃s₄s₆ （７） o_x＝（c₁c₄−s₁s₄c₂₃）c₆＋s₆（s₁s₂₃c₅＋s₄s₅c₁＋s₁s₅c₂₃c₄） （８） o_y＝（s₁c₄−s₄c₁c₂₃）c₆−s₆（s₂₃c₁c₅−s₁s₄s₅＋s₅c₁c₂₃c₄） （９） o_z＝−（s₂₃s₄）c₆＋s₆（s₂₃s₅c₄−c₂₃c₅） （10） a_x＝s₁s₂₃s₅−s₁c₂₃c₄c₅−s₄c₁c₅ （11） a_y＝c₁c₂₃c₄c₅−s₂₃s₅c₁−s₁s₄c₅ （12） a_z＝−（s₂₃c₄c₅＋s₅c₂₃） （13） P_x＝−a₆（s₁s₂₃c₅＋s₄s₅c₁＋s₁s₅c₂₃c₄）c₆＋a₆s₆（c₁c₄−s₁s₄c₂₃）− a₅（s₁s₂₃c₅＋s₄s₅c₁＋s₁s₅c₂₃c₄）−（a₃a₄）s₁s₂₃−a₂s₁s₂ （14） P_y＝a₆（s₂₃c₁c₅−s₁s₄s₅＋s₅c₁c₂₃c₄）c₆＋a₆s₆（s₁c₄＋s₄c₁c₂₃）＋ a₅（s₂₃c₁c₅−s₁s₄s₅＋s₅c₁c₂₃c₄）＋（a₃＋a₄）s₂₃c₁＋a₂s₂c₁ （15） P_z＝a₆（c₂₃c₅−s₂₃s₅c₄）c₆−a₆s₆（s₂₃s₄）＋a₅（c₂₃c₅−c₂₃s₅c₄）＋ （a₃＋a₄）c₂₃＋a₂c₂＋a₁ （16） まず、（11），（12）からs₁,c₁について整理した共
通項を基準変数を使ってつぎのように記述する。

c₂₃c₄c₅−s₂₃s₅＝a_yc₁−a_xs₁＝w₁（θ₁,θ₂,θ_３） （17） s₄c₅＝−（a_ys₁＋a_xc₁）＝w₂（θ₁,θ₂,θ_３）（＝X₁） （18） また、（13）、（17）から c₄c₅＝w₁c₂₃−a_zs₂₃ （＝X₂） （19） s₅＝−（a_xc₂₃＋w₁s₂₃） （＝X₃） （20） が得られ、関節角θ₄,θ_５と基準変数との関係が導き出
せる。すなわち、（18）、（19）、（20）から 但し、X₂＝w₁c₂₃−a_xs₂₃≠０ （22） 但し、X₁＝−（a_ys₁＋a_xc₁）≠０ （24） が直ちに求まる。残るはθ_６で、これを含む式として 例えば（５）のc₆,s₆の係数 Ａ＝s₁s₂₃c₅＋s₄s₅c₁＋s₁s₅c₂₃c₄ （25） Ｂ＝c₁c₄−s₁s₄c₂₃ （26） に着目する。このA,Bは上で求めた関節角の値から計算
できるが、また（８）と関連づけると Ａ＝−n_xc₆＋o_xs₆ （27） Ｂ＝n_xs₆＋o_xc₆ （28） とも表現できることから、 但し、X₅＝Bo_x−An_x≠０ （30） と表される。

このように、姿勢の関係式からθ₄,θ₅,θ_６が基準変
数で表されたので、つぎに（４）に従ってマニピュレー
タの指先位置P_x,P_y,P_zを初期値θ_０の近傍で線形近似す
る。すなわち、 P_x（θ_０＋Δθ）＝P_x（θ_０）＋L₁Δθ_１＋L₂Δθ_２＋L₃Δθ_３ P_y（θ_０＋Δθ）＝P_y（θ_０）＋M₁Δθ_１＋M₂Δθ_２＋M₃Δθ_３ （31） P_z（θ_０＋Δθ）＝P_z（θ_０）＋N₁Δθ_１＋N₂Δθ_２＋N₃Δθ_３ （但し、θ_０＝（θ_１゜，θ_２゜，θ_３゜）とする。） ここに、微小変位の各係数L_i,M_i,N_iは、つぎのように定
まり、また（∂P_x/∂θ_ｊ）などは（35）を用いる。

P_x1＝−P_y P_x2＝−Ｇ（d₃,k₂,−a₆s₆,s₂₃s₄,d₂,1）s₁ P_x3＝P_x2＋a₂s₁c₂ P_x4＝−Ｇ（d₃,k₅s₅,a₅s₆,k₇,0,0） P_x5＝−Ｇ（d₃,k₉,0,0,0,0） P_x6＝Ｇ（a₆,ox,0,0,0,0） P_y1＝P_x P_y2＝−P_x2c₁/s₁ （35） P_y3＝P_y2−a₂c₁c₁ P_y4＝−Ｇ（d₃,k₆s₆,a₆s₆,s₁s₄−c₁c₂₃c₄,0,0） P_y5＝Ｇ（d₃,ay,0,0,0,0） P_y6＝Ｇ（a₅,oy,0,0,0,0） P_z1＝０ P_z2＝−Ｇ（d₃,k₄,a₆s₆,s₄c₂₃,d₁,1） P_z3＝P_z2＋a₂s₂ P_z4＝Ｇ（d₃,s₄s₅,−a₆s₆,c₄,0,0）s₂₃ P_z5＝−Ｇ（d₃,k₃,0,0,0,0） P_z6＝Ｇ（a₆,oz,0,0,0,0） 但し、関数Ｇはつぎのように定める。

Ｇ（a,b,c,d,e,f）＝ab＋cd＋ef （36） 以上でマニピュレータの指先位置の基準変数に関する
線形化を行った。そこで、（31）の各計算値と与えられ
た指先目標値との偏差を２乗ノルムで表し、この値を１
つの収束判定基準が満たされるまで更新を続け所要の関
節解を導く。目的の評価関数は Ｊ={P_x(θ_０+Δθ)-P_x }^２+{P_y(θ_０+Δθ)-P_y}^２+{P
_z(θ_０+Δθ)-P_z }^２ （37） となり、これを展開してつぎのようになる。

ここに、m₁＝P_x（θ_０）−P_x ,m₂＝P_y（θ_０）−P_y ， m₃＝P_z（θ_０）−P_z ,M＝m₁ ²＋m₂ ²＋m₃ ³ （39）P_x, P_y, P_z,は指先の目標位置。

したがって、関数Ｊを極小にする各関節変数の微小変
位は、（38）の導関数を零におくこと（∂J/∂Δθ_ｊ）
_０＝０（ｉ＝1,2,3）でつぎの３元連立方程式の解とし
て求まる。

但し、 R₁₁＝Ｇ（L₁,L₁,M₁,M₁,N₁,N₁） R₁₂＝Ｇ（L₁,L₂,M₁,M₂,N₁,N₂）＝R₂₁ R₁₃＝Ｇ（L₁,L₃,M₁,M₃,N₁,N₃）＝R₃₁ R₂₂＝Ｇ（L₂,L₂,M₂,M₂,N₂,N₂） （41） R₂₃＝Ｇ（L₂,L₃,M₂,M₃,N₂,N₃）＝R₃₂ R₃₃＝Ｇ（L₃,L₃,M₃,M₃,N₃,N₃） b₁＝−Ｇ（m₁,L₁,m₂,M₁,m₃,N₁） b₂＝−Ｇ（m₁,L₂,m₂,M₂,m₃,N₃） b₂＝−Ｇ（m₁,L₃,m₂,M₃,m₃,N₃） 方程式の解はガウス・ザイデル法や掃きだし法などで
簡単に求めることができる。算出された各増分値Δθ₁,
Δθ₂,Δθ_３により関節角の初期推定値θ_ｉ゜が θ_ｉ＝θ_ｉ゜＋Δθ_ｉ（ｉ＝1,2,3） （42） と更新されるとともに、（21），（22），（29）から残
りの関節角が決定する。この時点で、関節解を線形化し
ない原式（３）に代入してノルムの大きさを再計算す
る。その大きさが指定された収束半径内にあれば解とし
て採択され、そうでない場合は、（42）のθ_ｉを初期値
として一連の操作を最初に戻って繰返す。以上が計算手
順の概要である。いくつかの数値実験によりこのアルゴ
リズムの性能を評価すると、指先の位置と目標値との隔
たりが10cm以内にあればたかだか４回以内で収束を達成
することを示した。勿論これは安定な挙動を与えた例で
あって、マニピュレータの任意姿勢に対して与える初期
値や評価関数の非線形特性に依存して収束状況は変り得
る。線形理論に基づいた解探索法ではこのように適用範
囲が自ずと規定されてしまうので、それを拡大させて利
用できれば重宝である。現在のアルゴリズムでも初期変
位を大きくとれば当然数値的不安定性を招き発散してし
まう。そこで、線形領域から逸脱する関節変位について
も解を安定に得るための検討を行った。繰返し計算で
は、最初のステップにおけるΔθの大小が以後の解挙動
への鍵を握っているので、この初期段階における相対的
大きさをある程度抑え、適切に改善・補正することが有
力と考えた。結果が示すように、この操作後はΔθの値
が多少大きくても最小化手法が有効に働いて目標値との
隔たりをさして障害とせずに目標値へむけて急速に収束
する傾向が示された。つまり、こうした数値的発散傾向
にブレーキをかける操作は、従来の線形近似による解の
探索能力を向上させる突破口として意義がある。具体的
には、関節変位をつぎのように補正して計算を行った。

ｉ）初期変位が300（mm）以内の場合（ｉ＝1,2,3） Δθ_ｉ＝Δθ_i/1.5（｜Δθ_i|＞（deg）） ii）それを越える場合 Δθ_ｉ＝Δθ_i/1.5(10deg)＞｜Δθ_i|＞１(deg)) Δθ_ｉ＝Δθ_i/2.0(30deg)＞｜Δθ_i|＞10(deg)) Δθ_ｉ＝Δθ_i/3.0（｜Δθ_i|＞30（deg）） その結果を第２図にまとめる。初期推定値として図の
A,B,Cは、１つの関節値を変動させ他を基準値に固定し
た３つの計算カテゴリーで、X,●，□の印で表す。その
上の数値は収束回数を示す。例えば、点Ｐは基準値から
θ_１を30（deg）、点Ｑはθ_２を−20（deg）、点Ｒはθ
_３を−50（deg）変動させたサンプルで、縦軸に示した
指先目標値との隔たりＤに対して収束解がそれぞれ4,4,
6回で得られたことを表している。補正前に成立した斜
線領域に比べどのケースも線形化モデルの制限枠を大き
く改善している。計算開始時に目標値との隔たりが約35
0mm以内にあれば、この補正により評価関数の各項を構
成する偏差が単調に零に漸近することを示した。つぎ
に、各関節を同時に変化させた場合のテスト計算（表１
参照）では10回以内に安定な解が得られた。本モデルは
相当に大きな角度変位に対しても十分耐えて、指先での
隔たりが400（mm）以内で正常解が引出された。尚、収
束した解の精度は厳密解と対比して小数点以下３位まで
一致していた。

以上、最小化手法、線形近似、補正操作など一連の取
扱いを通して首尾よく解が求められた。発散要因となる
初期変位の大きさについて直接制御しさえすれば正常解
に到達できる見通しが示唆された。例示したように、指
先目標値と30〜50cmの隔たりに対して精度0.1mm、反復
回数10回内で達成できれば十分実用に供すると考えられ
る。

（効 果） 本発明の効果は以下の通りである。

（１） ６変数を一挙に取扱う煩雑さを回避するため、
独立変数を形式的に３つにへらし偏差２乗からなる評価
関数と関連づけ、線形最適化手法とカップリングさせた
解の誘導手順は、極めて単純である。

（２） 初期の推定値が線形化の常識を超えた場合で
も、Δθの補正を行うことで極値探索による手法が効果
的に寄与して（反復回数を増加することなく）高精度な
解が期待できる。

（３） 本概念は原理的にあらゆるタイプの６リンク・
マニピュレータに適用可能である。

（４） ヤコビアン計算に比べ処理が容易であるが、陰
関数が成立しない条件は念頭におく必要がある。線形化
の宿命である初期値依存性や微小領域での取扱いなどヤ
コビ手法で経験する類似の問題点を最適化と変位補正に
より高精度な解に到達できたことは利点であろう。

【図面の簡単な説明】

第１図は本発明を適用した６関節形マニピュレータの斜
視図、第２図は本発明の方法を用いて計算した結果、線
形モデルに繰返し巾を改善することで大きな変動に対し
ても解が引出せることを示すグラフである。 10……ベース,11〜16……リンク， 21〜26……関節。

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】回転又は旋回可能な６つの関節を有する多
   関節形マニピュレータの関節角を制御する方法であっ
   て、前記６つの関節間の複数変数を含む運動方程式を３
   つの独立変数のみの体系に整理し、目標点と現在点との
   距離を最小かつ零にする繰返し計算を行い、該繰返し計
   算の繰返し幅を毎回補正して前記関節角を決定すること
   を特徴とする多関節形マニピュレータの関節角を制御す
   る方法。