機器人學——3.3-逆運動學

上一節已經展示了機械臂的正運動學求解方法，即在給定關節坐標以及可選工具和基座變換的條件下，如何確定末端執行器的位姿。
實際應用中的另一個問題是運動學的逆問題：給定期望的末端執行器位姿 $\xi_E$，如何求取所需的關節坐標？例如，如果我們知道一個物體的笛卡兒坐標位姿，那么機器人需要什么樣的關節坐標才能接近這個物體？這就是逆運動學問題，寫成函數形式如下：
$q=\mathcal{K}^{-1}(\xi)$一般而言這個函數的解不是唯一的，而且對於某些類型的機械臂甚至沒有解析解存在，只能求出一個數值解。

解析解

對於一個六軸機器人，其逆運動學具有解析解的一個必要條件是：三個腕關節的軸相交於一個點。這意味着腕關節的運動只改變末端執行器的姿態，而不改變其位置。這種機構被稱為球腕，而且幾乎所有的工業機器人都具有這樣的腕關節。
我們將繼續使用Puma機器人模型來探討逆運動學問題。機器人標准狀態的關節坐標為

>> mdl_puma560
>> qn
qn =
         0    0.7854    3.1416         0    0.7854         0

對應的末端執行器位姿是

>> T = p560.fkine(qn)
T =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1501
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000

Puma560 是一種包含一個球腕的六軸機器人，因此我們使用ikine6s方法來計算其逆運動學的封閉解（ikine6s方法會檢查機器人的 D-H 參數，以確定它是否滿足這些標准）。要實現位姿T，所需的關節坐標為

>> qi = p560.ikine6s(T)
qi =
    2.6486   -3.9270    0.0940    2.5326    0.9743    0.3734

奇怪的是，這些解與我們開始采用的標准狀態關節坐標完全不同。但是，如果我們進一步探究一下：

>> p560.fkine(qi)
ans =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1500
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000

我們發現，兩組不同的關節坐標值得到了相同的末端執行器位姿，而這兩組坐標值對應的機器人位形如圖所示。該 Puma 機器人的肩關節相對於腰部有個水平偏移，所以一組坐標解對應的機械臂在腰部的左側，而另一組則對應在右側，它們分別被稱為左手運動學位形和右手運動學位形。一般來說，有 $8$ 組不同的關節坐標值可以得到相同的末端執行器位姿——正如前面所提到的，逆解不是唯一的。


我們可以人為指定獲得右手位形解：

>> qi = p560.ikine6s(T, 'ru')
qi =
   -0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000

它就是我們最初的那組關節角度，並且是肘關節在上的右手位形。
除了左手位形和右手位形的解，還有一些解對應肘關節在上或在下，以及手腕翻轉或不翻轉。對於 Puma560 機器人的腕關節 $\theta_4$ ，具有一個較大的旋轉范圍，因此其可以采用兩個相差 $\pi$ 的關節角度。又由於大多數機器人夾持器只需兩個手指，因此相差 $\pi$ 並不會改變它抓取物體的效果。

由ikine6s返回的運動學位形由一個或多個字來控制

左手或右手	'l'，'r'
肘部在上或在下	'u'，'d'
手腕是否翻轉	'f'，'n'

由於關節轉角上的機械限制以及連桿之間可能發生的碰撞，並不是所有 $8$ 種位形解在物理上都可以實現，有時甚至可能不存在可以實現的解。例如：

>> p560.ikine6s(transl(3, 0, 0))
警告: point not reachable 
> In SerialLink/ikine6s (line 210) 
ans =
   NaN   NaN   NaN   NaN   NaN   NaN

以上位姿就沒有對應的解，這僅僅是因為手臂的長度不足以達到這個位姿。另外，由於奇異位形的存在，即關節軸線重合而減少了有效自由度（再次出現萬向節鎖問題），某個姿態也可能是無法實現的。Puma560 有一個奇異位形，即當 $q_5$ 等於零時，關節 $4$ 和關節 $6$ 之間的軸線重合了。在這種情況下，ikine6s可以做到最好的是限制 $q_4+q_6$，但它們各自的數值卻是任意的。例如，考慮位形：

>> q = [0 pi/4 pi 0.1 0 0.2];

其中 $q_4+q_6=0.3$，逆運動學的解為

>> qi = p560.ikine6s(p560.fkine(q), 'ru')
qi =
   -0.0000    0.7854    3.1416   -3.0243    0.0000   -2.9589

$q_4$ 和 $q_6$ 的值完全不一樣，但是它們的和始終等於 $0.3$（在一個周期內）

