基于改進粒子濾波算法的水下目標跟蹤

周偉江，董 博，許偉杰

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

0 引 言

目標運動分析(Target Motion Analysis，TMA)對于水下作業的開展具有重要意義。在被動測量的情況下，僅利用目標的方位信息來實時估計運動目標參數的過程被稱作純方位目標運動分析(Bearings-only Target Motion Analysis，BO-TMA)[1]。水下被動目標跟蹤問題具有非線性和弱可觀性的特點，這導致了算法處理困難。

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

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

1 常規粒子濾波

1.1 非線性濾波模型

通常動態系統的狀態空間模型可以用狀態轉移方程和狀態觀測方程進行描述：

式中：f(?)和h(?)分別表示狀態轉移函數和觀測函數，xk表示k時刻的系統狀態向量，zk表示k時刻的觀測向量，wk和vk分別表示過程噪聲和觀測噪聲。

1.2 粒子濾波算法

粒子濾波算法是通過非參數化的蒙特卡洛方法來實現遞推的貝葉斯估計，其實質是由加權的隨機樣本即粒子組成離散的隨機測度來近似相關概率分布[2-3]。

貝葉斯濾波包括預測和更新兩個步驟。在預測階段，需要估計狀態的先驗概率密度，在更新階段，使用新的觀測值來修正先驗概率密度，進而獲得后驗概率密度。

(1) 預測過程，根據獲得

如果狀態xk只與有關，可得：

對進行積分，可得:

其中表示0至k-1時刻的觀測向量.

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

在得到最新觀測值zk后：

假設觀測值只由狀態值決定，那么：

因此，

式中表示歸一化常數：

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

在粒子濾波中，假設可以從后驗概率密度中抽取N個相互獨立、分布相同的隨機樣本則后驗概率分布可以近似地表示為

其中，z0:k表示0至k時刻的觀測向量，當時，

然而，通常無法直接從后驗概率分布中采樣，因此引入一個便于采樣的重要性概率密度函數從中抽取樣本粒子

則后驗概率密度可以表示為

經過數次迭代后，只有少數粒子的權值較大，狀態空間中的有效粒子數較少，使得估計性能下降，需要進行重采樣的操作，以便改善權值退化的現象。

由此，任意函數的期望值估計可以表示為

1.3 常規粒子濾波算法流程

根據1.2節中粒子濾波算法原理，可以得到常規粒子濾波算法的流程如下：

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

步驟2：重要性采樣。對由重要性概率密度函數生成采樣粒子

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

2 改進的粒子濾波算法

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

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

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

下面先從信息論的角度對于粒子數目自適應機制的有效性加以說明，然后給出粒子數目自適應的算法。

2.1 Kullback-Leibler距離

KL(Kullback-Leibler)距離是評價數據之間偏差程度的測度，它的定義見式(14)[7]：

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

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

根據卡方分布[11]的分位數關系，得到式(16)：

式中表示具有個自由度的χ2分布表示自由度為k-1的1-δ分位數。

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

通常，由Wilson-Hilferty變換，可以得到n的近似計算表達式：

式中，z1-δ為標準正態分布的1-δ上側分位數。

2.2 基于KLD重采樣的改進粒子濾波

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

針對上述改進的粒子濾波算法，其算法流程如下：

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

步驟3：權值更新。計算粒子權值并進行歸一化。

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

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

如果，則跳轉到步驟4。否則轉到步驟5。

3 仿真實驗及分析

3.1 水下目標機動模型

純方位目標跟蹤中只利用含有噪聲的目標方位信息來估計目標的位置和速度參量。跟蹤系統的離散狀態方程和觀測方程可以分別表示為

其中為目標狀態向量；zk為方位的觀測向量；wk和υk分別為過程噪聲和觀測噪聲；F為狀態轉移矩陣；G為過程噪聲轉移矩陣，其中T為采樣周期。

F的選擇與目標的機動模型相關，常見的機動模型包括：恒速度模型(Constant Velocity，CV)、恒加速度模型(Constant Acceleration, CA)、恒轉向模型(Constant Turn，CT)、Singer模型、當前機動模型等。本文涉及到的是CV模型和CT模型[12]。

在CV模型下，狀態轉移矩陣F可以表示為式(21)：

在CT模型下，狀態轉移矩陣F可以表示為式(22)：

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

3.2 仿真與分析

為了驗證改進的粒子濾波算法在純方位水下目標跟蹤中的性能，我們分別使用常規粒子濾波算法和改進粒子濾波算法對于目標在不同機動狀態下的跟蹤效果進行了比較研究。

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

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

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

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

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

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

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

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

圖5 小區域尺寸閾值與粒子數和RMSE的關系Fig.5 Curves of particle numbers and RMSE in different bin size

4 結 論

本文針對常規粒子濾波過程中粒子數目不能自適應改變的問題，提出了一種基于KL距離重采樣的改進粒子濾波算法，它能夠保證在一定濾波精度前提下，自適應地調整粒子數目大小，提高了粒子濾波方法對環境的適應能力，并將該算法應用于水下目標的跟蹤問題中。通過仿真實驗，證明該方法在達到較好跟蹤效果的同時，避免了常規粒子濾波算法計算量膨脹的問題，工程實現簡單，適合實際應用。

參考文獻

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

[2] 朱志宇. 粒子濾波算法及其應用[M].北京: 科學出版社, 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] 常發亮, 馬麗, 劉增曉, 等. 復雜環境下基于自適應粒子濾波器的目標跟蹤[J]. 電子學報, 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]. 北京: 電子工業出版社.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] 武愛文, 馮衛國, 衛淑芝, 等. 概率論與數理統計[M]. 上海: 上海交通大學出版社, 2011.

[12] 孫仲康, 郭福成, 馮道旺, 等. 單站無源定位跟蹤技術[M]. 北京:國防工業出版社, 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.