基于交互式多模型和容積卡爾曼濾波的汽車狀態(tài)估計?

張家旭，李 靜

基于交互式多模型和容積卡爾曼濾波的汽車狀態(tài)估計?

張家旭1，2，李 靜1

(1.吉林大學，汽車仿真與控制國家重點實驗室，長春 130022； 2.中國第一汽車集團技術中心，長春 130011)

基于UniTire輪胎模型建立了包含時變噪聲統(tǒng)計特性的汽車動力學7自由度整車模型。針對系統(tǒng)狀態(tài)噪聲和觀測噪聲統(tǒng)計特性未知的問題，提出了一種基于交互式多模型和容積卡爾曼濾波(IMM-CKF)車輛狀態(tài)估計算法。該算法采用包含不同系統(tǒng)狀態(tài)噪聲和觀測噪聲統(tǒng)計特性的汽車動力學模型作為交互式多模型算法的模型集，用容積卡爾曼濾波器對每個子模型的車輛狀態(tài)進行估計，并使融合輸出結果始終保持跟蹤估計誤差小的子模型輸出。最后利用實車場地環(huán)境下多種駕駛工況的測試數(shù)據(jù)對IMM-CKF算法進行離線驗證，并與容積卡爾曼濾波器的估計結果進行對比，結果表明其估計性能優(yōu)于容積卡爾曼濾波器。

汽車動力學；容積卡爾曼濾波；交互式多模型；汽車狀態(tài)估計

Keywords:vehicle dynamics； cubature Kalman filter； interactive multiple model； vehicle state estimation

前言

汽車的縱向和側向速度是車輛主動安全系統(tǒng)中重要的控制變量，對于功能高度集成的電子穩(wěn)定控制系統(tǒng)(electronic stability program，ESP)，車輛縱向和側向速度是進一步確定控制邏輯中車輪滑移率、質心側偏角等控制變量的重要參數(shù)，還是車載故障診斷系統(tǒng)中的關鍵技術之一，因此準確實時獲取車輛的縱向和側向速度是實現(xiàn)車輛主動安全控制的必要前提。由于技術和成本方面的原因，這些信息通常無法直接測量，由此衍生出的基于車載傳感器獲得的車輛狀態(tài)信息進行汽車縱向和側向速度估計成為近年來國內外研究的熱點[1-3]。

文獻[4]中應用無跡卡爾曼濾波器(unscented Kalman filter，UKF)對汽車狀態(tài)進行估計，由于文中采用包含時不變噪聲統(tǒng)計特性的非線性車輛動力學模型作為算法設計的標稱模型，故不準確系統(tǒng)過程噪聲和觀測噪聲統(tǒng)計特性會影響估計精度。文獻[5]中采用3自由度的非線性車輛模型作為雙擴展卡爾曼濾波器(dual extended Kalman filter，DEKF)設計的標稱模型，并利用DEKF對車輛狀態(tài)和路面附著系數(shù)進行估計，在迭代估計過程中需要計算系統(tǒng)狀態(tài)方程和量測方程的雅可比矩陣，容易導致出錯及估計器實時性降低。文獻[6]中采用自適應神經(jīng)模糊推理系統(tǒng)建立車輛的側向加速度、橫擺角速度、轉向盤轉角、縱向速度與質心側偏角之間的非線性輸入-輸出關系，通過輸入信號直接計算車輛的質心側偏角，避免標稱模型的不確定性對估計結果的影響，但該方法需要大量的訓練數(shù)據(jù)進行自學習，且外延性差。文獻[7]中設計非線性觀測器估計車輛的縱向速度、側向速度和路面附著系數(shù)，并構造李雅普諾夫函數(shù)證明系統(tǒng)的穩(wěn)定性，但該方法未考慮標稱模型的不確定性、外界擾動對估計精度和系統(tǒng)穩(wěn)定性的影響。文獻[8]中融合慣性傳感器和GPS傳感器的測量信息，采用自適應擴展卡爾曼濾波器估計車輛的質心側偏角，由于傳感器的成本較高，限制了其適用范圍。因此，在設計車輛縱向和側向速度估計器時，應充分考慮系統(tǒng)過程噪聲和觀測噪聲的不確定性對估計精度的影響，以及最小化估計算法對硬件資源的需求。

