機器人學——2.2-分段軌跡

機器人應用中經常要求機器人平滑地沿一條路徑運動，並不停頓地經過一個或多個中間節點。這樣做或許是要在工作空間中避開障礙物，也可能是為了執行一項需要高精度連續軌跡的任務，如在制造業中要塗抹一圈密封膠。

為了使問題一般化，考慮該軌跡含有 $N$ 個中間點 $x_k$， $k\in [1, N]$，因此有 $N-1$ 個運動段。設 $x_k\in\mathbb{R}^M$ 是位姿的一種向量表示。

機器人從靜止狀態 $x_1$ 開始運動，到 $x_N$ 結束停止，但在通過（或接近）中間點時不能停頓。生成這個運動軌跡的最大問題是過約束。為了實現速度連續，我們只能舍棄讓軌跡能達到每個中間點。在一維情況下這個問題更容易理解，如圖所示。該運動軌跡包括直線運動段與多項式曲線段的混合，如同lspb函數，但在這里我們選擇了五次多項式，因為它們能夠滿足位置、速度和加速度在起點和終點的邊界條件。

軌跡的第一段從速度為零的初始位姿 $x_1$ 開始加速，然后匯接入指向第二個位姿 $x_2$ 的直線段。匯接時間被設為常數 $t_{acc}$，在到達 $x_2$ 之前的 $t_{acc}/2$ 時間，直線軌跡又匯接上一段多項式曲線，持續 $t$ 時間后再次匯接到從 $x_2$ 到 $x_3$ 的直線段，然后就一直重復這一過程。可以給每個直線段設定一個恆速值 $\dot{x}_k$。因此，匯接混合中的平均加速度是
$\ddot{x}_{av}=\dfrac{\dot{x}_{k+1}-\dot{x}_k}{t_{acc}}$如果該軸的最大加速能力已知，那么就可以計算出最小混合時間（該軸會達到其峰值加速度，而不是平均加速度。在五次多項式系數已知時，可以用五階多項式的二階導數計算出混合的峰值加速度）。
對於多軸的情況，很可能在某個特定運動段其中一些軸要比其他維度需要移動更多距離，而如果各個關節有不同的速度限制時，將會使軌跡生成變得復雜。為解決上述問題，第一步需要確定哪個軸將是完成最慢的，依據是每個軸對應這個運動段的運動距離以及該軸最大可達速度。這樣可以算出該段的運動持續時間和各軸實際所需的速度。這確保了所有軸在同一時間達到下一個目標 $x_k$。
工具箱函數mstraj可以基於中間點矩陣生成一個多段多軸軌跡。例如，有 $4$ 個中間點的兩軸運動可以用以下方法生成：

>> via = [ 4,1; 4,4; 5,2; 2,5 ];
>> q = mstraj(via, [2,1], [], [4,1], 0.05, 0);

函數mstraj的第一個參數是中間點矩陣，每點對應矩陣的一行。剩余的參數分別是：每軸的最大速度向量，每段的運動時間向量，起點各軸坐標，采樣時間間隔，以及加速時間。函數mstraj返回一個矩陣，行對應的是時間每步，列對應的是各個軸。在沒有提供輸出參數時mstraj將直接繪制出軌跡曲線，如圖所示。從這個例子所給的參數表明，第一個軸的最大速度比第二個軸更大。然而在最后一個運動段，兩軸是以相同的速度運動的，因為這段的運動時間是由最慢的軸決定的。

如果增加加速時間：

>> q = mstraj(via, [2,1], [], [4,1], 0.05, 1);

軌跡會變得更圓滑，如上圖所示。這是因為增加加速時間意味着軌跡上可以有更多的時間來完成多項式曲線段的混合。該函數也接受可選的起點和終點的速度參數，而且 $t_{acc}$ 可以是一個向量，給出每次混合的加速度時間。

值得注意的是，這個函數只是簡單地對向量代表的位姿進行插值。在這個例子中，向量被假定為笛卡兒坐標向量，但這個函數也適用於歐拉角或橫滾-俯仰-偏航角，然而這個函數不是對坐標系旋轉進行插值的理想方式。有關對姿態的插值我們后面會繼續討論。