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

path

trajectory

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

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

從A到B叫path：

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

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

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

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

Trapezoidal Trajectories（梯形路徑）

梯形路徑指的是：

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

Polynomial Trajectories（多項式路徑）

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

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

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

B-splines（B樣條曲線）

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

(圖片來自于Wikipedia)

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

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

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

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

1. Trajectory interpolated in task space

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

2.Trajectory interpolated in joint space

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

角度方向插值(orientation interpolation)

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

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

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

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

MATLAB函數(shù)：

Simulink模塊：

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

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

生成角度方向的 trajectory

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

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

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

Simulink示例

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

用一張表格總結如下：

這里有兩點值得注意：

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

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