基于自適應(yīng)模糊擴展卡爾曼濾波的車輛運動狀態(tài)聯(lián)合估計*

劉明春 彭志波 吳曉建

（1.南昌大學(xué)，南昌 330031；2.金龍聯(lián)合汽車工業(yè)（蘇州）有限公司，蘇州 215000）

主題詞：車輛狀態(tài)估計 擴展卡爾曼濾波 模糊控制 聯(lián)合估計 車輛動力學(xué)模型

1 前言

車輛駕駛過程中的狀態(tài)估計是底盤主動控制系統(tǒng)和車載故障診斷系統(tǒng)的關(guān)鍵技術(shù)之一。車輛的某些重要參數(shù)難以直接測量或測量所用的傳感器成本較高，如縱向速度、橫擺角速度和質(zhì)心側(cè)偏角等。對上述重要參數(shù)的獲取，可利用低成本傳感器得到易獲取的狀態(tài)信息，再通過估計算法獲得。

目前，車輛狀態(tài)信息估計的研究方法主要為卡爾曼濾波（Kalman Filter，KF）算法及其各種相關(guān)改進(jìn)算法，如擴展卡爾曼濾波（Extended Kalman Filter，EKF）和無跡卡爾曼濾波（Unscented Kalman Filter，UKF）等，此外還有模糊邏輯和神經(jīng)網(wǎng)絡(luò)等估計算法。文獻(xiàn)[4]、文獻(xiàn)[5]采用擴展卡爾曼濾波算法，以非線性3自由度車輛動力學(xué)模型為基礎(chǔ)，通過泰勒級數(shù)展開略去高階項達(dá)到線性化的效果進(jìn)行狀態(tài)參數(shù)的估計。文獻(xiàn)[6]、文獻(xiàn)[7]為增強控制系統(tǒng)的魯棒性和準(zhǔn)確度，將無跡變換（Unscented Transform）與標(biāo)準(zhǔn)卡爾曼濾波體系相結(jié)合，通過無跡變換的方式使非線性車輛模型適用于線性的標(biāo)準(zhǔn)卡爾曼濾波形式。文獻(xiàn)[8]、文獻(xiàn)[9]提出一種基于模糊邏輯與車輛運動學(xué)模型的濾波框架，開發(fā)了模糊決策模塊以實現(xiàn)更好的融合性能。文獻(xiàn)[10]～文獻(xiàn)[12]采用神經(jīng)網(wǎng)絡(luò)算法提出一種有監(jiān)督的機器學(xué)習(xí)方案，通過慣性測量單元（Inertial Measurement Unit，IMU）等低成本傳感器獲取易測量的參數(shù)，以此獲得所需的變量。上述車輛狀態(tài)信息估計方法并未充分考慮傳感器獲取測量參數(shù)時，測量參數(shù)受環(huán)境等因素干擾所產(chǎn)生的噪聲影響，在估算效果的穩(wěn)定性和精確性方面有待提高。另外，上述估計算法主要針對單一車輛狀態(tài)進(jìn)行估算，存在估計對象單一、難以同時估算多個狀態(tài)的問題。

針對上述問題，本文針對存在測量噪聲情況下的車輛橫、縱向運動狀態(tài)信息進(jìn)行聯(lián)合估計，提出一種基于自適應(yīng)模糊擴展卡爾曼濾波（Adaptive Fuzzy Extended Kalman Filter，AFEKF）的車輛狀態(tài)聯(lián)合估計算法，估算車輛縱向速度、橫擺角速度和質(zhì)心側(cè)偏角，并通過仿真和實車試驗對聯(lián)合估計算法的有效性進(jìn)行驗證。

2 車輛動力學(xué)建模

2.1 車輛模型

本文所建立的車輛動力學(xué)模型包括14 自由度車輛動力學(xué)模型和3 自由度車輛動力學(xué)模型。其中，14自由度車輛動力模型用于模擬真實車輛運動狀態(tài)，并得到估計算法所需的輸入狀態(tài)量和被估計狀態(tài)的比較狀態(tài)量。3 自由度車輛動力學(xué)模型通過微分方程變換、離散化后與擴展卡爾曼濾波算法結(jié)合，建立估計算法。

