機器人位置運動學03—D-H參數法

一、關節與連桿

　　機器人一般由一系列關節和連桿按任意的順序連接而成，這些關節可能是滑動（線性）的或旋轉（轉動）的，它們可能處在不同的平面，旋轉軸之間可能存在偏差。連桿也可以是任意長度的（包括零），它可能被扭曲或彎曲，也可能位於任意的平面上。所以任何一組關節和連桿都可以構成機器人。我們必須要能對任何機器人進行建模和分析。
　　所以，我們需要給每個關節指定一個參考座標系，然後確定從一個關節到下一個關節（一個座標系到下一個座標系）進行變換的步驟。如果將從基座到第1關節，再從第1關節到第2關節直至最後一個關節的所有變換結合起來，就得到了機器人的總變換矩陣。
　　假設機器人由任意多的連桿和關節以任意形式構成，如圖所示，有3個順序的關節和2個連桿，每個關節都可能是旋轉的、滑動的或兩者都是。指定第1個關節爲關節n$n$ ，第2個關節爲關節n+1$n+1$ ，第3個關節爲關節n+2$n+2$ 。連桿n$n$ 位於關節n$n$ 與n+1$n+1$ 之間，連桿n+1$n+1$ 位於關節n+1$n+1$ 與n+2$n+2$ 之間。
　　

　　　　

二、D-H參數法中x,y,z$x,y,z$ 軸及參數說明

　　爲了用D-H表示法對機器人建模，所要做的第一件事是爲每個關節指定一個本地的參考座標系。因此，對於每個關節，都必須指定一個z$z$ 軸和x$x$ 軸，通常並不需要指定y$y$ 軸，因爲y$y$ 軸總是垂直於x$x$ 軸和z$z$ 軸。

• z$z$ 軸：所有關節，無一例外都用z$z$ 軸表示。如果關節是旋轉的，那麼z$z$ 軸位於按右手規則旋轉的方向。如果關節是滑動的，那麼z$z$ 軸爲沿直線運動的方向。繞關節n+1$n+1$ 運動的z$z$ 軸是zn$z_n$ 。對於旋轉關節，繞z$z$ 軸的旋轉角θ$\theta$ 是關節變量。對於滑動關節，沿z$z$ 軸的連桿長度d$d$ 是關節變量。
• x$x$ 軸：通常關節不一定平行或相交。因此，z$z$ 軸也許是斜線，但總有一條距離最短的公垂線，它正交於任意兩條斜線。通常在公垂線方向上定義本地參考座標系的x$x$ 軸。所以如果an$a_n$ 表示zn−1$z_n-1$ 與zn$z_n$ 之間的公垂線，則定義xn$x_n$ 的方向爲沿an$a_n$ 的方向。
• 特殊情況：
  (1)：如果兩個關節的z$z$ 軸平行，那麼它們之間就有無數條公垂線。這時可挑選與前一關節的公垂線共線的一條公垂線。
  (2)：如果兩個相鄰關節的z$z$ 軸是相交的，那麼它們之間就沒有公垂線（或者公垂線的距離爲零）。這時可將垂直於兩條軸線構成的平面的直線指定爲x$x$ 軸，即其公垂線是垂直於包含了兩條z$z$ 軸平面的直線。
• θ$\theta$ ：表示繞z$z$ 軸的旋轉角
• d$d$ ：表示在z$z$ 軸上兩條相鄰的公垂線之前的距離（關節偏移）
• a$a$ ：表示每一條公垂線的長度（連桿長度）
• α$\alpha$ ：表示兩個相鄰的z$z$ 軸之間的角度（扭角）
  通常只有θ$\theta$ 和d$d$ 是關節變量。

三、D-H參數法的座標變換矩陣

