許立太

(蘭州石化職業(yè)技術(shù)學(xué)院,甘肅蘭州 730060)

0 引言

在目標(biāo)定位跟蹤過(guò)程中,觀測(cè)站僅能獲得包含噪聲的目標(biāo)方位信息,造成了觀測(cè)方程的非線性,不能直接利用線性濾波方法獲得目標(biāo)的運(yùn)動(dòng)狀態(tài),是目標(biāo)跟蹤[1-2]、機(jī)器視覺(jué)及傳感器網(wǎng)絡(luò)定位研究領(lǐng)域的共同難題。擴(kuò)展卡爾曼濾波器(Extended Kalman Filter,EKF)等傳統(tǒng)濾波器試圖使用線性化方法解決該難題,但圍繞狀態(tài)預(yù)測(cè)估計(jì)的一階展開方法往往近似程度不高,造成EKF等濾波器在純方位跟蹤應(yīng)用中跟蹤效果不理想,甚至出現(xiàn)濾波發(fā)散的現(xiàn)象。最近,研究人員提出一種新的用于解決非線性濾波問(wèn)題的濾波器,它是基于這樣一種考慮:近似一種高斯分布要比近似任何一種非線性方程容易的多。他們將這種濾波器稱為無(wú)跡卡爾曼濾波器(Unscented Kalman Filter,UKF)[3-5]。實(shí)驗(yàn)證明UKF給出的估計(jì)結(jié)果比EKF更準(zhǔn)確,尤其是它能給出更精確的系統(tǒng)狀態(tài)方差估計(jì)。然而,UKF的使用具有一定的限制,它不適用于一般的非高斯分布的模型。解決非線性濾波問(wèn)題的一種更新的方法是粒子濾波器(particle filter,PF)[6-9],其基本思想是用一組帶有權(quán)值的粒子集合來(lái)表示解決問(wèn)題時(shí)需要的后驗(yàn)概率密度[10],然后用這一近似的表示來(lái)計(jì)算系統(tǒng)的狀態(tài)估計(jì)。然而,粒子濾波器主要的問(wèn)題在于粒子退化問(wèn)題,由于“退化”會(huì)產(chǎn)生粒子數(shù)減少,進(jìn)而會(huì)導(dǎo)致失去狀態(tài)的多樣性并最終產(chǎn)生“失真”問(wèn)題。導(dǎo)致退化的主要原因在于第一,建議分布函數(shù)的選擇;第二,粒子數(shù)的確定(由此要用到重采樣的方法)。針對(duì)以上問(wèn)題本文提出通過(guò)選擇新的建議分布函數(shù)的方法來(lái)減少粒子濾波的“退化”問(wèn)題。新算法稱之為改進(jìn)建議分布的混合粒子濾波算法(UEKF)。

1 非線性濾波算法

1.1 EKF濾波算法

擴(kuò)展卡爾曼濾波器中系統(tǒng)的狀態(tài)分布用高斯隨機(jī)變量(GRV)來(lái)表示。在某一時(shí)刻,EKF方法將系統(tǒng)的非線性方程在當(dāng)前關(guān)于系統(tǒng)狀態(tài)X的估計(jì)處,展開成一階泰勒展式。EKF的具體算法見(jiàn)參考文獻(xiàn)[11-12]。不失一般性,非線性系統(tǒng)可表示如下:

式中,x k∈Rn表示系統(tǒng)狀態(tài)量,vk∈Rq表示系統(tǒng)過(guò)程噪聲,y k∈Rp表示系統(tǒng)輸出,w k∈Rr表示系統(tǒng)觀測(cè)噪聲。以EKF為建議分布就得到了擴(kuò)展卡爾曼粒子濾波器。

1.2 UKF濾波算法

對(duì)于UKF,這里仍采用如式(1)所示系統(tǒng)狀態(tài)描述,其濾波算法如下:

1.2.1 UT變換

UT變換是計(jì)算進(jìn)行非線性傳遞的隨機(jī)向量概率的一種方法,它是基于這樣考慮:近似一種概率分布比近似一種任意的非線性方程或者非線性變換要容易的多,設(shè)x是nx維的隨機(jī)向量,f:nx→nz是一非線性函數(shù)z=f(x),考慮將x通過(guò)非線性函數(shù)f傳遞,假定x的均值和協(xié)方差分別為^x和p x,為了計(jì)算關(guān)于z的統(tǒng)計(jì)量,我們首先選擇2nx+1個(gè)帶有權(quán)值的樣本點(diǎn)(也稱Sigma點(diǎn)),使其能夠完全獲取隨機(jī)變量x的真實(shí)的均值和協(xié)方差。

