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

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

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

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

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

1.Dijkstra算法

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

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

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

2.A*算法

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

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

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

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

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

3.D*算法

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

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

缺點(diǎn)：不適用于高維空間，計(jì)算量大

02、人工勢(shì)場(chǎng)法

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

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

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

03、基于圖搜索的運(yùn)動(dòng)規(guī)劃方法

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

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

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

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

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

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

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

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

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

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

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

2.快速擴(kuò)展隨機(jī)樹方法（RRT）

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

05、基于曲線插值方法

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

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

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

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

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

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

2.貝塞爾曲線

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

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

貝塞爾曲線

表達(dá)式為：

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

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

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

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

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

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

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

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