一種面向彈道再入目標(biāo)跟蹤的HPD-SRCQSPF算法

楊 峰，鄭麗濤

(1. 西北工業(yè)大學(xué)自動化學(xué)院，西安 710129；2. 信息融合技術(shù)教育部重點實驗室，西安 710129)

0 引 言

非線性濾波廣泛應(yīng)用在生活中的各個領(lǐng)域，常見的非線性濾波器有EKF、UKF[1]和最近幾年提出的CQKF[2]。其中，CQKF[2]算法是CKF[3]算法的一般化形式。這三種濾波算法在工程中都得到了廣泛應(yīng)用，然而針對不同的非線性系統(tǒng)，三種濾波算法有著各自的優(yōu)勢和缺陷。通常，EKF算法的濾波精度為一階；UKF算法的濾波精度為二階；CQKF算法的濾波精度為三階[2,4]。另外，當(dāng)系統(tǒng)階次不同時，這三種濾波算法的濾波精度也存在差異。當(dāng)系統(tǒng)階次大于3時，CQKF算法的濾波估計效果是最好的[2,4]。因此，對于彈道再入目標(biāo)軌跡跟蹤模型，CQKF顯然有著明顯的優(yōu)勢。但CQKF算法在迭代中，由于協(xié)方差矩陣的舍入誤差累積，會導(dǎo)致協(xié)方差矩陣漸漸失去正定性，甚至?xí)斐蔀V波發(fā)散。文獻(xiàn)[5]提出了平方根容積求積卡爾曼濾波(SRCQKF)算法，其在時間和量測的更新過程中不需要直接計算協(xié)方差矩陣，而是通過協(xié)方差矩陣的平方根形式來代替，這避免了協(xié)方差矩陣非正定時導(dǎo)致濾波發(fā)散的問題，從而對彈道再入目標(biāo)軌跡跟蹤模型有著更好的估計效果。

在實際生活中，彈道目標(biāo)跟蹤問題是一個強非線性和非高斯問題[6]，其在巡航飛行[7]、月球探測[8]和火星探測[9]等多個領(lǐng)域有著廣泛的應(yīng)用。因此，本文中，過程噪聲為非高斯環(huán)境下的閃爍噪聲。文獻(xiàn)[10]提出，在閃爍噪聲條件下，SPF的估計效果要優(yōu)于EKF、UKF等次優(yōu)卡爾曼濾波。因此，SPF對于閃爍噪聲條件下的彈道再入目標(biāo)軌跡跟蹤模型有著很好的跟蹤效果。SPF[11]算法是一種基于貝葉斯估計理論的序貫蒙特卡洛模擬方法，然而在粒子濾波中不可避免地存在著粒子退化[12]問題。針對該問題，可以選擇濾波估計后的值作為粒子濾波算法采樣的重要性密度函數(shù)。文獻(xiàn)[13]選擇SRCQKF估計后的值作為粒子濾波算法中的重要性密度函數(shù),即平方根容積求積粒子濾波(SRCQPF)算法。

SRCQPF算法雖然解決了粒子濾波的退化問題，但也同樣帶來了運算量大，運算時間過長等問題[13-14]?；谖墨I(xiàn)[15]提出的混合建議分布的框架，本文提出了平方根容積求積采樣粒子濾波(HPD-SRCQSPF)算法，該算法是將平方根容積求積卡爾曼濾波估計后的值作為多模型采樣粒子濾波的一個基本建議分布。由于HPD-SRCQSPF算法是采用了多個建議分布，該算法的運算時間會略長于SPF算法，但其濾波精度卻遠(yuǎn)遠(yuǎn)高于SPF算法和SRCQPF算法；特別是在彈道再入目標(biāo)軌跡跟蹤問題中，本文算法可以取得很好的跟蹤效果。

1 標(biāo)準(zhǔn)粒子濾波(SPF)

粒子濾波的基本思想是依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗分布[14]，在狀態(tài)空間中產(chǎn)生一組被稱為粒子的隨機離散樣本集合。然后根據(jù)最新的量測用似然函數(shù)來調(diào)整粒子所占的權(quán)值，從而對最初的經(jīng)驗分布進(jìn)行修正。

考慮如下動態(tài)系統(tǒng)，即狀態(tài)空間模型。其中狀態(tài)方程和量測方程分別為：

xk=f(xk-1,wk-1)

(1)

zk=h(xk,vk)

(2)

式中：xk∈Rnx為k時刻的系統(tǒng)狀態(tài)向量，zk∈Rny為k時刻的量測向量，wk-1∈Rnw和vk∈Rnv分別表示過程噪聲和量測噪聲。f:Rnx×Rnw→Rnx為狀態(tài)轉(zhuǎn)移函數(shù)，h:Rnz×Rnv→Rnz為量測函數(shù)。