　　現在需要將一個參考座標系變換到下一個參考座標系。假設現在位於本地參考座標系xn−zn$x_n-z_n$ ，則通過以下4步標準運動即可到達下一個本地參考座標系xn+1−zn+1$x_{n+1}-z_{n+1}$ 。
　　(1)繞zn$z_n$ 軸旋轉θn+1${\theta }_{n+1}$ ，使得xn$x_n$ 和xn+1$x_{n+1}$ 互相平行。因爲an$a_n$ 和an+1$a_{n+1}$ 都是垂直於zn$z_n$ 軸的，因此繞zn$z_n$ 軸旋轉θn+1${\theta }_{n+1}$ 確實可使xn$x_n$ 和xn+1$x_{n+1}$ 平行（且共面）。
　　(2)沿zn$z_n$ 軸平移dn+1$d_{n+1}$ 距離，使得xn$x_n$ 和xn+1$x_{n+1}$ 共線。因爲xn$x_n$ 和xn+1$x_{n+1}$ 已經平行且垂直於zn$z_n$ ，則沿着zn$z_n$ 移動可使它們相互重疊在一起。
　　(3)沿已經旋轉過的xn$x_n$ 軸平移an+1$a_{n+1}$ 的距離，使得xn$x_n$ 和xn+1$x_{n+1}$ 的原點重合。這時兩個參考座標系的原點處在同一位置。
　　(4)將z$z$ 軸繞xn+1$x_{n+1}$ 軸旋轉αn+1${\alpha }_{n+1}$ ，使得zn$z_n$ 軸與zn+1$z_{n+1}$ 軸對準。這時座標系n$n$ 和n+1$n+1$ 完全相同。這樣就實現了從一個座標系變換到下一個座標系。
　　在座標系n+1$n+1$ 和n+2$n+2$ 之間，嚴格地按照上述的4個步驟就可以將一個座標系變換到下一個座標系。通過重複以上步驟，可以實現一系列相鄰座標系之間的變換。從機器人的參考座標系開始，我們可以將其轉換到機器人的第1個關節，在轉換到第2個關節，以此類推，直至轉換到末端執行器。
　　表示前面4個運動的兩個依次座標系之間的變換nTn+1${}^n{T}_{n+1}$ （稱爲An+1$A_{n+1}$ ）是4個運動變換矩陣的乘積。由於所有的變換都是相對於當前的座標系進行的，因此所有的變換都是右乘的。即
　　

nTn+1=An+1=Rot(z,θn+1)×Trans(0,0,dn+1)×Trans(an+1,0,0)×Rot(x,αn+1)$${}^n{T}_{n+1}=A_{n+1}=Rot(z,{\theta }_{n+1})\times Trans(0,0,d_{n+1})\times Trans(a_{n+1},0,0)\times Rot(x,{\alpha }_{n+1})$$

　

=⎡⎣⎢⎢⎢cosθn+1sinθn+100−sinθn+1cosθn+10000100001⎤⎦⎥⎥⎥×⎡⎣⎢⎢⎢10000100001000dn+11⎤⎦⎥⎥⎥×⎡⎣⎢⎢⎢100001000010an+1001⎤⎦⎥⎥⎥×⎡⎣⎢⎢⎢10000cosαn+1sinαn+100−sinαn+1cosαn+100001⎤⎦⎥⎥⎥$$=\left[\begin{array}{cccc}cos{\theta }_{n+1}& -sin{\theta }_{n+1}& 0& 0\\ sin{\theta }_{n+1}& cos{\theta }_{n+1}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_{n+1}\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& a_{n+1}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos{\alpha }_{n+1}& -sin{\alpha }_{n+1}& 0\\ 0& sin{\alpha }_{n+1}& cos{\alpha }_{n+1}& 0\\ 0& 0& 0& 1\end{array}\right]$$

An+1=⎡⎣⎢⎢⎢cosθn+1sinθn+100−sinθn+1cosαn+1cosθn+1cosαn+1sinαn+10sinθn+1sinαn+1−cosθn+1sinαn+1cosαn+10an+1cosθn+1an+1sinθn+1dn+11⎤⎦⎥⎥⎥$$A_{n+1}=\left[\begin{array}{cccc}cos{\theta }_{n+1}& -sin{\theta }_{n+1}cos{\alpha }_{n+1}& sin{\theta }_{n+1}sin{\alpha }_{n+1}& a_{n+1}cos{\theta }_{n+1}\\ sin{\theta }_{n+1}& cos{\theta }_{n+1}cos{\alpha }_{n+1}& -cos{\theta }_{n+1}sin{\alpha }_{n+1}& a_{n+1}sin{\theta }_{n+1}\\ 0& sin{\alpha }_{n+1}& cos{\alpha }_{n+1}& d_{n+1}\\ 0& 0& 0& 1\end{array}\right]$$

　　在機器人基座上，可以從第1個關節開始變換到第2個關節，然後到第3個關節等等，直到機器人手和最終的末端執行器。若把每個變換定義爲An+1$A_{n+1}$ ，則可以得到許多表示變換的A$A$ 矩陣。在機器人的基座與手之間的總變換則爲：
　　

RTH=RT11T22T3...n−1Tn=A1A2A3...An$${}^R{T}_H{=}^R{T}_1^1{T}_2^2{T}_3..{.}^{n-1}{T}_n=A_1{A}_2{A}_3...A_n$$

　　其中n$n$ 是關節數，有幾個自由度就有幾個A$A$ 矩陣。通常可以製作一張關節和連桿參數的表格，其中每個連桿和關節的參數值可從機器人的機構示意圖上確定。

連桿	θ$\theta$	d$d$	a$a$	α$\alpha$
0-1
1-2

四、正運動學例題

例1：如圖所示的簡單2軸平面機器人，根據D-H表示法，建立必要的座標系，填寫D-H參數表，導出每個機器人的正運動學方程。

