路徑跟蹤控制算法仿真分析與試驗(yàn)驗(yàn)證*

趙穎 俞庭 張琪 楊洪濤 宋勝

（西南大學(xué)，重慶 400715）

主題詞：路徑跟蹤 預(yù)瞄理論 純跟蹤算法 前輪反饋控制算法 模型預(yù)測控制算法

1 前言

隨著車輛智能駕駛技術(shù)的快速進(jìn)步，底層架構(gòu)的路徑跟蹤控制算法不斷發(fā)展。目前廣泛應(yīng)用的算法主要包括純跟蹤（Pure Pursuit，PP）算法、前輪反饋控制算法、后軸反饋算法和模型預(yù)測控制（Model Predictive Control，MPC）算法等。

張龑等人提出了一種基于速度自適應(yīng)預(yù)瞄的無模型控制算法，對(duì)于高度復(fù)雜的跟蹤路徑，車輛在低速和高速條件下均具有穩(wěn)定的跟蹤性能。靳欣宇提出了基于前輪反饋控制算法的自適應(yīng)最優(yōu)預(yù)瞄模型，能夠滿足車輛在復(fù)雜道路上的魯棒性要求。劉毅等人通過添加PID控制器和低通濾波器使純跟蹤算法得到改善，獲得了較好的跟蹤效果。Abdelmoniem 等人基于車輛未來狀態(tài)的離散預(yù)測模型，允許在控制器輸出增強(qiáng)的未來預(yù)測狀態(tài)時(shí)使用前輪反饋控制算法，減小了橫向誤差，提高了平均偏航穩(wěn)定性。張涌等人提出基于預(yù)瞄控制和模糊滑?？刂频能囕v橫向控制算法，該融合算法使車輛路徑跟蹤的魯棒性得到明顯提高。王瑞等人提出了基于純跟蹤算法和樽海鞘優(yōu)化算法的路徑跟蹤算法，跟蹤準(zhǔn)確性與車輛運(yùn)動(dòng)穩(wěn)定性得到了提高。Zhang等人基于實(shí)時(shí)更新系統(tǒng)模型，提出了一種模型預(yù)測控制方案，以實(shí)現(xiàn)路徑跟蹤，使車輛在不同駕駛條件下?lián)碛懈玫倪m應(yīng)性。吳施鵬等人基于二自由度車輛單軌模型，在傳統(tǒng)MPC 算法基礎(chǔ)上采用遺傳算法尋找車輛行駛最優(yōu)時(shí)域參數(shù)，減小了車輛跟蹤誤差，同時(shí)加入了前輪側(cè)偏角約束，提升了車輛在低附著系數(shù)路面和高速路面上的行駛穩(wěn)定性。范賢波等人設(shè)計(jì)了一種基于MPC 的自適應(yīng)軌跡跟蹤控制器，采用模糊控制對(duì)預(yù)測時(shí)域和控制時(shí)域進(jìn)行了在線優(yōu)化，實(shí)現(xiàn)預(yù)測時(shí)域可根據(jù)橫向和縱向車速進(jìn)行自適應(yīng)選擇，提高了軌跡跟蹤精度和穩(wěn)定性。Xu等人基于MPC和預(yù)覽跟隨器理論提出了一種軌跡跟蹤方案，計(jì)算了預(yù)覽點(diǎn)處的參考橫向加速度以更新狀態(tài)變量，并在每個(gè)預(yù)測點(diǎn)生成了參考偏航率，同時(shí)將MPC問題轉(zhuǎn)化為線性時(shí)變的MPC控制器，構(gòu)建了連續(xù)在線線性化預(yù)測模型。

