再入彈道目標(biāo)跟蹤的球面單純形-徑向容積卡爾曼濾波算法

李春月，廖育榮，倪淑燕，陳 帥

0 引 言

彈道導(dǎo)彈具有射程遠(yuǎn)、速度快、精度高、突防能力強(qiáng)、殺傷威力大、效費(fèi)比高等優(yōu)點(diǎn)，已成為現(xiàn)代戰(zhàn)爭(zhēng)非常重要的進(jìn)攻性武器。為了遏制日益加劇的彈道導(dǎo)彈威脅，世界各國(guó)都在大力發(fā)展導(dǎo)彈防御系統(tǒng)。雷達(dá)是導(dǎo)彈防御系統(tǒng)中的核心探測(cè)器，對(duì)彈道導(dǎo)彈的實(shí)時(shí)跟蹤精度直接影響到導(dǎo)彈防御系統(tǒng)對(duì)導(dǎo)彈攔截的成功率。因此，高精度彈道導(dǎo)彈實(shí)時(shí)跟蹤算法是目前研究的熱點(diǎn)問(wèn)題。

彈道目標(biāo)實(shí)時(shí)跟蹤問(wèn)題本質(zhì)上為高維非線性系統(tǒng)的最優(yōu)狀態(tài)估計(jì)問(wèn)題?？柭鼮V波是目前最優(yōu)狀態(tài)估計(jì)中應(yīng)用最為廣泛的一種算法，經(jīng)典的非線性卡爾曼濾波算法主要有擴(kuò)展卡爾曼濾波（Extended KalmanFilter，EKF）算法[1]和無(wú)跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法[2,3]。其中，EKF算法的核心思想是將非線性的系統(tǒng)狀態(tài)函數(shù)做線性化處理，具體方法是利用泰勒級(jí)數(shù)展開(kāi)法對(duì)非線性狀態(tài)方程進(jìn)行一階展開(kāi)，然后再用離散卡爾曼濾波進(jìn)行最優(yōu)狀態(tài)估計(jì)，這種算法對(duì)非線性較強(qiáng)的系統(tǒng)估計(jì)精度不夠高；UKF算法的出發(fā)點(diǎn)是基于“對(duì)概率分布進(jìn)行近似要比對(duì)非線性函數(shù)近似容易很多”的思想，采用Sigma點(diǎn)的分布近似表示非線性函數(shù)的分布，有效提高了估計(jì)精度，但是對(duì)于高維非線性系統(tǒng)（維數(shù)n≥4），UKF算法中的自由調(diào)節(jié)參數(shù)κ＜ 0，使得中心采樣點(diǎn)的權(quán)值κ＜ 0，從而使 UKF 算法在濾波過(guò)程中可能會(huì)出現(xiàn)協(xié)方差非正定情況，導(dǎo)致最終收斂結(jié)果不穩(wěn)定甚至發(fā)散，并且隨著系統(tǒng)維數(shù)的增加，采樣點(diǎn)與中心點(diǎn)距離不斷增大，導(dǎo)致非局部效應(yīng)，嚴(yán)重影響 UKF 算法的估計(jì)精度。2009年，Arasaratnam等人[4,5]采用Spherical -Radial規(guī)則近似非線性函數(shù)傳遞的后驗(yàn)均值和協(xié)方差的方法，依據(jù)高斯濾波框架提出容積卡爾曼濾波（Cubature Kalman Filter，CKF）算法，相比于UKF算法，CKF算法有嚴(yán)格的數(shù)學(xué)推導(dǎo)過(guò)程，降低了運(yùn)算量，提升了估計(jì)精度，得到廣泛應(yīng)用[6～9]。文獻(xiàn)[10]對(duì)UKF算法和CKF算法的估計(jì)精度和數(shù)值穩(wěn)定性做了比較，指出當(dāng)系統(tǒng)狀態(tài)維數(shù)小于3維時(shí)，UKF算法估計(jì)精度高于CKF算法，當(dāng)系統(tǒng)狀態(tài)維數(shù)等于3維時(shí)，二者估計(jì)精度相當(dāng)，當(dāng)系統(tǒng)狀態(tài)維數(shù)大于3維時(shí)，無(wú)論估計(jì)精度和數(shù)值穩(wěn)定性，CKF算法都高于 UKF算法；受到Spherical -Radial規(guī)則的啟發(fā)，文獻(xiàn)[11]提出了更多容積點(diǎn)的CKF算法，以提高估計(jì)精度，但是也相應(yīng)增加了計(jì)算量；文獻(xiàn)[12]為了進(jìn)一步提高 CKF 算法的估計(jì)精度，提出迭代 CKF 算法，并將其應(yīng)用于再入彈道目標(biāo)狀態(tài)估計(jì)，其效果優(yōu)于 CKF 算法。

