Neutrino's Blog: 電腦動畫中的反向動力法 (Inverse Kinematics)

¶ 1. 介紹

在電腦動畫或機器人學中，我們可能會希望一個人物或機器人做到特定的動作，例如伸手能碰到手把或伸手接到球。

一個物件可能會有很多關節，假設每個關節都是 5 個維度 (DoF)，一個手臂假設是三段關節，那就會是 15 個維度，如果以正向動力學 (Forward Kinematics, FK) 的方式去推敲怎樣達到我們想要的姿勢幾乎不可能。

這時候就會採用反向動力學 (Inverse Kinematics, IK)，當我們給物件目標位置後，用反推的方式算出物件如何運動可以達到該位置。

ik example
(用 IK 算出怎樣讓手臂動到目標位置)

反向動力學通常要解一個高維的問題，造成它十分難算，需要花費大量運算且可以有很多種解。常見的方法包含 Cyclic Coordinate Descent 方法、Jacobian Pseudoinverse 方法、Jacobian Transpose 方法、Levenberg-Marquardt Damped Least Squares 方法、Quasi-Newton and Conjugate Gradient 方法、神經網路方法。

根據所需的計算時間、精準度、反推的動作姿勢等需求，在做 IK 時也會選不同的方法，例如 Cyclic Coordinate Descent 都會是指尖先動才去動手臂，Jacobian Pseudoinverse 則會一次動所有關節，神經網路則可以快速預判和做出比較擬真的結果。而本文將介紹反向動力法的 Jacobian Pseudoinverse 方法。

¶ 2. Jacobian Pseudoinverse 原理與實作

¶ 2.1 參數定義

假設有一個機械手臂，我們希望他的手指可以動到特定位置，並設定從手指到手臂中間的每個關節的初始位置 $\mathbf s_i$ 都會轉動，透過中間這些關節運動，使得手指可以達到目標位置 $\mathbf t$。

定義有 $n$ 維度的世界座標，$n$ 通常是 3，代表 $x, y, z$ 三個維度，但也可以轉成其他維度空間。

第 $i$ 個關節的初始位置向量表示為：

$$
\mathbf s_i =
\begin{bmatrix}
s_x & s_y &s_z & \dots & s_n
\end{bmatrix}
$$

目標位置向量為：

$$
\mathbf t =
\begin{bmatrix}
t_x & t_y &t_z & \dots & t_n
\end{bmatrix}
$$

定義第 $i$ 個關節的 End Effector $\mathbf e$：

$$\mathbf e_i = \mathbf t - \mathbf s_i$$

所以每個關節的 End Effector 向量表示為：

$$
\mathbf e =
\begin{bmatrix}
e_1 & e_2 &e_3 & \dots & e_N
\end{bmatrix}
$$

對於所有關節包含的所有 $M$ 個可動維度 (DoF) 的旋轉角度向量為：

$$
\mathbf \theta =
\begin{bmatrix}
\theta_1 & \theta_2 & \theta_3 & \dots & \theta_M
\end{bmatrix}
$$

¶ 2.2 動力學

在正向動力法 (Forward Kinematics) 中，可以由已知的轉動角度，透過 $f$ 函數算出下一步的 End Effector $e$，我們有以下算式：

$$\mathbf e = f(\mathbf \theta)$$

在反向動力學中 (Inverse Kinematics) 中，我們將關係反過來，我們給定 End Effector，希望得到旋轉的角度：

$$\mathbf \theta = f^{-1}(\mathbf e)$$

¶ 2.3 Jacobian

向量微積分的 Jacobian 代表一個函數 $f$ 的線性近似值，我們可以用 Jacobian $J$ 從 End Effector $\mathbf e$ 去回推 d$\mathbf \theta$。

$J = \frac{d e}{d θ}$

$$d\mathbf e = \mathbf J d \mathbf \theta$$

$\mathbf J$ 表示成：

