兩萬字簡述自動駕駛路徑規(guī)劃的常用算法

自動駕駛自誕生那天起，其志向便已立下，成為熟知城市每一處道路的“老司機”，成為乘客更安全、更舒適、更高效出行的“守護神”。在搞錢撈錢的大背景下，這個無私追求樸素得令人敬畏。 在自動駕駛的分工中，決策規(guī)劃將承擔(dān)上述志向?qū)崿F(xiàn)的大部分工作，也因此被毫不吝嗇的稱為自動駕駛的大腦。決策規(guī)劃這塊網(wǎng)上已經(jīng)有數(shù)量眾多的優(yōu)秀科普文章，但他們都沒有長成《十一號組織》的樣子。抱著將所有自動駕駛知識都“擼一遍”的偉大理想，我決定用兩萬字簡述決策規(guī)劃的常用算法。

01 概述

1. 1 自動駕駛系統(tǒng)分類

自動駕駛系統(tǒng)沒有嚴(yán)謹(jǐn)?shù)姆诸悾袠I(yè)內(nèi)普遍喜歡將自動駕駛系統(tǒng)區(qū)別為模塊化的和端到端的，圖1所示為兩者系統(tǒng)的原理框圖對比。

圖1 模塊化和端到端自動駕駛系統(tǒng)原理簡圖

1.1.1 模塊化自動駕駛系統(tǒng)

這是最經(jīng)典也是業(yè)界采用最多的一種自動駕駛系統(tǒng)，也是最簡明清爽的一種結(jié)構(gòu)，其作用是實時地求解出連續(xù)的控制輸出使得自動駕駛車輛可以安全地由初始位置行駛到目標(biāo)位置?；谀K化的思想，將自動駕駛系統(tǒng)劃分為三層：環(huán)境感知層、決策規(guī)劃層和控制執(zhí)行層。每一層還可以劃分為不同的模塊，每個模塊還可以劃分為不同的子模塊……。

環(huán)境感知層就像是人的眼睛和耳朵，負(fù)責(zé)對外部環(huán)境進(jìn)行感知并將感知結(jié)果送入決策規(guī)劃層。決策規(guī)劃層就像是人的大腦，在接收到感知信息后進(jìn)行分析、決策，并生成加減速、變道、直行等控制命令?？刂茍?zhí)行層就像人的雙手和雙腳，在接收到控制命令后控制執(zhí)行器完成加速、轉(zhuǎn)向等操作。 模塊化自動駕駛系統(tǒng)中每一層都是關(guān)鍵和核心。但從實現(xiàn)自動駕駛功能的角度，環(huán)境感知層是基礎(chǔ)，決策規(guī)劃層是核心，控制執(zhí)行層是保障。作為核心的決策規(guī)劃層帶著自動駕駛往“更安全、更舒適、更高效”的道路上狂奔，畢竟小小的失誤小則影響乘坐舒適性、通行效率，大則影響人身財產(chǎn)安全。

在模塊化自動駕駛系統(tǒng)中，不同團隊負(fù)責(zé)不同的模塊，可以實現(xiàn)更好的分工合作，從而提高開發(fā)效率。同時團隊內(nèi)部可以對負(fù)責(zé)的模塊進(jìn)行充分的評估，了解各模塊的性能瓶頸所在，從而讓我們能對最后的0.1%的不足有更清晰的認(rèn)知，技術(shù)的迭代、更新。

缺點就是整個系統(tǒng)非常復(fù)雜、龐大、需要人工設(shè)計成百上千個模塊。二是對車載硬件計算能力要求高，如果越來越多的子模塊采用深度學(xué)習(xí)網(wǎng)絡(luò)，這將帶來災(zāi)難性的計算需求爆炸?；谀K化的自動駕駛系統(tǒng)，我們可能花10%的時間就實現(xiàn)了99.9%的問題，但我們還需要花90%的時間去解決最后0.1%的不足。 這個系統(tǒng)的難度之大，已經(jīng)遠(yuǎn)超一家公司的能力范圍，需要一個協(xié)作的生態(tài)。

1.1.2 端到端自動駕駛系統(tǒng)

術(shù)語端到端（End to End）來源于深度學(xué)習(xí)，指的是算法直接由輸入求解出所需的輸出，即算法直接將系統(tǒng)的輸入端連接到輸出端。2016年NVIDIA將端到端的深度學(xué)習(xí)技術(shù)應(yīng)用在自動駕駛汽車之后，端到端自動駕駛迅速捕獲圈內(nèi)一眾大佬的芳心，各種demo更是層出不窮。 所謂端到端自動駕駛是指車輛將傳感器采集到的信息（原始圖像數(shù)據(jù)、原始點云數(shù)據(jù)等），直接送入到一個統(tǒng)一的深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)，神經(jīng)網(wǎng)絡(luò)經(jīng)過處理之后直接輸出自動駕駛汽車的駕駛命令（方向盤轉(zhuǎn)角、方向盤轉(zhuǎn)速、油門踏板開度、制動踏板開度等）。

2016年NVIDIA發(fā)表了論文《End to End Learning for Self-Driving Cars》，拉開了端到端自動駕駛內(nèi)卷的序幕。 論文首先展示了訓(xùn)練數(shù)據(jù)的采集系統(tǒng)，如圖2所示。論文中只涉及了車道保持功能，因此訓(xùn)練數(shù)據(jù)也只對攝像機的視頻數(shù)據(jù)和人類駕駛員操作方向盤的角度數(shù)據(jù)進(jìn)行了采集。

圖2 數(shù)據(jù)采集系統(tǒng)框圖 三架攝像機安裝在采集車的擋風(fēng)玻璃后面，并按照左中右依次布置，這樣布置是為了捕獲完整的前向路面信息。一臺NVIDIA DRIVETM PX被用來作為采集車的計算單元。攝像機生成的每一幀視頻數(shù)據(jù)（30FPS）都與人類駕駛員的轉(zhuǎn)向角度進(jìn)行時間同步。 采集車最終在各式道路以及多樣照明和天氣條件組合下采集了72小時的駕駛數(shù)據(jù)。訓(xùn)練數(shù)據(jù)包含視頻采樣得到的單一圖像，搭配相應(yīng)的轉(zhuǎn)向命令。

但是只有來自人類駕駛員的正確數(shù)據(jù)是不足以完成訓(xùn)練的，神經(jīng)網(wǎng)絡(luò)還必須學(xué)習(xí)如何從任何錯誤中恢復(fù)，否則自動駕駛汽車就將慢慢偏移道路。因此訓(xùn)練數(shù)據(jù)還擴充了額外的圖像，這些圖像顯示了遠(yuǎn)離車道中心的偏離程度以及不同道路方向上的轉(zhuǎn)動。兩個特定偏離中心的變化圖像可由左右兩個攝像機捕獲。 訓(xùn)練數(shù)據(jù)準(zhǔn)備完畢之后，將其送入一個卷積神經(jīng)網(wǎng)絡(luò)（CNN），訓(xùn)練系統(tǒng)框圖如圖3所示。

