基于關鍵節點的改進A*無人車路徑規劃算法

張輝 張瑞亮,2 許小慶 范政武,2

（1.太原理工大學，太原 030024；2.山西省汽車設計工程技術研究中心，太原 030024）

1 前言

無人車系統主要包括感知、決策、規劃和控制4 個模塊[1]，其中規劃模塊負責基于目標任務生成最優路徑。按照搜索方式可將路徑規劃算法分為基于圖搜索、基于采樣及基于人工智能的方法。其中，基于圖搜索的A*算法因其尋優能力強、場景適應度高等特性而得到廣泛應用[2]。

陳若男等[3]結合距離及角度信息改進傳統A*算法代價函數，設計鄰域選擇策略，提升了算法效率和路徑安全性。楊瑤等[4]通過改進距離估算方式，引入車身輪廓代價和障礙物距離代價，提升了A*算法規劃效率和路徑安全性。馮來春等[5]將A*算法和快速搜索隨機樹（Rapid-Exploring Random Tree，RRT）算法結合，降低了RRT 算法的采樣盲目性。孟珠李等[6]將A*算法與B 樣條曲線結合，改善了A*算法規劃路徑的平滑性。余文凱等[7]采用K-均值（K-Means）聚類算法對地圖進行預處理，量化不同區域地圖復雜度，依據復雜度設置搜索權重，應用弗洛伊德（Floyd）算法對路徑進行平滑，提高了路徑規劃效率和路徑平滑性。祁玄玄等[8]從目標性拓展等多個方面對傳統A*算法進行改進，提升了A*算法的搜索效率。Frantisek等[9]研究了各種改進A*算法，如跳點搜索（Jump Points Search，JPS）、Phi*等，并基于部分算法進行了仿真驗證。Hao等[10]提出了一種可以自適應調整搜索深度的Cell A*算法，使高復雜度場景地圖下的計算耗時進一步縮短。Zahra等[11]在A*算法中引入動態可變柵格，增加了危險懲罰代價，提升了路徑安全性和計算效率。

搜索效率和路徑安全性及可行性是規劃算法的核心，但A*算法多循環判斷機制導致其路徑性能和計算效率難以同時得到有效改善。因此，本文提出一種改進A*算法，基于地圖預覽模塊提取柵格地圖關鍵節點，規劃模塊依據關鍵節點信息判斷是否開啟增量式擴展搜索，引入基于安全距離的碰撞場模型改進代價函數，應用準均勻三次B 樣條曲線對生成路徑進行平滑處理得到最終的規劃路徑。最后與對比算法在不同復雜度場景下進行仿真驗證。

2 傳統A*算法

傳統A*算法融合了廣度優先搜索（Breadth First Search，BFS）算法和深度優先搜索（Depth First Search，DFS）算法的優勢[3]，其基于當前節點與起點和目標點之間的距離信息進行節點擴展和選擇。傳統A*算法作為典型圖搜索算法，其規劃地圖通常以柵格地圖形式表達，如圖1所示為某柵格地圖示例。柵格地圖尺寸決定了地圖表達的精度和規模[12]，廣義柵格地圖僅包含障礙物信息。

圖1 柵格地圖

如圖1所示，在規劃過程中，車輛從起點出發，通過擴展循環最終行駛至目標點，傳統A*算法具體流程如圖2所示。

圖2 傳統A*算法流程

節點擴展時依據場景特點及無人車運動學特性選取四鄰域方式進行節點擴展；代價函數f(N)通過當前節點與起始點間的真實距離g(N)和當前節點與目標節點間的預估距離h(N)累加得到[13-16]，h(N)一般采用曼哈頓（Manhattan）、歐幾里得（Euclidean）、切比雪夫（Chebyshev）距離。

3 改進A*算法

傳統A*算法存在搜索路徑多曲折、緊貼障礙物邊緣、不平滑及搜索時間隨柵格地圖規模增大而呈現指數型增長趨勢等缺陷。一般結構化道路作為車輛的主要應用場景，其特點為道路多正向直行區間、多交叉區域、空間障礙物類型較多，因此其規劃的路徑無法直接適應無人車在此類環境下的工作需求。本文針對此類場景需求提出改進A*算法，流程如圖3所示。

