---
tags: 機器狗勾
---

# 04. robot kinematics analysis (forward)
機器人運動學可分為兩種：
* **順向運動學(forward kinematics)**
已知各關節角度或伸縮長度，推算末端的座標的過程。
* **逆向運動學(backward kinematics)**
已知機器人的末端目標點，回推各關節應該如何旋轉或伸縮。

此節前面的鋪陳，都是為了最後學習 Denavit-Hartenberg Conevntion，一種<font color="yellow">很有系統的分析正向運動學的方法</font>。

## <font color = "orange">4-1. pronoun definition</font>
> 這些名詞之後都會用英文表示，避免歧義。

 

* <font color="magenza"> Joint </font>：**有自由度的點**。分為有 revolute 和 prismatic 兩種。
* revolute：自由度是旋轉。實際機構就是各種馬達。
* prismatic：自由度是伸縮。實際機構有線性滑軌、滑塊等。
* <font color="magenza"> link</font>：**連接兩點的物件**。本身不具有自由度。
* <font color="magenza"> frame</font>：**坐標系**。通常我們的 frame 會以各個 joint 為參考點。
* <font color="magenza"> end effector</font>：**機器人末端接點**。在左圖中是 gripper，右圖中是 frame4 的原點。

## <font color = "orange">4-2. matrix</font>
我們先重新檢視一下高中學過的矩陣，和一些額外的小補充。

### <font color = "pink">4-2-1. reference frame</font>
我們可慮二維平面中有一點 $P$，和兩個 frame。


$P$ 在 frame 0 中的位置可以表示為 $P^0(6,8)$。
$P$ 在 frame 1 中的位置則可以表為 $P^1(-5,7)$。

:::info
**注意上標代表的是 reference frame，代表我們是從什麼 frame 去看的。**
:::

類似地，上圖中的向量可以表示如下（長度亂寫，只須在意正負號）：
$V^0_1=(6,8)$、$V^1_1=(8,0)$、$V^1_2=(-5,7)$、$V^0_2=(-4,2)$

### <font color = "pink">4-2-2. 2D rotation matrix </font>
從 frame $XY$ 旋轉到 frame $X'Y'$：


從這個例子我們可以輕易得到旋轉矩陣：$\begin{bmatrix}cos(\theta) & -sin(\theta) \\sin(\theta) & cos(\theta) \end{bmatrix}$。

單從幾何學分析看待有點太「特殊」了，我們用更宏觀的方式去看待旋轉矩陣。
從 frame 0 旋轉到 frame 1，<font color="yellow">旋轉矩陣表示了 frame 1 的 X、Y 軸在 frame 0 中的樣貌</font>：

$R^1_0=\begin{bmatrix}x^0_1 &|& y^0_1\end{bmatrix}=\begin{bmatrix}x_1.x_0 & y_1.x_0\\x_1.y_0 & y_1.y_0 \end{bmatrix}=\begin{bmatrix}cos(\theta) & -sin(\theta) \\sin(\theta) & cos(\theta) \end{bmatrix}$

上式的 $xy$ 都是單位向量。
在上圖中，透過旋轉矩陣我們可以計算新的 P 點：

