基于圖搜索和幾何曲線的自主代客泊車路徑規(guī)劃

柏 鑒,徐元浩，冀 杰，陳云飛，張宇航

(西南大學(xué) 工程技術(shù)學(xué)院， 重慶 400715)

0 引言

自主代客泊車(automated valet parking，AVP)是目前自動(dòng)駕駛汽車技術(shù)中最有可能應(yīng)用落地和商業(yè)化的應(yīng)用之一。路徑規(guī)劃問題是自動(dòng)代客泊車系統(tǒng)所面臨的主要挑戰(zhàn)之一[1]。路徑規(guī)劃算法旨在許多障礙物之間找到一條從初始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑[2]。目前，根據(jù)自主泊車實(shí)現(xiàn)方式的不同，可將廣泛應(yīng)用的路徑規(guī)劃算法分為以下4類：基于圖搜索的算法、基于隨機(jī)采樣的算法、基于幾何曲線插值的算法和數(shù)值優(yōu)化法[3]。

對于圖搜索算法，Yu等[4]定義了基于車輛泊車的停車場拓?fù)涞貓D，采用Dijkstra算法分別得到了每個(gè)停車位到停車場入口的泊車路徑。王永勝等[5]定義了考慮路徑規(guī)劃參數(shù)的停車場拓?fù)涞貓D，并基于該地圖通過A*算法得到了泊車全局路徑。圖搜索算法靈活直觀，但對于先驗(yàn)地圖的依賴較重；而在隨機(jī)采樣算法中， Zheng等[6]在傳統(tǒng)RRT算法基礎(chǔ)上引入車輛運(yùn)動(dòng)學(xué)約束，以獲得垂直、平行、斜列式3種泊車路徑，提高了計(jì)算效率，但所得路徑質(zhì)量的穩(wěn)定性還需進(jìn)一步提升。文獻(xiàn)[7-9]通過幾何曲線優(yōu)化方式，使泊車路徑滿足車輛行駛的曲率連續(xù)要求。此外，Dubins等[10]基于車輛最小轉(zhuǎn)彎半徑，提出了滿足車輛行駛且兩點(diǎn)間最短的路徑曲線，即Dubins曲線。Reeds和Shepp[11]在此基礎(chǔ)上進(jìn)行了拓展，提出了Reeds Shepp(RS)曲線?；趲缀吻€的算法的共同缺點(diǎn)是路徑規(guī)劃需要建立于無障礙環(huán)境，難以直接應(yīng)用于自動(dòng)泊車。還有一些學(xué)者通過數(shù)值優(yōu)化的算法，生成符合車輛行駛要求的無碰撞路徑。錢立軍等[12-13]對車輛行駛時(shí)的多種約束進(jìn)行分析，通過hp偽譜法將路徑規(guī)劃問題轉(zhuǎn)化成非線性規(guī)劃問題進(jìn)行求解，得出自主泊車的最佳路徑。Zhang等[14]通過引入虛擬保護(hù)框架和放大參數(shù)，從幾何角度提出了一種新的碰撞檢測準(zhǔn)則，優(yōu)化了碰撞約束結(jié)構(gòu)。但該類算法計(jì)算繁瑣復(fù)雜，計(jì)算效率不高。Dolgov等[15]提出Hybrid A*算法，該算法以A*算法為基礎(chǔ)結(jié)合車輛非完整約束及RS曲線進(jìn)行拓展，生成的路徑質(zhì)量較好。但Hybrid A*算法在復(fù)雜的環(huán)境表現(xiàn)不佳，不適應(yīng)停車場環(huán)境，易在一些死角處浪費(fèi)大量探索時(shí)間。Jhang等[16]將雙向RRT*算法和RS曲線相結(jié)合，并針對停車場環(huán)境修改了RRT*算法中的擴(kuò)展方式。通過該算法直接獲得車輛從停車場入口到完成泊車的完整路徑。該算法考慮了停車場環(huán)境的限制，但由于RRT*算法的隨機(jī)性質(zhì)，使得規(guī)劃路徑的質(zhì)量和效率均不夠穩(wěn)定。

