陀螺与磁强计组合定姿及陀螺漂移估计

2017-11-25王献忠

航天控制 2017年4期

关键词：磁强计惯性陀螺

王献忠张肖刘艳

1. 上海市空间智能控制技术重点实验室，上海201109 2. 上海航天控制技术研究所，上海201109 3. 上海航天技术研究院, 上海 201109

陀螺与磁强计组合定姿及陀螺漂移估计

王献忠1,2张肖1,2刘艳3

1. 上海市空间智能控制技术重点实验室，上海201109 2. 上海航天控制技术研究所，上海201109 3. 上海航天技术研究院, 上海 201109

随着微陀螺和微磁强计在小卫星上得到广泛应用，陀螺和磁强计组合定姿研究越来越受到重视。首先利用磁强计测得的前后时刻磁场强度，基于双矢量定姿确定本体相对惯性系姿态，并与陀螺积分姿态比较得到姿态误差；其次基于地磁场矢量修正后的姿态误差校正陀螺积分姿态，并基于PI滤波估计陀螺漂移；最后进行了仿真验证，结果表明该方法可以有效估计陀螺漂移，姿态确定精度在1°左右。

陀螺；磁强计；组合定姿；漂移估计；双矢量定姿

低轨卫星在轨运行期间通过三轴磁强计测得的地磁场矢量与应用国际地磁场模型(IGRF)计算得到的地磁场矢量来比较估计卫星姿态，在某一时刻单独利用磁强计只能确定二轴姿态，要确定三轴姿态至少需要具有一定夹角的双矢量观测，如与太阳敏感器测得的太阳矢量、地平仪测得的地心矢量等组合定姿。

单独利用磁强计不能连续确定三轴姿态，但可以基于轨道运动断续获取三轴姿态，其在定姿应用上有一定的局限性，通常将磁强计与陀螺组合进行定姿。

随着微电子、微机械等新技术的发展，航天产品越来越小型化，如具备三轴角速率测量功能的硅微陀螺只有几十克，具备三轴磁场强度测量功能的磁强计甚至更轻，这些小型化的产品在小卫星上得到了广泛应用，出现了纳星、皮星等微小卫星。

由陀螺测得的三轴姿态角速率积分也可以确定卫星姿态，但其测量精度受陀螺角速率漂移影响，只能用于短期姿态确定。将陀螺与磁强计组合，利用磁强计的测量值估算和修正陀螺角速率漂移，同时利用陀螺弥补磁强计定姿实时性差的缺点。

文献[1-6]基于卡尔曼滤波进行陀螺与磁强计组合定姿。文献[7] 研究了利用UKF(Unscented Kalman Filter)处理地磁场测量数据，进行低轨道(LEO)卫星自主定姿的算法。文献[8]对扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)磁强计组合定姿算法进行了综合分析和对比。

工程应用中还必须考虑敏感器测量误差对姿态确定精度的影响，本文首先利用磁强计测得的前后时刻磁场强度，及理论计算的地磁场矢量，基于双矢量定姿确定本体相对惯性系姿态；其次将其与陀螺积分姿态比较得到姿态误差；考虑到姿态误差在垂直于地磁场矢量方向敏感性强，而沿地磁场矢量方向敏感性弱，在用姿态误差修正陀螺积分四元数时减小其沿地磁场矢量方向的权重；最后基于地磁场矢量修正后的姿态误差校正陀螺积分姿态，并基于PI滤波估计陀螺漂移。

1 地磁场强度计算

考虑星载计算机的计算能力，地磁场的球谐波模型取一阶近似如下：

(1)

(2)

(3)

则地固系下地磁场矢量为：

(4)

将地固系磁场强度转换到惯性系：

Bi=Aie·Be

(5)

其中，Aie为地固系到惯性系转换矩阵。

2 基于磁强计前后时刻二次测量双矢量定姿

2.1 不同轨道位置惯性系地磁场矢量

随着卫星轨道运动，不同位置处的地磁场矢量在惯性系中的方向不同，图1中Bi,k和Bi,k-1为惯性系前后时刻地磁场矢量。

图1 前后时刻惯性系地磁场矢量示意图

2.2 前后时刻本体系地磁场矢量

设前后时刻本体系陀螺测得的经零位修正后的角速率分别为ωb′i,k-1和ωbi,k，则前后时刻陀螺积分姿态增量dQbb′：

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

(6)

其中，T为积分步长。

求得姿态四元数增量dqbb′：

(7)

假设磁强计沿星体平行安装，磁强计测得的是不同时刻本体系相对惯性系的磁场强度，设前后时刻磁强计测得的本体系地磁场分别Bb′,k-1和Bb,k，将前一时刻本体系地磁场转换到当前时刻本体系：

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

(8)

其中，Abb′为前一时刻本体系到当前时刻本体系的转换矩阵，可由姿态四元数增量dqbb′求得。

