基于自適應調參濾波的路面坡度估計算法

【摘要】為解決低配電動車型缺少直接的道路坡度信息問題，提出基于現有控制器局域網（CAN）總線信號的路面坡度估計算法。首先，對總線輸入的車速和縱向加速度信號進行預處理，并根據坡度與車速、縱向加速度間的關系構建卡爾曼濾波方程，估算道路坡度；然后，分析卡爾曼濾波過程中噪聲方差參數對坡度估算結果的影響，設計自適應調參的卡爾曼濾波器，對坡度估計結果進行優化；最后，通過實車測試驗證了坡度估計算法的實時性與有效性。

主題詞：路面坡度估計 CAN總線信號 卡爾曼濾波 自適應調參

中圖分類號：U461.1" "文獻標志碼：A" "DOI： 10.19620/j.cnki.1000-3703.20230786

Road Slope Estimation Algorithm Based on Adaptive Parameter Tuning Filtering

【Abstract】At present， low-end electric vehicle do not have direct slope signal information. To address this issue， this article proposed a road slope estimation algorithm based on the existing CAN bus signals. Firstly， vehicle speed signal and longitudinal acceleration signal values input from CAN were preprocessed， and a Kalman filtering equation was constructed based on the relationship between slope， vehicle speed and longitudinal acceleration， to estimate the road slope. Then， the impact of noise variance parameters on the slop estimation results was analyzed， and an adaptive tuning Kalman filter was designed to optimize the slope estimation results. Finally， the vehicle test was conducted， verifying the accuracy and real-time performance of the ramp estimation algorithm.

Key words： Road slope estimation， CAN bus signal， Kalman filtering， Adaptive parameter tuning

1 前言

道路環境信息的挖掘和處理對車輛智能化發展至關重要[1]。其中，路面坡度估計在車輛自動換擋[2]、坡道起步[3]、自適應巡航控制[4]等領域的研究中均具有重要意義。

目前，坡度估計算法的研究主要基于全球定位系統（GPS）信號與力學模型。基于GPS信號的坡度估計算法主要根據導航坐標系中垂向速度與水平速度的比值估算坡度[5-7]。Bae等[8]分別使用單天線GPS和雙天線差分GPS方案來估算坡度角；Sahlholm等[9]、Boroujeni等[10]利用卡爾曼濾波算法將車載傳感器信息與GPS定位信息進行數據融合，獲得較高精度的道路坡度估計值，但該算法模型對GPS信號精度要求較高。

基于力學模型的坡道估計算法依據車輛縱向動力學模型中坡度與驅動力、加速度、滾動阻力、風阻之間的關系，采用模型參數辨識[11]、卡爾曼濾波[12-13]等算法對縱向坡度進行估計。劉琳[11]等以動力學模型為基礎，設計了帶有遺忘因子的最小二乘算法，但該方法無法估算車輛靜止狀態下的道路坡度，且對于配備能量回收系統的電動車輛而言，當能量回收系統工作時，動力學模型會失真，坡度估計的結果會出現較大偏差。雍文亮等[14]基于多傳感器融合濾波算法對動力學和加速度傳感器數據進行加權融合，獲得了精確的道路坡度估計值，但是模型涉及的整車參數需要進行精確標定，且整車質量未進行解耦估算，坡度估計結果會隨整車質量的不同出現較大波動。

針對大量低配電動車型無法提供直接的坡度信息問題，本文提出一種對道路坡度進行實時準確估計的低成本方案。首先，根據總線現有的車速信號和縱向加速度信號構建離散狀態空間狀態方程；然后，利用采集的實車數據離線分析卡爾曼濾波算法中噪聲方差參數對坡度估計結果的影響，同時提出自適應在線調參方法，使坡道估計響應延遲及追蹤效果均得到優化；最后，搭載量產電動車型完成實車驗證。

2 基于運動學模型的坡道角估計

2.1 車輛運動學模型

