水下目標跟蹤的改進非線性濾波快速算法?

石桂欣 鄢社鋒 劉 宇

(1 中國科學院聲學研究所 北京 100190)

(2 中國科學院大學 北京 100049)

0 引言

水下目標跟蹤在保障海洋安全、維護海洋權益和開發(fā)海洋資源中具有重要意義，高精度的水下目標跟蹤算法一直是水聲信號處理領域的研究熱點和難點問題[1?3]。當狀態(tài)轉移模型和觀測模型已知時，采用濾波(比如卡爾曼濾波)算法對目標的狀態(tài)進行估計是最常用且有效的方法。卡爾曼濾波是線性高斯系統(tǒng)的最優(yōu)濾波器，但是實際的跟蹤系統(tǒng)往往含有非線性結構。擴展卡爾曼濾波(Extended Kalman filter,EKF)借助Jacobian 矩陣，將非線性系統(tǒng)進行一階泰勒近似，把非線性問題轉化為線性問題。但是EKF 只適用于系統(tǒng)非線性程度較弱的情況，且需要事先求解狀態(tài)方程和觀測方程的Jacobian 矩陣，給實際應用帶來不便?；跓o跡變換(Unscented transform,UT)，Julier 等[4?5]提出的無跡卡爾曼濾波(Unscented Kalman filter,UKF)突破了卡爾曼濾波的限制，適用于強非線性目標跟蹤系統(tǒng)。雖然UKF 需要調整參數(shù)以避免濾波發(fā)散，但是由于其高精度的估計能力，在過去二十多年里得到了廣泛的應用[6?7]。文獻[1,8–11]研究了基于UKF的水下純方位被動目標跟蹤算法。文獻[12]將自適應UKF 算法應用于深海條件下航行器的長基線定位和跟蹤。文獻[13]則進一步考慮了水下聲速不確定的移動長基線(Moving long baseline,MLBL)定位跟蹤場景，并使用UKF 算法來估計目標的狀態(tài)。最近，Arasaratnam 等[14]基于容積準則(Cubature rules)提出了一種新的濾波算法——容積卡爾曼濾波(Cubature Kalman filter,CKF)。CKF參數(shù)簡單，運算量稍小于UKF，也受到廣泛關注，并逐漸被應用于諸多場景[15?16]。文獻[17]提出了一種基于方差平方根的CKF 算法，用于魚雷的跟蹤。文獻[18]將CKF 算法應用于水下多UUV的協(xié)同定位場景中。文獻[19]研究了基于CKF 算法的水下航行器動基座對準技術，以提高初始對準的精度。為了降低水下長基線定位系統(tǒng)中航跡推算(Dead reckoning,DR)系統(tǒng)的累積誤差，文獻[20]基于Sage-Husa 最大后驗概率準則(Sage-Husa maximum a posterior)提出了一種自適應平方根CKF算法，在過程噪聲時變條件下比標準CKF算法估計精度更高。UKF 和CKF已經成為水下目標跟蹤中常用的算法。為了更好應用UKF 和CKF 這兩種水下目標跟蹤中常用的濾波算法，本文結合實際應用環(huán)境，對UKF 和CKF 進行了針對性研究。結合使用距離聯(lián)合方位信息對目標進行跟蹤的實際情況，改進提出了適用于測距誤差有偏的跟蹤算法，推導出兩種算法在運動方程線性、觀測方程非線性條件下的簡化形式并分析了算法的運算復雜度和目標軌跡跟蹤性能。

1 狀態(tài)-空間模型

假設系統(tǒng)在時刻k的狀態(tài)為Xk ∈Rnx，Zk ∈Rnz為對應狀態(tài)的觀測信號，nx為狀態(tài)變量的維數(shù)，nz為量測向量的維數(shù)。采用如下狀態(tài)空間模型描述離散化動態(tài)系統(tǒng)：

其中，f(·)為狀態(tài)函數(shù)，h(·)為量測函數(shù)；wk?1和vk分別為過程噪聲和量測噪聲。本文假設wk?1和vk都是服從高斯分布的零均值噪聲，其方差分別為Qk?1和Rk。

2 測距誤差有偏情況下的改進濾波算法及其簡化形式

2.1 改進濾波算法的狀態(tài)方程和測量方程

