馮雪麗 顏伏伍 胡杰

摘 要： 汽車(chē)電子控制系統(tǒng)對(duì)動(dòng)態(tài)測(cè)速提出較高的要求，使用位差法和擴(kuò)展卡爾曼濾波融合算法用于GPS測(cè)速比GPS位置差分測(cè)速方法具有更高的精度，但在汽車(chē)快速轉(zhuǎn)彎時(shí)誤差較大。為此通過(guò)有限差分運(yùn)算獲取濾波驗(yàn)前、驗(yàn)后誤差協(xié)方差矩陣，增強(qiáng)濾波過(guò)程的收斂性，再以有限差分?jǐn)U展卡爾曼濾波算法對(duì)汽車(chē)實(shí)時(shí)位置進(jìn)行運(yùn)算以獲取實(shí)時(shí)車(chē)速。實(shí)驗(yàn)結(jié)果表明，相比擴(kuò)展卡爾曼濾波GPS測(cè)速方法，該文算法具有更高的測(cè)速精度。

關(guān)鍵詞： 汽車(chē)電子控制系統(tǒng)； 全球定位系統(tǒng)； 動(dòng)態(tài)測(cè)速； 擴(kuò)展卡爾曼濾波算法； 有限差分運(yùn)算

中圖分類(lèi)號(hào)： TN61?34 文獻(xiàn)標(biāo)識(shí)碼： A 文章編號(hào)： 1004?373X（2016）08?0030?03

Application of improved extended Kalman filtering algorithm in

velocity determination with GPS

FENG Xueli1， 2， YAN Fuwu1， HU Jie1

（1. Automotive Engineering Institute， Wuhan University of Technology， Wuhan 430070， China；

2. Mechanical and Electrical Engineering College， Hangzhou Polytechnic， Hangzhou 311402， China）

Abstract： Dynamic velocity determination had to meet the higher requirement from the vehicle electronic control system. The algorithm combining finite?difference algorithm with extended Kalman filtering algorithm has higher accuracy in GPS velocity determination than GPS location difference velocity determination method， but there is a larger measuring error during the fast turning of car. The finite?difference operation is adopted to achieve the priori error covariance matrix and the posterior error covariance matrix of filtering to enhance the convergence of the filtering process， and then the finite?difference extended Kalman filtering algorithm is used to calculate cars real?time location， so as to get the real?time speed of the vehicle. The experiment results show that the improved algorithm has higher measuring accuracy than that of the extended Kalman filtering velocity determination algorithm.

Keywords： vehicle electronic control system； global positioning system； dynamic velocity determination； extended Kalman filtering algorithm； finite?difference operation

近年來(lái)由于信息技術(shù)、計(jì)算機(jī)技術(shù)和電子技術(shù)的快速發(fā)展和應(yīng)用普及，汽車(chē)電子控制技術(shù)得到長(zhǎng)足的發(fā)展，在智能化、網(wǎng)絡(luò)化、控制精度、控制范圍等領(lǐng)域均取得了一定的突破性成果[1]，進(jìn)一步提升了現(xiàn)代汽車(chē)的發(fā)展水平；但同時(shí)汽車(chē)電子控制技術(shù)的智能化、網(wǎng)絡(luò)化和集成化也對(duì)汽車(chē)狀態(tài)的測(cè)量提出了更高效更快捷更準(zhǔn)確的要求[2]。汽車(chē)行駛過(guò)程中的動(dòng)態(tài)參數(shù)如加速度、車(chē)速、質(zhì)心側(cè)偏角、橫擺角速度等不僅是汽車(chē)性能評(píng)估的重要因素，還是汽車(chē)電子控制的重要參數(shù)，對(duì)上述參數(shù)的低成本、高精度的測(cè)量是提升汽車(chē)電子控制水平的關(guān)鍵。目前應(yīng)用于車(chē)輛動(dòng)態(tài)測(cè)速的方法[3]主要以基于GPS動(dòng)態(tài)測(cè)速方法為主，包含GPS多普勒頻移測(cè)速、GPS差分測(cè)速和GPS擴(kuò)展卡爾曼濾波測(cè)速，但都存在一定不足，前兩種方法電路復(fù)雜且成本高，不利于產(chǎn)品的小型化和推廣，GPS擴(kuò)展卡爾曼濾波測(cè)速雖然成本相對(duì)較低，但在車(chē)輛快速轉(zhuǎn)彎時(shí)測(cè)速準(zhǔn)確性有待提高[4]。為此本文提出了一種使用有限差分運(yùn)算改進(jìn)擴(kuò)展卡爾曼濾波算法用于車(chē)輛動(dòng)態(tài)測(cè)速，以提高算法在車(chē)輛快速轉(zhuǎn)彎時(shí)的測(cè)速性能，并進(jìn)行實(shí)驗(yàn)驗(yàn)證。

