汽車垂直泊車路徑規劃與路徑跟蹤研究*

高 強，陸 洲，段晨東，徐 婷

（1.長安大學汽車學院，西安 710064；2.長安大學電子與控制工程學院，西安 710064）

前言

高級駕駛輔助系統（advanced driving assistant system，ADAS）是當前汽車領域 熱門的研究方向。ADAS系統通過車載傳感器感知周圍環境和汽車信息，進行分析和處理后，實現汽車定速巡航、車道保持和碰撞預警等功能［1］。自動泊車系統（automatic parking system，APS）是ADAS的組成部分，是智能汽車重要的研究內容之一。APS能夠自動尋找停車位并泊入，有效減小駕駛員在泊車過程中的緊張程度，降低新手對狹小車位的泊車難度，減少剮蹭事故的發生［2］。國內外學者對自動泊車技術進行了多方面的研究［3-9］。

城市中垂直車位比較常見。當前垂直自動泊車技術的研究大多基于汽車初始位置航向角為零的情況（理想狀況），沒有考慮初始位置航向角誤差對泊車的影響。在實際自動泊車過程中，初始位置航向角通常不會為零（即相對理想狀態存在誤差），此時如按零初始位置航向角進行路徑規劃和泊車，會造成汽車實際路徑與規劃路徑存在較大偏差，從而在泊車中造成剮蹭或泊車失敗等問題。針對上述不足，本文中研究了汽車初始位置航向角不為零時，垂直泊車路徑規劃與路徑跟蹤問題。首先對汽車初始位置航向角不為零時的泊車路徑進行規劃，使用四次樣條函數改進規劃路徑；然后針對常用的預瞄誤差前饋方法在路徑跟蹤中有時存在較大誤差的問題，設計了預瞄誤差前饋和航向角反饋相結合的控制方法，對期望路徑進行跟蹤；通過Simulink/CarSim聯合仿真，研究了所設計垂直泊車算法的有效性；最后通過實車試驗對該算法進行了驗證。

1 研究方法

1.1 自動泊車系統工作流程

自動泊車系統工作流程如圖1所示。駕駛員開啟自動泊車功能后，APS系統對汽車進行控制，汽車向前行駛，同時車載環視攝像頭對汽車周圍環境進行感知。當檢測到停車位時，APS對當前車位進行路徑規劃，然后控制汽車進行路徑跟蹤，直到汽車成功泊入停車位。泊車完成后轉向盤回正并掛駐車擋，自動泊車系統關閉。

圖1 自動泊車系統工作流程圖

1.2 汽車運動學模型

汽車輪廓形狀直接影響泊車成功率。由于實際汽車的輪廓形狀較為復雜且難以準確建模，這里在給予一定裕度的情況下使用矩形替代汽車模型，其長度和寬度分別為汽車的最大長度和最大寬度，簡化后的汽車模型如圖2所示。在本文計算、仿真和實車試驗中，汽車主要參數如表1所示。

圖2 簡化汽車模型

表1 汽車主要參數表

選取APS系統開始工作時刻后輪中心點為汽車參考點。在低速泊車工況下，根據阿克曼轉向原理，將汽車視為平面剛體，即只具有x方向、y方向和橫擺3個自由度。汽車在坐標系的坐標為(x，y)，航向角為ψ，汽車運動學方程為

式中：v為車速；δf為等效前輪轉角。

1.3 垂直泊車基礎路徑規劃策略

假設汽車垂直泊車時初始位置航向角為θ，以汽車前進方向為X軸，自動泊車系統開始工作時刻汽車后軸中心為原點O，原點左側垂直方向為Y軸建立坐標系，如圖3所示。通過攝像頭模塊探測得到垂直車位的兩個頂點坐標為(xa，ya)和(xb，yb)，可得汽車相對于車位的初始位置航向角θ為

圖3 垂直泊車第1次停車點示意圖

如圖3定義初始位置航向角向上為正，向下為負。進行泊車路徑規劃時要確定以下幾個關鍵點的坐標。

（1）泊車起始點S1坐標。假設汽車行駛到S1點時，以最小轉向半徑R倒車，考慮車位右頂點(xb，yb)的約束，汽車在S2點第1次停車，駛過的圓弧路徑為

根據汽車運動幾何關系，易證車身M點在倒車過程中與車位右頂點(xb，yb)距離最近。設安全閾值LS1=0.1 m，可得點O1(xO1，yO1)的坐標為

