基于前饋和反饋相結(jié)合的路徑跟蹤算法*

冀同濤 李剛 王明家 任建平

（遼寧工業(yè)大學汽車與交通工程學院）

無人駕駛車輛是一個非常復雜的系統(tǒng)，主要包括環(huán)境感知、導航定位、路徑規(guī)劃以及路徑跟蹤等部分[1]。車輛路徑跟蹤控制是無人駕駛車輛的一項關鍵技術，它分為縱向控制和橫向控制。其中，縱向控制指對車輛行駛速度的控制；橫向控制指對汽車行駛方向的控制，一般指控制汽車前輪轉(zhuǎn)角，使汽車能沿著參考路徑行駛，減小跟蹤誤差，同時保證汽車行駛的穩(wěn)定性和舒適性[2]。文章是對橫向控制進行研究。在路徑跟蹤方面的研究成果從原理上來講主要分為基于預瞄理論和基于模型預測理論2 類。目前常見的路徑跟蹤算法主要有模糊控制算法[3]、PID 控制算法[4]、純追蹤控制算法[5-6]、LQR 算法及MPC 算法等。針對純追蹤模型在跟蹤非固定曲率路徑下誤差較大的問題，文章提出了一種改良后的路徑跟蹤算法。經(jīng)過仿真驗證，該算法能有效改善路徑跟蹤效果，減小航向誤差和橫向誤差，保證車輛的行駛穩(wěn)定性。

1 純追蹤算法及其分析

1.1 純追蹤算法介紹

純追蹤算法是一種非常典型的橫向控制方法。該算法對外界的魯棒性較好，可以取得較好的橫向控制效果。它基于幾何原理，根據(jù)預瞄點與汽車位置的關系可計算出轉(zhuǎn)彎半徑，再通過汽車軸距和轉(zhuǎn)彎半徑求出前輪轉(zhuǎn)角。純追蹤算法示意圖，如圖1所示。

圖1 純追蹤算法示意圖

由正弦定理可推出：

這里采用自行車模型簡化前輪轉(zhuǎn)角與后軸行駛曲率的關系，如圖2所示，該模型是對阿克曼轉(zhuǎn)向模型的簡化。

圖2 自行車模型

δ 與曲率關系的表達式，如式（2）所示。

結(jié)合式（1）和式（2）可得到：

1.2 純追蹤算法分析

對該算法進行更深入的研究，可以得到式（4）。

式中：μ——汽車位置和預瞄點的橫向誤差，m。

則圓弧曲率（K）的計算，如式（5）所示。

由上述內(nèi)容可知，純追蹤控制器本質(zhì)上是一個橫向轉(zhuǎn)角的P 控制器，該控制器的控制效果與前視距離有很大關系。前視距離過小會導致汽車轉(zhuǎn)角變化大，汽車控制不穩(wěn)定甚至振蕩；前視距離過大則會使汽車在過彎道時轉(zhuǎn)向不足，會出現(xiàn)“抄近路”的現(xiàn)象，難以很好地跟蹤路徑。此外，純追蹤算法屬于前饋控制，只考慮預瞄點信息，在路徑曲率變化大的情況下有一定的不足。

2 后輪反饋控制算法及其分析

2.1 后輪反饋控制算法介紹

后輪反饋控制（如圖3所示）是利用后輪中心的跟蹤偏差來進行轉(zhuǎn)向控制量計算的方法。在Frenet 坐標系下，（t，ε，γ）的變化率，如式（6）所示。

式中：t——汽車的縱向距離，m；

ε——汽車和參考路徑的橫向偏差，m；

γ——參考路徑與汽車的航向偏差，rad；

k——汽車距離參考路徑最近點的曲率；

vr——車速，m/s；

w——汽車橫擺角速度，rad/s。

圖3 后輪反饋控制圖

由汽車行駛時的橫向偏差和航向偏差可得到李雅普諾夫方程，如式（7）所示。

為使李雅普諾夫方程局部漸進收斂，w 表示為：

式中：k1——橫擺角偏差反饋控制增益；

k2——橫向位置偏差反饋控制增益。

由車輛運動學方程可得到δ，如式（9）所示。

2.2 后輪反饋控制算法分析

后輪反饋控制算法對路徑平滑性要求較高，通過設計車輛橫擺角速度，實現(xiàn)李雅普諾夫方程局部漸進收斂，從而實現(xiàn)路徑跟蹤。在低、中等速度下該算法的跟蹤性能以及魯棒性較好。然而，有時候該算法會出現(xiàn)轉(zhuǎn)向過度的情形，且轉(zhuǎn)角變化幅度大。

3 基于前饋和反饋相結(jié)合的路徑跟蹤算法

