深空探測器接近段自主導航的滑動窗口自適應濾波方法

張文佳， 馬 辛

(北京航空航天大學 儀器科學與光電工程學院，北京 100091)

深空探測器在著陸目標行星之前將經(jīng)歷巡航段、接近段、環(huán)繞軌道段.與相對穩(wěn)定的巡航和環(huán)繞軌道段相比，接近段至關重要，決定了航天器能否進入圍繞目標行星的預定軌道[1-2].導航系統(tǒng)作為深空探測任務的重要組成部分，直接影響深空探測器的運行狀態(tài)[3].一般來說，在接近段，導航系統(tǒng)的位置精度為1～10 km，而每次軌道確定所需的無線電測量和延遲時間為10～100 min.因此，深空探測器導航系統(tǒng)的精度和實時性是保證深空探測任務成功的兩個重要因素.

天文導航是深空中使用的主要自主導航方法之一，包括水手號和旅行者號在內的航天器都使用天文導航.深空探測器的自主導航系統(tǒng)一般由軌道動力學、量測模型和最優(yōu)狀態(tài)估計方法組成.濾波方法對導航系統(tǒng)的性能至關重要，它決定了參數(shù)估計的準確性和實時性.為獲得深空探測器的位置和速度，模型加速度通過數(shù)值積分器融合到濾波器中，通常采用具有固定步長的四階龍格庫塔積分法[4-5].

對于深空探測器而言，在接近階段探測器會遇到快速時變的誤差問題.這是目標星體的引力影響急劇增加，而其他天體的引力影響相對穩(wěn)定所導致的.由于深空探測器目標星體的引力快速變化，所以濾波器中的數(shù)值積分誤差會給導航性能帶來額外的不確定性，這被視為系統(tǒng)模型誤差，并且難以確定其統(tǒng)計特性.

對于無法準確估計噪聲的問題，很多學者進行了研究并提出3類解決方法[6-8]：基于協(xié)方差估計的濾波方法、方差匹配方法和多模型自適應方法.Fang等[6]提出一種基于新息的自適應擴展卡爾曼濾波方法，以提高全球定位系統(tǒng)的對準精度.Ning等[7]根據(jù)新息序列和殘差序列的變化，提出一種具有自適應測量噪聲協(xié)方差的無跡卡爾曼濾波器(Unscented Kalman Filter, UKF).Xiong等[9]提出一種多模型自適應估計方法，用于未知系統(tǒng)噪聲統(tǒng)計特性和測量不確定度的系統(tǒng).然而，這種方法很難防止濾波器發(fā)散，更換狀態(tài)模型會導致精度降低.Liu等[10]使用模糊自適應無跡卡爾曼濾波解決車輛慣性導航的問題.Gao等[11]提出基于強化學習的自適應卡爾曼濾波方法.Fraser等[12]提出基于殘差的模糊自適應擴展卡爾曼濾波方法.卞鴻巍等[13]使用基于新息的卡爾曼濾波算法提高了艦用組合系統(tǒng)的性能.Huang等[14]在慣性/衛(wèi)星組合導航中使用變分自適應卡爾曼濾波算法.Zhang等[15]使用多模態(tài)變分自適應卡爾曼濾波船舶組合導航系統(tǒng)的量測噪聲.這種方法設計的濾波系統(tǒng)復雜，計算量較大.此外，Zhang等[16]使用一種非線性優(yōu)化方法解決了復雜環(huán)境下移動機器人的自主導航問題.

針對深空探測器接近段無法準確估計狀態(tài)噪聲和現(xiàn)有濾波方法計算量大以及難以防止濾波器發(fā)散的問題，提出一種基于系統(tǒng)噪聲協(xié)方差調整的滑動窗口自適應無跡卡爾曼濾波方法(Sliding Window Adaptive Unscented Kalman Filter Based on System Noise Covariance Adjustment, AQUKF)，通過將前一時刻系統(tǒng)噪聲協(xié)方差與噪聲協(xié)方差的變化進行加權，提高導航系統(tǒng)性能.

1 問題描述

1.1 軌道動力學模型

深空探測器在接近目標行星時沿雙曲線軌道移動，其中心天體是目標行星，太陽和其他行星被認為是擾動.此外，太陽光壓輻射和推進器脈沖等因素也會影響探測器的運動.考慮以上因素，以火星為目標天體，火心慣性坐標系統(tǒng)中的軌道動力學方程可描述如下[7]：

