低軌衛星緊組合導航UKF方法

姬曉琴，高曉穎

(北京航天自動控制研究所宇航智能控制技術國家級重點實驗室，100854 北京)

低軌衛星緊組合導航UKF方法

姬曉琴，高曉穎

(北京航天自動控制研究所宇航智能控制技術國家級重點實驗室，100854 北京)

針對緊組合導航系統狀態方程及量測方程的非線性，以低軌衛星為應用對象開展了無跡卡爾曼濾波UKF方法研究.給出了慣性系下的系統模型及算法模型，其中姿態直接采用修正Rodrigues參數來表述以避免四元數歸一化條件的限制，系統狀態更新采用四階Runge-Kutta法以適應衛星的高速運動;之后通過數學仿真與廣義卡爾曼濾波EKF進行了比較分析.結果表明:UKF濾波對于姿態精度明顯優于EKF，提高了一個數量級，對于速度、位置精度兩者濾波效果相當，但對于運算時間UKF耗時較長.因此實際應用中可根據導航精度與運算時間需求決定是否采用UKF方法.

無跡卡爾曼濾波UKF;緊組合導航;擴展卡爾曼波EKF;低軌衛星

慣性/衛星緊組合導航系統本質上是一個非線性系統，如慣性導航系統誤差模型、偽距及偽距率量測方程等，而常規卡爾曼濾波(Kalman Filter，KF)僅適用于線性系統，是一種線性無偏最小方差估計.對于一般的非線性系統，在理論上還難以找到一種嚴格的遞推方法，通常都是用近似方法來解決非線性濾波問題，但都不是最優的.其中擴展卡爾曼濾波(Extended Kalman Filter，EKF)應用最為廣泛，但由于線性化誤差的影響，EKF是一個次優有偏的估計器.此外還有無跡卡爾曼濾波［1－5］(Unscented Kalman Filter，UKF)、粒子濾波及模型預測濾波等非線性濾波方法.UKF適用于非線性高斯系統的濾波狀態估計問題，尤其對于強非線性系統其濾波精度及穩定性較EKF明顯提高［6］.

文獻［2］對不同精度的慣性儀表與GPS的緊組合導航系統進行了UKF研究，姿態采用四元數表示，但這無法滿足濾波過程四元數歸一化條件約束;文獻［3］基于無人機進行了慣性/GPS/磁力計多傳感器融合的UKF研究，給出隨著系統維數的增加，UKF的計算量要大于EKF的結論;文獻［4］基于運動載體進行松組合導航系統UKF研究，全局姿態用四元數表述，而局部姿態誤差用推廣的Rodrigues參數定義，這雖然克服了四元數歸一化難題，但計算過程比較復雜;文獻［7］進行了中高軌衛星的緊組合導航系統研究，姿態處理方法同文獻［4］.上述文獻基本都有一共性結論:UKF與EKF濾波精度大致相當，只是當初始誤差較大時，UKF收斂速度較EKF快，但最終濾波精度是一致的.文獻［5］對UAV慣性/GPS/氣壓高度計組合導航系統進行了研究，狀態更新用一階Euler法，在采用強非線性氣壓高度計測量模型及強非線性GPS速度位置測量模型(考慮到GPS接收機與IMU安裝點不重合)時，UKF濾波精度較EKF提高至少30%，體現了UKF對于強非線性系統的優越性.文獻［8］將UKF方法應用在慣性導航初始對準中，但仍舊使用誤差狀態變量作為系統狀態.文獻［9］設計了一種新的UT代表點，擴展了UKF算法，仿真表明該擴展算法精度略優于原UKF，但計算量要高20%.

文中針對低軌衛星基于偽距、偽距率的慣性/衛星緊組合導航系統，開展了UKF濾波算法研究，給出了慣性系下的系統模型及算法模型，其中姿態直接采用修正 Rodrigues參數［10－12］作為姿態表述以避免四元數歸一化條件的限制，計算過程較文獻［4，7］簡單，并提高了姿態濾波精度，最后通過數學仿真驗證了所提方法對于姿態修正的有效性.

1 無跡卡爾曼濾波UKF

設非線性系統模型為

式中:xk為n維狀態向量，zk為m維觀測向量，wk為q維系統噪聲，vk為r維測量噪聲，兩者均為高斯白噪聲，即

式中:Qk假定為非負定陣，Rk假定為正定陣，假設x0是與{wk}、{vk}都互不相關的隨機向量.

首先增廣狀態向量 Xa=［xτwτvτ］τ，其維數為L=n+q+r，則UKF濾波方程可寫為

