多算法結(jié)合的汽車參數(shù)和狀態(tài)估計(jì)方法研究

胡均平， 朱萬(wàn)霞， 李科軍2， 李勇成， 任常吉

(1.中南大學(xué) 機(jī)電工程學(xué)院，湖南 長(zhǎng)沙 410083； 2.中南林業(yè)科技大學(xué) 物流與交通學(xué)院，湖南 長(zhǎng)沙 410004)

汽車的行駛過(guò)程中，汽車狀態(tài)信息的準(zhǔn)確獲取，是汽車動(dòng)態(tài)安全控制系統(tǒng)穩(wěn)定工作的前提和基礎(chǔ)。但直接使用車速儀等技術(shù)成本過(guò)高，短時(shí)間內(nèi)難以普及。由此衍生出的采用加速度傳感器等較低成本傳感器，結(jié)合技術(shù)理論以實(shí)現(xiàn)汽車狀態(tài)估計(jì)逐漸成為研究的熱點(diǎn)[1-2]。

目前應(yīng)用最廣泛的是UKF(Unscented Kalman Filter)算法[3]、CKF(Cubature Kalman Filter)算法[4]及其改進(jìn)算法[5-6]等。Boada等人[7]提出了自適應(yīng)神經(jīng)網(wǎng)絡(luò)與UKF結(jié)合的車輛狀態(tài)觀測(cè)算法，對(duì)車速等狀態(tài)量進(jìn)行了聯(lián)合觀測(cè)；張家旭等人[8]采用交互式多模型與CKF結(jié)合的算法，實(shí)現(xiàn)了車速及橫擺角速度的估計(jì)。但上述方法都是將汽車參數(shù)(汽車質(zhì)量)或噪聲協(xié)方差設(shè)置為已知常量來(lái)實(shí)現(xiàn)狀態(tài)估計(jì)的。若汽車參數(shù)或噪聲協(xié)方差發(fā)生變化，濾波精度將會(huì)降低，嚴(yán)重時(shí)甚至?xí)斐蔀V波發(fā)散。

為實(shí)現(xiàn)汽車參數(shù)及狀態(tài)信息的并行估計(jì)，本文提出了一種遞推最小二乘算法(Recursive Least Squares,RLS)、蟻群優(yōu)化算法(Ant Colony Optimization,ACO)及容積卡爾曼濾波算法(CKF)多算法結(jié)合的估計(jì)算法。該算法對(duì)汽車參數(shù)及狀態(tài)信息進(jìn)行估計(jì)時(shí)，對(duì)噪聲協(xié)方差進(jìn)行實(shí)時(shí)尋優(yōu)，以實(shí)現(xiàn)算法的動(dòng)態(tài)自適應(yīng)。

1 汽車動(dòng)力學(xué)模型

1.1 整車模型

假設(shè)：① 忽略滾動(dòng)阻力對(duì)汽車狀態(tài)的影響；② 不考慮汽車的垂向運(yùn)動(dòng)；③ 不考慮汽車的俯仰及側(cè)傾運(yùn)動(dòng)。建立的三自由度非線性整車模型[9]，如圖1所示。

圖1 三自由度非線性整車模型

汽車運(yùn)動(dòng)微分方程如下。

縱向運(yùn)動(dòng)：

(1)

ax=[(Fx1+Fx2)cosθ-(Fy1+Fy2)sinθ+Fx3+Fx4]/m

(2)

側(cè)向運(yùn)動(dòng):

(3)

ay=[(Fx1+Fx2)sinθ+(Fy1+Fy2)cosθ+Fy3+Fy4]/m

(4)

橫擺運(yùn)動(dòng):

(5)

式中，ax、ay分別為縱向加速度和側(cè)向加速度；vx、vy分別為縱向速度和側(cè)向速度；r為橫擺角速度；m為汽車質(zhì)量；θ為前輪轉(zhuǎn)角；Fxi、Fyi(i=1,2,3,4)分別為各車輪上的縱向力和側(cè)向力；Iz為整車?yán)@垂直軸的轉(zhuǎn)動(dòng)慣量；Tf、Tr分別為前輪和后輪的輪距；L1、L2分別為整車質(zhì)心到前軸和后軸的距離。

1.2 Dugoff 輪胎模型

