基于庫(kù)位跟蹤的自動(dòng)泊車決策規(guī)劃系統(tǒng)*

熊璐 嚴(yán)森煒 余卓平 張培志

（同濟(jì)大學(xué)，上海 201804）

主題詞：自動(dòng)泊車 庫(kù)位跟蹤 多次規(guī)劃 動(dòng)態(tài)調(diào)整 卡爾曼濾波

1 前言

自動(dòng)泊車技術(shù)已經(jīng)問(wèn)世多年，但目前仍不能較好地解決如下問(wèn)題：對(duì)不同庫(kù)位環(huán)境和初始位姿的普適性、算法良好的實(shí)時(shí)性、算法的可靠性和可控性、最后停車的理想位姿。自動(dòng)泊車算法主要分為4種：一是基于路徑規(guī)劃的控制算法[1-6]，即根據(jù)環(huán)境約束規(guī)劃幾何曲線（包括搜索采樣算法），然后通過(guò)路徑跟蹤和底盤控制決定車輛的動(dòng)作，優(yōu)點(diǎn)是規(guī)劃運(yùn)算耗時(shí)較短、規(guī)劃路徑基本可控，缺點(diǎn)是路徑跟蹤和底盤控制存在誤差，導(dǎo)致最后泊車位姿不理想；二是基于經(jīng)驗(yàn)的控制算法[7-11]，即利用模糊邏輯或神經(jīng)網(wǎng)絡(luò)，根據(jù)實(shí)時(shí)的環(huán)境輸入得到航向角和車速的控制量，優(yōu)點(diǎn)是閉環(huán)控制，缺點(diǎn)是控制參數(shù)難以確定、泊車路線不可控、實(shí)車應(yīng)用效果不佳；三是基于數(shù)值優(yōu)化的算法[12-13]，通過(guò)建立系統(tǒng)運(yùn)動(dòng)學(xué)方程和結(jié)合參數(shù)約束進(jìn)行路徑最優(yōu)化求解，優(yōu)點(diǎn)是對(duì)不同環(huán)境適用性廣，缺點(diǎn)是計(jì)算量大、實(shí)時(shí)性差；四是基于學(xué)習(xí)的算法[14-16]，利用行為克隆或強(qiáng)化學(xué)習(xí)進(jìn)行從環(huán)境到車輛縱、側(cè)向控制的映射，優(yōu)點(diǎn)是可以將各模塊的誤差直接融合在算法中消除，缺點(diǎn)是學(xué)習(xí)算法訓(xùn)練難以收斂，容易陷入局部最優(yōu)值，泛化能力一般。

本文基于幾何規(guī)劃，結(jié)合Reeds-Shepp曲線路徑最短[2]和螺旋線適用性廣的優(yōu)點(diǎn)設(shè)計(jì)了對(duì)自車初始位姿無(wú)要求的自動(dòng)泊車決策規(guī)劃算法。通過(guò)融合環(huán)視庫(kù)位坐標(biāo)信息和車輛底盤運(yùn)動(dòng)學(xué)信息對(duì)目標(biāo)庫(kù)位進(jìn)行跟蹤，同時(shí)對(duì)航位推算結(jié)果進(jìn)行修正。對(duì)于路徑跟蹤和底盤執(zhí)行器控制帶來(lái)的誤差，通過(guò)庫(kù)位跟蹤和多次規(guī)劃的方法予以減小。此外，設(shè)計(jì)了多次規(guī)劃和動(dòng)態(tài)調(diào)整算法解決泊車過(guò)程中由于空間狹窄難以入庫(kù)的問(wèn)題。本文設(shè)計(jì)的算法對(duì)各類庫(kù)位的適應(yīng)性強(qiáng)、實(shí)時(shí)性好、可靠性強(qiáng)，并且大幅改善了最后泊車姿態(tài)的理想程度。上述算法通過(guò)仿真和實(shí)車試驗(yàn)進(jìn)行了驗(yàn)證。

2 方案設(shè)計(jì)

2.1 自動(dòng)泊車系統(tǒng)架構(gòu)

