劉知陽, 崔立堃, 耿璽鈞, 馮緒永, 韓 晉

陜西理工大學(xué) 機(jī)械工程學(xué)院, 陜西 漢中 723000

汽車的出現(xiàn)使人們的出行交通更加便利,但這種便利也帶來了許多現(xiàn)實問題,其中泊車就是一大難題。泊車受限于狹窄的視野環(huán)境,不可控的緊張情緒,還要同時面對不同的泊車環(huán)境,一不當(dāng)心就會發(fā)生事故。自動泊車系統(tǒng)(Automatic Parking System,APS)的出現(xiàn)便能很好地降低事故的發(fā)生概率,將其限制在一個可控的范圍內(nèi)[1]。自動泊車系統(tǒng)也分為垂直停車和平行停車。垂直停車也就是倒車入庫,一般用于停車場、車庫停車;平行停車也就是側(cè)方位停車,一般用于路邊停車。

在國內(nèi)外研究者的持續(xù)努力下,自動泊車技術(shù)研究取得了許多顯著成果?；旌螦*算法于2008年由斯坦福的Dmitri Dolgov等[2]首次提出,該算法是一種基于車輛運動學(xué)的規(guī)劃算法,能夠滿足車輛運動學(xué)的要求,解決了連續(xù)空間內(nèi)規(guī)劃車輛朝向和位置的問題,但規(guī)劃效率低下、規(guī)劃時間過長、規(guī)劃路徑無法保證成功率。Sedighi S等[3]將著名的混合A*搜索引擎與Visibility Diagram規(guī)劃相結(jié)合,在混合(連續(xù)-離散)環(huán)境下尋找最短的非完整路徑,為混合A*規(guī)劃非完整約束下的最優(yōu)路徑提供了正確的路徑點,但需要一定要求的高清靜態(tài)地圖部署。瑞典斯德哥爾摩KTH皇家理工學(xué)院綜合交通研究實驗室(ITRL)的研究概念車(RCV)開發(fā)了適用于概念車的混合A*算法路徑規(guī)劃算法,生成的算法需要以一種便于部署的方式進(jìn)行包裝,并且可以與研究車上的其他不同系統(tǒng)進(jìn)行交互,同時也對實現(xiàn)實驗測試所需的實時能力的方法以及如何建立模擬和調(diào)試的可視化環(huán)境提供見解,證明了混合A*算法的實用性,但對算法的運行效率沒有顯著提升[4]。中國學(xué)者LI Chao等[5]提出了一種基于A*算法和RS算法的路徑規(guī)劃算法,該算法引入了導(dǎo)引點的概念,通過RS算法直接生成無碰撞路徑,并針對各種工作場景,采用混合A*算法離線計算導(dǎo)引點,提升了算法的運行效率,但失去了算法最優(yōu)路徑的效果,且無法保證完全不會碰撞障礙物。因此,本文提出了融合雙向搜索的混合A*算法,在保證避免碰撞且保持最優(yōu)路徑的前提下提升算法的運行效率。

本文主要設(shè)計了一種融合雙向搜索算法的改進(jìn)混合A*算法,同時設(shè)置起點和終點作初始點,結(jié)合車輛碰撞信息,并設(shè)置合理的代價函數(shù),相互搜尋最佳路徑信息,能有效地縮減單向搜索后期繁多的無效節(jié)點并減少搜索次數(shù),同時利用柵格法劃分地圖,有效地避免了在相同區(qū)域內(nèi)重復(fù)搜索的可能性,同時結(jié)合柵格法和雙向搜索算法,極大地提高了正反向搜索過程中相交的可能性。

1 算法概述

1.1 傳統(tǒng)混合A*算法

