駱 嫚，熊勝健，隋立洋，魯若宇，江 輝*

基于預瞄PID的自動泊車路徑跟蹤分析

駱 嫚1，熊勝健1，隋立洋2，魯若宇1，江 輝*1

（1.東風悅享科技有限公司，湖北 武漢 430058；2.武漢理工大學汽車工程學院，湖北 武漢 430070）

為減少路徑跟蹤的誤差，文章設計了基于預瞄誤差的比例-積分-微分（PID）控制器。首先，采用直線-圓弧-回旋曲線拼接的方法生成了垂直泊車場景下曲率連續(xù)的全局路徑。其次，設計了預瞄點、參考點及預瞄誤差的計算方法，并將預瞄誤差作為PID的輸入以設計路徑跟蹤控制器。最后，通過Simulink/CarSim聯(lián)合仿真對預瞄PID算法進行了驗證，仿真及試驗的結果表明，文章所提出的基于預瞄PID的路徑跟蹤算法雖然原理簡單，但是跟蹤精度較高，可以滿足自動泊車的需求。

自動泊車；垂直泊車；路徑跟蹤；預瞄誤差；PID；Simulink/CarSim

自動泊車系統(tǒng)（Automated Parking System, APS）作為高級輔助駕駛系統(tǒng)（Advanced Driving Assistance System, ADAS）的重要組成部分，成了目前研究的熱點。當駕駛員將車輛開到車位附近并開啟APS功能后，系統(tǒng)通過環(huán)視相機及超聲波雷達對車位進行檢測及定位，根據(jù)自車位置相對車位的位姿規(guī)劃出一條入庫的全局路徑，路徑跟蹤模塊根據(jù)規(guī)劃下發(fā)的全局路徑及自車實時位置實時計算方向盤轉角，控制車輛精準泊車入庫。路徑跟蹤模塊是自動泊車的關鍵技術，提高跟蹤的精度可避免車輛在泊車入庫過程中與周邊車位剮蹭。

國內外針對泊車路徑跟蹤技術的研究有很多，主要使用的算法有比例-積分-微分（Proporti- onal Integral Derivative, PID）控制、滑膜控制、模糊控制、線性二次型調節(jié)器（Linear Quadratic Regulator, LQR）和模型預測控制（Model Predictive Control, MPC）等[1-8]。文獻[1]提出基于前饋+PID反饋的橫向控制模型，相比純PID控制提高了路徑跟蹤精度。文獻[2-4]基于終端滑模理論推導了非時間參考的泊車路徑跟蹤滑膜控制律，提高了泊車路徑跟蹤控制的魯棒性。文獻[5]采用模糊控制的方法對車輛路徑進行跟蹤控制?；谀：刂频穆窂礁櫩刂品椒ㄊ歉鶕?jù)駕駛員及專家知識經(jīng)驗，選取合適的隸屬度函數(shù)和模糊邏輯以及變量值作為模糊控制器的輸入量和輸出量。文獻[6]設計了LQR反饋控制器來解決泊車路徑跟蹤問題，選擇改進的遺傳算法對LQR控制器的加權矩陣對角參數(shù)進行優(yōu)化，文獻[7]和文獻[8]分別采用線性和非線性的模型預測控制模型進行自動泊車的路徑跟蹤，非線性的方法相比線性的方法跟蹤精度更高，但是算法的實時性有所下降。

城市中的垂直車位較為常見，以直線-圓弧-回旋曲線生成垂直車位的全局路徑，作為路徑跟蹤算法的輸入對路徑跟蹤算法進行驗證。本文提出以預瞄點的誤差作為PID控制器的輸入，實時糾正車輛在入庫過程中的誤差，在保證實時性的前提下提高了路徑跟蹤的精度。

1 垂直泊車路徑規(guī)劃方法

基于直線-圓弧規(guī)劃的幾何法是自動泊車常用的路徑規(guī)劃算法，其原理簡單、算法復雜度低、實時性好。但是在直線和圓弧連接處存在曲率不連續(xù)的情況，提高了路徑跟蹤算法的跟蹤難度，為減少因路徑規(guī)劃帶來的不利影響，本文采取回旋曲線對曲率進行優(yōu)化，以生成曲率連續(xù)的泊車全局路徑。

1.1 基于直線-圓弧法的垂直泊車路徑規(guī)劃

基于直線-圓弧法的垂直泊車路徑規(guī)劃方法有一段式、兩段式和三段式等。一段式方法較為簡單，可一次性倒車入庫，但其對行車通道的寬度有一定要求，若行車通道寬度較窄或車輛起始位置距離車位上邊沿的橫向距離較近，則無法通過一段式的方法入庫。本文假設行車通道寬度滿足要求，以一段式的方法進行垂直泊車的路徑規(guī)劃。

