JPH07146703A - 多関節リンク機構の順運動学解析法及び逆運動学解析法 - Google Patents

Abstract

(57)【要約】 【目的】 多関節リンク機構の順運動解析と逆運動解析
が能率よく行えるようにする。 【構成】 複数のリンクが回転又は並進機能をもつ関節
により直列連鎖状に連結された多関節リンク機構につい
て、隣接リンク間の機構学的関係を示す関係式と最終リ
ンクの位置及び姿勢が与えられたときに、非線形最小二
乗法に用いるＧａｕｓｓ−Ｎｅｗｔｏｎ法に非線形特性
を模擬する補正係数λを導入し、目的関数が単調減少す
るよう補正係数を変えつつ反復計算を行い、大域的に実
現可能な関節角度を誘導する。これにより、初期値が真
値からある程度ずれている場合でも、或いは非線形性の
強い方程式に対しても、発散振動等を招くことなく数値
的に安定な解を求めることが可能になる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、任意の多関節構造をも
つリンク機構について、関節角度からの指先位置・姿勢
の計算及び指先位置・姿勢からの関節角度の計算が、正
確かつ迅速に行えるようにした多関節リンク機構の順運
動学解析法及び逆運動学解析法に関する。

【０００２】

【従来の技術】多関節形リンク機構からなるロボット・
マニピュレータにおいて，どの関節をどの程度動かせば
指先がどの位置にくるかを直感的に知ることは容易でな
い。通常この種の指先の位置・姿勢問題は、現在の腕の
姿勢から各関節の回転或いは並進量を知って座標変換行
列を利用して計算する。その計算は関節の自由度が増大
するにつれ極めて煩雑化し、簡単な構成のマニピュレー
タを除き、機構方程式の誘導には多大な労力を要する。
また、仮に解析対象の機構方程式が確立されても、次に
指先を必要な位置・姿勢にもっていくためには、各関節
角度をどのように変化させるべきかというより困難な問
題に遭遇する。ロボット・マニピュレータの動きは、常
にこうした順問題と逆問題を念頭におく必要があるが、
行列積をいくつか直接実行することで基本的な機構方程
式を誘導する従来の解析法は、６自由度以上の多関節冗
長機構を取り扱う場合、マニュアル誘導が不可避であっ
た。また、従来の逆問題の解析法は、基本方程式を線形
化してＮｅｗｔｏｎ法によって近似的な関節角度を求め
ていたが、この方法は、一般に求める解の極めて近傍で
しか適用できない欠点をもち、解の探索範囲を広げると
発散振動を招いたりして、数値的に安定な解を求めるこ
とが困難であった。さらに、汎用的取扱いができないこ
とから、解析対象となるロボットごとに機構関係式を用
意して個々に計算しなければならないなどの欠点があっ
た。

【０００３】

【発明が解決しようとする課題】関節・リンクを直列連
鎖状に連結したロボット・マニピュレータの運動解析
が、各関節変位（回転，旋回および並進）を与えて指先
の位置・姿勢を求める順運動学解析と、逆に指先の位置
・姿勢からこれを実現するための各関節角を決定する逆
運動学解析に大別できることは、前に述べた通りであ
る。順運動学解析は、Ｄｅｎａｖｉｔ−Ｈａｒｔｅｎｂ
ｅｒｇの考えを基礎にＰａｕｌによる同次座標変換行列
により原理的に計算可能なことが明らかにされている。
しかし、実際の計算ではこの変換行列を自由度の数だけ
乗じ、計算機を用いた計算か又は簡単な機構構成の場合
は手計算により指先位置・姿勢を関節関数を含んだ関係
として誘導しており、その計算過程には三角関数を含ん
だ関係式が何回も出現するため、取扱いが非常に面倒で
あり、計算ミスを侵しやすいものであった。また、仮に
そうした指先の位置・姿勢に関する機構方程式が確立で
きたとしても、関節角度を計算する逆運動学解析には、
回転角の正弦・余弦を多数含む連立非線形方程式を解か
ねばならず、解析解が求めにくく、その数値計算も非常
に複雑であった。一般に、解析的アプローチが期待でき
るのは持定の構造をもつリンク機構に限られていた。

【０００４】ところで、在来法として定着しているＮｅ
ｗｔｏｎ法は、線形近似したヤコビ行列から逐次反復計
算により位置・姿勢に関する関節角度を近似的に求める
方法であるが、この方法はアルゴリズムが単純であると
いう利点がある反面、一般的には求める解の近傍でしか
適用できない能力的限界があり、探索域を広げると収束
の保証がないといった課題を抱えていた。また、ヤコビ
行列が零となるいわゆる特異点の発生で行列のランク落
ち（退化の問題）を招来してしまい、指先がある方向ヘ
動けなくなる事態に至る欠点があった。さらに、非線形
特性の強い状況下では収束回数を増やしても所望の精度
をもつ関節角度が探索しずらいなど、解を導く上で改善
すべき点を多々抱えるものであった。

【０００５】本発明は、このような従来の計算上の欠点
や弱点を見直し、汎用的なマニピュレータ機構の制御に
適用できる定石となり得る処理能力にすぐれた解析法を
提供することを目的とするものであり、特に一般家庭に
普及しているパーソナルコンピュータ又は同程度の計算
能力をもって十分に所期の性能が発揮できる簡潔なソフ
トウェアを提供することを目的とするものである。

【０００６】

【課題を解決するための手段】本発明は、複数のリンク
が回転又は並進機能をもつ関節により直列連鎖状に連結
された多関節リンク機構について、原始リンクから最終
リンクまでの個々のリンクの位置及び姿勢を、隣接リン
ク間の機構学的関係を示す漸化式からなる再帰関係式に
より規定し、各関節角度が与えられたときに前記再帰関
係式に基づいて逐次演算を実行し、原始リンクから順に
最終リンクまで個々のリンクの位置及び姿勢を割り出す
ことを特徴とする多関節リンク機構の順運動学解析法を
提供することにより、前記目的を達成するものである。