與傳統(tǒng)EKF相比，容積卡爾曼濾波器(cubature Kalman filter，CKF)具有估計精度高和實時性強的特點[9]。但二者均需要較精確的系數(shù)數(shù)學模型和噪聲統(tǒng)計特性，否則會導致預測和估計精度降低甚至產(chǎn)生發(fā)散現(xiàn)象。為此，本文中將CKF與交互式多模型算法(interacting multiple model，IMM)結合形成一種交互式多模型容積卡爾曼濾波算法(IMM-CKF)，該算法采用包含不同系統(tǒng)狀態(tài)噪聲和觀測噪聲統(tǒng)計特性的7自由度車輛模型作為IMM的模型集，用CKF對每個子模型的車輛縱向和側向速度進行估計，并使融合輸出結果始終保持跟蹤估計誤差小的子模型輸出。最后，在Matlab/Simulink環(huán)境下利用多種工況的實車場地試驗數(shù)據(jù)進行離線驗證，并與CKF的估計結果進行對比分析。結果表明，IMMCKF算法的估計性能優(yōu)于CKF。

1 車輛動力學模型

1.1 整車模型

本文中采用7自由度車輛動力學模型作為車輛狀態(tài)估計算法設計的標稱模型，如圖1所示，該模型包括車輛的縱向運動、側向運動、橫擺運動和4個車輪的回轉運動。

圖1 7自由度車輛動力學模型示意圖

對于圖1所示的模型，車輛運動的微分方程為

式中:vx為縱向速度；vy為側向速度；ax為縱向加速度；ay為側向加速度；γ為橫擺角速度；Iz為整車繞垂直軸的轉動慣量；Fx1，F(xiàn)x2，F(xiàn)x3和 Fx4分別為 4個車輪上的縱向力；Fy1，F(xiàn)y2，F(xiàn)y3和 Fy4分別為 4 個車輪上的側向力；δ為前輪轉角；tf和tr分別為前輪距寬和后輪距寬；a和b分別為整車質心到前后軸的距離。

忽略空氣阻力的影響，根據(jù)達朗貝爾原理建立車輛加速度與輪胎力之間的關系:

式中m為整車質量。

對于式(1)和式(2)中涉及的輪胎力，考慮其非線性動態(tài)特性，本文中采用郭孔輝院士提出的UniTire輪胎模型[10]來描述輪胎的縱向力和側向力。

首先定義輪胎的縱向滑移率Sx、側向滑移率Sy與無量綱的相對縱向滑移率φx、相對側向滑移率φy和相對總滑移率φ:

式中:Ω為輪胎滾動角速度；v為輪心的移動速度；α為輪胎側偏角；Re為有效滾動半徑；Kx和Ky分別為輪胎的縱向剛度和側偏剛度；μx和μy分別輪胎接觸印跡內縱向和側向摩擦因數(shù)；Fz為輪胎所受的垂直載荷。

輪胎所受的縱向力Fx和側向力Fy[10]可表示為

式中Ex和Ey為UniTire輪胎模型的待匹配參數(shù)，可通過輪胎試驗數(shù)據(jù)辨識得到。

1.2 包含噪聲的車輛狀態(tài)方程

本文中討論的車輛狀態(tài)估計問題根據(jù)車載傳感器測量得到的縱向加速度ax、側向加速度ay、橫擺角速度γ、前輪轉角δ(由轉向盤轉角間接測量得到)和輪速[w1w2w3w4]T信息，估計車輛的縱向速度vx、側向速度vy和橫擺角速度γ。其中，將γ作為被估計狀態(tài)是為了充分利用其測量信息。因此，根據(jù)7自由度車輛動力學模型，設狀態(tài)向量為

系統(tǒng)輸入為

觀測向量為

由式(1)～式(3)和前向歐拉積分法可得車輛系統(tǒng)離散化的狀態(tài)方程和觀測方程為

式中wk和vk分別為狀態(tài)估計的過程噪聲和觀測噪聲，二者相互獨立，均為零均值的高斯白噪聲，設它們的協(xié)方差矩陣分別為Q和R。

研究發(fā)現(xiàn)[11]:在過程噪聲協(xié)方差Q一定的情況下，較大的測量噪聲協(xié)方差R會減小過程噪聲對濾波算法估計精度的影響，但會降低其收斂速度；在測量噪聲誤差協(xié)方差R一定的情況下，較大的過程噪聲協(xié)方差Q會減少濾波算法的時間延遲，但會加大測量噪聲對其估計精度的影響，使估計誤差變大。為實時保持濾波器的最優(yōu)性能，避免濾波發(fā)散，需要根據(jù)不同的環(huán)境調試噪聲參數(shù)。本文中采用一組并行的濾波器，其中每個濾波器采用不同的噪聲參數(shù)，并根據(jù)觀測量單獨進行濾波，然后根據(jù)濾波殘差對各個濾波器的估計值進行加權。

