ヤコビアンを用いた逆運動学

ここでは，順運動学と対をなす技法である逆運動学について説明します．逆運動学には様々な計算手法が提案されていますが，本稿ではヤコビアンを用いた数値解法を解説します

また，この記事の発展版として，クォータニオンを用いた逆運動学法についてもまとめています．ひとまずこの記事に目を通した後にクォータニオン逆運動学を参照ください．

逆運動学
ヤコビアン
擬似逆行列，加重擬似逆行列
冗長変数
擬似逆行列解の問題点
DirectX Graphicsにおける実装例
まとめ

逆運動学 †

前回解説したように，順運動学（FK）とは多関節リンク構造を構成する関節の回転角度から各リンクの位置や姿勢を求める計算です．これと逆に，特定のリンクの位置や方向を満たすような関節角度を求める計算を逆運動学計算（inverse kinematics．IK）といいます．以下ではエフェクタの位置や方向からリンク構造を構成する全ての関節角度を計算する問題を考えることにします．エフェクタの位置ベクトル $a math image$ ，リンク構造を構成する関節角度ベクトルを $a math image$ とするとき， IK は FK の写像関数 $a math image$ の逆写像関数によって定式化されます．

$a math image$ は一般的に作業空間ベクトル（workspace vector）と呼ばれます． $a math image$ にはエフェクタの位置ベクトルだけでなく，方向ベクトルを合わせた 6 次元ベクトルも与えられます．さらに，2 つのリンクの状態を同時に指定する場合などにはより次元数の大きなベクトルが与えられることもあり，操作の目的に応じて $a math image$ の次元数 $a math image$ は変化します．また，関節角度ベクトル $a math image$ の次元数はリンク構造の自由度 $a math image$ に一致します．

ここで，作業空間の次元数 $a math image$ とリンク構造の自由度 $a math image$ の関係によって，IK の解の性質が変化することに注意する必要があります．

の場合
- 作業空間の次元数とリンク構造の自由度が一致する場合，FK，IK ともに解が一意に計算できます．つまり，ある関節角度ベクトルを与えるとエフェクタの位置が必ず 1 つだけ決定し，またある座標にエフェクタが位置するような関節角度ベクトルは必ず 1 つだけ存在します．したがって $a math image$ の場合の IK は解析的な手法によって解くことが可能です．
の場合
- 作業空間の次元数よりリンク構造の自由度が大きい場合，FK は解が一意に決定しますが， IK は解が無数に存在します．これを（作業空間に対する）リンク構造の冗長性（redundancy）と呼びます．例えば，Fig.1 のようにルートとエフェクタの位置が固定されている場合でも，リンク構造は様々な姿勢をとることが可能です．リンク構造に冗長性が存在する場合，IK は解析的に計算できないので，ヤコビアンを用いた数値計算や，最適化計算，探索法等を用いて解くことになります．
の場合
- 作業空間の次元数がリンク構造の自由度より大きい場合，FK は解が一意に決定しますが作業空間より次元の低い空間しか扱えません．例えば，1 リンクの向き（2 自由度）しか操作できない場合，エフェクタの位置は 3 次元で表されますが，実際にはリンク長を半径とする球面上しか移動できません．また IK では解が存在しない場合があります．
  
  Fig.1 リンク構造の冗長性

↑

ヤコビアン †

リンク $a math image$ のリンク長と初期方向を表す平行移動行列を $a math image$ ，関節 $a math image$ の回転を表す回転行列を $a math image$ とするとき， FKは次のように定式化されました．

上式のように，FKの写像関数 $a math image$ は回転行列，すなわち非線形関数である三角関数により構成されています．作業空間の次元数 $a math image$ とリンク構造の自由度 $a math image$ が等しい場合は，連立三角方程式を解くことで解析的に逆写像関数が計算できますが，次元数が大きくなるに従って計算が指数的に複雑になります．また $a math image$ の場合には解が解析的に求まりません．

そこで数値計算によって解を求めることを考えます．まず，関節角度 $a math image$ に微小角変位 $a math image$ を加えたときのエフェクタの微小変位 $a math image$ を求めます．