提出了一種新的自動(dòng)代客泊車路徑規(guī)劃方法，分為全局路徑規(guī)劃和泊車路徑規(guī)劃兩個(gè)部分。全局路徑規(guī)劃算法將廣義維諾圖算法與Hybrid A*算法相結(jié)合，以廣義維諾圖生成的初步路徑作為導(dǎo)向，引導(dǎo)混合A*的拓展，使規(guī)劃的路徑符合車輛行駛，避免了無效的搜索，節(jié)省了時(shí)間。泊車路徑規(guī)劃方面，通過對逆向泊車的碰撞約束，對文獻(xiàn)[16]所提出的C字型泊車算法進(jìn)行了改進(jìn)，擴(kuò)展了泊車起始點(diǎn)的選擇范圍，較固定航向角的泊車算法更為靈活。

1 自動(dòng)泊車路徑規(guī)劃系統(tǒng)框架

以垂直停車位方案為例，得到AVP系統(tǒng)的路徑規(guī)劃方法，如圖1所示。

圖1 路徑規(guī)劃流程框圖

當(dāng)接收到停車命令時(shí)，全局泊車路徑規(guī)劃首先需要找到一條基于廣義維諾圖的行駛路徑。Hybrid A*算法根據(jù)方向引導(dǎo)，生成一條從停車場入口到最佳停車點(diǎn)位置的全局路徑；泊車路徑規(guī)劃采用C字型泊車算法生成停車入位的泊車路徑。將全局路徑和泊車路徑相結(jié)合，得到AVP的最終路徑。

本研究基于阿克曼轉(zhuǎn)向模型完成泊車過程[17]，即在轉(zhuǎn)彎過程中使所有4個(gè)車輪圍繞一個(gè)公共節(jié)點(diǎn)滾動(dòng)而不打滑。公共節(jié)點(diǎn)在圖2中稱為瞬時(shí)曲率中心O。阿克曼轉(zhuǎn)向模型的運(yùn)動(dòng)學(xué)關(guān)系如式(1)所示：

(1)

圖2中，Wd是車輛的寬度；Wx是輪距的長度；Lx是車輛軸距的長度；lf、lr分別為車輛的前懸、后懸；φ1是內(nèi)側(cè)車輪的轉(zhuǎn)向角，φ2而是外側(cè)車輪的轉(zhuǎn)向角。

圖2 阿克曼轉(zhuǎn)向幾何模型示意圖

2 自動(dòng)泊車全局路徑規(guī)劃

2.1 廣義維諾圖

維諾圖由多組不同的兩鄰點(diǎn)的垂直平分線所組成的多邊形構(gòu)成[18]。在移動(dòng)機(jī)器人路徑規(guī)劃領(lǐng)域，使用廣義維諾圖進(jìn)行路徑規(guī)劃可使得路徑規(guī)劃問題減少一個(gè)維度，從而提高計(jì)算效率[19]。

對于自主泊車系統(tǒng)而言，路徑規(guī)劃可看成一個(gè)二維的問題，假設(shè)行駛的汽車為一個(gè)質(zhì)點(diǎn)x，在2維平面中行駛。平面內(nèi)充斥著凸形障礙物c1,c2,…,cn，非凸形障礙物建模為凸集的并集，其總集合為{ci}，且平面的邊界也屬于不可行駛區(qū)域，由一系列凸集組成，是{ci}的子集。則質(zhì)點(diǎn)x離障礙物的距離di(p)可被定義為：

(2)

式中：di(x)的梯度為：

(3)

式中: ▽di(x)為到x的單位方向向量，c0為集合{ci}中距離x最近的一個(gè)點(diǎn)。對于凸集最近點(diǎn)總是唯一且存在的。對于R平面往往有多個(gè)障礙物，因此，可將多障礙物距離函數(shù)定義為：

(4)

廣義維諾圖的基本構(gòu)成單元是與障礙物ci、cj距離相等的點(diǎn)集，且該點(diǎn)集中任何一點(diǎn)與其他障礙物的距離更大。該點(diǎn)集被稱為兩等距面。

