無人駕駛路徑規(guī)劃算法綜述

海振洋，王健，牟思凱，杜若飛，費明哲，李晶瑋

（250399 山東省 濟南市 山東交通學院）

0 引言

自動駕駛技術(shù)作為智能交通中重要的一個環(huán)節(jié)，已經(jīng)成為汽車工業(yè)發(fā)展過程中的新興趨勢。2020 年11 月，李克強教授在2020 世界智能網(wǎng)聯(lián)汽車大會上公布了《智能網(wǎng)聯(lián)汽車技術(shù)路線圖2.0》[1]，該文件為汽車產(chǎn)業(yè)未來15 年的發(fā)展設(shè)定了技術(shù)路線，中國智能網(wǎng)聯(lián)汽車的技術(shù)架構(gòu)和關(guān)鍵技術(shù)已經(jīng)形成了三橫兩縱的格局，智能決策技術(shù)是三橫中的核心之一。行業(yè)研究和學術(shù)界之所以青睞它，是因為它可以充分利用資源，改善駕駛環(huán)境，減少道路擁堵。算法策略使硬件發(fā)揮出充分的效能，實現(xiàn)多功能控制，負責邏輯制定的規(guī)劃算法在控制中占據(jù)重要地位[2]。運動規(guī)劃的方法是通過規(guī)劃路徑，躲避障礙物，優(yōu)化路線，使車輛精準、快捷地抵達目的地，提高了司機的工作效率，增加了安全性。

本文闡述了自動駕駛汽車路徑規(guī)劃的各種方法，深入分析路規(guī)劃算法的屬性。從時間處理的復雜程度（算法運行過程所需要的工作量）、空間處理的復雜度（算法在計算過程中會占用的內(nèi)存空間量）和規(guī)劃準確度來剖析算法的局限性、可行性。同時，對這些算法的改進方案、方式進行歸納總結(jié)，提出算法研究方式、方向以及優(yōu)化算法的可能性。

1 路徑規(guī)劃算法介紹

規(guī)劃是對未來時域、空域中車輛一系列行為方式的計劃。從涉及的時空大小可以分為局部(微觀)路徑規(guī)劃和全局(宏觀)路徑規(guī)劃。局部路徑規(guī)劃也稱動態(tài)路徑規(guī)劃，是根據(jù)環(huán)境感知的信息，實時改變行駛狀態(tài)，包括換道，避障等。全局路徑規(guī)劃也稱靜態(tài)路徑規(guī)劃，根據(jù)當前定位在已知的地圖上規(guī)劃一條到目標位置的路徑。

路徑規(guī)劃一般分為3 步：（1）建立環(huán)境模型：根據(jù)現(xiàn)實的道路情況，建立一個抽象的環(huán)境模型，為算法策略提供地圖模型，便于計算機進行路徑規(guī)劃；（2）搜索路徑：根據(jù)算法策略在抽象的環(huán)境模型上規(guī)劃一條從初始點到目標位置的路徑，盡可能找到一條最優(yōu)路徑；（3）路徑處理：對規(guī)劃出的路徑進行優(yōu)化，刪除冗余節(jié)點，改善平滑度。

1.1 全局路徑規(guī)劃

1.1.1 Dijkstra 算法

Dijkstra 算法[3]是1959 年狄克斯特拉提出的一種遍歷各個節(jié)點的最短路徑算法，它能夠高效解決小范圍內(nèi)最短路徑問題。Dijkstra 算法采用廣度優(yōu)先思想和貪婪搜索策略，通過一個數(shù)組集合存儲從起點到每個節(jié)點的最短距離，并處理最短路徑點進行保存，一邊進行數(shù)據(jù)處理，一邊進行數(shù)據(jù)分類，解決無向圖或加權(quán)有向圖的單源最短路徑問題。算法步驟如下：

第1步：根據(jù)如圖1所示路徑節(jié)點，設(shè)起始點A，把路徑節(jié)點做上標記如表1 所示，每一節(jié)點臨近節(jié)點如表2 所示，設(shè)置兩個數(shù)組集合S 和U。S 代表每個節(jié)點到起點的最短距離數(shù)據(jù)集合，U 表示未經(jīng)過數(shù)據(jù)處理的節(jié)點集合。最短路徑min[i，j]為距離起點最短的點（i 為S 數(shù)組中節(jié)點，j 為U 數(shù)組中節(jié)點）。初始時，S 集合內(nèi)只有節(jié)點A；U 集合中有除S集合外的其他節(jié)點。出發(fā)點設(shè)置為S={A（0）}，U={B（min[A，B]，C（min[A，C]），D（∞），E（∞），F(xiàn)（∞）}。