圖3 訓(xùn)練系統(tǒng)框圖 CNN計算一個被推薦的轉(zhuǎn)向命令，這個被推薦的轉(zhuǎn)向命令會與該圖像的期望命令相比較，CNN權(quán)重就會被調(diào)整以使其實際輸出更接近期望輸出。在這個框架中，只要提供足夠的訓(xùn)練數(shù)據(jù)，即人類駕駛員駕駛攜帶有攝像頭的車輛累計駕駛大量的里程，再加上人為創(chuàng)造系統(tǒng)的“極限”道路狀態(tài)——偏離道路線的各種工況，CNN就會得到充分的訓(xùn)練，而變得足夠強大。 一旦訓(xùn)練完成，網(wǎng)絡(luò)就能夠從單中心攝像機（single center camera）的視頻圖像中生成轉(zhuǎn)向命令，圖4展示了這個配置。

圖4 訓(xùn)練過的網(wǎng)絡(luò)用于從單中心前向攝像機中生成轉(zhuǎn)向命令

在端到端自動駕駛中，沒有人工設(shè)計的繁復(fù)規(guī)則，只需要極少的來自人類的訓(xùn)練數(shù)據(jù)，深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)就會學(xué)會駕駛。且不用關(guān)心有沒有高精地圖覆蓋、此時是行駛在高速主干路還是城區(qū)道路、道路上車道線有沒有缺失等。 相比模塊化自動駕駛系統(tǒng)，端到端自動駕駛系統(tǒng)設(shè)計難度低，硬件成本小，還能借助數(shù)據(jù)的多樣性獲得不同場景下的泛用性。各方面條件得天獨厚，從理論層面看堪稱自動駕駛的終極夢想。

然而端到端深度學(xué)習(xí)神經(jīng)網(wǎng)絡(luò)是一個完完全全的黑盒子，不具解釋分析性，可靠性、靈活性差，工程師們沒有辦法對它進(jìn)行系統(tǒng)化的解釋分析，而是只能依靠推測和實驗進(jìn)行調(diào)整。最終帶來的結(jié)果是安全難以得到保障，而自動駕駛最最關(guān)注的恰是安全。

比如端到端自動駕駛系統(tǒng)下汽車做出一個汽車減速左轉(zhuǎn)的行動，工程師們無法確定這是因為汽車看到行人，還是因為看到較遠(yuǎn)處的紅燈。但是，在模塊化的自動駕駛系統(tǒng)下，由于多個識別系統(tǒng)嵌套，相對好理解到底汽車所做的每一個舉動背后的邏輯。

這也意味著，如果端到端系統(tǒng)出現(xiàn)問題時，工程師們并不能對其對癥下藥，做出合理的應(yīng)對。更多情況下甚至只能簡單向模型灌注更多的數(shù)據(jù)，希冀它能在進(jìn)一步的訓(xùn)練中“自行”解決問題。這也會大大降低端到端自動駕駛系統(tǒng)原本開發(fā)簡單的優(yōu)勢。

1.2 決策規(guī)劃分層架構(gòu)

決策規(guī)劃的任務(wù)，就是在對感知到的周邊物體的預(yù)測軌跡的基礎(chǔ)上，結(jié)合結(jié)合自動駕駛車輛的和當(dāng)前位置，對車輛做出最合理的決策和控制。 正如人的大腦又分為左腦和右腦、并負(fù)責(zé)不同的任務(wù)一樣，模塊化自動駕駛系統(tǒng)中決策規(guī)劃層也可以繼續(xù)細(xì)分為執(zhí)行不同任務(wù)的子層。而這一分層設(shè)計最早其實是源自2007年舉辦的DAPRA城市挑戰(zhàn)賽，比賽中多數(shù)參賽隊伍都將自動駕駛系統(tǒng)的決策規(guī)劃方式包括三層：全局路徑規(guī)劃層（Route Planning）、行為決策層（Behavioral Layer）和運動規(guī)劃層（Motion Planning），如圖5所示。

圖5決策規(guī)劃分層架構(gòu)

全局路徑規(guī)劃層聚焦在相對頂層的路徑規(guī)劃，聚焦在分鐘到小時級別的規(guī)劃。該層在接收到輸入的目的地信息后，基于存儲的地圖信息搜索出一條自起始點至目標(biāo)點的一條可通過的路徑。如圖6所示，在藍(lán)色起點和黃色終點之間，黑色就是搜索出來的一條可通行的路徑，當(dāng)然路徑不止一條，如何搜索出最優(yōu)是下文將要介紹的內(nèi)容。

在全局路徑規(guī)劃的時候，也可以基于地圖精度和豐富度，提前考慮道路曲率半徑、坡度等信息，來避免搜索出部分參數(shù)超出ODD要求的全局路徑。但是高度隨機的交通參與者、高度動態(tài)的交通流以及高度復(fù)雜的道路結(jié)構(gòu)，全局路徑規(guī)劃是無法考慮周到的，因此還需要基于具體的行為決策進(jìn)行后面的運動規(guī)劃，也就是局部路徑規(guī)劃。

行為決策層在收到全局路徑后，結(jié)合感知環(huán)境信息、交通規(guī)則信息、車輛狀態(tài)信息、駕駛場景信息等，推導(dǎo)判斷下一分鐘或下一秒時刻的情況，作出車道保持、車輛跟隨、車道變換和制動避撞等的適合當(dāng)前交通環(huán)境的駕駛行為。如圖7所示，自車在檢測到前方存在低速行駛車輛，且右側(cè)車道滿足變道條件后，作出向右變道的駕駛行為決策。

運動規(guī)劃層也被稱為局部路徑規(guī)劃層，與全局路徑規(guī)劃聚焦在分鐘到小時級別的規(guī)劃不同，運動規(guī)劃聚焦在毫秒級到秒級的規(guī)劃。規(guī)劃的時候，根據(jù)輸入的行為決策信息、結(jié)合車輛實時位姿信息、局部環(huán)境信息、全局路徑參考信息等，在“安全、舒適、效率”的精神引領(lǐng)下，規(guī)劃生成一條滿足特定約束條件的平滑軌跡軌跡（包括行駛軌跡、速度、方向等），并輸入給控制執(zhí)行層。 如圖8所示，在車輛收到行為決策層的左變道指令后，主車基于各種信息規(guī)劃出幾條可行的路徑，如何規(guī)劃出最優(yōu)的路徑也是下文要介紹的內(nèi)容。

全局路徑規(guī)劃與運動規(guī)劃作為兩個層級的不同規(guī)劃，現(xiàn)將其特點匯總為表1。

表1 全局路徑規(guī)劃與運動規(guī)劃特點對比

02 全局路徑規(guī)劃常用算法

正菜之前，我們先來了解一下圖（包括有向圖和無向圖）的概念。圖是圖論中的基本概念，用于表示物體與物體之間存在某種關(guān)系的結(jié)構(gòu)。在圖中，物體被稱為節(jié)點或頂點，并用一組點或小圓圈表示。節(jié)點間的關(guān)系稱作邊，可以用直線或曲線來表示節(jié)點間的邊。