眾多文獻已經給出了常用UKF算法和CKF算法的主要步驟[4,14]，本文不再贅述。將其應用于目標跟蹤系統(tǒng)時，需要先建立狀態(tài)向量和運動模型，式(3)給出了一種常用的狀態(tài)向量：

其中，[Bx,By]是觀測節(jié)點的坐標，rk和θk分別是觀測節(jié)點對目標的測距和測向信息，vk是噪聲向量。常規(guī)方法是得到式(4)中的觀測信息Zk后，直接使用UKF或CKF算法進行跟蹤。

但是，常規(guī)方法未考慮距離信息往往是有偏的情況，這會引起額外的跟蹤誤差。為了提升目標跟蹤精度，定義εk為偏差系數(shù)，同時，將εk也看作狀態(tài)變量之一，構建一個新的狀態(tài)向量，記為Yk，

記?為采樣間隔，則狀態(tài)轉移矩陣為

狀態(tài)轉移方程為

其中，wk?1是噪聲向量。觀測信息服從以下模型：

其中，Hk=[0,0,0,0,0,0,1]，rk和θk定義同式(4)，εk是一個接近于0 的正或負的未知數(shù)，可能是常數(shù)，也可能隨時間變化。使用UKF 或者CKF 算法進行濾波后，即可得到當前時刻狀態(tài)的估計值根據(jù)上文分析易得收斂速度很慢。為了使算法更快收斂，每次濾波操作后，對k的值進行如下修正：

式(9)中，ζ為修正系數(shù)，Zk(1)為觀測向量中的第一項(距離信息)，k為利用估計出的距離信息。為區(qū)別于標準的UKF 和CKF 算法(使用Xk為狀態(tài)向量)，將本節(jié)改進提出的算法分別記為IUKF(Improved Unscented Kalman filter)算法和ICKF(Improved Cubature Kalman filter)算法(使用Yk為狀態(tài)向量)。

2.2 改進算法在UKF 框架下的簡化及復雜度分析

實際場景中，經常出現(xiàn)目標運動模型為線性方程、觀測模型為非線性方程的情況。對于非機動目標，跟蹤系統(tǒng)常采用的目標運動模型有勻速運動模型(Constant velocity,CV)、勻加速運動模型(Constant acceleration,CA)等；對于機動目標，常用的有Singer 模型、當前統(tǒng)計模型、半馬爾可夫模型等[21]。這些運動模型均可以寫成線性方程的形式。針對這些場景，本文進一步對改進算法進行簡化分析和推導。考慮將式(1)重寫為式(7)所示的線性運動模型，狀態(tài)轉移矩陣Φ是一個nx(nx是狀態(tài)向量的維數(shù))維的方陣，具體取值由運動模型決定。根據(jù)式(7)表示的狀態(tài)方程，可以將UKF算法中的一步預測做相應的化簡。記,i=0,1,2,··· ,2nx為濾波算法的采樣點，可得

易得

然后可得狀態(tài)預測協(xié)方差矩陣為

量測更新的步驟保持不變，即可得到簡化的UKF 算法，記為SUKF(Simplified Unscented Kalman filter)算法。2.1 節(jié)所提的改進算法顯然也可以使用該簡化步驟，記為IS-UKF(Improved simplified Unscented Kalman filter)算法?；喓蟮牟襟E采用矩陣-向量形式執(zhí)行時間更新過程，只需要執(zhí)行式(12)和式(15)兩步，即可得到狀態(tài)向量的一步預測值及對應的協(xié)方差矩陣。因此，在時間更新過程中，不必求2nx+1個Sigma 點，也就不再需要對k?1,k?1進行Cholesky分解的操作。SUKF的時間更新步驟中，含有約次乘法和加法操作。而標準UKF 算法大約需要次乘法和加法操作，以及一次nx維的Cholesky 分解(約含有次乘法和加法操作)。

顯然，ηS-UKF是關于nx/nz的單調遞減函數(shù)。根據(jù)前文分析可知，如果不考慮量測函數(shù)的運算量，當nz的取值接近nx時，ηS-UKF25%；當nz的取值很小(接近1)時，ηS-UKF50%。

2.3 改進算法在CKF 框架下的簡化及復雜度分析

類似地，易得相同條件下CKF 算法的簡化形式。

類似地，狀態(tài)向量的一步預測結果為

可得

因此狀態(tài)預測協(xié)方差矩陣為