上式のように， $a math image$ は（写像関数 $a math image$ の $a math image$ に関する偏微分 × 微小角変位 $a math image$ で近似されます．同様にして，関節 $a math image$ の微小角変位 $a math image$ によるエフェクタ微小変位 $a math image$ は次のように近似されます．

さらに，以上の式を拡張することで，関節微小角変位ベクトル $a math image$ を加えたときのエフェクタ微小変位 $a math image$ を近似することができます．

上式で導出された $a math image$ がヤコビアン（Jacobian．ヤコビ行列）と呼ばれる行列で，関節微小角変位ベクトルからエフェクタ微小変位ベクトルへの写像を表します．このように，微小変位を扱う時の FK は線形演算によって計算されることがわかります．したがって，ヤコビアン $a math image$ の逆変換となる行列 $a math image$ を計算することによって，エフェクタ微小変位ベクトルから関節微小角変位ベクトルへの写像を計算することが可能です．

$a math image$ を用いた逆運動学計算の簡単な例として，与えられた目標位置に向かってエフェクタを直線的に移動させる場合の計算手順を以下に示します．

エフェクタの目標位置を $a math image$ とする．
現在のエフェクタの位置 $a math image$ から $a math image$ へ伸びる長さ $a math image$ のベクトルを計算し，目標エフェクタ変位ベクトル $a math image$ （ $a math image$ は計算刻み幅）を計算する．
ヤコビアン $a math image$ を計算し，その逆写像行列 $a math image$ を計算する．
$a math image$ により関節角変位ベクトルを計算する．
$a math image$ により，関節角度ベクトルを更新する．
更新後の $a math image$ （ $a math image$ は許容誤差）ならば計算終了，それ以外の場合は 2. に戻る．

しかし前述のように，リンク構造の自由度と作業空間の次元数の関係によって $a math image$ の性質は異なります．具体的には，ヤコビアン $a math image$ の階数 $a math image$ がM以上ならば $a math image$ を計算することが可能です．特に $a math image$ かつ $a math image$ ならば， $a math image$ となり，単純にヤコビアンの逆行列を計算することで $a math image$ が求まります．一方， $a math image$ の場合は $a math image$ は無数に存在するため一意に計算できないので，次節で説明する擬似逆行列のように任意の拘束条件下で解を計算することになります．

↑

擬似逆行列，加重擬似逆行列 †

前述のように， $a math image$ の場合にはヤコビアン $a math image$ の逆写像行列 $a math image$ は無数に存在します．つまり，リンク構造に冗長性が存在する場合は，1 つのエフェクタ位置に対応するリンク構造の姿勢は無数に存在することになります．例えば，人間の腕は肩 3 自由度，肘 2 自由度，手首 2 自由度の計 7 自由度を持つため，肩の位置と手の位置と方向（6 自由度）を固定した状態でも肘を移動させることができます．こうした冗長性を解決して $a math image$ を一意的に計算するためには，任意の拘束条件を導入して解を計算する必要があります．その代表的な方法が，擬似逆行列（pseudoinverse）を用いる方法です．詳細な説明は省略しますが，ヤコビアンの擬似逆行列 $a math image$ はこれによって計算される関節角変位ベクトル $a math image$ の自乗和 $a math image$ を最小化するように一意に計算されます．こうして得られる解を（ヤコビアンの）擬似逆行列解（pseudoinverse solution）といいます．

さらに，各関節の剛性（stiffness），いわば動かしやすさを指定する場合には，加重擬似逆行列（weighted pseudoinverse）を用いることで関節角変位ベクトルの加重自乗和 $a math image$ を最小化するような解を計算できます．ここで，加重行列 $a math image$ は $a math image$ の正定値対称行列である必要があります．

↑

冗長変数 †

前節の擬似逆行列解を一般化した一般化擬似逆行列解が次の式で与えられます．

ここで， $a math image$ は冗長変数（redundant coefficients）と呼ばれる $a math image$ 次元のベクトルです．前節で示した擬似逆行列解は一般化擬似逆行列解において $a math image$ の場合の特殊解です．この冗長変数は上式の両辺に右から $a math image$ を乗算するとわかるように，エフェクタの状態には影響しません．

