李兆銘 楊文革 丁丹 廖育榮

1)(裝備學(xué)院研究生院,北京 101416)

2)(裝備學(xué)院光電裝備系,北京 101416)

1 引 言

對(duì)低軌衛(wèi)星進(jìn)行在軌監(jiān)視需要實(shí)時(shí)確定衛(wèi)星的軌道狀態(tài),地基雷達(dá)由于具備全天時(shí)、全天候的工作能力而成為空間監(jiān)視系統(tǒng)中的一類重要的傳感器[1].由于衛(wèi)星軌道動(dòng)力學(xué)模型具有較強(qiáng)的非線性,因此,在數(shù)據(jù)處理層面,實(shí)時(shí)定軌問題的本質(zhì)便是在考慮軌道攝動(dòng)的影響下,利用雷達(dá)輸出的帶噪聲的測(cè)距、測(cè)速和測(cè)角數(shù)據(jù),通過非線性濾波算法得到軌道狀態(tài)的最優(yōu)估計(jì)[2],具有重要的研究?jī)r(jià)值.

非線性卡爾曼濾波算法采用遞歸方法實(shí)現(xiàn)對(duì)狀態(tài)向量的估計(jì),其假設(shè)后驗(yàn)概率密度服從高斯分布,從時(shí)間更新與量測(cè)更新的五個(gè)關(guān)鍵積分可以得到其核心是計(jì)算形如“非線性函數(shù)×高斯概率密度函數(shù)”的多維向量積分[3],一般難以求得該積分的解析解,因此研究中經(jīng)常采用數(shù)值容積準(zhǔn)則對(duì)其進(jìn)行逼近.作為非線性濾波的一種重要方法,容積卡爾曼濾波[3,4](cubature Kalman f i lter,CKF)采用三階球面-徑向容積準(zhǔn)則對(duì)非線性函數(shù)的高斯加權(quán)積分進(jìn)行近似,具有比無跡卡爾曼濾波[5,6]更高的濾波精度和數(shù)值穩(wěn)定性,已經(jīng)在許多領(lǐng)域得到了廣泛的應(yīng)用[7?10].為了進(jìn)一步提高非線性濾波精度,Jia等[11,12]利用任意階全對(duì)稱球面插值準(zhǔn)則和矩匹配原理,提出了五階容積卡爾曼濾波(f i fth-degree cubature Kalman f i lter,5-CKF),將CKF的精度從三階提高到五階,該算法目前已經(jīng)應(yīng)用于目標(biāo)跟蹤、慣性導(dǎo)航初始對(duì)準(zhǔn)和多源數(shù)據(jù)融合等領(lǐng)域[13,14].文獻(xiàn)[15]將強(qiáng)跟蹤濾波與5-CKF相結(jié)合,利用漸消因子在線調(diào)整協(xié)方差矩陣,有效提高了算法的魯棒性.文獻(xiàn)[16]利用統(tǒng)計(jì)線性回歸模型近似非線性量測(cè)模型,提出了基于Huber的5-CKF算法.文獻(xiàn)[17]利用矩陣對(duì)角化變換代替5-CKF中的Cholesky分解,提高了濾波計(jì)算的穩(wěn)定性.上述研究雖然改進(jìn)了5-CKF算法,但并沒有在本質(zhì)上改進(jìn)算法所采用的五階容積準(zhǔn)則.Wang等[18]利用球面單純形變換群計(jì)算球面積分,提出了五階球面單純形-徑向容積卡爾曼濾波(f i fth-degree spherical simplex-radial cubature Kalman f i lter,5-SSRCKF),并指出在高維條件下,5-SSRCKF具有比5-CKF更高的濾波精度.Singh和Bhaumik[19]指出5-CKF和5-SSRCKF均是采用矩匹配法計(jì)算徑向積分,而該方法無法保證徑向準(zhǔn)則的最優(yōu)性,進(jìn)而選擇采用高階高斯-拉蓋爾公式計(jì)算徑向積分,提出了高階容積求積分卡爾曼濾波(high-degree cubature quadrature Kalman filter,HDCQKF).然而,上述濾波算法在提高精度的同時(shí)帶來了積分點(diǎn)個(gè)數(shù)的增多,且積分點(diǎn)個(gè)數(shù)隨著系統(tǒng)維數(shù)呈多項(xiàng)式增長(zhǎng),一旦算法應(yīng)用于高維系統(tǒng),或者處理器性能較低時(shí),可能會(huì)造成較重的計(jì)算負(fù)擔(dān),進(jìn)而降低算法的實(shí)時(shí)性.衛(wèi)星軌道的實(shí)時(shí)確定需要在一個(gè)濾波周期內(nèi)實(shí)現(xiàn)對(duì)量測(cè)信號(hào)的解析并輸入濾波算法執(zhí)行,對(duì)算法的計(jì)算效率和實(shí)時(shí)應(yīng)用能力具有較高的要求.因此,研究如何降低五階濾波定軌算法的計(jì)算量是十分必要的.

