基于自適應A*算法的自動駕駛車輛路徑規劃方法研究

張 涌,成海飛,趙奉奎

(南京林業大學 汽車與交通工程學院,江蘇 南京 210037)

0 引 言

路徑規劃是自動駕駛車輛自主完成導航任務的重要環節和關鍵技術之一,也是實現完全自動駕駛的核心難題[1]。它的目的主要是根據已知或者未知障礙物的地圖環境與所需的約束條件,尋找到一條從起點到終點的最優或次優的安全路徑。在自動駕駛車輛路徑規劃領域,眾多學者對算法的開發與探索展開了大量研究,主要包括基于采樣的快速搜索隨機樹方法[2]、Voronoi圖方法等,基于節點的Dijkstra算法、A*算法,基于模型的人工勢場法[3]、動態窗口法等,以及基于生物啟發式的神經網絡算法、遺傳算法[4]等。

A*算法作為一種啟發式搜索算法,節點的拓展具備一定的方向性,且計算量小,被廣泛應用于全局路徑規劃算法,但傳統的A*算法存在搜索效率低、拐點數量較多和轉折角度較大等問題[5-7]。針對傳統A*算法的這些問題,許多學者針對性地進行了相關研究。姜媛媛等[6]利用垂距限值法快速刪除搜索路徑中存在的冗余節點,并通過B樣條曲線平滑擬合出一條光滑的曲線,仿真實驗表明該改進算法縮短了運算時間和減少了路徑長度;吳鵬等[7]采用目標正反向搜索交替機制使兩路徑的最終節點在起點和終點連線的中點鄰域相遇,提高了搜索效率;王維等[8]通過考慮當前節點與其父節點的估計代價并采用指數衰減的方式對估價函數進行加權、五次多項式平滑處理的方法使規劃路徑可以在復雜環境下可以可靠地到達目標;孔慧芳等[9]采用加權曼哈頓距離與轉彎修正代價作為新的啟發函數,改進的算法可以有效減少AGV路徑規劃時的節點遍歷數與轉彎次數;張建光等[10]在四節點擴展方式的基礎優先考慮目標點方位的相鄰節點,并在估價函數中添加略大于1的權重因子與拐點處轉彎代價,使生成的路徑能有效的減少搜索節點數目以及搜尋路徑的轉彎次數;孟珠李等[11]將傳統A*算法得到的路徑作為控制點,并利用3次B樣條插值使路徑尖峰處得到優化,并生成一條平滑的曲線,使得農用機器人運行更加順暢;田海波等[12]通過在估價函數中考慮轉彎成本、預判斷規劃策略、冗余拐點剔除策略3個方面對傳統A*算法進行優化處理,使生成的路徑長度縮短,拐點數量減少,轉折角變小。上述研究通過節點擴展、平滑處理等方法優化了路徑,但是并沒有綜合考慮得到的路徑是否符合自動駕駛車輛的實際運動約束。

為了解決上述問題,針對A*算法計算時間較長的問題,筆者采用指數增長法對啟發式函數進行加權,從而提高搜索效率。考慮自動駕駛車輛在實際運行過程中自身轉向特性的限制,得到長度更短、轉折角度更小的路徑。此外,采用三次B樣條曲線對生成路徑的拐點處進行平滑處理,使路徑具有連續的曲率,有利于自動駕駛車輛的控制。仿真結果表明：與傳統A*算法相比,改進的A*算法生成的路徑長度明顯縮短,總航向角更小,路徑更加平滑。

1 算法描述

1.1 環境建模描述

自動駕駛車輛進行路徑規劃前,須根據傳感器感知的環境信息進行地圖模型搭建。目前,使用最多的地圖種類有3種：可視地圖、柵格地圖、拓撲地圖。其中,柵格地圖由于自身模型易構建以及位置表示的唯一性等優點,在路徑規劃時被廣泛采用,因此筆者將采用柵格法來構建地圖。

在柵格地圖上構建障礙物時,為保證環境信息的真實性,通常將障礙物的原始形狀作為首要選擇,此方法會導致障礙物的邊緣失真,從而影響路線的可追溯性和安全性。現有的研究大多采用矩形封閉障礙物的原始形狀的方法來避免邊緣局部失真[14]。同樣應用文獻[13]所提及的障礙物處理辦法,地圖環境中的障礙物形狀統一用矩形表征,圖1為利用矩形代替圓形和不規則五邊形的實際外形。由圖1可以看出,此時原障礙物的安全性問題轉化為矩形障礙物的安全性問題。

