自動駕駛汽車路徑規(guī)劃算法研究*

李梓欣 李軍

（重慶交通大學機電與車輛工程學院）

自動駕駛汽車使用傳感器感知環(huán)境，并依照合理的算法在復雜環(huán)境中實現(xiàn)自主運動，使其能在道路上安全、高效地行駛。作為自動駕駛汽車研究的一個重要環(huán)節(jié)，路徑規(guī)劃就是根據(jù)給定的環(huán)境模型，在一定的約束條件下，利用路徑規(guī)劃算法規(guī)劃出一條連接車輛當前位置和目標位置的無碰撞路徑。在此基礎(chǔ)上可按照不同需求選擇一條最優(yōu)路徑。隨著各個領(lǐng)域的不斷發(fā)展，傳統(tǒng)路徑規(guī)劃算法在復雜環(huán)境模型下存在規(guī)劃速度慢、數(shù)據(jù)計算量大等問題，難以滿足路徑規(guī)劃實時性、準確性的要求?，F(xiàn)結(jié)合相關(guān)文獻，從A*算法、蟻群算法等目前廣泛使用的算法入手，介紹其基本原理與研究進展，并對其未來發(fā)展方向做出展望。

1 路徑規(guī)劃算法分類

依照路徑規(guī)劃算法在原理上的不同，可將算法大致分為基于搜索的路徑規(guī)劃算法、基于采樣的路徑規(guī)劃算法以及基于智能算法的路徑規(guī)劃算法[1]。其中，基于搜索的路徑規(guī)劃算法主要包括Dijkstra算法、A*算法等；基于采樣的路徑規(guī)劃算法主要包括RRT算法[2]、PRM算法[3]等；基于智能算法的路徑規(guī)劃算法主要包括遺傳算法[4]、蟻群算法等。這些算法由于其不同的計算原理，其完備性與最優(yōu)性也不盡相同，按照不同路段與工況需求選擇不同算法以獲得較好完備性與最優(yōu)性是學者共同追求的目標。

依照算法在進行路徑搜索時是否有啟發(fā)信息介入，可將路徑規(guī)劃算法分為傳統(tǒng)路徑規(guī)劃方法與啟發(fā)式搜索算法。傳統(tǒng)路徑規(guī)劃方法，諸如Dijkstra算法、Floyd算法等，盲目搜索整個環(huán)境空間，雖然算法較為簡單，但其計算量極大，在復雜環(huán)境下難以高效完成路徑規(guī)劃。啟發(fā)式算法在路徑規(guī)劃時引入啟發(fā)因子或啟發(fā)函數(shù)，按照一定的評價指標估計節(jié)點代價值，從而引導路徑朝著預期方向進行規(guī)劃，以提升效率。故對于啟發(fā)式算法，評價指標的選取極為重要，將大大影響路徑規(guī)劃的質(zhì)量。

2 典型的路徑規(guī)劃算法

2.1 A*算法

傳統(tǒng)Dijkstra算法從起點開始逐步擴展，每一步為一節(jié)點找到最優(yōu)路徑。在尋優(yōu)過程中利用貪心思想，遍歷鄰接節(jié)點更新距離。由于其未使用啟發(fā)函數(shù)，算法計算量與空間占用都較大。

A*算法是建立在Dijkstra算法上的智能搜索算法，它通過引入與目標點有關(guān)的啟發(fā)信息，引導算法沿著最優(yōu)方向進行搜索。可通過不同的啟發(fā)信息估計當前點與目標點間的距離，在設(shè)計代價函數(shù)時結(jié)合所需的啟發(fā)信息，并優(yōu)先選擇代價值最小的節(jié)點。圖1示出Dijkstra算法規(guī)劃出的路徑，其中紅色路線為已規(guī)劃的路徑，藍色“x”點為已搜索的路徑節(jié)點；圖2示出A*算法規(guī)劃出的路徑，由圖可知A*算法在啟發(fā)函數(shù)的影響下所需搜索的路徑節(jié)點更少，算法效率更高。

圖1 Dijkstra算法

圖2 A*算法

A*算法利用啟發(fā)函數(shù)，使得搜索方向更趨于目標點，但A*算法所規(guī)劃的路徑轉(zhuǎn)折點較多，無法保證路徑的平滑度；且存在多個最小值時無法保證路徑最優(yōu)。故國內(nèi)外很多學者提出不同方法優(yōu)化A*算法，試圖改進其不足。