本文提出一種新的逼近積分點(diǎn)個(gè)數(shù)下限的5-CKF定軌算法,該算法在近似非線性函數(shù)的高斯加權(quán)積分時(shí)所需的積分點(diǎn)個(gè)數(shù)僅比五階容積準(zhǔn)則積分點(diǎn)個(gè)數(shù)的理論下限多一個(gè)積分點(diǎn),從而有效地降低了算法的計(jì)算量.仿真實(shí)驗(yàn)結(jié)果表明,本文所提算法在定軌精度方面與已有五階濾波算法相當(dāng),但所需的積分點(diǎn)最少,實(shí)時(shí)性最高,從而驗(yàn)證了本文算法的有效性.

2 逼近積分點(diǎn)個(gè)數(shù)下限的五階容積準(zhǔn)則

考慮如下向量函數(shù)的積分,

(1)式所示的積分一般難以求得解析解,因此在研究中經(jīng)常采用被積函數(shù)在某些確定點(diǎn)上的函數(shù)值的加權(quán)和來對(duì)該積分進(jìn)行數(shù)值逼近,構(gòu)成如下容積準(zhǔn)則,

式中,xi為積分點(diǎn),ωi為積分權(quán)值,N為積分點(diǎn)個(gè)數(shù).研究中經(jīng)常采用代數(shù)精度來描述積分準(zhǔn)則(2)的逼近程度,為使容積準(zhǔn)則達(dá)到五階代數(shù)精度,需要的積分點(diǎn)個(gè)數(shù)的理論下限為[20]

目前在5-CKF中常見的容積準(zhǔn)則有兩種,分別為五階球面-徑向容積準(zhǔn)則和五階球面單純形-徑向容積準(zhǔn)則,其積分點(diǎn)個(gè)數(shù)分別為2n2+1和n2+3n+3.可以看出,該兩種準(zhǔn)則的積分點(diǎn)個(gè)數(shù)與理論下限尚有差距.為了進(jìn)一步逼近積分點(diǎn)個(gè)數(shù)的下限,降低數(shù)值運(yùn)算量,提高算法的實(shí)時(shí)性,文獻(xiàn)[20]給出了如下容積準(zhǔn)則:

式中,μ,γ和η滿足如下等式:

該準(zhǔn)則所需的積分點(diǎn)個(gè)數(shù)為n2+n+2,僅比(3)式中的理論下限多一個(gè)積分點(diǎn),但該準(zhǔn)則僅對(duì)2≤n≤7成立.該積分準(zhǔn)則包含8個(gè)參數(shù),分別是μ,γ,η,λ,ξ,A,B和C,文獻(xiàn)[20]中給出了這些參數(shù)的具體值,本文僅針對(duì)實(shí)時(shí)定軌系統(tǒng)給出n=6時(shí)的參數(shù)值,列于表1.

為了將(4)式化簡(jiǎn)為便于應(yīng)用的形式,定義e為如下n階單位矩陣:

表1 容積準(zhǔn)則參數(shù)表(n=6)Table 1.Parameters of cubature rule(n=6).

用矩陣下標(biāo)i表示矩陣的第i列,利用單位矩陣e可以將(4)式寫成如下形式:

進(jìn)一步地,定義如下變量

則(9)式可以簡(jiǎn)化為如下形式:

相比較(4)式的形式,通過線性變換改寫成的(10)式的形式更便于應(yīng)用于非線性濾波算法.