如果給圖的每條邊規(guī)定一個方向，那么得到的圖稱為有向圖，其邊也稱為有向邊，如圖9所示。在有向圖中，與一個節(jié)點相關(guān)聯(lián)的邊有出邊和入邊之分，而與一個有向邊關(guān)聯(lián)的兩個點也有始點和終點之分。相反，邊沒有方向的圖稱為無向圖。

圖9 有向圖示例

數(shù)學(xué)上，常用二元組G =（V，E）來表示其數(shù)據(jù)結(jié)構(gòu)，其中集合V稱為點集，E稱為邊集。對于圖6所示的有向圖，V可以表示為{A，B，C，D，E，F(xiàn)，G}，E可以表示為{，，，，，，}。表示從頂點A發(fā)向頂點B的邊，A為始點，B為終點。

在圖的邊中給出相關(guān)的數(shù)，稱為權(quán)。權(quán)可以代表一個頂點到另一個頂點的距離、耗費等，帶權(quán)圖一般稱為網(wǎng)。

在全局路徑規(guī)劃時，通常將圖10所示道路和道路之間的連接情況，通行規(guī)則，道路的路寬等各種信息處理成有向圖，其中每一個有向邊都是帶權(quán)重的，也被稱為路網(wǎng)（Route Network Graph）。

那么，全局路徑的規(guī)劃問題就變成了在路網(wǎng)中，搜索到一條最優(yōu)的路徑，以便可以盡快見到那個心心念念的她，這也是全局路徑規(guī)劃算法最樸素的愿望。而為了實現(xiàn)這個愿望，誕生了Dijkstra和A*兩種最為廣泛使用的全局路徑搜索算法。

2.1Dijkstra算法

戴克斯特拉算法（Dijkstra’s algorithm）是由荷蘭計算機科學(xué)家Edsger W. Dijkstra在1956年提出，解決的是有向圖中起點到其他頂點的最短路徑問題。

假設(shè)有A、B、C、D、E、F五個城市，用有向圖表示如圖11，邊上的權(quán)重代表兩座城市之間的距離，現(xiàn)在我們要做的就是求出起點A城市到其它城市的最短距離。

圖11 五個城市構(gòu)建的有向圖

用Dijkstra算法求解步驟如下：

（1）創(chuàng)建一個二維數(shù)組E來描述頂點之間的距離關(guān)系，如圖12所示。E[B][C]表示頂點B到頂點C的距離。自身之間的距離設(shè)為0，無法到達(dá)的頂點之間設(shè)為無窮大。

圖12 頂點之間的距離關(guān)系

（2）創(chuàng)建一個一維數(shù)組Dis來存儲起點A到其余頂點的最短距離。一開始我們并不知道起點A到其它頂點的最短距離，一維數(shù)組Dis中所有值均賦值為無窮大。接著我們遍歷起點A的相鄰頂點，并將與相鄰頂點B和C的距離3（E[A][B]）和10（E[A][C]）更新到Dis[B]和Dis[C]中，如圖13所示。這樣我們就可以得出起點A到其余頂點最短距離的一個估計值。

圖13 Dis經(jīng)過一次遍歷后得到的值

（3）接著我們尋找一個離起點A距離最短的頂點，由數(shù)組Dis可知為頂點B。頂點B有兩條出邊，分別連接頂點C和D。因起點A經(jīng)過頂點B到達(dá)頂點C的距離8（E[A][B] + E[B][C] = 3 + 5）小于起點A直接到達(dá)頂點C的距離10，因此Dis[C]的值由10更新為8。同理起點A經(jīng)過B到達(dá)D的距離5（E[A][B] + E[B][D] = 3 + 2）小于初始值無窮大，因此Dis[D]更新為5，如圖14所示。

圖14Dis經(jīng)過第二次遍歷后得到的值

（4）接著在剩下的頂點C、D、E、F中，選出里面離起點A最近的頂點D，繼續(xù)按照上面的方式對頂點D的所有出邊進(jìn)行計算，得到Dis[E]和Dis[F]的更新值，如圖15所示。

圖15 Dis經(jīng)過第三次遍歷后得到的值

（5）繼續(xù)在剩下的頂點C、E、F中，選出里面離起點A最近的頂點C，繼續(xù)按照上面的方式對頂點C的所有出邊進(jìn)行計算，得到Dis[E]的更新值，如圖16所示。

圖16 Dis經(jīng)過第四次遍歷后得到的值

（6）繼續(xù)在剩下的頂點E、F中，選出里面離起點A最近的頂點E，繼續(xù)按照上面的方式對頂點E的所有出邊進(jìn)行計算，得到Dis[F]的更新值，如圖17所示。

圖17 Dis經(jīng)過第五次遍歷后得到的值

（7）最后對頂點F所有點出邊進(jìn)行計算，此例中頂點F沒有出邊，因此不用處理。至此，數(shù)組Dis中距離起點A的值都已經(jīng)從“估計值”變?yōu)榱恕按_定值”。

基于上述形象的過程，Dijkstra算法實現(xiàn)過程可以歸納為如下步驟：

（1）將有向圖中所有的頂點分成兩個集合P和Q，P用來存放已知距離起點最短距離的頂點，Q用來存放剩余未知頂點。可以想象，一開始，P中只有起點A。同時我們創(chuàng)建一個數(shù)組Flag[N]來記錄頂點是在P中還是Q中。對于某個頂點N，如果Flag[N]為1則表示這個頂點在集合P中，為1則表示在集合Q中。

（2）起點A到自己的最短距離設(shè)置為0，起點能直接到達(dá)的頂點N，Dis[N]設(shè)為E[A][N]，起點不能直接到達(dá)的頂點的最短路徑為設(shè)為∞。

（3）在集合Q中選擇一個離起點最近的頂點U（即Dis[U]最?。┘尤氲郊螾。并計算所有以頂點U為起點的邊，到其它頂點的距離。例如存在一條從頂點U到頂點V的邊，那么可以通過將邊U->V添加到尾部來拓展一條從A到V的路徑，這條路徑的長度是Dis[U]+e[U][V]。如果這個值比目前已知的Dis[V]的值要小，我們可以用新值來替代當(dāng)前Dis[V]中的值。

（4）重復(fù)第三步，如果最終集合Q結(jié)束，算法結(jié)束。最終Dis數(shù)組中的值就是起點到所有頂點的最短路徑。

2.2A*算法

1968年，斯坦福國際研究院的Peter E. Hart, Nils Nilsson以及Bertram Raphael共同發(fā)明了A*算法。A*算法通過借助一個啟發(fā)函數(shù)來引導(dǎo)搜索的過程，可以明顯地提高路徑搜索效率。

下文仍以一個實例來簡單介紹A*算法的實現(xiàn)過程。如圖18所示，假設(shè)小馬要從A點前往B點大榕樹底下去約會，但是A點和B點之間隔著一個池塘。為了能盡快提到達(dá)約會地點，給姑娘留下了一個守時踏實的好印象，我們需要給小馬搜索出一條時間最短的可行路徑。

