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

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

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

1 前言

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

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

2 自動泊車路徑搜索

2.1 碰撞檢測

在自動駕駛研究中描述車輛周圍環(huán)境信息的方法一般包括柵格地圖、拓?fù)涞貓D和幾何地圖等方法[8]，而考慮到自動泊車在非結(jié)構(gòu)化道路復(fù)雜環(huán)境下進行，并且本文所研究的混合A*算法基于柵格進行搜索，因此選用柵格地圖來描述環(huán)境信息，即在感知系統(tǒng)獲得真實世界坐標(biāo)后通過設(shè)置合適的分辨率將其轉(zhuǎn)變?yōu)闁鸥竦貓D，利用柵格存儲相應(yīng)的節(jié)點信息進行節(jié)點擴展和路徑回溯。

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

圖1 幾種常見的包圍盒

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

圖2 障礙物簡化形式

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

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

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

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

圖3 節(jié)點擴展方式對比

與A*算法相同，混合A*算法也使用評價函數(shù)選擇更優(yōu)的節(jié)點：

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

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

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

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

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

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

圖4 分段泊車路徑

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

圖5 節(jié)點擴展方式

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

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

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

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

2.4 路徑搜索算法框架

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

圖7 路徑搜索算法流程

3 仿真分析

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

3.1 水平車位

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

表1 水平車位泊車場景仿真結(jié)果數(shù)據(jù)

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

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

3.2 垂直車位

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

表2 垂直車位泊車場景仿真結(jié)果數(shù)據(jù)

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

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

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

4 結(jié)束語

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

完整的自動泊車路徑規(guī)劃系統(tǒng)應(yīng)包括搜索、優(yōu)化，并對速度進行規(guī)劃得到完整的泊車軌跡，本文只開展了路徑規(guī)劃中的搜索工作，未對搜索到的路徑進行優(yōu)化，這也是本文后續(xù)需要研究的內(nèi)容。