2.2 14自由度車輛動力學(xué)模型

為了較全面地反映車輛各方向的動力學(xué)狀態(tài)，所建立的14自由度車輛動力學(xué)模型包括車身的縱向、側(cè)向、垂向、側(cè)傾、俯仰和橫擺運動，4 個車輪的回轉(zhuǎn)運動和4個簧下質(zhì)量的垂向運動，如圖1所示。

圖1 14自由度車輛動力學(xué)模型示意

車身的6自由度運動微分方程如下：

縱向、側(cè)向和垂向動力學(xué)方程分別為：

式中，v、v、v分別為車輛質(zhì)心沿、、坐標(biāo)軸的縱向、側(cè)向和垂向運動速度；ω、ω、ω分別為側(cè)傾、俯仰和橫擺角；為重力加速度；為整車質(zhì)量；為車身質(zhì)量；為車輛在行駛過程中受到的阻力之和；為坡道阻力；為滾動阻力；為空氣阻力；為迎風(fēng)面積；為空氣密度；為坡道角度；為空氣阻力系數(shù)；為滾動阻力系數(shù)；F、F、F分別為各車輪受到的縱向力、側(cè)向力、垂向力；=1,2 分別表示車輛前、后軸；=l,r分別表示左、右側(cè)。

側(cè)傾、俯仰和橫擺動力學(xué)方程分別為：

式中，為輪距；M、M分別為懸掛質(zhì)量的離心力和重力引起的側(cè)傾力矩、俯仰力矩；M、M分別為第軸左、右側(cè)輪胎的回正力矩；I、I、I分別為車輛繞、、軸的轉(zhuǎn)動慣量；、分別為質(zhì)心到前、后軸的距離；F、F分別為第軸左、右側(cè)懸架的垂向力。

輪胎力在車輛坐標(biāo)系和輪胎坐標(biāo)系間的轉(zhuǎn)換關(guān)系為：

式中，δ為各車輪的轉(zhuǎn)向角；F、F分別為輪胎坐標(biāo)系上第軸側(cè)的縱向力和側(cè)向力。

懸架運動微分方程為：

式中，、分別為車輛靜態(tài)平衡條件下第軸側(cè)的懸架力、輪胎載荷；、分別為車輛第軸側(cè)動態(tài)懸架力、輪胎載荷；F、F分別為綜合靜態(tài)和動態(tài)條件下作用于懸架與輪胎上的力，得到的第軸側(cè)懸架系統(tǒng)的垂向力和車輪的靜態(tài)垂向載荷。

對懸架系統(tǒng)進(jìn)行受力分析，當(dāng)車輛處于側(cè)傾和俯仰運動時，懸架會產(chǎn)生一定程度的形變，導(dǎo)致載荷轉(zhuǎn)移，通過左、右側(cè)懸架和前、后軸變形量可以得到懸架動態(tài)載荷。

車輪垂向運動和旋轉(zhuǎn)運動的微分方程為：

式中，為側(cè)輪胎滾動半徑；為各輪心的垂向位移；為各輪的質(zhì)量；、Δ分別為車輪在回轉(zhuǎn)滾動過程中產(chǎn)生彈性遲滯現(xiàn)象形成的滾動阻力矩和偏向接地幾何中心一側(cè)的距離；、、分別為各車輪的轉(zhuǎn)動慣量、輸出力矩和旋轉(zhuǎn)角速度。

此外，14 自由度車輛動力學(xué)模型還包含了“PAC2002”魔術(shù)公式輪胎模型、駕駛員模型和電機模型等，使車輛模型各部件之間的耦合關(guān)系和動力學(xué)特性更為接近真實車輛，以適用于車輛主動安全控制的研究。

2.3 3自由度車輛動力學(xué)模型

為了考慮估計算法的準(zhǔn)確性和魯棒性需求，建立包含縱向、側(cè)向和橫擺運動的非線性3自由度車輛模型如圖2所示，以此作為車輛狀態(tài)信息的估計模型。

圖2 3自由度車輛動力學(xué)模型示意

