基于引入多普勒速度的水下高速小目標(biāo)快速跟蹤方法

張思宇,何心怡,陳 菁,祝 琳

(海軍研究院,北京 100161)

隨著技術(shù)的發(fā)展進(jìn)步,世界海軍強(qiáng)國(guó)發(fā)展了數(shù)量眾多的新型航行器,給各國(guó)海上安全構(gòu)成了嚴(yán)重威脅,水下監(jiān)視能力成為世界主要海洋強(qiáng)國(guó)的發(fā)展重點(diǎn),其核心在于能否有效地探測(cè)和跟蹤水下目標(biāo)。由于部分水下航行器呈現(xiàn)體積小、航速高等特點(diǎn),傳統(tǒng)基于目標(biāo)距離和方向角的跟蹤濾波方法由于跟蹤遞推信息量不足,難以保證在水下環(huán)境對(duì)目標(biāo)跟蹤高精度[1-2]。而水下目標(biāo)高速運(yùn)動(dòng)使可利用的跟蹤濾波數(shù)據(jù)點(diǎn)較少,但也產(chǎn)生了較大的目標(biāo)多普勒速度,即目標(biāo)相對(duì)于聲吶系統(tǒng)的徑向速度,將其引入跟蹤濾波器的觀測(cè)方程可以有效增加跟蹤濾波的遞推計(jì)算信息量,提高了跟蹤濾波速度和精度。

基于目標(biāo)距離和方向角的方程均為非線(xiàn)性方程,通常利用擴(kuò)展卡爾曼濾波器(Extend Kalman Filter, EKF)或無(wú)跡卡爾曼濾波器(Unscented Kalman Filter, UKF)實(shí)現(xiàn)對(duì)目標(biāo)跟蹤[3-4]。EKF基于Taylor級(jí)數(shù)展開(kāi)并省略高階項(xiàng)的數(shù)學(xué)思想實(shí)現(xiàn)線(xiàn)性化近似,存在部分觀測(cè)方程雅克比矩陣求解困難或無(wú)法求解和強(qiáng)非線(xiàn)性方程的線(xiàn)性化近似誤差偏大的問(wèn)題。而UKF通過(guò)目標(biāo)距離和方向角的采樣點(diǎn)近似表示其概率分布，并通過(guò)無(wú)跡變換(Unscented Transform, UT)逼近目標(biāo)狀態(tài)量的后驗(yàn)概率分布,避免了雅克比(Jaccobian)矩陣的復(fù)雜計(jì)算和均值等量的非線(xiàn)性傳遞,保證了較高精度的跟蹤濾波。

目標(biāo)多普勒速度可以認(rèn)為是目標(biāo)距離的一階導(dǎo)數(shù),其雅克比矩陣求解困難,因基于UKF跟蹤濾波框架將目標(biāo)多普勒速度引入觀測(cè)方程[5]。綜上,文中提出了一種基于UKF引入目標(biāo)多普勒速度的水下快速跟蹤方法,實(shí)現(xiàn)在少數(shù)據(jù)點(diǎn)條件下水下近程高速小目標(biāo)的快速跟蹤。

1 UKF原理及步驟

UKF是通過(guò)對(duì)目標(biāo)運(yùn)動(dòng)狀態(tài)先驗(yàn)概率密度分布取近似,通過(guò)采樣Sigma點(diǎn)逼近目標(biāo)運(yùn)動(dòng)狀態(tài)后驗(yàn)概率密度分布,利用UT變換解決目標(biāo)運(yùn)動(dòng)狀態(tài)均值和協(xié)方差的非線(xiàn)性傳遞更新問(wèn)題,避免了求解傳遞函數(shù)的雅克比矩陣,進(jìn)而在卡爾曼濾波(Kalman Filter, KF)的框架下實(shí)現(xiàn)遞推計(jì)算。因?yàn)椋琔KF避免了對(duì)非線(xiàn)性目標(biāo)運(yùn)動(dòng)狀態(tài)方程和觀測(cè)方程本身進(jìn)行線(xiàn)性化近似,不存在高階項(xiàng)省略問(wèn)題,保證了跟蹤濾波的精度和魯棒性[2]。

