基于交互式多模型的主被動(dòng)站跟蹤算法

郁 凱,王驤予涵,盧鴻謙

(1. 哈爾濱工業(yè)大學(xué)航天學(xué)院,哈爾濱 150000;2. 哈爾濱工程大學(xué)航天與建筑工程學(xué)院,哈爾濱 150001)

0 引言

隨著技術(shù)的不斷更新,現(xiàn)代飛行器的速度顯著提高。因此,以往將飛行器的運(yùn)動(dòng)狀態(tài)簡(jiǎn)化為勻速運(yùn)動(dòng)的做法已經(jīng)不再具有代表性。如果地面設(shè)備用于觀測(cè)飛行器,考慮到駕駛員的操控行為和控制指令,目標(biāo)可能隨時(shí)進(jìn)行轉(zhuǎn)彎、閃避或采取其他特殊的攻擊姿態(tài)等機(jī)動(dòng)動(dòng)作[1]。通過(guò)使用單個(gè)或多個(gè)主動(dòng)或被動(dòng)站點(diǎn),探測(cè)目標(biāo)(散射體或輻射體)并獲取時(shí)間信息TOA(time of arrival)以及角度信息AOA(angle of arrival)等數(shù)據(jù)[2],然后結(jié)合適當(dāng)?shù)臄?shù)據(jù)處理方法(例如三角定位、時(shí)差定位、頻差定位等),可以確定機(jī)動(dòng)目標(biāo)在三維空間中的位置[3]。這是探測(cè)系統(tǒng)用于對(duì)機(jī)動(dòng)目標(biāo)進(jìn)行定位和跟蹤的基本過(guò)程。

最初的卡爾曼濾波適用于線性系統(tǒng),利用之前時(shí)刻的狀態(tài)對(duì)當(dāng)前時(shí)刻的狀態(tài)進(jìn)行估計(jì),并用觀測(cè)量對(duì)估計(jì)狀態(tài)進(jìn)行修正。這種算法在高斯條件下不僅滿足極大似然估計(jì),還是最小方差估計(jì)的最優(yōu)結(jié)果。目前,對(duì)于機(jī)動(dòng)目標(biāo)定位算法研究里面最普遍的方法仍是基于上述方法及其優(yōu)化方法[4]。優(yōu)化方法主要包括擴(kuò)展卡爾曼濾波器(extended Kalman filter,EKF)、粒子濾波器等[5]。羅笑冰等[6]利用雷達(dá)提供的量測(cè)信息求取噪聲參數(shù),進(jìn)而利用噪聲信息優(yōu)化Singer模型加速度先驗(yàn)信息,提高了該模型的適用性范圍。在此基礎(chǔ)上為了更好地融合多站提供的量測(cè)信息,又發(fā)展出了多模型算法[7],這種算法在每次迭代濾波過(guò)程中充分考慮其他各個(gè)濾波器模型估計(jì)結(jié)果,引入基于貝葉斯估計(jì)的交互理念,實(shí)現(xiàn)更高精度的目標(biāo)跟蹤。典型的有交互式多模型算法 (interacting multiple models,IMM)。該方法可以根據(jù)存儲(chǔ)器靈活選取馬爾可夫階數(shù),完成更高精度的運(yùn)動(dòng)建模與濾波估計(jì)融合[8]。臧勤等[9]為了解決對(duì)地目標(biāo)定位過(guò)程中出現(xiàn)的不連續(xù)收斂現(xiàn)象,將時(shí)間信息引入交互式多模型,但是這種方法對(duì)時(shí)間噪聲較為敏感,適用于遮擋物較少的空間跟蹤。Phani和Arye[10]從IMM貝葉斯原理出發(fā),使用順序貝葉斯濾波完成跟蹤過(guò)程,該算法極大降低了原有濾波方法的計(jì)算復(fù)雜度和觀測(cè)空間復(fù)雜度,但是也舍棄了一定精度。為了削弱單站定位的故障性影響,提高系統(tǒng)可靠性和穩(wěn)定性,葉瑾等[11]首先對(duì)傳感器的量測(cè)信息進(jìn)行精度方面的加權(quán)融合,然后使用對(duì)非線性量測(cè)關(guān)系一階泰勒展開(kāi),在交互式多模型的框架下完成了初步的信息融合跟蹤,但是該算法子濾波器運(yùn)動(dòng)模型單一,在目標(biāo)做逃避飛行等運(yùn)動(dòng)時(shí),精度降低。

