基于改進的擴展卡爾曼濾波在GPS測速中的應用

馮雪麗 顏伏伍 胡杰

摘 要： 汽車電子控制系統對動態測速提出較高的要求，使用位差法和擴展卡爾曼濾波融合算法用于GPS測速比GPS位置差分測速方法具有更高的精度，但在汽車快速轉彎時誤差較大。為此通過有限差分運算獲取濾波驗前、驗后誤差協方差矩陣，增強濾波過程的收斂性，再以有限差分擴展卡爾曼濾波算法對汽車實時位置進行運算以獲取實時車速。實驗結果表明，相比擴展卡爾曼濾波GPS測速方法，該文算法具有更高的測速精度。

關鍵詞： 汽車電子控制系統； 全球定位系統； 動態測速； 擴展卡爾曼濾波算法； 有限差分運算

中圖分類號： TN61?34 文獻標識碼： A 文章編號： 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

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

1 GPS差分測速方法

使用GPS可以快捷、可靠、經濟地測量運動中物體的動態速度，測量準確性較高，精度可至毫米級。

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

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

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

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

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

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

2 有限差分擴展卡爾曼濾波GPS測速算法

2.1 擴展卡爾曼濾波測速算法

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

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

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

對兩個非線性傳遞函數在[Xk]一階泰勒展開，并設：

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

式中，[X+k]為GPS設備采樣時刻后續的狀態值估計結果，設汽車東向運動的加速度為[ak，E]，則有：

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

同時設對應的東向加速度變化率為[ik，E]，則有汽車東向運動狀態分量[Xk，E=Vk，E，ak，E，ik，E]。設汽車在[t]時刻、[t+Δt]時刻、[t+2Δt]時刻對應的行駛距離分別為[Sk]，[Sk+1]，[Sk+2]，可得[Sk+1]，[Sk+2]的計算公式為：

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

式中，[t+2Δt]時刻對應的行駛速度[Vk+2]為：

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

綜上可得式（3）中汽車東向速度狀態方程為：

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

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

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

[W]是汽車的系統噪聲，使用噪聲方差矩陣[Q]表示，[Q]是常系數矩陣，與GPS設備采樣頻率相關。

[Q=t520t48t36t48t36t22t36t22t×0.09] （10）

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

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

最后使用擴展卡爾曼濾波算法對汽車東向速度的估計方程為：

