水下聲自導(dǎo)武器目標(biāo)跟蹤方法研究

楊向鋒，楊云川，陳亞林

中國船舶重工集團(tuán)公司 第705研究所，西安 710075

1 引言

目標(biāo)跟蹤是水下聲學(xué)觀測系統(tǒng)的重要功能之一，卡爾曼濾波方法是進(jìn)行目標(biāo)跟蹤的主要方法[1]，利用卡爾曼濾波方法對水下目標(biāo)進(jìn)行跟蹤近年來得到了廣泛的研究[2-6]，但主要是針對水下聲學(xué)觀測系統(tǒng)基于被動觀測方式的純方位目標(biāo)跟蹤方法研究，其觀測平臺靜止或按固定航路低速運(yùn)動，其采樣間隔固定。水下聲自導(dǎo)武器因其多采用主動觀測方式并且具有高速運(yùn)動特性，其目標(biāo)跟蹤方法研究不多。水下聲自導(dǎo)武器工作在主動方式時，通過發(fā)射聲信號并接收目標(biāo)反射信號對目標(biāo)進(jìn)行檢測、參量估計，獲得目標(biāo)相對的空間信息后通過彈道機(jī)動向目標(biāo)運(yùn)動，在向目標(biāo)運(yùn)動過程中繼續(xù)進(jìn)行檢測、參量估計、按一定規(guī)律調(diào)整運(yùn)動方向跟蹤目標(biāo)，由此可見水下聲自導(dǎo)武器作為觀測平臺一直處于高速機(jī)動狀態(tài)。在作戰(zhàn)過程中，水下聲自導(dǎo)武器與目標(biāo)的相對位置不斷變化，水下聲自導(dǎo)武器會根據(jù)與目標(biāo)的距離實(shí)時調(diào)整探測波形和檢測周期，同時水下聲自導(dǎo)武器進(jìn)行彈道機(jī)動時需要一定的時間實(shí)現(xiàn)，最終導(dǎo)致其采樣間隔處于變化之中。綜合以上兩方面的因素，水下聲自導(dǎo)武器跟蹤目標(biāo)的過程表現(xiàn)為一個動基座時變過程。本文主要研究水下主動聲自導(dǎo)武器標(biāo)跟蹤問題，分析了水下主動聲自導(dǎo)武器的工作特點(diǎn)并建立了水下主動聲自導(dǎo)武器主動目標(biāo)跟蹤模型，對跟蹤模型進(jìn)行了仿真實(shí)驗。

2 跟蹤建模

卡爾曼濾波方法對目標(biāo)進(jìn)行跟蹤時，要求目標(biāo)的多次觀測必須處于同一個觀測坐標(biāo)系下，而采樣間隔必須是確知的，因此水下聲自導(dǎo)武器利用卡爾曼濾波方法對目標(biāo)進(jìn)行跟蹤必須解決基座運(yùn)動和觀測時變的問題。

2.1 坐標(biāo)變換

定義O1x1y1z1為水下聲自導(dǎo)武器坐標(biāo)系，原點(diǎn)O1為水下聲自導(dǎo)武器質(zhì)心，軸向指向頭部方向為O1x1軸，O1y1軸垂直于O1x1并指向正上方，O1z1軸垂直于O1x1z1平面并符合右手坐標(biāo)系[7]；空間任一點(diǎn)目標(biāo)在水下聲自導(dǎo)武器坐標(biāo)系中的檢測信息為(r,α,β)，如圖1所示。

圖1 水下聲自導(dǎo)武器坐標(biāo)系

由幾何關(guān)系可知其水下聲自導(dǎo)武器坐標(biāo)系的坐標(biāo)計算公式如下：

