CN106052716A - 惯性系下基于星光信息辅助的陀螺误差在线标定方法 - Google Patents

Abstract

本发明公开了惯性系下基于星光信息辅助的陀螺误差在线标定方法，该方法首先建立陀螺误差模型，包括陀螺安装误差模型、标度因数误差模型和随机漂移误差模型；随后，将陀螺误差扩展为系统状态变量，建立惯性系下惯性/星光组合导航卡尔曼滤波状态方程和量测方程；最后在载体动态飞行过程中对陀螺误差进行在线标定与实时修正，获得陀螺误差修正后的惯性导航系统导航结果。本发明方法能够在飞行器动态飞行过程中有效利用星敏感器高精度姿态信息,实现对陀螺误差的在线标定和修正，提高惯性导航系统性能，适用于工程应用。

Description

惯性系下基于星光信息辅助的陀螺误差在线标定方法

技术领域

本发明涉及惯性系下基于星光信息辅助的陀螺误差在线标定方法，属于惯性导航惯性传感器误差标定技术领域。

背景技术

近年来，随着高超声速飞行器和空天飞行器等高速、高机动飞行器的研制发展，对导航系统性能提出更高要求。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点，必将成为未来高超和空天飞行器导航系统的重要信息单元。

惯性导航系统的误差主要由惯性传感器(IMU-加速度计和陀螺仪)测量误差引起，由于加速度计测量精度较高，陀螺仪误差成为影响惯导系统性能的主要因素。陀螺仪误差包括随机漂移误差和安装误差、标度因数误差等确定性误差，确定性的误差可以设法通过补偿加以消除，如国内外通常借助速率转台实现陀螺仪安装误差和标度因数误差的修正。而在高超声速飞行器和空天飞行器高动态飞行过程中，由于其飞行环境的影响，极有可能导致陀螺安装误差和标度因数误差较实验室标定值发生很大变化，在飞行器高速飞行下引起极大的导航误差。因此，在飞行器飞行过程中如何利用外部信息源估计并修正陀螺输出误差，实现在线标定对于提高系统实用精度具有重大意义。

星敏感器作为一种高精度的姿态测量仪器，可直接提供误差不随时间积累的角秒级姿态信息，可弥补惯性导航系统误差随时间积累的缺点。但以地理系为参考系的惯性导航系统输出的导航信息和星光信息参考坐标系不一致，传统的方法是将星敏感器输出的相对于惯性坐标系的姿态信息转换到地理坐标系下的姿态信息，然后与惯性导航系统输出的姿态信息进行组合，该转换过程耦合了由惯性器件误差所带来的导航误差，无法充分发挥星光高精度姿态的误差修正作用。因此如何在保证高精度星敏姿态信息不受损的前提下，实现惯性和星光的有效组合，具有重要的研究意义。

发明内容

本发明所要解决的技术问题是：提供惯性系下基于星光信息辅助的陀螺误差在线标定方法，在飞行器动态飞行过程中有效利用星敏感器高精度姿态信息，对陀螺误差进行在线标定和修正，显著提高了惯性导航系统精度。

本发明为解决上述技术问题采用以下技术方案：

惯性系下基于星光信息辅助的陀螺误差在线标定方法，包括如下步骤：

步骤1，建立陀螺误差模型，所述陀螺误差包括陀螺安装误差、标度因数误差和随机漂移误差；

步骤2，在步骤1对陀螺误差建模的基础上，将步骤1所述三类误差的误差参数扩展为系统状态变量，构建惯性系下惯性/星光组合卡尔曼滤波状态方程；

步骤3，将步骤1得到的陀螺原始输出信息进行惯性系下姿态解算，得到惯性系下载体姿态四元数，结合星敏感器输出的惯性系下载体姿态四元数，构建惯性系下惯性/星光组合卡尔曼滤波量测方程；

步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行开环跟踪估计，从而得到陀螺安装误差、标度因数误差和随机游走误差的标定结果，在线对陀螺原始输出信息进行修正，然后反馈到地理系下惯性导航系统进行解算。

作为本发明的一种优选方案，步骤1所述陀螺误差模型为：

${\widetilde{\mathrm{\ω}}}_{ib}^b={\mathrm{\ω}}_{ib}^b+{\mathrm{\ω}}_m+{\mathrm{\ω}}_k+{\mathrm{\ω}}_b,$

