張麗平，李 環(huán)

(沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽 110159)

車載GPS/DR組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合方法研究

張麗平，李 環(huán)

(沈陽理工大學(xué) 信息科學(xué)與工程學(xué)院，遼寧 沈陽 110159)

GPS/DR組合導(dǎo)航系統(tǒng)是非線性的，擴(kuò)展卡爾曼濾波(EKF)可以利用線性化技巧將其轉(zhuǎn)化為線性濾波問題，但這一過程會(huì)使得濾波結(jié)果出現(xiàn)很大誤差。針對(duì)這一問題，將改進(jìn)的粒子濾波方法(UPF)，即將無跡卡爾曼濾波(UKF)與粒子濾波(PF)相結(jié)合，應(yīng)用到GPS/DR組合導(dǎo)航系統(tǒng)中，避免了EKF的線性化近似過程，同時(shí)優(yōu)化了PF算法，提高了定位精度。實(shí)驗(yàn)結(jié)果表明，與 EKF和PF 算法相比，UPF算法具有更高的魯棒性和更好的定位效果。

GPS/DR組合導(dǎo)航；擴(kuò)展Kalman濾波；無跡Kalman濾波；粒子濾波

GPS/DR組合導(dǎo)航的關(guān)鍵技術(shù)是將全球定位系統(tǒng)(GPS)和航位推算(DR)兩個(gè)系統(tǒng)輸出的數(shù)據(jù)進(jìn)行融合。在以往的組合導(dǎo)航中，一般是將GPS與DR系統(tǒng)作簡單切換，系統(tǒng)正常工作時(shí)GPS作為主定位系統(tǒng)，當(dāng)GPS信號(hào)較差或者丟失時(shí)便切換至DR系統(tǒng)[1]。一定程度上解決了GPS信號(hào)的失鎖問題，但在復(fù)雜環(huán)境下，DR系統(tǒng)會(huì)隨著時(shí)間的積累產(chǎn)生嚴(yán)重的誤差。因此，將GPS與DR系統(tǒng)作簡單切換已不能滿足應(yīng)用要求，需要采用合理的數(shù)據(jù)融合技術(shù)。國內(nèi)外常用的數(shù)據(jù)融合技術(shù)有卡爾曼濾波、加權(quán)平均法、貝葉斯估計(jì)、統(tǒng)計(jì)決策理論、模糊推理和神經(jīng)網(wǎng)絡(luò)等。而在組合導(dǎo)航系統(tǒng)中，應(yīng)用最廣泛，最成功的數(shù)據(jù)融合技術(shù)是卡爾曼濾波[2]。

卡爾曼濾波應(yīng)用在GPS/DR組合導(dǎo)航系統(tǒng)中的關(guān)鍵技術(shù)是將GPS和DR的信息同時(shí)用于定位解的求解過程中，使得DR系統(tǒng)的數(shù)據(jù)在濾波過程中不斷得到修正，同時(shí)，DR系統(tǒng)的推算結(jié)果可用于補(bǔ)償部分GPS定位中的隨機(jī)誤差，當(dāng)GPS衛(wèi)星信號(hào)被遮擋時(shí)由DR繼續(xù)定位[3]。但是經(jīng)典的卡爾曼濾波只適用于線性系統(tǒng)，而GPS/DR車輛組合導(dǎo)航系統(tǒng)的狀態(tài)模型和觀測(cè)模型都具有一定程度的非線性。有很多學(xué)者采用擴(kuò)展卡爾曼濾波(EKF)[4]來進(jìn)行GPS/DR組合導(dǎo)航的數(shù)據(jù)融合，但應(yīng)用擴(kuò)展卡爾曼濾波時(shí)所進(jìn)行的線性化過程會(huì)導(dǎo)致濾波結(jié)果出現(xiàn)較大誤差。

近幾年,基于蒙特卡羅方法(Monte Carlo，MC)和遞推貝葉斯估計(jì)的粒子濾波(Particle Filter，PF)算法備受廣大學(xué)者關(guān)注。該方法不受非線性、非高斯問題的限制，可以處理強(qiáng)非線性和非高斯系統(tǒng)模型的數(shù)據(jù)融合。但粒子濾波也存在著一些問題，比如需要耗費(fèi)大量的樣本數(shù)去得到較精確的后驗(yàn)概率密度，重采樣階段也會(huì)造成樣本有效性和多樣性的損失，導(dǎo)致樣本貧化現(xiàn)象等。

