機器人學——3.4-軌跡規划

機器人學中最常見的要求之一，是把機器人末端執行器平滑地從位姿 $A$ 移動至位姿 $B$。基於之前討論過的軌跡的內容，我們將討論兩種方法來產生軌跡：在關節空間中的直線和在笛卡兒空間中的直線。這兩種直線分別被稱為關節空間運動和笛卡兒空間運動。

關節空間運動

我們考慮末端執行器在兩個笛卡兒位姿之間移動：

>> T1 = transl(0.4, 0.2, 0) * trotx(pi);
>> T2 = transl(0.4, -0.2, 0) * trotx(pi/2);

它們位於 $xy$ 平面內，且末端執行器指向朝下。與這兩個位姿對應的初始和最終關節坐標向量為

>> q1 = p560.ikine6s(T1);
>> q2 = p560.ikine6s(T2);

而我們要求整個運動發生在 $2$ 秒時間內，采樣時間間隔為 $50$ 微秒。

>> t = [0:0.05:2]';

通過在q1和q2兩個位形之間的平滑插值就能得到一個關節空間軌跡。我們可以用之前的標量插補函數tpoly或lspb，再配合多軸驅動函數mtraj一起實現：

>> q = mtraj(@tpoly, q1, q2, t);

或

>> q = mtraj(@lspb, q1, q2, t);

其結果是得到一個 $50×6$ 階的矩陣q，其中每行對應一個采樣 time step，每列對應一個關節值。但在這里及以后，我們將使用jtraj這個更方便的函數：

>> q = jtraj(q1, q2, t);

其作用相當於具有tpoly插值的mtraj，但它對多軸情況進行了優化，而且還允許使用額外的參數來設置初始和最終速度。對於mtraj+tpoly，最后一個參數可以是一個時間向量，如示例，或者是一個指定采樣時間步數的整數。

我們可以通過可選的輸出參數，獲得隨時間變化的關節速度和加速度向量：

>> [q,qd,qdd] = jtraj(q1, q2, t);

SerialLink類中的jtraj方法為上述步驟提供了一個更簡潔的表達方式：

>> q = p560.jtraj(T1, T2, t);

所得軌跡可以繪制出動畫進行觀察：

>> p560.plot(q)

我們也可以繪制出關節角的圖形，例如所有隨時間變化的關節角：

>> plot(t, q)

關節坐標軌跡雖然是平滑的，但我們並不知道機器人末端執行器在笛卡兒空間中將如何移動。然而通過將正運動學作用於關節坐標軌跡，可以很容易地確定其效果

>> T = p560.fkine(q);

其結果得到一個三維的笛卡兒軌跡。該軌跡的位移部分為

>> p = transl(T)；
>> about(p)
p [double] : 41x3 (984 bytes)

其中，每一行代表一個采樣時間上的末端執行器位置向量。繪制出的各坐標分量隨時間變化的圖形如圖所示。

我們也可以繪制出末端執行器在 $xy$ 平面內的軌跡：

>> plot(p(:,1), p(:,2))

顯然，這條軌跡不是一條直線。這是預料之中的，因為我們僅僅指定了兩個端點的笛卡兒坐標。當機器人在運動過程中旋轉其腰部關節時，末端執行器的運動軌跡會自然地划出一個圓弧。在實際工作中，這可能會導致機器人和附近物體之間發生碰撞，即使它們不在位姿A和B之間的路徑上。

我們還可以繪制出末端執行器的姿態隨時間變化的圖形（橫滾-俯仰-偏航角形式）：

>> plot(t, tr2rpy(T))

注意初始橫滾角顯示為 $-\pi$，但在下一個采樣時間步它變為 $\pi$。這是一個有限精度算法的產物，且這兩個角度在圓周上是等價的。

我們也可以在 Simulink 中實現上述示例：

>> mdl_puma560
>> sl_jspace

其框圖模型如圖所示。jtraj模塊的參數是關節坐標的初始值和最終值，以及運動的持續時間（Simulink 的積分時間需要設置為等於運動段時間，設置可以通過Simulation菜單或通過 MATLAB 命令>> sim('sl_jspace','10);）。平滑變化的關節角被分成兩路傳送，一路到plot模塊中，該模塊能在一個單獨的窗口中顯示機器人運動的動畫；另一路到fkine模塊用於計算機器人的正運動學。plot和fkine兩個模塊有一個共同的SerialLink對象參數，本示例中該參數為p560。使用T2xyz模塊能夠將末端執行器位置中的笛卡兒位置提取出來，它類似於工具箱函數transl。XY Graph模塊可以繪制隨x變化的y。

笛卡爾運動

前面是在位形空間中做直線運動而笛卡爾空間不為直線，現在我們討論在笛卡爾空間中的直線運動。可以通過使用工具箱函數ctraj來實現，這個函數之前已經介紹過。它的用法與jtraj很相似：

>> Ts = ctraj(T1, T2, length(t));

其輸入參數是初始和最后的位姿，以及時間步數，它返回的是一個三維矩陣形式的軌跡。
對於前面的關節空間運動的例子，我們將提取並繪制其位置分量：

>> plot(t, transl(Ts))

以及姿態分量：

>> plot(t, tr2rpy(Ts))

各分量的運動以及 $xy$ 平面內末端執行器的路徑如下圖所示。相比關節空間運動的圖，我們注意到一些重要的差異。第一，笛卡爾運動中的位置和姿態隨時間線性變化。姿態的俯仰角是為零的常數且不隨路徑變化。其次，末端執行器在 $xy$ 平面內沿着一條直線運動。

>> qc = p560.ikine6s(Ts);
>> qplot(t, qc);

通過奇異點的運動

我們特意選擇一個通過機器人奇異點的運動軌跡。我們將先前示例中的笛卡兒端點變為

>> T1 = transl(0.5, 0.3, 0.44) * troty(pi/2);
>> T2 = transl(0.5, -0.3, 0.44) * troty(pi/2);

其結果是得到一個沿着 $y$ 方向的運動，且末端執行器的 $z$ 軸指向 $x$ 方向。其笛卡兒路徑是

>> Ts = ctraj(T1, T2, length(t));

我們將其轉換到關節坐標：

>> qc = p560.ikine6s(Ts);

各關節軌跡如圖所示。在 $t\approx 0.7s$ 時，我們觀察到手腕關節角度 $q_4$ 和 $q_6$ 的變化率非常高。其原因是 $q_5$ 幾乎已經為零，這意味着 $q_4$ 和 $q_6$ 的旋轉軸幾乎對齊重合了，也就是萬向節死鎖情況或奇異點。

關節軸重合意味着該機器人已經失去了一個自由度，實際上現在是一個五軸機器人。從運動學上我們只能求解出 $q_4+q_6$ 的和值，而針對同一個和值可以有無數的 $q_4$ 和 $q_6$ 解。但如果是采用兩個位姿之間的關節空間運動，則可以避免上述問題，因為它不需要求解逆運動學。然而，關節空間運動不會在整個路徑中始終維持工具在 $x$ 方向上的姿態，除了在兩個端點。廣義逆運動學的方法ikine能很容易地處理奇異性問題。