UT變換是根據(jù)目標(biāo)運(yùn)動(dòng)狀態(tài)先驗(yàn)概率密度分布在目標(biāo)運(yùn)動(dòng)狀態(tài)均值和協(xié)方差的估計(jì)值附近采樣,使采樣點(diǎn)的概率密度分布服從于目標(biāo)運(yùn)動(dòng)狀態(tài)概率密度分布,再將采樣點(diǎn)代入非線(xiàn)性狀態(tài)方程求取目標(biāo)運(yùn)動(dòng)狀態(tài)后驗(yàn)概率密度分布,并以采樣點(diǎn)UT變換后均值和協(xié)方差作為目標(biāo)運(yùn)動(dòng)狀態(tài)的更新值,其原理如圖1所示。

圖1 跟蹤濾波更新原理圖

對(duì)于過(guò)程噪聲和量測(cè)噪聲服從高斯分布的非線(xiàn)性方程,通過(guò)UT變換后Sigma點(diǎn)的均值和協(xié)方差精度可達(dá)到3階Taylor級(jí)數(shù)展開(kāi)精度,即使是非線(xiàn)性分布也可達(dá)到2階Taylor級(jí)數(shù)展開(kāi)精度。

1)求取2n+1個(gè)采樣點(diǎn)UT變換后的Sigma點(diǎn),得

(1)

2)求Sigma點(diǎn)對(duì)應(yīng)的權(quán)值ω

(2)

式中,λ=α2(n+κ)-n表示權(quán)值縮短參數(shù),α表示Sigma點(diǎn)分布狀態(tài)控制參數(shù),κ和β表示待選參數(shù)。

假設(shè)k時(shí)刻的非線(xiàn)性目標(biāo)運(yùn)動(dòng)狀態(tài)方程和觀測(cè)方程為

(3)

式中,W(k)和V(k)分別表示目標(biāo)運(yùn)動(dòng)狀態(tài)過(guò)程噪聲向量和觀測(cè)噪聲向量,均服從高斯分布且互補(bǔ)相關(guān)。此時(shí),UKF遞推過(guò)程如下。

1)選取一組Sigma點(diǎn)集并求其權(quán)值

(4)

2)求Sigma點(diǎn)集的目標(biāo)運(yùn)動(dòng)狀態(tài)一步預(yù)測(cè)值為

X(j)(k+1|k)=f[k,X(j)(k|k)]

(j=1,…,2n+1)

(5)

3)通過(guò)求目標(biāo)運(yùn)動(dòng)狀態(tài)向量一步預(yù)測(cè)值加權(quán)平均值,得到目標(biāo)運(yùn)動(dòng)狀態(tài)一步預(yù)測(cè)估計(jì)值

(6)

(7)

4)基于式(6)和式(7)一步預(yù)測(cè)值代入U(xiǎn)T變換,獲得更新Sigma點(diǎn)集

X(j)(k+1|k)=

(8)

5)將式(8)中新的Sigma點(diǎn)集代入式(3)的目標(biāo)運(yùn)動(dòng)狀態(tài)觀測(cè)一步預(yù)測(cè)值

Y(j)(k+1|k)=h[X(j)(k+1|k)]

(9)

6)將式(9)求得的目標(biāo)運(yùn)動(dòng)狀態(tài)觀測(cè)一步預(yù)測(cè)值,通過(guò)加權(quán)求和得觀測(cè)一步預(yù)測(cè)值的均值和協(xié)方差矩陣

(10)

7)求UKF增益

(11)

8)更新目標(biāo)運(yùn)動(dòng)狀態(tài)和協(xié)方差矩陣

(12)

P(k+1|k+1)=P(k+1|k)-K(k+1)PYkYkKT(k+1)

(13)

通過(guò)UKF遞推過(guò)程可知,其采樣目標(biāo)運(yùn)動(dòng)狀態(tài)估計(jì)值點(diǎn),并進(jìn)行UT變換獲得服從先驗(yàn)概率分布的Sigma點(diǎn),通過(guò)UKF遞推求得新的Sigma點(diǎn)均值和協(xié)方差矩陣,得到當(dāng)前時(shí)刻目標(biāo)運(yùn)動(dòng)狀態(tài)的概率分布函數(shù)的一種統(tǒng)計(jì)學(xué)近似[6]。

2 引入目標(biāo)多普勒速度的觀測(cè)方程

傳統(tǒng)的水下目標(biāo)跟蹤方法主要是基于目標(biāo)距離和方向角構(gòu)建的二維觀測(cè)方程可利用信息有限,目標(biāo)跟蹤精度、收斂速度和魯棒性均較難進(jìn)一步提高[7]。文中,以近程小目標(biāo)高速接近聲吶系統(tǒng)場(chǎng)景進(jìn)行研究,可將新型聲吶系統(tǒng)直接測(cè)得的目標(biāo)多普勒速度引入U(xiǎn)KF觀測(cè)方程,通過(guò)增加觀測(cè)方程的多普勒速度維度,增加跟蹤濾波的信息量,提高了跟蹤精度和濾波收斂速度及魯棒性。傳統(tǒng)的水下跟蹤濾波的目標(biāo)運(yùn)動(dòng)方程和觀測(cè)方程可分別表示為