また， $a math image$ の場合には $a math image$ となり，冗長変数の項そのものが消失します．つまり冗長変数は，リンク構造の冗長性を利用して，エフェクタの状態を保ちながら関節角度を操作するための変数だといえます．

↑

擬似逆行列解の問題点 †

擬似逆行列解は IK の代表的な数値解法ですが，ロボット工学分野で発展した経緯もあり，キャラクタアニメーションに応用する場合にはいくつかの問題もあります．

↑

特異姿勢 †

$a math image$ が存在する条件は前述の通り $a math image$ ですが， $a math image$ つまりランク落ちが発生する姿勢を特異姿勢（singular posture）と呼びます．例えば，Fig.2 に示すような 2 次元上の 3 自由度多関節リンク構造のヤコビアンを計算すると，第 1 列の成分はそれぞれ 0 以外の値をとります．一方，第 2 列の成分は全て 0 になりランク落ちとなります．直観的にみても Fig.2 の姿勢で各関節に微小角変位を与えても，エフェクタは x 軸方向には大きく移動しますが，y 軸方向にはほとんど移動しない状態にあることがわかると思います．特異姿勢では逆運動学の解が存在しないので，計算において特異姿勢を回避するような工夫が必要となります．

Fig.2 特異姿勢

また，特異姿勢付近では $a math image$ のいずれかの列の成分が小さくなり， $a math image$ の対応する成分が非常に大きな値をとります．したがって Fig.2 に近い姿勢において同図のような目標変位を与えると，非常に大きな関節角変位ベクトルが出力されるため動作が不自然になるという問題もあります．そのため，ロボット工学の分野では特異点付近でも自然な関節角変位ベクトルを与えるような $a math image$ を計算する方法も提案されています．

↑

関節可動域 †

擬似逆行列解の計算手順を見るとわかるように，計算はあくまで幾何学的な数値計算であり，キャラクタやロボットの関節可動域を全く考慮したものではありません．したがって，キャラクタの姿勢によっては肘が逆に曲がるなど不自然な動作がしばしば発生します．そのため，冗長変数を制御することで関節可動域を満足する解を計算する手法も提案されています．

↑

計算量 †

擬似逆行列解はあくまでも数値計算なので，解析的な方法と比べて当然計算量が大きくなりがちです．特にヤコビアンの計算には $a math image$ の行列乗算演算が必要であり，大きなボトルネックとなります．計算刻み幅 $a math image$ を調整することである程度の高速化は可能ですが，これは計算誤差とのトレードオフになります．

↑

DirectX Graphicsにおける実装例 †

今回のサンプルは，多関節リンク構造のエフェクタが指定された目標点に向かって直線的に移動するようなプログラムです．なおサンプルでは一般化加重擬似逆行列解を実装していますが，加重行列 $a math image$ は常に単位行列 $a math image$ ，冗長変数 $a math image$ はゼロベクトル $a math image$ としています．

サンプルではヤコビアンを計算するために GNU Scientific Library を用いています．実際にはそれほど難しい計算ではありませんが，慣れたライブラリに頼って手抜きしました…） GSL が導入されていない環境ではコンパイルできませんが，関数名やコメントを参照しつつ，各自のライブラリに置き換えていただければ，と思います．また，アルゴリズムとは直接関係のない部分は説明を割愛させてもらいますので，詳細はソースコード中のコメント等を参照してください．

サンプルプロジェクトは以下になります（プロジェクト名: JacIK）．ビルドするためには Summer2004 以降の DirectX9.0 SDK と GSL がインストールされている必要になります．実行後 [Test] メニューから [IK] を選択するとダイアログが出現し，目標位置を入力するとその点に向かってエフェクタが直線的に移動します．エフェクタが原点に移動するアニメーションをキャプチャした MPEG ムービーも置いておきます（途中画面が数回ブレれます）．

VC++.NET2003プロジェクトファイル(35Kb)

サンプルMPEGムービー(55Kb)

↑

リンクモデル †

サンプルで用いたリンク構造モデルは Fig.3 に示すように 3 関節 8 自由度を持ちます．全てのリンクは初期状態で Z 軸沿いの長さ 1 のリンク $a math image$ です．よって，エフェクタの位置を求める FK の式は次のように表されます．

