基于Gauss-Newton和UKF結(jié)合的微小衛(wèi)星姿態(tài)確定算法

，，，

1. 南京航空航天大學(xué) 微小衛(wèi)星研究中心，南京 210016 2. 上海衛(wèi)星工程研究所，上海 201109

微小衛(wèi)星姿態(tài)確定的精度取決于姿態(tài)敏感器的精度及姿態(tài)確定算法的精度[1]，微小衛(wèi)星由于成本、體積、質(zhì)量及功耗的限制，無法像傳統(tǒng)大衛(wèi)星一樣自由選擇姿態(tài)敏感器[2]，可以考慮采用微型器件，如微機(jī)電系統(tǒng)、光纖器件等，這些器件的性能指標(biāo)低于傳統(tǒng)器件，需要研究合適的姿態(tài)確定算法提升姿態(tài)確定精度。

目前微小衛(wèi)星姿態(tài)確定的方法有兩類，確定性算法和狀態(tài)估計(jì)法[3]。確定性算法主要是求解Wahba問題[4]進(jìn)行矢量定姿，典型的有TRIAD[5]、QUEST[6]、SVD[7]、FORM[8]、Eualr-q[9]等；對(duì)于求解誤差能量函數(shù)最小問題，在數(shù)字信號(hào)處理領(lǐng)域廣泛應(yīng)用的還有“最陡梯度下降法”、Newton法[10]和Gauss-Newton(GN)算法，最陡梯度下降法能保證計(jì)算收斂，但收斂速度較慢；Newton法收斂速度較快，但計(jì)算復(fù)雜，需計(jì)算損失函數(shù)的二階微分Hessian矩陣；Gauss-Newton算法僅計(jì)算一階微分Jacobian矩陣，減小了計(jì)算量，而且保證計(jì)算收斂。確定性算法結(jié)構(gòu)簡(jiǎn)單，設(shè)計(jì)容易，缺點(diǎn)是無法抑制觀測(cè)噪聲對(duì)姿態(tài)確定精度的影響[3]。

狀態(tài)估計(jì)法采用非線性濾波方法實(shí)現(xiàn)最優(yōu)姿態(tài)估計(jì)，應(yīng)用最廣泛的是擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)[11]，但EKF只保留了Taylor的一階近似項(xiàng)，不可避免引入線性化誤差，如果線性化假設(shè)不成立，會(huì)導(dǎo)致濾波器的性能下降甚至造成濾波發(fā)散[12]。UKF濾波[13]基于UT變換對(duì)非線性函數(shù)的概率密度分布進(jìn)行近似，未將高階項(xiàng)忽略，對(duì)于非線性濾波有較高的計(jì)算精度，能有效解決EKF估計(jì)精度低、穩(wěn)定性差的問題[14]。文獻(xiàn)[15]利用地磁矢量作為測(cè)量信息，針對(duì)某LEO通信衛(wèi)星仿真對(duì)比了EKF和UKF算法的估計(jì)精度、收斂時(shí)間和運(yùn)算量，指出UKF在收斂速度、估計(jì)精度及跟蹤姿態(tài)的快速變化等方面都優(yōu)于EKF；而UKF的計(jì)算量大約為EKF的2n+1倍。

本文基于磁強(qiáng)計(jì)/太陽敏感器/陀螺的輕型低成本姿態(tài)敏感器配置，將確定性算法中的Gauss-Newton算法和狀態(tài)估計(jì)法中的UKF濾波有機(jī)結(jié)合，提出一種組合算法，既克服了測(cè)量誤差對(duì)姿態(tài)確定精度的影響，又減小了計(jì)算量。

1 衛(wèi)星姿態(tài)描述

1.1 衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程

設(shè)q為軌道坐標(biāo)系到星體坐標(biāo)系的四元數(shù)，則由四元數(shù)來描述星體系相對(duì)于軌道系的姿態(tài)，得到衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程如下：

(1)

