基于自適應擴展卡爾曼濾波算法的道路縱向坡度估計

符 凱

（長安大學 汽車學院，陜西 西安 710064）

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

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

1 坡道識別原理

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

圖1 汽車縱向受力分析

汽車縱向平衡方程為

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

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

進一步推導式（2），可以得到關于坡度角的方程式為

式中，F(xiàn)mid和v都可以通過傳感器獲取[4]，F(xiàn)w可以通過計算獲得，所以當已知汽車質(zhì)量m時，能夠求解出道路坡度角的正弦值。結合實際情況分析，公路設計標準中道路的縱向坡度不能大于9%，及對應的角度則是在 5°以內(nèi)，因為角度小，可以認為坡度角的正弦值和正切值相等。

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

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

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

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

式中，X(t)為系統(tǒng)狀態(tài)量；U(t)為系統(tǒng)控制量，因為Fmid是觀測驅(qū)動力、制動阻力和滾動阻力的合力，包含U(t)，所以此時U(t)=0。結合式（3）將式（4）展開：

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

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

觀測矩陣為

噪聲向量為

觀測噪聲向量為

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

2.2 噪聲方差估計

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

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

時序更新：

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

觀測更新：

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

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

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

2.3 濾波器工作流程

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

圖2 自適應擴展卡爾曼濾波器工作過程

3 仿真過程

3.1 道路縱向坡度仿真模型

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

圖3 Simulink道路縱向坡度估計模型

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

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

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

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

圖4 道路縱向坡度仿真估計結果

4 結論

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