基于限定記憶隨機加權(quán)擴展卡爾曼濾波的車輛狀態(tài)估計

胡敬宇 汪 嚴永俊 耿可可 柏 碩 殷國棟

(東南大學機械工程學院, 南京 211189)

隨著科學技術(shù)的進步發(fā)展，車輛主動安全系統(tǒng)已經(jīng)被證明是減少交通事故發(fā)生的最有效方式之一.而車輛狀態(tài)信息的精確獲取是主動安全系統(tǒng)有效運行的重要前提[1].然而，受限于測量技術(shù)的發(fā)展，部分車輛狀態(tài)信息無法依靠目前的車載傳感器直接獲取.為了解決這一問題，一種方法是利用價格昂貴的傳感器去測量，但由于該方法成本較高，難以滿足量產(chǎn)需求，僅停留在實驗階段；另一種方法是結(jié)合已有的車載傳感器，并利用一些先進的濾波、觀測器等，去構(gòu)造獲取車輛狀態(tài)信息的算法.后者因其具有成本低、操作方便、估計精準等優(yōu)勢，成為眾多專家學者研究的熱點.

基于車輛運動學模型的車輛狀態(tài)估計具有極好的魯棒性，可以忽略車輛模型參數(shù)變化的影響，但該方法對傳感器有著極高的安裝、標定以及精度要求；基于車輛動力學模型的車輛狀態(tài)估計對傳感器要求低，但系統(tǒng)模型本身要盡可能準確地反映車輛動力學特性，且噪聲統(tǒng)計特性的不準確也將會影響狀態(tài)估計的精確性[2].在車輛狀態(tài)估計問題的研究中，常見的方法有卡爾曼濾波算法[3-4]、龍貝格(Luenberger)觀測器[5]、魯棒觀測器[6]、滑模觀測器[7]、基于李雅普諾夫理論的非線性觀測器[8]、神經(jīng)網(wǎng)絡觀測器[9-10]以及模糊觀測器[11]等.其中，卡爾曼濾波作為一種時域濾波方法，采用狀態(tài)空間方程對系統(tǒng)加以描述，算法采用遞推形式，使得其數(shù)據(jù)存儲量小，不僅可以處理簡單線性系統(tǒng)，在不斷地發(fā)展之下也可以處理復雜的多維非線性系統(tǒng)[12]，因此卡爾曼濾波算法一直是車輛狀態(tài)估計算法領(lǐng)域研究中的主流[13].

標準擴展卡爾曼濾波方法以線性最小方差估計為理論依據(jù)，通過遞推算法對狀態(tài)變量進行濾波估計，雖然具有較高的估計精度，但與傳統(tǒng)卡爾曼濾波器同為無限增長的記憶濾波器，其計算效率和估計精度會隨著時間的推移而降低，也無法有效解決模型不準或噪聲特性未知對濾波算法的影響.文獻[14]提出一種限定記憶的濾波器，其儲存一定記憶長度的歷史量測數(shù)據(jù)，在非線性條件下計算條件概率密度函數(shù)及其參數(shù)，即均值和協(xié)方差矩陣，在一定程度上克服了濾波的發(fā)散；文獻[15]針對新息殘差序列對漸變噪聲的統(tǒng)計滯后問題，提出一種限定記憶噪聲在線估計的擴展卡爾曼算法，一定程度上提高了濾波檢測速度和計算精度；文獻[16]將最小模型誤差準則與擴展卡爾曼濾波相結(jié)合，提出考慮任意非線性模型誤差和高斯白噪聲的狀態(tài)估計系統(tǒng)，該系統(tǒng)可以有效發(fā)現(xiàn)動態(tài)誤差并將誤差用于模型的更新；文獻[17]提出新的自適應隨機加權(quán)濾波算法，建立限定記憶條件下的隨機加權(quán)理論[18]，通過自適應調(diào)整測量噪聲統(tǒng)計量的權(quán)重來在線估計測量噪聲及其協(xié)方差，仿真分析表明，該算法可以有效抑制測量噪聲統(tǒng)計特性未知情況下對狀態(tài)估計的影響.