${\mathrm{\ω}}_m={\mathrm{\δG}}_{\mathrm{\θ}}{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}0& {\mathrm{\θ}}_{xz}& -{\mathrm{\θ}}_{xy}\\ -{\mathrm{\θ}}_{yz}& 0& {\mathrm{\θ}}_{yx}\\ {\mathrm{\θ}}_{zy}& -{\mathrm{\θ}}_{zx}& 0\end{array}\right]{\mathrm{\ω}}_{ib}^b,$

${\mathrm{\ω}}_k={\mathrm{\δG}}_k{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}{k}_x& 0& 0\\ 0& k_y& 0\\ 0& 0& k_z\end{array}\right]{\mathrm{\ω}}_{ib}^b,$

ω_b＝ε_b+w_g，

其中，为陀螺原始输出信息，为载体真实角速率，ω_m、ω_k、ω_b分别为陀螺安装误差、标度因数误差、随机漂移误差；δG_θ为陀螺安装误差系数矩阵，θ_xy、θ_xz、θ_yx、θ_yz、θ_zx、θ_zy均为陀螺安装误差角；δG_k为标度因数误差系数矩阵，k_x、k_y、k_z分别对应为陀螺x、y、z轴方向的标度因数；ε_b为陀螺随机游走误差，w_g为高斯白噪声。

作为本发明的一种优选方案，步骤2所述状态方程为：

$\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ =\left[\begin{array}{cccc}-\[{\widehat{\mathrm{\ω}}}_{ib}^b\×\]& -\frac{1}{2}{\mathrm{\Ω}}_m& -\frac{1}{2}{\mathrm{\Ω}}_k& -\frac{1}{2}{I}_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_r\end{array}\right]\end{array}$

$\begin{array}{c}X\left(t\right)=\left[\begin{array}{ccccccccc}{\mathrm{\δq}}_1& {\mathrm{\δq}}_2& {\mathrm{\δq}}_3& {\mathrm{\δ\θ}}_{xy}& {\mathrm{\δ\θ}}_{xz}& {\mathrm{\δ\θ}}_{yx}& {\mathrm{\δ\θ}}_{yz}& {\mathrm{\δ\θ}}_{zx}& {\mathrm{\δ\θ}}_{zy}\end{array}\right.\\ {\left.\begin{array}{cccccc}{\mathrm{\δk}}_x& {\mathrm{\δk}}_y& {\mathrm{\δk}}_z& {\mathrm{\δ\ϵ}}_{bx}& {\mathrm{\δ\ϵ}}_{by}& {\mathrm{\δ\ϵ}}_{bz}\end{array}\right]}^T\end{array},$

其中，X(t)为系统状态量，为状态量X(t)的一阶导数，F(t)为系统矩阵，G(t)为噪声系数矩阵，W(t)为噪声矩阵；δq₁,δq₂,δq₃为姿态误差四元数的矢量部分，δθ_xy,δθ_xz,δθ_yx,δθ_yz,δθ_zx,δθ_zy均为陀螺安装误差角状态量，δk_x,δk_y,δk_z均为标度因数误差状态量，δε_bx,δε_by,δε_bz均为陀螺随机游走误差状态量；为的反对称矩阵，为载体真实角速率的估计值，I为单位矩阵， 分别为在x、y、z轴方向的分量，w_g为高斯白噪声，w_r为陀螺随机游走驱动白噪声。

作为本发明的一种优选方案，步骤3所述量测方程为：

$Z\left(t\right)={\left(Q_i\right)}^{-1}\⊗Q_c={(Q\⊗{\mathrm{\δQ}}^{-1})}^{-1}\⊗(Q+{\mathrm{\δQ}}_{\mathrm{\ϵ}1})=\mathrm{\δ}Q+{\left(Q_i\right)}^{-1}\⊗{\mathrm{\δQ}}_{\mathrm{\ϵ}1},$

其中，Z(t)为姿态量测矢量，(Q_i)^-1为姿态解算得到的惯性系下载体姿态四元数Q_i的逆，(Q_i)^-1＝[q_i0 -q_i1 -q_i2 -q_i3]^T；Q_c为星敏感器输出的惯性系下载体姿态四元数，Q为载体真实姿态四元数，Q＝[q_t0 q_t1 q_t2 q_t3]^T；δQ^-1为误差四元数δQ的逆，δQ_ε1为星敏感器测量误差四元数，δQ_ε1＝[1 δq_ε1 δq_ε2 δq_ε3]^T；