上述文獻(xiàn)雖對(duì)不同算法在提升車輛路徑跟蹤精度方面進(jìn)行了研究，但并未對(duì)不同算法在不同工況下的適用性規(guī)律進(jìn)行探究。本文分別構(gòu)建基于PP 算法、前輪反饋控制算法和MPC算法的車輛模型，采用Simulink和CarSim聯(lián)合仿真分析的方法，考慮不同道路工況和不同速度工況下3種算法的軌跡跟蹤效果，并進(jìn)行實(shí)車試驗(yàn)對(duì)比驗(yàn)證3種算法的跟蹤性能。

2 理論模型

2.1 車輛轉(zhuǎn)向模型

為了達(dá)到理想的傳動(dòng)效果，使車輛在轉(zhuǎn)向過程中各車輪處于純滾動(dòng)狀態(tài)，采用車輛阿克曼轉(zhuǎn)向模型，如圖1所示。

圖1 阿克曼轉(zhuǎn)向模型

由圖1可得：

式中，、分別為車輛內(nèi)、外側(cè)車輪轉(zhuǎn)向角；為車輛軸距；為轉(zhuǎn)向半徑；為后輪距。

由幾何關(guān)系可知，車輛等效轉(zhuǎn)向角為：

2.2 算法理論

2.2.1 基于預(yù)瞄的純跟蹤算法

基于預(yù)瞄的純跟蹤算法的核心在于基于預(yù)瞄路徑與當(dāng)前車輛位置以及轉(zhuǎn)向盤轉(zhuǎn)角之間所建立的幾何關(guān)系，推導(dǎo)出車輛當(dāng)前位置與預(yù)瞄路徑點(diǎn)的關(guān)系式，從而得出當(dāng)前位置的轉(zhuǎn)向盤轉(zhuǎn)角。圖2所示為基于預(yù)瞄路徑的純跟蹤算法模型示意，在阿克曼轉(zhuǎn)向模型的基礎(chǔ)上引入預(yù)瞄路徑，在預(yù)瞄路徑上取預(yù)瞄點(diǎn)，車輛位置參考點(diǎn)為車輛后軸中心點(diǎn)。

圖2 基于預(yù)瞄的純跟蹤算法模型

由圖2可知，基于車輛位置參考點(diǎn)和預(yù)瞄點(diǎn)擬合圓得到轉(zhuǎn)向盤轉(zhuǎn)角，進(jìn)而控制車輛參考點(diǎn)通過預(yù)瞄路徑點(diǎn)，最終將車輛約束在參考路徑上。

為使車輛參考點(diǎn)順利通過預(yù)瞄路徑點(diǎn)，現(xiàn)控制兩點(diǎn)位于同一圓上，其中，圓的半徑為車輛基于阿克曼轉(zhuǎn)向模型的轉(zhuǎn)向半徑，此時(shí)前輪轉(zhuǎn)角為，擬合路線半徑為，預(yù)瞄距離為，由幾何關(guān)系可得扇形圓心角為2。

根據(jù)幾何關(guān)系可得：

代入式（3）可得車輛轉(zhuǎn)向角為：

2.2.2 前輪反饋控制算法

前輪反饋控制算法是基于簡單車輛運(yùn)動(dòng)學(xué)模型的直觀轉(zhuǎn)向控制律的算法模型，同樣使用了阿克曼轉(zhuǎn)向模型，其幾何模型如圖3所示。

圖3 前輪反饋控制算法幾何模型

前輪反饋控制算法車輛位置參照點(diǎn)為車輛前軸與車輛縱向中心軸的交點(diǎn)。與基于預(yù)瞄的純跟蹤算法不同，該算法采用目標(biāo)路徑上與點(diǎn)距離最近的點(diǎn)為投影點(diǎn)。設(shè)、之間的距離偏差為，車速為，車輛中軸線與路徑切向夾角為，橫向位置偏差補(bǔ)償轉(zhuǎn)角為，車輛前輪實(shí)際轉(zhuǎn)向角為，分別受車輛當(dāng)前預(yù)瞄軌跡中預(yù)瞄路徑點(diǎn)處的切向角和橫向位置偏差補(bǔ)償轉(zhuǎn)角影響。假設(shè)車輛預(yù)期軌跡在距離處與給定路徑最近點(diǎn)切線相交，其中與速度相關(guān)，引入增益代表橫向位置偏差在車輛行駛速度下的影響程度，根據(jù)幾何關(guān)系可得：