(1)

設狀態(tài)矢量X=[xyzvxvyvz]T，f(·)為過程函數(shù)，W為過程噪聲，式(1)可寫為

(2)

1.2 星光角距量測模型

背景恒星和火星、火衛(wèi)一、火衛(wèi)二之間的星光角距可作為量測量[17].如圖1所示，本文使用行星敏感器得出恒星與目標行星之間的角度.圖中：θpi,θdi分別為第i顆恒星與火衛(wèi)一、火衛(wèi)二之間的角度；lpp,lpd分別為火衛(wèi)一、火衛(wèi)二到探測器的視線矢量；spi為第i顆恒星在火衛(wèi)一傳感器視場的矢量.

圖1 恒星和導航天體之間的角度Fig.1 Angle between the star and Mars

在火星探測器導航系統(tǒng)中，星光角距量測模型可以表示為

(3)

式中：i=1, 2, 3；θmi為第i顆恒星與火星之間的角度；lpm為火星到探測器的視線矢量；smi,sdi分別為第i顆恒星在火星、火衛(wèi)二傳感器視場的矢量；vm,vp,vd分別為第i顆恒星與火星、火衛(wèi)一、火衛(wèi)二之間的量測噪聲.

設量測矢量Z=[θmiθpiθdi]T，h(·)為量測模型函數(shù)，V為量測噪聲，式(3)可寫為

Z(t)=h(X(t),t)+V(t)

(4)

1.3 不確定性誤差及其傳遞特性

1.3.1積分誤差分析 無論何種數(shù)值積分方法，都是軌道動力學微分方程的近似解，因此會產(chǎn)生計算誤差.由于計算過程可以等效于前后兩步之間斜率的加權平均值與積分步長的乘積，所以斜率的加權平均值會產(chǎn)生較大的近似誤差(即非線性微分方程誤差的截斷).積分步長越小，近似誤差越小.因此，積分誤差主要受數(shù)值積分步長的影響.另外，由數(shù)值積分方法引起的攝動加速度誤差可以表示為

(5)

1.3.2積分誤差傳遞分析 軌道動力學模型積分誤差傳播模型可以表示為

(6)

式中：t1和t2分別為起、止時間.

在接近階段結束時，四階數(shù)值積分的誤差為

(7)

以中心天體重力加速度為例可得：

(8)

如式(6)所示，軌道動力學模型的積分誤差隨積分步長而變化.當軌道高度為距火星200 km、積分步長為250 s時，軌道動力學模型引起的加速度誤差約為 0.002 1 m/s2.剎車后，繼續(xù)飛行約10 min，速度誤差約為1.26 m/s，位置誤差為750 m.因此，積分誤差會影響導航精度，尤其是在接近階段.

當航天器遇到目標天體時，目標天體會導致航天器加速度急劇增加.如果積分時間步長較大，則無法快速跟蹤加速度的變化，會引入較大的積分誤差.因此，狀態(tài)模型的誤差在接近段急劇增加，過程噪聲在航天器接近目標時無法保持不變.為準確估計航天器的位置和速度，需要采用自適應非線性濾波估計過程噪聲協(xié)方差.一方面，為保證導航精度，需要減小狀態(tài)模型誤差；另一方面，為獲得更好的導航性能，應該跟蹤或估計狀態(tài)模型的變化.

2 自適應無跡卡爾曼濾波

2.1 傳統(tǒng)無跡卡爾曼濾波

非線性離散隨機系統(tǒng)在特定時刻k的狀態(tài)模型考慮如下：

(9)

式中：Xk和Zk分別為k時刻的狀態(tài)矢量和觀測矢量；wk和vk分別為k時刻的過程噪聲和量測噪聲.假設量測噪聲和過程噪聲是具有可加性的零均值高斯白噪聲且互不相關，則

(10)

式中：Qk和Rk分別為k時刻的過程噪聲協(xié)方差和量測噪聲協(xié)方差；當k=j時，克羅內克函數(shù)δkj取值為1，當k≠j時，δkj取值為0.

對于具有可加性噪聲的非線性系統(tǒng)式(9)，傳統(tǒng)的UKF計算過程如下.

步驟1設定濾波初值.當k=0，有

(11)