A*算法的第一步就是簡化搜索區(qū)域，將搜索區(qū)域劃分為若干柵格。并有選擇地標(biāo)識出障礙物不可通行與空白可通行區(qū)域。一般地，柵格劃分越細(xì)密，搜索點數(shù)越多，搜索過程越慢，計算量也越大；柵格劃分越稀疏，搜索點數(shù)越少，相應(yīng)的搜索精確性就越低。

如圖19所示，我們在這里將要搜索的區(qū)域劃分成了正方形（當(dāng)然也可以劃分為矩形、六邊形等）的格子，圖中藍(lán)色格子代表A點（小馬當(dāng)前的位置），紫色格子代表B點（大榕樹的位置），灰色格子代表池塘。同時我們可以用一個二維數(shù)組S來表示搜素區(qū)域，數(shù)組中的每一項代表一個格子，狀態(tài)代表可通行和不可通行。

圖19 經(jīng)過簡化后的搜索區(qū)域

接著我們引入兩個集合OpenList和CloseList，以及一個估價函數(shù)F = G + H。OpenList用來存儲可到達(dá)的格子，CloseList用來存儲已到達(dá)的格子。G代表從起點到當(dāng)前格子的距離，H表示在不考慮障礙物的情況下，從當(dāng)前格子到目標(biāo)格子的距離。F是起點經(jīng)由當(dāng)前格子到達(dá)目標(biāo)格子的總代價，值越小，綜合優(yōu)先級越高。

G和H也是A*算法的精髓所在，通過考慮當(dāng)前格子與起始點的距離，以及當(dāng)前格子與目標(biāo)格子的距離來實現(xiàn)啟發(fā)式搜索。對于H的計算，又有兩種方式，一種是歐式距離，一種是曼哈頓距離。

歐式距離用公式表示如下，物理上表示從當(dāng)前格子出發(fā)，支持以8個方向向四周格子移動（橫縱向移動+對角移動）。

曼哈頓距離用公式表示如下，物理上表示從當(dāng)前格子出發(fā)，支持以4個方向向四周格子移動（橫縱向移動）。這是A*算法最常用的計算H值方法，本文H值的計算也采用這種方法。

現(xiàn)在我們開始搜索，查找最短路徑。首先將起點A放入到OpenList中，并計算出此時OpenList中F值最小的格子作為當(dāng)前方格移入到CloseList中。由于當(dāng)前OpenList中只有起點A這個格子，所以將起點A移入CloseList，代表這個格子已經(jīng)檢查過了。

接著我們找出當(dāng)前格子A上下左右所有可通行的格子，看它們是否在OpenList當(dāng)中。如果不在，加入到OpenList中計算出相應(yīng)的G、H、F值，并把當(dāng)前格子A作為它們的父節(jié)點。本例子，我們假設(shè)橫縱向移動代價為10，對角線移動代價為14。

我們在每個格子上標(biāo)出計算出來的F、G、H值，如圖20所示，左上角是F，左下角是G，右下角是H。通過計算可知S[3][2]格子的F值最小，我們把它從OpenList中取出，放到CloseList中。

圖20 第一輪計算后的結(jié)果

接著將S[3][2]作為當(dāng)前格子，檢查所有與它相鄰的格子，忽略已經(jīng)在CloseList或是不可通行的格子。如果相鄰的格子不在OpenList中，則加入到OpenList，并將當(dāng)前方格子S[3][2]作為父節(jié)點。

已經(jīng)在OpenList中的格子，則檢查這條路徑是否最優(yōu)，如果非最優(yōu)，不做任何操作。如果G值更小，則意味著經(jīng)由當(dāng)前格子到達(dá)OpenList中這個格子距離更短，此時我們將OpenList中這個格子的父節(jié)點更新為當(dāng)前節(jié)點。

對于當(dāng)前格子S[3][2]來說，它的相鄰5個格子中有4個已經(jīng)在OpenList，一個未在。對于已經(jīng)在OpenList中的4個格子，我們以它上面的格子S[2][2]舉例，從起點A經(jīng)由格子S[3][2]到達(dá)格子S[2][2]的G值為20（10+10）大于從起點A直接沿對角線到達(dá)格子S[2][2]的G值14。顯然A經(jīng)由格子S[3][2]到達(dá)格子S[2][2]不是最優(yōu)的路徑。當(dāng)把4個已經(jīng)在OpenList 中的相鄰格子都檢查后，沒有發(fā)現(xiàn)經(jīng)由當(dāng)前方格的更好路徑，因此我們不做任何改變。

對于未在OpenList的格子S[2][3]（假設(shè)小馬可以斜穿墻腳），加入OpenList中，并計算它的F、G、H值，并將當(dāng)前格子S[3][2]設(shè)置為其父節(jié)點。經(jīng)歷這一波騷操作后，OpenList中有5個格子，我們需要從中選擇F值最小的那個格子S[2][3]，放入CloseList中，并設(shè)置為當(dāng)前格子，如圖21所示。

圖21第二輪計算后的結(jié)果

重復(fù)上面的故事，直到終點也加入到OpenList中。此時我們以當(dāng)前格子倒推，找到其父節(jié)點，父節(jié)點的父節(jié)點……，如此便可搜索出一條最優(yōu)的路徑，如圖22中紅色圓圈標(biāo)識。

圖22 最后計算得到的結(jié)果

基于上述形象的過程，A*算法實現(xiàn)過程可以歸納為如下步驟：

（1）將搜索區(qū)域按一定規(guī)則劃分，把起點加入OpenList。

（2）在OpenList中查找F值最小的格子，將其移入CloseList，并設(shè)置為當(dāng)前格子。

（3）查找當(dāng)前格子相鄰的可通行的格子，如果它已經(jīng)在OpenList中，用G值衡量這條路徑是否更好。如果更好，將該格子的父節(jié)點設(shè)置為當(dāng)前格子，重新計算F、G值，如果非更好，不做任何處理；如果不在OpenList中，將它加入OpenList中，并以當(dāng)前格子為父節(jié)點計算F、G、H值。

（4）重復(fù)步驟（2）和步驟（3），直到終點加入到OpenList中。

2.3兩種算法比較

Dijkstra算法的基本思想是“貪心”，主要特點是以起點為中心向周圍層層擴展，直至擴展到終點為止。通過Dijkstra算法得出的最短路徑是最優(yōu)的，但是由于遍歷沒有明確的方向，計算的復(fù)雜度比較高，路徑搜索的效率比較低。且無法處理有向圖中權(quán)值為負(fù)的路徑最優(yōu)問題。

A*算法將Dijkstra算法與廣度優(yōu)先搜索（Breadth-First-Search，BFS）算法相結(jié)合，并引入啟發(fā)函數(shù)（估價函數(shù)），大大減少了搜索節(jié)點的數(shù)量，提高了搜索效率。但是A*先入為主的將最早遍歷路徑當(dāng)成最短路徑，不適用于動態(tài)環(huán)境且不太適合高維空間，且在終點不可達(dá)時會造成大量性能消耗。