假設(shè)已知狀態(tài)的初始概率密度函數(shù)為p(x0|y0)，且x0,x1,…,xk服從于一階馬爾科夫過程，狀態(tài)序列為Xk={x0,x1,…,xk}，觀測序列為Zk={z0,z1,…,zk}。在貝葉斯框架下，濾波分布p(xk|zk)的遞推通過預(yù)測與更新兩步完成。

預(yù)測：

(3)

式中：p(zk|xk-1)為狀態(tài)方程的一步預(yù)測轉(zhuǎn)移密度，而p(xk-1|zk-1)為前一時刻的后驗密度。

更新：

(4)

(5)

(6)

針對粒子退化的情況，可以根據(jù)退化程度加入重采樣步驟。

2 平方根容積求積粒子濾波(SRCQPF)

2.1 平方根容積求積卡爾曼濾波(SRCQKF)

傳統(tǒng)的CKF算法是將多維高斯積分轉(zhuǎn)換成徑向積分和球面積分的形式，CQKF算法在求取徑向積分時是通過高斯—拉蓋爾積分準(zhǔn)則去逼近積分運算。因此，CQKF是CKF濾波算法的一般化形式。

在實際應(yīng)用中，CQKF算法在遞推過程中協(xié)方差矩陣P由于舍入誤差累積的原因會導(dǎo)致其逐漸的失去正定性，最終導(dǎo)致濾波算法發(fā)散。平方根容積求積卡爾曼濾波(SRCQKF)在時間和量測的更新過程中通過求解協(xié)方差矩陣的平方根形式來代替協(xié)方差矩陣，這避免了協(xié)方差矩陣非負(fù)定時導(dǎo)致濾波發(fā)散的問題。因此，相比于CQKF算法，SRCQKF算法有著更好的工程實際應(yīng)用效果。

2.2 SRCQPF算法

標(biāo)準(zhǔn)粒子濾波算法雖然在處理非高斯問題中有著明顯的優(yōu)勢，但該算法不可避免的一個問題就是粒子退化問題，而選擇適合的重要密度函數(shù)可以有效改善該問題。SRCQPF算法[13]就是把SRCQKF算法和SPF算法結(jié)合起來，將SRCQKF濾波后的值作為SPF算法的重要性密度函數(shù)，這可以提高粒子的濾波精度，使得采樣后的粒子向似然比高的區(qū)域移動，從而減輕粒子退化問題。

SRCQPF算法的具體步驟如下：

2)用SRCQKF對每一粒子進(jìn)行狀態(tài)更新。

3)計算粒子點對應(yīng)的權(quán)值

4)Neff

5)將k時刻的粒子點作為k+1時刻的初始粒子點。

3 基于混合建議分布的平方根容積求積采樣粒子濾波(HPD-SRCQSPF)

3.1 混合建議分布(HPD)

文獻(xiàn)[15]提出了混合建議分布的概念，其核心是對建議分布進(jìn)行創(chuàng)新，即用幾種基本建議分布的加權(quán)來代替原來單一的建議分布。

(7)

關(guān)于基本建議分布，有下式：

(8)

所以混合建議分布可以表示為：

(9)

(10)

式(10)中，等式右面第一項反映了量測信息的重要性，其中p(yt|zt)=pw(zt-yt)=p(zt|yt)。等式右面第二項反映的是預(yù)測信息的重要性。

為了計算的可行性，可以將式(10)轉(zhuǎn)化為式(11)所示的矩陣形式。

(11)

式中：

(12)

(13)

(14)

(15)

其中，

(16)

(17)

(18)

則

確定了Δjl和Δj的值后，求解式(10)的最小值問題就顯得很容易了。

3.2 HPD-SRCQSPF算法

在本文中，令m=2，即混合建議分布由兩個基本的建議分布組成。

那么，混合建議分布可表示為

式(10)的最小值問題可以簡化為

(19)

為了直觀地表示HPD-SRCQSPF算法，現(xiàn)將混合建議分布的示意圖表示如圖1。

圖1 混合建議分布示意圖Fig.1 The picture of hybrid proposal distribution

可以看出，在圖1的情況下，只利用先驗分布得到的粒子點會遠(yuǎn)離真實值。而通過混合建議分布后，粒子點可以分布在真實值附近，從而使濾波估計值更加準(zhǔn)確。

HPD-SRCQSPF算法的具體步驟如下：

3)計算出Δ11,Δ12,Δ22,Δ1,Δ2。

