一、概述

　　機器人運動學研究的是機械臂各個連桿之間的位移關系、速度關系和加速度關系。比較經典的一本書推薦大家讀讀熊有倫的《機器人技術基礎》下載網址在這。本篇博文將從剛體的位姿描述講起，逐步過渡到D-H法運動學建模的方法與步驟，結合前幾篇博客所樹的Rob機器人的手臂建立D-H運動學模型，並編寫一個逆運動學運動學求解的程序。

　　（1）位姿描述

　　我們知道，剛體在世界坐標系裏需要通過位置和姿態兩個維度來描述。首先，位置描述很容易理解，就是坐標，例如點P的位姿通過坐標Px，Py，Pz來描述，如下所示。

　　目前它是三維形式，但是我們需要在其下面添加一維，使其變為四維形式，如下所示，這就是齊次坐標，多增加的這一維 ω 稱為比例因子，這裏取1。

　　姿態的描述的描述相對復雜一些，它表征的是剛體相對於世界坐標系的旋轉情況，我們首先需要在剛體上添加一個坐標系，用這個坐標系的姿態來代表剛體的姿態，如圖一所示，在世界坐標系中的剛體，我們建立坐標系X‘，Y‘，Z‘，然後分別額在這個坐標系上取單位失向量n，o，b。

圖1

　　剛體上的每個坐標軸 X‘，Y‘，Z‘ 都和世界坐標軸 X, Y, Z 之間在空間上存在夾角，我們以這些夾角為基礎對其姿態進行描述，便得到下面這個式子，矩陣 R 就是剛體相對於世界坐標系的姿態描述。此外還有其他的描述方法，比如歐拉角描述方法，四元數描述方法等。

　　我們將上面的 R 矩陣由三行三列的矩陣變換為四行三列的矩陣，最後一行補零，就得到了剛體姿態的其次矩陣，然後在將位置齊次矩陣加入進來組成四行四列的矩陣，就是剛體位姿描述的整體矩陣：T（也可以稱為轉換矩陣）

　　我們也可以將其理解為這是相鄰的兩個坐標系之間的轉換關系或相對關系，只是這個相對關系通過位置和姿態兩個維度來描述的。我們的機械臂是由一系列桿件和運動關節組成，如果我們以一定的規則在機械臂的關節處建立坐標系，就得到一系列的坐標系，如圖2所示，如果我們又知道這一系列坐標系中每相鄰兩個的轉換關系（即轉換矩陣），那麽通過推導我們就知道了首位兩個坐標系之間的轉換關系了，從而就可以實現機械臂的運動學描述了。

圖2

　　（2）正逆運動學

　　我們知道機器人運動學研究的是連桿之間的位移關系、速度關系和加速度關系，它又分為正運動學和逆運動學。

　　正運動學：知道機械臂各個關節的關節變量（如每個轉動關節轉了多少度，移動關節移動了對少距離）推導出末端執行器相對於世界坐標系的位姿關系（坐標系n相對坐標系0）。（解決的是我在哪的問題

　　逆運動學：知道末端執行器相對於世界坐標系的位姿關系（坐標系n相對坐標系0），推導出要到達這個位姿械臂各個關節的關節變量（如每個轉動關節需要轉多少度，移動關節需要移動多少距離）。（解決的是如何到那的問題）

二、D-H法

　　通過第一部分我們就可以分析知道，所謂的機器人運動學建模通俗講分為以下幾個步驟：

　　1. 以一定的規則在機械臂上創建一系列的坐標系。

　　2. 通過這些坐標系中每相鄰兩個坐標系之間的關系推導出最後一個坐標系（坐標系n）相對於第一個坐標系（坐標系0）的轉換關系（轉換矩陣）。

　　3. 通過這個轉換關系獲得機械臂正運動學矩陣。

　　4. 通過正運動學矩陣獲得逆運動學的求解方法。

　　具體以什麽樣的規則創建坐標系？如何才能快速獲得兩個相鄰坐標系之間的轉換關系？如何求取逆運動學解？這一些列問題早在1995年兩位前輩就給出了答案（膜拜，經典的東西往往經久不衰），它們就是 Denavit 和 Hartenberg，下面我們一步步看看它們如何解決這些問題的。

（1）描述連桿和連桿連接的參數

　　上述兩位大師將相鄰兩個連桿（剛體）之間的關系簡化成4個參數來描述，其中另個參數用來描述連桿，兩個參數用來描述兩個連桿的連接關系，先來說說描述連桿的兩個參數：ai-1 和 αi-1 ，如圖3所示。

圖3

　　Link length 連桿長度 ai-1 ，關節軸 i-1 和關節軸 i 之間的公垂線（圖3紅色直線）的長度ai-1。