以上研究表明，融合了限定記憶算法的擴展卡爾曼濾波，可以很好地解決歷史量測數(shù)據(jù)誤差帶來的影響.車輛關(guān)鍵狀態(tài)信息的準確獲取對車輛穩(wěn)定性控制、車輛主動安全系統(tǒng)等至關(guān)重要，而在車輛狀態(tài)估計領(lǐng)域，融合限定記憶濾波和隨機加權(quán)思想的擴展卡爾曼濾波的研究甚少.因此，本文提出一種基于限定記憶隨機加權(quán)擴展卡爾曼濾波(limited memory random weighted extended Kalman filter，LMRWEKF)算法，將擴展卡爾曼濾波算法和限定記憶濾波相結(jié)合，通過引入服從Dirichlet分布的加權(quán)系數(shù)矩陣，提高濾波估計過程中對量測新值的利用程度，降低舊測量數(shù)據(jù)誤差的影響，有效提高了濾波算法的精確性，仿真實驗表明，該算法可以很好地抑制由噪聲波動引起的濾波估算偏差問題，實現(xiàn)量測噪聲特性未知條件下車輛橫擺角速度、質(zhì)心側(cè)偏角和縱向加速度的有效估計.

1 非線性三自由度車輛模型

在較為準確地描述車輛動力學的基礎(chǔ)上盡量減少控制算法的計算量.通常情況下，在考慮車輛平面運動時忽略懸架系統(tǒng)的影響，重點考慮純側(cè)偏輪胎特性，忽略縱橫向輪胎力的耦合關(guān)系、輪胎的橫向載荷轉(zhuǎn)移以及車輛的俯仰角和側(cè)傾角，在線性二自由度車輛模型的基礎(chǔ)上，引入一個縱向運動自由度構(gòu)建車輛非線性三自由度模型[19].動力學模型示意圖如圖1所示，估算模型質(zhì)心與車輛坐標系原點重合，X軸正向表示車輛行駛方向，Y軸正向表示車輛行駛方向的左方，橫擺方向正方向為繞坐標軸原點逆時針方向，則在上述前提條件下，建立非線性三自由度車輛動力學模型.圖中，vx和vy分別為車體坐標系下質(zhì)心的縱向速度和側(cè)向速度；a和b分別為車輛質(zhì)心與前、后軸的距離；γ為橫擺角速度；β為質(zhì)心側(cè)偏角；δ為車輛前輪轉(zhuǎn)角；a1和a2分別為前輪側(cè)偏角和后輪側(cè)偏角；vx1和vx2分別為前輪縱向速度和后輪縱向速度；Fy1和Fy2分別為前輪側(cè)向力和后輪側(cè)向力.

圖1 三自由度車輛模型

狀態(tài)方程

(1)

(2)

(3)

量測方程

(4)

式中，k1和k2分別為車輛前、后輪側(cè)偏剛度總和；Iz為車輛繞z軸的轉(zhuǎn)動慣量；ax和ay分別為車輛縱向、側(cè)向加速度;m為車輛質(zhì)量.

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

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

根據(jù)式(1)～(4)，建立非線性車輛三自由度系統(tǒng)的狀態(tài)方程和量測方程

(5)

狀態(tài)變量

x(t)=[γ,β,vx]T

(6)

量測輸出

z(t)=[ay]

(7)

輸入控制變量

u(t)=[δ,ax]T

(8)

式中，f為狀態(tài)轉(zhuǎn)移函數(shù)；h為測量函數(shù)；w(t)和v(t)分別為過程噪聲和觀測噪聲，且兩者序列彼此獨立.Q和R分別為噪聲w(t)和v(t)的統(tǒng)計特性，即過程噪聲和觀測噪聲的協(xié)方差矩陣.