$P'=R^1_0\,P\Rightarrow\begin{bmatrix}P_{x'}\\P_{y'}\end{bmatrix}=\begin{bmatrix}cos(\theta) & -sin(\theta) \\sin(\theta) & cos(\theta) \end{bmatrix}\begin{bmatrix}P_{x}\\P_{y}\end{bmatrix}$

如果經歷了多次旋轉變換則是：

$P_m=R^{m-1}_0\,P_{m-1}=R^{m-1}_0\,R^{m-2}_0\,P_{m-2}=\;...\,=R^{m-1}_0\,R^{m-2}_0...R^1_0\,P_0$

### <font color = "pink">4-2-3. 3D rotation matrix </font>
我們上小節定義了 2D 的旋轉矩陣：

$R^1_0=\begin{bmatrix}x^0_1 &|& y^0_1\end{bmatrix}=\begin{bmatrix}x_1.x_0 & y_1.x_0\\x_1.y_0 & y_1.y_0 \end{bmatrix}$

現在推廣到 3D：
$R^1_0=\begin{bmatrix}x^0_1 &|& y^0_1 &|& z^0_1\end{bmatrix}=\begin{bmatrix}x_1.x_0 & y_1.x_0 & z_1.x_0\\x_1.y_0 & y_1.y_0 & z_1.y_0\\x_1.z_0 & y_1.z_0 & z_1.z_0\end{bmatrix}$

:::warning
2D 的旋轉只有 theta，但 3D 有三種軸向的旋轉 raw, yall, pitch。
用旋轉角度表達旋轉矩陣就會很麻煩。
**這是為什麼上個小節要先以內積的方式來表達旋轉矩陣。
不管在概念或數學表示上我個人認為都比較好。**
:::

接著我們考慮 Z軸不動，只有 XY 座標旋轉的三維旋轉：


根據上述內積定義，我們可以得到

$R^1_0=\begin{bmatrix}cos(\theta) & -sin(\theta) & 0\\sin(\theta) & cos(\theta) & 0\\0 & 0 & 1\end{bmatrix}$，因為 Z 軸固定，特別記為 $R_{z,\theta}$。

同理分別對於 X 軸、Y 軸固定的旋轉也可表示為：

$R_{x,\theta}=\begin{bmatrix}1 & 0 & 0\\0 & cos(\theta) & sin(\theta)\\0 & -sin(\theta) & cos(\theta)\end{bmatrix}$$\;$, $\;$$R_{y,\theta}=\begin{bmatrix}cos(\theta)& 0 & -sin(\theta)\\0 & 1 & 0\\sin(\theta) & 0 & cos(\theta)\end{bmatrix}$

理論上對於三維的任意旋轉都可以將上三個矩陣相乘而得。
但如果坐標系是動態的，即隨物體坐標系轉動而轉動，將產生有著名的 [gimble lock problem](https://www.youtube.com/watch?v=zc8b2Jo7mno&t=418s&ab_channel=GuerrillaCG)。

不過這不是本節重點，現在知道旋轉矩陣怎麼應用的就可以了。

### <font color = "pink">4-2-4. 4x4 matrix in 3D </font>
雖然 3D 中只有三個座標，但實務上我們更喜歡使用四階矩陣來描述：

$P =\begin{bmatrix}x&y& z &1\end{bmatrix}^T =\begin{bmatrix}x\\y\\z\\1\end{bmatrix}$

$H =\begin{bmatrix}x_1.x_0 & y_1.x_0 & z_1.x_0 & t_x\\x_1.y_0 & y_1.y_0 & z_1.y_0& t_y\\x_1.z_0 & y_1.z_0 & z_1.z_0& t_z\\0 & 0 & 0& 1\end{bmatrix}$

其中 $t_x$、$t_y$、$t_z$ 是平移量，因此矩陣 $H$ 是旋轉矩陣與平移矩陣的融合。

<font color="yellow">這個矩陣 $H$ 是為了方便我們計算而構建出來的，與數學證明無關。</font>
多出來的一個維度並沒有實質上的意義，僅僅為了計算方便。
我們只考慮平移而不旋轉就會懂為什麼要這樣了：

$HP =\begin{bmatrix}1 & 0 & _0 & t_x\\0 & 1 & 0& t_y\\0 & 0 & 1& t_z\\0 & 0 & 0& 1\end{bmatrix}\begin{bmatrix}x\\y\\z\\1\end{bmatrix}=\begin{bmatrix}x+t_x\\y+t_y\\z+t_z\\1\end{bmatrix}$

之後的 Devanit-Hartenberg's convention 中的 H 就會是這個形式。
<font color="yellow">H 代表的是 Homogeneous Transformation Matrix。</font>

## <font color = "orange">4-3. Denavit-Hartenberg Convention</font>

終於 QQ .... 這是一個**超棒的**分析機器人正向運動學的方法。

這個方法分為三大步驟：
1. 標示 joint frame
2. 建立 parameters table
3. 代公式 homogeneous transformation matrix (HTM)

### <font color = "pink">4-3-1. frame -- drawing</font>

根據上述所說，一般我們只會標示機器人的 joint 和 frame 資訊：


有時候為了標示清楚 revolute joint 的軸向會加上平面，但這看個人習慣。


這邊先注意關節的畫法，完整的 frame 後面小節會有。

</br>

### <font color = "pink">4-3-2. frame -- XYZ axis rule</font>

<font color = "yellow">以下是 Convention</font> :
* 所有 frame 都是右手座標系。
* Z 軸必須是轉動軸或伸縮軸，非 joint 時則自定義。
* X 軸必須是垂直於當前的 Z 軸與上一個 frame 的 Z 軸。
* 起頭的 frame 0 的 X 軸只要注意不要和 frame 1 的 Z 軸同向。
* 最後的 frame n 的 X 軸只要注意不要和 frame n-1 的 Z 軸。
* 當 X、Z 軸確定時，根據右手法則 Y 軸必有唯一解。
* 兩相鄰 frame M 和 M-1 的原點不能在 M-1 的 Y 軸上有偏移。如果可以透過 M-1 的旋轉使得該 Y 軸向的原點偏移為零則允許。

<font color = "yellow">以下是建立順序</font>：
1. 根據 joint 軸向標出所有 frame 的 Z 軸，起頭和最後的 frame 之 Z 軸自定義。
2. 標出所有 frame 的 X 軸，注意 X 不可和相鄰的 frame 之 Z 軸同向。
3. 以右手法則標出所有 Y 軸。
4. 檢查相鄰坐標系沒有 Y 軸向上的偏移，有的話回第二步重建 frame。

</br>

### <font color = "pink">4-3-3. parameters table and HTM</font>

| $\theta$ | $\alpha$ | $d$ | $l$ |
|:------------:|:------------:|:--------------------:|:--------------------------:|
| Z 軸旋轉角度 | X 軸旋轉角度 | frame 中心 的 Z 軸偏移 | frame 中心的 X 軸偏移 |

這邊所有轉幾度、移了多少都是以前一個坐標系為參考。即 $X_m^{m-1}$。