解：首先兩個關節都在x−y$x-y$ 平面內旋轉，座標系xH−zH$x_H-z_H$ 表示機器人的末端。先指定關節的z$z$ 軸，將關節1指定爲z0$z_0$ ，關節2指定爲z1$z_1$ ，座標系0是固定不動的，機器人相對於它而運動。
　　然後爲每一個座標系指定x$x$ 軸，因爲第1個座標系（座標系0）是在機器人基座上，在它之前沒有關節，因此x0$x_0$ 的方向是任意的。我們可以指定x0$x_0$ 的方向與全局座標系的x$x$ 軸相同。因爲z0$z_0$ 和z1$z_1$ 是平行的，他們之間的公垂線就在兩者之間的方向上，所以x1$x_1$ 軸如圖所示。
　　下表顯示了該機器人的變量表。根據D-H的常規步驟，按照如下從一個座標系到下一個座標系所必須的4個變換，可以來確認變量表中的這些值。

連桿	θ$\theta$	d$d$	a$a$	α$\alpha$
0-1	θ1${\theta }_1$	0	a1$a_1$	0
1-H$H$	θ2${\theta }_2$	0	a2$a_2$	0

　　(1)繞z0$z_0$ 軸旋轉θ1${\theta }_1$ ，使x0$x_0$ 和x1$x_1$ 平行；
　　(2)由於x0$x_0$ 和x1$x_1$ 已經在一個平面內了，不需要在平移z0$z_0$ 軸，因此沿着z0$z_0$ 軸的偏移量d$d$ 是0；
　　(3)沿着已經旋轉過的x0$x_0$ 軸平移距離a1$a_1$ ；
　　(4)因爲z0$z_0$ 和z1$z_1$ 是平行的，不需要在旋轉，因此繞x1$x_1$ 軸的旋轉角α1${\alpha }_1$ 是0。
　　座標系1到座標系H$H$ 之間的變換與上面的過程類似。
　　由於有兩個旋轉關節，因此存在兩個未知的變量，即關節角θ1${\theta }_1$ 和θ2${\theta }_2$ 。將D-H參數表中的這些參數代入相應的A矩陣中，可以得到機器人的正運動學方程爲：
　　

