張 艾，李 勇

(中國空間技術(shù)研究院錢學(xué)森空間技術(shù)實驗室，北京 100094)

0 引 言

隨著高分辨率對地觀測、深空探測、空間在軌服務(wù)以及一系列商業(yè)航天活動的蓬勃開展，以分布式衛(wèi)星系統(tǒng)為代表的多航天器協(xié)同工作模式已成為空間技術(shù)發(fā)展的新趨勢。分布式衛(wèi)星系統(tǒng)實現(xiàn)自主協(xié)同運行需要具備的核心和基礎(chǔ)能力是自主導(dǎo)航，即可以不依賴地面測控站及全球?qū)Ш叫亲到y(tǒng)的支持，僅利用各個衛(wèi)星自備的測量設(shè)備實時確定各自的位置和速度[1]。近年來，基于星間相對測量的自主導(dǎo)航技術(shù)受到廣泛關(guān)注，測量量包括相對距離、視線角和相對位置矢量等[2-6]。

文獻(xiàn)[7]首先提出了不需要地面信息，僅利用星間相對位置矢量測量的雙星定軌方法，并指出這種方法不適用于兩衛(wèi)星運行在共面圓軌道且半長軸相等的情況，但未對此方法給出嚴(yán)格的理論證明。文獻(xiàn)[8]從物理角度對上述方法進(jìn)行了解釋，將兩衛(wèi)星視為一個巨型加速度計，通過協(xié)方差分析對定位誤差進(jìn)行研究。文獻(xiàn)[9]對該自主導(dǎo)航方法從非線性系統(tǒng)能觀性角度進(jìn)行理論分析，通過數(shù)學(xué)證明，給出系統(tǒng)可觀測的充分條件，即兩衛(wèi)星瞬時地心距不相等。文獻(xiàn)[1]在上述研究基礎(chǔ)上提出非線性系統(tǒng)局部k階能觀性的概念，并給出該自主導(dǎo)航系統(tǒng)局部4-6階能觀的充要條件。文獻(xiàn)[10]考慮能觀性強(qiáng)弱對濾波算法的影響，給出了一個基于相對位置矢量測量的自主導(dǎo)航系統(tǒng)能觀性秩條件可觀度評價標(biāo)準(zhǔn)，并據(jù)此對UKF算法進(jìn)行改進(jìn)，提高了導(dǎo)航定位的精度。但在該算法中，評價標(biāo)準(zhǔn)只能基于位置、速度估計值而非其真值得到，不能真實反映系統(tǒng)能觀度大小。

衛(wèi)星導(dǎo)航系統(tǒng)是一個典型非線性系統(tǒng)，一般采用擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)算法進(jìn)行估計[11-17]。EKF算法采用線性化方法近似非線性函數(shù)，當(dāng)系統(tǒng)非線性程度較強(qiáng)時估計效果不佳。20世紀(jì)90年代以來，多種非線性濾波算法例如無跡卡爾曼濾波(Unscented Kalman Filter, UKF)算法[18]，粒子濾波(Particle Filtering, PF)算法[19]等紛紛涌現(xiàn)。UKF算法適用于非線性程度較強(qiáng)的系統(tǒng)，估計精度高于EKF，但在星間測量自主導(dǎo)航這類高維非線性系統(tǒng)的濾波過程中可能出現(xiàn)協(xié)方差非正定，導(dǎo)致濾波數(shù)值不穩(wěn)定且有可能發(fā)散[20]。PF算法基于隨機(jī)采樣點進(jìn)行估計，選取的粒子數(shù)越多估計精度越高，但計算量也隨之大量增加，不利于在線估計。2009年Arasaratnam等人提出了容積卡爾曼濾波(Cubature Kalman Filter，CKF)算法[21]，由于其設(shè)計簡潔，數(shù)學(xué)理論嚴(yán)密，估計性能好，一經(jīng)提出便受到眾多學(xué)者關(guān)注[22-24]。CKF在高維非線性系統(tǒng)中能有效避免出現(xiàn)UKF的濾波發(fā)散現(xiàn)象和PF計算量大的問題，適用于星間測量自主導(dǎo)航系統(tǒng)。文獻(xiàn)[25]對UKF算法和CKF算法進(jìn)行了比較，指出當(dāng)系統(tǒng)狀態(tài)維數(shù)大于3維時，CKF算法的估計精度和數(shù)值穩(wěn)定性都優(yōu)于UKF。但是CKF算法對非線性系統(tǒng)估計時舍去了部分近似化誤差，會造成濾波不滿足擬一致性[26]，從而無法對狀態(tài)真值進(jìn)行準(zhǔn)確估計。文獻(xiàn)[27-28]針對系統(tǒng)模型參數(shù)不準(zhǔn)確或噪聲統(tǒng)計特性未知的情況，提出自適應(yīng)CKF算法和自適應(yīng)強(qiáng)跟蹤C(jī)KF算法，并在目標(biāo)跟蹤系統(tǒng)中驗證了其有效性，但不適用于系統(tǒng)可觀性較差的情況。由于不滿足擬一致性且在可觀性較差的區(qū)段估計性能較差，CKF算法應(yīng)用于星間測量自主導(dǎo)航系統(tǒng)時仍需進(jìn)行在線修正和改進(jìn)以提高性能。

