基于擴(kuò)展卡爾曼濾波的車(chē)輛狀態(tài)可靠估計(jì)

李 旭 宋 翔 張為公

(東南大學(xué)儀器科學(xué)與工程學(xué)院，南京210096)

車(chē)輛運(yùn)行狀態(tài)估計(jì)是汽車(chē)動(dòng)力學(xué)中的重要問(wèn)題，是實(shí)現(xiàn)汽車(chē)主動(dòng)安全控制系統(tǒng)的關(guān)鍵技術(shù)之一.通過(guò)車(chē)輛運(yùn)行狀態(tài)估計(jì)能夠確定汽車(chē)在行駛狀態(tài)下的縱向前進(jìn)速度、橫擺角速度、側(cè)向速度以及質(zhì)心側(cè)偏角等關(guān)鍵狀態(tài)變量［1］，從而控制這些參數(shù)以實(shí)現(xiàn)汽車(chē)主動(dòng)安全控制.

這些狀態(tài)變量可以用傳感器直接測(cè)量或通過(guò)簡(jiǎn)單的數(shù)學(xué)推算獲取，但直接測(cè)量必須依賴(lài)光電五輪儀或高精度GPS等價(jià)格昂貴的高精度傳感器［2-3］才能保證其精度，成本高且需要特定的安裝固定方式，難以大范圍推廣應(yīng)用.以GPS直接測(cè)量為例，需利用單個(gè)GPS測(cè)量得到的航跡角，減去雙天線(xiàn)GPS測(cè)量得到的車(chē)輛航向角來(lái)獲得車(chē)輛滑移角，所需成本較高;而利用慣性、輪速傳感器等低成本的車(chē)載傳感器［4-6］所測(cè)量的運(yùn)動(dòng)學(xué)信號(hào)進(jìn)行簡(jiǎn)單的積分等數(shù)學(xué)推算來(lái)間接獲取車(chē)輛運(yùn)行狀態(tài)，雖然成本較低，卻由于傳感器精度較差且推算處理過(guò)于簡(jiǎn)單而存在著較大的測(cè)量誤差，因而影響了控制效果.

近年來(lái)，為了兼顧成本和精度，車(chē)輛模型法引起了廣泛的重視.該方法通過(guò)對(duì)汽車(chē)的運(yùn)行過(guò)程進(jìn)行動(dòng)力學(xué)建模，同時(shí)將低成本的車(chē)載傳感器信息作為觀測(cè)信息，進(jìn)而利用適當(dāng)?shù)臑V波估計(jì)算法實(shí)現(xiàn)對(duì)汽車(chē)運(yùn)行狀態(tài)的估計(jì).車(chē)輛模型法不僅可實(shí)現(xiàn)對(duì)難于直測(cè)量的估計(jì)，擴(kuò)大狀態(tài)估計(jì)的維數(shù)，還可提高有關(guān)直測(cè)量的精度，同時(shí)成本較低，國(guó)內(nèi)外學(xué)者在該方面進(jìn)行了許多研究［7-10］.但已有的研究主要利用對(duì)整車(chē)或輪胎做了較多線(xiàn)性化假定的動(dòng)力學(xué)模型［7-9］，這些模型在車(chē)輛較平穩(wěn)運(yùn)行時(shí)能獲得較好的估計(jì)效果和精度，但在需要頻繁轉(zhuǎn)向以及加減速的較高機(jī)動(dòng)運(yùn)行狀況下由于難以反映車(chē)輛的實(shí)際非線(xiàn)性動(dòng)力學(xué)行為導(dǎo)致估計(jì)精度較低.

針對(duì)汽車(chē)較高機(jī)動(dòng)運(yùn)行狀況，本文提出了一種基于改進(jìn)擴(kuò)展卡爾曼濾波(EKF)的車(chē)輛運(yùn)行狀態(tài)估計(jì)方法.該方法利用非線(xiàn)性整車(chē)動(dòng)力學(xué)模型和輪胎模型建立系統(tǒng)狀態(tài)模型，同時(shí)利用低成本的車(chē)載輪速和方向盤(pán)轉(zhuǎn)角傳感器信息來(lái)建立濾波系統(tǒng)的外部輸入量和觀測(cè)量，進(jìn)而通過(guò)改進(jìn)的擴(kuò)展卡爾曼濾波遞推算法在較高機(jī)動(dòng)環(huán)境下實(shí)現(xiàn)對(duì)車(chē)輛關(guān)鍵運(yùn)行狀態(tài)的準(zhǔn)確、可靠估計(jì).

