陀螺與磁強(qiáng)計(jì)組合定姿及陀螺漂移估計(jì)

王獻(xiàn)忠 張 肖 劉 艷

1. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海201109 2. 上海航天控制技術(shù)研究所，上海201109 3. 上海航天技術(shù)研究院, 上海 201109

陀螺與磁強(qiáng)計(jì)組合定姿及陀螺漂移估計(jì)

王獻(xiàn)忠1,2張 肖1,2劉 艷3

1. 上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海201109 2. 上海航天控制技術(shù)研究所，上海201109 3. 上海航天技術(shù)研究院, 上海 201109

隨著微陀螺和微磁強(qiáng)計(jì)在小衛(wèi)星上得到廣泛應(yīng)用，陀螺和磁強(qiáng)計(jì)組合定姿研究越來(lái)越受到重視。首先利用磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度，基于雙矢量定姿確定本體相對(duì)慣性系姿態(tài)，并與陀螺積分姿態(tài)比較得到姿態(tài)誤差；其次基于地磁場(chǎng)矢量修正后的姿態(tài)誤差校正陀螺積分姿態(tài)，并基于PI濾波估計(jì)陀螺漂移；最后進(jìn)行了仿真驗(yàn)證，結(jié)果表明該方法可以有效估計(jì)陀螺漂移，姿態(tài)確定精度在1°左右。

陀螺；磁強(qiáng)計(jì)；組合定姿；漂移估計(jì)；雙矢量定姿

低軌衛(wèi)星在軌運(yùn)行期間通過(guò)三軸磁強(qiáng)計(jì)測(cè)得的地磁場(chǎng)矢量與應(yīng)用國(guó)際地磁場(chǎng)模型(IGRF)計(jì)算得到的地磁場(chǎng)矢量來(lái)比較估計(jì)衛(wèi)星姿態(tài)，在某一時(shí)刻單獨(dú)利用磁強(qiáng)計(jì)只能確定二軸姿態(tài)，要確定三軸姿態(tài)至少需要具有一定夾角的雙矢量觀測(cè)，如與太陽(yáng)敏感器測(cè)得的太陽(yáng)矢量、地平儀測(cè)得的地心矢量等組合定姿。

單獨(dú)利用磁強(qiáng)計(jì)不能連續(xù)確定三軸姿態(tài)，但可以基于軌道運(yùn)動(dòng)斷續(xù)獲取三軸姿態(tài)，其在定姿應(yīng)用上有一定的局限性，通常將磁強(qiáng)計(jì)與陀螺組合進(jìn)行定姿。

隨著微電子、微機(jī)械等新技術(shù)的發(fā)展，航天產(chǎn)品越來(lái)越小型化，如具備三軸角速率測(cè)量功能的硅微陀螺只有幾十克，具備三軸磁場(chǎng)強(qiáng)度測(cè)量功能的磁強(qiáng)計(jì)甚至更輕，這些小型化的產(chǎn)品在小衛(wèi)星上得到了廣泛應(yīng)用，出現(xiàn)了納星、皮星等微小衛(wèi)星。

由陀螺測(cè)得的三軸姿態(tài)角速率積分也可以確定衛(wèi)星姿態(tài)，但其測(cè)量精度受陀螺角速率漂移影響，只能用于短期姿態(tài)確定。將陀螺與磁強(qiáng)計(jì)組合，利用磁強(qiáng)計(jì)的測(cè)量值估算和修正陀螺角速率漂移，同時(shí)利用陀螺彌補(bǔ)磁強(qiáng)計(jì)定姿實(shí)時(shí)性差的缺點(diǎn)。

文獻(xiàn)[1-6]基于卡爾曼濾波進(jìn)行陀螺與磁強(qiáng)計(jì)組合定姿。文獻(xiàn)[7] 研究了利用UKF(Unscented Kalman Filter)處理地磁場(chǎng)測(cè)量數(shù)據(jù)，進(jìn)行低軌道(LEO)衛(wèi)星自主定姿的算法。文獻(xiàn)[8]對(duì)擴(kuò)展卡爾曼濾波(EKF)和無(wú)跡卡爾曼濾波(UKF)磁強(qiáng)計(jì)組合定姿算法進(jìn)行了綜合分析和對(duì)比。