圖1 障礙物代替示意Fig.1 Schematic diagram of obstacle replacement

靜態全局環境下,柵格地圖實例如圖2,左下角柵格為起點,右上角柵格為終點,空白柵格表示自動駕駛車輛可以通過(可通行區域);黑色柵格表示不可通行的障礙物(不可通行區域)。

圖2 柵格地圖實例Fig.2 Grid map example

1.2 傳統A*算法

A*算法是一種經典啟發式搜索算法,在Dijkstra算法和BFS算法基礎上,結合二者的優點,通過啟發式搜索,利用代價函數作為尋路依據,避免了搜索過程中方向的盲目性,從而獲取最優路徑。A*算法的基本思想是將起始點S作為第1個父節點添加至Open list列表中,利用代價函數估值從父節點到周圍節點n的價值,選擇最小的節點作為下一個父節點,直至搜索到目標節點G并且將目標節點G添加至Close list列表為止。啟發函數由兩部分組成,如式(1)：

f(n)=g(n)+h(n)

(1)

式中：f(n)為起始節點S到目標節點G的路徑總代價;g(n)為從起始點S到當前點n的實際代價;h(n)為當前節點n到目標節點G的預估距高。

一般地,計算h(n)的典型距離計算公式有曼哈頓距離、歐幾里得距離、切比雪夫距離,對應表達式分別如式(2)～式(4)：

h(n)=|xn-xm|+|yn-ym|

(2)

(3)

h(n)=max(|xn-xm|,|yn-ym|)

(4)

對比3種距離運算公式且基于行走規則,式(2)的曼哈頓距離運算簡單,搜索效率高,但其運動方向只允許朝上下左右4個方向移動導致生成的路徑質量偏低;式(3)的歐幾里得距離易求得最短路徑,且可使自動駕駛車輛朝任何方向移動,但計算時必須先平方和后開根,造成搜索效率低;式(4)的切比雪夫距離在進行估值運算時與最優路徑的估值誤差不大,但在斜向搜索時會低估距離(沒有考慮斜向路徑的實際長度且路徑規劃過程中允許朝8個方向移動,以車輛所處位置為中心單元,在3×3鄰域里,八方向指的是前、后、左、右、左前、左后、右前、右后)。自動駕駛車輛完成駕駛任務過程時,運動方向是任意的且使用歐幾里得距離更逼近實際駕駛過程,所以筆者使用歐幾里得距離公式計算h(n)的值。

2 A*算法改進

傳統A*算法及文獻[11]的改進A*算法在任意地圖中自適應性較差,在路徑規劃時存在遍歷節點數多、總轉折角度大、規劃路徑不夠平滑、尋路時間較長等問題。綜合文獻[11]所提改進思路,在設計思路時需要避免出現因提高某一方面性能指標導致其他性能降低的情況。通過設計自適應調整權重因子、轉彎成本函數(航向角影響因子)、路徑平滑處理方式來改進A*算法,保證最終規劃出的路徑相對最優,提高算法的魯棒性和搜索效率,減少拐點數和總航向角。

2.1 啟發函數自適應權重因子

A*算法的預估代價函數是影響算法效率的重要因素,h(n)的值越接近實際值,搜索的效率就越高。若h(n)=0時,A*算法退化為Dijkstra算法,此時算法可以在全局范圍內搜索到一條最短的路徑,但是需要遍歷所有節點,搜索效率以及時間成本會大大增加。若h(n)>>g(n)時,啟發函數中g(n)的作用可忽略不計,此刻A*算法會演變成為 BFS算法,該算法嚴格遵守最短距離擴展節點,但這僅僅保證了相對最優的路徑,而不是最優選擇。綜上,筆者在啟發函數的預估代價部分引入自適應權重因子w,則原算法公式中的h(n)就變成了w×h(n),由此式(1)可表示為：

f(n)=g(n)+w×h(n)

(5)

(6)

w的引入可以為h(n)帶來更大的權重。自適應權重因子w的值取決于從起始節點S到當前節點n的實際代價與從當前節點n到目標節點G的預估代價的比值(呈正比例關系)。在改進的A*算法的執行過程中,需要收集實時的數據,啟發函數中的動態調整自適應權重因子w的g(n)/h(n)部分會隨著路徑搜索進程的變化而不斷更新取值,使得算法更加注重平衡搜索效率和路徑質量。具體來說,如果當前搜索節點與起點距離較近,那么改進的A*算法會更加注重搜索效率,從而在搜索過程中更加注重速度;如果當前搜索節點與起點距離較遠,那么改進的A*算法會更加重視啟發式函數,從而在搜索過程中更加注重路徑質量。

