基于擴(kuò)展卡爾曼濾波的縱向車速與坡度估計

中圖分類號：U463.6 文獻(xiàn)標(biāo)識碼：A 文章編號：1003-8639（2025）07-0086-06

Estimation of Longitudinal Velocity and Road Slope Based on Extended Kalman Filter

Jin Changming，Hu Zhanzhan，Wang Zhenlong（Geely Automobile Research Institute（Ningbo）Co.，Ltd.， Ningbo 315336，China）

【Abstract】As errors in vehicle speed and slope estimation directly affect the performance of controllrs such as vehicle slipratecontrolandyawratecontrol，resultinginadeclineinvehiclestability.Thispaperemploystheextended Kalmanfilter algorithm.Basedonthereal-timeevaluationresultsof vehicle speed confidence，the QRnoise covariance matrixoftheextendedKalman filterisadjustedinrealtime toachievethedynamicadjustmentof theextendedKalman filtereffct.Additionall，thelongitudinalacelerationvalueof theinertialsensoriscompensatedtoreducetheinfluence of vehiclepitchon slopecalculation.Finaly，themethod isverifiedbyjointsimulationwith MATLAB/Simulinkand Carsim.The results show that the estimated longitudinal vehicle speedand slopeare basicall consistent with the actual values，demonstrating the effectivenessof the longitudinal vehicle speedand slope joint estimation method.

【Key words】longitudinal vehicle speed；road slope；confidence factor；extended Kalman filter；Carsim/Simulink co-simulation

0 引言

車速與坡度是車輛橫擺控制、驅(qū)動防滑等車輛穩(wěn)定性控制器的重要輸入?yún)?shù)，通常運用于控制目標(biāo)的計算2，車速與坡度的準(zhǔn)確估算直接影響其控制的性能，甚至導(dǎo)致誤激活。

縱向車速的估算分為運動學(xué)方法和動力學(xué)方法?；趧恿W(xué)的方法所需要的傳感器信號少，但需要充分了解輪胎和路面信息，受環(huán)境變化影響較大[3]。一般對于存在非驅(qū)動輪的車輛的車速估算，會通過非驅(qū)動輪經(jīng)過轉(zhuǎn)換得到車輛質(zhì)心處的縱向速度作為車速，對于四輪驅(qū)動的車輛無法直接使用。第二種是加速度積分法，由于慣性傳感器的縱向加速度包含縱向坡度產(chǎn)生的加速度，因此這種方法依賴于縱向坡度的計算準(zhǔn)確性，且長時間計算會由于傳感器誤差的累計導(dǎo)致發(fā)散。因此采用擴(kuò)展卡爾曼濾波算法對兩種車速計算結(jié)果進(jìn)行濾波融合，從而得到更加準(zhǔn)確、魯棒性更高的縱向車速。

1系統(tǒng)架構(gòu)設(shè)計

本文采用的縱向車速和道路坡度估算方法主要流程框架如圖1所示。該系統(tǒng)分為參考車速計算和車速坡度聯(lián)合計算兩部分。參考車速計算模塊輸出參考車速及其置信度因子作為后續(xù)縱向車速與坡度擴(kuò)展卡爾曼濾波器的輸入，其中參考車速作為擴(kuò)展卡爾曼濾波器的觀測值，置信度因子用于計算測量噪聲協(xié)方差矩陣。具體計算步驟如下。

首先，根據(jù)橫擺角速度、摩擦系數(shù)、前輪轉(zhuǎn)角、車輪線速度、輪速變化率、車輪縱向力計算輪速在車輛質(zhì)心處的等效縱向速度和置信度因子，然后，以置信度因子為權(quán)重計算參考車速并得到置信度因子。在縱向車速與坡度估算模塊中，根據(jù)參考車速置信度動態(tài)調(diào)整擴(kuò)展卡爾曼濾波的協(xié)方差矩陣，修正由于車輪打滑導(dǎo)致參考車速的誤差，提高縱向車速的準(zhǔn)確度。

