陳貴芳 郁 豐 王 潤

1.“空間光電探測(cè)與感知”工信部重點(diǎn)實(shí)驗(yàn)室，南京 210016 2.南京航空航天大學(xué)航天學(xué)院，南京 210016

0 引言

作為一種自主的、無長期積累誤差且具有較強(qiáng)抗干擾能力的導(dǎo)航技術(shù)[1]，地磁定軌技術(shù)適合對(duì)儀器復(fù)用率高的納衛(wèi)星使用。

當(dāng)前，地磁定軌多使用批處理算法和卡爾曼濾波算法進(jìn)行數(shù)據(jù)融合。前者經(jīng)多年發(fā)展，已有基于無跡變換的批處理算法(unscented batch filter, UBF)提出并應(yīng)用于地磁定軌中[2]。但批處理算法的數(shù)據(jù)需提前采集，不適合衛(wèi)星的實(shí)時(shí)定位。后者包含擴(kuò)展卡爾曼濾波器(extended Kalman filter，EKF)和無跡卡爾曼濾波器(unscented Kalman filter，UKF)。EKF引入泰勒近似實(shí)現(xiàn)了對(duì)非線性系統(tǒng)的狀態(tài)估計(jì)。但地磁模型復(fù)雜，求導(dǎo)困難。文獻(xiàn)[3-4]建立了多項(xiàng)式擬合的地磁模型，文獻(xiàn)[5]則在計(jì)算觀測(cè)矩陣時(shí)采用低階地磁模型，均減小了系統(tǒng)的計(jì)算復(fù)雜度，但對(duì)定位精度提升不多。由于沒有忽略高階項(xiàng)，UKF的精度通常高于EKF。通過引入地平儀[6]，太陽敏感器[7-8]等傳感器，基于UKF的地磁定軌系統(tǒng)的定位精度可達(dá)到千米級(jí)。此外，上述傳感器也是常用的姿態(tài)敏感器，文獻(xiàn)[9-10]利用衛(wèi)星的位置信息估計(jì)在慣性系下的地磁矢量信息和地心矢量(或太陽方向矢量)信息，再結(jié)合衛(wèi)星的姿態(tài)信息將其轉(zhuǎn)換到衛(wèi)星本體坐標(biāo)系下，和傳感器的輸出值進(jìn)行比較，實(shí)現(xiàn)了衛(wèi)星的位置和姿態(tài)一體化估計(jì)。文獻(xiàn)[11-12]研究了地磁場(chǎng)幅值和衛(wèi)星軌道根數(shù)在頻域上的關(guān)系，據(jù)此實(shí)現(xiàn)了衛(wèi)星半長軸的高精度估計(jì)。文獻(xiàn)[13]在此基礎(chǔ)上實(shí)現(xiàn)了碟狀飛行器的姿態(tài)估計(jì)。

定義磁強(qiáng)計(jì)誤差和外源磁場(chǎng)擾動(dòng)為地磁偏差，地磁定軌系統(tǒng)受其影響精度不佳。隨科技進(jìn)步，后者的影響逐漸減小[14]。文獻(xiàn)[15]建立了磁強(qiáng)計(jì)誤差和地理緯度間的函數(shù)，在衛(wèi)星工作時(shí)實(shí)時(shí)補(bǔ)償?shù)卮判畔?，減小了定位誤差。但磁強(qiáng)計(jì)誤差還會(huì)隨著時(shí)間而變化，因此該方法不適合衛(wèi)星長期使用。文獻(xiàn)[16]推導(dǎo)并建立了外源磁場(chǎng)的模型并將地磁場(chǎng)模型的系數(shù)作為狀態(tài)向量參與濾波，提升了地磁定軌的精度。但該方法計(jì)算量大，不適合納衛(wèi)星使用。文獻(xiàn)[17]將地磁主磁場(chǎng)模型中構(gòu)成磁耦極子模型的3個(gè)系數(shù)作為狀態(tài)向量參與濾波，仿真證明了該方法的可行性。外源磁場(chǎng)在磁暴時(shí)變化劇烈，文獻(xiàn)[18]對(duì)地磁偏差中的時(shí)變部分建模并改進(jìn)了EKF算法，減小了磁暴下地磁定軌系統(tǒng)的位置誤差，但對(duì)無磁暴影響時(shí)的系統(tǒng)性能提升不大。

