,,,
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ì)算量。
設(shè)q為軌道坐標(biāo)系到星體坐標(biāo)系的四元數(shù),則由四元數(shù)來描述星體系相對(duì)于軌道系的姿態(tài),得到衛(wèi)星姿態(tài)運(yùn)動(dòng)學(xué)方程如下:
(1)
不含撓性部件的微小衛(wèi)星可視為剛體,其相對(duì)慣性系的動(dòng)力學(xué)方程為:
式中:I為衛(wèi)星的慣量矩陣;Tb為作用于衛(wèi)星的總力矩:
(4)
確定性算法直接根據(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
矢量觀測(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ī)化。
(13)
(14)
Z=I7×7X+V
(15)
(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)
仿真參數(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,仿真步長(zhǎng)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ì)算量。
本文基于磁強(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).