## ¶ 1. 介紹

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

## ¶ 2. Jacobian Pseudoinverse 原理與實作

### ¶ 2.1 參數定義

$$\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}$$

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

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

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

### ¶ 2.2 動力學

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

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

### ¶ 2.3 Jacobian

$\mathbf{J}=\frac{d\mathbf{e}}{d\theta }$

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

$\mathbf J$ 表示成：

$\mathbf{J}=\left(\begin{array}{cccc}\frac{\mathrm{\partial }{f}_{1}}{d{\theta }_{1}}& \frac{\mathrm{\partial }{f}_{1}}{\mathrm{\partial }{\theta }_{2}}& \cdots & \frac{\mathrm{\partial }{f}_{1}}{\mathrm{\partial }{\theta }_{n}}\\ \frac{\mathrm{\partial }{f}_{2}}{\mathrm{\partial }{\theta }_{1}}& \frac{\mathrm{\partial }{f}_{2}}{\mathrm{\partial }{\theta }_{2}}& \cdots & \frac{\mathrm{\partial }{f}_{2}}{\mathrm{\partial }{\theta }_{n}}\\ ⋮& ⋮& \cdots & ⋮\\ \frac{\mathrm{\partial }{f}_{k}}{\mathrm{\partial }{\theta }_{1}}& \frac{\mathrm{\partial }{f}_{k}}{\mathrm{\partial }{\theta }_{2}}& \cdots & \frac{\mathrm{\partial }{f}_{k}}{\mathrm{\partial }{\theta }_{n}}\end{array}\right)$

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

### ¶ 2.4 求 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 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}$$

$$\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$$

$${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)$$

### ¶ 2.5 Inverse of Jacobian

$$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(J)=UΣV^∗$$

$\begin{array}{rl}{\mathbf{J}}^{+}& =\left({\mathbf{J}}^{\ast }\mathbf{J}{\right)}^{-1}{\mathbf{J}}^{\ast }\\ & =\left(V\mathrm{\Sigma }{U}^{\ast }U\mathrm{\Sigma }{V}^{\ast }{\right)}^{-1}V\mathrm{\Sigma }{U}^{\ast }\\ & =\left(V{\mathrm{\Sigma }}^{2}{V}^{\ast }{\right)}^{-1}V\mathrm{\Sigma }{U}^{\ast }\\ & =\left({V}^{\ast }{\right)}^{-1}{\mathrm{\Sigma }}^{-2}{V}^{-1}V\mathrm{\Sigma }{U}^{\ast }\\ & =V{\mathrm{\Sigma }}^{-2}\mathrm{\Sigma }{U}^{\ast }\\ & =V{\mathrm{\Sigma }}^{-1}{U}^{\ast }\end{array}$

### ¶ 2.6 IK 演算法

$$\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$$

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

### ¶ 3.2 可動關節數量對 IK 的影響

Case 1: 設定可以動手臂，以 IK 計算出反推位置：

Case 2: 設定可以動手臂和上半身，以 IK 計算出反推位置：

Case 3: 設定可以動屁股以上，以 IK 計算出反推位置：

## ¶ 4. 結論

IK 有許多種方法，本文只介紹了 Jacobian Pseudoinverse 方法，其特性是會一次更動所有關節，看似會比較真實，但實際上人物動作通常會有比較自然的姿勢，那就要改用會參考真實動作的演算法。根據使用需求可以採用不同演算法，這部分就留給讀者自行研究。