在保證準(zhǔn)確估算輪胎力的前提下，為減少計(jì)算時(shí)間，采用了參數(shù)較少的Dugoff 輪胎模型。作用在輪胎上的縱向力和側(cè)向力[10]分別為

(6)

(7)

(8)

(9)

式中，F(xiàn)zi(i=1,2,3,4)為輪胎垂向載荷；μ為路面附著系數(shù)；si為縱向滑移率；Cx、Cy分別為輪胎縱向剛度和側(cè)偏剛度；αi為輪胎側(cè)偏角；ε為速度影響因子。

輪胎模型的輸入?yún)?shù)Fzi、αi及si可以表示為θ、ax、ay、vx、vy的函數(shù)形式，詳見文獻(xiàn)[10]。

2 基于RLS算法的汽車參數(shù)估計(jì)

2.1 遞推最小二乘算法

RLS算法的估計(jì)精度高、實(shí)時(shí)性好，因此廣泛應(yīng)用于單輸入單輸出模型的參數(shù)識(shí)別。其輸入輸出遞推方程為[11]

(10)

式中，u(k)為輸入；y(k)為輸出；n為待估計(jì)參數(shù)個(gè)數(shù)；e(k)為噪聲或建模引起的誤差。將式(10)改寫為

y(k)=φ(k)Tψ+e(k)

(11)

式中，ψ=(a1,a2,…,an,b1,b2,…,bn)T;φ(k)=[-y(k-1),…,-y(k-n),u(k-1),…,u(k-n)]T。

算法的實(shí)現(xiàn)步驟如下。

① 參數(shù)估計(jì)增益。

K(k)=W(k-1)φ(k)[φ(k)TW(k-1)φ(k)+1]-1

(12)

② 參數(shù)估計(jì)更新。

(13)

③ 估計(jì)誤差更新。

W(k)=[I-K(k)φ(k)T]W(k-1)

(14)

式中，W(k)為誤差協(xié)方差；I為單位矩陣，其階數(shù)等于待估計(jì)參數(shù)個(gè)數(shù)。

2.2 汽車參數(shù)估計(jì)模型

本文所說(shuō)的汽車參數(shù)，特指汽車質(zhì)量。為獲取精確的汽車狀態(tài)估計(jì)，汽車起步時(shí)就進(jìn)行參數(shù)估計(jì)，此時(shí)輪胎處于線性區(qū)域[12]，因此建立汽車質(zhì)量估計(jì)模型。

(15)

式中，k1、k2分別為汽車前輪及后輪的側(cè)偏剛度總和。

整理式(15)，得到汽車質(zhì)量估計(jì)方程

(16)

3 基于ACOCKF算法的汽車狀態(tài)估計(jì)

3.1 蟻群優(yōu)化算法

ACO算法是Marco Dorigo提出的一種啟發(fā)式全局優(yōu)化方法，其靈感來(lái)自螞蟻尋找食物時(shí)出現(xiàn)的路徑選擇現(xiàn)象。在t時(shí)刻，螞蟻p從位置i向位置j轉(zhuǎn)移的概率為[13]

(17)

式中，τij(t)為t時(shí)刻螞蟻p留在位置i和位置j之間的信息素；ηij(t)為螞蟻p從位置i轉(zhuǎn)移到位置j的啟發(fā)程度；A為螞蟻p下一步可以選擇的位置集合；α表示軌跡的相對(duì)重要性；γ表示能見度的相對(duì)重要性。

一次循環(huán)完成后，(t+1)時(shí)刻各路徑上的信息素濃度按照下式進(jìn)行實(shí)時(shí)更新，即

(18)

(19)

3.2 容積卡爾曼濾波算法

CKF算法的實(shí)現(xiàn)步驟如下[14]：

(20)

(21)

(2) 時(shí)間更新。

① 誤差協(xié)方差Pk|k的Cholesky分解：

(22)

(23)

② 輸出容積點(diǎn)：

(24)

③ 狀態(tài)預(yù)測(cè)值：

(25)

④ 協(xié)方差預(yù)測(cè)值：

(26)

(3) 量測(cè)更新。

① 誤差協(xié)方差Pk+1|k的Cholesky分解：

(27)

(28)

② 傳播容積點(diǎn)：

Zi,k+1|k=h(Xi,k+1|k,uk)

(29)

③ 量測(cè)預(yù)測(cè)值：

(30)

④ 新息協(xié)方差：

(31)

⑤ 互協(xié)方差：

