基于庫位跟蹤的自動泊車決策規劃系統*

熊璐 嚴森煒 余卓平 張培志

（同濟大學，上海 201804）

主題詞：自動泊車 庫位跟蹤 多次規劃 動態調整 卡爾曼濾波

1 前言

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

本文基于幾何規劃，結合Reeds-Shepp曲線路徑最短[2]和螺旋線適用性廣的優點設計了對自車初始位姿無要求的自動泊車決策規劃算法。通過融合環視庫位坐標信息和車輛底盤運動學信息對目標庫位進行跟蹤，同時對航位推算結果進行修正。對于路徑跟蹤和底盤執行器控制帶來的誤差，通過庫位跟蹤和多次規劃的方法予以減小。此外，設計了多次規劃和動態調整算法解決泊車過程中由于空間狹窄難以入庫的問題。本文設計的算法對各類庫位的適應性強、實時性好、可靠性強，并且大幅改善了最后泊車姿態的理想程度。上述算法通過仿真和實車試驗進行了驗證。

2 方案設計

2.1 自動泊車系統架構

自動泊車系統架構如圖1所示。環境感知模塊利用環視相機給出庫位坐標，結合超聲波雷達檢測障礙物，將探測到的環境信息送到決策規劃模塊用以決策和規劃泊車路徑。運動控制模塊中，底盤執行機構包括電動助力轉向系統（EPS）以及驅動、制動執行器。

圖1 自動泊車系統架構

2.2 決策規劃系統流程

自動泊車系統的目標功能為：駕駛員將車輛駛入停車場或沿停車位行駛時，啟動自動泊車功能后，自車接管車輛控制，前進過程中自動判別庫位類型，實現自動泊車。在泊車過程中，自車通過多次更新庫位坐標減小庫位坐標檢測誤差和航位推算累積誤差，使泊車姿態更理想。對于平行泊車，設計了多次規劃模塊，即庫位長度無法滿足一次倒車入庫時，自動增加調整次數，直至能夠泊入車位，理論上只需車位長度大于車長即可入庫；對于垂直泊車，停車通道寬度不足時，進行動態調整，使泊車規劃不受障礙物的限制。簡要流程如圖2所示。

圖2 決策規劃系統流程

2.3 車輛模型

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

式中，v為車速。

圖3 阿克曼轉向車輛模型

3 基礎路徑規劃

3.1 平行泊車

自車起始時（首次獲得環視系統得到的庫位角點P0、P1時，庫位寬度通過超聲波雷達測算或按國標推算），后軸中點位于S點，將S點設為規劃的原點，建立的坐標系和基礎路徑規劃結果如圖4所示。車輛沿X軸方向前進，到達某點A時，以最小轉彎半徑Rmin倒車，再以為半徑反向倒車，后軸中心點軌跡分別為和BC，此時車身與庫位平行，然后前進至庫位中心位置。

圖4 平行泊車基礎路徑規劃示意

C點的坐標為：

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

第2段、第1段圓弧的圓心O2、O1的坐標分別為：

規劃輔助計算角β1和β2分別為：

中間過渡點B的X坐標為：

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

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

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

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

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

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

圖6 平行泊車多次規劃示意

3.2 垂直泊車

自車起始時（首次獲得環視系統得到的庫位角點P0、P1坐標時），后軸中點位于S點，將S點設為規劃原點，如圖7所示。車輛沿X軸方向前進，到達某點A時，以Rmin為半徑倒車，車輛后軸中心點軌跡為AB。

圖7 垂直泊車A點計算示意

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

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

由式（12）和式（13）聯立解得O1的X坐標（即A的X坐標）為：

特殊地，當θ=0°時：

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

式中，Lsafe4=0.1 m為安全距離，可設與泊車車速正相關。

圖8 垂直泊車一次規劃入庫示意

反之，則需多次規劃入庫。當式（16）取等號時停車，車輛后軸中心位于點B，根據此時自車相對庫位的位姿規劃后續路徑。為便于表述，將坐標系原點重設為泊車目標點E，如圖9所示。E點的X坐標為：

圖9 垂直泊車基礎路徑規劃示意

從B點開始以Rmin為半徑車輛前進，車輛后軸中心點軌跡為，可推得其圓心O2的X'坐標為：

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

則有車輛從點B以Rmin前進至點C及從點C以Rmin倒車至點D的停止判別條件為航向角分別達到：

式中，ψB、ψC、ψD分別為B、C、D點處自車航向角。

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

圖10 垂直泊車動態調整示意

4 垂直泊車規劃算法優化

在實車試驗中發現，由于垂直泊車的側向空間小，對路徑跟蹤、底盤執行器控制和航位推算誤差敏感，因此考慮在C點增加一次庫位跟蹤規劃，即在C點根據自車和庫位的相對位姿進行重新規劃。

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

該曲線對應的車輛航向角為：

圖11 改進后的垂直泊車路徑

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

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

結合式（24）和式（25），借助MIDACO優化工具進行求解，設置優化條件為曲線終點與目標點歐式距離最短和整個路徑的長度最短，同時滿足環境約束。另外，需要滿足曲率約束條件：

式中，κmax為最大曲率。

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

式中，sg為積分區間的右邊界。

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

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

圖12 改進的規劃路徑庫（選取部分離散初始條件）

在實車試驗中若出現MIDACO求解的結果不符合要求的情況，可微調目標點(Xg,Yg,θg,kg)再次運行MIDACO求解。若多次微調目標點的結果均不符合要求，則進行自車位姿微調后根據新的相對位姿進行求解。

5 庫位跟蹤

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

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

圖13 卡爾曼濾波的坐標系和狀態量定義

系統運動模型為：

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

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

系統觀測模型：

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

狀態轉移函數的雅可比矩陣為：

測量函數的雅可比矩陣為：

首先預測k時刻的系統狀態向量初步估計值和誤差協方差矩陣的初步估計值P(k)-：

式中，Q、R分別為狀態轉移協方差矩陣和測量噪聲協方差矩陣。

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

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

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

6 仿真與實車試驗驗證

6.1 仿真車輛主要參數

基于Simulink和PreScan聯合仿真，參數見表1。

表1 仿真車輛主要參數 m

6.2 平行泊車仿真驗證

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

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

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

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

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

圖17 多次規劃的平行泊車仿真路徑

6.3 垂直泊車仿真驗證

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

無論初始角度正、負，最終均能準確停入庫位。此規劃算法適用于現實工況中可能的初始角度，如圖19所示。

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

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

動態調整是無目標終點的規劃，無法預知調整次數，可行區域越小，調整次數越多，一旦滿足無碰撞入庫條件即終止動態調整開始沿規劃路徑入庫，如圖20所示。

圖20 動態調整的垂直泊車仿真路徑

6.4 實車試驗驗證

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

表2 試驗車輛主要參數 m

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

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

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

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

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

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

7 結束語

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