顏丙峰,李星秀,吳盤龍

(南京理工大學(xué)自動(dòng)化學(xué)院,江蘇 南京 210094)

隨著反電子偵察、隱身攻擊等技術(shù)的不斷發(fā)展,傳統(tǒng)的雷達(dá)、激光等有源定位機(jī)制面臨易受電子干擾、容易暴露自身位置招致敵方攻擊等問題,存在極大的安全隱患[1]。相比于有源定位技術(shù),無源探測(cè)定位技術(shù)具有隱蔽性強(qiáng)、抗干擾能力強(qiáng)、成本低等優(yōu)點(diǎn),而雙站系統(tǒng)是無源定位中最常用的形式[2]。在實(shí)際戰(zhàn)爭(zhēng)場(chǎng)景中,飛行器的機(jī)動(dòng)性能越來越高,來襲目標(biāo)常采用機(jī)動(dòng)運(yùn)動(dòng)方式躲避觀測(cè)器跟蹤和火炮攔截,而觀測(cè)站能夠獲得的目標(biāo)特征數(shù)據(jù)卻非常有限,目標(biāo)的角度信息幾乎成了唯一可靠的參數(shù),因此,利用目標(biāo)角度信息估計(jì)機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)狀態(tài),具有十分重要的軍事意義。

無跡卡爾曼濾波算法(UKF)用一系列的確定樣本逼近狀態(tài)的后驗(yàn)概率密度,不需要對(duì)非線性函數(shù)進(jìn)行近似,不會(huì)忽略高階項(xiàng),因此，對(duì)于非線性分布的統(tǒng)計(jì)量有較高的計(jì)算精度。然而,UKF算法需要對(duì)協(xié)方差矩陣進(jìn)行Cholesky分解,在多次迭代計(jì)算過程中積累的舍入誤差往往會(huì)導(dǎo)致協(xié)方差矩陣失去正定和對(duì)稱性,造成濾波算法不穩(wěn)定甚至發(fā)生異常。平方根UKF(SRUKF)算法利用協(xié)方差平方根代替協(xié)方差進(jìn)行遞推運(yùn)算,確保了協(xié)方差矩陣的非負(fù)定性,同時(shí)避免了非線性系統(tǒng)的線性化,計(jì)算量較少,實(shí)時(shí)性強(qiáng),具有更快的運(yùn)算速度[3]。文獻(xiàn)[3]和文獻(xiàn)[4]分別將SRUKF運(yùn)用于水下目標(biāo)和地面越野目標(biāo)的跟蹤,驗(yàn)證了其良好的跟蹤效果。此外,當(dāng)濾波算法采用的運(yùn)動(dòng)模型與目標(biāo)實(shí)際運(yùn)動(dòng)狀態(tài)不符合時(shí),將會(huì)產(chǎn)生很大的跟蹤誤差。針對(duì)此問題,Blom提出了交互式多模型算法(IMM)[5],利用不同的模型對(duì)機(jī)動(dòng)目標(biāo)的運(yùn)動(dòng)狀態(tài)進(jìn)行匹配。文獻(xiàn)[6]基于當(dāng)前統(tǒng)計(jì)模型,將IMM算法和UKF算法相結(jié)合,使得算法具有對(duì)不同目標(biāo)機(jī)動(dòng)模式的自適應(yīng)跟蹤能力,但該算法抗干擾能力不強(qiáng);文獻(xiàn)[7]提出了基于IMM-UKF的融合濾波模型,將機(jī)載雷達(dá)和電子支援措施傳感器兩種觀測(cè)數(shù)據(jù)融合后作為濾波輸入,提高了算法的跟蹤精度。

本文將IMM算法和平方根UKF算法相結(jié)合,用于雙站無源定位跟蹤。首先，分析測(cè)向交叉定位的原理,求出系統(tǒng)的測(cè)量方程;接著，引入球形無跡變換,取代傳統(tǒng)的對(duì)稱sigma點(diǎn)采樣,并利用協(xié)方差平方根代替協(xié)方差進(jìn)行遞推運(yùn)算,減少了計(jì)算量,提高了算法的穩(wěn)定性;最后，將其與IMM算法相結(jié)合,通過仿真對(duì)比,驗(yàn)證了IMM-SRUKF的優(yōu)越性。

1 雙站測(cè)向交叉定位原理