$\begin{array}{c}Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\\ =\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×12}\end{array}\right]X\left(t\right)+\left[\begin{array}{cccc}{q}_{i0}& q_{i1}& q_{i2}& q_{i3}\\ -q_{i1}& q_{i0}& q_{i3}& -q_{i2}\\ -q_{i2}& -q_{i3}& q_{i0}& q_{i1}\\ -q_{i3}& q_{i2}& q_{i1}& q_{i0}\end{array}\right]\end{array}\left[\begin{array}{ccc}-q_{t1}& -q_{t2}& -q_{t2}\\ q_{t0}& -q_{t3}& q_{t2}\\ q_{t3}& q_{t0}& -q_{t1}\\ -q_{t2}& q_{t1}& q_{t0}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δq}}_{\mathrm{\ϵ}1}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}2}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}3}\end{array}\right],$

其中，H(t)为姿态量测系数矩阵，X(t)为系统状态量，V(t)为姿态观测噪声阵，I为单位矩阵。

作为本发明的一种优选方案，所述步骤4的具体过程为：

(401)将系统状态方程和量测方程离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的姿态量测矩阵,H_k为t_k时刻的姿态量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；

(402)采用卡尔曼滤波对状态量进行开环跟踪估计：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$

其中，是状态量X_k-1的卡尔曼滤波估值,是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻系统噪声协方差矩阵,P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻姿态量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,P_k为t_k时刻滤波状态估计协方差矩阵；

(403)在(402)得到陀螺误差的标定结果后，在线对陀螺误差模型的原始输出信息进行修正，修正模型为：

${\widehat{\mathrm{\ω}}}_{ib}^b=(I-\mathrm{\δ}{\hat{G}}_{\mathrm{\θ}}-\mathrm{\δ}{\hat{G}}_k)({\widetilde{\mathrm{\ω}}}_{ib}^b-{\widehat{\mathrm{\ϵ}}}_b),$

其中，I为单位矩阵，为陀螺安装误差系数矩阵估计值，为陀螺标度因数系数矩阵估计值，为陀螺随机游走误差估计值，则为陀螺经过误差在线修正后的输出信息，然后将其反馈到地理系下惯性导航系统进行解算。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明方法对陀螺仪误差进行建模，包括安装误差、标度因数误差和随机漂移误差模型，并将上述误差扩展为卡尔曼滤波器系统状态变量，利用星敏感器输出的高精度姿态信息，应用卡尔曼滤波方法对陀螺误差进行在线开环跟踪估计和修正，本方法既保证了星敏感器高精度信息的有效利用，又实时修正了陀螺仪误差，显著提高了飞行器惯性导航系统的精度。

2、本发明方法通过对陀螺误差进行在线标定和修正，可以有效减小惯性导航系统误差，提高高速、高机动飞行器在复杂飞行环境中惯性导航系统精度，适合工程应用。

附图说明

图1是本发明惯性系下基于星光信息辅助的陀螺误差在线标定方法的架构图。

图2是本发明陀螺安装误差角定义示意图。

图3是本发明飞行器航迹示意图。

图4(a)-图4(c)是本发明陀螺误差标定结果，其中，图4(a)是陀螺随机游走误差标定曲线，图4(b)是陀螺标度因数误差标定曲线，图4(c)是陀螺安装误差标定曲线。

图5(a)-图5(c)是本发明陀螺误差修正纯惯性导航结果与陀螺误差未修正纯惯性导航结果比较图，图5(a)是姿态误差比较曲线，图5(b)是位置误差比较曲线，图5(c)是速度误差比较曲线。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

如图1所示，本发明所述的惯性系下基于星光信息辅助的陀螺误差在线标定方法的原理是：陀螺仪的测量输出为其中包括载体真实角速率和安装误差、标度因数误差和随机漂移误差。惯性系下陀螺误差标定模块通过建立惯性系下卡尔曼滤波状态方程和量测方程，融合陀螺仪原始输出角速率经过积分解算得到的载体系相对与地心惯性坐标系姿态四元数Q_i和星敏感器输出惯性系姿态信息Q_c，在线估计得到陀螺误差估计值利用该信息实时修正陀螺原始输出信息修正后的陀螺仪输出进入地理系下惯性导航解算模块，以提高惯性导航系统性能。

本发明的具体实施方式如下：

1、建立陀螺误差模型

陀螺仪误差包括安装误差和标度因数误差引起的确定性误差和随机漂移误差，陀螺误差模型可以看作是一个由影响陀螺输出的各误差源组成的线性模型，那么陀螺测量输出的角速率为：