(32)

⑥ 增益矩陣：

(33)

⑦ 狀態(tài)變量：

(34)

⑧ 誤差協(xié)方差：

(35)

3.3 用于CKF算法優(yōu)化的ACO算法目標(biāo)函數(shù)確定

根據(jù)式(1)～式(9)，建立非線性汽車系統(tǒng)離散化后的狀態(tài)空間方程，即

(36)

式中，wk、vk分別為符合高斯分布、互不相關(guān)的過(guò)程噪聲和量測(cè)噪聲，它們的協(xié)方差分別為Q和R。

根據(jù)傳感器測(cè)量的θ、ax、ay、r及輪速w1、w2、w3、w4，估算vx、vy、r、μ。因此，狀態(tài)變量為x= [vx,vy,r,μ]T，系統(tǒng)輸入為u= [θ,w1,w2,w3,w4]T，觀測(cè)向量為z= [ax,ay,r]T。其中，將r設(shè)置為被估計(jì)量的目的是更好地利用量測(cè)信息，將μ設(shè)置為被估計(jì)量的目的是使算法能夠適應(yīng)各種路面。因此，可以得到過(guò)程噪聲協(xié)方差Q=diag[q11,q22,q33,q44]，量測(cè)噪聲協(xié)方差R=diag[r11,r22,r33]。其中，qii、rii分別為協(xié)方差Q和R的對(duì)角元素。

傳統(tǒng)的CKF算法將噪聲協(xié)方差Q和R設(shè)置為已知定值，但由于汽車系統(tǒng)的強(qiáng)非線性及其運(yùn)動(dòng)的復(fù)雜性，Q和R一般時(shí)變且未知。為使算法表現(xiàn)出良好的估計(jì)能力，將Q和R作為ACO算法的待優(yōu)化參數(shù)。為避免尋優(yōu)過(guò)程過(guò)早收斂，以便獲得全局最優(yōu)解，選取新息的實(shí)際方差作為目標(biāo)函數(shù)，以其值最小為目標(biāo)進(jìn)行尋優(yōu)。

定義目標(biāo)函數(shù)如下:

(37)

(38)

采用ACO算法對(duì)Q和R進(jìn)行實(shí)時(shí)優(yōu)化，以實(shí)現(xiàn) CKF算法的動(dòng)態(tài)自適應(yīng)。至此，建立了用于式(36)所示非線性汽車系統(tǒng)的狀態(tài)信息估計(jì)算法，即蟻群優(yōu)化容積卡爾曼濾波算法(Ant Colony Optimization Cubature Kalman Filter,ACOCKF) (式(17)～式(35)、式(37)、式(38))。

4 仿真驗(yàn)證

在CarSim中選擇某b型車為仿真車輛，部分參數(shù)為：m=1560 kg,L1=1.48 m,L2=1.08 m,Iz=1342 kg·m2,Tf=1.50 m,Tr=1.50 m,k1= -264500 N/rad,k2=-226790 N/rad。基于RLS+ACOCKF算法建立的CarSim/Simulink聯(lián)合仿真原理圖，如圖2所示。

圖2 聯(lián)合仿真原理圖

選擇蛇形、雙移線兩種工況，將CarSim的狀態(tài)輸出結(jié)果作為真實(shí)值，同CKF算法、RLS+CKF算法、ACOCKF算法及RLS+ACOCKF算法的估計(jì)結(jié)果進(jìn)行對(duì)比分析。為檢驗(yàn)本文提出算法對(duì)含有不準(zhǔn)確模型參數(shù)及未知時(shí)變?cè)肼暤姆蔷€性系統(tǒng)的估計(jì)性能，汽車參數(shù)初始值設(shè)置為m0=1200 kg，過(guò)程噪聲及量測(cè)噪聲均設(shè)為具有時(shí)變統(tǒng)計(jì)特性的高斯白噪聲，信息揮發(fā)系數(shù)ρ=0.28，采樣時(shí)間間隔T=0.02 s。

4.1 蛇形線仿真工況

汽車的蛇形線行駛路徑，如圖3所示。

圖3 蛇形線路徑

仿真工況：路面附著系數(shù)為0.8，車速為120 km/h。初值設(shè)置為x0=[120/3.6,0,0,0.8]T;P0=diag[1,1,1,1];Q0=0.1·diag[1,1,1,1];R0=0.5·diag[1,1,1];ψ0=1200;W0=1。基于RLS+ACOCKF算法的汽車質(zhì)量估計(jì)結(jié)果，如圖4所示。