工程應(yīng)用中還必須考慮敏感器測(cè)量誤差對(duì)姿態(tài)確定精度的影響，本文首先利用磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度，及理論計(jì)算的地磁場(chǎng)矢量，基于雙矢量定姿確定本體相對(duì)慣性系姿態(tài)；其次將其與陀螺積分姿態(tài)比較得到姿態(tài)誤差；考慮到姿態(tài)誤差在垂直于地磁場(chǎng)矢量方向敏感性強(qiáng)，而沿地磁場(chǎng)矢量方向敏感性弱，在用姿態(tài)誤差修正陀螺積分四元數(shù)時(shí)減小其沿地磁場(chǎng)矢量方向的權(quán)重；最后基于地磁場(chǎng)矢量修正后的姿態(tài)誤差校正陀螺積分姿態(tài)，并基于PI濾波估計(jì)陀螺漂移。

1 地磁場(chǎng)強(qiáng)度計(jì)算

考慮星載計(jì)算機(jī)的計(jì)算能力，地磁場(chǎng)的球諧波模型取一階近似如下：

(1)

(2)

(3)

則地固系下地磁場(chǎng)矢量為：

(4)

將地固系磁場(chǎng)強(qiáng)度轉(zhuǎn)換到慣性系：

Bi=Aie·Be

(5)

其中，Aie為地固系到慣性系轉(zhuǎn)換矩陣。

2 基于磁強(qiáng)計(jì)前后時(shí)刻二次測(cè)量雙矢量定姿

2.1 不同軌道位置慣性系地磁場(chǎng)矢量

隨著衛(wèi)星軌道運(yùn)動(dòng)，不同位置處的地磁場(chǎng)矢量在慣性系中的方向不同，圖1中Bi,k和Bi,k-1為慣性系前后時(shí)刻地磁場(chǎng)矢量。

圖1 前后時(shí)刻慣性系地磁場(chǎng)矢量示意圖

2.2 前后時(shí)刻本體系地磁場(chǎng)矢量

設(shè)前后時(shí)刻本體系陀螺測(cè)得的經(jīng)零位修正后的角速率分別為ωb′i,k-1和ωbi,k，則前后時(shí)刻陀螺積分姿態(tài)增量dQbb′：

dQbb′=[ωb′i,k-1+(ωbi,k-ωb′i,k-1)/2]·T/2

(6)

其中，T為積分步長(zhǎng)。

求得姿態(tài)四元數(shù)增量dqbb′：

(7)

假設(shè)磁強(qiáng)計(jì)沿星體平行安裝，磁強(qiáng)計(jì)測(cè)得的是不同時(shí)刻本體系相對(duì)慣性系的磁場(chǎng)強(qiáng)度，設(shè)前后時(shí)刻磁強(qiáng)計(jì)測(cè)得的本體系地磁場(chǎng)分別Bb′,k-1和Bb,k，將前一時(shí)刻本體系地磁場(chǎng)轉(zhuǎn)換到當(dāng)前時(shí)刻本體系：

Bb,k-1=Abb′·Bb′,k-1

(8)

其中，Abb′為前一時(shí)刻本體系到當(dāng)前時(shí)刻本體系的轉(zhuǎn)換矩陣，可由姿態(tài)四元數(shù)增量dqbb′求得。

前后時(shí)刻磁場(chǎng)測(cè)量值在當(dāng)前時(shí)刻本體系投影Bb,k-1和Bb,k如圖2所示。

圖2 前后時(shí)刻磁場(chǎng)測(cè)量值在當(dāng)前時(shí)刻 本體系投影示意圖

