CN103090870A - 一种基于mems传感器的航天器姿态测量方法 - Google Patents

Abstract

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

Description

一种基于MEMS传感器的航天器姿态测量方法

技术领域

本发明涉及一种基于MEMS传感器的航天器姿态测量方法，属于姿态测量和航天测控技术领域。

背景技术

航天器姿态信息是航天器最重要参数之一，航天器姿态实时测量是实现航天器姿态精确控制的关键。航天器的姿态信息包括三个姿态角，即俯仰角θ，横滚角γ和偏航角ψ。现代微型航天器（或航天器附属机构）的发展对姿态测量系统的可靠性，精度、体积，重量、成本等提出了更高的要求。

通过国家专利局检索库检索，目前，关于姿态测量的专利主要是清华大学的“微小型动态载体姿态测量装置及其测量方法”（专利号：200510011763.7）。这一发明主要包括3轴速率陀螺、3轴磁场计、单轴加速度计、微处理器，以及相应软件测量方法。该组合姿态测量系统的核心技术是kalman滤波算法，以系统姿态变换矩阵相关元素为状态，根据陀螺的输出构造kalman滤波的状态方程。利用加速度计和三轴磁强计测量重力加速度分量和地磁矢量为量测,建立kalman滤波的量测方程，然后进行滤波，实现变换阵元素的最优估计，然后解算姿态角。该系统的主要缺点是：（1）航天器处于微重力或者失重的环境中，加速度计测量不到重力加速度或者由于重力加速度量值太小而无法分辨，其输出信号主要是载体过载加速度，系统的量测方程建立不正确。(2)在动态情况下，惯性器件无法分辨重力加速度和过载加速度，导致系统量测误差很大。

发明内容

本发明目的在于克服现有航天器姿态测量技术的不足，提供一种基于MEMS传感器的航天器姿态测量方法，具有较高的精度、可靠性、动态性能。

为了实现上述目的，本发明提供了一种基于MEMS传感器的航天器姿态测量方法。所述的航天器姿态测量方法,主要利用GPS双天线、三轴MEMS陀螺仪、三轴MEMS磁强计等传感器。所述姿态测量方法采用地理坐标系（g）作为导航坐标系（n）。将测量系统与航天器载体固连，使得传感器的敏感轴分别位于载体坐标系的三个轴上。

所述的姿态测量方法步骤如下：

步骤1：系统初始对准；在初始对准时刻t₀，系统利用GPS双天线测量俯仰角横滚角三轴MEMS磁强计测量航向角

求得初始时刻的姿态阵

和姿态四元数Q(t₀)；具体求解公式如下：

