什么是 trajectory（路徑）規(guī)劃

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

1. path

2. trajectory

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

設(shè)想機(jī)械臂的 end-effector 要從 A 點(diǎn)運(yùn)動(dòng)到 B 點(diǎn)：

從 A 到 B 叫 path：

如果我們規(guī)定從 A 到 B 這個(gè) path，必須在特定的時(shí)間（t0 – t6），經(jīng)過這 7 個(gè)點(diǎn)。

那么這 7 個(gè)點(diǎn)叫 waypoints。路徑（trajectory）指的是通過這 7 個(gè)點(diǎn)的具體計(jì)劃（例如什么時(shí)間，以什么速度等等）。規(guī)劃我們?nèi)绾谓?jīng)過這 7 個(gè)點(diǎn)的算法，叫做路徑規(guī)劃（trajectory planning）算法。

上面這 7 個(gè)點(diǎn)，如果機(jī)械臂的 end-effector 在通過的時(shí)候，位置連續(xù)、速度連續(xù)、甚至加速度也是連續(xù)的，那么我們說這個(gè) trajectory 是平滑的（smooth）。

機(jī)械臂中有幾種比較常見的 trajectory：

Trapezoidal Trajectories（梯形路徑）

梯形路徑指的是：

end-effector 在相鄰 waypoints 之間的速度是象梯形一樣，先線性加速，然后速度保持不變，在接近目標(biāo)后線性減速。梯形路徑的優(yōu)點(diǎn)是比較簡(jiǎn)單實(shí)用，但是在每個(gè) waypoint 會(huì)有停頓。下圖是一個(gè)機(jī)械臂走梯形路徑的樣例：在過 waypoints 的時(shí)候，會(huì)完全停止，所以走的是直線，速度和加速度肯定不連續(xù)。

Polynomial Trajectories（多項(xiàng)式路徑）

其中又分三次多項(xiàng)式和五次多項(xiàng)式。分別寫作：

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

下圖是一個(gè)機(jī)械臂走五次多項(xiàng)式路徑的樣例：為了保證過 waypoints 的時(shí)候速度不停頓，走的是曲線。

B-splines（B樣條曲線）

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

(圖片來自于Wikipedia)

如果一個(gè) trajectory 只有這 7 個(gè)點(diǎn)，無論機(jī)械臂控制器的運(yùn)動(dòng)控制模塊如何強(qiáng)大，都無法保證做到“平滑”的運(yùn)行。我們必須對(duì)路徑點(diǎn)進(jìn)行插值（interpolation）。

下圖的橙色點(diǎn)為插值點(diǎn)示意：

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

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

1. Trajectory interpolated in task space

在這種模式下，中間插值的點(diǎn)是在 task space（即 XYZ 坐標(biāo)系，或者叫笛卡爾坐標(biāo)系）進(jìn)行插值。下圖表明了這一過程，這時(shí)候 trajectory generation 只需要做一次，產(chǎn)生所有的 waypoints 和 interpolation points （插值點(diǎn)）。然后運(yùn)動(dòng)控制系統(tǒng)在每個(gè)控制周期都必須調(diào)用 inverse kinematics 算法去做路徑跟隨（trajectory following）。

2.Trajectory interpolated in joint space

在這種模式下，中間插值點(diǎn)的點(diǎn)是在 joint space（即對(duì)關(guān)節(jié)角度插值）。下圖表明了這一過程。這時(shí)候 trajectory generation 也只需要做一次，但做的過程和 task space 的做法不同。即在每一個(gè) waypoint 調(diào)用一次 inverse kinematics，然后對(duì)關(guān)節(jié)角度的進(jìn)行插值。Trajectory following 模塊只需執(zhí)行插值后的角度指令。

角度方向插值 (orientation interpolation)

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

可以看到，隨著 end-effector 的移動(dòng)，它的方向也在不斷做出平滑的調(diào)整，在 end-effector 運(yùn)行到目標(biāo)位置后，它的方向也調(diào)整到位。當(dāng)然，有很多場(chǎng)合角度方向是不需要調(diào)整的。例如：噴漆機(jī)械臂，永遠(yuǎn)將 end-effector 指向噴漆作業(yè)面。

MATLAB 中的機(jī)械臂路徑規(guī)劃算法
在 R2019a 發(fā)布后 Robotics System Toolbox (RST) 中多了幾個(gè)機(jī)械臂路徑規(guī)劃的 MATLAB 函數(shù)和 Simulink 模塊。

MATLAB 函數(shù)：

Simulink 模塊：

不管是 MATLAB 函數(shù)還是類似 Simulink block，大概這么幾項(xiàng)功能：

生成梯形或者多項(xiàng)式的位置 trajectory

生成角度方向的 trajectory

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

讓我們寫幾行代碼，來產(chǎn)生一個(gè)三次多項(xiàng)式的 trajectory：

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

Simulink示例
在 MATLAB file exchange 里搜索 “Trajectory Planning for Robot Manipulators”，會(huì)找到一個(gè)用多種方式產(chǎn)生 trajectory 的例子。

用一張表格總結(jié)如下：

這里有兩點(diǎn)值得注意：

1. 無論是 MATLAB 函數(shù)還是 Simulink block，所有角度方向的插值都是基于 SLERP (spherical linear interpolation)。這是一種基于四元數(shù)（quaternions）的插值法。不會(huì)造成下圖所示的“不走近路，繞遠(yuǎn)路”的問題。例如，角度要從 0 度轉(zhuǎn)到 30 度。因?yàn)閳A周是 360 度，錯(cuò)誤的方式是從 0 度轉(zhuǎn)到 330 度 – 雖然也能到達(dá)指定位置或者角度，但它是錯(cuò)誤的。在manipJointTrajectory 模型中，因?yàn)橛玫搅嘶?joint space（關(guān)節(jié)角度）的插值，讀者可以注意一下如何處理這個(gè)問題。

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