　　Link twist 連桿轉角 αi-1，假設作一個平面,並使該平面與兩關節軸之間的公垂線垂直（圖3粉紅色平面）,然後把關節軸 i-1 和關節軸 i 投影到該平面上,在平面內軸 i-1 按照右手法則轉向軸 i ，測量兩軸角之間的夾角為αi-1.

　　再看一下描述兩個連桿的連接關系的兩個參數 di 和 θi，如圖4所示。

圖4

　　link offset 連桿偏距 di，相鄰兩個連桿之間有一個公共的關節, 沿著兩個相鄰連桿公共法線（圖4兩條黃線）的距離可以用一個參數描述為連桿偏距di。（當 i 為移動關節時,連桿偏距為一變量）

　　joint angle 關節角 θi， 兩個相鄰連桿的兩條公共法線（圖4兩條黃線）繞公共軸線（圖4紅色軸線）旋轉的夾角（圖4藍色夾角） θi。

　　用這四個參數就可以完成相鄰兩個連桿（剛體）之間的關系的描述。

（2）在機械臂上建立坐標系

　　Denavit 和 Hartenberg 確定了連桿關系的描述方法之後，又制定了坐標系的建立規則，長坐標系{0}到坐標系{n}的確定步驟如下：

　　a. 確定關節軸，並畫出軸的延長線。

　　b. 找出關節軸 i -1 和 i 的公垂線（圖3紅色直線）關節軸 i -1 的交點 ，作為坐標系 i-1 的原點 Oi-1。

　　c. 規定 Zi-1 的指向是沿著第 i-1 個關節軸。

　　d. 規定 Xi-1 軸的指向是沿著軸 i-1 和 i 的公垂線（圖3紅色直線）的方向，如果關節軸 i-1 和 i 相交，則Xi-1軸垂直於關節軸 i-1 和 i 所在的平面。

　　e. Yi-1軸的方向由右手定則確定 Yi-1 = Zi-1 × Xi-1 。

　　此外，對於坐標系{0} ，Z0軸沿著關節軸1的方向,當坐標系{1}的關節變量為0時，設定參考坐標系{0}與{1}重合，且a0=0, α0=0，當關節1為轉動關節，d1=0；當關節1為移動關節，θ1=0。

　　坐標系{n} 通常規定:　　　

　　　　對於轉動關節 n，設定θn=0，此時 Xn 和 Xn-1 軸的方向相同，選取坐標系{n} 的原點位置,使之滿足 dn=0;
　　　　對於移動關節 n， 設定Xn軸的方向使之滿足θn=0，當dn=0時,選取坐標系{n} 的原點位於Xn-1軸與關節軸n的交點位置。

（3）根據機械臂上所建立的坐標系確定描述連桿和連桿連接的參數

　　建立完坐標系以後，就需要根據坐標系來確定小節（1）中所說的描述連桿和連桿連接的四個參數，由於我們是根據特定規則來建立的坐標系，所以我們很容易得到四個參數，得到的方法如圖5所示。

圖5

三、Rob手臂的D-H法建模

　　下面我們解結合前面博文所介紹的Rob機器人的一個手臂，講解如何在它的手臂上建立坐標系，獲得描述參數，最後得到運動學關系，首先手臂的模型抽象如下圖6所示。

圖6

　　為了方便起見，我們讓手臂擡起來伸直，如圖7所示，這樣方便我們建立坐標系，然後我們按照第二部分的（2）小節介紹的方法建立坐標系，首先繪制關節軸線（關節轉軸） i1 到 i5（圖7中綠色虛線），然後根據規則確定原點 O1 到 O6的位置，然後確定每個坐標系Z軸的方向（圖7紅色軸，和關節軸線共線），然後確定各個坐標系的X軸，最後根據右手定則確定各個坐標系的Y軸，此外各個連桿的物理尺寸如圖7中的藍色虛線所標識。

圖7

　　一旦確定了坐標系，我們根據第二部分的（3）小節中圖5所示的法則就可以快速確定每個連桿（坐標系）之間的描述參數，如下表所示。

　　至此其實我們已經得到了Rob手臂上的坐標系{0}到坐標系{6}之間每兩個相鄰坐標系的轉換關系了，就是上表中 i 行的數據。但是拿到這些數據怎麽轉換成第一部分的（1）小節中所介紹的轉換矩陣 T 呢？如何根據兩個相鄰坐標系的轉換關系得到坐標系{6}到坐標系{0}的轉換關系呢？又如何求取逆運動學解呢？這幾個問題在下一篇博客中揭曉，並給出逆運動學求解的C++源代碼。

<-- 本篇完 -->

ROS系統MoveIt玩轉雙臂機器人系列（五）--淺議機器人運動學與D-H建模