同時,為保證路徑搜索時間w≥1,筆者在g(n)/h(n)的基礎上引入以e為底的指數函數。因此在路徑搜索初期,w的值較小,可以通過增強算法的發散性達到擴大路徑搜索范圍的目的;路徑搜索后期,w的值較大,可以通過增強算法快速收斂的性能以提高路徑搜索的效率。

2.2 考慮航向角影響因子的啟發函數

路徑規劃時,由于障礙物的分布與最短路徑效應,會使規劃生成的路徑有較多轉折點數量且出現較大轉折角的情況,對應地,自動駕駛車輛在轉彎過程中會出現先減速后加速的情況,通過轉彎所花費的時間代價相對來說比直線行駛高,從而使得整體的效率下降,而且生成的路徑質量無法得到保證。因此,將航向角成本也考慮進A*算法的啟發函數信息中,使自動駕駛車輛盡可能選擇滿足其自身轉彎特性的路徑。要控制自動駕駛車輛的運動,就必須建立準確的車輛運動數學模型,此模型不僅需要真實反映車輛的某些特性,還應該盡可能地簡單易用。所以,對自動駕駛車輛模型簡化,將自動駕駛車輛作為一個整體,只考慮航向角對全局路徑規劃的影響,因此,得到其簡化模型如圖3。

圖3 某一時刻自動駕駛車輛航跡Fig.3 The track of the autonomous vehicle at a certain moment

自動駕駛車輛運動時,位置由節點A→B→C的變化過程中,為了方便計算與更好地描述,在慣性坐標系XOY下,對以下坐標點假設：父節點前一節點A(n0,m0),子節點C(nn,mn),子節點C的父節點B(nn-1,mn-1),ΔL為節點A到節點C的直線距離。故自動駕駛車輛的航向角的表達式可以寫成如下：

(7)

α=arccosb

(8)

(9)

式中：α為自動駕駛車輛某時刻運動狀態的航向角;d為α轉換為弧度制下的值。考慮到車輛本身的最大轉向角度限制。在自適應權重因子的A*算法啟發函數的基礎上添加航向角影響因子q,其中,q為自動駕駛車輛在進行轉向時因航向角變化而引起的代價。其運算公式如式(10)：

q=d

(10)

綜上所述,改進后的A*算法的啟發函數如式(11)：

f(n)=g(n)+w×h(n)+q

(11)

2.3 B樣條算法光滑處理

A*算法規劃出來的路徑是由一些有向線段組成的,雖然筆者提出的改進方法在一定程度上優化了轉折角過大的問題,但是在某兩線段的轉折點,如圖4的(9.5,15.5)處,路徑比較尖銳,此時會因為運動方向突變使自動駕駛車輛的安全性得不到保證。因此筆者將采用B樣條算法,以產生出連續、平滑的路徑曲線。

圖4 考慮自適應調整權重因子與航向角影響因子時某段生成的路徑Fig.4 The path generated by a certain segment considering the adaptive adjustment of the weight factor and the heading angle influence factor

B樣條算法的優點為曲線會落在曲線階數的控制點所形成的凸多邊形內。3次B樣條算法在節點連接處二階連續,應用于路徑規劃時,可以滿足自動駕駛車輛速度和加速度連續性的要求,因此筆者采用3次B樣條算法來平滑A*算法已規劃出路徑[14]。

k次B樣條算法的數學表達式為：

(12)

式中：Pi為控制轉折點i的坐標;Fi,k(t)為k次B樣條的基函數,其表達式為：

(13)

(14)

由式(12)、式(13)可得3次B樣條曲線的基函數數學表達式

(15)

將式(15)代入式(12)可得：

P(t)=[(-C0+3C1-3C2+C3)t3+(3C0-6C1+3C2)t2+(-3C0+3C2)t+(C0+4C1+C2)]/6

(16)

將式(16)用矩陣形式表達為：

(17)

式中：t∈[0,1]。

3 仿真結果和分析

為驗證改進A*算法的有效性和靈活性,筆者在2組不同的環境下進行了仿真試驗,仿真軟件平臺為MATLAB R2019a,硬件平臺為Intel Core i5-10200H 2.40GHz CPU,RAM 16 GB。

