基于Lawden改進(jìn)型方程的編隊(duì)Unscented Kalman Filter濾波估計(jì)

溫 洲 邵曉巍 陶久亮 龔德仁

1.上海交通大學(xué)航空航天學(xué)院，上海200240

2.中國(guó)航天科技集團(tuán)公司第一研究院，北京100076

衛(wèi)星編隊(duì)是近年來國(guó)內(nèi)外航天領(lǐng)域興起的熱點(diǎn)研究方向之一，它是將多顆衛(wèi)星組合起來，并在一定空間范圍內(nèi)周期性相互運(yùn)動(dòng)的衛(wèi)星飛行模式。相對(duì)于單顆大衛(wèi)星而言，衛(wèi)星編隊(duì)具有獨(dú)特的優(yōu)勢(shì):相對(duì)低廉的成本和維護(hù)，更低的更替成本，風(fēng)險(xiǎn)更小，以及多個(gè)任務(wù)的協(xié)同完成。衛(wèi)星編隊(duì)不僅可以提高執(zhí)行太空任務(wù)的可能，同時(shí)也將開辟許多新的研究應(yīng)用領(lǐng)域。

目前，衛(wèi)星編隊(duì)主要應(yīng)用于合成孔徑雷達(dá)(Synthetic Aperture Radar，SAR)來進(jìn)行地面的高精度測(cè)繪和地面動(dòng)目標(biāo)跟蹤(Ground Motion Target Indicator，GMTI)、分布式氣象衛(wèi)星三位立體成像以及高分辨率合成孔徑光學(xué)干涉技術(shù)等。目前國(guó)內(nèi)外運(yùn)行或即將運(yùn)行的衛(wèi)星編隊(duì)有德國(guó)“TanDEM －X”雙星編隊(duì)系統(tǒng)，瑞典棱鏡“Prisma”雙星編隊(duì)系統(tǒng)、美國(guó)“A－Train”衛(wèi)星編隊(duì)系統(tǒng)以及中國(guó)的“神七”伴飛小衛(wèi)星編隊(duì)系統(tǒng)等［1－2］。

在衛(wèi)星編隊(duì)任務(wù)中，很多關(guān)鍵技術(shù)的解決都依賴于編隊(duì)衛(wèi)星間相對(duì)狀態(tài)的確定，即編隊(duì)相對(duì)位置和相對(duì)速度的濾波估計(jì)，如編隊(duì)隊(duì)形保持技術(shù)、多星姿態(tài)協(xié)同技術(shù)、防碰撞技術(shù)以及航天器對(duì)接技術(shù)等。所以高精度的編隊(duì)相對(duì)運(yùn)動(dòng)狀態(tài)對(duì)編隊(duì)任務(wù)的順利完成特別重要。在1996年，Garrison 提出了采用EKF(Extended Kalman Filter)進(jìn)行衛(wèi)星編隊(duì)飛行的相對(duì)導(dǎo)航設(shè)計(jì)方法，以及Eric A，Wan 等學(xué)者對(duì)2002年出現(xiàn)的UKF 算法進(jìn)行了相關(guān)研究［3］，并應(yīng)用于衛(wèi)星編隊(duì)飛行導(dǎo)航中，當(dāng)然還有很多其他的濾波估計(jì)方法也適用于衛(wèi)星編隊(duì)的濾波估計(jì)，如文獻(xiàn)［4］中提出的有限域RKF 濾波方法以及文獻(xiàn)［5］中對(duì)UKF 進(jìn)行改進(jìn)得到的自適應(yīng)UKF 濾波算法等。

在編隊(duì)的實(shí)際運(yùn)行環(huán)境中，由于受到各種攝動(dòng)的影響，比如地球扁狀，大氣流動(dòng)，太陽或者月球的引力影響等，其運(yùn)行軌跡是時(shí)變的，其于運(yùn)動(dòng)方程將呈現(xiàn)非線性的。如果用線性的Lawden 方程來描述編隊(duì)相對(duì)運(yùn)動(dòng)方程，將會(huì)產(chǎn)生一定的誤差，導(dǎo)致編隊(duì)相對(duì)狀態(tài)估計(jì)的精確度不夠高，從而對(duì)編隊(duì)的相對(duì)導(dǎo)航及其他任務(wù)等產(chǎn)生一定的影響。

本文基于太空任務(wù)中對(duì)編隊(duì)相對(duì)狀態(tài)的高精度要求，結(jié)合編隊(duì)的非線性運(yùn)動(dòng)性質(zhì)，基于Lawden 方程改進(jìn)得到編隊(duì)非線性運(yùn)動(dòng)方程，即Lawden 改進(jìn)型方程，并采用適合非線性系統(tǒng)的UKF 濾波方法對(duì)編隊(duì)狀態(tài)進(jìn)行了濾波估計(jì)，仿真結(jié)果表明在基于Lawden 改進(jìn)型方程下，相對(duì)于EKF 濾波方法，UKF濾波能將編隊(duì)的相對(duì)距離估計(jì)精度提高70%左右，相對(duì)速率估計(jì)精度提高17%左右，具有一定的理論研究和工程應(yīng)用參考價(jià)值。