為了克服上述缺點(diǎn)，本文將改進(jìn)的粒子濾波算法引入GPS/DR組合導(dǎo)航系統(tǒng)。以UKF為建議密度的粒子濾波方法，稱為UPF算法(Unscented Particle Filter)[5]。UKF算法不需要像EKF算法一樣進(jìn)行線性化，因此避開了線性化過程造成的誤差問題，并且UKF能夠非常容易地將最新觀測(cè)量引入狀態(tài)估計(jì)。UPF算法首先使用UKF算法得到比PF算法更好的重要性分布函數(shù)，然后從中抽樣得到粒子，最后再使用PF算法濾波。因此UPF算法能夠大大提高粒子濾波的精度。

本文研究由經(jīng)典卡爾曼濾波衍生的EKF算法，UKF算法和PF算法等融合算法，著重研究UPF算法以及UPF算法在GPS/DR組合導(dǎo)航系統(tǒng)中的應(yīng)用。仿真結(jié)果表明UPF較EKF、PF具有更高的定位精度。

1 GPS/DR車載組合導(dǎo)航系統(tǒng)

1.1 狀態(tài)方程的建立

車載GPS/DR組合導(dǎo)航定位系統(tǒng)的運(yùn)動(dòng)模型采用最為常用的“當(dāng)前”統(tǒng)計(jì)模型[6]，通過卡爾曼濾波將觀測(cè)到的信息進(jìn)行融合，得到誤差較小，精度較理想的位置和速度，從而達(dá)到較好的定位效果。

車載GPS/DR組合導(dǎo)航系統(tǒng)的狀態(tài)向量為X=(e,ve,ae,n,vn,an)T，其中e和n分別為車輛東向、北向的位置分量，ve和vn分別為車輛東向、北向的速度分量，ae和an分別為車輛東向、北向的加速度分量。

系統(tǒng)的離散狀態(tài)方程為

X(k)=Φ(k,k-1)X(k-1)+W(k-1)

(1)

式中:W(k-1)為系統(tǒng)噪聲，狀態(tài)轉(zhuǎn)移矩陣為Φ(k,k-1)=diag{Φe,Φn}

(2)

式中：T為采樣周期。

1.2 觀測(cè)方程的建立

GPS/DR車輛組合導(dǎo)航系統(tǒng)的觀測(cè)量分別來自GPS、角速率陀螺、里程計(jì)的信息。系統(tǒng)的觀測(cè)量為Z=(egps,ngps,w,s)，其中egps和ngps分別為GPS接收機(jī)輸出的東向、北向的位置坐標(biāo)分量，ω為速度陀螺儀輸出的角速度，s為里程計(jì)在采樣周期內(nèi)前進(jìn)的距離。系統(tǒng)離散觀測(cè)方程為

(3)

即：Z(k)=h[k,X(K)]+V(K)

(4)

式中:V(k)為觀測(cè)噪聲;v1(k)、v2(k)分別為GPS接收機(jī)東向、北向的輸出誤差;vω(k)、vs(k)分別為陀螺、里程計(jì)的輸出誤差。

2 粒子濾波(PF算法)

PF是一種基于蒙特卡羅方法和遞推貝葉斯估計(jì)的濾波方法[7]，它采用一組從概率密度函數(shù)上隨機(jī)抽取的并附帶相關(guān)權(quán)值的粒子集來逼近后驗(yàn)概率密度，其中的核心算法是序貫重要性采樣算法。序貫重要性采樣算法(Sequential Importance Sampling，SIS)[8]是從重要性概率密度函數(shù)中生成采樣粒子，并隨著測(cè)量值的依次到來遞推求得相應(yīng)的權(quán)值，最終以粒子加權(quán)和的形式來描述后驗(yàn)概率密度，進(jìn)而得到狀態(tài)估計(jì)。粒子濾波在解決非線性、非高斯問題上具有很大的優(yōu)越性，因此獲得了廣泛的應(yīng)用，如應(yīng)用在自動(dòng)目標(biāo)識(shí)別、移動(dòng)機(jī)器人定位、計(jì)算機(jī)視覺、可視化跟蹤等。

