中正平和的機器人學筆記——1. 機械臂正運動學（附MATLAB代碼）

中正平和的機器人學筆記——1.正運動學

0. 何爲運動學？何爲正運動學？

運動學主要研究的就是機械臂的運動特性（位置、速度、加速度等），但是不考慮使機械臂產生運動時施加的力和力矩（這部分是動力學）。而正運動學的研究即是將關節變量作爲自變量，進而研究操作臂末端執行器的位姿與基座間的函數關係。

1. D-H參數法

想要研究一個物體就要有一種模型/一種方法去描述它，在這個基礎上再進行研究。毋庸多言，直接給出最經典的D-H參數法，DH是兩位大佬（Denavit和Hartenberg）名字的縮寫。另外DH參數法有標準形式有改進形式，如果四個參數的下標是相同的“i”則是標準形式，如果有a和α下標是“i-1”而d和θ是“i”則是改進型是，改進形式記起來麻煩但是實際應用時則更爲方便。

D-H參數法採用4個運動學參數來描述一個連桿：

由上圖可以看到，四個參數分別爲：
連桿長度（link length） a_i-1：關節軸i-1和關節軸i之間公垂線的長度；
連桿扭轉角（link twist）α_i-1：關節軸i-1和關節軸i投影到垂直於 a_i-1的平面後構成的角（遵從右手法則，從i-1軸繞a_i-1轉向i軸）；
連桿偏距（link offset）d_i：沿i軸方向，a_i-1和a_i的距離，即a_i-1與關節軸i的交點到a_i與關節軸i的交點的距離；
關節角（joint angle）θ_i：平移a_i-1和a_i，繞關節軸i旋轉成的夾角。
通過D-H參數，我們可以有效地描述一個機構（連桿），例如對於6自由度（6個關節全是旋轉關節）機械臂，我們需要6組18個參數就可以完全描述固定的運動學參數（a，α，d），θ是自變量。

2. 連桿座標系

通常按照如下方式確定連桿上的座標系：座標系{i}的Z軸稱爲Z_i，並與i軸重合；原點位於連桿長a_i與關節軸i的交點處，X_i沿a_i方向由關節i指向關節i+1；Y_i則由右手定則確定。建立好的座標系如下圖：

根據上圖座標系，D-H參數也可定義爲：
a_i-1：沿X_i-1軸，從Z_i-1移動到Z_i的距離；
α_i-1：繞X_i-1軸，從Z_i-1旋轉到Z_i的角度；
d_i： 沿Z_i-1軸，從X_i-1移動到X_i的距離；
θ_i： 繞Z_i-1軸，從X_i-1旋轉到X_i的角度。

這樣理解D-H參數後，不論是使用還是記憶起來也會更加方便。
這裏想說一點，在機器人學的學習過程中，一定要注意不能僅僅記憶公式，要通過聯想空間中座標系的變化取理解，死記硬背是不行的（這一點在動力學部分更是如此）。

3. 連桿變換的推導—結合實例Puma560機械臂

數學工具我們都已經準備好了，現在就着手開始進行正運動學的推導！
首先介紹一下Puma560機械臂。它是一個六自由度的機械臂，且所有關節均爲轉動關節，即RRRRRR，6R機構（R代表該關節是旋轉關節，P代表該關節是位移關節）。這款機械臂大概是所有機器人學教材都會採用的一個非常經典的例子。人們對其分析已經很多很清晰了。

我們的分析步驟如下：

1. 建立6個連桿座標系；
2. 分析D-H參數；
3. 進行連桿變換推導；

最終我們就可以得到運動學方程。

3.1 座標系的建立

座標系的建立規則如下：

1. 找出各個關節軸，爲了便於分析可以畫出其延長線。每次分析時僅考慮相鄰兩個軸（i和i+1）；
2. 找出軸i和軸i+1的公垂線或者i軸和i+1軸的交點，以軸和公垂線的交點或者兩軸的交點爲連桿座標系{i}的原點，注意，上述第二種情況（i軸和i+1軸相交，這在工業機器人中是非常常見的），在這種情況下a_i-1顯然爲0；
3. 規定Z_i的指向；
4. 規定X_i的指向；如果軸i和軸i+1相交，則規定X_i垂直於軸i和軸i+1構成的平面；
5. 按右手定則確定Y_i的方向；
6. 當第一個關節變量爲0時，規定座標系{0}和座標系{1}重合。
   
   以上兩圖即是建立好的座標系，希望大家可以動手畫一畫，自己試着建立連桿座標系。包括後面正運動學和逆運動學的推導，都希望大家拿出紙筆自己試着畫一畫，算一算，不要只憑腦補。