定義O xyz為大地坐標(biāo)系，原點(diǎn)O為發(fā)射點(diǎn)，O y軸垂直向上，Ox軸為發(fā)射方向，Oz軸垂直于Oxy平面并符合右手坐標(biāo)系[8]；水下聲自導(dǎo)武器作為一個具有六自由度的剛體，其坐標(biāo)系相對于大地坐標(biāo)系的位置就由六個坐標(biāo)來確定，即水下聲自導(dǎo)武器坐標(biāo)系原點(diǎn)在大地坐標(biāo)系中的坐標(biāo)(x0,y0,z0)和水下聲自導(dǎo)武器坐標(biāo)系與大地坐標(biāo)系之間的三個夾角(ψ,θ,φ)。三個角度分別表示水下聲自導(dǎo)武器的偏航角ψ、俯仰角θ和橫滾角φ，偏航角從尾部看向左為正，俯仰角向從尾部看向上為正，橫滾角從尾部看向右為正，水下聲自導(dǎo)武器坐標(biāo)系與大地坐標(biāo)系的旋轉(zhuǎn)關(guān)系如圖2所示。

由幾何關(guān)系可知，水下聲自導(dǎo)武器坐標(biāo)系到大地坐標(biāo)系的旋轉(zhuǎn)轉(zhuǎn)換矩陣可以表示為：

若水下聲自導(dǎo)武器坐標(biāo)系原點(diǎn)在大地坐標(biāo)系中的坐標(biāo)為(x0,y0,z0)，與大地坐標(biāo)系之間的三個夾角為(ψ,θ,φ)，則水下聲自導(dǎo)武器坐標(biāo)系中任一點(diǎn)目標(biāo)(x1,y1,z1)在大地坐標(biāo)系下的坐標(biāo)(x,y,z)可以表示為：

水下聲自導(dǎo)武器導(dǎo)航定位系統(tǒng)可以實(shí)時提供其在大地坐標(biāo)系的六個坐標(biāo)(x0,y0,z0,ψ,θ,φ)，通過坐標(biāo)變換將目標(biāo)坐標(biāo)從水下聲自導(dǎo)武器坐標(biāo)系變換到大地坐標(biāo)系，解決了觀測基座運(yùn)動的問題。坐標(biāo)變換使得目標(biāo)的多次觀測處于同一個觀測坐標(biāo)系下，滿足卡爾曼濾波器觀測平臺靜止的要求。

2.2 目標(biāo)狀態(tài)方程

對水下目標(biāo)而言，在未受到攻擊時，在水下可能靜止或勻速運(yùn)動；在受到水下聲自導(dǎo)武器攻擊時的運(yùn)動不外乎加速、轉(zhuǎn)彎、下潛等，在運(yùn)動過程中其加速度一般恒定或變化不大可認(rèn)為恒定，因此目標(biāo)的坐標(biāo)方程、速度方程、加速度方程如下：

式中，xk、yk、zk，vxk、vyk、vzk，axk、ayk、azk分別是目標(biāo)在大地坐標(biāo)系x軸、y軸、z軸上的坐標(biāo)、速度、加速度，T是采樣周期。

水下聲自導(dǎo)武器攻擊目標(biāo)的過程是一個時變過程，相鄰兩次檢測之間可能存在水下聲自導(dǎo)武器彈道的變換過程，同時根據(jù)自導(dǎo)系統(tǒng)配置可能存在周期時間調(diào)整，而且在自導(dǎo)工作過程中可能出現(xiàn)某周期丟失目標(biāo)，下周期又檢測到目標(biāo)的情況，即式（4）中的T是變化的，而卡爾曼濾波方法要求系統(tǒng)采樣周期必須是確知且等間隔，T變化導(dǎo)致狀態(tài)轉(zhuǎn)移矩陣不確定，因此無法直接使用卡爾曼濾波方法。

圖2 水下聲自導(dǎo)武器坐標(biāo)系與大地坐標(biāo)系的旋轉(zhuǎn)關(guān)系

水下聲自導(dǎo)武器自導(dǎo)系統(tǒng)可以在統(tǒng)一時鐘下提供每次檢測的精確時間，采樣周期可以通過相鄰兩次觀測的時間進(jìn)行實(shí)時計算，即

tk為第k次觀測時間，通過實(shí)時計算采樣周期Tk,k-1就解決了觀測時變問題。實(shí)時計算采樣周期Tk,k-1后，雖然不能滿足標(biāo)準(zhǔn)卡爾曼濾波方法對采樣周期等間隔的要求，但狀態(tài)轉(zhuǎn)移矩陣可以利用Tk,k-1進(jìn)行實(shí)時計算，能滿足卡爾曼濾波方法遞推計算的需要。

