多星對合作目標的分布式協同導航濾波算法

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

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

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

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

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

1 分布式協同導航數學模型

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

圖1 分布式協同導航示意圖

1.1 協同導航狀態方程

地球J2項非球形攝動是衛星所受到的最主要的攝動力。在J2000地心慣性坐標系中，考慮J2項攝動影響，衛星的軌道動力學模型為[14]

(1)

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

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

(2)

1.2 協同導航量測方程

(3)

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

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

(4)

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

2 分布式協同導航濾波算法

2.1 球面單形-徑向容積求積分準則

(5)

(6)

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

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

(7)

(8)

(9)

式中：tj為求積分點；Aj為對應的權值。求積分點可以由如下p階切比雪夫-拉蓋爾多項式[12]的解求得：

(10)

相應的權值可以由式(11)解得：

(11)

該準則的近似精度取決于求積分點的個數，當p=2時可以得到

(12)

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

(13)

(14)

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

(15)

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

(16)

(17)

2.2 DSSRCQKF算法

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

(18)

步驟1濾波初始化。

(19)

循環k=1,2,…，完成以下步驟。

(20)

(21)

(22)

(23)

(24)

計算交叉協方差矩陣Pxz：

(25)

(26)

步驟5對接收到的信息進行融合。

(27)

(28)

(29)

從式(18)可以看出，本文算法并沒有對非線性函數的具體形式進行約束，因此DSSRCQKF算法不僅適用于本文所述的多星對合作目標的協同導航問題，同樣可以應用于傳感器網絡目標跟蹤、編隊衛星協同導航和地基測控協同定軌等非線性系統協同濾波問題。

3 仿真實驗

濾波初值為

-4 528 939 1 715)T

初始協方差矩陣為

圖2 衛星地面實驗模擬器

衛 星半長軸/km偏心率軌道傾角/(°)升交點赤經/(°)近地點幅角/(°)真近點角/(°)目標星16378.1027353010導航星115378.10456000導航星215378.1010601520導航星315378.103860200導航星415378.107060300

圖3 4顆導航星間的通信拓撲結構

圖4 導航星1對目標星的實時定軌RMSE

圖5 導航星2對目標星的實時定軌RMSE

圖6 導航星3對目標星的實時定軌RMSE

圖7 導航星4對目標星的實時定軌RMSE

導航星平均定位RMSE/mDEKFDSSRCQKF導航星130.77518.633導航星230.67918.166導航星331.45618.969導航星430.39318.189

表3 協同導航平均定速RMSE

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

4 結 論

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

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

3) 本文提出的DSSRCQKF算法還可以進一步應用于其他非線性系統協同濾波問題。

參考文獻(References)

[1] 王曉初,尤政,趙開春,等.分布式衛星系統的協同導航估計方法[J].宇航學報,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] 張艾,李勇.基于星間測距的編隊衛星一致性導航算法[J].空間控制技術與應用,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] 錢華明,黃蔚,葛磊,等.基于四元數平方根容積卡爾曼濾波的姿態估計[J].北京航空航天大學學報，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的超高速強機動目標跟蹤算法[J].北京航空航天大學學報,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] 李璟璟,張迎春,鄭靖,等.基于信息融合的大橢圓軌道衛星組合導航方法[J].宇航學報,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] 朱奇光,袁梅,王梓巍,等.機器人球面單徑容積FastSLAM算法[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).