根據(jù)牛頓第二定律，得到車輛動力學(xué)平衡方程為：

式中，a、a分別為縱向、橫向加速度；、分別為質(zhì)心到前、后軸的距離；為前輪轉(zhuǎn)角；為橫擺角速度；分別為前、后輪輪距。

由上述動力學(xué)平衡方程計算可得3 自由度模型的狀態(tài)方程和測量方程為：

式中，為質(zhì)心側(cè)偏角；為轉(zhuǎn)向盤轉(zhuǎn)角；、分別為前、后輪胎的側(cè)偏剛度；為轉(zhuǎn)向盤與前輪的角轉(zhuǎn)向傳動比。

設(shè)二維控制輸入向量為=[ a]，三維狀態(tài)向量為=[γ β v]，一維量測輸出向量為=[a]。

3 基于AFEKF估計算法的車輛狀態(tài)聯(lián)合估計

3.1 AFEKF算法總體方案設(shè)計

本文設(shè)計的車輛橫、縱向狀態(tài)聯(lián)合估計算法方案如圖3所示。以14自由度車輛動力學(xué)模型的輸出狀態(tài)參數(shù)作為車輛的真實運動狀態(tài)，、a和a分別為14自由度動力學(xué)模型輸出的前輪轉(zhuǎn)角、縱向加速度和橫向加速度，經(jīng)過高斯噪聲、的疊加后，模擬得到帶有噪聲的測量值。

圖3 行駛狀態(tài)聯(lián)合估計算法總體方案

AFEKF 聯(lián)合估計算法包含1 個擴展卡爾曼濾波器和1個模糊控制器，擴展卡爾曼濾波器通過輸入的前輪轉(zhuǎn)角、縱向加速度和橫向加速度來估算質(zhì)心側(cè)偏角、橫擺角速度和縱向速度，模糊控制器可以在估計過程中對觀測噪聲協(xié)方差矩陣進(jìn)行實時調(diào)整，使得擴展卡爾曼濾波算法具有自適應(yīng)性，提高算法精度。

3.2 擴展卡爾曼濾波離散化

由于3自由度車輛模型具有非線性的特點，EKF算法的核心是通過線性化方法將非線性系統(tǒng)轉(zhuǎn)換為近似的線性系統(tǒng)，圍繞濾波值將非線性函數(shù)略去二階及以上項的泰勒級數(shù)展開，得到一個近似線性化的模型，并通過3自由度車輛模型的動力學(xué)方程式（18）～式（20）建立狀態(tài)空間方程：

式中，()為三維狀態(tài)向量；()為二維控制輸入向量；()為一維量測輸出向量；()、()分別為相互獨立且均值為零的系統(tǒng)噪聲和測量噪聲，二者的協(xié)方差矩陣分別為、。

函數(shù)和均為非線性函數(shù)，為了得到線性化模型，分別對()求偏導(dǎo)得到雅克比矩陣()和()：

時刻狀態(tài)轉(zhuǎn)移矩陣為：

式中，Δ為采樣周期；為單位矩陣。

根據(jù)上述公式，采用歐拉法將3自由度車輛的狀態(tài)空間方程進(jìn)行離散化：

3.3 自適應(yīng)模糊擴展卡爾曼濾波算法

車輛在實際行駛過程中環(huán)境復(fù)雜多變，傳感器測量的精度水平會受不同工況等因素的影響，從而使得測量噪聲協(xié)方差矩陣相應(yīng)地變化，本文通過模糊控制對其進(jìn)行動態(tài)調(diào)整，提高算法的適應(yīng)性。設(shè)計離散對象的系統(tǒng)狀態(tài)方程和測量方程分別為：

式中，X、Y分別為時刻系統(tǒng)的狀態(tài)變量和輸出變量；F為狀態(tài)矩陣，為式（22）中的雅克比矩陣()；B為控制矩陣；H為測量矩陣，為式（23）中的雅克比矩陣()；對應(yīng)第個采樣時刻。

擴展卡爾曼濾波算法可分為時間更新和測量更新2個部分：

時間更新是通過時刻的X預(yù)測(+1)時刻的X：

