郁 豐，華 冰，吳云華，康國華

（南京航空航天大學(xué) 航天學(xué)院，南京 210016）

基于自適應(yīng)卡爾曼濾波的簡化地磁定軌

郁 豐，華 冰，吳云華，康國華

（南京航空航天大學(xué) 航天學(xué)院，南京 210016）

地磁定軌對追求低成本、高功能密度比的微小衛(wèi)星具有較重要的價值，但是目前地磁定軌尚存在計(jì)算量大、易受各類誤差影響導(dǎo)致精度過低等不足。在分析軌道動力學(xué)方程誤差量級的基礎(chǔ)上，建立了簡化的狀態(tài)轉(zhuǎn)移矩陣，根據(jù)地磁模型分析了地磁強(qiáng)度隨著階次變高，梯度顯著變小的特點(diǎn)，提出了高階截斷的簡化地磁測量方程；將復(fù)雜的磁強(qiáng)計(jì)測量誤差近似建模成隨機(jī)游走形式，用多項(xiàng)式對磁強(qiáng)計(jì)誤差估計(jì)值進(jìn)行實(shí)時擬合去噪，并辨識出磁強(qiáng)計(jì)誤差的變化特征作為自適應(yīng)卡爾曼濾波器的調(diào)節(jié)依據(jù)，提高了弱可觀測地磁定軌的性能。數(shù)學(xué)仿真證明了簡化地磁定軌模型的有效性，自適應(yīng)濾波器能夠更精確地實(shí)現(xiàn)定軌計(jì)算，定位精度約為6 km，測速精度約為4 m/s。

自主導(dǎo)航；地磁定軌；國際地磁參考場；自適應(yīng)卡爾曼濾波器

隨著航天技術(shù)的飛速發(fā)展，微小衛(wèi)星自主定軌已經(jīng)成為衛(wèi)星軌道測量領(lǐng)域的發(fā)展方向。地磁導(dǎo)航是一種特別適合近地微小衛(wèi)星的中低精度定軌方法[1]。由于近地軌道上有豐富的地磁場資源，并且地磁場可以采用高斯球諧函數(shù)來描述，磁場的強(qiáng)度與方向能表示為衛(wèi)星位置的函數(shù)[2]，所以可用磁測的方法對衛(wèi)星進(jìn)行自主定軌。目前微小衛(wèi)星普遍安裝三軸磁強(qiáng)計(jì)作為星上的定姿敏感器，如果能夠?qū)⒋艔?qiáng)計(jì)復(fù)用作定軌傳感器，能進(jìn)一步滿足微小衛(wèi)星輕質(zhì)量、小功耗、高可靠性和低成本的要求。

自從美國Cornell大學(xué)的學(xué)者首先提出測量地球磁場來確定衛(wèi)星軌道的概念以來，已經(jīng)有較多文獻(xiàn)討論了地磁定軌技術(shù)。一般采用地磁場的?；蛘叽艌鍪噶?，并結(jié)合卡爾曼濾波技術(shù)實(shí)現(xiàn)了定軌計(jì)算，文獻(xiàn)[3]建立了地磁定軌的基本方法。為進(jìn)一步解決地磁定軌精度低的不足，文獻(xiàn)[4-6]提出了地磁場信息結(jié)合太陽或者星光矢量來提高定位精度的方法，仿真研究表明效果明顯；另一方面，地磁定軌精度低導(dǎo)致了 EKF在一階線性截斷時誤差較大，文獻(xiàn)[7-10]采用UKF/PF算法改善了定軌計(jì)算的性能。上述方法較少討論磁強(qiáng)計(jì)測量誤差等因素導(dǎo)致的定軌性能下降的問題，文獻(xiàn)[11]探討了在標(biāo)定磁強(qiáng)計(jì)零偏與比例因子誤差條件下的定軌方法。