離散非線性狀態(tài)空間模型：

xk+1=Φ(xk)+wk

(5)

yk=h(xk)+vk

(6)

式中：xk為系統(tǒng)維狀態(tài)向量；Φ為n維向量函數(shù)；h為m維向量函數(shù)；wk為p維隨機(jī)過程噪聲；vk為q維隨機(jī)觀測(cè)噪聲；yk為系統(tǒng)n維觀測(cè)向量。

(7)

式中，δ(·)為狄拉克三角函數(shù)；令xi近似于q(x)，i=1,2,…,N,q(x)為重要性密度函數(shù)，N為粒子數(shù)。這樣通過加權(quán)就可以得到密度p(xk)為

(8)

式中

(9)

q(x0：k|y1:k)=q(xk|x0:k-1,y1:k)·q(x0:k-1|y1：k-1)

(10)

那么權(quán)重公式為

(11)

由于最常見的重要性密度函數(shù)選擇為

(12)

(13)

將該權(quán)值進(jìn)行歸一化得到歸一化權(quán)值為

(14)

雖然PF算法是解決非線性非高斯問題的有效工具，但仍然存在問題。其中最主要的問題就是為了得到應(yīng)用系統(tǒng)較為精確的后驗(yàn)概率密度需要耗費(fèi)龐大的樣本數(shù)據(jù)。因此本文通過改進(jìn)的粒子濾波選擇更好的重要性密度函數(shù)來提高濾波精度。

3 UPF算法

與EKF不同,UKF以UT變換為基礎(chǔ)，摒棄了對(duì)非線性函數(shù)進(jìn)行線性化的傳統(tǒng)做法，采用卡爾曼線性濾波框架，對(duì)于一步預(yù)測(cè)方程，使用UT變換來處理均值和協(xié)方差的非線性傳遞，這樣就形成了UKF算法[9]。其中UT變換的基本原理是在估計(jì)點(diǎn)附近確定性采樣，這些采樣點(diǎn)能完全得知狀態(tài)隨機(jī)變量的均值和協(xié)方差矩陣變換，然后通過泰勒展開的二階精度逼近后驗(yàn)均值和協(xié)方差。因此UKF在減少采樣粒子數(shù)的同時(shí)保證了逼近精度，有效克服了EKF算法估計(jì)精度低、穩(wěn)定性差的缺陷。

UPF的具體步驟為

(15)

式中:Pw為系統(tǒng)噪聲方差，Pv為測(cè)量噪聲方差。

2)使用UPF算法更新粒子集

(16)

計(jì)算Sigma點(diǎn)：

(17)

式中：λ為合成比例參數(shù)

λ=α2(nα+κ)-nα,κ為縮放比例參數(shù)，它控制Sigma點(diǎn)到均值的距離，通常κ=0；0≤α≤1為縮放比例參數(shù)，它控制Sigma點(diǎn)的分布范圍，通常取一個(gè)較小的數(shù)值，以避免在強(qiáng)非線性情況下產(chǎn)生采樣的非局部影響；β≥0反應(yīng)密度函數(shù)高階矩信息，當(dāng)高斯先驗(yàn)時(shí)β=2。

時(shí)間更新：

(18)

量測(cè)更新：

(19)

輸出：

(20)

3)分別通過式(13)，式(14)對(duì)粒子進(jìn)行權(quán)重計(jì)算和歸一化。

4 仿真分析

X(0)=[0,10,0,0,10,0]，

P(0)=diag{202,12,0.22,202,12,0.22}

仿真時(shí)間為100s，采樣粒子N=500；

系統(tǒng)噪聲：Q=diag{10,1,0.1,10,1,0.1}；

量測(cè)噪聲：R=diag{10,10,0.01,4}。

仿真結(jié)果如圖1～圖4和表1所示。

圖1 東向位置誤差

圖2 北向位置誤差

圖3 UPF算法濾波前后比較

圖4 UPF算法目標(biāo)跟蹤軌跡

表1 EKF、PF和UPF濾波誤差比較