1 GPS差分測(cè)速方法

使用GPS可以快捷、可靠、經(jīng)濟(jì)地測(cè)量運(yùn)動(dòng)中物體的動(dòng)態(tài)速度，測(cè)量準(zhǔn)確性較高，精度可至毫米級(jí)。

其中較為簡(jiǎn)單的GPS測(cè)速方法是GPS位置差分法[5]，坐標(biāo)系采用NED（North East Down）坐標(biāo)系，以汽車(chē)質(zhì)心為O點(diǎn)，有三條坐標(biāo)軸，分別是N軸、E軸和D軸，分別指向地理坐標(biāo)系的北向、東向和地心方向，該方法建立的模型是將車(chē)輛行駛等效為地表平面上的二維運(yùn)動(dòng)，將地心方向上的運(yùn)動(dòng)等效為噪聲，在車(chē)輛上使用GPS設(shè)置獲取被測(cè)車(chē)輛的經(jīng)度、緯度、速度、方向角和GPS時(shí)間等數(shù)據(jù)參數(shù)[6]，在[t-Δt]時(shí)刻獲取的車(chē)輛位置為[sk-1]，在[t+Δt]時(shí)刻獲取的車(chē)輛位置為[sk+1]。其中[sk-1]，[sk+1]都是位置向量，可得[t]時(shí)刻車(chē)輛速度[Vk]為：

[Vk=12?Δtsk+1-sk-1] （1）

式中，[Δt]是GPS設(shè)備的采樣時(shí)間，其值為GPS設(shè)備工作頻率的倒數(shù)，單位為s。

在式（1）的基礎(chǔ)上可得汽車(chē)在[t]時(shí)刻的北向速度[Vk，N]和東向速度[Vk，E]，如下所示：

[Vk，N=Vk?cosαVk，E=Vk?sinα] （2）

式中，[α]是汽車(chē)在[t]時(shí)刻的行駛方向角。這樣可獲得車(chē)輛在任意采樣時(shí)刻的速度、北向速度和東向速度，并可根據(jù)速度獲取車(chē)輛對(duì)應(yīng)的加速度、北向加速度和東向加速度。但是GPS差分測(cè)速度算法抗觀測(cè)噪聲和系統(tǒng)噪聲的能力較差[7]，因此引入擴(kuò)展卡爾曼濾波算法進(jìn)行GPS測(cè)速，以提高測(cè)速算法對(duì)噪聲的抑制能力。

2 有限差分?jǐn)U展卡爾曼濾波GPS測(cè)速算法

2.1 擴(kuò)展卡爾曼濾波測(cè)速算法

基于擴(kuò)展卡爾曼濾波的GPS測(cè)速算法首先需要建立離散非線性模型[8]，如式（3）所示。

[Xk+1，E=fXk，Uk+WYk+1，E=gXk，Uk+ω] （3）

式中：[Xk+1，E]是系統(tǒng)東向狀態(tài)變量；[Yk+1，E]是系統(tǒng)東向輸出結(jié)果；[f?]、[g?]是非線性的傳遞函數(shù)；[ω]、[W]是互不相干的測(cè)量噪聲和系統(tǒng)噪聲，測(cè)量噪聲是由測(cè)量傳感器造成的，系統(tǒng)噪聲是由汽車(chē)自身因素造成的誤差，包含器件振動(dòng)噪聲及路面顛簸引起的噪聲等，相對(duì)應(yīng)的系統(tǒng)方差陣分別是[R、Q]，傳遞函數(shù)的第二個(gè)參數(shù)[Uk]表征控制變量，由于GPS沒(méi)有控制變量，因此[Uk]設(shè)為0。式（3）的是模型的狀態(tài)方程和輸出觀測(cè)方程，其中類(lèi)似地，系統(tǒng)的北向分量和地心分量也由式（3）計(jì)算獲取。

對(duì)兩個(gè)非線性傳遞函數(shù)在[Xk]一階泰勒展開(kāi)，并設(shè)：

[AE=?f?XX=X+kCE=?g?XX=X+k] （4）