圖4 蛇形線工況下質(zhì)量估計(jì)結(jié)果

由圖4可知，待估計(jì)的汽車質(zhì)量能夠在3 s內(nèi)跟蹤到真實(shí)值附近，且之后波動(dòng)較小，估計(jì)值與真實(shí)值的最大偏差為18.8 kg，相對(duì)誤差為1.21%，估計(jì)精度滿足控制要求。可見RLS+ACOCKF算法能夠較好地補(bǔ)償模型參數(shù)誤差，使得狀態(tài)估計(jì)建立在較準(zhǔn)確的模型之上。真實(shí)值同CKF算法、RLS+CKF算法、ACOCKF算法及RLS+ACOCKF算法的汽車狀態(tài)信息估計(jì)結(jié)果對(duì)比情況，如圖5～圖8所示。

圖5 蛇形線工況下縱向速度估計(jì)結(jié)果

圖6 蛇形線工況下側(cè)向速度估計(jì)結(jié)果

圖7 蛇形線工況下橫擺角速度估計(jì)結(jié)果

圖8 蛇形線工況下路面附著系數(shù)估計(jì)結(jié)果

由圖5～圖8可知，對(duì)于縱向速度，RLS+ACOCKF算法的估計(jì)值基本同真實(shí)值保持一致，估計(jì)精度優(yōu)于CKF算法、RLS+CKF算法及ACOCKF算法。對(duì)于側(cè)向速度及橫擺角速度，RLS+ACOCKF算法在估計(jì)過(guò)程中能對(duì)真實(shí)值進(jìn)行良好跟蹤，估計(jì)值同真實(shí)值基本吻合， CKF算法、RLS+CKF算法及ACOCKF算法在幅值處出現(xiàn)了比較大的瞬態(tài)估計(jì)誤差， RLS+ACOCKF算法的整體估計(jì)效果優(yōu)于其余3種算法。對(duì)于路面附著系數(shù)，由于汽車的連續(xù)快速轉(zhuǎn)向運(yùn)動(dòng)，輪胎進(jìn)入了非線性區(qū)域，4種算法的估計(jì)過(guò)程都出現(xiàn)了波動(dòng)，但RLS+ACOCKF算法在估計(jì)精度及收斂速度上仍具有優(yōu)越性。

4.2 雙移線仿真工況

汽車的雙移線行駛路徑，如圖9所示。

圖9 雙移線路徑

仿真工況為：路面附著系數(shù)為0.5，車速為80 km/h。初值設(shè)置為x0=[80/3.6,0,0,0.5]T;P0=diag[1,1,1,1];Q0=0.1·diag[1,1,1,1];R0=0.5·diag[1,1,1];Ψ0=1200;W0=1。基于RLS+ACOCKF算法的汽車質(zhì)量估計(jì)結(jié)果，如圖10所示。

圖10 雙移線工況下質(zhì)量估計(jì)結(jié)果

由圖10可知，待估計(jì)的汽車參數(shù)能夠在3 s內(nèi)實(shí)現(xiàn)對(duì)真實(shí)值的良好跟蹤，3 s后估計(jì)值與真實(shí)值的最大偏差為23.9 kg，相對(duì)誤差為1.53%，估計(jì)精度可以滿足控制要求。真實(shí)值同CKF算法、RLS+CKF算法、ACOCKF算法及RLS+ACOCKF算法的汽車狀態(tài)信息估計(jì)結(jié)果對(duì)比情況，如圖11～圖14所示。

圖11 雙移線工況下縱向速度估計(jì)結(jié)果

圖12 雙移線工況下側(cè)向速度估計(jì)結(jié)果

圖13 雙移線工況下橫擺角速度估計(jì)結(jié)果

圖14 雙移線工況下路面附著系數(shù)估計(jì)結(jié)果