以上仿真圖中，圖1、圖2和表1是EKF算法、PF算法、UPF算法在相同條件下的濾波結(jié)果。結(jié)果表明，EKF算法的濾波誤差平均為10m左右，PF算法的濾波誤差平均為5m左右，而UPF算法的濾波誤差平均為3m左右，從而體現(xiàn)了UPF算法比其他兩種算法有更好的定位精度。通過仿真圖也可以明顯看到，UPF算法的穩(wěn)定性更好。圖3為UPF算法濾波前后的誤差比較，從圖可以看出，通過UPF算法濾波后，誤差明顯減小，在水平線附近浮動(dòng)范圍很小，而濾波前誤差則浮動(dòng)很大。圖4為UPF算法的目標(biāo)跟蹤軌跡，從圖明顯看到，UPF具有很高的定位精度。綜上所述，不論是算法的穩(wěn)定性，還是定位精度，UPF算法都優(yōu)于其他兩種算法。

5 結(jié)束語

研究了UPF算法在GPS/DR組合導(dǎo)航系統(tǒng)中的應(yīng)用，并將其與EKF、PF算法進(jìn)行比較。比較結(jié)果表明，UPF算法有效避免了EKF的線性化近似過程和PF算法嚴(yán)重的退化現(xiàn)象，具有較高的穩(wěn)定性和更好的定位效果。因此，UPF算法在GPS/DR組合導(dǎo)航系統(tǒng)中具有更好的應(yīng)用前景。

[1]吳富梅,楊元喜,田育民.GPS/DR 組合導(dǎo)航自適應(yīng) Kalman 濾波算法[J].測(cè)繪科學(xué)技術(shù)學(xué)報(bào),2008,25(3):206-208.

[2]楊東凱,寇艷紅,吳今培,等.智能交通系統(tǒng)中的地圖匹配定位方法[J].交通運(yùn)輸系統(tǒng)工程與信息,2003,3(3):38-43.

[3]周翟和,劉建業(yè),賴際舟,等.一種新的改進(jìn)高斯粒子濾波算法及其在 SINS/GPS 深組合導(dǎo)航系統(tǒng)中的應(yīng)用[J].控制與決策,2011,26(1):85-88.

[4]Kalman R E.A new approach to linear filtering and prediction problems[J].Journal of basic Engineering,1960,82(1):35-45.

[5]Van Der Merwe R,Doucet A,De Freitas N,et al.The unscented particle filter[C].NIPS.2000:584-590.

[6]周宏仁.機(jī)動(dòng)目標(biāo)“當(dāng)前”統(tǒng)計(jì)模型與自適應(yīng)跟蹤算法[J].航空學(xué)報(bào),1983,4(1):73-86.

[7]辛云宏,王保平,楊萬海.基于序貫重要采樣算法的被動(dòng)單站機(jī)動(dòng)目標(biāo)跟蹤[J].西安電子科技大學(xué)學(xué)報(bào),2005,32(5):820-824.

[8]Arulampalam S.Maskell.A tutorial on particle filters for on line non linear/non Gaussian Bayesian tracking[J].IEEE Transactions on Signal Processing,2002,50(20):174-188.

[9]Julier S,Uhlmann J,Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].Automatic Control,IEEE Transactions on,2000,45(3):477-482.

[10]Arulampalam S.Maskell.A tutorial on particle filters for on line non-linear/non-gaussian bayesian tracking[J].IEEE Transactions on Signal Processing,2002,50(20):174-188.

ResearchonDataFusionMethodofVehicleGPS/DRIntegratedNavigationSystem

ZHANG Liping，LI Huan

(Shenyang Ligong University,Shenyang 110159,China)

GPS/DR integrated navigation system is a nonlinear system,which leads to a big error in realizing system linearization by using the extended Kalman filter (EKF).Aiming at solving the problem,the improved particle filter algorithm (UPF) combining the unscented Kalman filter (UKF) and particle filter (PF) is applied to the GPS/DR integrated navigation system.It not only avoids the linear approximation of EKF,but also optimizes the PF algorithm and improves the positioning accuracy.Experimental results show that,compared with the EKF algorithm and PF algorithm,UPF algorithm has higher robustness and better positioning effect.

GPS/DR integrated navigation;extended Kalman filter;unscented Kalman filter;particle filte

2013-06-25

張麗平(1987—)，女，碩士研究生；通訊作者：李環(huán)(1964—)，女，教授，研究方向:擴(kuò)頻通信技術(shù)及應(yīng)用，信號(hào)處理等.

1003-1251(2014)01-00013-05

TN967.2

A

馬金發(fā))