寧曉琳，李卓，黃盼盼，楊雨青，劉剛，房建成

（1. 北京航空航天大學(xué)　儀器與光電工程學(xué)院，北京 100191；2. 新南威爾士大學(xué)　土木與環(huán)境工程學(xué)院，悉尼 2052）

火星探測器捕獲段自適應(yīng)卡爾曼濾波方法

寧曉琳1，李卓1，黃盼盼2，楊雨青1，劉剛1，房建成1

（1. 北京航空航天大學(xué)儀器與光電工程學(xué)院，北京 100191；2. 新南威爾士大學(xué)土木與環(huán)境工程學(xué)院，悉尼 2052）

天文導(dǎo)航是一種廣泛應(yīng)用于深空探測任務(wù)中的節(jié)能、高效的導(dǎo)航方式。基于軌道動力學(xué)模型和星光角距的卡爾曼濾波方法已經(jīng)被成功應(yīng)用在天文導(dǎo)航系統(tǒng)中。在捕獲段由于探測器所處動力學(xué)環(huán)境復(fù)雜，未建模的加速度誤差，星歷誤差等都會造成過程噪聲統(tǒng)計特性不完全。針對以上問題，提出一種根據(jù)新息和殘差序列的變化趨勢來調(diào)節(jié)過程噪聲協(xié)方差陣的自適應(yīng)平方根容積卡爾曼的方法（AQSCKF）。該方法先分別利用新息和殘差計算調(diào)節(jié)因子，然后判斷新息和殘差的變化趨勢，當(dāng)新息和殘差的變化趨勢一致時，取二者調(diào)節(jié)因子的均值作為過程噪聲方差陣的調(diào)節(jié)因子，對其進行調(diào)節(jié)。此外，本文還將該方法與傳統(tǒng)的只利用新息或殘差在線調(diào)節(jié)協(xié)方差陣的平方根容積卡爾曼濾波（SCKF）方法進行對比，仿真結(jié)果表明，在解決由于過程噪聲統(tǒng)計特性不能完全已知的問題上，AQSCKF算法不僅能顯著提高導(dǎo)航精度，并且具有很好的穩(wěn)定性。

自適應(yīng)濾波；天文導(dǎo)航；過程噪聲；卡爾曼濾波

引用格式：寧曉琳，李卓，黃盼盼，等. 火星探測器捕獲段自適應(yīng)卡爾曼濾波方法［J］. 深空探測學(xué)報，2016，3（3）：237-245.

Reference format: Ning X L，Li Z，Huang P P，et al. An adaptive Kalman filter for Mars spacecraft acquisition phase ［J］. Journal of Deep Space Exploration，2016，3（3）：237-245.

0　引　言