首先，對泊車的局部地圖進行建模。如圖1所示，以車位左下角1為坐標原點建立坐標系111。圖1中，為道路寬度；為車位長度；為車位寬度；1、2、3分別為安全距離要求，人為設置但需大于等于0；為車輛在泊車轉向起始點距離車位上邊界的距離。結合車輛自身參數(shù)和以上參數(shù)可計算得出一段式泊車入庫的三個關鍵點：倒車轉向起始點1、泊車入庫時車身與庫位平行的點2和泊車過程的終點3。

圖1 泊車過程建模示意圖

泊車過程中存在碰撞風險的點主要有三個，分別為車輛下邊界與車位下邊界、車位右上角與車身右輪廓、車輛左前角與行車通道上邊界，這三個碰撞點分別與1、2、3有關。停車場的寬度通常在6 m左右，車輛左前角與行車通道上邊界發(fā)生碰撞的概率比較低，而通過設置1可有效地控制車輛不與車位下邊界發(fā)生碰撞，故需要重點分析車位右上角的碰撞風險，該點的碰撞風險與和2有關，越小則該點的碰撞風險越大。設該點不發(fā)生碰撞的最小值為min，參考文獻[9]，可計算出以一段式入庫min的表達式：

當xr,yr)。當=min時，可計算出r的坐標如下：

設1、2、3的坐標分別為(x1,y1)、(x2,y2)、(x3,y3)，則三點的具體坐標表達式如下：

1.2 基于回旋曲線的優(yōu)化

基于直線-圓弧的泊車路徑規(guī)劃方法較為簡單，但是實際中若想要跟蹤這條路徑，車輛需要在1處停車，將方向盤右打滿后開始倒車，在2點處停車，回正方向盤后再倒車。這不符合駕駛員的倒車習慣，會增加泊車的時間，且原地轉向會加劇輪胎的磨損，故需要優(yōu)化。該問題的根本在于直線的曲率為0，圓弧的曲率為1/min，路徑在直線和圓弧拼接處的曲率不連續(xù)。因此，本文提出基于回旋線拼接圓弧的方法，在圖1的1點附近加入一段回旋曲線使曲率由0緩慢增加至1/min，在2點附近加入一段回旋曲線使曲率由1/min緩慢下降至0，以解決泊車路徑曲率不連續(xù)的問題。

回旋曲線是曲率隨著曲線長度線性增加的一類曲線。設曲率變化率為，其表達式如下：

式中，p為泊車過程的最高車速，一般≤1 m/s；max為前輪最大轉角，根據(jù)實車數(shù)據(jù)，max=30°；max為前輪轉角最大變化率，經(jīng)實測，取max=30 °/s；H的值越大，回旋曲線曲率變化越緩慢，根據(jù)以上分析，取H=1.5 m。

根據(jù)下式可計算回旋曲線的散點位姿，其中、分別為橫縱坐標，為航向角。

參考文獻[3]使用回旋曲線對直線-圓弧的連接處M1點和M2點進行拼接，以生成曲率連續(xù)的全局參考路徑。經(jīng)回旋曲線優(yōu)化后車輛的路徑如圖2所示，可以看出，泊車全局路徑緩慢連續(xù)變化。

2 基于預瞄PID的泊車橫向控制算法

規(guī)劃模塊下發(fā)車輛在泊車坐標系的全局參考路徑后，定位模塊可給出車輛在泊車坐標系的實際位置，根據(jù)全局路徑和實時位姿可計算出車輛的橫向誤差，橫向跟蹤模塊需實時調整方向盤轉角的輸出來糾正橫向誤差，使智能車精準地沿全局參考路徑行駛。以車輛的后軸中心點處的橫向誤差作為PID的輸入進行反饋控制具有一定的滯后性，本文采用一定的預瞄距離計算預瞄點，以預瞄點向全局路徑做投影，找到相應的參考點并計算預瞄點的橫向誤差，以預瞄點的橫向誤差作為預瞄誤差，并采用PID控制器對預瞄誤差進行實時反饋修正。

2.1 預瞄點選取

圖3中111為泊車坐標系，為車輛后軸中心坐標系，車輛后軸中心點的位置為(,)，為汽車朝向與軸的夾角，可由定位模塊直接給出。(prepre)為預瞄點的坐標，(rr)為參考點的坐標，pre為從后軸中心出發(fā)沿汽車行駛方向的預瞄距離，倒車時pre取負值，pre為預瞄誤差，因泊車速度較低，故預瞄距離采用固定值，通過仿真試驗調整而得。

圖3 預瞄點選擇示意圖

預瞄點可由下式計算：

