陀螺与磁强计组合定姿及漂移估计算法优化

王献忠 张 肖 刘 艳

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

0 引言

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

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

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

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

文献[1]基于陀螺、太阳敏感器、磁强计和星敏感器4种敏感器，研究了微小卫星在速率阻尼模式、太阳捕获模式、对日对地定向及维持模式、试验模式下的系统建模方法,以及基于Unscented卡尔曼滤波(UKF)的组合定姿方法。文献[2]基于卫星运动学建立状态方程,应用EKF进行陀螺与磁强计组合定姿研究。文献[3]基于低成本低精度的MEMS陀螺、太阳敏感器、磁强计组合，设计了Unscented Kalman融合滤波算法。文献[4-5]仅利用三轴磁强计作为测量设备，分别通过EKF和UKF处理测量数据,实时地估计无陀螺小卫星的姿态角与角速度。文献[6]提出了一种利用梯度下降法对加速度计和磁强计的数据进行姿态解算，并采用互补滤波算法将其与陀螺仪积分得到的结果进行融合的定姿方法。文献[7]基于四元数的卡尔曼滤波进行陀螺与磁强计组合定姿。

以上算法多是采用EKF、或UKF或其它比较复杂的滤波算法进行姿态估计，工程应用中还必须考虑姿态确定算法复杂度对软件运行存贮空间和运行时间的影响，基于相同的敏感器获得同等的姿态确定精度，姿态确定算法越简单越适合于工程应用；对于仅采用磁强计作为观测量的算法由于其是基于轨道运动断续获取三轴姿态，在使用上有一定的局限性。本文是在文献[8]的基础上进一步改进，首先利用磁强计测得的当前时刻磁场强度，与卫星轨道和姿态角速率推算的当前时刻磁场强度比较，估计陀螺积分姿态误差；接着利用磁强计测得的前后时刻磁场强度，与基于卫星轨道和姿态角速率推算的前后时刻磁场强度比较，估计姿态推算误差；然后基于前后时刻磁场强度估计的姿态误差校正陀螺积分姿态，并基于PI滤波估计陀螺漂移；最后基于当前磁强计的一般测量精度并同时考虑常值误差和噪声进行仿真验证。

1 轨道地磁场推算及变化规律

1.1 基于轨道推算的惯性系地磁场强度

基于轨道根数计算地固系磁场强度推算用轨道位置参数：

(1)

(2)

η=π/2-arcsin(sinu·sini)

(3)

其中：a为轨道半长轴;e为轨道偏心率;f为真近点角;Ω为升交点赤经;u为轨道纬度幅角，i轨道倾角;Sg为格林尼治恒星时。

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

(4)

(5)

(6)

(7)

(8)

(9)

将地固系磁场强度转换到惯性系：Bci=Aie·Bce；其中，Aie为地固系到惯性系转换矩阵。

1.2 陀螺推算的本体系磁场强度

假定陀螺坐标系与卫星本体系一致，基于修正漂移后的陀螺角速率，求得前后节拍姿态变化四元数ΔQω：

(10)

Δqω=[(ωk-ωk-1)/2+ωk-1]·T/2

(11)

(12)

其中：ωk为陀螺输出的当前节拍本体系角速率，ωk-1为陀螺输出的前一节拍本体系角速率。

基于前一节拍修正后的本体相对惯性系姿态四元数Qbi,k-1，推算当前节拍本体相对惯性系姿态四元数Qbi,k/k-1：

1.3 磁场强度变化及规律

考虑星载计算机的计算能力，地磁场的球谐波模型取一阶近似得到地固系下地磁场矢量Bce，近似如下：

(13)

By=0

(14)

(15)

卫星从南纬最高纬度到北纬最高纬度升轨飞行，X轴和Z轴磁场强度都变化了1周，卫星从北纬最高纬度到南纬最高纬度降轨飞行，X轴和Z轴磁场强度也都变化了1周。

姿态保持惯性定向，地磁场每轨平均以2倍的轨道角速率变化；姿态保持对地定向，姿态按轨道角速率变化，地磁场每轨平均以1倍的轨道角速率变化。地磁场在卫星本体系指向随卫星轨道运动和姿态转动变化。

2 基于磁场强度估计姿态误差

2.1 基于当前时刻磁场强度估计姿态误差

假定磁强计坐标系与本体系一致，磁强计输出的本体系磁场强度和基于轨道及陀螺推算的本体系磁场强度如图1，求得姿态四元数误差Δqe1，其矢量部分ΔQe1由式(16)计算得到：

图1 基于当前节拍磁场强度估计姿态误差

(16)

其中：Bcb,k/k-1为基于轨道及姿态推算的磁场强度;Bmb,k为当前节拍磁强计测得的磁场强度。

2.2 基于前后时刻磁场强度估计姿态误差

磁强计测得的前后时刻磁场强度如图2，求得前后节拍磁场强度等效的姿态变化四元数Δqm，其矢量部分ΔQm由式(17)计算得到：

(17)

其中：Bmb,k-1为前一节拍磁强计测得的磁场强度;Bmb,k为当前节拍磁强计测得的磁场强度。

图2 磁强计测得的前后节拍磁场强度

基于轨道位置推算的前后节拍磁场强度如图3，求得前后节拍轨道位置变化导致的姿态变化四元数Δqr，其矢量部分ΔQr由式(18)计算得到：

图3 轨道位置导致的前后节拍磁场强度变化

(18)

其中：Bce,k-1为基于轨道推算的前一节拍地固系磁场强度;Bce,k为基于轨道推算的当前节拍地固系磁场强度。

轨道位置和姿态角速率导致的前后节拍姿态变化四元数ΔQc：

ΔQc=ΔQω⊗ΔQr

(19)

磁强计测得的前后节拍姿态变化由轨道位置、姿态运动和姿态推算误差导致，求得姿态推算误差四元数ΔQe2：

(20)

2.3 姿态四元数误差修正

基于磁强计前后时刻磁场强计估计的姿态误差四元数，求得姿态误差四元数ΔQe：

ΔQe=ΔQe1⊗ΔQe2

(21)

考虑误差为小量，简化为：

(22)

姿态四元数误差修正量：

(23)

3 陀螺漂移估计

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

(24)

其中：kp为比例系数;ki为积分系数。

陀螺角速率漂移修正：

ωbi,k=ωbi-Δω

(25)

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

4 算例仿真验证

磁强计三轴常值偏差100nT，噪声100nT；陀螺三轴常值漂移分别为0.008(°)/s、0.004(°)/s、-0.006(°)/s，三轴噪声0.001(°)/s；三轴初始姿态误差都为5°。姿态角测量误差如图4和图5所示，陀螺漂移估计如图6和图7所示。

图4 姿态角测量误差曲线

图5 姿态角测量误差曲线末端放大曲线

图6 陀螺漂移估计曲线

图7 陀螺漂移估计放大曲线

磁强计常值偏差1000nT，噪声1000nT，噪声增大滤波作用要相应加强；三轴初始姿态误差分别为50°、50°、50°，磁强计增大测量误差在初始大姿态误差下仍能收敛，如图8和图9所示。

图8 姿态角测量误差曲线

图9 姿态角测量误差曲线末端放大曲线

5 结束语

利用磁强计测得的前后时刻磁场强度，与卫星轨道和姿态角速率推算的磁场强度比较，估计陀螺积分姿态误差；基于磁强计估计的姿态误差校正陀螺积分姿态，并基于PI滤波估计陀螺漂移，设计了适合于星上应用的陀螺与磁强计组合定姿算法。仿真试验结果表明姿态确定精度1°左右，适合在纳星、皮星等微小卫星上应用。