采用狀態(tài)反饋的無人車路徑跟蹤橫向控制

寇發(fā)榮，楊慧杰，張新乾，鄭文博，王思俊

(西安科技大學(xué) 機(jī)械工程學(xué)院,西安 710054)

智能化和網(wǎng)聯(lián)化是未來汽車發(fā)展的趨勢[1]。無人駕駛車輛將環(huán)境感知、自主決策與規(guī)劃、動(dòng)力學(xué)控制[2]等關(guān)鍵技術(shù)整合,從而提高道路安全性與利用率、車輛智能化水平[3]。路徑跟蹤算法是智能車輛動(dòng)力學(xué)控制的核心問題,車輛需要按照預(yù)設(shè)路徑行駛,同時(shí)保證行駛精度和穩(wěn)定性。

模型預(yù)測控制作為一種車輛路徑跟蹤優(yōu)化控制算法,通過對線性模型的輸入和狀態(tài)變量添加約束,可將結(jié)果轉(zhuǎn)化為帶約束的最優(yōu)控制問題[4-5]。線性時(shí)變模型預(yù)測控制在車輛路徑跟蹤實(shí)時(shí)性控制中有明顯的優(yōu)勢[6]。采用動(dòng)力學(xué)模型的路徑跟蹤控制器考慮輪胎與地面的附著極限,車輛可以在復(fù)雜路面下以高速穩(wěn)定行駛。國內(nèi)外學(xué)者在車輛路徑跟蹤模型預(yù)測控制領(lǐng)域做了大量研究: Falcone等[7]研究了用于車輛動(dòng)力學(xué)的模型預(yù)測控制器設(shè)計(jì)和實(shí)現(xiàn)問題,并通過實(shí)車試驗(yàn)驗(yàn)證了高速車輛的穩(wěn)定性。斯坦福大學(xué)學(xué)者使用模型預(yù)測控制器和反饋控制器設(shè)計(jì)框架,協(xié)調(diào)解決了車輛緊急避撞及其穩(wěn)定性問題[8]。國內(nèi)學(xué)者采用模型預(yù)測控制方法對車輛底盤集成控制和穩(wěn)定性開展研究[9-10]。針對路徑跟蹤控制器跟蹤精度問題,趙治國等[11]采用粒子群優(yōu)化算法實(shí)現(xiàn)了預(yù)瞄距離自適應(yīng)控制;白國星等[12]分析了車輛穩(wěn)態(tài)轉(zhuǎn)角、車速和預(yù)測時(shí)域的變化規(guī)律,通過調(diào)節(jié)參數(shù)實(shí)現(xiàn)路徑跟蹤控制器的改進(jìn)優(yōu)化。上述文獻(xiàn)中采用的路徑跟蹤模型預(yù)測控制算法在彎道內(nèi)路徑跟蹤精度難以保障,行駛時(shí)會(huì)出現(xiàn)轉(zhuǎn)向系統(tǒng)反應(yīng)超前或滯后[13],從而影響控制器跟蹤效果。狀態(tài)反饋控制方法能夠?qū)崿F(xiàn)閉環(huán)控制,達(dá)到系統(tǒng)期望的性能指標(biāo)。斯坦福大學(xué)Snider[14]將狀態(tài)反饋控制器運(yùn)用于路徑跟蹤控制。Kapania等[15]使用穩(wěn)態(tài)質(zhì)心側(cè)偏角跟蹤參考軌跡方向,改善了路徑跟蹤性能。魏建偉等[16]基于LQR最優(yōu)控制提出了主動(dòng)轉(zhuǎn)向和直接橫擺力矩控制的聯(lián)合控制策略,提高了車輛的側(cè)向穩(wěn)定性。線性二次調(diào)節(jié)器LQR通過狀態(tài)反饋實(shí)現(xiàn)閉環(huán)控制,其性能指標(biāo)是關(guān)于狀態(tài)變量和控制變量的二次型函數(shù),可以實(shí)現(xiàn)系統(tǒng)控制過程中的動(dòng)態(tài)誤差和能量消耗最低[17]。

