李兆銘， 楊文革， 丁丹， 廖育榮

(1. 裝備學(xué)院 研究生院, 北京 101416； 2. 裝備學(xué)院 光電裝備系, 北京 101416)

隨著小衛(wèi)星功能模塊化、一箭多星和應(yīng)急發(fā)射等技術(shù)的發(fā)展，在軌衛(wèi)星的數(shù)量急劇增加。而傳統(tǒng)地面測控站受布站位置的約束，無法提供連續(xù)的測控能力。一旦小衛(wèi)星的導(dǎo)航星座信號受到干擾，而自身又不具備自主導(dǎo)航功能，則需要利用天基測量信息為其提供一種應(yīng)急導(dǎo)航能力。因此，有必要研究利用多星測距對空間合作目標(biāo)提供協(xié)同導(dǎo)航的方法[1-2]。

本文研究的協(xié)同導(dǎo)航方法在本質(zhì)上屬于多源信息融合濾波問題，主要有集中式和分布式2種。集中式濾波要求融合中心匯集所有節(jié)點(diǎn)的測量信息，進(jìn)行集中式數(shù)據(jù)處理，一般會對融合中心造成較高的通信和計算壓力，而且一旦該中心出現(xiàn)故障，則容易導(dǎo)致系統(tǒng)崩潰。為了分散通信和計算壓力，降低節(jié)點(diǎn)故障對系統(tǒng)的影響，分布式濾波逐漸成為研究的熱點(diǎn)問題[3]。Olfati-Saber[4]建立了一種分布式卡爾曼濾波(Distributed Kalman Filter，DKF)計算框架，但該方法在實(shí)際工程應(yīng)用中容易受到系統(tǒng)非線性因素的限制。進(jìn)而，Battistelli和Chisci[5]提出了適用于非線性系統(tǒng)的分布式擴(kuò)展卡爾曼濾波(Distributed Extended Kalman Filter，DEKF)。然而，DEKF對非線性系統(tǒng)的一階線性化近似精度較低，并且需要計算系統(tǒng)的雅可比矩陣。為了提高非線性系統(tǒng)濾波精度，學(xué)者們相繼提出了無跡卡爾曼濾波(Unscented Kalman Filter，UKF)[6]和容積卡爾曼濾波(Cubature Kalman Filter，CKF)[7]。其中，CKF將非線性高斯加權(quán)積分分解為球面積分和徑向積分，并采用一組等權(quán)值的容積點(diǎn)計算非線性高斯加權(quán)積分，具有比EKF和UKF更高的濾波精度和數(shù)值計算穩(wěn)定性[8]，已在工程中取得廣泛的應(yīng)用[9-10]。Wang等[11]采用正則單形變換群計算球面積分，提出了球面單形-徑向容積卡爾曼濾波(Spherical Simplex-Radial Cubature Kalman Filter，SSRCKF)，并且指出球面單形準(zhǔn)則具有比CKF中的球面準(zhǔn)則更高的精度。然而，CKF和SSRCKF 2種方法均采用矩匹配法計算徑向積分，而該方法無法保證徑向積分計算的最優(yōu)性。為了提高徑向積分精度，Shovan和Swati[12]提出一種容積求積分卡爾曼濾波(Cubature Quadrature Kalman Filter，CQKF)，其采用高階高斯-拉蓋爾求積分準(zhǔn)則計算徑向積分，具有比矩匹配法更高的精度。然而，上述非線性濾波需要向分布式濾波中推廣。

本文將球面單形準(zhǔn)則與高斯-拉蓋爾求積分準(zhǔn)則相結(jié)合，提出了一種新的球面單形-徑向容積求積分準(zhǔn)則，將該準(zhǔn)則嵌入DKF框架，得到了一種適用于非線性系統(tǒng)的分布式球面單形-徑向容積求積分卡爾曼濾波(Distributed Spherical Simplex-Radial Cubature Quadrature Kalman Filter，DSSRCQKF)算法，并將該算法應(yīng)用于解決多顆衛(wèi)星對空間合作目標(biāo)的協(xié)同導(dǎo)航問題，仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了本文算法的有效性。

1 分布式協(xié)同導(dǎo)航數(shù)學(xué)模型