$C_b^n\left(t_0\right)=\left[\begin{array}{ccc}\cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0+\sin {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\ψ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0-\cos {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0\\ -\cos {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0+\sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0& \cos {\widetilde{\mathrm{\ψ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& -\sin {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0-\cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0\\ -\sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\θ}}}_0& \cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0\end{array}\right]---\left(1\right)$

设初始对准后，姿态阵初值为则姿态四元数的初始值Q(t₀)可以由下面公式确定：

$\left\{\begin{array}{c}{q}_0=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\\ q_1=\frac{1}{2}\mathrm{sign}(T_{32}-T_{23})\sqrt{1+T_{11}-T_{22}-T_{33}}\\ q_2=\frac{1}{2}\mathrm{sign}(T_{13}-T_{31})\sqrt{1-T_{11}+T_{22}-T_{33}}\\ q_3=\frac{1}{2}\mathrm{sign}(T_{21}-T_{12})\sqrt{1-T_{11}-T_{22}+T_{33}}\end{array}\right.---\left(2\right)$

步骤2：信号采集；输入通道按照一定的采样周期T，采集传感器的输出信号；

步骤3：陀螺仪姿态更新；利用四元数的初始值Q(t₀)和陀螺输出角速度信号，采用等效旋转矢量三子样优化算法，递推解算姿态四元数Q(t_k)和三个姿态角实时值ψ_k、θ_k、γ_k；

设时间间隔[t_k-1,t_k]内，Δθ_i（i=1,2,3）为姿态更新周期h的三等分时间间隔内陀螺的角增量输出

则优化三子样旋转矢量算法公式如下：

$\mathrm{\Φ}\left(h\right)={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{2}({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)---\left(3\right)$

$q\left(h\right)=\cos \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)+\frac{\mathrm{\Φ}}{\left|\mathrm{\Φ}\right|}\sin \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)---\left(4\right)$

$Q\left(t_k\right)=Q\left(t_{k-1}\right)\⊗q\left(h\right)---\left(5\right)$

设Q(t_k)＝[q₀  q₁  q₂  q₃]^T，由Q(t_k)依次求出

和姿态角ψ_k、θ_k、γ_k；

Ψ和γ的真值可以根据主值查询反正切函数的真值表得到；

步骤4：双天线GPS/磁强计姿态实时计算；系统运行过程中，利用GPS双天线测量俯仰角和横滚角的实时值三轴磁强计测量实时值

航天器所在轨道位置的磁偏角为α_k，计算得到ψ_magmain,k；

${\mathrm{\ψ}}_{\mathrm{magmain},k}=\arctan (-\frac{\cos {\widetilde{\mathrm{\γ}}}_k\·H_{x,k}^b+\sin {\widetilde{\mathrm{\γ}}}_k\·H_{z,k}^b}{\sin {\widetilde{\mathrm{\γ}}}_k\·\sin {\widetilde{\mathrm{\θ}}}_k\·H_{x,k}^b+\cos {\widetilde{\mathrm{\θ}}}_k\·H_{y,k}^b-\cos {\widetilde{\mathrm{\γ}}}_k\·\sin {\widetilde{\mathrm{\θ}}}_k\·H_{z,k}^b})---\left(8\right)$

查真值表，得到磁航向的真值ψ_mag,k，再根据

得到航向角的实时值

步骤5：kalman滤波；

构造状态

和量测 $Z_k={\left[\begin{array}{ccc}{\widetilde{\mathrm{\θ}}}_k-{\mathrm{\θ}}_k& {\widetilde{\mathrm{\γ}}}_k-{\mathrm{\γ}}_k& {\widetilde{\mathrm{\ψ}}}_k-{\mathrm{\ψ}}_k\end{array}\right]}^T,$ 建立系统的状态空间模型；进行kalman滤波递推；

系统的状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(9\right)$

其中，w＝[w_gx  w_gy  w_gz  w_ex  w_ey  w_ez]^T为系统的驱动噪声，方差矩阵为q_f；F_6×6的非零元为：

$F_{\mathrm{1,2}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{1,3}}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})$

F_1,4＝-T₁₁ F_1,5＝-T₁₂

F_1,6＝-T₁₃ $F_{\mathrm{2,1}}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$F_{\mathrm{2,3}}=-\frac{V_N}{R_M+h}$ F_2,4＝-T₂₁

F_2,5＝-T₂₂  F_2,6＝-T₂₃

$F_{\mathrm{3,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{3,2}}=\frac{V_N}{R_M+h}$

F_3,4＝-T₃₁  F_3,5＝-T₃₂

F_3,6＝-T₃₃ $F_{\mathrm{4,4}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ex}}}$

$F_{\mathrm{5,5}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ey}}}$ $F_{\mathrm{6,6}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{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,3

G_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滤波量测方程如下：

$Z_k={\left[\begin{array}{ccc}{\widetilde{\mathrm{\θ}}}_k-{\mathrm{\θ}}_k& {\widetilde{\mathrm{\γ}}}_k-{\mathrm{\γ}}_k& {\widetilde{\mathrm{\ψ}}}_k-{\mathrm{\ψ}}_k\end{array}\right]}^T=H_k{X}_k+V_k---\left(11\right)$

其中，H_k＝[I_3×3  0_3×3]，V_k为量测白噪声序列，方差矩阵为R_k；

公式(10)和公式(11)联立构成离散kalman滤波的状态空间模型，利用离散型kalman滤波基本方程进行递推计算就得到各时刻状态的最优估计

步骤6：输出校正；利用

对陀螺姿态更新的姿态角进行前馈校正，如下式所示：

步骤7：返回步骤2；返回步骤2循环执行步骤2～步骤7。

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

附图说明

图1是描述导航坐标系和载体坐标系，其中(A)为地理坐标系，(B)为载体坐标系。

图2是该姿态测量系统中传感器的分布示意图。

图3是该姿态测量系统对应的算法流程图。

图4是该组合姿态测量系统的输出校正原理图。

具体实施方式

下面结合附图及本发明的具体实施例，介绍该姿态测量方法的实施过程。

图1是描述导航坐标系(n)和载体坐标系(b)，其中(A)为地理坐标系(n=g)，(B)为载体坐标系(b)。地理坐标系定义为：原点位于载体的质心，三轴分别指向所在地的地理东、北、天。航天器载体坐标系（b系）定义为：原点位于载体的质心，三轴分别指向航天器载体的右、前、上。

图2是该姿态测量系统中传感器的分布示意图。双天线GPS模块包括第一GPS天线(2011)和第二GPS天线(2012)，这两个GPS天线(2011)、(2012)分别安装在载体坐标系的原点和Z轴负半轴上，用来测量载体的俯仰角θ和横滚角γ。三轴MEMS陀螺仪包括第一陀螺(2021)、第二陀螺(2022)、第三陀螺(2023)，这三个陀螺(2021)、(2022)、(2023)沿着载体坐标系三轴平行配置，用来测量载体旋转角速度在载体坐标系的三轴分量。三轴MEMS磁强计包括第一磁强计(2031)、第二磁强计(2032)、第三磁强计(2033),这三个磁强计沿着载体坐标系三轴平行配置，用于测量地磁感应强度在载体坐标系三轴的分量。

图3是该姿态测量系统对应的算法流程图。介绍了该姿态测量系统从初始对准到解算出最优姿态的整个算法流程。

记姿态矩阵为：

姿态四元数Q＝[q₀  q₁  q₂  q₃]^T。

所述具体算法流程如下：

步骤1：系统初始对准。系统的初始姿态信息可以根据GPS双天线模块/三轴MEMS磁强计的测量值进行计算得到，初始对准算法如下：

在初始对准时刻t₀，GPS双天线测量得俯仰角

横滚角

三轴磁强计测量初始值为 ${\stackrel{\→}{H}}_0^b={\left[\begin{array}{ccc}{H}_{x0}^b& H_{y0}^b& H_{z0}^b\end{array}\right]}^T,$ 航天器所在轨道位置的磁偏角为α₀，由公式(13)可以得到磁航向的初始主值为ψ_0magmain：

${\mathrm{\ψ}}_{\mathrm{magmain}}=\arctan (-\frac{\cos \mathrm{\γ}\·H_x^b+\sin \mathrm{\γ}\·H_z^b}{\sin \mathrm{\γ}\·\sin \mathrm{\θ}\·H_x^b+\cos \mathrm{\θ}\·H_y^b-\cos \mathrm{\γ}\·\sin \mathrm{\θ}\·H_z^b})---\left(13\right)$

查真值表得磁航向真值ψ_0mag。航向角的初值为：

${\widetilde{\mathrm{\ψ}}}_0={\mathrm{\ψ}}_{0\mathrm{mag}}+{\mathrm{\α}}_0---\left(14\right)$

由初始姿态角

求得初始时刻的姿态阵

和姿态四元数Q(t₀)。

步骤2：信号采集。DSP控制器控制系统的输入通道按照一定的采样周期T_s，采集GPS双天线模块输出的两个倾角信号、三轴MEMS陀螺仪输出的角速率信号和三轴MEMS磁强计输出的地磁强度信号。

步骤3：陀螺仪姿态更新。利用四元数的初始值Q(t₀)和陀螺输出角速度信号

递推计算各个时刻的姿态四元数Q(t_k)和三个姿态角。为减小旋转的不可交换误差，这里采用等效旋转矢量三子样优化算法。设时间间隔[t_k-1,t_k]内，Δθ_i（i=1,2,3）为姿态更新周期h的三等分时间间隔内陀螺的角增量输出

那么

则优化三子样旋转矢量算法公式如下：

$\mathrm{\Φ}\left(h\right)={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{20}({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)---\left(15\right)$

$q\left(h\right)=\cos \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)+\frac{\mathrm{\Φ}}{\left|\mathrm{\Φ}\right|}\sin \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)---\left(16\right)$

$Q\left(t_k\right)=Q\left(t_{k-1}\right)\⊗q\left(h\right)---\left(17\right)$

由Q(t_k)依次求出

和姿态角ψ_k、θ_k、γ_k。

步骤4：双天线GPS/磁强计姿态实时计算。系统运行过程中，设GPS双天线测得俯仰角和横滚角的实时值为：

三轴磁强计测量实时值为 ${\stackrel{\→}{H}}_k^b={\left[\begin{array}{ccc}{H}_{x,k}^b& H_{y,k}^b& H_{z,k}^b\end{array}\right]}^T,$ 航天器所在轨道位置的磁偏角为α_k，由公式(13)计算得到ψ_magmain,k，查对应的真值表，得到磁航向的真值ψ_mag,k，再根据

得到航向角的实时值

步骤5：kalman滤波。本发明根据组合导航的相关知识，为系统建立了一种简单有效的kalman滤波状态空间模型。设陀螺仪姿态更新的姿态误差角为

MEMS陀螺仪的漂移为

为陀螺的随机常值漂移，

为陀螺的相关漂移，近似为一阶马尔可夫过程，

为陀螺随机漂移的白噪声分量。

（i=X,Y,Z），w_ri为陀螺相关漂移的驱动白噪声。陀螺漂移的非白噪声部分可以等效为一阶马尔可夫漂移：

并且满足

w_ei为陀螺等效一阶马尔可夫漂移的驱动白噪声。由前面姿态解算得姿态阵取状态变量为：

建立系统的状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(18\right)$

其中，w＝[w_gx  w_gy  w_gz  w_ex  w_ey  w_ez]^T为系统的驱动噪声，方差矩阵为q_f。F_6×6的非零元为：

$F_{\mathrm{1,2}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{1,3}}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})$