針對(duì)如何進(jìn)一步提高對(duì)再入彈道目標(biāo)的實(shí)時(shí)跟蹤精度，本文提出將另一種基于Spherical Simplex-Radial準(zhǔn)則的SSRCKF算法用于再入彈道目標(biāo)實(shí)時(shí)跟蹤中。該算法將Spherical Simplex準(zhǔn)則引入CKF算法，以一種新的容積點(diǎn)選擇方法來(lái)計(jì)算球面積分，進(jìn)而近似計(jì)算高斯域下的貝葉斯積分，有效改善了CKF算法的估計(jì)精度，將其用于再入彈道目標(biāo)跟蹤中，提高彈道目標(biāo)的實(shí)時(shí)跟蹤精度，最后通過(guò)仿真分析，驗(yàn)證了算法的有效性。

1 彈道目標(biāo)跟蹤數(shù)學(xué)模型

1.1 再入段動(dòng)力學(xué)模型

對(duì)再入彈道目標(biāo)進(jìn)行動(dòng)力學(xué)建模，首先，對(duì)再入彈道目標(biāo)進(jìn)行受力分析。設(shè)彈道目標(biāo)一般保持零度攻角再入，受到的空氣動(dòng)力表現(xiàn)為大氣阻力，大氣阻力加速度方向與再入速度方向相反。定義以彈道測(cè)量雷達(dá)為原點(diǎn)的北天東（North-Up-East）坐標(biāo)系，O-xyz，Ox指向正北方向，Oy垂直于地球表面指向正上方，Oz指向正東方向，由于再入彈道導(dǎo)彈速度快，再入時(shí)間短，忽略地球自轉(zhuǎn)角速度的影響。對(duì)再入目標(biāo)建模如圖 1所示，假設(shè)地球?yàn)闃?biāo)準(zhǔn)球體，EO為地球質(zhì)心，ag和af分別為引力加速度和大氣阻力加速度，彈道目標(biāo)的位置、速度分量和彈道系數(shù)構(gòu)成七維狀態(tài)向量，給出再入彈道目標(biāo)的系統(tǒng)狀態(tài)方程。

圖1 雷達(dá)觀測(cè)再入彈道目標(biāo)示意Fig.1 Radar Observation Reentry Ballistic Target Signal

式 中 [ω1( t ) … ω7(t )]Τ為 系 統(tǒng) 狀 態(tài) 噪 聲 ， 滿(mǎn) 足ω～0(,)Q的統(tǒng)計(jì)特性 ；μ為地球重力常數(shù)，μ= G M = 3 .986× 1 014m3/s2；為 地 球 半 徑 ，Re= 6 378137m ； R(t)為目標(biāo)到地心距離，()Vt為目標(biāo)再入速度，ρ(t)為大氣密度模型，ρ(t ) = ρ0exp ( ?(R (t) ? Re)H0)，H0為大氣密度標(biāo)高，H0=6700km，ρ0為海平面的大氣密度，ρ0= 1 .225kg/m3；β( t)為彈道系數(shù)模型，有[13]：

式中DC為氣動(dòng)阻力系數(shù)；m為彈道目標(biāo)質(zhì)量；A為彈道目標(biāo)有效截面積。為了保證彈道系數(shù)的非負(fù)性，防止濾波器發(fā)散采用式（2）的指數(shù)模型；()tγ的變化率可以用零均值高斯白噪聲表示，即：

由此可以得到再入彈道目標(biāo)的7維狀態(tài)方程，狀態(tài)量，為了保證濾波器的精度，采用四階龍格-庫(kù)塔方法對(duì)狀態(tài)方程進(jìn)行離散，最終得到離散非線性系統(tǒng)狀態(tài)方程：

1.2 量測(cè)模型

以觀測(cè)雷達(dá)為坐標(biāo)系原點(diǎn)，在北天東坐標(biāo)系下建立再入彈道目標(biāo)量測(cè)模型。如圖1所示，雷達(dá)對(duì)彈道目標(biāo)的距離為 r，俯仰角為η，方位角為ε，有z = [r,ε,η]Τ。由觀測(cè)值相對(duì)彈道目標(biāo)位置的幾何關(guān)系建立非線性量測(cè)方程：

式中 vk為雷達(dá)測(cè)量噪聲向量，，滿(mǎn)足v～0(,)R的統(tǒng)計(jì)特性，且kv，kω和kx互不相關(guān)。由此得到系統(tǒng)的量測(cè)方程：

2 球面單徑容積卡爾曼濾波跟蹤算法