圖3 改進A*算法流程

相對于傳統A*算法，算法的主要改進如下：

a.引入地圖預覽模塊提取道路轉向關鍵節點和道路死角關鍵節點，并基于目標向量信息選取關鍵節點，基于當前節點和擬選擇關鍵節點形成搜索域，基于障礙物判定規則判斷是否開啟增量式擴展搜索；

b.對不同類型障礙物進行分類，提出一種基于安全距離的碰撞場模型，并結合其對傳統A*算法代價函數進行改進；

c.應用準均勻三次B 樣條曲線對規劃路徑在轉折處進行平滑，生成滿足車輛運動學約束的最終路徑。

3.1 基于關鍵節點信息的擴展搜索策略

3.1.1 關鍵節點信息獲取

傳統柵格地圖作為地理信息載體[17]，其將環境抽象為柵格狀態，僅反映不同位置障礙物信息。為進一步提高算法效率，本文提出一種基于地圖預覽模塊獲取柵格地圖關鍵節點的方式。

獲取柵格地圖后，地圖預覽模塊從柵格群中提取關鍵節點至關鍵節點列表中，并將道路邊界節點集合提取至道路邊界節點列表中。其中，關鍵節點包括道路轉向關鍵節點和道路死角節點兩類，分別存放于關鍵節點列表的不同列中，兩類節點見圖1，其中，道路死角關鍵節點僅能作為起點位置，一旦規劃過程擬選定道路死角關鍵節點，則放棄該操作，重新進行節點選擇。

3.1.2 目標向量定義

無人車在進行節點選擇時，依據當前節點與目標點間向量，即目標向量進行判斷，選取策略如圖4所示，其中，S節點為起點，A、B、C、D節點為車輛可選關鍵節點，G節點為目標點。車輛從起點S出發，并基于當前位置的向量SG（當車輛行駛至新位置時，目標向量由當前節點與目標點構成）選擇方向趨近于目標點的關鍵節點，判定擬選定的關鍵節點是否為道路死角關鍵節點，如果是，則放棄此節點，重新選擇其他可行節點，反之，判斷當前節點至擬選定節點間搜索域內是否存在障礙物，如果不存在，則直接將車輛從當前位置節點移至擬選定關鍵節點處，如果存在，則進行增量式擴展，得到該區域內無碰撞的安全路徑。當車輛駛至擬選定關鍵節點后，依據直行優先原則，結合當前節點至目標節點向量進行關鍵節點選擇，直至到達目標點G，跳出循環，輸出最終路徑。

圖4 基于目標向量的關鍵節點選取策略

3.2 改進代價函數

傳統A*算法的代價函數僅考慮當前節點與起始點和目標點之間的距離關系。其中：當前節點與起始節點之間真實距離代價g(N)可使已搜索路徑最優，但無法抑制無效節點擴展；當前節點與目標節點之間的預估代價h(N)可以有效改善算法節點擴展盲目性，但目標趨向也將導致A*算法規劃路徑過早轉折或緊貼障礙物邊界，不利于行車安全。因此，本文結合地圖預覽模塊獲得的關鍵節點信息和道路邊界信息，對A*算法進行改進。

3.2.1 距離表達函數

為了適應結構化道路特性和無人駕駛汽車運動學特性，本文選用Manhattan 函數計算當前節點至目標節點的距離[18]。Manhattan 距離是為規劃方型建筑區塊的最短行車路徑而提出的，因此采用Manhattan 函數預估結構化道路中兩點之間距離更接近真實值[19]。以圖1為例，節點S與目標點G的距離為：

式中，dM為當前節點與目標節點之間的Manhattan距離；(Sx,Sy)、(Gx,Gy)分別為節點S和目標節點G的坐標。

3.2.2 障礙物判定規則

無人車在目標向量引導下選取下一關鍵節點，如圖5所示，車輛在規劃過程中基于障礙物判定開啟增量式擴展搜索。當前車輛下一擬選定節點為節點N，此時車輛搜索域為圖5 所示的長度為|Nx-Sx|、寬度為5 m（道路寬度）的矩形區域。其中，Nx為車輛擬選定節點的橫坐標。判斷此區域是否存在障礙物占據柵格節點，如果存在，啟用增量式擴展進行無碰撞路徑規劃，反之，直接將車輛移動至擬選定節點N。