6)計算粒子點對應(yīng)的權(quán)值

7)Neff

8)將k時刻的粒子點作為k+1時刻的初始粒子點。

9)重復(fù)步驟2)～8)

最后得到k時刻狀態(tài)量的估計為

4 仿真校驗

4.1 仿真場景

文獻(xiàn)[6]和文獻(xiàn)[10]已經(jīng)對彈道再入目標(biāo)的數(shù)學(xué)模型進(jìn)行了較為系統(tǒng)的分析。根據(jù)文獻(xiàn)[16]，假定地球表面為平面，目標(biāo)為質(zhì)點且不考慮自旋。

則彈道再入目標(biāo)數(shù)學(xué)模型的運動方程和觀測方程可表示為式(20)和式(21)。

xk+1=f(xk,β)+wk

(20)

zk+1=h(xk+1)+vk+1

(21)

p(vk+1)= 0.5p1(vk+1)+0.5p2(vk+1)=

0.5N(vk+1;0,R1)+0.5N(vk+1;0,R2)

其中，

(22)

(23)

狀態(tài)方程中的f(xk,β)可表示為式(24)

(24)

式(24)中，等式右面的后兩項表示空氣阻力和重力對目標(biāo)狀態(tài)變量的影響作用。g是重力加速度，m(xk,β)是空氣動力阻力函數(shù)，β是彈道系數(shù)，為了簡化模型，根據(jù)文獻(xiàn)[10]，可以將彈道系數(shù)定為確定參數(shù),即β=40000 kg/ms2。

(25)

空氣動力阻力函數(shù)m(xk,β)定義為：

(26)

ρ(·)是隨著海拔高度呈指數(shù)衰落的空氣密度函數(shù)，可以定義ρ(yk)=c1e-c2yp,k-1，當(dāng)yp,k-1<9144時，c1=1.227,c2=1.093×10-4；當(dāng)yp,k-1>9144時，c1=1.754,c2=1.49×10-4。因此，在yp,k-1=9144時，目標(biāo)會有明顯的機動運動。

量測方程h(·)可表示為

(27)

仿真中，仿真時間為200 s，蒙特卡洛次數(shù)為500次，粒子數(shù)為600。目標(biāo)的初始狀態(tài)為

其余的仿真參數(shù)設(shè)置如表1所示。

表1 仿真參數(shù)設(shè)置Table 1 The simulation parameters

4.2 仿真分析

分別用SPF[11]算法、SRCQPF[13]算法和本文提出的HPD-SRCQSPF算法對上述的彈道再入目標(biāo)數(shù)學(xué)模型進(jìn)行仿真，仿真結(jié)果如圖2所示。

圖2 單次跟蹤效果Fig.2 Single tracking effect

從圖2可以看出，在單次跟蹤中，比起SPF和SRCQPF算法，HPD-SRCQSPF算法明顯更接近于真實值，且在圖2中小圖標(biāo)記出的位置尤為明顯。這是因為本文算法的建議分布采用了多個基本建議分布加權(quán)的形式，所以可以更好地逼近后驗分布，因此有著很好的估計效果。

圖3 x軸方向位置的RMSEFig.3 RMSE in the position of x direction

圖4 x軸方向速度的RMSEFig.4 RMSE in the velocity of x direction

圖5 y軸方向位置的RMSEFig.5 RMSE in the position of y direction

圖6 y軸方向速度的RMSEFig.6 RMSE in the velocity of y direction

從圖3～6可以看出，45 s左右各個算法對目標(biāo)的跟蹤精度都有較大的波動，這是因為空氣密度函數(shù)ρ(·)的值在此刻發(fā)生了變化，從而導(dǎo)致目標(biāo)在此時發(fā)生了強機動性運動。

前45 s，HPD-SRCQSPF算法對目標(biāo)的跟蹤效果略好于SPF算法，且它們的跟蹤效果都好于SRCQPF算法。這是因為此時目標(biāo)的運動較平穩(wěn)，本文算法的建議分布中先驗分布占了很大的權(quán)重，所以本文算法和SPF算法都有著較好的估計效果。

50 s之后，HPD-SRCQSPF算法明顯好于SPF和SRCQPF算法，且收斂速度和最終的濾波精度都略優(yōu)于另外兩種算法。這是因為本文算法用了混合建議分布的框架，可以動態(tài)地選擇每個基本建議分布所占的權(quán)重，所以其比起另外兩種算法會有著較好的估計效果。

SPF，SRCQPF和HPD-SRCQSPF算法對該數(shù)學(xué)模型進(jìn)行仿真的各項算法性能如表2所示。