2.3 基于雙矢量定姿確定本體相對(duì)慣性系姿態(tài)

不考慮磁強(qiáng)計(jì)的測(cè)量誤差，同一時(shí)刻的Bb,k-1和Bi,k-1，及Bb,k和Bi,k在空間的指向是相同的，可以基于雙矢量確定本體相對(duì)慣性系的姿態(tài)。

令：

u1=Bb,k-1，u2=Bb,k

(9)

v1=Bi,k-1，v2=Bi,k

(10)

基于慣性系地磁場(chǎng)理論計(jì)算值構(gòu)造標(biāo)稱(chēng)矢量陣Ms：

(11)

基于磁強(qiáng)計(jì)測(cè)量計(jì)算的本體系磁場(chǎng)強(qiáng)度構(gòu)造標(biāo)稱(chēng)矢量陣Mr：

(12)

設(shè)慣性系到本體系姿態(tài)轉(zhuǎn)換矩陣為Abi，則：

u1=Abi·v1，u2=Abi·v2，Mr=Abi·Ms。

求得姿態(tài)轉(zhuǎn)換矩陣Abi：

基于3-1-2轉(zhuǎn)序求得姿態(tài)轉(zhuǎn)換矩陣：

其中，Rx(φ)，Ry(θ)，Rz(ψ)分別為繞X軸、Y軸和Z軸轉(zhuǎn)動(dòng)的轉(zhuǎn)換矩陣。

如果|a23|gt;0.9999，求得姿態(tài)角為：

否則，

由姿態(tài)角求姿態(tài)四元數(shù)qbi：

3 姿態(tài)誤差四元數(shù)計(jì)算及姿態(tài)修正

基于陀螺積分一步預(yù)報(bào)姿態(tài)四元數(shù)qbi,k/k-1：

qbi,k/k-1=dqbb′?qbi,k-1

(13)

其中，dqbb′為陀螺積分求得的姿態(tài)四元數(shù)增量，由式(7)解算；qbi,k-1為上一節(jié)修正后的本體系相對(duì)慣性系的陀螺積分四元數(shù)。

設(shè)姿態(tài)誤差四元數(shù)為dq，基于雙矢量定姿求得的本體相對(duì)慣性系姿態(tài)四元數(shù)為qbi，得：

qbi,k/k-1=dq?qbi

(14)

由此求得姿態(tài)誤差四元數(shù)dq：

(15)

令：

(16)

設(shè)dQ與Bb矢量間夾角為α：

(17)

考慮到姿態(tài)誤差在垂直于地磁場(chǎng)Bb矢量方向敏感性強(qiáng)，沿地磁場(chǎng)Bb矢量方向敏感性弱，因此對(duì)姿態(tài)誤差四元數(shù)dq進(jìn)一步修正如下：

dQe=k0·sinα·dQ

(18)

其中，k0為誤差修正強(qiáng)弱系數(shù)。

求得修正后姿態(tài)誤差四元數(shù)dqe：

(19)

基于修正后姿態(tài)四元數(shù)誤差校正本體相對(duì)慣性系姿態(tài)四元數(shù)如下：

qbi,k=dqe?qbi,k/k-1

(20)

4 基于PI濾波估計(jì)陀螺漂移估計(jì)

PI濾波估計(jì)陀螺漂移如下：

(21)

其中，dQe由式(18)解算，kp為比例系數(shù)，ki為積分系數(shù)。

陀螺角速率漂移修正：

ωbi,k=ωbi-dω

(22)

其中，ωbi為陀螺當(dāng)前節(jié)拍測(cè)量值。

5 算例仿真驗(yàn)證

1)算例1

磁強(qiáng)計(jì)三軸常值偏差100nT，噪聲100nT；陀螺三軸常值漂移分別為0.005(°)/s，0.003(°)/s，0.002(°)/s，三軸噪聲為0.001(°)/s；三軸初始姿態(tài)誤差均為5°。姿態(tài)角測(cè)量誤差如圖3所示，陀螺漂移估計(jì)如圖4所示。

