江　丹, 鄧新蒲, 吳　京, 莫邵文, 楊媛媛

(國防科學技術大學 電子科學與工程學院,湖南 長沙 410073)

?

幾何擴展卡爾曼濾波算法在衛(wèi)星姿態(tài)確定系統(tǒng)中的應用*

江丹, 鄧新蒲, 吳京, 莫邵文, 楊媛媛

(國防科學技術大學 電子科學與工程學院,湖南 長沙 410073)

卡爾曼濾波因其良好的性能廣泛應用于衛(wèi)星姿態(tài)確定中。經典的擴展卡爾曼濾波(EKF)算法在估計姿態(tài)坐標系中表示估計誤差矢量，由于沒有考慮到估計姿態(tài)坐標系與真實姿態(tài)坐標系之間存在偏差，從而導致姿態(tài)估計精度下降。針對這個問題，Andrle M S通過幾何變換引入誤差一致性表示，在此基礎上，提出了幾何擴展卡爾曼濾波(GEKF)算法，將姿態(tài)誤差四元數(shù)和陀螺漂移增量通過幾何變換進行一致性表示，解決了估計誤差矢量表示不一致的問題。本文介紹了誤差一致性表示的原理，并將GEKF算法應用于含常值漂移與時間相關漂移的陀螺模型中，仿真實驗表明：GEKF算法比MEKF對陀螺漂移的估計更加精確，在濾波精度上取得了明顯改善。

姿態(tài)確定； 卡爾曼濾波； 誤差一致性； 陀螺漂移

0　引　言

利用星敏感器和陀螺進行組合來確定衛(wèi)星的姿態(tài)已經得到廣泛應用，陀螺能夠以較高的頻率對衛(wèi)星平臺的三軸角速度進行測量，并通過積分得到姿態(tài)角。 但是陀螺的漂移使測量誤差隨時間累計，這會嚴重影響測量的精度。星敏感器測量精度高，沒有誤差累積和漂移現(xiàn)象，其缺陷是測量頻率較低[1]。因此，將星敏感器和陀螺進行組合來確定衛(wèi)星的姿態(tài)可以克服各自的缺陷，實現(xiàn)優(yōu)勢互補。

常用的姿態(tài)表示方法有歐拉角、四元數(shù)、旋轉矢量法和修正的羅德里格參數(shù)(modified Rodrigue parameters,MRP)等[2,3]，四元數(shù)因其具有較小維數(shù)且無奇異值的出現(xiàn)而被廣泛應用于衛(wèi)星姿態(tài)確定算法中。然而，在傳統(tǒng)的擴展卡爾曼濾波(extended Kalman filtering,EKF)算法中并沒有解決四元數(shù)的歸一化約束問題。乘性EKF(multiplicative EKF,MEKF)算法將姿態(tài)四元數(shù)表示為誤差四元數(shù)與估計四元數(shù)的乘積解決了四元數(shù)歸一化約束問題[4,5]，其算法的主要思想是利用EKF算法對四元數(shù)的矢量部分進行估計，同時利用校正的四元數(shù)來表示衛(wèi) 滿足四元數(shù)單位化約束，又可以避免濾波過程中出現(xiàn)奇異值。

在實際應用中，由于不知道真實的衛(wèi)星姿態(tài)，EKF等算

法通常將誤差矢量表示在估計姿態(tài)坐標系中。需要注意的是,真實的姿態(tài)坐標系與估計姿態(tài)坐標系之間是存在偏差的，將矢量及其估計值表示在估計坐標系中，矢量將會因兩個坐標系之間的偏差而產生一定的偏移，如果坐標系之間的偏差值較大，建立在這種矢量誤差定義基礎上的EKF等算法其估計精度將會受到嚴重影響。針對這個問題，Andrle M S在姿態(tài)確定問題中引入了一種誤差一致性表示方法[6,7]，通過幾何變換將狀態(tài)誤差矢量表示為真實坐標系下，基于該方法提出了用于姿態(tài)確定的幾何EKF(geometric EKF,GEKF)算法[6]。文獻中僅給出了常值漂移陀螺漂移模型下的算法。本文首先介紹誤差一致性表示的原理，然后將其應用于含常值漂移與指數(shù)漂移的陀螺模型中，推導了該應用下的GEKF算法，并完成了該應用下GEKF算法與MEKF算法仿真比較。

1　姿態(tài)確定的數(shù)學模型

1.1陀螺測量模型

理想情況下，陀螺的測量值正比于衛(wèi)星本體相對于慣性系的轉速沿陀螺輸入軸的分量。但是在實際中，陀螺受自身設計、工作機理、運動環(huán)境等的影響，不可避免會存在誤差。

陀螺的測量模型[8,9]為

ωg=ω+d+b+ηv

(1)

式中ω為真實的角速度， d為陀螺的時間相關漂移，b為陀螺常值漂移，ηv為測量噪聲，建模為零均值高斯白噪聲。d,b滿足如下方程