天文導(dǎo)航方法作為一種自主的導(dǎo)航方式，已經(jīng)被廣泛應(yīng)用在許多深空探測任務(wù)中。例如2001年“深空1號”（Deep Space 1）成功飛躍Borrelly彗星［1］，同年“星塵號”成功飛躍wild2彗星［2］，以及2005年“深度撞擊號”飛躍Temple1［3］。天文導(dǎo)航的基本原理是利用探測器上安裝的天體敏感器觀測天體的方位信息，通過星歷表獲得天體在慣性空間中任意時刻的位置，從而確定探測器在該時刻的位置和姿態(tài)信息。在天文導(dǎo)航系統(tǒng)中，由于過程噪聲和量測噪聲的存在，通常采用最小二乘法［4］或基于軌道動力學(xué)方程的卡爾曼濾波方法來減小噪聲對導(dǎo)航結(jié)果的影響。最小二乘法是天文導(dǎo)航系統(tǒng)常用的一種濾波方法，該方法主要是使得量測量和由狀態(tài)估計所確定的量測估計的差值的平方和達到最?。?］?？柭鼮V波算法是一種遞推的估計算法，該方法是基于最小方差估計的最優(yōu)估計方法［5］。在天文導(dǎo)航系統(tǒng)中，由于系統(tǒng)狀態(tài)和量測方程的非線性特性，經(jīng)常使用非線性濾波方法來獲得較高的導(dǎo)航精度。目前常用的非線性濾波方法主要有擴展卡爾曼濾波（EKF）［6］、無跡卡爾曼濾波（UKF）［7］、容積點卡爾曼濾波（CKF）［8］和平方根容積卡爾曼濾波（SCKF）［9］。其中UKF、CKF以及SCKF是利用采樣點來獲取狀態(tài)均值和協(xié)方差的濾波方法。與EKF相比，UKF、CKF和SCKF有更好的濾波性能［10-11］。本文采用SCKF來進行仿真分析，與CKF相比，該方法不僅降低了計算復(fù)雜度，可以獲得更高的濾波效果，并且能有效地避免濾波器的發(fā)散，提高濾波的收斂速度和數(shù)值穩(wěn)定性［12］。同EKF、UKF和CKF一樣，SCKF依賴于過程噪聲和量測噪聲的先驗信息，但由于捕獲段探測器所處的動力學(xué)環(huán)境復(fù)雜，未建模加速度，天體星歷誤差的存在等都使得無法得到準(zhǔn)確的過程噪聲統(tǒng)計特性［13］。因此，在初始時刻設(shè)置濾波初值時，很難獲得準(zhǔn)確的過程噪聲協(xié)方差陣。

針對捕獲段探測器天文導(dǎo)航過程噪聲統(tǒng)計特性難以精確確定的問題，許多學(xué)者已經(jīng)進行了研究。目前主要是通過自適應(yīng)濾波的方法來解決該問題，常用的3種自適應(yīng)濾波方法主要有：在線估計法、多模型估計法（MME）以及在線調(diào)節(jié)法。在線估計的方法是基于貝葉斯估計、最大似然估計、相關(guān)法和協(xié)方差匹配等法則直接對過程噪聲協(xié)方差陣進行估計，該方法雖然計算較為簡單，但是濾波精度有限［10-11，13-15］。多模型估計方法是指一些具有不同模型參數(shù)的濾波器同時進行濾波，并且通過對每個濾波器的濾波結(jié)果進行融合最后得到最優(yōu)的濾波估計。該方法雖然在導(dǎo)航精度上有了一定的提高，但是由于需要設(shè)置多組模型參數(shù)逼近真實系統(tǒng)，故其計算較為復(fù)雜［16-17］。在線調(diào)節(jié)的方法引入調(diào)節(jié)因子在線調(diào)節(jié)噪聲方差陣，其中調(diào)節(jié)因子的計算方法一般有兩種：一種是基于新息的計算方式，另一種是基于殘差的計算方式。以上兩種方法均是通過計算新息或殘差的估計誤差方差陣與其對應(yīng)理論誤差方差陣的比值得到。在火星探測器捕獲段，由于過程噪聲和量測噪聲變化是不確定的，而新息受量測噪聲的影響較大，殘差序列又受到前一時刻的估計值的影響，因此，單獨使用新息序列或是殘差序列來調(diào)節(jié)過程噪聲方差陣的方法經(jīng)常會導(dǎo)致過度調(diào)節(jié)或者是估計精度不高。為了增加噪聲方差陣的穩(wěn)定性，本文提出了一種基于容積卡爾曼濾波算法的新的自適應(yīng)過程噪聲方差陣的濾波方法（AQSCKF）。該方法中，調(diào)節(jié)因子的值是通過同時判斷新息序列和殘差序列的變化趨勢，進而在二者變化趨勢一致時，取二者的均值而得到。此外，本文還通過仿真試驗將該算法與傳統(tǒng)的SCKF——只利用新息或殘差的在線調(diào)節(jié)方法進行了對比，仿真結(jié)果表明AQSCKF可以解決因噪聲統(tǒng)計特性不確定所引起的導(dǎo)航精度下降的問題，并且具有很高的穩(wěn)定性。

1　火星探測器天文導(dǎo)航系統(tǒng)模型

