適用于車頭泊入的路徑規劃和跟蹤控制新方法

詹瑞典，黃經偉，張學習，肖 淳，侯 帥，蔡述庭

1.廣東工業大學 先進制造學院，廣州 510006

2.廣東工業大學 自動化學院，廣州 510006

隨著人工智能的發展，自動駕駛為解決交通擁堵和事故等問題提供了一種新途徑[1-4]。自動泊車系統作為自動駕駛的一部分，已經能夠有效實現汽車自動泊車功能，但都是基于側方位倒車和垂直倒車方法實現的。然而，對于電動汽車而言，許多充電口都安裝在車頭，使用垂直倒車方法可能會導致充電線太短無法充電或拉線過長存在安全隱患等問題。在未來幾十年，電動汽車代替燃油汽車成為主流已是必然[5-7]。因此，設計一款針對電動汽車的自動泊車系統至關重要。另一方面，現有的先進控制算法對單片機系統運算速度和存儲空間都具有極高的要求，極有可能出現運算爆炸和存儲爆炸的問題，這將極大限制它在汽車工業中的應用。泊車過程是一個低速行駛過程，對于系統的實效性要求并不高，本文采用PID方法控制汽車的泊車過程，克服“計算爆炸”問題，并有效地實現車頭泊入。

本文針對上述問題提出基于車頭泊入的自動泊車新方法。該方法主要包括對車輛進行運動學建模，根據汽車與車庫的相對位置，基于幾何推導進行路徑規劃，并利用PID 控制算法減少系統計算量使之更符合工程應用。在文獻[8-9]中，主要使用Matlab平臺建立車輛運動學模型，并設計路徑規劃及跟蹤控制器進行仿真分析，該方法不能夠全面地對自動泊車過程進行運動仿真分析。有鑒于此，本文使用Matlab和CarSim聯合仿真，通過車輛運動學模型和實車參數在CarSim中搭建實驗模型車，聯合Matlab設計泊車路徑跟蹤控制器進行聯合仿真實驗，實驗結果驗證了本文設計的控制方法的有效性。

1 車輛運動學模型的建立

將泊車過程設置為一個極低速的（通常低于5 km/h）運動過程，此時車輪低速滾動，車輛一般不會發生側向滑動，可忽略側向力，因而沒有車輪側偏角。在實際工程中，通常對汽車運動模型進行相應的簡化，建立車輛運動學模型。本文基于阿克曼轉向原理[10]建立車輛運動學模型，如圖1所示。

圖1 汽車運動學模型Fig.1 Car kinematics model

圖1 中，2d為輪距，L為車輛軸距，φ表示車輛等效的轉角，φl為左輪轉角，φr為右輪轉角。為了方便計算，將四輪車簡化為兩輪車，根據阿克曼轉向原理可得：

由實際條件可知，在滿足阿克曼轉向原理條件下，車輛方向盤轉角和等效阿克曼前輪角之間存在近似線性的轉換關系：

其中，θsw為方向盤角度，k為轉換系數，由實際測量結果可得，k≈16.2。以后軸中心為參考點的汽車運動學方程為：

其中，xr、yr分別為后軸中心對應的x坐標和y坐標，v為車輛中心速度，θ為車輛的橫擺角。

2 基于幾何推導的路徑規劃算法

泊車系統通過傳感器感知泊車環境信息規劃泊車路徑，若路徑符合要求則進行泊車操作，否則系統關閉。

車頭泊入的泊車方式分為垂直泊入和斜向泊入，本文對垂直泊入進行規劃研究。

2.1 極限位置的確定

車庫模型設置為長5 200 mm、寬為2 500 mm 的矩形，模型車體設置為長4 542 mm、寬1 786 mm的矩形。

設計泊車過程共3個階段，分別如下：

汽車前進階段軌跡：汽車由泊車中途停車點至車頭抵達車庫階段，汽車以最小轉彎半徑駛入車庫，整個階段方向盤處于打滿狀態，此時中途停車點與車庫入口的垂直距離最短，即最極限位置。

汽車入庫階段軌跡：可由上一階段的后軸中心位置和航向角θ以及完全入庫汽車最終位置的后軸中心規劃出路徑。

汽車后退階段軌跡：泊車中途停車點應保證汽車的姿態不變，在后退階段對應的圓與前進的第一階段對應的圓應該保持相切關系；根據汽車的初始位置、航向角和前進階段求得的圓，然后求出后退階段對應的圓、中途停車點（即兩個圓的相切點）和相應的方向盤角度。

2.2 前進階段

汽車前進階段分為兩段軌跡：第一段軌跡為汽車自中途停車點前進至車庫；第二段為入庫軌跡。