2 容積卡爾曼濾波器

由Arasaratnam等人提出的CKF算法基于球面徑向積分準則，利用2n個具有相同權重的容積點對狀態(tài)的概率密度分布函數(shù)進行逼近，避免復雜的雅克比矩陣的運算，提高了濾波的精度和穩(wěn)定性。CKF算法步驟如下[12]。

步驟1:時間更新

式中:Sk｜k為 Pk｜k的平方根；ξi＝ n[1]i，[1]i為基本容積點；Xi，k｜k為計算的容積點；X?i，k+1｜k為通過狀態(tài)方程傳播輸出的容積點；x＾k+1｜k為狀態(tài)向量的一步預測值；Pk+1｜k為誤差協(xié)方差一步預測值。

步驟2:量測更新

式中:Sk+1｜k為 Pk+1｜k的平方根；Xi，k+1｜k為計算的容積點；Zi，k+1｜k為通過測量方程傳播輸出的容積點；y＾k+1｜k為測量的一步預測值；Pyk+1yk+1為新息協(xié)方差矩陣；Pxk+1yk+1為交叉協(xié)方差矩陣的一步預測值；Kk+1為卡爾曼增益矩陣；x＾k+1｜k+1為當前時刻的狀態(tài)向量；Pk+1｜k+1為當前時刻的誤差協(xié)方差矩陣。

3 交互式多模型容積卡爾曼濾波算法

交互式多模型算法具有自適應的特點，能夠有效地對每個子模型的概率進行實時調整，并根據(jù)模型轉移矩陣使融合輸出結果始終保持跟蹤誤差小的子模型輸出[13]。 定義 M＝{m1，m2，…，mN}為描述系統(tǒng)的模型集合，k時刻模型mi出現(xiàn)的條件概率為μi，k＝p{mi，k，yk}，k 時刻到 k+1 時刻從模型 mi到模型 mj的轉移概率為 pij＝p{mj，k+1｜mi，k}，交互式多模型容積卡爾曼濾波算法的具體計算步驟如下。

步驟1:模型mi與模型mj的混合概率ui/j，k可表示為式中 μj，k+1｜k為模型 mj的預報概率。

步驟2:輸入交互

步驟3:采用式(8)和式(9)對每個模型進行時間更新和量測更新。

步驟4:模型概率更新

模型似然:

模型概率:

步驟5:經(jīng)濾波后進行組合，將每個模型的輸出結果組合得到最終的濾波輸出交互:

4 實車試驗驗證

為了驗證IMM-CKF算法對車輛狀態(tài)估計的準確性和可靠性，利用某三廂轎車進行操縱穩(wěn)定性場地試驗，該試驗車安裝有輪速傳感器、轉向盤轉角傳感器、GPS/INS慣性組合導航系統(tǒng)RT3000和轉向機器人等，數(shù)據(jù)采集儀的采樣頻率為1 000Hz。試驗車的參數(shù)如表1所示。

表1 車輛參數(shù)

考慮車輛行駛中的系統(tǒng)過程噪聲和觀測噪聲的不確定性，引入一組系統(tǒng)過程噪聲和觀測噪聲，得到一組模型集 M＝{m1，m2，m3}。

設定模型m1是噪聲較小的情況，其過程噪聲和觀測噪聲協(xié)方差為

設定模型m2是噪聲適中的情況，其過程噪聲和觀測噪聲協(xié)方差為

設定模型m3是噪聲較大的情況，其過程噪聲和觀測噪聲協(xié)方差為

設定3個模型的馬爾科夫轉移概率矩陣為

考慮車輛側傾角的影響，對GPS/INS慣性組合導航系統(tǒng)RT3000測得的車輛側向加速度進行修正:

式中:aym為GPS/INS慣性組合導航系統(tǒng)RT3000測量的車輛側向加速度；φ為GPS/INS慣性組合導航系統(tǒng)RT3000測量的車輛側傾角；ay為車輛側向加速度的修正值。

設定車輛質心處的縱向速度、側向速度和橫擺角速度3個狀態(tài)量的估計初始值為GPS/INS慣性組合導航系統(tǒng)RT3000記錄的試驗初始值。在Matlab/Simulink環(huán)境下實現(xiàn)IMM-CKF算法，并通過離線仿真將實車試驗數(shù)據(jù)與IMM-CKF算法的估計結果進行對比，如圖2所示。

圖2 算法驗證流程

4.1 斜坡輸入試驗