上坡過程中，車輛的加速度傳感器工作原理如圖1所示。傳感器采集的車輛縱向加速度信號由重力加速度在坡道方向的分量與自車加速度的縱向分量構成。根據縱向加速度采集原理可知：

[asenx=gsini+aCANv]" " " " " " " " " " " " " " "（1）

式中：asenx為加速度傳感器采集的縱向加速度；g為重力加速度；i為道路坡道角；[aCANv]為自車加速度估計值，[aCANv=dvdt]，其中v為CAN總線車速信號，t為時間。

當asenx與[aCANv]已知，可根據式（1）計算出坡道角估計值。但CAN總線的車速信號精度較低，當采樣周期較短時，[aCANv]分量波動較大（見圖2）。實際坡道角以傳感器采集的車輛俯仰角（Pitch）信號為參考（見圖3）。由圖3可知，路面坡道角估計值抖動劇烈，估計結果誤差較大，數據無法使用。

2.2 卡爾曼濾波坡道角估計算法

2.2.1 卡爾曼濾波

卡爾曼濾波算法相對成熟，其迭代過程包括時間更新與測量更新。時間更新為對下一時刻的狀態及誤差協方差進行預測；測量更新主要計算卡爾曼增益系數，根據卡爾曼增益系數計算當前時刻狀態值及誤差協方差，濾波方程如下：

2.2.2 縱向運動學方程離散化

定義系統狀態量x=[v asenx i]T、觀測量z=[v asenx]T，對狀態系統進行離散化，當坡道角較小時，取sini=i：

式中：v（k）為當前時刻車速；asenx（k）為當前時刻加速傳感器測量結果；i（k）為k時刻坡道角；T為離散化時間周期，根據實際總線信號，采樣周期設置為10 ms；[w1（k）]、[w2（k）]、[w3（k）]分別為速度、加速度和坡道角的過程噪聲。

因此，離散化狀態方程表達式為：

[v2（k）]分別為速度和加速度量的觀測噪聲。

3 濾波參數對坡道角估計結果的影響分析

3.1 總線信號預處理

受傳感器精度及系統噪聲影響，采集的縱向加速度信號與總線車速信號存在較大誤差，因此，需對數據進行預處理。由于重力加速度沿坡道的分量和汽車自身的加速度信號均為低頻信號，汽車振動導致的加速度噪聲屬于高頻信號，可通過一階低通濾波器濾除高頻信號：

ak=αaraw+（1-α）ak-1" " " " " " " " " " " " " （5）

式中：ak為低通濾波器k時刻輸出值，araw為原始輸入信號，α為濾波系數。

3.2 卡爾曼濾波參數對坡道角估計結果的影響

將預處理后的總線車速信號與加速度信號作為卡爾曼濾波器的觀測狀態量輸入。在卡爾曼濾波算法中，對濾波結果影響較大的參數主要是過程噪聲方差Q和測量噪聲方差R，由于過程噪聲wk和觀測噪聲vk為彼此獨立且符合正態分布的高斯白噪聲，因此滿足：

通過實車采集上坡過程中的總線參數，分析其速度信號、加速度信號隨時間變化的關系（見圖4）。計算速度信號方差和加速度信號方差分別為0.312 9 m2/s2、0.083 1 m2/s4，因此，取測量噪聲方差矩陣R1為：

根據上坡過程中采集的整車數據，令Q和R1為卡爾曼濾波噪聲參數，估計路面坡道角，結果如圖5所示。此時，坡道角估計值與參考值基本相符，但數據抖動較大，部分時刻誤差極大，導致估計結果無法使用。

應延時也會增加。因此，在設置濾波器的參數時，需選擇合適的觀測噪聲R兼顧響應延遲和估計誤差。

時，濾波后去噪效果不佳。

4 卡爾曼濾波器自適應調參

4.1 自適應調參濾波器

