無人駕駛系統(tǒng)的核心可分為四個部分：感知、定位、規(guī)劃和控制。規(guī)劃承接環(huán)境感知，并下啟車輛控制，是實現(xiàn)無人駕駛的關(guān)鍵技術(shù)之一。

規(guī)劃是指無人車為了到達(dá)某一目的地而做出決策和計劃的過程，其規(guī)劃出來的軌跡是帶速度信息的路徑，對于無人駕駛車輛而言，這個過程包括從起始地到達(dá)目的地，要避開障礙物，同時要不斷優(yōu)化行車路線和軌跡行為，以保證車輛安全舒適到達(dá)目的地。

根據(jù)這兩點要求可將路徑規(guī)劃分為全局路徑規(guī)劃和局部路徑規(guī)劃，全局路徑規(guī)劃為靜態(tài)規(guī)劃，局部路徑規(guī)劃為動態(tài)規(guī)劃。全局路徑規(guī)劃需要掌握所有的環(huán)境信息，根據(jù)環(huán)境地圖的所有信息進(jìn)行路徑規(guī)劃；局部路徑規(guī)劃只需要與傳感器實時采集環(huán)境信息，了解環(huán)境地圖信息，然后確定出所在地圖的位置及其局部的障礙物分布情況，從而可以選出從當(dāng)前結(jié)點到某一子目標(biāo)結(jié)點的最優(yōu)路徑。

常用路徑規(guī)劃算法大致可分為以下幾類：

01、最常用的傳統(tǒng)經(jīng)典算法

1.Dijkstra算法

Dijkstra算法是由計算機科學(xué)家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用來尋找圖形中節(jié)點之間的最短路徑的算法。采用貪心算法的策略，每次遍歷到始點距離最近且未訪問過的頂點的鄰接節(jié)點，直到擴展到終點為止。Dijkstra算法在擴展的過程中，都是取出未訪問結(jié)點中距離該點距離最小的結(jié)點，然后利用該結(jié)點去更新其他結(jié)點的距離值。

優(yōu)點：如果最優(yōu)路徑存在，那么一定能找到最優(yōu)路徑。

缺點：有權(quán)圖中可能是負(fù)邊；擴展的節(jié)點很多，效率低。

2.A*算法

A* 算法發(fā)表于1968年，A* 算法是將Dijkstra算法與廣度優(yōu)先搜索算法（BFS, Breath First Search）二者結(jié)合而成，通過借助啟發(fā)式函數(shù)的作用，能夠使該算法能夠更快的找到最優(yōu)路徑。A*算法是靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法。

貪婪的最佳優(yōu)先搜索算法與Dijkstra有著類似的工作方式，但是使用的是以每個節(jié)點到達(dá)終點的距離作為優(yōu)先級，Dijkstra是以每個節(jié)點距離起點的移動代價進(jìn)行優(yōu)先級排序的，優(yōu)先選擇代價最小的作為下一個遍歷的節(jié)點。最佳優(yōu)先搜索算法比Dijkstra算法運行更快。

Dijkstra算法不同之處在于，A* 算法是一個“啟發(fā)式”算法，它已經(jīng)有了一些我們告訴它的先驗知識，如“朝著終點的方向走更可能走到”。它不僅關(guān)注已走過的路徑，還會對未走過的點或狀態(tài)進(jìn)行預(yù)測。因此A* 算法相較于Dijkstra而言，調(diào)整了進(jìn)行BFS的順序，少搜索了哪些“不太可能經(jīng)過的點”，更快地找到目標(biāo)點的最短路徑。另外一點，由于H選取的不同，A * 算法找到的路徑可能并不是最短的，但是犧牲準(zhǔn)確率帶來的是效率的提升。

優(yōu)點：利用啟發(fā)式函數(shù)，搜索范圍小，提高了搜索效率；如果最優(yōu)路徑存在，那么一定能找到最優(yōu)路徑。

缺點：A*算法不適用于動態(tài)環(huán)境；不適用于高維空間，計算量大；目標(biāo)點不可達(dá)時會造成大量性能消耗。 

3.D*算法

D* 算法(Dynamic A Star)是卡耐基梅隆機器人中心的Stentz在1994年提出的主要用于機器人探路，并且美國火星探測器上就是應(yīng)用的D* 路徑規(guī)劃算法。A* 算法適用于在靜態(tài)路網(wǎng)中尋路，在環(huán)境變化后，往往需要replan，由于A* 不能有效利用上次計算的信息，故計算效率較低。D* 算法由于儲存了空間中每個點到終點的最短路徑信息，故在重規(guī)劃時效率大大提升。A* 是正向搜索，而D* 特點是反向搜索，即從目標(biāo)點開始搜索過程。在初次遍歷時候，與Dijkstra算法一致，它將每個節(jié)點的信息都保存下來。