2.2 參考點選擇

全局參考路徑是如圖4所示的離散點，已知汽車當前位置的預瞄點0，需在離散點中找到當前位置的參考點ref。遍歷離散點可找到最近匹配點m當離散點很稠密時，m可等效為ref，但是當離散點稀疏時，m0與m點的切線方向不垂直，導致計算出的橫向誤差存在偏差。為解決該問題，取m前后兩個離散點1、2，設過1、2的直線斜率為l，1、2點的坐標分別為(x1,y1)、(x2,y2)，則l的表達如下：

設參考點r的坐標為(rr)，則r和r的表達如下：

除此之外，預瞄誤差的計算還需要參考點的航向角r。分別用12表示點2、r距離點1的距離。

令221，以1和2分別表示點1、2的航向角，則r的表達如下：

2.3 基于預瞄誤差的PID橫向控制模型

得到預瞄點和參考點后，計算兩點之間的距離可得到預瞄點的橫向誤差，但這樣無法區(qū)分正負，故采用如下式來求預瞄誤差：

以預瞄誤差作為PID控制的輸入，前輪轉角作為輸出，參考PID模型表達式，可建立基于預瞄PID的泊車橫向控制模型。前輪轉角輸出值需滿足

使用CarSim和Simulink進行仿真時可直接控制車輛的左右前輪轉角，前輛等效前輪轉角與左右前輪轉角需滿足阿克曼轉向關系，如式（17）、式（18）所示，以保證車輛在轉彎時內外輪的軌跡接近同心圓，避免輪胎過度磨損。

式中，l、f分別為左、右前輪的轉角；a為車輛前輪的輪距；為車輛的軸距。

進行實車控制時，控制量為方向盤轉角，方向盤轉角與前輪轉角之間可參考下式進行換算。

式中，為轉向系的傳動比，可通過車輛出廠時的試驗標定得到；s為方向盤轉角。

3 仿真及驗證

基于CarSim和Simulink進行泊車橫向控制的聯(lián)合仿真，車輛及車位的參數(shù)如表1所示。在CarSim中搭建泊車的場景及車輛動力學模型，在Simulink中搭建橫向控制模型，泊車路徑由MATLAB的工作空間導入。泊車橫向控制器的輸出為車輛的左、右前輪轉角，輸入后軸中心點實時位置、航向角及參考路徑。CarSim車輛模型輸出的位置為車輛前軸中心點的坐標，故需結合軸距、實時航向角計算車輛后軸中心點的位置。車輛及車位的參數(shù)如表1所示。

表1 車輛參數(shù)表

參數(shù)符號定義取值 Wc車輛寬度/mm1 600 Wa前輪距離/mm1 415 lf前軸與車輛前端的距離/mm1 200 lr后軸與車輛后端的距離/mm1 200 l前后軸之間的距離/mm2 600 LcWWcWcδmaxi車輛長度/mm車位寬度/mm車位長度mm/行車通道寬度/mm最大前輪轉角/(°)轉向系傳動比/mm5 0002 6005 5006 0003016.8

CarSim可以回放車輛仿真過程的實時動畫及車輛運動的拖影圖，泊車過程的車輛輪廓圖如圖5所示，可以看出車輛可以平穩(wěn)且無碰撞地泊車入庫。

圖5 泊車過程車輛拖影圖

本文與同樣基于預瞄距離進行控制的純跟蹤算法進行了對比，從橫向誤差和航向誤差兩個角度分析兩個算法的跟蹤精度。純跟蹤算法的原始輸出存在抖動，本文對其控制量采用一階濾波算法進行平滑處理，控制量的平滑度及路徑跟蹤的精度都有較大的提升。預瞄PID和純跟蹤算法的實際路徑與參考路徑地對比如圖6所示?？梢钥闯鰞蓚€算法在圓弧倒車階段的路徑跟蹤效果比較好，但在車輛與庫位接近平行的直線倒車階段，純跟蹤算法誤差較大，實際路徑表現(xiàn)為輕微的曲線。在泊車的終點位置，預瞄PID的跟蹤誤差更小，預瞄PID和純跟蹤算法的橫向誤差分別為-0.01 m和0.08 m。

圖6 實際行駛路徑與參考路徑對比圖

圖7 橫向誤差對比圖

圖8 航向誤差對比圖

圖7和圖8展示了兩種算法在泊車過程中的橫向誤差和航向誤差，在橫向誤差方面，預瞄PID的橫向誤差在?0.03～0.005 m，純跟蹤算法的橫向誤差在?0.05～0.1 m，預瞄PID在橫向誤差上的表現(xiàn)要優(yōu)于純跟蹤算法。在航向誤差方面，預瞄PID的航向誤差在?0.8～4.5°，終點位置的航向誤差為0.9°；純跟蹤算法的航向誤差在?1.1～4°，終點位置的航向誤差為?1.1°，兩者相差不大。

