# 機器人學

###### tags: `Robotics`
## 移動
向量表達空間關係的兩個方式
* A position in space(i.e. position vector)
以此表達body frame的原點
* A vector(i.e. displacement, frame basis)
以此表達body frame上的principal axes的方向
## 轉動
> 描述{B}香對於{A}的姿態

### Rotation Matrix
以B relative to A，即為R的三個column為由{A}看frame{B}的basis: $\hat{X}_B$, $\hat{Y}_B$, $\hat{Z}_B$
$$^A_BR=
\begin{bmatrix}
\hat{X}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{X}_A
\\
\hat{X}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Y}_A
\\\hat{X}_B \cdot \hat{Z}_A & \hat{Y}_B \cdot \hat{Z}_A & \hat{Z}_B \cdot \hat{Z}_A
\end{bmatrix}\quad$$
上式的運算又稱作"direct cosines"

**範例**
目標為找出
$$^A_BR=\begin{bmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{bmatrix}$$

$$
^A\hat{X}_B=\begin{bmatrix}
\hat{X}_B \cdot \hat{X}_A
\\
\hat{X}_B \cdot \hat{Y}_A
\\\hat{X}_B \cdot \hat{Z}_A
\end{bmatrix}=\begin{bmatrix}0.866\\0.5\\0\end{bmatrix}
其他兩個以此類推
$$
最後可以求出
$$^A_BR=\begin{bmatrix}0.866 & -0.5&0\\ 0.5&0.866&0\\0&0&1\end{bmatrix}$$

### Rotaion Matrix特性
因為內積的可交換性質，前述的$^A_BR$相當於$^B_AR^T$
$$
^A_BR=\begin{bmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{bmatrix}
=\begin{bmatrix}^B\hat{X}_A^T\\^B\hat{Y}_A^T\\^B\hat{Z}_A^T\end{bmatrix}=^B_AR^T
$$
將$^A_BR^T$與$^A_BR$相乘
$$^A_BR^T\ ^A_BR$$$$=^A_BR^{-1}\ ^A_BR=I_3$$
此為orthogonal matrix
* always invertible
* columns: orthogonal basis
* Length =1
* mutually perpendicular
* Rotation matrix( R )有9個數字，但是前述條件產生6個constraints因此仍然是只有3個DOF。
* determinent =1

### Rotaion matrix用於轉換向量之座標
original coordinate
$$^BP=^BP_X\ \hat{X}_B+^BP_Y\ \hat{Y}_B+^BP_Z\ \hat{Z}_B$$
new cooridinate
$$^BP=^AP_X\ \hat{X}_A+^AP_Y\ \hat{Y}_A+^AP_Z\ \hat{Z}_A$$
有以下關係
$$
^AP=\begin{matrix}\\\\\\\end{matrix}^A{\begin{bmatrix}P_X\\P_Y\\P_Z\end{bmatrix}}=
\begin{bmatrix}\hat{X}_B \cdot \hat{X}_A & \hat{Y}_B \cdot \hat{X}_A & \hat{Z}_B \cdot \hat{X}_A
\\
\hat{X}_B \cdot \hat{Y}_A & \hat{Y}_B \cdot \hat{Y}_A & \hat{Z}_B \cdot \hat{Y}_A
\\\hat{X}_B \cdot \hat{Z}_A & \hat{Y}_B \cdot \hat{Z}_A & \hat{Z}_B \cdot \hat{Z}_A\end{bmatrix}
\begin{matrix}\\\\\\\end{matrix}^B{\begin{bmatrix}P_X\\P_Y\\P_Z\end{bmatrix}}=^A_BR\ ^BP
$$
**範例**

### 描述物體轉動狀態
以針對三個pricial axes旋轉matrix為基礎
針對$\hat{Z}_A$旋轉$\theta$
$$R_{\hat{Z}_A}(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta & 0\\
\sin\theta & \cos\theta & 0\\
0 & 0 &1\end{bmatrix}$$
針對$\hat{X}_A$旋轉$\theta$
$$R_{\hat{X}_A}(\theta)=\begin{bmatrix}
1 & 0 &0\\
0 &\cos\theta & -\sin\theta\\
0 & \sin\theta & \cos\theta\end{bmatrix}$$
針對$\hat{Y}_A$旋轉$\theta$
$$R_{\hat{Y}_A}(\theta)=\begin{bmatrix}
\cos\theta & 0 & \sin\theta\\
0 & 1 &0\\
-\sin\theta & 0 &\cos\theta\end{bmatrix}$$
**範例**

### Rotation Matrix與轉角
Rotatin matrix並非commutable，即不可任意調換轉動運算子的順序。
如何將一般的rotation matrix所表達的姿態拆解到3次旋轉角度以對應到3個DOF？

兩個拆解方法
1. 對方向「固定不動」的旋轉軸旋轉：Fixed angles
2. 對「轉動的frame當下所在」的旋轉軸方向旋轉：Euler angles

#### Fixed Angles
**轉軸由X到Y到Z的例子**
$$^A_BR_{XYZ}(\gamma, \beta, \alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)$$因為轉動運算子操作的順序，先轉的放在「後面」。

**由R推算angle**
使用[$\text{atan2}$](https://zh.wikipedia.org/wiki/Atan2)，在幾何意義上，$\text{atan2}(y,x)$等價於$\text{atan}(\frac{y}{x})$，但$\text{atan2}$的最大優勢是可以正確處理$x=0$而$y\neq0$的情況，而不必進行會引發除以零異常的$\frac{y}{x}$操作。
$$^A_BR_{XYZ}(\gamma, \beta, \alpha)=
\begin{bmatrix}
\cos\alpha\cos\beta & \cos\alpha\sin\beta\sin\gamma-\sin\alpha\cos\gamma & \cos\alpha\sin\beta\cos\gamma+\sin\alpha\sin\gamma\\
\sin\alpha\cos\beta & \sin\alpha\sin\beta\sin\gamma+\cos\alpha\cos\gamma & \sin\alpha\sin\beta\cos\gamma-\cos\alpha\sin\gamma\\
-\sin\beta & \cos\beta\sin\gamma &\cos\beta\cos\gamma\end{bmatrix}=\begin{bmatrix}r_{11} & r_{12} & r_{13}\\
r_{21} & r_{22} & r_{23}\\
r_{31} & r_{32} &r_{33}\end{bmatrix}
$$
解上式得到
if $\beta\neq90^{\circ}$
$$\beta=\text{atan2}(-r_{31}, \sqrt{r_{11}^2+r_{21}^2})\\
\alpha=\text{atan2}(r_{21}/c\beta, r_{11}/c\beta)\\
\gamma=\text{atan2}(r_{32}/c\beta, r_{33}/c\beta)$$

if $\beta=90^{\circ}$
$$\alpha=0^{\circ}\\\gamma=\text{atan2}(r_{12},r_{22})$$

if $\beta=-90^{\circ}$
$$\alpha=0^{\circ}\\\gamma=-\text{atan2}(r_{12},r_{22})$$
**範例**

#### Euler Angles
**Z-Y-X Euler Angles**
$$^A_BR_{Z'Y'X'}=_{B'}^AR\ _{B''}^{B'}R\ _{B}^{B''}R=R_{Z'}(\alpha)R_{Y'}(\beta)R_{X'}(\gamma)$$
> 先轉的放在「前面」：以mapping來想對某一個向量，從最後一個frame，「逐漸移動或轉動」來回到第一個frame

$$
=\begin{bmatrix}\cos\theta & -\sin\theta & 0\\
\sin\theta & \cos\theta & 0\\
0 & 0 &1\end{bmatrix}\begin{bmatrix}
\cos\theta & 0 & \sin\theta\\
0 & 1 &0\\
-\sin\theta & 0 &\cos\theta\end{bmatrix}
\begin{bmatrix}
1 & 0 &0\\
0 &\cos\theta & -\sin\theta\\
0 & \sin\theta & \cos\theta\end{bmatrix}
\\=R_Z(\alpha)R_{Y}(\beta)R_{X}(\gamma)=^A_BR_{XYZ}(\gamma, \beta, \alpha)
$$
最後得到相同的$R$。

**Z-Y-Z Euler Angles**
$$^A_BR_{Z'Y'Z'}=R_{Z'}(\alpha)R_{Y'}(\beta)R_{Z'}(\gamma)\\
=\begin{bmatrix}c\alpha c\beta c\gamma - s\alpha s\gamma & -c\alpha c\beta s\gamma - s\alpha c\gamma & c\alpha s\beta\\s\alpha c\beta c\gamma + c\alpha s\gamma & -s\alpha c\beta s\gamma + c\alpha c\gamma & s\alpha s\beta \\ -s\beta c\gamma & s\beta s\gamma & c\beta\end{bmatrix}$$

**由R推算angle**
$$^A_BR_{XYZ}(\gamma, \beta, \alpha)=
\begin{bmatrix}c\alpha c\beta c\gamma - s\alpha s\gamma & -c\alpha c\beta s\gamma - s\alpha c\gamma & c\alpha s\beta\\s\alpha c\beta c\gamma + c\alpha s\gamma & -s\alpha c\beta s\gamma + c\alpha c\gamma & s\alpha s\beta \\ -s\beta c\gamma & s\beta s\gamma & c\beta\end{bmatrix}=\begin{bmatrix}r_{11} & r_{12} & r_{13}\\
r_{21} & r_{22} & r_{23}\\
r_{31} & r_{32} &r_{33}\end{bmatrix}
$$
解上式得到
if $\beta\neq0^{\circ}$
$$\beta=\text{atan2}( \sqrt{r_{31}^2+r_{32}^2},r_{33})\\
\alpha=\text{atan2}(r_{23}/s\beta, r_{13}/s\beta)\\
\gamma=\text{atan2}(r_{32}/s\beta, -r_{31}/s\beta)$$

if $\beta=0^{\circ}$
$$\alpha=0^{\circ}\\\gamma=\text{atan2}(-r_{12},r_{11})$$

if $\beta=180^{\circ}$
$$\alpha=0^{\circ}\\\gamma=\text{atan2}(r_{12},-r_{11})$$

**範例**

**小結**
Euler/Fixed angles
* 對任意的$R$我們有12種Euler angles和12種fixed angles的拆解方法($3\times 2\times 2$)
* 存在Duality(共有12種對principal axes連三次轉動的拆解方法)

其他表達法
[Quaternion 表達法](https://en.wikipedia.org/wiki/Quaternion)
Angle-axis 表達法