本文以地磁場(chǎng)幅值作為觀測(cè)量，推導(dǎo)了自適應(yīng)卡爾曼濾波算法(adaptive Kalman filter，AKF)，并以此實(shí)現(xiàn)了納衛(wèi)星的自主導(dǎo)航。該算法將地磁偏差近似建模為隨機(jī)游走模型并作為狀態(tài)變量參與濾波。然后，依據(jù)新息的變化趨勢(shì)，實(shí)時(shí)調(diào)整濾波器的參數(shù)。以Swarm-A的觀測(cè)數(shù)據(jù)的實(shí)驗(yàn)表明，該算法有效提升了地磁偏差的估計(jì)精度，從而提升了系統(tǒng)的性能。和傳統(tǒng)濾波算法對(duì)比，AKF的收斂速度更快，其定位精度優(yōu)于EKF，和UKF相當(dāng)，最大定位誤差約6 km，但計(jì)算量卻遠(yuǎn)小于UKF，適合納衛(wèi)星等使用。

1 地磁定軌模型建立

1.1 狀態(tài)方程

以衛(wèi)星的軌道動(dòng)力學(xué)方程作為系統(tǒng)的狀態(tài)方程。為簡(jiǎn)化地磁模型的計(jì)算，該方程描述于地心地固坐標(biāo)系下。由于地磁定軌精度不高，本文只考慮了J2攝動(dòng)項(xiàng)。

(1)

式中：μ為地球引力常數(shù)，Re為地球的參考半徑，ω為地球的自轉(zhuǎn)角速度，J2為攝動(dòng)項(xiàng)系數(shù)，f=[fx,fy,fz]為攝動(dòng)力。

對(duì)該方程離散化，得到離散型干擾式：

δXk,k-1=φk,k-1δXk-1,k-1+Γk,k-1Wk-1

(2)

1.2 觀測(cè)方程

選取國際地磁參考場(chǎng)(international geomagnetic reference field，IGRF)作為地球主磁場(chǎng)模型，其磁位勢(shì)函數(shù)如下：

(3)

將磁位勢(shì)對(duì)北向、東向和徑向求導(dǎo)，則地磁場(chǎng)在北東地坐標(biāo)系下的分量可表示為：

(4)

選擇地磁場(chǎng)幅值作為觀測(cè)量，觀測(cè)方程為：

(5)

式中：Bb為地磁偏差，將其建模為隨機(jī)游走模型，并擴(kuò)充到狀態(tài)變量中，η為觀測(cè)噪聲。

(6)

式中：ab為地磁偏差的驅(qū)動(dòng)噪聲。

對(duì)觀測(cè)方程離散化，有：

δB=H·δX+η

(7)

(8)

式中：

地磁模型的高階導(dǎo)數(shù)對(duì)軌道修正能力弱[5]，為節(jié)約星上計(jì)算量，該偏微分陣僅考慮地磁模型的低階項(xiàng)：

(9)

式中：k為低階截?cái)嚯A次。

2 自適應(yīng)濾波算法推導(dǎo)

圖1是用Swarm-A衛(wèi)星在2015年4月19日至20日的實(shí)測(cè)地磁幅值數(shù)據(jù)和解算IGRF模型獲得的地磁幅值數(shù)據(jù)做差獲得的地磁偏差曲線。由圖1可知，地磁偏差隨時(shí)間和衛(wèi)星位置的變化而變化，因此常規(guī)卡爾曼濾波器的性能會(huì)受影響。本文提出了一種自適應(yīng)濾波算法，依據(jù)新息序列的變化趨勢(shì)，實(shí)時(shí)調(diào)節(jié)濾波器的協(xié)方差矩陣和狀態(tài)噪聲矩陣，可提升地磁偏差的估計(jì)精度，從而提升地磁導(dǎo)航的精度。

定義系統(tǒng)的新息如下：

(10)

式中：Zk為系統(tǒng)觀測(cè)量。

新息vk的協(xié)方差矩陣可由式(11)獲得[20]：

(11)

式中：Pk為系統(tǒng)的協(xié)方差矩陣，R為系統(tǒng)的噪聲矩陣。

圖1 地磁偏差

在系統(tǒng)協(xié)方差矩陣時(shí)間更新方程中引入變量S(S≥1)，有：

(12)

則，

(13)

其中，tr(O)表示求矩陣O的跡。

因S≥1，有：

(14)

新息vk的協(xié)方差矩陣可用實(shí)際測(cè)量的最新N個(gè)新息的協(xié)方差均值近似表示：

(15)

式中：M為估計(jì)窗大小，由經(jīng)驗(yàn)給定，i0=k-M。

結(jié)合式(14)和式(15)，有：

(16)

本文認(rèn)為，若式(16)的右式小于1，則S=1，否則，式(16)取等號(hào)。

綜上，AKF可表示為：

(17)

3 仿真校驗(yàn)

