基于自適應(yīng)擴(kuò)展卡爾曼濾波算法的道路縱向坡度估計(jì)

符 凱

（長(zhǎng)安大學(xué) 汽車學(xué)院，陜西 西安 710064）

道路縱向坡度定義為海拔高度關(guān)于水平距離的變化率，以百分?jǐn)?shù)表示，上坡為正值，下坡為負(fù)值[1]。道路縱向坡度會(huì)影響汽車行駛過(guò)程的能量變化，縱向坡度較大時(shí)，在上坡路段，車輛容易出現(xiàn)熄火現(xiàn)象，在下坡路段，由于重力的影響，車輛會(huì)加速行駛，反復(fù)使用制動(dòng)器容易出現(xiàn)熱衰退現(xiàn)象，導(dǎo)致剎車失靈，增加駕駛員的操作難度，易引發(fā)交通事故，獲取實(shí)時(shí)的道路縱向坡度可以使車輛更加高效和安全地運(yùn)行[2]。

本文主要是針對(duì)道路縱向坡度的研究，主要是通過(guò)輪胎縱向力和車速來(lái)估計(jì)坡度大小，但因?yàn)楂@取的參數(shù)存在未知的噪聲干擾，采用自適應(yīng)擴(kuò)展卡爾曼濾波（Adaptive Extended Kalman Filter,AEKF）算法來(lái)過(guò)濾噪聲[3]，在 MATLAB軟件下的Simulink中建立仿真模型，設(shè)置工況來(lái)驗(yàn)證模型準(zhǔn)確性和實(shí)時(shí)性。

1 坡道識(shí)別原理

當(dāng)汽車在坡道上行駛時(shí)，受到的力有坡度阻力Fi，驅(qū)動(dòng)力Ft，滾動(dòng)阻力Ff、空氣阻力Fw和加速阻力Fj，縱向受力分析如圖1所示。

圖1 汽車縱向受力分析

汽車縱向平衡方程為

將G=mg、Fj-˙、Fi=mgsinα帶入式（1），并將取δ=1可得加速度公式：

式中，v為車輛縱向車速，m/s；m為整車質(zhì)量，kg；Fmid為等效在車輪中心的輪胎縱向力，N；α為道路縱向坡度角，°；α上坡時(shí)取正號(hào)，下坡時(shí)取負(fù)號(hào)；G為汽車所受重力，N；g為重力加速度，m/s2，取 9.8 m/s2。

進(jìn)一步推導(dǎo)式（2），可以得到關(guān)于坡度角的方程式為

式中，F(xiàn)mid和v都可以通過(guò)傳感器獲取[4]，F(xiàn)w可以通過(guò)計(jì)算獲得，所以當(dāng)已知汽車質(zhì)量m時(shí)，能夠求解出道路坡度角的正弦值。結(jié)合實(shí)際情況分析，公路設(shè)計(jì)標(biāo)準(zhǔn)中道路的縱向坡度不能大于9%，及對(duì)應(yīng)的角度則是在 5°以內(nèi)，因?yàn)榻嵌刃。梢哉J(rèn)為坡度角的正弦值和正切值相等。

2 基于卡爾曼濾波算法的道路縱向坡度估計(jì)

2.1 建立非線性狀態(tài)空間模型

本文是利用測(cè)得車軸端力傳感器數(shù)據(jù)和汽車控制器局域網(wǎng)（Controller Area Network, CAN）總線上獲取的實(shí)時(shí)車速來(lái)估測(cè)道路縱向坡度，但因?yàn)橛^測(cè)器產(chǎn)生的噪聲會(huì)影響計(jì)算數(shù)值的準(zhǔn)確性，因此，用自適應(yīng)擴(kuò)展卡爾曼濾波算法過(guò)濾產(chǎn)生的噪聲，降低噪聲影響。

基于卡爾曼濾波算法建立非線性狀態(tài)空間模型[5]：

式中，X(t)為系統(tǒng)狀態(tài)量；U(t)為系統(tǒng)控制量，因?yàn)镕mid是觀測(cè)驅(qū)動(dòng)力、制動(dòng)阻力和滾動(dòng)阻力的合力，包含U(t)，所以此時(shí)U(t)=0。結(jié)合式（3）將式（4）展開(kāi)：

式中，A為迎風(fēng)面積；CD為空氣阻力系數(shù)；ρ為空氣密度。將Δt為間隔，使用歐拉算法將系統(tǒng)狀態(tài)空間轉(zhuǎn)移模型離散化用以實(shí)現(xiàn)遞歸運(yùn)算：

上式中系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣為

觀測(cè)矩陣為

噪聲向量為

觀測(cè)噪聲向量為

通過(guò)式（6）—式（10）獲得的參數(shù)可以求解道路縱向坡度。

