基于改進(jìn)混合A*算法的自動(dòng)泊車系統(tǒng)路徑搜索方法

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

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

主題詞：自動(dòng)泊車 混合A*算法 路徑搜索

1 前言

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

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

2 自動(dòng)泊車路徑搜索

2.1 碰撞檢測

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

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

圖1 幾種常見的包圍盒

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

圖2 障礙物簡化形式

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

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

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

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

圖3 節(jié)點(diǎn)擴(kuò)展方式對(duì)比

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

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

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

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

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

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

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

圖4 分段泊車路徑

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

圖5 節(jié)點(diǎn)擴(kuò)展方式

2.3 基于A*算法的代價(jià)地圖

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

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

圖6 基于A*算法得到的代價(jià)地圖

2.4 路徑搜索算法框架

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

圖7 路徑搜索算法流程

3 仿真分析

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

3.1 水平車位

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

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

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

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

3.2 垂直車位

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

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

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

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

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

4 結(jié)束語

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

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