章 靜,王社陽,魏喜慶
(1.上海航天技術(shù)研究院,上海 201109;2.上海機(jī)電工程研究所,上海 201109)
導(dǎo)彈對目標(biāo)的跟蹤時(shí),傳統(tǒng)主動(dòng)雷達(dá)制導(dǎo)方法易受敵方電子干擾,當(dāng)應(yīng)用紅外等被動(dòng)傳感器對目標(biāo)進(jìn)行跟蹤,可較好地解決抗干擾問題。隨著被動(dòng)測量技術(shù)在跟蹤和制導(dǎo)中的廣泛應(yīng)用,在僅有角度和距離的無源測量條件下對目標(biāo)的狀態(tài)進(jìn)行估計(jì)引起了廣泛關(guān)注。數(shù)學(xué)上,可在直角坐標(biāo)系中用一個(gè)線性動(dòng)態(tài)模型和一個(gè)非線性觀測模型描述該問題,也可在極坐標(biāo)系中用一個(gè)非線性動(dòng)態(tài)模型和一個(gè)線性觀測模型描述[1-4]。被動(dòng)傳感器獲取目標(biāo)的方位角和距離信息,當(dāng)跟蹤坐標(biāo)系為直角坐標(biāo)系時(shí),因測量方程為非線性,故無法直接采用線性濾波方法。一種最直接的方法是將非線性模型進(jìn)行近似線性化,對線性化的系統(tǒng)采用卡爾曼濾波框架,即擴(kuò)展卡爾曼濾波(EKF)[5]。但經(jīng)一階線性化近似后,EKF忽略了模型部分非線性特性,當(dāng)初始誤差較大時(shí),存在估計(jì)效果急劇下降和濾波收斂速度緩慢的問題。為進(jìn)一步改善非線性系統(tǒng)估計(jì)性能,文獻(xiàn)[6]根據(jù)對隨機(jī)變量的概率分布進(jìn)行逼近較對非線性函數(shù)進(jìn)行逼近更容易的方法,提出了無跡卡爾曼濾波(UKF)[6]。UKF通過經(jīng)無跡變換后的采樣點(diǎn)集逼近非線性函數(shù)概率分布,并繼承了卡爾曼濾波框架,其非線性估計(jì)性能優(yōu)于EKF。UKF無需計(jì)算非線性系統(tǒng)的雅可比矩陣,因其良好適應(yīng)性受到了廣泛關(guān)注。用高斯埃爾米特積分公式得到的高斯埃爾米特濾波器,具有較UKF更好的估計(jì)精度和數(shù)值穩(wěn)定性,但其計(jì)算量隨狀態(tài)維數(shù)增加而呈指數(shù)級增長,龐大的運(yùn)算量導(dǎo)致其只能適于低維系統(tǒng)。文獻(xiàn)[7]通過三階容積法則的數(shù)值積分方法,近似高斯加權(quán)積分推導(dǎo)出了容積濾波(CKF),認(rèn)為CKF是UKF在特殊參數(shù)選取情形下的一個(gè)特例,但具有更好的濾波精度和數(shù)值穩(wěn)定性。
為此,本文提出了一種二維目標(biāo)的相對狀態(tài)估計(jì)算法。
本文研究在二維平面內(nèi)對目標(biāo)的無源跟蹤,數(shù)學(xué)表達(dá)式為
假設(shè)觀測雷達(dá)位于坐標(biāo)原點(diǎn),則觀測值信息
式中:xk,θk分別為距離和角度測量值,vk∈N(0,Rk)為測量噪聲。該測量模型具有典型的非線性特性[8]。
具加性噪聲的非線性系統(tǒng)狀態(tài)方程和量測方程可表示為
非線性高斯濾波的核心是求解多變量非線性函數(shù)與高斯密度函數(shù)乘積的積分。文獻(xiàn)[7]通過三階容積積分法則,利用2n個(gè)容積點(diǎn)加權(quán)求和替代加權(quán)高斯積分。對函數(shù)f(x)的加權(quán)高斯積分
a)時(shí)間預(yù)測
計(jì)算容積點(diǎn)
容積點(diǎn)傳播
估計(jì)預(yù)測均值和協(xié)方差陣
式中:i=1,…,2n。
b)量測更新
計(jì)算容積點(diǎn)
容積點(diǎn)傳播
c)量測預(yù)測值、新息方差和協(xié)方差矩陣計(jì)算
d)計(jì)算量測更新
無源測量模型具強(qiáng)非線性特性,為獲得良好的濾波效果,本文用LM算法改進(jìn)CKF的量測更新過程。與加權(quán)最小二乘算法類似,定義時(shí)刻k的代價(jià)函數(shù)
式中:為狀態(tài)估值。設(shè)時(shí)刻k第j步的狀態(tài)迭代預(yù)測值已知,則未知狀態(tài)與當(dāng)前狀態(tài)和修正值Δxk,j的關(guān)系可表示為
定義第j步迭代估計(jì)時(shí)的殘差值
第j+1步迭代的殘差值
將式(22)代入式(17),代價(jià)函數(shù)變?yōu)?/p>
確定修正估計(jì)值Δxk,j的原則是其滿足代價(jià)函數(shù)Jk,j+1最小。最小化Jk,j+1的問題與求解加權(quán)最小二乘問題等價(jià),易求得
式中:Πk,j為方差陣,且Πk,j= ((Hk,j)T(Rk)-1+ηIn×n)-1。此處:η為加權(quán)系數(shù);In×n為n維單位陣。當(dāng)η趨于0時(shí),式(24)等價(jià)于高斯最小二乘微分修正算法(GLSDC);當(dāng)η趨于無窮時(shí),式(24)等價(jià)于最速下降法。
LM算法克服了高斯最小二乘微分修正算法在先驗(yàn)估值與真值相差較大時(shí)收斂速度較慢的不足,以及在估值接近最優(yōu)值時(shí)最速下降法收斂速度變慢的缺點(diǎn)。基于LM算法的改進(jìn)CKF算法在進(jìn)一步提高了CKF估計(jì)精度的同時(shí),具有較快的收斂速度。將狀態(tài)量測更新過程用LM算法替代為
式中:取為計(jì)算得到的預(yù)測狀態(tài)值。由于線性系統(tǒng)的互協(xié)方差陣的值為
代入式(25)
定義偽測量矩陣
式中:Pk,0=Pk;Pxz,k,j=Pxz,k[14]。則狀態(tài)量測更新
定義迭代卡爾曼增益
則式(29)可簡化為
協(xié)方差矩陣
當(dāng)代價(jià)函數(shù)Jk,j隨迭代次數(shù)的增加而變化較小時(shí),滿足迭代終止條件
按以下準(zhǔn)則選取LM算法中的η:η初值取一個(gè)較大的值(通常取 (Hk,j)T(Rk)-1Hk,j范數(shù)10倍以上)。將式(31)迭代計(jì)算的狀態(tài)估計(jì)值代入式(23)中,若Jk,j+1≥Jk,j,則舍去這次的狀態(tài)迭代值并將η變?yōu)?η;若Jk,j+1<Jk,j,則保留狀態(tài)估計(jì)值并將η變?yōu)棣牵?。
適當(dāng)選取式(33)迭代終止條件的系數(shù)ε,本算法能在達(dá)到良好估計(jì)精度時(shí)及時(shí)跳出迭代,一定程度保證了算法的實(shí)時(shí)性。
目標(biāo)進(jìn)行無源跟蹤仿真時(shí),設(shè)仿真步長Δt為1s;Rk=diag[100m210rad2];仿真時(shí)長T=100s;狀態(tài)初值及其估值分別為
x1= [0m 0m/s 0.4m -0.05m/s 0rad]T,相應(yīng)的協(xié)方差陣P0對角線元素分別為100m2,10m2/s2,100mrad2/s2。此處:x01=0.05m;x02=0.01m/s;x03=0.7m;x04=-0.055m/s;x05=0rad。定義橫向位置均方根誤差γpx、縱向位置均方根誤差γpy、位置均方根誤差γpos、速度均方根誤差γvel和角速度均方根誤差γtum-rate分別為
進(jìn)行了100次蒙特卡洛仿真,目標(biāo)的跟蹤軌跡與真實(shí)飛行軌跡如圖1所示。由圖可知:因測量信息較少導(dǎo)致EKF的精度較低,在時(shí)間大于50s后出現(xiàn)了嚴(yán)重的濾波發(fā)散;UKF,CKF的估計(jì)精度較高,估計(jì)曲線與真實(shí)軌跡幾乎重合。因EKF估計(jì)結(jié)果發(fā)散,UKF,CKF的均方根誤差分別如圖2~4所示。由圖可知:CKF,UKF的速度和角速度估計(jì)精度非常相似,CKF的位置估計(jì)精度略優(yōu)于UKF。
圖1 飛行軌跡Fig.1 RMSE trajectory
位置、速度與角速度的RTAMSE值見表1。由表可知:CKF的精度略高于UKF,而EKF的跟蹤結(jié)果發(fā)散,無法實(shí)現(xiàn)有效。
表1 RTAMSE表Tab.1 RTAMSE table
圖2 位置均方根誤差Fig.2 RMSE in position
圖3 速度均方根誤差Fig.3 RMSE in velocity
圖4 角速度均方根誤差Fig.4 RMSE in turn rate
本文對僅有角度和距離測量條件下的一種無源目標(biāo)跟蹤算法進(jìn)行了研究。采用容積濾波作為跟蹤算法,提出基于龍貝格-馬爾塔(LM)算法改進(jìn)的迭代容積無源跟蹤算法以提高跟蹤精度。仿真結(jié)果驗(yàn)證了迭代容積濾波算法有良好的跟蹤精度。
[1] BALAKRISHNAN S N.Extention to modified polar coordinates and application with passive measurements[J].Journal of Guidance,Control,and Dynamics,1989,12(6):906-912.
[2] SONG,T L SPEYER J.A stochastic analysis of a modified gain extended Kalman filter with applications to estimation with bearings only measurements[J].IEEE Transactions on Automatic Control,1985,30(10):940-949.
[3] SONG T L.Observability of target tracking with bearing-only measurements[J].IEEE Trans on Aerosp Electron Syst,1996,32(4):1468-1471.
[4] CADRE J P,JAUFFRET C.Discrete-time observability and estimability analysis for bearings-only target motion analysis[J].IEEE Trans on Aerosp Elec-tron Syst,1997,33(1):178-201.
[5] 孫仲康,郭福成,馮道旺,等.單站無源定位跟蹤技術(shù)[M].北京:國防工業(yè)出版社,2008:186-199.
[6] CRASSIDIS J,JUNKINS J.Optimal estimation of dynamic systems[M].Boca Raton:Chapman &Hall,2004:419-433.
[7] ARASARATNAM I,HAYKIN S.Cubature kalman filters[J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.
[8] 周 荻,胡振坤,胡恒章.自適應(yīng)推廣Kalman濾波應(yīng)用于導(dǎo)彈的被動(dòng)制導(dǎo)問題[J].宇航學(xué)報(bào),1997,18(4):31-36.