(5)

如圖3所示，F(xiàn)ijk是兩等距線Fij、Fik以及Fjk的交點(diǎn)，根據(jù)廣義維諾圖的定義可知該點(diǎn)與障礙物ci、cj、ck的距離相等且唯一。

圖3 廣義維諾圖

基于以上定義，利用式(6)表示廣義維諾圖(GVG)。其中，F(xiàn)2為所有Fij的集合，F(xiàn)3為所有Fijk的集合，在二維平面內(nèi)，F(xiàn)2可視為廣義維諾邊的集合，F(xiàn)3為所有廣義維諾頂點(diǎn)的集合。結(jié)合廣義維諾邊以及廣義維諾頂點(diǎn)，可構(gòu)造針對該地圖的廣義維諾圖。

GVG=(F2,F3)

(6)

對于靜態(tài)地圖而言，利用已構(gòu)造的廣義維諾圖，將初始點(diǎn)和目標(biāo)點(diǎn)與廣義維諾邊匹配可以迅速找到一條粗略的可行駛路徑，如圖4所示。圖中黑色部分為障礙物等不可行駛區(qū)域，灰色實(shí)線為生成的廣義維諾圖，黑色實(shí)線為基于廣義維諾圖所生成的連接目標(biāo)點(diǎn)與起始點(diǎn)的路徑。可以看出，廣義維諾圖將原本二維平面轉(zhuǎn)化成了一維的點(diǎn)線匹配，極大地提高了運(yùn)算效率。

圖4 用于路徑規(guī)劃的廣義維諾圖

將廣義維諾圖用于路徑規(guī)劃最顯著的優(yōu)點(diǎn)是操作復(fù)雜度低、搜索時(shí)間消耗少，同時(shí)，也保證了路徑與障礙物之間的距離。但此種方法算法實(shí)時(shí)性相對較差，由于未考慮車輛的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)約束，該路徑難以直接用于車輛的行駛。

在自主泊車中，可利用廣義維諾圖對停車場地圖進(jìn)行預(yù)處理。即將停車場環(huán)境中所有停車位視為障礙物，從而得到對應(yīng)的廣義維諾圖，用于表示停車場內(nèi)車輛的可行駛區(qū)域?；谕粓鼍跋律傻膹V義維諾圖相同這一性質(zhì)，對于同一停車場，該廣義維諾圖可反復(fù)使用。

2.2 Hybrid A*算法

Hybrid A*算法與傳統(tǒng)A*算法之間最大的區(qū)別在于擴(kuò)展節(jié)點(diǎn)的方式不同。Hybrid A*算法以多種轉(zhuǎn)向操作(左轉(zhuǎn)、右轉(zhuǎn)或不轉(zhuǎn)彎)從父節(jié)點(diǎn)進(jìn)行擴(kuò)展，同時(shí)結(jié)合車輛運(yùn)動(dòng)學(xué)模型確定新的節(jié)點(diǎn)，從而保證子節(jié)點(diǎn)與父節(jié)點(diǎn)之間的運(yùn)動(dòng)連續(xù)性要求，因此，以該種方式所生成的路徑能夠滿足車輛行駛的需求，如圖5所示。

圖5 Hybrid A*拓展節(jié)點(diǎn)方式示意圖

在路徑規(guī)劃中，可使用三維坐標(biāo)(x,y,θ)來描述車輛的位姿。式中，x是橫坐標(biāo)，y是縱坐標(biāo)，θ是車輛的航向角。Hybrid A*算法節(jié)點(diǎn)擴(kuò)展過程如圖6所示，圖中Ni-1與Ni分別代表前一個(gè)節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)，基本原理如式(7)—(11)所示：

圖6 Hybrid A*生成新節(jié)點(diǎn)原理示意圖

(7)

(8)

xi=xi+1+Ri[sin(θi+1+β)-sin(θi-1)]

(9)

yi=xi+1-Ri[cos(θi+1+β)+cos(θi-1)]

(10)

θi=θi-1+β

(11)