2參考車速估計

2.1參考車速估計置信度因子

在基于運動學(xué)的坡度估算過程中，作為觀測量的參考車速和慣性傳感器獲得的縱向加速度信號的質(zhì)量會對坡度估算結(jié)果產(chǎn)生很大的影響，因此需要對參考車速的估計結(jié)果進(jìn)行置信度評價，采用模糊規(guī)則從車輪附著系數(shù)、車輪線速度變化率、車輪垂向力以及驅(qū)動與制動時的輪速特征獲得各指標(biāo)的置信度，其中附著系數(shù)為車輪縱向力與垂向力的比值，如式（1）：

當(dāng)車輛處于驅(qū)動狀態(tài)時，車輪存在打滑的趨勢，因此輪速較小的車輪與真實車輛縱向速度更加接近，當(dāng)車輛處于制動狀態(tài)時，車輪存在鎖止的趨勢，因此輪速較大的車輪與真實車輛縱向車速更加接近，當(dāng)車輛驅(qū)動時輪速差值的計算方法如下：

v_diff=v_x-min（v_xFL，v_xFR，v_xRL，v_xRR）

式中：Ux —車輪線速度轉(zhuǎn)換至車輛質(zhì)心處的速度。

當(dāng)車輛制動時，輪速差值的計算方法如下：

輸入附著系數(shù)、車輪線速度變化率、車輪垂向力以及輪速差值，通過以下隸屬度函數(shù)計算對應(yīng)的置信度 Q₁ 、 Q₂ 、 Q₃ 、 Q₄

式中：a、b——根據(jù)不同指標(biāo)進(jìn)行給定的常數(shù)。綜合各指標(biāo)計算各車輪線速度置信度，即：

在基于輪速的車速估算中，縱向車速估算值質(zhì)量的上限由最優(yōu)車輪線速度質(zhì)量決定，因此在得到

各車輪的置信度因子后，取各置信度的最大值為縱向車速估算值的置信度：

Q_veh=max（Q_FL，Q_FR，Q_?RL，Q_?RR）

2.2 基于置信度的參考車速估計

根據(jù)車輪與車輛質(zhì)心處的位置關(guān)系，得到車輪線速度與車輛縱向速度的關(guān)系。前輪轉(zhuǎn)換關(guān)系見式（7），后輪轉(zhuǎn)換關(guān)系見式（8）：

最后，各個車輪線速度的置信度因子 Q 為權(quán)重，加權(quán)平均得到參考車速。

3 縱向車速與坡度估計

3.1縱向加速度傳感器補(bǔ)償

由于一般慣性傳感器安裝于車輛上車體，車身的俯仰、側(cè)傾姿態(tài)會影響到傳感器的數(shù)值4，因此基于慣性傳感器和運動學(xué)模型計算的道路縱向坡度中包含了車身的俯仰角。

受到車身姿態(tài)影響的傳感器數(shù)值為：

a_x=a_x-snsrcosθ_y-veh-gsin（θ_y+θ_y-veh）

式中： a_x.snsr -車輛縱向加速度傳感器值；θy_veh車身俯仰角。一般車身俯仰角很小， cosθ_y-veh≈0 ，即：

a_x=a_x-snsr-gsin（θ_y+θ_y-veh）

基于此式計算的道路坡度 θ_y-calc 為：

θ_y-calc=θ_y+θ_y-veh

根據(jù)車輛實際縱向加速度值與車身俯仰角之間的關(guān)系：

式中： m —車輛質(zhì)量； h ——質(zhì)心離地高度;L —前后軸距離； K_i —前懸架剛度； K_r —后懸架剛度。

將式（13）代入式（12）中，得：

3.2基于垂向慣性傳感器的運動學(xué)估計模型

首先，根據(jù)車輛垂向速度的運動學(xué)微分方程：

