基于LQR和PID的智能車軌跡跟蹤控制算法設計與仿真

徐明澤，劉清河

(哈爾濱工業大學(威海)汽車工程學院，山東 威海 264209)

智能車軌跡跟蹤的精確性及穩定性是車輛安全自主行駛的基礎，主要研究內容通過相應的控制策略盡可能使智能車實際行駛的路線接近已規劃好的車輛路徑，使實際路徑相對于預期路徑的橫向偏差與航向偏差盡可能小[1-3]。智能車的軌跡跟蹤控制主要包括縱向的速度跟隨控制以及橫向的軌跡跟隨控制。橫向控制主要是根據橫向控制算法控制轉向輪轉角或轉向盤轉角，從而使其能夠精確跟蹤上期望軌跡。縱向控制主要是調節智能車的驅動和制動，從而使其達到跟蹤期望車速的目的。

目前用于智能車輛的軌跡跟蹤技術的相關控制算法包括模糊邏輯控制算法、滑模控制算法、模型預測控制算法以及最優控制算法等[4-6]。模糊控制方法適用于車輛的狀態未知的情況，且可以選用較為簡單的模型，但是人為判斷較強，缺乏一定的理論性[7-8]。滑模控制的優點在于能夠很好地匹配車輛模型，缺點是在高速時未考慮動力學約束、控制不穩定[9-10]。模型預測控制建模方便，能夠及時補償模型誤差，控制較為精確，但是涉及到大量約束條件時，處理非線性最優問題的計算量增大，實時性會降低[11-12]。

對于軌跡跟蹤橫縱向控制的問題，國內外許多學者都對此進行了研究。文獻[13-16]提出了新的橫縱向跟蹤控制方法，其中文獻[13]針對智能車輛動態系統高度非線性和不可匹配性，使用基于逆推的變結構控制設計橫向和縱向組合運動控制器；文獻[14]在七自由度車輛動力學和非線性輪胎模型的基礎上，利用神經網絡PID算法及模糊預瞄理論對車輛進行軌跡跟蹤的橫縱向控制；文獻[15]提出一種視覺預瞄式橫縱向耦合控制，將預瞄距離和車速同時引入到模型預測控制器中；文獻[16]等以模型預測控制為基礎，提出一種全新的橫縱向控制算法。文獻[17]基于一種全自動制導框架研究汽車縱向和橫向耦合問題，采用非線性模型預測控制的轉向控制。文獻[18-19]則進一步設計了橫縱向控制器，其中文獻[18]設計了橫縱向控制系統，在綜合道路曲率和縱向車速最優的情況下，搭建最優駕駛員橫向控制模型。并使用縱向模糊控制算法控制油門、制動壓力，最后基于縱向車速將其耦合；文獻[19]設計了基于閉環駕駛員模型的縱向-橫向控制器，該控制器通過預測車輛前方的路徑來調節轉向角和油門/制動壓力。

LQR控制算法的控制對象為線性系統，評價指標為被控制系統的狀態變量的二次型積分，其最優控制律通常可以通過求解Riccati方程得到，因此可達到閉環控制的目的[20-21]。而現有的LQR橫向控制算法會存在一定的穩態橫向偏差，并且在曲率快速變化時超調比較嚴重。在此基礎上加以改進，提出了一種具有前饋的LQR軌跡跟蹤控制算法，這樣穩態橫向偏差就能被很大程度的消除掉，并采用遺傳算法確定其參數，減小計算量。使用PID控制算法可以提高縱向速度控制的精確性，進一步減小軌跡跟蹤偏差。因此，本論文在橫向控制上采用線性二次調節器 (linear quadratic regulator，LQR)，在縱向控制上采用雙PID的控制方法來進行車輛軌跡跟蹤，能夠根據路徑偏差進行實時調節，明顯提高了車輛軌跡跟蹤的控制精度和穩定性。

1 車輛跟蹤誤差模型

1.1 車輛動力學模型

為了保證車輛跟蹤軌跡的精確性和實時性，本文采用車輛二自由度動力學模型，其微分方程如下：

(1)

上述為非線性模型，對于控制器的設計來說過于復雜，因此需要進一步簡化為線性模型。首先，對前輪轉角δ進行小角度(小于5°)假設，即cosδ=1；其次，輪胎模型可以被簡化為輪胎側偏力和側偏角的一次線性關系，即

Fyf=Cαfαf,
Fyr=Cαrαr.

(2)

式中：αf，αr為車輛前輪、后輪的側偏角，rad；Cαf，Cαr車輛前輪、后輪的側偏剛度，N/rad.

由于線性化輪胎模型只在輪胎側偏角較小時具有較高的擬合精度，則用小角度假設可以得到車輛前后輪的側偏角：