式中：Larc為Hybrid A*算法每次擴(kuò)展的步長，α為車輛轉(zhuǎn)向角，在進(jìn)行擴(kuò)展時(shí)，也可直接令Ri為車輛的最小轉(zhuǎn)彎半徑。

Hybrid A*算法實(shí)時(shí)判斷是否使用無碰撞的RS曲線連接當(dāng)前點(diǎn)與目標(biāo)點(diǎn)，如滿足連接條件，則直接使用RS曲線作為路徑輸出。

2.3 結(jié)合廣義維諾圖的Hybrid A*算法

為了解決廣義維諾圖生成的路徑不符合車輛行駛動(dòng)力學(xué)的問題，Hybrid A*算法在路徑生成時(shí)充分考慮了車輛的非完整特性。但是，Hybrid A*算法有時(shí)會在交叉口浪費(fèi)大量時(shí)間，如圖7中圈出的路徑。在停車場中，交叉口會大量存在，這意味著若直接將傳統(tǒng)的Hybrid A*算法應(yīng)用于AVP，會浪費(fèi)大量算力、效率較低。

圖7 Hybrid A*算法規(guī)劃結(jié)果示意圖

為解決該問題，提出了定向的Hybrid A*算法，首先基于停車場場景生成廣義維諾圖，然后通過廣義維諾圖得到通往泊車起始點(diǎn)的大體路徑V-path。該路徑為混合動(dòng)力A*提供正確的方向，以便在考慮車輛非完整約束的情況下快速生成可行駛的路徑。

Hybrid A*算法在選取拓展節(jié)點(diǎn)時(shí)，會計(jì)算周邊可拓展點(diǎn)的成本，如式(12)所示：

F(x)=g(x)+h(x)

(12)

式中：F(x)為該節(jié)點(diǎn)總成本，g(x)為初始節(jié)點(diǎn)到該節(jié)點(diǎn)所花費(fèi)的成本，h(x)為A*算法的啟發(fā)式函數(shù)，表示該節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)成本，一般采用曼哈頓距離進(jìn)行描述。定向Hybrid A*算法計(jì)算拓展成本時(shí)，引入了函數(shù)V(x)，描述該點(diǎn)與V-path之間的關(guān)系，如式(13)所示：

F(x)=kgg(x)+khh(x)+kvV(x)

(13)

式中：kg、kh、kv分別表示不同的權(quán)重系數(shù)。

定向Hybrid A*規(guī)劃結(jié)果如圖8所示。由于V(x)函數(shù)的引入，使得該算法在選取拓展節(jié)點(diǎn)時(shí)，具有了一個(gè)方向上的指引，從而避免了冗余搜索。其次，維諾圖具有良好的避障能力，V(x)函數(shù)的引入使得該算法所規(guī)劃的路徑更傾向于遠(yuǎn)離障礙物，提高了安全性能，解決了傳統(tǒng)路徑規(guī)劃繞墻走的問題。因此在實(shí)際使用中，無須膨脹障礙物，進(jìn)一步提升了算法的計(jì)算效率。

圖8 定向Hybrid A*規(guī)劃結(jié)果示意圖

故定向Hybrid A*算法可以有效地為AVP生成一條可行的全局路徑。全局路徑規(guī)劃算法的具體步驟如下：

1) 獲取基于停車場的廣義維諾圖、車輛初始位姿、泊車目標(biāo)位姿和障礙物位置。

2) 確立泊車坐標(biāo)系，對垂直停車而言，以車輛后軸中心為原點(diǎn)定義坐標(biāo)系，如圖9所示，圖中d為預(yù)留安全距離(即車體與后方障礙物的距離)，車體與車位邊界平行，車輛后軸中心與泊車目標(biāo)點(diǎn)重合。

圖9 泊車位移計(jì)算示意圖

3) 計(jì)算泊車起始點(diǎn)，通過改進(jìn)的基于幾何曲線的泊車路徑規(guī)劃算法(具體內(nèi)容見下節(jié))，計(jì)算出泊車起始點(diǎn)的位姿。

4) 通過廣義維諾圖，找到一條從起始位置到全局路徑目標(biāo)位置的大體路徑(V-path)，為下一步的全局路徑規(guī)劃提供參考方向。