式中： v_z（k） 、 v_z（k-1） —車輛第 k 、 k-1 時刻的垂向速度； T_s —采樣周期。

當(dāng)?shù)缆反嬖谄露葧r，垂向慣性傳感器數(shù)值：

聯(lián)立（15）、（16）兩式，得：

v_z（k）=v_z（k-1）+（a_zsnsr-gcos（θ_y）-ω_xv_y）Ts

式中： a_?z-snsr. 車輛垂向傳感器加速度； ω_s 1車輛側(cè)傾角速度； v_y 1 側(cè)向速度。

當(dāng)車輛行駛道路坡度發(fā)生變化時，可以將車輛視為在 xoz 平面內(nèi)繞 o 點作圓周運動，如圖2所示。

在單位時間內(nèi)，由車輛縱向運動得到弧長 L_a

另一方面，根據(jù)車輛繞 o 點轉(zhuǎn)動過的角度等于坡度變化，得：

由以上兩式，得：

根據(jù)向心加速度公式，得：

即：

式中： dθ_y/dt —車輛俯仰角的微分； r —繞o 點轉(zhuǎn)動的半徑。

代入式（20），得：

即：

代人公式 中，得到坡度的狀態(tài)轉(zhuǎn)移方程：

根據(jù)縱向車速運動學(xué)微分方程：

慣性傳感器縱向加速度 a_x-snsr 和車輛縱向速度變化率 dv_x/dt 的關(guān)系為：

將式（27）代入式（26）中，得到縱向車速的狀態(tài)轉(zhuǎn)移方程：

由于 θ_y 和 v_x 之間為非線性關(guān)系，以上一時刻為展開點對其進(jìn)行一階泰勒展開線性化，即將式（25）和式（28）線性化，得到擴(kuò)展卡爾曼濾波狀態(tài)轉(zhuǎn)移矩陣A（雅可比矩陣）：

得到 k 時刻的狀態(tài)空間方程：

式中： x_k=[θ_yv_x]^T;z_k=v_x;B=0;H=[01]^T w_k 一過程噪聲； v_k ——測量噪聲。均服從標(biāo)準(zhǔn)正態(tài)分布，協(xié)方差矩陣分別為 Q 和 R 。

如圖3所示，卡爾曼濾波更新過程分為時間更新和測量更新5。時間更新過程是通過上一時刻的值獲得此時刻的先驗值，測量更新過程是通過測量誤差對先驗值進(jìn)行校正，最終得到本時刻的估計值。

時間更新過程：

式中： ——k時刻的先驗估計值； 1時刻的后驗估計值； P_k^- —時刻的先驗誤差協(xié)方差； P_k-1. —k時刻的后驗估計協(xié)方差。

測量更新過程：

式中： K_k ?k 時刻的卡爾曼增益； 時刻的后驗估計值； P_k ——k時刻的后驗估計協(xié)方差。

4仿真及試驗驗證

對所提出的縱向速度與坡度估算方法進(jìn)行Carsim仿真和試驗驗證。在實車驗證中，本文采用某國產(chǎn)純電車型。

對于車速和坡度估算結(jié)果的評價指標(biāo)主要有三個方面：最大絕對誤差、均方誤差（Mean Squared Error，MSE）和平均絕對誤差（MeanAbsoluteError，MAE）。由于估算過程不可避免地存在一定的誤差且車速與坡度存在零點附近的計算值，因此采用絕對誤差而非相對誤差來評價估算結(jié)果是否存在誤差超過合理范圍的異常值，計算方法是對絕對誤差求絕對值后再取最大值，即：

δ_max=max（|x_est-x_real|）

式中： δ_max. 最大絕對誤差； x_est 估算值；x_real 真實值。

均方誤差用于評價模型的擬合程度，由于均方誤差對局部較大的誤差比較敏感，因此可以很好地評估模型對較大誤差的控制程度，即：

式中： x_estⁱ 估算值； x_realⁱ 真實值； N? 樣本數(shù)量。