本文分析了地磁模型高階項(xiàng)梯度顯著減小的特點(diǎn)，提出了地磁模型截斷高階項(xiàng)保留低階項(xiàng)來建立測量方程的方法，同時考慮到地磁定軌精度相對較低，并且卡爾曼濾波器能持續(xù)修正軌道參數(shù)，所以忽略了中心引力 1‰及以下的項(xiàng)，建立了簡化地磁定軌的數(shù)學(xué)模型，一定程度上降低了計(jì)算復(fù)雜度；另一方面，地磁定軌是一個弱可觀測性系統(tǒng)，磁強(qiáng)計(jì)測量誤差會導(dǎo)致較大定軌誤差，所以本文將誤差成分復(fù)雜的磁場測量誤差作為待估計(jì)項(xiàng)，并近似建模成隨機(jī)游走形式，然后在軌統(tǒng)計(jì)磁場測量誤差的變化率作為自適應(yīng)濾波器的調(diào)節(jié)依據(jù)，提高了在時變磁場測量誤差條件下的地磁定軌精度。

1 簡化地磁定軌模型

1.1 地磁定軌的狀態(tài)方程

采用衛(wèi)星軌道動力學(xué)方程作為地磁定軌的狀態(tài)方程，本文在地球固聯(lián)坐標(biāo)系中描述衛(wèi)星軌道動力學(xué)方程，從而簡化后續(xù)測量方程的建模，若用戶需要地心慣性系中的定軌參數(shù)，可通過地固系與慣性系之間的坐標(biāo)轉(zhuǎn)換得到。由于地磁定軌精度相對較低，所以在諸多的攝動力中僅考慮 J2攝動項(xiàng)，則軌道預(yù)報模型如下：

式中，μ為地球引力常數(shù)， J2為二階的帶諧項(xiàng)系數(shù)，Re為地球的參考半徑，ω為地球的自轉(zhuǎn)角速度，f為其余攝動力。

采用擴(kuò)展卡爾曼濾波器實(shí)現(xiàn)定軌計(jì)算需要對狀態(tài)方程線性化得到線性干擾方程，由于基于卡爾曼濾波器的定軌過程是采用觀測值不斷修正的過程，待估狀態(tài)量的改正值已經(jīng)較小，同時考慮到兩次修正間隔的定軌弧段很短，并且地磁定軌的精度相對較低，所以對狀態(tài)轉(zhuǎn)移矩陣進(jìn)行簡化。根據(jù)數(shù)量級估算，忽略近地軌道上不大于中心引力 1‰的項(xiàng)，在計(jì)算狀態(tài)轉(zhuǎn)移矩陣時的軌道動力學(xué)模型可以簡化成：

經(jīng)過泰勒級數(shù)展開并保留一階項(xiàng)，并對該方程離散化，得到離散型線性干擾方程式：

1.2 地磁定軌的測量方程

地磁場常采用國際參考地球磁場模型（IGRF）描述，磁位勢的函數(shù)為：

由于微小衛(wèi)星的姿態(tài)測量精度有限，所以采用磁場強(qiáng)度的模作為觀測值，這樣衛(wèi)星定軌計(jì)算不依賴衛(wèi)星的姿態(tài)，更方便星上使用，則測量方程有：

式中，ε為測量噪聲，磁強(qiáng)計(jì)該部分誤差并不大，例如某型衛(wèi)星用磁強(qiáng)計(jì)噪聲為數(shù)nT， Bb為磁強(qiáng)計(jì)測量的偏置誤差，該誤差包含磁強(qiáng)計(jì)三軸合成的標(biāo)定因子誤差、磁強(qiáng)計(jì)零偏、軟磁周期磁化導(dǎo)致的測量誤差等，由于這些誤差項(xiàng)很難在地面實(shí)驗(yàn)室條件下精確的測量，或者也并不是一成不變的，所以很難精確建模，但是這些誤差會受在軌地磁場變化或者磁強(qiáng)計(jì)的工作時間而緩慢改變，從而影響地磁定軌的精度。所以將 Bb擴(kuò)充到狀態(tài)方程中，并將其建模成隨機(jī)游動形式來近似：