針對傳統(tǒng)A*算法未考慮運動學性質(zhì)且無法進行多步長路徑規(guī)劃，文獻[5]利用高斯分布動態(tài)改變A*算法代價函數(shù)的權(quán)重系數(shù)，減小算法運算時間，防止其陷入局部最優(yōu)。

傳統(tǒng)A*算法遍歷節(jié)點數(shù)目多，搜索效率低，文獻[6]引入雙向搜索機制，通過2個方向交替進行路徑搜索，加快搜索速度；設(shè)置合理的啟發(fā)函數(shù)和代價函數(shù)減少對無關(guān)節(jié)點的搜索，提高算法效率。文獻[7]設(shè)置關(guān)鍵點代替OPEN表中的節(jié)點減小計算量，提高算法效率；利用反向搜索策略判斷障礙物位置，降低路徑長度。

針對傳統(tǒng)A*算法搜索速度慢、路徑轉(zhuǎn)折點多且不適用于動態(tài)環(huán)境，文獻[8]依據(jù)不同節(jié)點構(gòu)成的軌跡間的夾角刪除和新增節(jié)點；當環(huán)境模型較大時，算法設(shè)置局部子區(qū)域，進行多重A*算法搜索目標點。

文獻[9]在A*算法估價函數(shù)中引入懲罰機制，通過不同曲率計算懲罰代價值，減少AGV轉(zhuǎn)向次數(shù)；以小車電量、行駛距離、沖突節(jié)點作為優(yōu)先級的判定依據(jù)，以路徑長度與小車利用率作為多AGV協(xié)同規(guī)劃的復合指標，進行多AGV無碰撞動態(tài)路徑規(guī)劃。

針對傳統(tǒng)A*算法路徑不平滑、可行路徑不是最短、動態(tài)環(huán)境下效率較低等問題，文獻[10]提出一種融合動態(tài)窗口算法的改進7×7的A*算法。將傳統(tǒng)A*算法的搜索鄰域由3×3擴大為7×7，同時去除同向多余子節(jié)點，優(yōu)化算法角度，提高算法效率。

2.2 快速擴展隨機樹（RRT）算法

RRT算法在環(huán)境模型中隨機生成節(jié)點，以最近節(jié)點作為基準點，按照一定步長向遠離障礙物或邊界的節(jié)點進行生長，直到到達目標點或與目標點在一定距離內(nèi)時停止生長，隨后算法連接起點與終點的最短路徑，此為所需的規(guī)劃路徑。

RRT算法搜索能力較強且易于解決非完整約束尋優(yōu)問題，在高維環(huán)境與復雜環(huán)境中應(yīng)用較廣。但RRT算法計算量較大，且生成路徑穩(wěn)定性較差。文獻[11]提出改進RRT算法，新生成節(jié)點與目標點距離作為評價指標，引入權(quán)重系數(shù)，設(shè)置動態(tài)距離增量，防止算法陷入局部最優(yōu)。

文獻[12]加入目標點信息，生長距離由隨機采樣和勢場共同引導，斥力場保證算法避開障礙，路徑可直接與目標點相連。

RRT算法雖然具有概率完備性，但其無法保證路徑最優(yōu)。故在此基礎(chǔ)上提出RRT*算法以規(guī)劃出一條漸進最優(yōu)路徑[13]。RRT*算法通過重新選擇父節(jié)點，并且在一定范圍內(nèi)重新連接節(jié)點，通過多次迭代實現(xiàn)漸進最優(yōu)。如圖3所示，RRT*算法規(guī)劃得到的路徑（圖中紅色路線所示）相比RRT算法所得路徑（如圖4所示）更短。

圖3 RRT*算法

圖4 RRT算法

文獻[14]在RRT*算法基礎(chǔ)上對采樣點方向進行約束，同時對目標點方向更大權(quán)重，增強算法效率。文獻[15]刪除碰撞檢測計算，設(shè)置碰撞風險評估函數(shù)實現(xiàn)避障。

為提升找到漸進最優(yōu)路徑的速度，文獻[16]提出Informed RRT*算法，該算法將可能的最優(yōu)解集中在初始解構(gòu)成的橢球中，以提高算法效率。如圖5所示，Informed RRT*算法采樣得到的路徑（圖中綠色線路）相比圖4中均集中在藍色虛線構(gòu)成的橢圓內(nèi)，由此可提高算法效率。

圖5 Informed RRT*算法

2.3 蟻群算法

蟻群算法（Ant Colony Optimization）由意大利學者Dorigo最早提出，是一種源于大自然的仿生類算法。蟻群算法的基本原理為，螞蟻在走過的路徑釋放信息素，該信息素與路徑長度有關(guān)。螞蟻優(yōu)先選擇信息素濃度較高的路徑，遇到未走過的交叉位置便隨機選擇路徑，最優(yōu)路徑信息素濃度增大，直至螞蟻找到最優(yōu)路徑。