1.2.2 UKF

UKF是UT的直接擴(kuò)展,系統(tǒng)的狀態(tài)分布仍然用一個(gè)高斯隨機(jī)變量來(lái)表示,用一個(gè)經(jīng)過(guò)選擇的Sigma點(diǎn)的集合來(lái)具體表示,但是狀態(tài)隨機(jī)變量被重新定義為原始狀態(tài)和噪聲變量的擴(kuò)張向量=上標(biāo)T表示轉(zhuǎn)秩。通常,UKF使用高斯近似來(lái)表示先驗(yàn)及后驗(yàn)密度,同樣能夠獲得先驗(yàn)及后驗(yàn)密度上的離散度。關(guān)于UKF算法的具體實(shí)現(xiàn),參看文獻(xiàn)[3-5]。

1.3 粒子濾波算法

但是,粒子濾波算法的一個(gè)主要問(wèn)題是退化問(wèn)題,即經(jīng)過(guò)幾步迭代以后,除了極少數(shù)粒子外,其他的粒子權(quán)值小到可以忽略不計(jì)的程度。減少退化現(xiàn)象影響的方法一般有兩種:一是選擇好的重要密度函數(shù),即選擇較好的建議分布;另一種是使用再采樣技術(shù)[3,11-13]。再采樣方法就是去除那些權(quán)值較小的粒子,而復(fù)制權(quán)值較大的粒子,即采集更多的權(quán)值較大的粒子;但會(huì)失去狀態(tài)信息的多樣性。

2 改進(jìn)建議分布的混合粒子濾波算法

針對(duì)上述濾波方法在純方位跟蹤中的缺點(diǎn),這里采用EKF與UKF混合濾波器作為建議分布,新算法簡(jiǎn)稱為改進(jìn)建議分布的混合粒子濾波算法(UEPF),新算法通過(guò)改進(jìn)粒子濾波的建議分布進(jìn)而改進(jìn)粒子濾波的“退化”問(wèn)題,而且通過(guò)新的建議分布函數(shù)進(jìn)而改進(jìn)其跟蹤精度。具體過(guò)程為:在時(shí)刻k首先用UKF更新粒子,采用表達(dá)式UKF算法的更新步驟獲得狀態(tài)估值^x k,然后計(jì)算系統(tǒng)模型與測(cè)量模型的雅克比矩陣,利用EKF更新粒子,此時(shí)采用所獲得的狀態(tài)估值^xk作為k-1時(shí)刻的粒子濾波的狀態(tài)估計(jì),經(jīng)過(guò)計(jì)算得到k時(shí)刻最終的狀態(tài)及其相應(yīng)的協(xié)方差估計(jì)值ˉxik和^Pik。從而可以從建議分布中抽取粒子。假設(shè)k-1時(shí)刻的狀態(tài)及使用UKF更新粒子。Sigma點(diǎn)的選擇依據(jù)UT變換。此后Sigma點(diǎn)分別通過(guò)系統(tǒng)模型與測(cè)量模型向前傳遞,得到狀態(tài)與協(xié)方差的預(yù)測(cè)值,其均值可按表達(dá)式UKF預(yù)測(cè)步驟計(jì)算得到,得到新的測(cè)量值y k后,預(yù)測(cè)狀態(tài)估計(jì)量^xk|k-1按照表達(dá)式UKF的更新步驟計(jì)算得到,其中Kk=為卡爾曼增益,亦可按照表達(dá)式UKF的更新步驟來(lái)計(jì)算,由此可獲得狀態(tài)估計(jì)量^x k。然后利用EKF執(zhí)行粒子更新過(guò)程。首先預(yù)測(cè)狀態(tài)及協(xié)方差,據(jù)此求取卡爾曼增益,修正預(yù)測(cè)量得到最終所需的估計(jì)量。