1 Lawden 方程介紹

Lawden 方程是簡(jiǎn)化的編隊(duì)相對(duì)運(yùn)動(dòng)方程。首先建立主星的地心軌道坐標(biāo)系，如下圖1 所示，坐標(biāo)系OeXeYeZe為赤道慣性坐標(biāo)系，Xe軸由地心指向春分點(diǎn)γ，Ze軸垂直于赤道平面，Ye軸由右手定則確定。

圖1 雙星編隊(duì)坐標(biāo)系示意圖

OeXcYcZc為主星的地心軌道坐標(biāo)系，其中Xc軸由地心指向衛(wèi)星瞬時(shí)時(shí)刻狀態(tài)點(diǎn)，Zc軸垂直于主星軌道平面，與衛(wèi)星動(dòng)量矩矢量Hc同向，Yc軸由右手定則確定，并沿著衛(wèi)星速度方向，其單位向量為(uxuyuz)。Sc代表主星，Sd代表輔星。

rc，rd分別為主星和輔星在慣性坐標(biāo)系OeXeYeZe下的位置矢量。令星間相對(duì)距離位置矢量ρ=rd－rc。根據(jù)衛(wèi)星軌道運(yùn)動(dòng)，得到編隊(duì)的相對(duì)運(yùn)動(dòng)方程:

根據(jù)式(1)可以推導(dǎo)出編隊(duì)的相對(duì)運(yùn)動(dòng)方程［6－7］:

當(dāng)星間相對(duì)距離滿足max(x，y，z)＜＜r 時(shí)，其中r 為主星到地心的距離，式(2)可簡(jiǎn)化為:

其中

以及

AL為時(shí)變系數(shù)矩陣，是真近點(diǎn)角f 的函數(shù)?？梢耘袛郘awden 方程并非完全意義上的定常線性運(yùn)動(dòng)方程，而是某一時(shí)間點(diǎn)的瞬時(shí)線性運(yùn)動(dòng)方程，也可稱為單點(diǎn)線性運(yùn)動(dòng)方程。

關(guān)于Lawden 方程的解析解，有Carter 提出的解法［8］，以及Yamanaka 和Ankersen 提出的解法［9］，還有Broucke 提出的解法等大約五六種不同的編隊(duì)相對(duì)狀態(tài)求解解法［10］。

2 Lawden 改進(jìn)型相對(duì)運(yùn)動(dòng)方程

可由式(1)得到另一種編隊(duì)的非線性描述方程:

其中

轉(zhuǎn)換成狀態(tài)空間形式:

或者為

Lawden 改進(jìn)型方程在充分考慮了各種攝動(dòng)影響的前提下，將過去時(shí)刻的位置狀態(tài)變量融入相對(duì)運(yùn)動(dòng)方程中，從而實(shí)現(xiàn)編隊(duì)狀態(tài)的非線性描述，相比于Lawden 描述方程，更具有非線性和實(shí)時(shí)特性。

3 編隊(duì)狀態(tài)UKF 濾波估計(jì)

在進(jìn)行編隊(duì)的狀態(tài)濾波估計(jì)方法上，普遍采用的是EKF 濾波算法。EKF 主要是用線性模型近似編隊(duì)的實(shí)時(shí)非線性運(yùn)動(dòng)模型，無法對(duì)實(shí)際的非線性編隊(duì)運(yùn)動(dòng)模型進(jìn)行濾波估計(jì)。然而基于本文提出的Lawden 改進(jìn)型方程，由于其非線性的特性，能夠很好的描述編隊(duì)的實(shí)時(shí)運(yùn)動(dòng)，采用EKF 濾波方法必然會(huì)忽略掉其非線性的特性，導(dǎo)致無法較好的對(duì)編隊(duì)狀態(tài)進(jìn)行精確估計(jì)。

在非線性系統(tǒng)中，UKF 濾波方法通過粒子濾波器產(chǎn)生一定數(shù)量的狀態(tài)采樣點(diǎn)，然后利用采樣點(diǎn)的分布逼近系統(tǒng)狀態(tài)分布，完全適合于非線性系統(tǒng)，所以本文采用UKF 濾波算法來進(jìn)行編隊(duì)狀態(tài)的濾波估計(jì)。

3.1 UKF 濾波算法基本介紹