首先分析各濾波器的定位性能。圖2給出了常規(guī)濾波器和自適應(yīng)濾波器的位置誤差曲線。地磁場(chǎng)的可觀測(cè)性弱，濾波器收斂較慢。在0～20000 s間，各濾波器均有不同程度的超調(diào)，UKF最大，達(dá)到近44 km，AKF最小，僅17 km，并迅速減小到極小值。在系統(tǒng)運(yùn)行20000 s后，其最大位置誤差小于5 km時(shí)濾波器收斂。從收斂速度上看，AKF最快，需要約40000 s，而EKF和UKF分別需要50000 s和60000 s才能收斂。從定位精度上看，AKF和UKF的精度最高，而使用EKF的地磁定軌系統(tǒng)的位置誤差最大，約10 km。因此，AKF收斂速度最快，精度和UKF相當(dāng)，最大誤差約為6km。

接下來比較各濾波器的計(jì)算量。從內(nèi)存占用上看。UKF通過sigma點(diǎn)集來逼近非線性函數(shù)，每次迭代相較于EKF和自適應(yīng)算法需分別保存2n+1個(gè)狀態(tài)量和觀測(cè)量的sigma點(diǎn)集(n為狀態(tài)向量維數(shù))，內(nèi)存需求更大。從運(yùn)行速度上看。使用一臺(tái)系統(tǒng)為Windows 10，安裝有matlab 2019a的電腦分別以這3種濾波器對(duì)同一組數(shù)據(jù)進(jìn)行仿真，其仿真時(shí)間如表1?？梢姡琔KF的運(yùn)行速度遠(yuǎn)低于EKF和AKF。因此，EKF和AKF對(duì)計(jì)算資源的需求遠(yuǎn)低于UKF，更適合納衛(wèi)星使用。

圖2 地磁導(dǎo)航位置誤差

表1 各濾波器仿真時(shí)間

常規(guī)濾波器的位置誤差曲線有類周期特征。圖3給出了地磁定軌系統(tǒng)(EKF)在僅做時(shí)間更新時(shí)和正常工作時(shí)的位置誤差曲線。當(dāng)系統(tǒng)僅做時(shí)間更新時(shí)，曲線無類周期性，說明該波動(dòng)與狀態(tài)方程無關(guān)。引入量測(cè)修正后，系統(tǒng)的位置誤差減小，同時(shí)引入了類周期波動(dòng)。取120000～140000 s的位置誤差，在該區(qū)間內(nèi)出現(xiàn)了約3.5個(gè)震蕩，每次用時(shí)約95 min，和Swarm-A的周期相同[22]。由于Swarm-A為極軌衛(wèi)星，在不同周期，其在地磁場(chǎng)的位置不同。地磁場(chǎng)不同區(qū)域的可觀測(cè)性不同，因此在不同的衛(wèi)星周期內(nèi)，其定位性能不一，從而導(dǎo)致了該類周期性波動(dòng)的出現(xiàn)。

圖3 常規(guī)濾波(EKF)和僅時(shí)間更新的位置誤差曲線

最后分析各濾波器對(duì)地磁偏差的估計(jì)性能。由圖4，EKF和UKF的地磁偏差估計(jì)曲線近乎重合，說明UKF并不能提升地磁偏差的估計(jì)性能。圖5是濾波器的地磁偏差的估計(jì)誤差曲線，這里只比較EKF和AKF。當(dāng)系統(tǒng)開始工作時(shí)，AKF的地磁偏差的估計(jì)誤差迅速達(dá)到一個(gè)極小值，在圖2(a)表現(xiàn)為位置誤差的超調(diào)量遠(yuǎn)小于常規(guī)算法。在20000～40000 s之間，AKF的地磁偏差的誤差和EKF相近，在位置誤差中表現(xiàn)為二者的最大誤差接近。40000s后2個(gè)濾波器逐步收斂，此后AKF對(duì)地磁偏差的估計(jì)性能始終優(yōu)于EKF，其定位誤差也更小。因此，本文提出的自適應(yīng)濾波算法有效地提升了地磁偏差的估計(jì)性能，進(jìn)而提升了地磁定軌的性能。

圖4 常規(guī)濾波器的地磁偏差

圖5 地磁偏差的估計(jì)誤差

4 結(jié)論

對(duì)于地磁定軌易受地磁偏差影響的問題，針對(duì)性地將其建模為隨機(jī)游走模型并使之參與濾波。提出了自適應(yīng)濾波算法，有效克服了空間磁場(chǎng)擾動(dòng)和磁強(qiáng)計(jì)誤差的不確定性。實(shí)驗(yàn)表明，該算法計(jì)算量低，定位精度高，優(yōu)于傳統(tǒng)濾波算法，適合納衛(wèi)星使用。