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

中圖分類號(hào)：U463.6 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：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ū)動(dòng)防滑等車輛穩(wěn)定性控制器的重要輸入?yún)?shù)，通常運(yùn)用于控制目標(biāo)的計(jì)算2，車速與坡度的準(zhǔn)確估算直接影響其控制的性能，甚至導(dǎo)致誤激活。

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

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

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

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

2參考車速估計(jì)

2.1參考車速估計(jì)置信度因子

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

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

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

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

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

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

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

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

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

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

2.2 基于置信度的參考車速估計(jì)

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

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

3 縱向車速與坡度估計(jì)

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

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

受到車身姿態(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）

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

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

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

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

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

3.2基于垂向慣性傳感器的運(yùn)動(dòng)學(xué)估計(jì)模型

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

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

當(dāng)?shù)缆反嬖谄露葧r(shí)，垂向慣性傳感器數(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ā)生變化時(shí)，可以將車輛視為在 xoz 平面內(nèi)繞 o 點(diǎn)作圓周運(yùn)動(dòng)，如圖2所示。

在單位時(shí)間內(nèi)，由車輛縱向運(yùn)動(dòng)得到弧長(zhǎng) L_a

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

由以上兩式，得：

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

即：

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

代入式（20），得：

即：

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

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

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

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

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

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

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

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

時(shí)間更新過(guò)程：

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

測(cè)量更新過(guò)程：

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

4仿真及試驗(yàn)驗(yàn)證

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

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

δ_max=max（|x_est-x_real|）

式中： δ_max. 最大絕對(duì)誤差； x_est 估算值；x_real 真實(shí)值。

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

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

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

4.1 仿真驗(yàn)證

4.1.1 仿真驗(yàn)證1

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

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

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

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

4.1.2 仿真驗(yàn)證2

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

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

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

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

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

4.2 實(shí)車試驗(yàn)驗(yàn)證

4.2.1 試驗(yàn)驗(yàn)證1

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

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

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

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

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

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

4.2.2 試驗(yàn)驗(yàn)證2

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

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

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

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

5結(jié)論

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

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

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

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

參考文獻(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)，汶渭棟.基于卡爾曼濾波車速估算的電動(dòng)汽車換擋策略[J].汽車電器，2024（8）：15-17，20.

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

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

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

（編輯楊凱麟）