1 車(chē)輛與輪胎模型

車(chē)速估計(jì)通常使用二自由度線(xiàn)性車(chē)輛模型，該模型中車(chē)輛的縱向前進(jìn)速度被認(rèn)為是定常數(shù)，只適合縱向速度不變或變化緩慢的運(yùn)行情況，而實(shí)際道路車(chē)輛行駛中存在著較為頻繁的加減速和轉(zhuǎn)向，因而線(xiàn)性模型存在較大的建模誤差.本文采用三自由度的汽車(chē)非線(xiàn)性動(dòng)力學(xué)模型，忽略左右輪差異后模型可等效簡(jiǎn)化為相當(dāng)于前、后車(chē)輪被分別集中在汽車(chē)前、后軸中點(diǎn)而構(gòu)成的一個(gè)Bicycle模型，如圖1所示.該模型包括縱向運(yùn)動(dòng)、側(cè)向運(yùn)動(dòng)以及橫擺轉(zhuǎn)動(dòng)3個(gè)自由度，對(duì)車(chē)輛的縱向前進(jìn)速度無(wú)定常數(shù)的限定，故既可適應(yīng)一般機(jī)動(dòng)環(huán)境也可適應(yīng)較高機(jī)動(dòng)環(huán)境下車(chē)輛運(yùn)行狀態(tài)的準(zhǔn)確估計(jì).

據(jù)牛頓力學(xué)，車(chē)輛的動(dòng)力學(xué)模型［11］可描述為

圖1 車(chē)輛動(dòng)力學(xué)模型

式中，vx，vy，ωz分別為汽車(chē)的縱向前進(jìn)速度、側(cè)向速度和橫擺角速度;m和Iz分別為車(chē)輛的質(zhì)量和繞oz軸的轉(zhuǎn)動(dòng)慣量;a，b分別為汽車(chē)前輪和后輪輪軸中心到質(zhì)心的距離;δf為前輪轉(zhuǎn)向角，由方向盤(pán)轉(zhuǎn)角傳感器測(cè)得的方向盤(pán)轉(zhuǎn)角δ除以從方向盤(pán)到前輪的轉(zhuǎn)向傳動(dòng)比來(lái)確定;Cd為空氣阻力系數(shù);Af為車(chē)輛前向面積;ρa(bǔ)為空氣密度;Ftf，F(xiàn)tr分別為作用在單個(gè)前輪和后輪上的縱向力;Fsf，F(xiàn)sr為作用在單個(gè)前輪和后輪上的側(cè)向力.對(duì)于行駛在一般道路交通環(huán)境下的車(chē)輛，通?？蓪⒆饔迷诟鬏喩系膫?cè)向力表示為

式中，Cαf，Cαr分別為前、后輪胎的側(cè)偏剛度;αf，αr分別為前、后輪胎的側(cè)偏角，可表示為

