王獻忠 張 肖 劉 艷

1. 上海市空間智能控制技術重點實驗室，上海201109 2. 上海航天控制技術研究所，上海201109 3. 上海航天技術研究院, 上海 201109

陀螺與磁強計組合定姿及陀螺漂移估計

王獻忠1,2張 肖1,2劉 艷3

1. 上海市空間智能控制技術重點實驗室，上海201109 2. 上海航天控制技術研究所，上海201109 3. 上海航天技術研究院, 上海 201109

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

陀螺；磁強計；組合定姿；漂移估計；雙矢量定姿

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

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

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

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

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

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

1 地磁場強度計算

考慮星載計算機的計算能力，地磁場的球諧波模型取一階近似如下：

(1)

(2)

(3)

則地固系下地磁場矢量為：

(4)

將地固系磁場強度轉換到慣性系：

Bi=Aie·Be

(5)

其中，Aie為地固系到慣性系轉換矩陣。

2 基于磁強計前后時刻二次測量雙矢量定姿

2.1 不同軌道位置慣性系地磁場矢量

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

圖1 前后時刻慣性系地磁場矢量示意圖

2.2 前后時刻本體系地磁場矢量

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

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

(6)

其中，T為積分步長。

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

(7)

假設磁強計沿星體平行安裝，磁強計測得的是不同時刻本體系相對慣性系的磁場強度，設前后時刻磁強計測得的本體系地磁場分別Bb′,k-1和Bb,k，將前一時刻本體系地磁場轉換到當前時刻本體系：

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

(8)

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

前后時刻磁場測量值在當前時刻本體系投影Bb,k-1和Bb,k如圖2所示。

圖2 前后時刻磁場測量值在當前時刻 本體系投影示意圖

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

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

令：

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

(9)

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

(10)

基于慣性系地磁場理論計算值構造標稱矢量陣Ms：

(11)

基于磁強計測量計算的本體系磁場強度構造標稱矢量陣Mr：

(12)

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

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

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

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

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

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

否則，

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

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

基于陀螺積分一步預報姿態(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é)修正后的本體系相對慣性系的陀螺積分四元數(shù)。

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

qbi,k/k-1=dq?qbi

(14)

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

(15)

令：

(16)

設dQ與Bb矢量間夾角為α：

(17)

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

dQe=k0·sinα·dQ

(18)

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

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

(19)

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

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

(20)

4 基于PI濾波估計陀螺漂移估計

PI濾波估計陀螺漂移如下：

(21)

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

陀螺角速率漂移修正：

ωbi,k=ωbi-dω

(22)

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

5 算例仿真驗證

1)算例1

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

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

圖4 陀螺漂移估計曲線

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

2)算例2

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

圖5 基于粗精度磁強計組合定姿誤差曲線

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

6 結論

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

[1] 張銳, 朱振才, 張靜. 基于磁強計的微小衛(wèi)星姿態(tài)確定[J]. 宇航學報, 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] 張曉霞, 曹詠弘. 基于磁強計/陀螺的卡爾曼濾波定姿算法[J]. 探測與控制學報, 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] 王獻忠. 陀螺與磁強計組合定姿及工程應用分析[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] 田菁, 吳美平, 胡小平. 三軸磁強計姿態(tài)確定[J]. 國防科技大學學報, 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] 周健, 朱振才, 王健宇. 利用磁強計數(shù)據(jù)確定衛(wèi)星三軸姿態(tài)的方法[J]. 宇航學報，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] 李果. 磁強計測量卡爾曼濾波定姿方法研究[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] 朱建豐, 徐世杰. 基于地磁場測量估計衛(wèi)星姿態(tài)的UKF算法[J]. 宇航學報, 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]. 中國空間科學技術, 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

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