L2 正則化粒子濾波在水下無人平臺純方位角跟蹤的應用

田德艷，張小川,2，鄒司宸，馬士全，呂勇

(1. 青島海洋科學與技術試點國家實驗室，山東 青島 266237；2. 海軍潛艇學院，山東 青島 266199)

0 引 言

水下無人移動平臺搭載傳感器進行目標探測與跟蹤是當前的研究熱點與難點。水下滑翔機因其低功耗、長航時、低噪聲、強隱蔽性在海洋目標監測、探測方面應用廣泛。將水下聲學滑翔機作為移動觀測站可進行目標的探測與跟蹤，笪良龍教授團隊[1-4]在這方面已經取得了一定的成果。但僅依賴測量數據存在弊端，當測量數據異常突變、測量丟失以及故意干擾等情況下都會影響跟蹤效果，甚至導致跟蹤失敗。

卡爾曼濾波器作為傳統最優線性遞歸方法，綜合考慮測量值和估計值，在水下目標跟蹤中得到廣泛應用。但由于其基于參數的高斯濾波特性，不適合用于水下運動目標的非線性問題。對于非線性的水下目標跟蹤問題，粒子濾波以其序慣蒙特卡羅的思想，趨近于最優估計，取得了一定的研究成果。李天成等[5]對粒子濾波理論、發展脈絡以及研究進展做了很詳細的總結分析。宋緒棟等[6]研究了幾種適用于單、雙觀測站的水下目標純方位角跟蹤算法，并進行詳細的仿真分析，結果表明非靜止單觀測站雖不能獲得完全觀測，但在一定先驗信息條件下，可實現目標軌跡估計。張林琳等[7]結合水下目標的被動跟蹤應用背景，比較了擴展卡爾曼濾波、不敏卡爾曼濾波以及粒子濾波在水下目標跟蹤中的性能差異，表明粒子濾波能較好的用于非線性、非高斯條件下的水下目標跟蹤，但是粒子濾波隨著時間推移也存在粒子貧化問題導致目標跟蹤變差。針對粒子濾波貧化的問題，各種重采樣改進算法[8-19]相繼提出，金盛龍等[20]提出一種粒子濾波的聯合檢測與跟蹤方法，在狀態濾波過程中不需要方位觀測值的輸入，直接根據波束能量評估粒子的似然函數，結合交叉變異算子提高粒子多樣性，改善粒子貧化。此外，為了解決粒子濾波重采樣引入的采樣枯竭問題，正則化粒子濾波[21-23]（RPF）也被提取出來，RPF 利用概率密度原理，通過對每個粒子的核密度采樣來實現對整個連續近似分布的采樣，從而生成具有不同粒子位置的新粒子系統。

以上算法的改進確實在一定程度上改善了粒子貧化，減緩了粒子的發散，但是以上算法大都是基于一定的先驗知識。由于水下無人平臺在實際工作中，不知道自己的位置，更不知道目標初始位置的以及運動狀態信息的，故以上改進方法對實際應用帶來了一些限制條件。

為了解決以上存在問題，針對水下聲學滑翔機探測跟蹤系統，本文提出適用于該單站純方位角跟蹤測量系統的濾波方法。首先結合最小二乘擬合方法，由矢量水聽器前幾個周期接收數據經過復聲強器后，得到的目標方位信息，作為該系統的初始狀態，然后經過基于L2-RPF 算法，輸出該系統對水下目標跟蹤的結果。本文給出基于L2-RPF 的理論推導過程以及簡化形式，仿真和試驗結果表明，該方法相較于其他粒子濾波方法，能夠提高目標跟蹤精度，方位估計結果更接近真實值，對于真實環境具有較好的應用。

1 水下無人平臺跟蹤系統建模

1.1 常規單站單目標跟蹤系統建模

目標跟蹤方法都是基于模型的，以單個觀測站單個目標為例，在水下運動目標初始狀態信息以及觀測站初始狀態信息已知的前提條件下，可對該系統進行數學建模。

在笛卡爾坐標系下，目標的狀態轉移方程為：

其中：T 為采樣數據間隔， wk表示系統過程噪聲。

若目標作勻加速運動，則目標狀態量表示為：

在純方位角跟蹤系統中，測量量為Zk=[θk]， θk表示方位角，系統測量方程為：vk

其中：為觀測站的坐標， 為系統測量噪聲。

1.2 實際應用環境系統建模

上述單觀測站單目標探測系統基于一定的先驗知識，要預先知道觀測站與水下目標的初始狀態信息。但實際應用時，水下目標的初始位置及運動狀態都是未知的，因此，需要建立實際環境下的系統模型。結合實際項目需求，由水下聲學滑翔機作為觀測站，在水下作剖面滑翔進行探測跟蹤。在先驗知識缺失的條件下，矢量水聽器被動接收信號，轉換到頻率域，經過復聲強器后，輸出由測量值得到的探測方位角。