通過多組離線數據分析，以固定觀測噪聲方差R處理多組試驗數據時，部分數據處理結果并不理想，因此，設計自適應調參濾波器對坡道角估計結果進行進一步優化處理。本文選取濾波效果較好的Q2作為過程噪聲方差參數輸入，針對R進行在線調參設計，k時刻對應的傳感器噪聲方差為：

式中：a（k）為k時刻傳感器采集總線數據；N為采樣時間段，本文取N=100。

則k時刻對應的觀測噪聲方差R（k）可以表示為：

式中：D（v（k））、D（asenx（k））分別為k時刻車速信號、縱向加速度信號方差；p為增益系數，本文取p=1×104。

4.2 坡道角估計結果比較

所采集的數據包括5組上坡過程和5組下坡過程數據，將響應延遲、坡道角估計偏差范圍及坡道角估計偏差方差作為評價標準，對自適應濾波器和穩定濾波器的坡道角估計結果進行分析，如表1所示。其中，a組數據為自適應調參濾波器統計結果，b組數據為穩定參數濾波器統計結果。上坡和下坡過程中各取一組數據，如圖10、圖11所示。

由表1可知，在大多數工況下，自適應調參的濾波算法響應更快，穩定后的坡道角估計結果偏差范圍更小，對應的偏差波動方差更低，在坡道角估計結果中性能明顯優于穩定參數的濾波器。

由圖10a和圖11a可知，上、下坡過程中，2種濾波器均能快速追蹤坡道角的變化，自適應調參濾波器對估計結果的追蹤及對估計噪聲的濾波效果相對更優。由圖10b和圖11b可知，濾波器穩定運行后，當坡道角發生變化時，濾波器固有的響應延遲導致實時估計結果會出現較大偏差。

4.3 環境仿真及實車驗證

本文試驗使用Simulink提供的Vector CANoe工具箱，定義總線波特率、信號輸入輸出通道、信號采樣周期及CAN數據庫解析文件，實現CAN總線數據和Simulink模型在線通信。將CANoe接入整車網絡，設置Simulink為在線仿真模式，仿真步長設置為定步長10 ms，仿真時間設置為無窮大，路面坡道角估計算法流程如圖12所示。

完成軟、硬件環境搭建后，進行實車測試，試驗場地選擇地下車庫具有一定坡度的路面。實車采集坡道角結果如圖13所示，整個過程中，道路坡度角估計偏差為1°，估計結果穩定，抖動較小。因此，本文坡道角估計算法具有較好的準確性和實時性，可依據現有的總線數據準確估算出坡度，具備較強的適用性。

5. 結束語

本文利用車輛總線上現有信號，根據運動學原理，建立坡度角與車速和縱向加速度間的關系，并使用卡爾曼濾波算法消除總線信號噪聲對坡道角估計結果的影響。通過分析卡爾曼濾波參數對坡道角估計值的影響，設計了針對觀測噪聲方差參數R的自適應調參濾波器。驗證結果表明，自適應調參濾波器在響應延遲、估計誤差以及對坡道角變化時的追蹤能力均優于穩定參數的穩定濾波器。最后利用實車試驗驗證了本文提出的算法可以準確地估計當前道路坡度角，實現實時在線處理。

本文方案無需加裝額外傳感器，無整車成本增加，且低配車型同樣適配。未來，將進一步研究減速帶對路面坡度估計的影響，增強本文方案對復雜路況的適應能力。

參 考 文 獻

[1] 李海青， 楊秀建， 陳蜀喬， 等. 載貨汽車質量與路面坡度聯合估計方法研究[J]. 汽車技術， 2015（8）： 54-58.

LI H Q， YANG X J， CHEN S Q， et al. Research on Co-Estimation Method of Mass and Grade for Heavy Vehicles[J]." Automobile Technology， 2015（8）： 54-58.

[2] 李明清， 靳光盈， 朱寶全. 基于載荷和坡道識別的純電動客車自適應換擋策略研究[J]. 汽車技術， 2019（12）： 38-42.