用導(dǎo)航星表示自身具備自主導(dǎo)航能力的衛(wèi)星，用目標(biāo)星表示空間合作目標(biāo)，協(xié)同導(dǎo)航示意圖如圖1所示。s個導(dǎo)航星處在一個分布式通信網(wǎng)絡(luò)中，使用無向圖G=(V,E)對該通信網(wǎng)絡(luò)進(jìn)行建模[13]。其中，V={1,2,…,s}為導(dǎo)航星集合，E={(d,q)|d,q∈V}為通信鏈路集合。當(dāng)導(dǎo)航星d和q之間可以通信時，(d,q)∈E，此時稱導(dǎo)航星d和q互為鄰居星。導(dǎo)航星d的鄰居星集合用Nd表示，同時記Jd=Nd∪(d)為鄰居星集與自身的并集。假設(shè)每個導(dǎo)航星均具備對目標(biāo)星的測距能力，通過與其鄰居星的分布式數(shù)據(jù)融合實(shí)現(xiàn)對目標(biāo)星的協(xié)同一致定軌。

圖1 分布式協(xié)同導(dǎo)航示意圖

1.1 協(xié)同導(dǎo)航狀態(tài)方程

地球J2項(xiàng)非球形攝動是衛(wèi)星所受到的最主要的攝動力。在J2000地心慣性坐標(biāo)系中，考慮J2項(xiàng)攝動影響，衛(wèi)星的軌道動力學(xué)模型為[14]

(1)

用四階龍格庫塔法可以將式(1)寫成如下離散狀態(tài)方程的形式：

Xk=f(Xk-1)+wk-1

(2)

1.2 協(xié)同導(dǎo)航量測方程

(3)

將式(3)的幾何關(guān)系寫成如下離散量測方程的形式：

zd,k=h(Xk)+vd,k

(4)

式中：zd,k為k時刻的量測值；vd,k為量測噪聲。

2 分布式協(xié)同導(dǎo)航濾波算法

2.1 球面單形-徑向容積求積分準(zhǔn)則

(5)

(6)

式中：Un為單位球體表面；σ(y)為面積分元素。

一般難以得到積分S(r)和R的解析解，因此考慮采用數(shù)值積分的方法對其進(jìn)行近似。由文獻(xiàn)[11,15]可知，可以采用如下由2n+2個積分點(diǎn)構(gòu)成的三階球面單形準(zhǔn)則對球面積分進(jìn)行近似：

(7)

(8)

(9)

式中：tj為求積分點(diǎn)；Aj為對應(yīng)的權(quán)值。求積分點(diǎn)可以由如下p階切比雪夫-拉蓋爾多項(xiàng)式[12]的解求得：

(10)

相應(yīng)的權(quán)值可以由式(11)解得：

(11)

該準(zhǔn)則的近似精度取決于求積分點(diǎn)的個數(shù)，當(dāng)p=2時可以得到

(12)

由式(10)、式(11)解得t1、A1、t2和A2的值為

(13)

(14)

結(jié)合式(12)和t1、t2、A1、A2，可以得到如下計算IN的球面單形-徑向容積求積分準(zhǔn)則：

(15)

特別的，當(dāng)式(9)中的p=1時，可以解得t1=n/2，A1=Γ(n/2)，進(jìn)而可推導(dǎo)出文獻(xiàn)[11,15]中的球面單形-徑向容積準(zhǔn)則。由此可見，本文提出的球面單形-徑向容積求積分準(zhǔn)則的精度要高于球面單形-徑向容積準(zhǔn)則。

(16)

(17)

2.2 DSSRCQKF算法

由狀態(tài)方程式(2)和量測方程式(4)構(gòu)成如下濾波系統(tǒng)方程，為了使算法研究更具有一般性，考慮狀態(tài)向量xk∈Rn，量測向量zk∈Rc，同時為了使公式的表述更為簡潔，在沒有歧義的情況下省略下標(biāo)d。

(18)

步驟1濾波初始化。

(19)

循環(huán)k=1,2,…，完成以下步驟。

(20)

(21)

(22)

(23)

(24)

計算交叉協(xié)方差矩陣Pxz：

(25)

(26)

步驟5對接收到的信息進(jìn)行融合。

(27)

(28)

(29)

從式(18)可以看出，本文算法并沒有對非線性函數(shù)的具體形式進(jìn)行約束，因此DSSRCQKF算法不僅適用于本文所述的多星對合作目標(biāo)的協(xié)同導(dǎo)航問題，同樣可以應(yīng)用于傳感器網(wǎng)絡(luò)目標(biāo)跟蹤、編隊(duì)衛(wèi)星協(xié)同導(dǎo)航和地基測控協(xié)同定軌等非線性系統(tǒng)協(xié)同濾波問題。

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