表1 字母-數(shù)字節(jié)點對應表Tab.1 Correspondence of alpha-numeric nodes

表2 節(jié)點臨近節(jié)點表Tab.2 Adjacent nodes

圖1 路徑節(jié)點Fig.1 Path nodes

第2 步：在U 集合中選取下一個相鄰節(jié)點，計算所選中節(jié)點離起點最短距離min（A，j）。

第3 步：計算起點A 到U 中每個節(jié)點的距離。更新當前U 中節(jié)點的距離，由第2 步中確定了的min（A，j）求出最短路徑的節(jié)點，利用min（A，j）來計算其它節(jié)點的距離；例如，min（A，j）的距離可能大于min（A，i）+min（i，j）的距離。

第4 步：重復第2 步和第3 步，直至終點，算法結(jié)束。S 集合中的結(jié)果就是最短路徑。

Dijkstra 算法優(yōu)缺點：由于Dijkstra 算法遍歷了全部節(jié)點，因此得到的最短路徑準確度好，成功率較高，且魯棒性較好。但在實際應用于復雜的大范圍路徑拓撲網(wǎng)絡(luò)時，時間、空間處理復雜度高，導致效率低下，消耗的時間長，內(nèi)存消耗大；其次，算法忽略了交通網(wǎng)絡(luò)中的沖突、擁擠等因素。

梁彧[4]在傳統(tǒng)算法基礎(chǔ)上引入了對路徑代價進行估計的估價函數(shù)，改善了Dijkstra 算法的空間復雜程度，提高了路徑規(guī)劃的效率。

1.1.2 A*算法

A*算法[5]是一種圖搜索法，它在經(jīng)典Dijkstra算法中加入了啟發(fā)式函數(shù)和估價函數(shù)進行約束，估價函數(shù)作為核心部分之一可以根據(jù)環(huán)境信息進行調(diào)整，在靜態(tài)路網(wǎng)搜索中效果顯著，估價函數(shù)如下：

式中：g（n）——從初始狀態(tài)到當前狀態(tài)n 的實際代價；h（n）——從當前狀態(tài) n 到目標狀態(tài)的估計代價；f（n）——從初始狀態(tài)通過當前狀態(tài)n 到目標狀態(tài)的總代價估計。

若采用曼哈頓距離作為啟發(fā)函數(shù)：

式中：xd——目標位置的橫坐標；yd——目標位置的縱坐標；xn——節(jié)點n 的橫坐標；yn——節(jié)點n的縱坐標。

估價函數(shù)h（n）作為A*算法中的核心，能夠決定A*算法的時間、空間復雜程度。在估價函數(shù)中，若h（n）與節(jié)點n 運動到目標節(jié)點的實際代價相等，算法能夠在最短時間內(nèi)找到一條有效路徑；若h（n）比從節(jié)點n 運動到目標節(jié)點的實際代價小，不僅內(nèi)存消耗增加，路徑中的冗余節(jié)點也會增加；如果 h（n）比節(jié)點n 運動到目標節(jié)點的實際代價大，雖然消耗的時間短，但是對A*準確度影響較大[6-7]。

A*算法具體步驟如下：

第1 步：對初始點和目標點初始化，設(shè)置open 和close 兩個集合。open 集合為待分析節(jié)點數(shù)據(jù)集合，close 為已經(jīng)處理過的數(shù)據(jù)集合。初始后，把初始位置作為父節(jié)點，放入close 集合中，初始位置周圍8 個節(jié)點設(shè)為子節(jié)點，放到open 中；

第2 步：根據(jù)估價函數(shù)式1，處理8 個節(jié)點，可以橫向、縱向、斜向移動，選擇最小節(jié)點放入close 中，把 最小的節(jié)點設(shè)為新的父節(jié)點。若新父節(jié)點相鄰節(jié)點已經(jīng)在open 中，處理生成最佳路徑，更新open 和close 集合；

第3 步：循環(huán)前兩步，直到找到目標點為止。

A*算法存在規(guī)劃路徑轉(zhuǎn)折點多、冗余節(jié)點多、轉(zhuǎn)折角度過大且距離障礙物過近等問題，使算法控制效率降低，時間、空間復雜程度高，在大型地圖環(huán)境效率低。周克帥[8]等對A*算法和人工勢場算法進行改進,并把兩種算法融合，仿真結(jié)果不僅在靜態(tài)環(huán)境下可以快速規(guī)劃出一條最優(yōu)路徑，而且在復雜的動態(tài)環(huán)境下也可以實時躲避障礙物。

1.1.3 蟻群算法