LI M Q， JIN G Y， ZHU B Q. Research on Adaptive Shift Strategy of Pure Electric Bus Based on Load and Slope" " Identification[J]. Automobile Technology， 2019（12）： 38-42.

[3] 李偉， 吳光耀， 周升輝， 等. 一種新能源車輛坡道起步扭矩控制方法[J]. 汽車實用技術， 2022， 47（24）： 9-13.

LI W， WU G Y， ZHOU S H， et al. A Torque Control Method for Ramp Starting of New Energy Vehicles[J]. Automobile Applied Technology， 2022， 47（24）： 9-13.

[4] 馮莉， 曾輝. 考慮道路坡度的汽車自適應巡航控制策略[J]. 重慶交通大學學報（自然科學版）， 2023， 42（12）： 143-150.

FENG L， ZENG H. Vehicle Adaptive Cruise Control" " " "Strategy Considering Road Slope[J]. Journal of Chong Qing Jiao Tong University （Natural Science）， 2023， 42（12）： 143-150.

[5] RYU， JIHAN， GERDES， et al. Integrating Inertial Sensors with Global Positioning System （GPS） for Vehicle Dynamics Control[J]. Journal of Dynamic Systems Measurement amp;" " Control， 2004， 126（2）： 243-254.

[6] JO K， LEE M， SUUWOO M. Road Slope Aided Vehicle" " "Position Estimation System Based on Sensor Fusion of GPS and Automotive Onboard Sensors[J]. IEEE Transactions on Intelligent Transportation Systems， 2016， 17（1）： 250-263.

[7] ZHANG T， YANG D G， LI T， et al. Vehicle State Estimation System Aided by Inertial Sensors in GPS Navigation[C]// 2010 International Conference on Electrical and Control" "Engineering. Wuhan， China： IEEE， 2010.

[8] BAE H， RYU J， GERDES C. Road Grade and Vehicle" " " "Parameter Estimation for Longitudinal Control Using GPS[C]// Proceedings of the IEEE Conference on Intelligent Transportation Systems. Oakland， California： IEEE， 2001.

[9] SAHLHOLM P， JOHANSSON K H. Road Grade Estimation for Look-Ahead Vehicle Control Using Multiple Measurement Runs[J]. IFAC Proceedings Volumes， 2008， 41（2）： 3380-3385.

[10] BOROUJENI B Y， FREY H C. Road Grade Quantification Based on Global Positioning System Data Obtained from Real-World Vehicle Fuel Use and Emissions Measurements[J]. Atmospheric Environment， 2014， 85（3）： 179-186.

[11] 劉琳， 任彥君， 沙文瀚， 等. 融合整車質量估計的電動汽車坡道識別[J]. 吉林大學學報（工學版）， 2024（1）： 1-10.

LIU L， REN Y J， SHA W H， et al. Electric Vehicle Ramp Recognition Based on Fusion of Vehicle Mass Estimation[J]. Journal of Jilin University （Enginearing and Technology Edition）， 2024（1）： 1-10.

[12] LINGMAN P， SCHMIDTBAUER B. Road Slope and" " " " Vehicle Mass Estimation Using Kalman Filtering[J]." " " " " Vehicle System Dynamics， 2002， 37（S1）： 12-23.

[13] 戴卓， 吳光強. 基于車輛動力學的道路坡度與整車質量估計[J]. 汽車技術， 2018（1）： 20-24.

DAI Z， WU G Q. Estimation Method of Road Slope and" " Vehicle Mass Based on Vehicle Dynamics[J]. Automobile Technology， 2018（1）： 20-24.

[14] 雍文亮， 管欣， 王博， 等. 基于多傳感數據融合濾波的縱向坡度識別算法[J]. 機械工程學報， 2018， 54（14）： 116-124.

YONG W L， GUAN X， WANG B， et al. Identification" " " Algorithm of Longitudinal Road Slope Based" on" Multi-Sensor Data Fusion Filtering[J]. Journal of Mechanical" " Engineering， 2018， 54（14）： 116-124.