李智生,余義德,杜寅峰,李 釗

(1.中國(guó)人民解放軍91550部隊(duì),遼寧大連116023;2.中國(guó)人民解放軍65547部隊(duì),遼寧海城114200;3.第二炮兵駐石家莊地區(qū)軍事代表室,河北石家莊050002)

0 引言

水下目標(biāo)運(yùn)動(dòng)測(cè)量系統(tǒng)用于精確測(cè)量目標(biāo)航跡的大地坐標(biāo)和目標(biāo)相對(duì)大地的航速。由于目標(biāo)聲納基陣波動(dòng)和測(cè)量引起的誤差,使得測(cè)得的目標(biāo)位置圍繞真值上下波動(dòng)。為此,系統(tǒng)所測(cè)得數(shù)據(jù)仍是一種隨機(jī)變量,若不對(duì)該數(shù)據(jù)近一步的處理,就無(wú)法得到滿意的精度[1]。

研究應(yīng)用擴(kuò)展卡爾曼濾波方法對(duì)測(cè)量數(shù)據(jù)進(jìn)行精度評(píng)估,推導(dǎo)了相應(yīng)的濾波公式,建立了一種擴(kuò)展卡爾曼濾波算法。通過(guò)系統(tǒng)的仿真結(jié)果表明該算法收斂速度和估計(jì)精度能夠滿足系統(tǒng)要求。

1 卡爾曼濾波公式的推導(dǎo)

卡爾曼濾波是以最小均方誤差為準(zhǔn)則的最優(yōu)估計(jì),是一種對(duì)動(dòng)態(tài)系統(tǒng)進(jìn)行數(shù)據(jù)處理的有效方法。用于目標(biāo)跟蹤定位的卡爾曼濾波器通常是一種典型的非線性濾波器,它具有下列矩陣方程[2,3]:

狀態(tài)方程:

量測(cè)方程:

預(yù)報(bào)估計(jì):

濾波估計(jì):

預(yù)報(bào)誤差協(xié)方差陣:

濾波誤差協(xié)方差陣:

最佳增益矩陣:

式中,φK/K-1為N×N狀態(tài)轉(zhuǎn)移矩陣;WK為動(dòng)態(tài)噪聲;VK為觀測(cè)噪聲,且WK與VK是互不相關(guān)的零均值白噪聲序列;ΓK為N×N噪聲轉(zhuǎn)移矩陣,即對(duì)所有k,j,有EWK=0,EVK=0;ZK為N×1測(cè)量向量。

根據(jù)上述遞推公式之后,如果已知K-1時(shí)刻的狀態(tài)估計(jì) ^XK-1及估計(jì)誤差的協(xié)方差陣 PK-1,就可以根據(jù)K時(shí)刻的量測(cè)值ZK得到K時(shí)刻的估計(jì)^XK和相應(yīng)的誤差協(xié)方差矩陣PK。

2 卡爾曼濾波在系統(tǒng)中的應(yīng)用

測(cè)量系統(tǒng)利用線纜拖曳加載GPS天線的移動(dòng)通信平臺(tái),GPS天線能夠?qū)崟r(shí)獲取移動(dòng)通信平臺(tái)坐標(biāo)參數(shù),同時(shí)移動(dòng)通信平臺(tái)上裝載的水聽(tīng)器能夠?qū)崟r(shí)接收目標(biāo)發(fā)出的聲納信號(hào),通過(guò)計(jì)算聲納脈沖的時(shí)延時(shí)間和目標(biāo)的方位信息,便可實(shí)時(shí)計(jì)算目標(biāo)相對(duì)移動(dòng)通信平臺(tái)的相對(duì)距離和方位,從而可以準(zhǔn)確推算目標(biāo)的位置。

2.1 系統(tǒng)模型

建立恰當(dāng)?shù)臄?shù)學(xué)模型是卡爾曼濾波模型的濾波成敗的關(guān)鍵。一個(gè)好的數(shù)學(xué)模型既要反映目標(biāo)的動(dòng)態(tài)特征,又要足夠簡(jiǎn)化,以保證計(jì)算簡(jiǎn)便和速度。這里針對(duì)目標(biāo)的運(yùn)動(dòng)狀態(tài)來(lái)建立相應(yīng)的模型,目標(biāo)在水下航行過(guò)程可基本視為勻速運(yùn)動(dòng),且大體上按某個(gè)設(shè)定的深度航行。因此,把狀態(tài)方程和量測(cè)方程都設(shè)置在直角坐標(biāo)系中[4]。

在直角坐標(biāo)系中,可用一個(gè)線性動(dòng)態(tài)模型和一個(gè)非線性觀測(cè)模型來(lái)建立目標(biāo)的運(yùn)動(dòng)模型。在直角坐標(biāo)系中,把移動(dòng)通信平臺(tái)O和目標(biāo)都看作質(zhì)點(diǎn)來(lái)處理,則系統(tǒng)的動(dòng)態(tài)模型為:

式中,狀態(tài)向量X=(x,y,z,vx,vy,vz)T為目標(biāo)相對(duì)于移動(dòng)通信平臺(tái)O的相對(duì)位置;v為相對(duì)目標(biāo)速度。