3 逼近積分點(diǎn)個(gè)數(shù)下限的5-CKF算法

考慮如下加性噪聲的非線性系統(tǒng):

式中,xk∈Rn為系統(tǒng)狀態(tài)向量,zk∈Rp為量測(cè)向量,f(xk)和h(xk)是已知的非線性函數(shù),噪聲wk∈Rn和vk∈Rp是不相關(guān)的高斯白噪聲,其協(xié)方差矩陣分別為Qk和Rk.貝葉斯濾波理論采用遞歸方法實(shí)現(xiàn)對(duì)狀態(tài)向量xk的估計(jì),其關(guān)鍵是計(jì)算貝葉斯濾波框架中以下幾個(gè)積分[19]:

式中,N(x;)為高斯概率密度函數(shù),且隨機(jī)變量x的均值為,協(xié)方差矩陣為Px;為k時(shí)刻的先驗(yàn)狀態(tài)估計(jì),為先驗(yàn)誤差協(xié)方差矩陣,為量測(cè)預(yù)測(cè)值,Pz為量測(cè)預(yù)測(cè)協(xié)方差矩陣,Pxz為交叉協(xié)方差矩陣.從(12)—(16)式可以看出,非線性卡爾曼濾波的關(guān)鍵是計(jì)算形如的多維向量函數(shù)積分,其中,g(x)為任意非線性函數(shù).又因?yàn)樵摲e分具有如下等價(jià)表示[3]:

結(jié)合積分準(zhǔn)則(10),可以得到非線性函數(shù)的高斯加權(quán)積分為

步驟1 濾波初始化

循環(huán)k=1,2,···,完成以下步驟.

步驟2 時(shí)間更新

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

計(jì)算容積點(diǎn)的非線性傳遞

計(jì)算先驗(yàn)狀態(tài)估計(jì)

計(jì)算先驗(yàn)誤差協(xié)方差矩陣

步驟3 量測(cè)更新

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

計(jì)算容積點(diǎn)的非線性傳遞

計(jì)算量測(cè)預(yù)測(cè)值

計(jì)算量測(cè)誤差協(xié)方差矩陣

計(jì)算交叉協(xié)方差矩陣

步驟4 狀態(tài)更新

計(jì)算卡爾曼濾波增益

計(jì)算后驗(yàn)狀態(tài)估計(jì)

計(jì)算后驗(yàn)誤差協(xié)方差矩陣

4 低軌衛(wèi)星實(shí)時(shí)定軌數(shù)學(xué)模型

4.1 衛(wèi)星軌道動(dòng)力學(xué)模型

對(duì)于運(yùn)行在近地軌道上的衛(wèi)星,地球J2項(xiàng)非球形攝動(dòng)和大氣阻力攝動(dòng)是衛(wèi)星所受到的最主要的攝動(dòng)力.在J2000地心慣性坐標(biāo)系(O-XY Z)中,考慮上述兩種攝動(dòng)影響,衛(wèi)星的軌道動(dòng)力學(xué)模型為

式中,cd為大氣阻力系數(shù);為衛(wèi)星面質(zhì)比;ρd為大氣密度;vrel為衛(wèi)星與大氣間的相對(duì)速度,假設(shè)大氣隨著地球轉(zhuǎn)動(dòng),則有

4.2 地面雷達(dá)量測(cè)模型

雷達(dá)測(cè)量模型建立在雷達(dá)地平坐標(biāo)系(OXhYhZh)中,而軌道模型建立在O-XY Z中,因此需要利用WGS84地球固連坐標(biāo)系(O-XeYeZe)實(shí)現(xiàn)從O-XY Z到O-XhYhZh的轉(zhuǎn)換.假設(shè)衛(wèi)星在OXY Z中的軌道狀態(tài)為,在O-XeYeZe中的軌道狀態(tài)為T,在O-XhYhZh中的軌道狀態(tài)為T,分兩步完成軌道狀態(tài)的轉(zhuǎn)換.

步驟1 從O-XY Z到O-XeYeZe的轉(zhuǎn)換