平均絕對誤差通過計算估算值和真實值之間誤差的絕對值的平均值得到。平均絕對誤差體現(xiàn)了估算值和真實值整體的差異程度，受到局部的異常值影響小，因此用于評價模型整體的擬合優(yōu)度，即：

4.1 仿真驗證

4.1.1 仿真驗證1

仿真驗證1工況為道路左右側(cè)附著系數(shù)分別為0.15和1的對開路面，道路坡度為 15% ，從水平路面靜止加速上坡。主要用于驗證車輪輪速置信度的計算方法能否有效地區(qū)分不同路面與不同驅(qū)動狀態(tài)的車輪輪速。

由圖4a可知，低附側(cè)的驅(qū)動輪發(fā)生了打滑，左后輪輪速高于車速和其他車輪輪速，高附側(cè)的驅(qū)動輪存在較小的滑移，前軸的兩個從動輪輪速基本與車速一致。由圖4b可知，兩從動輪的置信度始終在0.7左右，而兩后驅(qū)動輪的置信度分別為0與0.01，各車輪輪速置信度計算結(jié)果與實際車速的一致性較高，說明了該置信度因子計算方式的準(zhǔn)確性。

由圖5可知，車速和坡度估算均與真實值貼合較好。計算車速估算的最大絕對誤差、均方誤差和平均絕對誤差見表1。

由表1可知，擴(kuò)展卡爾曼濾波輸出的縱向車速在異常值的控制與整體擬合度上均有很好的性能，估算結(jié)果準(zhǔn)確。

4.1.2 仿真驗證2

仿真驗證2主要為驗證基于加速度計算俯仰角對坡度進(jìn)行補(bǔ)償?shù)挠行?。仿真工況道路附著系數(shù)為1的高附路面，車輛在平路上起步上坡后制動靜止，然后再全油門起步加速。

圖6a中的曲線為擴(kuò)展卡爾曼濾波后的縱向車速和從Carsim得到的真實縱向車速。

計算縱向車速估算結(jié)果的最大絕對誤差、均方誤差和平均絕對誤差，見表2。

由表2可知，在高附路面上縱向車速估算結(jié)果整體擬合度高且不存在局部異常值。圖6b中的曲線分別為未進(jìn)行補(bǔ)償縱向坡度、車輛俯仰姿態(tài)補(bǔ)償?shù)目v向坡度和Carsim得到的真實坡度。計算最大絕對誤差、均方誤差和平均絕對誤差，見表3。

由圖6b和表3可知，由于車輛的慣性傳感器安裝于上車體，因此在全油門加速階段，誤差隨著車身俯仰變化而變化，經(jīng)過補(bǔ)償后坡度估算結(jié)果在整體擬合度和急加減速時的誤差控制上均有更高的精度。

4.2 實車試驗驗證

4.2.1 試驗驗證1

當(dāng)前對于道路條件良好的非極限工況中，車輛的行駛速度能夠基于輪速非常容易地得到，因此本文主要采用冰雪路面ESC激活的極限工況對縱向車速估算算法進(jìn)行驗證。

試驗驗證1的測試道路狀況為冰雪對開路面，車輛采用直線靜正起步加減速再減速的方式。四個車輪輪速如圖7所示。

在加速起步階段，驅(qū)動輪左后輪在2＼～11s之間頻繁觸發(fā)驅(qū)動防滑，驅(qū)動輪右后輪在5＼～11s之間，間歇發(fā)生打滑，在制動11.5＼～17s階段全程ABS激活。EKF濾波后縱向車速結(jié)果與各車輪置信度計算如圖8所示。

由圖8可知，EKF濾波后的車速基本與車輪所反映的車速趨勢一致。

由圖9可知，在加速階段，驅(qū)動輪存在不同程度打滑，輪速的置信度很小。在11s后的制動階段中，整體速度較高的右后輪的置信度最高，說明該車輪輪速評價方法能夠很好地識別與實際車速接近的車輪。