由初始一段時間測量值經過復聲強器得到的探測方位[角來擬合出]狀態方程，單矢量水聽器的輸出量表示為P,Vx,Vy,Vz，根據復聲強器法：

其中： θk表示k 時刻目標相對于平臺的方位角度；IRy,k表示k 時刻聲壓與振速y 通道的互譜；IRx,k表示k 時刻聲壓與振速x 通道的互譜；

考慮到正切函數在90°和270°趨于無窮大的情況，將系統的測量方程表示為正切函數和余弦函數的分段組合，具體為：

2 粒子濾波原理

粒子濾波（PF）是一種基于序慣重要性采樣（SIS）的序慣蒙特卡羅方法，其不受系統線性化誤差和高斯假設的限制，但存在計算量大、粒子退化問題（采樣粒子枯竭）嚴重。為解決粒子貧化，1993 年Gordon 等提出重采樣算法，重新分配所有粒子權值，但由于重采樣算法是基于大權值粒子的復制保留，這樣降低粒子多樣性。基于此，Gordon 提出基于貝葉斯采樣估計的重要性重采樣濾波算法（SIR），選擇最優的重要性密度函數改善粒子貧化和退化的問題。

根據上述系統模型，得到系統的狀態方程和測量方程，進而可得狀態轉移概率密度p(xk|xk-1)和似然概率密度p(zk|xk)。粒子濾波算法通過一個重要概率密度函數來近似估計未知狀態的后驗概率分布p(xk|zk)，從而估計該時刻的狀態變量。

該過程主要分為預測和更新兩階段。

預測：

PF 算法具體描述如下：

步驟1初始采樣。從初始先驗分布p(x0)中采樣得到樣本粒子集合