1.2 衛(wèi)星姿態(tài)動(dòng)力學(xué)方程

不含撓性部件的微小衛(wèi)星可視為剛體，其相對(duì)慣性系的動(dòng)力學(xué)方程為：

式中：I為衛(wèi)星的慣量矩陣；Tb為作用于衛(wèi)星的總力矩：

(4)

2 基于磁強(qiáng)計(jì)/太陽敏感器/陀螺 的姿態(tài)確定算法設(shè)計(jì)

2.1 姿態(tài)確定方案設(shè)計(jì)

確定性算法直接根據(jù)姿態(tài)敏感器的測(cè)量值確定姿態(tài)，算法簡(jiǎn)單，計(jì)算量小，但很難克服測(cè)量誤差對(duì)姿態(tài)確定精度的影響[16]；而狀態(tài)估計(jì)法能克服測(cè)量誤差的影響，提供統(tǒng)計(jì)意義上的最優(yōu)解，其中UKF濾波的精度和穩(wěn)定性都要優(yōu)于常用的EKF，但缺點(diǎn)是計(jì)算量較大[15]。

因此，本文基于“磁強(qiáng)計(jì)+太陽敏感器+陀螺”的輕型低成本姿態(tài)敏感器配置，將確定性算法中的Gauss-Newton和狀態(tài)估計(jì)法中的UKF濾波有機(jī)結(jié)合，提出一種組合算法，如圖1所示，先用Gauss-Newton初步解算，將磁強(qiáng)計(jì)和太陽敏感器的6維觀測(cè)量轉(zhuǎn)換為最優(yōu)四元數(shù)qm，降低觀測(cè)維數(shù)，并將觀測(cè)方程轉(zhuǎn)換成了線性方程，顯著減少計(jì)算量，然后以最優(yōu)四元數(shù)qm聯(lián)合陀螺數(shù)據(jù)作為觀測(cè)量，以姿態(tài)四元數(shù)和慣性系下的角速度作為狀態(tài)量進(jìn)行UKF濾波，以降低測(cè)量誤差的影響，進(jìn)一步提高姿態(tài)解算的精度。

圖1 姿態(tài)確定方案Fig.1 Attitude determination scheme

2.2 Gauss-Newton算法初步解算

矢量觀測(cè)是姿態(tài)確定的主要方法，矢量觀測(cè)就是利用姿態(tài)敏感器對(duì)當(dāng)前時(shí)刻某些矢量(如恒星矢量、太陽矢量、地磁矢量等)的觀測(cè)來計(jì)算衛(wèi)星相對(duì)某參考坐標(biāo)系的姿態(tài)，矢量觀測(cè)方程可描述為：

AVi=Ui(i=1,2,…,n)(5)

式中：Vi為星體系中的觀測(cè)矢量；Ui為參考坐標(biāo)系中的參考矢量；A為星體系到參考系的姿態(tài)轉(zhuǎn)換矩陣。確定性算法就是如何根據(jù)一組矢量觀測(cè)值求出星體的姿態(tài)矩陣A。

當(dāng)姿態(tài)敏感器選擇為磁強(qiáng)計(jì)和太陽敏感器時(shí)，觀測(cè)矢量有兩對(duì)，即n=2。V1為磁強(qiáng)計(jì)測(cè)得的星體系下的地磁矢量觀測(cè)值Bb，U1為根據(jù)衛(wèi)星軌道參數(shù)和地球磁場(chǎng)模型計(jì)算得到的軌道系下的地磁矢量參考值Bo，V2為太陽敏感器測(cè)得的星體系下的太陽矢量觀測(cè)值Sb，U2為根據(jù)衛(wèi)星軌道參數(shù)和太陽矢量模型計(jì)算得到的軌道系下的太陽矢量參考值So。

Wahba將矢量觀測(cè)描述為最小二乘估計(jì)問題，給出了一個(gè)求解姿態(tài)矩陣的、包含所有矢量測(cè)量信息的最小二乘性能指標(biāo)，即能量損失函數(shù)[4]：