優(yōu)點：適用于動態(tài)環(huán)境的路徑規(guī)劃，搜索效率高

缺點：不適用于高維空間，計算量大

02、人工勢場法

人工勢場法是由Khatib在1985年提出的一種基于虛擬力場的局部路徑規(guī)劃算法，該算法的基本思想是：假設(shè)行駛目標(biāo)點對車輛產(chǎn)生引力，而障礙物對車輛產(chǎn)生斥力，控制車輛沿勢場中“勢峰”間的“勢谷”前進(jìn)。其中，引力與車輛到行駛目標(biāo)點的距離成正比，斥力與車輛到障礙物的距離成反比。通過求解車輛所受引力和斥力的合力作為車輛的合外力來控制車輛的行駛速度和運動方向。該方法具有易于數(shù)學(xué)表達(dá)、反應(yīng)速度快、易于實現(xiàn)算法與環(huán)境形成閉環(huán)控制等優(yōu)點，但它在求解過程中極易出現(xiàn)局部最優(yōu)解而導(dǎo)致產(chǎn)生死鎖現(xiàn)象。

優(yōu)點：規(guī)劃出的路徑一般是比較平滑且安全；人工勢場法是一種反饋控制策略，具有一定的魯棒性

缺點：容易陷入局部最優(yōu)的問題；距離目標(biāo)點較遠(yuǎn)時，引力特別大，斥力相對較小，可能會發(fā)生碰撞；當(dāng)目標(biāo)點附近有障礙物時，斥力非常大，引力較小，很難到達(dá)目標(biāo)點

03、基于圖搜索的運動規(guī)劃方法

路徑規(guī)劃首先需要建立規(guī)劃模型，利用狀態(tài)空間法描述規(guī)劃模型是建立非線性優(yōu)化模型的關(guān)鍵，圖搜索算法可以很好地解決該問題。

基本思想是：將車輛的初始位姿和目標(biāo)位姿映射到一個狀態(tài)空間，然后將狀態(tài)空間離散化，并將其構(gòu)成一個圖，隨后從圖中搜索滿足約束條件的最優(yōu)軌跡。目前主流的方法主要包括Voronoi圖、柵格地圖與代價地圖、Lattice狀態(tài)圖、駕駛通道圖等。為了兼顧實時性與障礙物約束空間處理能力，一般采用Lattice和通道圖方法生成安全軌跡。

圖搜索算法包括Dijkstra算法和A*算法等，以及A*算法的變種。

04、基于隨機采樣的路徑規(guī)劃算法

隨機采樣方法的基本思想是在構(gòu)型空間中隨機采樣，并篩選出滿足性能需求的最優(yōu)采樣點，具備概率完備性，但其最大的缺點是舒適性較差，且計算效率隨著障礙物數(shù)量的增長而下降。最常用的方法包括概率路標(biāo)算法（PRM）以及快速搜索隨機樹算法（RRT）。

1.概率路線圖方法（PRM）

PRM算法首先在可行使空間中進(jìn)行離線采樣，構(gòu)造出路線網(wǎng)絡(luò)圖（Roadmap），隨后根據(jù)起始點與目標(biāo)點在路線網(wǎng)絡(luò)圖中進(jìn)行查詢，得到可行的行駛路徑。整個過程分為預(yù)處理階段和查詢階段。

1.預(yù)處理階段：對狀態(tài)空間內(nèi)的安全區(qū)域均勻隨機采樣ｎ個點，每個采樣點分別與一定距離內(nèi)的鄰近采樣點連接，并丟棄掉與障礙物發(fā)生碰撞的軌跡，最終得到一個連通圖。

2.查詢階段：對于給定的一對初始和目標(biāo)狀態(tài)，分別將其連接到已經(jīng)構(gòu)建的圖中，再使用搜索算法尋找滿足要求的軌跡。

優(yōu)點：適用于高維空間和復(fù)雜約束的路徑規(guī)劃問題；搜索效率高，搜索速度快

缺點：概率完備但不是最優(yōu)

2.快速擴展隨機樹方法（RRT）

RRT算法是適用于高維空間，通過對狀態(tài)空間中的采樣點進(jìn)行碰撞檢測，避免了對空間的建模，較好地處理帶有非完整約束的路徑規(guī)劃問題，有效地解決了高維空間和復(fù)雜約束的路徑規(guī)劃問題。該算法是概率完備但不是最優(yōu)的算法。

