基于改進混合A*算法的自動泊車系統路徑搜索方法

曹彥博 顏京才 李旭升 曹立波

（1.湖南大學，汽車車身先進設計制造國家重點實驗室，長沙 410082；2.毫末智行科技有限公司保定分公司，保定 071000）

主題詞：自動泊車 混合A*算法 路徑搜索

1 前言

路徑規劃作為自動泊車系統的重要組成部分，是保障行駛安全、泊車效率和乘員舒適性的關鍵。路徑搜索是路徑規劃的重要步驟，路徑搜索算法一般可分為圖搜索算法、樹搜索算法和智能優化算法[1]。圖搜索算法[2-3]的原理是將車輛附近的環境信息轉換為離散圖，然后通過一定的搜索算法得到基礎路徑，如A*算法[4]、D*算法[5]、混合A*算法[6]等；樹搜索算法在高維空間可行，可以得到安全無碰撞的路徑，但由于采樣具有隨機性，導致前、后規劃的路徑有可能不一致，路徑曲率連續性差；智能優化算法[7]將路徑規劃視為集安全、邊界和最短距離的約束，包括蟻群算法、遺傳算法和細菌覓食法等。

傳統的路徑搜索算法得到的路徑普遍存在一些缺點，如路徑不夠平滑、難以跟蹤控制、搜索效率不高等。針對這些問題，本文基于較隨機采樣更加穩定的圖搜索算法，提出一種改進的混合A*算法對短距離的自動泊車進行路徑搜索：針對障礙物較多、避障更復雜的車位內的終點和周圍相對較寬敞的起點，采用反向搜索的方法結合適用于該場景的線碰撞檢測方法獲得路徑；利用A*算法獲得代價地圖，在進行路徑搜索時通過查表得到啟發值，大幅縮短搜索時間；通過設置可變的車輛轉角分辨率在節點擴展時增加其擴展方向數量，提高路徑的平滑度，使其更易于被優化和跟蹤控制。

2 自動泊車路徑搜索

2.1 碰撞檢測

在自動駕駛研究中描述車輛周圍環境信息的方法一般包括柵格地圖、拓撲地圖和幾何地圖等方法[8]，而考慮到自動泊車在非結構化道路復雜環境下進行，并且本文所研究的混合A*算法基于柵格進行搜索，因此選用柵格地圖來描述環境信息，即在感知系統獲得真實世界坐標后通過設置合適的分辨率將其轉變為柵格地圖，利用柵格存儲相應的節點信息進行節點擴展和路徑回溯。

基于柵格地圖的碰撞檢測通常用包圍盒來描述車身輪廓，然后通過逐一判斷障礙物占據的柵格與車身的距離進行碰撞檢測。常用的包圍盒包括軸對齊包圍盒（Axis-Aligned Bounding Box，AABB）、有向包圍盒（Oriented Bounding Box，OBB）和球包圍盒。其中，AABB使用與柵格平行的直線包圍車身，OBB采用有向的邊界框，能更好地描述物體的原形狀，球包圍盒則用若干個圓包裹車輛，各包圍盒具體形式如圖1所示。這種柵格占據的碰撞檢測方式可以較好地描述車身輪廓并通過判斷與障礙物的距離進行碰撞檢測，但需要對所有障礙物占據的柵格進行判斷，增加了搜索時間，并且將障礙物離散化為柵格容易出現誤差，同時較小的分辨率會增加時間成本，較大的分辨率則會導致對障礙物的描述不準確。

圖1 幾種常見的包圍盒

本文重點研究車輛已在車位附近的短距離自動泊車場景下的路徑搜索，此場景下車輛起點周圍環境簡單，通常不會有復雜的障礙物，只需考慮停入車位過程中的避障即可。因此，為了簡化碰撞檢測的計算，本文將車位邊框設置為障礙物線，只保留泊車的入口，如圖2 所示。然后通過向量叉積法判斷車身輪廓線與障礙物線是否相交進行碰撞檢測，即在已經設定好的泊車場景中，利用車身參數和當前所處位置計算出車身4個頂點的坐標，如D點坐標(xD,yD)：

圖2 障礙物簡化形式

式中，(xr，yr)為車輛后軸中心坐標；lr為車輛后軸到車身后端的距離；W為車身寬度；θ為車輛軸線與水平方向的夾角。

得到4個頂點的坐標即可獲得描述車身輪廓的4條線段，然后通過判斷其與障礙物線是否相交進行碰撞檢測。這種碰撞檢測方法計算簡單，可以節省混合A*算法搜索路徑的時間，并且這一過程在世界坐標下進行，無需進行障礙物占據柵格轉化，誤差較小。