捕獲段火星探測器天文導(dǎo)航系統(tǒng)中，系統(tǒng)模型為由軌道動力學(xué)模型作為狀態(tài)模型，恒星星光角距作為量測模型，進而通過濾波算法進行最優(yōu)估計得到探測器的位置、速度、姿態(tài)等信息。

1.1狀態(tài)模型

當(dāng)火星探測器處于捕獲段時，其軌道動力學(xué)模型可以看作圓形限制性四體問題，該方法經(jīng)常需要考慮在太陽和附近天體中心引力對探測器的作用。則探測器在日心慣性系坐標(biāo)系下的軌道動力學(xué)模型可以表示為

1.2量測模型

火星探測捕獲段的天文導(dǎo)航方法通常是以火星及其衛(wèi)星與恒星之間的星光角距作為量測量?；鹦?、火衛(wèi)一、火衛(wèi)二和恒星之間的星光角距的位置關(guān)系如圖1所示。

圖1　恒星與導(dǎo)航天體間的星光角距Fig.1　Star angles between the stars and the navigation celestial body

2　AQSCKF算法

本小節(jié)中首先給出SCKF算法的簡要算法流程，然后給出AQSCKF算法的具體算法流程。

2.1SCKF算法

SCKF是在CKF的基礎(chǔ)上，基于平方根濾波的思想提出的一種新的濾波方法。與CKF相比，SCKF不僅降低了計算的復(fù)雜度，而且提高了濾波效率，提高了濾波的收斂速度［12］。

1）時間更新

容積點計算：

通過非線性狀態(tài)方程傳播的容積點：

狀態(tài)估計預(yù)測值和一步預(yù)測誤差方差陣的平方根計算：

其中：Sk|k-1表示一步預(yù)測誤差方差陣的平方根；表示矩陣的QR分解。

2）量測更新

容積點計算：

通過非線性量測方程傳播后的容積點：

量測預(yù)測值，預(yù)測誤差方差陣平方根和互協(xié)方差陣的計算：

其中：Syy表示預(yù)測誤差方差陣的平方根；為量測誤差方差陣。

其中：Pxy表示互協(xié)方差陣。

增益陣、狀態(tài)估計值和狀態(tài)估計誤差方差陣的計算

2.2傳統(tǒng)調(diào)節(jié)方法

利用SCKF算法對系統(tǒng)進行濾波時，要求噪聲統(tǒng)計特性準(zhǔn)確已知，并在濾波過程中保持不變，但在火星探測器捕獲段天文導(dǎo)航系統(tǒng)中，過程噪聲的統(tǒng)計特性是部分已知，或完全未知的并且隨時間變化的。此時，若采用傳統(tǒng)的濾波方法將會導(dǎo)致濾波性能變差，甚至發(fā)散。故為提高濾波精度和穩(wěn)定性，并適應(yīng)過程噪聲統(tǒng)計特性時變的特性，應(yīng)采用自適應(yīng)Q陣的方法來提高Q陣的穩(wěn)定性和濾波精度。

通常情況下，自適應(yīng)調(diào)節(jié)Q陣是通過引入一個調(diào)節(jié)因子來對過程噪聲統(tǒng)計特性進行在線調(diào)節(jié)，調(diào)節(jié)過程如下：

文獻［20］給出了另外一種基于殘差序列的計算調(diào)節(jié)因子計算方式，具體過程如下：

2.3AQSCKF

2.3.1自適應(yīng)調(diào)節(jié)Q陣的條件

2.3.2調(diào)節(jié)因子Sk的計算

在AQSCKF算法中，只有在滿足調(diào)節(jié)Q陣的條件下，才對Q陣進行調(diào)節(jié)，具體調(diào)節(jié)過程如下：

圖2　AQSCKF算法計算流程圖Fig.2　Diagram of AQSCKF

AQSCKF的具體計算流程如圖2所示。

下面給出AQSCKF算法的詳細計算過程。

1）時間更新

2）量測更新

3）自適應(yīng)Q陣