自動(dòng)泊車系統(tǒng)架構(gòu)如圖1所示。環(huán)境感知模塊利用環(huán)視相機(jī)給出庫(kù)位坐標(biāo)，結(jié)合超聲波雷達(dá)檢測(cè)障礙物，將探測(cè)到的環(huán)境信息送到?jīng)Q策規(guī)劃模塊用以決策和規(guī)劃泊車路徑。運(yùn)動(dòng)控制模塊中，底盤執(zhí)行機(jī)構(gòu)包括電動(dòng)助力轉(zhuǎn)向系統(tǒng)（EPS）以及驅(qū)動(dòng)、制動(dòng)執(zhí)行器。

圖1 自動(dòng)泊車系統(tǒng)架構(gòu)

2.2 決策規(guī)劃系統(tǒng)流程

自動(dòng)泊車系統(tǒng)的目標(biāo)功能為：駕駛員將車輛駛?cè)胪＼噲?chǎng)或沿停車位行駛時(shí)，啟動(dòng)自動(dòng)泊車功能后，自車接管車輛控制，前進(jìn)過(guò)程中自動(dòng)判別庫(kù)位類型，實(shí)現(xiàn)自動(dòng)泊車。在泊車過(guò)程中，自車通過(guò)多次更新庫(kù)位坐標(biāo)減小庫(kù)位坐標(biāo)檢測(cè)誤差和航位推算累積誤差，使泊車姿態(tài)更理想。對(duì)于平行泊車，設(shè)計(jì)了多次規(guī)劃模塊，即庫(kù)位長(zhǎng)度無(wú)法滿足一次倒車入庫(kù)時(shí)，自動(dòng)增加調(diào)整次數(shù)，直至能夠泊入車位，理論上只需車位長(zhǎng)度大于車長(zhǎng)即可入庫(kù)；對(duì)于垂直泊車，停車通道寬度不足時(shí)，進(jìn)行動(dòng)態(tài)調(diào)整，使泊車規(guī)劃不受障礙物的限制。簡(jiǎn)要流程如圖2所示。

圖2 決策規(guī)劃系統(tǒng)流程

2.3 車輛模型

本文使用阿克曼轉(zhuǎn)向性質(zhì)的車輛模型。將車輛視為平面剛體，具有3個(gè)自由度，選取后軸中心作為參考點(diǎn)，車輛在世界坐標(biāo)系下的坐標(biāo)為(x,y)，航向角為φ，則車輛位置唯一確定，如圖3所示。圖3中，γ為轉(zhuǎn)向輪轉(zhuǎn)角，r、κ分別為車輛轉(zhuǎn)彎半徑和曲率，L為軸距。車輛的狀態(tài)方程為：

式中，v為車速。

圖3 阿克曼轉(zhuǎn)向車輛模型

3 基礎(chǔ)路徑規(guī)劃

3.1 平行泊車

自車起始時(shí)（首次獲得環(huán)視系統(tǒng)得到的庫(kù)位角點(diǎn)P0、P1時(shí)，庫(kù)位寬度通過(guò)超聲波雷達(dá)測(cè)算或按國(guó)標(biāo)推算），后軸中點(diǎn)位于S點(diǎn)，將S點(diǎn)設(shè)為規(guī)劃的原點(diǎn)，建立的坐標(biāo)系和基礎(chǔ)路徑規(guī)劃結(jié)果如圖4所示。車輛沿X軸方向前進(jìn)，到達(dá)某點(diǎn)A時(shí)，以最小轉(zhuǎn)彎半徑Rmin倒車，再以為半徑反向倒車，后軸中心點(diǎn)軌跡分別為和BC，此時(shí)車身與庫(kù)位平行，然后前進(jìn)至庫(kù)位中心位置。

圖4 平行泊車基礎(chǔ)路徑規(guī)劃示意

C點(diǎn)的坐標(biāo)為：

初始位置自車縱軸線與庫(kù)位外側(cè)連線的夾角為：

第2段、第1段圓弧的圓心O2、O1的坐標(biāo)分別為：

