曹占啟，孟華

（中國(guó)人民解放軍91388部隊(duì)廣東湛江524022）

實(shí)際作戰(zhàn)中潛艇聲納采用被動(dòng)工作方式可以實(shí)現(xiàn)對(duì)敵隱蔽攻擊，利用被動(dòng)聲納獲取攻擊目標(biāo)的方位，進(jìn)而估計(jì)出目標(biāo)的運(yùn)動(dòng)狀態(tài)參數(shù)的過(guò)程稱(chēng)為純方位跟蹤。由于被動(dòng)聲納只獲得來(lái)自目標(biāo)的含有噪聲的角度信息，因此純方位跟蹤的實(shí)質(zhì)是非線性估計(jì)的問(wèn)題。常用的非線性估計(jì)方法有擴(kuò)展卡爾曼濾波（EKF）、偽線性估計(jì)器和無(wú)味卡爾曼濾波（UKF）。擴(kuò)展卡爾曼濾波采用非線性函數(shù)在估計(jì)點(diǎn)附近進(jìn)行泰勒級(jí)數(shù)展開(kāi)，并用一個(gè)等價(jià)于常規(guī)卡爾曼濾波的近似矩陣來(lái)代替非線性函數(shù)。但是，這種局部線性化的方法會(huì)引起濾波值和協(xié)方差矩陣的增大，甚至導(dǎo)致濾波發(fā)散。文獻(xiàn)[1]指出了直角坐標(biāo)系內(nèi)的擴(kuò)展卡爾曼濾波器容易表現(xiàn)出不穩(wěn)定行為。文獻(xiàn)[2]提出采用偽線性濾波器對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)進(jìn)行估計(jì)，盡管偽線性濾波算法具有算法穩(wěn)定、計(jì)算簡(jiǎn)單和易于實(shí)現(xiàn)等特點(diǎn)，但其估計(jì)值是有偏的[3]。無(wú)味卡爾曼濾波采用實(shí)際的非線性模型，用一個(gè)最小的樣本點(diǎn)集來(lái)近似系統(tǒng)狀態(tài)的分布函數(shù)，理論上能夠捕捉到任何非線性函數(shù)后驗(yàn)均值與方差的二階項(xiàng)，特別適合于解決高階非線性問(wèn)題。但UKF方法仍然是用一個(gè)高斯隨機(jī)變量來(lái)表征系統(tǒng)狀態(tài)分布，在非線性非高斯條件下，這種基于模型線性化和高斯假設(shè)的方法在估計(jì)系統(tǒng)狀態(tài)和方差時(shí)的誤差仍然較大，并有可能引起發(fā)散。

對(duì)于水下被動(dòng)聲納的高測(cè)量噪聲及純方位被動(dòng)跟蹤的弱可觀測(cè)性，基于模型線性化的方法不能取得好的跟蹤效果。粒子濾波算法是一種基于仿真的統(tǒng)計(jì)濾波方法，不受模型線性和高斯假設(shè)的約束，適用于任意非線性非高斯的隨機(jī)系統(tǒng)。本文將粒子濾波算法應(yīng)用到水下目標(biāo)的被動(dòng)跟蹤，并通過(guò)仿真計(jì)算與EKF和UKF算法的跟蹤性能進(jìn)行了比較，結(jié)果表明PF算法的確比EKF和UKF算法有更好的跟蹤性能。

1 問(wèn)題描述

系統(tǒng)變化過(guò)程可以由動(dòng)態(tài)系統(tǒng)模型來(lái)表述，非線性非高斯?fàn)顟B(tài)狀態(tài)空間模型是最常見(jiàn)的動(dòng)態(tài)系統(tǒng)模型，其基本的描述形式如下公式所示：

其中，wt和et分別為獨(dú)立的過(guò)程誤差和測(cè)量誤差，它們是相對(duì)獨(dú)立的隨機(jī)噪聲過(guò)程。函數(shù)f和h描述了狀態(tài)變量和測(cè)量變量隨時(shí)間變換的關(guān)系，由于過(guò)程噪聲和測(cè)量噪聲的引入和實(shí)際過(guò)程中簡(jiǎn)化程度不同，使該模型有不同的表示形式。假定觀測(cè)平臺(tái)和目標(biāo)艇在同一平面，純方位跟蹤態(tài)勢(shì)簡(jiǎn)化為二維平面，其幾何態(tài)勢(shì)如圖1所示。

直角坐標(biāo)系下，觀測(cè)站的位置和速度分別為（rox，roy）和（vox，voy），目標(biāo)艇的位置和速度分別為（rtx，rty）和（vtx，vty）。其中rx、ry為目標(biāo)和觀測(cè)站在X軸、Y軸位置分量，vx、vy為目標(biāo)和觀測(cè)站在X軸、Y軸位置速度分量。目標(biāo)與觀測(cè)站之間的相對(duì)距離和相對(duì)速度分別為r=rt-ro=（rx，ry）T和v=vt-vo=（vx，vy）T。則t時(shí)刻系統(tǒng)的狀態(tài)變量矩陣為