通過(guò)第一小節(jié)的分析可以得出EKF算法通過(guò)泰勒展式將系統(tǒng)非線性化進(jìn)行局部線性化,因此系統(tǒng)非線性性質(zhì)得不到好的描述,并且在計(jì)算中要計(jì)算雅克比矩陣,因此大大增加了計(jì)算量,故在純方位跟蹤中與其所需的實(shí)時(shí)性相悖,另外該算法只能達(dá)到一階的精度。UKF算法是用確定的采樣來(lái)近似狀態(tài)的后驗(yàn)概率密度函數(shù),可以有效解決由系統(tǒng)非線性的加劇而引起的濾波發(fā)散問(wèn)題。但UKF仍是用高斯分布來(lái)近似逼近系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù),所以在系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)是非高斯的情況下,濾波結(jié)果將有極大的誤差。粒子濾波算法較簡(jiǎn)單,但在采樣過(guò)程中有時(shí)會(huì)出現(xiàn)比較嚴(yán)重的退化現(xiàn)象。抑制粒子濾波算法退化的主要手段就是增加粒子數(shù)和重采樣。但是,重采樣會(huì)降低粒子的多樣性;而大量增加粒子數(shù),將大大增加計(jì)算量。因此,主要依賴選擇好的重要密度函數(shù),而本文的濾波算法就是基于這種思想的一種PF改進(jìn)算法。在粒子濾波中選取狀態(tài)轉(zhuǎn)移概率密度作為重要函數(shù)沒(méi)有考慮最新觀測(cè)到的數(shù)據(jù),使得粒子嚴(yán)重依賴于系統(tǒng)狀態(tài)轉(zhuǎn)移模型,如果模型不準(zhǔn)確,從重要性函數(shù)抽取的樣本與真實(shí)的后驗(yàn)概率密度函數(shù)產(chǎn)生的樣本存在較大的偏差,特別是當(dāng)似然函數(shù)出現(xiàn)轉(zhuǎn)移概率密度函數(shù)的尾部或者似然函數(shù)與轉(zhuǎn)移概率密度函數(shù)相比過(guò)于集中(呈尖峰)時(shí),這種情況在高精度測(cè)量場(chǎng)合經(jīng)常遇到,此時(shí)偏差尤為明顯。本文算法就是針對(duì)上述問(wèn)題加以改進(jìn),通過(guò)混合建議分布產(chǎn)生的重要密度函數(shù)與真實(shí)狀態(tài)概率密度函數(shù)的支集重疊部分更大,估計(jì)精度高,即便在非高斯情況下其誤差也較小,通過(guò)對(duì)算法的實(shí)驗(yàn)仿真驗(yàn)證了該算法的優(yōu)越性。

3 仿真分析

將UEPF應(yīng)用于純方位跟蹤問(wèn)題中與其他方法相比較來(lái)驗(yàn)證此方法的有效性。這里通過(guò)勻速直線運(yùn)動(dòng)和勻速轉(zhuǎn)彎?rùn)C(jī)動(dòng)目標(biāo)兩個(gè)純方位跟蹤仿真實(shí)例,與其他濾波器進(jìn)行了仿真對(duì)比,分析了跟蹤性能和誤差,本文首先用該算法對(duì)一個(gè)xy平面上作勻速直線運(yùn)動(dòng)和轉(zhuǎn)彎?rùn)C(jī)動(dòng)目標(biāo)進(jìn)行了跟蹤仿真。同時(shí),本文也對(duì)擴(kuò)展卡爾曼濾波(EKF),無(wú)跡卡爾曼濾波(UKF),以及一般的粒子濾波(PF)作了仿真實(shí)驗(yàn)并對(duì)仿真結(jié)果進(jìn)行了比較,其狀態(tài)轉(zhuǎn)移方程為:

狀態(tài)轉(zhuǎn)移方程表示的是一機(jī)動(dòng)目標(biāo)在 xy平面上作非線性運(yùn)動(dòng),其中 ωk+1=ωk+wω,k為轉(zhuǎn)彎速度模型,(x,y)為目標(biāo)位置,(˙x,˙y)為目標(biāo)速度,T為采樣間隔,wk,wω,k為系統(tǒng)噪聲。量測(cè)方程為:

式中,r為目標(biāo)的斜距,θ為目標(biāo)的方位角,˙r為距離變化率。vk=(vk,r,vk,θ,vk,r)為量測(cè)噪聲。由于本文僅考慮對(duì)狀態(tài)的濾波跟蹤問(wèn)題,因此假設(shè)系統(tǒng)狀態(tài)服從一階馬爾可夫過(guò)程,且觀測(cè)量獨(dú)立于已知的狀態(tài)量,即由概率密度描述:

這里Unscented變換參數(shù)分別設(shè)定為α=1,β=2,k=0。對(duì)模型和算法在如下背景和參數(shù)下進(jìn)行仿真:首先跟蹤勻速直線運(yùn)動(dòng)目標(biāo),仿真時(shí)長(zhǎng)200 s,采樣頻率1 Hz,粒子數(shù)為1 000,單觀測(cè)站位于原點(diǎn)開始勻速直線運(yùn)動(dòng),初始航向?yàn)?0°(以逆時(shí)針?lè)较驗(yàn)檎?,速度為10 m/s。在時(shí)刻100 s時(shí)進(jìn)行轉(zhuǎn)向機(jī)動(dòng),保持速度大小不變,航向角為60°。接下來(lái)目標(biāo)勻速直線運(yùn)動(dòng),初始距離為10 000 m,速度為15 m/s,目標(biāo)的初始方位為60°,航向角為150°。這里系統(tǒng)噪聲和觀測(cè)噪聲均為獨(dú)立的零均值高斯白噪聲,系統(tǒng)噪聲均方差0.01 m,速度為0.000 1 m/s,觀測(cè)方位噪聲均方差為1°。仿真初始階段采用偽線性法獲得粒子濾波器的估計(jì)初值x 0,用于產(chǎn)生先驗(yàn)概率p(x0)。進(jìn)行100次仿真,得到4種濾波算法對(duì)目標(biāo)的軌跡跟蹤曲線以及RMSE對(duì)比,如圖1、圖2所示。

圖1 勻速直線運(yùn)動(dòng)軌跡跟蹤Fig.1 Constant speed linear motion trajectory tracking

圖2 RM SE對(duì)比Fig.2 RMSE compare

從仿真結(jié)果可以看出,在觀察站機(jī)動(dòng)前,EKF與UKF有較大的偏差,呈現(xiàn)發(fā)散的特征;在觀測(cè)站機(jī)動(dòng)后EKF與UKF緩慢收斂,但仍然有偏差,跟蹤效果較差。由 PF及 UEPF曲線變化可看出,與EKF和UKF相比,以粒子為基礎(chǔ)的濾波器對(duì)目標(biāo)跟蹤具有較好的效果,在觀測(cè)站機(jī)動(dòng)前,雖然沒(méi)有收斂,但其收斂性能要好于EKF與UKF;但當(dāng)觀測(cè)站機(jī)動(dòng)時(shí),由于粒子濾波器對(duì)于傳感器觀測(cè)值的突變非常敏感,所以用先驗(yàn)概率作為建議分布的PF濾波在觀測(cè)站機(jī)動(dòng)后誤差增大很明顯,而UEPF使用UKF與EKF產(chǎn)生建議分布,能夠適應(yīng)觀測(cè)值的突變,快速地減小 RMES。

以下為跟蹤勻速轉(zhuǎn)彎?rùn)C(jī)動(dòng)目標(biāo)。仿真初始條件為測(cè)量點(diǎn)在坐標(biāo)原點(diǎn),機(jī)動(dòng)目標(biāo)轉(zhuǎn)彎角速率為3°/s,初始位置與速度為x0=[0,0,0,100]T,系統(tǒng)噪聲協(xié)方差陣為diag([5,0.1,5,0.1]),觀測(cè)噪聲協(xié)方差陣為diag([200,0.1])采樣時(shí)間為60 s,采樣間隔為1 s,圖3為幾種算法的仿真結(jié)果。

圖3 目標(biāo)在轉(zhuǎn)彎過(guò)程中的位置誤差Fig.3 The position error of the target in the process of turning