上述方法對(duì)于機(jī)動(dòng)目標(biāo)的跟蹤大多基于單個(gè)運(yùn)動(dòng)模型或者機(jī)動(dòng)性不強(qiáng)的運(yùn)動(dòng)模型組來(lái)計(jì)算,無(wú)法降低模型失配帶來(lái)的跟蹤損失[12]。為此,本文首先將勻速直線運(yùn)動(dòng)、勻加速直線運(yùn)動(dòng)、慢機(jī)動(dòng)Singer模型和快機(jī)動(dòng)Singer模型引入交互式多模型來(lái)預(yù)測(cè)目標(biāo)機(jī)動(dòng)過(guò)程。其中勻速直線運(yùn)動(dòng)和勻加速直線運(yùn)動(dòng)適用于目標(biāo)加速度穩(wěn)定變化的目標(biāo)運(yùn)動(dòng),快Singer和慢Singer模型則從時(shí)間的相關(guān)領(lǐng)域模擬目標(biāo)加速度機(jī)動(dòng)程度。然后利用TOA-AOA幾何關(guān)系求解的目標(biāo)位置作為濾波初始值狀態(tài),結(jié)合擴(kuò)展卡爾曼子濾波器構(gòu)建交互式多模型算法。由于單純利用TOA-AOA幾何關(guān)系就可以求解得到目標(biāo)位置,所以相較于其他濾波隨機(jī)生成的高斯初值模型,本文濾波算法從一開(kāi)始就可以收斂,且收斂穩(wěn)定。最后,本文算法在目標(biāo)機(jī)動(dòng)情況下,通過(guò)交互式多模型框架切換模型子濾波器應(yīng)對(duì)目標(biāo)非線性狀態(tài)轉(zhuǎn)移,進(jìn)而有效跟蹤目標(biāo)。

1 主被動(dòng)站跟蹤目標(biāo)數(shù)學(xué)模型

假設(shè)主動(dòng)站O的坐標(biāo)為(0,0,0),發(fā)出電磁波的時(shí)間為T0,(xA,yA,zA)為被動(dòng)站A的坐標(biāo),目標(biāo)M的坐標(biāo)為(x,y,z) ,A對(duì)M的觀測(cè)量為(φ,ψ,TA)′,TA為被動(dòng)站A接收波的時(shí)間,φ為被動(dòng)站A接收波方位角,ψ為被動(dòng)站A接收波俯仰角,量測(cè)模型如下

(1)

(2)

(TA-T0)·c=

(3)

其中c為光速。

Xk+1=φXk+W

(4)

式中,k代表時(shí)刻,φ代表狀態(tài)轉(zhuǎn)移矩陣,W為系統(tǒng)噪聲。

觀測(cè)方程為

(5)

(6)

觀測(cè)方程為非線性方程,根據(jù)泰勒展開(kāi)式保留一階項(xiàng)的方法求得其擴(kuò)展卡爾曼濾波量測(cè)矩陣為

(7)

2 交互式多模型算法

