基于改進(jìn)粒子濾波算法的水下目標(biāo)跟蹤

周偉江，董 博，許偉杰

(1. 中國(guó)人民解放軍92493部隊(duì)，遼寧葫蘆島125000；2. 中國(guó)科學(xué)院聲學(xué)研究所東海研究站，上海200032)

0 引 言

目標(biāo)運(yùn)動(dòng)分析(Target Motion Analysis，TMA)對(duì)于水下作業(yè)的開(kāi)展具有重要意義。在被動(dòng)測(cè)量的情況下，僅利用目標(biāo)的方位信息來(lái)實(shí)時(shí)估計(jì)運(yùn)動(dòng)目標(biāo)參數(shù)的過(guò)程被稱作純方位目標(biāo)運(yùn)動(dòng)分析(Bearings-only Target Motion Analysis，BO-TMA)[1]。水下被動(dòng)目標(biāo)跟蹤問(wèn)題具有非線性和弱可觀性的特點(diǎn)，這導(dǎo)致了算法處理困難。

粒子濾波是一種基于蒙特卡洛(Monte Carlo)和遞推貝葉斯估計(jì)的方法，不受模型非線性和非高斯噪聲的限制。理論上只要粒子足夠多，就能較為精確的表達(dá)觀測(cè)量及狀態(tài)的后驗(yàn)分布。但根據(jù)大量的研究和實(shí)驗(yàn)表明，粒子數(shù)目能夠顯著影響粒子濾波算法的性能及其復(fù)雜度。采用較多的粒子數(shù)可以獲得良好的濾波效果，但其復(fù)雜度可能對(duì)于系統(tǒng)資源提出挑戰(zhàn)。在保證一定濾波精度的前提下，應(yīng)盡量減少粒子數(shù)目[2-6]。

本文將KL距離(Kullback-Leibler Divergence)[7]引入粒子濾波重采樣的過(guò)程中，改進(jìn)了常規(guī)粒子濾波算法中粒子數(shù)目不能自適應(yīng)改變的問(wèn)題，在保證濾波性能的同時(shí)提高了計(jì)算效率，并在純方位水下目標(biāo)跟蹤情況下，對(duì)所提算法進(jìn)行了仿真實(shí)驗(yàn)，取得了較好的跟蹤性能。

1 常規(guī)粒子濾波

1.1 非線性濾波模型

通常動(dòng)態(tài)系統(tǒng)的狀態(tài)空間模型可以用狀態(tài)轉(zhuǎn)移方程和狀態(tài)觀測(cè)方程進(jìn)行描述：

式中：f(?)和h(?)分別表示狀態(tài)轉(zhuǎn)移函數(shù)和觀測(cè)函數(shù)，xk表示k時(shí)刻的系統(tǒng)狀態(tài)向量，zk表示k時(shí)刻的觀測(cè)向量，wk和vk分別表示過(guò)程噪聲和觀測(cè)噪聲。

1.2 粒子濾波算法

粒子濾波算法是通過(guò)非參數(shù)化的蒙特卡洛方法來(lái)實(shí)現(xiàn)遞推的貝葉斯估計(jì)，其實(shí)質(zhì)是由加權(quán)的隨機(jī)樣本即粒子組成離散的隨機(jī)測(cè)度來(lái)近似相關(guān)概率分布[2-3]。

貝葉斯濾波包括預(yù)測(cè)和更新兩個(gè)步驟。在預(yù)測(cè)階段，需要估計(jì)狀態(tài)的先驗(yàn)概率密度，在更新階段，使用新的觀測(cè)值來(lái)修正先驗(yàn)概率密度，進(jìn)而獲得后驗(yàn)概率密度。

(1) 預(yù)測(cè)過(guò)程，根據(jù)獲得

如果狀態(tài)xk只與有關(guān)，可得：

對(duì)進(jìn)行積分，可得:

其中表示0至k-1時(shí)刻的觀測(cè)向量.

(2) 更新過(guò)程，需要通過(guò)得到

在得到最新觀測(cè)值z(mì)k后：

假設(shè)觀測(cè)值只由狀態(tài)值決定，那么：

因此，

式中表示歸一化常數(shù)：