規(guī)劃輔助計(jì)算角β1和β2分別為：

中間過(guò)渡點(diǎn)B的X坐標(biāo)為：

式中，Lr為車輛后軸與后端的距離；Lw為輪距；Lsafe1、Lsafe2分別為車輛縱向、橫向安全距離；Lpark為庫(kù)位寬度（P0與P1的距離）。

為了保證車輛不與庫(kù)位發(fā)生碰撞，車輛在2段圓弧上行駛時(shí)均不能發(fā)生碰撞，如圖5所示。通過(guò)幾何分析，第1段、第2段圓弧不碰撞的判斷條件分別為：

其中，O1與K、P1及O2與I、P2的距離分別為：

式中，Lf為車輛后軸與前端的距離。

圖5 2段圓弧不發(fā)生碰撞的判斷條件

實(shí)際泊車過(guò)程中，庫(kù)位長(zhǎng)度可能不滿足一次規(guī)劃的要求，即式（8）、式（9）不成立。參考人類駕駛員的泊車經(jīng)驗(yàn)，本文提出多次規(guī)劃算法，即降低一次規(guī)劃的目標(biāo)，使車身部分停入庫(kù)位，再通過(guò)不斷規(guī)劃調(diào)整，最終停入庫(kù)位中心。如圖6，一次規(guī)劃計(jì)算發(fā)現(xiàn)會(huì)發(fā)生碰撞，因此逐漸增加目標(biāo)終點(diǎn)從庫(kù)位中心向外側(cè)的偏移值，直至能夠滿足無(wú)碰撞條件。當(dāng)行駛完一次規(guī)劃路徑后，根據(jù)與庫(kù)位的相對(duì)位置再次規(guī)劃路徑S'A'B'C'D'，直至自車停在庫(kù)位中心。庫(kù)位長(zhǎng)度越短，單次調(diào)整縱向距離受限，因此橫向距離變化也越小，所需調(diào)整次數(shù)越多。

圖6 平行泊車多次規(guī)劃示意

3.2 垂直泊車

自車起始時(shí)（首次獲得環(huán)視系統(tǒng)得到的庫(kù)位角點(diǎn)P0、P1坐標(biāo)時(shí)），后軸中點(diǎn)位于S點(diǎn)，將S點(diǎn)設(shè)為規(guī)劃原點(diǎn)，如圖7所示。車輛沿X軸方向前進(jìn)，到達(dá)某點(diǎn)A時(shí)，以Rmin為半徑倒車，車輛后軸中心點(diǎn)軌跡為AB。

圖7 垂直泊車A點(diǎn)計(jì)算示意

θ的計(jì)算見(jiàn)式（3），O1位于直線：

幾何推導(dǎo)易證，車輛外輪廓中，右后輪K處的軌跡與P1點(diǎn)距離最近，因而將安全距離Lsafe3定義為K與P1間的距離。因此O1同時(shí)滿足：

由式（12）和式（13）聯(lián)立解得O1的X坐標(biāo)（即A的X坐標(biāo)）為：

特殊地，當(dāng)θ=0°時(shí)：

若需完成一次倒車入庫(kù)（見(jiàn)圖8），則由A到B的行駛過(guò)程中，某時(shí)刻自車縱軸線與車庫(kù)縱軸線達(dá)到平行且過(guò)程中一直滿足：

式中，Lsafe4=0.1 m為安全距離，可設(shè)與泊車車速正相關(guān)。

圖8 垂直泊車一次規(guī)劃入庫(kù)示意

反之，則需多次規(guī)劃入庫(kù)。當(dāng)式（16）取等號(hào)時(shí)停車，車輛后軸中心位于點(diǎn)B，根據(jù)此時(shí)自車相對(duì)庫(kù)位的位姿規(guī)劃后續(xù)路徑。為便于表述，將坐標(biāo)系原點(diǎn)重設(shè)為泊車目標(biāo)點(diǎn)E，如圖9所示。E點(diǎn)的X坐標(biāo)為：

