趙穎 俞庭 張琪 楊洪濤 宋勝

（西南大學，重慶 400715）

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

1 前言

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

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

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

2 理論模型

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

為了達到理想的傳動效果，使車輛在轉(zhuǎn)向過程中各車輪處于純滾動狀態(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 基于預瞄的純跟蹤算法

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

圖2 基于預瞄的純跟蹤算法模型

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

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

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

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

2.2.2 前輪反饋控制算法

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

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

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

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

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

2.2.3 模型預測控制算法

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

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

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

圖4 車輛運動模型

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

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

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

式中，為采樣周期。

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

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

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

將式（15）簡化為：

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

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

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

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

計算整理后有：

式（19）可寫為：

式中，=+；=。

則式（17）可改寫為：

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

3 仿真分析

3.1 基于預瞄的純跟蹤算法

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

圖5 仿真道路模型

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

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

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

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

式中，=1/(2)；為車輛最大制動加速度；為最短制動距離；為車輛遇到異常時需要的反應時間；為車輛的最小轉(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）工況下，進行圓環(huán)及蛇行道路仿真分析，仿真結(jié)果分別如圖8～圖10所示。

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

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

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

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

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

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

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)的控制性能，橫向誤差更小，蛇行彎道下橫向誤差的波動更小，具有更好的魯棒性。在5 km/h的低速工況下，MPC算法橫向誤差小于性能較好的純跟蹤算法橫向誤差。在120 km/h的高速工況下，MPC算法橫向誤差小于高速情況下性能較好的前輪反饋控制算法橫向誤差。其原因是，MPC算法具有良好的橫、縱向耦合控制性能，能夠?qū)Ψ抡孳囕v的速度進行控制，使得算法具有良好的預測作用。當車輛需在彎道轉(zhuǎn)向時，MPC算法能夠在車輛到達彎道轉(zhuǎn)向前進行減速控制，使車輛在彎道轉(zhuǎn)向時不至于失穩(wěn)，轉(zhuǎn)向過程更加平滑。

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

4 實車試驗

4.1 試驗平臺與場地

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

圖13 無人駕駛平臺

圖14 試驗運動場局部示意

4.2 實車試驗結(jié)果

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

圖15 跟蹤算法試驗橫向誤差曲線

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

5 結(jié)束語

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

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