2.2 基于混合A*算法的反向搜索

A*算法采用貪心策略，通過設計啟發函數選擇代價估計值小的點進行搜索，相較于廣度優先搜索和深度優先搜索有著更高的搜索效率[9]。傳統A*算法搜索出的路徑是折線，顯然不符合車輛的運動學條件。與傳統A*算法相比，混合A*算法改變了連通圖結構，額外考慮車輛位姿，引入θ這一狀態量，優化了節點拓展方式，并以車輛的最小轉彎半徑作為約束條件，使得搜索出的路徑滿足車輛的運動特性，從而易被跟蹤執行，2 種算法的節點擴展方式如圖3所示。

圖3 節點擴展方式對比

與A*算法相同，混合A*算法也使用評價函數選擇更優的節點：

式中，f(n)為從初始狀態經由狀態n到目標狀態的成本估計量；g(n)為在狀態空間中從初始狀態到狀態n的實際成本；h(n)為從狀態n到目標狀態的最佳路徑的成本估計。

但混合A*算法的內涵與普通A*算法存在較大差別。g函數除原有意義外，還應針對不理想因素施加懲罰，如對反復前進倒退、頻繁變化轉角等情況進行懲罰來體現當前節點的競爭力[10]。啟發值h的計算方法一般為：

式中，hc_av為考慮避障因素但不考慮運動學可行性時的路徑長度；hk_c為考慮車輛運動學約束但忽略碰撞時的路徑長度。

式（3）的含義為：如果當前節點與終點距離較近，生成路徑的難度在于保證運動可行性，此時hk_c更能反映路徑長度；反之，路徑生成難度在于避障，需要hc_av作為啟發值。通常用RS（Reeds-Shepp）曲線估算hk_c，用經典A*算法確定hc_av的取值。

一般情況下，混合A*算法從起始位姿開始，朝著目標位姿擴展節點，同時嘗試使用無碰的RS 曲線連接當前節點和目標點以加快搜索速度。但在車位較小或終點附近環境復雜時，連接終點的RS 曲線無法滿足避障要求，導致節點擴展過多，最終出現搜索不到路徑或耗時較久的情況。

在本文所研究的泊車場景中，車輛的起點處環境簡單，根據2.1節的分析，僅考慮車位附近的避障，即車位線，因此目標點（車位內）避障要求較高。針對此情景，本文提出一種基于混合A*算法從目標位姿向起始位姿反向搜索的搜索策略，將泊車路徑分為2 段，如圖4 所示。第1 段從空間狹窄的車位中的目標點開始向起點不斷進行節點擴展（E-O段），第2段嘗試在滿足避障要求的條件下用RS 曲線從當前節點連接起點（O-S段），最終將所有點逆序輸出即可得到搜索出的泊車路徑，而由于起點附近空間較大，RS 曲線會較早實現無碰連接起始位姿來加速搜索。同時，終點附近狹窄的車位也由擴展節點結合碰撞檢測保證路徑的最優性。

圖4 分段泊車路徑

此外，傳統的混合A*算法在節點擴展時僅有6 個方向，即直行的前進、后退以及轉向盤向左、向右最大轉角狀態下的前進和后退，導致車輪的轉向角較大，難以實現車輛的控制并且影響乘員的乘坐體驗。本文通過設置車輪轉向角的分辨率增加搜索過程中的節點擴展方向，其擴展方式如圖5 所示，同時考慮了轉向角的代價值，使得搜索出的路徑更加平滑，避免車輛轉向角過大導致各段路徑之間連接處曲率不連續，使路徑易于被優化和跟蹤。在代價函數方面，除為使路徑更加平滑設置的轉向角度代價外，還加入了車輛前進/后退擋位切換代價、轉向代價等，并將其加權放入函數g中，共同保證了路徑的平滑和泊車過程中乘員的舒適性。

圖5 節點擴展方式

2.3 基于A*算法的代價地圖

由于式（3）中hc_av指使用A*算法得到的當前節點到目標點的啟發值，因此通常在搜索過程中需要不斷使用A*算法計算當前柵格到終點柵格的h，耗費大量時間。