圖24是兩種算法路徑搜索效率示意圖，左圖為Dijkstra算法示意圖，右圖為A*算法示意圖，帶顏色的格子表示算法搜索過的格子。由圖23可以看出，A*算法更有效率，手術(shù)的格子更少。

圖23 Dijkstra算法和A*算法搜索效率對比圖（圖片來源：https://mp.weixin.qq.com/s/myU204Uq3tfuIKHGD3oEfw）

03 行為決策常用算法

作為L4級自動駕駛的優(yōu)秀代表Robotaxi，部分人可能已經(jīng)在自己的城市欣賞過他們不羈的造型，好奇心強烈的可能都已經(jīng)體驗過他們的無人“推背”服務(wù)。作為一個占有天時地利優(yōu)勢的從業(yè)人員，我時常在周末選一個人和的時間，叫個免費Robotaxi去超市買個菜。

剛開始幾次乘坐，我的注意力全都放在安全員的雙手，觀察其是否在接管；過了一段時間，我的注意力轉(zhuǎn)移到中控大屏，觀察其夢幻般的交互方式；而現(xiàn)在，我的注意力轉(zhuǎn)移到了智能上，觀察其在道路上的行為決策是否足夠聰明。 而這一觀察，竟真總結(jié)出不少共性問題。比如十字路口左轉(zhuǎn)，各家Robotaxi總是表現(xiàn)的十分小心謹(jǐn)慎，人類司機一腳油門過去的場景，Robotaxi總是再等等、再看看。且不同十字路口同一廠家的Robotaxi左轉(zhuǎn)的策略基本一致，完全沒有人類司機面對不同十字路口、不同交通流、不同天氣環(huán)境時的“隨機應(yīng)變”。

面對復(fù)雜多變場景時自動駕駛行為決策表現(xiàn)出來的小心謹(jǐn)慎，像極了人類進(jìn)入一個新環(huán)境時采取的猥瑣發(fā)育策略。但在自動駕駛終局到來的那天，自動駕駛的決策規(guī)劃能否像人類一樣，在洞悉了人情社會的生活法則之后，做到“見人說人話”、“見人下飯”呢？ 在讓自動駕駛車輛的行為決策變得越來越像老司機的努力過程中，主要誕生了基于規(guī)則和基于學(xué)習(xí)的兩大類行為決策方法。

3.1基于規(guī)則的方法

在基于規(guī)則的方法中，通過對自動駕駛車輛的駕駛行為進(jìn)行劃分，并基于感知環(huán)境、交通規(guī)則等信息建立駕駛行為規(guī)則庫。自動駕駛車輛在行駛過程中，實時獲取交通環(huán)境、交通規(guī)則等信息，并與駕駛行為規(guī)則庫中的經(jīng)驗知識進(jìn)行匹配，進(jìn)而推理決策出下一時刻的合理自動駕駛行為。

正如全局路徑規(guī)劃的前提是地圖一樣，自動駕駛行為分析也成為基于規(guī)則的行為決策的前提。不同應(yīng)用場景下的自動駕駛行為不完全相同，以高速主干路上的L4自動駕駛卡車為例，其自動駕駛行為可簡單分解為單車道巡航、自主變道、自主避障三個典型行為。

單車道巡航是卡車L4自動駕駛系統(tǒng)激活后的默認(rèn)狀態(tài)，車道保持的同時進(jìn)行自適應(yīng)巡航。此駕駛行為還可以細(xì)分定速巡航、跟車巡航等子行為，而跟車巡航子行為還可以細(xì)分為加速、加速等子子行為，真是子子孫孫無窮盡也。 自主變道是在變道場景（避障變道場景、主干路變窄變道場景等）發(fā)生及變道空間（與前車和后車的距離、時間）滿足后進(jìn)行左/右變道。自主避障是在前方出現(xiàn)緊急危險情況且不具備自主變道條件時，采取的緊急制動行為，避免與前方障礙物或車輛發(fā)生碰撞。其均可以繼續(xù)細(xì)分，此處不再展開。

上面列舉的駕駛行為之間不是獨立的，而是相互關(guān)聯(lián)的，在一定條件滿足后可以進(jìn)行實時切換，從而支撐起L4自動駕駛卡車在高速主干路上的自由自在?，F(xiàn)將例子中的三種駕駛行為之間的切換條件簡單匯總?cè)绫?，真實情況比這嚴(yán)謹(jǐn)、復(fù)雜的多，此處僅為后文解釋基于規(guī)則的算法所用。

表2 狀態(tài)間的跳轉(zhuǎn)事件

在基于規(guī)則的方法中，有限狀態(tài)機（FiniteStateMaechine，F(xiàn)SM）成為最具有代表性的方法。2007年斯坦福大學(xué)參加DARPA城市挑戰(zhàn)賽時的無人車“Junior”，其行為決策采用的就是有限狀態(tài)機方法。 有限狀態(tài)機是一種離散的數(shù)學(xué)模型，也正好符合自動駕駛行為決策的非連續(xù)特點，主要用來描述對象生命周期內(nèi)的各種狀態(tài)以及如何響應(yīng)來自外界的各種事件。有限狀態(tài)機包含四大要素：狀態(tài)、事件、動作和轉(zhuǎn)移。

事件發(fā)生后，對象產(chǎn)生相應(yīng)的動作，從而引起狀態(tài)的轉(zhuǎn)移，轉(zhuǎn)移到新狀態(tài)或維持當(dāng)前狀態(tài)。 我們將上述駕駛行為定義為有限狀態(tài)機的狀態(tài)，每個狀態(tài)之間在滿足一定的事件(或條件)后，自動駕駛車輛執(zhí)行一定的動作后，就可以轉(zhuǎn)移到新的狀態(tài)。

比如單車道巡航狀態(tài)下，前方車輛低速行駛，自車在判斷旁邊車道滿足變道條件要求后，切換到自主變道狀態(tài)。自主變道完成后，系統(tǒng)再次回到單車道巡航狀態(tài)。 結(jié)合表2中的切換條件，各個狀態(tài)在滿足一定事件(或條件)后的狀態(tài)跳轉(zhuǎn)示意圖如圖24所示。



圖24 狀態(tài)跳轉(zhuǎn)示意圖

基于有限狀態(tài)機理論構(gòu)建的智能車輛自動駕駛行為決策系統(tǒng)，可將復(fù)雜的自動駕駛過程分解為有限個自動駕駛駕駛行為，邏輯推理清晰、應(yīng)用簡單、實用性好等特點，使其成為當(dāng)前自動駕駛領(lǐng)域目前最廣泛使用的行為決策方法。 但該方法沒有考慮環(huán)境的動態(tài)性、不確定性以及車輛運動學(xué)以及動力學(xué)特性對駕駛行為決策的影響，因此多適用于簡單場景下，很難勝任具有豐富結(jié)構(gòu)化特征的城區(qū)道路環(huán)境下的行為決策任務(wù)。