本章算法以TOA-AOA幾何關(guān)系求解得到的目標(biāo)位置作為勻速直線運(yùn)動(dòng)擴(kuò)展卡爾曼子濾波器、勻加速直線運(yùn)動(dòng)擴(kuò)展卡爾曼子濾波器、快Singer模型擴(kuò)展卡爾曼子濾波器和慢Singer模型擴(kuò)展卡爾曼子濾波器的初始值,直接保證了算法收斂性;接著將各個(gè)模型子濾波器的狀態(tài)估計(jì)作為交互式多模型的輸入,根據(jù)高斯似然比分配各個(gè)子濾波器權(quán)值比重,最后加權(quán)得到下一時(shí)刻的目標(biāo)位置估計(jì)。通過(guò)模型切換子濾波器組合的非線性關(guān)系,從加速度層面實(shí)時(shí)模擬目標(biāo)可能出現(xiàn)的任何機(jī)動(dòng)運(yùn)動(dòng)。

2.1 交互式多模型原理推導(dǎo)

在目標(biāo)機(jī)動(dòng)運(yùn)動(dòng)時(shí),使用單一加速度建模方式無(wú)法避免模型失配帶來(lái)的損失。動(dòng)態(tài)多模型算法可以緩解該矛盾,其中突出的算法就是IMM算法。IMM法來(lái)源于廣義貝葉斯估計(jì),在各類濾波器模型的基礎(chǔ)上,根據(jù)高斯置信度模型實(shí)時(shí)選擇最優(yōu)機(jī)動(dòng)濾波器模型[13]。

若量測(cè)噪聲滿足高斯假設(shè),則根據(jù)卡爾曼濾波的量測(cè)模型,可確定兩隨機(jī)變量量測(cè)參數(shù)Z、狀態(tài)參數(shù)X之間的關(guān)系,可由如下概率密度函數(shù)表示

(8)

其中n代表量測(cè)參數(shù)Z的維度。

1)狀態(tài)估計(jì)的交互處理:

(9)

其中

Poj(k-1|k-1)=

(10)

2)模型修正:

(11)

Poj(k|k-1)=φPoj(k-1|k-1)φT+

Γ(k-1)Q(k-1)Γ(k-1)T

(12)

Kj(k)=Poj(k|k-1)Hj(k)T·

(Hj(k)Poj(k|k-1)Hj(k)T)-1

(13)

Hj(k)Xoj(k|k-1))

(14)

Pj(k|k)=(I-Kj(k)Hj(k))Poj(k|k-1)

(15)

3)模型可能性計(jì)算:

(16)

(17)

(18)

4)模型概率更新:

(19)

(20)

(21)

(22)

2.2 目標(biāo)機(jī)動(dòng)運(yùn)動(dòng)模型

2.2.1 離散勻速直線運(yùn)動(dòng)模型

目標(biāo)運(yùn)動(dòng)過(guò)程一般是平穩(wěn)運(yùn)動(dòng)和機(jī)動(dòng)運(yùn)動(dòng)的結(jié)合,平穩(wěn)運(yùn)動(dòng)狀態(tài)可以用離散勻速(constant velocity, CV)模型來(lái)表述,下文叫CV模型。

其狀態(tài)方程可以寫為

XT,k+1=φXT,k+Γvk

(23)

其中δT為采樣步長(zhǎng),本文統(tǒng)一為1 s,v1k,v2k,v3k為相互獨(dú)立的高斯白噪聲。

因此狀態(tài)噪聲的方差陣為

(24)

記

2.2.2 離散勻加速直線運(yùn)動(dòng)模型

對(duì)于大部分目標(biāo)的不劇烈機(jī)動(dòng)運(yùn)動(dòng)狀態(tài)建模,為了計(jì)算方便,常以用離散勻加速(constant acceleration, CA)模型來(lái)表述,也就是CA模型。

它的狀態(tài)方程為

XT,k+1=φXT,k+Γvk

(25)

式中

vk[v1k,v2k,v3k]T各元素均服從高斯噪聲,它們代表目標(biāo)三軸運(yùn)動(dòng)的加速度變化量。于是狀態(tài)噪聲協(xié)方差陣為

(26)

2.2.3 Singer運(yùn)動(dòng)模型