圖1 觀測(cè)平臺(tái)與目標(biāo)幾何態(tài)勢(shì)圖Fig.1 Typical geometry scenario with observation platform and the target geometry tren

在上述的定義和標(biāo)記下，系統(tǒng)的狀態(tài)方程為：

其中A（t，t0）為系統(tǒng)狀態(tài)轉(zhuǎn)移矩。

定義z（t）為傳感器獲得被加性量測(cè)噪聲污染的目標(biāo)角度測(cè)量值，則系統(tǒng)的觀測(cè)方程由式（6）表示。

水下目標(biāo)純方位跟蹤問(wèn)題就是通過(guò)傳感器獲得的含有噪聲的方位序列值z(mì)（t）來(lái)估計(jì)出目標(biāo)的運(yùn)動(dòng)狀態(tài)x（t）。

2 極坐標(biāo)下純方位跟蹤的系統(tǒng)方程

對(duì)于僅含有角度量測(cè)的目標(biāo)運(yùn)動(dòng)分析，采用極坐標(biāo)系能比直角坐標(biāo)系更好的穩(wěn)定性[4]。極坐標(biāo)下，我們用方位角，方位角率，距離導(dǎo)數(shù)和距離變化率與距離比值表示系統(tǒng)狀態(tài)方程，如下式（7）所示：

通過(guò)y（t）對(duì)微分及直角坐標(biāo)系到極坐標(biāo)系映射變換，得到極坐標(biāo)系下的狀態(tài)方程

其中

和直角坐標(biāo)系下正好相反，極坐標(biāo)下量測(cè)方程為線性方程

其中，hy（y（t））=[0 0 1 0]y（t），公式（8）和公式（17）構(gòu)成了極坐標(biāo)系下的系統(tǒng)方程。

3 算法原理及流程

3.1 粒子濾波算法原理

粒子濾波是一種基于蒙特卡羅和遞推貝葉斯估計(jì)的濾波方法[5]。該方法首先依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布，在狀態(tài)空間產(chǎn)生一組隨機(jī)樣本集合，這些樣本稱(chēng)為粒子，然后根據(jù)觀測(cè)量不斷地調(diào)整粒子的權(quán)重和位置，通過(guò)調(diào)整后的粒子的信息，修正最初的經(jīng)驗(yàn)條件分布。

假定t-t0為常數(shù)k，則貝葉斯框架下的最優(yōu)濾波就在每個(gè)時(shí)刻k[6]，利用所獲得觀測(cè)信息求得狀態(tài)的后驗(yàn)概率密度函數(shù)p（xk|Zk），從而得到k時(shí)刻的狀態(tài)估計(jì)，即：

因此，構(gòu)造后驗(yàn)概率密度函數(shù)p（xk|Zk）就成為了一個(gè)必要的過(guò)程。對(duì)于大多數(shù)情況下，后驗(yàn)概率密度函數(shù)的其解析解是不存在的，可以近似算法進(jìn)行逼近。粒子濾波采用蒙特卡洛積分方法進(jìn)行近似求解，其收斂性通過(guò)大數(shù)定理得到保證。

3.2 水下被動(dòng)目標(biāo)跟蹤粒子濾波算法流程

水下被動(dòng)目標(biāo)的粒子濾波算法可以通過(guò)一下幾個(gè)步驟來(lái)完成：

步驟1：初始化

從先驗(yàn)概率密度p（x0）隨機(jī)抽取N個(gè)粒子x10，x10，…xN0，權(quán)重w（i）0均為1/N，i=1，…，N；

步驟2：預(yù)測(cè)

利用系統(tǒng)方程預(yù)測(cè)狀態(tài)的先驗(yàn)概率密度。k-1時(shí)刻的后驗(yàn)概率密度p（xk-1|Zk-1）可以由樣本粒子～近似重構(gòu)。假設(shè)已知，其中p（x0|Z0）=p（x0）。則通過(guò)C-K方程可得到k時(shí)刻狀態(tài)的先驗(yàn)概率密度：

其中，δ（·）是狄克拉函數(shù)。由式（19）和式（20）可以得到k時(shí)刻先驗(yàn)概率密度分布狀態(tài)采樣x（i）k～π（xk|x0：k-1，z1：k）。

步驟3：更新

用k時(shí)刻的量測(cè)值z(mì)k來(lái)修正狀態(tài)的先驗(yàn)概率密度，從而得到該時(shí)刻的后驗(yàn)概率密度。假定k時(shí)刻樣本的重要性函數(shù)能夠進(jìn)行分解，使得

那么就可以利用已有樣本以及新的狀態(tài)采用得到樣本x（i）0:k～π（x0：k|z1：k）。利用貝葉斯公式計(jì)算更新后的重要性權(quán)重。

只考慮濾波的情況下，那么重要性密度函數(shù)僅僅依賴于xk-1和zk，因此，只要存儲(chǔ)x（i）k就可以了，因此修正后的權(quán)值為

歸一化的權(quán)值為

步驟4：重采樣