3.2 D-H參數的確立

這裏參照第1節，大家可以推導出Puma560 6自由度機器人的D-H參數表。如果覺得太難，可以自己畫一個簡單的平面兩連桿機械臂來推導D-H參數，練練手後再來看這一節。
大家可以根據上表來檢查一下自己推導的結果是否正確，同時也可以參照這個表，加深對D-H參數和連桿座標系的理解。

3.3 連桿變換推導

這裏就要用到齊次變換矩陣的知識，不熟悉的同學可以搜一搜相關知識再來推導。
首先是座標系{0}到座標系{1}的變換，顯然是最簡單的旋轉變換；
座標系{1}到座標系{2}的變換，根據D-H參數表是繞X₁旋轉了-90°；
座標系{2}到座標系{3}的變換，是有位移也有旋轉；
後面的同理，大家可以根據位姿變換的原理推導出變換矩陣如下：

對於齊次變換矩陣的理解希望大家能夠結合實例深入理解。

比如座標系{3}到座標系{4}的變換，根據齊次變換矩陣可以看出：
{3}的原點沿X₃方向移動了a₃，沿Y₃方向移動了d₄，然後{4}中一點P(x,y,z)的z就是{3}中的y，當{4}沿軸4旋轉時，P的x和y則由{3}的x和z表示。

這個可能第一次看有點繞，因此更需要大家自行推導，結合實例去理解齊次變換矩陣。至少要達到看到齊次變換矩陣就能想到空間中兩個座標系位姿的圖來，這樣纔有利於理解後面的知識。

實際上到這一步我們的任務基本上就完成了。
將上述變換矩陣連乘。最終就可以得到末端座標下一點在基座標系下的位姿。

最後這個矩陣就是正運動學方程，矩陣中自變量是6個關節角，因此當確定了6個關節的角度以後，我們就可以確定機械臂末端位姿在基座標系下的位姿！

4. 座標系的標準命名

最後我們再完成一點收尾工作，對於一些座標系的命名我們需要明確一下。

根據上圖，我們給幾個常用的座標系說明一下：
基座標系（the base frame）{B}：位於操作臂的基座上，就是{0}。

固定座標系（the station frame）{S}：與任務相關，通常位於機器人工作臺上的一角，通常根據{B}確定，有時也稱之爲任務座標系（the task frame）、通用座標系（the universe frame）、世界座標系（the world frame）。

腕部座標系（The wrist frame）{W}：原點位於操作臂手腕上，隨着操作臂的末端連桿移動。

工具座標系（The tool frame）{T}：原點定義在機器人抓持工具的末端。

目標座標系（The goal frame）{G}：對機器人移動工具到達的位置描述。特指在機器人運動結束後時，工具座標系應當與目標座標系重合。

5. 正運動學MATLAB的實現

這裏不得不提一下Prof. Peter Corke的工具箱¹:
Robotics Toolbox和Machine Vision Toolbox。
這兩個工具箱非常便捷，當然現在MATLAB官方的Robotics工具箱也很好，大家可以都嘗試一下。
這裏我就以最簡單的兩連桿爲例，利用工具箱實現其正運動學。

function T = FK_position(a1, q1, a2, q2)
%a1：連桿1的長度
%q1：連桿1的關節角
%a2：連桿2的長度
%q2：連桿2的關節角

L(1) = Link([0 0 a1 0]); %建立連桿1，輸入D-H參數θ、d、α、a
L(2) = Link([0 0 a2 0]);
two_link = SerialLink(L, 'name', 'two link');%建立完整的機械臂對象

TOOL = two_link.fkine([q1 q2]) 
%fkine函數是foward kinematics的縮寫，即正運動學求解
%返回末端位姿的齊次變換矩陣
x = TOOL.t(1);
y = TOOL.t(2);
q0 = rad2deg(atan(y/x));
T = [x;y;q0];

two_link.plot([q1 q2])  %作圖顯示兩連桿機械臂

正運動學部分就講完啦，當然，講得很簡略，畢竟只是我個人學習過程的一點記錄，而且我總不能把自己演算的步驟也一步步貼上來，那未免太過無味冗長。這個系列我還會繼續寫下去，希望大家能多提意見，謝謝！

---

1. www.petercorke.com/RVC ↩︎