3　仿真結(jié)果及分析

3.1仿真條件

本文所采用的標(biāo)準(zhǔn)軌跡為1997年NASA發(fā)射的Pathfinder，其軌道數(shù)據(jù)由STK直接產(chǎn)生［21］。仿真中所使用探測器的初始軌道參數(shù)表1所示。

表1　初始時刻軌道參數(shù)Table 1　The parameter of initial orbit

仿真中所用探測器，火星、火衛(wèi)一、火衛(wèi)二的位置信息也是從STK中利用DE421和SPICE星歷產(chǎn)生?；鹦?、火衛(wèi)一、火衛(wèi)二在日心黃道慣性坐標(biāo)系下的軌跡如圖3所示。

圖3　火星，火衛(wèi)一和火衛(wèi)二的標(biāo)準(zhǔn)軌跡Fig.3　The ideal trajectories of Mars，Phobos，Deimos and explorer

3.2仿真結(jié)果

3.2.1SCKF與AQSCKF的導(dǎo)航結(jié)果對比仿真過程中，設(shè)過程噪聲方差陣Q=q*diag。圖4和表2給出了在q分別為0.01，1和100時，SCKF和AQSCKF算法下的導(dǎo)航性能對比。

從仿真結(jié)果可以看出，q值大小對于SCKF的濾波結(jié)果有很大的影響。當(dāng)q取值太大時，由于濾波中設(shè)置的q值固定，此時認(rèn)為狀態(tài)模型精度差，狀態(tài)估計的精度主要依賴于量測量的精度，從而導(dǎo)致不能有效利用狀態(tài)模型對狀態(tài)量進行估計。反之，當(dāng)q取值太小時，濾波器此時濾波器認(rèn)為狀態(tài)模型可靠，從而減小對于量測信息的依賴，進而造成不能有效利用量測信息對狀態(tài)預(yù)測的估計進行修正。導(dǎo)致濾波精度不高，甚至發(fā)散。從表2 中的濾波結(jié)果可以看出，不論q值如何設(shè)定，AQSCKF算法的濾波結(jié)果總是穩(wěn)定在4～6 km的范圍內(nèi)。這是因為不同于SCKF算法，AQSCKF算法同時利用了新息和殘差的變化趨勢信息在濾波過程中實現(xiàn)了自適應(yīng)調(diào)節(jié)Q陣，使得當(dāng)預(yù)設(shè)的q值與實際Q陣不符時，該算法能自適應(yīng)調(diào)節(jié)Q值大小，直到Q值與實際值相符。

圖4　SCKF與AQSCKF導(dǎo)航性能對比Fig.4　The navigation performance of SCKF and AQSCKF

表2　SCKF與AQSCKF導(dǎo)航結(jié)果Table 2　Simulation result of SCKF and AQSCKF

3.2.2傳統(tǒng)自適應(yīng)濾波方法與AQSCKF的導(dǎo)航結(jié)果對比

傳統(tǒng)的自適應(yīng)調(diào)節(jié)Q陣是通過引入一個調(diào)節(jié)因子來對過程噪聲統(tǒng)計特性進行再線調(diào)節(jié)。其中調(diào)節(jié)因子的計算方法一般分為兩種：基于新息序列計算調(diào)節(jié)因子［19］和基于殘差序列計算調(diào)節(jié)因子［20］。本文中，式（24）給出了基于新息計算調(diào)節(jié)因子的方式，式（25）給出了基于殘差序列計算調(diào)節(jié)因子的方式。圖5和表3給出單獨利用基于新息和殘差自適應(yīng)調(diào)節(jié)Q陣的仿真結(jié)果，仿真中，估計窗口值M為200。

圖5　基于新息和殘差的自適應(yīng)濾波導(dǎo)航性能Fig.5　The navigation performance of adaptive filter based on innovation and residual

表3　基于新息和殘差的自適應(yīng)濾波與AQSCKF導(dǎo)航結(jié)果對比Table 3　The comparison of the adaptive filter based on innovation and residual and AQSCKF

