王宏健，徐金龍，么洪飛，2，張愛華

(1.哈爾濱工程大學自動化學院，黑龍江哈爾濱150001;2.齊齊哈爾大學計算機與控制工程學院，黑龍江齊齊哈爾161006)

水下目標跟蹤包括基于多聲吶的目標跟蹤和基于單聲吶的目標跟蹤2種方式。其中，水下目標純方位角跟蹤(bearing-only tracking，BOT)是基于單聲吶目標跟蹤的重要方法。BOT是通過被動聲吶平臺的水聽器陣列獲得的一系列方位角測量數(shù)據(jù)序列來估計目標運動參數(shù)的技術［1］。

在BOT定位跟蹤中，濾波算法是核心技術。目前，BOT跟蹤系統(tǒng)中應用通常采用改進的卡爾曼濾波算法，包括擴展卡爾曼濾波(extended Kalman filter，EKF)［2-3］及無跡卡爾曼濾波算法 (unscented Kalman filter，UKF)［4-5］等。EKF 算法的性能依賴于系統(tǒng)的局部非線性強度，UKF算法原理主要是通過構造一組加權樣本點來逼近待估計參數(shù)，避免EKF算法線性化的缺點，是一種可以直接應用于非線性系統(tǒng)狀態(tài)估計的濾波方法。但UKF算法同樣由于噪聲信號及計算誤差等因素而容易導致誤差協(xié)方差矩陣負定，從而使得濾波結果發(fā)散。

本文在EKF算法的基礎上，提出了基于二階差分濾波器(second-order divided difference filter，DDF2)的BOT算法。該算法在BOT跟蹤系統(tǒng)線性化時采用Stirling插值法，避免了因計算Jacobian矩陣或Hessian矩陣而導致計算的負擔，同時也放寬了跟蹤系統(tǒng)的狀態(tài)方程必須滿足可微性的約束。

2005年，Lee［6］提出了 DDF 濾波方法。文獻［7-8］在彈道導彈跟蹤問題中應用了DDF1、DDF2并與EKF、UKF方法進行了狀態(tài)估計性能對比。文獻［9-10］提出了一種基于數(shù)據(jù)融合算法的DDF2，并且應用于剛體的姿態(tài)角估計中。文獻［11］將DDF1、DDF2應用于月球車的運動狀態(tài)參數(shù)估計中。文獻［12］將DDF1、DDF2應用于全球位置導航系統(tǒng)，并且得出了DDF1、DDF2的估計精度要明顯高于EKF。

本文將DDF2引入到對水下目標的純方位角跟蹤問題中，并用與DDF1及傳統(tǒng)的EKF方法進行仿真對比分析，結果表明DDF2及DDF1算法的濾波精度要明顯優(yōu)于 EKF，且由于 DDF2采用二階Stirling插值法線性化BOT跟蹤系統(tǒng)模型中的非線性項，DDF2算法的濾波精度要高于 DDF1，而且DDF2的濾波穩(wěn)定性要優(yōu)于另外2種方法。

1 問題描述

在BOT問題研究中，被動聲吶平臺和水下目標可以被看作空間中的2個質點，本文假設目標的加速度變化滿足高斯隨機過程［13］。

1.1 水下目標的運動學模型

由于通常水下目標相對于被動聲吶平臺距離很遠，可以不考慮水下目標在垂直于地面方向上的距離的變化。因此為了使問題簡化，本文僅考慮二維平面中對勻速直線運動的水下目標跟蹤問題。在水平面直角坐標系中，目標在X-Y上的位置坐標為(xt，yt)，各方向的速度記為()，如圖1所示。將目標的狀態(tài)向量定義為

被動聲吶平臺的狀態(tài)向量定義為:

式中:(xo，yo)及)分別為被動聲吶平臺的位置和速度。這樣目標與平臺的相對運動狀態(tài)向量可以表示為

若假設目標作勻速直線運動，根據(jù)文獻［14］，水下目標與被動聲吶平臺的相對運動狀態(tài)方程為

式中:F、Γ為狀態(tài)轉移矩陣，如式(5)、(6)所示;T為采樣周期;νk為過程噪聲且滿足形如 νk～N(ν-，Qk)的高斯分布;Qk=σaI2，σa為標量，I2為2×2單位矩陣。

被動聲吶平臺與水下目標相對運動情況如圖1所示，圖中ψt表示水下目標的航向，Vt表示水下目標的運動速度，Vo表示被動聲吶平臺的運動速度，(r0…rk)為目標與被動聲吶平臺的相對距離序列。

圖1 平臺與目標運動的幾何關系Fig.1 Geometric relationship between platform and target

1.2 被動聲吶測量模型

由于本文僅考慮被動聲吶平臺通過對水下目標與平臺相對方位角信息的觀測來完成目標跟蹤。因此，第k時刻被動聲吶平臺的測量模型可以表示為

