胡敬宇 汪 嚴(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ì).

1 非線性三自由度車(chē)輛模型

在較為準(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ì)量.

2 基于LMRWEKF的車(chē)輛狀態(tài)估計(jì)算法及其實(shí)現(xiàn)

2.1 狀態(tài)空間方程模型

根據(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)

2.2 模型線性化

利用非線性函數(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 LMRWEKF算法設(shè)計(jì)

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)

3 仿真實(shí)驗(yàn)

本文設(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)行比較分析.

3.1 高附著路面仿真實(shí)驗(yà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.2 低附著路面仿真實(shí)驗(yàn)

從圖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ì)比

4 結(jié)論

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)景廣泛.