步驟2重要性采樣。從重要性概率密度函數q(xk|xk-1,zk)中采樣得到k 時刻粒子集 {，其中，表示k 時刻第i 個粒子的狀態值，表示k 時刻第i 個粒子的權值，重要性概率密度函數一般選取為狀態轉移概率密度函數p(xk|xk-1)。

步驟3更新權值。根據當前測量值 zk，可計算對應粒子集 {。

其中，p(zk|xk)為似然函數，根據測量值與先驗估計測量值以及測量過程噪聲方差重建高斯分布，作為似然函數，更新權值：

似然函數反映的是濾波器先驗預測的值與傳感器測量值的接近程度，兩值越接近，其權值越大；反之，權值越小。

歸一化權值：

假設，Nef f＜Nth重采樣得新粒子集。

步驟5狀態估計。

3 L2 正則化粒子濾波原理

RF 采用重要性重采樣算法來改善粒子的退化問題，但隨時間推移，大權重粒子被保留，小權重粒子被舍棄，導致粒子多樣性減少，估計精度下降。正則化粒子（RPF）濾波結合核函數理論，將離散分布問題轉化為連續分布問題，即根據密度估計理論計算出后驗連續概率密度分布，從中采樣得到粒子集，改善粒子退化。

RPF 是對后驗概率密度函數 的連續近似分布進行重采樣，從而得到粒子集：

基于正則化近似連續概率密度分布函數計算得到采樣正則化粒子和權值，具體描述如下：

步驟1利用概率密度原理，中核函數K(·)演變思想，通過對核密度采樣來實現對整個連續近似分布的采樣，生成新粒子集及對應權值中：NReg 為連續近似分布中采樣粒子個數，表示k 時刻第j 個新粒子的目標方位角，表示k 時刻第j 個新粒子的權值；是對核密度進行重新標度的核密度。

最佳核密度函數是Epanechnikov 核密度，表示為：

式中： cnx為為 Rnx內單位超球體的體積； nx為狀態量的維度。

其中A 表示粒子狀態協方差矩陣S 經過Cholesky 分解得到的上三角矩陣，若取p=1，表示L1 范數正則化；若取p=2，表示L2 范數正則化。

步驟3計算正則化粒子權值歸一化：

4 仿真研究

為了驗證所提出L2-RPF 算法的有效性，針對水下目標運動單站純方位角跟蹤這一典型非線性系統進行Monte Carlo 仿真分析，并將L1-RPF 與PF 算法在仿真初始條件相同情況下進行性能對比。

本實驗考慮遠場平面波模型，假設觀測站與跟蹤目標處于同一水平面上，且深度信息已知，采用地理坐標系，取正北方向為0°，只考慮x 和y 兩個方向的坐標。假設觀測平臺作勻速直線運動，目標作勻速和勻加速交替運動，在不同時間段觀測平臺和目標具體的運動狀態設置如表1 所示，觀測時間間隔為1 s，觀測總時長為110 s，觀測平臺初始狀態向量為Bx0=[-150,700,3,3,0,0]T，目標初始狀態向量為X0=[5,3,5,2,0,0]T，過程噪聲協方差為Q=diag[1,1.3,0.4,0.1,0.01,0.01]，初始協方差矩陣為P0=diag[1,1,0.1,0.1,0.01,0.01]，觀測平臺和目標軌跡如圖1 所示。

表 1 目標及平臺運動狀態Tab. 1 The movement status of target and platform

PF 算法粒子數N=100，L1-RPF 與L2-RPF 算法正則化粒子數NReg=5*N，粒子采樣子空間半徑為Net=1。采用3 種算法對水下目標進行方位跟蹤，跟蹤曲線圖2 所示。

分析可知，L1-RPF 算法估計的目標方位與真實方位基本保持一致，L2-PRF 算法次之，PF 算法隨著時間推移，濾波發散，估計方位與真實值偏離較大。

圖 1 目標及平臺軌跡圖Fig. 1The diagram of target and platform trajectory

圖 2 PF，L1-RPF，L2-RPF 方位跟蹤Fig. 2The bearing tracking of PF, L1-RPF, L2-RPF

為了進一步定量分析L1-RPF，L2-RPF 與PF 算法在此種工況系統下的方位跟蹤誤差，基于上述初始條件，將3 種算法進行50 次Monte Carlo 仿真實驗對比，選用平均誤差ME 和均方根誤差RMSE 來衡量誤差大小，其中ME 為多次Monte Carlo 實驗對應每個時刻目標方位估計值與真值絕對誤差的均值，表示為：

RMSE 為觀測值與真值偏差的平方和與實驗次數M 比值的平方根，表示為：

其中，M 表示Monte Carlo 實驗次數，xm,t表示第m 次實驗t 時刻方位真值，表示第m 次實驗t 時刻估計值。

得到觀測時間內的跟蹤誤差曲線圖如圖3 和圖4所示。

通過對比分析可以得到：

1）相同的初始仿真條件下，L2-RPF 算法的ME和RMSE 誤差小于L1-RPF 小于PF，相較于L1-RPF 算法和PF 算法，L2-RPF 算法跟蹤精度更高些；曲線抖動小，也說明該算法魯棒性更好些；

圖 3 ME errorFig. 3ME error

圖 4 RMSE errorFig. 4RMSE error

2）隨著時間推移，3 種算法的跟蹤誤差有不同程度變大，L2-RPF 跟蹤誤差小于L1-RPF 小于PF。PF 算法由于粒子貧化，濾波發散，導致跟蹤誤差變大；L2-RPF 算法跟蹤平均誤差在0.5°以內，均方根誤差在1°以內，相對較小。

5 海試數據處理與分析

利用在南海北部海域開展的水下聲學滑翔機平臺探測跟蹤性能驗證試驗數據，針對水下移動觀測平臺在海上對水下目標采用PF，L1-RPF，L2-RPF 三種算法的純方位角跟蹤性能。

試驗海區深度約為1 500 m，試驗期間氣象水文條件良好，風級約為2 級，XCTD 測量結果顯示，聲速剖面在深度40 m 以內是均勻層，深度40～200 m 范圍內為聲速主越變層，聲道軸在1 000 m 附近深度上。試驗中，水下聲學滑翔機在10:55 以24°俯仰角下潛向下在水下做剖面滑翔探測跟蹤目標，滑翔速度為1 kn，一個剖面時長大約4 h。在12:52～13:49 時間段內，1 艘船長42 m，船寬6 m，航速8.4 kn 的水面航船以301°航向與水下聲學滑翔機滑翔軌跡垂直而過，水下滑翔機與水面航船的態勢圖如圖5 所示。

采用PF，L1-RPF，L2-RPF 算法處理試驗數據，并根據水下聲學滑翔機推算位置和水面目標GPS 位置得到近似真實的方位，結果如圖6 所示。分析可知，通過復聲強器法得到的初始濾波結果，存在野點，且跟蹤精度不高，對比幾種算法和GPS 推算方位，L2-RPF算法和L1-RPF 算法要比PF 算法目標方位跟蹤精度相對較高，PF 算法由于粒子貧化，導致濾波器發散，影響收斂時間和估計精度。但由于RPF 算法是在測量值和估計值之間的均衡，測量值出現野點也會影響RPF 算法的方位跟蹤結果。從圖6 細節處可以看到，受野點的影響，RPF 算法也出現抖動，跟蹤效果稍差，分析這是濾波因子選取不當引起的濾波效果變差。

圖 5 水下平臺與目標位置態勢圖Fig. 5Situation map of underwater platform and target positon

圖 6 PF，L1-RPF，L2-RPF 算法目標方位跟蹤結果Fig. 6Target bearing tracking result of PF, L1-RPF, L2-RPF

6 語 結

本文所述針對水下聲學滑翔機對水下目標進行純方位角跟蹤的實際應用，采用PF，L1-RPF，L2-RPF 算法，理論仿真和試驗表明，L2-RPF 算法相較于其他粒子濾波算法，改善了粒子貧化，提高了目標跟蹤精度。L2-RPF 算法在水下聲學滑翔機目標探測跟蹤上的應用，為多水下無人平臺多目標的探測跟蹤奠定了基礎，對于水下無人平臺組網探測跟蹤水下目標有較好的應用前景。