(2)

式中ηd,ηb為零均值高斯白噪聲，-Dτ為相關時間常數(shù)τ構成的對角陣。

將噪聲寫成如下的矢量形式

w=[ηvηdηb]T

(3)

協(xié)方差為E{w(t)wT(τ)}=Q(t)δ(t-τ)。其中

1.2星敏感器測量模型

星敏感器以恒星作為測量基準，得到星敏感器的三軸姿態(tài)，經變換得到衛(wèi)星本體坐標系的姿態(tài)。然而在工程應用中由于安裝誤差等影響，星敏感器測量值不可避免地存在誤差，實際的星敏感器的測量模型[10,11]如下

b=Ar+ν

(4)

式中b為衛(wèi)星本體坐標系中的矢量，r為參考坐標系中的矢量，A為從參考坐標系到衛(wèi)星本體坐標系的姿態(tài)旋轉矩陣，ν為量測誤差，建模為零均值高斯白噪聲。

1.3姿態(tài)運動學方程

姿態(tài)四元數(shù)定義如下[13]

q=[ρTq4]T

(5)

式中ρ為四元數(shù)的矢量部分，q4為四元數(shù)的標量部分。利用四元數(shù)表示的姿態(tài)矩陣為

(6)

由平臺的姿態(tài)四元數(shù)q和姿態(tài)角速度ω描述的平臺姿態(tài)運動的姿態(tài)運動學方程為[13]

(7)

式中

(8)

2　誤差一致性表示原理

在姿態(tài)確定的EKF算法中，對于模型(1)～(7)定義

(9)

式中

(10)

minE{ΔxTΔx}

(11)

(12)

圖1　誤差矢量一致與非一致表示

記一致表示方式下b,d真值與估計值誤差矢量為db,d d，由圖1(c)可知，通過幾何變換得到db與Δb之間的關系式如下

(13)

同理有

(14)

記

d q=[d ρTdq4]T

(15)

式中dρT為誤差四元數(shù)的矢量部分，當dq很小時

下式成立

dα≈2dρ

(16)

式中dα為姿態(tài)角誤差矢量。推導得到

A(dq)≈A(dα)≈I3×3-[dα×]

(17)

定義

dx=[dαTddTddT]T

(18)

推導可得

Δx=Cdx

(19)

3　GEKF算法

文獻[7]根據(jù)前述的誤差一致性表示原理提出了GEKF算法，本節(jié)并將GEKF算法推廣應用于含常值漂移與時間相關漂移的陀螺模型中。

3.1狀態(tài)變量與狀態(tài)方程

傳統(tǒng)的EKF算法將姿態(tài)四元數(shù)，陀螺的漂移量作為狀態(tài)變量。即x=[qTdTbT]T。運動學方程近似為

(20)

式中線性化矩陣

在GEKF算法中，使用誤差一致性表示方法來定義誤差矢量，此時的狀態(tài)誤差矢量定義為

d x=[d αTd dTd bT]T

(21)

可得運動學方程的微分形式

(22)

式中

(23)

(24)

3.2測量變量與測量方程

取兩個星敏感器輸出光軸矢量作為測量值

則測量方程為

zk=hk(xk)+vk

(25)

式中hk(xk)=A(qk)rk。

3.3濾波算法

1)預測

MEKF算法其狀態(tài)轉移矩陣為

將其離散化得到差分形式的狀態(tài)轉移矩陣

ΦMEKF=I+FΔt+F2Δt2

(26)

式中Δt為采樣周期。

相應的GEKF算法其狀態(tài)轉移矩陣為

Φk=T-1ΦMEKFT

(27)

式中

(28)

(29)

狀態(tài)變量的預測值為

(30)

預測誤差協(xié)方差矩陣Pk/k-1可由式(30)求解

(31)

2)增益

卡爾曼增益為

(32)

式中

(33)

3)更新

狀態(tài)變量增量為

(34)

狀態(tài)變量更新為

(35)

姿態(tài)四元數(shù)歸一化

(36)

誤差協(xié)方差的更新值為

(37)

式中

(38)

4　仿真分析

本仿真實驗基于GEKF算法進行，將星敏感器和陀螺的測量值經不同濾波算法得到衛(wèi)星平臺的姿態(tài)。將仿真結果與經典的MEKF算法進行比較。

4.1參數(shù)設置

傳感器的測量噪聲和濾波初值如表1所示。

表1　算法參數(shù)設置

4.2仿真分析

分別對MEKF算法與GEKF算法進行仿真，取1 000次Monte—Carlo仿真結果進行統(tǒng)計，得到不同算法的均方根誤差、相應的姿態(tài)角估計和陀螺漂移估計。

表2給出了不同算法的姿態(tài)角均方根誤差。

表2　不同算法的RMSE

從表2可以看出，在相同的仿真條件下，GEKF算法與MEKF算法相比，姿態(tài)角估計RMSE明顯降低。