式中：P0為協(xié)方差矩陣P的初始值.

步驟2計算k-1時刻的Sigma點：

(12)

步驟3計算k時刻的一步預測值.首先根據(jù)非線性函數(shù)f(·)，計算Sigma點的變換結果：

(13)

然后計算出一步預測結果和對應的協(xié)方差矩陣：

(14)

Qk-1

(15)

步驟4計算k時刻的一步預測Sigma樣本點：

(16)

步驟5計算一步預測量測值及其自協(xié)方差矩陣、互協(xié)方差矩陣.根據(jù)非線性函數(shù)h(·)，量測Sigma樣本點的變換為

(17)

那么一步預測量測值為

(18)

自協(xié)方差矩陣為

(19)

與一步預測狀態(tài)量之間的協(xié)方差矩陣為

(20)

步驟6計算卡爾曼增益、狀態(tài)估計結果及其協(xié)方差矩陣：

(21)

(22)

(23)

步驟7對k+1重復執(zhí)行步驟2～6.

對步驟2～6的參數(shù)設置如下：

(24)

λ=α2(n+τ)-n

(25)

式中：α為很小的正數(shù)；β為X的分布信息；參數(shù)τ常取τ=3-n，故式(25)又可以寫為

λ=3α2-n

(26)

2.2 自適應協(xié)方差調整的UKF

(27)

(28)

因此，使用寬度為N的滑動窗口對式(28)進行平穩(wěn)化處理：

(29)

如果

(30)

令

(31)

式中：T為濾波周期.

那么，可以重新得到狀態(tài)噪聲估計：

(32)

3 仿真和分析

3.1 仿真參數(shù)設置

以火星探測器為例進行仿真分析，仿真數(shù)據(jù)由系統(tǒng)工具包產(chǎn)生.坐標系采用J2000.0火心慣性坐標系，軌跡參數(shù)如表1所示.表中：a′為長半軸；e為偏心率；i′為軌道傾角.

表1 火星探測器軌道參數(shù)Tab.1 Orbit parameters of Mars probe

參考軌道由RKF89數(shù)字積分器生成，該積分器使用1 s的固定步長，時間為1997年7月1日0時0分0秒至1997年7月8日0時0分0秒.在接近段，火星和探測器之間的最小距離為 5 211 km.仿真中使用的行星星歷和恒星數(shù)據(jù)庫分別是JPL DE421星表和第谷恒星星表.量測中的導航天體為火星、火衛(wèi)一和火衛(wèi)二.火星、火衛(wèi)一、火衛(wèi)二和恒星傳感器的精度設置為0.1像素，火星、火衛(wèi)一和火衛(wèi)二的光學傳感器參數(shù)如表2所示.表中：f′ 為焦距；

表2 光學傳感器的參數(shù)Tab.2 Parameters of optical sensor

FOV為視場角；R為分辨率.

濾波器的初始參數(shù)設置如下.

初始狀態(tài)為

X0=[rv]=

[1.590 5×1096.504 4×1082.829 5×107

-4.925×103-2.030 5×10376.742 2]

初始狀態(tài)協(xié)方差為

P0=diag(106, 106, 106, 102, 102, 102)

初始狀態(tài)噪聲協(xié)方差為

Q0=diag(10-3, 10-3, 10-3, 10-8, 10-8, 10-8)

初始量測協(xié)方差為

R=

窗口寬度N設置為10.

3.2 結果分析

分別使用UKF和擴展卡爾曼濾波方法對非線性模型式(9)進行濾波，然后與AQUKF進行比較分析，最后對AQUKF方法進行影響因素分析.

3.2.1UKF與AQUKF濾波方法的結果比較 圖2和圖3分別給出了使用UKF和AQUKF方法的濾波結果.仿真結果的統(tǒng)計時間段從第1天開始

圖2 UKF方法濾波結果Fig.2 Results of UKF algorithm

圖3 AQUKF方法濾波結果Fig.3 Results of AQUKF algorithm

至結束.圖2中：Dr=‖Dr‖=‖[DxDyDz]>‖為航天器位置的誤差；Dv=‖Dv‖=‖[DvxDvyDvz]>‖為航天器速度的誤差.

