基于自適應(yīng)強跟蹤濾波器的汽車行駛狀態(tài)軟測量

周聰， 肖建

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

0 引言

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

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

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

1 汽車的非線性動力學(xué)模型

用于分析汽車橫擺和側(cè)向運動的2自由度模型是一個假設(shè)汽車以勻速行駛的線性模型。但在實際當(dāng)中，縱向車速是隨時可能發(fā)生變化的，而變化的縱向車速對汽車的橫擺和側(cè)向運動有顯著的影響，且構(gòu)成狀態(tài)變量之間的非線性關(guān)系。本文以橫擺、側(cè)向、縱向3自由度非線性汽車動力學(xué)模型為基礎(chǔ)，構(gòu)建汽車狀態(tài)估計的軟測量模型。圖1為3自由度汽車動力學(xué)模型。

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

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

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

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

2 基于軟測量技術(shù)的汽車狀態(tài)估計

2.1 軟測量技術(shù)

軟測量技術(shù)也稱為軟儀表技術(shù)［10］?；驹頌?利用那些與難于檢測的主導(dǎo)變量有密切關(guān)系的、容易檢測到的輔助變量，通過數(shù)學(xué)模型運算，得到主導(dǎo)變量的估計值。軟測量技術(shù)常用的方法包括機理建模分析、回歸分析、狀態(tài)估計、模式識別、神經(jīng)網(wǎng)絡(luò)、模糊數(shù)學(xué)、過程層析成像以及相關(guān)分析等。本文采用的基于狀態(tài)估計的軟測量技術(shù)，是在建立非線性汽車軟測量模型的條件下，根據(jù)可測信息集通過運用自適應(yīng)強跟蹤濾波算法推出主導(dǎo)變量的估計值，實現(xiàn)用常規(guī)方法難以測量的狀態(tài)參數(shù)的測量。圖2為基于軟測量技術(shù)的汽車狀態(tài)估計系統(tǒng)。

圖2 汽車行駛狀態(tài)估計系統(tǒng)Fig．2 Block diagram of vehicle state estimation system

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

2.2 基于自適應(yīng)強跟蹤濾波器的軟測量算法

設(shè)系統(tǒng)采樣周期為T，可將系統(tǒng)離散化為標(biāo)準(zhǔn)的非線性離散系統(tǒng)的狀態(tài)方程和觀測方程， 其形式為

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

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

式中，Vk為殘差協(xié)方差陣。

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

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

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

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

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

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

證明:由式(5)可得

同理由式(5)可得

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

根據(jù)STF算法，在每一步迭代過程中，應(yīng)通過在線調(diào)整增益矩陣Kk+1，從而滿足E(Sk+1+j)=0，k=0，1，2，…，j=1，2，…。根據(jù)定理 1，可將此條件轉(zhuǎn)換為選擇適當(dāng)?shù)臅r變增益矩陣 Kk+1，使得式(17)成立，則正交性原理必然滿足。

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

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

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

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

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

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

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

6)k+1→k，若k達到設(shè)定結(jié)束步數(shù)，則終止算法;否則，轉(zhuǎn)向步驟2)繼續(xù)迭代循環(huán)。

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

3.1 Carsim和Matlab/SIMULINK聯(lián)合仿真環(huán)境

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

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

3.2 急劇雙移線試驗

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

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

為了驗證ASTF算法對含噪聲非線性汽車系統(tǒng)的估計性能，本文對ASTF算法與EKF算法進行比較。圖5～圖7為基于ASTF算法和EKF算法的車輛縱向速度、橫擺角速度、質(zhì)心側(cè)偏角等關(guān)鍵狀態(tài)參數(shù)的估計結(jié)果，并將其與Carsim的實際輸出值進行比較。

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

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

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

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

綜上所述，EKF算法的性能受噪聲協(xié)方差陣、量測噪聲的水平等因素的影響很大，而ASTF算法則對此不敏感，能滿足系統(tǒng)較大范圍內(nèi)的不確定性;而且當(dāng)狀態(tài)變量發(fā)生突變時，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 質(zhì)心側(cè)偏角的估計值與Carsim輸出值的對比曲線Fig．7 Comparison of side slip angle between estimated value and Carsim value

4 結(jié)語

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

［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，F(xiàn)ORSSELL 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］趙林輝，劉志遠(yuǎn)，陳虹.一種車輛狀態(tài)滑模觀測器的設(shè)計方法［J］.電機與控制學(xué)報，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］周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴展卡爾曼濾波器［J］.自動化學(xué)報，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］李海青，黃志堯.軟測量技術(shù)原理及其應(yīng)用［M］.北京:化學(xué)工業(yè)出版社，2000:30-37.

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

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