式中:H(xk)為被動聲吶測得的方位角，如式(8)所示;ωk為測量噪聲，且滿足形如ωk～ N(ω-k，Rk)的高斯分布。

目標跟蹤系統(tǒng)的數(shù)學模型由方程由式(4)、(7)組成。目標跟蹤算法便能夠通過給定的方位角測量序列Zk={zi}來估計狀態(tài)向量xk。由于被動聲吶的測量模型是關于狀態(tài)向量的非線性函數(shù)，因此，BOT目標跟蹤屬于典型的非線性濾波估計。文獻［15-16］指出，被動聲吶平臺必須做適當?shù)臋C動策略才能完成對目標的跟蹤。

2 基于DDF2的BOT算法

本文將利用二階Stirling插值公式的多變量擴展形式來近似處理被動聲吶的測量模型中非線性項，進行二階截斷，并且DDF2采用與EKF算法相同的預測-校正結構［17］，以得到被動聲吶測量的預測值及新息的協(xié)方差矩陣，修正目標狀態(tài)的估計值，實現(xiàn)純方位角跟蹤。

首先定義協(xié)方差矩陣的Cholesky分解［18］:

DDF2濾波器計算狀態(tài)預測向量x^-k+1的計算方法為

式中:h為插值步長，nx=4及nν=2分別為狀態(tài)向量的維數(shù)及噪聲向量的維數(shù)，Sx，p為矩陣Sx的第p列向量，Sν，p為矩陣Sv的第p列向量。

式中:

式中:Sx，j為矩陣 Sx的第 j列向量，Sν，j為矩陣 Sv的第 j列向量;i，j=1，2…4。

式中:nω=2為噪聲向量的維數(shù)，為矩陣的第p列向量，Sω，P為矩陣Sω的第p列向量。觀測協(xié)方差矩陣如下:

式中:

濾波器增益 κk+1，狀態(tài)估計向量及狀態(tài)估計協(xié)方差矩陣如下:

以上給出了基于DDF2算法的BOT跟蹤算法的詳細推導過程，在此基礎上給出該算法的流程圖，如圖2所示。

圖2 DDF2算法流程圖Fig.2 Block diagram of DDF2

3 仿真分析

3.1 Monte Carlo仿真

仿真案例為被動聲吶平臺跟蹤某勻速直線運動的水下目標，設定仿真時間為1 000 s，仿真步長T=1 s。假定被動聲吶平臺在跟蹤水下目標的過程中做360°轉向機動。水下目標及被動聲吶平臺的運動狀態(tài)初始值設定如表1所示。

在設定濾波器的初始狀態(tài)時，假設初始時刻水下目標和被動聲吶平臺的相對距離r0為已知，而水下目標的運動速度未知，初始時刻通過被動聲吶測得水下目標的方位角為 z0。令M=，則濾波器的初始狀態(tài)x0及初始協(xié)方差矩陣P0表示為

表1 水下目標及被動聲吶平臺的運動狀態(tài)設置Table 1 Target and passive sonar platform motion state set

在上述初始條件下，分別將本文所提出的基于DDF2的BOT目標跟蹤算法與前面所提及的DDF1和EKF算法進行100次Monte Carlo仿真實驗。通過這3種方法對水下目標的位置、速度、航向進行濾波估計，并估計出平臺和水下目標的相對距離，仿真結果如圖3所示。

由圖3可以看出，在相同的初始條件下，若被動聲吶平臺也做相同的機動，本文所提出的方法能夠快速跟蹤到目標的位置、速度等真值。從圖3(b)可以看出，雖然DDF1及EKF能夠快速的對目標的速度進行狀態(tài)估計響應，但是其不能收斂到目標真值，而DDF2則能收斂到目標真值，說明DDF2的跟蹤能力要明顯強于DDF1及EKF，而DDF1的跟蹤能力也要強于EKF。

圖3 目標的位置、速度、距離及艏向跟蹤Fig.3 Position，velocity，distance and course tracking

同時也能夠看到，在初始的一段時間里，這3種方法對水下目標的跟蹤都出現(xiàn)了一個較大的偏差，這主要是由目標的初始速度未知所導致的。

3.2 均方差統(tǒng)計特性

為了深入的對比和分析本文所提出的方法和基于DDF1及EKF算法的性能，通過狀態(tài)的均方根誤差(RMSE)統(tǒng)計特性來有效的表征這3種方法的狀態(tài)估計精度。經(jīng)過100次Monte Carlo仿真試驗得到的位置及速度的RMSE統(tǒng)計特性如圖4所示。在相同的仿真條件下，DDF2具有更高的估計精度，而EKF的估計精度最低。

圖4 位置及速度跟蹤均方根誤差Fig.4 RMSE of position and velocity tracking