5) 通過定向Hybrid A*算法，生成最終的全局路徑。

該全局路徑規(guī)劃算法不僅考慮了車輛的非完整特性，還避免了冗余搜索，極大提高了路徑規(guī)劃效率。

3 局部泊車路徑規(guī)劃

泊車路徑規(guī)劃可以根據(jù)停車位和車輛輪廓等參數(shù)，為指定的停車位生成停車路徑。選用停車場常見的垂直停車場景進(jìn)行局部泊車路徑規(guī)劃。C字型泊車規(guī)劃能夠使車輛實(shí)現(xiàn)垂直泊車入位的目標(biāo)[16]。然而，在大多數(shù)的研究中，此類泊車算法中的每個(gè)停車位的停車起始節(jié)點(diǎn)、泊車路線相對固定，并且停車起始航向角必須為零，此外，汽車距車位邊界的臨界側(cè)向距離h需大于hmin。因此，難以適用于一些復(fù)雜的泊車場景?；跓o碰撞約束對C型泊車規(guī)劃加以優(yōu)化，使得每個(gè)泊車位有多條泊車路線以及多個(gè)泊車起始點(diǎn)，能夠擴(kuò)大泊車起始點(diǎn)的選擇范圍。

(14)

式中：Rmin為小轉(zhuǎn)彎半徑。

如圖10所示，C型泊車規(guī)劃可分為兩段：方向盤右轉(zhuǎn)車輛從M1倒退至M2點(diǎn)；最終方向盤保持平衡車輛從M2倒退至M3點(diǎn)即泊車結(jié)束點(diǎn)?？蓪⑵湟话慊囕v先沿著一段圓弧倒退行駛一段距離，再沿著垂直方向倒退一段距離最終達(dá)到泊車位置?；谏衔乃⒌淖鴺?biāo)系，設(shè)泊車過程中車輛的豎直和水平位移分別為W、H：

圖10 C型泊車算法示意圖

H=R(1-sinθ)

(15)

W=Rcosθ+S

(16)

式中：R為泊車過程中車輛轉(zhuǎn)彎半徑，S為車輛最后一段垂直倒車的距離，θ為泊車起始時(shí)的航向角，當(dāng)θ=0時(shí)，此時(shí)路徑為傳統(tǒng)C型規(guī)劃。

得到W、H的數(shù)值后，結(jié)合已知的泊車位置等信息便可得到泊車起始位姿，再通過C型泊車規(guī)劃得到相應(yīng)泊車路徑。由于左、右兩側(cè)的停車過程是對稱的，此處只討論從停車位右側(cè)停車的情況。

為了得到最優(yōu)的泊車起始點(diǎn)，需得到泊車起始點(diǎn)的選取范圍。根據(jù)式(15)和(16)，參數(shù)R、θ、S決定了W、H的值，而W、H的值直接決定了泊車起始點(diǎn)的位置。因此，需確定參數(shù)R、θ、S的范圍。

對于泊車起始點(diǎn)航向角θ來說，其最大值為π/2，即車輛與停車位平行時(shí)。當(dāng)θ取最小值時(shí)，車輛相對停車位的位置如圖11所示。此時(shí)，θmin滿足：

圖11 最小起始航向角示意圖

(17)

RRF×sin(θi-θmin)=R

(18)

(19)

整理可得：

(20)

R、S均屬于泊車路徑中的幾何參數(shù)，理論上車輛的泊車軌跡和車輛逆向泊車軌跡保持一致。因此，可通過定義逆向泊車過程中的無碰撞約束從而確定其取值范圍。

逆向停車過程中可能發(fā)生的碰撞情況包括：

1) 車輛駛出停車位時(shí)，車輛右側(cè)邊線可能和邊界相撞，如圖12所示。為避免碰撞發(fā)生，應(yīng)滿足式(21)中的條件：

圖12 車輛右側(cè)的碰撞示意圖

(21)

2)車輛駛出停車位時(shí)，車輛左后側(cè)可能和邊界相撞，如圖13所示。為避免碰撞發(fā)生，應(yīng)滿足式(22)中的條件：

