機器人學——2.3-姿態插值和笛卡爾運動

之前我們提到mstraj函數並不是不是對坐標系旋轉進行插值的理想方式。

在機器人學中，我們經常需要對姿態進行插值。例如，我們需要機器人的末端執行器平滑地從姿態 $\xi_0$ 和改變到 $\xi_1$。假設某個函數 $\xi(s)=\sigma(\xi_0, \xi_1, s)$，其中 $s\in[0,1]$，可以看做是一條歸一化了的路徑，函數的邊界條件為 $\sigma(\xi_0, \xi_1, 0)=\xi_0$， $\sigma(\xi_0, \xi_1, 1)=\xi_1$，而且 $\sigma(\xi_0, \xi_1, s)$ 要能平滑地經過 $s$ 內定義的一系列中間位姿。我們之間已經了解了多種位姿的描述方式，我們不妨分別討論。

如果位姿由正交旋轉矩陣表示， $\xi\sim R\in SO(3)$，如果用簡單的線性插值 $\sigma(R_0,R_1,s)=(1-s)R_0+sR_1$。但這是不可行的，這樣插值后得到的通常不再是有效的正交矩陣，因為正交矩陣必須滿足的列向量范數為1以及列向量之間正交的條件。

一般是選擇三角度表示法，如歐拉角或橫滾-俯仰-偏航角， $\xi\sim\Gamma\in\mathbb{S}^3$，這樣就可以使用線性插值法：
$\sigma(\Gamma_0, \Gamma_1, s)=(1-s)\Gamma_0+s\Gamma_1$例如，我們定義了兩個姿態：

>> R0 = rotz(-1) * roty(-1);
>> R1 = rotz(1) * roty(1);

得到等價橫滾-俯仰-偏航角：

>> rpy0 = tr2rpy(R0);
>> rpy1 = tr2rpy(R1);

然后分50個時間步在它們之間生成一條軌跡：

>> rpy = mtraj(@tpoly, rpy0, rpy1, 50);

通過動畫演示，很容易觀察軌跡變化過程（rpy是一個 $50×3$ 矩陣，而rpy2tr的結果是 $4×4×50$ 矩陣。）：

>> tranimate(rpy2tr(rpy));

由於姿態變化較大，可以看出坐標系的旋轉軸線沿軌跡的變化。這個動作雖然平滑，但有時看起來也不太協調。另外，如果在所采用的三角度系統中有 $\xi_0$ 或 $\xi_1$ 與之一接近奇異點，也會出現問題。

采用單位四元數的插值法只比三角度向量法復雜一點點，它在空間中產生一個繞固定軸旋轉的姿態變化。首先，我們利用機器人工具箱函數找到與這兩個位姿等價的四元數：

>> q0 = Quaternion(R0);
>> q1 = Quaternion(R1);

然后對它們進行插值並進行動畫演示：

>> q = interp(q0, q1, [0:49]'/49);
>> about(q)
q [Quaternion] : 1x50 (1.6 kB)
>> tranimate(q)

四元數插值法是通過使用球形線性插值（slerp）來實現的，其中單位四元數要在一個四維超球面上沿一個大圓路徑運動。而在三維空間中的結果就是繞定軸的轉動。

笛卡爾運動

另一個常見的需求是在 $SE(3)$ 中生成兩位姿之間的光滑路徑，它同時涉及位置及姿態的變化。在機器人學中這通常被稱作笛卡兒運動。我們將初始和最終位姿都表示成齊次變換矩陣：

>> T0 = transl(0.4, 0.2, 0) * trotx(pi);
>> T1 = transl(-0.4, -0.2, 0.3) * troty(pi/2) * trotz(-pi/2);

工具箱函數trinterp提供了沿路徑單位化距離 $s\in[0,1]$ 中的位姿插值。例如，其中間的位姿是

>> trinterp(T0, T1, 0.5)
ans =
    0.6667   -0.3333    0.6667         0
   -0.6667   -0.6667    0.3333         0
    0.3333   -0.6667   -0.6667    0.1500
         0         0         0    1.0000

其中平移部分是用線性插值，旋轉部分是用四元數插值法interp進行球形插值。
兩個位姿之間 $50$ 個分步的軌跡可用以下方法生成：

>> Ts = trinterp(T0, T1, [0:49]'/49);

參數分別對應的是起點和終點的位姿，以及從 $0$ 到 $1$ 線性變化的路徑長度。由此產生的軌跡Ts是一個三維矩陣：

>> about(Ts)
Ts [double] : 4x4x50 (6.4 kB)

代表的是每個時間分步（第三個指數 $50$ ）對應的齊次變換矩陣（前兩個指數 $4×4$ ）。其中，路徑上第一點的齊次變換是：

>> Ts(:,:,1)
ans =
    1.0000         0         0    0.4000
         0   -1.0000         0    0.2000
         0         0   -1.0000         0
         0         0         0    1.0000

最簡單直觀的表現方法仍然是通過動畫：

>> tranimate(Ts)

它將展現出坐標系從位姿T1平移、旋轉到位姿T2的變化過程。
該軌跡的平移部分由以下方式獲得：

>> P = transl(Ts);

它將以矩陣形式返回軌跡的笛卡兒位置坐標：

>> about(P)
P [double] : 50x3 (1.2 kB)

可以看出，P有 $50$ 行 $3$ 列，每一行對應每個時間分步上的位置向量。
位置向量的變化可以用plot 函數畫出：

>> plot(P)

以橫滾-俯仰-偏擺角格式畫的姿態變化曲線：

>> rpy = tr2rpy(Ts);
>> plot(rpy);

從圖中看到，位置坐標隨時間的變化既平滑又呈線性，而姿態隨時間的變化也平滑，但不是線性的。橫滾-俯仰-偏航角之所以不呈現時間線性，因為它們是對線性變化的四元數進行非線性變換的結果。最后兩個點之間的不連續性，是因為對於橫滾-俯仰一偏航角，其最終姿態是一個奇異點。

然而，平移運動在第一個和最后一個點上的速度和加速度是不連續的。問題在於雖然軌跡在空間中是平滑的，但沿軌跡的步距 $s$ 在時間上並不平滑。路徑起點上的速度值從零跳躍到有限值，然后在終點又突降至零——沒有相應的起始加速和結束減速。我們可以用前面討論過的標量函數tpoly和lspb來創建一個時間上平滑的 $s$，這樣沿路徑的運動也平滑了。我們只需將傳遞給trinterp的第三個參數更改為一個沿路徑的單位化距離向量即可：

>> Ts = trinterp(T0, T1, lspb(0, 1, 50))

這樣做對於軌跡是不變的，但這時坐標系會沿路徑逐漸加速到一個恆定速度，然后結束時再減速，這一點可以從下圖中軌跡姿態部分較光滑的曲線反映出來。

工具箱中還提供了一個方便的速記函數ctraj來進行軌跡插值，其中的參數是初始和最終的位姿，以及分步數量。：

>> Ts = ctraj(T0, T1, 50);