混合A*算法是自動駕駛汽車常用的全局路徑規(guī)劃算法,其方法可以看作是A*算法結(jié)合實際汽車動力學(xué)模型后生成的算法,其原理與A*算法類似,從起點開始向四周搜索子節(jié)點,并計算子節(jié)點的代價值,取其中最小代價值的點為下一個父節(jié)點,直至搜索到終點[6]。不同于A*算法的是,混合A*算法搜索方式主要分為前進(jìn)和后退兩種模式,并結(jié)合車輛模型,平均分割其轉(zhuǎn)角,并在轉(zhuǎn)角范圍內(nèi)進(jìn)行子節(jié)點的搜索,計算當(dāng)前的車輛位姿到子節(jié)點的車輛位姿變換是否會與障礙物發(fā)生碰撞,并建立Openlist表接收轉(zhuǎn)角范圍內(nèi)搜索到的子節(jié)點,建立Closelist表接收已經(jīng)選取過的父節(jié)點和最小代價的子節(jié)點[7]。

圖1(a)所示為A*算法搜索點的搜索模式[8],圖1(b)所示為混合A*算法搜索鄰近點所遵循動力學(xué)約束的搜索模式。

(a) A*算法搜索方法 (b) 混合A*算法搜索方法圖1 兩種算法搜索路徑對比

混合A*算法的代價函數(shù)為

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

(1)

其中,f(n)為當(dāng)前節(jié)點n的代價值,g(n)表示起始點到當(dāng)前點n的路徑代價值,h(n)為當(dāng)前點到目標(biāo)點的預(yù)估代價值[9]。混合A*算法沿用A*算法的啟發(fā)函數(shù),主要有曼哈頓距離、歐氏距離、切比雪夫距離等[10]。其中歐式距離在程序中容易實現(xiàn),只占用少量的計算資源[11],且適合數(shù)據(jù)的值域比較相似的場景,因此,本文選用歐氏距離來計算節(jié)點的預(yù)估代價值h(n)。

歐氏距離表達(dá)式為

(2)

其中,p1(x1,y1)為當(dāng)前點,p2(x2,y2)為終點。

傳統(tǒng)的混合A*算法雖然比A*算法更加貼合無人駕駛的實際應(yīng)用場景,但也存在些許不足,例如在泊車搜索路徑中,若存在障礙物較多,搜索過程就會比較緩慢;若搜索路徑較長,搜索節(jié)點數(shù)會迅速增大,搜索時間會比較長[12]。為了使混合A*算法在以上情況也能保持良好的時效性,本文選擇雙向搜索算法和混合A*算法相融合,以解決混合A*算法在障礙物較多時搜索時間較長的問題。

1.2 雙向搜索算法

地圖數(shù)據(jù)通?？梢杂脠D(Graph)這類數(shù)據(jù)結(jié)構(gòu)表示,在圖結(jié)構(gòu)中用到的搜索算法就叫圖搜索算法[13]。雙向搜索算法是一種圖搜索算法,用于尋找圖中一點到另一點之間的路徑。Ira Pohl在1971年第一次設(shè)計并實現(xiàn)了雙向啟發(fā)式搜索算法[14],該算法同時從起點和終點進(jìn)行搜索,當(dāng)兩者在中間搜索的點重復(fù)時,鏈接兩條路徑并停止搜索,所得到的由兩條路徑拼接后的生成路徑就是雙向搜索算法的最佳路徑[15]。

如圖2所示,假設(shè)我們要查找0到14的路徑,普通搜索流程是從節(jié)點0開始查找,每個節(jié)點查找一次共需要14次,而雙向搜索各自從節(jié)點0和14同時開始查找,在查找到7時就已經(jīng)找到了0到14的節(jié)點路徑。假設(shè)搜索一棵分支因子為b的樹,初始節(jié)點到目標(biāo)節(jié)點的距離為d,該算法的正向和反向搜索復(fù)雜度都是O(bd/2),算法路徑復(fù)雜時,兩者相加遠(yuǎn)遠(yuǎn)小于單向搜索的復(fù)雜度O(bd)[16]。

圖2 雙向搜索原理示意圖

2 算法優(yōu)化與融合

2.1 雙向搜索算法優(yōu)化