目標(biāo)與觀測(cè)站的幾何關(guān)系如圖1所示,分別以兩個(gè)觀測(cè)站所在位置為原點(diǎn)建立坐標(biāo)系。為了實(shí)現(xiàn)目標(biāo)定位,需要求得兩觀測(cè)站之間基線AB的長(zhǎng)度,以及基線與AT、BT兩條方位線的夾角,然后通過正弦定理,即可求得目標(biāo)的斜距及位置。

圖1 目標(biāo)與觀測(cè)站幾何關(guān)系圖

1)通過高精度的定位設(shè)備測(cè)得兩個(gè)觀測(cè)站的經(jīng)緯高坐標(biāo)(L1,B1,H1)和(L2,B2,H2),將觀測(cè)站在大地坐標(biāo)系中的坐標(biāo)轉(zhuǎn)換到需要的東北天坐標(biāo)系中。該過程分兩步進(jìn)行。

(1)

其次，將空間直角坐標(biāo)系轉(zhuǎn)換到各觀測(cè)站相應(yīng)的東北天坐標(biāo)系(ai,bi,ci):

(2)

其中，(a1,b1,c1)表示第一個(gè)觀測(cè)站在第二個(gè)觀測(cè)站東北天坐標(biāo)系下的坐標(biāo),(a2,b2,c2)表示第二個(gè)觀測(cè)站在第一個(gè)觀測(cè)站東北天坐標(biāo)系下的坐標(biāo)。

ψi=arccos((aicosβicosαi+bicosβisinαi+

(3)

3)由三角形的正弦定理可知,目標(biāo)與各個(gè)觀測(cè)站之間的距離為

(4)

基于此距離信息以及上面求得的方位線與基線夾角,可以求得目標(biāo)在兩個(gè)觀測(cè)站東北天坐標(biāo)系下的定位信息為

(5)

2 IMM-SRUKF算法

純幾何定位獲取的目標(biāo)定位信息,其定位精度往往不滿足系統(tǒng)需求,需采用濾波算法進(jìn)一步處理。在本應(yīng)用場(chǎng)景中,無論狀態(tài)方程還是觀測(cè)方程,都是非線性的。常用的非線性濾波算法中,UKF相比于EKF算法,無須計(jì)算復(fù)雜的雅克比(Jacobian)矩陣,不會(huì)存在忽略Taylor展開高階項(xiàng)導(dǎo)致的濾波發(fā)散問題;相比于PF算法,UKF不存在處理高維狀態(tài)向量系統(tǒng)計(jì)算量極其巨大的問題?？傮w上,UKF在計(jì)算量與濾波精度上綜合效果更優(yōu)。

但是UKF算法需要對(duì)協(xié)方差矩陣進(jìn)行Cholesky 分解,而積累的舍入誤差經(jīng)常會(huì)導(dǎo)致協(xié)方差矩陣失去正定和對(duì)稱性,導(dǎo)致濾波算法不穩(wěn)定甚至發(fā)生異常。針對(duì)此問題,可以利用協(xié)方差平方根代替協(xié)方差進(jìn)行遞推運(yùn)算。

2.1 測(cè)量模型

目標(biāo)的測(cè)量模型如下:

Zk+1=h(Xk+1)+Vk+1

(6)

式中,Xk為n×1維的狀態(tài)向量,Zk為4×1維的觀測(cè)向量。每個(gè)觀測(cè)站分別觀測(cè)到目標(biāo)的高低角和方位角信息,根據(jù)上一節(jié)中提到的目標(biāo)幾何關(guān)系,可以得到雙站量測(cè)模型:

(7)

式中,βi與αi分別為兩個(gè)觀測(cè)站觀測(cè)到的高低角和方位角。假設(shè)目標(biāo)在k時(shí)刻的位置信息為[xk,yk,zk],兩個(gè)觀測(cè)站的坐標(biāo)分別為[a1,b1,c1]和[a2,b2,c2],則上述的四個(gè)角度可表示為:

(8)

2.2 SRUKF算法流程

1)濾波器初始化

(9)

(10)

式中，X0為目標(biāo)初始狀態(tài)矢量,S0為狀態(tài)誤差協(xié)方差矩陣的初始平方根,fchol表示矩陣的Cholesky分解。

2)球形無跡變換