2.2 噪聲方差估計(jì)

假設(shè)w為系統(tǒng)噪聲；n為觀測(cè)噪聲，兩者均是滿足方差一定、均值為零的高斯白噪聲，服從條件：

Q和R代表白噪聲的方差，Q和R取值會(huì)影響卡爾曼增益?？梢杂绊慟的因素有模型誤差、舍入誤差、離散化誤差和控制量誤差。參數(shù)R的取值決定于傳感器特性。采用自適應(yīng)擴(kuò)展卡爾曼濾波器對(duì)道路縱向坡度進(jìn)行估計(jì)，能在不影響準(zhǔn)確性的前提下，確保濾波器更快的收斂，但需要反饋來(lái)實(shí)現(xiàn)遞推過(guò)程。在每個(gè)時(shí)間步上，都需要時(shí)序更新和觀測(cè)更新。

時(shí)序更新：

式中，為估計(jì)中間量；為觀測(cè)中間量；P為狀態(tài)向量誤差協(xié)方差；θ為衰減因子；s(k-1)和X(k-1)變化有關(guān)，呈正相關(guān)性。

觀測(cè)更新：

式中，e為觀測(cè)誤差；E為單位矩陣；時(shí)狀態(tài)向量誤差協(xié)方差中間量。

通過(guò)最大似然原則的方法，得到系統(tǒng)噪聲協(xié)方差Q和觀測(cè)噪聲協(xié)方差R，如下：

式中，N為利用過(guò)去時(shí)刻的數(shù)量；G為觀測(cè)向量誤差方差。

2.3 濾波器工作流程

矩陣P是一個(gè)3×3的對(duì)角矩陣，當(dāng)對(duì)角線上的元素越大時(shí)，系統(tǒng)狀態(tài)向量X?(k)估計(jì)的置信度越小。在開(kāi)始試驗(yàn)時(shí)需要采集較大的數(shù)值。濾波器可以在選取完參數(shù)X?(k)和P后，從初始狀態(tài)進(jìn)行實(shí)時(shí)更新，實(shí)現(xiàn)道路縱向坡度的估計(jì)。

圖2 自適應(yīng)擴(kuò)展卡爾曼濾波器工作過(guò)程

3 仿真過(guò)程

3.1 道路縱向坡度仿真模型

在Simulink中搭建仿真模型，建立典型道路模型，車輛參數(shù)在力傳感器和CAN總線處獲取，代入道路坡度仿真模型，用仿真的結(jié)果和設(shè)計(jì)的坡度進(jìn)行對(duì)比，來(lái)確定此道路坡度估計(jì)方法的準(zhǔn)確性和實(shí)時(shí)性。

圖3 Simulink道路縱向坡度估計(jì)模型

依據(jù)《公路工程技術(shù)JT J001—1997標(biāo)準(zhǔn)》中的公路設(shè)計(jì)規(guī)范如表1所示[6]。

表1 各級(jí)公路參數(shù)表

本文不考慮彎道情況，設(shè)置仿真道路總長(zhǎng)為4 km的直線道路，道路縱向坡度變化周期為0.2 km，路面超高為 0，附著系數(shù)為 0.85的瀝青混凝土路面，初始車速72 km/h。

如圖4所示，在變化坡度上自適應(yīng)卡爾曼濾波器可以根據(jù)當(dāng)前狀態(tài)不斷調(diào)整估計(jì)值，所預(yù)測(cè)出的道路縱向坡度和所設(shè)計(jì)的坡度偏差在0.2%之內(nèi)，根據(jù)設(shè)計(jì)車速，完成估計(jì)值的整個(gè)過(guò)程車輛前進(jìn)行程不超過(guò)0.05 km。綜上仿真結(jié)果表明，所提出的道路縱向坡度估計(jì)方法能夠?qū)ν蛔兊缆返目v坡進(jìn)行穩(wěn)定的估計(jì)和預(yù)測(cè)，并且具有較高的準(zhǔn)確性和實(shí)時(shí)性。

圖4 道路縱向坡度仿真估計(jì)結(jié)果

4 結(jié)論

本文結(jié)合車輛在道路上行駛時(shí)，裝配的力傳感器和CAN總線獲取的數(shù)據(jù)，運(yùn)用自適應(yīng)擴(kuò)展卡爾曼濾波算法來(lái)實(shí)時(shí)估計(jì)道路的縱向坡度，可以有效地降低預(yù)測(cè)過(guò)程中產(chǎn)生的誤差，優(yōu)化預(yù)測(cè)結(jié)果，達(dá)到了實(shí)時(shí)、準(zhǔn)確預(yù)測(cè)道路縱向坡度的目的，可以用于商用車或者智能車的路徑規(guī)劃問(wèn)題，優(yōu)化車輛在坡道路段行駛的能力。