${\widetilde{\mathrm{\ω}}}_{ib}^b={\mathrm{\ω}}_{ib}^b+{\mathrm{\ω}}_m+{\mathrm{\ω}}_k+{\mathrm{\ω}}_b---\left(1\right)$

其中，为载体真实角速率，ω_m、ω_k分别为陀螺安装误差和标度因数误差引起的陀螺测量误差，ω_b为陀螺随机漂移误差。

在捷联式惯导系统中，陀螺仪直接安装在飞行器上，理论上讲，陀螺仪的输入轴应该与载体坐标系的三个轴完全一致。但实际上，由于飞行器所处的环境影响或者剧烈的飞行动态可能导致陀螺仪的输入轴线与载体系的三轴不重合，陀螺仪坐标系成了非正交坐标系，由于存在非正交误差，从而导致陀螺仪的输出中包含测量误差。

由安装误差引起的陀螺测量误差模型为：

${\mathrm{\ω}}_m={\mathrm{\δG}}_{\mathrm{\θ}}{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}0& {\mathrm{\θ}}_{xz}& -{\mathrm{\θ}}_{xy}\\ -{\mathrm{\θ}}_{yz}& 0& {\mathrm{\θ}}_{yx}\\ {\mathrm{\θ}}_{zy}& -{\mathrm{\θ}}_{zx}& 0\end{array}\right]{\mathrm{\ω}}_{ib}^b---\left(2\right)$

式(2)中，δG_θ为陀螺安装误差系数矩阵，θ_xy、θ_xz、θ_yx、θ_yz、θ_zx、θ_zy为陀螺安装误差角，具体定义如图2所示，OX_bY_bZ_b为载体正交坐标系，为OX_fY_fZ_f陀螺输入非正交坐标系，X'为轴X_f在平面X_bOZ_b上的投影，X'与OX_b轴和OX_f轴的夹角分别为θ_xy和θ_xz；Y'为轴Y_f在平面X_bOY_b上的投影，Y'与OY_b轴和OY_f轴的夹角分别为θ_yz和θ_yx；Z'为轴Z_f在平面Y_bOZ_b上的投影，Z'与OZ_b轴和OZ_f轴的夹角分别为θ_zx和θ_zy。

由标度因数误差引起的陀螺测量误差模型为：

${\mathrm{\ω}}_k={\mathrm{\δG}}_k{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}{k}_x& 0& 0\\ 0& k_y& 0\\ 0& 0& k_z\end{array}\right]{\mathrm{\ω}}_{ib}^b---\left(3\right)$

式(3)中，δG_k为陀螺标度因数系数矩阵，k_x、k_y、k_z分别对应陀螺x、y、z轴方向的标度因数。

陀螺的随机漂移除白噪声外，主要是有色噪声，取陀螺随机漂移为随机游走误差ε_b和高斯白噪声w_g，即ω_b＝ε_b+w_g，其中陀螺随机游走误差ε_b的模型为：

${\stackrel{\·}{\mathrm{\ϵ}}}_b=w_r---\left(4\right)$

式(4)中，w_r为陀螺随机游走驱动白噪声。

2、建立基于陀螺误差模型的卡尔曼滤波器模型

(2.1)基于陀螺原始输出的惯性系姿态解算模型

在进行惯性系下陀螺误差估计修正之前，首先需要根据陀螺仪输出的原始角速率信息求解得到载体坐标系相对于地心惯性坐标系的姿态信息Q_i。将陀螺仪的原始三维输出信息扩展为标量部分为零，矢量部分为的四元数ω_qi，其与姿态矩阵对应的四元数Q_i(t)有如下微分方程关系：

${\stackrel{\·}{Q}}_i\left(t\right)=\frac{1}{2}{Q}_i\left(t\right)\⊗{\mathrm{\ω}}_{qi}---\left(5\right)$

式(5)中，符号表示四元数的乘法，Q_i(t)＝[q_i0 q_i1 q_i2 q_i3]^T。

采用毕卡逼近法求解，并用等效旋转矢量进行补偿,可得四元数解析表达式为：