【０００７】また、本発明は、複数のリンクが回転又は
並進機能をもつ関節により直列連鎖状に連結された多関
節リンク機構について、隣接リンク間の機構学的関係を
示す関係式と最終リンクの位置及び姿勢が与えられたと
きに、非線形最小二乗法に用いるＧＡｕｓｓ−Ｎｅｗｔ
ｏｎ法に非線形特性を模擬する補正係数を導入し、目的
関数が単調減少するよう前記補正係数を変えつつ反復計
算を行い、大域的に実現可能な関節角度を誘導すること
を特徴とする多関節リンク機構の逆運動学解析法を提供
することにより、前記目的を達成するものである。

【０００８】さらにまた、本発明は、６自由度を越える
冗長な関節構造を有する多関節リンク機構について、隣
接リンク間の機構学的関係を示す関係式と最終リンクの
位置及び姿勢が与えられたときに、まず直接探索法によ
り収束領域を限定する予備計算を行い、該予備計算によ
り得られた粗い近似値を初期値として線形最小二乗法に
よる反復計算を行い、最小二乗最小ノルム解として一意
的に関節角度を誘導することを特徴とする多関節リンク
機構の逆運動学解析法を提供することにより、前記目的
を達成するものである。

【０００９】

【作用】本発明によれば、指先の位置・姿勢を求める煩
わしい計算や関節変数を含む関係式の誘導をすべて計算
機に処理させることができ、従来行列計算によって誘導
していたリンク相互の機構関係を前進型の簡単な漸化式
に置き換え、多自由度機構の計算にも対応できる解析ア
ルゴリズムを提供することができる。また、逆問題を解
く方法として、数理計面や０Ｒの分野で広く活用されて
いる最適化手法の潜在的有用性に着目し、非線形最小二
乗法により効率よく解く解析法を提供することができ
る。また、その場合に、従来の単純な線形化手法では扱
えなかったヘッセ行列の非線形部分を取り入れ、しかも
計算負担の少ない簡単なモデルで模擬することで、解の
安定性や特異点回避を図りつつ大域的な関節角度の誘導
を果たす。さらに、６自由度を越える冗長な関節構造に
対し、シンプレックス法と線形最小二乗計算との混合計
算によりー意的に実現可能な解を決定する。

【００１０】

【実施例】以下、本発明の実施例について、図１ないし
図３を参照して説明する。図１は、本発明の多関節リン
ク機構の逆運動学解析法の一実施例を示すフローチャー
ト、図２は、図１に示した逆運動学解析法の変形例を示
すフローチャート、図３は、６自由度を越える冗長機構
に適用した本発明の多関節リンク機構の逆運動学解析法
の他の実施例を示すフローチャートである。

【００１１】まず、順運動学と逆運動学の根底をなす計
算式すなわち指先位置・姿勢の決定に欠かせぬ機構学的
関係式について説明する。

【００１２】複数(ｎ個)の剛体リンクから構成されるマ
ニピュレータ（多関節リンク機構）は、回転または並進
（スライド）機能をもつ関節を複数のリンク間に結合
（１自由度結合）したものであり、各関節の動きを適宜
制御することで最終リンクに取り付けたハンドの位置決
め操作や対象物のマニピュレーション等を実行させるこ
とができる。ここでは、関節の種類，関節軸の捩れ（捩
れ角α_i），自由度の数，リンクの長さ等、一連の機構
学的属性が任意に設定できるもっとも―般的なロボット
・マニピュレータを対象とする。このマニピュレータの
運動学モデルは、古典力学でよく知られた剛体の位置及
び姿勢を表現する座標系の概念に基礎をおくものであ
る。通常、隣接する関節軸Ｚ_i-1，Ｚ_i上に設定した座標
系Σ_i-1，Σ_iに対して、Ｄｅｎａｖｉｔ−Ｈａｒｔｅｎ
ｂｅｒｇが提唱した同次座標変換行列^i-1Λ_iでは、Σ
_i-1におけるリンクｉの姿勢・位置が次式のように表さ
れる。なお、式中、行括弧（行列の両端を示す縦線シン
ボル）は左上隅括弧”「”と右下隅括弧”」”とに代え
て便宜的に簡略表記してある。

【００１３】 「ｃ_i −ｓ_iγ_i ｓ_iη_i ａ_iｃ_i ^i-1Λ_i＝ ｓ_i ｃ_iγ_i −ｃ_iη_i ａ_iｓ_i （１） ０ η_i γ_i ｄ_i ０ ０ ０ １ 」 （１≦ｉ≦ｎ） ここに、 γ_i＝cosα_i，η_i＝sinα_i （２） (γ_i ²＋η_i ²＝１) であり、いずれもリンクｉとリンク（ｉ＋１）相互の捩
れ具合を表すパラメータである。なお、各関節角をθ_i
としたときに、ｓ_i，ｃ_iは、sinθ_i＝ｓ_i，cosα_i＝ｃ_i
を略記する略記号であり、ａ_i，ｄ_iは、それぞれリンク
の長さとオフセット（直動関節の場合は並進量）を表
す。

【００１４】いま、マニピュレータ本体を支える台座に
基準座標系Σ_oを固定し、そこからみた（ｉ−１）番目
のリンクの先端の位置・姿勢を と記す。なお、この場合も左右の行括弧は便宜的に簡略
表記してある。

【００１５】添字（ｉ−１）をもつ行列要素は，行列
^k-1Λ_kを（ｉ−１）個乗ずる操作から容易に理解できる
ように、第１関節から第（ｉ−１）関節までの関節変数
を含む関数である。この変換行列は、左上に位置するい
わゆる３×３回転行列（方向余弦行列）が姿勢を、また
右端の３×１列ベクトル（Ｘ_i-1，Ｙ_i-1，Ｚ_i-1）^Tが位
置を表している。従って、ｉ番目のリンク先端の位置・
姿勢Ｔ_iは、式（３）に示した行列にｉ番目の関節の回
転（または並進）操作を表す変換行列（１）式を右から
掛けて求まる。その結果、次の関係式が導き出せる。な
お、ここでは、姿勢に関する関係式について、３つの
（単位）列ベクトル要素をまとめて１つの式番号で整理
してある。