2.1 Spherical Simplex-Radial準(zhǔn)則

高斯域下的貝葉斯濾波可以一般化為下面的積分公式：

的求解一般可以采用一系列函數(shù)通過(guò)點(diǎn)集加權(quán)求和的近似方法，即：

式中 N為總點(diǎn)數(shù)； xi， wj分別為正交點(diǎn)集和權(quán)重。對(duì)式（7）采用Spherical Radial變換： x =ry且 yΤy = 1；xΤx =r2，則式（7）可重寫(xiě)為

式中 σ（·）為球面區(qū)域= { y ∈ RnyΤy = 1}的面積微元。通過(guò)式（9）可以看出，高斯域下的貝葉斯濾波公式通過(guò)Spherical-Radial準(zhǔn)則變換，被分解為一個(gè)球面積分和一個(gè)徑向積分。這兩個(gè)積分同樣無(wú)法直接求得，此時(shí)再利用式（8），球面積分用包含sN個(gè)點(diǎn)的球面積分準(zhǔn)則計(jì)算[4]：

徑向積分可由包含rN個(gè)點(diǎn)的高斯求積準(zhǔn)則求得：

下面以這兩個(gè)積分為基礎(chǔ)介紹 Spherical Simplex準(zhǔn)則和Radial準(zhǔn)則。

2.1.1 Spherical Simplex準(zhǔn)則

首先計(jì)算球面積分，球面積分 ()Sr的一種高效率的計(jì)算方法是：選取一系列包含 n個(gè)正則單行頂點(diǎn)的點(diǎn)集 aj=[aj,1, aj,2, … ,aj,n]Τ, j = 1 ,2,… ,n +1作為容積點(diǎn)[14,15]（n為狀態(tài)維數(shù)），采用中心對(duì)稱(chēng)的容積準(zhǔn)則來(lái)近似球面面積。其中容積點(diǎn)的每個(gè)元素,jia 選取規(guī)則為

利用ja構(gòu)造Spherical Simplex準(zhǔn)則形式如下：

式中為單位球的表面積，，且

計(jì)算徑向積分S(r)rn?1e?r2dr ，與式（13）相對(duì)應(yīng)，通過(guò)與式（11）進(jìn)行匹配可以得到 Nr=1的Radial準(zhǔn)則[12]：

2.1.2 Radial準(zhǔn)則

由 Γ ( n + 1 ) = n Γ ( n)對(duì)第 2個(gè)等式進(jìn)行化簡(jiǎn)，解出，權(quán)重 wr,1=Γ( n/2)/2。

非線性函數(shù)與高斯概率密度的乘積的積分是高斯域 下 貝 葉 斯 濾 波 器 的 核 心 ， 當(dāng)(x ) = e?xΤx，w2(x ) = N (x ; 0, I)時(shí)，通過(guò)式（7）可得：

由式（13）、式（14）和式（20）可以求得：

把式（13）、式（16）代入式（18），即可得到 Spherical Simplex-Radial準(zhǔn)則，用 ()Qf表示：

權(quán)重為 N (x;μ,P)時(shí)，μ，P分別為x的均值與協(xié)方差，對(duì)式（19）進(jìn)行線性變換，得到一般形式Spherical Simplex-Radial 準(zhǔn)則：

2.2 SSRCKF算法

基于Spherical Simplex-Radial 準(zhǔn)則的容積卡爾曼濾波采用一系列等權(quán)值的容積點(diǎn)對(duì)高斯域下的貝葉斯濾波進(jìn)行非線性逼近。其中容積點(diǎn)的選取規(guī)則為

式中 2m n= ，（n為狀態(tài)空間維數(shù)）；為權(quán)值；,j = 1,2,… ,n +1，由式（12）得到；的第i列。

算法具體實(shí)現(xiàn)步驟如下：

a）步驟1：濾波器初始化。

SSRCKF濾波器初始化如下：

循環(huán)，完成以下步驟。

b）步驟2：時(shí)間更新。

首先對(duì)進(jìn)行 Cholesky分解，有公式，取下三角陣，計(jì)算容積點(diǎn)：

狀態(tài)方程傳遞容積點(diǎn)：

估計(jì)k時(shí)刻狀態(tài)預(yù)測(cè)值：

估計(jì)k時(shí)刻的先驗(yàn)估計(jì)誤差協(xié)方差：

c）步驟3：量測(cè)更新。

對(duì)先驗(yàn)估計(jì)誤差協(xié)方差k?P進(jìn)行Cholesky分解：

計(jì)算容積點(diǎn)：

量測(cè)方程傳遞容積點(diǎn)：

估計(jì)k時(shí)刻量測(cè)預(yù)測(cè)值：