UKF 和EKF 的基本框架是大致相同的，主要的區(qū)別在于EKF 用線性模型近似系統(tǒng)的非線性模型，而UKF 主要用粒子濾波器來產(chǎn)生一組采樣點(diǎn)，然后利用采樣點(diǎn)的分布近似逼近系統(tǒng)狀態(tài)的分布，適合于非線性的衛(wèi)星編隊(duì)狀態(tài)濾波估計(jì)。

根據(jù)文獻(xiàn)［10 －11］關(guān)于UKF 的分析研究，可以用如下圖2 所示的流程圖清晰地表達(dá)出來。

圖2 UKF 濾波流程圖

3.2 UKF 初始參數(shù)確定

根據(jù)圖2 的UKF 流程圖，首先需要設(shè)定UKF濾波系統(tǒng)的初始參數(shù)值:

4 編隊(duì)狀態(tài)濾波仿真

根據(jù)Lawden 改進(jìn)型方程，經(jīng)過離散化的變形，得到單步相對(duì)運(yùn)動(dòng)方程，由式(10)得到:

其中L=I+Δt·A 為過程矩陣，Δt 為單步運(yùn)行時(shí)間，I 為單位矩陣，D=Δt·C 為單步偽輸入量，即D 并非是真是意義上的輸入量，而是類似于輸入控制量U，也可以認(rèn)為是單步反饋控制量。

利用仿真來檢驗(yàn)Lawden 改進(jìn)型方程的特性，首先設(shè)定仿真參數(shù)，如表1 和表2 所示。本文采用雙星編隊(duì)，基于Lawden 改進(jìn)型方程來進(jìn)行編隊(duì)狀態(tài)EKF 濾波和UKF 濾波估計(jì)仿真實(shí)驗(yàn)。

表1 主星的軌道根數(shù)和編隊(duì)相關(guān)初始值設(shè)定

表2 雙星編隊(duì)初始相對(duì)狀態(tài)

4.1 EKF 濾波仿真

基于給定的編隊(duì)初始條件和軌道參數(shù)，經(jīng)過過程估計(jì)和觀測(cè)估計(jì)，即可求得編隊(duì)的EKF 狀態(tài)濾波估計(jì)值，其位置和速度估計(jì)值與真實(shí)值的誤差分別如圖3 和圖4 所示。

圖3 EKF濾波的三軸向位置估計(jì)誤差

圖4 EKF濾波的三軸向速度估計(jì)誤差

從上面的仿真結(jié)果可以得出，基于Lawden 改進(jìn)型方程，利用EKF 濾波估計(jì)的編隊(duì)相對(duì)距離和相對(duì)速率的精度分別為3.0m 和2.8 ×10－3m/s 左右，其中在X，Y，Z 軸向位置分量的相對(duì)精度分別為0.067%，0.2%，0.2%，速度分量的相對(duì)精度分別為0.36%，0.03%，0.2%。

4.2 UKF 濾波仿真

在設(shè)定完系統(tǒng)參數(shù)值之后，基于表1 和表2 的數(shù)據(jù)和圖2 的UKF 濾波步驟，通過過程估計(jì)和觀測(cè)估計(jì)可以得到編隊(duì)的UKF 狀態(tài)濾波估計(jì)值，其位置和速度估計(jì)值與真實(shí)值的誤差如圖5 和圖6所示。

圖5 UKF濾波的三軸向位置估計(jì)

圖6 UKF濾波的三軸向速度估計(jì)

從上面的結(jié)果可以得出，基于Lawden 改進(jìn)型方程，利用UKF 濾波估計(jì)的編隊(duì)相對(duì)距離和相對(duì)速率的絕對(duì)精度分別大約為0.8m 和2.0 ×10－3m/s，其中在X，Y，Z 軸向位置分量的相對(duì)精度分別為0.027%，0.08%，0.08%，速度分量的相對(duì)精度分別為0.3%，0.023%，0.15%。

4.3 EKF 濾波與UKF 濾波估計(jì)效果比較

綜合比較EKF 和UKF 濾波的編隊(duì)狀態(tài)估計(jì)精度，其位置和速度分量以及絕對(duì)相對(duì)距離的估計(jì)誤差比較如圖7 和圖8 所示。

圖7 編隊(duì)相對(duì)距離的估計(jì)誤差

圖8 編隊(duì)Z向相對(duì)速率的估計(jì)誤差

顯然，在基于Lawden 改進(jìn)型方程的編隊(duì)狀態(tài)估計(jì)中，相對(duì)于EKF 濾波方法，采用UKF 濾波方法可以得到精度更高的狀態(tài)估計(jì)值，由上圖可以大致計(jì)算得到在相對(duì)距離估計(jì)方面，相對(duì)于EKF 濾波方法，UKF 可以提高70%的估計(jì)精度，而在相對(duì)速率估計(jì)方面，可以提高25%的估計(jì)精度，所以總體而言在編隊(duì)狀態(tài)估計(jì)中，UKF 濾波方法優(yōu)于EKF 濾波方法。