本文在上述研究的基礎(chǔ)上，針對基于星間相對位置矢量觀測的分布式衛(wèi)星自主導(dǎo)航系統(tǒng)，提出一種改進(jìn)容積卡爾曼濾波算法。算法基于系統(tǒng)可觀度大小對濾波增益陣進(jìn)行在線調(diào)整，使系統(tǒng)在可觀性較差的區(qū)段濾波精度得到提升，同時滿足擬一致性[29]，從而能夠通過在線估計給出實時導(dǎo)航精度范圍。數(shù)學(xué)仿真表明該算法具有較高的精度和較快的趨穩(wěn)速度。

1 系統(tǒng)模型

本文考慮的衛(wèi)星系統(tǒng)由兩顆能夠進(jìn)行相對觀測的衛(wèi)星A和B組成，測量關(guān)系如圖1所示。

用下標(biāo)或上標(biāo)A和B分別表示衛(wèi)星A和衛(wèi)星B對應(yīng)的量，O是地球質(zhì)心，rA和rB分別是地心到兩衛(wèi)星的矢量，rAB是衛(wèi)星A到衛(wèi)星B的位置矢量。已知衛(wèi)星A搭載了星敏感器和無線電測距儀，可以測量自身在慣性空間中的姿態(tài)，以及AB間的相對距離和B星在A星本體坐標(biāo)系中的方位角，通過坐標(biāo)轉(zhuǎn)換可以得到兩衛(wèi)星在慣性空間中的相對位置矢量。該導(dǎo)航系統(tǒng)的目標(biāo)是在僅有星間位置矢量測量的條件下，實時確定衛(wèi)星A和衛(wèi)星B在慣性空間中的位置和速度。

圖1 星間相對位置矢量測量示意圖Fig.1 Relative position vector measurement between satellites

選取地心赤道慣性坐標(biāo)系，坐標(biāo)原點為地球質(zhì)心，基準(zhǔn)面為赤道面，X軸指向春分點，Z軸指向北極，Y軸與X軸和Z軸構(gòu)成右手直角坐標(biāo)系?？紤]J2攝動力作用下單個衛(wèi)星的軌道動力學(xué)方程[30]

(1)

(2)

衛(wèi)星的位置速度矢量與軌道動力學(xué)方程直接對應(yīng)，一般選取兩個衛(wèi)星的位置和速度矢量作為雙星系統(tǒng)的狀態(tài)變量，即

X為12維狀態(tài)變量。二體軌道下的系統(tǒng)狀態(tài)方程可以簡化為

(3)

式中：

系統(tǒng)觀測量為慣性系下兩衛(wèi)星的相對位置矢量，觀測方程為

Y=h(X)+v=rA-rB+v=HX+v

(4)

2 容積卡爾曼濾波

2.1 三階球面-徑向積分規(guī)則[21]

在n維實數(shù)空間Rn中，考慮如下向量函數(shù)積分

(5)

已知式(5)可以通過積分變換轉(zhuǎn)化為

式中：x=ry,r=|x|，y為單位矢量，Un為n維單位球面，σ(·)為Un上的元素。應(yīng)用球面容積規(guī)則和一階高斯-拉蓋爾積分規(guī)則，選取容積點為單位球面與各坐標(biāo)軸的交點，即點集{1}，得到式(5)的近似積分

(6)

通過以上步驟得到了非線性高斯濾波積分公式的一種近似。

2.2 容積卡爾曼濾波算法步驟

考慮如下所示的離散非線性高斯系統(tǒng)：

(7)

CKF的具體步驟為：

1)時間更新

①假設(shè)在k-1時刻，后驗概率密度是已知的，做矩陣的平方根分解：

(8)

②計算容積點集(i=1,2,…,2n)：

(9)

式(9)中由下式確定

(10)

式中：{1}i表示全對稱容積點集{1}中的第i個點。