圖5 障礙物檢測區域示意

3.2.3 基于安全距離的碰撞場模型

實際行駛中，車輛應保持足夠的安全距離。根據本車和前車的運行狀態及相關因素估算實施制動干預距離，保證車輛不發生追尾碰撞[20]。典型的安全距離包括固定安全距離、基于自由滑行時間的安全距離、基于駕駛員預估模型的安全距離和基于車間距的安全距離等[21]。本文基于安全距離提出一種新的碰撞場模型，并將其應用于代價函數改進。

本文依據不同物體特點將障礙物主要分為道路邊界、移動概率高但形狀尺寸較小的障礙物和移動概率低但形狀尺寸較大的障礙物。其中，道路邊界選用固定距離碰撞場模型。針對另外兩種類型的障礙物，經過對駕駛員駕乘習慣的分析，基于車間距模型設計，得到一種基于安全距離的碰撞場模型。

基于安全距離的碰撞場模型由車輛當前行駛速度、路面附著系數等參數共同決定：

式中，ds為不同類型障礙物的安全距離；k為不同類型障礙物的權重；v為車輛當前行駛速度；μ為車輛當前所在路面的附著系數；g=9.8 m/s2為重力加速度；du為單位距離，此處取值為1 m。

其中，不同障礙物類型定義如下：

a.沿道路寬度方向投影長度l∈[1,2)m 的物體，即為形狀尺寸較大的障礙物，如道路側方的靜止車輛等，其移動概率較低，不易突然運動；

b.l∈(0,1)m的物體，即為形狀尺寸較小的障礙物，如道路邊沿的靜止行人等，其移動概率較高，易突然運動。

經過仿真及對實際駕駛情況的分析，上述不同類型障礙物安全距離計算權重中，形狀尺寸較大的障礙物取值為1.5，形狀尺寸較小的障礙物取值為1.2。如圖6 和圖7 所示分別為不同形狀尺寸的障礙物所對應的碰撞場模型。隨著道路附著系數的降低和車速的增大，碰撞場距離增長速度呈現由緩及增的趨勢，這與人類駕駛員駕乘習慣相符。

圖6 形狀尺寸較大的障礙物碰撞場模型

一般路面的附著系數不小于0.7。由圖6、圖7 可知，當路面附著系數達到0.7及以上時，考慮車速最高為20 m/s的情況，形狀尺寸較大的障礙物碰撞場距離最大為44.16 m，形狀尺寸較小的障礙物碰撞場距離最大為35.59 m，符合實際駕駛工況需求。

圖7 形狀尺寸較小的障礙物碰撞場模型

由ds可求得擴展柵格的總層數c為：

將碰撞場距離重新賦值為10 m×c，并作為擴展柵格最內層柵格代價值，依次向外以10 m的倍數進行遞減賦值，如果障礙物擴展柵格中包含障礙物柵格，則不對其進行賦值。如圖8所示，當車速為10 m/s，路面附著系數為0.8時，由式（2）可得ds=8.7 m，由式（3）可得c=1，因此直接將代價值10 m賦予障礙物邊界外第1層柵格，其代價值等同于算法循環中車輛移動1個柵格距離的代價值。

圖8 基于碰撞場距離擴展柵格

3.2.4 總體代價函數

改進A*算法代價函數為：

式中，o(N)為不同障礙物的碰撞場距離代價；k1、k2為不同代價函數的權重。

其中k1越大，路徑趨向目標點速率越快，計算耗時越短，但這將會影響路徑最優性，而k2取值則會直接影響規劃路徑與障礙物邊界的距離，進而影響車輛行駛安全性。經過多次仿真和驗證，本文取k1=2、k2=2。

3.3 路徑平滑方式

準均勻B樣條曲線不僅能夠保證路徑曲率連續，還能夠滿足路徑的首尾約束，保證擬合路徑部分與原路徑平滑匹配[22]。

設準均勻B樣條曲線為：