蟻群算法是1991 年Dorigo[9]等根據(jù)模仿螞蟻尋找食物的方式提出的一種仿生算法。螞蟻尋找食物時會釋放信息素在經(jīng)過的道路上，螞蟻之間可以相互感知信息素。信息素濃度隨時間而降低，當螞蟻遇到岔路時,通常會選擇信息素濃度高的路徑通過，與此同時,會釋放自己的信息素標記這條路徑上，使信息素濃度増加,大量螞蟻通過后,在正反饋的作用下,螞蚊會找到條最短路徑。

設(shè)螞蟻總數(shù)量m，節(jié)點數(shù)量n，節(jié)點i、j 之間距離表示為（i，j=1，2，…，n），設(shè)時間為t 時，節(jié)點i、j 之間道路上信息素濃度為τij（t）。初始時，設(shè)整個路網(wǎng)信息素濃度相同，設(shè)τi（jt）=τ0。第k 只螞蟻（k=1，2，…，m）選擇下一個目標節(jié)點時，依據(jù)路徑上的信息濃度來判斷，設(shè)（t）為t 時刻第k 只螞蟻從節(jié)點i 運動到節(jié)點j 的概率，如式（3）：

式中：τij——路網(wǎng)上信息素濃度；α——信息素重要因子；β——啟發(fā)函數(shù)因子；p——信息素揮發(fā)因子；Lk——在當前遍歷中最優(yōu)路徑；Q——總信息素量。

式（3）中，設(shè)τij（t）為啟發(fā)函數(shù)，是螞蟻從節(jié)點i 運動到節(jié)點j 的期望值，有。

當螞蟻完成第1 次遍歷時，更新各個路網(wǎng)上的信息素濃度。

蟻群算法在路徑規(guī)劃中具有正反饋、強魯棒性和較好適應性的特點，但收斂速度慢，時間復雜程度高，易陷入局部收斂，內(nèi)存消耗大，準確率不高[10]。何雅穎[11]等通過局部分塊優(yōu)化策略分別優(yōu)化各子區(qū)域，改良信息素的變化過程，不僅避免陷入局部最優(yōu)，而且使算法收斂速度有所提高。

1.1.4 RRT算法

Rapidly-Exploring Random Tree 是Lavalle[12]在1998 年提出的一種基于采樣的快速擴展隨機樹算法。采樣算法適用于普遍動態(tài)模型，算法的增量特性使其更易于實現(xiàn)動態(tài)實時。RRT 算法是從初始位置出發(fā)后一邊搜索拓展一邊建圖。快速擴展隨機樹在每次迭代更新中隨機選擇新的配置頂點，拓展路徑結(jié)構(gòu)，直到抵達目的地為止。算法步驟如下：

第1 步：初始化后，定義qinit為初始點，定義qrand為采樣點，qrand為狀態(tài)空間中隨機一點；

第2 步：以qinit到qrand方向為樹枝延伸方向，拓展函數(shù)從起點出發(fā)向qrand方向擴展一段距離，設(shè)新到達的節(jié)點為qnew；若qnew1向qrand生長時經(jīng)過障礙物，放棄此次生長，若qnew1向qrand生長時未經(jīng)過障礙物，此路徑有效；

第3 步：重復第2 步，直到qnew和目標點距離小于既定值，路徑規(guī)劃成功。為了使算法盡可能滿足需要，可設(shè)定生長次數(shù)上限。如果在生長次數(shù)限制內(nèi)無法規(guī)劃出一條完整的路徑,則此次規(guī)劃失敗。

RRT 算法在運行過程中不需要根據(jù)周圍環(huán)境建立模型，算法簡明，結(jié)構(gòu)簡單，對高維度空間的問題和一些復雜環(huán)境有很好的適用性，對未知空間的快速探索強，但是算法搜索具有盲目性，對附近障礙物距離強烈依賴，空間復雜程度高。戶望力[13]通過對擴展節(jié)點拓展方式進行改良，使得車輛進行路徑規(guī)劃時更加具有方向性，能夠?qū)崿F(xiàn)在復雜環(huán)境下也具有較好的適用性，算法的精確度有了較大改善，減少了時間復雜程度。

1.2 局部路徑規(guī)劃

1.2.1 模擬退火算法

模擬退火算法是1953 年Metropolis 等根據(jù)固體退火的過程提出的一種近似求解算法[14]，在大范圍的復雜環(huán)境優(yōu)化問題中有很強的適用性。在一般組合的優(yōu)化問題加入概率突跳特性。概率突跳特性可以使得局部最優(yōu)解能概率性地跳出，導致得到的解概率性是全局最優(yōu)。模擬退火的過程如下：