(3)

將公式(2)和公式(3)聯合代入公式(1)可得到車輛動力學方程的矩陣形式：

(4)

1.2 跟蹤誤差模型

跟蹤誤差模型是智能車輛軌跡跟蹤控制中常用的車輛運動模型之一，如圖1所示。其中，距離偏差ed定義為車輛質心位置到參考軌跡投影點處的直線距離，θ、θr分別表示為車身縱軸線和參考軌跡在P點處切線的航向角，則航向偏差eφ=θ-θr.在本文中，為了便于與式(4)中的車輛動力學方程相聯系，將航向偏差公式中的航向角θ替換為橫擺角φ.由于車輛的航向角θ與橫擺角φ之間相差一個質心側偏角β，那么eφ可以近似等于φ-θr，但是其穩態誤差不為0，為-β.

圖1 車輛跟蹤誤差模型Fig.1 Vehicle tracking error model

(5)

當航向偏差eφ足夠小時，車輛質心處的距離偏差ed和航向偏差eφ可以表示為：

(6)

(7)

將公式(6)和(7)代入到公式(4)中，整理可得：

(8)

可將式(8)改寫成狀態空間方程形式，則有

(9)

式中：

2 橫縱向控制器的設計

2.1 橫向控制器的設計

LQR控制算法方法的控制效果比較穩定，可利用較小的控制量使系統誤差趨于零，且同時可以對不穩定系統進行整定[22]。本文所設計的橫向控制器的算法流程如圖2所示，其中矩陣A，B的計算已經在前文給出，在這里該模塊不再敘述。

2.1.1最優LQR控制計算

LQR控制算法就是確立最優控制u*(t)=-Kerr(t)，使得下列性能指標函數達到最小值[23]。

(10)

式中：Q為半正定實對稱常數矩陣，反映了對狀態量的權重；R為正定實對稱常數矩陣，反映了對控制量的權重。

為使性能指標函數J取得最小值，以達到能量消耗以及動態誤差的綜合最優，首先構建哈密爾頓函數：

(11)

當輸入信號沒有約束時候，對式(11)進行求導，令其為0，得到最優控制信號為：

u*(t)=R-1BTλ(t) .

(12)

式中：λ(t)的值可以由λ(t)=P(t)err(t)給出，P(t)為以下方程的解：

PA+ATP-PBR-1BPT+Q=0 .

(13)

上式為黎卡提方程。式(13)經過求解后可以求得矩陣P，由此可得到最優控制律反饋矩陣K=R-1BTP，K=[K1K2K3K4].

由于車速vx與K為一一對應關系，為提高控制系統的計算速度，本文首先離線計算出不同車速vx和K的對應表，實際計算中可通過查表來獲取不同車速下的反饋矩陣，提高橫向控制的實時性。可利用Matlab中的lqr(A,B,Q,R)求解函數進行K的求解。

2.1.2離散軌跡點誤差矩陣及曲率計算

一般來說，規劃出的期望軌跡可以按照規劃時間離散化成軌跡點，不僅可以更方便地求出投影點，方便接下來的計算，而且不需要處理多值問題。求解其誤差及曲率的步驟如下：

1) 根據離散軌跡規劃點的位置，找到與車輛真實位置(x,y)最近的點，稱為匹配點。定義匹配點所在的序列記為dm，則匹配點坐標記為(xdm,ydm)，匹配點處所在切線的航向角記為θdm，匹配點處的曲率記為kdm.

圖2 橫向控制算法流程圖Fig.2 Flow chart of lateral control algorithm

2) 由于匹配點與投影點的弧長比較小，本文假設匹配點到投影點的曲率不變，即kr=kdm；假設匹配點到投影點的軌跡可以用圓弧來代替，得到控制位置偏差es，即es=-(x-xdm)cosθdm+(y-ydm)sinθdm.因此，可以計算出匹配點到投影點的距離偏差ed，即ed=-(x-xdm)sinθdm+(y-ydm)cosθdm.

3) 由曲率和匹配點到投影點的弧長的定義式可以求出投影點處所在切線的航向角，即θr=θdm+kdmes.那么，根據投影點處的航向角可以求得航向偏差，航向偏差的導數及速度偏差的導數，如式(14)：

(14)

(15)

2.1.3前饋控制計算

將之前計算出的最優控制律u*(t)=-Kerr(t)代入到式(9)中，得到系統的穩態反饋狀態方程

(16)