[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）

結合式（3）和式（11），可得使用擴展卡爾曼濾波算法獲取的t時刻汽車東向狀態分量最優估計值[Xk，Ek]，同樣地可得汽車北向狀態分量最優估計值[Xk，Nk]，從而得到汽車在該時刻的水平速度[Xkk]。

使用擴展卡爾曼濾波算法對GPS位差測速算法進行改進可有效抑制噪聲對測速精度的影響并提高測速精度，但是擴展卡爾曼濾波算法在收斂速度方面[10]存在一定的問題，在汽車快速轉彎時誤差較大，因此使用有限差分算法進行優化以進一步提高算法性能。

2.2 有限差分改進

有限差分的原理是利用一階中心差分法和多項式近似技術求解非線性函數的偏導數，可實現二階非線性近似功能[11]。設[y=gx]為非線性函數，該函數在[x=x]處的二階有限中心差分展開為：

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

式中：[gx]為函數[gx]在[x=x]處的函數值；[gDD′x]是使用差分方法計算出的展開式一階項系數，其計算如下：

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

式中，[l]是區間長度參數，[fDD″x2！]是展開式二次項系數，同樣使用差分方法計算獲取，其計算如下：

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

對式（12）進行泰勒級數展開變為：

[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）后兩項比較發現后兩項與高階項對應，并由[h]來控制精度，通過驗證比較，在非線性函數中使用中心差分代替一階、二階導數后的精度比代替前的精度要高，這也是將有限差分算法用于改進擴展卡爾曼濾波算法用于GPS測速的原因所在，可有效處理車輛高速轉變過程中的非線性變化得到更高的測速精度。有限差分擴展卡爾曼濾波算法步驟為：

（1） 初始化，獲取GPS測速模型的狀態方程和輸出觀測方程、[ω]和[W]的系統方差陣分別是[R，Q]及[AE]，[CE]，并引進矩陣：

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

（2） 進行狀態一步預測，即式（11a）；

（3） 對汽車東向、北向的誤差協方差矩陣進行預測，此處使用一階中心差分法獲取誤差協方差矩陣，東向分量的誤差協方差矩陣為：

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

式中：[AE，i]由步長的調節系數[h]調整；[h]可按噪聲分布規律設置。

（4） 進行量測預測，如式（18）所示。

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

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

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

（6） 進行濾波估，如式（11b）所示。

（7） 進行誤差協方差陣估計，如式（11d）所示。

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

3 實驗及分析

為對本文的算法性能進行分析驗證，使用Vkel的GPS模塊獲取車輛的GPS數據，并分別使用擴展卡爾曼濾波GPS測速算法模塊和有限差分擴展卡爾曼濾波GPS測速算法模塊接收GPS數據并進行測速；對前文中的部分參數進行設置，對調節系數[h]，因統計噪聲屬于白噪聲[9]，因此設定[h=3]；GPS設備采樣頻率為1 Hz，對應的采樣時間[Δt=1 s]。車輛行駛中設置多次高速轉彎，將兩種算法計算的車輛行駛水平速度分別與水平速度真值進行比較，得到的速度測量誤差如圖1所示。

相比擴展卡爾曼濾波GPS測速算法，改進后算法測量誤差更小，其均方根誤差是0.051 9，而擴展卡爾曼濾波GPS測速算法的均方根誤差為0.231 8，在車輛多次高速轉變情況下，算法改進后均方根誤差得到明顯提高。改進后算法平均耗時0.148 1 s，而改進前算法平均耗時0.117 4 s，可見有限差分擴展卡爾曼濾波GPS測速算法使用有限差分運算獲取濾波驗前、驗后誤差協方差矩陣改進擴展卡爾曼濾波算法增加了算法復雜度從而需要消耗更長的運算時間，但仍在系統設計需求許可范圍內。

4 結 語

本文針對位差法和擴展卡爾曼濾波融合算法用于GPS測速時汽車快速轉彎時測速誤差較大的問題，使用有限差分運算獲取濾波驗前、驗后誤差協方差矩陣改進擴展卡爾曼濾波算法增強了算法的收斂性；從而提高了算法在車輛高速轉彎時的測速精度，使速度測量值的均方根誤差減小，雖然測速響應時間增大但仍在系統設計需求允許范圍內；表明基于有限差分擴展卡爾曼濾波的GPS動態測速算法具有更高的性能和應用價值。

參考文獻

[1] 郭可怡.汽車電子控制裝置中計算機檢測控制技術的應用[J].電子技術與軟件工程，2015（6）：258.

[2] 周娟英.計算機檢測控制技術在汽車電子控制系統中的應用分析[J].自動化與儀器儀表，2014，7（4）：107?108.

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

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

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

[6] 余婭，李軍.基于車載GPS導航的非線性動態濾波算法[J].計算機與數字工程，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] 楊宏，李亞安，李國輝.一種改進擴展卡爾曼濾波新方法[J].計算機工程與應用，2010，46（19）：18?20.

[9] 潘盛輝，郝東亞，張興達.基于擴展卡爾曼濾波的GPS動態測速算法[J].信息技術，2014，13（12）：13?16.

[10] 何棟煒，彭俠夫，蔣學程，等.永磁同步電機的改進擴展卡爾曼濾波測速算法[J].西安交通大學學報，2011，45（10）：59?64.

[11] 巫春玲，韓崇昭.用于彈道目標跟蹤的有限差分擴展卡爾曼濾波算法[J].西安交通大學學報，2008，42（2）：143?146.