$J = (\begin{matrix} \frac{\partial f_{1}}{d θ_{1}} & \frac{\partial f_{1}}{\partial θ_{2}} & \dots & \frac{\partial f_{1}}{\partial θ_{n}} \\ \frac{\partial f_{2}}{\partial θ_{1}} & \frac{\partial f_{2}}{\partial θ_{2}} & \dots & \frac{\partial f_{2}}{\partial θ_{n}} \\ ⋮ & ⋮ & \dots & ⋮ \\ \frac{\partial f_{k}}{\partial θ_{1}} & \frac{\partial f_{k}}{\partial θ_{2}} & \dots & \frac{\partial f_{k}}{\partial θ_{n}} \end{matrix})$

回顧我們剛剛得到 $d\mathbf e = \mathbf J d \mathbf \theta$，我們移項得到：

$$ d\mathbf \theta = \mathbf J^{-1} d \mathbf e$$

這個旋轉角度變化量 $d\mathbf \theta$ 就是我們想得到的，因為我們可以用 $d\mathbf \theta$ 去回推上一步的位置。

¶ 2.4 求 Jacobian 近似解

定義世界座標是三維度 (3 DoF)，為解 $d\mathbf \theta = \mathbf J^{-1} d \mathbf e$ 我們需要先求 Jacobain。以下算式都是建立在世界座標之下。

先看 Jacobian 的其中一欄：

$$
\mathbf J_i =
\frac{\partial \mathbf e}{d\theta_1}=
\begin{bmatrix}
\frac{\partial e_{x}}{\partial\theta_i} \\
\frac{\partial e_{y}}{\partial\theta_i} \\
\frac{\partial e_{z}}{\partial\theta_i}
\end{bmatrix}
$$

取 $\mathbf \theta$ 和 $\mathbf e$ 的線性微小變化量來近似偏微分：

$$
\mathbf J_i =
\frac{\partial \mathbf e}{d\theta_i}=
\frac{\Delta \mathbf e}{\Delta\theta_i}=
\begin{bmatrix}
\frac{\Delta e_{x}}{\Delta\theta_i} \\
\frac{\Delta e_{y}}{\Delta\theta_i} \\
\frac{\Delta e_{z}}{\Delta\theta_i}
\end{bmatrix}
$$

對於某關節 $A$ 的其中一維度的旋轉，該維度下，關節 $A$ 的 End Effector $\mathbf e$ 的線性的變化 $d\mathbf e$，等於關節 $A$ 在此維度下旋轉的變化量 ($\mathbf \omega \mathbf rdt$)。

$$
\frac{d\mathbf e}{dt} =
\vert \mathbf \omega \vert {\frac{\mathbf \omega}{\vert \mathbf \omega \vert }} \times \mathbf r =
\frac{d \mathbf \theta}{dt} \mathbf a \times \mathbf r
$$

其中 $\mathbf a= \frac{\mathbf \omega}{\vert \omega \vert}$，代表單位長度的旋轉向量。

移項得到：

$${d\mathbf e \over d\mathbf \theta} = \mathbf a \times \mathbf r$$

於是我們得到：

$$
\mathbf J_1 =
\frac{\Delta \mathbf e}{\Delta\theta_1}=
\begin{bmatrix}
\frac{\Delta e_{x}}{\Delta\theta_1} \\
\frac{\Delta e_{y}}{\Delta\theta_1} \\
\frac{\Delta e_{z}}{\Delta\theta_1}
\end{bmatrix}=
\mathbf a_i \times (\mathbf e - \mathbf r_i)
$$

其中 $\mathbf r_i$ 為第 $i$ 個關節的座標向量。

注意到這個算法都是 $\mathbf J_i$ 都是關節的其中 1 維度，所以所有關節加起來如果有 M 個維度，那 $\mathbf J$ 就會是 $3 \times M$ 的矩陣。

