胡敬宇 汪 嚴(yán)永俊 耿可可 柏 碩 殷國棟

(東南大學(xué)機(jī)械工程學(xué)院, 南京 211189)

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

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

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

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

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

在較為準(zhǔn)確地描述車輛動力學(xué)的基礎(chǔ)上盡量減少控制算法的計(jì)算量.通常情況下，在考慮車輛平面運(yùn)動時忽略懸架系統(tǒng)的影響，重點(diǎn)考慮純側(cè)偏輪胎特性，忽略縱橫向輪胎力的耦合關(guān)系、輪胎的橫向載荷轉(zhuǎn)移以及車輛的俯仰角和側(cè)傾角，在線性二自由度車輛模型的基礎(chǔ)上，引入一個縱向運(yùn)動自由度構(gòu)建車輛非線性三自由度模型[19].動力學(xué)模型示意圖如圖1所示，估算模型質(zhì)心與車輛坐標(biāo)系原點(diǎn)重合，X軸正向表示車輛行駛方向，Y軸正向表示車輛行駛方向的左方，橫擺方向正方向?yàn)槔@坐標(biāo)軸原點(diǎn)逆時針方向，則在上述前提條件下，建立非線性三自由度車輛動力學(xué)模型.圖中，vx和vy分別為車體坐標(biāo)系下質(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)估計(jì)算法及其實(shí)現(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)分別為過程噪聲和觀測噪聲，且兩者序列彼此獨(dú)立.Q和R分別為噪聲w(t)和v(t)的統(tǒng)計(jì)特性，即過程噪聲和觀測噪聲的協(xié)方差矩陣.

根據(jù)式(1)～(4)，非線性三自由度車輛動力學(xué)方程中，狀態(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，相應(yīng)的雅可比矩陣的求解過程如下所示：

(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è)計(jì)

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

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

(17)

(18)

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

(19)

2.3.2 限定記憶濾波

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

2.3.3LMRWEKF算法

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

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

(20)

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

其處理的基本流程為

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

(21)

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

(22)

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

(23)

⑤ 觀測方程預(yù)測，即

(24)

⑥ 得到觀測誤差，為

(25)

⑦ 判斷，

若k≤N

(26)

若k>N

(27)

(28)

⑧ 計(jì)算濾波增益，計(jì)算式為

(29)

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

(30)

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

(31)

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

(32)

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

(33)

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

本文設(shè)計(jì)限定記憶隨機(jī)加權(quán)擴(kuò)展卡爾曼濾波器對車輛橫擺角速度、質(zhì)心側(cè)偏角和縱向速度進(jìn)行濾波估計(jì)，建立Carsim與Matlab/Simulink聯(lián)合仿真平臺進(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ì)值可以更好地接近參考值，而且在曲線波峰和波谷處偏離相對較小，并且對噪聲有明顯的抑制作用；從圖2(f)中可以看出，高附著工況下LMRWEKF算法的縱向速度估計(jì)值明顯比傳統(tǒng)EKF算法更加接近參考值.針對噪聲條件未知的情況，LMRWEKF算法對噪聲的抑制作用更強(qiáng)，具有更優(yōu)的穩(wěn)定性和魯棒性.

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

表1 高附著工況下RMSE指標(biāo)對比

3.2 低附著路面仿真實(shí)驗(yàn)

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

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

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

表2 低附著工況下RMSE指標(biāo)對比

4 結(jié)論

1)本文在線性二自由度車輛模型的基礎(chǔ)上引入一個縱向運(yùn)動自由度，建立包括縱向、側(cè)向和橫擺3個自由度的非線性車輛動力學(xué)模型，將擴(kuò)展卡爾曼濾波與限定記憶濾波相結(jié)合，在限定記憶長度區(qū)間內(nèi)，通過檢測新息殘差序列，利用數(shù)理統(tǒng)計(jì)的方法得到當(dāng)前時刻測量噪聲矩陣的協(xié)方差矩陣，從而降低因模型不準(zhǔn)導(dǎo)致的估計(jì)誤差；利用隨機(jī)加權(quán)理論，引入隨機(jī)加權(quán)系數(shù)，提高濾波系統(tǒng)對新量測值的利用程度，從而提高濾波算法的精度，增強(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í)時動態(tài)地調(diào)整濾波估計(jì)算法的噪聲統(tǒng)計(jì)特性，因此能夠應(yīng)對噪聲特性未知的問題，其限定記憶的算法特性可以在計(jì)算能力較差的條件下仍然具有較快的響應(yīng)速度和估計(jì)精度，實(shí)際應(yīng)用場景廣泛.