1)初始化.

2)計算2L+1個sigma點(采樣點).

3)狀態更新(預測).

狀態變量預測值的均值和方差為

4)量測更新(預測).

根據采樣點預測值計算2L+1個預測觀測值為

預測觀測值的均值和方差為

狀態量和觀測量的互協方差為

2 緊組合導航系統

2.1 緊組合導航系統狀態方程

首先介紹姿態表述的修正Rodrigues參數，它實際上是由姿態四元數推導而來.與繞空間某單位軸e旋轉θ角對應的單位四元數q可寫為

可以看出，R參數旋轉角 θ的范圍變為(-2π，2π)，在θ=±2π處奇異.

需說明的是，四元數雖然是全局無奇異的姿態表述，但由于四元數不是姿態的最小實現，在濾波遞推過程中由因無法始終滿足四元數歸一化條件而導致濾波誤差增大.所以本文直接選取接近全局的最小姿態實現——修正的Rodrigues參數作為姿態表述，既降低了系統維數又保證了濾波精度.

設ω為星體旋轉角速度，則姿態四元數滿足如下動態方程:

則可導出修正Rodrigues參數的動態方程為

基于修正Rodrigues參數的低軌衛星緊組合導航系統狀態方程可寫為(只考慮引力，不考慮攝動力):

式中:X為位置，V為速度，R為修正Rodrigues參數，K0為加速度計零位誤差，D0為陀螺零漂，cδt為與接收機時鐘誤差等效的距離誤差，cδf為與接收機時鐘頻率誤差等效的距離率誤差;為彈體系到慣性系的轉換矩陣，G為引力加速度;aB、ωB分別為加表、陀螺的測量值;?a、?g分別為加速度計、陀螺的測量噪聲;wcδt、wcδf為白噪聲.

2.2 緊組合導航系統量測方程

偽距、偽距率量測方程為［13］:

式中:ρj為偽距測量值，˙ρj為偽距率測量值，j代表衛星編號，vρj為偽距測量噪聲，v˙ρj為偽距率測量噪聲;為慣導計算的第j號導航衛星到接收機的距離，=(X-Xj)τ(V-Vj)/為慣導計算的距離變化率，Xj、Vj為第j顆衛星在慣性系下的位置、速度矢量.

3 數學仿真

3.1 仿真計算條件

1)假設地球為圓形，衛星軌道高度為600 km;

2)濾波器相關參數選取:假設初始狀態無誤差，假設速度誤差 0.1 m/s(1σ)，位置誤差20 m(1σ)，姿態 Rodrigues參數誤差 2e-4(1σ)，加表零位誤差 3 ×10－4g0(1σ)，陀螺零漂0.6(°)/h(1σ)，等效距離誤差 300 m(1σ)，等效距離率誤差0.03 m/s(1σ)，偽距測量誤差50 m(1σ)，偽距率測量誤差0.3 m/s(1σ);

3)接收機收星數目:假設低軌衛星上的GPS接收天線的安裝方式能保證任一時刻都能收到至少4個衛星信號，即至少有4個偽距、偽距率觀測值;

4)系統狀態更新:因為低軌衛星在作高速運動，為了保證預測精度以使濾波順利進行，仿真采用四階Runge-Kutta法進行系統狀態更新.

3.2 UKF仿真結果及分析

基于低軌衛星慣性/衛星緊組合導航系統，本文分別對如下3種濾波方法進行了仿真研究并對其結果進行了比較分析:針對非線性系統方程及量測方程進行了UKF仿真(記為UKF方法1);針對線性化系統方程及量測方程進行了EKF仿真;針對線性化系統方程、非線性量測方程進行了UKF仿真(記為UKF方法2).圖1、圖2分別給出了EKF與UKF方法1的姿態誤差曲線圖.仿真結果表明:

1)對于位置、速度、姿態精度，UKF方法2與EKF濾波的效果相當.這說明偽距、偽距率量測方程的線性化對濾波精度的影響不大.

2)對于姿態精度，UKF方法1的效果明顯優于EKF濾波，姿態精度由0.8°變為0.03°，提高了一個數量級.分析其原因，應是姿態狀態方程的強非線性使得UKF的優勢得以發揮.

3)對于位置、速度精度，UKF方法1與EKF濾波的效果相當，這說明速度狀態方程的線性化精度足夠高，沒有體現出UKF的優勢.