從而求得起始點S1的坐標為

圖4 旋轉坐標系后的垂直泊車工況

式中LS2=0.1 m，為設置的S2點安全閾值。同時為保證泊車終止點位姿良好，應使汽車最終停車點的航向角在OX1Y1坐標系下接近90°，如圖5所示。若汽車沿規劃路徑行駛中滿足上述條件，則可實現一步泊車入庫。

圖5 垂直泊車一步泊車入庫

（2）停車點S2坐標。若汽車行駛到S2點時車身左后頂點Nd不滿足式（5），則須多步入庫，此時須進行下一步路徑規劃。汽車基于OX1Y1坐標系下航向角變化量如圖6所示。

圖6 多步泊車路徑航向角變化

由圖6幾何關系可以求出：

汽車從路徑點S1到路徑點S2駛過的航向角ψ1為

根據求得的航向角ψ1，計算S2點的坐標為

（3）前進點S3坐標。接下來汽車應調整轉向盤轉角為零，沿著直線路徑S2S3朝前行駛到S3點，最后再以最小轉向半徑R倒車入庫，路徑曲線為規劃后多步泊車路徑如圖7所示。

圖7 垂直泊車多步泊車入庫

設直線段路徑S2S3的長度為Lb，則根據幾何關系可以求得

則S3點坐標為

汽車從路徑點S3到路徑點S4駛過的航向角ψ2為

根據ψ2的值可得路徑的圓心坐標：

最后求得S4點坐標：

1.4 垂直泊車路徑改進

當汽車行駛到S3點時，需要重新對路徑進行規劃。實際泊車過程中，由于存在傳感器、執行器等多種誤差，可能會導致汽車不能準確跟蹤期望路徑，那么基于最小轉向半徑的路徑規劃有可能導致路徑跟蹤失敗（因需要的轉向半徑小于最小半徑），進而造成泊車失敗。為增加泊車算法的魯棒性，同時減小汽車原地轉向現象，本文中選用四次樣條函數改進段路徑，使其曲率連續并滿足汽車轉向約束。四次樣條函數公式可表示為

根據規劃路徑起始點位置和航向角，可得

根據路徑終止點位置和航向角，可得

最后，確定障礙物約束，即在倒車過程中不能與障礙物發生刮蹭。選取一個約束點為中間點，可得

根據式（16）～式（20），可以得到一條唯一的四次樣條曲線，該曲線即為規劃的路徑。在垂直泊車過程中為使得生成路徑滿足汽車轉向半徑約束，將S3的位置沿當前航向角方向延長一段距離Ld1，則延長后的樣條函數起始點坐標為

相應地將泊車終止點坐標S4向下延長一段距離Ld2，則延長后樣條函數終止點坐標為

選取不同的中間點，會生成不同曲率的曲線，這里以考慮車位左端點約束時的S2點為參考點，并對其進行修正作為中間點：

式中Δs為修正值。根據式（21）～式（23），可以得到系數確定的四次樣條函數曲線。

1.5 垂直泊車橫向跟蹤控制模塊

自動泊車系統控制轉向盤轉角，使汽車在前進方向的垂向產生速度，從而使汽車沿已規劃路徑行駛稱為橫向控制，主要使用的算法有預瞄控制、PID控制、模糊控制、LQR和MPC控制等［3-7］。國外MacAdam提出了最優預瞄控制模型［8］。

國內郭孔輝院士提出了單點預瞄最優曲率模型［9］，其原理如圖8所示。假設汽車以恒定速度v行駛，汽車跟蹤的路徑方程為f(t)，路徑上點即為理想預瞄點。駕駛員根據前方路徑點的信息，結合汽車自身運動狀態，對汽車到達期望點的誤差進行估計，計算一個最優圓弧路徑，并根據圓弧曲率半徑與轉向盤轉角的關系得到一個轉向盤轉角的期望輸入。最優圓弧路徑在實踐中一般通過實車反復調試得到。若汽車后軸中心點的實際軌跡為y(t)，汽車側向加速度為y，在較低行駛速度下，根據阿克曼轉向原理，汽車軌跡曲率可視為與等效前輪轉角成正比：

圖8 最優預瞄曲率原理

式中：r為道路曲率半徑；δf為等效前輪轉角；R1為汽車轉彎半徑；i為轉向系傳動比。