式中，[X+k]為GPS設(shè)備采樣時(shí)刻后續(xù)的狀態(tài)值估計(jì)結(jié)果，設(shè)汽車(chē)東向運(yùn)動(dòng)的加速度為[ak，E]，則有：

[ak，E=1ΔtVk，E-Vk-1，E] （5）

同時(shí)設(shè)對(duì)應(yīng)的東向加速度變化率為[ik，E]，則有汽車(chē)東向運(yùn)動(dòng)狀態(tài)分量[Xk，E=Vk，E，ak，E，ik，E]。設(shè)汽車(chē)在[t]時(shí)刻、[t+Δt]時(shí)刻、[t+2Δt]時(shí)刻對(duì)應(yīng)的行駛距離分別為[Sk]，[Sk+1]，[Sk+2]，可得[Sk+1]，[Sk+2]的計(jì)算公式為：

[Sk+1=Sk+Vkt+0.5?ak?t2Sk+2=Sk+1+Vk+1t+0.5?ak+1?t2] （6）

式中，[t+2Δt]時(shí)刻對(duì)應(yīng)的行駛速度[Vk+2]為：

[Vk+2=Vk+1+ak+1t+0.5?ik+1?t2] （7）

綜上可得式（3）中汽車(chē)東向速度狀態(tài)方程為：

[Xk+1，E=Vk，Eak，E?t0.5?ik，E?t20ak，Eik，E?t00ik，E+W] （8）

將其代入式（4），可得：

[AE=1t0.5?t201t001] （9）

[W]是汽車(chē)的系統(tǒng)噪聲，使用噪聲方差矩陣[Q]表示，[Q]是常系數(shù)矩陣，與GPS設(shè)備采樣頻率相關(guān)。

[Q=t520t48t36t48t36t22t36t22t×0.09] （10）

在式（9）、式（10）中，[t]是GPS設(shè)備的采樣周期，其值為GPS設(shè)備工作頻率的倒數(shù)，單位為s。

同樣地，結(jié)合式（3）～式（7）有[CE=1，1，1，]觀測(cè)噪聲[ω]由GPS設(shè)備決定，隨設(shè)備變化而變化[9]。

最后使用擴(kuò)展卡爾曼濾波算法對(duì)汽車(chē)東向速度的估計(jì)方程為：

[Pk，Ek-1=AE?Pk-1，Ek-1?ATE+Q （11a）Xk，Ek=Xk，Ek-1+Kk，g?Yk，E-CE?Xk，Ek-1 （11b）Kk，g=Pk，Ek-1?CTECE?Pk，Ek-1?CTE+R （11c）Pk，Ek=I-Kk，g?CE?Pk，Ek-1 （11d）] （11）

結(jié)合式（3）和式（11），可得使用擴(kuò)展卡爾曼濾波算法獲取的t時(shí)刻汽車(chē)東向狀態(tài)分量最優(yōu)估計(jì)值[Xk，Ek]，同樣地可得汽車(chē)北向狀態(tài)分量最優(yōu)估計(jì)值[Xk，Nk]，從而得到汽車(chē)在該時(shí)刻的水平速度[Xkk]。

使用擴(kuò)展卡爾曼濾波算法對(duì)GPS位差測(cè)速算法進(jìn)行改進(jìn)可有效抑制噪聲對(duì)測(cè)速精度的影響并提高測(cè)速精度，但是擴(kuò)展卡爾曼濾波算法在收斂速度方面[10]存在一定的問(wèn)題，在汽車(chē)快速轉(zhuǎn)彎時(shí)誤差較大，因此使用有限差分算法進(jìn)行優(yōu)化以進(jìn)一步提高算法性能。

2.2 有限差分改進(jìn)

有限差分的原理是利用一階中心差分法和多項(xiàng)式近似技術(shù)求解非線性函數(shù)的偏導(dǎo)數(shù)，可實(shí)現(xiàn)二階非線性近似功能[11]。設(shè)[y=gx]為非線性函數(shù)，該函數(shù)在[x=x]處的二階有限中心差分展開(kāi)為：

[gx≈gx+gDD′xx-x+fDD″x2！x-x2] （12）

式中：[gx]為函數(shù)[gx]在[x=x]處的函數(shù)值；[gDD′x]是使用差分方法計(jì)算出的展開(kāi)式一階項(xiàng)系數(shù)，其計(jì)算如下：

[gDD′x=gx+l-gx-l2l] （13）

