CCD IK - 機械学習基礎理論独習

CCD IKとは

CCD IKとは、目標位置 ${\bf g}\in{\mathbb R}^3$ が与えられたとき、先端ジョイント ${\bf o}_{n+1}$ が目標位置 ${\bf g}$ に一致するようなジョイント ${\bf o}_1,{\bf o}_2,\cdots,{\bf o}_n$ の角度を計算する逆運動学のアルゴリズムです。
Cyclic Coordinate Descent Inverse Kinematics の略です。

アルゴリズム

先端に最も近いジョイント ${\bf o}_n$ から間接角度を計算します。
ジョイント ${\bf o}_n$ から先端ジョイント ${\bf o}_{n+1}$ の方向指す単位ベクトルを、 ${\bf m}\in{\mathbb R}^3$ とし、
ジョイント ${\bf o}_n$ から目標位置 ${\bf g}$ の方向指す単位ベクトルを、 ${\bf n}\in{\mathbb R}^3$ とします。

$\begin{eqnarray} {\bf m}=\frac{{\bf o}_{n+1}-{\bf o}_n}{||{\bf o}_{n+1}-{\bf o}_n||}\tag{1}\\ {\bf n}=\frac{{\bf g}-{\bf o}_n}{||{\bf g}-{\bf o}_n||}\tag{2}\\ \end{eqnarray}$

$\Delta{\bf q}\in{\mathbb H}$ は ${\bf m}$ を $\bf n$ に回転するクォータニオンとします。
$\Delta{\bf q}$ の回転軸は ${\bf m}\times{\bf n}$ で求まり、回転角は $\cos^{-1}{\bf m}^\top{\bf n}$ で求まるので、 $\Delta{\bf q}$ が計算できます。
$\Delta{\bf q}$ を行列 ${\bf R}_{\bf q}$ に変換し、ジョイント ${\bf o}_n$ の姿勢をあらわす行列の右側から掛ければよい。
あとは、ジョイント ${\bf o}_{n-1},\cdots,{\bf o}_1$ とルートまで同様の更新処理を行えばOK。
この一連の計算を行った後に、先端ジョイント ${\bf o}_{n+1}$ と目標位置 ${\bf g}$ の距離を計算し、 $\epsilon>0$ 以内なら処理終了、そうでなければ ${\bf o}_n$ の更新処理からやり直す。