基于自適應強跟蹤濾波器的汽車行駛狀態軟測量

周聰， 肖建

(西南交通大學電氣工程學院，四川成都 610031)

0 引言

現代汽車應用越來越多的電子控制系統來提高汽車的安全性、穩定性以及駕駛舒適性。電子穩定程序(electronic stability program，ESP)、四輪轉向控制(four wheel steering，4WS)、主動轉向控制(active steering control，ASC)等通過控制車身的側向和橫擺運動來提高汽車的操縱穩定性;主動側傾控制(active roll control，ARC)、主動車身控制系統(active body control，ABC)等通過控制車身的垂直、傾覆運動來改善汽車的駕駛舒適性。要實現這些電子控制系統的特定功能，前提條件就是要準確獲得汽車行駛過程中的狀態參數，如縱向車速、側向加速度、橫擺角速度、質心側偏角等。

由于測量技術和測量成本的原因，要直接測量所有重要的狀態變量存在一定難度。目前，基于狀態估計的軟測量技術已被成功應用于估計汽車的動力學和運動學參數信息。Venhovens［1］和 Gustaffson［2］等采用卡爾曼濾波器(Kalman filter，KF)估計汽車的側向加速度、橫擺角速度、輪胎力和路胎摩擦系數;Wenzel［3-4］等采用雙擴展卡爾曼濾波器(dual extended Kalman filter，DEKF)并行估計車輛的狀態變量和參數變量;另外，也有一部分學者采用龍貝格觀測器和滑模觀測器估計汽車的質心側偏角和側向力［5-7］。

在這些估計算法中，基于KF的狀態估計方法應用最為廣泛，在實車試驗中也取得了較好的效果。但是KF算法的設計難點為:1)要求精確知道被觀測系統的模型;2)要求已知系統噪聲和測量噪聲的統計特性。因此，要提高KF算法的估計效果，必須同時從模型和測量兩個方面著手。Sage-Husa自適應卡爾曼濾波通過引入虛擬噪聲補償系統的未建模動態，提高了濾波算法的精確度和魯棒性［8］。強跟蹤濾波器(strong track filter，STF)采用多重漸消因子有效地削弱了以往數據對模型誤差的影響，具有準確跟蹤狀態突變的能力［9］。本文以3自由度非線性汽車動力學模型為基礎，將自適應卡爾曼濾波器和STF算法相融合，提出一種基于自適應強跟蹤濾波(adaptive strong track filter，ASTF)的汽車行駛狀態估計算法，用于汽車的行駛狀態變量的軟測量中。該方法有效克服由于汽車動力學模型中的未建模動態引起的估計誤差過大和行駛過程中突變狀態的跟隨問題。在此基礎上，通過Carsim和Matlab/SIMULINK聯合仿真，分別運用ASTF和EKF對車輛的縱向車速、橫擺角速度及質心側偏角等關鍵狀態進行實時估計。

1 汽車的非線性動力學模型

用于分析汽車橫擺和側向運動的2自由度模型是一個假設汽車以勻速行駛的線性模型。但在實際當中，縱向車速是隨時可能發生變化的，而變化的縱向車速對汽車的橫擺和側向運動有顯著的影響，且構成狀態變量之間的非線性關系。本文以橫擺、側向、縱向3自由度非線性汽車動力學模型為基礎，構建汽車狀態估計的軟測量模型。圖1為3自由度汽車動力學模型。

圖1 3自由度汽車動力學模型Fig．1 3 DOF vehicle dynamic model

如圖1所示，XaOaYa為固定在水平面上的絕對坐標系，xoy為固定在汽車質心上的車輛坐標系。其中，x軸與汽車的縱向對稱軸重合，規定向前為正;y軸通過車輛的質心o點，規定向左為正;所有XaOaYa平面內的角度和力矩以逆時針方向為正，各矢量的分量以與坐標軸通向為正。該3自由度非線性汽車動力學模型的狀態方程和觀測方程如式(1)、式(2)所示，即

式中:ω為橫擺角速度;β為質心側偏角;vx為縱向車速;δf為前輪轉角;ay為側向加速度;ax為縱向加速度;m為車輛質量;Iz為繞z軸的轉動慣量;lf為前輪軸距;lr為后輪軸距;Cf為前輪等效側偏剛度;Cr為后輪側偏剛度。