圖9 垂直泊車基礎(chǔ)路徑規(guī)劃示意

從B點(diǎn)開(kāi)始以Rmin為半徑車輛前進(jìn)，車輛后軸中心點(diǎn)軌跡為，可推得其圓心O2的X'坐標(biāo)為：

此后車輛以Rmin為半徑沿倒車，最終使車輛縱軸線與車庫(kù)縱軸線重合，可推得的圓心的X'坐標(biāo)為：

則有車輛從點(diǎn)B以Rmin前進(jìn)至點(diǎn)C及從點(diǎn)C以Rmin倒車至點(diǎn)D的停止判別條件為航向角分別達(dá)到：

式中，ψB、ψC、ψD分別為B、C、D點(diǎn)處自車航向角。

在垂直泊車工況中，常遇到泊車通道寬度不足的問(wèn)題，如較窄的停車通道或泊車規(guī)劃路徑上存在墻或其他車輛，導(dǎo)致自車無(wú)法按照原規(guī)劃路徑行駛，此時(shí)需要進(jìn)行如圖10所示的動(dòng)態(tài)調(diào)整。自車按原規(guī)劃路徑行駛過(guò)程中，車前方超聲波雷達(dá)探測(cè)到障礙物距離小于某一閾值，則進(jìn)行動(dòng)態(tài)調(diào)整，將轉(zhuǎn)向盤轉(zhuǎn)至右極限位置，路徑為，到達(dá)B2點(diǎn)后按的規(guī)劃方法，規(guī)劃和，按規(guī)劃路徑泊車入庫(kù)。若仍檢測(cè)到障礙物距離小于安全閾值，則重復(fù)動(dòng)態(tài)調(diào)整，直至順利入庫(kù)。

圖10 垂直泊車動(dòng)態(tài)調(diào)整示意

4 垂直泊車規(guī)劃算法優(yōu)化

在實(shí)車試驗(yàn)中發(fā)現(xiàn)，由于垂直泊車的側(cè)向空間小，對(duì)路徑跟蹤、底盤執(zhí)行器控制和航位推算誤差敏感，因此考慮在C點(diǎn)增加一次庫(kù)位跟蹤規(guī)劃，即在C點(diǎn)根據(jù)自車和庫(kù)位的相對(duì)位姿進(jìn)行重新規(guī)劃。

為了繼承基礎(chǔ)路徑規(guī)劃的思路，將CD段（此時(shí)基礎(chǔ)路徑規(guī)劃的BC段和原CD段圓弧半徑稍大于Rmin）路徑調(diào)整為3段曲線，如圖11所示，中間段仍采用圓弧曲線，前后兩段采用二次螺旋線便于位姿調(diào)整。二次螺旋線的表達(dá)式為：

該曲線對(duì)應(yīng)的車輛航向角為：

圖11 改進(jìn)后的垂直泊車路徑

設(shè)第1段螺旋線的長(zhǎng)度為s1，終點(diǎn)處的車輛位姿為X1=(X1,Y1,θ1)，中間段圓弧的圓心坐標(biāo)是(XR,YR)，圓弧結(jié)束點(diǎn)處車輛位姿為X2=(X2,Y2,θ2)，第2段螺旋線的長(zhǎng)度為s2。設(shè)兩組螺旋線方程的未知參數(shù)分別為(a1,b1,c1)和(a2,b2,c2)。初始點(diǎn)位姿為X0=(X0,Y0,θ0)=(Xi,Yi,θi)，終點(diǎn)位姿為X3=(X3,Y3,θ3)=(Xg,Yg,θg)，κ0、κ3分別為起點(diǎn)和終點(diǎn)的曲率。根據(jù)邊界條件有：

式（24）有(a1,b1,c1,s1)、(a2,b2,c2,s2)、(X1,Y1,θ1)、(X2,Y2,θ2)共14個(gè)未知量，設(shè)中間圓弧段的半徑為Rmin，用圓心坐標(biāo)(XR,YR)和(θ1,θ2)表示(X1,Y1)和(X2,Y2)：