本文提出了一種基于狀態(tài)反饋的路徑跟蹤模型預(yù)測控制方法。以車輛動(dòng)力學(xué)模型建立路徑跟蹤模型預(yù)測控制器,選擇合適的控制器時(shí)域參數(shù)值,在前輪主動(dòng)轉(zhuǎn)向的基礎(chǔ)上以車輛當(dāng)前位置的橫擺角偏差設(shè)計(jì)狀態(tài)反饋調(diào)節(jié)器,使用LQR算法對轉(zhuǎn)向角進(jìn)行校正,最后利用MATLAB/Simulink和Carsim軟件進(jìn)行聯(lián)合仿真分析驗(yàn)證算法的有效性。

1 車輛狀態(tài)反饋控制系統(tǒng)框架

1.1 車輛動(dòng)力學(xué)模型

路面附著、輪胎等非線性因素對無人駕駛車輛轉(zhuǎn)向特性有顯著影響,給控制器設(shè)計(jì)帶來一定挑戰(zhàn)。為準(zhǔn)確描述這些影響因素,忽略車輛垂向運(yùn)動(dòng)和軸荷轉(zhuǎn)移,對車輛模型進(jìn)行適當(dāng)簡化使其具有橫向、縱向和橫擺這3個(gè)自由度,簡化后的車輛動(dòng)力學(xué)模型如圖1所示。圖中：XOY為固定于地面的慣性坐標(biāo)系;xoy為固定于車身的車輛坐標(biāo)系;o為車輛質(zhì)心位置;x軸沿車輛縱軸方向;y軸與車輛縱軸垂直。

圖1 3自由度車輛模型

根據(jù)牛頓第二定律,得到沿x軸、y軸和z軸的受力平衡方程，即

(1)

考慮到前輪轉(zhuǎn)角較小,輪胎所受縱向力和側(cè)向力呈線性變化,將其表示為輪胎側(cè)偏角和滑移率的線性關(guān)系，即

(2)

式中：Ccf、Ccr為前后輪胎側(cè)偏剛度;Clf、Clr為輪胎縱向剛度;slf、slr為輪胎滑移率；αcf、αcr為前后輪胎側(cè)偏角。

最終得到基于線性輪胎模型和小角度假設(shè)下的車輛非線性動(dòng)力學(xué)模型:

(3)

1.2 車輛狀態(tài)反饋控制器

智能車輛路徑跟蹤控制框圖如圖2所示。

圖2 路徑跟蹤控制器框圖

路徑跟蹤狀態(tài)反饋控制器包括模型預(yù)測控制器(MPC)和LQR狀態(tài)反饋調(diào)節(jié)器兩部分。模型預(yù)測控制器根據(jù)道路信息和車輛約束條件計(jì)算出預(yù)測時(shí)域內(nèi)前輪轉(zhuǎn)角,狀態(tài)反饋調(diào)節(jié)器根據(jù)道路參考橫擺角φref和車輛實(shí)際橫擺角φ計(jì)算橫擺角偏差Δφ,并通過LQR算法對車輛前輪轉(zhuǎn)角進(jìn)行校正,最終將修正后的轉(zhuǎn)角δ輸出到Carsim中實(shí)現(xiàn)對車輛的控制。

2 模型預(yù)測橫向控制器設(shè)計(jì)

基于狀態(tài)空間的模型預(yù)測控制算法具有預(yù)測模型、滾動(dòng)優(yōu)化和反饋校正3個(gè)特征,該算法汲取反饋控制思想,預(yù)測時(shí)域的滾動(dòng)優(yōu)化使系統(tǒng)在受到內(nèi)部或外界的擾動(dòng)時(shí)及時(shí)補(bǔ)償和校正,具有很強(qiáng)的魯棒性[4]。