式中，[l]是區(qū)間長(zhǎng)度參數(shù)，[fDD″x2！]是展開(kāi)式二次項(xiàng)系數(shù)，同樣使用差分方法計(jì)算獲取，其計(jì)算如下：

[gDD″x=gx+l+gx-l-2gxl2] （14）

對(duì)式（12）進(jìn)行泰勒級(jí)數(shù)展開(kāi)變?yōu)椋?/p>

[gx=gx+g′xx-x+f″x2！x-x2+ g3x3！h2+g5x5！h4+…x-x+ g4x4！h2+g6x6！h4+…x-x2] （15）

將式（13）、式（14）與式（15）后兩項(xiàng)比較發(fā)現(xiàn)后兩項(xiàng)與高階項(xiàng)對(duì)應(yīng)，并由[h]來(lái)控制精度，通過(guò)驗(yàn)證比較，在非線性函數(shù)中使用中心差分代替一階、二階導(dǎo)數(shù)后的精度比代替前的精度要高，這也是將有限差分算法用于改進(jìn)擴(kuò)展卡爾曼濾波算法用于GPS測(cè)速的原因所在，可有效處理車(chē)輛高速轉(zhuǎn)變過(guò)程中的非線性變化得到更高的測(cè)速精度。有限差分?jǐn)U展卡爾曼濾波算法步驟為：

（1） 初始化，獲取GPS測(cè)速模型的狀態(tài)方程和輸出觀測(cè)方程、[ω]和[W]的系統(tǒng)方差陣分別是[R，Q]及[AE]，[CE]，并引進(jìn)矩陣：

[Pk，Ek=SXSTXPk+1，Ek=SXSTXQ=SWSTWR=SVSTV] （16）

（2） 進(jìn)行狀態(tài)一步預(yù)測(cè)，即式（11a）；

（3） 對(duì)汽車(chē)東向、北向的誤差協(xié)方差矩陣進(jìn)行預(yù)測(cè)，此處使用一階中心差分法獲取誤差協(xié)方差矩陣，東向分量的誤差協(xié)方差矩陣為：

[SXi，j=fiXk，Ek-1+hAE，i-fiXk，Ek-1-hAE，i2h] （17）

式中：[AE，i]由步長(zhǎng)的調(diào)節(jié)系數(shù)[h]調(diào)整；[h]可按噪聲分布規(guī)律設(shè)置。

（4） 進(jìn)行量測(cè)預(yù)測(cè)，如式（18）所示。

[Zk，Ek-1=CE?Xk，Ek-1] （18）

（5） 計(jì)算濾波增益，如式（19）所示。

[K=k，Ek-1CTECEPk，Ek-1CTE+R-1] （19）

（6） 進(jìn)行濾波估，如式（11b）所示。

（7） 進(jìn)行誤差協(xié)方差陣估計(jì)，如式（11d）所示。

（8） 求解估計(jì)誤差的Choleskey分解，并代入到式（11）中進(jìn)行計(jì)算獲取t時(shí)刻汽車(chē)東向狀態(tài)分量最優(yōu)估計(jì)值[Xk，Ek]，同樣地可得汽車(chē)北向狀態(tài)分量最優(yōu)估計(jì)值[Xk，Nk]，從而得到汽車(chē)在該時(shí)刻的水平速度[Vkk=V2k，Nk+V2k，Ek]。

3 實(shí)驗(yàn)及分析

為對(duì)本文的算法性能進(jìn)行分析驗(yàn)證，使用Vkel的GPS模塊獲取車(chē)輛的GPS數(shù)據(jù)，并分別使用擴(kuò)展卡爾曼濾波GPS測(cè)速算法模塊和有限差分?jǐn)U展卡爾曼濾波GPS測(cè)速算法模塊接收GPS數(shù)據(jù)并進(jìn)行測(cè)速；對(duì)前文中的部分參數(shù)進(jìn)行設(shè)置，對(duì)調(diào)節(jié)系數(shù)[h]，因統(tǒng)計(jì)噪聲屬于白噪聲[9]，因此設(shè)定[h=3]；GPS設(shè)備采樣頻率為1 Hz，對(duì)應(yīng)的采樣時(shí)間[Δt=1 s]。車(chē)輛行駛中設(shè)置多次高速轉(zhuǎn)彎，將兩種算法計(jì)算的車(chē)輛行駛水平速度分別與水平速度真值進(jìn)行比較，得到的速度測(cè)量誤差如圖1所示。

