基于Lawden改進型方程的編隊Unscented Kalman Filter濾波估計

溫 洲 邵曉巍 陶久亮 龔德仁

1.上海交通大學航空航天學院，上海200240

2.中國航天科技集團公司第一研究院，北京100076

衛星編隊是近年來國內外航天領域興起的熱點研究方向之一，它是將多顆衛星組合起來，并在一定空間范圍內周期性相互運動的衛星飛行模式。相對于單顆大衛星而言，衛星編隊具有獨特的優勢:相對低廉的成本和維護，更低的更替成本，風險更小，以及多個任務的協同完成。衛星編隊不僅可以提高執行太空任務的可能，同時也將開辟許多新的研究應用領域。

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

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

在編隊的實際運行環境中，由于受到各種攝動的影響，比如地球扁狀，大氣流動，太陽或者月球的引力影響等，其運行軌跡是時變的，其于運動方程將呈現非線性的。如果用線性的Lawden 方程來描述編隊相對運動方程，將會產生一定的誤差，導致編隊相對狀態估計的精確度不夠高，從而對編隊的相對導航及其他任務等產生一定的影響。

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

1 Lawden 方程介紹

Lawden 方程是簡化的編隊相對運動方程。首先建立主星的地心軌道坐標系，如下圖1 所示，坐標系OeXeYeZe為赤道慣性坐標系，Xe軸由地心指向春分點γ，Ze軸垂直于赤道平面，Ye軸由右手定則確定。

圖1 雙星編隊坐標系示意圖

OeXcYcZc為主星的地心軌道坐標系，其中Xc軸由地心指向衛星瞬時時刻狀態點，Zc軸垂直于主星軌道平面，與衛星動量矩矢量Hc同向，Yc軸由右手定則確定，并沿著衛星速度方向，其單位向量為(uxuyuz)。Sc代表主星，Sd代表輔星。

rc，rd分別為主星和輔星在慣性坐標系OeXeYeZe下的位置矢量。令星間相對距離位置矢量ρ=rd－rc。根據衛星軌道運動，得到編隊的相對運動方程:

根據式(1)可以推導出編隊的相對運動方程［6－7］:

當星間相對距離滿足max(x，y，z)＜＜r 時，其中r 為主星到地心的距離，式(2)可簡化為:

其中

以及

AL為時變系數矩陣，是真近點角f 的函數。可以判斷Lawden 方程并非完全意義上的定常線性運動方程，而是某一時間點的瞬時線性運動方程，也可稱為單點線性運動方程。

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

2 Lawden 改進型相對運動方程

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

其中

轉換成狀態空間形式:

或者為

Lawden 改進型方程在充分考慮了各種攝動影響的前提下，將過去時刻的位置狀態變量融入相對運動方程中，從而實現編隊狀態的非線性描述，相比于Lawden 描述方程，更具有非線性和實時特性。

3 編隊狀態UKF 濾波估計

在進行編隊的狀態濾波估計方法上，普遍采用的是EKF 濾波算法。EKF 主要是用線性模型近似編隊的實時非線性運動模型，無法對實際的非線性編隊運動模型進行濾波估計。然而基于本文提出的Lawden 改進型方程，由于其非線性的特性，能夠很好的描述編隊的實時運動，采用EKF 濾波方法必然會忽略掉其非線性的特性，導致無法較好的對編隊狀態進行精確估計。

在非線性系統中，UKF 濾波方法通過粒子濾波器產生一定數量的狀態采樣點，然后利用采樣點的分布逼近系統狀態分布，完全適合于非線性系統，所以本文采用UKF 濾波算法來進行編隊狀態的濾波估計。

3.1 UKF 濾波算法基本介紹

UKF 和EKF 的基本框架是大致相同的，主要的區別在于EKF 用線性模型近似系統的非線性模型，而UKF 主要用粒子濾波器來產生一組采樣點，然后利用采樣點的分布近似逼近系統狀態的分布，適合于非線性的衛星編隊狀態濾波估計。

根據文獻［10 －11］關于UKF 的分析研究，可以用如下圖2 所示的流程圖清晰地表達出來。

圖2 UKF 濾波流程圖

3.2 UKF 初始參數確定

根據圖2 的UKF 流程圖，首先需要設定UKF濾波系統的初始參數值:

4 編隊狀態濾波仿真

根據Lawden 改進型方程，經過離散化的變形，得到單步相對運動方程，由式(10)得到:

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

利用仿真來檢驗Lawden 改進型方程的特性，首先設定仿真參數，如表1 和表2 所示。本文采用雙星編隊，基于Lawden 改進型方程來進行編隊狀態EKF 濾波和UKF 濾波估計仿真實驗。

表1 主星的軌道根數和編隊相關初始值設定

表2 雙星編隊初始相對狀態

4.1 EKF 濾波仿真

基于給定的編隊初始條件和軌道參數，經過過程估計和觀測估計，即可求得編隊的EKF 狀態濾波估計值，其位置和速度估計值與真實值的誤差分別如圖3 和圖4 所示。

圖3 EKF濾波的三軸向位置估計誤差

圖4 EKF濾波的三軸向速度估計誤差

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

4.2 UKF 濾波仿真

在設定完系統參數值之后，基于表1 和表2 的數據和圖2 的UKF 濾波步驟，通過過程估計和觀測估計可以得到編隊的UKF 狀態濾波估計值，其位置和速度估計值與真實值的誤差如圖5 和圖6所示。

圖5 UKF濾波的三軸向位置估計

圖6 UKF濾波的三軸向速度估計

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

4.3 EKF 濾波與UKF 濾波估計效果比較

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

圖7 編隊相對距離的估計誤差

圖8 編隊Z向相對速率的估計誤差

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

5 結論

在編隊相對運動模型中，經過非線性運動方程的簡化可以得到線性化的Lawden 編隊描述方程，但是此簡化過程忽略了編隊運行的非線性特性，導致描述不精確。本文基于Lawden 描述方程，提出了非線性的Lawden 改進型方程，使其更符合描述編隊的非線性運動特性。

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

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

［1］劉 明.小衛星編隊飛行相對運動與隊形保持控制研究［D］.上海交通大學，2007.

［2］郝繼剛.分布式衛星編隊構形控制研究［D］.國防科學技術大學，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］章仁為.衛星軌道姿態動力學與控制［M］. 北京:北京航空航天大學出版社，2006.

［7］陶久亮，邵曉巍，等.基于魯棒卡爾曼濾波的編隊衛星相對導航. 航天控制，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］吳江飛，黃珹.分布逼近的卡爾曼濾波及其在星載衛星定軌中的應用［J］. 天文學報，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.)