針對(duì)式（6），按照誤差比例調(diào)整橫向偏差：當(dāng)與同時(shí)增大時(shí)，轉(zhuǎn)向反饋更強(qiáng)烈；較小至收斂至0時(shí)，決定了收斂速率。在高速工況下，應(yīng)選取較大的取值以保證橫向誤差對(duì)于前輪轉(zhuǎn)角的控制效果；在低速工況下，應(yīng)選取較小的取值以獲得穩(wěn)定的轉(zhuǎn)向反饋效果。

則前輪反饋控制算法的幾何關(guān)系可表示為：

2.2.3 模型預(yù)測控制算法

MPC 是一種基于預(yù)測模型的閉環(huán)優(yōu)化控制策略。MPC 算法包括預(yù)測模型、滾動(dòng)優(yōu)化、反饋控制3個(gè)部分。

由阿克曼轉(zhuǎn)向模型可知，車輛轉(zhuǎn)彎半徑為：

圖4 所示為車輛運(yùn)動(dòng)模型，其中、為大地坐標(biāo)系下車輛質(zhì)心的坐標(biāo)，為大地坐標(biāo)系下的車輛航向角，為前輪轉(zhuǎn)角，由圖4可知，車輛的狀態(tài)量為、和。由狀態(tài)量推導(dǎo)出狀態(tài)空間為：

圖4 車輛運(yùn)動(dòng)模型

顯然，上述狀態(tài)空間呈現(xiàn)非線性，為了采用模型預(yù)測控制算法，需要將其處理為線性模型。

線性狀態(tài)空間關(guān)系為：

由于處理后的線性模型是連續(xù)模型，仍不能用于最終的模型預(yù)測控制，還需對(duì)其進(jìn)行離散化。此時(shí)：

式中，為采樣周期。

此時(shí)，用于預(yù)測控制的離散模型構(gòu)建完成，得到線性時(shí)變的狀態(tài)空間：

目標(biāo)函數(shù)形式為：

目標(biāo)函數(shù)用來計(jì)算未來一段時(shí)間系統(tǒng)的輸出值，基于線性誤差模型預(yù)測車輛未來時(shí)刻輸出的目標(biāo)函數(shù)。

將式（15）簡化為：

狀態(tài)空間采樣矩陣；=[Δ() Δ(+1) Δ(+2) …Δ(+-1)]為控制量偏差變化量采樣矩陣。

優(yōu)化模型的目標(biāo)是使當(dāng)前狀態(tài)盡快收斂到參考值，即盡可能收斂到，同時(shí)輸入量的改變量盡可能小，即趨向于0。為量化各參數(shù)對(duì)控制性能的影響程度，通過求加權(quán)平方和的方式來體現(xiàn)：

式中，為半正定狀態(tài)加權(quán)矩陣；為正定的控制加權(quán)矩陣。

控制的目的是使式（17）的值盡可能小。但式（17）并不是二次規(guī)劃問題，其涉及的控制量過多，而在實(shí)際控制中，控制量為輸入。定義偏差為：

計(jì)算整理后有：

式（19）可寫為：

式中，=+；=。

則式（17）可改寫為：

此時(shí)，目標(biāo)函數(shù)轉(zhuǎn)為標(biāo)準(zhǔn)二次規(guī)劃問題，將優(yōu)化目標(biāo)函數(shù)的問題表示為以為變量求解該二次規(guī)劃使得目標(biāo)函數(shù)最小的問題。式（21）的約束條件為≤≤，、分別為控制量的下界和上界。通過解決二次規(guī)劃問題，可求解控制量，并在中提取第1 項(xiàng)輸入量()，將其輸出至控制車輛以實(shí)現(xiàn)對(duì)車輛的模型預(yù)測控制。