線性化得到相應(yīng)的干擾方程為：

同樣對測量方程線性化，得到測量方程的線性干擾方程，則有：

對式(10)中的第 2個偏微分矩陣進(jìn)行研究分析表明，隨著磁場模型的階次越高，不但對應(yīng)的磁場強(qiáng)度越小，而且其位置梯度變化也越小，所以磁場模型的高階偏微分項(xiàng)對軌道精密修正的作用也越??；另一方面，利用磁場模型低階項(xiàng)估計(jì)出大部分軌道誤差后，剩余軌道誤差導(dǎo)致的磁場模型高階項(xiàng)對應(yīng)的磁場強(qiáng)度差異是可以近似忽略的。同時，磁場模型階次越高，該階的計(jì)算量也越大，所以為節(jié)約星上運(yùn)算量可以考慮該偏微分矩陣的計(jì)算僅考慮磁場模型低階項(xiàng)，則有：

式中，k為低階截斷階次。

2 自適應(yīng)卡爾曼濾波器的設(shè)計(jì)

根據(jù)第1節(jié)中的簡化地磁定軌模型，采用擴(kuò)展卡爾曼濾波器即可實(shí)現(xiàn)軌道估計(jì)。但是地磁場定軌是一個弱可觀測系統(tǒng)，所以當(dāng)磁強(qiáng)計(jì)存在測量誤差并且不能準(zhǔn)確在軌估計(jì)時，會導(dǎo)致定軌精度的下降。根據(jù)式(7)可知，磁強(qiáng)計(jì)誤差建模成隨機(jī)游走的形式是一定精度的近似，并且很難進(jìn)行高精度的建模，擬對 Bb的估計(jì)值進(jìn)行實(shí)時檢測與判斷，當(dāng) Bb偏離常值時適當(dāng)放大相應(yīng)的動態(tài)噪聲，從而更置信于測量值，得到對 Bb更準(zhǔn)確的估計(jì)，當(dāng) Bb更表現(xiàn)出常值特征時適當(dāng)減小相應(yīng)的動態(tài)噪聲，從而更置信于狀態(tài)預(yù)報值，通過這樣的自適應(yīng)方法的處理，能進(jìn)一步改善地磁定軌的精度。為消除 Bb的估計(jì)誤差對 Bb數(shù)據(jù)特征檢測與判別的影響，首先利用一系列進(jìn)行多項(xiàng)式擬合，

利用求得的擬合系數(shù)計(jì)算得到當(dāng)前的 Bb的擬合值，利用一系列的平滑后的后，前后做差判斷Bb的數(shù)據(jù)變化趨勢，

3 仿真與驗(yàn)證

首先開展地磁導(dǎo)航測量方程對應(yīng)的線性干擾方程采用低階地磁模型的可行性論證分析，設(shè)置兩條相近軌道，并讓其初始時刻在相同的位置，經(jīng)過一段時間飛行后，兩者位置差異約為14 km，如圖1中的橫坐標(biāo)所示。圖1中的1號曲線指的是地磁模型均取到4階時，兩地的地磁強(qiáng)度差異；圖1中的2號曲線指的是地磁模型的5～10階對應(yīng)的兩地磁場強(qiáng)度差異。由此可見，兩地的位置差異導(dǎo)致的地磁強(qiáng)度差異主要體現(xiàn)在低階項(xiàng)上，仿真表明隨著階次變高，磁場強(qiáng)度的梯度越小，所以求取測量線性干擾方程的觀測矩陣時可以僅考慮地磁模型的低階項(xiàng)，忽略掉計(jì)算更耗時、估計(jì)作用并不大的高階項(xiàng)，經(jīng)過卡爾曼濾波的序貫修正，即可估計(jì)出大部分定軌誤差。不同階數(shù)的地磁模型的位置梯度差異比較本文不再羅列。值得指出的是，地磁場高階項(xiàng)本身并不小，選取2條軌道中的1條，并計(jì)算10階與4階的地磁場強(qiáng)度差，達(dá)到數(shù)百至上千（如圖2所示），說明在測量方程線性化過程中計(jì)算地磁場強(qiáng)度的預(yù)報值 B?時是不能忽略地磁模型高階項(xiàng)的。