引入最優LQR反饋控制的目的是在車輛能順利跟蹤到期望軌跡的基礎上，盡量去減小橫向誤差。但是由式(16)可知，無論反饋系數K取何值，汽車在橫向運動過程中的橫向偏差和橫擺角偏差，即err和err的導數都不可能同時為零。因此，需要原來的控制律的基礎上加一個合適的前饋控制量δf，來抵消穩態狀態下的橫擺角偏差，前饋控制的控制規律為：

u=-Kerr+δf.

(17)

將公式(16)代入到公式(17)中，可得

(18)

當系統穩定后，使得err的導數為零，由此可得

(19)

對式(19)計算并化簡求得

(20)

式中：K1和K3分別為第一部分計算出的反饋矩陣K的第一和第三項。

(21)

式中：k為第二部分計算出的曲率。

2.2 縱向控制器的設計

2.2.1PID控制算法原理

PID控制器的輸出可表示為：

(22)

式中：KP為比例系數；KI為積分系數；KD為微分系數；t為時間；τ為積分變數。調節比例系數KP可快速、及時、按比例調節偏差，提高控制靈敏度；調節積分系數KI主要用于減小穩態誤差，提高控制精度；調節微分系數KD能減小超調量、提高控制穩定性。

在本文中，通過反復調整參數，為了滿足低中高3種仿真工況的普適性，以及同時兼顧控制精度和穩定性的要求，所采取的系數如下：KP=3；KI=0.01；KD=1.

2.2.2雙PID控制算法

(23)

因此，本文設計了雙PID控制器，分別對車速和位置進行控制，其控制目標分別為控制速度偏差ev和控制位置誤偏差es，從而能夠達到實時調節的目的。

2.2.3驅動制動標定表的制作

在制作油門剎車標定表之前，首先應該確定所采用的驅動/制動模型。由于本文的仿真對象為電動SUV，因此采用電動機模型，具體可描述為

P=Tn.

(24)

式中：P為電機所需功率，n為電機所需轉速，T為電機的轉矩，即

(25)

式中：Tmax為電機的最大扭矩，N·m；thr為加速踏板或制動踏板的開度；nb為基速，r/min，即最大功率Pmax與最大扭矩Tmax的比值。

根據此模型，可以利用仿真實驗，得到速度、加速度和加速踏板或制動踏板的開度的三維散點，再利用雙線性插值算法把未知的速度v、加速度a所對應的加速踏板或制動踏板的開度thr計算出來，得到關于這3個物理量的一個表格關系，即驅動制動標定表，如表1所示。其目的是根據車輛行駛過程中的速度和加速度值來實時控制車輛的加速踏板或制動踏板的開度，從而控制車輛的驅動或制動。

表1 驅動制動標定表Table 1 Driving braking calibration table

3 軌跡跟蹤控制仿真

本文將通過Carsim與Simulink的聯合仿真來驗證上述的軌跡跟蹤控制算法的效果，仿真流程圖如圖3所示。

圖3 聯合仿真流程圖Fig.3 Joint simulation flow chart

3.1 參考軌跡設計

為了更好地實現跟蹤效果，所設計的參考軌跡的位置、速度、加速度、航向角、曲率在每一時刻應連續。基于五次多項式曲率連續平滑，無較大突變且舒適性較好(Jerk變化率比較小)的優點，本文采用5次多項式進行參考軌跡的設計，可表示為