$Q_i(t+T)\{\cos \frac{{\mathrm{\Δ\θ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}(\[\mathrm{\Δ}\mathrm{\θ}(n)\]+\frac{1}{12}\[\mathrm{\Δ}\mathrm{\θ}(n-1)\×\mathrm{\Δ}\mathrm{\θ}(n)\])\}{Q}_i\left(t\right)---\left(6\right)$

式(6)中，Q_i(t+T)为t+T时刻飞行器的姿态四元数,I为4×4的单位矩阵,Δθ₀、Δθ、[Δθ]分别对应如下：

$\mathrm{\Δ}\mathrm{\θ}=\left[\begin{array}{c}{\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z\end{array}\right]={\∫}_t^{t+T}\left[\begin{array}{c}{\widetilde{\mathrm{\ω}}}_{ibx}^b\\ {\widetilde{\mathrm{\ω}}}_{iby}^b\\ {\widetilde{\mathrm{\ω}}}_{ibz}^b\end{array}\right]dt---\left(7\right)$

Δθ₀ ²＝Δθ_x ²+Δθ_y ²+Δθ_z ² (8)

$\[\mathrm{\Δ}\mathrm{\θ}\]=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_x& 0\end{array}\right]---\left(9\right)$

(2.2)基于陀螺误差模型的卡尔曼滤波状态方程

定义基于陀螺误差建模的系统状态变量X(t)为：

$\begin{array}{c}X\left(t\right)=\left[\begin{array}{ccccccccc}{\mathrm{\δq}}_1& {\mathrm{\δq}}_2& {\mathrm{\δq}}_3& {\mathrm{\δ\θ}}_{xy}& {\mathrm{\δ\θ}}_{xz}& {\mathrm{\δ\θ}}_{yx}& {\mathrm{\δ\θ}}_{yz}& {\mathrm{\δ\θ}}_{zx}& {\mathrm{\δ\θ}}_{zy}\end{array}\right.\\ {\left.\begin{array}{cccccc}{\mathrm{\δk}}_x& {\mathrm{\δk}}_y& {\mathrm{\δk}}_z& {\mathrm{\δ\ϵ}}_{bx}& {\mathrm{\δ\ϵ}}_{by}& {\mathrm{\δ\ϵ}}_{bz}\end{array}\right]}^T\end{array}---\left(10\right)$

式(10)中，δq₁,δq₂,δq₃为姿态误差四元数的矢量部分，δθ_xy,δθ_xz,δθ_yx,δθ_yz,δθ_zx,δθ_zy为陀螺安装误差角状态量，δk_x,δk_y,δk_z为陀螺标度因数误差状态量，δε_bx,δε_by,δε_bz为陀螺随机游走误差状态量。

建立系统状态方程为：

$\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ =\left[\begin{array}{cccc}-\[{\widehat{\mathrm{\ω}}}_{ib}^b\×\]& -\frac{1}{2}{\mathrm{\Ω}}_m& -\frac{1}{2}{\mathrm{\Ω}}_k& -\frac{1}{2}{I}_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_r\end{array}\right]\end{array}---\left(11\right)$

式(11)中，X(t)为系统状态量，为状态变量X(t)的一阶导数，F(t)为系统矩阵，G(t)为噪声系数矩阵，W(t)为噪声矩阵，为的估计值，为的反对称矩阵。

${\mathrm{\Ω}}_m=\left[\begin{array}{cccccc}-{\mathrm{\ω}}_{ibz}^b& {\mathrm{\ω}}_{iby}^b& 0& 0& 0& 0\\ 0& 0& {\mathrm{\ω}}_{ibz}^b& -{\mathrm{\ω}}_{ibx}^b& 0& 0\\ 0& 0& 0& 0& -{\mathrm{\ω}}_{iby}^b& {\mathrm{\ω}}_{ibx}^b\end{array}\right]---\left(12\right)$

${\mathrm{\Ω}}_k=\left[\begin{array}{ccc}{\mathrm{\ω}}_{ibx}^b& & \\ & {\mathrm{\ω}}_{iby}^b& \\ & & {\mathrm{\ω}}_{ibz}^b\end{array}\right]---\left(13\right)$

(2.3)基于星光量测信息的卡尔曼滤波量测方程

根据陀螺仪输出求解得到姿态四元数Q_i＝[q_i0 q_i1 q_i2 q_i3]^T和星敏感器输出的姿态四元数Q_c，构建系统量测方程为：

$Z\left(t\right)={\left(Q_i\right)}^{-1}\⊗Q_c={(Q\⊗{\mathrm{\δQ}}^{-1})}^{-1}\⊗(Q+{\mathrm{\δQ}}_{\mathrm{\ϵ}1})=\mathrm{\δ}Q+{\left(Q_i\right)}^{-1}\⊗{\mathrm{\δQ}}_{\mathrm{\ϵ}1}---\left(14\right)$

式(14)中，Q为载体真实姿态四元数，Q＝[q_t0 q_t1 q_t2 q_t3]^T,δQ为姿态误差四元数，δQ^-1为误差四元数δQ的逆，(Q_i)^-1为四元数Q_i的逆，由于其为规范化四元数，因而(Q_i)^-1＝[q_i0-q_i1 -q_i2 -q_i3]^T，δQ_ε1为星敏感器测量误差四元数，δQ_ε1＝[1 δq_ε1 δq_ε2 δq_ε3]^T，根据四元数乘法定则，式(14)可进一步得：

$\begin{array}{c}Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\\ =\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×12}\end{array}\right]X\left(t\right)+\left[\begin{array}{cccc}{q}_{i0}& q_{i1}& q_{i2}& q_{i3}\\ -q_{i1}& q_{i0}& q_{i3}& -q_{i2}\\ -q_{i2}& -q_{i3}& q_{i0}& q_{i1}\\ -q_{i3}& q_{i2}& q_{i1}& q_{i0}\end{array}\right]\end{array}\left[\begin{array}{ccc}-q_{t1}& -q_{t2}& -q_{t2}\\ q_{t0}& -q_{t3}& q_{t2}\\ q_{t3}& q_{t0}& -q_{t1}\\ -q_{t2}& q_{t1}& q_{t0}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δq}}_{\mathrm{\ϵ}1}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}2}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}3}\end{array}\right]---\left(15\right)$