(4)

計(jì)算機(jī)對連續(xù)系統(tǒng)作實(shí)時(shí)控制,需要使用一階差商的方法將式(4)線性時(shí)變連續(xù)系統(tǒng)進(jìn)行近似離散化,得到離散的狀態(tài)空間表達(dá)式為

(5)

其中:

Ak,t=I+TAdyn(t);Bk,t=TBdyn(t)。

模型預(yù)測控制器根據(jù)系統(tǒng)預(yù)測模型下一時(shí)刻輸入量和當(dāng)前時(shí)刻狀態(tài)量,確定未來預(yù)測時(shí)域內(nèi)的輸出,由式(5)動(dòng)態(tài)方程將系統(tǒng)未來一段時(shí)域的輸出表示為

Y(t)=ψtξ(t)+ΘtΔU(t)

(6)

其中：

將預(yù)測時(shí)域內(nèi)輸出量和時(shí)域內(nèi)的控制增量作為優(yōu)化目標(biāo),使系統(tǒng)穩(wěn)定的跟隨期望軌跡,得到最終的目標(biāo)函數(shù)為

(7)

式中:Q、R、ρ為權(quán)重系數(shù)矩陣;ε為松弛因子,防止計(jì)算過程中出現(xiàn)無解情況。

考慮系統(tǒng)中控制量和控制增量極限約束,同時(shí)由車輛橫向速度和橫擺角速度定義車輛滑移穩(wěn)定包絡(luò)線約束[18]。車輛在穩(wěn)態(tài)條件下的橫擺角速度為

(8)

臨界條件下假設(shè)輪胎縱向力為0,得到橫擺角速度極值為

(9)

車輛穩(wěn)定性條件的另一個(gè)約束是后軸輪胎的側(cè)向力飽和,后輪側(cè)滑造成整車失控危害較大,需要對輪胎側(cè)偏角進(jìn)行限制。

由圖1可得到后輪側(cè)向力產(chǎn)生的側(cè)偏角,后輪側(cè)偏角與質(zhì)心運(yùn)動(dòng)參數(shù)的關(guān)系為

(10)

車輛最大側(cè)向力產(chǎn)生的后輪最大側(cè)偏角為

(11)

圖3 車輛滑移穩(wěn)定性包絡(luò)線約束

式(9)和式(10)也可以表示為矩陣形式

|H(t)·ξ(t)|≤G+s

(12)

綜合上述條件,基于模型預(yù)測控制器的無人車前輪轉(zhuǎn)角最優(yōu)控制問題可以轉(zhuǎn)化為最優(yōu)二次型:

minJ(ξ(t),u(t-1),ΔU(t))

(13)

3 LQR狀態(tài)反饋調(diào)節(jié)器設(shè)計(jì)

3.1 車輛跟蹤誤差模型

車輛在彎道行駛時(shí)的跟蹤誤差模型[19]如圖4所示,模型參考點(diǎn)位于質(zhì)心o處,選取路徑點(diǎn)P定義車輛當(dāng)前位置行駛偏差,其中橫擺角偏差Δφ是期望路徑點(diǎn)切線與車輛x軸夾角,ye為車輛橫向位置偏差,車輛x軸與行駛車速v之間的夾角為質(zhì)心側(cè)偏角β,路徑點(diǎn)P處的曲率半徑為Rref。

圖4 車輛跟蹤誤差模型

由圖4幾何關(guān)系可得,車輛當(dāng)前時(shí)刻的橫向偏差變化率為

(14)

在小角度假設(shè)下,由sin(β+Δφ)≈β+Δφ,將三角函數(shù)線性化

(15)

車輛沿期望路徑行駛時(shí),質(zhì)心處的橫擺角偏差變化率為

(16)