σi[Ui-AVi]T[Ui-AVi] =

式中：σi為各組矢量對(duì)的權(quán)系數(shù)；εi(q)為第i組測(cè)量與參考矢量之間的誤差。最小化能量損失函數(shù)L(A)，即可求得加權(quán)最小二乘意義下姿態(tài)矩陣A的最優(yōu)解。根據(jù)Gauss-Newton收斂算法，使得能量損失函數(shù)最小的四元數(shù)為[17]：

qk+1=qk-[J(qk)TJ(qk)]-1J(qk)Tε(qk)(7)

式中：J為Jacobian矩陣，定義為:

(8)

式(7)中的ε(qk)為觀測(cè)矢量與參考矢量之間的誤差：

式(7)中的J為如式(8)定義的Jacobian矩陣，具體為：

(10)

式中：yo,yb,M分別為：

式中：A為用四元數(shù)描述的星體系到軌道系的姿態(tài)轉(zhuǎn)換矩陣

則根據(jù)式(7)進(jìn)行迭代計(jì)算即可求得最優(yōu)四元數(shù)qm。其中由于姿態(tài)四元數(shù)的4個(gè)變量不獨(dú)立，存在約束條件‖q‖=1，所以在整個(gè)姿態(tài)確定算法的過程中，都要進(jìn)行四元數(shù)范數(shù)運(yùn)算以保證四元數(shù)正規(guī)化。

2.3 濾波狀態(tài)方程

(13)

2.4 濾波觀測(cè)方程

(14)

Z=I7×7X+V

(15)

2.5 UKF濾波

(1)步驟1

UT變換，獲得一組Sigma點(diǎn)集。設(shè)非線性系統(tǒng)離散化后的模型為：

(16)

計(jì)算2n+1個(gè)Sigma采樣點(diǎn)，這里的n指的是狀態(tài)量的維數(shù)：

(17)

計(jì)算這些采樣點(diǎn)相應(yīng)的權(quán)值：

(18)

式中：ωm為計(jì)算均值時(shí)的采樣點(diǎn)權(quán)值；ωc為計(jì)算協(xié)方差時(shí)的采樣點(diǎn)權(quán)值，上標(biāo)表示第幾個(gè)采樣點(diǎn)。

如此便通過UT變換獲得了一組采樣點(diǎn)(稱為Sigma點(diǎn)集)及其相應(yīng)權(quán)值：

(2)步驟2

將Sigma點(diǎn)集帶入式(13)的系統(tǒng)非線性狀態(tài)方程計(jì)算Sigma點(diǎn)集的一步預(yù)測(cè)值。

(3)步驟3

計(jì)算系統(tǒng)狀態(tài)量的一步預(yù)測(cè)及協(xié)方差陣，由Sigma點(diǎn)集的一步預(yù)測(cè)值加權(quán)求和計(jì)算系統(tǒng)狀態(tài)量的一步預(yù)測(cè)及協(xié)方差陣:

(20)

P(k+1|k)=

(4)步驟4

根據(jù)一步預(yù)測(cè)值，再次使用式(17)進(jìn)行UT變換，產(chǎn)生新的Sigma點(diǎn)集：

X(i)(k+1|k)=

(5)步驟5

將新的Sigma點(diǎn)集帶入線性觀測(cè)方程式(15)，得到系統(tǒng)預(yù)測(cè)的均值和協(xié)方差。

(6)步驟6

預(yù)測(cè)觀測(cè)量均值和協(xié)方差，由觀測(cè)量預(yù)測(cè)值加權(quán)求和得到：

·

根據(jù)式(23)～(25)，由觀測(cè)量預(yù)測(cè)值加權(quán)求和計(jì)算系統(tǒng)預(yù)測(cè)均值及協(xié)方差。

(7)步驟7

由式(26)計(jì)算卡爾曼增益矩陣，由式(27)(28)計(jì)算系統(tǒng)的狀態(tài)更新和協(xié)方差更新：