>> qi(4)+qi(6)+2*pi
ans =
    0.3000

數值解

對於不具有 $6$ 個關節和球形腕關節的機器人，我們需要使用一種迭代數值解。繼續使用上面的示例，我們使用ikine方法來計算一般逆運動學解（機器人工具箱9.x的高版本的函數有bug，建議使用低版本或者10.x版本，10.x版本的參數略有不同）：

>> T = p560.fkine(qn)
T =
   -0.0000    0.0000    1.0000    0.5963
   -0.0000    1.0000   -0.0000   -0.1501
   -1.0000   -0.0000   -0.0000   -0.0144
         0         0         0    1.0000
>> qi = p560.ikine(T)
qi =
    0.0000   -0.8335    0.0940   -0.0000   -0.8312  0.0000

它們與最初的關節數值不同：

>> qn
qn =
         0    0.7854    3.1416         0    0.7854         0

但它們仍能得到正確的工具位姿：

>> p560.fkine(qi)
ans =
    0.0000   -0.0000    1.0000    0.5963
   -0.0000    1.0000    0.0000   -0.1500
   -1.0000   -0.0000    0.0000   -0.0144
         0         0         0    1.0000

繪制該位姿的圖形：

>> p560.plot(qi)

結果清楚地顯示ikine方法找到了肘關節在下的位形。

這個通用數值解法的局限性是：它不像解析法能提供對手臂運動位形的明顯控制，只有通過設定關節坐標初始值（默認為零）的隱含控制。如果指定初始關節坐標：

>> qi = p560.ikine(T, [0 0 3 0 0 0])
qi =
    0.0000    0.7854    3.1416   -0.0000    0.7854    0.0000

這時其數值解收斂於肘關節在上的位形。

通用數值方法ikine要比解析方法ikine6s慢很多。然而，它也具有很大的優勢，它能夠求解處於奇異位形以及非 $6$ 關節型的機械臂。

欠驅動機械臂

一個欠驅動機械臂是指關節數少於 $6$ 個的機械臂，因此其末端執行器可以達到的位姿是受限制的。它們通常有一個 $x-y-z-\theta$ 的任務空間 $\mathcal{T}\subset\mathbb{R}^3\times\mathbb{S}$，以及一個位形空間 $\mathcal{C}\subset\mathbb{S}^3\times\mathbb{R}$。

我們以兩連桿機械臂為例，首先定義該機器人：

>> mdl_twolink

然后定義所期望的末端執行器的位姿：

>> T = transl(0.4, 0.5, 0.6);

然而此位姿對於該兩連桿機器人是過約束的，因為工具既不能滿足在 $\hat{x}_2$、 $\hat{y}_2$ 方向上的姿態約束，也不能實現在 $z$ 軸上除零之外的任何平移量。因此，我們要求ikine方法只考慮在 $x$ 軸和 $y$ 軸平移方向上的誤差，而忽略其他所有笛卡兒坐標的自由度。為了達到這個目的，我們指定一個mask vector作為第四個參數：

>> q = twolink.ikine(T, [0 0], [1 1 0 0 0 0])
q =
   -0.3488    2.4898

該mask vector的元素分別對應末端執行器坐標系中的 $3$ 個移動和 $3$ 個轉動： $t_x$， $t_y$， $t_z$， $r_x$， $r_y$， $r_z$。
在這個示例中，我們規定只考慮在 $x$ 軸和 $y$ 軸平移方向上的誤差（向量中的非零元素，mask vector的值只能為 $0$ 或 $1$）。產生的關節角度值對應於某個末端位姿。求解其正運動學解：

>> twolink.fkine(q)
ans =
   -0.5398   -0.8418         0    0.4000
    0.8418   -0.5398         0    0.5000
         0         0    1.0000         0
         0         0         0    1.0000

得到我們所期望的 $x$ 軸和 $y$ 軸上的平移量，但姿態方向和 $z$ 軸的平移是不正確的。

冗余機械臂

冗余機械臂是指具有多於 $6$ 個關節的機器人。正如前面提到的， $6$ 個關節在理論上就足以達到任何笛卡兒任務空間 $\mathcal{T}\subset SE(3)$ 中所期望的位姿。然而，由於關節限制和奇異位形等實際問題的存在，並非所有機器人可達空間內的位姿都可以實現。增加額外的關節是解決這個問題的一種方法。為了說明這一點，我們將創建一個 Puma560 冗余機械臂。把它放在一個能在 $xy$ 平面移動的平台上，用以模仿一個安裝在車輛上的機器人。這個新的機器人具有和 Puma 機器人相同的任務空間 $\mathcal{T}\subset SE(3)$，但其位形空間變為 $\mathcal{C}\subset\mathbb{R}^2\times\mathbb{S}^6$。位形空間的維數超過任務空間的維數。
此機器人基座的 D-H 參數如下：