以方向盤轉角δsw作為控制輸入，設定方向盤到前輪轉角的傳動比i=16，可得

2 基于軟測量技術的汽車狀態估計

2.1 軟測量技術

軟測量技術也稱為軟儀表技術［10］。基本原理為:利用那些與難于檢測的主導變量有密切關系的、容易檢測到的輔助變量，通過數學模型運算，得到主導變量的估計值。軟測量技術常用的方法包括機理建模分析、回歸分析、狀態估計、模式識別、神經網絡、模糊數學、過程層析成像以及相關分析等。本文采用的基于狀態估計的軟測量技術，是在建立非線性汽車軟測量模型的條件下，根據可測信息集通過運用自適應強跟蹤濾波算法推出主導變量的估計值，實現用常規方法難以測量的狀態參數的測量。圖2為基于軟測量技術的汽車狀態估計系統。

圖2 汽車行駛狀態估計系統Fig．2 Block diagram of vehicle state estimation system

圖2中，w、v和u分別為可測的過程噪聲、量測噪聲和控制變量。y(k)和(k)分別為對象輸出的實際值和估計值，(k)為對象狀態的估計值，離散時刻k=0，1，2，…。

2.2 基于自適應強跟蹤濾波器的軟測量算法

設系統采樣周期為T，可將系統離散化為標準的非線性離散系統的狀態方程和觀測方程， 其形式為

式中:xk為狀態向量;yk+1為觀測向量;uk為系統輸入;wk、vk+1分別為過程噪聲和量測噪聲，滿足為過程噪聲協防差陣，)=Rk，Rk為量測噪聲協方差陣，E(·)為期望函數;離散時刻k=0，1，2，…，j=1，2，…;Φ、h為非線性或線性向量函數，且具有關于狀態的一階連續偏導數。

傳統的擴展卡爾曼濾波器(extended Kalman filter，EKF)局限于需要精確知道系統的數學模型和噪聲的統計特性，在大多數情況下，EKF只能給出狀態估計的有偏估計，并且對模型誤差的魯棒性較差。汽車作為一個復雜系統，在行駛過程中的噪聲特性往往難以統計。為此，針對有未建模誤差的動態系統，以及模型參數和噪聲統計特性存在未知漂移等問題，本文基于自適應卡爾曼濾波器和STF算法，將系統未建模動態歸入虛擬噪聲，通過引入多重次優漸消因子在線修正虛擬噪聲統計特性來補償系統的建模誤差，得到一種自適應強跟蹤濾波軟測量算法。該算法的遞推過程如式(5)所示，即

式中，Vk為殘差協方差陣。

式中，β為引入的弱化因子。

根據STF定義，若引入多重漸消因子矩陣，本文所提出的自適應強跟蹤濾波算法必須滿足正交性原理，即滿足以下兩個條件:

2)殘差序列保持正交，即

若要證明該算法滿足上述正交性原理，先給出如下定理。

定理 1 令 εk=xk-，其中為上述 ASTF算法得到的狀態估計值，若計算法復雜度O［|εk|2］?O［|εk|］成立，則有下式成立，即

上述定理可按照文獻［11］中給出的方法證明，故此處只給出關鍵證明過程。

證明:由式(5)可得

同理由式(5)可得

將式(16)代入式(15)，即可得式(12)，于是定理1得證。

根據STF算法，在每一步迭代過程中，應通過在線調整增益矩陣Kk+1，從而滿足E(Sk+1+j)=0，k=0，1，2，…，j=1，2，…。根據定理 1，可將此條件轉換為選擇適當的時變增益矩陣 Kk+1，使得式(17)成立，則正交性原理必然滿足。

由此得到式(17)成立的充分條件為

對式(19)兩邊做求跡運算 tr(·)，并除以tr(Mk+1)，可得式(9)。

至此證明了本文所提出的算法滿足正交性原理。

下面給出ASTF算法的迭代步驟:

1)令k=0，設置初始值 x0，P0，噪聲協方差陣Q，設置虛擬觀測噪聲期望和協方差矩陣，設置參數b，ai，ρ，β。

3)由式(14)計算殘差協方差陣Vk+1，并連同式(15)～式(18)計算多重漸消因子矩陣Λk+1。

5)由式(7)計算 Pk+1，k，由式(8)、式(10)和式(11)更新增益矩陣Kk+1、狀態估計和誤差協方差陣Pk+1。