式中，{P1,P2,…,Pi,…,Pn-1,Pn}(Pi=(xi,yi),i=0,1,2,…,n)為曲線控制點坐標序列；{Ni,p(u)}(i=0,1,2,…,n)為非周期且非均勻節點矢量u=(0,0,0,0,up+1,up+2,…,un+1,un+2,un+2,un+2,un+2)上的p次B樣條基函數[23]。

將各區間中整體參數u所表示的基函數替換為局部參數t∈[0,1]，準均勻三次B 樣條曲線中第i段曲線的矩陣表達式為：

式中，di、di+1、di+2、di+3為特征多邊形的頂點坐標；Mi為p次準均勻B樣條的系數矩陣：

其中，mi,j計算公式為：

本文基于地圖預覽模塊獲取的道路轉向關鍵節點和在行駛過程中產生的避障轉向節點進行路徑平滑，采用準均勻三次B 樣條曲線對相應的軌跡進行優化。如圖9 所示為基于道路轉向關鍵節點和避障轉向節點進行的路徑平滑過程示意。其中，基于道路轉向關鍵節點進行路徑平滑時，共選用7 個控制節點，其位置分別為基于選定的關鍵轉向節點向兩側各擴展橫向相等間距為1 m的3個節點；基于避障轉向節點進行平滑時，以2個變向節點為控制點，向前、后兩側擴展橫向相等間距為1 m的3個節點。

圖9 路徑平滑方式

乘用車最小轉彎半徑通常為4～6 m，一般城市主干道交叉口轉彎半徑大于20 m，此處平滑后所生成的路徑既滿足汽車最小轉彎半徑約束，也滿足道路交叉口轉彎半徑約束。

4 仿真驗證

為了驗證改進A*算法的性能，本文分別將傳統A*算法、Weighted-A*算法和改進A*算法應用于基于校園地圖所繪制的區域柵格地圖上。

為驗證改進算法的泛化能力和改進性能，本文采用小區域低復雜度場景地圖和大區域高復雜度場景地圖進行仿真對比驗證。

本文設定相同的起始點和目標點，運行后分別記錄不同算法路徑長度la、平均計算耗時ta、擴展節點數量na、轉向次數sa、與道路邊界或其他障礙物邊界的最小距離da（假設車輛寬度為1.6 m）等參數。其中Weighted-A*算法啟發函數權重設為2，經仿真驗證可知，此時路徑效果和搜索速度較為均衡。

仿真環境計算機配置為：Windows 10 操作系統，Intel Core i5處理器，主頻為3.0 GHz，運行內存為8 GB。

4.1 小區域低復雜度場景

小區域低復雜度場景指不存在其他類型障礙物，且道路結構較簡單的場景。此類環境下，影響行車安全性和可行性的主要因素為車輛與道路邊界的距離及轉向次數。如圖10 所示，小區域低復雜度場景柵格地圖尺寸為30 m×30 m，對3 種算法設定相同起始點和目標點。A*算法屬于最優搜索算法，因此其輸出路徑是唯一的，但受系統測試環境及硬件性能的綜合影響，計算耗時在單次迭代中存在差異，經過多次迭代尋路的誤差累積后，最終導致不同測試過程中計算耗時出現差異。為消除計算耗時的隨機影響，在相同測試條件下對不同算法測試50 次，并計算各算法平均計算耗時ta，以充分驗證改進算法計算效率。某次規劃最終結果如圖10所示。

圖10 小區域低復雜度場景下不同算法行駛覆蓋軌跡

由圖10 可知，傳統A*算法和Weighted-A*算法得到的路徑結果一致，但其行駛覆蓋軌跡，即車輛行駛過程中Z向投影面積移動覆蓋區域，緊貼道路邊沿，且轉折較多，并不適用于該場景無人車行駛。由本文改進算法得到的路徑不僅能夠遠離障礙物，同時能夠盡量保持直行，保證了規劃路徑的安全性和可行性。

50 次測試計算耗時結果如圖11 所示。由圖11可知，改進A*算法在小區域低復雜度場景下計算耗時相比傳統A*算法明顯縮短，但其與Weighted-A*算法計算耗時接近。不同算法的計算耗時波動范圍均保持在20 ms 范圍內，可見改進算法的計算性能較為穩定。

圖11 小區域低復雜度場景下不同算法計算耗時