從以上仿真結(jié)果可以看出，基于新息計算的調(diào)節(jié)因子的自適應(yīng)方法幾乎不受q值的影響，且其導(dǎo)航精度相對于傳統(tǒng)SCKF濾波算法已經(jīng)有了一定的提高。與此同時，基于殘差計算的調(diào)節(jié)因子的自適應(yīng)濾波算法相對于傳統(tǒng)的SCKF算法也有了一定的提高。但在導(dǎo)航精度方面，二者雖然在不同程度上提高了導(dǎo)航精度，但提高有限。相比之下，AQSCKF算法在精度提高方面要遠優(yōu)于單獨利用基于新息和殘差的方法。

4　結(jié)　論

傳統(tǒng)的利用新息或殘差序列計算調(diào)節(jié)因子進而自適應(yīng)調(diào)節(jié)過程噪聲方差陣的方法，雖然一定程度上可以提供濾波的精度，但精度提高有限。本文從同時利用新息和殘差信息來自適應(yīng)調(diào)節(jié)過程噪聲方差陣的角度出發(fā)，提出了另外一種同時基于新息序列和殘差序列的計算的方法，即通過判斷新息和殘差的變化趨勢來判斷是否對Q陣進行自適應(yīng)調(diào)節(jié)，當(dāng)新息和殘差的變化趨勢一致時，調(diào)節(jié)因子的值為單獨利用新息或殘差調(diào)節(jié)時的調(diào)節(jié)因子的均值，若二者變化趨勢不一致，則不對Q陣進行調(diào)節(jié)。仿真結(jié)果證明，本文研究的AQSCKF算法在深空探測器捕獲段自主天文導(dǎo)航中，比單獨利用新息或殘差進行調(diào)節(jié)所獲得的導(dǎo)航精度高。能夠滿足在深空探測器捕獲段的復(fù)雜環(huán)境下，過程噪聲不確定所帶來的導(dǎo)航精度下降的問題。

［1］Bhaskaran S. Navigation of the Deep Space 1 spacecraft at Borrelly［Z］. AIAA paper，2002，4815：5-8.

［2］Bhaskaran S. Optical navigation for the stardust wild 2 encounter［C］//18th International Symposium on Space Flight Dynamics. German：［s. n.］，2004.

［3］Frauenholz R B. Deep impact navigation system performance ［J］. Journal of Spacecraft and Rockets，2008，45（1）：39-56.

［4］Bhaskaran S. Orbit determination performance evaluation of the deep space 1 autonomous navigation system［C］//AIAA/AAS Space Flight Mechanics Meeting. Monterey：AIAA，1998.

［5］Christian J A，Lightsey E G. Integrated performance of an autonomous optical navigation system for space exploration ［J］. Navigation，2010，2009：1.

［6］Woodard M. Orbit Determination of spacecraft in Earth-Moon L1 and L2 Libration Point Orbits［C］//AAS/AIAA Astrodynamics Specialist Conference. ［s. l.］: AAS/AIAA，2011

［7］Julier S J，Uhlmann J K. Unscented filtering and nonlinear estimation［J］. Proceedings of the IEEE，2004，92（3）：401-422.

［8］Arasaratnam I，Haykin S. Cubature kalman filters ［J］. Automatic Control，IEEE Transactions on. 2009，54（6）：1254-1269.

［9］Ge Q B，Li W B，Wen C L. SCKF-STF-CN：a universal nonlinear filter for maneuver target tracking［J］. Journal of Zhejiang University SCIENCE C. 2011，12（8）：678-686.

［10］Brunke S，Campbell M. Square root sigma point filtering for realtime，nonlinear estimation ［J］. Journal of Guidance，Control，And Dynamics，2004，27（2）：314-317.

［11］Ning X. Analysis of filtering methods for satellite autonomous orbit determination using celestial and geomagnetic measurement［J］. Mathematical Problems in Engineering，2012，2012：1-6.