3、陀螺误差在线标定与修正

(3.1)将系统状态方程和量测方程离散化处理：

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

Z_k＝H_kX_k+V_k (17)

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的姿态量测矩阵,H_k为t_k时刻的姿态量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵。

(3.2)采用卡尔曼滤波对状态量进行开环跟踪估计：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(18\right)$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_k-{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(19\right)$

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(20\right)$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})---\left(21\right)$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(22\right)$

上述公式中,是状态量X_k-1的卡尔曼滤波估值,是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻系统噪声协方差矩阵,P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻姿态量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,P_k为t_k时刻滤波状态估计协方差矩阵。

(3.3)在步骤(3.2)得到陀螺误差的标定结果后，在线对陀螺原始输出信息进行修正，修正模型为：

${\widehat{\mathrm{\ω}}}_{ib}^b=(I-\mathrm{\δ}{\hat{G}}_{\mathrm{\θ}}-\mathrm{\δ}{\hat{G}}_k)({\widetilde{\mathrm{\ω}}}_{ib}^b-{\widehat{\mathrm{\ϵ}}}_b)---\left(23\right)$

上式中，为陀螺安装误差系数矩阵估计值，为陀螺标度因数系数矩阵估计值，为陀螺随机游走误差估计值，则为陀螺经过误差在线修正后的输出信息，然后将其反馈到地理系下惯性导航系统解算流程。

为了验证发明所提出的惯性系下基于星光信息辅助的陀螺误差在线标定方法的正确性和有效性，采用本发明方法建立模型，进行Matlab仿真验证。因为陀螺安装误差和标度因数误差需要一定的姿态机动进行激励，设计飞行器标定航迹如图3所示，陀螺随机游走误差、标度因数误差和安装误差标定结果如图4(a)-图4(c)所示。

利用陀螺误差标定的结果对陀螺原始输出信息进行修正，然后将修正后的陀螺输出信息反馈到地理系下惯性导航系统，将陀螺误差修正后的纯惯性导航结果与陀螺误差未修正的惯性导航结果进行比较，比较曲线如图5(a)-图5(c)所示。

图4(a)-图4(c)中实线代表真实值，虚线代表标定结果。从图4(a)-图4(c)陀螺误差标定结果可以看出，采用本发明提出的惯性系下基于星光信息辅助的陀螺误差在线标定方法，基本上可以在线跟踪上实际设定的陀螺随机游走误差、标度因数误差和安装误差。图5(a)-图5(c)中实线代表陀螺误差未修正的地理系纯惯性导航结果，虚线代表陀螺误差修正后的地理系纯惯性导航结果。从图5(a)-图5(c)可以看出，利用陀螺误差标定结果对陀螺误差修正后，惯性导航系统精度明显提高，具有有益的工程应用价值。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.惯性系下基于星光信息辅助的陀螺误差在线标定方法，其特征在于，包括如下步骤：