在一定初始溫度和初始解的情況下，隨著溫度的降低，每個溫度狀態(tài)下的轉(zhuǎn)變會產(chǎn)生一個新的解。如果當前解小于上一次迭代生成的解，則接受當前解決方案，否則概率性接受新方案。經(jīng)過多次迭代可以得到最優(yōu)解。當固體從狀態(tài)i 經(jīng)過降溫變化到狀態(tài) j，它所具有的能量從E（i）變化到E（j）。顯然，如果E（j）＜E（i），接受新的狀態(tài) j，否則依據(jù)概率性P，決定能否接受新解。

式中：T——固體的溫度；K——玻爾茲曼常數(shù)。

x 表示溫度T 下的一個解，通過退火，可以生成一個新解x'，那么接受x'的概率為

模擬退火算法可以有效地求解多項式復雜程度的非確定性問題，但是也存在如下問題：

（1）模擬退火算法性能在全局搜索中會被溫度T 的初始值設(shè)置影響；

（2）同一溫度下會產(chǎn)生大量的搜索，使得時間、空間復雜程度增加。循環(huán)次數(shù)使得內(nèi)存消耗大；

（3）溫度管理問題也會影響算法的性能[15]。袁佳泉[16]融合模擬退火與蟻群2 種算法，利用2種算法設(shè)置雙層啟發(fā)式函數(shù)求最優(yōu)解，退火機制的概率突跳特性有效避免蟻群算法陷入局部最優(yōu)，既提高了全局搜索性能，也降低了時間復雜程度。

1.2.2 動態(tài)窗口算法

動態(tài)窗口算法是由Dieter Fox[17]等于1997 年基于曲率速度思想提出的一種局部路徑規(guī)劃方法，考慮到運動速度、載體的運動方向和到最近障礙物的距離3 個方面，通過離散搜索空間進行優(yōu)化。

DWA 算法要將位置控制轉(zhuǎn)化為速度控制，因此需要對車輛的運動模型進行分析。設(shè)vx為車輛在x 軸上的速度，vy為車輛在y 軸上的速度，θt為車輛當前朝向角，θt+Δt為Δt 時間后車輛朝向角，ω為車輛當前角度。在采樣周期t 內(nèi)，（xt，yt）為車輛當前位置坐標，Δ t 為時間間隔；（xt+Δt，yt+Δt）為Δt 時間后車輛位置坐標。其運動模型如下：

車輛受自身結(jié)構(gòu)限制，速度有最小值和最大值的約束。設(shè)Vm為小車最低速度和最高速度之間的速度集合，小車速度為（v，w），則Vm的取值為

因為電機力矩受到最大加速度和最大減速度的限制，所以在車輛軌跡前向模擬的周期內(nèi)，存在一個動態(tài)窗口，該窗口的速度集合是小車在下一個時間間隔內(nèi)實際能夠達到的速度。設(shè)t 為加速度v'和角加速度ω'作用的時間，Vc為車輛當前速度，ωc為車輛當前角速度，（vc，ωc）則為小車的實際速度，則動態(tài)窗口Vd定義為

為了防止小車與障礙物發(fā)生碰撞，需要在最大減速度條件下對小車的速度進行約束。設(shè)小車速度為（v，ω），則允許速度集合Va定義為

式中：dist（v，m）——速度對應的軌跡，也就是一段圓弧上，離障礙物的最短距離；vb'和ωb'為制動加速度。對速度的采樣搜索空間施加約束后，可以得到在動態(tài)窗口內(nèi)區(qū)域Vr，因此Vr可以定義為這些限制條件的交集，表示為

動態(tài)窗口法評價函數(shù)：DWA 算法可以根據(jù)速度采樣后的結(jié)果推演出不同的軌跡,再運用評價函數(shù)對軌跡和速度進行評價,通過評價函數(shù)選出的避障速度能夠保證車輛避開靜態(tài)障礙物。設(shè)評價函數(shù)G（v，w），可以通過式（14）計算：

式中：方位角heading（v，ω）——小車方向與目標方向的精確程度的指標；dis（tv，ω）——小車沿著當前路徑運動時到最近障礙物的距離；vel（v，ω）——評估小車在當前位置的速度及其進展過程。

DWA 算法有2 點不足：

（1）只有一個目標點，缺少中間指引在大范圍的復雜環(huán)境中概率性得到最優(yōu)路徑；

（2）未將已知障礙物和未知障礙物區(qū)分，遇到動態(tài)障礙物難以避障。