依據(jù)對(duì)sigma點(diǎn)集的選取方式不同,無跡變換的采樣方式可以分為對(duì)稱采樣和非對(duì)稱采樣。對(duì)稱采樣的sigma點(diǎn)需要2n+1個(gè),n為狀態(tài)變量的維數(shù)。各個(gè)采樣點(diǎn)的權(quán)值如下:

(11)

式中,下標(biāo)m為均值,c為協(xié)方差,上標(biāo)為采樣點(diǎn)序號(hào),參數(shù)λ=α2(n+κ)-n為比例參數(shù),用于降低總的預(yù)測(cè)誤差,α用于控制采樣點(diǎn)在其均值附近的分布情況。對(duì)于一個(gè)n維的狀態(tài),每次運(yùn)算需要計(jì)算2n+1個(gè)sigma點(diǎn),計(jì)算量相對(duì)較大。

相比之下,非對(duì)稱采樣的球形無跡變換只需n+2個(gè)sigma點(diǎn)即可實(shí)現(xiàn),且無須繁雜的調(diào)參。平方根形式的球型無跡變換sigma點(diǎn)計(jì)算如下:

(12)

(13)

其中，W0的取值范圍通常為[0,1),且多取值為0。由此可以看出,球形無跡變換在權(quán)值選取時(shí)無須提前設(shè)置α、β、κ等權(quán)值參數(shù),調(diào)參更為簡(jiǎn)單。

3)時(shí)間更新

(14)

(15)

(16)

(15)

4)量測(cè)更新

(18)

(19)

公式(19)的作用和公式(15)、(16)相同,用來計(jì)算輸出殘差的協(xié)方差平方根。

5)狀態(tài)更新

計(jì)算卡爾曼濾波增益,更新目標(biāo)的狀態(tài)和相應(yīng)的協(xié)方差矩陣:

(20)

2.3 IMM-SRUKF算法

在實(shí)際場(chǎng)景中,目標(biāo)不會(huì)以單一的運(yùn)動(dòng)模式進(jìn)行飛行,當(dāng)濾波算法采用的運(yùn)動(dòng)模型與目標(biāo)實(shí)際運(yùn)動(dòng)狀態(tài)不符合時(shí),將會(huì)產(chǎn)生很大的跟蹤誤差。交互多模型算法為每個(gè)模型都建立相應(yīng)的濾波器,各個(gè)濾波器采用并行工作方式,用模型的后驗(yàn)概率對(duì)濾波器的輸入、輸出進(jìn)行加權(quán)計(jì)算,可以很好地解決此問題。

圖2 IMM算法流程圖

1) 輸入交互

模型初始狀態(tài)為

(21)

模型初始協(xié)方差為

(22)

其中:

(23)

(24)

2) SRUKF濾波

根據(jù)不同的狀態(tài)模型,分別設(shè)計(jì)相應(yīng)的SRUKF濾波器,然后，把交互步驟得到的k-1時(shí)刻狀態(tài)估計(jì)和協(xié)方差估計(jì),結(jié)合k時(shí)刻量測(cè)數(shù)據(jù),輸入濾波器中。輸出各個(gè)濾波器的狀態(tài)和協(xié)方差估計(jì)值。

3) 各模型的概率更新

首先計(jì)算各個(gè)模型的可能性。各模型k時(shí)刻的似然值Λj(k)為

(25)

其中，M表示量測(cè)模型的維數(shù),由此可以求得各模型的校正概率μi(k):

(26)

(27)

狀態(tài)、協(xié)方差更新:

(28)

(29)

3 仿真驗(yàn)證

選定一個(gè)觀測(cè)站的位置作為坐標(biāo)原點(diǎn)建立坐標(biāo)系,假設(shè)兩個(gè)觀測(cè)站的坐標(biāo)分別為[0,0,0]和[500,0,0],分別采用CV、CA和CT模型構(gòu)造目標(biāo)運(yùn)動(dòng)模型,目標(biāo)運(yùn)動(dòng)狀態(tài)隨時(shí)刻的變化如表1所示。

表1 目標(biāo)各個(gè)時(shí)刻運(yùn)動(dòng)狀態(tài)表