圖13 車輛左后角的碰撞示意圖

(22)

3) 車輛駛出停車位時(shí)，車輛左前側(cè)可能和邊界相撞，如圖14所示。為避免碰撞發(fā)生，式(25)、(26)應(yīng)被滿足：

圖14 車輛左前角的碰撞示意圖

(23)

(24)

(25)

RLF

(26)

4) 車輛駛出停車位時(shí)，車輛右前側(cè)可能和邊界相撞，如圖15所示。為避免碰撞發(fā)生，應(yīng)滿足式(27)(28)中的條件：

圖15 車輛右前角的碰撞示意圖

L-d-lr

(27)

(28)

在整個(gè)逆向泊車的過程中，一共有上述4種潛在碰撞情況，可建立C型泊車規(guī)劃算法的完整約束方程，如式(29)所示：

(29)

通過該方程可確定參數(shù)R、S、θ的范圍，即泊車起始點(diǎn)的范圍。為得到最佳的泊車起始點(diǎn)，引入評價(jià)函數(shù)f(x)：

f(x)=S+θ+R

(30)

取可行域內(nèi)f(x)計(jì)算結(jié)果最小值為最佳參數(shù)組合。根據(jù)式(15)、(16)即可得到對應(yīng)泊車過程中的水平和豎直位移，進(jìn)而得到泊車起始點(diǎn)。

4 仿真實(shí)驗(yàn)分析

4.1 算法效率比較

建立模擬停車場結(jié)構(gòu)的柵格地圖，并以同一起始點(diǎn)，3種不同的終點(diǎn)位姿作為輸入，分別對所提出的全局規(guī)劃算法和Hybrid A*算法進(jìn)行仿真和比較。為進(jìn)一步體現(xiàn)算法性能，仿真過程中障礙物均未做膨脹處理，使用的處理器為Intel Core i5-10400 CPU 2.90 GHz。

仿真結(jié)果如圖16、表1所示。圖16中，菱形為起始點(diǎn)，圓點(diǎn)為目標(biāo)點(diǎn)，黑色粗實(shí)線為規(guī)劃路徑。細(xì)實(shí)線和虛線分別代表算法向前、向后探索的過程。第一行的三幅圖分別為傳統(tǒng)Hybrid A*算法在場景1、場景2、場景3的規(guī)劃結(jié)果；第二行的三幅圖分別為定向Hybrid A*算法在場景1、場景2、場景3的規(guī)劃結(jié)果。

表1 算法仿真結(jié)果對比

圖16 算法仿真結(jié)果

具體的試驗(yàn)數(shù)據(jù)見表1，在場景1中，改進(jìn)算法用時(shí)2.3 s左右，而傳統(tǒng)Hybrid A*算法用時(shí)3.7 s左右；場景2中改進(jìn)算法用時(shí)3.8 s左右，而傳統(tǒng)算法用時(shí)6.2 s左右；在場景3中改進(jìn)算法用時(shí)6.8 s左右，傳統(tǒng)算法用時(shí)9.12 s左右。在運(yùn)行時(shí)間方面，改進(jìn)算法所用的時(shí)間均小于傳統(tǒng)的Hybrid A*算法，且隨著環(huán)境的復(fù)雜，其優(yōu)勢越明顯。在拓展節(jié)點(diǎn)方面，改進(jìn)算法的節(jié)點(diǎn)遠(yuǎn)遠(yuǎn)少于Hybrid A*的節(jié)點(diǎn)數(shù)量。