步骤1，建立陀螺误差模型，所述陀螺误差包括陀螺安装误差、标度因数误差和随机漂移误差；

步骤2，在步骤1对陀螺误差建模的基础上，将步骤1所述三类误差的误差参数扩展为系统状态变量，构建惯性系下惯性/星光组合卡尔曼滤波状态方程；

步骤3，将步骤1得到的陀螺原始输出信息进行惯性系下姿态解算，得到惯性系下载体姿态四元数，结合星敏感器输出的惯性系下载体姿态四元数，构建惯性系下惯性/星光组合卡尔曼滤波量测方程；

步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行开环跟踪估计，从而得到陀螺安装误差、标度因数误差和随机游走误差的标定结果，在线对陀螺原始输出信息进行修正，然后反馈到地理系下惯性导航系统进行解算。

2.根据权利要求1所述惯性系下基于星光信息辅助的陀螺误差在线标定方法，其特征在于，步骤1所述陀螺误差模型为：

${\widetilde{\mathrm{\ω}}}_{ib}^b={\mathrm{\ω}}_{ib}^b+{\mathrm{\ω}}_m+{\mathrm{\ω}}_k+{\mathrm{\ω}}_b,$

${\mathrm{\ω}}_m={\mathrm{\δG}}_{\mathrm{\θ}}{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}0& {\mathrm{\θ}}_{xz}& -{\mathrm{\θ}}_{xy}\\ -{\mathrm{\θ}}_{yz}& 0& {\mathrm{\θ}}_{yx}\\ {\mathrm{\θ}}_{zy}& -{\mathrm{\θ}}_{zx}& 0\end{array}\right]{\mathrm{\ω}}_{ib}^b,$

${\mathrm{\ω}}_k={\mathrm{\δG}}_k{\mathrm{\ω}}_{ib}^b=\left[\begin{array}{ccc}{k}_x& 0& 0\\ 0& k_y& 0\\ 0& 0& k_z\end{array}\right]{\mathrm{\ω}}_{ib}^b,$

ω_b＝ε_b+w_g，

其中，为陀螺原始输出信息，为载体真实角速率，ω_m、ω_k、ω_b分别为陀螺安装误差、标度因数误差、随机漂移误差；δG_θ为陀螺安装误差系数矩阵，θ_xy、θ_xz、θ_yx、θ_yz、θ_zx、θ_zy均为陀螺安装误差角；δG_k为标度因数误差系数矩阵，k_x、k_y、k_z分别对应为陀螺x、y、z轴方向的标度因数；ε_b为陀螺随机游走误差，w_g为高斯白噪声。

3.根据权利要求1所述惯性系下基于星光信息辅助的陀螺误差在线标定方法，其特征在于，步骤2所述状态方程为：

$\begin{array}{c}\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)\\ =\left[\begin{array}{cccc}-\[{\widehat{\mathrm{\ω}}}_{ib}^b\×\]& -\frac{1}{2}{\mathrm{\Ω}}_m& -\frac{1}{2}{\mathrm{\Ω}}_k& -\frac{1}{2}{I}_{3\×3}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]X\left(t\right)+\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{6\×3}& 0_{6\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{w}_g\\ w_r\end{array}\right]\end{array}$

$\begin{array}{c}X\left(t\right)=\left[\begin{array}{ccccccccc}{\mathrm{\δq}}_1& {\mathrm{\δq}}_2& {\mathrm{\δq}}_3& {\mathrm{\δ\θ}}_{xy}& {\mathrm{\δ\θ}}_{xz}& {\mathrm{\δ\θ}}_{yx}& {\mathrm{\δ\θ}}_{yz}& {\mathrm{\δ\θ}}_{zx}& {\mathrm{\δ\θ}}_{zy}\end{array}\right.\\ {\left.\begin{array}{cccccc}{\mathrm{\δk}}_x& {\mathrm{\δk}}_y& {\mathrm{\δk}}_z& {\mathrm{\δ\ϵ}}_{bx}& {\mathrm{\δ\ϵ}}_{by}& {\mathrm{\δ\ϵ}}_{bz}\end{array}\right]}^T\end{array},$