【００１６】 Ｐ_i＝Ｐ_i-1ｃ_i＋Ｕ_i-1ｓ_i Ｑ_i＝Ｑ_i-1ｃ_i＋Ｖ_i-1ｓ_i （４） Ｒ_i＝Ｒ_i-1ｃ_i＋Ｗ_i-1ｓ_i Ｕ_i＝−Ｌ_iγ_i＋Ｄ_i-1η_i Ｖ_i＝−Ｍ_iγ_i＋Ｅ_i-1η_i （５） Ｗ_i＝−Ｎ_iγ_i＋Ｆ_i-1η_i Ｄ_i＝Ｌ_iη_i＋Ｄ_i-1γ_i Ｅ_i＝Ｍ_iη_i＋Ｅ_i-1γ_i （６） Ｆ_i＝Ｎ_iγ_i＋Ｆ_i-1γ_i Ｘ_i＝ａ_iＰ_i＋ｄ_iＤ_i-1＋Ｘ_i-1 （７） Ｙ_i＝ａ_iＱ_i＋ｄ_iＥ_i-1＋Ｙ_i-1 （８） Ｚ_i＝ａ_iＲ_i＋ｄ_iＦ_i-1＋Ｚ_i-1 （ｉ＝１，２，・・・，ｎ） （９） ただし、 Ｌ_i＝Ｐ_i-1ｓ_i−Ｕ_i-1ｃ_i Ｍ_i＝Ｑ_i-1ｓ_i−Ｖ_i-1ｃ_i （１０） Ｎ_i＝Ｒ_i-1ｓ_i−Ｗ_i-1ｃ_i ここで、式（３）で表した変換行列の各要素（方向余弦）の初期値を Ｐ_o＝Ｖ_o＝Ｆ_o＝１ Ｑ_o＝Ｒ_o＝Ｕ_o＝Ｗ_o＝Ｄ_o＝Ｅ_o＝０ （１１） と定めたときに、再帰関係式（４）〜（１０）を支持部
（ベース）から指先に向けて逐次実行することで、基準
座標系に関するｎリンク・マニピュレータの指先・姿勢
が、関節角度の関数として決定できることが判る。すな
わち、これまで同時変換行列の積演算から誘導していた
運動学方程式が、上記の簡単な逐次計算によって簡単に
求め得ることになる。ただし、マニピュレータの先端す
なわち指先は解放されているので座標軸の捩れはなく、
γ_n＝１，η_n＝０である。

【００１７】このように、開ループ特性をもつ―般的な
多関節リンク機構に関する順運動学は、（４）〜（１
０）の漸化式により解析でき、すべてのリンクの位置・
姿勢を簡単に算出することができる。また、ここで扱う
繰り返し計算法を用いることにより、従来の面倒な行列
代数の計算から解放され、しかも正確かつ確実に目的達
成が可能である。特に、自由度が１つ増えるごとに指数
的に煩雑さを増す多自由度冗長機構の取扱いに絶大なる
威力を発揮し、また再帰表現を数式処理言語ＲＥＤＵＣ
ＥやＭＡＴＨＥＭＡＴＩＣＡ等に適用することにより、
あらゆるタイプのマニピュレータの運動学方程式をシン
ボリックな記述により自動的に生成することも可能であ
る。実際、汎用リンク・マニピュレータの場合、（１）
式の行列積の演算結果には多くの紙面を費やすのが常で
あり、このことからも上述の再帰関係式がいかに簡潔で
計算機処理に適切であるかが理解されよう。

【００１８】次に、上記順運動学解析に用いた機構表現
を利用し、逆運動学解析すなわち関節角度（アーム解）
の一般的な解法について説明する。

【００１９】ｎリンク・マニピュレータの機構モデル
は、再帰関係式によって与えられたリンクの位置・姿勢
を基本とし、ｎ個の関節変数がつくる関節空間からデカ
ルト空間への非線形写像と考えられる。ここでは、この
逆写像についての取扱い（関節角の決定方法）について
述べる。この場合、関節変数ベクトルをθ＝（θ₁，
θ₂，・・・，θ_n）^T（∈Ｒⁿ），指先位置をＸ
_n（θ）．Ｙ_n（θ），Ｚ_n（θ），指先姿勢をＰ
_n（θ），Ｑ_n（θ），・・・，Ｆ_n（θ）で表した基本
関係式から、下記のようにそれぞれの目標値との残差で
書き直した数学モデルを作る。すなわち、マニピュレー
タの指先位置とその目標値との誤差をｆ₁（θ），ｆ
₂（θ），ｆ₃（θ）とし、姿勢のそれをｆ₄（θ），・
・・，ｆ_m（θ）で表す。以下、各関数ｆ_i（θ）は、特
に明記の要がある場合を除いて簡単にｆ_iと記す。

【００２０】 ｆ₁＝Ｘ_n（θ）−Ｘ_d ｆ₂＝Ｙ_n（θ）−Ｙ_d ｆ₃＝Ｚ_n（θ）−Ｚ_d ｆ₄＝Ｐ_n（θ）−ｎ_x ｆ₅＝Ｕ_n（θ）−ｏ_x ・ ・ （１２） ・ ｆ_m＝Ｆ_n（θ）−ａ_x ここに，Ｘ_d，Ｙ_d，Ｚ_dは指先の目標位置を、またｎ_x，
・・・，ａ_xは目標姿勢を表す。因に、ｆ₄のＰｎは、基
準系Σ_o（Ｘ_o座標軸）からみた指先（Ｘ_n方向）の方向
余弦を表し、ｆ_m（ｍ＝１２）のＦ_nは、基準系Σ_o（Ｚ_o
座標軸）からみた指先（Ｚ_n方向）の方向余弦を表す。

【００２１】さて、上記のモデルに対する評価基準とし
て、ｍ個の（制限条件のない）連立非線形方程式ｆ
（θ）＝｛ｆ₁，ｆ₂，・・・，ｆ_m｝^T＝０ ｛Ｒⁿ→
Ｒ^m｝の要素二乗和で構成される目的関数 を定義し、これを最小にする解θ_minを元の多関節リン
ク機構の近似的な関節角度（アーム解）とみなす。つま
り、解の導出は目的関数Ｍ（θ）：｛Ｒⁿ→Ｒⁱ｝に対す
る制約なしの最適化（又は最小化）問題と考える。

【００２２】ところで、目的関数Ｍ（θ）を構成する各
関数ｆ_iは、θ₁，・・・，θ_nの非線形関数であり、６
個を越える関節変数を含む最小化問題は、アーム全体と
して無限に多くの（解の）組合せが考えられ、極小点の
探索は不可能に近い。