4 結束語

本文所提算法可以避免傳統(tǒng)EKF利用Taylor級數(shù)展開進行線性化以及需要計算Jacobian矩陣而導致的計算過程復雜和難以進行實際應用的問題，且具有比DDF1更高的跟蹤精度。該方法不僅簡化了目標跟蹤的計算過程，而且具有良好跟蹤精度。最后，為了驗證本文所提出的算法的性能，通過Monte Carlo方法進行了100次仿真試驗，并將本文提出的方法與DDF1及EKF方法進行了對比分析。

仿真結果表明:在相同初始條件下，基于DDF2的目標跟蹤算法具有更快速準確的跟蹤響應。此外，通過RMSE統(tǒng)計方法得到的仿真結果也驗證了這種方法具有較高的估計精度。

［1］AIDALA V J，HAMMEL S.Utilization of polar coordinates for bearings-only tracking［J］.IEEE Transactions on Automatic Control，1983，28(2):283-294.

［2］MURPHY D J.Noise bearings-only target motion analysis［D］.Kirkland:Northwestern University，1970:151-157.

［3］岳劍平，梁國龍，王燕.水下動目標純方位被動跟蹤仿真研究［J］.哈爾濱工程大學學報，2003，24(5):500-504.YUE Jianping，LIANG Guolong，WANG Yan.Simulation study on passive tracking of underwater moving target for bearing only［J］.Journal of Harbin Engineering University，2003，24(5):500-504.

［4］曲毅，劉忠.基于UKF的水下目標純方位跟蹤算法［J］.艦船科學技術，2009，31(7):133-136.QU Yi，LIU Zhong.Research of underwater bearings-only target tracking algorithm based on UKF［J］.Ship Science and Technology，2009，31(7):133-136.

［5］吳盤龍，孔建壽.基于平方根UKF的水下純方位目標跟蹤［J］.南京理工大學學報:自然科學版，2009，33(6):751-755.WU Panlong，KONG Jianshou.Underwater bearing-only target tracking based on square-root UKF［J］.Journal of Nanjing University of Science and Technology:Natural Science，2009，33(6):751-755.

［6］LEE D J.Nonlinear Bayesian filtering with applications to estimation and navigation［D］.Texas:Texas A＆M University，2005:134-139.

［7］SAULSON B G，CHANG K.Nonlinear estimation comparision for ballistic missile tracking［J］.Optical Engineering，2004，43(6):1424-1438.

［8］WU C，HAN C.Second-order divided difference filter with application to ballistic target tracking［C］//Proceedings of the 7th World Congress on Intelligent Control and Automation.Chongqing，China，2008:6342-6347.

［9］SETOODEH P，KAYTIAIN A，F(xiàn)ARJAH E.Attitude estimation by divided difference filter-based sensor fusion［J］.The Journal of Navigation，2007，60(1):119-128.

［10］AHMADIM，KHAYATIAN A，KARIMAGHAEE P.Attitude estimation by divided difference filter in quaternion space［J］.Acta Astronautica，2012，75:95-107.

［11］BHALE P G，DWIVEDIP N，KUMAR P.Estimation of ballistic coefficient of reentry vehicle with divided difference filtering using noisy RF seeker data［C］//Proceedings of the IEEE International Conference on Industrial Technology.Mumbai，USA，2006:1087-1092.

［12］JWO D J，HSIEH M Y，LAI S Y.GPS navigation processing using the quaternion-based divided difference filter［J］.GPSSolut，2010，14:217-228.

［13］徐衛(wèi)明，劉雁春，殷曉東.基于交互多模型的水下目標跟蹤方法［J］.武漢大學學報:信息科學版，2007，32(9):782-785.XU Weiming，LIU Yanchun，YIN Xiaodong.Underwater target tracking based on interacting multiple model method［J］.Geomatics and Information Science of Wuhan University，2007，32(9):782-785.

［14］FRIENDLAND B.Optimum steady-state position and velocity estimation using noisy sampled position data［J］.IEEE Transactions on Aerospace and Electronic Systems，1973，9(6):906-911.

［15］ARULAMPALAM M S.RISTIC B，GORDON N.Bearings-only tracking of manoeuvring targets using particle filters［J］.EURASIP Journal on Applied Signal Processing，2004，2004(15):2351-2365.

［16］KIRUBARAJAN T，BAR-SHALOM Y，LERRO D.Bearing only tracking of manoeuvring targets using a batch-recursive estimator［J］.IEEE Transactions on Aerospace and Electronic Systems，2001，37(3):359-368.

［17］NORGAARD M，POULSEN N，RAVN O.New developments in state estimation for nonlinear systems［J］.Automatica，2000，36:1627-1638.

［18］張賢達.矩陣分析與應用［M］.北京:清華大學出版社，2009:347-360.