胡敬宇 汪 嚴(yán)永俊 耿可可 柏 碩 殷國(guó)棟
(東南大學(xué)機(jī)械工程學(xué)院, 南京 211189)
隨著科學(xué)技術(shù)的進(jìn)步發(fā)展,車(chē)輛主動(dòng)安全系統(tǒng)已經(jīng)被證明是減少交通事故發(fā)生的最有效方式之一.而車(chē)輛狀態(tài)信息的精確獲取是主動(dòng)安全系統(tǒng)有效運(yùn)行的重要前提[1].然而,受限于測(cè)量技術(shù)的發(fā)展,部分車(chē)輛狀態(tài)信息無(wú)法依靠目前的車(chē)載傳感器直接獲取.為了解決這一問(wèn)題,一種方法是利用價(jià)格昂貴的傳感器去測(cè)量,但由于該方法成本較高,難以滿足量產(chǎn)需求,僅停留在實(shí)驗(yàn)階段;另一種方法是結(jié)合已有的車(chē)載傳感器,并利用一些先進(jìn)的濾波、觀測(cè)器等,去構(gòu)造獲取車(chē)輛狀態(tài)信息的算法.后者因其具有成本低、操作方便、估計(jì)精準(zhǔn)等優(yōu)勢(shì),成為眾多專家學(xué)者研究的熱點(diǎn).
基于車(chē)輛運(yùn)動(dòng)學(xué)模型的車(chē)輛狀態(tài)估計(jì)具有極好的魯棒性,可以忽略車(chē)輛模型參數(shù)變化的影響,但該方法對(duì)傳感器有著極高的安裝、標(biāo)定以及精度要求;基于車(chē)輛動(dòng)力學(xué)模型的車(chē)輛狀態(tài)估計(jì)對(duì)傳感器要求低,但系統(tǒng)模型本身要盡可能準(zhǔn)確地反映車(chē)輛動(dòng)力學(xué)特性,且噪聲統(tǒng)計(jì)特性的不準(zhǔn)確也將會(huì)影響狀態(tài)估計(jì)的精確性[2].在車(chē)輛狀態(tài)估計(jì)問(wèn)題的研究中,常見(jiàn)的方法有卡爾曼濾波算法[3-4]、龍貝格(Luenberger)觀測(cè)器[5]、魯棒觀測(cè)器[6]、滑模觀測(cè)器[7]、基于李雅普諾夫理論的非線性觀測(cè)器[8]、神經(jīng)網(wǎng)絡(luò)觀測(cè)器[9-10]以及模糊觀測(cè)器[11]等.其中,卡爾曼濾波作為一種時(shí)域?yàn)V波方法,采用狀態(tài)空間方程對(duì)系統(tǒng)加以描述,算法采用遞推形式,使得其數(shù)據(jù)存儲(chǔ)量小,不僅可以處理簡(jiǎn)單線性系統(tǒng),在不斷地發(fā)展之下也可以處理復(fù)雜的多維非線性系統(tǒng)[12],因此卡爾曼濾波算法一直是車(chē)輛狀態(tài)估計(jì)算法領(lǐng)域研究中的主流[13].
標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波方法以線性最小方差估計(jì)為理論依據(jù),通過(guò)遞推算法對(duì)狀態(tài)變量進(jìn)行濾波估計(jì),雖然具有較高的估計(jì)精度,但與傳統(tǒng)卡爾曼濾波器同為無(wú)限增長(zhǎng)的記憶濾波器,其計(jì)算效率和估計(jì)精度會(huì)隨著時(shí)間的推移而降低,也無(wú)法有效解決模型不準(zhǔn)或噪聲特性未知對(duì)濾波算法的影響.文獻(xiàn)[14]提出一種限定記憶的濾波器,其儲(chǔ)存一定記憶長(zhǎng)度的歷史量測(cè)數(shù)據(jù),在非線性條件下計(jì)算條件概率密度函數(shù)及其參數(shù),即均值和協(xié)方差矩陣,在一定程度上克服了濾波的發(fā)散;文獻(xiàn)[15]針對(duì)新息殘差序列對(duì)漸變?cè)肼暤慕y(tǒng)計(jì)滯后問(wèn)題,提出一種限定記憶噪聲在線估計(jì)的擴(kuò)展卡爾曼算法,一定程度上提高了濾波檢測(cè)速度和計(jì)算精度;文獻(xiàn)[16]將最小模型誤差準(zhǔn)則與擴(kuò)展卡爾曼濾波相結(jié)合,提出考慮任意非線性模型誤差和高斯白噪聲的狀態(tài)估計(jì)系統(tǒng),該系統(tǒng)可以有效發(fā)現(xiàn)動(dòng)態(tài)誤差并將誤差用于模型的更新;文獻(xiàn)[17]提出新的自適應(yīng)隨機(jī)加權(quán)濾波算法,建立限定記憶條件下的隨機(jī)加權(quán)理論[18],通過(guò)自適應(yīng)調(diào)整測(cè)量噪聲統(tǒng)計(jì)量的權(quán)重來(lái)在線估計(jì)測(cè)量噪聲及其協(xié)方差,仿真分析表明,該算法可以有效抑制測(cè)量噪聲統(tǒng)計(jì)特性未知情況下對(duì)狀態(tài)估計(jì)的影響.
以上研究表明,融合了限定記憶算法的擴(kuò)展卡爾曼濾波,可以很好地解決歷史量測(cè)數(shù)據(jù)誤差帶來(lái)的影響.車(chē)輛關(guān)鍵狀態(tài)信息的準(zhǔn)確獲取對(duì)車(chē)輛穩(wěn)定性控制、車(chē)輛主動(dòng)安全系統(tǒng)等至關(guān)重要,而在車(chē)輛狀態(tài)估計(jì)領(lǐng)域,融合限定記憶濾波和隨機(jī)加權(quán)思想的擴(kuò)展卡爾曼濾波的研究甚少.因此,本文提出一種基于限定記憶隨機(jī)加權(quán)擴(kuò)展卡爾曼濾波(limited memory random weighted extended Kalman filter,LMRWEKF)算法,將擴(kuò)展卡爾曼濾波算法和限定記憶濾波相結(jié)合,通過(guò)引入服從Dirichlet分布的加權(quán)系數(shù)矩陣,提高濾波估計(jì)過(guò)程中對(duì)量測(cè)新值的利用程度,降低舊測(cè)量數(shù)據(jù)誤差的影響,有效提高了濾波算法的精確性,仿真實(shí)驗(yàn)表明,該算法可以很好地抑制由噪聲波動(dòng)引起的濾波估算偏差問(wèn)題,實(shí)現(xiàn)量測(cè)噪聲特性未知條件下車(chē)輛橫擺角速度、質(zhì)心側(cè)偏角和縱向加速度的有效估計(jì).
在較為準(zhǔn)確地描述車(chē)輛動(dòng)力學(xué)的基礎(chǔ)上盡量減少控制算法的計(jì)算量.通常情況下,在考慮車(chē)輛平面運(yùn)動(dòng)時(shí)忽略懸架系統(tǒng)的影響,重點(diǎn)考慮純側(cè)偏輪胎特性,忽略縱橫向輪胎力的耦合關(guān)系、輪胎的橫向載荷轉(zhuǎn)移以及車(chē)輛的俯仰角和側(cè)傾角,在線性二自由度車(chē)輛模型的基礎(chǔ)上,引入一個(gè)縱向運(yùn)動(dòng)自由度構(gòu)建車(chē)輛非線性三自由度模型[19].動(dòng)力學(xué)模型示意圖如圖1所示,估算模型質(zhì)心與車(chē)輛坐標(biāo)系原點(diǎn)重合,X軸正向表示車(chē)輛行駛方向,Y軸正向表示車(chē)輛行駛方向的左方,橫擺方向正方向?yàn)槔@坐標(biāo)軸原點(diǎn)逆時(shí)針?lè)较?,則在上述前提條件下,建立非線性三自由度車(chē)輛動(dòng)力學(xué)模型.圖中,vx和vy分別為車(chē)體坐標(biāo)系下質(zhì)心的縱向速度和側(cè)向速度;a和b分別為車(chē)輛質(zhì)心與前、后軸的距離;γ為橫擺角速度;β為質(zhì)心側(cè)偏角;δ為車(chē)輛前輪轉(zhuǎn)角;a1和a2分別為前輪側(cè)偏角和后輪側(cè)偏角;vx1和vx2分別為前輪縱向速度和后輪縱向速度;Fy1和Fy2分別為前輪側(cè)向力和后輪側(cè)向力.
圖1 三自由度車(chē)輛模型
狀態(tài)方程
(1)
(2)
(3)
量測(cè)方程
(4)
式中,k1和k2分別為車(chē)輛前、后輪側(cè)偏剛度總和;Iz為車(chē)輛繞z軸的轉(zhuǎn)動(dòng)慣量;ax和ay分別為車(chē)輛縱向、側(cè)向加速度;m為車(chē)輛質(zhì)量.
根據(jù)式(1)~(4),建立非線性車(chē)輛三自由度系統(tǒng)的狀態(tài)方程和量測(cè)方程
(5)
狀態(tài)變量
x(t)=[γ,β,vx]T
(6)
量測(cè)輸出
z(t)=[ay]
(7)
輸入控制變量
u(t)=[δ,ax]T
(8)
式中,f為狀態(tài)轉(zhuǎn)移函數(shù);h為測(cè)量函數(shù);w(t)和v(t)分別為過(guò)程噪聲和觀測(cè)噪聲,且兩者序列彼此獨(dú)立.Q和R分別為噪聲w(t)和v(t)的統(tǒng)計(jì)特性,即過(guò)程噪聲和觀測(cè)噪聲的協(xié)方差矩陣.
根據(jù)式(1)~(4),非線性三自由度車(chē)輛動(dòng)力學(xué)方程中,狀態(tài)轉(zhuǎn)移矩陣A、控制矩陣B、觀測(cè)矩陣H可分別表示為
(9)
(10)
(11)
利用非線性函數(shù)的局部線性特性,將其局部線性化.其中狀態(tài)轉(zhuǎn)移矩陣A和觀測(cè)矩陣H由式(5)中函數(shù)f和h的雅可比矩陣代替.假設(shè)狀態(tài)變量的維數(shù)為n,相應(yīng)的雅可比矩陣的求解過(guò)程如下所示:
(12)
根據(jù)式(1)~(3)以及式(12)可得矩陣
(13)
設(shè)采樣時(shí)間為Δt,則線性化后的狀態(tài)轉(zhuǎn)移矩陣Α*可表示為
A*(t)=eF(t)Δt≈I+F(t)Δt
(14)
(15)
根據(jù)式(4)以及式(15)可得線性化后的觀測(cè)矩陣H*為
(16)
2.3.1 隨機(jī)加權(quán)估計(jì)
假設(shè)(σ1,σ2,…,σn)是來(lái)自分布函數(shù)G(x)的獨(dú)立同分布樣本,且經(jīng)驗(yàn)分布函數(shù)為
(17)
(18)
g(σ1,σ2,…,σn)=Θ(n)
(19)
2.3.2 限定記憶濾波
傳統(tǒng)記憶增長(zhǎng)的濾波是一種無(wú)限增加的存儲(chǔ)濾波器,如果要在k時(shí)刻進(jìn)行最優(yōu)估計(jì),那么需要使用包括k-1時(shí)刻之前的所有數(shù)據(jù),這代表著隨著時(shí)間的遞增,舊數(shù)據(jù)在濾波中的比例會(huì)不斷增加,而新量測(cè)數(shù)據(jù)所占權(quán)重會(huì)相對(duì)減小.當(dāng)噪聲特性未知時(shí),會(huì)導(dǎo)致誤差過(guò)大而估計(jì)不準(zhǔn)甚至發(fā)散.為此,采用限定記憶濾波思想,只選取當(dāng)前時(shí)刻及之前的N-1個(gè)觀測(cè)值來(lái)進(jìn)行最優(yōu)估計(jì)計(jì)算,可以很好地避免上述問(wèn)題.
2.3.3LMRWEKF算法
本文引入以上2種思想,基于擴(kuò)展卡爾曼濾波提出LMRWEKF算法.
對(duì)于離散的狀態(tài)方程和量測(cè)方程
(20)
式中,Xk-1和Xk分別為k-1時(shí)刻和k時(shí)刻的狀態(tài)變量矩陣;Uk-1為k-1時(shí)刻的控制變量矩陣;Zk為k時(shí)刻的測(cè)量輸出矩陣;Ak|k-1為k-1時(shí)刻對(duì)k時(shí)刻進(jìn)行預(yù)測(cè)的狀態(tài)轉(zhuǎn)移矩陣;Bk|k-1為k-1時(shí)刻對(duì)k時(shí)刻進(jìn)行預(yù)測(cè)的控制矩陣;Hk為k時(shí)刻的觀測(cè)矩陣;Wk-1為k-1時(shí)刻的過(guò)程噪聲矩陣;Vk為k時(shí)刻的觀測(cè)噪聲矩陣.
其處理的基本流程為
② 狀態(tài)方程預(yù)測(cè),即
(21)
③ 將狀態(tài)轉(zhuǎn)移矩陣和觀測(cè)矩陣線性化,即
(22)
④ 協(xié)方差矩陣預(yù)測(cè),預(yù)測(cè)方程為
(23)
⑤ 觀測(cè)方程預(yù)測(cè),即
(24)
⑥ 得到觀測(cè)誤差,為
(25)
⑦ 判斷,
若k≤N
(26)
若k>N
(27)
(28)
⑧ 計(jì)算濾波增益,計(jì)算式為
(29)
⑨ 狀態(tài)更新,即
(30)
⑩ 協(xié)方差矩陣更新,得
(31)
參考文獻(xiàn)[20]簡(jiǎn)化協(xié)方差矩陣更新,計(jì)算如下:
(32)
即協(xié)方差矩陣更新又可以表示為
(33)
本文設(shè)計(jì)限定記憶隨機(jī)加權(quán)擴(kuò)展卡爾曼濾波器對(duì)車(chē)輛橫擺角速度、質(zhì)心側(cè)偏角和縱向速度進(jìn)行濾波估計(jì),建立Carsim與Matlab/Simulink聯(lián)合仿真平臺(tái)進(jìn)行仿真實(shí)驗(yàn),并與傳統(tǒng)標(biāo)準(zhǔn)擴(kuò)展卡爾曼仿真結(jié)果進(jìn)行比較分析.
(a)前輪轉(zhuǎn)角輸入
從圖2(d)和(e)中可以看出,相比于傳統(tǒng)EKF算法,LMRWEKF算法的濾波估計(jì)值可以更好地接近參考值,而且在曲線波峰和波谷處偏離相對(duì)較小,并且對(duì)噪聲有明顯的抑制作用;從圖2(f)中可以看出,高附著工況下LMRWEKF算法的縱向速度估計(jì)值明顯比傳統(tǒng)EKF算法更加接近參考值.針對(duì)噪聲條件未知的情況,LMRWEKF算法對(duì)噪聲的抑制作用更強(qiáng),具有更優(yōu)的穩(wěn)定性和魯棒性.
LMRWEKF可以很好地抑制噪聲波動(dòng),為進(jìn)一步比較算法的估計(jì)精度,采用均方根指標(biāo)來(lái)對(duì)估計(jì)精度進(jìn)行評(píng)估對(duì)比,如表1所示.分析表1數(shù)據(jù)可以看出,高附著工況下,基于LMRWEKF算法估計(jì)得到的橫擺角速度、質(zhì)心側(cè)偏角和縱向速度的均方根誤差RMSEγ、RMSEβ、RMSEvx較EKF算法分別降低了60.23%、19.63%、91.57%.
表1 高附著工況下RMSE指標(biāo)對(duì)比
從圖3(d)和(e)中可以看出,在曲線波峰和波谷處,相比于傳統(tǒng)EKF算法,LMRWEKF算法可以更好地貼近參考值,偏離相對(duì)較小,且對(duì)噪聲波動(dòng)有較強(qiáng)的抑制作用;從圖3(f)中可以看出,低附著工況下LMRWEKF算法的縱向速度估計(jì)值明顯比傳統(tǒng)EKF算法更加接近參考值,并且在噪聲特性未知時(shí),LMRWEKF算法抖動(dòng)更小,說(shuō)明對(duì)噪聲有很好的抑制作用,具有更強(qiáng)的穩(wěn)定性和魯棒性.
(a)前輪轉(zhuǎn)角輸入
LMRWEKF可以很好地對(duì)噪聲波動(dòng)進(jìn)行抑制,為進(jìn)一步比較算法的估計(jì)精度,采用均方根指標(biāo)來(lái)對(duì)估計(jì)精度進(jìn)行評(píng)估對(duì)比,如表2所示.分析表2數(shù)據(jù)可以看出,低附著工況下,基于LMRWEKF算法估計(jì)得到的橫擺角速度、質(zhì)心側(cè)偏角和縱向速度的均方根誤差RMSEγ、RMSEβ、RMSEvx較EKF算法分別降低了59.38%、13.92%、94.20%.
表2 低附著工況下RMSE指標(biāo)對(duì)比
1)本文在線性二自由度車(chē)輛模型的基礎(chǔ)上引入一個(gè)縱向運(yùn)動(dòng)自由度,建立包括縱向、側(cè)向和橫擺3個(gè)自由度的非線性車(chē)輛動(dòng)力學(xué)模型,將擴(kuò)展卡爾曼濾波與限定記憶濾波相結(jié)合,在限定記憶長(zhǎng)度區(qū)間內(nèi),通過(guò)檢測(cè)新息殘差序列,利用數(shù)理統(tǒng)計(jì)的方法得到當(dāng)前時(shí)刻測(cè)量噪聲矩陣的協(xié)方差矩陣,從而降低因模型不準(zhǔn)導(dǎo)致的估計(jì)誤差;利用隨機(jī)加權(quán)理論,引入隨機(jī)加權(quán)系數(shù),提高濾波系統(tǒng)對(duì)新量測(cè)值的利用程度,從而提高濾波算法的精度,增強(qiáng)了自適應(yīng)性.
2)為了驗(yàn)證LMRWEKF算法的有效性和精確性,設(shè)計(jì)高附著系數(shù)和低附著系數(shù)2種不同工況下的仿真實(shí)驗(yàn),仿真結(jié)果表明:以高附著工況下橫擺角速度為例,LMRWEKF算法的濾波估計(jì)值均方根誤差較傳統(tǒng)擴(kuò)展卡爾曼估計(jì)值誤差降低60.23%,整體估計(jì)性能更優(yōu).
3)本文提出的LMRWEKF算法可以實(shí)時(shí)動(dòng)態(tài)地調(diào)整濾波估計(jì)算法的噪聲統(tǒng)計(jì)特性,因此能夠應(yīng)對(duì)噪聲特性未知的問(wèn)題,其限定記憶的算法特性可以在計(jì)算能力較差的條件下仍然具有較快的響應(yīng)速度和估計(jì)精度,實(shí)際應(yīng)用場(chǎng)景廣泛.