Singer運(yùn)動(dòng)模型最早來(lái)源于全局統(tǒng)計(jì)模型[14],該模型根據(jù)大量實(shí)測(cè)機(jī)動(dòng)數(shù)據(jù)和領(lǐng)域加速度技術(shù)歸納而來(lái),基本滿足各類運(yùn)動(dòng)模式。但是這種算法對(duì)于經(jīng)驗(yàn)參數(shù)要求比較高,目標(biāo)機(jī)動(dòng)程度不一樣,Singer參數(shù)也需要變動(dòng)。為此,設(shè)計(jì)快機(jī)動(dòng)Singer模型和慢機(jī)動(dòng)Singer模型。

設(shè)定目標(biāo)機(jī)動(dòng)加速度為α(t), 其相關(guān)函數(shù)為

(27)

(28)

式中,P0為非機(jī)動(dòng)概率;典型的經(jīng)驗(yàn)取值范圍[15]:大氣擾動(dòng)κ1=1,慢速轉(zhuǎn)彎κ2=1/60,逃避機(jī)動(dòng)κ3=1/20。大氣擾動(dòng)參數(shù)通常指的是用于描述大氣環(huán)境對(duì)目標(biāo)運(yùn)動(dòng)的不確定性和擾動(dòng)的參數(shù)。大氣擾動(dòng)參數(shù)可以包括風(fēng)速、風(fēng)向、氣壓變化等與大氣環(huán)境相關(guān)的因素。這些因素會(huì)對(duì)目標(biāo)的運(yùn)動(dòng)軌跡產(chǎn)生影響,導(dǎo)致目標(biāo)的實(shí)際運(yùn)動(dòng)與理想模型存在偏差。

由于a(t)是色噪聲,因此可以用白噪聲w(t)通過(guò)一個(gè)成形濾波器[15]來(lái)產(chǎn)生,產(chǎn)生的關(guān)系為

(29)

式中w(t)的相關(guān)函數(shù)為

(30)

目標(biāo)連續(xù)運(yùn)動(dòng)狀態(tài)模型為

(31)

將其離散化,該模型可以變?yōu)?/p>

XδT,k+1=φ(δT,α)XδT,k+Uk

(32)

φ(T,α)=L-1[(sI-N)-1]

=Fj

(33)

因此,噪聲Uk的協(xié)方差矩陣為

(34)

Qk的精確表達(dá)式(Qk為對(duì)稱陣)為

(35)

2.3 TOA-AOA求解狀態(tài)初始值

交互式多模型濾波和擴(kuò)展卡爾曼濾波算法的收斂速度和收斂穩(wěn)定性與目標(biāo)位置初始有關(guān)系。可以利用TOA-AOA幾何關(guān)系,根據(jù)量測(cè)信息求取目標(biāo)位置解析解,以此作為濾波算法狀態(tài)迭代的初值。目標(biāo)M與主動(dòng)站O、被動(dòng)站A的空間位置如圖1所示。

圖1 目標(biāo)解算模型Fig.1 Target solution model

(36)

如果存在β=π-u的幾何關(guān)系,那么

f=LOA·cos(β)

(37)

b=LOA·sin(β)

(38)

(39)

M的坐標(biāo),即各個(gè)子濾波器濾波算法位置初值為

(40)

速度初值和加速度初值統(tǒng)一設(shè)置為0。

3 仿真分析

根據(jù)主-被動(dòng)結(jié)合定位方法的基本原理,針對(duì)不同機(jī)動(dòng)特性目標(biāo)進(jìn)行運(yùn)動(dòng)估計(jì)。在對(duì)機(jī)動(dòng)目標(biāo)進(jìn)行跟蹤時(shí),IMM算法可以在各類運(yùn)動(dòng)模型的擴(kuò)展卡爾曼子濾波器中實(shí)時(shí)轉(zhuǎn)換,從而實(shí)現(xiàn)更高精度的目標(biāo)定位跟蹤。但是,在一般IMM算法中,各個(gè)模型匹配的都是CV和CA模型的擴(kuò)展卡爾曼濾波器。當(dāng)目標(biāo)發(fā)生強(qiáng)機(jī)動(dòng)時(shí),跟蹤精度迅速降低,嚴(yán)重影響跟蹤效果。本章針對(duì)目標(biāo)飛行完整過(guò)程(包含起飛與降落)將融合勻速直線運(yùn)動(dòng)、勻加速直線運(yùn)動(dòng)、慢機(jī)動(dòng)Singer1模型和快機(jī)動(dòng)Singer2模型的IMM算法,簡(jiǎn)稱IMM2,與單個(gè)EKF狀態(tài)運(yùn)動(dòng)模型進(jìn)行比對(duì)仿真。