【００２３】従って、一般的連鎖機構を論ずるさいに機
構固有の自由度を考慮し、 １）６自由度（或いはそれ以下の）機構の汎用的な取扱
いと、 ２）条件付加を考慮した多自由度冗長系（ｎ＞６）の取
扱い とに大別して対処することにする。

【００２４】前者は、後述する非線形最小二乗法による
高精度の数値解析法が中心となり、ヘッセ行列のなかの
非線形要素（２次微分項）の情報を利用することでラン
ク落ちが回避できる利点がある。この最適化技法は、６
以下の自由度系の過剰決定の問題をも同時に扱うことが
可能である。一方、冗長機構（ｎ＞６）については、各
関数の線形近似とノルム最小化、特異値分解法を利用し
た一般的解法について後述する。

【００２５】まず、非線形最小二乗法による汎用６リン
ク・マニピュレータの解析には、方程式（１２）の各要
素の二乗和で定義した目的関数（１３）に対する逐次反
復計算を用いる非線形最小二乗法が、収束速度や計算精
度（安定性）の観点からもっとも適切である。この方法
は、これまでにも連立非線形方程式の解法や或いは曲線
の当てはめ、さらには回帰分析など広範囲に利用されて
おり、ここでは汎用的なマニピュレータの逆問題に適用
する。

【００２６】さて、制約条件のない６関節機構の（過剰
決定の状態を含めた）最小化問題について、目的関数Ｍ
（θ）の局所的最適点の候補として、停留点では が成立する。そこで、この連立非線形方程式にＮｅｗｔ
ｏｎ−Ｒａｐｈｓｏｎ法を適用し、 を導く。適当な初期値θ^oを出発点に系列｛θⁱ｝，ｉ＝
０，１，２・・・を逐次更新し、ｋ番目の探索（上付き
添字ｋで表す）における偏差ベクトルΔθ^kをこの方程
式から求めると、新しい点は と定まる。上式の大括弧のなかには関数ｆｉのもつ非線
形性を模擬した第２項が表現されており、これを省略し
ない場合はΔθ^kが最大精度で求められる。

【００２７】ここに、 ∇Ｍ（θ^k）＝［∂Ｍ／∂θ］＝２Ｊ^T(θ^k)ｆ(θ^k)（∈Ｒⁿグラジエント） Ｊ＝｛∂ｆ_i/∂θ_j｝（∈Ｒ^mxnヤコビアン） である。

【００２８】都合のよいことに、この反復法は（１６）
式の大括弧の部分、すなわちヘッセ行列∇²Ｍ(θ)が正
定であるように選ぶことで、探索方向Δθは下記のよう
に常にＭ（θ）の降下方向になる。