3.2 基于學(xué)習(xí)的方法

上文介紹的基于規(guī)則的行為決策方法依靠專家經(jīng)驗搭建的駕駛行為規(guī)則庫，但是由于人類經(jīng)驗的有限性，智能性不足成為基于規(guī)則的行為決策方法的最大制約，復(fù)雜交通工況的事故率約為人類駕駛員的百倍以上。鑒于此，科研工作者開始探索基于學(xué)習(xí)的方法，并在此基礎(chǔ)上了誕生了數(shù)據(jù)驅(qū)動型學(xué)習(xí)方法和強化學(xué)習(xí)方法。

數(shù)據(jù)驅(qū)動型學(xué)習(xí)是一種依靠自然駕駛數(shù)據(jù)直接擬合神經(jīng)網(wǎng)絡(luò)模型的方法，首先用提前采集到的老司機開車時的自然駕駛數(shù)據(jù)訓(xùn)練神經(jīng)網(wǎng)絡(luò)模型，訓(xùn)練的目標(biāo)是讓自動駕駛行為決策水平接近老司機。

而后將訓(xùn)練好的算法模型部署到車上，此時車輛的行為決策就像老司機一樣，穿行在大街小巷。讀者可參見端到端自動駕駛章節(jié)中介紹的NVIDIA demo案例。 強化學(xué)習(xí)方法通過讓智能體（行為決策主體）在交互環(huán)境中以試錯方式運行，并基于每一步行動后環(huán)境給予的反饋（獎勵或懲罰），來不斷調(diào)整智能體行為，從而實現(xiàn)特定目的或使得整體行動收益最大。通過這種試錯式學(xué)習(xí)，智能體能夠在動態(tài)環(huán)境中自己作出一系列行為決策，既不需要人為干預(yù)，也不需要借助顯式編程來執(zhí)行任務(wù)。

強化學(xué)習(xí)可能不是每個人都聽過，但DeepMind開發(fā)的圍棋智能AlphaGo（阿爾法狗），2016年3月戰(zhàn)勝世界圍棋冠軍李世石，2017年5月后又戰(zhàn)勝圍棋世界排名第一柯潔的事，大家應(yīng)該都有所耳聞。更過分的是，半年后DeepMind在發(fā)布的新一代圍棋智能AlphaZero（阿爾法狗蛋），通過21天的閉關(guān)修煉，就戰(zhàn)勝了家族出現(xiàn)的各種狗子們，成功當(dāng)選狗蛋之王。 而賦予AlphaGo及AlphaZero戰(zhàn)勝人類棋手的魔法正是強化學(xué)習(xí)，機器學(xué)習(xí)的一種。

機器學(xué)習(xí)目前有三大派別：監(jiān)督學(xué)習(xí)、無監(jiān)督學(xué)習(xí)和強化學(xué)習(xí)。監(jiān)督學(xué)習(xí)算法基于歸納推理，通過使用有標(biāo)記的數(shù)據(jù)進(jìn)行訓(xùn)練，以執(zhí)行分類或回歸；無監(jiān)督學(xué)習(xí)一般應(yīng)用于未標(biāo)記數(shù)據(jù)的密度估計或聚類； 強化學(xué)習(xí)自成一派，通過讓智能體在交互環(huán)境中以試錯方式運行，并基于每一步行動后環(huán)境給予的反饋（獎勵或懲罰），來不斷調(diào)整智能體行為，從而實現(xiàn)特定目的或使得整體行動收益最大。

通過這種試錯式學(xué)習(xí)，智能體能夠在動態(tài)環(huán)境中自己作出一系列決策，既不需要人為干預(yù)，也不需要借助顯式編程來執(zhí)行任務(wù)。 這像極了馬戲團訓(xùn)練各種動物的過程，馴獸師一個抬手動作（環(huán)境），動物（智能體）若完成相應(yīng)動作，則會獲得美味的食物（正反饋），若沒有完成相應(yīng)動作，食物可能換成了皮鞭（負(fù)反饋）。

時間一久，動物就學(xué)會基于馴獸師不同的手勢完成不同動作，來使自己獲得最多數(shù)量的美食。 大道至簡，強化學(xué)習(xí)亦如此。一個戰(zhàn)勝人類圍棋冠軍的“智能”也僅由五部分組成：智能體（Agent）、環(huán)境（Environment）、狀態(tài)（State）、行動（Action）和獎勵（Reward）。強化學(xué)習(xí)系統(tǒng)架構(gòu)如圖25所示，結(jié)合自動駕駛代客泊車中的泊入功能，我們介紹一下各組成的定義及作用。



圖25 強化學(xué)習(xí)系統(tǒng)架構(gòu)

代客泊車泊入功能的追求非常清晰，就是在不發(fā)生碰撞的前提下，實現(xiàn)空閑停車位的快速泊入功能。這個過程中，承載強化學(xué)習(xí)算法的控制器（域控制器/中央計算單元）就是智能體，也是強化學(xué)習(xí)訓(xùn)練的主體。智能體之外的整個泊車場景都是環(huán)境，包括停車場中的立柱、車輛、行人、光照等。

訓(xùn)練開始后，智能體實時從車載傳感器（激光雷達(dá)、相機、IMU、超聲波雷達(dá)等）讀取環(huán)境狀態(tài)，并基于當(dāng)前的環(huán)境狀態(tài)，采取相應(yīng)的轉(zhuǎn)向、制動和加速行動。如果基于當(dāng)前環(huán)境狀態(tài)采用的行動，是有利于車輛快速泊入，則智能體會得到一個獎勵，反之則會得到一個懲罰。 在獎勵和懲罰的不斷刺激下，智能體學(xué)會了適應(yīng)環(huán)境，學(xué)會了下次看到空閑車位時可以一把倒入，學(xué)會了面對不同車位類型時采取不同的風(fēng)騷走位。 從上述例子，我們也可以總結(jié)出訓(xùn)練出一個優(yōu)秀的“智能”，大概有如下幾個步驟：

（1）創(chuàng)建環(huán)境。定義智能體可以學(xué)習(xí)的環(huán)境，包括智能體和環(huán)境之間的接口。環(huán)境可以是仿真模型，也可以是真實的物理系統(tǒng)。仿真環(huán)境通常是不錯的起點，一是安全，二是可以試驗。

（2）定義獎勵。指定智能體用于根據(jù)任務(wù)目標(biāo)衡量其性能的獎勵信號，以及如何根據(jù)環(huán)境計算該信號。可能需要經(jīng)過數(shù)次迭代才能實現(xiàn)正確的獎勵塑造。

（3）創(chuàng)建智能體。智能體由策略和訓(xùn)練算法組成，因此您需要：

（a）選擇一種表示策略的方式（例如，使用神經(jīng)網(wǎng)絡(luò)或查找表）。思考如何構(gòu)造參數(shù)和邏輯，由此構(gòu)成智能體的決策部分。

