---
title: 【軟體】順向運動學求法
tags: TTennis Pickup Robot
disqus: hackmd
---

<h1 style="text-align: center; color: orange;">
🛠️ 【軟體】High-DoF Robots 🛠️
</h1>

<h2 style="text-align: center; color: skyblue;">順向運動學求法 </h2>

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

本章介系統性求正向運動學的方法 -- Denavit-Hartenberg Conevntion。

</br>

<h3><font color = "magenza">
4-1. 術語定義
</font></h3>

以下名詞都會用英文表示，避免歧義。

 

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

</br>

<h3><font color = "magenza">
4-2. Denavit-Hartenberg Convention
</font></h3>

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

</br>

### <font color = "pink">4-3-1. 畫圖 Joint & Link</font>

首先要將機器人簡化成如下的座標軸關聯圖。


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


</br>

### <font color = "pink">4-3-2. 標示 Frame</font>

以下是 Convention :
* 所有 frame 都是<font color = "yellow">右手座標系</font>。
* <font color = "yellow">Z 軸必須是轉動軸或伸縮軸</font>，end effector 時則自定義。
* <font color = "yellow">X 軸必須垂直於當前的 Z 軸與上一個 frame 的 Z 軸</font>。
* 起頭的 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 軸向的原點偏移為零則允許。

</br>

以下是建立順序：
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}$。

記錄完後就能計算 DH Transformation。

最後就能得出 end effector 的三維姿態。
其中旋轉矩陣 $R^0_n$ 是 end effector 的方向、$d^0_n$ 是 end effector 的位置。

</br> </br> </br> </br> </br>