相比擴(kuò)展卡爾曼濾波GPS測(cè)速算法，改進(jìn)后算法測(cè)量誤差更小，其均方根誤差是0.051 9，而擴(kuò)展卡爾曼濾波GPS測(cè)速算法的均方根誤差為0.231 8，在車(chē)輛多次高速轉(zhuǎn)變情況下，算法改進(jìn)后均方根誤差得到明顯提高。改進(jìn)后算法平均耗時(shí)0.148 1 s，而改進(jìn)前算法平均耗時(shí)0.117 4 s，可見(jiàn)有限差分?jǐn)U展卡爾曼濾波GPS測(cè)速算法使用有限差分運(yùn)算獲取濾波驗(yàn)前、驗(yàn)后誤差協(xié)方差矩陣改進(jìn)擴(kuò)展卡爾曼濾波算法增加了算法復(fù)雜度從而需要消耗更長(zhǎng)的運(yùn)算時(shí)間，但仍在系統(tǒng)設(shè)計(jì)需求許可范圍內(nèi)。

4 結(jié) 語(yǔ)

本文針對(duì)位差法和擴(kuò)展卡爾曼濾波融合算法用于GPS測(cè)速時(shí)汽車(chē)快速轉(zhuǎn)彎時(shí)測(cè)速誤差較大的問(wèn)題，使用有限差分運(yùn)算獲取濾波驗(yàn)前、驗(yàn)后誤差協(xié)方差矩陣改進(jìn)擴(kuò)展卡爾曼濾波算法增強(qiáng)了算法的收斂性；從而提高了算法在車(chē)輛高速轉(zhuǎn)彎時(shí)的測(cè)速精度，使速度測(cè)量值的均方根誤差減小，雖然測(cè)速響應(yīng)時(shí)間增大但仍在系統(tǒng)設(shè)計(jì)需求允許范圍內(nèi)；表明基于有限差分?jǐn)U展卡爾曼濾波的GPS動(dòng)態(tài)測(cè)速算法具有更高的性能和應(yīng)用價(jià)值。

參考文獻(xiàn)

[1] 郭可怡.汽車(chē)電子控制裝置中計(jì)算機(jī)檢測(cè)控制技術(shù)的應(yīng)用[J].電子技術(shù)與軟件工程，2015（6）：258.

[2] 周娟英.計(jì)算機(jī)檢測(cè)控制技術(shù)在汽車(chē)電子控制系統(tǒng)中的應(yīng)用分析[J].自動(dòng)化與儀器儀表，2014，7（4）：107?108.

[3] 杜娟，孫中豪，姚飛娟，等.GPS測(cè)速方法與精度分析[J].全球定位系統(tǒng)，2012，37（6）：13?16.

[4] 楊龍，劉焱雄，周興華，等.GPS測(cè)速精度分析與應(yīng)用[J].海洋測(cè)繪，2007，27（2）：26?29.

[5] 徐宏宇，王浩，王爾申.基于擴(kuò)展卡爾曼濾波的GPS定位數(shù)據(jù)處理方法研究[J].科學(xué)技術(shù)與工程，2012，12（31）：8137?8142.

[6] 余婭，李軍.基于車(chē)載GPS導(dǎo)航的非線性動(dòng)態(tài)濾波算法[J].計(jì)算機(jī)與數(shù)字工程，2012，40（11）：25?28.

[7] WU F M， YANG Y X， ZHANG L P. A new fusion scheme for accuracy enhancement and error modification in GPS/INS tight integrated navigation [J]. Survey review， 2012， 44（326）： 208?214.

[8] 楊宏，李亞安，李國(guó)輝.一種改進(jìn)擴(kuò)展卡爾曼濾波新方法[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（19）：18?20.

[9] 潘盛輝，郝東亞，張興達(dá).基于擴(kuò)展卡爾曼濾波的GPS動(dòng)態(tài)測(cè)速算法[J].信息技術(shù)，2014，13（12）：13?16.

[10] 何棟煒，彭俠夫，蔣學(xué)程，等.永磁同步電機(jī)的改進(jìn)擴(kuò)展卡爾曼濾波測(cè)速算法[J].西安交通大學(xué)學(xué)報(bào)，2011，45（10）：59?64.

[11] 巫春玲，韓崇昭.用于彈道目標(biāo)跟蹤的有限差分?jǐn)U展卡爾曼濾波算法[J].西安交通大學(xué)學(xué)報(bào)，2008，42（2）：143?146.