（b）選擇合適的訓(xùn)練算法。大多數(shù)現(xiàn)代強化學(xué)習(xí)算法依賴于神經(jīng)網(wǎng)絡(luò)，因為后者非常適合處理大型狀態(tài)/動作空間和復(fù)雜問題。

（4）訓(xùn)練和驗證智能體。設(shè)置訓(xùn)練選項（如停止條件）并訓(xùn)練智能體以調(diào)整策略。要驗證經(jīng)過訓(xùn)練的策略，最簡單的方法是仿真。

（5）部署策略。使用生成的 C/C++ 或 CUDA 代碼等部署經(jīng)過訓(xùn)練的策略表示。此時無需擔(dān)心智能體和訓(xùn)練算法；策略是獨立的決策系統(tǒng)。 強化學(xué)習(xí)方法除了具有提高行為決策智能水平的能力，還具備合并決策和控制兩個任務(wù)到一個整體、進(jìn)行統(tǒng)一求解的能力。將決策與控制進(jìn)行合并，這樣既發(fā)揮了強化學(xué)習(xí)的求解優(yōu)勢，又能進(jìn)一步提高自動駕駛系統(tǒng)的智能性。

實際上，人類駕駛員也是具有很強的整體性的，我們很難區(qū)分人類的行為中哪一部分是自主決策，哪一部分是運動控制。 現(xiàn)階段強化學(xué)習(xí)方法的應(yīng)用還處在摸索階段，應(yīng)用在自動駕駛的潛力還沒有被完全發(fā)掘出來，這讓我想起了母校的一句校歌：“能不奮勉乎吾曹？”

04 運動規(guī)劃常用算法

有了全局路徑參考信息，有了局部環(huán)境信息了，有了行為決策模塊輸入的決策信息，下一步自然而然的就要進(jìn)行運動規(guī)劃，從而生成一條局部的更加具體的行駛軌跡，并且這條軌跡要滿足安全性和舒適性要求。

考慮到車輛是一個具有巨大慣性的鐵疙瘩且沒有瞬間移動的功能，如果僅考慮瞬時狀態(tài)的行駛軌跡，不規(guī)劃出未來一段時間有前瞻性的行駛軌跡，那么很容易造成一段時間后無解。因此，運動規(guī)劃生成的軌跡是一種由二維空間和一維時間組成的三維空間中的曲線，是一種偏實時的路徑規(guī)劃。

運動規(guī)劃的第一步往往采用隨機采樣算法，即走一步看一步，不斷更新行駛軌跡。代表算法是基于采樣的方法：PRM、RRT、Lattice。這類算法通過隨機采樣的方式在地圖上生成子節(jié)點，并與父節(jié)點相連，若連線與障礙物無碰撞風(fēng)險，則擴展該子節(jié)點。重復(fù)上述步驟，不斷擴展樣本點，直到生成一條連接起點到終點的路徑。

4.1 PRM算法

概率路標(biāo)法 (Probabilistic Road Maps, PRM），是一種經(jīng)典的采樣方法，由Lydia E.等人在1996年提出。PRM主要包含三個階段，一是采樣階段，二是碰撞檢測階段，三是搜索階段。 圖26為已知起點A和終點B的地圖空間，黑色空間代表障礙物，白色空間代表可通行區(qū)域。在采樣階段中，PRM首先在地圖空間進(jìn)行均勻的隨即采樣，也就是對地圖進(jìn)行稀疏采樣，目的是將大地圖簡化為較少的采樣點。



圖26 PRM工作原理示意圖

在碰撞檢測階段，剔除落在障礙物上的采樣點，并將剩下的點與其一定距離范圍內(nèi)的點相連，同時刪除穿越障礙物的連線，從而構(gòu)成一張無向圖。

在搜索階段，利用全局路徑規(guī)劃算法章節(jié)介紹的搜索算法（Dijkstra、A*等）在無向圖中進(jìn)行搜索，從而找出一條起點A到終點B之間的可行路徑。

算法步驟可以總結(jié)為：

（1）構(gòu)造無向圖G =（V，E），其中V代表隨機采樣的點集，E代表兩采樣點之間所有可能的無碰撞路徑，G初始狀態(tài)為空。

（2）隨機撒點，并選取一個無碰撞的點c(i)加入到V中。

（3）定義距離r，如果c(i)與V中某些點的距離小于r，則將V中這些點定義為c(i)的鄰域點。

（4）將c(i)與其鄰域點相連，生成連線t，并檢測連線t是否與障礙物發(fā)生碰撞，如果無碰撞，則將t加入E中。

（5）重復(fù)步驟2-4，直到所有采樣點（滿足采樣數(shù)量要求）均已完成上述步驟。

（6）采用圖搜索算法對無向圖G進(jìn)行搜索，如果能找到起始點A到終點B的路線，說明存在可行的行駛軌跡。

PRM算法相比基于搜索的算法，簡化了環(huán)境、提高了效率。但是在有狹窄通道場景中，很難采樣出可行路徑，效率會大幅降低。

4.2 RRT

快速探索隨機樹（Rapidly Exploring Random Trees，RRT），是Steven M. LaValle和James J. Kuffner Jr.在1998年提出的一種基于隨機生長樹思想實現(xiàn)對非凸高維空間快速搜索的算法。與PRM相同的是兩者都是基于隨機采樣的算法，不同的是PRM最終生成的是一個無向圖，而RRT生成的是一個隨機樹。

RRT的最顯著特征就是具備空間探索的能力，即從一點向外探索拓展的特征。 RRT分單樹和雙樹兩種類型，單樹RRT將規(guī)起點作為隨機樹的根節(jié)點，通過隨機采樣、碰撞檢測的方式為隨機樹增加葉子節(jié)點，最終生成一顆隨機樹。而雙樹RRT則擁有兩顆隨機樹，分別以起點和終點為根節(jié)點，以同樣的方式進(jìn)行向外的探索，直到兩顆隨機樹相遇，從而達(dá)到提高規(guī)劃效率的目的。

下面以圖27所示的地圖空間為例介紹單樹RRT算法的實現(xiàn)過程。在此地圖空間中，我們只知道起點A和終點B以及障礙物的位置（黑色的框）。

圖27 RRT算法舉例的地圖空間

對于單樹RRT算法，我們將起點A設(shè)置為隨機樹的根，并生成一個隨機采樣點，如圖27所示，隨機采樣點有下面這幾種情況。

（1）隨機采樣點1落在自由區(qū)域中，但是根節(jié)點A和隨機采樣點1之間的連線存在障礙物，無法通過碰撞檢測，采樣點1會被舍棄，重新再生成隨機采樣點。

（2）隨機采樣點2落在障礙物的位置，采樣點2也會被舍棄，重新再生成隨機采樣點。

（3）隨機采樣點3落在自由區(qū)域，且與根節(jié)點A之間的連線不存在障礙物，但是超過根節(jié)點的步長限制。但此時這個節(jié)點不會被簡單的舍棄點，而是會沿著根節(jié)點和隨機采樣點3的連線，找出符合步長限制的中間點，將這個中間點作為新的采樣點，也就是圖28中的4。

圖28 不同隨機采樣點舉例

接著我們繼續(xù)生成新的隨機采樣點，如果新的隨機采樣點位于自由區(qū)域，那么我們就可以遍歷隨機樹中已有的全部節(jié)點，找出距離新的隨機采樣點最近的節(jié)點，同時求出兩者之間的距離，如果滿足步長限制的話，我們將接著對這兩個節(jié)點進(jìn)行碰撞檢測，如果不滿足步長限制的話，我們需要沿著新的隨機采樣點和最近的節(jié)點的連線方向，找出一個符合步長限制的中間點，用來替代新的隨機采樣點。

最后如果新的隨機采樣點和最近的節(jié)點通過了碰撞檢測，就意味著二者之間存在邊，我們便可以將新的隨機采樣點添加進(jìn)隨機樹中，并將最近的點設(shè)置為新的隨機采樣點的父節(jié)點。 重復(fù)上述過程，直到新的隨機采樣點在終點的步長限制范圍內(nèi)，且滿足碰撞檢測。則將新的隨機采樣點設(shè)為終點B的父節(jié)點，并將終點加入隨機樹，從而完成迭代，生成如圖29所示的完整隨機樹。

圖29隨機樹結(jié)算結(jié)果示例

相比PRM，RRT無需搜索步驟、效率更高。通過增量式擴展的方式，找到路徑后就立即結(jié)束，搜索終點的目的性更強。但是RRT作為一種純粹的隨機搜索算法，對環(huán)境類型不敏感，當(dāng)?shù)貓D空間中存在狹窄通道時，因被采樣的概率低，導(dǎo)致算法的收斂速度慢，效率會大幅下降，有時候甚至難以在有狹窄通道的環(huán)境找到路徑。 圖30展示了 RRT應(yīng)對存在狹窄通道地圖空間時的兩種表現(xiàn)，一種是RRT很快就找到了出路，一種是一直被困在障礙物里面。