由于路徑跟蹤模型預(yù)測控制器彎道內(nèi)過早轉(zhuǎn)向,導(dǎo)致車輛行駛軌跡偏向于彎道內(nèi)側(cè)。本文參考斯坦福大學(xué)stanley路徑跟蹤控制算法思想[20],以車輛質(zhì)心位置為控制點(diǎn),通過轉(zhuǎn)向角補(bǔ)償校正實(shí)現(xiàn)車輛橫擺角偏差反饋控制,同時(shí)降低當(dāng)前位置的橫向偏差,使車輛行駛方向更接近參考軌跡切線方向。

由此得到車輛當(dāng)前位置所滿足的運(yùn)動(dòng)學(xué)跟蹤誤差模型表達(dá)式為

(17)

式中:μ為道路附著系數(shù);k為曲率;δf為模型預(yù)測控制器計(jì)算得到的前輪轉(zhuǎn)角;δs為轉(zhuǎn)角校正量。

3.2 基于誤差模型的狀態(tài)反饋調(diào)節(jié)器設(shè)計(jì)

狀態(tài)反饋調(diào)節(jié)器可以保證閉環(huán)系統(tǒng)穩(wěn)定,同時(shí)改善控制器路徑跟蹤性能。本節(jié)結(jié)合車輛跟蹤誤差模型得到橫擺角偏差,利用LQR控制方法構(gòu)建狀態(tài)反饋調(diào)節(jié)器。

由式(17)狀態(tài)空間方程構(gòu)建二次型性能指標(biāo):

式(18)指標(biāo)中第一項(xiàng)反映了系統(tǒng)對調(diào)節(jié)過程的平穩(wěn)性和快速性要求,第二項(xiàng)反映了對能量消耗的要求。系統(tǒng)受到擾動(dòng)偏離參考軌跡時(shí),需要產(chǎn)生附加方向盤轉(zhuǎn)角,使跟蹤橫擺角偏差Δφ和能量消耗綜合最優(yōu)。

應(yīng)用拉格朗日乘子法構(gòu)造無約束優(yōu)化問題,建立增廣泛函，即

定義哈密爾頓函數(shù)為

H[x,u,λ,t]=θ=F[x(t),u(t),t]+λTf(x,u,t)(20)

使用黎卡提微分方程解出P(t)矩陣

P(t)B(t)R-1(t)BT(t)P(t)-Q(t)

(21)

將P(t)代入式(22)最終可得最優(yōu)控制量u*(t)為

u*(t)=-R(t)BTP(t)x*(t)=-K(t)x*(t)

(22)

式中K(t)為反饋增益矩陣。

4 路徑跟蹤控制器仿真分析

4.1 仿真道路及控制器參數(shù)設(shè)置

為驗(yàn)證基于狀態(tài)反饋的模型預(yù)測控制器跟蹤效果,本節(jié)設(shè)計(jì)了路徑跟蹤仿真對比試驗(yàn)。雙移線道路包括連續(xù)轉(zhuǎn)彎和回正操作,能夠全面測試車輛彎道內(nèi)的軌跡跟蹤性能,因此采用雙移線測試路段,路面附著良好,無路面超高。圖5為雙移線軌跡及參考橫擺角曲線,路徑縱向位移設(shè)置為200 m,曲率隨路徑變化,最小曲率半徑30 m,位于X=60 m處。

圖5 雙移線工況

由于模型預(yù)測控制器跟蹤誤差與預(yù)測時(shí)域步長、車速和道路曲率的關(guān)系難以用函數(shù)關(guān)系式表示,因此以車輛在雙移線軌跡的行駛數(shù)據(jù)為誤差參考依據(jù): 給出離散時(shí)間步長一定時(shí),不同預(yù)測時(shí)域下車輛路徑跟蹤橫向偏差的均方根值,使車輛能夠根據(jù)不同行駛工況選擇合適的控制器時(shí)域步長值。

圖6 不同車速和預(yù)測時(shí)域的跟蹤誤差

