星間相對(duì)測(cè)量自主導(dǎo)航的改進(jìn)容積卡爾曼濾波

張 艾，李 勇

(中國(guó)空間技術(shù)研究院錢(qián)學(xué)森空間技術(shù)實(shí)驗(yàn)室，北京 100094)

0 引 言

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

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

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

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

1 系統(tǒng)模型

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

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

圖1 星間相對(duì)位置矢量測(cè)量示意圖Fig.1 Relative position vector measurement between satellites

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

(1)

(2)

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

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

(3)

式中：

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

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

(4)

2 容積卡爾曼濾波

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

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

(5)

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

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

(6)

通過(guò)以上步驟得到了非線(xiàn)性高斯濾波積分公式的一種近似。

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

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

(7)

CKF的具體步驟為：

1)時(shí)間更新

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

(8)

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

(9)

式(9)中由下式確定

(10)

式中：{1}i表示全對(duì)稱(chēng)容積點(diǎn)集{1}中的第i個(gè)點(diǎn)。

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

(11)

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

(12)

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

(13)

2)量測(cè)更新

①做矩陣的平方根分解：

(14)

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

(15)

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

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

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

(16)

④估計(jì)測(cè)量的一步預(yù)測(cè)值：

(17)

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

(18)

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

(19)

⑦估計(jì)卡爾曼增益矩陣：

(20)

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

(21)

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

(22)

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

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

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

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

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

(23)

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

規(guī)定

(24)

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

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

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

(25)

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

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

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

4 仿真結(jié)果

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

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

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

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

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

圖2 兩星相對(duì)距離Fig.2 Distance between two satellites

(26)

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

圖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)一步，對(duì)相同初始條件下CKF和MCKF的仿真結(jié)果進(jìn)行橫向比較，衛(wèi)星A的結(jié)果如圖7所示，衛(wèi)星B的結(jié)果如圖8所示，圖中實(shí)線(xiàn)表示CKF濾波結(jié)果，虛線(xiàn)表示MCKF濾波結(jié)果。可以看出，MCKF在1000 s左右趨于穩(wěn)定，而CKF需要1800 s左右，MCKF比CKF收斂速度更快。

定義位置和速度估計(jì)誤差分別為

圖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é)果對(duì)比(衛(wèi)星A)Fig.7 comparison of RMS errors between MCKF and CKF(sat A)

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

5 結(jié) 論

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

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

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