結(jié)合式（24）和式（25），借助MIDACO優(yōu)化工具進(jìn)行求解，設(shè)置優(yōu)化條件為曲線終點(diǎn)與目標(biāo)點(diǎn)歐式距離最短和整個(gè)路徑的長(zhǎng)度最短，同時(shí)滿足環(huán)境約束。另外，需要滿足曲率約束條件：

式中，κmax為最大曲率。

式（24）中第4、5、9、10個(gè)方程為非線性方程，為了簡(jiǎn)化計(jì)算，可借助辛普森公式計(jì)算三角函數(shù)值：

式中，sg為積分區(qū)間的右邊界。

考慮到初值的選擇對(duì)于MIDACO求解有影響，因此考慮用成功生成的算例來(lái)訓(xùn)練神經(jīng)網(wǎng)絡(luò)，作為MIDACO求解的初值。流程如下：首先已知(Xi,Yi,θi,ki)和(Xg,Yg,θg,kg)，通過(guò)MIDACO求解待求參數(shù)a1、s1、a2、s2（在現(xiàn)有設(shè)備上的平均計(jì)算周期為1.2 s），用求解參數(shù)和初始條件生成曲線，二次驗(yàn)證篩選掉可能發(fā)生碰撞的結(jié)果，使用篩選后的數(shù)據(jù)集訓(xùn)練BP神經(jīng)網(wǎng)絡(luò)，含5層隱藏層，神經(jīng)元數(shù)分別為200、100、50、20、10，激活函數(shù)選擇Leaky ReLU，輸入、輸出均歸一化參與訓(xùn)練，選擇Mini-Batch和AdaGrad作為優(yōu)化方法。最終訓(xùn)練結(jié)果中4個(gè)參數(shù)的平均誤差約10%。

使用訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)輸出的4個(gè)參數(shù)作為MIDACO求解的初值，平均計(jì)算周期降低為0.8 s。圖12所示為選取的一定區(qū)域內(nèi)，初始航向角30°～90°（為了顯示清晰每5°采樣一次）的路徑庫(kù)。

圖12 改進(jìn)的規(guī)劃路徑庫(kù)（選取部分離散初始條件）

在實(shí)車試驗(yàn)中若出現(xiàn)MIDACO求解的結(jié)果不符合要求的情況，可微調(diào)目標(biāo)點(diǎn)(Xg,Yg,θg,kg)再次運(yùn)行MIDACO求解。若多次微調(diào)目標(biāo)點(diǎn)的結(jié)果均不符合要求，則進(jìn)行自車位姿微調(diào)后根據(jù)新的相對(duì)位姿進(jìn)行求解。

5 庫(kù)位跟蹤

因傳統(tǒng)自動(dòng)泊車規(guī)劃算法采用航位推算來(lái)估計(jì)自車的位姿，其使用的慣性導(dǎo)航系統(tǒng)或車輛底盤信息存在噪聲，同時(shí)，泊車工況的前進(jìn)、倒車切換頻繁，導(dǎo)致實(shí)際泊車中的航位推算累積誤差達(dá)到了分米級(jí)以上，這對(duì)于泊車工況來(lái)說(shuō)是難以接受的。環(huán)視系統(tǒng)的標(biāo)定、拼接、光照、陰影、遮擋等具有隨機(jī)性，此外，在檢測(cè)庫(kù)位時(shí)車輛非靜止而影像系統(tǒng)幀數(shù)有限，導(dǎo)致測(cè)量誤差的存在。

將航位推算信息和環(huán)視信息融合能夠有效地解決上述問(wèn)題。因方程非線性，采用拓展卡爾曼濾波算法。世界坐標(biāo)系XwOwYw、車輛坐標(biāo)系XvOvYv和狀態(tài)量的定義如圖13所示。

圖13 卡爾曼濾波的坐標(biāo)系和狀態(tài)量定義

系統(tǒng)運(yùn)動(dòng)模型為：