根據(jù)式（27）可知，預(yù)測值存在系統(tǒng)噪聲，因此需求出時刻的預(yù)測誤差協(xié)方差矩陣：

測量更新需要通過增益矩陣對X進(jìn)行修正，以得到(+1)時刻的估計值X：

然后更新(+1)時刻的協(xié)方差矩陣來表示估計誤差，用于開展下一階段的估算：

本文將模糊控制與EKF 算法相結(jié)合，以達(dá)到動態(tài)調(diào)整測量噪聲協(xié)方差矩陣的效果。設(shè)(+1)時刻的測量噪聲協(xié)方差矩陣為(+1)，且有(+1)=(1+Δ)()，其中Δ為的調(diào)節(jié)量。

定義測量狀態(tài)Y的實際協(xié)方差()和理論協(xié)方差()分別為：

將理論協(xié)方差與實際協(xié)方差間的差值作為模糊控制器的輸入，輸出為：

結(jié)合相關(guān)理論和大量仿真分析，實時調(diào)整模糊控制器的濾波參數(shù)和的值，積累和參考足夠的專家經(jīng)驗，制定模糊規(guī)則集，less N 表示小于0，equal AZ 表示約等于0，more P 表示大于0，解模糊化的方法使用重心法，二維曲線如圖4 所示，模糊規(guī)則集為：

圖4 輸入-輸出關(guān)系

模糊控制的隸屬度函數(shù)如圖5所示。

圖5 模糊控制的隸屬度函數(shù)

4 算法的仿真驗證和實車驗證

4.1 基于MATLAB/Simulink的仿真驗證

為了驗證所提出算法的有效性，在MATLAB/Simu?link 環(huán)境中進(jìn)行仿真驗證，采用雙移線工況，將自適應(yīng)模糊擴展卡爾曼濾波的效果分別與擴展卡爾曼濾波和14自由度車輛動力學(xué)模型輸出的參考狀態(tài)進(jìn)行對比分析，車輛的主要參數(shù)如表1所示。

表1 仿真車輛主要參數(shù)

在MATLAB/Simulink 中設(shè)置路面附著系數(shù)為0.8，車速為100 km/h，采樣周期為0.01 s，誤差協(xié)方差矩陣初值為=0.1，系統(tǒng)噪聲協(xié)方差矩陣=0.1，觀測噪聲協(xié)方差矩陣初值=[50 000]。

雙移線工況的行駛軌跡如圖6所示，對應(yīng)的轉(zhuǎn)向盤轉(zhuǎn)角經(jīng)角轉(zhuǎn)向傳動比轉(zhuǎn)換后，得到轉(zhuǎn)向輪轉(zhuǎn)角如圖7所示，圖8、圖9所示分別為縱向加速度和橫向加速度的14自由度參考值和加上高斯噪聲的模擬值。將轉(zhuǎn)向盤轉(zhuǎn)角、帶噪聲的橫向加速度和縱向加速度狀態(tài)作為估算器的輸入，經(jīng)過算法估計后得到如圖10～圖12所示的估計結(jié)果。圖10、圖11 的AFEKF 的質(zhì)心側(cè)偏角、橫擺角速度和縱向速度的估計曲線比EKF的更接近14自由度車輛動力學(xué)模型的參考狀態(tài)，尤其是曲線的拐角處較為顯著。造成此現(xiàn)象的原因可能是，曲線拐角處車輛狀態(tài)變動更加頻繁和卡爾曼濾波估計算法估計誤差在一階離散化的過程中隨時間的增加累積增大，而AFEKF 對協(xié)方差矩陣的調(diào)整、減小累積誤差和測量噪聲等因素的影響，使估計結(jié)果更準(zhǔn)確。

圖6 行駛軌跡隨時間變化

圖7 前輪轉(zhuǎn)角隨時間變化

圖8 縱向加速度隨時間變化

圖9 橫向加速度隨時間變化

圖10 質(zhì)心側(cè)偏角估計

圖11 橫擺角速度估計

圖12 縱向速度估計