05、基于曲線插值方法

曲線插值法的核心思想就是基于預(yù)先構(gòu)造的曲線類型，根據(jù)車輛期望達(dá)到的狀態(tài)（位置、速度、加速度、航向角等），將這些期望值作為邊界條件代入曲線類型進(jìn)行方程求解，獲得曲線的相關(guān)系數(shù)。所有系數(shù)計算出后，曲線軌跡規(guī)劃完成。包括多項式參數(shù)化模型、樣條曲線、螺旋線、回旋曲線和貝塞爾曲線等變種參數(shù)化曲線方法。

1.多項式參數(shù)化模型

設(shè)計思想是根據(jù)車輛的初始狀態(tài)和目標(biāo)狀態(tài)對變道軌跡進(jìn)行規(guī)劃，使車輛在指定的時間到達(dá)相鄰車道。試圖在用函數(shù)f(x,y,t)描述的函數(shù)族類中尋找一條軌跡，能充分描述車輛從起始位置過渡到目標(biāo)位置整個過程的動態(tài)特性。隨著多項式次數(shù)的變大，曲線的擬合效果越好，但次數(shù)的增多也會導(dǎo)致參數(shù)求解的運算量指數(shù)增長，通常選用五次多項式進(jìn)行變道軌跡的規(guī)劃。在x方向、y方向分別選用五次多項式構(gòu)造變道軌跡的曲線簇，如下式所示。

在道路結(jié)構(gòu)的約束下，由五次多項式規(guī)劃的曲線無論是在縱向上還是在側(cè)向上都能達(dá)到期望的位置，車輛能在規(guī)定的變道時間內(nèi)平順的完成變道，且軌跡曲線的曲率在起始點和終了點都能達(dá)到零的期望值。

但是，基于多項式的軌跡規(guī)劃方法也存在變道時間和終了點必須預(yù)先已知的局限，對多項式中參數(shù)的確定需要有較充分的條件，對縱向車速變化的情況和實際車輛變道過程中終了點并不唯一的機動性和自適應(yīng)性較差。

多項式曲線通常有三次、五次和七次多項式曲線，每個能確定的每一個期望點的維度數(shù)不一樣，三次多項式是位置和速度；五次多項式是位置、速度和加速度；七次多項式是位置、速度、加速度和加加速速度。

2.貝塞爾曲線

貝塞爾曲線于1962年，由法國工程師皮埃爾·貝濟(jì)埃（Pierre Bézier）所廣泛發(fā)表，他運用貝塞爾曲線來為汽車的主體進(jìn)行設(shè)計,貝塞爾曲線最初由保爾·德·卡斯特里奧于1959年運用德卡斯特里奧算法開發(fā)，以穩(wěn)定數(shù)值的方法求出貝塞爾曲線。

基于貝塞爾缺陷的路徑規(guī)劃方法通過控制點的選取來改變曲線的形狀，通常定義N階貝塞爾曲線由N+1個控制點組成。

貝塞爾曲線

表達(dá)式為：

Pi、t分別是控制點i的坐標(biāo)值與時間參數(shù)，上圖中式子2是Bi(t)是Bernstein多項式。

因其線條光滑且曲率值小的特點而被廣泛地應(yīng)用于軌跡曲線規(guī)劃中。 

以上幾種算法簡單總結(jié)：

最經(jīng)典普適的Dijkstra算法，如有最優(yōu)路徑存在一定能找到最與路徑，但是擴展的結(jié)點多，效率低下；和A*算法適合全局路徑規(guī)劃，利用啟發(fā)式函數(shù)，搜索范圍小，提高了搜索效率，但不適用于高維空間，計算量大；D*算法適用于局部路徑規(guī)劃，搜索效率高，但是計算量大。

圖搜索法適合做全局規(guī)劃，算法收斂慢，環(huán)境建模復(fù)雜，計算效率低；隨機采樣法適用于局部范圍場景，計算速度較快，但是難以勝任復(fù)雜條件下的運動規(guī)劃問題。

曲線插值法求解路徑的速度較快，并且能夠滿足路徑平滑性、可行性，但是無法充分發(fā)揮車輛的全部運動能力；人工勢場法計算速度快，實時性也好，但存在局部最優(yōu)解、復(fù)雜勢場難以搭建的情況。

以上為幾大常見規(guī)劃算法分享，歡迎評論區(qū)各位工程師們一起探討不同規(guī)劃算法的使用情況。

后面也會持續(xù)分享基于不同規(guī)劃算法的公式推導(dǎo)、建模的干貨。