將式(2)、(3)代入式(1)，并考慮到δf通常是小角度(即 sinδf≈ δf，cosδf≈ 1，且忽略二階及以上的高階微量，經(jīng)整理后可得

考慮到車(chē)輛運(yùn)行的高機(jī)動(dòng)特征，線(xiàn)性輪胎模型難以準(zhǔn)確描述較高機(jī)動(dòng)運(yùn)行狀況下輪胎的非線(xiàn)性特征，故輪胎縱向力Ftf和Ftr采用非線(xiàn)性輪胎模型來(lái)確定.魔術(shù)公式輪胎模型是公認(rèn)的擬合精度最高的經(jīng)驗(yàn)輪胎模型，但它是由三角函數(shù)組合而成的復(fù)雜的非線(xiàn)性函數(shù)，且模型中未知因子較多，計(jì)算量較大，不適合實(shí)時(shí)使用.同時(shí)考慮非線(xiàn)性運(yùn)行工況和計(jì)算效率的要求，本文縱向力的估計(jì)采用便于實(shí)時(shí)計(jì)算的非線(xiàn)性Dugoff輪胎模型［12］，即 Ftf和 Ftr通過(guò)下式來(lái)確定:

式中，Ctf和Ctr為單個(gè)前、后輪胎的縱向剛度;isf和isr為前、后輪胎的車(chē)輛縱向滑移率;pf，pr與ft由下式確定:

式中，μ為輪胎和地面間的附著系數(shù);εr為道路附著衰減因子.Fzf和Fzr表示分配到前、后輪軸上的垂向載荷，可按下式確定:

式中，g表示重力加速度.

2 卡爾曼濾波模型的建立與線(xiàn)性化

取狀態(tài)變量 X={vx，ωz，vy}T，系統(tǒng)外輸入向量為 U={δf，F(xiàn)tf，F(xiàn)tr}T，并以輪速傳感器所輸出的信息推算所得的包含較大噪聲的車(chē)輛縱向前進(jìn)速度vx_m和橫擺角速度ωz_m作為系統(tǒng)觀測(cè)量，即Z={vx_m，ωz_m}T，則根據(jù)式(4)可建立系統(tǒng)方程和觀測(cè)方程，對(duì)其按采樣時(shí)間離散化后得

式中，W與V為互不相關(guān)的零均值的系統(tǒng)高斯白噪聲向量和觀測(cè)白噪聲向量，其協(xié)方差矩陣分別為Q與R;γ為系統(tǒng)外輸入對(duì)應(yīng)的零均值高斯白噪聲向量，其協(xié)方差矩陣為Γ(k－1);f為狀態(tài)轉(zhuǎn)移函數(shù)，由(4)式確定為測(cè)量矩陣.系統(tǒng)觀測(cè)量利用后輪軸上2個(gè)輪速傳感器測(cè)得的角速度乘以輪胎半徑得到左后輪和右后輪的車(chē)輪線(xiàn)速度VRL和VRR.從運(yùn)動(dòng)學(xué)角度，圖1所示的車(chē)輛運(yùn)動(dòng)實(shí)際上是一個(gè)平面復(fù)合運(yùn)動(dòng)(縱向運(yùn)動(dòng)、側(cè)向運(yùn)動(dòng)和橫擺轉(zhuǎn)動(dòng)的復(fù)合)，由平面復(fù)合運(yùn)動(dòng)關(guān)系可得

式中，TW為后輪軸上2個(gè)后輪間的輪距.對(duì)式(10)重新整理，則可以獲取系統(tǒng)觀測(cè)量:

3 改進(jìn)卡爾曼濾波遞推

對(duì)于式(9)所描述的系統(tǒng)狀態(tài)方程和測(cè)量方程，可運(yùn)用卡爾曼濾波理論建立濾波遞推估計(jì)過(guò)程.由于狀態(tài)方程為非線(xiàn)性方程，在應(yīng)用卡爾曼濾波計(jì)算時(shí)，需先進(jìn)行線(xiàn)性化處理，將系統(tǒng)方程在附近按泰勒級(jí)數(shù)展開(kāi)(為X的濾波計(jì)算值)，保留一階微量、忽略高階微量后再進(jìn)行濾波遞推計(jì)算，即需按照擴(kuò)展卡爾曼濾波過(guò)程進(jìn)行濾波遞推.具體過(guò)程如下.

1)時(shí)間更新

狀態(tài)一步預(yù)測(cè)方程為

一步預(yù)測(cè)誤差方差陣為

式中，A為f對(duì)X求偏導(dǎo)的雅可比矩陣;B為f對(duì)U求偏導(dǎo)的雅可比矩陣.矩陣A和B的第i行第j列元素 ai，j和 bi，j(i=1，2，3;j=1，2，3)可通過(guò)下面的求導(dǎo)過(guò)程獲得:

2)測(cè)量更新

濾波增益矩陣為

狀態(tài)估計(jì)為

估計(jì)誤差方差陣為

由于上述擴(kuò)展卡爾曼濾波遞推過(guò)程在測(cè)量更新過(guò)程中(即計(jì)算K(k))存在矩陣求逆運(yùn)算，計(jì)算量大且容易造成數(shù)值計(jì)算的不穩(wěn)定.因此，本文在測(cè)量更新時(shí)不直接采用矩陣求逆的方法，而采用標(biāo)量化處理方法［13］.即時(shí)間更新過(guò)程可按照上述濾波過(guò)程進(jìn)行，而測(cè)量更新按以下改進(jìn)的遞推算法進(jìn)行:令，由于觀測(cè)向量維數(shù)為2，故將和R(k)陣分成2塊，即

對(duì)于i從1到2，進(jìn)行2次遞推計(jì)算，即

