什么是 trajectory（路徑）規劃

中文路徑在英語中可能有兩種翻譯：

1. path

2. trajectory

首先告訴大家，我們所說的“路徑”的是后者——trajectory。我們看一下這兩種“路徑”在機械臂的世界里有什么區別。

設想機械臂的 end-effector 要從 A 點運動到 B 點：

從 A 到 B 叫 path：

如果我們規定從 A 到 B 這個 path，必須在特定的時間（t0 – t6），經過這 7 個點。

那么這 7 個點叫 waypoints。路徑（trajectory）指的是通過這 7 個點的具體計劃（例如什么時間，以什么速度等等）。規劃我們如何經過這 7 個點的算法，叫做路徑規劃（trajectory planning）算法。

上面這 7 個點，如果機械臂的 end-effector 在通過的時候，位置連續、速度連續、甚至加速度也是連續的，那么我們說這個 trajectory 是平滑的（smooth）。

機械臂中有幾種比較常見的 trajectory：

Trapezoidal Trajectories（梯形路徑）

梯形路徑指的是：

end-effector 在相鄰 waypoints 之間的速度是象梯形一樣，先線性加速，然后速度保持不變，在接近目標后線性減速。梯形路徑的優點是比較簡單實用，但是在每個 waypoint 會有停頓。下圖是一個機械臂走梯形路徑的樣例：在過 waypoints 的時候，會完全停止，所以走的是直線，速度和加速度肯定不連續。

Polynomial Trajectories（多項式路徑）

其中又分三次多項式和五次多項式。分別寫作：

三次多項式可以記錄 end-effector 的速度、位置的信息。五次多項式可以記錄 end-effector 的位置、速度、加速度的信息。下圖是一個五次多項式的例子，可以看到速度、加速度都是連續的。三次或者五次多項式的 trajectory 在通過 waypoints 的時候速度都是連續的，這和梯形路徑是不一樣的。

下圖是一個機械臂走五次多項式路徑的樣例：為了保證過 waypoints 的時候速度不停頓，走的是曲線。

B-splines（B樣條曲線）

B 樣條曲線將除了起始點和終點外，將其他的 waypoints 看成是控制點（control points）。機械臂的 end-effector 并不會真正通過這些 control points，而在他們之間找一條平滑的曲線。這樣做的目的是在平滑加速度和曲線擬合的準確性之間找一個折衷方法。

(圖片來自于Wikipedia)

如果一個 trajectory 只有這 7 個點，無論機械臂控制器的運動控制模塊如何強大，都無法保證做到“平滑”的運行。我們必須對路徑點進行插值（interpolation）。

下圖的橙色點為插值點示意：

在了解插值后，我們來看一下路徑規劃出來的信息如何和傳遞到反向運動學（inverse kinematics）。反向運動學模塊將確保end-effector按照規劃出來的路徑運行，即路徑跟隨（trajectory following）。

這里，我們按插值類型情況分兩種模式：

1. Trajectory interpolated in task space

在這種模式下，中間插值的點是在 task space（即 XYZ 坐標系，或者叫笛卡爾坐標系）進行插值。下圖表明了這一過程，這時候 trajectory generation 只需要做一次，產生所有的 waypoints 和 interpolation points （插值點）。然后運動控制系統在每個控制周期都必須調用 inverse kinematics 算法去做路徑跟隨（trajectory following）。

2.Trajectory interpolated in joint space

在這種模式下，中間插值點的點是在 joint space（即對關節角度插值）。下圖表明了這一過程。這時候 trajectory generation 也只需要做一次，但做的過程和 task space 的做法不同。即在每一個 waypoint 調用一次 inverse kinematics，然后對關節角度的進行插值。Trajectory following 模塊只需執行插值后的角度指令。

角度方向插值 (orientation interpolation)

我們剛才談到的插值，都是針對 end-effector 的位置（position，即 XYZ 的坐標）。我們不應該忽略 end-effector 的角度方向（orientation）插值。我們看一段動畫來了解一下，什么是 orientation 的插值：

可以看到，隨著 end-effector 的移動，它的方向也在不斷做出平滑的調整，在 end-effector 運行到目標位置后，它的方向也調整到位。當然，有很多場合角度方向是不需要調整的。例如：噴漆機械臂，永遠將 end-effector 指向噴漆作業面。

MATLAB 中的機械臂路徑規劃算法
在 R2019a 發布后 Robotics System Toolbox (RST) 中多了幾個機械臂路徑規劃的 MATLAB 函數和 Simulink 模塊。

MATLAB 函數：

Simulink 模塊：

不管是 MATLAB 函數還是類似 Simulink block，大概這么幾項功能：

生成梯形或者多項式的位置 trajectory

生成角度方向的 trajectory

將上兩者合二為一的生成既包含位置，也包含角度方向（合稱 pose）的 trajectory——即基于 homogeneous transforms 的 pose trajectory。

讓我們寫幾行代碼，來產生一個三次多項式的 trajectory：

可以看到，位置是連續的。如果我們將速度 (qd)、加速度 (qdd) 畫出來，可以看到 qdd 是不連續的。

Simulink示例
在 MATLAB file exchange 里搜索 “Trajectory Planning for Robot Manipulators”，會找到一個用多種方式產生 trajectory 的例子。

用一張表格總結如下：

這里有兩點值得注意：

1. 無論是 MATLAB 函數還是 Simulink block，所有角度方向的插值都是基于 SLERP (spherical linear interpolation)。這是一種基于四元數（quaternions）的插值法。不會造成下圖所示的“不走近路，繞遠路”的問題。例如，角度要從 0 度轉到 30 度。因為圓周是 360 度，錯誤的方式是從 0 度轉到 330 度 – 雖然也能到達指定位置或者角度，但它是錯誤的。在manipJointTrajectory 模型中，因為用到了基于 joint space（關節角度）的插值，讀者可以注意一下如何處理這個問題。

2. Time-scaling。在 manipTransformTrajectoryTimeScaling 模型中，如果不加 time scaling 這個模塊，那么所有的角度變化和位置變化都是基于線性插值。如果加入了 “想要的” 速度、加速度信息 ---即 time scaling。我們可以使角度變化和位置變化按照我們想要的梯形或者多項式形式去調整。例如，我想讓角度一開始不變，然后快速變化，最后速度為0。