實作時，因為 $J_i$ 只有包含 1 維度，所以假設關節 $A$ 可以動的維度有 $(x, z)$，那其角加速度為 $\mathbf \omega_A = (\mathbf\omega_{Ax}, 0, \mathbf\omega_{Az})$。假設 $\mathbf \theta_i$ 代表 $A$ 的 $z$ 軸，那麼 $\mathbf \omega_i$ 為 $\mathbf \omega_{A} \times (0, 0 ,1) = \mathbf\omega_{Az}$。

¶ 2.5 Inverse of Jacobian

我們為了解 $d\mathbf \theta = \mathbf J^{-1} d \mathbf e$，已經得到 $\mathbf J$ 之後，我們要算 $\mathbf J^{-1}$，但因為反矩陣必須是方陣，但我們的 $\mathbf J$ 是 $3 \times M$ 的矩陣，所以無法直接取 Inverse，反之我們採用 Pseudo Inverse $\mathbf J^+$。

考慮以下步驟：

$$ d\mathbf e = \mathbf J d\mathbf \theta$$
$$ \mathbf J^T d\mathbf e = \mathbf J^T J d \mathbf \theta$$
$$ (\mathbf J^T \mathbf J)^{-1} \mathbf J^T d\mathbf e =(\mathbf J^T \mathbf J)^{-1}(\mathbf J^T \mathbf J)d \mathbf \theta$$
$$ (\mathbf J^T \mathbf J)^{-1} \mathbf J^T d\mathbf e =d \mathbf \theta$$
$$ \mathbf J^+ d\mathbf e =d \mathbf \theta$$
$$ \mathbf J^+ = (\mathbf J^T \mathbf J)^{-1} \mathbf J^T $$

運用 SVD 的性質：

$$SVD(J)=UΣV^∗$$

將剛剛的 $\mathbf J^+$ 代入拆解：

$\begin{aligned} J^{+} & = (J^{*} J)^{- 1} J^{*} \\ = (V Σ U^{*} U Σ V^{*})^{- 1} V Σ U^{*} \\ = (V Σ^{2} V^{*})^{- 1} V Σ U^{*} \\ = (V^{*})^{- 1} Σ^{- 2} V^{- 1} V Σ U^{*} \\ = V Σ^{- 2} Σ U^{*} \\ = V Σ^{- 1} U^{*} \end{aligned}$

¶ 2.6 IK 演算法

得到 $\mathbf J+$ 後我們就可以推算上一步的位置：

$$\mathbf \theta_{k} = \mathbf \theta_{k+1} - dt * \mathbf J^+(\mathbf \theta_{k+1}) * d\mathbf e$$

虛擬碼：

while(ABS(current_position - target_position) > epsilon and
      iterator < max_iterator):
    
    1. compute Jacobian J
    2. compute Pseoduinverse Jacobain J^+
    3. compute change in joint DoFs: dθ = (J^+)de
    4. apply the changes to joints. move a small step α: θ += αdθ
    5. update current_position to close target_position

¶ 3. 探討

¶ 3.1 參數影響

根據公式：
$$\mathbf \theta_{k} = \mathbf \theta_{k+1} - dt * \mathbf J^+(\mathbf \theta_{k+1}) * d\mathbf e$$
每次移動的步數 $dt$ 會影響計算速度，如果每次移動距離太小會導致計算太多次，但如果每次移動距離很大可能會不小心超過目標位置造成難以收斂。

為了避免因為無法收斂，導致迭代太多次，會設定一個誤差值 epsilon，在誤差容忍範圍內停止迭代，同時也會設定一個迭代的上限次數。

IK 事實上不一定有解，目標位置 $t$ 如果超過關節伸展最大長度，或是受限於可轉動的維度，是有機會達不到 $t$。想像一下你的手不管怎樣伸都碰不到天花板，你的手也不可能往外旋轉一直凹，這些情況都屬於無解。實作上也要考慮調整 $t$ 避免成是無解陷入崩潰。