在上述濾波遞推計(jì)算過(guò)程中，可確定汽車(chē)在每個(gè)時(shí)刻的汽車(chē)縱向前進(jìn)速度vx(k)、橫擺角速度ωz(k)和側(cè)向速度vy(k)，進(jìn)而根據(jù)下式可確定每個(gè)時(shí)刻的質(zhì)心側(cè)偏角:

4 仿真驗(yàn)證與分析

為檢驗(yàn)本文提出的基于改進(jìn)擴(kuò)展卡爾曼濾波的車(chē)輛運(yùn)行狀態(tài)估計(jì)方法的實(shí)際效果，在汽車(chē)動(dòng)力學(xué)仿真軟件CarSim上進(jìn)行了仿真驗(yàn)證實(shí)驗(yàn).

為檢驗(yàn)本文方法在較高機(jī)動(dòng)環(huán)境下的估計(jì)效果，仿真實(shí)驗(yàn)中設(shè)置汽車(chē)的方向盤(pán)轉(zhuǎn)角δ按幅值60°的正弦規(guī)律變化，同時(shí)汽車(chē)的縱向前進(jìn)速度也在不斷地做加速、制動(dòng)減速和勻速等變化，方向盤(pán)轉(zhuǎn)角和縱向前進(jìn)速度隨時(shí)間的具體變化過(guò)程如圖2所示，仿真時(shí)長(zhǎng)設(shè)置為100 s.實(shí)驗(yàn)車(chē)輛是一個(gè)前輪轉(zhuǎn)向的四輪車(chē)，主要參數(shù)如下:m=960 kg，Iz=1 382 kg·m2，a=0.948 m，b=1.422 m，Cαf=Cαr=25.692 kN/rad，Tw=1.390 m.設(shè)定 4 個(gè)車(chē)輪的線(xiàn)速度的測(cè)量噪聲均為均值是0、標(biāo)準(zhǔn)差是0.04 m/s的高斯白噪聲，方向盤(pán)轉(zhuǎn)角傳感器的測(cè)量噪聲為均值是0、標(biāo)準(zhǔn)差是8.73×10－2rad的高斯白噪聲.卡爾曼濾波的系統(tǒng)零均值高斯白噪聲的標(biāo)準(zhǔn)差分別為 0.05 m/s，0.02 rad/s，0.05 m/s;3 個(gè)外輸入的零均值高斯白噪聲的標(biāo)準(zhǔn)差分別為8.73×10－3rad，20 N，20 N;2 個(gè)觀測(cè)量的零均值高斯白噪聲的標(biāo)準(zhǔn)差分別為0.08 m/s和0.12 rad/s.

圖2 車(chē)輛方向盤(pán)轉(zhuǎn)角及縱向前進(jìn)速度的仿真設(shè)置

表1列出了對(duì)于整個(gè)過(guò)程利用直接推算法和本文所提出方法推算車(chē)輛運(yùn)行狀態(tài)的統(tǒng)計(jì)結(jié)果對(duì)比.直接推算法是指通過(guò)輪速傳感器直接測(cè)量換算得到的縱向前進(jìn)速度和橫擺角速度.圖3(a)給出了本文方法估計(jì)的質(zhì)心側(cè)偏角β的結(jié)果曲線(xiàn)以及相應(yīng)的Carsim的參考輸出值，圖3(b)給出了本文方法估計(jì)的β相對(duì)于Carsim輸出的β參考值的誤差曲線(xiàn).

表1 2種方法推算效果的對(duì)比

由表1和圖3可看出，本文方法相對(duì)于直接推算法在縱向前進(jìn)速度和橫擺角速度的推算精度方面有大幅的提高.同時(shí)，在難以直測(cè)的側(cè)向速度和質(zhì)心側(cè)偏角的估計(jì)方面，本文方法也具有很高的精度.因此，即使在較高機(jī)動(dòng)運(yùn)行環(huán)境下，本文提出的基于改進(jìn)擴(kuò)展卡爾曼濾波的車(chē)輛運(yùn)行狀態(tài)估計(jì)方法也能夠準(zhǔn)確地估計(jì)出車(chē)輛縱向前進(jìn)速度、側(cè)向速度、橫擺角速度以及質(zhì)心側(cè)偏角等信息，這些信息可滿(mǎn)足有關(guān)汽車(chē)主動(dòng)安全控制的需要.

圖3 質(zhì)心側(cè)偏角估計(jì)結(jié)果及誤差