6)k+1→k，若k達到設定結束步數，則終止算法;否則，轉向步驟2)繼續迭代循環。

3 基于Carsim和Matlab/SIMULINK聯合仿真的算法驗證與分析

3.1 Carsim和Matlab/SIMULINK聯合仿真環境

Carsim［12］是一款由美國 mechanical simulation corporation開發的汽車動力學仿真軟件。Carsim軟件采用實際車輛的數據可以仿真駕駛員、路面及車輛動力學輸入的響應，主要用來預測和仿真汽車的操縱穩定性、制動性、平順性、動力性和經濟性。本文基于Carsim與Matlab/SIMULINK聯合仿真環境建立自適應強跟蹤濾波器估算汽車狀態，通過他為濾波器提供控制輸入和測量輸出，將算法估計值與輸出值進行比較，驗證算法的有效性。圖3為基于Carsim和Matlab/SIMULINK的仿真框圖。

圖3 基于Carsim和Matlab/Simulink的仿真框圖Fig．3 Block diagram of simulation based on Carsim and Matlab/SIMULINK

3.2 急劇雙移線試驗

為了驗證ASTF算法的準確性、可靠性，采用Carsim中某款SUV的實車參數，并依據ISO3888-1-1999標準，在Carsim中虛擬仿真急劇雙移線試驗，采樣周期為0.025 s。實驗中，汽車快速通過路面設定的標桿，路徑如圖4所示，方向盤回轉，根據側向加速度的觀測量估算橫擺角速度、質心側偏角等。

圖4 雙移線試驗行駛路徑Fig．4 Path of double lane change test

為了驗證ASTF算法對含噪聲非線性汽車系統的估計性能，本文對ASTF算法與EKF算法進行比較。圖5～圖7為基于ASTF算法和EKF算法的車輛縱向速度、橫擺角速度、質心側偏角等關鍵狀態參數的估計結果，并將其與Carsim的實際輸出值進行比較。

設置算法的初始參數為:x0=［0 0 0］T;P0=105I3×3;b=0.98;ai=1，i=1，2，3。并設置 ASTF 和EKF具有相同的噪聲協方差陣 QASTF=10-5I3×3，QEKF=10-5I3×3。

從圖5～圖7中可以看出，基于ASTF和EKF算法得到的汽車非線性狀態符合實際情況，兩種算法的估計值非常接近，誤差在可控制的范圍內。在t=4～10 s區間內，汽車進入移線行駛狀態，縱向車速、橫擺加速度及質心側偏角等狀態在此區間內發生急劇變化。由圖5、圖6(a)和圖7(a)可知，由于狀態的噪聲協方差取值過小，EKF算法在曲線的上升和下降階段明顯存在滯后，而ASTF算法采用了多重次優漸消因子迫使估計殘差具有正交性，增強了對突變狀態的跟蹤能力，從而能夠很好地跟蹤變化中的狀態值。

由圖5可知，在汽車縱向速度的估算過程中，當移線行駛結束后，ASTF的估計精確度很高，其估計是無偏的。而EKF算法的估計存在偏差，且通過仿真可知，當取縱向車速的噪聲協方差QEKF1為原來的1/10時，EKF算法將獲得與ASTF近似的估計精確度。相反，增大QEKF，其估計精確度逐漸下降，甚至發散。在狀態變量橫擺角速度和質心側偏角變化曲線的波峰和波谷處，EKF算法的估計偏差要大于ASTF算法的估計偏差，此時各狀態變量處于明顯的非線性區域，由于ASTF算法中多重次優漸消因子產生調節作用，并自適應修正估計狀態的偏差，從而ASTF比EKF具有更高的估計精確度。

由圖6(b)和圖7(b)可知，當量測值中加入零均值、均方差為0.04的白噪聲時，ASTF能有效抑制噪聲，且仍具有很高的估計精確度，這是因為算法中所取的噪聲協方差較小，能夠有效削弱噪聲帶來的影響，避免引起估計值的波動，同時在多重次優漸消因子的作用下該算法仍能獲得很高的估計精確度。而EKF算法隨噪聲協方差增大，其抑制噪聲的能力變弱，導致估計精確度下降。