此時汽車橫向位置為y(t1)，設駕駛員預瞄距離為d，預瞄時間為T=d/v。經過一個預瞄時間T后，汽車橫向位置變成y(t1+T)，對該式進行泰勒級數展開，忽略高階項，保留其2階項得

根據誤差最小原則，駕駛員期望得到一條最優軌跡，使得汽車經過一段時間T后可以到達預期位置f(t1+T)。由式（24）～式（26）可以得到基于最優曲率的轉向盤轉角：

式中δsw為轉向盤轉角，且δsw=δfi。

根據控制需要，規劃出來的汽車路徑由一系列坐標點組成。在實際跟蹤過程中，根據當前汽車位置，選擇一個合理預瞄距離d，通過計算得到參考路徑距預瞄點最近的點(xj，yj)與預瞄點(xd，yd)的距離，這段距離稱之為預瞄誤差e(j)。預瞄誤差示意圖如圖9所示。由圖可知，為使汽車實際行駛路徑與期望路徑誤差最小，由式（27）可得預瞄誤差與此時轉向盤轉角δe之間的傳遞函數：

圖9 預瞄誤差

式中j為預瞄點序號。

當前很多研究采用預瞄誤差前饋控制方法對規劃路徑進行跟蹤。由于跟蹤中存在模型誤差和環境干擾等因素，僅依靠預瞄誤差前饋的控制方法有時難以保證精確的跟蹤效果和良好的跟蹤穩定性。考慮到實際泊車過程中，汽車航向角對路徑跟蹤效果影響較大，因此本文中增加汽車航向角反饋環節來同時對轉向盤轉角進行調節，即采用預瞄誤差前饋加航向角反饋的控制方法對規劃路徑進行跟蹤。

如圖10所示，在某一時刻汽車位置為(xrk，yrk，ψrk)，路 徑 上 距 離 汽 車 最 近 的 一 點 坐 標 為(xk，yk，ψk)，則汽車此時的航向角誤差λerr定義為

圖10 汽車航向角誤差示意圖

式中k為路徑點序號。

采用PID算法減小航向角誤差，調節轉向盤轉角。轉向盤轉角的PID控制表達式為

式中：kp、ki和kd分別為比例、積分、微分系數。考慮預瞄誤差前饋，結合航向角反饋，則轉向盤轉角δsw為

整個泊車路徑跟蹤模塊控制框架如圖11所示。

圖11 預瞄控制器框架圖

2 仿真驗證

基于Simulink/CarSim對所提出的泊車路徑跟蹤算法進行了聯合仿真。首先在CarSim中根據目標參數建立了汽車模型和停車位場景，并導入Simulink。然后在Simulink中搭建了基于樣條函數的路徑規劃模塊，并建立了預瞄誤差前饋加航向角反饋的跟蹤控制模塊，最后進行聯合仿真。

對汽車初始位置航向角分別為5°、0°和-5°時自動垂直泊車系統路徑規劃及路徑跟蹤結果進行仿真，結果如圖12～圖14所示。

圖12 初始位置航向角為5°的泊車仿真

圖14 初始位置航向角為-5°的泊車仿真

從仿真結果可以看出，在汽車初始位置航向角分別為5°、0°和-5°時，路徑規劃模塊都規劃出了泊車路徑。圖12（a）和圖13（a）泊車過程中，僅使用預瞄誤差前饋控制方法時，對曲率不變的路徑和跟蹤效果良好。在對四次樣條函數規劃的路徑進行跟蹤時，由于曲率是變化的，航向角也隨之改變，采用單一的預瞄誤差前饋控制跟蹤存在較大的滯后現象，跟蹤效果較差，最大橫向誤差分別達到了25和20 cm，最終泊車姿態不太理想，偏角分別為7°和5°。而在加入了航向角反饋后（圖12（b）、圖13（b）），在曲率不變路徑跟蹤效果同樣較好，而在曲率變化的路徑，最大橫向誤差分別減小到6和5 cm，跟蹤精度更高，跟蹤響應速度快，且最終泊車姿態良好，偏角均小于5°。由圖14（a）可以看出，在初始位置航向角為-5°時，由于曲率變化引起汽車航向角變化量較大，僅使用預瞄誤差前饋的控制模塊不能及時對汽車轉向盤轉角進行調整，導致汽車在跟蹤時側向偏差最大達到了40 cm（點P），最終泊車位置姿態角達到了35°，汽車輪廓線與車位輪廓線發生了剮蹭，泊車失敗。由圖14（b）可以看出，按本文方法加入了航向角反饋控制后，汽車轉向盤調整及時，最大側向誤差點P的誤差小于5 cm，汽車能夠準確地跟蹤規劃路徑，順利泊入車位，且最終泊車姿態良好，偏角小于5°。