(14)

式中,Y(k)=[R(k),θ(k)]T,R(k),θ(k)分別表示目標(biāo)距離和目標(biāo)運(yùn)動(dòng)方向角,V(k)表示觀測(cè)噪聲服從高斯白噪聲。觀測(cè)函數(shù)可表示為

(15)

目標(biāo)的多普勒速度是目標(biāo)到傳感器連線(xiàn)的徑向速度[8],等價(jià)于目標(biāo)距離一階導(dǎo)數(shù)可表示為

(16)

組成新的三維觀測(cè)方程可表示為

Y(k)=h[X(k)]+V(k)=

(17)

將改進(jìn)的觀測(cè)方程代入第一節(jié)中的UKF基本框架進(jìn)行步驟1)～步驟8)的遞推運(yùn)算,即可得到基于引入目標(biāo)多普勒速度的水下高精度目標(biāo)跟蹤方法。

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

和雷達(dá)跟蹤目標(biāo)基于極坐標(biāo)系不同,聲吶跟蹤目標(biāo)一般基于直角坐標(biāo)系。文中,以單基地主動(dòng)聲吶跟蹤高速近程小目標(biāo)為例,驗(yàn)證該算法的可行性和有效性。

假設(shè)以發(fā)現(xiàn)目標(biāo)的起始位置為坐標(biāo)原點(diǎn),聲吶中心處于(1000,1000)點(diǎn)處,聲吶信號(hào)發(fā)射周期T為1 s,目標(biāo)沿著45°方位角以x軸25 m/s和y軸25 m/s向聲吶中心運(yùn)動(dòng),目標(biāo)運(yùn)動(dòng)的自身位置和速度均方根誤差分別為10 m和1 m/s,聲吶量測(cè)的目標(biāo)距離、方向角和多普勒速度的均方根誤差分別為4 m,1°和1 m/s,UKF采用比例修正分布采樣Sigma點(diǎn),其參數(shù)設(shè)置為α=0.1、κ=0和β=2,跟蹤持續(xù)時(shí)間為25 s,并進(jìn)行了50次的蒙特卡洛試驗(yàn),得到仿真實(shí)驗(yàn)結(jié)果如圖2所示。

圖2 目標(biāo)跟蹤誤差圖

表1 不同方法跟蹤性能(50次)

由表1可知,引入了目標(biāo)多普勒速度的水下目標(biāo)跟蹤方法濾波后，目標(biāo)位置均方根誤差和速度均方根誤差都明顯優(yōu)于常規(guī)僅基于目標(biāo)距離和方位角的水下跟蹤濾波方法。通過(guò)引入目標(biāo)多普勒速度增加UKF的觀測(cè)方程維度,有效增加了跟蹤濾波的信息量,從而提高了跟蹤濾波的精度,保證在少數(shù)據(jù)點(diǎn)條件下有效跟蹤濾波;從圖2可知,引入目標(biāo)多普勒速度水下目標(biāo)跟蹤方法性能穩(wěn)定,能夠25 s時(shí)長(zhǎng)內(nèi)保持較小的跟蹤誤差,沒(méi)有出現(xiàn)濾波發(fā)散的趨勢(shì),具有較好的魯棒性,而常規(guī)僅基于目標(biāo)距離和方位角的跟蹤濾波方法在跟蹤20 s后,由于跟蹤誤差積累有較明顯的濾波發(fā)散的趨勢(shì),魯棒性較差。

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

本文提出了基于引入目標(biāo)多普勒速度的水下高速小目標(biāo)快速跟蹤方法,將聲吶測(cè)得目標(biāo)高速運(yùn)動(dòng)產(chǎn)生較大的多普勒速度引入基于UKF框架的觀測(cè)方程,增加了跟蹤濾波遞推計(jì)算的信息量,實(shí)現(xiàn)了跟蹤濾波方法在較少的數(shù)據(jù)點(diǎn)條件下的快速收斂,且保證了跟蹤濾波有較高的精度,并通過(guò)仿真驗(yàn)證了該方法達(dá)到了預(yù)期的效果。