圖1 地磁強(qiáng)度差與位置差的關(guān)系Fig.1 Geomagnetic intensity difference and position error

圖2 某軌道的高階地磁強(qiáng)度(5～10)Fig.2 Geomagnetic intensity of high order

然后利用仿真來研究簡化模型與傳統(tǒng)模型的軌道測量的性能分析，在標(biāo)準(zhǔn)軌道的模擬中，攝動力利用量級為[1E-7，0.5E-7，0.8E-7] N/kg，以軌道周期為周期的正弦函數(shù)來近似模擬，對應(yīng)的濾波器Q值設(shè)置為1E-12，標(biāo)準(zhǔn)軌道初值為[6800 km 0 km 0 km 0 m/s 4000 m/s 6696 m/s]，濾波器初值為[6 800 018 m -2119 m 2800 m 0.1 m/s 3999.8 m/s 6696.1 m/s]，磁場真值采用IGRF10階模型，磁強(qiáng)計(jì)測量誤差包括300 nT的零偏，并認(rèn)為其為隨機(jī)游走的形式，其驅(qū)動噪聲為0.01 nT/s，濾波器中對應(yīng)的Q值設(shè)置為1 ，磁強(qiáng)計(jì)噪聲為5 nT(1σ)。將傳統(tǒng)方法與本文的簡化方法仿真并比較，結(jié)果如圖3、圖4所示。

由圖3、圖4可知，兩個方法在濾波器穩(wěn)定后的精度是相當(dāng)?shù)?，也證明了本文設(shè)計(jì)方法的正確性，在濾波收斂過程中簡化方法比傳統(tǒng)方法略差。綜合來說本文可以減少5～10階地磁模型的雅克比矩陣的計(jì)算，狀態(tài)轉(zhuǎn)移矩陣也更簡潔，減少了星上計(jì)算量并最終實(shí)現(xiàn)了類似精度。

圖3 簡化與傳統(tǒng)模型的定位誤差Fig.3 Positioning errors of simplified & traditional model

圖4 簡化與傳統(tǒng)模型的測速誤差Fig.4 Speed measuring errors of simplified & traditional model

最后分析論證自適應(yīng)卡爾曼濾波在地磁定軌估計(jì)的作用，在前述仿真條件的基礎(chǔ)上，進(jìn)一步設(shè)定環(huán)境磁場30000 nT時導(dǎo)致的軟磁磁化誤差為30 nT，并假定軟磁材料沒有飽和，磁強(qiáng)計(jì)標(biāo)定因子誤差導(dǎo)致的測量誤差如圖5所示。由圖5可見，仿真的磁強(qiáng)計(jì)誤差在環(huán)境磁場30 000 nT以內(nèi)則較小，25 000 nT以上時磁強(qiáng)計(jì)誤差逐漸顯著變大。設(shè)置3組濾波器，其中兩組濾波器是常規(guī)的卡爾曼濾波器，其中Q值分別設(shè)置為1和8兩個定值，對應(yīng)圖中的曲線分別為1和2，自適應(yīng)濾波器的多項(xiàng)式擬合采用線性擬合，待擬合數(shù)據(jù)系列的長度設(shè)置為 100，用于判斷數(shù)據(jù)變化趨勢的數(shù)據(jù)序列長度為20，為簡便和更好的說明效果，自適應(yīng)濾波中的Q值動態(tài)地選擇1和8兩種值，判斷閾值κ選為13，對應(yīng)的濾波曲線為3。