2.3 跟蹤模型

水下聲自導(dǎo)武器攻擊目標(biāo)的過程相對短暫，可以忽略外界的環(huán)境對目標(biāo)運(yùn)動的干擾，選擇目標(biāo)在大地坐標(biāo)系x軸、y軸、z軸上的坐標(biāo)、速度、加速度為狀態(tài)向量。

則狀態(tài)方程及狀態(tài)轉(zhuǎn)移矩陣可表示為：

水下聲自導(dǎo)武器檢測目標(biāo)是直接測量，考慮到測量噪聲nk，觀測方程可表示為：

得到狀態(tài)方程和觀測方程后就可以給出Kalman濾波的遞推公式如下：

該模型為了降低復(fù)雜度而忽略了外界的環(huán)境對目標(biāo)運(yùn)動的干擾以及假設(shè)運(yùn)動過程中其加速度恒定，該簡化近似可能導(dǎo)致系統(tǒng)模型的不準(zhǔn)確。卡爾曼濾波是無限增長記憶的濾波，k時刻的最優(yōu)估計要用到k時刻以前的全部觀測數(shù)據(jù)，隨著k的增大，濾波值中的老數(shù)據(jù)比重太大，而新數(shù)據(jù)比重太小，當(dāng)系統(tǒng)存在模型誤差和計算誤差時，新的觀測數(shù)據(jù)對修正狀態(tài)估計的作用太小，不能有效地抑制誤差對狀態(tài)估計值的影響，從而導(dǎo)致卡爾曼濾波精度下降甚至濾波發(fā)散。為了減小模型的不準(zhǔn)誤差，人們對傳統(tǒng)的增長記憶卡爾曼濾波器進(jìn)行了改造[8-9]，衰減記憶卡爾曼濾波器便是比較成功且易于實(shí)現(xiàn)的一種改進(jìn)方法。

在Kalman遞推公式（11）中，計算 Pk,k-1時以 S Pk-1替代Pk-1，S稱為衰減因子。當(dāng)S＞1時，當(dāng)前數(shù)據(jù)的作用得到加強(qiáng)，歷史數(shù)據(jù)的作用隨著遞推計算慢慢弱化，實(shí)際應(yīng)用中衰減因子的選取對濾波結(jié)果產(chǎn)生很大影響，若采取固定值的方法，在一些情況下可能會發(fā)生異常，甚至導(dǎo)致濾波發(fā)散。文獻(xiàn)[8]利用新息序列與量測值匹配程度自動調(diào)整衰減因子取得較好的結(jié)果，本文利用目標(biāo)坐標(biāo)量測值與坐標(biāo)濾波估計值的匹配程度自動調(diào)整衰減因子，自適應(yīng)約束條件如下：

式中，xk、yk、zk為 k 時刻坐標(biāo)量測值，?、、為 k 時刻坐標(biāo)濾波估計值。

k+1時刻的衰減因子Sk+1按下式選取：

式中，a為一個正實(shí)數(shù)，λ為判別門限。

如果濾波正常，則經(jīng)過濾波得到的坐標(biāo)濾波估計值與坐標(biāo)量測值在一定誤差范圍內(nèi)是匹配的，此時取Sk=1，在k時刻不進(jìn)行衰減記憶濾波；如果濾波異常或發(fā)散，則經(jīng)過濾波得到的坐標(biāo)濾波估計值與坐標(biāo)量測值會出現(xiàn)較大的偏差，此時取Sk=aλ＞1，在k時刻進(jìn)行衰減記憶濾波，使得新數(shù)據(jù)對狀態(tài)估計的修正得到加強(qiáng)。

在工程上通常以初次檢測值為坐標(biāo)初值并認(rèn)為初始速度和加速度為零，若初次檢測值為[x0,y0,z0]，則狀態(tài)變量初值如下：

式（9）的Rnk是量測誤差矩陣，反映水下聲自導(dǎo)武器檢測造成的誤差，與檢測算法、信號波形、頻帶及水聲環(huán)境等有關(guān)，本文根據(jù)工程經(jīng)驗按下式選取。