從起點到終點的搜索過程是具有不確定性的,即從起點到終點的搜索路徑與從終點到起點的搜索路徑不一定能完全相交,而不相交的情況下兩條路徑都會執(zhí)行至搜索到目標(biāo)點為止,但這并不利于優(yōu)化混合A*算法。本文為了解決路徑不一定相交的問題,運用柵格法劃分地圖,將地圖按一定規(guī)律進(jìn)行行和列的規(guī)則劃分,形成許多有規(guī)律的網(wǎng)格,來方便管理已擴(kuò)展節(jié)點和未擴(kuò)展結(jié)點之間的關(guān)系[17]。當(dāng)正向搜索和反向搜索的子節(jié)點在同一個柵格中時,中斷搜索過程,鏈接反向搜索的父節(jié)點至正向搜索的子節(jié)點,最后輸出整個路徑,這樣就得到了一條由正向搜索和反向搜索組成的最終路徑。優(yōu)化算法前后的最終輸出路徑如圖3所示。

(a) 算法優(yōu)化前 (b) 算法優(yōu)化后圖3 雙向搜索優(yōu)化圖

圖3(a)為算法優(yōu)化前的雙向搜索交互情形,圖3(b)為算法優(yōu)化后的雙向搜索交互情形。圖3(a)描述了原來雙向搜索算法搜索過程中,正向搜索與反向搜索同時進(jìn)行時,即使大致方向一致,正常相遇的情況下,其正向搜索的子節(jié)點與反向搜索的子節(jié)點也不一定會完全重合,其搜索的子節(jié)點不同時在正反向搜索的Openlist表中,則正反向搜索過程都不會中止,最終會形成兩條搜索路徑。利用地圖尺寸與設(shè)置的柵格分辨率劃分柵格,把路徑末端點的實際坐標(biāo)轉(zhuǎn)換為柵格坐標(biāo),公式為

(3)

式中,Xindex、Yindex、θindex為柵格坐標(biāo)的行索引、列索引、朝向編號,x、y、θ為當(dāng)前路徑末端點的橫坐標(biāo)、縱坐標(biāo)、車輛擺角,gres為路徑分辨率,yres為車輛擺角分辨率。當(dāng)正向搜索和反向搜索的路徑末端點的柵格行索引和列索引相同時,只保留正向搜索的子節(jié)點并與反向搜索的父節(jié)點重新執(zhí)行混合A*算法,保留正向搜索的子節(jié)點至Closelist表,搜索成功后合成正向搜索和反向搜索的搜索路徑并輸出曲線。

2.2 算法融合

本文在傳統(tǒng)混合A*算法中融入雙向搜索算法,得到改進(jìn)混合A*搜索算法。其中的正向搜索算法流程圖見圖4。

圖4 改進(jìn)混合A*算法正向搜索流程圖

其反向搜索流程大體與正向一致。判斷D是否為0,若不為0,則進(jìn)入反向搜索流程。經(jīng)過與圖4的正向搜索流程相同的反向搜索,若沒有找到路徑,則重置D為0,再次經(jīng)過判別式,并進(jìn)入正向搜索流程。如此循環(huán)直到輸出最終路徑。

整體代碼設(shè)計流程：預(yù)先設(shè)置地圖形狀、車輛的參數(shù)信息和運動分辨率,再設(shè)置起點和終點的位姿信息(x,y,θ),其中x、y、θ為坐標(biāo)及車輛偏角的真實值;在已知車輛的起始位置、最終停車位置和地圖信息后,開始自動泊車全局路徑的路徑規(guī)劃;初始化柵格和4個列表;計算起始點到終點的歐氏距離,并以此為混合A*算法的啟發(fā)值;正向搜索以車輛目前位置為起始點,最終停車位置為終點(反向搜索相反),選取最小代價值的節(jié)點作為父節(jié)點;利用混合A*算法搜尋鄰近點并每搜尋一次鄰近點就獲得搜尋行為的代價值;判斷父節(jié)點到子節(jié)點的代價值和子節(jié)點到終點歐氏距離的代價值,獲得預(yù)估代價h(n),判斷父節(jié)點到起始點的花費,獲得實際代價g(n),判斷正向搜索和反向搜索是否有相同的擴(kuò)展點;重復(fù)操作直至正反向搜索有相同的擴(kuò)展點,合并列表并輸出曲線信息。