③計算傳導(dǎo)容積點集(i=1,2,…,2n)：

(11)

④估計狀態(tài)向量的一步預(yù)測值：

(12)

⑤估計誤差協(xié)方差一步預(yù)測值：

(13)

2)量測更新

①做矩陣的平方根分解：

(14)

②計算容積點集(i=1,2,…,2n)：

(15)

式中：ξi由式(10)給出。

③計算傳導(dǎo)容積點集(i=1,2,…,2n)：

Zi,k|k-1=h(Xi,k|k-1)

(16)

④估計測量的一步預(yù)測值：

(17)

⑤估計新息協(xié)方差矩陣：

(18)

⑥估計交叉協(xié)方差矩陣的一步預(yù)測值：

(19)

⑦估計卡爾曼增益矩陣：

(20)

⑧估計當(dāng)前時刻的狀態(tài)向量：

(21)

⑨估計當(dāng)前時刻的誤差協(xié)方差矩陣：

(22)

3 基于可觀度的改進(jìn)容積卡爾曼濾波

CKF是非線性高斯濾波遞推算法的一種近似方法，依據(jù)最小方差理論推導(dǎo)而來。然而與EKF、UKF一樣，CKF是次優(yōu)估計，在近似過程中存在誤差，不能嚴(yán)格保證估計結(jié)果的無偏性；算法中計算出的均方誤差陣PCKF與估計結(jié)果的實際均方誤差陣Pture之間不存在確定的大小關(guān)系。由于在實際系統(tǒng)中，估計誤差的真值無法得到，希望得到一種改進(jìn)的容積卡爾曼濾波算法，使PCKF能夠給出Pture的一個上界，即具有擬一致性[24]。擬一致算法計算得到的均方誤差陣大于或等于實際的均方誤差陣，便于對誤差真值進(jìn)行準(zhǔn)確估計。

可觀度是可觀性的度量，表征系統(tǒng)狀態(tài)可由輸出反映的程度。在非線性系統(tǒng)中，系統(tǒng)的可觀度大小不是固定的，會隨著狀態(tài)變化而改變，從而對濾波算法的估計精度產(chǎn)生一定影響。可觀度較大的時刻，系統(tǒng)由觀測反推狀態(tài)的能力較強(qiáng)，估計誤差較?。环粗?，可觀度較小的時刻，由觀測反推狀態(tài)的能力較弱，估計誤差較大。

目前常用的可觀度評價準(zhǔn)則有基于可觀性矩陣的評價準(zhǔn)則和基于濾波誤差協(xié)方差陣P的評價準(zhǔn)則?；诳捎^性矩陣的評價準(zhǔn)則需要計算可觀性矩陣的奇異值，求解繁瑣，要基于狀態(tài)真值進(jìn)行計算，不具備實際操作性，只能作為離線分析參考；基于濾波誤差協(xié)方差陣P的評價準(zhǔn)則不需額外求解可觀性矩陣，計算量小，但是在非線性濾波中，P陣不能代表真實的估計誤差協(xié)方差，所以此準(zhǔn)則不能真實反映系統(tǒng)可觀度的情況。

從可觀度的物理意義出發(fā)，基于濾波算法量測更新過程給出非線性系統(tǒng)可觀度的另外一種表征方式。

(23)

矩陣的Frobenius范數(shù)定義為：若A=(aij)∈Rm×n

規(guī)定

(24)

已知Frobenius范數(shù)具有如下性質(zhì)：

式中：λi(ATA)為矩陣ATA的第i個特征值。

依據(jù)以上分析，在濾波的時間更新過程中對一步預(yù)測估計誤差協(xié)方差陣的計算公式進(jìn)行修改，引入一個縮放參數(shù)ρ，將(13)式改寫為

(25)

式中：縮放參數(shù)ρ定義如下：

γ為大于0的常數(shù)。

由此，依據(jù)當(dāng)前時刻系統(tǒng)可觀度大小對誤差協(xié)方差陣進(jìn)行修正，在適當(dāng)擴(kuò)大PCKF的基礎(chǔ)上兼顧可觀度影響，克服了CKF的PCKF陣無法反映真實誤差協(xié)方差的弊端，減小了系統(tǒng)可觀度較低區(qū)段的估計誤差，使濾波更加平穩(wěn)，提高了濾波精度。由式(8)～式(12)，式(25)，式(14)～(22)構(gòu)成的算法本文稱之為改進(jìn)的容積卡爾曼濾波算法(Modified Cubature Kalman Filter, MCKF)。

4 仿真結(jié)果