上式に対応した C++ コードは次の通りです．

 
 
 
-
|
-
!
|
|
|
-
!
!

"MainDoc.cpp"
 
D3DXVECTOR3 GetEffectorPosition(gsl_vector *pvTheta)
{
  D3DXMATRIX mTmp;
  // FKによりエフェクタ位置を計算
  mTmp = OffsetZ(1.0) * RotY( gsl_vector_get(pvTheta, 7) ) * RotX( gsl_vector_get(pvTheta, 6) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 5) ) * RotY( gsl_vector_get(pvTheta, 4) ) * RotX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
 
  // ゼロベクトルとの乗算を省略し，平行移動項を取得
  return D3DXVECTOR3(mTmp(3, 0), mTmp(3, 1), mTmp(3, 2));
}

pvTheta が関節角度ベクトル $a math image$ に対応し，OffsetZ 関数は Z 軸沿いの平行移動移動行列 $a math image$ を作成する関数，RorX，RotY，RotZ はそれぞれ XYZ 軸まわりの回転行列を作成する関数になります．また，gsl_vector_get 関数はベクトルの成分を取得する GSL の関数です．コードに示すように，ゼロベクトルとの乗算は合成トランスフォームの平行移動項を取得することに等しいです．ベクトル×行列演算のほうが計算量は有利ですが，ここではコードの見通しを優先しています）．

↑

ヤコビアンの計算 †

今回のサンプルの要となるのがヤコビアンの計算部分です．ここで改めてヤコビアンの定義式を示します．

ヤコビアンは上段の式に示すように，FK の写像関数 $a math image$ の関節角に関する偏微分を行ベクトルとした行列です．そして下段の式に示すように，関節角度 $a math image$ に関する偏微分は， $a math image$ の計算式における回転行列 $a math image$ をその微分行列に置き換えることで計算されます．XYZ 各軸周りの回転行列を微分した行列を以下に示します．

以上に示したヤコビアン計算式の実装例は次の通りです．

 
 
 
-
|
|
-
!
|
|
|
|
-
!
|
-
!
|
|
|
|
|
|
|
-
!
|
|
|
|
|
|
|
-
!
|
|
|
|
|
|
-
!
|
|
|
|
|
|
-
!
|
|
|
|
|
|
-
!
|
|
|
|
|
-
!
|
|
|
|
|
-
!
|
|
|
|
|
!

"MainDoc.cpp"
 