根據(jù)上式可知：

由于任意兩次觀測相互獨(dú)立，所以有：

由式（17）、（18）、（19）可知：

衰減因子選擇的判別門限λ反映的是量測值與濾波估計值匹配時的偏差，可以依據(jù)量測值的方差選擇，文中給出了量測值的方差工程經(jīng)驗值σx,σy,σz，λ按下式計算：

已知觀測值（水下聲自導(dǎo)武器檢測值經(jīng)過坐標(biāo)變換）Zk、觀測時間tk，本文提出的水下聲自導(dǎo)武器自適應(yīng)衰減記憶Kalman濾波跟蹤算法流程如圖3。

圖3 自適應(yīng)衰減記憶Kalman濾波跟蹤算法流程圖

3 仿真實(shí)驗與分析

圖4 自適應(yīng)衰減記憶Kalman濾波跟蹤結(jié)果

圖6 自適應(yīng)衰減記憶Kalman濾波誤差曲線

為了驗證跟蹤模型的有效性，進(jìn)行了數(shù)學(xué)仿真實(shí)驗。目標(biāo)以大地坐標(biāo)系原點(diǎn)為起點(diǎn)，先在Oxz平面以5 m/s的速度沿與x軸夾角30o方向勻速直線運(yùn)動，一段時間后以0.43 m2/s的加速度加速并以7.5o/s的角速率逆時針轉(zhuǎn)彎，加速到10 m/s，運(yùn)動方向轉(zhuǎn)到與x軸夾角150o方向時勻速直線運(yùn)動；采樣間隔在完成轉(zhuǎn)彎前為5 s加入方差為2 s隨機(jī)高斯分布噪聲，完成轉(zhuǎn)完后為3 s加入方差為1 s隨機(jī)高斯分布噪聲，目標(biāo)量測值根據(jù)采樣間隔計算得到的理論值加入方差為10 m的隨機(jī)高斯分布噪聲。濾波初值=[0,0,0,0,0,0,0,0,0]T,P0=diag(100,100,100,200,200,200,400,400,400),Rnk=diag(10,10,10)，衰減因子初值S0=1，計算Sk+1時a=0.01，λ=30.0。該數(shù)據(jù)樣本模擬了水下聲自導(dǎo)武器檢測并跟蹤目標(biāo)的典型過程，開始水下聲自導(dǎo)武器距離目標(biāo)較遠(yuǎn)，檢測周期較長，水下聲自導(dǎo)武器跟蹤目標(biāo)一段時間后，目標(biāo)開始通過加速轉(zhuǎn)彎方式進(jìn)行機(jī)動逃逸，此時水下聲自導(dǎo)武器已經(jīng)距離目標(biāo)較近，檢測周期縮短。數(shù)據(jù)樣本由計算機(jī)模擬生成，分別應(yīng)用本文提出自適應(yīng)衰減記憶Kalman濾波跟蹤算法和普通Kalman濾波跟蹤算法對該數(shù)據(jù)樣本進(jìn)行了計算機(jī)仿真對比實(shí)驗，跟蹤及運(yùn)動參數(shù)估計結(jié)果如圖4～11所示。

仿真結(jié)果表明，在轉(zhuǎn)彎之前的勻速直線運(yùn)動過程中，自適應(yīng)衰減記憶Kalman濾波與普通Kalman濾波實(shí)際上是相同的，經(jīng)過10個觀測周期約50 s后濾波誤差明顯小于原始檢測值誤差，運(yùn)動學(xué)參數(shù)估計值收斂于數(shù)據(jù)樣本設(shè)計值，而傳統(tǒng)的純方位目標(biāo)跟蹤方法一般需要數(shù)百秒的時間才能完成跟蹤收斂。在加速轉(zhuǎn)彎過程及之后的勻速直線運(yùn)動過程中，兩種濾波方法的濾波結(jié)果出現(xiàn)了明顯的差異，雖然自適應(yīng)衰減記憶Kalman濾波在加速轉(zhuǎn)彎期間出現(xiàn)了較大的誤差，但是通過進(jìn)行自適應(yīng)衰減記憶，在完成加速轉(zhuǎn)彎后，經(jīng)過10個觀測周期約30 s后很快再次跟蹤上目標(biāo)，其運(yùn)動學(xué)參數(shù)估計二次收斂，估計值正確，而普通Kalman濾波從轉(zhuǎn)彎開始濾波出現(xiàn)了發(fā)散，不僅沒有跟蹤上目標(biāo)，而且運(yùn)動學(xué)參數(shù)估計錯誤。自適應(yīng)衰減記憶Kalman濾波跟蹤方法與傳統(tǒng)的純方位目標(biāo)跟蹤方法相比具有較快的收斂速度，與普通Kalman濾波跟蹤方法相比具有較強(qiáng)的機(jī)動目標(biāo)跟蹤能力。