圖13 初始位置航向角為0°的泊車仿真

因此，在泊車路徑跟蹤過程中，對于曲率不變路徑，預瞄誤差前饋控制就能較好地對路徑進行跟蹤，但對于曲率變化的路徑，預瞄誤差前饋控制跟蹤誤差變大且具有較大滯后現象。在預瞄誤差前饋的基礎上加入了航向角反饋控制后，對曲率不變和曲率變化路徑都具有較好的跟蹤效果，且最終泊車位姿良好。

3 垂直泊車路徑規劃與跟蹤實車驗證

3.1 試驗平臺

為了對本文中提出的垂直泊車路徑規劃和路徑跟蹤策略進行工程驗證，利用一輛試驗車進行了多組實車試驗。試驗車如圖15所示，該車具有ETRS電子排擋和EPS電動助力轉向系統，能夠實現對轉向盤轉角、加速、制動踏板和擋位的自動控制。

圖15 試驗汽車示意圖

汽車實車試驗平臺如圖16所示，主要由以下部分組成：（1）感知系統：主要包括環視攝像頭（檢測停車位信息）和12個超聲波雷達（實現避障）；（2）攝像頭控制器：分析環視攝像頭采集到的數據，以及汽車自身數據，得到停車位角點坐標；（3）泊車控制器（infineon TC233）：運行基于四次樣條函數的路徑規劃算法，以及預瞄誤差前饋加航向角反饋路徑跟蹤算法，輸出轉向盤轉角信號、擋位、加速踏板和制動踏板信號；（4）CAN盒（VPN1630）：負責整車的CAN通信，在其配套軟件CANoe上對整車報文進行解碼和編制收發目標報文；（5）UDE聯合調試器：負責將Hightec生成的可執行程序燒入控制器以及全局變量的觀測和代碼調試；（6）PC端（ThinkPad T470P）：進行數據分析、代碼修改和變量監控；（7）供電模塊：為控制器提供12 V穩定電壓。

圖16 汽車試驗平臺組成

3.2 試驗算法和結果分析

航向角是實車試驗中重要的參數，但它難以借助一般的車載傳感器直接獲得，本試驗通過以下算法計算得到車輛航向角：

其中：

式中：（x，y)為汽車后軸中心坐標；Δb為等效后軸位移增量；Δθ為航向角增量；Δbcl、Δbcr分別為左后輪和右后輪位移增量，可以通過輪速脈沖和車輪半徑求得。

分別對初始位置航向角為10°、5°、0°和-5°的情況進行了試驗。期望后軸中心坐標通過UDE聯合調試器從泊車控制器中讀出，實際后軸中心坐標從汽車慣導數據獲得，并通過CAN總線由CANoe處理讀出，最后在MATLAB中繪圖。試驗結果如圖17～圖20所示。

圖17 初始位置航向角10°的泊車試驗

圖18 初始位置航向角5°的泊車試驗

圖19 初始位置航向角0°的泊車試驗

圖20 初始位置航向角-5°的泊車試驗

由試驗結果可以看出，在初始位置航向角為10°、5°、0°和-5°的情況下，汽車均能順利規劃并跟蹤泊車路徑，泊入車位，跟蹤誤差小于5 cm，最終停車點位姿角小5°，跟蹤精度較高，控制效果較好。說明本文中提出的垂直泊車路徑規劃方法和控制策略能夠較好地實現實際交通環境中的垂直泊車問題，具有較好的應用前景。

4 結論

對具有初始位置航向角的泊車工況進行了路徑規劃，并結合四次樣條函數對路徑進行了改進。設計了一種預瞄誤差前饋加航向角反饋的跟蹤控制算法，在規劃路徑曲率不變和變化的情況下都能夠較好地跟蹤規劃路徑，跟蹤精度高，魯棒性強。基于Simulink/CarSim進行了聯合仿真，仿真結果表明在具有一定初始航向角的情況下，路徑規劃模塊均能規劃出一條可行路徑，路徑跟蹤控制模塊可以對其進行精準跟蹤。實車試驗也驗證了本文提出垂直泊車路徑規劃和路徑跟蹤方法的有效性。