圖9和圖10分別展示了兩種算法前輪轉角的輸出及前輪轉角的變化率。在剛開倒車階段，預瞄PID算法幾乎以最大的前輪轉角變化率增大前輪轉角，前輪轉角以很快的速度收斂到接近最大值，在方向盤回正階段，預瞄PID算法比純跟蹤在更早的時刻便開始回正，且回正后前輪轉角沒有出現(xiàn)振蕩。而純跟蹤算法回正的時間較為滯后，以此造成回正過程的橫向誤差較大，需要以較大的前輪轉角變化率來回正，故回正后的直線階段控制量超調，實際路徑出現(xiàn)曲線。

圖9 前輪轉角輸出對比圖

圖10 前輪轉角變化率對比圖

4 結論

采用回旋曲線-直線-圓弧拼接的方法生成了垂直車位下曲率連續(xù)變化的泊車路徑，針對自動泊車場景車速低、路徑曲率大等特點設計了基于預瞄誤差的PID路徑跟蹤控制器，最后通過CarSim和Simulink聯(lián)合仿真，驗證了路徑跟蹤控制器的有效性。

通過對仿真結果的分析，可得出結論：以預瞄PID算法進行自動泊車的路徑跟蹤控制，可得到較好的跟蹤效果。該算法相比其他路徑跟蹤控制算法具有一定優(yōu)越性，其相比于純跟蹤等其他不基于動力學模型的算法，跟蹤精度更高，相比于MPC等基于動力學模型的算法，其算法簡單、實時性好、對控制器算力要求更低。

[1] 杜文豪.自動平行泊車的路徑規(guī)劃及其跟蹤控制研究[D].重慶:重慶大學,2019.

[2] 張家旭,王晨,趙健,等.面向狹小平行泊車位的路徑規(guī)劃與跟蹤控制[J].吉林大學學報(工學版),2021,51 (5):1879-1886.

[3] 張家旭,趙健,施正堂,等.基于回旋曲線的垂直泊車軌跡規(guī)劃與跟蹤控制[J].東南大學學報(自然科學版),2020,50(1):182-191.

[4] 張家旭,趙健,施正堂,等.基于回旋曲線的平行泊車路徑規(guī)劃和跟蹤控制[J].吉林大學學報(工學版), 2020,50(6):2247-2257.

[5] 尤田田.自動泊車路徑規(guī)劃及跟蹤控制方法研究[D].合肥:合肥工業(yè)大學,2021.

[6] 盧慶志.基于遺傳算法的自動泊車路徑規(guī)劃及跟蹤控制研究[D].西安:長安大學,2020.

[7] 胡遠志,何朋聲,劉西.基于模型預測控制的平行泊車控制方法[J].重慶理工大學學報(自然科學),2020,34 (10):1-8.

[8] 顧青,白國星,孟宇,等.基于非線性模型預測控制的自動泊車路徑跟蹤[J].工程科學學報,2019,41(7): 947-954.

[9] 孫思.自動垂直泊車系統(tǒng)轉向控制策略的研究[D].北京:清華大學,2015.

Analysis of Automatic Parking Path Tracking Based on Preview PID

LUO Man1, XIONG Shengjian1, SUI Liyang2, LU Ruoyu1, JIANG Hui*1

( 1.Dongfeng Yuexiang Technology Company Limited, Wuhan 430058, China;2.College of Automotive Engineering, Wuhan University of Technology, Wuhan 430070, China )

In order to reduce the error of path tracking, a proportional integral derivative (PID) controller based on preview error is designed. Firstly, a global path with continuous curvature in vertical parking scenarios is generated by splicing line-arc-cyclotron curves. Secondly, the calculation method of preview point, reference point and preview error is designed, and the preview error is used as the input of PID to design the path tracking controller. Finally, the preview PID algorithm is verified by Simulink/CarSim co-simulation. The simulation and test results show that the path tracking algorithm based on preview PID proposed in this paper is simple in principle, but has high tracking accuracy, which can meet the requirements of automatic parking.

Automatic parking; Vertical parking; Path tracking; Preview error; PID;Simulink/CarSim

U495

A

1671-7988(2023)11-66-06

駱嫚（1986－），女，博士研究生，工程師，研究方向為自動駕駛規(guī)劃、控制、感知，E-mail:tc-luoman @dfmc.com.cn。

江輝（1998－），男，研究方向為自動駕駛規(guī)劃、控制，E-mail:jianghui_ecu@163.com。

10.16638/j.cnki.1671-7988.2023.011.011