低軌衛(wèi)星緊組合導(dǎo)航UKF方法

姬曉琴，高曉穎

(北京航天自動(dòng)控制研究所宇航智能控制技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，100854 北京)

低軌衛(wèi)星緊組合導(dǎo)航UKF方法

姬曉琴，高曉穎

(北京航天自動(dòng)控制研究所宇航智能控制技術(shù)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室，100854 北京)

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

無(wú)跡卡爾曼濾波UKF;緊組合導(dǎo)航;擴(kuò)展卡爾曼波EKF;低軌衛(wèi)星

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

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

文中針對(duì)低軌衛(wèi)星基于偽距、偽距率的慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)，開(kāi)展了UKF濾波算法研究，給出了慣性系下的系統(tǒng)模型及算法模型，其中姿態(tài)直接采用修正 Rodrigues參數(shù)［10－12］作為姿態(tài)表述以避免四元數(shù)歸一化條件的限制，計(jì)算過(guò)程較文獻(xiàn)［4，7］簡(jiǎn)單，并提高了姿態(tài)濾波精度，最后通過(guò)數(shù)學(xué)仿真驗(yàn)證了所提方法對(duì)于姿態(tài)修正的有效性.

1 無(wú)跡卡爾曼濾波UKF

設(shè)非線性系統(tǒng)模型為

式中:xk為n維狀態(tài)向量，zk為m維觀測(cè)向量，wk為q維系統(tǒng)噪聲，vk為r維測(cè)量噪聲，兩者均為高斯白噪聲，即

式中:Qk假定為非負(fù)定陣，Rk假定為正定陣，假設(shè)x0是與{wk}、{vk}都互不相關(guān)的隨機(jī)向量.

首先增廣狀態(tài)向量 Xa=［xτwτvτ］τ，其維數(shù)為L(zhǎng)=n+q+r，則UKF濾波方程可寫(xiě)為

1)初始化.

2)計(jì)算2L+1個(gè)sigma點(diǎn)(采樣點(diǎn)).

3)狀態(tài)更新(預(yù)測(cè)).

狀態(tài)變量預(yù)測(cè)值的均值和方差為

4)量測(cè)更新(預(yù)測(cè)).

根據(jù)采樣點(diǎn)預(yù)測(cè)值計(jì)算2L+1個(gè)預(yù)測(cè)觀測(cè)值為

預(yù)測(cè)觀測(cè)值的均值和方差為

狀態(tài)量和觀測(cè)量的互協(xié)方差為

2 緊組合導(dǎo)航系統(tǒng)

2.1 緊組合導(dǎo)航系統(tǒng)狀態(tài)方程

首先介紹姿態(tài)表述的修正Rodrigues參數(shù)，它實(shí)際上是由姿態(tài)四元數(shù)推導(dǎo)而來(lái).與繞空間某單位軸e旋轉(zhuǎn)θ角對(duì)應(yīng)的單位四元數(shù)q可寫(xiě)為

可以看出，R參數(shù)旋轉(zhuǎn)角 θ的范圍變?yōu)?-2π，2π)，在θ=±2π處奇異.

需說(shuō)明的是，四元數(shù)雖然是全局無(wú)奇異的姿態(tài)表述，但由于四元數(shù)不是姿態(tài)的最小實(shí)現(xiàn)，在濾波遞推過(guò)程中由因無(wú)法始終滿足四元數(shù)歸一化條件而導(dǎo)致濾波誤差增大.所以本文直接選取接近全局的最小姿態(tài)實(shí)現(xiàn)——修正的Rodrigues參數(shù)作為姿態(tài)表述，既降低了系統(tǒng)維數(shù)又保證了濾波精度.

設(shè)ω為星體旋轉(zhuǎn)角速度，則姿態(tài)四元數(shù)滿足如下動(dòng)態(tài)方程:

則可導(dǎo)出修正Rodrigues參數(shù)的動(dòng)態(tài)方程為

基于修正Rodrigues參數(shù)的低軌衛(wèi)星緊組合導(dǎo)航系統(tǒng)狀態(tài)方程可寫(xiě)為(只考慮引力，不考慮攝動(dòng)力):

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

