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

path

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ī)劃（trajectoryplanning）算法。

上面這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)是比較簡單實(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)（controlpoints）。機(jī)械臂的end-effector并不會(huì)真正通過這些controlpoints，而在他們之間找一條平滑的曲線。這樣做的目的是在平滑加速度和曲線擬合的準(zhǔn)確性之間找一個(gè)折衷方法。

(圖片來自于Wikipedia)

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

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

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

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

1. Trajectory interpolated in task space

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

2.Trajectory interpolated in joint space

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

角度方向插值(orientation interpolation)

我們剛才談到的插值，都是針對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)然，有很多場合角度方向是不需要調(diào)整的。例如：噴漆機(jī)械臂，永遠(yuǎn)將end-effector指向噴漆作業(yè)面。

MATLAB 中的機(jī)械臂路徑規(guī)劃算法

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

MATLAB函數(shù)：

Simulink模塊：

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

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

生成角度方向的 trajectory

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

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

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

Simulink示例

在MATLABfileexchange里搜索“TrajectoryPlanningforRobotManipulators”，會(huì)找到一個(gè)用多種方式產(chǎn)生trajectory的例子。

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

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

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

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