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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖1 泊車過程建模示意圖

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

當(dāng)xr,yr)。當(dāng)=min時，可計算出r的坐標(biāo)如下：

設(shè)1、2、3的坐標(biāo)分別為(x1,y1)、(x2,y2)、(x3,y3)，則三點(diǎn)的具體坐標(biāo)表達(dá)式如下：

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

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

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

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

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

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

2 基于預(yù)瞄PID的泊車橫向控制算法

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

2.1 預(yù)瞄點(diǎn)選取

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

圖3 預(yù)瞄點(diǎn)選擇示意圖

預(yù)瞄點(diǎn)可由下式計算：

2.2 參考點(diǎn)選擇

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

設(shè)參考點(diǎn)r的坐標(biāo)為(rr)，則r和r的表達(dá)如下：

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

令221，以1和2分別表示點(diǎn)1、2的航向角，則r的表達(dá)如下：

2.3 基于預(yù)瞄誤差的PID橫向控制模型

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

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

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

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

進(jìn)行實(shí)車控制時，控制量為方向盤轉(zhuǎn)角，方向盤轉(zhuǎn)角與前輪轉(zhuǎn)角之間可參考下式進(jìn)行換算。

式中，為轉(zhuǎn)向系的傳動比，可通過車輛出廠時的試驗(yàn)標(biāo)定得到；s為方向盤轉(zhuǎn)角。

3 仿真及驗(yàn)證

基于CarSim和Simulink進(jìn)行泊車橫向控制的聯(lián)合仿真，車輛及車位的參數(shù)如表1所示。在CarSim中搭建泊車的場景及車輛動力學(xué)模型，在Simulink中搭建橫向控制模型，泊車路徑由MATLAB的工作空間導(dǎo)入。泊車橫向控制器的輸出為車輛的左、右前輪轉(zhuǎn)角，輸入后軸中心點(diǎn)實(shí)時位置、航向角及參考路徑。CarSim車輛模型輸出的位置為車輛前軸中心點(diǎn)的坐標(biāo)，故需結(jié)合軸距、實(shí)時航向角計算車輛后軸中心點(diǎn)的位置。車輛及車位的參數(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最大前輪轉(zhuǎn)角/(°)轉(zhuǎn)向系傳動比/mm5 0002 6005 5006 0003016.8

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

圖5 泊車過程車輛拖影圖

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

圖6 實(shí)際行駛路徑與參考路徑對比圖

圖7 橫向誤差對比圖

圖8 航向誤差對比圖

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

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

圖9 前輪轉(zhuǎn)角輸出對比圖

圖10 前輪轉(zhuǎn)角變化率對比圖

4 結(jié)論

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

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

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

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

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

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

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

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

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

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

[9] 孫思.自動垂直泊車系統(tǒng)轉(zhuǎn)向控制策略的研究[D].北京:清華大學(xué),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

駱?gòu)牐?986－），女，博士研究生，工程師，研究方向?yàn)樽詣玉{駛規(guī)劃、控制、感知，E-mail:tc-luoman @dfmc.com.cn。

江輝（1998－），男，研究方向?yàn)樽詣玉{駛規(guī)劃、控制，E-mail:jianghui_ecu@163.com。

10.16638/j.cnki.1671-7988.2023.011.011