3 仿真分析

3.1 基于預(yù)瞄的純跟蹤算法

由前文可知，基于預(yù)瞄的純跟蹤算法的預(yù)瞄距離是影響橫向誤差的主要因素之一，因此，針對(duì)純跟蹤算法，車速分別選取5 km/h、50 km/h、80 km/h、120 km/h，在低、中、高速工況下改變預(yù)瞄距離進(jìn)行探究。仿真圓環(huán)道路模型和蛇行道路模型如圖5所示。共設(shè)計(jì)5組預(yù)瞄距離，最小預(yù)瞄距離應(yīng)大于車輛最小轉(zhuǎn)彎半徑，普通車輛最小轉(zhuǎn)彎半徑約為5 m，預(yù)瞄距離增加至15 m且車速120 km/h 時(shí)橫向誤差將超過1 m，跟蹤效果較差。因此，選取預(yù)瞄距離分別為5 m、8 m、10 m、12 m、15 m。仿真結(jié)果分別如圖6、圖7所示。

圖5 仿真道路模型

圖6 圓環(huán)道路不同預(yù)瞄距離下純跟蹤算法的橫向誤差

圖7 蛇行道路不同預(yù)瞄距離下純跟蹤算法的橫向誤差

由圖6、圖7 可知，圓環(huán)道路工況下，車輛行駛時(shí)橫向誤差逐漸收斂于確定的值?；陬A(yù)瞄的純跟蹤算法中車輛基于預(yù)瞄點(diǎn)的橫向誤差影響的大小，進(jìn)而影響車輛轉(zhuǎn)向角。圓環(huán)道路曲率固定為0.002 m，算法為控制車輛在參考軌跡上穩(wěn)定行駛，使車輛的橫向誤差逐漸收斂于確定的值。蛇行道路工況下，道路曲率規(guī)律性變化，橫向誤差也隨之規(guī)律性變化，80 km/h 和120 km/h 工況下，隨著預(yù)瞄距離的增大，橫向誤差的振蕩不斷減小，車輛跟蹤穩(wěn)定性提升。預(yù)瞄距離越短，最終收斂的橫向誤差越小，即算法跟蹤的精度越高，但收斂變慢，隨預(yù)瞄距離的增加，收斂橫向誤差逐漸增大，誤差收斂時(shí)間逐漸縮短，即算法的穩(wěn)定性提高。增大預(yù)瞄距離可使算法獲得更強(qiáng)的穩(wěn)定性，但預(yù)瞄距離過大會(huì)使橫向誤差和誤差收斂時(shí)間變大，跟蹤效果變差。

基于此，橫向誤差可以通過縮短預(yù)瞄距離的方式加以限制，部分文獻(xiàn)中選取預(yù)瞄距離為：

式中，=1/(2)；為車輛最大制動(dòng)加速度；為最短制動(dòng)距離；為車輛遇到異常時(shí)需要的反應(yīng)時(shí)間；為車輛的最小轉(zhuǎn)彎半徑。

3.2 前輪反饋控制算法

增益系數(shù)是影響前輪反饋控制算法控制性能的重要因素之一。因此，選取不同增益系數(shù)的取值（0.1、0.5、1.0、2.0、5.0、10.0、15.0、35.0、50.0），在低、中、高速（5 km/h、50 km/h、80 km/h、120 km/h）工況下，進(jìn)行圓環(huán)及蛇行道路仿真分析，仿真結(jié)果分別如圖8～圖10所示。

圖8 圓環(huán)道路不同增益系數(shù)下前輪反饋控制算法橫向誤差

圖9 蛇行道路不同增益系數(shù)下前輪反饋控制算法橫向誤差

圖10 圓環(huán)道路k=Dv+E下前輪反饋控制算法的橫向誤差