表2 各個算法的性能Table 2 The performance of each algorithm

從表2可以看出，HPD-SRCQSPF算法的運算時間雖然高于SPF算法，但卻遠(yuǎn)遠(yuǎn)低于SRCQPF算法。這是因為比起SPF算法，本文算法需要計算SRCQKF的估計值，因此其時間會略長于SPF算法。本文算法在x軸位置、速度以及y軸位置、速度方面的RMSE都遠(yuǎn)遠(yuǎn)低于另外兩種算法，這證明了基于先驗分布的基本建議分布和基于SRCQKF估計后值的基本建議分布的加權(quán)和形式的混合建議分布對彈道再入目標(biāo)數(shù)學(xué)模型的跟蹤是很有效的。

4.3 HPD-SRCQSPF算法的仿真分析

在HPD-SRCQSPF算法中，選擇SRCQKF估計后的值作為粒子濾波算法中的重要性密度函數(shù)。當(dāng)SRCQKF算法的切比雪夫多項式是采用一階近似計算時，HPD-SRCQKF算法即為HPD-SRCSPF算法，在本文中，將其記為HPD-SRCQSPF 1。當(dāng)SRCQKF算法的切比雪夫多項式是采用二階近似時，將其記為HPD-SRCQSPF 2。當(dāng)SRCQKF算法的切比雪夫多項式是采用三階近似時，將其記為HPD-SRCQSPF 3。

為了分析本文算法不同階數(shù)以及不同粒子數(shù)對最終估計精度的影響，用HPD-SRCQSPF 1,HPD-SRCQSPF 2和HPD-SRCQSPF 3三種算法分別在粒子數(shù)為100，200，300，400，500，600，700，800，900和1000的情況下對上述的彈道再入目標(biāo)數(shù)學(xué)模型進(jìn)行仿真，仿真結(jié)果如下。

表3 不同階數(shù)和不同粒子數(shù)的仿真時間Table 3 The simulation time of different orders and different particles

從表3可以看出，在單次的仿真試驗中，隨著粒子數(shù)的增多，仿真時間逐漸增多，每多100個粒子點大約會增加1 s。隨著切比雪夫階數(shù)的增加，會導(dǎo)致采樣點的數(shù)量增加，從而導(dǎo)致仿真時間也是不斷增加的。但由于不同階數(shù)HPD-SRCQSPF算法的采樣點及其權(quán)重是可以脫機運算的，因此切比雪夫階數(shù)每增加一階只會增加0.02 s的運算時間。

從圖7～10可以看出，隨著粒子點數(shù)量的增加，在各個方向上的估計精度是不斷提高的。但可以看出，在粒子數(shù)目大于600個粒子點后，估計精度的增加小到幾乎可以忽略不計，因此最優(yōu)的粒子點數(shù)目是600個。在HPD-SRCQSPF算法中，隨著切比雪夫階數(shù)的提高，估計精度也是逐漸提升的。這是因為高的切比雪夫階數(shù)可以更好地逼近后驗分布，因此會使得濾波的估計效果更好。

圖7 不同階數(shù)和不同粒子數(shù)在x軸方向位置的RMSEFig.7 The RMSE in the x position of different orders and different particles

圖8 不同階數(shù)和不同粒子數(shù)在x軸方向速度的RMSEFig.8 The RMSE in the x velocity of different orders and different particles

圖9 不同階數(shù)和不同粒子數(shù)在y軸方向位置的RMSEFig.9 The RMSE in the y position of different orders and different particles

圖10 不同階數(shù)和不同粒子數(shù)在y軸方向速度的RMSEFig.10 The RMSE in the y velocity of different orders and different particles

綜上可知，在彈道再入目標(biāo)軌跡跟蹤的數(shù)學(xué)模型中，綜合切比雪夫階數(shù)和粒子數(shù)，最優(yōu)的選取策略應(yīng)該是HPD-SRCQSPF 3算法在選取600個粒子點的情況下。

5 結(jié) 論

在彈道再入目標(biāo)軌跡跟蹤模型中，相比于SPF算法和SRCQPF算法，本文所提的HPD-SRCQSPF算法不僅有著較低的運算量，而且有著很好的跟蹤效果。特別是對于該模型中機動的情況，本文的算法可以在較短的時間內(nèi)獲得很高的估計精度。另外，在該模型中，綜合切比雪夫階數(shù)和粒子數(shù)，在HPD-SRCQSPF算法中，最優(yōu)的選取策略應(yīng)該是三階切比雪夫HPD-SRCQSPF 3算法在選取600個粒子點的情況下。

參 考 文 獻(xiàn)