【００２９】 (Δθ)^T∇Ｍ(θ)＝−[∇Ｍ(θ)]^T[∇²Ｍ(θ)]^-1[∇Ｍ(θ)]＜０ この計算は、十分よい初期随さえ与えられるならば２次
収束するが、反復ごとに各関数ｆ_iの２次偏微分計算を
必要とし、大きな計算負担を伴うことから必ずしも実用
的とはいえない。そこで、ここでは上記のヘッセ行列な
かに含まれる第２項Σ{ｆ_i(θ)∇²ｆ_i(θ)｝を無視し、
その代わりに他の簡便な方法で非線形特性をモデル化す
る。

【００３０】まず、ヘッセ行列の２次微分（∂²ｆ_i/∂
θ_jθ_k）を削除すると、次のようになる。

【００３１】 Ｊ^TＪΔθ^k＝ξ （１８） ここに、ξ＝−Ｊ^T（θ^k）ｆ（θ^k）（＝−∇Ｍ(θ^k)） であり、これは線形最小二乗法の正規方程式と基本的に
類似する。Ｎｅｗｔｏｎ法を基礎にして導かれたこの方
法は、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法と呼ばれ、解の近傍
では収束は速いものの、初期値が真値からある程度ずれ
る場合やＦ（θ）の大きな非線形性に遭遇した場合に、
安定性が悪く発散しやすい欠点がある。また、係数Ｊ^T
Ｊが常に非負定値となる好ましい性質をもつ反面、正則
でない場合や悪条件の場合に数値解法上の問題が生じや
すい。このため、例えば直接Δθに縮小因子を作用させ
て目的関数の減少を促す方法も考えられるが、ここでは
省略した２階偏微分項の存在を意識し、別の簡単な表現
で代用させることで全体として収束の安定化を図る。す
なわち、係数行列Ｊ^TＪの対角項に非負のパラメータλ
を加える補正操作 （Ｊ^TＪ＋λＩ）Δθ＝ξ （１９） を施し、Ｇａｕｓｓ−Ｎｅｗｔｏｎ法で得られるΔθの
改善を行う。ただし、Ｉは単位行列である。

【００３２】こうして、非線形方程式の解を線形化表現
の反復実行により求めることになるが、非線形特性はパ
ラメータλで模擬されるため、その評価は問題解決への
重要な鍵となる。例えば、λ＝０は明らかにＧａｕｓｓ
−Ｎｅｗｔｏｎ法そのものであり、λが十分大きな値
（λ≫‖Ｊ^TＪ‖）をとると、Δθ＝−∇Ｍ(θ)／λと
なり、Ｍ（θ）の最急降下方向（谷向き）に無限小の解
を見い出すことになる。従って、探索過程における計算
値が真の値から遠く離れていたり非線形性の強い状況に
直面した場合はλを大きくし、解に接近するにつれてλ
を小さくすることで安定した解の探索が期待できる。つ
まり、変位Δθはその大きさがλの増大とともに ｄ‖Δθ‖²／ｄλ＝２（Δθ）^Tｄ（Δθ）／ｄλ ＝−２（Δθ）^T（Ｊ^TＪ＋λＩ）（Δθ）＜０ のように単調減少するので，非線形性の大きなところで
はΔθを小さく、（つまりλを大きく）抑えることで十
分に不安定な挙動が回避できる。実際の計算では、適当
なλを初期値（０．１或いは１．０）とし、目的関数が
減少するまでλをｖ倍（ｖは固定値で１０又は２を使
う）に増やしていき、それが達成された時点すなわちＭ
（θ＋Δθ）＜Ｍ（θ）となったときに、こんどはｌ／
ｖに切替えてそれを初期値とする次のサイクルへ移行す
る。これは図１に示す簡単なアルゴリズムによって実現
できる。すなわち、同図のステップ（１０１）〜（１０
４）を通じて見い出したθ^k+1について、続く判断ステ
ップ（１０５）において、 Ｉｆ Ｍ（θ^k+1）＜Ｍ（θ^k） ，ｔｈｅｎ ｓｅｔ λ：＝（１／ｖ）λ・・ステップ（１０８） ，ｏｔｈｅｒｗｉｓｅ ｓｅｔ λ：＝ｖλ ・・ステップ（１０７） のごとく、初期値から関数の増減に応じてλをｖ倍或い
は１／ｖ倍に調節しながら解に接近する。この方法は、
単純ながら想像以上に解の探索に有効であることがこれ
までの経験や計算結果から明らかである。一例として、
４個の回転関節により結合された４自由度リンク機構を
例にとるならば、 機構パラメータ ａ(m) (0.10, 0.50, 0.22, 0.25) ｄ(m) (0.70, 0.40, 0.33, 0.35) α(deg) (-90, 15, -30, -90) 目標指先の 「 -0.96855D-1 0.95148D0 0.29206D0 -0.77229D0 位置・姿勢を -0.79125D0 0.10440D0 -0.60252D0 0.58288D0 示す行列要素 -0.60377D0 -0.28945D0 0.74275D0 0.69107D0 」 初期推定値θ^o(deg) ( -10, 30, 20, -15 ) 誤差Ｄ Ｄ＝‖Ｍ（θ^o）‖₂＝3.246 モデル化方法 最小二乗法 λ=1.0 関節角度θ(deg) （ 0.60000D2, -0.20000D2, 0.12000D3, 0.5000D2 ） 収束回数； ITN=7 再現された指先の 「 -0.96855D-1 0.95148D0 0.29206D0 -0.77229D0 位置・姿勢を -0.79125D0 0.10440D0 -0.60252D0 0.58288D0 示す行列要素 -0.60377D0 -0.28945D0 0.74275D0 0.69107D0 」 のごとく、ステップ（１０６）の収束条件が満たされる
までに１０回にも満たない僅かな収束回数ITNをもっ
て、目標と寸分違わぬ指先位置及び指先姿勢が得られる
ことが判る。同様のことは、他の例えば直動関節を含む
多関節リンクについても、実際の解析によって裏付けが
得られている。参考までに、第２関節と第４関節が直動
関節である６自由度リンク機構の解析結果を掲載してお
く。

【００３３】 機構パラメータ ａ(m) (0.20, 0.10, 0.30, 0.15, 0.75, 0.28 ) ｄ(m) (0.60, 0.40, 0.53, 0.55, 1.00, 0.10 ) α(deg) ( 60, 45, 140, 45, 90, 60 ) 目標指先の 「 -0.75138D0 -0.53261D0 0.38956D0 -0.10625D1 位置・姿勢を -0.65246D0 0.51139D0 -0.55927D0 -0.36469D0 示す行列要素 0.98653D-1 -0.67439D0 -0.73175D0 -0.41934D0 」 初期推定値θ^o(deg) ( 10, 1.0, 20, 1.0, 30, 0.0 ) 誤差Ｄ Ｄ＝‖Ｍ（θ^o）‖₂＝4.256 モデル化方法 Ｆｌｅｔｃｈｅｒアルゴリズムによる最小二乗法 関節角度θ(deg) （ 0.75000D2, 0.40000D2, -0.55000D2, -0.55000D0, 0.12000D3, 0.30000D2 ） 収束回数； ITN=22 （ 0.14332D1, 0.36117D0, -0.17497D3, -0.36469D0, 0.20218D2, -0.37769D2 ） 収束回数； ITN=15 再現された指先の 「 -0.75138D0 -0.53261D0 0.38956D0 -0.10625D1 位置・姿勢を -0.65246D0 0.51139D0 -0.55927D0 -0.36469D0 示す行列要素 0.98653D-1 -0.67439D0 -0.73175D0 -0.41934D0 」 なお、λの初期設定については、入力で与える以外にヤ
コビ行列のスペクトル・ノルム（ヤコビ行列における各
列ベクトルの最大ノルム） λ＝‖Ｊ（θ^o）‖＝Ｍａｘ｛‖Ｊｕ‖₂｜‖ｕ‖₂＝
１｝ を出発値として与えることもできる。なお、以下の説明
では‖・‖₂はユークリッド・ノルムとする。

【００３４】ところで、上記のようなλの指定方法は実
用上十分であるが、場合によりλをｖ倍（或いはｌ／ｖ
倍）にする急変操作が解の不安定性を招くことがある。
そこで、現在と次のステップでの関数値の比較に加え、
関数値の変化量に応じてλの取扱い方を変えることもで
きる。こうしたλの自動的な選定方法は、要するにある
非線形特性がその線形特性から逸脱する程度を考慮する
ものであり、逸脱度と線形化した目的関数の変化量との
比κを定義することで、この比κを利用してλの最良値
を以下のように設定することができる。

【００３５】いま、目的関数Ｍ（θ）の変化量ΔＭ＝Ｍ
（θ＋Δθ）−Ｍ（θ）に対して、関数ｆを線形近似し
たときの目的関数Ｍ'（θ）の予想変化量ΔＭ'は ΔＭ'＝Ｍ'（θ＋Δθ）−Ｍ'（θ）＝‖ＪΔθ−ｂ‖²−‖ｂ‖² ＝（ＪΔθ−ｂ）^T（ＪΔθ−ｂ）−ｂ^Tｂ＝Δθ^TＪ^TＪΔθ−２ｂ^TＪΔθ ＝Δθ^T（Ｊ^Tｂ−λＩΔθ）−２ｂ^TＪΔθ ＝−Δθ^TＪ^Tｂ−Δθ^TλＩΔθ ＝−Δθ^T（Ｊ^TＪ＋λＩ）Δθ−λ‖Δθ‖² （２１） となる。（θ＋Δθ）における関数値は常に減少する
が、実際の挙動は非線形性に左右され一定ではない。そ
こで、両者の変化量の比κ＝ΔＭ／ΔＭ'に応じて次の
ように場合分けして対応する。

【００３６】ｉ） ０．７５＜κ ： λ→λ／２ ii） ０．２５≦κ≦０．７５ ： 次のステップのλ
は現在値と同じ iii） κ＜０．２５ ： λをλ→λｕへと増加させる ｕ＝ｍａｘ｛２，ｍｉｎ（２−ｒ，１０）｝ iv） κ＜０ ： 非線形性が強まることからλ→λｕ
へと大きく増加させる ｕ＝ｍａｘ｛２，ｍｉｎ（２−ｒ，１０）｝ なお、このアルゴリズムにおけるλの出発値はλ＝０と
する。特に、計算途中で、λ＝０の近傍での（λ／ｖ）
倍は効果がないのでΔθのノルムを半分にするような遮
断値 を設定する。ただし、ｅ_kは（Ｊ^TＪ）の固有値で、Ｔｒ
は行列トレースであり、遮断値λｃは、最小固有値ｅ_n
よりも小さく抑えた値とされる。

【００３７】なお、上記の一連の計算過程は、図２に示
すステップ（２０１）〜（２１８）により実現される。
図２のフローでは、ステップ（２０１）がλ＝０で開始
される点を除き、ステップ（２０４）までは前記のステ
ップ（１０４）までと同じ流れを汲む。しかし、その
後、ステップ（２０５）に示す式（２１）から得られる
ΔＭ'から計算される比κを用い、上記の場合分けi）〜
iv）に対応するステップ（２０８）〜（２１５）を実行
する。そして、ステップ（２１６），（２１７）にて目
的関数が減少しかつ所期の収束条件が満たされたことが
判明した時点で解析を停止する。

【００３８】このように、式（１９）を解く場合に正規
方程式の係数行列（Ｊ^TＪ＋λＩ）をＣｈｏｌｅｓｋｙ
分解で三角行列に変換し、さらに後退代入方式で有効桁
数の「桁落ち」の少ない計算を行うことにより、通常の
Ｇａｕｓｓ消去法よりも効率よく高精度の解が得られ
る。

【００３９】次に、多自由度冗長機構に対する逆問題の
解法、すなわちシンプレックス法と線形最小二乗法によ
るハイブリッド法について説明する。

【００４０】マニピュレータの自由度が６より多く、そ
のヤコビ行列がフルランク（ｒ＝６）の場合、線形近似
解は一意的に決定できない。そこで、ある種の条件を取
入れた一般的な計算方法について言及する。ここでは、
その方法として、残差ベクトルＦのユークリッド・ノル
ム‖Ｆ‖₂を最小にする偏差Δθのなかで、特にΔθの
ユークリッド・ノルム‖Δθ‖₂を最小にする条件に着
目し、反復計算によりすべての関節角度を一義的に確定
する（最小二乗最小ノルム解）。

【００４１】まず、（１２）式の個々の関数を以下に示
すごとく線形近似する。

【００４２】 すなわち ＪΔθ＝ｂ ただし、ｂ＝−ｆ（θ） （２４） ここで、ｎ＞６を念頭において，残差ベクトルＦ（＝Ｊ
Δθ−ｂ）の二乗和最小化を実現する問題に置き換え
る。

【００４３】さて，任意のｍ×ｎ行列Ｊ（以下，ｍ＝６
とする）は，特異値分解定理により対角要素のみをもつ
ｍ×ｎ行列Σと直交行列Ｕ（ｍ×ｍ），Ｖ（ｎ×ｎ）で
構成される。

【００４４】 Ｊ＝ＵΣＶ^T （２５） この場合、Σの対角要素（特異値）σ_iは．｜ＪＪ^T−ｅ
Ｉ｜＝０、つまりＪＪ^Tの固有値ｅ_i（非負）の平方根
（σ_i＝ｅ_i ^1/2）として求まり、σ₁≧σ₂≧・・・≧σ_r
＞０，σ_r+1＝σ_r+2＝・・・＝σ_n＝０とする。

【００４５】そこで、この特異値分解を線形化した関係
式（２４）に代入すると ＵΣＶ^TΔθ＝ｂ （２６） となる。

【００４６】ところで，任意の直交行列Ｕに対して、ユ
ークリッド・ノルムは ‖ＵＦ‖₂ ²＝（ＵＦ）^T（ＵＦ）＝Ｆ^T（Ｕ^TＵ）Ｆ＝Ｆ^TＦ＝‖Ｆ‖₂ ² （２７） となる性質がある。従って、残差ベクトルＦのノルムは ‖Ｆ‖₂＝‖ＪΔθ−ｂ‖₂＝‖Ｕ^T（ＪΔθ−ｂ）‖₂ ＝‖ΣΔθ'−ｂ'‖₂ （２８） となる。ここで、 ｂ'＝Ｕ^Tｂ，Δθ'＝Ｖ^TΔθ （２９） の残差ノルムを最小にするためには、（ΣΔθ'−ｂ'）
が最小であればよい。

【００４７】従って、Σのランクをｒ（≦ｍ）とする
と、ΣΔθ'＝ｂ'から Δθ_i'＝ｂ_i'／σ_i （ｉ＝１，２，・・・，ｒ） （３０） と定まるが、残り（ｎ−ｒ）の解は任意となる。

【００４８】そこで解自身のノルム‖Δθ'‖₂を最小に
する条件として Δθ_i'＝０ （ｉ＝ｒ＋１，・・・，ｎ） （３１） を与えて解を確定させる。求める偏差Δθは、（２９）
式より直交変換Ｖを適用して Δθ＝ＶΔθ' （ｉ＝１，２，・・・，ｎ） （３２） を得る。ここに、Δθ'は式（３０），（３１）より次
のように表される。

【００４９】 「１／σ₁ ： 「ｂ₁' １／σ₂ ： ｂ₂' ・ ０ ： Δθ'＝ ・ ： ０ ＝Σ⁺ｂ ０ ・ ： ： （３３） １／σ_r ： ｂ_r' ．．．．．．．．．．．．．．．．：．．． ： ｂ_r+1' ０ ： ０ ： 」 ｂ_n' 」 また、式（３２）は Δθ（＝Δθ⁺）＝ＶΔθ'＝ＶΣ^-1ｂ'＝ＶΣ⁺ｂ'＝ＶΣ⁺Ｕ^Tｂ＝Ｊ⁺ｂ （３４） と書ける。すなわち、ＶΣ⁺Ｕ^Tはヤコビ行列Ｊの一般逆
行列Ｊ⁺であることが分かり、最小二乗ノルム解はＭｏ
ｏｒｅ−Ｐｅｎｒｏｓｅの一般解に対する１つの確定解
となる。

【００５０】こうして、冗長系の計算における各関節変
数の偏差が決定できるが、上記の線形最小二乗法を基礎
とする冗長関節を含んだ機構解析法は、前述の十分に広
い範囲（大域）で可能となる解の探索法と異なり、関節
角度の初期推定値が真値から大きく離れている場合に、
線形化の弱点がしばしば収束を抑制してしまうことがあ
る。そこで、未知領域での安定な解の探索を行うため、
まず直接計算法で収束解の近傍にもっていく予備計算を
導入し、予備計算で得られた粗い近似値を初期値として
前記アプローチを実行するようにしたのが、図３のステ
ップ（３０１）〜（３１０）によって与えられるハイブ
リッド方式アルゴリズムである。

【００５１】すなわち、同図に示すステップ（３０
１），（３０２）がシンプレックス法による予備計算の
ためのステップであり、ここで得られた粗い近似値θⁿ
を続くステップ（３０３）において初期値θ^oに設定す
る。さらに、ステップ（３０４）〜（３１０）に示す反
復計算により、特異点分解定理を活かして前述の最小二
乗最小ノルム解を求める。その場合、先行する予備計算
により既に収束領域はある程度絞り込まれているため、
短時間で精度の高い解を引出すことが可能である。な
お、ステップ（３０７）は、反復計算のためのステップ
幅Δθ^kを補正するステップであるが、ここで適切なス
テップ幅補正係数ａを選択することにより、さらに計算
速度の向上が可能である。

【００５２】なお、参考までにシンプレックス法につい
て概説する。直接探査法であるシンプレックス法は、ｎ
次元空間Ｒⁿ内に（ｎ＋１）個の頂点を選んで単体（凸
包）を構成し、その点における目的関数の値を比較しな
がら探索を行う。すなわち、折返し（ｒｅｆｌｅｃｔｉ
ｏｎ），拡大（ｅｘｐａｎｓｉｏｎ），収束（ｃｏｎｔ
ｒａｃｔｉｏｎ），縮小（ｒｅｄｕｃｔｉｏｎ）と呼ば
れる４種類の基本操作を繰返しながら徐々に単体を縮
め、最適点の或る近傍内に閉じ込めていく。

【００５３】いま、単体の頂点をｘⁱ（∈Ｒⁿ，ｉ＝１，
２，・・・，ｎ）で表し、ｘ^H，ｘ^S，ｘ^Lをそれぞれ目
的関数が最大値をとる頂点，２番目に大きい値をとる頂
点，最小値をとる頂点とする。また、ｘ^Gは、ｉ＝Ｈな
る頂点を除くすべての点の重心（図心）であり と計算できる。基本操作として、折り返しは重心に対し
て頂点ｘ^Hを ｘ^R＝（１＋α）ｘ^G−αｘ^H α＞０ に置き換える操作で、拡大は目的関数をさらに改善する
ために（ｘ^G−ｘ^R）方向に沿ってｘ^Rを ｘ^E＝γｘ^R＋（１−γ）ｘ^G γ＞１ に置き換える。一方、収縮は、ｘ^Hをｘ^G方向に ｘ^C＝βｘ^H＋（１−β）ｘ^G ０＜β＜１ としてシンプレックスを縮める。通常、α＝１，β＝
０．５，γ＝２とする。

【００５４】また、縮小操作はすべての頂点をｘ^L方向
に ｘⁱ＝（ｘⁱ＋ｘ^L）／２ （ｉ＝１，２，・・・，ｎ＋１） と縮小するものである。

【００５５】まず、初期点ｘ^o＝（ｘ₁，ｘ₂，・・・，
ｘ_n）^Tを与えてその周辺に単体を構成しなければならな
い。

【００５６】原点を１つの頂点とする正則な単体（辺の
長さがすべて等しい単体）は ｄ₁＝｛（ｎ＋１）^1/2＋ｎ−１｝／２^1/2ｎ ｄ₂＝｛（ｎ＋１）^1/2−１｝／２^1/2ｎ の値を使って表されるので、任意の点ｘⁱを頂点にもつ
辺の長さがｓのシンプレックスは ｘ¹＝（ｘ₁，ｘ₂，・・・，ｘ_n）^T ｘ²＝（ｘ₁＋ｄ₁ｓ，ｘ₂＋ｄ₂ｓ，ｘ₃＋ｄ₂ｓ，・・・，ｘ_n＋ｄ₂ｓ）^T ｘ³＝（ｘ₁＋ｄ₂ｓ，ｘ₂＋ｄ₁ｓ，ｘ₃＋ｄ₂ｓ，・・・，ｘ_n＋ｄ₂ｓ）^T ・・・ ｘⁿ＝（ｘ₁＋ｄ₂ｓ，ｘ₂＋ｄ₂ｓ，ｘ₃＋ｄ₂ｓ，・・・，ｘ_n＋ｄ₁ｓ）^T ｘⁿ⁺¹＝（ｘ₁＋ｄ₂ｓ，ｘ₂＋ｄ₂ｓ，ｘ₃＋ｄ₂ｓ，・・・，ｘ_n＋ｄ₁ｓ）^T と定められる。このｎ＋１個の頂点はｎ次元空間を張る
ことになる。

【００５７】シンプレックス法による具体的な計算手順
は、以下に示すステップ１〜６による。

【００５８】１．シンプレックスの頂点から、ｘ^H，
ｘ^S，ｘ^L，ｘ^Gを求め、収束判定を行う。 ２．ｘ^Hから折り返しｘ^Rを求め、Ｍ（ｘ^R）を計算す
る。

【００５９】３．Ｍ（ｘ^L）≦Ｍ（ｘ^R）≦Ｍ（ｘ^S）な
らば、ｘ^Hをｘ^Rで置き換え、ステップ１に戻る。

【００６０】４．Ｍ（ｘ^R）＜Ｍ（ｘ^L）ならば、（ｘ^R
−ｘ^G）方向に拡大し、ｘ^Eを求めて新しいシンプレック
スを作る。

【００６１】Ｍ（ｘ^E）＜Ｍ（ｘ^L）ならば、ｘ^Hをｘ^Eで
置き換えステップ１に戻る。

【００６２】Ｍ（ｘ^L）≦Ｍ（ｘ^E）の場合は、ｘ^Hをｘ^R
で置き換えステップ１に戻る。

【００６３】５．Ｍ（ｘ^S）＜Ｍ（ｘ^R）＜Ｍ（ｘ^H）の
場合は、ｘ^Hをｘ^Rで置き換えて縮小する。Ｍ（ｘ^H）≦
Ｍ（ｘ^R）の場合も縮小する。

【００６４】６．Ｍ（ｘ^C）＜Ｍ（ｘ^H）ならば、ｘ^Hを
ｘ^Cで置き換えてステップ１に戻る。

【００６５】Ｍ（ｘ^H）≦Ｍ（ｘ^C）の場合、ｘ^Cを中心
にｘⁱ＝（ｘⁱ＋ｘ^L）／２，ｉ＝１，２，・・・，ｎ＋
１と縮めてステップ１に戻る。

【００６６】なお、収束判定は、許容誤差εに対して が成立するか否かが判定基準となる。

【００６７】

【発明の効果】以上説明したように、本発明によれば、
複数のリンクが回転又は並進機能をもつ関節により直列
連鎖状に連結された多関節リンク機構について、原始リ
ンクから最終リンクまでの個々のリンクの位置及び姿勢
を、隣接リンク間の機構学的関係を示す漸化式からなる
再帰関係式により規定し、各関節角度が与えられたとき
に前記再帰関係式に基づいて逐次演算を実行し、原始リ
ンクから順に最終リンクまで個々のリンクの位置及び姿
勢を割り出すようにしたから、関節自由度や関節のタイ
プ或いは関節軸のねじれ具合やリンクの長さ或いはオフ
セットの大きさ等の機構学的属性をすべて考慮した上
で、行列演算に代わる前進型の漸化式で逐次計算を行う
ことにより、指先はもとより各リンクの位置・姿勢を割
り出すことができ、従って多関節形ロボット等の運動解
析に適用することにより、どの関節をどの程度動かせば
指先がどの位置にくるかが、行列積の演算によらず短時
間で正確に決定できる等の効果を奏する。

【００６８】また、本発明は、複数のリンクが回転又は
並進機能をもつ関節により直列連鎖状に連結された多関
節リンク機構について、隣接リンク間の機構学的関係を
示す関係式と最終リンクの位置及び姿勢が与えられたと
きに、非線形最小二乗法に用いるＧａｕｓｓ−Ｎｅｗｔ
ｏｎ法に非線形特性を模擬する補正係数を導入し、目的
関数が単調減少するよう補正係数を変えつつ反復計算を
行い、大域的に実現可能な関節角度を誘導するようにし
たから、初期値が真値からある程度ずれている場合で
も、或いは非線形性の強い方程式に対しても、発散振動
等を招くことなく数値的に安定な解を求めることが可能
であり、汎用的取扱いが可能である等の効果を奏する。

【００６９】さらにまた、本発明は、６自由度を越える
冗長な関節構造を有する多関節リンク機構について、隣
接リンク間の機構学的関係を示す関係式と最終リンクの
位置及び姿勢が与えられたときに、まず直接探索法によ
り収束領域を限定する予備計算を行い、該予備計算によ
り得られた粗い近似値を初期値として線形最小二乗法に
よる反復計算を行い、最小二乗最小ノルム解として一意
的に関節角度を誘導するようにしたから、シンプレック
ス法で収束領域を絞った上で線形最小二乗法により精度
の高い解を引出すことができ、これによりマニピュレー
タの自由度が６を越えていてヤコビ行列がフルランクで
あるために線形近似解が一意的に決定されないような多
自由度冗長機構に対し、数値的安定性に重点を置いて関
節角度を誘導することができ、大きさや構造が異なるあ
らゆるタイプのロボットについて、順問題と同様に逆問
題が容易に解決できることから、軌道計画やロボット設
計或いはロボット作業空間の解析やシミュレータの基本
ツールに活用できる等の効果を奏する。

【図面の簡単な説明】

【図１】本発明の多関節リンク機構の逆運動学解析法の
一実施例を示すフローチャートである。

【図２】図１に示した逆運動学解析法の変形例を示すフ
ローチャートである。

【図３】６自由度を越える冗長機構に適用した本発明の
多関節リンク機構の逆運動学解析法の他の実施例を示す
フローチャートである。

Claims (3)

【特許請求の範囲】
1. 【請求項１】 複数のリンクが回転又は並進機能をもつ
   関節により直列連鎖状に連結された多関節リンク機構に
   ついて、原始リンクから最終リンクまでの個々のリンク
   の位置及び姿勢を、隣接リンク間の機構学的関係を示す
   漸化式からなる再帰関係式により規定し、各関節角度が
   与えられたときに前記再帰関係式に基づいて逐次演算を
   実行し、原始リンクから順に最終リンクまで個々のリン
   クの位置及び姿勢を割り出すことを特徴とする多関節リ
   ンク機構の順運動学解析法。
2. 【請求項２】 複数のリンクが回転又は並進機能をもつ
   関節により直列連鎖状に連結された多関節リンク機構に
   ついて、隣接リンク間の機構学的関係を示す関係式と最
   終リンクの位置及び姿勢が与えられたときに、非線形最
   小二乗法に用いるＧＡｕｓｓ−Ｎｅｗｔｏｎ法に非線形
   特性を模擬する補正係数を導入し、目的関数が単調減少
   するよう前記補正係数を変えつつ反復計算を行い、大域
   的に実現可能な関節角度を誘導することを特徴とする多
   関節リンク機構の逆運動学解析法。
3. 【請求項３】 ６自由度を越える冗長な関節構造を有す
   る多関節リンク機構について、隣接リンク間の機構学的
   関係を示す関係式と最終リンクの位置及び姿勢が与えら
   れたときに、まず直接探索法により収束領域を限定する
   予備計算を行い、該予備計算により得られた粗い近似値
   を初期値として線形最小二乗法による反復計算を行い、
   最小二乗最小ノルム解として一意的に関節角度を誘導す
   ることを特徴とする多関節リンク機構の逆運動学解析
   法。