轉(zhuǎn)換矩陣為MJW=MPwMRoMNuMPr,其中,MPr為歲差矩陣,MNu為章動(dòng)矩陣,MRo為地球自轉(zhuǎn)矩陣,MPw為極移矩陣,進(jìn)而可以得到

將(36)和(37)式寫成矩陣形式為

步驟2 從O-XeYeZe到O-XhYhZh的轉(zhuǎn)換轉(zhuǎn)換矩陣MHW為

式中,λ為雷達(dá)地心經(jīng)度,φ為雷達(dá)地心緯度,同時(shí)可以換算成雷達(dá)在O-XeYeZe中的地心坐標(biāo)T,于是得

寫成矩陣形式為

將(40)式代入(44)式可得

于是,便得到軌道狀態(tài)從O-XY Z到OXhYhZh的轉(zhuǎn)換,如(45)式所示. 為了得到OXhYhZh中的量測(cè)值與軌道狀態(tài)的關(guān)系,將Xh和h寫成具體向量的形式為Xh=T,,則雷達(dá)的測(cè)距值R、測(cè)速值、方位角A和俯仰角E與軌道狀態(tài)之間的幾何關(guān)系為

(46)式可以寫成如下離散量測(cè)方程的形式:

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

首先,用兩個(gè)非線性系統(tǒng)濾波算例來驗(yàn)證本文算法的性能.

算例1 考慮如下包含三角函數(shù)、乘方運(yùn)算以及指數(shù)運(yùn)算的三維強(qiáng)非線性系統(tǒng):

式中,Q=0.1,R=1,濾波初值為0=將本文提出的算法與標(biāo)準(zhǔn)CKF,5-CKF,5-SSRCKF和HDCQKF算法相對(duì)比,仿真步長(zhǎng)為1,仿真步數(shù)為100,采用均方根誤差(root mean square error,RMSE)來描述估計(jì)精度,運(yùn)行1000次Monte Carlo仿真.仿真結(jié)果如圖1所示,統(tǒng)計(jì)RMSE的平均值和算法運(yùn)行時(shí)間列于表2.

圖1 (網(wǎng)刊彩色)五種算法濾波RMSE值Fig.1.(color online)RMSE of f i ve f i ltering algorithms.

表2 五種算法濾波平均RMSE值和運(yùn)行耗時(shí)Table 2.Mean RMSE and execution time of f i ve f i ltering algorithms.

算例2 考慮如下非線性系統(tǒng)模型,該模型是驗(yàn)證非線性濾波算法性能的常用模型,系統(tǒng)狀態(tài)模型和量測(cè)模型為

其中k=1,2,···,N.濾波初值為x0=0n×1,考慮系統(tǒng)維數(shù)n=5和n=7兩種情況,系統(tǒng)噪聲與量測(cè)噪聲均為零均值單位協(xié)方差高斯白噪聲.仿真步長(zhǎng)為1,步數(shù)為100,執(zhí)行1000次Monte Carlo仿真,結(jié)果如圖2所示,統(tǒng)計(jì)平均RMSE和算法運(yùn)行耗時(shí),列于表3.

從以上算例1和2的結(jié)果可以看出,CKF算法執(zhí)行所需的時(shí)間最短,這是因?yàn)镃KF采用三階球面-徑向容積準(zhǔn)則,容積點(diǎn)個(gè)數(shù)少,但該準(zhǔn)則僅具有三階精度,在系統(tǒng)具有強(qiáng)非線性時(shí)的濾波精度要低于五階濾波算法.以本文算例為例,在算例1中,相比CKF,本文算法將濾波精度提高了12.26%.在算例2中,相比CKF,本文算法將濾波精度分別提高了10.91%和5.72%.算例中對(duì)比的四種五階算法的濾波精度相當(dāng),而由于本文采用的五階容積準(zhǔn)則逼近容積點(diǎn)個(gè)數(shù)的下限,因此本文算法在保持五階濾波精度的同時(shí)具有最高的計(jì)算效率.