量測更新的過程不變，即可得到簡化后的SCKF(Simplified Cubature Kalman filter)算法。SCKF 的時間更新過程只需執(zhí)行式(18)和式(20)兩步。相應地，2.1 節(jié)所提的改進算法顯然也可以使用該簡化步驟，并記為IS-CKF(Improved simplified Cubature Kalman filter)算法。顯然，SCKF 和SUKF 的預測更新過程是一樣的，且與卡爾曼濾波預測更新過程是相同的。這并不難理解，當系統(tǒng)的狀態(tài)方程為線性方程時，非線性濾波算法中的時間更新過程自動退化成卡爾曼濾波中的時間更新過程?？柭鼮V波是高斯線性系統(tǒng)的最優(yōu)濾波器，說明本文化簡后的結果是該條件下最優(yōu)的簡化算法。

由于CKF 和UKF 的算法結構非常相似，對于同樣的狀態(tài)變量(Xk或者Yk)，運算量也很接近，因此SCKF 和SUKF 的運算量也很接近。SCKF 降低運算復雜度的分析與前文中對SUKF 的分析是幾乎一樣的，在此不再贅述。類似地，記OCKF、OS-CKF分別為標準CKF、SCKF 的復雜度，定義ηS-CKF為SCKF相對于標準CKF運算效率的提升比例，即

顯然，ηS-CKF也是關于nx/nz的單調遞減函數(shù)。由于UKF 和CKF 的運算量比較接近，SUKF 和SCKF的運算量也很接近。因此ηS-CKF和ηS-UKF的取值范圍幾乎是一樣的。

下面以IS-CKF 算法為例，給出本文所提算法的主要步驟，如表1 所示。IS-UKF 與IS-CKF 算法結構相同，只是對量測更新部分做了相應更改。

表1 IS-CKF 算法步驟Table 1 Improved simplified CKF algorithm

3 仿真實驗與結果分析

首先使用仿真實驗驗證本文所提改進算法的性能。使用MATLAB軟件進行蒙特-卡洛仿真實驗來統(tǒng)計上述幾種算法的平均運行時間和均方根誤差(Root mean square error,RMSE)，并進行比較分析。

本實驗中認為節(jié)點和目標均處于同一水平面上，且深度信息均已知，只考慮x(東西)和y(南北)兩個方向的坐標。如圖1 所示，假設只有一個觀測節(jié)點(坐標[Bx,By]= [?100 m,700 m])對目標進行測距和測向。目標從原點處開始先做CV 運動，速度為vx=5 m/s，vy=3 m/s；5 s 后加速度為ax=?0.1 m/s2，ay=0.1 m/s2并持續(xù)到55 s；55～105 s 期間的加速度為ax=0.1 m/s2，ay=?0.1 m/s2；105～110 s 期間做CV 運動，速度為上一階段結束時(105 s)的目標速度。實驗中采用CA 模型對目標的運動狀態(tài)進行建模，本文所提改進的UKF 和CKF 算法(IS-UKF、IS-CKF和Improved UKF、Improved CKF)的狀態(tài)向量如式(5)所示，狀態(tài)轉移矩陣如式(6)所示，觀測方程為式(8)；常規(guī)UKF 和CKF 算法(SUKF、SCKF 和標準UKF、標準CKF)的狀態(tài)向量如式(3)所示，狀態(tài)轉移矩陣如式(22)所示，觀測方程為式(4)。

常規(guī)算法初始狀態(tài)向量為X0=[5,3,5,3,0,0]T，過程噪聲方差為Q=diag[1,1,0.1,0.1,0.01,0.01]，初始協(xié)方差矩陣為P0,0=diag[1,1,0.1,0.1,0.01,0.01]。UKF 算法中的參數(shù)為α=1，κ=2，β=0，λ=2，以下同。改進算法初始狀態(tài)向量為Y0=[5,3,5,3,0,0,0]T，過程噪聲方差為Q=diag[1,1,0.1,0.1,0.01,0.01,0.0001]，初始協(xié)方差矩陣為P0,0=diag[1,1,0.1,0.1,0.01,0.01,0.0001]。給定真實的εk=0.005，量測噪聲方差為R=diag[25 m2,5×10?6rad2]。修正系數(shù)為ζ=0.8。

圖1 目標軌跡Fig.1 Real trajectory of the target