濾波初值為

-4 528 939 1 715)T

初始協(xié)方差矩陣為

圖2 衛(wèi)星地面實(shí)驗(yàn)?zāi)M器

衛(wèi) 星半長軸/km偏心率軌道傾角/(°)升交點(diǎn)赤經(jīng)/(°)近地點(diǎn)幅角/(°)真近點(diǎn)角/(°)目標(biāo)星16378.1027353010導(dǎo)航星115378.10456000導(dǎo)航星215378.1010601520導(dǎo)航星315378.103860200導(dǎo)航星415378.107060300

圖3 4顆導(dǎo)航星間的通信拓?fù)浣Y(jié)構(gòu)

圖4 導(dǎo)航星1對目標(biāo)星的實(shí)時定軌RMSE

圖5 導(dǎo)航星2對目標(biāo)星的實(shí)時定軌RMSE

圖6 導(dǎo)航星3對目標(biāo)星的實(shí)時定軌RMSE

圖7 導(dǎo)航星4對目標(biāo)星的實(shí)時定軌RMSE

導(dǎo)航星平均定位RMSE/mDEKFDSSRCQKF導(dǎo)航星130.77518.633導(dǎo)航星230.67918.166導(dǎo)航星331.45618.969導(dǎo)航星430.39318.189

表3 協(xié)同導(dǎo)航平均定速RMSE

對比本文提出的DSSRCQKF算法與DEKF算法，參數(shù)ε取為0.01，用均方根誤差(RMSE)描述導(dǎo)航星對目標(biāo)星的實(shí)時定軌精度，運(yùn)行200次Monte Carlo仿真，仿真結(jié)果如圖4～圖7所示。統(tǒng)計平均定軌RMSE，并分別列于表2和表3。從仿真結(jié)果可以看出，本文DSSRCQKF算法實(shí)時定位精度約為19 m，定速精度約為1.71 m/s，而DEKF算法定位精度約為30 m，定速精度約為1.73 m/s，從而表明本文算法具有更高的導(dǎo)航精度。對于多星協(xié)同導(dǎo)航問題，2種算法通過導(dǎo)航星間的分布式通信與數(shù)據(jù)融合，均可實(shí)現(xiàn)對目標(biāo)星軌道狀態(tài)的一致估計。每顆導(dǎo)航星的估計結(jié)果基本相同，結(jié)果間細(xì)微的差別主要由系統(tǒng)的非線性引起，因?yàn)樵诜蔷€性卡爾曼濾波中，假設(shè)后驗(yàn)概率密度服從高斯分布，本質(zhì)上是一種次優(yōu)濾波方法，無法像線性卡爾曼濾波一樣得到理論上的最優(yōu)估計。而且為了將非線性卡爾曼濾波嵌入分布式濾波中而引入的偽觀測矩陣同樣會帶來一些誤差，但每顆導(dǎo)航星估計值間的差別在應(yīng)用中是可以接受的。從導(dǎo)航星間的通信拓?fù)浣Y(jié)構(gòu)可以看出，每顆導(dǎo)航星僅與其鄰居星通信，濾波中間數(shù)據(jù)在整個無線網(wǎng)絡(luò)中分布式流動，從而避免了將所有數(shù)據(jù)發(fā)送到融合中心的集中式處理，提高了系統(tǒng)的靈活性。

4 結(jié) 論

1) 本文將球面單形準(zhǔn)則與高斯-拉蓋爾求積分準(zhǔn)則相結(jié)合，提出一種新的球面單形-徑向容積求積分準(zhǔn)則，并將該準(zhǔn)則嵌入DKF框架，得到一種適用于多星對空間目標(biāo)協(xié)同導(dǎo)航的DSSRCQKF算法。該算法要求每顆導(dǎo)航星僅與其鄰居星進(jìn)行通信和數(shù)據(jù)融合，通過數(shù)據(jù)的分布式流動實(shí)現(xiàn)對目標(biāo)星軌道狀態(tài)的一致估計，從而避免了傳統(tǒng)集中式處理中較高的通信和計算壓力。

2) 仿真實(shí)驗(yàn)結(jié)果表明，本文DSSRCQKF算法中各導(dǎo)航星對目標(biāo)星的實(shí)時定位精度約為19 m，定速精度約為1.71 m/s。DEKF算法中各導(dǎo)航星對目標(biāo)星的定位精度約為30 m，定速精度約為1.73 m/s，相比之下本文算法將定位精度提高了11 m，定速精度提高了0.02 m/s，從而驗(yàn)證了本文算法的有效性。

