基于二階差分濾波器的水下目標(biāo)純方位角跟蹤

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

(1.哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱150001;2.齊齊哈爾大學(xué)計(jì)算機(jī)與控制工程學(xué)院，黑龍江齊齊哈爾161006)

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

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

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

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

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

1 問題描述

在BOT問題研究中，被動(dòng)聲吶平臺(tái)和水下目標(biāo)可以被看作空間中的2個(gè)質(zhì)點(diǎn)，本文假設(shè)目標(biāo)的加速度變化滿足高斯隨機(jī)過程［13］。

1.1 水下目標(biāo)的運(yùn)動(dòng)學(xué)模型

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

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

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

若假設(shè)目標(biāo)作勻速直線運(yùn)動(dòng)，根據(jù)文獻(xiàn)［14］，水下目標(biāo)與被動(dòng)聲吶平臺(tái)的相對運(yùn)動(dòng)狀態(tài)方程為

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

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

圖1 平臺(tái)與目標(biāo)運(yùn)動(dòng)的幾何關(guān)系Fig.1 Geometric relationship between platform and target

1.2 被動(dòng)聲吶測量模型

由于本文僅考慮被動(dòng)聲吶平臺(tái)通過對水下目標(biāo)與平臺(tái)相對方位角信息的觀測來完成目標(biāo)跟蹤。因此，第k時(shí)刻被動(dòng)聲吶平臺(tái)的測量模型可以表示為

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

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

2 基于DDF2的BOT算法

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

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

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

式中: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)估計(jì)向量及狀態(tài)估計(jì)協(xié)方差矩陣如下:

以上給出了基于DDF2算法的BOT跟蹤算法的詳細(xì)推導(dǎo)過程，在此基礎(chǔ)上給出該算法的流程圖，如圖2所示。

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

3 仿真分析

3.1 Monte Carlo仿真

仿真案例為被動(dòng)聲吶平臺(tái)跟蹤某勻速直線運(yùn)動(dòng)的水下目標(biāo)，設(shè)定仿真時(shí)間為1 000 s，仿真步長T=1 s。假定被動(dòng)聲吶平臺(tái)在跟蹤水下目標(biāo)的過程中做360°轉(zhuǎn)向機(jī)動(dòng)。水下目標(biāo)及被動(dòng)聲吶平臺(tái)的運(yùn)動(dòng)狀態(tài)初始值設(shè)定如表1所示。

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

表1 水下目標(biāo)及被動(dòng)聲吶平臺(tái)的運(yùn)動(dòng)狀態(tài)設(shè)置Table 1 Target and passive sonar platform motion state set

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

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

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

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

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

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

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

4 結(jié)束語

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

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

［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］岳劍平，梁國龍，王燕.水下動(dòng)目標(biāo)純方位被動(dòng)跟蹤仿真研究［J］.哈爾濱工程大學(xué)學(xué)報(bào)，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的水下目標(biāo)純方位跟蹤算法［J］.艦船科學(xué)技術(shù)，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的水下純方位目標(biāo)跟蹤［J］.南京理工大學(xué)學(xué)報(bào):自然科學(xué)版，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)明，劉雁春，殷曉東.基于交互多模型的水下目標(biāo)跟蹤方法［J］.武漢大學(xué)學(xué)報(bào):信息科學(xué)版，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］張賢達(dá).矩陣分析與應(yīng)用［M］.北京:清華大學(xué)出版社，2009:347-360.