設(shè)置主動(dòng)站O位于(0,0,0),被動(dòng)站A位于(-10,0,0) km,被動(dòng)站B位于(0,10,0) km,被動(dòng)站C位于(10,0,0) km,被動(dòng)站D位于(0,-10,0) km,仿真時(shí)間長(zhǎng)2 600 s,采樣時(shí)間間隔2 s,蒙特卡羅仿真300次。目標(biāo)最大水平速度60 m/s,最大爬升率7 m/s,最大下降率8 m/s,最大使用高度5 km,最大俯仰角±50°,初始位置(-562,-153,-421) m,表1是目標(biāo)部分機(jī)動(dòng)時(shí)間表。

表1 目標(biāo)部分機(jī)動(dòng)時(shí)間表

設(shè)定IMM子濾波器初始參數(shù):Singer1模型的xyz三軸機(jī)動(dòng)頻率分別為1/60,1/60,1,單位:Hz。三軸機(jī)動(dòng)加速度的極大值分別為8 m/s2,5 m/s2,3 m/s2,三軸最大機(jī)動(dòng)概率為0.1,0.1,0.1,三軸非機(jī)動(dòng)概率為0.5,0.5,0.5;Singer2模型的xyz三軸機(jī)動(dòng)頻率分別為1/20,1/20,1/60,單位:Hz,三軸機(jī)動(dòng)加速度的極大值分別為8 m/s2,8 m/s2,3 m/s2,三軸最大機(jī)動(dòng)概率為0.2,0.3,0.1,三軸非機(jī)動(dòng)概率為0.2,0.1,0.1;CA模型的三軸狀態(tài)標(biāo)準(zhǔn)差分別為6 m/s2,4 m/s2,2 m/s2;CV模型的三軸狀態(tài)標(biāo)準(zhǔn)差分別為5 m/s2,3 m/s2,2 m/s2。

設(shè)置測(cè)角誤差為1°,時(shí)間誤差為20 ns,模型互相關(guān)轉(zhuǎn)移矩陣、模型置信度分別如下

表2所示為各類算法從0時(shí)刻開(kāi)始到達(dá)該時(shí)間段內(nèi)的位置定位誤差均值。對(duì)比仿真圖2、圖3和表2可以發(fā)現(xiàn),IMM2模型在機(jī)動(dòng)時(shí)刻的定位跟蹤效果更好,但是存在收斂速度較慢的問(wèn)題,這是由于前半段時(shí)間,目標(biāo)上升運(yùn)動(dòng)導(dǎo)致俯仰角變化太大。此外,不同濾波器參數(shù)的設(shè)置也會(huì)影響濾波效果。可以看出來(lái),針對(duì)該軌跡Singer1模型的機(jī)動(dòng)參數(shù)設(shè)置比Singer2模型的設(shè)置更加合理。接下來(lái)?yè)Q一個(gè)上升運(yùn)動(dòng)輕緩的運(yùn)動(dòng)軌跡,濾波器參數(shù)設(shè)置不變,如圖4,5,6所示。

表2 IMM與各類算法濾波誤差均值對(duì)比表

圖2 IMM與各類算法濾波效果圖Fig.2 Filtering effect of IMM and various algorithms

圖3 目標(biāo)劇烈俯仰運(yùn)動(dòng)的IMM與各類算法X軸定位誤差圖Fig.3 IMM of vigorous pitch motion of target and X-axis positioning error plot of various algorithms