式中，(x,y,φ)為自車相對(duì)于世界坐標(biāo)系的橫、縱坐標(biāo)和航向角；(x1,y1)為相對(duì)于世界坐標(biāo)系的庫(kù)位角點(diǎn)坐標(biāo)；V為自車車速；w(k)=(wx(k),wy(k),wφ(k),0,0)T為過(guò)程噪聲，假設(shè)為高斯白噪聲。

若非第一次檢測(cè)到此角點(diǎn)，則將該庫(kù)位點(diǎn)與已有庫(kù)位點(diǎn)匹配，匹配方法為：若與系統(tǒng)狀態(tài)中已有庫(kù)位點(diǎn)的歐式距離大于某一閾值，則認(rèn)為是新庫(kù)位點(diǎn)，此時(shí)需對(duì)系統(tǒng)的狀態(tài)進(jìn)行增廣，否則即利用EKF算法進(jìn)行更新。

系統(tǒng)觀測(cè)模型：

式中，v(k)=(vr(k),vθ(k))T為測(cè)量噪聲，假設(shè)為高斯白噪聲。

狀態(tài)轉(zhuǎn)移函數(shù)的雅可比矩陣為：

測(cè)量函數(shù)的雅可比矩陣為：

首先預(yù)測(cè)k時(shí)刻的系統(tǒng)狀態(tài)向量初步估計(jì)值和誤差協(xié)方差矩陣的初步估計(jì)值P(k)-：

式中，Q、R分別為狀態(tài)轉(zhuǎn)移協(xié)方差矩陣和測(cè)量噪聲協(xié)方差矩陣。

然后計(jì)算k時(shí)刻的卡爾曼濾波增益矩陣K(k)，更新變量估計(jì)值和誤差協(xié)方差矩陣的估計(jì)值P(k)：

增加了庫(kù)位跟蹤后，大幅度降低了傳感器誤差的影響，聯(lián)合前文所述的決策規(guī)劃算法，能夠顯著改善泊車效果，如圖14所示。

圖14 有、無(wú)庫(kù)位跟蹤的情況對(duì)比

6 仿真與實(shí)車試驗(yàn)驗(yàn)證

6.1 仿真車輛主要參數(shù)

基于Simulink和PreScan聯(lián)合仿真，參數(shù)見(jiàn)表1。

表1 仿真車輛主要參數(shù) m

6.2 平行泊車仿真驗(yàn)證

針對(duì)不同的橫向距離，自車均能準(zhǔn)確地停入庫(kù)位。此規(guī)劃算法適用于可能的任何橫向距離，如圖15所示。

此規(guī)劃算法適用于任何初始角度，無(wú)論初始角度正、負(fù)，最終均能準(zhǔn)確停入庫(kù)位，如圖16所示。

圖15 不同初始橫向距離的平行泊車仿真路徑

圖16 不同初始角度的平行泊車仿真路徑

多次規(guī)劃算法通過(guò)首次規(guī)劃將自車調(diào)整到與庫(kù)位平行。之后更新庫(kù)位與自車的相對(duì)位置，并再次規(guī)劃趨于庫(kù)位中心的路徑，如圖17所示。

圖17 多次規(guī)劃的平行泊車仿真路徑

6.3 垂直泊車仿真驗(yàn)證

隨著橫向距離遞增，第1段前進(jìn)直線段加長(zhǎng)，而第2段圓弧調(diào)整段縮短。但不同的橫向距離對(duì)于最終的泊車姿態(tài)沒(méi)有影響，如圖18所示。

無(wú)論初始角度正、負(fù)，最終均能準(zhǔn)確停入庫(kù)位。此規(guī)劃算法適用于現(xiàn)實(shí)工況中可能的初始角度，如圖19所示。

圖18 不同初始橫向距離的垂直泊車仿真路徑

圖19 不同初始角度的垂直泊車仿真路徑

動(dòng)態(tài)調(diào)整是無(wú)目標(biāo)終點(diǎn)的規(guī)劃，無(wú)法預(yù)知調(diào)整次數(shù)，可行區(qū)域越小，調(diào)整次數(shù)越多，一旦滿足無(wú)碰撞入庫(kù)條件即終止動(dòng)態(tài)調(diào)整開(kāi)始沿規(guī)劃路徑入庫(kù)，如圖20所示。