在干燥、平坦而清潔的水泥試驗場地，車速保持80km/h，利用轉向機器人進行斜率為40°/s，峰值為160°的轉向盤斜坡輸入，估計輸入如圖3(a)所示，試驗測量值、CKF算法和IMM-CKF算法的估計結果如圖3(b)～圖3(d)所示。該試驗可使車輛隨著轉向盤轉角增加而逐漸進入極限工況，且輪胎由線性工作區(qū)逐漸過渡到非線性工作區(qū)，從而導致車輛標稱模型的誤差增加。研究表明[14-16]:標稱模型的誤差可以折算成其系統(tǒng)噪聲，且標稱模型的系統(tǒng)噪聲表征外部真實系統(tǒng)噪聲的精確程度是影響系統(tǒng)狀態(tài)量的估計精度的重要因素。由圖3(b)～圖3(d)可見:在系統(tǒng)噪聲增加時，CKF算法對縱向速度和側向速度的估計偏差逐漸增大，但IMM-CKF算法可根據(jù)每個子模型的目標估計狀態(tài)和系統(tǒng)的當前測量值對其模型概率進行實時調整，并通過馬爾科夫轉移矩陣Φ使融合輸出結果始終平滑地保持跟蹤誤差小的子模型輸出，從而使該算法對對縱向速度和側向速度的估計保持較高的估計精度；對于橫擺角速度的估計，CKF算法和IMM-CKF算法均保持較高的估計精度。

圖3 斜坡輸入試驗的估計結果對比分析

4.2 急劇雙移線試驗

圖4 急劇雙移線試驗的估計結果對比分析

在干燥、平坦而清潔的水泥試驗場地進行急劇雙移線試驗，試驗車速為60km/h，試驗駕駛員使車輛不接觸標樁條件下通過雙移線通道，估計輸入如圖4(a)所示，試驗測量值、CKF算法和IMM-CKF算法的估計結果如圖4(b)～圖4(d)所示。該試驗工況用于驗證算法在車輛狀態(tài)快速改變時，對車輛的縱向速度、側向速度和橫擺角速度的估計精度。結果表明:IMM-CKF算法對縱向速度和橫擺角速度的估計均取得較好的效果，但CKF算法對二者的估計存在小幅度的波動；對于側向速度，IMM-CKF算法在轉向盤轉動過程中的估計結果與試驗測量值之間存在一定幅值的偏差，而CKF算法的估計結果存在較大的波動。

4.3 正弦延遲試驗

在干燥、平坦而清潔的水泥試驗場進行正弦延遲試驗，試驗車輛以(80±2)km/h的速度勻速直線行駛，待車速穩(wěn)定后轉向機器人啟動工作程序，以0.7Hz的頻率、180°的峰值進行正弦-停頓模式的轉向盤輸入，在負波峰位置保持500ms的停頓，估計輸入如圖5(a)所示，試驗測量值、CKF算法和IMMCKF算法的估計結果如圖5(b)～圖5(d)所示。結果表明:對縱向速度和橫擺角速度，IMM-CKF算法均取得較好的估計效果，而CKF算法對二者的估計結果在轉向盤保持階段存在一定幅值的波動；對側向速度，IMM-CKF算法在其峰值處的估計值與試驗測量值之間存在較小幅度的偏差，但總體的估計結果滿足工程需要，而CKF算法對其估計結果在轉向盤轉動過程中與試驗測量值之間存在一定幅值的偏差，在轉向盤保持過程中存在一定幅值的波動。

圖5 正弦延遲試驗的估計結果對比分析

上述3個試驗工況下，在車輛速度較大時執(zhí)行快速轉向動作，將導致較大的車輛側傾運動和輪胎進入非線性工作區(qū)域(車輛側向加速度已達到路面附著極限)，從而引起車輛參數(shù)攝動和模型誤差增大。這些擾動會引起系統(tǒng)過程噪聲和觀測噪聲的不確定性，相對于CKF算法，IMM-CKF算法包含不同過程噪聲和觀測噪聲協(xié)方差的子模型，且其融合輸出總是保持跟蹤估計誤差小的子模型輸出，有效抑制系統(tǒng)的不確定過程噪聲和觀測噪聲對車輛狀態(tài)估計結果的影響，提高系統(tǒng)狀態(tài)量的估計精度，增加系統(tǒng)的魯棒性。

5 結論

(1)本文中結合容積卡爾曼濾波器與交互式多模型算法，提出交互式多模型容積卡爾曼濾波算法來估計車輛狀態(tài)。經(jīng)試驗驗證，該算法對車輛狀態(tài)的估計具有良好的性能。