由圖11～圖14可知，對(duì)于縱向速度，RLS+ACOCKF算法的整體估計(jì)結(jié)果比較理想，收斂精度及收斂趨勢(shì)均優(yōu)于CKF算法、RLS+CKF算法及ACOCKF算法。對(duì)于側(cè)向速度及橫擺角速度，RLS+ACOCKF算法在估計(jì)過(guò)程中能對(duì)真實(shí)值進(jìn)行良好跟蹤，CKF算法、RLS+CKF算法及ACOCKF算法的估計(jì)值同真實(shí)值之間存在一定的誤差，在幅值處的瞬態(tài)估計(jì)誤差達(dá)到了最大。對(duì)于路面附著系數(shù)，RLS+ACOCKF算法的整體估計(jì)效果較好，估計(jì)值同真實(shí)值一直很接近，而其余三種算法的估計(jì)結(jié)果都出現(xiàn)了較大波動(dòng)，RLS+ACOCKF算法在估計(jì)精度及收斂速度上表現(xiàn)出良好的優(yōu)勢(shì)。

為了定量地評(píng)價(jià)CKF算法、RLS+CKF算法、ACOCKF算法及RLS+ACOCKF算法的預(yù)測(cè)效果，給出了平均絕對(duì)誤差(MAE)指標(biāo)及均方差根誤差(RMSE)指標(biāo)[15]。表達(dá)式分別為

(39)

(40)

4種算法的誤差指標(biāo)如表1和表2所示。從表1和表2中很直觀地看出RLS+ACOCKF算法的MAE指標(biāo)值及RMSE指標(biāo)值都低于CKF算法、RLS+CKF算法及ACOCKF算法，可見RLS+ACOCKF算法的估計(jì)精度優(yōu)于其余三種算法。

表1 蛇形線工況下4種算法的MAE指標(biāo)和RMSE指標(biāo)

表2 雙移線工況下4種算法的MAE指標(biāo)和RMSE指標(biāo)

由對(duì)系統(tǒng)狀態(tài)變量的估計(jì)結(jié)果分析可知，RLS+ACOCKF算法的整體估計(jì)效果顯著優(yōu)于CKF算法、RLS+CKF算法及ACOCKF算法。在估計(jì)過(guò)程中，CKF算法以不準(zhǔn)確的汽車參數(shù)初始值代替真實(shí)汽車參數(shù)且按照固定的協(xié)方差進(jìn)行計(jì)算，因此估計(jì)結(jié)果誤差較大。RLS+CKF算法只能實(shí)時(shí)更新汽車參數(shù)信息，但不能實(shí)時(shí)優(yōu)化噪聲協(xié)方差，ACOCKF算法只能對(duì)噪聲協(xié)方差進(jìn)行實(shí)時(shí)優(yōu)化，但不能實(shí)時(shí)校正汽車模型參數(shù)信息，因此這兩種算法的估計(jì)精度及收斂速度雖優(yōu)于CKF算法，但仍具有一定的局限性。而RLS+ACOCKF算法在進(jìn)行狀態(tài)估計(jì)時(shí)，實(shí)時(shí)校正汽車參數(shù)并對(duì)噪聲協(xié)方差進(jìn)行尋優(yōu)，因此減小了估計(jì)誤差，使得估計(jì)結(jié)果更加接近于真實(shí)值。

5 結(jié)束語(yǔ)

筆者提出了一種用于汽車參數(shù)信息及狀態(tài)信息估計(jì)的多算法結(jié)合的自適應(yīng)算法。該算法將RLS算法與CKF算法進(jìn)行有效結(jié)合，并利用ACO算法的尋優(yōu)功能對(duì)CKF算法的噪聲協(xié)方差進(jìn)行實(shí)時(shí)優(yōu)化，解決了時(shí)變統(tǒng)計(jì)特性噪聲的估計(jì)問(wèn)題，實(shí)現(xiàn)了汽車參數(shù)及狀態(tài)信息的并行估計(jì)。聯(lián)合仿真實(shí)驗(yàn)結(jié)果表明，在過(guò)程噪聲及量測(cè)噪聲具有未知時(shí)變統(tǒng)計(jì)特性時(shí)，該算法能夠保持較高的估計(jì)精度，具有一定的自適應(yīng)性。

選取蛇形及雙移線兩種典型工況開展對(duì)比試驗(yàn)，結(jié)果表明，RLS+ACOCKF算法的估計(jì)結(jié)果優(yōu)于傳統(tǒng)CKF算法、RLS+CKF算法及ACOCKF算法，可以更準(zhǔn)確地估計(jì)車速及橫擺角速度等狀態(tài)量，算法的穩(wěn)定性高、魯棒性強(qiáng)、實(shí)時(shí)性好。