3 實驗與分析

經(jīng)由MATLAB搭建仿真平臺[18],對混合A*雙向搜索算法的時效性、可行性、可靠性進(jìn)行驗證[19]。通過仿真平臺模擬真實車輛的行駛情況,模擬垂直入庫實驗和側(cè)方位停車實驗,從中選出一條符合動力學(xué)邏輯最小代價的路徑并驗證該算法的可靠性和時效性[20],同時對比傳統(tǒng)的混合A*算法,對雙向搜索的混合A*算法的時效性進(jìn)行比較驗證。

這里比較改進(jìn)前后的算法在垂直入庫和側(cè)方位停車時的搜索時長和路徑節(jié)點搜索情況。

3.1 垂直入庫實驗與分析

垂直入庫實驗通過上述仿真平臺,模擬自動泊車中垂直入庫的情況。該實驗只對其路徑規(guī)劃的長度、車輛前進(jìn)后退的頻率、轉(zhuǎn)向的曲率大小作要求,不對車輛的速度、受力做分析[21]。

垂直入庫是大部分停車時要遇到的情況,且通常困難的情況是空出一個車位的同時兩旁的車位都已經(jīng)被占用,這里就仿真該情況下采用改進(jìn)混合A*算法來進(jìn)行路徑規(guī)劃的過程。垂直入庫實驗具體路線如圖5所示。

圖5 傳統(tǒng)混合A*算法路徑曲線圖

分析圖5可知,傳統(tǒng)混合A*算法規(guī)劃的路徑并非實際最優(yōu),還存在轉(zhuǎn)角倒車的情況,雖然符合車輛運動的動力學(xué)要求,但是改變方向的實際代價值很高,因此最終代價值就不一定是最小代價值,最后路徑生成曲線就不一定是最優(yōu)路徑。

更換融合算法后,我們保持相同的環(huán)境再次重復(fù)相同的步驟,并保持相同的起點位姿為(x,y,θ),其中x為車輛矩形框中心點的x坐標(biāo)值,y為車輛矩形框中心點的y坐標(biāo)值,θ為車輛矩形框車頭方向(x的反方向)與x軸的夾角。圖6為更換融合算法后生成的路徑曲線圖,其中圖6(a)是在無阻擋物時生成的路徑曲線圖,6(b)是在設(shè)置了阻擋物后生成的路徑曲線圖。

(a) 融合算法路徑曲線 (b) 改變障礙物生成路徑 圖6 融合算法生成路徑曲線圖

分析圖6(a)可知,在與傳統(tǒng)混合A*算法相同的條件下,本文改進(jìn)的混合A*算法生成路徑曲線降低了車輛改變方向的次數(shù),運行軌跡雖然不平滑,但無停車和倒車的情況,降低了融合算法中實際路徑代價值,且其路徑搜索時間比傳統(tǒng)混合A*算法路徑搜索時間更短。圖6中無障礙與有障礙物兩種情況對比可知,融合算法生成路徑受障礙物影響較大,當(dāng)障礙物產(chǎn)生不同的變動時,其路徑規(guī)劃的時間存在較大的波動,其產(chǎn)生的結(jié)果不利于直觀地比較數(shù)據(jù)間的差異。因此可以改變不同起點位姿,比較改進(jìn)混合A*算法的單向路徑搜索和雙向路徑搜索的搜索時間、改變方向次數(shù)、節(jié)點數(shù),其結(jié)果見表1。

表1 垂直入庫改進(jìn)算法比較