基于前面給出的系統(tǒng)模型，選擇一個低軌近距離雙星編隊，初始軌道根數(shù)如表1所示。將初始軌道根數(shù)轉(zhuǎn)化為地心赤道慣性系下的位置速度向量，如表2所示。

定義兩衛(wèi)星相對距離為rAB=|rAB|，以表2的初始條件帶入式(1)的動力學(xué)模型進(jìn)行仿真，得到兩衛(wèi)星3個軌道周期內(nèi)的相對距離如圖2所示?？梢钥闯?，兩個衛(wèi)星的相對距離介于35 km和70 km之間。由于衛(wèi)星B的偏心率非零，所以兩星瞬時地心距在一個軌道周期內(nèi)有兩個時刻相等?；谖墨I(xiàn)[1]和[9]中結(jié)果，在這兩個時刻及其鄰域中系統(tǒng)的可觀性較弱。

假設(shè)衛(wèi)星A搭載了星敏感器和無線電測距儀，可以獲得A在慣性空間中姿態(tài)，同時可以測量AB間距和B星在A星本體坐標(biāo)系下的方位角，則相對位置矢量在慣性空間中的坐標(biāo)可由式(26)解得。

表1 兩衛(wèi)星初始軌道根數(shù)Table 1 Initial orbital parameters of two satellites

表2 初始狀態(tài)向量Table 2 Initial state vectors

圖2 兩星相對距離Fig.2 Distance between two satellites

(26)

設(shè)定初始位置誤差為100 km，初始速度誤差為10 m/s。采用蒙特卡洛(Monte Carlo)方法對以上自主導(dǎo)航系統(tǒng)進(jìn)行500次獨立仿真，分別用CKF算法和MCKF算法對兩衛(wèi)星在慣性空間中的位置和速度進(jìn)行估計，并對估計結(jié)果進(jìn)行比對。

圖3 CKF的位置速度濾波結(jié)果(衛(wèi)星A)Fig.3 RMS errors of position and velocity using CKF(satA)

圖4 CKF的位置速度濾波結(jié)果(衛(wèi)星B)Fig.4 RMS errors of position and velocity using CKF(satB)

進(jìn)一步，對相同初始條件下CKF和MCKF的仿真結(jié)果進(jìn)行橫向比較，衛(wèi)星A的結(jié)果如圖7所示，衛(wèi)星B的結(jié)果如圖8所示，圖中實線表示CKF濾波結(jié)果，虛線表示MCKF濾波結(jié)果?？梢钥闯?，MCKF在1000 s左右趨于穩(wěn)定，而CKF需要1800 s左右，MCKF比CKF收斂速度更快。

定義位置和速度估計誤差分別為

圖5 MCKF的位置速度濾波結(jié)果(衛(wèi)星A)Fig.5 RMS errors of position and velocity using MCKF(sat A)

圖6 MCKF的位置速度濾波結(jié)果(衛(wèi)星B)Fig.6 RMS errors of position and velocity using MCKF(sat B)

圖7 MCKF和CKF的濾波結(jié)果對比(衛(wèi)星A)Fig.7 comparison of RMS errors between MCKF and CKF(sat A)

定義濾波精度為到達(dá)穩(wěn)態(tài)后，位置速度估計誤差的均值加上其三倍均方根(3σ)。將兩種算法下的濾波精度進(jìn)行對比，如表3所示。不難看出，MCKF的濾波精度高于CKF。

5 結(jié) 論

在分布式衛(wèi)星技術(shù)背景下，研究僅利用星間相對位置測量的自主導(dǎo)航濾波問題。針對CKF算法的不足，提出一種MCKF算法，選擇系統(tǒng)可觀度作為標(biāo)準(zhǔn)對CKF算法的一步預(yù)測協(xié)方差陣進(jìn)行修正，對可觀度變化且部分時刻可觀性較差的系統(tǒng)估計性能良好，具有擬一致性。針對低軌近距離雙星任務(wù)系統(tǒng)開展數(shù)學(xué)仿真，相同條件下的濾波結(jié)果表明MCKF算法明顯優(yōu)于CKF算法，其收斂速度更快，精度更高。此外，該算法具有很好的移植性和普適性，無需修改即可適用于一般非線性系統(tǒng)。進(jìn)一步的研究工作為MCKF算法的參數(shù)選擇與穩(wěn)定性的關(guān)聯(lián)分析。

表3 MCFK和CKF濾波精度對比Table 3 Accuracy of MCKF and CKF

圖8 MCKF和CKF的濾波結(jié)果對比(衛(wèi)星B)Fig.8 Comparison of RMS errors between MCKF and CKF(sat B)