2.2.1 前進至車庫階段

取停車位左下角的點為坐標原點，長為x軸，寬為y軸，汽車與x軸的夾角為航向角θ，逆時針為正。

為確保汽車可以完全駛入車庫，可以設置汽車駛至車庫時的姿態如圖2所示。

圖2 車庫入口汽車角度示意圖Fig.2 Car angle of garage entrance

由幾何關系得此時汽車的航向角θ為：

其中，CW為車寬，CL為車長，PW為車庫寬，d取300 mm。

再求出此時后軸中心的坐標：

其中，Clf為汽車前懸長度，L為軸距，則此時前進階段對應圓心坐標為：

其中，Rmin為最小轉彎半徑。

2.2.2 入庫階段

由第2.2.1 小節設計的汽車開始入庫時的姿態，此時方向盤處于極限打滿狀態，想要確保汽車可以完全入庫只需保證汽車入庫對應的軌跡圓的半徑大于最小轉彎半徑即可。設計汽車完全入庫時的后軸中心處于車庫入口的中心點，則汽車入庫的軌跡示意圖如圖3所示。

圖3 入庫示意圖Fig.3 Driving into garage

由圖3可得此時圓心和半徑存在關系：

2.3 后退階段

假設汽車初始位置為(xf,yf)，航向角為θ，前進階段對應的圓心為(x2,y2)，后退階段對應的圓心為(x1,y1)，半徑為R1，如圖4所示。

圖4 后退軌跡示意圖Fig.4 Reversing trajectory

由幾何關系可知：

由上述方程可求出(x1,y1)和R1。

此時兩圓心的斜率為：

則兩個圓的交點坐標為：

綜上，泊車過程前兩段軌跡各自所需的方向盤轉角為：

整個泊車過程的后軸中心規劃路徑如圖5所示。

圖5 泊車軌跡規劃圖Fig.5 Parking trajectory planned

3 基于PID的路徑跟蹤控制算法

設計合理的路徑規劃是泊車成功的前提，而控制車輛跟蹤規劃好的泊車路徑是成功泊車的保證。

汽車常用的跟蹤控制方法主要有PID、MPC和LQR三種算法[11-13]。MPC 和LQR 算法根據狀態方程在一定的時域上求得最優解，如圖6所示。

圖6 MPC與LQR的工作時域Fig.6 Working time domain of MPC and LQR

PID 算法通過上一時刻的實際值與期望值的差值來調節控制量，因此雖然控制效果LQR 與MPC 整體優于PID，但相應的計算量LQR與MPC也遠遠高于PID[14-15]。對于泊車系統而言，整個過程是一個低速狀態，實效性的要求并沒有高速行駛狀態高，因此使用PID控制可以保證跟蹤效果并減少計算量，降低對單片機的要求，使系統更符合工程應用。

本文選用的汽車模型為自動擋汽車，且泊車過程為低速行駛過程，為了保證泊車過程的舒適性和減少泊車過程的誤差，設置泊車過程的最大車速為2 km/h，而自動擋汽車怠速可達7 km/h。泊車過程主要控制的參數為：（1）制動輪缸壓力；（2）方向盤角度。

如圖7 所示，汽車真實位置與參考位置的橫向誤差為：

圖7 泊車位置偏差分析圖Fig.7 Analysis of deviation of parking position

實際方向盤轉角與期望轉角誤差為：

可將誤差設置為：

其中，k為系數。

則PID控制器為：

4 基于CarSim和Matlab的聯合仿真

仿真驗證已經成為產品開發必不可少的一部分，傳統泊車實驗需要準備樣本車、硬件平臺、實驗場地等，實際實驗中可能由于算法不完善和操作失誤等問題會對實驗器材造成損壞，甚至導致不必要的人員受傷。隨著車輛仿真軟件功能的不斷集成，強大的仿真軟件已經可以代替部分實物測試。

本文基于CarSim車輛動力學仿真軟件建立目標車型模型和模擬泊車環境，并結合路徑規劃方法和PID路徑跟蹤控制器，搭建Simulink 模型進行聯合仿真，驗證路徑規劃方法的合理性以及控制器的控制效果。

4.1 Simulink與CarSim聯合仿真

在Matlab 軟件上搭建路徑規劃模塊和跟蹤控制模塊，并與設置好的CarSim 聯立。CarSim 通過數據接口將模型傳輸到目標目錄的mdl文件中，以S 函數的形式添加到Simulink模塊庫中。Simulink通過調用該S函數，與CarSim軟件進行數據交互，完成聯合仿真，如圖8所示。