根據(jù)式(1)～(4)，非線性三自由度車輛動力學方程中，狀態(tài)轉(zhuǎn)移矩陣A、控制矩陣B、觀測矩陣H可分別表示為

(9)

(10)

(11)

2.2 模型線性化

利用非線性函數(shù)的局部線性特性，將其局部線性化.其中狀態(tài)轉(zhuǎn)移矩陣A和觀測矩陣H由式(5)中函數(shù)f和h的雅可比矩陣代替.假設(shè)狀態(tài)變量的維數(shù)為n，相應的雅可比矩陣的求解過程如下所示：

(12)

根據(jù)式(1)～(3)以及式(12)可得矩陣

(13)

設(shè)采樣時間為Δt，則線性化后的狀態(tài)轉(zhuǎn)移矩陣Α*可表示為

A*(t)=eF(t)Δt≈I+F(t)Δt

(14)

(15)

根據(jù)式(4)以及式(15)可得線性化后的觀測矩陣H*為

(16)

2.3 LMRWEKF算法設(shè)計

2.3.1 隨機加權(quán)估計

假設(shè)(σ1,σ2,…,σn)是來自分布函數(shù)G(x)的獨立同分布樣本，且經(jīng)驗分布函數(shù)為

(17)

(18)

g(σ1,σ2,…,σn)=Θ(n)

(19)

2.3.2 限定記憶濾波

傳統(tǒng)記憶增長的濾波是一種無限增加的存儲濾波器，如果要在k時刻進行最優(yōu)估計，那么需要使用包括k-1時刻之前的所有數(shù)據(jù)，這代表著隨著時間的遞增，舊數(shù)據(jù)在濾波中的比例會不斷增加，而新量測數(shù)據(jù)所占權(quán)重會相對減小.當噪聲特性未知時，會導致誤差過大而估計不準甚至發(fā)散.為此，采用限定記憶濾波思想，只選取當前時刻及之前的N-1個觀測值來進行最優(yōu)估計計算，可以很好地避免上述問題.

2.3.3LMRWEKF算法

本文引入以上2種思想，基于擴展卡爾曼濾波提出LMRWEKF算法.

對于離散的狀態(tài)方程和量測方程

(20)

式中，Xk-1和Xk分別為k-1時刻和k時刻的狀態(tài)變量矩陣；Uk-1為k-1時刻的控制變量矩陣；Zk為k時刻的測量輸出矩陣；Ak|k-1為k-1時刻對k時刻進行預測的狀態(tài)轉(zhuǎn)移矩陣；Bk|k-1為k-1時刻對k時刻進行預測的控制矩陣；Hk為k時刻的觀測矩陣；Wk-1為k-1時刻的過程噪聲矩陣；Vk為k時刻的觀測噪聲矩陣.

其處理的基本流程為

② 狀態(tài)方程預測，即

(21)

③ 將狀態(tài)轉(zhuǎn)移矩陣和觀測矩陣線性化，即

(22)

④ 協(xié)方差矩陣預測，預測方程為

(23)

⑤ 觀測方程預測，即

(24)

⑥ 得到觀測誤差，為

(25)

⑦ 判斷，

若k≤N

(26)

若k>N

(27)

(28)

⑧ 計算濾波增益，計算式為

(29)

⑨ 狀態(tài)更新，即

(30)

⑩ 協(xié)方差矩陣更新，得

(31)

參考文獻[20]簡化協(xié)方差矩陣更新，計算如下:

(32)

即協(xié)方差矩陣更新又可以表示為

(33)

3 仿真實驗

本文設(shè)計限定記憶隨機加權(quán)擴展卡爾曼濾波器對車輛橫擺角速度、質(zhì)心側(cè)偏角和縱向速度進行濾波估計，建立Carsim與Matlab/Simulink聯(lián)合仿真平臺進行仿真實驗，并與傳統(tǒng)標準擴展卡爾曼仿真結(jié)果進行比較分析.

