機器人學——3.2-正運動學

機械臂的正運動學通常表述為如下的函數形式：
$\xi_E=\mathcal{K}(q)$它表明末端執行器的位姿是基於關節坐標的一個函數。若使用齊次變換，其表達式將是由之前推導的連桿坐標系變換矩陣方程所給的單個連桿變換矩陣 $^{j-1}A_j$ 的簡單乘積。對於一個 $N$ 軸機械臂，有
$\xi_E\sim{}^0T_E={}^0A_1{}^1A_2\cdots{}^{N-1}A_N$
對於任何一個串聯機械臂，無論其關節的數量和類型如何，都可以計算出其正向運動學的解。我們以后會詳細討論如何確定機器人上每個連桿的 D-H 參數。

末端執行器的位姿 $\xi_E\sim T_E\in SE(3)$ 共有 $6$ 個自由度—— $3$ 個移動自由度和 $3$ 個轉動自由度。因此為了使末端執行器具有任意的位姿，機械臂通常具有 $6$ 個關節或者自由度。對於一個 $6$ 軸機器人，其總的變換矩陣通常被寫作 $T_6$。

兩連桿機器人

我們討論一個如圖所示的兩連桿平面機械手。它的 D-H 參數如下

連桿	$\theta_i$	$d_i$	$a_i$	$\alpha_i$	$\sigma_i$
$1$	$q_1$	$0$	$1$	$0$	$0$
$2$	$q_2$	$0$	$1$	$0$	$0$

然后我們用這些參數創建一個Link對象的向量：

>> L(1) = Link([0 0 1 0]);
>> L(2) = Link([0 0 1 0]);
>> L
L = 
 theta=q1, d=          0, a=          1, alpha=          0, offset=          0 (R,stdDH)
 theta=q2, d=          0, a=          1, alpha=          0, offset=          0 (R,stdDH)

這個向量將被傳遞給構造函數SerialLink：

>> two_link = SerialLink(L, 'name', 'two link');
>> two_link
two_link = 
two link (2 axis, RR, stdDH, fastRNE)                            
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          1|          0|          0|
|  2|         q2|          0|          1|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
grav =    0  base = 1  0  0  0   tool =  1  0  0  0              
          0         0  1  0  0           0  1  0  0              
       9.81         0  0  1  0           0  0  1  0              
                    0  0  0  1           0  0  0  1      

它將會返回一個SerialLink對象，顯示如下：它為這個機器人提供了一種簡明的描述。可以看出，該機器人的結構字符串為“RR”，表明它有兩個轉動關節，而且它是使用標准的 D-H 參數定義的，另外重力默認作用於z軸方向（與機器人運動平面垂直）。同時對象中還列出了連桿的運動學參數，而關節變量用“q1”和“q2”表示。我們還為機器人取了一個名字，它在任何時候顯示機器人時都會出現。輸入指令：

>> mdl_planar2

它將直接執行上述步驟，將該機器人定義在 MATLAB 的工作空間，並創建名為twolink的SerialLink對象。
SerialLink對象的一些簡單的操作如下

>> two_link.n
ans =
     2

該指令返回關節數目。

>> links = two_link.links
links = 
 theta=q1, d=          0, a=          1, alpha=          0, offset=          0 (R,stdDH)
 theta=q2, d=          0, a=          1, alpha=          0, offset=          0 (R,stdDH)

該指令返回一個Link對象的向量，Link對象中包含機器人。我們還可以復制一個SerialLink對象，並命名為foo：

>> clone = SerialLink(two_link, 'name', 'foo')
clone = 
foo (2 axis, RR, stdDH, fastRNE)                                 
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          1|          0|          0|
|  2|         q2|          0|          1|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
grav =    0  base = 1  0  0  0   tool =  1  0  0  0              
          0         0  1  0  0           0  1  0  0              
       9.81         0  0  1  0           0  0  1  0              
                    0  0  0  1           0  0  0  1      

現在可以讓機械臂工作了。正向運動學的計算使用了fkine方法。這個例子中， $q_1=q_2=0$。

>> two_link.fkine([0 0])
ans =
     1     0     0     2
     0     1     0     0
     0     0     1     0
     0     0     0     1

該方法將返回代表機器人第二個連桿坐標系位姿的齊次變換 $T_2$。對於一個不同的位形，工具位姿為

>> two_link.fkine([pi/4 -pi/4])
ans =
    1.0000         0         0    1.7071
         0    1.0000         0    0.7071
         0         0    1.0000         0
         0         0         0    1.0000

按照慣例，機器人工具箱使用的關節坐標都是行向量。
用以下指令可以畫出機器人的圖形：

>> two_link.plot([0 0])

另一種位姿

>> two_link.plot([pi/4 -pi/4])

圖中包含了機器人的名稱，末端連桿的坐標系（此例的 $T_2$），所有的關節和它們的軸線，以及機器人投影在地面上的影子。有關plot方法的更多特性，如多視圖和多機器人繪制，之后有機會再介紹，更詳細的說明參見在線文檔。
上面介紹的簡單兩連桿機器人所能達到的姿態有限，因為它的運動完全在 $xy$ 平面內，其任務空間是 $\mathcal{J}\subset\mathbb{R}^2$。

六軸機器人

