04. robot kinematics analysis (forward)

機器人運動學可分為兩種：

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

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

4-1. pronoun definition

這些名詞之後都會用英文表示，避免歧義。

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

4-2. matrix

我們先重新檢視一下高中學過的矩陣，和一些額外的小補充。

4-2-1. reference frame

我們可慮二維平面中有一點

$P$，和兩個 frame。

$P$ 在 frame 0 中的位置可以表示為

$P^0(6,8)$。

$P$ 在 frame 1 中的位置則可以表為

$P^1(-5,7)$。

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

類似地，上圖中的向量可以表示如下（長度亂寫，只須在意正負號）：

$V_1^0=(6,8)$、

$V_1^1=(8,0)$、

$V_2^1=(-5,7)$、

$V_2^0=(-4,2)$

4-2-2. 2D rotation matrix

從 frame

$XY$ 旋轉到 frame

$X^{\prime }{Y}^{\prime }$：

從這個例子我們可以輕易得到旋轉矩陣：

$\left[\begin{array}{cc}cos(\theta )& -sin(\theta )\\ sin(\theta )& cos(\theta )\end{array}\right]$。

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

$R_0^1=\left[\begin{array}{ccc}{x}_1^0& |& y_1^0\end{array}\right]=\left[\begin{array}{cc}{x}_1.x_0& y_1.x_0\\ x_1.y_0& y_1.y_0\end{array}\right]=\left[\begin{array}{cc}cos(\theta )& -sin(\theta )\\ sin(\theta )& cos(\theta )\end{array}\right]$

上式的

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

$P^{\prime }=R_0^{1}P\Rightarrow \left[\begin{array}{c}{P}_{x^{\prime }}\\ P_{y^{\prime }}\end{array}\right]=\left[\begin{array}{cc}cos(\theta )& -sin(\theta )\\ sin(\theta )& cos(\theta )\end{array}\right]\left[\begin{array}{c}{P}_x\\ P_y\end{array}\right]$

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

$P_m=R_0^{m-1}{P}_{m-1}=R_0^{m-1}{R}_0^{m-2}{P}_{m-2}=...=R_0^{m-1}{R}_0^{m-2}...R_0^1{P}_0$

4-2-3. 3D rotation matrix

我們上小節定義了 2D 的旋轉矩陣：

$R_0^1=\left[\begin{array}{ccc}{x}_1^0& |& y_1^0\end{array}\right]=\left[\begin{array}{cc}{x}_1.x_0& y_1.x_0\\ x_1.y_0& y_1.y_0\end{array}\right]$

現在推廣到 3D：

$R_0^1=\left[\begin{array}{ccccc}{x}_1^0& |& y_1^0& |& z_1^0\end{array}\right]=\left[\begin{array}{ccc}{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{array}\right]$

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

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

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

$R_0^1=\left[\begin{array}{ccc}cos(\theta )& -sin(\theta )& 0\\ sin(\theta )& cos(\theta )& 0\\ 0& 0& 1\end{array}\right]$，因為 Z 軸固定，特別記為

$R_{z,\theta }$。

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

$R_{x,\theta }=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\theta )& sin(\theta )\\ 0& -sin(\theta )& cos(\theta )\end{array}\right]$

$$,

$$

$R_{y,\theta }=\left[\begin{array}{ccc}cos(\theta )& 0& -sin(\theta )\\ 0& 1& 0\\ sin(\theta )& 0& cos(\theta )\end{array}\right]$

理論上對於三維的任意旋轉都可以將上三個矩陣相乘而得。
但如果坐標系是動態的，即隨物體坐標系轉動而轉動，將產生有著名的 gimble lock problem。

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

4-2-4. 4x4 matrix in 3D

雖然 3D 中只有三個座標，但實務上我們更喜歡使用四階矩陣來描述：

$P={\left[\begin{array}{cccc}x& y& z& 1\end{array}\right]}^T=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$

$H=\left[\begin{array}{cccc}{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{array}\right]$

其中

$t_x$、

$t_y$、

$t_z$ 是平移量，因此矩陣

$H$ 是旋轉矩陣與平移矩陣的融合。

這個矩陣

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

$HP=\left[\begin{array}{cccc}1& 0& {}_0& t_x\\ 0& 1& 0& t_y\\ 0& 0& 1& t_z\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=\left[\begin{array}{c}x+t_x\\ y+t_y\\ z+t_z\\ 1\end{array}\right]$

之後的 Devanit-Hartenberg's convention 中的 H 就會是這個形式。
H 代表的是 Homogeneous Transformation Matrix。

4-3. Denavit-Hartenberg Convention

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

這個方法分為三大步驟：

1. 標示 joint frame
2. 建立 parameters table
3. 代公式 homogeneous transformation matrix (HTM)

4-3-1. frame – drawing

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

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

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

4-3-2. frame – XYZ axis rule

以下是 Convention :

• 所有 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 軸向的原點偏移為零則允許。

以下是建立順序：

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

4-3-3. parameters table and HTM

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

這邊所有轉幾度、移了多少都是以前一個坐標系為參考。即

$X_m^{m-1}$。