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

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

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

1 前言

自動泊車技術(shù)已經(jīng)問世多年，但目前仍不能較好地解決如下問題：對不同庫位環(huán)境和初始位姿的普適性、算法良好的實時性、算法的可靠性和可控性、最后停車的理想位姿。自動泊車算法主要分為4種：一是基于路徑規(guī)劃的控制算法[1-6]，即根據(jù)環(huán)境約束規(guī)劃幾何曲線（包括搜索采樣算法），然后通過路徑跟蹤和底盤控制決定車輛的動作，優(yōu)點(diǎn)是規(guī)劃運(yùn)算耗時較短、規(guī)劃路徑基本可控，缺點(diǎn)是路徑跟蹤和底盤控制存在誤差，導(dǎo)致最后泊車位姿不理想；二是基于經(jīng)驗的控制算法[7-11]，即利用模糊邏輯或神經(jīng)網(wǎng)絡(luò)，根據(jù)實時的環(huán)境輸入得到航向角和車速的控制量，優(yōu)點(diǎn)是閉環(huán)控制，缺點(diǎn)是控制參數(shù)難以確定、泊車路線不可控、實車應(yīng)用效果不佳；三是基于數(shù)值優(yōu)化的算法[12-13]，通過建立系統(tǒng)運(yùn)動學(xué)方程和結(jié)合參數(shù)約束進(jìn)行路徑最優(yōu)化求解，優(yōu)點(diǎn)是對不同環(huán)境適用性廣，缺點(diǎn)是計算量大、實時性差；四是基于學(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è)計了對自車初始位姿無要求的自動泊車決策規(guī)劃算法。通過融合環(huán)視庫位坐標(biāo)信息和車輛底盤運(yùn)動學(xué)信息對目標(biāo)庫位進(jìn)行跟蹤，同時對航位推算結(jié)果進(jìn)行修正。對于路徑跟蹤和底盤執(zhí)行器控制帶來的誤差，通過庫位跟蹤和多次規(guī)劃的方法予以減小。此外，設(shè)計了多次規(guī)劃和動態(tài)調(diào)整算法解決泊車過程中由于空間狹窄難以入庫的問題。本文設(shè)計的算法對各類庫位的適應(yīng)性強(qiáng)、實時性好、可靠性強(qiáng)，并且大幅改善了最后泊車姿態(tài)的理想程度。上述算法通過仿真和實車試驗進(jìn)行了驗證。

2 方案設(shè)計

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

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

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

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

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

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

2.3 車輛模型

本文使用阿克曼轉(zhuǎn)向性質(zhì)的車輛模型。將車輛視為平面剛體，具有3個自由度，選取后軸中心作為參考點(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 平行泊車

自車起始時（首次獲得環(huán)視系統(tǒng)得到的庫位角點(diǎn)P0、P1時，庫位寬度通過超聲波雷達(dá)測算或按國標(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時，以最小轉(zhuǎn)彎半徑Rmin倒車，再以為半徑反向倒車，后軸中心點(diǎn)軌跡分別為和BC，此時車身與庫位平行，然后前進(jìn)至庫位中心位置。

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

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

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

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

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

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

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

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

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

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

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

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

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

3.2 垂直泊車

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

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

θ的計算見式（3），O1位于直線：

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

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

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

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

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

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

反之，則需多次規(guī)劃入庫。當(dāng)式（16）取等號時停車，車輛后軸中心位于點(diǎn)B，根據(jù)此時自車相對庫位的位姿規(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)開始以Rmin為半徑車輛前進(jìn)，車輛后軸中心點(diǎn)軌跡為，可推得其圓心O2的X'坐標(biāo)為：

此后車輛以Rmin為半徑沿倒車，最終使車輛縱軸線與車庫縱軸線重合，可推得的圓心的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)處自車航向角。

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

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

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

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

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

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

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

設(shè)第1段螺旋線的長度為s1，終點(diǎn)處的車輛位姿為X1=(X1,Y1,θ1)，中間段圓弧的圓心坐標(biāo)是(XR,YR)，圓弧結(jié)束點(diǎn)處車輛位姿為X2=(X2,Y2,θ2)，第2段螺旋線的長度為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個未知量，設(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)歐式距離最短和整個路徑的長度最短，同時滿足環(huán)境約束。另外，需要滿足曲率約束條件：

式中，κmax為最大曲率。

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

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

考慮到初值的選擇對于MIDACO求解有影響，因此考慮用成功生成的算例來訓(xùn)練神經(jīng)網(wǎng)絡(luò)，作為MIDACO求解的初值。流程如下：首先已知(Xi,Yi,θi,ki)和(Xg,Yg,θg,kg)，通過MIDACO求解待求參數(shù)a1、s1、a2、s2（在現(xiàn)有設(shè)備上的平均計算周期為1.2 s），用求解參數(shù)和初始條件生成曲線，二次驗證篩選掉可能發(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個參數(shù)的平均誤差約10%。

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

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

在實車試驗中若出現(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ù)新的相對位姿進(jìn)行求解。

5 庫位跟蹤

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖14 有、無庫位跟蹤的情況對比

6 仿真與實車試驗驗證

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

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

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

6.2 平行泊車仿真驗證

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

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

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

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

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

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

6.3 垂直泊車仿真驗證

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

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

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

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

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

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

6.4 實車試驗驗證

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

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

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

表3 試驗車輛主要傳感器和控制器

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

圖22 平行泊車實車試驗路徑

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

圖23 垂直泊車實車試驗路徑

7 結(jié)束語

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