綜上所述，EKF算法的性能受噪聲協方差陣、量測噪聲的水平等因素的影響很大，而ASTF算法則對此不敏感，能滿足系統較大范圍內的不確定性;而且當狀態變量發生突變時，ASTF算法具有很好的跟蹤能力，而EKF算法相對較差。

圖5 縱向車速的估計值與Carsim輸出值的對比曲線Fig．5 Comparison of vehicle longitudinal velocity between estimated value and Carsim value

圖6 橫擺角速度的估計值與Carsim輸出值的對比曲線Fig．6 Comparison of yaw rate between estimated value and Carsim value

圖7 質心側偏角的估計值與Carsim輸出值的對比曲線Fig．7 Comparison of side slip angle between estimated value and Carsim value

4 結語

本文在自適應卡爾曼濾波和STF算法的基礎上提出一種適用于狀態突變的ASTF算法，并結合簡化的3自由度非線性汽車模型，建立了對車身縱向、側向和橫擺穩定性控制所需關鍵狀態的軟測量方法。在 Carsim虛擬急劇雙移線試驗中，采用ASTF算法得到的狀態估計值具有很高的精確度，滿足汽車狀態估計器的對軟件性能的要求。通過Carsim和Matlab/SIMULINK聯合仿真，比較同等條件下的EKF算法和ASTF算法，仿真結果表明，ASTF算法在估計精確度、跟蹤速度和噪聲抑制能力等方面均優于EKF算法，能夠有效地估計汽車行駛過程中的車速、質心側偏角和橫擺角速度，具有很強的實時性，適用于汽車狀態的在線估計。本文在采用實車參數的Carsim仿真環境下，驗證了基于ASTF的軟測量算法的正確性和有效性。下一步將通過硬件在環仿真技術，將所建立的軟測量模型和估計算法應用于實車試驗中，進一步驗證其實用性和可靠性。

［1］VENHOVENS P J T，NAAB K.Vehicle dynamics estimation using Kalman filters［J］.Vehicle System Dynamics，1999，32(2):171-184.

［2］GUSTAFFSON F，DREV? M，FORSSELL U，et al.Virtual sensors of tire pressure and road friction［C］//Proceedings of the 2001 Society of Automotive Engineers(SAE)World Congress，March 5-8，2001，Detroit，USA.2001:010796.

［3］WENZEL T A，BURNHAM K J，BLUNDELL M V.Dual extended Kalman filter for vehicle state and parameter［J］.Vehicle System Dynamics，2006，44(2):153-171.

［4］WENZEL T A，BURNHAM K J，BLUNDELL M V，et al.Kalman filter as a virtual sensor:applied to automotive stability systems［J］.Transactions of the Institute of Measurement and Control，2007，29(2):95-115.

［5］TSENG H E.A sliding mode lateral velocity observer［C］//Proceedings of the 6th International Symposium on Advanced Vehicle Control，September 9-13，2002，Hiroshima，Japan.2002:387-392.

［6］STéPHANT J，CHARARA A，MEIZEL D.Virtual sensor:application to vehicle sideslip angle and transversal forces［J］.IEEE Transactions on Industrial Electronics，2004，51(2)，278-289.

［7］趙林輝，劉志遠，陳虹.一種車輛狀態滑模觀測器的設計方法［J］.電機與控制學報，2009，13(4):565-570.

ZHAO Linhui，LIU Zhiyuan，CHEN Hong.Design method of sliding model observer for vehicle state［J］.Electric Machines and Control，2009，13(4):565-570.

［8］SAGE A P，HUSA G W.Adaptive filtering with unknown prior statistics［C］//Proceedings of Joint Automatic Control Conference，August 4-6，1969，Washington，USA.1969:760-769.

［9］周東華，席裕庚，張鐘俊.一種帶多重次優漸消因子的擴展卡爾曼濾波器［J］.自動化學報，1991，17(6):689-695.

ZHOU Donghua，XI Yugeng，ZHANG Zhongjun.A suboptimal multipal fading extended Kalman filter［J］.Acta Autmatica Sinica，1991，17(6):689-695.

［10］李海青，黃志堯.軟測量技術原理及其應用［M］.北京:化學工業出版社，2000:30-37.

［11］周東華，葉銀忠.現代故障診斷與容錯控制［M］.北京:清華大學出版社，2000:60-76.

［12］ANN Arbor.Carsim Reference Manual，Ver6.03［M］.Michigan:Mechanical Simulation Corporation，2005:30-53.