3) 本文提出的DSSRCQKF算法還可以進(jìn)一步應(yīng)用于其他非線性系統(tǒng)協(xié)同濾波問題。

參考文獻(xiàn)(References)

[1] 王曉初,尤政,趙開春,等.分布式衛(wèi)星系統(tǒng)的協(xié)同導(dǎo)航估計方法[J].宇航學(xué)報,2013,34(8):1107-1113.

WANG X C,YOU Z,ZHAO K C,et al.A cooperative navigation estimation method in distributed satellite system[J].Journal of Astronautics,2013,34(8):1107-1113(in Chinese).

[2] 張艾,李勇.基于星間測距的編隊(duì)衛(wèi)星一致性導(dǎo)航算法[J].空間控制技術(shù)與應(yīng)用,2015,41(3):24-27.

ZHANG A,LI Y.A consensus navigation algorithm for spacecraft formation based on distance ranging[J].Aerospace Control and Application,2015,41(3):24-27(in Chinese).

[3] ZHOU Z W,FANG H T,HONG Y G.Distributed estimation for moving target based on state-consensus strategy[J].IEEE Transactions on Automatic Control,2013,58(8):2097-2100.

[4] OLFATI-SABER R.Distributed Kalman filtering for sensor networks[C]∥Proceedings of the 46th IEEE Conference on Decision and Control.Piscataway,NJ:IEEE Press,2007:5492-5496.

[5] BATTISTELLI G,CHISCI L.Stability of consensus extended Kalman filtering for distributed state estimation[C]∥Preprints of the 19th World Congress the International Federation of Automatic Control.Amsterdam：Elsevier,2014:5520-5525.

[6] JULIER S J,UHLMANN J K,DURRANTWHYTE H F.A new method for nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactions on Automatic Control,2000,45(3):477-482.

[7] ARASARATNAM I,HAYKIN S.Cubature Kalman filters[J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.

[8] ZAREI J,SHOKRI E.Convergence analysis of non-linear filtering based on cubature Kalman filter[J].IET Science,Mea-surement & Technology,2015,9(3):294-305.

[9] 錢華明,黃蔚,葛磊,等.基于四元數(shù)平方根容積卡爾曼濾波的姿態(tài)估計[J].北京航空航天大學(xué)學(xué)報，2013,39(5):646-649.

QIAN H M,HUANG W,GE L,et al.Attitude estimation based on quaternion square-root cubature Kalman filter[J].Journal of Beijing University of Aeronautics and Astronautics,2013,39(5):646-649(in Chinese).

[10] 方君,戴邵武,許文明,等.基于ST-SRCKF的超高速強(qiáng)機(jī)動目標(biāo)跟蹤算法[J].北京航空航天大學(xué)學(xué)報,2016,42(8):1698-1708.

FANG J,DAI S W,XU W M,et al.Highly maneuvering hypervelocity-target tracking algorithm based on ST-SRCKF[J].Journal of Beijing University of Aeronautics and Astronautics,2016,42(8):1698-1708(in Chinese).

[11] WANG S Y,FENG J C,CHI K T.Spherical simplex-radial cubature Kalman filter[J].IEEE Signal Processing Letters,2014,21(1):43-46.

[12] SHOVAN B,SWATI.Cubature quadrature Kalman filter[J].IET Signal Processing,2013,7(7):533-541.

[13] BATTISTELLI G,CHISCI L,MUGNAI G,et al.Consensus-based linear and nonlinear filtering[J].IEEE Transactions on Automatic Control,2015,60(5):1410-1415.

[14] 李璟璟,張迎春,鄭靖,等.基于信息融合的大橢圓軌道衛(wèi)星組合導(dǎo)航方法[J].宇航學(xué)報,2012,33(9):1234-1239.

LI J J,ZHANG Y C,ZHENG J,et al.An information fusion based integrated navigation method for highly elliptical orbit satellite[J].Journal of Astronautics,2012,33(9):1234-1239(in Chinese).

[15] 朱奇光,袁梅,王梓巍,等.機(jī)器人球面單徑容積FastSLAM算法[J].機(jī)器人,2015,37(6):709-716.

ZHU Q G,YUAN M,WANG Z W,et al.A robot spherical simplex-radial cubature FastSLAM algorithm[J].Robot,2015,37(6):709-716(in Chinese).