(26)

(27)

P(k+1)=P(k+1|k)-K(k+1)PZk,ZkKT(k+1)

(28)

3 仿真結(jié)果與分析

仿真參數(shù)設(shè)定如下：

1)磁強(qiáng)計(jì)測(cè)量誤差300nT(3σ)，太陽敏感器測(cè)量誤差1.5°(3σ)。

2)陀螺常值漂移0.001(°)/s，測(cè)量白噪聲0.01(°)/s，隨機(jī)漂移白噪聲0.002(°)/s。

4)軌道高度600 km，軌道傾角97.4°，軌道角速度0.001 083 rad/s。

6)仿真時(shí)間5 000 s，仿真步長0.1 s。

分別仿真Gauss-Newton(GN)、直接UKF濾波(UKF)、Gauss-Newton與UKF濾波組合(GN+UKF)這3種算法，仿真結(jié)果如圖2～5所示。

圖2 姿態(tài)運(yùn)動(dòng)真實(shí)軌跡Fig.2 True trajectory of attitude motion

圖3 GN姿態(tài)確定結(jié)果Fig.3 Attitude determination results of GN

圖4 UKF姿態(tài)確定結(jié)果Fig.4 Attitude determination results of UKF

圖5 GN+UKF姿態(tài)確定結(jié)果Fig.5 Attitude determination results of GN+UKF

GN、UKF、GN+UKF算法的三軸角度和角速度估計(jì)誤差如表1所示。

表1 三種算法估計(jì)誤差對(duì)比

由圖3(c)、圖4(e)、圖5(e)對(duì)比GN、UKF、GN+UKF算法的三軸角度估計(jì)誤差，GN的三軸角度估計(jì)誤差在0.2°以內(nèi)，UKF的三軸角度估計(jì)誤差在0.02°以內(nèi)，精度相比GN提升了10倍，而GN+UKF算法的三軸角度估計(jì)誤差在0.01°以內(nèi)，相比UKF提升了2倍，相比GN提升了20倍。

由圖4(d)、圖5(d)對(duì)比UKF、GN+UKF的三軸角速度估計(jì)誤差，UKF的三軸角速度估計(jì)誤差在0.005(°)/s以內(nèi)，而GN+UKF的三軸角速度估計(jì)誤差在0.000 5(°)/s以內(nèi)，相比UKF提升了10倍。

由此說明，先用Gauss-Newton初步解算，再進(jìn)行UKF濾波的組合算法比單獨(dú)使用這兩種方法精度都要高，而且由于將觀測(cè)量從9維降階為7維，并將觀測(cè)方程轉(zhuǎn)化為線性方程，相比直接UKF還減小了計(jì)算量。

4 結(jié)束語

本文基于磁強(qiáng)計(jì)/太陽敏感器/陀螺的微型低成本姿態(tài)敏感器配置，設(shè)計(jì)了Gauss-Newton迭代算法和UKF濾波相結(jié)合的微小衛(wèi)星姿態(tài)確定算法，既減少了計(jì)算量，又提高了姿態(tài)確定精度，為微小衛(wèi)星提供了一種低成本高精度的姿態(tài)確定方案。

參考文獻(xiàn)(References)

[1] 張晨. 基于星跟蹤器的航天器姿態(tài)確定方法研究[D]. 武漢：華中科技大學(xué)，2005：10-11.

ZHANG C. Research on spacecraft attitude determination method based on star tracker[D]. Wuhan: Huazhong University of Science and Technology，2005：10-11(in Chinese).

[2] 邢艷軍，王永富，陸亞東. 微小衛(wèi)星魯棒自適應(yīng)姿態(tài)確定算法[J]. 中國空間科學(xué)技術(shù)，2015，35(1)：27-35.

XING Y J，WANG Y F，LU Y D. Robust adaptive attitude determination algorithm for micro satellite [J]. Chinese Space Science and Technology，2015，35(1)：27-35(in Chinese).