考慮如下情形:移動(dòng)通信平臺(tái)相對(duì)靜止不動(dòng),目標(biāo)在水下某一平面作勻速直線運(yùn)動(dòng),則在測(cè)量時(shí)間間隔內(nèi),通過(guò)移動(dòng)通信平臺(tái)O上的被動(dòng)聲納設(shè)備可測(cè)得目標(biāo)與移動(dòng)通信平臺(tái)的相對(duì)距離俯仰角和方位角的數(shù)據(jù)序列。設(shè)測(cè)量時(shí)噪聲為高斯白噪聲,因?yàn)橐苿?dòng)通信平臺(tái)靜止,所以此時(shí)v為目標(biāo)的絕對(duì)速度,其中vz為0,于是目標(biāo)的狀態(tài)向量為X=(x,y,z,vx,vy)T。

目標(biāo)在tK時(shí)刻的位置可以表示為:

式中,C為未知參數(shù)。則目標(biāo)的狀態(tài)方程為:

轉(zhuǎn)移矩陣為:

目標(biāo)與移動(dòng)通信平臺(tái)之間距離的量測(cè)方程為:

該距離是利用聲納脈沖的時(shí)延值計(jì)算得到的。

目標(biāo)的俯仰角測(cè)量方程為:

目標(biāo)的方位角測(cè)量方程為:

寫(xiě)成向量形式為:

將量測(cè)方程中的觀測(cè)矩陣H(tK)線性化,需要求其雅可比矩陣,即

至此,得出了目標(biāo)的狀態(tài)式(10)和量測(cè)方程式(14),根據(jù)式(3)～式(7),給出恰當(dāng)?shù)腦(0/0)和P(0/0),以及測(cè)量誤差方差,即可在線、實(shí)時(shí)地計(jì)算下去,并不斷地得到目標(biāo)運(yùn)動(dòng)要素的最小方差估計(jì)及估計(jì)誤差方差。

2.2 濾波初值的設(shè)定

式中,T為信號(hào)周期,在本系統(tǒng)中,T=0.1 s,而誤差協(xié)方差陣的定義為:

則狀態(tài)估值初始協(xié)方差陣為:

3 實(shí)例仿真

以移動(dòng)通信平臺(tái)為坐標(biāo)參考點(diǎn),在水深30 m處,水下航行器距離移動(dòng)通信平臺(tái)60 m處勻速直航靠近移動(dòng)通信平臺(tái),航速為4 m/s,水下航行器相對(duì)移動(dòng)通信平臺(tái)的初始方位為45°,狀態(tài)更新頻率為0.1 Hz。假設(shè)測(cè)量噪聲符合零均值、方差 δ2=3的高斯分布,則目標(biāo)的初始狀態(tài)為:X0=(21.21,21.21,51.96,1.4,1.4)。

利用r0,e0,A0的數(shù)據(jù)加噪聲干擾,作為測(cè)量值,根據(jù)測(cè)量值,用本文的卡爾曼濾波方法計(jì)算所有時(shí)刻X=(x,y,z,vx,vy)T的估計(jì)值。目標(biāo)真實(shí)航跡和經(jīng)卡爾曼濾波后的估計(jì)軌跡,其中虛線表示濾波后的估計(jì)軌跡如圖1所示。x方向位置的估計(jì)誤差如圖2所示。航速的軌跡誤差如圖3所示。從圖1可知:濾波算法在第60個(gè)濾波周期開(kāi)始收斂,且比較平穩(wěn)。圖2可以看出,整個(gè)航程內(nèi),x方向誤差都在10 m以內(nèi),符合系統(tǒng)設(shè)計(jì)要求。由圖3可知,航速估計(jì)在第10個(gè)周期內(nèi)開(kāi)始收斂,且誤差在1 m內(nèi),能夠滿足系統(tǒng)要求。

圖1 目標(biāo)真實(shí)航跡和經(jīng)卡爾曼濾波后的估計(jì)軌跡

圖2 x方向位置的估計(jì)誤差

圖3 航速的軌跡誤差

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

對(duì)基于擴(kuò)展卡爾曼濾波的水下目標(biāo)航跡測(cè)量系統(tǒng)的濾波算法進(jìn)行研究,建立了水下目標(biāo)運(yùn)動(dòng)狀態(tài)方程和測(cè)量方程,針對(duì)非線性測(cè)量方程,采用雅克比矩陣進(jìn)行了線性化處理。仿真結(jié)果表明,該濾波算法有較快的收斂性和較好的估計(jì)精度,能夠滿足系統(tǒng)要求。該方法也可以擴(kuò)展應(yīng)用到其他工程領(lǐng)域。

[1]AIDALA V J.Kalman Filter Behavior in Bearings-only Tracking Applications[J].IEEE trans.Aerospace and Engineering Systems,1979,15(1):29-39.

[2]SONG T L,SPEYER J L.A Stochastic Analysis of a Modified Gain Extended Kalman Filter with Application to Estimation with Bearings-only Measurements[J].IEEE Trans.Automatic Control,1985,30(10):940-949.

[3]周 豐.基于卡爾曼濾波水下彈道測(cè)量系統(tǒng)研究[J].彈道學(xué)報(bào),2004,16(3):66-69.

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