真正實用的機器人的任務空間是 $\mathcal{J}\subset SE(3)$，這就使得其末端執行器能到達任意的位置和姿態。這個任務空間有 $6$ 個空間自由度： $3$ 個平移和 $3$ 個轉動。這需要一個具有位形空間 $\mathcal{C}\subset\mathbb{R}^6$ 的機器人，即一個有 $6$ 關節的機器人。本節中，我們將以 Puma560 機器人作為所有全旋轉六軸機械臂的一個例子來介紹。我們使用如下指令來定義一個 Puma560 機器人的實例：

>> mdl_puma560

它將在工作區中創建一個名為p560的SerialLink對象：

>> p560
p560 = 
Puma 560 (6 axis, RRRRRR, stdDH, fastRNE)                        
 viscous friction; params of 8/95;                               
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|      1.571|          0|
|  2|         q2|          0|     0.4318|          0|          0|
|  3|         q3|       0.15|     0.0203|     -1.571|          0|
|  4|         q4|     0.4318|          0|      1.571|          0|
|  5|         q5|          0|          0|     -1.571|          0|
|  6|         q6|          0|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
grav =    0  base = 1  0  0  0   tool =  1  0  0  0              
          0         0  1  0  0           0  1  0  0              
       9.81         0  0  1  0           0  0  1  0              
                    0  0  0  1           0  0  0  1    

請注意， $a_j$ 和 $d_j$ 都采用國際標准單位，這意味着正向運動學的平移部分也將使用國際標准單位。

指令“md1_puma560”還在工作區中創建了大量關節坐標向量，它們代表了一些典型的機器人位形：

qz	$(0, 0, 0, 0, 0, 0)$	零角度
qr	$(0, \dfrac{\pi}{2}, -\dfrac{\pi}{2}, 0, 0, 0)$	就緒狀態，機械臂伸直且垂直
qs	$(0, 0, -\dfrac{\pi}{2}, 0, 0, 0)$	伸展狀態，機械臂伸直且水平
qn	$(0, \dfrac{\pi}{4}, -\pi, 0, \dfrac{\pi}{4}, 0)$	標准狀態，機械臂處於一個靈巧工作姿態（遠離奇異點）

使用plot方法繪制位形，例如

>> p560.plot(qz)

機器人的正向運動學計算如下：

>> p560.fkine(qz)
ans =
    1.0000         0         0    0.4521
         0    1.0000         0   -0.1500
         0         0    1.0000    0.4318
         0         0         0    1.0000

它返回一個對應末端執行器位姿的齊次變換 $T_6$。機器人上連桿 $6$ 坐標系 $\{6\}$ 的原點被定義為最后三個關節軸的交點——這個點在機器人的腕關節內部。通過如下的命令：

>> p560.tool = transl(0, 0, 0.2);

我們可以定義一個工具變換，從 $T_6$ 坐標系變換到實際的工具頂端。在本例中，工具頂端是沿着 $T_6$ 的 $z$ 軸延伸了 $200mm$。此時，工具頂端（也稱工具中心點或者 TCP）的位姿變成

>> p560.fkine(qz)
ans =
    1.0000         0         0    0.4521
         0    1.0000         0   -0.1500
         0         0    1.0000    0.6318
         0         0         0    1.0000

我們在定義上述運動學變換時，認為機器人的基座點是位於機器人結構內部的腰關節和肩關節軸線的交點上。Puma560 機器人有一個 $30$ 英寸高的底座結構。我們可以通過一個基座變換將機器人的原點從其內部轉移到底座的下端部：

>> p560.base = transl(0, 0, 30*0.0254);

其中，為了一致，我們將底座高度轉換為了國際標准單位。現在，經過基座變換和工具變換之后，正向運動學變為

>> p560.fkine(qz)
ans =
    1.0000         0         0    0.4521
         0    1.0000         0   -0.1500
         0         0    1.0000    1.3938
         0         0         0    1.0000

可以看到，現在工具的 $z$ 坐標值比以前更大。

我們還可以做更多有趣的事情，例如：

>> p560.base = transl(0, 0, 3) * trotx(pi);
>> p560.fkine(qz)
ans =
    1.0000         0         0    0.4521
         0   -1.0000   -0.0000    0.1500
         0    0.0000   -1.0000    2.3682
         0         0         0    1.0000

它將機器人的坐標原點比世界坐標原點升高了 $3m$，並讓機器人坐標系繞着 $x$ 軸旋轉 $180°$。現在機器人就像掛在天花板上一樣！（可能需要縮放后才能看得見）

機器人工具箱支持關節角按時間序列或按軌跡線的顯示方式，例如，我們定義一個不同時間的關節坐標值：

>> q
q =
         0         0         0         0         0         0
         0    0.0365   -0.0365         0         0         0
         0    0.2273   -0.2273         0         0         0
         0    0.5779   -0.5779         0         0         0
         0    0.9929   -0.9929         0         0         0
         0    1.3435   -1.3435         0         0         0
         0    1.5343   -1.5343         0         0         0
         0    1.5708   -1.5708         0         0         0

其中每一行表示不同時間步下的關節坐標值，而每列的數字則對應每個關節。此時再使用方法fkine：

>> T = p560.fkine(q)

它將返回一個三維矩陣：

>> about(T)
T [double] : 4x4x8 (1.0 kB)

其中，前兩維是一個 $4×4$ 階的齊次變換矩陣，第三維是time step。對應於q中第 $4$ 行關節坐標值的齊次變換矩陣是

>> T(:,:,4)
ans =
    1.0000    0.0000         0    0.3820
         0   -1.0000   -0.0000    0.1500
         0    0.0000   -1.0000    2.1323
         0         0         0    1.0000

之后我們還會討論如何創建一個軌跡。