本篇文章投稿者為Todd
簡介
讓我們接著了解軌跡規劃吧，想看上一節姿態預估的讀者可以參考這篇文章《Mobile Manipulation 實作系列 (1)：姿態預估》。
軌跡規劃用於指定機器人的任務，例如: 讓末端執行器跟蹤某個已知的移動物體。對於專題的youBot而言，它的任務是夾起方塊，並在指定的位置放下方塊。在這一節中，我們會簡介:
- 軌跡定義
- 軌跡模擬
那就讓我們開始吧～
軌跡定義
我們將路徑(path)定義為，其中\theta(s)是scalar parameter並且在路徑起始處為0、末端處為1，\theta(s)會對應機器人構型空間中\theta的一點:\theta:[0,1] → \Theta。 s可以隨著t變化，因此軌跡可以進一步定義成: \theta(s(t))，其中t是time scaling，表示: s:[0,T] → [0, 1] 。
簡介完軌跡定義之外，讓我們來看一個簡單的三次多項式吧：
三次多項式
\theta(s(t))可以表示成: \theta(s(t))=a_{0}+a_{1}t+a_{2}t^2+a_{3}t^3。可以透過施加約束去求解係數：
- s(0) = \dot{s}(0) = 0
- s(T) = \dot{s}(T) = 0
利用上述約束，可以解出：
s(t), \dot{s}(t), \ddot{s}(t)的示意圖如下：
考慮直線路徑的話，\theta(s(t))以及其路徑速度、加速度可以寫成：
不過從上圖可以發現三次多項式會造成\ddot{s}(0), \ddot{s}(t)產生加速度的不連續跳動，以下是可能的解決方案：
- 施加\ddot{s}(0) = \ddot{s}(t) = 0的約束，使用五次多項式去表達s(t)
- 應用加加速度(Jerk)，使用S-curve去表達s(t)
為了避免篇幅過長，這裡就不簡介五次多項式以及S-curve，下面列出相關資料供有興趣的讀者參考：
- 五次多項式介紹: MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL, Chapter 9
- S-curve介紹: Trajectory Planning for Automatic Machines and Robots, Chapter 3
構型空間的路徑
如果想將末端執行器的起始構型X_{start}移動至結束構型X_{end}，我們會怎麼做呢？ 一個想法是利用上述提到的直線路徑，將路徑表示成：
由於構型包含平移以及旋轉，這造成X_{end} - X_{start}的計算問題，那麼我們要怎麼定義路經將X_{start}移動至X_{end}呢？
一個作法是定義中X_{start}和X_{end}之間的screw axis，讓X_{start}以screw motion的方式移動至X_{end}：
假設起始構型X_{start}以及結束構型X_{end}可以在座標系中被定義，那麼我們可以利用下標消去法在起始座標系中表示結束構型:
從X_{start, end}分解出screw axis: \log(X^{-1}_{s, start}X_{s, end})，X_{start}沿著screw axis會在單位時間內到達X_{end}。
如此一來，構型的路徑就可以寫成 (screw axis的公式可以參考MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL, Chapter 3.3.3)：
在這個運動中，末端執行器會沿著screw axis做直線運動，可是在直角座標空間會沿著螺旋運動。如果要得到直角座標空間的直線運動，可以將旋轉運與平移運動從構型中分離：
下圖展示螺旋運動以及分離旋轉/平移運動的效果：
modern-robotics library實做了上述的運動，有興趣的讀者可以參考：
- 螺旋運動：ScrewTrajactory
- 分離旋轉/平移運動： CartesianTrajactory
軌跡模擬
我們利用CoppeliaSim展示library的ScrewTrajactory以及CartesianTrajactory的效果並簡介如何建立夾取方塊的路徑。為了方便起見，這裡會直接利用筆者做好的程式碼(詳細程式碼可見這裡)。
為了展示ScrewTrajactory以及CartesianTrajactory，我們會利用trajectory.py，trajectory.py會依照使用者指定的構型以ScrewTrajactory或CartesianTrajactory進行計算。
在介紹trajectory.py之前，先讓我們了解方塊以及一些基本的模擬環境設定:
方塊構型:
- 起始構型(\phi, x, y, z) = (0°, 1, 0, 0.025)
- 結束構型(\phi, x, y, z) = (-90°, 0, -1, 0.025)
模擬環境使用CoppeliaSim以及課程提供的scenes，詳細架設可以參考這個連結。
在一節中我們會用到課程提供的Scene8_gripper_csv.ttt
模擬輸入: CSV檔案，檔案紀錄下列資訊:
r11, r12, r13, r21, r22, r23, r31, r32, r33, px, py, pz, gripper state
前12項是構型矩陣T:
gripper_state表示夾爪的狀態:
- 0: 夾爪打開
- 1: 夾爪關閉
接著，讓我們來了解trajectory.py是如何產生軌跡的吧，trajectory.py的主要函式是generate_trajectory，這個函式會依照使用者設定的構型做計算：
- T_{s, e, init}：夾爪在座標系的初始構型
- T_{s, c, init}：方塊在座標系的初始構型
- T_{s, c, final}：方塊在座標系的結束構型
- T_{c, e, grasp}：在夾取方塊時，夾爪相對方塊座標系的構型
- T_{c, e, standoff}：在夾取方塊前，夾爪相對方塊座標系的構型，表示夾爪相對方塊的一個偏移
有了上面的輸入，generate_trajectory便會利用ScrewTrajactory或是CartesianTrajactory去拼接構型之間的軌跡，拼接的方式簡介如下:
- 將T_{s, e, init}移動至起始方塊的standoff位置
- 從起始方塊的standoff位置移動至起始方塊的夾取位置
- 關閉夾爪
- 從起始方塊的夾取位置回到起始方塊的standoff位置
- 從起始方塊的standoff位置移動至結束方塊的standoff位置
- 從結束方塊的standoff位置移動至結束方塊的夾取位置
- 打開夾爪
- 從結束方塊的夾取位置回到結束方塊的standoff位置
這裡，我們會利用第一段軌跡來展示ScrewTrajactory以及CartesianTrajactory的效果：
第一段軌跡是將T_{s, e, init}移動至起始方塊的standoff位置，為了更好的觀察的軌跡，我們將T_{s, e, init}設定成：
並把T_{c, e, standoff}設定成：
接著，修改generate_trajectory函式，只留下Segment 1，註解其餘部份：
利用下標消去法，我們可以得到T_{s, e, standoff}，這裡我們使用ScrewTrajectory去產生軌跡。分別使用ScrewTrajectory以及CartesianTrajactory之後，我們得到下面的結果。可以發現ScrewTrajectory會讓夾爪做螺旋運動，而CartesianTrajactory則是做直線運動。
最後，我們重新啟用generate_trajectory的其他部份，模擬夾取方塊的完整軌跡，可以得到下面的結果。 (Note: 影片中模擬的速度是2倍，且利用的是trajectory.py內的預設設定。另外因模擬環境被沒有設定物體之間的接觸，因此會看到夾爪穿透方塊)
至此，軌跡的介紹就告一段落了，希望這些介紹能帶來一點幫助，如果有其他問題的話，歡迎隨時留言提問。