由圖8～圖10可知，在所選的不同車速工況條件下，值的選取對(duì)前輪反饋控制算法橫向控制的性能皆有顯著影響。在同一增益系數(shù)、不同速度工況條件下，橫向誤差均收斂于一定的誤差范圍內(nèi)。5 km/h 車速工況條件下，仿真車輛橫向誤差曲線出現(xiàn)振蕩，且振幅隨著增益系數(shù)的增加而增加。其原因是此時(shí)式（7）中與速度的比值較大，進(jìn)而橫向誤差對(duì)車輛轉(zhuǎn)角的影響較大。同理，=0.1,0.5,1.0,2.0 時(shí)，圓環(huán)道路120 km/h 車速、蛇行道路80 km/h、120 km/h車速工況下，橫向誤差不斷累積增大，無法收斂，原因在于較小的增益系數(shù)會(huì)降低橫向誤差對(duì)車輛轉(zhuǎn)角的影響，使橫向誤差無法收斂。同一速度下，增益系數(shù)與橫向誤差并非簡單的線性關(guān)系。4種速度下，算法可找出與速度相關(guān)的最優(yōu)增益系數(shù)（由于＜5時(shí)高速工況下橫向誤差無法收斂，僅考慮≥5的5組），可進(jìn)一步探尋最優(yōu)增益系數(shù)以獲得最小誤差?；诖耍瑸槭骨拜喎答伩刂扑惴ň哂懈训目刂菩阅?，可使增益與車輛速度呈現(xiàn)一定的函數(shù)關(guān)系，使算法在不同速度工況下得出最佳的增益。對(duì)此，進(jìn)行進(jìn)一步仿真探究，根據(jù)上述4組不同速度工況下相對(duì)應(yīng)的最佳增益系數(shù)，提出增益與速度的一次函數(shù)擬合關(guān)系：

式中，、為可調(diào)節(jié)的常數(shù)。

由仿真數(shù)據(jù)進(jìn)行擬合得到不同速度下達(dá)到最小橫向誤差時(shí)增益系數(shù)的取值。進(jìn)一步，通過一次項(xiàng)直線擬合得出=0.388、=4.125時(shí)，在不同速度工況下可得到最小橫向誤差。該一次函數(shù)擬合關(guān)系下，4種速度下的橫向誤差分別為0.065 47 m、0.020 43 m、0.015 34 m、0.055 69 m，均低于對(duì)應(yīng)速度下增益系數(shù)=5,10,15,35,50時(shí)的橫向誤差。因此，通過本文所提出的增益與速度的一次函數(shù)擬合關(guān)系，可計(jì)算出對(duì)應(yīng)速度下的增益，使算法達(dá)到更好的控制效果。

3.3 MPC算法

MPC 算法在5 km/h、50 km/h、80 km/h、120 km/h 車速工況下，圓環(huán)及蛇行路徑下的仿真結(jié)果分別如圖11、圖12所示。

圖11 圓環(huán)道路下MPC算法的橫向誤差

圖12 蛇行道路下MPC算法的橫向誤差

由圖11、圖12可知，MPC算法表現(xiàn)出優(yōu)異的跟蹤效果，4種速度下的橫向誤差均維持在一定范圍內(nèi)。相較于純跟蹤及前輪反饋控制算法，MPC算法在各速度工況下皆具有更優(yōu)的控制性能，橫向誤差更小，蛇行彎道下橫向誤差的波動(dòng)更小，具有更好的魯棒性。在5 km/h的低速工況下，MPC算法橫向誤差小于性能較好的純跟蹤算法橫向誤差。在120 km/h的高速工況下，MPC算法橫向誤差小于高速情況下性能較好的前輪反饋控制算法橫向誤差。其原因是，MPC算法具有良好的橫、縱向耦合控制性能，能夠?qū)Ψ抡孳囕v的速度進(jìn)行控制，使得算法具有良好的預(yù)測作用。當(dāng)車輛需在彎道轉(zhuǎn)向時(shí)，MPC算法能夠在車輛到達(dá)彎道轉(zhuǎn)向前進(jìn)行減速控制，使車輛在彎道轉(zhuǎn)向時(shí)不至于失穩(wěn)，轉(zhuǎn)向過程更加平滑。