由于其軌跡近似相同,難以看出較大變化,故這里只給出其在坐標(biāo)平面上每個(gè)采樣時(shí)刻估計(jì)航跡的位置和實(shí)際航跡的位置的距離偏差,從圖3中可清晰看出:EKF由于線性化損失的影響,具有較大的跟蹤誤差,PF則由于在觀測(cè)更新過(guò)程中,建議分布未能包含新的觀測(cè)信息,所以跟蹤性能不夠理想,UEPF算法明顯好于EKF、UKF及PF算法,主要由于UEPF充分利用了每次測(cè)量的信息,故優(yōu)于一般的濾波算法。在實(shí)際應(yīng)用當(dāng)中由于使用EKF及UKF產(chǎn)生建議分布,UEPF在提高純方位性能的時(shí)候,由于要處理多個(gè)粒子及多次計(jì)算雅克比矩陣,故也增加了濾波所需的時(shí)間,但UEPF在每個(gè)采樣點(diǎn)狀態(tài)估計(jì)平均時(shí)間約為0.2 s相對(duì)與觀測(cè)頻率(1～3 s),約占觀察時(shí)間的7%～20%,可以滿足實(shí)際應(yīng)用中在線式跟蹤的實(shí)時(shí)性要求,其次針對(duì)其運(yùn)算復(fù)雜性的增加,隨著計(jì)算機(jī)技術(shù)發(fā)展,多核多任務(wù)的出現(xiàn),計(jì)算機(jī)完全能滿足其復(fù)雜性的增加。

4 結(jié)論

通過(guò)對(duì)UEPF的分析及在純方位跟蹤中的實(shí)驗(yàn)仿真可得出:傳統(tǒng)的粒子濾波方法由于其“退化”問(wèn)題,不能很好地應(yīng)用于純方位跟蹤問(wèn)題中。采用EKF、UKF混合作為建議分布應(yīng)用于粒子濾波中可以很好的減少粒子退化問(wèn)題,并且增加粒子的多樣性以及提高濾波精度。該算法在每次遞推產(chǎn)生新的粒子時(shí)充分考慮了當(dāng)前時(shí)刻的量測(cè),使得該算法能很好的利用量測(cè)帶來(lái)的信息,通過(guò)仿真實(shí)驗(yàn)證明了UEPF在純方位跟蹤應(yīng)用中具有良好的跟蹤性能。

[1]DENG Xiaolong,XIE Jianying,GUO Weizhong.Bayesian target tracking based on particle filter[J].Journal of Systems Engineering and Electronics,2005,16(3):545-549.[2]CHANG Cheng,Ansari Rashid.Kernel particle filter for visual tracking[J].IEEE Signal Processing Letters,2005,12(3):242-245.

[3]Merwe R,Doucet A,Freitas Nando de,et al.Theunscented particle filter[R].Cambridge University,engineering department:Technical report,CUED/F-INFENG/TR 380,2000.

[4]Julier SJ,Uhlmann J K.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE,2004,92(3):401-422.

[5]Wan Eric A,Merwe R.The unscented kalman filter for nonlinear estimation[C]//Proceedings of International Symposium on Adaptive Systems for Signal Processing,Communications and Control.Canada:Alberta,2000:153-158.

[6]Pitt M K,Shephard N.Filtering via simulation:auxiliary particle filters[J].Journal of the American Statistical Association,1999,94(2):590-599.

[7]Kotecha Jayesh H,Djuric Petar M.Gaussian particlefiltering[J].IEEE Transactions on signal processing,2003,51(10):2 592-2 601.

[8]Jayesh H Kotecha,Petar M Djuric.Gaussian sum particle filtering[J].IEEE Transactions on signal processing,2003,51(10):2 602-2 611.

[9]Lehn Schiler T,Erdogmus D,Principe J C.Parzen particle filters[C]//Proceedings of International Conference on Acoustics,Speech,and Signal Processing.Montreal,Canada:2004(V):781-784.

[10]袁澤劍,鄭南寧,賈新春.高斯-厄米特粒子濾波器[J].電子學(xué)報(bào),2003,31(7):970-973.YUAN Zejian,ZHENG Nanning,JIA Xinchun.The Gauss-Hermite Particle Filter[J].Acta Electronica Sinica,2003,31(7):970-973.

[11]Anderson B D O,Moore J B.Optimal Filtering.Englewood Cliffs[M].New Jersey:Prentice-Hail,1979.

[12]Welch G,Bishop G.An introduction to the Kalman filter[R].University of North Carolina at Chapel Hill:Technical Report TR 95-014,2004.

[13]Arulampalam M S,Maskell S,Gordon N,Clapp T.A tutorial on particle filters for on-linenonlinear/non-gaussian bayesian tracking[J].IEEE Transactions on Signal Processing,2002,50(2):174-188.