gsl_matrix* CMainDoc::ComputeJacobian(const gsl_vector *pvTheta) const
{
  gsl_matrix *pmJac;
 
  // ヤコビアンは8×3（関節角度ベクトル長×3次元位置ベクトル長）の行列
  pmJac = gsl_matrix_alloc(pvTheta->size, 3);
  ASSERT(pmJac);
 
  D3DXMATRIX mTmp, mBack;
 
  // 第3リンクのオフセット行列
  mBack = OffsetZ(1.0);
 
  // 第8関節に関する偏微分
  mTmp = mBack * DiffY( gsl_vector_get(pvTheta, 7) ) * RotX( gsl_vector_get(pvTheta, 6) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 5) ) * RotY( gsl_vector_get(pvTheta, 4) ) * RotX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotY( gsl_vector_get(pvTheta, 7) );
  gsl_matrix_set(pmJac, 7, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 7, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 7, 2, mTmp(3, 2));
 
  // 第7関節に関する偏微分
  mTmp = mBack * DiffX( gsl_vector_get(pvTheta, 6) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 5) ) * RotY( gsl_vector_get(pvTheta, 4) ) * RotX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotX( gsl_vector_get(pvTheta, 6) ) * OffsetZ(1.0);
  gsl_matrix_set(pmJac, 6, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 6, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 6, 2, mTmp(3, 2));
 
  // 第6関節に関する偏微分
  mTmp = mBack * DiffZ( gsl_vector_get(pvTheta, 5) ) * RotY( gsl_vector_get(pvTheta, 4) ) * RotX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotZ( gsl_vector_get(pvTheta, 5) );
  gsl_matrix_set(pmJac, 5, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 5, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 5, 2, mTmp(3, 2));
 
  // 第5関節に関する偏微分
  mTmp = mBack * DiffY( gsl_vector_get(pvTheta, 4) ) * RotX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotY( gsl_vector_get(pvTheta, 4) );
  gsl_matrix_set(pmJac, 4, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 4, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 4, 2, mTmp(3, 2));
 
  // 第4関節に関する偏微分
  mTmp = mBack * DiffX( gsl_vector_get(pvTheta, 3) )
    * OffsetZ(1.0) * RotZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotX( gsl_vector_get(pvTheta, 3) ) * OffsetZ(1.0);
  gsl_matrix_set(pmJac, 3, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 3, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 3, 2, mTmp(3, 2));
 
  // 第3関節に関する偏微分
  mTmp = mBack * DiffZ( gsl_vector_get(pvTheta, 2) ) * RotY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotZ( gsl_vector_get(pvTheta, 2) );
  gsl_matrix_set(pmJac, 2, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 2, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 2, 2, mTmp(3, 2));
 
  // 第2関節に関する偏微分
  mTmp = mBack * DiffY( gsl_vector_get(pvTheta, 1) ) * RotX( gsl_vector_get(pvTheta, 0) );
  mBack *= RotY( gsl_vector_get(pvTheta, 1) );
  gsl_matrix_set(pmJac, 1, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 1, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 1, 2, mTmp(3, 2));
 
  // 第1関節に関する偏微分
  mTmp = mBack * DiffX( gsl_vector_get(pvTheta, 0) );
  gsl_matrix_set(pmJac, 0, 0, mTmp(3, 0));
  gsl_matrix_set(pmJac, 0, 1, mTmp(3, 1));
  gsl_matrix_set(pmJac, 0, 2, mTmp(3, 2));
 
  return pmJac;
}

ここで，DiffX，DiffY，DiffZ 関数はそれぞれ XYZ 軸周りの回転行列の微分行列を取得する関数です．余分な乗算演算を省くために作業用行列 mBack 行列を導入していますが，基本的には FK の式から導出されていることが確認できると思います．

↑

加重擬似逆行列，冗長項の計算 †

擬似逆行列や冗長変数の節で示した計算式にしたがい， GSL の関数によって実装しています．

 
 
-
!
-
-
!
|
|
|
|
-
!
|
|
-
!
|
|
|
|
|
|
|
|
!
 
-
!
-
-
!
|
|
-
!
|
|
|
|
-
!
|
|
|
|
-
!
|
|
-
!
|
|
|
|
-
!
|
|
|
|
|
|
|
|
|
|
!
 
-
!
 
-
-
!
|
|
|
|
-
!
|
|
|
|
|
|
|
|
!

"MainDoc.cpp"
 
// 擬似逆行列
gsl_matrix* CMainDoc::ComputePseudoInverse(const gsl_matrix *pmJac) const
{
  // J^T * J
  gsl_matrix *pmJtJ;
  pmJtJ = gsl_matrix_alloc(pmJac->size2, pmJac->size2);
  ASSERT(pmJtJ);
  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, pmJac, pmJac, 0.0, pmJtJ);
 
  // (J^T * J)^-1
  gsl_matrix *pmJtJi;
  pmJtJi = MatrixInverse(pmJtJ);
 
  // (J^T * J)^-1 * J^T
  gsl_matrix *pmPI;
  pmPI = gsl_matrix_alloc(pmJtJi->size1, pmJac->size1);
  ASSERT(pmPI);
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, pmJtJi, pmJac, 0.0, pmPI);
 
  gsl_matrix_free(pmJtJ);
  gsl_matrix_free(pmJtJi);
 
  return pmPI;
}
 