圖5 模擬的磁強(qiáng)計(jì)標(biāo)定因子誤差Fig.5 Simulated error of magnetometer calibration factor

圖6 自適應(yīng)濾波與常規(guī)濾波器的定位精度Fig.6 Positioning errors of adaptive filtering and EKF

圖7 自適應(yīng)濾波與常規(guī)濾波器的測速精度Fig.7 Speed measuring errors of adaptive filtering and EKF

從圖6、圖7中可以看出，當(dāng)Q較小時，濾波收斂過程中振蕩較大，最大定位誤差達(dá)18 km和16 m/s，而Q較大和自適應(yīng)濾波器在收斂過程中沒有出現(xiàn)大的振蕩過程；當(dāng)濾波器收斂穩(wěn)定后，Q較大時對應(yīng)的2號曲線的定位與測速誤差最大，1號曲線次之。而自適應(yīng)濾波器在整個定軌過程中，無論是收斂性能還是穩(wěn)態(tài)精度都表現(xiàn)最佳。最終的定位精度約為6 km，測速精度約為4 m/s。

圖 8表明，濾波器較好地跟蹤了磁強(qiáng)計(jì)誤差真值，采用線性擬合方式得到的平滑值也能較好地擬合估計(jì)曲線，當(dāng)磁強(qiáng)計(jì)誤差變化率較大時，通過自適應(yīng)調(diào)節(jié)能夠加速跟蹤誤差真值，而在磁強(qiáng)計(jì)誤差變化率較小時，磁強(qiáng)計(jì)誤差真值與平滑值基本重合，可見通過自適應(yīng)策略較好地估計(jì)了磁強(qiáng)計(jì)誤差，所以基于自適應(yīng)濾波的地磁定軌方法具有更好的精度。

圖8 磁強(qiáng)計(jì)誤差的真值、估計(jì)值與平滑值Fig.8 True, estimated & smoothed value of magnetometer error

4 結(jié) 論

針對地磁定軌的計(jì)算量大，易受磁強(qiáng)計(jì)測量誤差等因素的影響，本文在分析模型各項(xiàng)精度的基礎(chǔ)上，推導(dǎo)了簡化地磁定軌模型，然后采用多項(xiàng)式擬合實(shí)時去噪、辨識磁強(qiáng)計(jì)測量誤差的變化特征，并采用自適應(yīng)卡爾曼濾波實(shí)現(xiàn)了更高精度的地磁定軌。仿真證明了各項(xiàng)措施的有效性。

（References）：

[1] Abdelrahman M, Park S Y. Simultaneous spacecraft attitude and orbit estimation using magnetic field vector measurements[J]. Aerospace Science and Technology, 2011, 15: 653-669.

[2] 劉元元，王仕成，張金生，等. 最新國際地磁參考模型IGRF11研究[J]. 地震學(xué)報，2013，35(1)：125-134.

LIU Yuan-yuan, WANG Shi-cheng, ZHANG Jin-sheng, et al. Research on the eleventh generation IGRF[J]. Acta Seismologica Sinica, 2013, 35(1): 125-134.

[3] Psiaki M L, Huang Le-jin. Ground tests of magnetometerbased autonomous navigation for low-earth-orbiting spacecraft[J]. Journal of Guidance, Control and Dynamics, 1993, 16(1): 206-212.

[4] 謝祥華，張銳，張靜. 基于磁強(qiáng)計(jì)與太陽敏感器的衛(wèi)星自主定軌算法[J]. 宇航學(xué)報，2009，30(3)：919-923.

XIE Xiang-hua, ZHANG Rui, ZHANG Jing. Satellite autonomous orbit determination based on magnetometers [J]. Journal of Astronautics, 2009, 30(3): 919-923.