圖5 普通Kalman濾波跟蹤結(jié)果

圖7 普通Kalman濾波誤差曲線

圖8 自適應(yīng)衰減記憶Kalman濾波速度估計結(jié)果

圖10 自適應(yīng)衰減記憶Kalman濾波加速度估計結(jié)果

4 結(jié)束語

主動水下聲自導(dǎo)武器根據(jù)目標(biāo)檢測信息將自身導(dǎo)向目標(biāo)的過程中一直處于高速機(jī)動狀態(tài)，水下聲自導(dǎo)武器變換檢測周期、實(shí)施彈道機(jī)動以及受水聲環(huán)境影響某周期丟失目標(biāo)后下周期又檢測到目標(biāo)均會導(dǎo)致其觀測時變。本文通過分析主動水下聲自導(dǎo)武器的工作特點(diǎn)，通過坐標(biāo)變換及實(shí)時計算采樣時間解決了其觀測基座運(yùn)動和觀測時變的問題，并建立了基于自適應(yīng)衰減記憶Kalman濾波的主動水下聲自導(dǎo)武器目標(biāo)跟蹤模型，給出了濾波初值在工程應(yīng)用中的選取方法，仿真實(shí)驗證明自適應(yīng)衰減記憶Kalman濾波算法不僅具有較快的收斂速度和較高的運(yùn)動學(xué)參數(shù)估計精度，而且能有效地跟蹤高速機(jī)動目標(biāo)，具有一定的工程應(yīng)用價值。

圖9 普通Kalman濾波速度估計結(jié)果

圖11 普通Kalman濾波加速度估計結(jié)果

[1]權(quán)太范.目標(biāo)跟蹤新理論與技術(shù)[M].北京：國防工業(yè)出版社，2009.

[2]劉偉，王昌明，趙輝.基于卡爾曼濾波的水下近距目標(biāo)運(yùn)動分析[J].彈道學(xué)報，2008，20（4）：28-31.

[3]吳盤龍，孔建壽.基于平方根UKF的水下純方位目標(biāo)跟蹤[J].南京理工大學(xué)學(xué)報：自然科學(xué)版，2009，33（6）：751-755.

[4]石章松，劉忠.單站純方位目標(biāo)多模型卡爾曼濾波跟蹤算法的研究[J].系統(tǒng)仿真學(xué)報，2006，18（7）：1802-1805.

[5]曲毅，劉忠，屈津竹.基于時延的水中目標(biāo)純方位跟蹤算法[J].系統(tǒng)工程與電子技術(shù)，2007，29（1）：107-109.

[6]葉靈軍，曲毅，劉忠，等.水下純方位目標(biāo)運(yùn)動分析的UKF濾波算法[J].火力與指揮控制，2009，34（8）：132-134.

[7]嚴(yán)衛(wèi)生.魚雷航行力學(xué)[M].西安：西北工業(yè)大學(xué)出版社，2005.

[8]劉健，劉忠，曲毅.純方位目標(biāo)運(yùn)動分析的自適應(yīng)衰減記憶濾波算法[J].中國海洋大學(xué)學(xué)報，2006，36（4）.

[9]章飛，周杏鵬，陳小惠.基于衰減記憶濾波的平方根UKF被動目標(biāo)跟蹤算法[J].測控技術(shù)，2010，29（4）：22-26.