圖6中曲線為3個(gè)速度下不同預(yù)測步長的跟蹤誤差均方根值。由圖6可知,車輛以36 km/h和72 km/h車速在彎道行駛時(shí),預(yù)測時(shí)域步長與跟蹤誤差成正比關(guān)系,預(yù)測時(shí)域越長,跟蹤誤差越大;在108 km/h車速下,隨著預(yù)測時(shí)域步長增加,跟蹤誤差先減小后逐步增大。同時(shí)預(yù)測時(shí)域步長不應(yīng)過小,小步長易使車輛繞參考軌跡擺動(dòng),中高速情況下容易失去穩(wěn)定性。由此可以確定不同速度下合適的預(yù)測時(shí)域步長,結(jié)果如表1所示,其它速度區(qū)間的最佳時(shí)域步長可以通過線性回歸預(yù)測得到。

表1 不同速度下確定的預(yù)測步長

本文使用Carsim搭建整車模型,仿真所用車輛模型為前輪驅(qū)動(dòng)C級轎車,模型及仿真過程中的參數(shù)設(shè)置如表2所示。

表2 車輛參數(shù)及權(quán)重矩陣設(shè)置

4.2 路徑跟蹤控制器對比分析

將基于狀態(tài)反饋的路徑跟蹤模型預(yù)測控制器與傳統(tǒng)模型預(yù)測控制器的車輛行駛軌跡、橫擺角及偏差曲線進(jìn)行對比分析。不同車速下改進(jìn)控制器性能表現(xiàn)不同,本文分別在36 km/h、72 km/h和108 km/h速度下進(jìn)行仿真試驗(yàn),采用車輛參考軌跡與實(shí)際行駛軌跡誤差平均值和最大值指標(biāo)作為衡量軌跡跟蹤精度好壞的依據(jù),并結(jié)合仿真結(jié)果分析車輛在不同控制器下的路徑跟蹤效果。

圖7為36 km/h速度下得到的仿真結(jié)果,圖7a)為車輛行駛軌跡,圖7b)和圖7c)為車輛相對參考軌跡的橫向偏差和橫擺角偏差,圖7d)為前輪轉(zhuǎn)角對比圖,算法改進(jìn)前后車輛軌跡和前輪轉(zhuǎn)角的結(jié)果對比通過箭頭標(biāo)注。

圖7 36 km/h車速仿真結(jié)果

從圖7a)曲線可以看出,采用傳統(tǒng)模型預(yù)測控制的車輛在40～50 m區(qū)間入彎較晚,在60～70 m區(qū)間因提早轉(zhuǎn)向而駛離車道,車輛進(jìn)入下一彎道時(shí)導(dǎo)致偏差累積,而使用本文方法的車輛在駛?cè)牒婉傠x彎道時(shí)更加接近參考軌跡。由圖7b)和圖7c)可以看出,車輛行駛最大偏差出現(xiàn)在彎道內(nèi),傳統(tǒng)模型預(yù)測控制器的最大橫向偏差和橫擺角偏差都大于狀態(tài)反饋控制器。由圖7d)車輛前輪轉(zhuǎn)角對比分析可知,車輛在40～50 m和60～70 m彎道內(nèi)校正后的方向盤轉(zhuǎn)角幅值略大于傳統(tǒng)模型預(yù)測控制器,意味著車輛入彎時(shí)刻更早,而出彎時(shí)刻稍晚,同時(shí)可以看出進(jìn)入下一彎道后轉(zhuǎn)向更快速,有利于車輛提升彎道跟蹤的準(zhǔn)確性。由計(jì)算可知,雙移線工況下,汽車以36 km/h定速行駛,使用本文方法的控制器橫向誤差平均值為0.138 m,誤差最大值為0.353 m,位于橫向位置74 m處;而傳統(tǒng)模型預(yù)測控制器橫向誤差平均值為0.165 m,誤差最大值為0.518 m,位于橫向位置76 m處。

圖8為車輛以72 km/h行駛時(shí)得到的仿真結(jié)果。