本文使用一種代價地圖來獲得啟發值h，在混合A*算法開始路徑搜索前先將地圖柵格化，接著利用A*算法計算每個柵格到終點柵格的代價值，將該值與柵格對應存儲即得到代價地圖，如圖6所示，圖中數值為A*算法計算得到的柵格代價值，Inf 表示該柵格代價值為無窮大，為車輛不可到達的區域。因此，在后續路徑搜索過程中無需再逐一進行計算，可以直接從代價地圖中通過查表方式獲取啟發值，可大幅節省時間，提高路徑規劃效率。且本文設計的搜索算法從終點向起點反向搜索路徑，再使用A*算法計算代價地圖時的目標點即為泊車起點，即可得到所有柵格到起點的代價值，相當于同時得到了多個車位到起點的啟發值。多車位下全自動的泊車路徑規劃同樣具有重要意義，在本文研究的自動泊車系統中具體表現為：自動泊車系統啟動后，系統首先自動計算代價地圖，用戶選擇想要泊入的車位后泊車系統進行路徑搜索，在搜索過程中省略了使用A*算法計算啟發值的步驟，直接從代價地圖中查表獲得，極大節省了搜索時間。

圖6 基于A*算法得到的代價地圖

2.4 路徑搜索算法框架

綜上，本文提出的路徑搜索算法流程如圖7 所示。通過傳感器等得到環境信息和起止點后首先通過A*算法得到整體的代價地圖，再使用改進混合A*算法從目標位姿反向搜索路徑，其間不斷嘗試用RS 曲線連接當前節點與起點并進行碰撞檢測，直到成功利用無碰撞的RS 曲線連接或直接擴展到起點時結束搜索，最后逆序輸出所有節點即可獲得有效泊車路徑。在搜索過程中通過open 和close 2 個集合對節點進行管理，open 集存放待擴展節點，將已經擴展過的節點存入close集。

圖7 路徑搜索算法流程

3 仿真分析

城市中常見的車位類型有水平式和垂直式，因此本文使用MATLAB 分別設計針對這2 種常見車位的泊車場景，對改進的混合A*算法和原始算法進行對比仿真分析，以驗證其合理性和有效性。

3.1 水平車位

在水平車位泊車場景下分別使用普通混合A*算法和本文提出的改進混合A*算法進行泊車路徑搜索，結果如圖8所示。普通混合A*算法的節點擴展方向只有6種，因此本文在進行時間比較時將改進混合A*算法的節點擴展方向設置為與其相等，以驗證本文設計的反向搜索結合代價地圖查表的混合A*算法的實時性。2 種算法的仿真結果數據如表1所示。

表1 水平車位泊車場景仿真結果數據

圖8 水平車位泊車場景下2種算法的路徑搜索結果

結合圖8和表1可以看出，改進混合A*算法用時更短，得到的路徑也更短、更加平滑，提高了泊車效率，易于后續對車輛的控制，從而實現較好的軌跡跟蹤。

3.2 垂直車位

垂直車位泊車場景下仿真條件設置與水平車位場景相同。圖9 所示為普通混合A*算法和改進混合A*算法的路徑搜索結果，其仿真結果數據對比如表2所示。

表2 垂直車位泊車場景仿真結果數據

圖9 垂直車位泊車場景下2種算法的路徑搜索結果

從圖9 中可以看出，改進算法得到的路徑更優，且由表2 可知，改進算法比原始算法節省了2/3 以上的時間，極大提高了規劃效率。

綜合2種車位泊車場景下的仿真結果，本文提出的改進混合A*算法在水平車位泊車場景和垂直車位泊車場景下均比傳統混合A*算法用時更短，說明在短距離泊車、車位附近環境復雜的場景下，反向搜索結合代價地圖的使用可以有效提高路徑搜索效率。并且結合仿真數據和得到的路徑可以看出，改進的算法獲得的路徑更短、更平滑，驗證了變分辨率節點擴展的優越性。

4 結束語

本文基于混合A*算法，針對短距離自動泊車提出一種反向搜索的路徑搜索算法，結合適合此泊車情景的較簡單的線碰撞檢測方法，使其從終點開始擴展，同時嘗試用RS曲線連接起點，通過設置轉向角分辨率增加路徑搜索過程中的節點擴展方向，并結合使用A*算法得到的代價地圖，使其能夠適用于有多個可選車位的泊車場景，然后使用MATLAB分別設計了平行和垂直車位泊車場景，對提出的算法進行仿真分析，并與普通混合A*算法進行對比。結果顯示，2種場景下改進的混合A*算法均有效提高了搜索效率，且得到的路徑更短、更平滑。

完整的自動泊車路徑規劃系統應包括搜索、優化，并對速度進行規劃得到完整的泊車軌跡，本文只開展了路徑規劃中的搜索工作，未對搜索到的路徑進行優化，這也是本文后續需要研究的內容。