蟻群算法搜索路徑能力較為穩(wěn)定，易與其他算法進行融合，但它也存在收斂慢、易早熟等問題。現(xiàn)階段國內(nèi)外提出多種優(yōu)化算法。

針對蟻群算法存在的尋路時間長、收斂速度慢等問題，文獻[17]將起始點與目標點連接，精英螞蟻沿此連線搜索路徑，利用該路徑平滑處理后構(gòu)建封閉區(qū)域，再計算轉(zhuǎn)彎代價與設(shè)置的門限值進行比較，從而確定最終路徑。

針對傳統(tǒng)蟻群算法參數(shù)影響過大的問題，文獻[18]提出一種結(jié)合粒子群算法的改進蟻群算法。改進算法通過動態(tài)慣性權(quán)重調(diào)整粒子群算法參數(shù)，并對蟻群信息素更新策略進行調(diào)整。文獻[19]在邊緣柵格與中心區(qū)域設(shè)定不同濃度信息素，對部分路徑信息素重新賦值，保證算法隨機性，防止其陷入局部最優(yōu)。

針對蟻群算法在復雜環(huán)境下收斂慢、易出現(xiàn)“早熟”現(xiàn)象等問題，文獻[20]將蟻群算法與A*算法結(jié)合，并設(shè)置信息素矩陣以保證算法向目標點收斂且效率更高。文獻[21]引入通過極值限定策略、精英策略和信息素擴散策略，動態(tài)調(diào)整信息素濃度，以兼顧算法效率和質(zhì)量。

2.4 算法比較

通常使用以下4個指標評價路徑規(guī)劃算法：

1）完備性。若在起始點與目標點之間有路徑存在，則必能得到路徑，否則無路徑存在。

2）概率完備性。若在起始點與目標點之間有路徑存在，只要經(jīng)過足夠時間的規(guī)劃，必能得到路徑。

3）最優(yōu)性。規(guī)劃得到的路徑在某個評價指標上是最優(yōu)的。

4）漸進最優(yōu)性。經(jīng)有限次規(guī)劃迭代后得到的路徑是接近最優(yōu)的路徑，且每次迭代后都更加接近最優(yōu)。

各算法比較如表1所示：

表1 路徑規(guī)劃算法比較

對于上述各算法規(guī)劃速度，基于采樣的路徑規(guī)劃算法由于其隨機采樣的特性，使得規(guī)劃速度更快，其中RRT*算法相對RRT算法具有漸進最優(yōu)性，從而其速度更快?；谒阉鞯穆窂揭?guī)劃算法需要廣泛搜索狀態(tài)空間，因此算法速度相比RRT算法較慢，其中A*算法由于引入了啟發(fā)函數(shù)使得其無需遍歷搜索，比Dijkstra算法效率更高，速度更快?；谥悄芩惴ǖ穆窂揭?guī)劃算法需要大量迭代，需要更久的時間才能得到所需路徑，因此相較其他2種路徑規(guī)劃算法速度更慢。

3 結(jié)論

文章將路徑規(guī)劃算法依照其算法原理與啟發(fā)信息大致分類，并對部分規(guī)劃算法的原理、特點及國內(nèi)外改進方法進行簡要闡述。對于不同路徑規(guī)劃算法，其原理與特點各不相同，從而使得不同算法在不同環(huán)境條件下有多種使用方式。如在復雜立體交通中利用基于采樣的路徑規(guī)劃算法，發(fā)揮其在高維環(huán)境中規(guī)劃的優(yōu)勢；也可通過優(yōu)化啟發(fā)因子，使得A*算法等啟發(fā)式路徑規(guī)劃算法在復雜道路環(huán)境中可獲得更加安全、節(jié)能的路線。

現(xiàn)有的路徑規(guī)劃算法在靜態(tài)全局路徑規(guī)劃及已知環(huán)境信息的動態(tài)路徑規(guī)劃問題中進行了大量的研究，不同的算法在其進行路徑規(guī)劃時都具有一定優(yōu)勢。然而，對于未知環(huán)境下的路徑規(guī)劃，關(guān)于不確定環(huán)境因素與多車協(xié)同的路徑規(guī)劃研究較少。因此，通過多種算法融合，在高維、動態(tài)、復雜以及多變的環(huán)境中進行路徑規(guī)劃的研究，將成為自動駕駛汽車算法研究的主流方向。