深空探測器接近段自主導(dǎo)航的滑動(dòng)窗口自適應(yīng)濾波方法

張文佳， 馬 辛

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

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

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

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

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

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

1 問題描述

1.1 軌道動(dòng)力學(xué)模型

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

(1)

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

(2)

1.2 星光角距量測模型

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

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

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

(3)

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

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

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

(4)

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

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

(5)

1.3.2積分誤差傳遞分析 軌道動(dòng)力學(xué)模型積分誤差傳播模型可以表示為

(6)

式中：t1和t2分別為起、止時(shí)間.

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

(7)

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

(8)

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

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

2 自適應(yīng)無跡卡爾曼濾波

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

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

(9)

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

(10)

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

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

步驟1設(shè)定濾波初值.當(dāng)k=0，有

(11)

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

步驟2計(jì)算k-1時(shí)刻的Sigma點(diǎn)：

(12)

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

(13)

然后計(jì)算出一步預(yù)測結(jié)果和對(duì)應(yīng)的協(xié)方差矩陣：

(14)

Qk-1

(15)

步驟4計(jì)算k時(shí)刻的一步預(yù)測Sigma樣本點(diǎn)：

(16)

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

(17)

那么一步預(yù)測量測值為

(18)

自協(xié)方差矩陣為

(19)

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

(20)

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

(21)

(22)

(23)

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

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

(24)

λ=α2(n+τ)-n

(25)

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

λ=3α2-n

(26)

2.2 自適應(yīng)協(xié)方差調(diào)整的UKF

(27)

(28)

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

(29)

如果

(30)

令

(31)

式中：T為濾波周期.

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

(32)

3 仿真和分析

3.1 仿真參數(shù)設(shè)置

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

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

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

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

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

濾波器的初始參數(shù)設(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設(shè)置為10.

3.2 結(jié)果分析

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

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

圖2 UKF方法濾波結(jié)果Fig.2 Results of UKF algorithm

圖3 AQUKF方法濾波結(jié)果Fig.3 Results of AQUKF algorithm

至結(jié)束.圖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濾波方法的結(jié)果Tab.3 Results of UKF and AQUKF filtering methods

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

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

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

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

(1) 濾波周期對(duì)導(dǎo)航方法精度的影響.

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

表4 濾波周期對(duì)導(dǎo)航方法精度的影響

(2) 窗口大小對(duì)導(dǎo)航方法精度的影響.

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

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

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

表5 窗口大小對(duì)導(dǎo)航方法精度的影響

4 結(jié)語

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