楊向鋒，楊云川，陳亞林

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

1 引言

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

2 跟蹤建模

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

2.1 坐標(biāo)變換

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

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

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

定義O xyz為大地坐標(biāo)系，原點(diǎn)O為發(fā)射點(diǎn)，O y軸垂直向上，Ox軸為發(fā)射方向，Oz軸垂直于Oxy平面并符合右手坐標(biāo)系[8]；水下聲自導(dǎo)武器作為一個(gè)具有六自由度的剛體，其坐標(biāo)系相對(duì)于大地坐標(biāo)系的位置就由六個(gè)坐標(biāo)來(lái)確定，即水下聲自導(dǎo)武器坐標(biāo)系原點(diǎn)在大地坐標(biāo)系中的坐標(biāo)(x0,y0,z0)和水下聲自導(dǎo)武器坐標(biāo)系與大地坐標(biāo)系之間的三個(gè)夾角(ψ,θ,φ)。三個(gè)角度分別表示水下聲自導(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)系之間的三個(gè)夾角為(ψ,θ,φ)，則水下聲自導(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í)時(shí)提供其在大地坐標(biāo)系的六個(gè)坐標(biāo)(x0,y0,z0,ψ,θ,φ)，通過(guò)坐標(biāo)變換將目標(biāo)坐標(biāo)從水下聲自導(dǎo)武器坐標(biāo)系變換到大地坐標(biāo)系，解決了觀測(cè)基座運(yùn)動(dòng)的問(wèn)題。坐標(biāo)變換使得目標(biāo)的多次觀測(cè)處于同一個(gè)觀測(cè)坐標(biāo)系下，滿足卡爾曼濾波器觀測(cè)平臺(tái)靜止的要求。

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

對(duì)水下目標(biāo)而言，在未受到攻擊時(shí)，在水下可能靜止或勻速運(yùn)動(dòng)；在受到水下聲自導(dǎo)武器攻擊時(shí)的運(yùn)動(dòng)不外乎加速、轉(zhuǎn)彎、下潛等，在運(yùn)動(dòng)過(guò)程中其加速度一般恒定或變化不大可認(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)的過(guò)程是一個(gè)時(shí)變過(guò)程，相鄰兩次檢測(cè)之間可能存在水下聲自導(dǎo)武器彈道的變換過(guò)程，同時(shí)根據(jù)自導(dǎo)系統(tǒng)配置可能存在周期時(shí)間調(diào)整，而且在自導(dǎo)工作過(guò)程中可能出現(xiàn)某周期丟失目標(biāo)，下周期又檢測(cè)到目標(biāo)的情況，即式（4）中的T是變化的，而卡爾曼濾波方法要求系統(tǒng)采樣周期必須是確知且等間隔，T變化導(dǎo)致狀態(tài)轉(zhuǎn)移矩陣不確定，因此無(wú)法直接使用卡爾曼濾波方法。

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

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

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

2.3 跟蹤模型

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

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

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

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

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

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

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

k+1時(shí)刻的衰減因子Sk+1按下式選?。?/p>

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

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

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

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

根據(jù)上式可知：

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

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

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

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

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

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

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

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

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

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

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

圖7 普通Kalman濾波誤差曲線

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

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

4 結(jié)束語(yǔ)

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

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

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

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

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

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

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

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

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

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

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

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