[3] 王新國，李愛華，許化龍. SUKF在導(dǎo)彈姿態(tài)估計(jì)中的應(yīng)用[J]. 系統(tǒng)工程與電子技術(shù)，2009，31(1)：170-173.

WANG X G，LI A H，XU H L. Application of SUKF in missile attitude estimation[J]. Systems Engineering and Electronics，2009，31(1)：170-173(in Chinese).

[4] WAHBA G. A least-square estimate of spacecraft attitude[J]. SLAM Rev.，1965，409(7)：12.

[5] SHUSTER M D，OH S D. Three-axis attitude determination from vector observations[J]. Journal of Guidance，Control，and Dynamics，1981，4(1)：70-77.

[6] CHOUKROUN D，BAR-ITZHACK I Y， OSHMAN Y. Optimal-request algorithm for attitude determination[J]. Journal of Guidance，Control，and Dynamics，2004，27(3)：418-425.

[7] MARKLEY F L. Attitude determination using vector observations and the singular value decomposition[J]. Journal of the Astronautical Sciences，1987，38(3)：245-258.

[8] MARKLEY F L. Attitude determination from vector observations: a fast optimal matrix algorithm[J]. Journal of the Astronautical Sciences，1993，41(2)：261-280.

[9] MORTARI D. Euler-q algorithm for attitude determination from vector observations[J]. Journal of Guidance Control & Dynamics，1998，21(2)：328-334.

[10] 劉海穎. 微小衛(wèi)星姿態(tài)控制系統(tǒng)關(guān)鍵技術(shù)研究[D]. 南京：南京航空航天大學(xué)，2008：34-35.

LIU H Y. Research on key technologies of micro-satellite attitude control system[D]. Nanjing: Nanjing University of Aeronautics and Astronautics，2008：34-35(in Chinese).

[11] LEFFERTS E J，MARKLEY F L，SHUSTER M D. Kalman filtering for spacecraft attitude estimation[J]. Journal of Guidance Control & Dynamics，1982，5(4)：536-542.

[12] 李鵬，唐健，段廣仁，等. 基于自適應(yīng)聯(lián)邦濾波的衛(wèi)星姿態(tài)確定[J]. 中國空間科學(xué)技術(shù)，2013，33(2)：67-71.

LI P，TANG J，DUAN G R，et al. Satellite attitude determination based on adaptive federal filter[J]. Chinese Space Science and Technology，2013，33(2)：67-71(in Chinese).

[13] JULIER S J，UHLMANN J K. New extension of the Kalman filter to nonlinear systems[J]. Proceedings of SPIE-The International Society for Optical Engineering，1999，3068：182-193.

[14] 徐成剛. 基于非線性預(yù)測(cè)濾波和UKF的狀態(tài)估計(jì)方法[J]. 科技信息，2013(19)：174-176.

XU C G. A state estimation method based on nonlinear predictive filtering and UKF[J]. Science and Technology Information，2013(19)：174-176(in Chinese).

[15] 楊斌，徐廣涵，靳瑾，等. 磁定姿近地軌道衛(wèi)星EKF與UKF算法比較[J]. 中國空間科學(xué)技術(shù)，2012，32(6)：23-30.

YANG B，XU G H，JIN J，et al. Comparisons of EKF and UKF algorithms for near-earth orbit satellites[J]. Chinese Space Science and Technology，2012，32(6)：23-30(in Chinese).

[16] MARINS J L，YUN X P，BACHMANN E R，et al. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[C]. Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems，Hawaii，USA，2001：2003-2011.

[17] 謝祥華. 微小衛(wèi)星姿態(tài)控制系統(tǒng)研究[D]. 南京：南京航空航天大學(xué)，2007：4-5.

XIE X H. Research on microsatellite attitude control system[D]. Nanjing: Nanjing University of Aeronautics and Astronautics，2007：4-5(in Chinese).