3.1 高附著路面仿真實驗

(a)前輪轉(zhuǎn)角輸入

從圖2(d)和(e)中可以看出，相比于傳統(tǒng)EKF算法，LMRWEKF算法的濾波估計值可以更好地接近參考值，而且在曲線波峰和波谷處偏離相對較小，并且對噪聲有明顯的抑制作用；從圖2(f)中可以看出，高附著工況下LMRWEKF算法的縱向速度估計值明顯比傳統(tǒng)EKF算法更加接近參考值.針對噪聲條件未知的情況，LMRWEKF算法對噪聲的抑制作用更強，具有更優(yōu)的穩(wěn)定性和魯棒性.

LMRWEKF可以很好地抑制噪聲波動，為進一步比較算法的估計精度，采用均方根指標來對估計精度進行評估對比，如表1所示.分析表1數(shù)據(jù)可以看出，高附著工況下，基于LMRWEKF算法估計得到的橫擺角速度、質(zhì)心側(cè)偏角和縱向速度的均方根誤差RMSEγ、RMSEβ、RMSEvx較EKF算法分別降低了60.23%、19.63%、91.57%.

表1 高附著工況下RMSE指標對比

3.2 低附著路面仿真實驗

從圖3(d)和(e)中可以看出，在曲線波峰和波谷處，相比于傳統(tǒng)EKF算法，LMRWEKF算法可以更好地貼近參考值，偏離相對較小，且對噪聲波動有較強的抑制作用；從圖3(f)中可以看出，低附著工況下LMRWEKF算法的縱向速度估計值明顯比傳統(tǒng)EKF算法更加接近參考值，并且在噪聲特性未知時，LMRWEKF算法抖動更小，說明對噪聲有很好的抑制作用，具有更強的穩(wěn)定性和魯棒性.

(a)前輪轉(zhuǎn)角輸入

LMRWEKF可以很好地對噪聲波動進行抑制，為進一步比較算法的估計精度，采用均方根指標來對估計精度進行評估對比，如表2所示.分析表2數(shù)據(jù)可以看出，低附著工況下，基于LMRWEKF算法估計得到的橫擺角速度、質(zhì)心側(cè)偏角和縱向速度的均方根誤差RMSEγ、RMSEβ、RMSEvx較EKF算法分別降低了59.38%、13.92%、94.20%.

表2 低附著工況下RMSE指標對比

4 結(jié)論

1)本文在線性二自由度車輛模型的基礎(chǔ)上引入一個縱向運動自由度，建立包括縱向、側(cè)向和橫擺3個自由度的非線性車輛動力學模型，將擴展卡爾曼濾波與限定記憶濾波相結(jié)合，在限定記憶長度區(qū)間內(nèi)，通過檢測新息殘差序列，利用數(shù)理統(tǒng)計的方法得到當前時刻測量噪聲矩陣的協(xié)方差矩陣，從而降低因模型不準導致的估計誤差；利用隨機加權(quán)理論，引入隨機加權(quán)系數(shù)，提高濾波系統(tǒng)對新量測值的利用程度，從而提高濾波算法的精度，增強了自適應性.

2)為了驗證LMRWEKF算法的有效性和精確性，設(shè)計高附著系數(shù)和低附著系數(shù)2種不同工況下的仿真實驗，仿真結(jié)果表明：以高附著工況下橫擺角速度為例，LMRWEKF算法的濾波估計值均方根誤差較傳統(tǒng)擴展卡爾曼估計值誤差降低60.23%，整體估計性能更優(yōu).

3)本文提出的LMRWEKF算法可以實時動態(tài)地調(diào)整濾波估計算法的噪聲統(tǒng)計特性，因此能夠應對噪聲特性未知的問題，其限定記憶的算法特性可以在計算能力較差的條件下仍然具有較快的響應速度和估計精度，實際應用場景廣泛.