綜上，相較于純跟蹤及前輪反饋控制算法，MPC 算法具有更好的路徑跟蹤控制性能。在高速及低速工況下，橫向誤差更小，算法的魯棒性更好，適宜各速度工況。

4 實(shí)車試驗(yàn)

4.1 試驗(yàn)平臺(tái)與場地

本文選用搭載高精度全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System，GNSS）/慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）的無人駕駛平臺(tái)進(jìn)行試驗(yàn)，該平臺(tái)如圖13 所示。試驗(yàn)場地為某400 m 運(yùn)動(dòng)場圓弧跑道，試驗(yàn)運(yùn)動(dòng)場局部如圖14 所示。受試驗(yàn)條件制約，在5 km/h速度工況下進(jìn)行算法路徑跟蹤試驗(yàn)。

圖13 無人駕駛平臺(tái)

圖14 試驗(yàn)運(yùn)動(dòng)場局部示意

4.2 實(shí)車試驗(yàn)結(jié)果

由于GNSS/INS 組合導(dǎo)航系統(tǒng)可提供連續(xù)、可靠和完備的高精度導(dǎo)航參數(shù)，選用GNSS/INS 組合導(dǎo)航獲取的定位數(shù)據(jù)作為車身位置。試驗(yàn)結(jié)果如圖15所示。

圖15 跟蹤算法試驗(yàn)橫向誤差曲線

由圖15可知，MPC算法具有更小的橫向誤差、更好的跟蹤性能。以純跟蹤算法橫向誤差為基準(zhǔn)，MPC算法實(shí)車試驗(yàn)中的橫向誤差較小，橫向最大誤差僅0.02 m左右，這是由于MPC 算法能夠?qū)囕v的橫向偏差進(jìn)行預(yù)測，使得車輛跟蹤誤差能夠迅速減小的同時(shí)避免控制的發(fā)散，算法能發(fā)揮更優(yōu)良的預(yù)測作用。由此實(shí)車試驗(yàn)驗(yàn)證了該工況下MPC算法具有更好的路徑跟蹤控制性能。

5 結(jié)束語

本文對(duì)基于預(yù)瞄的純跟蹤算法、前輪反饋控制算法、MPC 算法3 種控制算法進(jìn)行CarSim-Simulink 聯(lián)合仿真分析，并通過試驗(yàn)進(jìn)行仿真結(jié)果驗(yàn)證。分析結(jié)果顯示：基于預(yù)瞄的純跟蹤算法在車輛低速工況下的路徑跟蹤魯棒性較好，高速橫向誤差較大，預(yù)瞄距離是影響其跟蹤性能的主要因素之一；相較于基于預(yù)瞄的純跟蹤算法，前輪反饋控制算法在高速工況下的路徑跟蹤效果具有更好的魯棒性；MPC 算法具有更好的橫向控制性能，在高速和低速工況下皆具有更優(yōu)的預(yù)測效果。

本文僅對(duì)道路車輛的路徑跟蹤橫向控制進(jìn)行了研究，后續(xù)將針對(duì)崎嶇、顛簸等復(fù)雜道路工況下，綜合考慮橫、縱向等多方向，對(duì)車輛的多向控制進(jìn)行研究，進(jìn)一步提升車輛路徑跟蹤效果的同時(shí)增強(qiáng)魯棒性。所研究的3種算法在不同工況下可表現(xiàn)出差異化的跟蹤性能，在實(shí)際運(yùn)行過程中，車輛面對(duì)不同道路狀況的變化、車速限制等，可通過車載傳感器反饋車輛的真實(shí)狀態(tài)，以精確判斷工況，從而實(shí)現(xiàn)算法的自適應(yīng)選擇，這將是后續(xù)研究的重點(diǎn)。