結(jié)果顯示，在效率方面，即使場景1中傳統(tǒng)Hybrid A*沒有交叉口的冗余探索，改進(jìn)算法的仿真時(shí)間和節(jié)點(diǎn)數(shù)仍小于傳統(tǒng)Hybrid A*算法；在場景2、場景3中改進(jìn)算法避免了傳統(tǒng)Hybrid A*算法在交叉口的冗余探索，改進(jìn)算法的仿真時(shí)間和節(jié)點(diǎn)數(shù)均小于傳統(tǒng)Hybrid A*算法。在路徑質(zhì)量方面，改進(jìn)算法所規(guī)劃的路徑在路徑前半段存在一些曲折的現(xiàn)象，原因在于所規(guī)劃廣義維諾圖在三方障礙物交界時(shí)，多為Y型，對拓展的引導(dǎo)造成了干擾。且傳統(tǒng)Hybrid A*算法所規(guī)劃的路徑十分貼近未經(jīng)膨脹的障礙物，而定向Hybrid A*算法所規(guī)劃的路徑具有顯著的遠(yuǎn)離障礙物的特性。

因此,相對于傳統(tǒng)Hybrid A*算法，改進(jìn)算法在規(guī)劃效率以及避障性能方面有著不小的提升。

4.2 仿真試驗(yàn)結(jié)果

為了驗(yàn)證該路徑規(guī)劃方法的可行性，以實(shí)際停車場的測量數(shù)據(jù)為依據(jù)，建立柵格地圖并進(jìn)行仿真。車輛參數(shù)見表2，停車場地見圖17。

表2 仿真參數(shù)

圖17 停車場地

圖18中的4張圖分別為第1個(gè)泊車位的全局路徑、泊車路徑及其車輛輪廓圖。圖中黑色曲線是所得的泊車路徑，黑色方框表示車輛輪廓線。圖19為第2個(gè)泊車位對應(yīng)的路徑規(guī)劃結(jié)果。

圖18 泊車場景1規(guī)劃路徑

圖19 泊車場景2規(guī)劃路徑

在路徑曲率方面，圖20(a)為場景1全局路徑的路徑曲率曲線；圖20(b)為場景2全局路徑的路徑曲率曲線?？梢娫谡麠l路徑中，曲率不存在階躍式的突變，路徑的曲率連續(xù)，第1個(gè)場景中路徑最大曲率為0.097 m-1；第2個(gè)場景中路徑最大曲率為0.107 3 m-1，滿足車輛的行駛要求。泊車路徑均為圓弧直線的組合，圓弧上曲率不變，故不做比較。在避障方面，場景1全局路徑中車輛輪廓與障礙物的最短距離為0.73 m；泊車路徑中車輛輪廓與停車位車道線的最短距離為0.45 m。場景2全局路徑中車輛輪廓與障礙物的最短距離為1.34 m；泊車路徑中車輛輪廓與停車位車道線的最短距離為 0.15 m?？梢姡撍惴ǔ晒σ?guī)劃了對應(yīng)泊車位的無碰撞全局路徑和泊車路徑，保證了車輛的安全行駛。

圖20 不同場景下全局路徑的曲率

通過以上仿真結(jié)果可以發(fā)現(xiàn)，該方法在停車場一類的環(huán)境下，克服了傳統(tǒng)Hybrid A*算法容易過度冗余搜索的問題；同時(shí)在障礙物未經(jīng)膨脹處理的情況下，所規(guī)劃的路徑仍然能保持一定的安全距離。因此該方法具有計(jì)算效率高、避障性好等優(yōu)點(diǎn)，在停車場場景中能夠得到較好的應(yīng)用。

為了驗(yàn)證規(guī)劃路徑的可行性，對所規(guī)劃的路徑采用了基于Matlab/Simulink和CarSim的聯(lián)合仿真。在路徑跟蹤方面，采用基于五次多項(xiàng)式的方式生成速度曲線，縱向控制采用了PI控制，橫向控制采用了Sanley控制器[20]。結(jié)果如圖21所示，其中，圖21(a)為路徑跟蹤時(shí)的實(shí)際軌跡與規(guī)劃路徑，實(shí)線表示路徑跟蹤車輛的實(shí)際軌跡，虛線是規(guī)劃路徑；圖21(b)為路徑跟蹤時(shí)實(shí)際的車輛輪廓圖，用來顯示路徑跟蹤過程中的車輛姿態(tài)，判斷是否發(fā)生碰撞。

圖21 路徑跟蹤仿真實(shí)驗(yàn)結(jié)果