圖30 RRT面對狹窄通道時的表現(xiàn)

圍繞如何更好的“進(jìn)行隨機采樣”、“定義最近的點”以及“進(jìn)行樹的擴展”等方面，誕生了多種改進(jìn)型的算法，包括雙樹RRT-Connect（雙樹）、lazy-RRT, RRT-Extend等。 PRM和RRT都是一個概率完備但非最優(yōu)的路徑規(guī)劃算法，也就是只要起點和終點之間存在有效的路徑，那么只要規(guī)劃的時間足夠長，采樣點足夠多，必然可以找到有效的路徑。但是這個解無法保證是最優(yōu)的。

采用PRM和RRT等隨機采樣算法生成的行駛軌跡，大多是一條條線段，線段之間的曲率也不不連續(xù)，這樣的行駛軌跡是不能保證舒適性的，所以還需要進(jìn)一步進(jìn)行曲線平滑、角度平滑處理。代表算法是基于曲線插值的方法：RS曲線、Dubins曲線、多項式曲線、貝塞爾曲線和樣條曲線等。 所有基于曲線插值方法要解決的問題就是：在圖31上的若干點中，求出一條光滑曲線盡可能逼近所有點。下文以多項式曲線和貝塞爾曲線為例，介紹曲線插值算法的示例。

4.3多項式曲線

找到一條曲線擬合所有的點，最容易想到的方法就是多項式曲線。常用的有三階多項式曲線、五階多項式曲線和七階多項式曲線。理論上只要多項式的階數(shù)足夠高，就可以擬合各種曲線。但從滿足需求和工程實現(xiàn)的角度，階數(shù)越低越好。 車輛在運動規(guī)劃中，舒適度是一個非常重要的指標(biāo)，在物理中衡量舒適性的物理量為躍度(Jerk)，它是加速度的導(dǎo)數(shù)。

Jerk的絕對值越小意味著加速度的變化越平緩，加速度的變化越平緩意味著越舒適。而五次多項式曲線則被證明是在運動規(guī)劃中可以使Jerk比較小的多項式曲線。 以圖30所示換道場景為例，已知Frenet坐標(biāo)系下?lián)Q道起點和終點的六個參數(shù)s0、v0、a0、st、vt、at，采用橫縱向解耦分別進(jìn)行運動規(guī)劃的方法，可得橫向位置x(t)和縱向位置y(t)關(guān)于時間t的五次多項式表達(dá)式。

五次多項式中存在六個未知量，將起點和終點已知的六個參數(shù)代入便可這個六個未知量。然后根據(jù)時間t進(jìn)行合并即可得到橫縱向聯(lián)合控制的曲線，即最終運動規(guī)劃的曲線。

4.4 貝塞爾曲線

對于比較少的點來說，采用多項式曲線非常合理。但是當(dāng)點比較多時，為了逼近所有點，就不得不增加多項式的次數(shù)，而由此帶來的負(fù)面影響就是曲線震蕩。退一步講，即使震蕩能夠被消除，獲得的曲線由于存在非常多的起伏，也不夠光順。而貝塞爾曲線的出現(xiàn)，正好解決了上述問題。

1959年，法國數(shù)學(xué)家保爾·德·卡斯特里使用獨家配方求出貝塞爾曲線。1962年，法國雷諾汽車公司工程師皮埃爾·貝塞爾將自己在汽車造型設(shè)計的一些心得歸納總結(jié)，并廣泛發(fā)表。貝塞爾在造型設(shè)計的心得可簡單總結(jié)為：先用折線段勾畫出汽車的外形大致輪廓，再用光滑的參數(shù)曲線去逼近這個折線多邊形。

繪制貝塞爾曲線之前，我們需要知道起點和終點的參數(shù)，然后再提供任意數(shù)量的控制點的參數(shù)。如果控制點的數(shù)量為0，則為一階貝塞爾曲線，如果控制點的數(shù)量為1，則為二階貝塞爾曲線，如果控制點的數(shù)量為2，則為三階貝塞爾曲線，依次類推。不論是起點、終點還是控制點，它們均代表坐標(biāo)系下的一個向量。

下面我們以經(jīng)典的二階貝塞爾曲線為例，介紹其繪制方法。如圖32所示，P0和P2為已知的參數(shù)的起點和終點，P1為已知參數(shù)的控制點。首先我們按照起點、控制點、終點的順序依次連接，生成兩條直線。



圖32 二階貝塞爾曲線示例

接著我們以每條直線的起點開始，向各自的終點按比例t取點，如圖中的A和B。隨后我們將A和B相連得到一條直線，也按相同的比例t取點，便可得到C點，這也是二階貝塞爾曲線在比例為t時會經(jīng)過的點。比列t滿足如下的公式。

當(dāng)我們比例t一點點變大（從0到1），就得到起點到終點的所有貝塞爾點，所有點相連便繪制出完整的二階貝塞爾曲線C(t)，用公式表達(dá)為。

由二階貝塞爾曲線拓展到N階貝塞爾曲線，可得數(shù)學(xué)表達(dá)式如下。

審核編輯：劉清