粒子濾波的一個(gè)重要缺陷是算法存在粒子匱乏現(xiàn)象，隨著迭代次數(shù)的增加，只有一個(gè)粒子的權(quán)重非常大，其他粒子的權(quán)重非常小，失去了粒子的多樣性，從而導(dǎo)致算法導(dǎo)致發(fā)散現(xiàn)象，采用重采樣能夠有效緩解粒子匱乏程度。這里采用文獻(xiàn)7給定方法計(jì)算有效樣本數(shù)，如果有效粒子數(shù)超過(guò)一定程度，則不需要重采樣，直接進(jìn)行步驟5；否則可以采用不同的重采樣算法對(duì)相應(yīng)的權(quán)值進(jìn)行重采樣，得到新的粒子集{xik，1/N}。

步驟5：狀態(tài)估計(jì)

4 算法仿真分析

本文以EKF和UKF算法對(duì)比，來(lái)驗(yàn)證PF算法的濾波性能。假定目標(biāo)和觀察站在同一平面運(yùn)動(dòng)，目標(biāo)做勻速運(yùn)動(dòng)，初始狀態(tài)設(shè)置為[30 km，0.004 km/s，10 km，0.003 km/s]。觀測(cè)站做蛇形行機(jī)動(dòng)；速度為vo=0.003 km/s其他仿真參數(shù)設(shè)置如下：采樣周期T=10s，測(cè)量角度標(biāo)準(zhǔn)差σ=6 mrad；UKF的尺度設(shè)定為α=1；β=2；κ=0；PF中粒子數(shù)目設(shè)定為N=10 000，有效粒子數(shù)Nth=2/3，采樣長(zhǎng)度L=100；蒙特卡洛仿真次數(shù)MC=100。

圖2 X軸方向均方根誤差曲線Fig.2 Rms error curve of X-axis direction

圖3 Y軸方向均方根誤差曲線Fig.3 Rms error curve of Y-axis direction

表1 3種算法均方根誤差比較Tab.1 Comparison of three algorithm s Rm s error

圖2、圖3給出了3種濾波算法的均方根曲線，從圖中我們看到粒子濾波估計(jì)精度優(yōu)于EKF和UKF算法，在100次蒙特卡洛仿真中，基于PF的算法沒(méi)有發(fā)散，可見(jiàn)極坐標(biāo)系下的濾波算法提高了算法的穩(wěn)定性。表1給出了3種算法的均方根誤差值和相對(duì)運(yùn)行時(shí)間。從計(jì)算時(shí)間上來(lái)看，PF明顯大于UKF和EKF，這也算粒子濾波的一個(gè)重要缺點(diǎn)，即以犧牲計(jì)算量換取計(jì)算精度。另外初始參數(shù)的選擇非常重要，合適的參數(shù)可以加快收斂速度。從仿真結(jié)果上來(lái)看，粒子濾波的估計(jì)精度和收斂速度還比較差，很多情況下不能滿足實(shí)際需求，因此需要進(jìn)一步研究改進(jìn)方法。

5 結(jié)論

文中介紹了基于粒子濾波水下被動(dòng)目標(biāo)運(yùn)動(dòng)狀態(tài)估計(jì)算法，該算法采用了極坐標(biāo)系下系統(tǒng)方程，通過(guò)遞歸貝葉斯估計(jì)目標(biāo)狀態(tài)的后驗(yàn)概率分布。典型跟蹤態(tài)勢(shì)下三種非線性濾波算法仿真對(duì)比表明，粒子濾波算法提高了跟蹤精度和穩(wěn)定性。但是，粒子濾波本身存在缺陷，如計(jì)算時(shí)間過(guò)長(zhǎng)，下一步將會(huì)對(duì)重采樣過(guò)程進(jìn)行改進(jìn)，提高重采樣效率，縮短濾波時(shí)間，使算法更好地實(shí)現(xiàn)對(duì)運(yùn)動(dòng)目標(biāo)的精確跟蹤。

[1] Aidala V J.Kalman filter Behavior in bearing-only tracking applications[J].IEEE Transactions on Aerospace and Engineering Systems，1979（AES-15）：29-39.

[2] Lindgren A G，Gong K F.Position and velocity estimation via bearing observations[J].IEEE Transactions on Aerospace and Engineering Systems，1979（AES-14）：564-577.

[3] Aidala V J and Nardone S C.Biased Estimation Properties of the Pseudolinear Tracking Filter[J].IEEE Trans on AES，1982，18（4）:432-441.

[4] 胡仕強(qiáng)，敬忠良.粒子濾波原理及其應(yīng)用[M].北京：科學(xué)出版社，2010.

[5] Crisan D，Doucet A.A survey of convergence results on particle filtering methods for practitioners[J].IEEE Trans.On Signal Processing，2002，50（2）:736-746.

[6] Bar-Shalom Y，Kirubarajan T.Estimation with Applications to Tracking and Navigation[M].New York:John Wiley&Sons，2001.

[7] Liu J S，Chen R.Sequential Monte Carlo methods for dyamicl systems[J].Journal of the American Statistical Association，1998（93）:1032-1044.