// 加重擬似逆行列
gsl_matrix* CMainDoc::ComputeWeightedPseudoInverse(const gsl_matrix *pmJac, const gsl_matrix *pmWeight) const
{
  // W^-1
  gsl_matrix *pmWI;
  pmWI = MatrixInverse(pmWeight);
 
  // J^T * W^-1
  gsl_matrix *pmJtWi;
  pmJtWi = gsl_matrix_alloc(pmJac->size2, pmWI->size2);
  ASSERT(pmJtWi);
  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, pmJac, pmWI, 0.0, pmJtWi);
 
  // J^T * W^-1 * J
  gsl_matrix *pmJtWiJ;
  pmJtWiJ = gsl_matrix_alloc(pmJtWi->size1, pmJac->size2);
  ASSERT(pmJtWiJ);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, pmJtWi, pmJac, 0.0, pmJtWiJ);
 
  // (J^T * W^-1 * J)^-1
  gsl_matrix *pmJtWiJi;
  pmJtWiJi = MatrixInverse(pmJtWiJ);
 
  // (J^T * W^-1 * J)^-1 * J^T
  gsl_matrix *pmJtWiJiJt;
  pmJtWiJiJt = gsl_matrix_alloc(pmJtWiJi->size1, pmJac->size1);
  ASSERT(pmJtWiJiJt);
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, pmJtWiJi, pmJac, 0.0, pmJtWiJiJt);
 
  // (J^T * W^-1 * J)^-1 * J^T * W^-1
  gsl_matrix *pmWPI;
  pmWPI = gsl_matrix_alloc(pmJtWiJiJt->size1, pmWI->size2);
  ASSERT(pmWPI);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, pmJtWiJiJt, pmWI, 0.0, pmWPI);
 
  gsl_matrix_free(pmJtWi);
  gsl_matrix_free(pmJtWiJ);
  gsl_matrix_free(pmJtWiJi);
  gsl_matrix_free(pmJtWiJiJt);
 
  return pmWPI;
}
 
// 冗長項
gsl_vector* CMainDoc::ComputeRedundantCoefficients(const gsl_vector *pvEta, const gsl_matrix *pmJac,
    const gsl_matrix *pmJacPI) const
{
  // η * J
  gsl_vector *pvTmp;
  pvTmp = gsl_vector_alloc(pmJac->size2);
  ASSERT(pvTmp);
  gsl_blas_dgemv(CblasTrans, 1.0, pmJac, pvEta, 0.0, pvTmp);
 
  // vRC = η - η * J * J^+
  gsl_vector *pvRC;
  pvRC = gsl_vector_alloc(pmJacPI->size2);
  ASSERT(pvRC);
  gsl_vector_memcpy(pvRC, pvEta);
  gsl_blas_dgemv(CblasTrans, -1.0, pmJacPI, pvTmp, 1.0, pvRC);
 
  gsl_vector_free(pvTmp);
 
  return pvRC;
}

↑

関節角度ベクトルの更新処理 †

目標エフェクタ変位ベクトルとヤコビアンの加重擬似逆行列の乗算結果と冗長項を，元の関節角度ベクトルに加えて更新する処理です．

 
 
 
-
|
|
-
!
|
|
|
-
!
-
!
-
!
-
!
|
|
|
-
!
-
!
-
!
|
|
|
|
|
|
|
|
|
!

"MainDoc.cpp"
 
bool CMainDoc::UpdateAngles(const gsl_vector *pvDir, double dStep) const
{
  gsl_matrix *pmJac, *pmJacPI, *pmJacWPI;
 
  // 加重行列を単位行列で初期化
  gsl_matrix *pmWeight;
  pmWeight = gsl_matrix_alloc(m_pvTheta->size, m_pvTheta->size);
  gsl_matrix_set_identity(pmWeight);
 
  // ヤコビアンの計算
  pmJac = ComputeJacobian(m_pvTheta);
  // ヤコビアンの擬似逆行列
  pmJacPI = ComputePseudoInverse(pmJac);
  // ヤコビアンの加重擬似逆行列
  pmJacWPI = ComputeWeightedPseudoInverse(pmJac, pmWeight);
  // 目標エフェクタ変位×加重擬似逆行列
  gsl_blas_dgemv(CblasTrans, 1.0, pmJacWPI, pvDir, 1.0, m_pvTheta);
 
  gsl_vector *pvEta, *pvRC;
  pvEta = gsl_vector_alloc(m_pvTheta->size);
  // 冗長変数etaをゼロクリア
  gsl_vector_set_all(pvEta, 0.0);
  // 冗長項の計算
  pvRC = ComputeRedundantCoefficients(pvEta, pmJac, pmJacPI);
  // 冗長項を関節角度ベクトルに加える
  gsl_vector_add(m_pvTheta, pvRC);
 
  gsl_vector_free(pvRC);
  gsl_vector_free(pvEta);
  gsl_matrix_free(pmJacPI);
  gsl_matrix_free(pmJacWPI);
  gsl_matrix_free(pmJac);
  gsl_matrix_free(pmWeight);
 
  return true;
}