5 結(jié)語(yǔ)

本文研究了基于擴(kuò)展卡爾曼濾波的車(chē)輛狀態(tài)估計(jì)方法.首先在非線(xiàn)性的整車(chē)動(dòng)力學(xué)模型和輪胎縱向力模型的基礎(chǔ)上，建立了汽車(chē)運(yùn)行過(guò)程的系統(tǒng)模型.然后，根據(jù)卡爾曼濾波理論，以低成本的車(chē)載輪速傳感器信息作為系統(tǒng)觀測(cè)量，通過(guò)改進(jìn)的卡爾曼濾波遞推算法高精度地推算出汽車(chē)前進(jìn)速度、側(cè)向速度、橫擺角速度以及質(zhì)心側(cè)偏角等車(chē)輛關(guān)鍵運(yùn)行狀態(tài).仿真試驗(yàn)證明，該方法不僅可顯著提高汽車(chē)縱向前進(jìn)速度和橫擺角速度等直測(cè)量的精度，而且可實(shí)現(xiàn)對(duì)質(zhì)心側(cè)偏角、側(cè)向速度等難于直測(cè)量的準(zhǔn)確估計(jì).在今后的研究中，對(duì)于輪胎處于高度飽和的非線(xiàn)性區(qū)域時(shí)的滑移角準(zhǔn)確估計(jì)還有待深入.

References)

［1］余卓平，高曉杰.車(chē)輛行駛過(guò)程中的狀態(tài)估計(jì)問(wèn)題綜述［J］.機(jī)械工程學(xué)報(bào)，2009，45(5):20-33.Yu Zhuoping，Gao Xiaojie.Review of vehicle state estimation problem under driving situation［J］.Journal of Mechanical Engineering，2009，45(5):20-33.

［2］Leung K T，Whildborne J F，Purdy D，et al.A review of ground vehicle dynamic state estimations utilising GPS/INS ［J］.Vehicle Syst Dyn，2011，49(1/2):29-58.

［3］Bevly D M，Ryu J H，Gerdes J C.Integrating INS sensors with GPS measurements for continuous estimation of vehicle sideslip，roll，and tire cornering stiffness［J］.IEEE Trans Intell Transport Syst，2006，7(4):483-493.

［4］Chumsamutr R，F(xiàn)ujioka T，Abe M.Sensitivity analysis of side-slip angle observer based on a tire model［J］.Vehicle Syst Dyn，2006，44(7):513-527.

［5］Rajamani R，Grogg J A，Lew J Y.Development and experimental evaluation of a slip angle estimator for vehicle stability control［J］.IEEE Trans Control Syst Technol，2009，17(1):78-88.

［6］Chen B C，Hsieh F C.Sideslip angle estimation using extended Kalman filter［J］.Vehicle Syst Dyn，2008，46(1):353-364.

［7］Baffet G，Charara A，Lechner D.Estimation of vehicle sideslip，tire force and wheel cornering stiffness［J］.Control Eng Pract，2009，17(11):1255-1264.

［8］Tuononen A J.Vehicle lateral state estimation based on measured tyre forces［J］.Sensors，2009，9(11):8761-8775.

［9］林棻，趙又群.汽車(chē)側(cè)偏角估計(jì)方法比較［J］.南京理工大學(xué)學(xué)報(bào):自然科學(xué)版，2009，30(1):122-126.Lin Fen，Zhao Youqun.Comparison of methods for estimating vehicle side slip angle［J］.Journal of Nanjing University of Science and Technology:Natural Science，2009，30(1):122-126.

［10］Zong C F，Hu D，Zheng H Y.Dual extended Kalman filter for combined estimation of vehicle state and road friction［J］.Chinese Journal of Mechanical Engineering，2013，26(2):313-324.

［11］Smith D E，Starkey J M.Effects of model complexity on the performance of automated vehicle steering controllers:model development，validation and comparison［J］.Vehicle Syst Dyn，1995，24(2):163-181.

［12］Li L，Wang F Y，Zhou Q Z.Integrated longitudinal and lateral tire/road friction modeling and monitoring for vehicle motion control［J］.IEEE Trans Intelli Transport Syst，2006，7(1):1-19.

［13］Farrell J A，Givargis T D，Barth M J.Real-time differential carrier phase GPS-aided INS［J］.IEEE Trans Control Syst Technol，2000，8(4):709-720.