[发明专利]一种基于MEMS传感器的航天器姿态测量方法

摘要

本发明公开了一种基于MEMS传感器的航天器姿态测量方法，步骤如下：系统初始对准；信号采集；陀螺仪姿态更新；双天线GPS/磁强计姿态实时计算；kalman滤波；输出校正。本发明根据航天器轨道空间具有微重力、GPS信号接收效果好的特点，选择三轴MEMS陀螺仪/三轴MEMS磁强计/双天线GPS的组合模式，相对于其他组合方式具有更高测量精度和可靠性。本发明还设计了组合姿态测量算法，采用了算法误差最小的优化三子样旋转矢量算法，补偿了有限转动不可交换误差；为系统建立了一种简单有效的kalman滤波状态空间模型，对两个独立信息源的姿态信息进行了融合，实现了陀螺测姿态和双天线GPS测姿态的优势互补，提高了系统的动态可靠性和精度。

主权项

1.一种基于MEMS传感器的航天器姿态测量方法。所述的航天器姿态测量方法,主要利用GPS双天线、三轴MEMS陀螺仪、三轴MEMS磁强计等传感器。所述姿态测量方法采用地理坐标系（g）作为导航坐标系（n）。将测量系统与航天器载体固连，使得传感器的敏感轴分别位于载体坐标系的三个轴上；所述的姿态测量方法步骤如下：步骤1：系统初始对准；在初始对准时刻t₀，系统利用GPS双天线测量俯仰角横滚角三轴MEMS磁强计测量航向角求得初始时刻的姿态阵和姿态四元数Q(t₀)；具体求解公式如下：Cbn(t0)=cosγ~0cosψ~0+sinγ~0sinψ~0sinθ~0sinψ~0cosθ~0sinγ~0cosψ~0-cosγ~0sinψ~0sinθ~0-cosγ~0sinψ~0+sinγ~0cosψ~0sinθ~0cosψ~0cosθ~0-sinγ~0sinψ~0-cosγ~0cosψ~0sinθ~0-sinγ~0cosθ~0sinθ~0cosγ~0cosθ~0---(1)]]>设初始对准后，姿态阵初值为则姿态四元数的初始值Q(t₀)可以由下面公式确定：q0=121+T11+T22+T33q1=12sign(T32-T23)1+T11-T22-T33q2=12sign(T13-T31)1-T11+T22-T33q3=12sign(T21-T12)1-T11-T22+T33---(2)]]>步骤2：信号采集；输入通道按照一定的采样周期T，采集传感器的输出信号；步骤3：陀螺仪姿态更新；利用四元数的初始值Q(t₀)和陀螺输出角速度信号，采用等效旋转矢量三子样优化算法，递推解算姿态四元数Q(t_k)和三个姿态角实时值ψ_k、θ_k、γ_k；设时间间隔[t_k-1,t_k]内，Δθ_i（i=1,2,3）为姿态更新周期h的三等分时间间隔内陀螺的角增量输出则优化三子样旋转矢量算法公式如下：Φ(h)=Δθ1+Δθ2+Δθ3+92(Δθ1×Δθ3)+2740Δθ2×(Δθ3-Δθ1)---(3)]]>q(h)=cos(|Φ|2)+Φ|Φ|sin(|Φ|2)---(4)]]>Q(tk)=Q(tk-1)⊗q(h)---(5)]]>设Q(t_k)＝[q₀  q₁  q₂  q₃]^T，由Q(t_k)依次求出和姿态角ψ_k、θ_k、γ_k；Ψ和γ的真值可以根据主值查询反正切函数的真值表得到；步骤4：双天线GPS/磁强计姿态实时计算；系统运行过程中，利用GPS双天线测量俯仰角和横滚角的实时值三轴磁强计测量实时值H→kb=Hx,kbHy,kbHz,kbT,]]>航天器所在轨道位置的磁偏角为α_k，计算得到ψ_magmain,k；Ψmagmain,k=arctan(-cosγ~k·Hx,kb+sinγ~k·Hz,kbsinγ~k·sinθ~k·Hx,kb+cosθ~k·Hy,kb-cosγ~k·sinθ~k·Hz,kb) ---(8)]]>查真值表，得到磁航向的真值ψ_mag,k，再根据得到航向角的实时值步骤5：kalman滤波；构造状态和量测Zk=θ~k-θkγ~k-γkψ~k-ψkT,]]>建立系统的状态空间模型；进行kalman滤波递推；系统的状态方程：X·=FX+Gw---(9)]]>其中，w＝[w_gx w_gy w_gz w_ex w_ey w_ez]^T为系统的驱动噪声，方差矩阵为q_f；F_6×6的非零元为：F1,2=ωiesinL+VERN+htanL]]>F1,3=-(ωiecosL+VERN+h)]]>F_1,4＝-T₁₁  F_1,5＝-T₁₂F_1,6＝-T₁₃F2,1=-(ωiesinL+VERN+htanL)]]>F2,3=-VNRM+h]]>F_2,4＝-T₂₁F_2,5＝-T₂₂  F_2,6＝-T₂₃F3,1=ωiecosL+VERN+h]]>F3,2=VNRM+h]]>F_3,4＝-T₃₁  F_3,5＝-T₃₂F_3,6＝-T₃₃F4,4=-1τex]]>F5,5=-1τey]]>F6,6=-1τez]]>上面F矩阵元素中，L、h、V_E、V_N分别为航天器的实时纬度、高度、东向速度、北向速度，均由GPS实时提供；ω_ie为地球自转角速度，为常数；R_M、R_N分别为所在位置的子午圈曲率半径和卯酉圈曲率半径，由L按照经验公式计算得到；τ_ex、τ_ey、τ_ez为三轴MEMS陀螺等效一阶马尔可夫漂移的相关时间；G_6×6矩阵的非零元为：G_1,1＝-T_1,1 G_1,2＝-T_1,2 G_1,3＝-T_1,3 G_2,1＝-T_2,1 G_2,2＝-T_2,2 G_2,3＝-T_2,3G_3,1＝-T_3,1 G_3,2＝-T_3,2 G_3,3＝-T_3,3 G_4,4＝1 G_5,5＝1 G_6,6＝1设滤波周期为T,对公式(9)离散化得如公式（10）所示：X_k＝Φ_k,k-1X_k-1+W_k-1(10)其中，Φ_k,k-1＝I+TF_k-1，W_k-1为系统的驱动白噪声序列，方差矩阵为Q_f,k；Q_f,k按照下面的方法计算：M_i+1＝F_kM_i+(F_kM_i)^T（i=1,2,3…），离散形式的kalman滤波量测方程如下：Zk=θ~k-θkγ~k-γkψ~k-ψkT=HkXk+Vk---(11)]]>其中，H_k＝[I_3×3  0_3×3]，V_k为量测白噪声序列，方差矩阵为R_k；公式(10)和公式(11)联立构成离散kalman滤波的状态空间模型，利用离散型kalman滤波基本方程进行递推计算就得到各时刻状态的最优估计步骤6：输出校正；利用对陀螺姿态更新的姿态角进行前馈校正，如下式所示：步骤7：返回步骤2；返回步骤2循环执行步骤2～步骤7。