在通過上述兩個(gè)算例驗(yàn)證本文算法性能之后,將本文算法應(yīng)用于地基實(shí)時(shí)定軌之中,圖3所示為衛(wèi)星地面模擬器,其上運(yùn)行高精度軌道預(yù)報(bào)算法,該算法考慮了地球高階非球形引力、大氣阻力、太陽光壓、三體引力和潮汐等攝動(dòng)影響.其中,考慮21階地球模型,大氣阻力系數(shù)cd=2.2,衛(wèi)星面質(zhì)比為0.02 m2/kg,大氣密度模型采用Jacchia-Roberts模型,太陽光壓系數(shù)cr=1.實(shí)驗(yàn)中考慮衛(wèi)星的軌道歷元為2016年9月1日16:00:00(UTC),軌道六根數(shù)分別為

圖2 (網(wǎng)刊彩色)五種算法濾波RMSE值 (a)五維系統(tǒng);(b)七維系統(tǒng)Fig.2.(color online)RMSE of f i ve f i ltering algorithms:(a)Five dimension system;(b)seven dimension system.

表3 五種算法濾波平均RMSE值和運(yùn)行耗時(shí)Table 3.Mean RMSE and execution time of f i ve f i ltering algorithms.

圖3 衛(wèi)星模擬器Fig.3.Satellite simulator.

雷達(dá)經(jīng)緯度和衛(wèi)星過境時(shí)間如圖4所示,雷達(dá)最低測(cè)量仰角為10°,假設(shè)雷達(dá)測(cè)距誤差為60 m,測(cè)速誤差為0.1 m/s,測(cè)角誤差為0.02°.濾波初值為

前述算例已經(jīng)表明,五階算法的濾波精度要高于標(biāo)準(zhǔn)CKF,故此處不再考慮CKF算法,而將本文提出的算法與5-CKF,5-SSRCKF和HDCQKF算法進(jìn)行對(duì)比,用RMSE來評(píng)價(jià)定軌精度,運(yùn)行200次Monte-Carlo仿真,結(jié)果如圖5—圖8所示,統(tǒng)計(jì)300—530 s內(nèi)平均定位RMSE和平均定速RMSE并列于表4,可以看出,四種算法的收斂速度基本一致,定軌精度基本相同.

對(duì)比四種算法的積分點(diǎn)個(gè)數(shù)和運(yùn)行200次Monte-Carlo仿真所需的時(shí)間列于表5,可見在同等定軌精度的條件下,本文提出的算法所需的積分點(diǎn)個(gè)數(shù)最少,算法運(yùn)行所需的時(shí)間最短,因此實(shí)時(shí)性最高.

圖4 實(shí)時(shí)定軌示意圖Fig.4.Schematic diagram of orbit determination.

表4 300—530 s平均定軌RMSETable 4.Mean RMSE of orbit determination during 300–530 s.

表5 三種算法積分點(diǎn)個(gè)數(shù)與執(zhí)行時(shí)間Table 5.The number of quadrature points and execution time of three algorithm.

圖5 (網(wǎng)刊彩色)1—10 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.5.(color online)RMSE of orbit determination during 1–10 s:(a)Position RMSE;(b)velocity RMSE.

圖6 (網(wǎng)刊彩色)150—200 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.6.(color online)RMSE of orbit determination during 150–200 s:(a)Position RMSE;(b)velocity RMSE.

圖7 (網(wǎng)刊彩色)300—350 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.7.(color online)RMSE of orbit determination during 300–350 s:(a)Position RMSE;(b)velocity RMSE.

圖8 (網(wǎng)刊彩色)450—500 s定軌RMSE (a)位置RMSE;(b)速度RMSEFig.8.(color online)RMSE of orbit determination during 450–500 s:(a)Position RMSE;(b)velocity RMSE.

6 結(jié) 論