圖8 聯合仿真示意圖Fig.8 Co-simulation illustration

聯合仿真中CarSim提供了完善準確的汽車運動模型，并設計了相應的泊車環境，而Simulink/Matlab 便于計算分析，兩者結合可以讓仿真快速準確完成并生成相應的參數圖表、仿真過程的動畫等，能夠有效避免使用單一軟件導致的效果不佳等問題。

4.2 CarSim參數設置

參考市面上的車型參數，設置實驗模型車主要參數如表1所示。

表1 汽車模型主要參數Table 1 Main parameters of car model

泊車過程為低速過程，對于自動擋汽車只用控制制動輪缸壓力來控制速度，CarSim輸入輸出見表2。

表2 CarSim輸入輸出Table 2 CarSim input and output

4.3 路徑規劃及跟蹤控制器

Simulink模型由三個主要模塊組成，分別是跟蹤控制模塊、路徑規劃模塊、CarSim 的S 函數模塊，如圖9所示。其中，路徑規劃模塊包括坐標軸的轉換、初始位置參數的獲取以及兩段軌跡的規劃數據。

圖9 聯合仿真模型Fig.9 Co-simulation model

跟蹤控制器根據路徑規劃得到的期望信息和汽車的實時信息，通過控制算法計算得到輸出制動輪缸壓力、方向盤角度及擋位，進而反饋給CarSim的S函數，保證汽車完成泊車任務。路徑規劃和跟蹤控制流程圖如圖10所示。

圖10 路徑規劃和跟蹤控制示意圖Fig.10 Path planning and tracking control

由上述內容可知整個泊車過程分為以下幾個步驟：

（1）將方向盤轉至θsw1，并倒車至中途停車點；

（2）將方向盤轉至θsw2，前進至車庫入口；

（3）汽車駛入車庫并控制方向盤逐漸將車身擺至與車庫平行。

4.4 聯合仿真實驗結果

通過CarSim 與Simulink 聯合仿真實驗驗證路徑跟蹤控制模型的有效性，實驗結果如圖11所示。

由圖11 可以看出，在本文所設計的跟蹤控制模型的作用下，整個泊車過程中模型車能較好地沿著規劃路徑完成整個泊車過程。

圖11 仿真動畫Fig.11 Simulation animation

泊車過程中汽車的主要參數變化如圖12所示。

由圖12 可以看出，聯合仿真過程中模型車的方向盤轉角和車速變化均可以通過PID 得到有效控制。其中方向盤轉角變化均勻且具有較好的角度維持性，車速能夠保持低速狀態（低于2 km/h）且制動輪缸壓力小于1.6 MPa（一般汽車制動輪缸壓力要求不大于8 MPa）。汽車運動軌跡與規劃軌跡對比圖如圖13所示。

圖12 泊車過程中汽車的主要參數變化Fig.12 Change of car main parameters during parking

由圖13 可以看出，在聯合仿真下，設計的PID 控制器能夠很好地跟蹤參考路徑行駛至目標位置。在倒車階段，車輛起始位置與路徑起始位置一致，車輛能較好地跟蹤到參考路徑，中途發生偏差也可以調整回來，汽車的行駛路徑與參考路徑基本一致，誤差較小；在前進至車庫入口階段，由于泊車過程中汽車實際停車點與參考停車點的偏差較大，而且受到最小轉彎半徑的影響偏差較上一階段大，但仍基本符合規劃路徑；汽車駛入車庫階段，此時需要逐漸擺正汽車車身和方向盤，汽車方向盤需要一定時間調整，因此無法完全符合規劃要求，但整個泊車過程中模型車能較好地跟蹤理想泊車路徑。本文設計的入庫車輛的頂點與車庫頂點的距離為30 cm，泊車過程最大偏差不超過15 cm，仿真結果偏差仍在可接受范圍內。

圖13 汽車運動軌跡與規劃軌跡對比圖Fig.13 Comparison of car trajectory and planned trajectory

5 總結

針對電動汽車泊車系統設計了一款車頭泊入的泊車方式，包括倒車、前進和入庫三段規劃路徑，同時結合PID 控制跟蹤路徑，減少了常用泊車算法的計算量，具有較好的工程實踐意義。基于車輛運動學模型和實車參數在CarSim 中搭建了實驗模型車，聯合Simulink 建立了仿真實驗平臺，設計了路徑跟蹤控制模型并進行了仿真實驗。從聯合仿真結果可以看出：通過控制模型的輸入、輸出變量，可以實現模型車對規劃路徑的良好跟蹤，證明了本文設計的路徑跟蹤控制方法的有效性。