相較于EKF 估計值更為接近14 自由度車輛動力學(xué)模型的參考值，估計效果較好，尤其在橫擺角速度峰值處AFEKF 的估計精度明顯提高，因此可知AFEKF 對測量噪聲矩陣進(jìn)行了自適應(yīng)調(diào)整，使得質(zhì)心側(cè)偏角、橫擺角速度和縱向速度的估計效果要優(yōu)于EKF。

4.2 實車試驗驗證

基于某量產(chǎn)車型進(jìn)行實車試驗。試驗車安裝有CANoe、Kistler 和MicroAutoBox 等相關(guān)設(shè)備和傳感器來采集車輛關(guān)鍵參數(shù)，包括轉(zhuǎn)向盤轉(zhuǎn)角、縱向加速度a、橫向加速度a、質(zhì)心側(cè)偏角、橫擺角速度和縱向車速v，試驗現(xiàn)場如圖13、圖14所示，控制車速為40 km/h，采樣周期為0.01 s，標(biāo)樁間距=50 m。

圖13 GB/T 6323—2014蛇行繞樁工況

圖14 實車試驗場地

實車試驗方案如圖15 所示，利用Kistler 車載傳感采集測試車輛的運動狀態(tài)信息，AFEKF 估計算法運行于MicroAutoBox控制器，在計算機中將實車采集的數(shù)據(jù)結(jié)果與AFEKF 聯(lián)合估計算法進(jìn)行比較，驗證算法的有效性。

圖15 實車試驗估計流程

圖16～圖18所示分別為轉(zhuǎn)向盤轉(zhuǎn)角、縱向加速度和橫向加速度狀態(tài)量隨時間的變化情況；圖19～圖21所示分別為質(zhì)心側(cè)偏角、橫擺角速度和縱向速度狀態(tài)量的實車試驗值與AFEKF算法估計值的對比結(jié)果。

圖16 轉(zhuǎn)向盤轉(zhuǎn)角隨時間變化

圖17 縱向加速度隨時間變化

圖18 橫向加速度隨時間變化

圖19 質(zhì)心側(cè)偏角估計對比分析

圖20 橫擺角速度估計對比分析

圖21 縱向速度估計對比分析

由圖19～圖21可知：傳感器采集的車輛真實狀態(tài)值與估計值存在一定的偏差，尤其是起步階段偏差相對較大，這是因為車輛需要一定的加速起動時間才能達(dá)到設(shè)定車速和試驗路段，此時縱向加速度和橫向加速度不穩(wěn)定以及噪聲較大所致，質(zhì)心側(cè)偏角真實值受Kistler等車載傳感器的安裝位置和測量誤差等因素的影響，也是造成其真實值與AFEKF 估計值曲線存在差距的重要因素；隨后估計值與車輛真實值較為接近，逼近度較好，說明AFEKF算法得到的估計結(jié)果具有較好的準(zhǔn)確性。

5 結(jié)束語

本文針對車輛行駛過程中的橫、縱向動力學(xué)狀態(tài)聯(lián)合估計問題展開研究，基于自適應(yīng)模糊擴展卡爾曼濾波（AFEKF）理論建立了聯(lián)合估計算法。首先建立了14自由度車輛動力學(xué)整車模型模擬真實車輛行駛狀態(tài)；然后根據(jù)3 自由度車輛動力學(xué)模型的微分方程建立基于擴展卡爾曼濾波（EKF）的估計算法模型，將所測量的轉(zhuǎn)向盤轉(zhuǎn)角和帶噪聲的縱向加速度、橫向加速度狀態(tài)信息作為算法輸入，實現(xiàn)對質(zhì)心側(cè)偏角、橫擺角速度和縱向速度的聯(lián)合估計，并在EKF 算法中應(yīng)用模糊控制器來自適應(yīng)調(diào)整測量噪聲，降低測量噪聲對估計結(jié)果的影響，使估計精度更高。仿真和實車測試結(jié)果表明，在存在測量噪聲的情況下，針對車輛橫、縱向運動狀態(tài)的聯(lián)合估計，自適應(yīng)模糊擴展卡爾曼濾波（AFEKF）估計算法具有較好的準(zhǔn)確性，可以準(zhǔn)確地估算車輛的縱向車速、橫擺角速度和質(zhì)心側(cè)偏角。