↑

エフェクタの目標位置への到達アニメーション生成 †

エフェクタの目標位置への直線的な到達アルゴリズムに従い，ダイアログから入力された目標位置へエフェクタが一致するまで関節角度ベクトルを繰り返し更新します．ただ，ループ中の特異姿勢はチェックしていませんので，計算が発散して無限ループになる場合もあるかも知れません…

 
 
 
-
|
|
|
|
-
!
|
|
|
|
|
|
-
!
-
|
|
!
|
-
!
|
|
|
|
-
-
!
-
!
-
!
|
|
-
!
|
|
|
|
-
!
-
!
!
|
|
!

"MainDoc.cpp"
 
void CMainDoc::OnTestIK()
{
  D3DXVECTOR3 vTarget, vPos, vDisp;
  const double dStep = 0.01;
  float fDist;
 
  // ダイアログ入力から目標位置を取得
  CTargetDlg dlg;
  if (dlg.DoModal() != IDOK)
    return;
  vTarget.x = dlg.m_fX;
  vTarget.y = dlg.m_fY;
  vTarget.z = dlg.m_fZ;
 
  // リンク構造の全長より遠い位置は到達不可能
  if (D3DXVec3Length(&vTarget) >= 3.0f)
  {
    MessageBox(NULL, "Out of workspace", NULL, 0);
    return;
  }
 
  // D3DXVECTOR3から値を受け取るGSLベクトル構造体
  gsl_vector *pvDir;
  pvDir = gsl_vector_alloc(3);
  ASSERT(pvDir);
 
  while (1)
  {
    // 現在のエフェクタの位置を取得
    vPos = GetEffectorPosition(m_pvTheta);
    // 目標位置とエフェクタ位置の差分の計算
    vDisp = vTarget - vPos;
    // 差分が(step / 2)より小さければ計算終了
    if (fDist = D3DXVec3Length(&vDisp), fDist < dStep / 2.0)
      break;
 
    // 目標エフェクタ変位 = (差分ベクトル / 差分ベクトル長) * step
    vDisp *= static_cast<float>(dStep) / fDist;
    gsl_vector_set(pvDir, 0, vDisp.x);
    gsl_vector_set(pvDir, 1, vDisp.y);
    gsl_vector_set(pvDir, 2, vDisp.z);
 
    // 目標エフェクタ変位にしたがって関節角度ベクトルを更新
    UpdateAngles(pvDir, dStep);
    // 再描画
    UpdateAllViews(NULL);
  }
 
  gsl_vector_free(pvDir);
}

↑

まとめ †

今回は IK の導入偏として，リンク構造の冗長性に起因する IK の問題点と，IK の数値解法の 1 つであるヤコビアンの擬似逆行列解を解説してみました．これらのさらに詳しい解説は，運動計画（motion planning）をキーワードにロボット工学の書籍等を参照してみてください．特に，さらに踏み込んだIKに興味のある方には，東京大学の山根先生のメカトロ演習の講義ノートをお勧めします．SEGA アニマニウムのIKエンジンであるUT-Poserの片鱗に触れることができます．

ただ，擬似逆行列解はロボットアームのような機械的なキャラクタのアニメーション生成には効果的ですが，人体など有機的なキャラクタへの適用は非常に難しいと思います．加重行列や冗長変数である程度の調整は可能ですが直観的とは言いがたく，そんなややこしいことをするなら FK でモーション付けしたほうが早い場合が多いと思います．実際の制作ではあらかじめ FK で大まかにモーション付けし，IK でエフェクタの位置を補正する，といった使い方になるのではないでしょうか？