5 結(jié)論

在編隊(duì)相對(duì)運(yùn)動(dòng)模型中，經(jīng)過非線性運(yùn)動(dòng)方程的簡(jiǎn)化可以得到線性化的Lawden 編隊(duì)描述方程，但是此簡(jiǎn)化過程忽略了編隊(duì)運(yùn)行的非線性特性，導(dǎo)致描述不精確。本文基于Lawden 描述方程，提出了非線性的Lawden 改進(jìn)型方程，使其更符合描述編隊(duì)的非線性運(yùn)動(dòng)特性。

然后在編隊(duì)狀態(tài)的濾波估計(jì)方面，綜合分析比較了EKF 與UKF 的特點(diǎn)，其中EKF 常用于可以線性化的編隊(duì)系統(tǒng)模型，而UKF 適合于非線性系統(tǒng)模型，無需將系統(tǒng)進(jìn)行線性化改造。這將有利于對(duì)無法線性化的系統(tǒng)進(jìn)行分析。

最后利用EKF 和UKF 濾波方法分別對(duì)編隊(duì)狀態(tài)進(jìn)行濾波估計(jì)仿真。由仿真結(jié)果得到，利用UKF濾波方法得到的編隊(duì)狀態(tài)估計(jì)精度明顯好于采用EKF 濾波方法得到的估計(jì)精度，其中相對(duì)距離的精度可以提高70%左右，相對(duì)速率的精度可以提高25%左右。這也證明了Lawden 改進(jìn)型方程能夠很好的描述非線性的編隊(duì)運(yùn)動(dòng)特性，而且利用UKF 濾波方法可以更加精確地對(duì)編隊(duì)狀態(tài)進(jìn)行濾波估計(jì)，有利于滿足編隊(duì)的相對(duì)導(dǎo)航等任務(wù)的高精確狀態(tài)需求，具有一定的理論和工程參考利用價(jià)值。

［1］劉 明.小衛(wèi)星編隊(duì)飛行相對(duì)運(yùn)動(dòng)與隊(duì)形保持控制研究［D］.上海交通大學(xué)，2007.

［2］郝繼剛.分布式衛(wèi)星編隊(duì)構(gòu)形控制研究［D］.國(guó)防科學(xué)技術(shù)大學(xué)，2006.

［3］Eric A Wan，Rudolph van der Merwe. The Unscented Kalman Filter for Nonlinear Estimation［J］. Adaptive systems for signal Processing，Communications，and Control Symposium 2000，As － SPCC. The IEEE 2000，58(7):153-158.

［4］Zhe Dong，Zheng You. Finite-Horizon Robust Kalman Filtering for Uncertain Discrete Time-Varying Systems With Uncertain-Covariance White Noises［J］.IEEE Signal Processing Letters，2006，13(8).

［5］SONG Qi，HAN Jian-Da. An Adaptive UKF Algorithm for the State and Parameter Estimations of a Mobile Robot［J］.Acta Automatica Sinica，2008，34(1):72-79.

［6］章仁為.衛(wèi)星軌道姿態(tài)動(dòng)力學(xué)與控制［M］. 北京:北京航空航天大學(xué)出版社，2006.

［7］陶久亮，邵曉巍，等.基于魯棒卡爾曼濾波的編隊(duì)衛(wèi)星相對(duì)導(dǎo)航. 航天控制，2011，29(5):8-14. (TAO Jiuliang，SHAO Xiaowei，et al. Relative Navigation for Spacecraft Formation Flying Based on Robust Kalman Filter［J］.Aerospace Control，2011，29(5):8-14.)

［8］Carter T E. State Transition Matrices for Terminal Rendezvous Studies:Brief Survey and New Examples［J］.Journal of Guidance，Control，and Dynamics，1998，21(1):148-155.

［9］Yamanaka K，Ankersen F. New State Transition Matrix for Relative Motion on an Arbitrary Elliptical Orbit［J］.Journal of Guidance，Control，and Dynamics，2002，25(1):60-66.

［10］Kyle T，Alfriend，Srinivas R，Vadali，Pini Gurfil，Jonathan P，How，Louis S，Breger. Spacecraft Formation Flying Dynamics，control and nabigation［M］. Elsevier Astrodynamics Series，2010.

［11］吳江飛，黃珹.分布逼近的卡爾曼濾波及其在星載衛(wèi)星定軌中的應(yīng)用［J］. 天文學(xué)報(bào)，2005，46(1):55-60.(Wu Jiang-fei，Hunag Cheng.Unscented Kalman Filter and Its Application In GPS-Based Satellite Orbit Determination［J］.Acta Astronomica Sinca，2005，46(1):55-60.)