表3給出了UKF和AQUKF方法濾波收斂后的平均誤差和最大誤差.由表3可知，本文提出的AQUKF方法位置精度比UKF提高90.97%；速度精度比UKF提高66.17%.從圖2(a)可以看出，探測器在第3天末x方向和y方向的位置誤差出現(xiàn)較

表3 UKF和AQUKF濾波方法的結果Tab.3 Results of UKF and AQUKF filtering methods

大發(fā)散，并且直到第7天也沒有收斂回來；從圖2(b)可以看出，探測器在第3天末x方向和y方向的速度誤差同樣出現(xiàn)較大發(fā)散.與位置誤差發(fā)散不同的是，速度誤差在發(fā)散后出現(xiàn)短暫收斂，并在一個恒定誤差上下波動.從圖3可以看出，本文提出的AQUKF有效抑制了UKF帶來的發(fā)散問題，提高了導航系統(tǒng)的穩(wěn)定性和精度.

圖4為UKF與AQUKF位置誤差和速度誤差對比.從圖中可以清楚地看出，在約3.7 d時，UKF濾波位置誤差和速度誤差開始發(fā)散.從圖4(b)中可以看出，3.7 d時AQUKF出現(xiàn)的尖峰比UKF稍大一些，約增加1.37 m/s，但是AQUKF可以在1.5 h內恢復收斂，而UKF并未恢復收斂.這說明探測器在某一時刻受到干擾時，利用AQUKF濾波可以很快恢復收斂，提高了導航系統(tǒng)的穩(wěn)定性.另外從圖4(b)可以看出，AQUKF在恢復收斂之后速度誤差仍然很小.

圖4 UKF與AQUKF估計誤差對比Fig.4 Comparison of estimation error between UKF algorithm and AQUKF algorithm

3.2.2影響因素分析 對基于AQUKF的火星探測器導航方法在不同影響因素下的濾波性能進行仿真分析，包括濾波周期、窗口大小.其余仿真條件與3.1節(jié)保持一致.

(1) 濾波周期對導航方法精度的影響.

表4和圖5給出了窗口大小為10、不同濾波周期時的濾波結果.根據(jù)圖表可知濾波周期越短導航系統(tǒng)性能越好，隨著濾波周期減小，精度提升速度變慢，因此調整濾波周期的大小對導航濾波精度的提高是有限的.當濾波周期較長時，導航濾波結果所受影響較大，原因是系統(tǒng)軌道動力學模型的線性化誤差隨時間積分進一步增大，致使系統(tǒng)的性能下降.當采樣周期大于600 s時，估計誤差曲線波動幅度變大，特別是在接近火星時位置和速度均出現(xiàn)較大誤差，導航性能較差.

表4 濾波周期對導航方法精度的影響

(2) 窗口大小對導航方法精度的影響.

圖6和表5給出了濾波周期為600 s、不同窗口大小時的濾波結果.根據(jù)圖表可知，窗口較大或者較小時導航系統(tǒng)性能均有所下降.當N<10，在探測器靠近火星時會利用更多當前時刻的信息去自適應過程噪聲協(xié)方差，這樣就會導致導航系統(tǒng)性能在該時刻附近突然降低，進而影響整個時間段的系統(tǒng)性能.而隨著時間的增加，誤差也會累積增加，因此當N>10時，在利用較多歷史信息的同時，也導致誤差的累積.當N=10時，平衡了當前時刻和歷史時刻信息帶來的誤差，導航性能相對最佳.

圖5 濾波周期對位置誤差和速度誤差的影響Fig.5 Influence of filtering period on position error and velocity error

圖6 窗口大小對位置誤差和速度誤差的影響Fig.6 Influence of window size on position error and velocity error

表5 窗口大小對導航方法精度的影響

4 結語

本文提出一種深空探測器自主天文導航的滑動窗口自適應UKF方法，即AQUKF，相比于使用傳統(tǒng)UKF的算法，AQUKF算法通過自適應調節(jié)系統(tǒng)噪聲協(xié)方差，不僅解決系統(tǒng)發(fā)散問題，提高系統(tǒng)的穩(wěn)定性，還可以大幅提高導航精度.由于AQUKF對窗口大小的敏感程度較小，所以不需要為選擇合適的窗口消耗更多時間，也不需要通過優(yōu)化目標方程選擇窗口大小，損失系統(tǒng)的實時性.該方法可為我國未來深空探測器任務自主導航系統(tǒng)的研制提供參考.