［12］Tang X，Wei J，Chen K. Square-root adaptive cubature Kalman filter with application to spacecraft attitude estimation. in Information Fusion（FUSION）［C］//2012 15th International Conference on.［S.l.］：IEEE，2012.

［13］Stastny N B，Geller D K. Autonomous optical navigation at Jupiter：a linear covariance analysis ［J］. Journal of Spacecraft and Rockets，2008，45（2）：290-298.

［14］Lyzhoft J. GPU-Based Optical Navigation and Terminal Guidance Simulation of a Hypervelocity Asteroid Intercept Vehicle［C］//AIAA Cuidance，Navigation and Control（GNC）Conference. Washington DC：AIAA，2013:19-22

［15］Ahn H S，Won C H. Fast alignment using rotation vector and adaptive Kalman filter［J］. IEEE Transactions on Aerospace and Electronic Systems，2006，42（1）：70-83.

［16］Yu M J. INS/GPS integration system using adaptive filter for estimating measurement noise variance ［J］. Aerospace and Electronic Systems，IEEE Transactions on，2012，48（2）：1786-1792.

［17］Hu C. Adaptive Kalman filtering for vehicle navigation ［J］. Positioning，2009，1（04）：0.

［18］Hide C，Moore T，Smith M. Adaptive Kalman filtering for low-cost INS/GPS［J］. The Journal of Navigation，2003，56（01）：143-152.

［19］Ding W. Improving adaptive Kalman estimation in GPS/INS integration ［J］. Journal of Navigation，2007，60（03）：517-529.

［20］Yang Y，Gao W. An optimal adaptive Kalman filter ［J］. Journal of Geodesy，2006，80（4）：177-183.

［21］Kallemeyn P. The Mars pathfinder navigation System ［J］. AIAA Paper，1996：96-3656.

通信地址：北京市海淀區(qū)學(xué)院路北京航空航天大學(xué)儀器科學(xué)與光電工程學(xué)院17系（100191）

電話：（010）82338820

E-mail：ningxiaolin@buaa.edu.cn

An Adaptive Kalman Filter for Mars Spacecraft Acquisition Phase

NING Xiaolin1，LI Zhuo1，HUANG Panpan2，YANG Yuqing1，LIU Gang1，F(xiàn)ANG Jiancheng1
（1. School of Instrumentation Science & Opto-electronics Engineering，BeiHang University （BUAA），Beijing 100191，China；2. School of Civil and Environmental Engineering，University of New South Wales，Sydney 2052，Australia）

Celestial navigation is an energy saving and efficient way of autonomous navigation for deep space probes. Kalman filter has been successfully applied in the Celestial navigation system. During the acquisition phase，due to the complex dynamic environment，unmolded acceleration error and the ephemeris error etc. may cause the statistics of process noise uncertainty. To overcome the problem，a method named AQSCKF based on the trend of the innovation sequences and residual sequences to scale the process noise covariance matrix is proposed in this paper. In the first place，it calculates the scale factor based on the innovation and residual separately. Then，by comparing the trend of the two factors，the scale factor of the new method is set as the average. In addition，the navigation performance of traditional SCKF，the method only using innovation or residual to scale Q and AQSCKF is also compared by simulation. The simulation results show that the new method yields better performance than the traditional methods in solving the problem caused by the uncertainty of the process noise，furthermore it also shows a good stability.

adaptive filter；celestial navigation；process noise；Kalman filter

V249.32+3

A

2095-7777（2016）03-0237-8

10.15982/j.issn.2095-7777.2016.03.007

寧曉琳（1979—），女，副教授，博士生導(dǎo)師。主要研究方向：自適應(yīng)濾波，航天器自主導(dǎo)航與控制技術(shù)，慣性導(dǎo)航，天文導(dǎo)航，視覺導(dǎo)航及組合導(dǎo)航，最優(yōu)估計與信息融合。

［責(zé)任編輯：楊曉燕］

2016-05-26；

2016-06-19

國家重點基礎(chǔ)研究發(fā)展計劃（2014CB744202）