[1] Julier S, Uhlmann J, Durrant-Whyte H F. A new method for the nonlinear transformation of means and covariances in filters and estimators [J]. IEEE Transactions on Automatic Control, 2001, 45(3): 477-482.

[2] Bhaumik S. Cubature quadrature Kalman filter [J]. IET Signal Processing, 2013, 7(7): 533-541.

[3] 孫楓，唐李軍.Cubature 卡爾曼濾波-卡爾曼濾波算法[J].控制與決策，2012, 27(10): 1561-1565. [Sun Feng, Tang Li-jun. Cubature Kalman filter-Kalman filter algorithm [J]. Control and Decision, 2012, 27(10): 1561-1565.]

[4] 王小旭, 潘泉, 黃鶴,等. 非線性系統(tǒng)確定采樣型濾波算法綜述 [J]. 控制與決策, 2012, 27(6): 801-812. [Wang Xiao-xu, Pan Quan, Huang He, et al. Overview of deterministic sampling filtering algorithms for nonlinear system [J]. Control and Decision, 2012, 27(6): 801-812.]

[5] Bhaumik S. Square-root cubature-quadrature Kalman filter [J]. Asian Journal of Control, 2014, 16(2): 617-622.

[6] 劉也, 余安喜, 朱炬波, 等. 彈道目標(biāo)實時跟蹤中的濾波方法綜述[J]. 宇航學(xué)報, 2013, 34(11): 1417-1426. [Liu Ye, Yu An-xi, Zhu ju-bo, et al. Survey of filter algorithms for ballistic target real-time tracking [J]. Journal of Astronautics, 2013, 34(11): 1417-1426.]

[7] 郭行, 符文星, 付斌, 等. 吸氣式高超聲速飛行器巡航段突防彈道規(guī)劃[J]. 宇航學(xué)報, 2017, 38(3): 287-295. [Guo Hang, Fu Wen-xing, Fu Bin, et al. Penetration trajectory programming for air-breathing hypersonic vehicles during the cruise phase [J]. Journal of Astronautics, 2017, 38(3): 287-295.]

[8] 宋葉志, 黃勇, 胡小工, 等. 月球探測軟著陸與采樣返回段彈道確定[J]. 宇航學(xué)報, 2016, 37(10): 1157-1163. [Song Ye-zhi, Huang Yong, Hu Xiao-gong, et al. Trajectory determination for lunar probe soft landing and sampling return [J]. Journal of Astronautics, 2016, 37(10): 1157-1163.]

[9] 高興龍, 張青斌, 豐志偉，等. 集成火星進(jìn)入彈道的開傘過程動力學(xué)特性研究 [J]. 宇航學(xué)報, 2016, 37(6): 664-670. [Gao Xing-long, Zhang Qing-bin, Feng Zhi-wei, et al. Study on dynamic characteristic of opening process integrating with Mars entry trajectory [J]. Journal of Astronautics, 2016, 37(6): 664-670.]

[10] 田亞菲, 莫驊, 于飛. 彈道再入目標(biāo)軌跡跟蹤中非線性濾波算法研究 [J]. 艦船電子工程, 2014, 34(3): 39-43. [Tian Ya-fei, Mo hua, Yu fei. Nonlinear filtering algorithm on ballistic reentry target trajectory tracking [J]. Ship Electronic Engineering, 2014, 34(3): 39-43.]

[11] Cappé O, Godsill S J, Moulines E. An overview of existing methods and recent advances in sequential Monte Carlo [J]. Proceedings of the IEEE, 2007, 95(5): 899-924.

[12] Christoffersson J. A resampling method for regression models with serially correlated errors [J]. Computational Statistics & Data Analysis, 1997, 25(1): 43-53.

[13] Kiani M, Pourtakdoust S H. Adaptive square-root cubature-quadrature Kalman particle filter for satellite attitude determination using vector observations [J]. Acta Astronautica, 2014, 105(1): 109-116.

[14] 胡士強，敬忠良.粒子濾波算法綜述[J].控制與決策，2005, 20(4): 361-371. [Hu Shi-qiang, Jing Zhong-liang. Overview of particle filter algorithm [J]. Control and Decision, 2005, 20(4): 361-371.]

[15] Zuo J, Liang Y, Zhang Y Z, et al. Particle filter with multimode sampling strategy [J]. Signal Processing, 2013, 93(11): 3192-3201.

[16] Li X R, Jilkov V P. Survey of maneuvering target tracking. Part II: motion models of ballistic and space targets [J]. IEEE Transactions on Aerospace & Electronic Systems, 2010, 46(1): 96-119.