在貝葉斯濾波遞推的過(guò)程中需要計(jì)算積分，粒子濾波就是其中一種估計(jì)方法。

在粒子濾波中，假設(shè)可以從后驗(yàn)概率密度中抽取N個(gè)相互獨(dú)立、分布相同的隨機(jī)樣本則后驗(yàn)概率分布可以近似地表示為

其中，z0:k表示0至k時(shí)刻的觀測(cè)向量，當(dāng)時(shí)，

然而，通常無(wú)法直接從后驗(yàn)概率分布中采樣，因此引入一個(gè)便于采樣的重要性概率密度函數(shù)從中抽取樣本粒子

則后驗(yàn)概率密度可以表示為

經(jīng)過(guò)數(shù)次迭代后，只有少數(shù)粒子的權(quán)值較大，狀態(tài)空間中的有效粒子數(shù)較少，使得估計(jì)性能下降，需要進(jìn)行重采樣的操作，以便改善權(quán)值退化的現(xiàn)象。

由此，任意函數(shù)的期望值估計(jì)可以表示為

1.3 常規(guī)粒子濾波算法流程

根據(jù)1.2節(jié)中粒子濾波算法原理，可以得到常規(guī)粒子濾波算法的流程如下：

步驟1：初始化。對(duì)于i= 1 ,2,… ,N，由先驗(yàn)概率p(x0) 生成初始粒子集合

步驟2：重要性采樣。對(duì)由重要性概率密度函數(shù)生成采樣粒子

步驟4：重采樣。計(jì)算有效粒子數(shù)判斷是否需要進(jìn)行重采樣。如不需要，則進(jìn)行步驟5；否則，對(duì)粒子集合進(jìn)行重采樣，重采樣之后的粒子集合為

2 改進(jìn)的粒子濾波算法

常規(guī)前述的粒子濾波算法在計(jì)算過(guò)程中，粒子數(shù)目是保持不變的，恒定的樣本容量會(huì)直接影響到算法的實(shí)時(shí)性和計(jì)算精度。較大的粒子數(shù)目會(huì)帶來(lái)精度的提升，但同時(shí)也會(huì)增加計(jì)算負(fù)擔(dān)，不滿足應(yīng)用中的實(shí)時(shí)性需求；反之，較小的粒子數(shù)目雖然加快了計(jì)算速度，但又不能滿足濾波精度的需求。因此，選擇合適的濾波性能測(cè)度，使得粒子濾波算法在估計(jì)過(guò)程中能根據(jù)測(cè)度自適應(yīng)地改變粒子數(shù)目是非常重要的。

華盛頓大學(xué)的Fox在文獻(xiàn)[8]中，提出了一種基于KLD采樣的自適應(yīng)粒子濾波算法，利用KL距離來(lái)描述粒子濾波的近似誤差，根據(jù)樣本的近似分布與真實(shí)分布之間的KL距離來(lái)調(diào)整粒子數(shù)目。該方法隱含地假設(shè)樣本從真實(shí)分布中抽取，并沒(méi)有考慮真實(shí)后驗(yàn)分布與重要性概率密度分布之間的差異。

本文提供了一種改進(jìn)的算法，在粒子濾波重采樣的過(guò)程中引入了 KLD方法。該算法確定了需要進(jìn)行重采樣的粒子數(shù)目，以保證在重采樣前和重采樣后粒子分布的KL距離不超過(guò)某一差異閾值。從而，當(dāng)概率密度集中在狀態(tài)空間中的某區(qū)域時(shí)，需要重采樣較少的粒子數(shù)，而當(dāng)概率密度分散在狀態(tài)空間的絕大部分區(qū)域時(shí)，需要重采樣較多的粒子數(shù)。

下面先從信息論的角度對(duì)于粒子數(shù)目自適應(yīng)機(jī)制的有效性加以說(shuō)明，然后給出粒子數(shù)目自適應(yīng)的算法。

2.1 Kullback-Leibler距離

KL(Kullback-Leibler)距離是評(píng)價(jià)數(shù)據(jù)之間偏差程度的測(cè)度，它的定義見(jiàn)式(14)[7]：

K可以用來(lái)表示不同的概率分布p1與q1之間的接近程度，該值越小，表示p1越接近于q1。