圖4 目標(biāo)輕微俯仰運(yùn)動(dòng)的IMM與各類算法X軸定位誤差圖Fig.4 IMM of slight pitch motion of target and X-axis positioning error plot of various algorithms

圖5 IMM與各類算法濾波效果圖Fig.5 Filtering effect of IMM and various algorithms

圖6 IMM三軸跟蹤誤差圖Fig.6 IMM triaxial tracking error

可以發(fā)現(xiàn),在飛行目標(biāo)針對(duì)基站俯仰角變化不劇烈時(shí),IMM2模型收斂速度明顯提升且定位誤差在50 m左右。

接下來(lái)使用第一個(gè)目標(biāo)軌跡研究IMM濾波參數(shù)和噪聲變化對(duì)定位影響,如表3所示。

表3 IMM不同參數(shù)下的目標(biāo)跟蹤誤差

其中

μ1=[0.2,0.4,0.3,0.1],μ2=[0.4,0.2,0.1,0.3]

由表3可知,模型狀態(tài)方程的初始狀態(tài)轉(zhuǎn)移矩陣對(duì)于跟蹤效果有直接影響,但是隨著迭代更新,算法會(huì)根據(jù)各個(gè)子濾波器殘差和似然檢驗(yàn)比實(shí)時(shí)選擇濾波器,所以算法依然可以有效跟蹤機(jī)動(dòng)目標(biāo),且誤差波動(dòng)不大;模型誤差相較于量測(cè)信息誤差,對(duì)于目標(biāo)的定位精度影響更大。

4 結(jié)論

本文在高斯白噪聲條件下,根據(jù)主被動(dòng)站提供的與目標(biāo)之間的距離和方位角,俯仰角量測(cè)信息進(jìn)行對(duì)機(jī)動(dòng)目標(biāo)的定位跟蹤研究。鑒于使用單一運(yùn)動(dòng)模型的擴(kuò)展卡爾曼濾波在目標(biāo)機(jī)動(dòng)時(shí)刻跟蹤誤差變大的問(wèn)題,以TOA-AOA解析解為初始值,設(shè)計(jì)了一種融合勻速直線運(yùn)動(dòng)、勻加速直線運(yùn)動(dòng)、快機(jī)動(dòng)Singer2運(yùn)動(dòng)和慢機(jī)動(dòng)Singer1運(yùn)動(dòng)的IMM算法。通過(guò)仿真表明:

1)融合了CV、CA、慢機(jī)動(dòng)Singer1和快機(jī)動(dòng)Singer2的IMM濾波算法相較于單一運(yùn)動(dòng)模型的EKF算法,在目標(biāo)與基站幾十千米范圍、時(shí)間量測(cè)誤差20 ns、角度誤差2°的條件下,對(duì)機(jī)動(dòng)目標(biāo)的跟蹤收斂誤差提升30%。

2)Singer模型對(duì)于機(jī)動(dòng)運(yùn)動(dòng)的狀態(tài)轉(zhuǎn)移預(yù)測(cè)性要好于勻加速直線運(yùn)動(dòng)模型20%。

3)IMM濾波算法具有很強(qiáng)的對(duì)象特性。如果知道目標(biāo)先驗(yàn)加速度和機(jī)動(dòng)參數(shù)范圍可以有效跟蹤機(jī)動(dòng)目標(biāo);如果目標(biāo)先驗(yàn)參數(shù)信息不足,Singer模型的針對(duì)性會(huì)降低,需要根據(jù)量測(cè)信息估計(jì)目標(biāo)加速度等信息的范圍。

4)本實(shí)驗(yàn)針對(duì)高斯白噪聲,缺乏非高斯噪聲下的優(yōu)化設(shè)計(jì)工作,擬根據(jù)粒子濾波原理,在非高斯白噪聲條件下,設(shè)計(jì)適用于任何噪聲的交互式多模型算法。