圖8 72 km/h車速仿真結(jié)果

從圖8a)中曲線可以看出,在40～50 m和60～70 m區(qū)間車輛變換彎道時(shí),兩種控制方法下車輛駛離彎道的位置基本相同,使用本文方法的車輛橫向位置誤差和橫擺角偏差更小。從圖8b)和圖8c)可知,基于狀態(tài)反饋的路徑跟蹤控制器橫向偏差和橫擺角偏差均得到明顯改善。由圖8d)可知,在位置50～60 m區(qū)間,使用本文方法的車輛前輪轉(zhuǎn)角在達(dá)到最大值后沒有立刻回正,而持續(xù)了一小段時(shí)間,由圖8a)可知此時(shí)彎道內(nèi)使用狀態(tài)反饋控制器的車輛更加接近參考軌跡。由計(jì)算得狀態(tài)反饋控制器橫向誤差平均值為0.148 m,誤差最大值為0.454 m;傳統(tǒng)模型預(yù)測控制器橫向誤差平均值為0.225 m,誤差最大值為0.681 m。

圖9為車輛以108 km/h行駛時(shí)的仿真結(jié)果。

圖9 108 km/h車速仿真結(jié)果

由于車輛高速行駛時(shí)輪胎接近附著極限,兩種控制方法得到的路徑跟蹤誤差均較大,在動(dòng)力學(xué)約束條件下本文方法對高速彎道軌跡改善程度較小。

車輛行駛偏差數(shù)據(jù)對比如表3和表4所示,由表可以看出,72 km/h速度下基于狀態(tài)反饋的跟蹤控制器行駛精度改善程度最大,車輛橫向偏差平均值和幅值降低33%以上,橫擺角偏差的幅值降低達(dá)61%;36 km/h速度下也有很好的跟蹤效果,車輛在中低速過彎時(shí)車輛偏差的最大值和平均值均小于傳統(tǒng)模型預(yù)測控制器;而以108 km/h高速行駛時(shí)橫擺角偏差有所改善,橫向偏差最大值有所增加。但由于彎道內(nèi)行駛車速一般較低,故基于狀態(tài)反饋的模型預(yù)測控制器能夠適用于彎道行駛。

表3 雙移線工況橫向偏差對比

表4 雙移線工況橫擺角偏差對比

5 結(jié)論

本文為解決車輛駛?cè)牒婉傠x彎道時(shí)行駛精度低的問題,提出了基于狀態(tài)反饋的路徑跟蹤橫向控制策略。在路徑跟蹤模型預(yù)測控制器基礎(chǔ)上,對于復(fù)雜行駛路況,在滿足控制量約束和輸出量約束的橫向控制器基礎(chǔ)上,加入針對輪胎的滑移穩(wěn)定性約束,提高車輛行駛安全性,根據(jù)工況調(diào)整合適的控制器時(shí)域參數(shù)使誤差均方根值最小。在傳統(tǒng)模型預(yù)測控制器基礎(chǔ)上,建立了基于LQR的狀態(tài)反饋調(diào)節(jié)器。以路面附著良好的雙移線道路為基本仿真工況,通過MATLAB/Simulink和Carsim對所設(shè)計(jì)的控制器進(jìn)行仿真驗(yàn)證,結(jié)果表明:相比于傳統(tǒng)模型預(yù)測控制器,基于橫擺角偏差反饋的改進(jìn)狀態(tài)反饋控制器在36 km/h車速下橫向偏差降低了16%以上,橫擺角偏差降低了33%以上;72 km/h車速下橫向偏差降低了33%以上,橫擺角偏差降低了了61%以上;108 km/h車速下偏差略有改善,驗(yàn)證了改進(jìn)控制器能夠提升車輛彎道內(nèi)中低速行駛時(shí)的跟蹤精度,使車輛駛離彎道時(shí)不會(huì)過早的轉(zhuǎn)向而偏離車道。