路徑規(guī)劃和路徑跟蹤聯(lián)合仿真的結(jié)果表明，提出的路徑規(guī)劃方法能夠生成一條無碰撞路徑，并能夠?qū)崿F(xiàn)自動(dòng)泊車目標(biāo)。

4.3 實(shí)車試驗(yàn)結(jié)果

為進(jìn)一步驗(yàn)證所設(shè)計(jì)路徑規(guī)劃方法的可行性，以圖22所示的符合阿克曼轉(zhuǎn)向幾何模型的無人駕駛移動(dòng)平臺為試驗(yàn)樣車進(jìn)行了實(shí)車試驗(yàn)。

圖22 實(shí)車試驗(yàn)平臺

試驗(yàn)結(jié)果如圖23所示。其中紅色曲線為該方法所規(guī)劃的自主泊車路徑，藍(lán)色的線為司南導(dǎo)航系統(tǒng)輸出的GPS數(shù)據(jù)即車輛軌跡。

圖23 實(shí)車跟蹤軌跡

結(jié)果顯示，所規(guī)劃的路徑有較好的可跟蹤性能，使得車輛能夠成功地泊進(jìn)車位。

針對試驗(yàn)數(shù)據(jù)進(jìn)行分析，圖24為規(guī)劃路徑的曲率。在規(guī)劃路徑中，路徑點(diǎn)1～88為全局路徑，88～100為泊車路徑?？梢娫诼窂角史矫?，整條路徑中，曲率僅有一些輕微抖動(dòng)，這是由于路徑取點(diǎn)存在間隔以及未經(jīng)平滑處理導(dǎo)致的，但曲率不存在階躍式的突變，足以滿足車輛的行駛需求。

圖24 規(guī)劃路徑曲率

圖25為車輛自主泊車過程中當(dāng)前位置與最近規(guī)劃路徑點(diǎn)的航向角誤差與橫向誤差。從圖25(a)中可以看出，在自主泊車過程中航向角誤差不超過±0.2 rad，完成泊車時(shí)，航向角誤差為 -0.005 4 rad；從圖25(b)中可以看出，在路徑跟蹤過程中橫向誤差不超過±0.23 m，在泊車過程中橫向誤差不超過0.102 m。

圖25 泊車過程航向角誤差與橫向誤差

造成航向角和橫向誤差在一定程度上振蕩的主要原因有：試驗(yàn)車輛部分動(dòng)力學(xué)參數(shù)存在不確定性，試驗(yàn)時(shí)采用簡單的幾何運(yùn)動(dòng)學(xué)模型，同時(shí)控制算法精度不夠?；谝陨蠠o法避免的影響因素，其自主泊車過程跟蹤誤差仍在合理范圍內(nèi)，且該規(guī)劃路徑能夠?qū)崿F(xiàn)自主泊車的目標(biāo)，同時(shí)具有較好的可跟蹤性，進(jìn)而證明了所設(shè)計(jì)的自主泊車路徑規(guī)劃算法在真實(shí)停車場的有效性。

5 結(jié)論

1) 提出了一種新的自動(dòng)泊車路徑規(guī)劃算法，即基于廣義維諾圖的Hybrid A*算法。仿真結(jié)果表明，該算法比傳統(tǒng)的Hybrid A*算法具有效率更高、避障性更好等優(yōu)點(diǎn)。該算法所規(guī)劃的路徑前段可能存在一些不必要的曲折，需要對路徑優(yōu)化做進(jìn)一步研究。

2) 通過對幾何曲線的碰撞約束進(jìn)行分析，改進(jìn)了C型泊車路徑規(guī)劃算法，使停車起始節(jié)點(diǎn)選擇范圍更廣，提高了適用性。其他類型的停車幾何曲線也可以用同樣的方法改進(jìn)。

3) 在同一場景下，對提出的算法和Hybrid A*算法進(jìn)行了對比分析，證明了該算法的優(yōu)勢。再通過聯(lián)合仿真及實(shí)車試驗(yàn)的方式表明所得路徑具有良好的可跟蹤性，驗(yàn)證了該算法在真實(shí)停車場的有效性。