圖3 姿態(tài)角誤差曲線(xiàn)

圖4 陀螺漂移估計(jì)曲線(xiàn)

從仿真結(jié)果可以看出，基于上述單機(jī)測(cè)量誤差及初始偏差情況下，陀螺+磁強(qiáng)計(jì)組合定姿收斂后姿態(tài)確定精度優(yōu)于1°，且估計(jì)的陀螺三軸零位與預(yù)設(shè)值基本一致。

2)算例2

增大磁強(qiáng)計(jì)的常值偏差和噪聲均為1000nT；三軸初始姿態(tài)誤差分別為50°，50°，50°。仿真過(guò)程中考慮到磁強(qiáng)計(jì)噪聲增大而相應(yīng)增強(qiáng)濾波作用，最終姿態(tài)角確定誤差如圖5所示。

圖5 基于粗精度磁強(qiáng)計(jì)組合定姿誤差曲線(xiàn)

從圖5的仿真結(jié)果可以看出，磁強(qiáng)計(jì)測(cè)量誤差增大，且在初始大姿態(tài)誤差情況下基于陀螺+磁強(qiáng)計(jì)的組合定姿仍能收斂。

6 結(jié)論

利用磁強(qiáng)計(jì)測(cè)得的前后時(shí)刻磁場(chǎng)強(qiáng)度，基于雙矢量定姿確定本體相對(duì)慣性系姿態(tài)，并與陀螺積分姿態(tài)比較得到姿態(tài)誤差。將姿態(tài)誤差權(quán)重修正后對(duì)陀螺積分姿態(tài)進(jìn)行修正，同時(shí)基于PI濾波估計(jì)陀螺漂移，形成了適合于星上應(yīng)用的陀螺與磁強(qiáng)計(jì)組合定姿算法。仿真試驗(yàn)結(jié)果表明，基于當(dāng)前磁強(qiáng)計(jì)測(cè)量精度在正常初始姿態(tài)偏差情況下的姿態(tài)確定精度為1°左右，在磁強(qiáng)計(jì)測(cè)量偏差增大10倍且初始姿態(tài)異常情況下仍能收斂穩(wěn)定。適合在無(wú)高精度定姿要求的納星、皮星等微小衛(wèi)星上應(yīng)用，也可以作為其它中大型衛(wèi)星姿態(tài)確定的備份策略。

[1] 張銳, 朱振才, 張靜. 基于磁強(qiáng)計(jì)的微小衛(wèi)星姿態(tài)確定[J]. 宇航學(xué)報(bào), 2009, 27(5): 43-47. (Zhang Rui, Zhu Zhencai, Zhang Jing. Micro-Satellite Attitude Determination Based on Magnetometer[J]. Journal of Astronautics, 2009, 27(5): 43-47.)

[2] 張曉霞, 曹詠弘. 基于磁強(qiáng)計(jì)/陀螺的卡爾曼濾波定姿算法[J]. 探測(cè)與控制學(xué)報(bào), 2011, 33(4): 48-50,55. (Zhang Xiaoxia, Cao Yonghong. Kalman Filtering of Attitude Measurement with Magnetometer and Gyroscope[J]. Journal of Detection amp; Control, 2011, 33(4): 48-50,55.)

[3] 王獻(xiàn)忠. 陀螺與磁強(qiáng)計(jì)組合定姿及工程應(yīng)用分析[J]. 航天控制, 2009, 27(5): 43-47. (Wang Xianzhong. Attitude Determination and Analysis for Engineering Application Based on Gyroscope and Magnetometer[J]. Aerospace Control, 2009, 27(5): 43-47.)

[4] 田菁, 吳美平, 胡小平. 三軸磁強(qiáng)計(jì)姿態(tài)確定[J]. 國(guó)防科技大學(xué)學(xué)報(bào), 2001, 23(5): 17-21. (Tian Jing, Wu Meiping, Hu Xiaopin. Three-Axis Magnetometer Attitude Determination[J]. Journal of National University of Defense Technology, 2001, 23(5): 17-21.)