A1=⎡⎣⎢⎢⎢cosθ1sinθ100−sinθ1cosθ1000010a1cosθ1a1sinθ101⎤⎦⎥⎥⎥,　A2=⎡⎣⎢⎢⎢cosθ2sinθ200−sinθ2cosθ2000010a2cosθ2a2sinθ201⎤⎦⎥⎥⎥$$A_1=\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& a_{1}cos{\theta }_1\\ sin{\theta }_1& cos{\theta }_1& 0& a_{1}sin{\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right], A_2=\left[\begin{array}{cccc}cos{\theta }_2& -sin{\theta }_2& 0& a_{2}cos{\theta }_2\\ sin{\theta }_2& cos{\theta }_2& 0& a_{2}sin{\theta }_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

　　

0TH=A1×A1=⎡⎣⎢⎢⎢⎢cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010a2cos(θ1+θ2)+a1cosθ1a2sin(θ1+θ2)+a1sinθ101⎤⎦⎥⎥⎥⎥$${}^0{T}_H=A_1\times A_1=\left[\begin{array}{cccc}cos({\theta }_1+{\theta }_2)& -sin({\theta }_1+{\theta }_2)& 0& a_{2}cos({\theta }_1+{\theta }_2)+a_{1}cos{\theta }_1\\ sin({\theta }_1+{\theta }_2)& cos({\theta }_1+{\theta }_2)& 0& a_{2}sin({\theta }_1+{\theta }_2)+a_{1}sin{\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

　　給定θ1、θ2、a1${\theta }_1、{\theta }_2、a_1$ 和a2$a_2$ ，根據正運動學方程就可以求出機器人末端的位置和姿態。

五、逆運動學例題

　　有了逆運動學解才能確定每個關節的值，從而使機器人到達期望的位姿。
例2：求解例1中的關節角。
解1：設期望的位置和姿態爲：

0TH=A1×A1=⎡⎣⎢⎢⎢⎢cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010a2cos(θ1+θ2)+a1cosθ1a2sin(θ1+θ2)+a1sinθ101⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢nxnynz0oxoyoz0axayaz0pxpypz1⎤⎦⎥⎥⎥⎥$${}^0{T}_H=A_1\times A_1=\left[\begin{array}{cccc}cos({\theta }_1+{\theta }_2)& -sin({\theta }_1+{\theta }_2)& 0& a_{2}cos({\theta }_1+{\theta }_2)+a_{1}cos{\theta }_1\\ sin({\theta }_1+{\theta }_2)& cos({\theta }_1+{\theta }_2)& 0& a_{2}sin({\theta }_1+{\theta }_2)+a_{1}sin{\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]$$

則可得：sin(θ1+θ2)=ny$sin({\theta }_1+{\theta }_2)=n_y$ 且cos(θ1+θ2)=nx$cos({\theta }_1+{\theta }_2)=n_x$ ，所以：θ1+θ2=ATAN2(ny,nx)${\theta }_1+{\theta }_2=ATAN2(n_y,n_x)$ 。
又因爲a2cos(θ1+θ2)+a1cosθ1=px⇒a2nx+a1cosθ1=px$a_{2}cos({\theta }_1+{\theta }_2)+a_{1}cos{\theta }_1=p_x\Rightarrow a_2{n}_x+a_{1}cos{\theta }_1=p_x$ ，所以：cosθ1=px−a2nxa1$cos{\theta }_1=\frac{p_x-a_2{n}_x}{a_1}$ 。
又因爲a2sin(θ1+θ2)+a1sinθ1=py⇒a2ny+a1sinθ1=py$a_{2}sin({\theta }_1+{\theta }_2)+a_{1}sin{\theta }_1=p_y\Rightarrow a_2{n}_y+a_{1}sin{\theta }_1=p_y$ ，所以：sinθ1=px−a2nxa1$sin{\theta }_1=\frac{p_x-a_2{n}_x}{a_1}$ 。
所以：θ1=ATAN2(sinθ1,cosθ1)=ATAN2(px−a2nxa1,px−a2nxa1)${\theta }_1=ATAN2(sin{\theta }_1,cos{\theta }_1)=ATAN2(\frac{p_x-a_2{n}_x}{a_1},\frac{p_x-a_2{n}_x}{a_1})$ 。所以θ2=θ1+θ2−θ1${\theta }_2={\theta }_1+{\theta }_2-{\theta }_1$ 。
解2：在等式中通常乘A−12$A_2^{-1}$ ，使得θ1${\theta }_1$ 從θ2${\theta }_2$ 中解耦。（常用於多自由度的）

A1×A2×A−12=⎡⎣⎢⎢⎢⎢nxnynz0oxoyoz0axayaz0pxpypz1⎤⎦⎥⎥⎥⎥×A−12⇒A1=⎡⎣⎢⎢⎢⎢nxnynz0oxoyoz0axayaz0pxpypz1⎤⎦⎥⎥⎥⎥×A−12$$A_1\times A_2\times A_2^{-1}=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\times A_2^{-1}\Rightarrow A_1=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\times A_2^{-1}$$

⎡⎣⎢⎢⎢cosθ1sinθ100−sinθ1cosθ1000010a1cosθ1a1sinθ101⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢nxnynz0oxoyoz0axayaz0pxpypz1⎤⎦⎥⎥⎥⎥×⎡⎣⎢⎢⎢cosθ2sinθ200−sinθ2cosθ2000010−a2001⎤⎦⎥⎥⎥$$\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& a_{1}cos{\theta }_1\\ sin{\theta }_1& cos{\theta }_1& 0& a_{1}sin{\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]\times \left[\begin{array}{cccc}cos{\theta }_2& -sin{\theta }_2& 0& -a_2\\ sin{\theta }_2& cos{\theta }_2& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

⎡⎣⎢⎢⎢cosθ1sinθ100−sinθ1cosθ1000010a1cosθ1a1sinθ101⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢cosθ2nx−sinθ2oxcosθ2ny−sinθ2oycosθ2nz−sinθ2oz0sinθ2nx+cosθ2oxsinθ2ny+cosθ2oysinθ2nz+cosθ2oz0axayaz0px−a2nxpy−a2nypz−a2nz1⎤⎦⎥⎥⎥⎥$$\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& a_{1}cos{\theta }_1\\ sin{\theta }_1& cos{\theta }_1& 0& a_{1}sin{\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}cos{\theta }_2{n}_x-sin{\theta }_2{o}_x& sin{\theta }_2{n}_x+cos{\theta }_2{o}_x& a_x& p_x-a_2{n}_x\\ cos{\theta }_2{n}_y-sin{\theta }_2{o}_y& sin{\theta }_2{n}_y+cos{\theta }_2{o}_y& a_y& p_y-a_2{n}_y\\ cos{\theta }_2{n}_z-sin{\theta }_2{o}_z& sin{\theta }_2{n}_z+cos{\theta }_2{o}_z& a_z& p_z-a_2{n}_z\\ 0& 0& 0& 1\end{array}\right]$$

對應位置矩陣元素相等即可求得θ1${\theta }_1$ 和θ2${\theta }_2$ ，其結果和解1一樣。

參考文獻：SaeedB.Niku 等，機器人學導論——分析、系統及應用（第二版），電子工業出版社，2013.2.