由表1可知,在垂直入庫實驗中,融合算法在路徑搜索時間上比傳統(tǒng)混合A*算法用時平均減少了22.22%,傳統(tǒng)混合A*算法至少改變一次行駛方向,而融合算法則減少了改變方向次數(shù),減少了部分實際路徑代價值的生成,表中雙向搜索節(jié)點數(shù)雖然有部分的減少,但也有未改變的情況,這與設(shè)置的起點和終點位姿有關(guān),設(shè)置的起點位姿不同,生成節(jié)點數(shù)就不固定,單向搜索出個別路徑時,雙向搜索則無法更新最優(yōu)路徑,生成的節(jié)點數(shù)也無法減少。

3.2 側(cè)方位停車實驗與分析

側(cè)方位停車通常是在道路旁停車,與倒車入庫實驗相同,用融合算法和傳統(tǒng)混合A*算法對該情況進(jìn)行路徑規(guī)劃,傳統(tǒng)混合A*算法側(cè)方位停車路徑曲線如圖7所示。

圖7 傳統(tǒng)混合A*算法側(cè)方位停車路徑圖

在該側(cè)方位停車的過程中起點位姿為(-20,20,0),終點位姿為(1,7.5,-π)。由圖7可知,側(cè)方位停車中傳統(tǒng)混合A*算法生成路徑時,路徑存在多次改變行駛方向的問題,且其重復(fù)路徑規(guī)劃過多,無法在狹窄的環(huán)境中準(zhǔn)確生成適合的路徑曲線,實際代價值過大,無法確保該路徑曲線為最優(yōu)路徑。由圖8可知,在側(cè)方位停車實驗中,融合算法能更好地規(guī)劃正確的路徑,減少改變行駛方向的次數(shù),且在狹窄的空間中也能很好地完成路徑規(guī)劃,比傳統(tǒng)混合A*算法生成的路徑實際代價值更低,更接近最優(yōu)路線的標(biāo)準(zhǔn)。

圖8 融合算法側(cè)方位停車路徑圖

更改起始點,比較傳統(tǒng)混合A*算法和融合算法在側(cè)方位停車情況下的搜索路徑時間、改變方向次數(shù)、節(jié)點數(shù),結(jié)果見表2。

表2 側(cè)方位停車改進(jìn)算法比較

由表2可知,在側(cè)方位停車中,融合算法的路徑搜索時間比傳統(tǒng)混合A*算法平均減少了28.20%,減少了改變方向次數(shù),與垂直入庫情況相同的是,當(dāng)單向搜索算法搜索到個別路徑時,雙向搜索算法無法更新最優(yōu)路徑,搜索的路徑節(jié)點數(shù)無法減少。

綜合分析垂直入庫和側(cè)方位停車,融合算法比傳統(tǒng)混合A*算法的搜索時間平均縮短了25.21%。

4 結(jié)語

針對傳統(tǒng)混合A*算法在自動泊車路徑規(guī)劃過程中搜索時間過長的問題,本文提出了融入雙向搜索算法的混合A*算法,設(shè)計了融合方法,制定了算法的具體計算流程。并針對雙向搜索中正向搜索和反向搜索擴(kuò)展節(jié)點不容易重合的問題,引用柵格法優(yōu)化了雙向搜索算法來解決該問題。對比了傳統(tǒng)混合A*算法與融合算法的搜索時間、改變方向次數(shù)、節(jié)點數(shù),結(jié)果表明融合算法不僅比傳統(tǒng)混合A*算法的搜索時間平均縮短了25.21%,而且改變方向次數(shù)普遍比傳統(tǒng)混合A*算法少。說明融合算法最后路徑執(zhí)行代價更小、實時性更好,更能滿足車輛的運動條件。

本文雖然融合了雙向搜索算法,利用柵格法優(yōu)化了雙向搜索時的相遇機(jī)率,但還是存在有雙向搜索無法相遇的情況,導(dǎo)致生成的路徑并非最短路徑,這也是后續(xù)研究需要解決的問題。