圖20 動(dòng)態(tài)調(diào)整的垂直泊車仿真路徑

6.4 實(shí)車試驗(yàn)驗(yàn)證

實(shí)車試驗(yàn)車輛主要參數(shù)見(jiàn)表2，車輛如圖21所示。

表2 試驗(yàn)車輛主要參數(shù) m

環(huán)視相機(jī)通過(guò)USB總線與工控機(jī)通訊。工控機(jī)、超聲波雷達(dá)、路徑跟蹤所需的傳感器和執(zhí)行器通過(guò)CAN總線與嵌入式控制器通訊。本文的決策規(guī)劃程序在工控機(jī)中運(yùn)行，不同模塊間通過(guò)輕量級(jí)通信與數(shù)據(jù)封送庫(kù)（Lightweight Communications and Marshalling，LCM）通訊。主要傳感器和控制器的型號(hào)如表3所示。

表3 試驗(yàn)車輛主要傳感器和控制器

為了對(duì)比庫(kù)位跟蹤的效果，對(duì)于平行泊車，先不開(kāi)啟庫(kù)位跟蹤模塊，即在首次收到環(huán)視系統(tǒng)獲得的庫(kù)位坐標(biāo)后僅用航位推算更新自車相對(duì)庫(kù)位的位姿，以此為基礎(chǔ)進(jìn)行規(guī)劃，最終泊車位姿并不理想，自車與真實(shí)庫(kù)位不平行。開(kāi)啟庫(kù)位跟蹤模塊后，重新更新庫(kù)位的坐標(biāo)，進(jìn)行規(guī)劃調(diào)整，調(diào)整后的泊車位姿較調(diào)整前有明顯改善，如圖22所示。

圖22 平行泊車實(shí)車試驗(yàn)路徑

對(duì)于垂直泊車，在同一初始位姿進(jìn)行2次試驗(yàn)。首先通過(guò)測(cè)量得到庫(kù)位相對(duì)自車初始位置的真值。第1次不使用庫(kù)位跟蹤，在首次收到庫(kù)位坐標(biāo)后僅用航位推算更新自車相對(duì)庫(kù)位的位姿，以此為基礎(chǔ)進(jìn)行規(guī)劃。第2次使用庫(kù)位跟蹤，用卡爾曼濾波實(shí)時(shí)更新自車的相對(duì)位姿，以此為基礎(chǔ)進(jìn)行規(guī)劃，同時(shí)記錄載波相位差分（Real-Time Kinematic，RTK）定位結(jié)果。結(jié)果如圖23所示，基于庫(kù)位閉環(huán)的決策規(guī)劃算法能夠明顯改善泊車位姿。

圖23 垂直泊車實(shí)車試驗(yàn)路徑

7 結(jié)束語(yǔ)

本文設(shè)計(jì)的自動(dòng)泊車決策規(guī)劃系統(tǒng)能夠較大限度地發(fā)揮幾何規(guī)劃路徑可靠、算法實(shí)時(shí)性好的優(yōu)點(diǎn)，同時(shí)對(duì)自車初始位姿無(wú)要求。通過(guò)庫(kù)位跟蹤，能夠有效降低航位推算和環(huán)視系統(tǒng)庫(kù)位檢測(cè)的誤差，提高車輛與庫(kù)位的相對(duì)定位精度。將庫(kù)位跟蹤和本文所設(shè)計(jì)的自動(dòng)泊車決策規(guī)劃算法結(jié)合起來(lái)，能夠較大幅度提升自動(dòng)泊車最終泊車位姿的理想程度。此外，對(duì)于自動(dòng)泊車場(chǎng)景中常見(jiàn)的平行泊車長(zhǎng)度不足或垂直泊車可行通道較窄的問(wèn)題設(shè)計(jì)了多次規(guī)劃和動(dòng)態(tài)調(diào)整算法，能夠有效地解決上述問(wèn)題。