由圖10可知，通過車輪輪速得到的參考車速在四個車輪均發(fā)生打滑時不可避免地產(chǎn)生誤差。盡管沒有與真實車速進(jìn)行比較，也可以看出通過EKF濾波后，能夠極大地抑制四輪輪速均不可信時的誤差，提高了縱向車速的估算精度。

4.2.2 試驗驗證2

試驗驗證2主要為驗證車輛姿態(tài)補(bǔ)償對車身具有較大的俯仰時的誤差控制作用。試驗工況為先靜止于基本水平的路面上，起步上坡度為 10% 的坡道，然后在坡中制動并停于半坡，再次啟動后到達(dá)坡頂。

由圖11可知，未經(jīng)過車輛姿態(tài)補(bǔ)償?shù)钠露仍?6.5＼～18.5s間，由于上坡制動時車身俯仰姿態(tài)與坡度方向相反，因此估算的坡度結(jié)果會偏小，最大瞬時誤差在17.8s為 9% ，經(jīng)過補(bǔ)償后的縱向坡度最大瞬時誤差在18.24s為 5.56% 。

采用車輛在14＼～25s坡上段的估算結(jié)果來計算均方誤差（MSE）和平均絕對誤差（MAE）。

根據(jù)表4，經(jīng)過車身姿態(tài)補(bǔ)償后的縱向坡度在均方誤差、平均絕對誤差和瞬時最大誤差上均小于未補(bǔ)償?shù)目v向坡度，證明了基于加速度的車身俯仰姿態(tài)補(bǔ)償策略的有效性。

5結(jié)論

本文提出了一種車輪輪速置信度評估方法及基于擴(kuò)展卡爾曼濾波的縱向車速和坡度估算方法。通過Carsim仿真和實車試驗驗證得到以下結(jié)論。

1）本文設(shè)計的車輪輪速置信度因子計算方法能夠準(zhǔn)確識別與真實車速接近的車輪，以及通過各車輪置信度和輪速計算得到的參考車速在非四輪失效的情況下能夠得到準(zhǔn)確的縱向車速。

2）采用的基于擴(kuò)展卡爾曼濾波的車速和坡度估算方法在參考車速失效時，能夠在參考車速置信度減小時及時調(diào)整觀測協(xié)方差矩陣，增大預(yù)測車速在擴(kuò)展卡爾曼濾波輸出中的比重，在四車輪輪速均異常時能夠獲得更加精準(zhǔn)穩(wěn)定的縱向車速。

3）本文所采用的基于前后軸垂向剛度和加速度信息進(jìn)行的坡度估算結(jié)果補(bǔ)償方法，在急加減速有明顯車身自身俯仰的工況中具有一定的校正效果，能夠明顯減小平均誤差與瞬時最大誤差。

參考文獻(xiàn)

[1]ZhangM，F(xiàn)an X，GanJ.Research on speed estimation of"hub-motor vehicle based on multi-sensor information fusion[J].International Journal ofVehicle Systems ModellingandTesting，2021，15（1）：1-20.

[2]陳鵬輝，茹強(qiáng)，汶渭棟.基于卡爾曼濾波車速估算的電動汽車換擋策略[J].汽車電器，2024（8）：15-17，20.

[3]王詩超，齊志權(quán)，陳思忠.基于車輛運動學(xué)的縱向車速估計方法研究[C]//2019中國汽車工程學(xué)會年會論文集（4），2019：605-609.

[4]馮繼豪，秦大同，劉永剛，等.基于雙卡爾曼濾波及概率最近鄰數(shù)據(jù)關(guān)聯(lián)的道路坡度實時估計[J].機(jī)械工程學(xué)報，2022，58（16）：258-269.

[5]趙健，李至軒，朱冰，等.基于交互多模型的車輛質(zhì)量與道路坡度估計[J].中國公路學(xué)報，2019，32（12）：58-65.

（編輯楊凱麟）