2.2 緊組合導(dǎo)航系統(tǒng)量測(cè)方程

偽距、偽距率量測(cè)方程為［13］:

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

3 數(shù)學(xué)仿真

3.1 仿真計(jì)算條件

1)假設(shè)地球?yàn)閳A形，衛(wèi)星軌道高度為600 km;

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

3)接收機(jī)收星數(shù)目:假設(shè)低軌衛(wèi)星上的GPS接收天線的安裝方式能保證任一時(shí)刻都能收到至少4個(gè)衛(wèi)星信號(hào)，即至少有4個(gè)偽距、偽距率觀測(cè)值;

4)系統(tǒng)狀態(tài)更新:因?yàn)榈蛙壭l(wèi)星在作高速運(yùn)動(dòng)，為了保證預(yù)測(cè)精度以使濾波順利進(jìn)行，仿真采用四階Runge-Kutta法進(jìn)行系統(tǒng)狀態(tài)更新.

3.2 UKF仿真結(jié)果及分析

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

1)對(duì)于位置、速度、姿態(tài)精度，UKF方法2與EKF濾波的效果相當(dāng).這說(shuō)明偽距、偽距率量測(cè)方程的線性化對(duì)濾波精度的影響不大.

2)對(duì)于姿態(tài)精度，UKF方法1的效果明顯優(yōu)于EKF濾波，姿態(tài)精度由0.8°變?yōu)?.03°，提高了一個(gè)數(shù)量級(jí).分析其原因，應(yīng)是姿態(tài)狀態(tài)方程的強(qiáng)非線性使得UKF的優(yōu)勢(shì)得以發(fā)揮.

3)對(duì)于位置、速度精度，UKF方法1與EKF濾波的效果相當(dāng)，這說(shuō)明速度狀態(tài)方程的線性化精度足夠高，沒(méi)有體現(xiàn)出UKF的優(yōu)勢(shì).

4)對(duì)于仿真運(yùn)行時(shí)間，完成UKF方法2需時(shí)約為400 s，而EKF需時(shí)約為360 s，這與 EKF大致相當(dāng);完成UKF方法1需時(shí)約為525 s，耗時(shí)較多，這主要是因?yàn)樾l(wèi)星在高速運(yùn)動(dòng)，而GPS采樣信號(hào)為1 s一次，故狀態(tài)更新若用Euler法則預(yù)測(cè)精度無(wú)法保證，仿真中采用四階Runge-Kutta法雖然保證了精度卻導(dǎo)致運(yùn)算時(shí)間增加.

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

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

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

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

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

4 結(jié)論

基于低軌衛(wèi)星偽距、偽距率慣性/衛(wèi)星緊組合導(dǎo)航系統(tǒng)，本文針對(duì)系統(tǒng)方程及量測(cè)方程的非線性，進(jìn)行了無(wú)跡卡爾曼濾波UKF研究，仿真結(jié)果顯示姿態(tài)濾波精度得到大幅提高，表明了直接采用修正Rodrigues參數(shù)作為全局姿態(tài)表述的有效性，體現(xiàn)了UKF對(duì)強(qiáng)非線性姿態(tài)運(yùn)動(dòng)方程的優(yōu)勢(shì).但是此結(jié)果的普遍性和適用性還有待于進(jìn)一步深入研究.

［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］趙琳，王小旭，丁繼成，等.組合導(dǎo)航系統(tǒng)非線性濾波算法綜述［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17(1):46－52，58.

［7］范利濤.自動(dòng)轉(zhuǎn)移飛行器自主導(dǎo)航方法研究［D］.長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué)，2009:61－80.

［8］張衛(wèi)明.UKF方法在慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中的應(yīng)用研究［J］.系統(tǒng)工程與電子技術(shù)，2007，29(4):589－592.

［9］李濤.非線性濾波方法在導(dǎo)航系統(tǒng)中的應(yīng)用研究［D］.長(zhǎng)沙:國(guó)防科學(xué)技術(shù)大學(xué)，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］胡小平.自主導(dǎo)航理論與應(yīng)用［M］.長(zhǎng)沙:國(guó)防科技大學(xué)出版社，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—)，女，高級(jí)工程師.

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

(編輯 苗秀芝)