劉建娟[18]等融合動態(tài)窗口法與A*算法，量化環(huán)境中的障礙物信息，根據(jù)障礙物信息調(diào)節(jié)A*算法啟發(fā)函數(shù)，根據(jù)弗洛伊德算法思想改良路網(wǎng)節(jié)點算法，最終使得融合算法更加平滑，減少了轉(zhuǎn)折節(jié)點，刪除冗余節(jié)點，在復雜的環(huán)境中也保證了計算出最優(yōu)路徑的情況下，可以做到實時避障。

1.2.3 人工勢場法

人工勢場法是1985 年Khatib 博士提出一種抽象的虛擬力場法[19]，該算法是把汽車在現(xiàn)實環(huán)境中的運動抽象成為一種在力場中的運動。目標位置對于車輛相當于引力勢場，離目標點距離越遠引力勢能越強。障礙物對于車輛相當于斥力勢場，車輛離障礙物越遠斥力越小，通過引力場與斥力場的合力可實現(xiàn)同步規(guī)劃路徑和軌跡平滑度的處理，根據(jù)搜索勢函數(shù)的下降方向可以規(guī)劃出一條最佳路徑。

設(shè)小車在空間位置為X=[x y]T，目標位置為Xg=[xgyg]T，引力場增益為k，則小車在X 點受引力場函數(shù)Uatt（X）為；

相應的引力場負梯度Fat（tX）為；

設(shè)η為斥力場增益常量，λ為小車到最近障礙物的距離，λ0為障礙物影響的最短距離，斥力勢場函數(shù)如式（17），斥力勢場函數(shù)的負梯度為式（18），汽車所受合力F（x）為

人工勢場法算法簡明，時間復雜程度低，準確度高，能夠?qū)崟r控制車輛行駛狀態(tài)，且所規(guī)劃路線較為平滑，受到大量的使用與改進。但是當處于多障礙物的復雜環(huán)境時，車輛受到的引力與斥力的合力可能為0，導致車輛停止前進，陷入局部最小值。有時候會規(guī)劃道路失敗。張珂[20]等基于傳統(tǒng)人工勢場算法改善了障礙物勢場力設(shè)置中的不合理以及道路邊界定義模糊的問題，仿真結(jié)果在綜合性能上比傳統(tǒng)算法平順。

2 展望

經(jīng)過多年的實踐和應用，路徑規(guī)劃算法在不同學科、不同領(lǐng)域的相互融合中逐漸成熟，以下是對算法優(yōu)化的可行性方案總結(jié)和展望。

（1）對于經(jīng)典路徑規(guī)劃本身的優(yōu)化，每一種算法在實踐過程中都會表現(xiàn)出自身的局限性，可以針對對應的局限性加約束條件優(yōu)化結(jié)果，也可以加入強化學習方法，從基于值和基于策略兩類改進常規(guī)路徑規(guī)劃，增強環(huán)境適應性；

（2）對于路徑規(guī)劃融合算法，面對復雜的交通環(huán)境時，任何一種路徑規(guī)劃算法都不可能處理一切路徑規(guī)劃問題。加上開發(fā)新算法的難度太大，且不一定能夠比傳統(tǒng)算法表現(xiàn)更優(yōu)秀。路徑規(guī)劃算法之間的性能互補為解決此問題提供了可能。如A*與DWA 算法融合[21]。

（3）路徑規(guī)劃算法和環(huán)境建模的融合是跨學科處理復雜問題的方法。在動態(tài)環(huán)境中，三維連續(xù)動態(tài)環(huán)境信息的多變性，使得算法本身難以做到有效的應對，適合的建模技術(shù)和與之適應的路徑規(guī)劃算法相結(jié)合可以使結(jié)果更加平滑、高效，增加了求解的準確度。如蟻群算法和柵格法的聯(lián)合[22]。

（4）對于基于Vehicle to X 的路徑規(guī)劃算法設(shè)計，隨著科學技術(shù)應用的拓展，多種交通設(shè)備相互協(xié)作變得越加默契，在應用中得到很好的實踐。這使得路徑規(guī)劃算法設(shè)計能有新的拓展方向。

3 結(jié)語

路徑規(guī)劃算法作為無人駕駛?cè)蝿諞Q策的核心，吸引著各研究人員進行探索，逐漸形成百家爭鳴的景象。本文通過分析經(jīng)典路徑規(guī)劃算法的方法和性能，從時間復雜程度、空間復雜程度以及準確度方面進行討論；其次，分析各個算法優(yōu)化的方式，優(yōu)化后算法的可行性、拓展性；最后，依據(jù)改良方式以及學科領(lǐng)域的拓展，對無人駕駛路徑規(guī)劃的發(fā)展前景做出了一個符合實際的展望。