圖2 給出了某一次實驗中SCKF 和IS-CKF 兩種算法估計的軌跡。由于引入了偏差系數(shù)εk對濾波結果進行修正，IS-CKF 的濾波精度得以提升。SUKF 和IS-UKF 的軌跡對比結果與圖2 類似，因此未再給出。表2和表3給出了這幾種算法進行500次蒙特-卡洛實驗后的均方根誤差、平均運行時間對比。由于簡化前后的算法精度基本不變，表3 只給出了IS-UKF、IS-CKF、SUKF、SCKF 四種算法的精度對比結果。該場景下，相比常規(guī)算法，改進算法的估計精度有一定的提升，稍高于常規(guī)算法，雖然狀態(tài)向量維數(shù)多了一維，但IS-UKF和IS-CKF算法的運行時間分別和相應的常規(guī)算法(標準UKF和標準CKF)相當。

表2 幾種算法的運行時間對比(仿真)Table 2 Comparations of running time among improved filters and conventional filters (Simulation)

表3 幾種算法的估計精度對比(仿真)Table 3 Comparations of RMSEs among improved filters and conventional filters(Simulation)

圖2 目標軌跡的估計結果對比Fig.2 Estimated trajectories by SCKF and ISCKF,respectively

4 千島湖實驗與結果分析

為了進一步驗證算法的效果，取2018年3月的一次主動探測湖上實驗數(shù)據(jù)進行改進算法的性能驗證分析。目標船的真實軌跡由差分GPS 給出，觀測節(jié)點的真實坐標由GPS 給出，且目標和觀測節(jié)點的深度已知，均為5 m。因此以下實驗研究仍然只考慮目標的二維平面運動狀態(tài)信息。本次實驗的觀測節(jié)點坐標為[?500 m,?394 m]。水聲測距信號間隔2 s 發(fā)送和采集一次，節(jié)點共采集到740 組距離觀測信息(方差約為25 m2)，方位信息由GPS 數(shù)據(jù)加高斯噪聲得到(噪聲均值為零、方差為4×10?6rad2)。該跟蹤系統(tǒng)狀態(tài)空間模型與第3 節(jié)仿真實驗中相同。常規(guī)算法和改進算法的初始狀態(tài)向量分別為X0=[105,?25,1,?1,0,0]T和Y0=[105,?25,1,?1,0,0,0]T，其他參數(shù)與第3 節(jié)相同。圖3 給出了目標的軌跡和節(jié)點的坐標。實驗中并未測量目標的速度和加速度信息，因此本節(jié)只對比位置坐標的精度，表4 和表5 分別給出了500次蒙特- 卡洛實驗后上述各個算法的平均運行時間和對應的RMSE。圖4 給出了某一次實驗中SCKF和IS-CKF兩種算法估計的軌跡，圖5給出該次實驗中IS-CKF 算法估計的?εk變化曲線。在該實驗場景下，改進算法的位置估計精度明顯優(yōu)于常規(guī)算法。另外，本文所提出的簡化算法分別與標準UKF、標準CKF 算法的精度相當，而平均運行時間則顯著減少了。

圖3 目標軌跡(湖上試驗)Fig.3 Real trajectory of the target (Lake trial)

圖4 目標軌跡的估計結果對比(湖上試驗)Fig.4 Estimated trajectories by SCKF and ISCKF,respectively (Lake trial)

表4 幾種算法的運行時間對比Table 4 Comparations of running time among improved filters and conventional filters

表5 幾種算法的估計精度對比Table 5 Comparations of RMSEs among improved filters and conventional filters

圖5 k 變化曲線Fig.5 The curve of k against time

5 結論

結合距離與方位聯(lián)合進行水下目標跟蹤的實際應用特點，本文考慮了測距誤差有偏的情況，基于標準UKF 和CKF 算法提出了相應的改進算法，并給出了線性狀態(tài)方程條件下的簡化形式。仿真實驗和湖試實驗數(shù)據(jù)的仿真處理結果表明，在目標的方位信息比較可靠、誤差較小的條件下，改進算法估計精度更高，而運算量與常規(guī)算法相當，可應用于水下目標跟蹤。實際應用中，水下環(huán)境復雜多變，在將來的工作中，將關注復雜模型下的改進算法及其在復雜多變的跟蹤場景中的應用。