本文提出了一種新的逼近積分點(diǎn)個(gè)數(shù)下限的5-CKF定軌算法,采用一種數(shù)值容積準(zhǔn)則對(duì)非線性函數(shù)的高斯加權(quán)積分進(jìn)行近似,并在貝葉斯濾波算法框架下推導(dǎo)出算法的更新步驟.該算法所需的積分點(diǎn)個(gè)數(shù)為n2+n+2,比五階代數(shù)精度容積準(zhǔn)則的積分點(diǎn)個(gè)數(shù)理論下限僅多一個(gè),相比已有的五階容積濾波算法,比5-CKF少n2?n?1,比5-SSRCKF少2n+1,可見該算法降低了計(jì)算的復(fù)雜度,有效提高了算法的實(shí)時(shí)性,且隨著系統(tǒng)狀態(tài)維數(shù)的增加,該算法的優(yōu)勢(shì)更加明顯.在地基雷達(dá)對(duì)低軌衛(wèi)星實(shí)時(shí)定軌的仿真實(shí)驗(yàn)中,本文算法的定軌精度與5-CKF,5-SSRCKF算法定軌精度相當(dāng),定位精度均為22 m左右,定速精度均為0.11 m/s左右,但算法的運(yùn)行時(shí)間最短,運(yùn)行200次Monte-Carlo仿真相比5-CKF少了113.91 s,相比5-SSRCKF少了46.35 s,從而驗(yàn)證了該算法的有效性.雖然該算法在保持五階濾波精度的同時(shí)具有相比其他算法更高的計(jì)算效率,但僅針對(duì)系統(tǒng)維數(shù)2≤n≤7時(shí)有效,然而該維數(shù)范圍已經(jīng)可以滿足大部分的應(yīng)用需求.

[1]Ning X,Ye C M,Yang J,Shen B 2014Chin.J.Radio29 27(in Chinese)[寧夏,葉春茂,楊健,沈彬2014電波科學(xué)學(xué)報(bào)29 27]

[2]Abhinoy K S,Shovan B 2014IEEE International Symposium on Signal Processing and Information TechnologyNoida,India,December 15–17,2014 p114

[3]Arasaratnam I,Haykin S 2009IEEE Trans.Autom.Control54 1254

[4]Jafar Z,Ehsan S 2015IET Sci.Meas.Technol.9 294

[5]Julier S J,Uhlmann J K,Whyte H F D 2000IEEE Trans.Autom.Control45 477

[6]Xiong K,Zhang H Y,Chan C W 2006Automatica42 261

[7]Zhang L J,Yang H B,Lu H P,Zhang S F,Cai H,Qian S 2014Acta Astronaut.105 254

[8]Chen J G,Wang N,Ma L L,Xu B G 2015IET Radar Sonar Navig.9 324

[9]Lu Z Y,Wang D M,Wang J H,Wang Y 2015Acta Phys.Sin.64 150502(in Chinese)[逯志宇,王大鳴,王建輝,王躍2015物理學(xué)報(bào)64 150502]

[10]Wu H,Chen S X,Yang B F,Chen K 2015Acta Phys.Sin.64 218401(in Chinese)[吳昊,陳樹新,楊賓峰,陳坤2015物理學(xué)報(bào)64 218401]

[11]Jia B,Xin M,Cheng Y 2012Automatica49 510

[12]Jia B,Xin M,Cheng Y 2012IEEE Conference on Decision and ControlMaui Hawaii,USA,December 10–13,2012 p4095

[13]Huang X Y,Tang X Q,Wu M 2015Syst.Eng.Electron.37 633(in Chinese)[黃湘遠(yuǎn),湯霞清,武萌2015系統(tǒng)工程與電子技術(shù)37 633]

[14]Zhang X C 2014Circuits Syst.Signal Process33 1799[15]Zhang L,Cui N G,Wang X G,Yang F,Lu B G 2015Acta Aeronaut.Astronaut.Sin.36 3885(in Chinese)[張龍,崔乃剛,王小剛,楊峰,盧寶剛2015航空學(xué)報(bào)36 3885]

[16]Zhang W J,Wang S Y,Feng Y L,Feng J C 2016Acta Phys.Sin.65 088401(in Chinese)[張文杰,王世元,馮亞麗,馮久超2016物理學(xué)報(bào)65 088401]

[17]Zhao L Q,Chen K Y,Wang J L,Yu T 2016Control Decis.31 1080(in Chinese)[趙利強(qiáng),陳坤云,王建林,于濤2016控制與決策31 1080]

[18]Wang S Y,Feng J C,Tse C K 2014IEEE Signal Process.Lett.21 43

[19]Singh A K,Bhaumik S 2015Int.J.Control Autom.Syst.13 1097

[20]Lu J,Darmofal D L 2004SIAM J.Sci.Comput.26 613