采樣周期選取為1 s,目標(biāo)狀態(tài)Xk為9×1維的狀態(tài)向量,包含目標(biāo)在三維空間的位置、速度和加速度信息,初始的狀態(tài)通過幾何定位計(jì)算得出,作為濾波初值,輸入濾波器中,量測(cè)噪聲矩陣選取為diag([0.1mrad;0.1mrad;0.1mrad;0.1mrad]),假設(shè)各模型的初始概率值為[0.3,0.3,0.4],模型i到模型j之間的概率轉(zhuǎn)移矩陣為

(30)

不同時(shí)刻各個(gè)模型的概率如圖3所示。

圖3 各模型不同時(shí)刻模型概率曲線圖

對(duì)比圖3和表1可以發(fā)現(xiàn),不同模型的主導(dǎo)時(shí)刻與目標(biāo)的真實(shí)運(yùn)動(dòng)狀態(tài)基本符合,驗(yàn)證了IMM-SRUKF算法模型概率預(yù)測(cè)的準(zhǔn)確性。此外,根據(jù)圖4和圖5可以看出,在測(cè)角誤差為0.1 mrad時(shí),目標(biāo)各個(gè)方向的位置和速度估計(jì)均可達(dá)到良好的跟蹤精度。

圖4 各個(gè)方向位置跟蹤誤差

圖5 各個(gè)方向速度跟蹤誤差

此外,在其他初始條件均相同的情況下,選取三個(gè)不同的量測(cè)噪聲,將本文的IMM-SRUKF算法和IMM-EKF算法、IMM-UKF算法進(jìn)行對(duì)比,觀測(cè)站測(cè)量的角度誤差分別選取為[0.1 mrad;1 mrad;5 mrad],性能指標(biāo)采取相對(duì)距離誤差RRE來衡量:

(31)

對(duì)每一個(gè)場(chǎng)景執(zhí)行100組Monte Carlo仿真,若在跟蹤結(jié)束時(shí)刻RRE<15%,則認(rèn)為本次收斂,否則認(rèn)為發(fā)散。然后,對(duì)所有能收斂的樣本中跟蹤結(jié)束時(shí)刻的定位均方根誤差(RMSE)進(jìn)行統(tǒng)計(jì),求取其平均值,得到相對(duì)的定位精度。最終的仿真結(jié)果如表2和圖6、圖7和圖8所示。

表2 各跟蹤算法穩(wěn)定性和定位精度

圖6 角度誤差為0.1 mrad時(shí)各算法跟蹤誤差

圖7 角度誤差為1 mrad時(shí)各算法跟蹤誤差

圖8 角度誤差為5 mrad時(shí)各算法跟蹤誤差

通過對(duì)比表2和圖6～8中的數(shù)據(jù)可以發(fā)現(xiàn),在觀測(cè)精度較高時(shí),各算法均有較好的表現(xiàn)。如圖6所示,三種算法在測(cè)量角度誤差均為0.1 mrad時(shí),收斂時(shí)間和精度上都取得了良好的效果。隨著觀測(cè)值精度的降低,各個(gè)算法性能的差異性開始體現(xiàn),IMM-EKF算法相比于IMM-UKF算法和IMM-SRUKF算法,發(fā)散的次數(shù)明顯增多,收斂時(shí)間以及精度都相對(duì)較差,這主要緣于EKF算法在泰勒展開時(shí)高階項(xiàng)的忽略。隨著觀測(cè)精度的進(jìn)一步降低,IMM-UKF算法由于在計(jì)算過程中的舍入誤差,經(jīng)常會(huì)引起協(xié)方差矩陣的負(fù)定,導(dǎo)致發(fā)散和穩(wěn)定性大幅降低;而IMM-SRUKF算法采用平方根代替協(xié)方差參與遞推運(yùn)算,有效地解決了此問題,提高了濾波器的穩(wěn)定性和精度。

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

本文以雙站純角度定位為應(yīng)用場(chǎng)景,首先，從測(cè)向交叉定位原理出發(fā),分析雙站定位的幾何模型,得出系統(tǒng)的測(cè)量方程;然后，介紹了平方根SRUKF的算法流程,對(duì)常規(guī)UKF算法進(jìn)行改進(jìn);最后，將IMM算法與平方根UKF相結(jié)合,通過仿真在收斂次數(shù)、跟蹤精度上和其他的交互多模型算法進(jìn)行了比較。最終的仿真結(jié)果表明,IMM-SRUKF算法在收斂性以及定位精度上有著明顯的提高,算法有很好的穩(wěn)定性。