xr(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5,

yr(t)=b0+b1t+b2t2+b3t3+b4t4+b5t5.

(26)

式中：a0，a1，…，a5，b0，b1，…，b5為待定系數。

另外，根據常見的行駛場景以及城市道路工程設計規范[24]，規劃出的參考軌跡需滿足起點和終點的邊界條件：

其中，T為從起點到終點所行駛的時間，xend為終點的橫坐標，通過設置不同的邊界條件，進而解出式(26)對應的待定系數a0，a1，…，a5，b0，b1，…，b5.同時，將y(x)轉換成y(t)，便于縱坐標的計算：

(27)

進而可以計算出參考軌跡的切線角θr和曲率kr：

(28)

最終規劃出的參考軌跡還應該對加速度進行限制：

(29)

3.2 仿真參數設置

3.2.1車輛參數設置

本文試驗車型為SUV轎車，在Carsim中進行車輛相關參數設置，具體參數如表2所示。

表2 車輛基本參數Table 2 Basic vehicle parameters

3.2.2仿真工況設置

本文所設置的仿真工況有低速大轉角、中速小轉角、高速小轉角，分別對應道路停車、城市道路變道、高速公路變道3種工況，具體參數如表3所示。默認起點處的橫向位移和縱向位移為0，并設定縱向速度和加速度為0，以及起點處和終點處的橫向加速度為0，進而可以求得五次多項式的系數，得到參考軌跡。

表3 仿真工況設置Table 3 Simulation condition setting

3.3 LQR參數確定

通常情況下，LQR算法的參數確定通常依靠人工試湊的方法，不但耗時耗力，沒有科學的規律和求解方式，而且還不能保證LQR控制器發揮出最佳的跟蹤效果[25]。針對無法選取最優加權矩陣參數Q和R的問題，本文采用遺傳算法(GA)對加權矩陣Q、R的對角線參數進行求解，使LQR控制器具有優良的魯棒性，并使車輛能夠較精確的跟蹤到參考路徑。

通常情況下，Q值增大可以提高系統的響應速率，算法性能較好。R值越大，軌跡跟蹤過程比較平緩，可以降低系統的誤差的幅值大小。選取加權矩陣Q和R需要綜合考慮LQR控制器的準確性和穩定性。因此，本文將利用遺傳算法對加權矩陣Q1、Q3和R三個參數進行確定，如圖4所示。

圖4 遺傳算法優化流程Fig.4 Genetic algorithm optimization process

(30)

設定最大遺傳次數100，原始種群的個體數設置成30，交叉概率設置成Pc=0.6，變異概率設置成Pm=0.03，起始的Q1、Q3和R分別設為15，5和20，圖5所示為經GA優化的結果。并選用一段正弦曲線軌跡進行跟蹤對比，如圖6所示。

從圖5(a)的Q1、Q3曲線與圖5(b)的R優化過程可以看出，在尋優初期搜索速度較快，最終收斂于第25次迭代，最優參數值分別穩定于30、5和10，并且遺傳算法優化后的LQR參數在追蹤精度上與未優化相比表現的更出色。最終LQR的權重矩陣設置為：Q=diag(30,1,5,1)，R=10，控制器能準確且穩定地跟蹤上目標軌跡。

圖5 經GA優化的結果Fig.5 Results optimized by genetic algorithm

圖6 路徑跟蹤軌跡對比Fig.6 Path tracking trajectory comparison

3.4 仿真結果分析

3種工況下的規劃軌跡和跟蹤軌跡、目標速度和跟蹤速度、前輪轉角、距離偏差、橫擺角速度的仿真結果如圖7所示。

在低速工況下，車輛可以跟蹤規劃出的期望軌跡和期望速度，跟蹤效果良好，并且其距離偏差在0.04 m之內，這表明本控制器在低速工況下可以保持較高的跟蹤精度。前輪轉角和橫擺角速度在低速情況下變化較平穩，沒有出現抖動，且車輛完成泊車后能很快地收斂到穩定狀態，說明控制器具有比較高的魯棒性。

圖7 路徑跟蹤仿真效果Fig.7 Path tracking simulation results

在中速工況下，車輛可以跟蹤規劃出的期望軌跡和期望速度，跟蹤效果良好，并且其距離偏差在0.025 m之內，相對于低速工況來說距離偏差更小，這表明本控制器在中速工況下跟蹤精度更高。前輪轉角和橫擺角速度相對于低速情況下減小，變化仍然較平穩，在12 s附近會有小幅度的抖動，導致跟蹤速度與目標速度之間會有一些偏差，說明該控制器的魯棒性隨著速度的增加會有一定程度的降低。

在高速工況下，車輛可以跟蹤規劃出的期望軌跡和期望速度，跟蹤效果良好，并且其距離偏差在0.02 m之內，這表明該控制器在高速工況下跟蹤精度非常高。前輪轉角和橫擺角速度較低，可以被控制在較小的范圍內，在12 s附近出現抖動現象，說明該控制器在高速工況下的魯棒性低于低速工況。

4 結論

本文針對智能車軌跡跟蹤問題，采用橫縱向綜合控制策略對其進行研究。為了更好地驗證軌跡跟蹤控制實際效果，保證軌跡跟蹤的實時性，建立了車輛二自由度動力力學模型，只考慮汽車的橫擺和側向運動。在此基礎上考慮距離偏差和航向偏差，得到了跟蹤誤差模型。軌跡跟蹤控制主要是車輛的縱向速度跟蹤控制和橫向軌跡跟蹤控制。本文通過縱向雙PID控制器來調整車輛的位置誤差和速度誤差，進而控制車輛的驅動與制動。另外，根據期望的路徑，LQR算法及前饋控制，橫向控制器會生成一個合適的方向盤轉角，并通過遺傳算法來確定橫向控制器的加權矩陣參數，從而實現一個準確的路徑跟蹤。在CarSim和Simulink仿真環境分別對低速泊車、中速駛入、高速駛入三種工況進行了聯合仿真分析。結果表明該控制器能夠完成多種工況下的聯合仿真試驗，跟隨所給定的路徑模型行駛，具有良好的控制精度和穩定性，并且保證車輛的靈活性。