[5] 劉睿，王常虹，李葆華. 利用地磁/星光觀測角度的飛行器自主導(dǎo)航方法[J]. 紅外與激光工程，2011，40(2)：223-228.

LIU Rui, WANG Chang-hong, LI Bao-hua. Autonomous navigation method using the angle between geomagnetic and starlight vector[J]. Infrared and Laser Engineering, 2011, 40(2): 223-228

[6] Psiaki M L. Autonomous LEO orbit determination from magnetometer and sun sensor data[J]. Journal of Guidance, Control and Dynamics, 1999, 22(2): 296 -306.

[7] 王向磊，丁碩，蘇牧丹. EKF/UKF在基于地磁場的衛(wèi)星自主定軌中的應(yīng)用比較[J]. 測繪科學(xué)技術(shù)學(xué)報，2011，28(1)：50-53.

WANG Xiang-lei, DING Shuo, SU Mu-dan. Compare on the application of EKF/UKF in satellite autonomous orbit determination using geomagnetic field[J]. Journal of Geomatics Science and Technology, 2011, 28(1): 50-53.

[8] 王向磊，趙東明. UKF在基于地磁場的自主導(dǎo)航中的應(yīng)用分析[J]. 大地測量與地球動力學(xué)，2010，30(6)：144-149.

WANG Xiang-lei, ZHAO Dong-ming. On application of UKF in autonomous navigation based on geomagnetic field[J]. Journal of Geodesy and Geodynamics, 2010, 30 (6): 144-149.

[9] Wu Jin-jie, Liu Kun, Wei Jing-bo, et al. Particle filter using a new resampling approach applied to LEO satellite autonomous orbit determination with a magnetometer[J]. Acta Astronautica, 2012, 81: 512-522.

[10] Cheon Yee-Jin. Fast convergence of orbit determination using geomagnetic field measurement in target pointing satellite[J]. Aerospace science and technology, 2013, 30: 315-322.

[11] Juang Jyh-Ching, Tsai Yung-Fu, Tasi Chiu-Teng. Design and verification of a magnetometer-based orbit determination and sensor calibration algorithm[J]. Aerospace Science and Technology, 2012, 21: 47-54.

Simplified geomagnetic orbit determination based on adaptive Kalman filter

YU Feng, HUA Bing, WU Yun-hua, KANG Guo-hua
(College of Astronautics, Nanjing University of Aeronautics and Astronautics, Nanjing 210016, China)

Geomagnetic orbit determination is important for small satellites to realize low cost and high power/density ratio, but it has disadvantages such as large amount of calculation and insufficient precision due to various error sources. In this paper, a simplified state transition matrix is established based on the analysis of the orbital dynamics equation. In view that the geomagnetic gradient decreases significantly with higher order geomagnetic model, a simplified measurement equation with higher-order truncation is put forward. The complex magnetometer error is approximately modeled as random walk, and then a polynomial fitting method is employed to de-noise the magnetometer error estimate in real-time. The performance of the weak observable geomagnetic orbit determination is improved by an adaptive Kalman filter regulated by the magnetometer error variation characteristics which is identified from the de-noised estimate. Mathematical simulations verify the validity of the simplified geomagnetic orbit determination model, and the adaptive filter can more accurately achieve orbit calculation, in which the location and speed precisions are about 6 km and 4 m/s, respectively.

autonomous navigation; magnetic orbit determination; international geomagnetic reference field; adaptive Kalman filter

郁豐（1980—），男，副研究員，從事微小衛(wèi)星控制技術(shù)、衛(wèi)星導(dǎo)航研究。E-mail：yufeng@nuaa.edu.cn

1005-6734(2014)04-0519-06

10.13695/j.cnki.12-1222/o3.2014.04.018

V448.22+4

A

2014-04-15；

2014-07-30

國家自然科學(xué)基金（61203197，61203188）；中國博士后科學(xué)基金（2013M531352）