連桿	$\theta_i$	$d_i$	$a_i$	$\alpha_i$	$\sigma_i$
1	0	$q_1$	0	$-\dfrac{\pi}{2}$	1
2	$-\dfrac{\pi}{2}$	$q_2$	0	$\dfrac{\pi}{2}$	1

然后我們從D-H參數中創建serialLink對象

>> platform = SerialLink([0 0 0 -pi/2 1; -pi/2 0 0 pi/2 1], 'base', troty(pi/2), 'name', 'platform')

D-H 參數要求移動關節產生沿局部坐標 $z$ 軸方向的平移，基座變換則將該 $z$ 軸旋轉為世界坐標的 $x$ 軸方向。這個機器人的關節坐標即是其 $x$ 和 $y$ 坐標位置。我們可以通過測試來證明我們使用的 D-H 參數是正確的。

>> platform.fkine([1, 2])
ans =
    1.0000    0.0000    0.0000    1.0000
   -0.0000    1.0000    0.0000    2.0000
         0   -0.0000    1.0000    0.0000
         0         0         0    1.0000

我們看到，旋轉子矩陣為單位矩陣，這意味着該平台的坐標系與世界坐標系相平行，且 $x$ 和 $y$ 方向上的位移正是我們所要求的。
我們通過將兩個機器人串聯起來從而將 Puma 機器人安裝在此平台上。在工具箱中，我們可以用兩種不同的方式來表達這種串聯。一種是通過乘法：

>> p8 = platform * p560;

另一種是通過串聯：

>> p8 = SerialLink([platform, p560]);

串聯方式還允許指定所創建 serialLink對象的其他屬性。

然而，機器人的串聯也存在一個小的問題。我們想要把 Puma 手臂安裝在平台頂部的高大底座上。在此之前，我們通過基座變換增加了底座，但基座變換只能存在於一個運動鏈的開始，而現在這個變換是在八軸機器人的關節 $2$ 和關節 $3$ 之間。因此這里就直接通過設置 Puma 的 $d_1$ 為底座高度來實現該底座添加：

>> p560.links(1).d = 30 * 0.0254;

現在可以將這兩個機器人結合起來了：

>> p8 = SerialLink([platform, p560], 'name', 'p8')
p8 = 
p8 (8 axis, PPRRRRRR, stdDH)                         
+---+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |
+---+-----------+-----------+-----------+-----------+
|  1|          0|         q1|          0|     -1.571|
|  2|     -1.571|         q2|          0|      1.571|
|  3|         q3|      0.762|          0|      1.571|
|  4|         q4|          0|     0.4318|          0|
|  5|         q5|       0.15|     0.0203|     -1.571|
|  6|         q6|     0.4318|          0|      1.571|
|  7|         q7|          0|          0|     -1.571|
|  8|         q8|          0|          0|          0|
+---+-----------+-----------+-----------+-----------+
                                                     
grav =    0  base =  0  0  1  0   tool =  1  0  0  0 
          0          0  1  0  0           0  1  0  0 
       9.81         -1  0  0  0           0  0  1  0 
                     0  0  0  1           0  0  0  1 

其結果即是命名為P8的一個新的八軸機器人。我們注意到，新的機器人繼承了該平台的基座變換，且機器人底座位移以 $d_3$ 參數出現。
我們使用通用的數值解方法可以找到這個新冗余機械臂的逆運動學解。例如，將末端執行器移動到 $(0.5,1.0,0.7)$，並且使工具在 $xz$ 平面中指向下方，其笛卡兒位姿為

>> T = transl(0.5, 1.0, 0.7) * rpy2tr(0, 3*pi/4, 0);

所需的關節坐標為（不唯一）

>> qi = p8.ikine(T)
qi=
	-0.2854	1.0528	0.1246	-0.5377	-0.6091	-0.0939 -1.2147 0.1212

其中前兩個元素是移動關節在基平面內的位移，后面六個是機器人手臂的關節角度。我們看到，這個解很好地利用了機器人的第二個關節，即平台沿y軸的移動，使機械臂的底座靠近所期望的點。我們可以繪制出機器人的該位形圖。