上述2 種路徑跟蹤算法各有其優(yōu)勢和劣勢，純追蹤算法控制屬于前饋控制，而后輪反饋控制算法則屬于反饋控制。純追蹤算法在跟蹤過程中并沒有考慮汽車當前位姿與參考路徑的誤差，而后輪反饋控制算法雖然考慮到汽車橫向誤差和航向誤差，但只是根據(jù)誤差來控制轉(zhuǎn)角，不考慮前方路徑情形，會導致轉(zhuǎn)向過度，車輛穩(wěn)定性較差。將2 種算法結(jié)合，既考慮汽車橫向誤差和航向誤差，又考慮前方參考路徑對汽車轉(zhuǎn)向的影響，不僅能改善跟蹤效果，還能保證車輛穩(wěn)定性，具有一定的魯棒性。

式中：δp——純追蹤算法計算得出的前輪轉(zhuǎn)角，rad；

δr——后輪反饋控制算法得到的前輪轉(zhuǎn)角，rad；

kp，kr——比例因子。

其中，kp和kr的選擇應根據(jù)汽車橫向誤差和航向誤差來確定，應滿足kp+kr=1。汽車當前航向誤差（γ）和橫向誤差（ε）越大，kr應該越大，kp應該越小，如式（11）和式（12）所示。

4 車輛運動學模型

無人駕駛車輛的運動規(guī)劃與控制需要通過對車輛運動學或者動力學系統(tǒng)的控制來實現(xiàn)。汽車在運動時的動力學過程是十分復雜的，過于復雜的模型并不是研究重點，對于一般情況，建立能夠準確描述無人駕駛車輛運動關系約束的運動學模型或者動力學約束的動力學模型即可滿足要求[7]。本試驗根據(jù)車輛運動學模型進行仿真，車輛轉(zhuǎn)向運動模型，如圖4所示。

圖4 車輛運動學模型

運動學模型為：

式中：Xr，Yr——汽車后軸中心位置橫、縱坐標，m；

θ——航向角，rad。

5 仿真分析

在MATLAB/Simulink 環(huán)境下采用上述車輛運動學模型進行無人駕駛車輛的路徑跟蹤仿真。δ 和vr為車輛運動模型的輸入量，輸出量為汽車位置和航向信息，用以模擬GPS/INS 模塊，得到無人駕駛車輛的實時位姿。參考路徑的一個基本要求就是能夠通過汽車的控制來執(zhí)行該路徑，一種簡化的處理方式就是保證路徑的連續(xù)性和平滑性，這要求該路徑的1 階導數(shù)和2 階導數(shù)是連續(xù)可導的。為此給出參考路徑函數(shù)為三次函數(shù)，如式（14）所示。其中，a=0.01，b=-0.345，c=3.6，d=0。

分別采用改進后的路徑跟蹤算法和傳統(tǒng)的純追蹤算法進行路徑跟蹤，車速設置為18 km/h，汽車初始位置設置為（0.1，0）；仿真結(jié)果，如圖5～圖9所示。

圖5 純追蹤模型跟蹤效果圖

圖6 改進后的路徑跟蹤模型跟蹤效果圖

圖7 前輪轉(zhuǎn)角對比圖

圖8 路徑跟蹤橫向誤差對比圖

圖9 路徑跟蹤航向誤差對比圖

由圖5和圖6可知，基于前饋和反饋的路徑跟蹤方法比傳統(tǒng)純追蹤算法的效果要好一些。從跟蹤參考軌跡的對比中能看到，傳統(tǒng)純追蹤算法在曲率變化明顯的地方跟蹤效果并不是很好，這是由于在前視距離固定的時候，路徑曲率會影響跟蹤情況，大曲率的時候會出現(xiàn)“走捷徑”的現(xiàn)象，而加上汽車當前位姿與參考路徑的實時橫向誤差和航向誤差反饋后，可以很好地改善這個問題。

從圖7可看出，改進后的算法轉(zhuǎn)角會更大一點，但滿足轉(zhuǎn)向機構(gòu)的要求，并沒有超出行程，而且前輪轉(zhuǎn)角變化也較為合理，能保證汽車的行駛穩(wěn)定性。

從圖8可看出，算法改進有較為明顯的效果，純追蹤算法橫向誤差最大值達到0.25 m，誤差較大，而改進后的算法橫向誤差控制在0.05 m 以內(nèi)，有較好的路徑跟蹤效果。

從圖9可看出，純追蹤算法航向誤差最大值達到0.1 rad，而基于前饋和反饋相結(jié)合的路徑跟蹤算法的航向誤差較小，控制在0.03 rad 內(nèi)，說明該算法可以較好地跟蹤參考路徑。

6 結(jié)論

文章通過對純追蹤算法原理的分析，探究引起跟蹤誤差的原因，并提出基于前饋和反饋相結(jié)合的路徑跟蹤方法，改善了純追蹤算法由于前視距離難以很好地調(diào)控所引起的路徑跟蹤誤差。

通過前饋和反饋的方式，可以跟蹤絕大多數(shù)的路徑，并且受路徑形狀的干擾影響較小，能夠減小汽車行駛中的橫向誤差和航向誤差，改善曲率變化明顯時的路徑跟蹤效果。文章所述方法可使汽車具有較好的路徑跟蹤準確性和穩(wěn)定性，且具有一定的魯棒性。