前后时刻磁场测量值在当前时刻本体系投影Bb,k-1和Bb,k如图2所示。

图2 前后时刻磁场测量值在当前时刻本体系投影示意图

2.3 基于双矢量定姿确定本体相对惯性系姿态

不考虑磁强计的测量误差，同一时刻的Bb,k-1和Bi,k-1，及Bb,k和Bi,k在空间的指向是相同的，可以基于双矢量确定本体相对惯性系的姿态。

令：

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

(9)

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

(10)

基于惯性系地磁场理论计算值构造标称矢量阵Ms：

(11)

基于磁强计测量计算的本体系磁场强度构造标称矢量阵Mr：

(12)

设惯性系到本体系姿态转换矩阵为Abi，则：

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

求得姿态转换矩阵Abi：

基于3-1-2转序求得姿态转换矩阵：

其中，Rx(φ)，Ry(θ)，Rz(ψ)分别为绕X轴、Y轴和Z轴转动的转换矩阵。

如果|a23|gt;0.9999，求得姿态角为：

否则，

由姿态角求姿态四元数qbi：

3 姿态误差四元数计算及姿态修正

基于陀螺积分一步预报姿态四元数qbi,k/k-1：

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

(13)

其中，dqbb′为陀螺积分求得的姿态四元数增量，由式(7)解算；qbi,k-1为上一节修正后的本体系相对惯性系的陀螺积分四元数。

设姿态误差四元数为dq，基于双矢量定姿求得的本体相对惯性系姿态四元数为qbi，得：

qbi,k/k-1=dq⊗qbi

(14)

由此求得姿态误差四元数dq：

(15)

令：

(16)

设dQ与Bb矢量间夹角为α：

(17)

考虑到姿态误差在垂直于地磁场Bb矢量方向敏感性强，沿地磁场Bb矢量方向敏感性弱，因此对姿态误差四元数dq进一步修正如下：

dQe=k0·sinα·dQ

(18)

其中，k0为误差修正强弱系数。

求得修正后姿态误差四元数dqe：

(19)

基于修正后姿态四元数误差校正本体相对惯性系姿态四元数如下：

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

(20)

4 基于PI滤波估计陀螺漂移估计

PI滤波估计陀螺漂移如下：

(21)

其中，dQe由式(18)解算，kp为比例系数，ki为积分系数。

陀螺角速率漂移修正：

ωbi,k=ωbi-dω

(22)

其中，ωbi为陀螺当前节拍测量值。

5 算例仿真验证

1)算例1

磁强计三轴常值偏差100nT，噪声100nT；陀螺三轴常值漂移分别为0.005(°)/s，0.003(°)/s，0.002(°)/s，三轴噪声为0.001(°)/s；三轴初始姿态误差均为5°。姿态角测量误差如图3所示，陀螺漂移估计如图4所示。

图3 姿态角误差曲线

图4 陀螺漂移估计曲线

从仿真结果可以看出，基于上述单机测量误差及初始偏差情况下，陀螺+磁强计组合定姿收敛后姿态确定精度优于1°，且估计的陀螺三轴零位与预设值基本一致。

2)算例2

增大磁强计的常值偏差和噪声均为1000nT；三轴初始姿态误差分别为50°，50°，50°。仿真过程中考虑到磁强计噪声增大而相应增强滤波作用，最终姿态角确定误差如图5所示。

图5 基于粗精度磁强计组合定姿误差曲线

从图5的仿真结果可以看出，磁强计测量误差增大，且在初始大姿态误差情况下基于陀螺+磁强计的组合定姿仍能收敛。

6 结论

利用磁强计测得的前后时刻磁场强度，基于双矢量定姿确定本体相对惯性系姿态，并与陀螺积分姿态比较得到姿态误差。将姿态误差权重修正后对陀螺积分姿态进行修正，同时基于PI滤波估计陀螺漂移，形成了适合于星上应用的陀螺与磁强计组合定姿算法。仿真试验结果表明，基于当前磁强计测量精度在正常初始姿态偏差情况下的姿态确定精度为1°左右，在磁强计测量偏差增大10倍且初始姿态异常情况下仍能收敛稳定。适合在无高精度定姿要求的纳星、皮星等微小卫星上应用，也可以作为其它中大型卫星姿态确定的备份策略。

[1] 张锐, 朱振才, 张静. 基于磁强计的微小卫星姿态确定[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] 田菁, 吴美平, 胡小平. 三轴磁强计姿态确定[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] 周健, 朱振才, 王健宇. 利用磁强计数据确定卫星三轴姿态的方法[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] 朱建丰, 徐世杰. 基于地磁场测量估计卫星姿态的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] 杨斌, 徐广涵, 靳瑾，等. 磁定姿近地轨道卫星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