為保證筆者所提出算法的可靠性與優越性,設置了20×20、30×30和50×50的3組柵格地圖進行仿真驗證,最小柵格大小為1 m×1 m。每組地圖分別應用傳統A*算法、文獻[11]的改進A*算法(BA*算法)、自適應權重因子A*算法(WBA*算法)與最終改進A*算法(AWBA*算法)。其中,BA*算法是一種基于A*算法的路徑規劃算法,通過引入B樣條平滑技術來提高路徑的平滑度;WBA*算法是在傳統A*算法的基礎上進行改進的搜索算法,它考慮了自適應權重因子和3次B樣條平滑處理方法;AWBA*算法是在WBA*算法的基礎上進一步改進的搜索算法,它引入了航向角影響因子來更好地適應具有方向性的問題。構建的柵格地圖和傳統 A*算法路徑規劃仿真結果如圖5(a)、圖6(a)與圖7(a)。

圖5 20×20柵格地圖路徑規劃Fig.5 20×20 raster map path planning

圖6 30×30柵格地圖路徑規劃Fig.6 30×30 raster map path planning

在文獻[11]中,利用3次B樣條算法對傳統A*算法規劃出的路徑進行了平滑處理,并設計了相關系統驗證了此算法的可行性,即優化后的光滑路徑可以使農用機器人順暢運行。相比于BA*算法,AWBA*算法雖然在搜索時間方面沒有較大提升,但是在路徑長度與總航向角等方面的提升較大。

表1比較了3種不同地圖環境中4種算法的性能指標。結合表1、圖5(c)圖6(c)與圖7(c)可以看出,20×20、30×30與50×50的3組柵格地圖中,WBA*算法相較于傳統A*算法路徑長度分別減了5.48%、6.33%、9.03%,搜索時間縮短了21.43%、23.73%、7.59%,總航向角減少了1.83%、0.18%、12.57%。搜索時間的顯著減少,表明WBA*算法更高效,可以在更短的時間內完成路徑搜索。

圖7 50×50柵格地圖路徑規劃Fig.7 50×50 raster map path planning

20×20的柵格地圖環境中,AWBA*較傳統A*算法相比,生成的路徑長度縮短了11.14 %,總航向角減小了2.15%,搜索時間無明顯變化,但是拐點數量較于傳統A*算法略增。30×30的柵格地圖環境中,AWBA*較傳統A*算法規劃出的路徑長度縮短了7.34 %,拐點數量減少了42.86%,總航向角減少了1.17%,搜索時間縮短了8.47%。50×50的柵格地圖環境中,AWBA*算法較傳統A*算法規劃出的路徑長度縮短了18.64%,拐點數量減少了66.67%,總航向角減少了23.08%,搜索時間縮短了2.07%。

表1 4種算法性能指標對比Table 1 Comparison of performance indicators of four kinds of algorithms

仿真結果表明：20×20的柵格地圖環境下,WBA*算法和AWBA*算法生成的路徑拐點數量相較于傳統A*算法略增,說明算法在搜索過程中使用了更多的信息,需要優化除了路徑長度之外的其他目標,如航向角影響因子等,導致算法選擇一條拐點數量略增的路徑,以滿足這些優化目標。傳統A*算法預估代價函數不準確可能會找到次優解,但是WBA*算法和AWBA*算法一定程度上克服了預估代價函數的不準確性,導致搜索得到的路徑長度更短。在30×30與50×50的柵格地圖環境,WBA*算法和AWBA*算法生成的路徑拐點數量明顯少于傳統A*算法,其他指標也均優于傳統A*算法與BA*算法,說明WBA*算法和AWBA*算法都能夠適應復雜的地圖環境并保持其可行性和優越性。

綜合評估,地圖環境愈加復雜,AWBA*算法雖然搜索時間并沒有太大的提升,但路徑長度明顯縮短、總航向角減少,并在生成路徑的基礎上使用3次B樣條算法對拐點處進行平滑處理,使最終路徑更適合自動駕駛車輛運動的特性。

4 結 語

針對傳統 A*算法在路徑規劃中存在的不足,提出了具有自適應調整權重因子、考慮航向角影響因子的啟發函數、路徑平滑處理的改進A*算法。該算法通過指數增長法使啟發函數中的估價函數隨路徑變化提高搜索效率;隨后在啟發函數中考慮轉彎所帶來的影響(航向角影響因子),使自動駕駛車輛滿足其基本的轉向特性;最后利用3次B樣條算法平滑處理生成的路徑。通過仿真對比分析,提出的改進A*算法在愈加復雜的環境下拐點數更少、路徑更為平滑、路徑距離更短,證明了所提算法的優越性和可行性。接下來將結合其他智能算法,研究如何在動態環境中快速準確地規劃出自動駕駛車輛最優路徑。