估計(jì)k時(shí)刻的量測(cè)誤差協(xié)方差：

估計(jì)k時(shí)刻的一步預(yù)測(cè)互相關(guān)誤差協(xié)方差：

計(jì)算k時(shí)刻濾波增益：

計(jì)算k時(shí)刻的狀態(tài)估計(jì)值：

計(jì)算k時(shí)刻的后驗(yàn)估計(jì)誤差協(xié)方差：

通過(guò)完成上述3個(gè)步驟，完成了由k-1時(shí)刻到k時(shí)刻的后驗(yàn)估計(jì)值與后驗(yàn)估計(jì)誤差協(xié)方差的更新。

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

在Matlab（R2010b）環(huán)境下對(duì)雷達(dá)觀測(cè)載入彈道目標(biāo)系統(tǒng)進(jìn)行建模仿真，對(duì)提出的基于 Spherical Simplex-Radial 準(zhǔn)則的容積卡爾曼濾波算法進(jìn)行驗(yàn)證。首先基于狀態(tài)模型，在雷達(dá)站坐標(biāo)系下生成帶有狀態(tài)噪聲的載入彈道目標(biāo)的真實(shí)軌跡，用四階龍格-庫(kù)塔方法對(duì)狀態(tài)方程進(jìn)行離散時(shí)，離散步長(zhǎng)h=1；然后根據(jù)量測(cè)模型生成帶有量測(cè)噪聲的測(cè)量信息用于濾波。再入目標(biāo)初始狀態(tài)以及彈道系數(shù)常值為

雷達(dá)每秒對(duì)再入彈道目標(biāo)進(jìn)行一次測(cè)量，狀態(tài)噪聲協(xié)方差Q與量測(cè)噪聲協(xié)方差R分別為

通過(guò)仿真，得到再入彈道目標(biāo)的運(yùn)動(dòng)軌跡，如圖2所示。

圖2 再入彈道目標(biāo)的運(yùn)動(dòng)軌跡Fig.2 The Trajectory of the Ballistic Trajectory

給定濾波初值如下：

分別以UKF，CKF和SSRCKF算法對(duì)再入彈道目標(biāo)進(jìn)行實(shí)時(shí)跟蹤，設(shè)蒙特卡洛打靶次數(shù)為500。根據(jù)速度和位置均方根誤差對(duì)比 3種算法的性能。位置均方根誤差定義為

式中 N為蒙特卡洛打靶次數(shù)；，分別為第 n次打靶時(shí) k時(shí)刻再入彈道目標(biāo)的真實(shí)值與濾波值，速度均方根誤差與式（37）方法相同。

仿真結(jié)果如圖3、圖4所示。圖3為彈道目標(biāo)速度均方根誤差，圖4為彈道目標(biāo)位置均方根誤差。從圖3、圖4中，可以看出，SSRCKF算法相比于UKF算法與CKF算法有更高的再入彈道目標(biāo)實(shí)時(shí)跟蹤精度。

圖3 彈道目標(biāo)速度均方根誤差Fig.3 Velocity Root-mean-square Error of the Ballistic Target

圖4 彈道目標(biāo)位置均方根誤差Fig.4 Root Mean Square Error of Trajectory Target

統(tǒng)計(jì) 200～250 s，UKF、CKF、SSRCKF 3 種算法對(duì)再入彈道目標(biāo)實(shí)時(shí)跟蹤的速度與位置的均方根誤差，求出其平均值，如表1所示。由表1可以看出：相同條件下對(duì)再入彈道目標(biāo)進(jìn)行實(shí)時(shí)跟蹤，SSRCKF算法在定位精度上比CKF算法提高了約4.5 m，比UKF算法提高了5 m；在定速精度上比CKF算法提高了約0.6 m/s，比UKF算法提高了0.7 m/s，充分證明了算法的有效性。

表1 200～250 s速度與位置均方根誤差平均值Tab. 1 Mean Square Root Mean Square Error of 200～250 s

4 結(jié) 論

本文首先對(duì)再入彈道目標(biāo)進(jìn)行動(dòng)力學(xué)建模，然后以推導(dǎo)的基于Spherical Simplex-Radial準(zhǔn)則的容積卡爾曼濾波算法對(duì)再入彈道目標(biāo)進(jìn)行實(shí)時(shí)跟蹤，該算法在沒(méi)有明顯提高計(jì)算量的前提下，有效提高了對(duì)再入彈道目標(biāo)的實(shí)時(shí)跟蹤精度；最后通過(guò)仿真驗(yàn)證，證明所提出的方法較UKF與經(jīng)典的CKF算法，有更好的性能。