F_1,4＝-T₁₁  F_1,5＝-T₁₂

F_1,6＝-T₁₃   $F_{\mathrm{2,1}}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$F_{\mathrm{2,3}}=-\frac{V_N}{R_M+h}$ F_2,4＝-T₂₁

F_2,5＝-T₂₂  F_2,6＝-T₂₃

$F_{\mathrm{3,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{3,2}}=\frac{V_N}{R_M+h}$

F_3,4＝-T₃₁  F_3,5＝-T₃₂

F_3,6＝-T₃₃ $F_{\mathrm{4,4}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ex}}}$

$F_{\mathrm{5,5}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ey}}}$ $F_{\mathrm{6,6}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{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,3

G_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,对公式(18)离散化得如公式（19）所示：

X_k＝Φ_k,k-1X_k-1+W_k-1(19)

其中，Φ_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…），

测量Z向量取为步骤4的结果和步骤3的结果姿态矢量的差值，建立离散形式的kalman滤波量测方程如下：

$Z_k={\left[\begin{array}{ccc}{\widetilde{\mathrm{\θ}}}_k-{\mathrm{\θ}}_k& {\widetilde{\mathrm{\γ}}}_k-{\mathrm{\γ}}_k& {\widetilde{\mathrm{\ψ}}}_k-{\mathrm{\ψ}}_k\end{array}\right]}^T=H_k{X}_k+V_k---\left(20\right)$

其中，H_k＝[I_3×3  0_3×3]，V_k为量测白噪声序列，方差矩阵为R_k。

公式(19)和公式(20)联立构成离散kalman滤波的状态空间模型，利用离散型kalman滤波基本方程进行递推计算就得到各时刻状态的最优估计

步骤6：输出校正。图4说明了输出校正的原理。从

中提取姿态误差角的最优估计，对陀螺姿态更新的姿态角进行校正。采用输出校正方式，校正公式如下：

步骤7：返回步骤2。返回步骤2循环执行步骤2～步骤7的操作。

Claims (1)

1.一种基于MEMS传感器的航天器姿态测量方法。所述的航天器姿态测量方法,主要利用GPS双天线、三轴MEMS陀螺仪、三轴MEMS磁强计等传感器。所述姿态测量方法采用地理坐标系（g）作为导航坐标系（n）。将测量系统与航天器载体固连，使得传感器的敏感轴分别位于载体坐标系的三个轴上；

所述的姿态测量方法步骤如下：

步骤1：系统初始对准；在初始对准时刻t₀，系统利用GPS双天线测量俯仰角

横滚角

三轴MEMS磁强计测量航向角

求得初始时刻的姿态阵

和姿态四元数Q(t₀)；具体求解公式如下：

$C_b^n\left(t_0\right)=\left[\begin{array}{ccc}\cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0+\sin {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\ψ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0-\cos {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0\\ -\cos {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0+\sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0& \cos {\widetilde{\mathrm{\ψ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& -\sin {\widetilde{\mathrm{\γ}}}_0\sin {\widetilde{\mathrm{\ψ}}}_0-\cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\ψ}}}_0\sin {\widetilde{\mathrm{\θ}}}_0\\ -\sin {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0& \sin {\widetilde{\mathrm{\θ}}}_0& \cos {\widetilde{\mathrm{\γ}}}_0\cos {\widetilde{\mathrm{\θ}}}_0\end{array}\right]---\left(1\right)$

设初始对准后，姿态阵初值为

则姿态四元数的初始值Q(t₀)可以由下面公式确定：

$\left\{\begin{array}{c}{q}_0=\frac{1}{2}\sqrt{1+T_{11}+T_{22}+T_{33}}\\ q_1=\frac{1}{2}\mathrm{sign}(T_{32}-T_{23})\sqrt{1+T_{11}-T_{22}-T_{33}}\\ q_2=\frac{1}{2}\mathrm{sign}(T_{13}-T_{31})\sqrt{1-T_{11}+T_{22}-T_{33}}\\ q_3=\frac{1}{2}\mathrm{sign}(T_{21}-T_{12})\sqrt{1-T_{11}-T_{22}+T_{33}}\end{array}\right.---\left(2\right)$

步骤2：信号采集；输入通道按照一定的采样周期T，采集传感器的输出信号；

步骤3：陀螺仪姿态更新；利用四元数的初始值Q(t₀)和陀螺输出角速度信号

，采用等效旋转矢量三子样优化算法，递推解算姿态四元数Q(t_k)和三个姿态角实时值ψ_k、θ_k、γ_k；

设时间间隔[t_k-1,t_k]内，Δθ_i（i=1,2,3）为姿态更新周期h的三等分时间间隔内陀螺的角增量输出

则优化三子样旋转矢量算法公式如下：

$\mathrm{\Φ}\left(h\right)={\mathrm{\Δ\θ}}_1+{\mathrm{\Δ\θ}}_2+{\mathrm{\Δ\θ}}_3+\frac{9}{2}({\mathrm{\Δ\θ}}_1\×{\mathrm{\Δ\θ}}_3)+\frac{27}{40}{\mathrm{\Δ\θ}}_2\×({\mathrm{\Δ\θ}}_3-{\mathrm{\Δ\θ}}_1)---\left(3\right)$

$q\left(h\right)=\cos \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)+\frac{\mathrm{\Φ}}{\left|\mathrm{\Φ}\right|}\sin \left(\frac{\left|\mathrm{\Φ}\right|}{2}\right)---\left(4\right)$

$Q\left(t_k\right)=Q\left(t_{k-1}\right)\⊗q\left(h\right)---\left(5\right)$

设Q(t_k)＝[q₀  q₁  q₂  q₃]^T，由Q(t_k)依次求出

和姿态角ψ_k、θ_k、γ_k；

Ψ和γ的真值可以根据主值查询反正切函数的真值表得到；

步骤4：双天线GPS/磁强计姿态实时计算；系统运行过程中，利用GPS双天线测量俯仰角和横滚角的实时值

三轴磁强计测量实时值 ${\stackrel{\→}{H}}_k^b={\left[\begin{array}{ccc}{H}_{x,k}^b& H_{y,k}^b& H_{z,k}^b\end{array}\right]}^T,$ 航天器所在轨道位置的磁偏角为α_k，计算得到ψ_magmain,k；

${\mathrm{\Ψ}}_{\mathrm{magmain},k}=\arctan (-\frac{\cos {\widetilde{\mathrm{\γ}}}_k\·H_{x,k}^b+\sin {\widetilde{\mathrm{\γ}}}_k\·H_{z,k}^b}{\sin {\widetilde{\mathrm{\γ}}}_k\·\sin {\widetilde{\mathrm{\θ}}}_k\·H_{x,k}^b+\cos {\widetilde{\mathrm{\θ}}}_k\·H_{y,k}^b-\cos {\widetilde{\mathrm{\γ}}}_k\·\sin {\widetilde{\mathrm{\θ}}}_k\·H_{z,k}^b})---\left(8\right)$

查真值表，得到磁航向的真值ψ_mag,k，再根据

得到航向角的实时值

步骤5：kalman滤波；

构造状态

和量测 $Z_k={\left[\begin{array}{ccc}{\widetilde{\mathrm{\θ}}}_k-{\mathrm{\θ}}_k& {\widetilde{\mathrm{\γ}}}_k-{\mathrm{\γ}}_k& {\widetilde{\mathrm{\ψ}}}_k-{\mathrm{\ψ}}_k\end{array}\right]}^T,$ 建立系统的状态空间模型；进行kalman滤波递推；

系统的状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(9\right)$

其中，w＝[w_gx w_gy w_gz w_ex w_ey w_ez]^T为系统的驱动噪声，方差矩阵为q_f；

F_6×6的非零元为：

$F_{\mathrm{1,2}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{1,3}}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})$

F_1,4＝-T₁₁  F_1,5＝-T₁₂

F_1,6＝-T₁₃ $F_{\mathrm{2,1}}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$F_{\mathrm{2,3}}=-\frac{V_N}{R_M+h}$ F_2,4＝-T₂₁

F_2,5＝-T₂₂  F_2,6＝-T₂₃

$F_{\mathrm{3,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{3,2}}=\frac{V_N}{R_M+h}$

F_3,4＝-T₃₁  F_3,5＝-T₃₂

F_3,6＝-T₃₃ $F_{\mathrm{4,4}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ex}}}$

$F_{\mathrm{5,5}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ey}}}$ $F_{\mathrm{6,6}}=-\frac{1}{{\mathrm{\τ}}_{\mathrm{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,3

G_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滤波量测方程如下：

$Z_k={\left[\begin{array}{ccc}{\widetilde{\mathrm{\θ}}}_k-{\mathrm{\θ}}_k& {\widetilde{\mathrm{\γ}}}_k-{\mathrm{\γ}}_k& {\widetilde{\mathrm{\ψ}}}_k-{\mathrm{\ψ}}_k\end{array}\right]}^T=H_k{X}_k+V_k---\left(11\right)$ 其中，H_k＝[I_3×3  0_3×3]，V_k为量测白噪声序列，方差矩阵为R_k；

公式(10)和公式(11)联立构成离散kalman滤波的状态空间模型，利用离散型kalman滤波基本方程进行递推计算就得到各时刻状态的最优估计

步骤6：输出校正；利用对陀螺姿态更新的姿态角进行前馈校正，如下式所示：

步骤7：返回步骤2；返回步骤2循环执行步骤2～步骤7。

Images