分別記錄不同算法在50 次測試中的主要參數如表1 所示。其中改進率表示本文改進算法與其他算法不同參數值之差的絕對值占其他算法該參數取值的比例。

表1 小區域低復雜度場景下不同算法測試結果

由表1 可知，在小區域低復雜度場景下，本文算法擴展節點數量相對傳統A*算法和Weighted-A*算法大幅減少，但本文算法引入循環判斷，因此計算耗時相比Weighted-A*算法提升效果并不明顯，僅提升8.3%。本文改進A*算法相對傳統A*算法和Weighted-A*算法轉折次數更少，路徑與不同類型障礙物邊界的最小距離也足以保障行車安全性。綜上所述，本文改進算法的效果更加突出。

如圖12所示為本文算法經過路徑平滑處理后得到的優化路徑，以微分的方法計算規劃路徑的曲率，得到此環境下路徑最小曲率半徑為6.5 m，滿足車輛最小轉彎半徑約束，同時全路徑曲率保持連續。為進一步判斷平滑后路徑的性能，對平滑路徑與未平滑路徑之間的偏離量及平滑路徑與道路邊界的距離最小值分別進行計算。由路徑平滑方式可知，平滑路徑在曲率最大處與未平滑路徑偏離量最大，最大值為1.41 m，此時，其與道路邊界最小距離為1.32 m。綜上可知，平滑后的路徑不僅滿足車輛的運動學約束，同時仍能夠與道路邊界保持足夠的安全距離，可見其相比未平滑路徑更適合車輛的穩定控制[24]。

圖12 本文改進A*算法平滑路徑

4.2 大區域高復雜度場景

為進一步驗證本文算法在大區域高復雜度場景下的性能，選擇存在道路死角節點和不同類型障礙物的大尺寸柵格地圖進行仿真。此類環境中影響行車安全性和可行性的主要因素為車輛與道路邊界及不同類型障礙物邊界的距離及轉向次數。大區域高復雜度場景柵格地圖尺寸為450 m×200 m，各算法某次規劃結果如圖13所示。

由圖13 可知，傳統A*算法和Weighted-A*算法在大區域高復雜度場景下，路徑多轉折、緊貼障礙物等問題更加突出，無法保證行車過程的安全性和可行性。但本文改進A*算法規劃的路徑不僅能夠與道路邊界保持一定距離，而且在應對不同類型障礙物時能夠基于碰撞場模型進一步規劃出更加安全可靠的避障路徑，改進A*算法的綜合優勢更加明顯。

50 次測試的計算耗時結果如圖14 所示。改進A*算法在大區域高復雜度場景下計算耗時相比傳統A*算法和Weighted-A*算法明顯縮短。

圖14 大區域高復雜度場景下不同算法計算耗時

不同算法測試參數如表2所示，由圖2可知：在大區域高復雜度場景下，本文算法擴展節點數量相對傳統A*算法和Weighted-A*算法大幅減少，計算耗時明顯縮短；本文改進A*算法相對傳統A*算法和Weighted-A*算法轉折次數更少，路徑與不同類型障礙物邊界的最小距離也足以保障行車安全。

表2 大區域高復雜度場景下不同算法測試結果

如圖15所示為本文算法經過路徑平滑處理后得到的優化路徑，以微分方法計算規劃路徑的曲率，得到此環境下路徑最小曲率半徑為6.2 m，滿足車輛最小轉彎半徑約束，同時全路徑曲率保持連續。對車輛平滑及未平滑路徑之間的偏離度和平滑路徑與道路邊界的最小距離進行測量，測量結果同小區域場景，可見平滑路徑有利于車輛的穩定控制。

圖15 本文改進算法平滑路徑

5 結束語

本文針對傳統A*算法的缺陷進行了改進，通過地圖模塊的應用、基于安全距離的碰撞場模型的引入以及三次準均勻B樣條曲線路徑平滑方式，改善了算法的計算效率、可行性及安全性。

由于本文算法主要針對低速結構化道路場景進行路徑規劃，同時，搜索過程中僅考慮了單向行駛和靜態障礙物，因此改進算法對高度復雜交互類交通區域仍無法適應。本文后續研究的重點是基于交通規則、駕駛行為的進一步探索和針對高速行駛等復雜動態工況的適應性研究[25-27]。