圖 2、圖3分別給出了不同算法的姿態(tài)角估計與姿態(tài)角估計誤差。

圖2　姿態(tài)角估計

圖3　姿態(tài)角估計誤差

從圖2可以看出兩種算法都能估計出平臺的姿態(tài)角，圖3給出了兩種算法的姿態(tài)角估計誤差，從中可以看出，使用GEKF算法可以使姿態(tài)角估計誤差保持在±0.6″以內，而使用MEKF算法得到的姿態(tài)角其估計誤差在±1.2″范圍內波動。

圖4給出了不同算法的陀螺漂移估計。

圖4　陀螺漂移估計

從圖4可以看出，GEKF算法對陀螺漂移的估計相對于MEKF算法更加精確。

5　結　論

本文針對傳統(tǒng)的MEKF算法中矢量誤差表示不一致的問題，將GEKF算法應用于衛(wèi)星姿態(tài)確定系統(tǒng)中。該算法首先將誤差矢量在估計姿態(tài)坐標系下進行一致性表示來解決誤差矢量表示不一致的問題，然后對重新定義的狀態(tài)誤差矢量進行EKF濾波來估計出姿態(tài)四元數(shù)增量和陀螺漂移增量，并通過姿態(tài)修正得到衛(wèi)星的姿態(tài)。仿真實驗表明:GEKF算法能夠精確地估計出陀螺的常值漂移和時間相關漂移，并且在濾波精度上較MEKF有明顯改善。

[1]呂振鐸.雷擁軍.衛(wèi)星姿態(tài)測量與確定[M]．北京：國防工業(yè)出版社,2013．

[2]Crassidis J L,Markley F L,Cheng Y．A survey of attitude estimation methods[J]．Journal of Guidance,Control,and Dynamics,2007,30(1):12-28．

[3]Karlgaard C D,Schaub H.Nonsingular attitude filtering using modified rodrigues parameters[J]．The Journal of the Astronauti-

cal Sciences，2009,57(4):777-791．

[4]Markley F L,Crassidis J L．Fundamentals of spacecraft attitude determination and control[M].New York:Springer,2014．

[5]Farrenkopf R L．Analytic steady-state accuracy solutions for two common spacecraft attitude estimators[J]．J Guid Contr,1978,1(4):282-284．

[6]Andrle M S,Crassidis J L.Attitude estimation employing commom frame error representations[C]∥JAIAA Guidance,Navigation,and Control Conference,2016．

[7]Andrle M S，Crassidis J L．Geometric integration of quaternion-s[J]．Journal of Guidance Control and Dynamics，2013,36:1762-1767．

[8]Markley F L.Attitude error representations for Kalman filte-ring[J].Journal of Guidance，Control and Dynamics，2003,26(2):311-317.

[9]郭曉鴻,楊忠,陳喆.EKF和互補濾波器在飛行姿態(tài)確定中的應用[J].傳感器與微系統(tǒng),2011,30(11):149-152.

[10] Shuster M D．Kalman filtering of spacecraft attitude and the QUEST model[J].Journal of the Astronautical Sciences，1990,38(3):377-393．

[11] Shuster M D,Oh S D.Three-axis attitude determination from vector observations[J].Journal of Guidance and Control,1981,4(1):70-77．

[12] Markley F L,Crassidis,J L．Fundamentals of spacecraft attitude determination and control[M].New York:Springer,2014．

[13] Shuster M D,A survey of attitude representations[J].Journal of the Astronautical Science,1993,41(4):439-517．

Application of geometric extended Kalman filtering algorithm in satellite attitude determination system*

JIANG Dan, DENG Xin-pu, WU Jing, MO Shao-wen, YANG Yuan-yuan

(College of Electronic Science and Engineering，National University of Defense Technology，Changsha 410073,China )

Kalman filtering is widely used in satellite attitude determination because of its good performance.The classical EKF algorithm represents the estimated error vector in the estimated attitude coordinate system,because there is no consideration to estimate the deviation between the attitude coordinate system and the true attitude coordinate system,which leads to decrease of attitude estimation precision.To solve this problem,Andrle M S introduces error consistent represent method through geometric transformation,and on this basis,he proposes GEKF algorithm attitude error quaternion and gyro drift increment through geometric transformation of consistency representation,solve the problem that estimate error vector representation is not consistent.The principle of error consistency representation is introduced,and apply GEKF algorithm to gyro model with constant drift and time drift.Simulation experiments show that the GEKF algorithm is more accurate than MEKF on gyro drift estimation,and filtering precision is improved significantly.

attitude determination; Kalman filtering; error consistency; gyro drift

10.13873/J.1000—9787(2016)09—0156—05

2015—06—14

國家自然科學基金資助項目(61401474)

TP 212

A

1000—9787(2016)09—0156—05

江丹(1991-),男，安徽安慶人，碩士研究生，主要研究方向為衛(wèi)星姿態(tài)確定。