其中，X(t)为系统状态量，为状态量X(t)的一阶导数，F(t)为系统矩阵，G(t)为噪声系数矩阵，W(t)为噪声矩阵；δq₁,δq₂,δq₃为姿态误差四元数的矢量部分，δθ_xy,δθ_xz,δθ_yx,δθ_yz,δθ_zx,δθ_zy均为陀螺安装误差角状态量，δk_x,δk_y,δk_z均为标度因数误差状态量，δε_bx,δε_by,δε_bz均为陀螺随机游走误差状态量；为的反对称矩阵，为载体真实角速率的估计值，I为单位矩阵， 分别为在x、y、z轴方向的分量，w_g为高斯白噪声，w_r为陀螺随机游走驱动白噪声。

4.根据权利要求1所述惯性系下基于星光信息辅助的陀螺误差在线标定方法，其特征在于，步骤3所述量测方程为：

$Z\left(t\right)={\left(Q_i\right)}^{-1}\⊗Q_c={(Q\⊗{\mathrm{\δQ}}^{-1})}^{-1}\⊗(Q+{\mathrm{\δQ}}_{\mathrm{\ϵ}1})=\mathrm{\δ}Q+{\left(Q_i\right)}^{-1}\⊗{\mathrm{\δQ}}_{\mathrm{\ϵ}1},$

其中，Z(t)为姿态量测矢量，(Q_i)^-1为姿态解算得到的惯性系下载体姿态四元数Q_i的逆，(Q_i)^-1＝[q_i0 -q_i1 -q_i2 -q_i3]^T；Q_c为星敏感器输出的惯性系下载体姿态四元数，Q为载体真实姿态四元数，Q＝[q_t0 q_t1 q_t2 q_t3]^T；δQ^-1为误差四元数δQ的逆，δQ_ε1为星敏感器测量误差四元数，δQ_ε1＝[1 δq_ε1 δq_ε2 δq_ε3]^T；

$\begin{array}{c}Z\left(t\right)=H\left(t\right)X\left(t\right)+V\left(t\right)\\ =\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×12}\end{array}\right]X\left(t\right)+\left[\begin{array}{cccc}{q}_{i0}& q_{i1}& q_{i2}& q_{i3}\\ -q_{i1}& q_{i0}& q_{i3}& -q_{i2}\\ -q_{i2}& -q_{i3}& q_{i0}& q_{i1}\\ -q_{i3}& q_{i2}& -q_{i1}& q_{i0}\end{array}\right]\end{array}\left[\begin{array}{ccc}-q_{t1}& -q_{t2}& -q_{t2}\\ q_{t0}& -q_{t3}& q_{t2}\\ q_{t3}& q_{t0}& -q_{t1}\\ -q_{t2}& q_{t1}& q_{t0}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δq}}_{\mathrm{\ϵ}1}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}2}\\ {\mathrm{\δq}}_{\mathrm{\ϵ}3}\end{array}\right],$

其中，H(t)为姿态量测系数矩阵，X(t)为系统状态量，V(t)为姿态观测噪声阵，I为单位矩阵。

5.根据权利要求1所述惯性系下基于星光信息辅助的陀螺误差在线标定方法，其特征在于，所述步骤4的具体过程为：

(401)将系统状态方程和量测方程离散化处理：

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的姿态量测矩阵,H_k为t_k时刻的姿态量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；

(402)采用卡尔曼滤波对状态量进行开环跟踪估计：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})$

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$

其中，是状态量X_k-1的卡尔曼滤波估值,是状态量X_k-1的一步预测估计值,P_k-1为t_k-1时刻滤波状态估计协方差矩阵,Q_k-1为t_k-1时刻系统噪声协方差矩阵,P_k,k-1为t_k-1时刻到t_k时刻的状态一步预测协方差矩阵,R_k为t_k时刻姿态量测噪声协方差矩阵,K_k为t_k时刻滤波增益矩阵,P_k为t_k时刻滤波状态估计协方差矩阵；

(403)在(402)得到陀螺误差的标定结果后，在线对陀螺误差模型的原始输出信息进行修正，修正模型为：

${\widehat{\mathrm{\ω}}}_{ib}^b=(I-\mathrm{\δ}{\hat{G}}_{\mathrm{\θ}}-\mathrm{\δ}{\hat{G}}_k)({\widetilde{\mathrm{\ω}}}_{ib}^b-{\widehat{\mathrm{\ϵ}}}_b),$

其中，I为单位矩阵，为陀螺安装误差系数矩阵估计值，为陀螺标度因数系数矩阵估计值，为陀螺随机游走误差估计值，则为陀螺经过误差在线修正后的输出信息，然后将其反馈到地理系下惯性导航系统进行解算。