幾何擴展卡爾曼濾波算法在衛星姿態確定系統中的應用*

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

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

?

幾何擴展卡爾曼濾波算法在衛星姿態確定系統中的應用*

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

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

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

姿態確定； 卡爾曼濾波； 誤差一致性； 陀螺漂移

0　引　言

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

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

在實際應用中，由于不知道真實的衛星姿態，EKF等算

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

1　姿態確定的數學模型

1.1陀螺測量模型

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

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

ωg=ω+d+b+ηv

(1)

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

(2)

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

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

w=[ηvηdηb]T

(3)

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

1.2星敏感器測量模型

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

b=Ar+ν

(4)

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

1.3姿態運動學方程

姿態四元數定義如下[13]

q=[ρTq4]T

(5)

式中ρ為四元數的矢量部分，q4為四元數的標量部分。利用四元數表示的姿態矩陣為

(6)

由平臺的姿態四元數q和姿態角速度ω描述的平臺姿態運動的姿態運動學方程為[13]

(7)

式中

(8)

2　誤差一致性表示原理

在姿態確定的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為誤差四元數的矢量部分，當dq很小時

下式成立

dα≈2dρ

(16)

式中dα為姿態角誤差矢量。推導得到

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

(17)

定義

dx=[dαTddTddT]T

(18)

推導可得

Δx=Cdx

(19)

3　GEKF算法

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

3.1狀態變量與狀態方程

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

(20)

式中線性化矩陣

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

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算法其狀態轉移矩陣為

將其離散化得到差分形式的狀態轉移矩陣

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

(26)

式中Δt為采樣周期。

相應的GEKF算法其狀態轉移矩陣為

Φk=T-1ΦMEKFT

(27)

式中

(28)

(29)

狀態變量的預測值為

(30)

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

(31)

2)增益

卡爾曼增益為

(32)

式中

(33)

3)更新

狀態變量增量為

(34)

狀態變量更新為

(35)

姿態四元數歸一化

(36)

誤差協方差的更新值為

(37)

式中

(38)

4　仿真分析

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

4.1參數設置

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

表1　算法參數設置

4.2仿真分析

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

表2給出了不同算法的姿態角均方根誤差。

表2　不同算法的RMSE

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

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

圖2　姿態角估計

圖3　姿態角估計誤差

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

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

圖4　陀螺漂移估計

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

5　結　論

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

[1]呂振鐸.雷擁軍.衛星姿態測量與確定[M]．北京：國防工業出版社,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和互補濾波器在飛行姿態確定中的應用[J].傳感器與微系統,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-),男，安徽安慶人，碩士研究生，主要研究方向為衛星姿態確定。