[5] 周健, 朱振才, 王健宇. 利用磁強(qiáng)計(jì)數(shù)據(jù)確定衛(wèi)星三軸姿態(tài)的方法[J]. 宇航學(xué)報(bào)，2001，22(2)：106-110.(Zhou Jian, Zhu Zhencai, Wang Jianyu. Attitude Determination with Magnetometer-only[J]. Journal of Astronautics, 2001, 22(2): 106-110.)

[6] 李果. 磁強(qiáng)計(jì)測(cè)量卡爾曼濾波定姿方法研究[J]. 控制工程, 1995, 2(4): 20-26. (Li Guo. Research on Attitude Determination Based on Magnetometer and EKF[J]. Control Engineering of China, 1995, 2(4): 20-26.)

[7] 朱建豐, 徐世杰. 基于地磁場(chǎng)測(cè)量估計(jì)衛(wèi)星姿態(tài)的UKF算法[J]. 宇航學(xué)報(bào), 2006, 27(6): 1401-1405. (Zhu Jianfeng, Xu Shijie. UKF for Satellite Attitude Estimation Based on Geomagnetic Field[J]. Journal of Astronautics, 2006, 27(6): 1401-1405.)

[8] 楊斌, 徐廣涵, 靳瑾，等. 磁定姿近地軌道衛(wèi)星EKF與UKF算法比較[J]. 中國(guó)空間科學(xué)技術(shù), 2012, 32(6): 23-30,38. (Yang Bin, Xu Guanghan, Jin Jin，et al. Comparison on EKF and UKF for Geomagnetic Attitude Estimation of LEO Satellites[J]. Chinese Space Science and Technology, 2012, 32(6): 23-30,38.)

AttitudeDeterminationandDriftEstimationBasedonGyroandMagnetometer

Wang Xianzhong1,2, Zhang Xiao1,2, Liu Yan3

1. Shanghai Key Laboratory of Aerospace Intelligent Control Technology, Shanghai 201109, China 2. Shanghai Aerospace Control Technology Institute, Shanghai 201109, China 3. Shanghai Academy of Spaceflight Technology, Shanghai 201109, China

Withmicro-gyrosandmicro-magnetometersarewidelyappliedtosmallsatellites,moreandmoreattentionispaidtotheresearchoftheintegratedattitudedeterminationbasedonthemicro-gyroandthemicro-magnetometer.Firstly,usingdoublegeomagneticfieldvectorsmeasuredatpreviousmomentandcurrentmomentbymagnetometers,theattitudeofthebodywithrespecttotheinertialsystemisdetermined,andtheattitudeerrorisgainedbycomparingwiththeintegratedattitudeofthegyro.Then,theintegratedattitudeofthegyroiscorrectedbytheattitudeerrorrevisedbasedongeomagneticfieldvectorandthegyrodriftisestimatedbasedonPIfilter.Thesimulationresultshowsthatthegyrodriftcanbeestimatedeffectivelyandtheattitudedeterminationprecisionisabout1°byusingthismethod.

Gyro;Magnetometer;Integratedattitudedetermination;Driftestimation;Double-vectorattitudedetermination

V44

A

1006-3242(2017)04-0015-05

2016-06-27

王獻(xiàn)忠(1971-)，男，江蘇泰倉(cāng)人，博士，研究員，主要研究方向?yàn)轱w行器控制系統(tǒng)設(shè)計(jì)；張肖(1981-)，女，浙江舟山人，碩士，高級(jí)工程師，主要研究方向?yàn)轱w行器姿態(tài)控制系統(tǒng)設(shè)計(jì)；劉艷(1980-)，女，安徽蕭縣人，碩士，高級(jí)工程師，主要研究方向?yàn)轱w行器控制系統(tǒng)設(shè)計(jì)。