(2)將非線性7自由度車輛動力學模型作為標稱模型的IMM-CKF算法可較為準確地估計車輛縱向速度、側向速度和橫擺角速度。特別是在系統(tǒng)過程噪聲和觀測噪聲不確定時，該算法的估計精度依然較高，具有較強的魯棒性。

[1] 沈法鵬，趙又群，孫秋云，等.基于IEKF-APF算法的汽車狀態(tài)估計[J].機械工程學報，2014，50(22):136-141.

[2] ACARMAN T.Observation of vehicle states by using steering wheel angle and wheel angular speeds[C].IEEE International Conference on Vehicular Electronics and Safety.Columbus， OH，2008:189-194.

[3] BEST M C， GORDON T J， DIXON P J.An extended adaptive Kalman filter for real-time state estimation of vehicle handling dynamics[J].Vehicle System Dynamics，2000，34(1):57-75.

[4] ANTONOV S，F(xiàn)EHN A，KUGI A.Unscented Kalman filter for vehicle state estimation[J].Vehicle System Dynamics，2011，49(9):1497-1520.

[5] ZONG Changfu， HU Dan， ZHENG Hongyu.Dual extended Kalman filter for combined estimation of vehicle state and road friction[J].Chinese Journal of Mechanical Engineering，2013，26(2):313-324.

[6] BEATRIZ L B，MARIA J L，ANTONIO G，et al.Sideslip angle estimator based on ANFIS for vehicle handling and stability[J].Journal of Mechanical Science and Technology，2015，29(4):1473-1481.

[7] SUN F，HUANG X H，JOACHIM R，et al.Vehicle state estimation for anti-lock control with nonlinear observer[J].Control Engineering Practice，2015，43(2015):69-84.

[8] MARIO H，JOSKO D，DANIJEL P，et al.Adaptive EKF-based estimator of sideslip angle using fusion of inertialsensors and GPS[C].SAE Paper 2011-01-0953.

[9] FERNANDEZ-PRADES C，VILA-VALLS J.Bayesian nonlinear filtering using quadrature and cubature rules applied to sensor data fusion for positioning[C].IEEE International Conference on CommunicationsRadar Science and Technology， Castelldefels， Spain，2010:1-5.

[10] GUO K H，LU D.UniTire:unified tire model for vehicle dynamic simulation[J].Vehicle System Dynamics，2007，45(S1):79-99.

[11] 劉錚.UKF算法及其改進算法的研究[D].長沙:中南大學，2009.

[12] ARASARATNAM I，HAYKIN S.Cubature Kalman filters[J].IEEE Transactions on Automatic Control，2009，54(6):1254-1269.

[13] LI X R，JILKOV V P.A survey of maneuvering target trackingpart five:multiple-model methods[J].IEEE Transactions on Aerospace and Electronic Systems，2005，41(4):1225-1321.

[14] XIONGK，ZHANGH Y，CHEN CW.Performance evaluation of UKF-based nonlinear filtering[J].Automatica，2006，42(2006):261-270.

[15] WU Y X，HU D W，HU X P.Comments on“Performance evaluation of UKF-based nonlinear filtering”[J].Automatica，2007，43(2007):567-568.

[16] XIONG K，ZHANG H Y，CHAN C W.Author's reply to“Comments on‘Performance evaluation of UKF-based nonlinear filtering’”[J].Automatica，2007，43(3):569-570.

Vehicle State Estimation Based on Interactive Multiple Model and Cubature Kalman Filter

Zhang Jiaxu1，2＆ Li Jing1
1.Jilin University， State Key Laboratory of Automotive Simulation and Control， Changchun 130022；2.Research and Development Center， China FAWGroup Corporation， Changchun 130011

A 7-DOF vehicle dynamics model with time-varying noise statistical characteristics is established based on UniTire model.For the unknown system statistical characteristics of state noise and observation noise，a vehicle state estimation algorithm based on interactive multiple model(IMM)and cubature Kalman filter(CKF)is proposed.The algorithm adopts the vehicle dynamics model with different system statistical characteristics of state noise and observation noise as model set of IMM algorithm and uses CKF to estimate the vehicle state of each submodel to make fusion output results constantly track the sub-model output with small estimation error.Finally，the measured data of several driving conditions under real vehicle test environment to conduct off-line verification on IMM-CKF algorithm with the results compared with that using CKF estimation.The outcomes show that the estimation performance of IMM-CKF algorithm is superior to that of CKF.

10.19562/j.chinasae.qcgc.2017.09.001

?國家自然科學基金(51275206)資助。

原稿收到日期為2016年9月20日。

張家旭，博士研究生，E-mail:zhjx_686＠163.com。