4)對于仿真運行時間，完成UKF方法2需時約為400 s，而EKF需時約為360 s，這與 EKF大致相當;完成UKF方法1需時約為525 s，耗時較多，這主要是因為衛星在高速運動，而GPS采樣信號為1 s一次，故狀態更新若用Euler法則預測精度無法保證，仿真中采用四階Runge-Kutta法雖然保證了精度卻導致運算時間增加.

此外，為了進一步考察UKF方法一對姿態的濾波精度，將陀螺精度降低為6°/h(1σ)，在其他濾波條件不變的情況下進行了仿真，仿真結果如圖3、圖4所示，姿態精度由22°變為0.4°，提高了將近兩個數量級，再次驗證了本文所用修正Rodrigues參數表述姿態的有效性.

圖1 姿態誤差曲線(EKF，D0=0.6°/h(1σ)

圖2 姿態誤差曲線(UKF方法1，D0=0.6°/h(1σ)

圖3 姿態誤差曲線(EKF，D0=6°/h(1σ)

圖4 姿態誤差曲線(UKF方法1，D0=6°/h(1σ)

4 結論

基于低軌衛星偽距、偽距率慣性/衛星緊組合導航系統，本文針對系統方程及量測方程的非線性，進行了無跡卡爾曼濾波UKF研究，仿真結果顯示姿態濾波精度得到大幅提高，表明了直接采用修正Rodrigues參數作為全局姿態表述的有效性，體現了UKF對強非線性姿態運動方程的優勢.但是此結果的普遍性和適用性還有待于進一步深入研究.

［1］JULIER S J，UHLMANN J K.A new approach for filtering nonlinear system［C］//Proceedings of the American Control Conference.Washington:Seattle，American Automatic Control Council，1995:1628 －1632.

［2］JAN W，ANDREAS M，JURGEN M，et al.Comparison of Extended and Sigma-Point Kalman Filters for Tightly Coupled GPS-INS Integration［R］.2005，AAIA－2005－6055.

［3］OH Seung-min，JOHNSON E N.Development of UAV Navigation System Based on Unscented Kalman Filter［R］.2005，AIAA-2006-6351，2006.

［4］JOHN L C.Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation［R］.2005，AIAA-2005-6052.

［5］VAN D M R，WAN E A.Sigma-point kalman filters for integrated navigation［C］//60th Annual Meeting of the Institute of Navigation.Dayton，OH，United States:Institute of Navigation，2004.

［6］趙琳，王小旭，丁繼成，等.組合導航系統非線性濾波算法綜述［J］.中國慣性技術學報，2009，17(1):46－52，58.

［7］范利濤.自動轉移飛行器自主導航方法研究［D］.長沙:國防科學技術大學，2009:61－80.

［8］張衛明.UKF方法在慣性導航系統初始對準中的應用研究［J］.系統工程與電子技術，2007，29(4):589－592.

［9］李濤.非線性濾波方法在導航系統中的應用研究［D］.長沙:國防科學技術大學，2003:30－39.

［10］WIENER T F.Theoretical analysis of himballess inertial reference equipment using delta-modulated instruments［D］.Cambridge，MA:MIT Dept of Aeronautics and Astronautics，1962:69 －71.

［11］MARKLEY F L.Attitude error reperesentations for Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2003，26(2):311 －319.

［12］SHUSTER M D.A survey of attitude representations［J］.Journal of the Astronautical Sciences，1993，41(4):439－517.

［13］胡小平.自主導航理論與應用［M］.長沙:國防科技大學出版社，2002:207－210.

UKF for Tightly Coupled Integration in LEO

JI Xiao-qin1，GAO Xiao-ying2

(Beijing Aerospace Automatic Control Institute，National Key Laboratory of Science and Technology on Aerospace Intelligent Control，100854 Beijing，China)

For the nonlinearity of the system dynamic and measurement model in Tightly Coupled Integration of LEO，this article holds research on Unscented Kalman Filter(UKF).The system model and UKF method in inertial frame are introduced.The attitude is represented by modified Rodrigues parameters instead of quaternion for its unit norm constraint，and the state is predicated by the fourth Runge-Kutta technique for satellite＇s high speed.The performance of UKF is compared to Extended Kalman Filter(EKF)by simulations.The results indicate that UKF is better on attitude precision than EKF by one order of magnitude，equal on position and velocity precision，but higher on computation time.

Unscented Kalman Filter(UKF);tightly coupled navigation;Extended Kalman Filter(EKF);low-earth orbit satellite

U666.1

A

0367－6234(2012)07－0135－04

2011－01－25.

姬曉琴(1971—)，女，高級工程師.

姬曉琴，ji－xiaoqin@126.com.

(編輯 苗秀芝)