根據(jù)文獻(xiàn)[8-10]中的推導(dǎo)，使用p1(x)表示基于樣本的最大似然后驗(yàn)概率分布，q1(x)表示真實(shí)的后驗(yàn)概率分布，為了保證它們之間的KL距離不超過(guò)一定的差異閾值ε，樣本數(shù)目n應(yīng)該滿足如式(15)所示的關(guān)系：

根據(jù)卡方分布[11]的分位數(shù)關(guān)系，得到式(16)：

式中表示具有個(gè)自由度的χ2分布表示自由度為k-1的1-δ分位數(shù)。

于是選擇樣本數(shù)目n如式(17)，就可以1-δ的概率保證基于樣本的最大似然后驗(yàn)概率分布與真實(shí)的后驗(yàn)概率分布之間的KLD不超過(guò)ε，即

通常，由Wilson-Hilferty變換，可以得到n的近似計(jì)算表達(dá)式：

式中，z1-δ為標(biāo)準(zhǔn)正態(tài)分布的1-δ上側(cè)分位數(shù)。

2.2 基于KLD重采樣的改進(jìn)粒子濾波

本文的改進(jìn)算法在重采樣的過(guò)程中，使用式(18)確定需要進(jìn)行重采樣的粒子數(shù)目。將粒子集分割成多個(gè)小區(qū)域{bk'}，然后根據(jù)粒子的權(quán)重進(jìn)行分段重采樣，直到粒子數(shù)目滿足式(18)的要求。這就保證了粒子數(shù)目能夠與概率密度在狀態(tài)空間的集中程度相一致。同時(shí)，由于改進(jìn)算法是在重采樣的過(guò)程中參照粒子權(quán)重采用 KLD方法，正好符合從后驗(yàn)概率分布中抽取，從而避免了上文提到的之前KLD采樣當(dāng)中的問(wèn)題。

針對(duì)上述改進(jìn)的粒子濾波算法，其算法流程如下：

步驟1：初始化。對(duì)于由先驗(yàn)概率p(x0)生成初始粒子集合輸入預(yù)測(cè)后驗(yàn)概率密度與真實(shí)后驗(yàn)概率密度間的KL距離差異閾值ε，標(biāo)準(zhǔn)正態(tài)分布的上分位數(shù)z1-δ，小區(qū)域尺寸閾值L，最大粒子數(shù)目Nmax，k時(shí)刻的預(yù)測(cè)粒子集合Pk=Φ，k'=0，n=0。

步驟3：權(quán)值更新。計(jì)算粒子權(quán)值并進(jìn)行歸一化。

步驟4：重采樣。根據(jù)權(quán)值對(duì)于表示的粒子集合進(jìn)行抽取，將新的粒子添加到集合中，更新抽取粒子數(shù)n，令

判斷新抽取的粒子是否屬于空的小區(qū)域bk'，如果屬于，則更新區(qū)域序號(hào)k'，令k'=k'+1；將bk'標(biāo)識(shí)為已進(jìn)行重采樣區(qū)域；根據(jù)式(18)計(jì)算樣本數(shù)目N'：

如果，則跳轉(zhuǎn)到步驟4。否則轉(zhuǎn)到步驟5。

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

3.1 水下目標(biāo)機(jī)動(dòng)模型

純方位目標(biāo)跟蹤中只利用含有噪聲的目標(biāo)方位信息來(lái)估計(jì)目標(biāo)的位置和速度參量。跟蹤系統(tǒng)的離散狀態(tài)方程和觀測(cè)方程可以分別表示為

其中為目標(biāo)狀態(tài)向量；zk為方位的觀測(cè)向量；wk和υk分別為過(guò)程噪聲和觀測(cè)噪聲；F為狀態(tài)轉(zhuǎn)移矩陣；G為過(guò)程噪聲轉(zhuǎn)移矩陣，其中T為采樣周期。

F的選擇與目標(biāo)的機(jī)動(dòng)模型相關(guān)，常見(jiàn)的機(jī)動(dòng)模型包括：恒速度模型(Constant Velocity，CV)、恒加速度模型(Constant Acceleration, CA)、恒轉(zhuǎn)向模型(Constant Turn，CT)、Singer模型、當(dāng)前機(jī)動(dòng)模型等。本文涉及到的是CV模型和CT模型[12]。

在CV模型下，狀態(tài)轉(zhuǎn)移矩陣F可以表示為式(21)：

在CT模型下，狀態(tài)轉(zhuǎn)移矩陣F可以表示為式(22)：

其中：ω為角速度，T為采樣周期。

3.2 仿真與分析

為了驗(yàn)證改進(jìn)的粒子濾波算法在純方位水下目標(biāo)跟蹤中的性能，我們分別使用常規(guī)粒子濾波算法和改進(jìn)粒子濾波算法對(duì)于目標(biāo)在不同機(jī)動(dòng)狀態(tài)下的跟蹤效果進(jìn)行了比較研究。

仿真時(shí)長(zhǎng)為100 s，采樣周期1 s，粒子數(shù)目N為1000。目標(biāo)初始位于(1000，1000) m處，開(kāi)始按照CV模型以初始速度(15,-10) m.s-1做勻速直線運(yùn)動(dòng)，60 s后按照CT模型改做2.5°/s的勻速轉(zhuǎn)彎運(yùn)動(dòng)。系統(tǒng)噪聲和觀測(cè)噪聲均為獨(dú)立的零均值高斯白噪聲，系統(tǒng)噪聲的距離均方差σr為10-1m，速度均方差σv為10-2m.s-1，方位角的觀測(cè)均方差為0.5°。

在上述條件下進(jìn)行500次蒙特卡洛仿真，獲得改進(jìn)粒子濾波算法與常規(guī)粒子濾波算法對(duì)目標(biāo)的軌跡跟蹤曲線如圖1所示；按照公式(23)定義的目標(biāo)位置均方根誤差(Root Mean Square Error，RMSE)，兩種算法的RMSE對(duì)比曲線如圖2所示；計(jì)算所需的粒子數(shù)目的對(duì)比曲線如圖3所示。

由圖1可知，采用改進(jìn)的自適應(yīng)粒子濾波算法，跟蹤曲線與真實(shí)軌跡吻合度較高，能有效地完成跟蹤任務(wù)；由圖2和3可知，在第一部分勻速直線運(yùn)動(dòng)中(60s以前)，常規(guī)粒子濾波算法和改進(jìn)粒子濾波算法均能獲得較好的誤差性能，但是改進(jìn)型粒子濾波所需要的粒子數(shù)目更少，能夠節(jié)省更多的計(jì)算資源；當(dāng)進(jìn)入第二部分(60s以后)勻速轉(zhuǎn)彎階段，兩種算法的誤差均有所增大，但是改進(jìn)型粒子濾波算法自適應(yīng)地增大了粒子數(shù)目，更好地控制了誤差(見(jiàn)圖2)，在機(jī)動(dòng)狀態(tài)改變的過(guò)程中體現(xiàn)了較好的效果。由此可見(jiàn)，改進(jìn)的粒子濾波算法能夠有效地適應(yīng)水下跟蹤的多變環(huán)境，具有良好的跟蹤性能。

圖1 兩種粒子濾波算法跟蹤曲線Fig.1 Target tracking scenes of two different methods

圖2 兩種粒子濾波算法的RMSE比較Fig.2 RMSE Comparison of two different methods

圖3 兩種粒子濾波算法所需的粒子數(shù)目比較Fig.3 Comparison of particle numbers in two different methods

在改進(jìn)的粒子濾波算法中，計(jì)算所需的平均粒子數(shù)目與KL距離的差異閾值ε以及小區(qū)域尺寸閾值L有很大的關(guān)系。設(shè)置不同的參數(shù)取值，進(jìn)行500次蒙特卡洛仿真，得到結(jié)果如圖4和圖5所示。由圖4可知，隨著所設(shè)置的差異閾值不斷變大，改進(jìn)的粒子濾波算法所需要的粒子數(shù)目逐漸變小(見(jiàn)圖4(a))，同時(shí)均方根誤差逐漸增大(見(jiàn)圖4(b))。圖5表明，隨著設(shè)置的小區(qū)域尺度閾值不斷變大，粒子數(shù)目逐漸變小(見(jiàn)圖5(a))，均方根誤差變大(見(jiàn)圖5(b))，但是改變小區(qū)域尺度閾值對(duì)于粒子數(shù)目的影響不如改變KL距離的差異閾值對(duì)粒子數(shù)目的影響大。因此，可以根據(jù)系統(tǒng)對(duì)于精度的需求，選擇合適的差異閾值和尺度閾值，從而兼顧效率與精度。

圖4 KL距離差異閾值與粒子數(shù)和RMSE的關(guān)系Fig.4 Curves of particle numbers and RMSE in different KLD error bound threshold

圖5 小區(qū)域尺寸閾值與粒子數(shù)和RMSE的關(guān)系Fig.5 Curves of particle numbers and RMSE in different bin size

4 結(jié) 論

本文針對(duì)常規(guī)粒子濾波過(guò)程中粒子數(shù)目不能自適應(yīng)改變的問(wèn)題，提出了一種基于KL距離重采樣的改進(jìn)粒子濾波算法，它能夠保證在一定濾波精度前提下，自適應(yīng)地調(diào)整粒子數(shù)目大小，提高了粒子濾波方法對(duì)環(huán)境的適應(yīng)能力，并將該算法應(yīng)用于水下目標(biāo)的跟蹤問(wèn)題中。通過(guò)仿真實(shí)驗(yàn)，證明該方法在達(dá)到較好跟蹤效果的同時(shí)，避免了常規(guī)粒子濾波算法計(jì)算量膨脹的問(wèn)題，工程實(shí)現(xiàn)簡(jiǎn)單，適合實(shí)際應(yīng)用。

參考文獻(xiàn)

[1] MUSICKI D. Bearings only muti-sensor maneuvering target tracking[J]. Syetems and Control Letters, 2008, 57(3): 216-221

[2] 朱志宇. 粒子濾波算法及其應(yīng)用[M].北京: 科學(xué)出版社, 2010,1-10, 114-125.ZHU Zhiyu. Particle Filter Algorithm and Application[M]. Beijing: Science Press, 2010, 1-10, 114-125.

[3] ARULAMPALAM M S, MASKELL S, GORDON N, et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing,2002, 50(2): 174-188.

[4] 常發(fā)亮, 馬麗, 劉增曉, 等. 復(fù)雜環(huán)境下基于自適應(yīng)粒子濾波器的目標(biāo)跟蹤[J]. 電子學(xué)報(bào), 2006, 12(12): 2150-2153.CHANG Faliang, MA Li, LIU Zengxiao, et al. Target Tracking Based on Adaptive Particle Filter Under Complex Background[J].Acta Electronica Sinica, 2006, 12(12): 2150-2153.

[5] CLOSAS P, C. Fernandez-Prades. Particle filtering with adaptive number of particles[C]//IEEE Aerospace Conference, 2011, 1-7.

[6] CORNEBISE J, MOULINES é, OLSSON J. Adaptive methods for sequential importance sampling with application to state space models[J]. Statistics and Computing, 2008, 18(4): 461-480.

[7] 麥克爾里思. 信息論與編碼理論[M]. 北京: 電子工業(yè)出版社.2006.

[8] FOX D. Adapting the sample size in particle filters through KLD-Sampling[J]. International Journal of Robotics Research,2003, 22(12): 985-1003.

[9] SOTO A. Self adaptive Particle Filter[C]//Proceedings of International Joint Conferences on Artificial Intelligence, 2005, 1398-1406.

[10] LI T, SUN S, SATTAR T. Adapting sample size in particle filters through KLD-resampling, electronics letters[J]. Electronics Letters,2013, 49(12): 740-742.

[11] 武愛(ài)文, 馮衛(wèi)國(guó), 衛(wèi)淑芝, 等. 概率論與數(shù)理統(tǒng)計(jì)[M]. 上海: 上海交通大學(xué)出版社, 2011.

[12] 孫仲康, 郭福成, 馮道旺, 等. 單站無(wú)源定位跟蹤技術(shù)[M]. 北京:國(guó)防工業(yè)出版社, 2008, 205-220.SUN Zhongkang, GUO Fucheng, FENG Daowang, et al. Passive location and tracking technology by single observer[M]. Beijing:National Defend Industy Press, 2008, 205-220.