CN103245359B - 一种惯性导航系统中惯性传感器固定误差实时标定方法 - Google Patents

Abstract

本发明公开了一种惯性导航系统中惯性传感器固定误差实时标定方法，该方法包括以下步骤：首先建立惯性传感器在飞行器动态飞行过程中的固定性误差模型，固定误差包括安装误差和标度因数误差；在传统的IMU随机误差模型及所建立的固定性误差模型的基础上，随后建立包含惯性传感器固定误差在内的滤波状态方程及位置、速度和姿态线性量测方程；最后在飞行器动态飞行过程中对惯性传感器固定性误差进行实时动态标定与校正，获得惯性传感器固定性误差补偿校正后的惯性导航系统导航结果。本方法能够在飞行器的动态飞行过程中实现对惯性导航系统中惯性传感器安装误差和标度因数误差的标定及校正，有效提高惯性导航系统性能，适合于工程应用。

Description

一种惯性导航系统中惯性传感器固定误差实时标定方法

技术领域

本发明公开了一种惯性导航系统中惯性传感器固定误差实时标定方法，属于惯性导航惯性传感器误差标定技术领域。

背景技术

近年来，随着新型飞行器的研制，对导航系统性能的要求日益提高。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点必是未来新型飞行器的核心导航信息单元。

惯性传感器（IMU-陀螺仪和加速度计）的测量误差是影响惯性导航系统精度的主要因素。传统IMU误差模型中，往往认为其固定性误差—安装误差和标度因数误差在静态标定后完全补偿，IMU误差中仅包含随机漂移误差。而在飞行器动态飞行过程中，由振动冲击、气流扰动等影响导致的机体变形将引起IMU的轴线不能与机体轴线完全重合，进而导致安装误差及标度因数误差显著增大。这些误差如果在飞行器动态飞行过程中不能进行标定补偿将在很大程度上影响导航精度。

传统的IMU误差标定方法多利用转台在室内进行位置试验及速率试验实现对安装误差和标度因数误差的标定。但在飞行器动态飞行过程中，由于环境影响，传统利用转台的标定方法将不再适用。飞行器动态飞行过程中，传统对惯性传感器的误差修正仅针对其随机性误差，不考虑其安装误差和标度因数误差等固定性误差的影响。而由于飞行振动等影响引起的安装误差和标度因数误差将会很大程度上影响惯性导航系统精度，必须对其进行标定和补偿修正。因此，研究一种在飞行器动态飞行过程中对惯性传感器固定性误差的标定方法，能够有效提高飞行器动态飞行过程中的惯性导航系统精度，将具有突出的应用价值。

发明内容

本发明的目的在于提出一种惯性导航系统惯性传感器的固定性误差建模及实时标定方法，以满足飞行器动态飞行过程中的对导航系统的高精度要求。

本发明为解决上述技术问题采用以下技术方案，一种惯性导航系统中惯性传感器固定误差实时标定方法，所述方法具体步骤如下：

步骤1、建立惯性传感器的固定性误差模型，包括加速度计标度因数误差矩阵和陀螺仪的标度因数误差矩阵：

加速度计标度因数误差矩阵为δK_A＝diag[δK_Ax δK_Ay δK_Az]，其中，δK_Ax、δK_Ay、δK_Az分别为X轴、Y轴及Z轴加速度计的标度因数误差；加速度计安装误差矩阵为 $\mathrm{\δA}=\left[\begin{array}{ccc}0& {\mathrm{\δA}}_z& -{\mathrm{\δA}}_y\\ -{\mathrm{\δA}}_z& 0& {\mathrm{\δA}}_x\\ {\mathrm{\δA}}_y& -{\mathrm{\δA}}_x& 0\end{array}\right],$ 其中，δA_x、δA_y、δA_z分别为X轴、Y轴及Z轴加速度计的安装误差角；δK_A和δA均取为随机常数，X、Y、Z三轴的加速度计误差模型相同，如下所示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{K}}_A=0\\ \mathrm{\δ}\stackrel{\·}{A}=0\end{array}\right.$

上式中，为标度因数误差矩阵δK_A的一阶导数，为安装误差矩阵δA的一阶导数；

陀螺仪的标度因数误差矩阵为δK_G＝diag[δK_Gx δK_Gy δK_Gz]，δK_Gx、δK_Gy、δK_Gz分别为X轴、Y轴及Z轴陀螺的标度因数误差；陀螺安装误差矩阵为 $\mathrm{\δG}=\left[\begin{array}{ccc}0& {\mathrm{\δG}}_z& -{\mathrm{\δG}}_y\\ -{\mathrm{\δG}}_z& 0& {\mathrm{\δG}}_x\\ {\mathrm{\δG}}_y& -{\mathrm{\δG}}_x& 0\end{array}\right],$ δG_x、δG_y、δG_z分别为X轴、Y轴及Z轴陀螺的安装误差；标度因数误差和安装误差均取为随机常数，X、Y、Z三轴的陀螺仪误差模型相同，如下所示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{K}}_G=0\\ \mathrm{\δ}\stackrel{\·}{G}=0\end{array}\right.$

上式中，为标度因数误差矩阵δK_G的一阶导数，为安装误差矩阵δG的一阶导数；

步骤2、在步骤1对IMU的安装误差和标度因数误差建模的基础上，将IMU的安装误差和标度因数误差作为系统状态变量，构建基于惯性传感器全状态误差模型的滤波器状态方程和位置、速度及姿态线性化量测方程：

基于惯性传感器全状态误差建模的系统状态变量X为：

X＝[φ_E φ_N φ_U δV_E δV_N δV_U δL δλ δh ε_bx ε_by ε_bz ε_rx ε_ry ε_rz ▽_x ▽_y ▽_zδA_x δA_y δA_z δK_Ax δK_Ay δK_Az δG_x δG_y δG_z δK_Gx δK_Gy δK_Gz]^T

上式中，φ_E,φ_N,φ_U分别为惯性导航系统中的东向、北向及天向平台误差角状态量；δV_E,δV_N,δV_U分别为惯性导航系统误差中的东向、北向及天向速度误差状态量；δL,δλ,δh分别为惯性导航系统中的纬度误差状态量、经度误差状态量及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中的X轴、Y轴、Z轴方向陀螺的常值漂移误差状态量；ε_rx,ε_ry,ε_rz分别为X轴、Y轴和Z轴方向的陀螺一阶马尔科夫漂移误差状态量；▽_x,▽_y,▽_z分别为X轴、Y轴及Z轴方向的加速度计一阶马尔科夫零偏误差状态量；δA_x,δA_y,δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的安装误差状态量；δK_Ax,δK_Ay,δK_Az分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的标度因数误差状态量；δG_x,δG_y,δG_z分别为惯性导航系统中X轴、Y轴及Z轴三个方向上陀螺的安装误差状态量；δK_Gx,δK_Gy,δK_Gz为惯性导航系统中X轴、Y轴及Z轴方向上陀螺的标度因数误差状态量；

建立惯性导航系统的误差状态方程，如式(4)所示：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

上式中，X(t)为系统状态变量；为状态变量X(t)的一阶导数；F(t)为系统矩阵；G(t)为噪声系数矩阵；W(t)为噪声矩阵；

采用地理系下位置、速度和姿态线性化观测原理，建立地理系下卫星导航位置观测量、速度观测量及星敏感器姿态观测量和惯性导航系统误差状态量之间的线性化量测方程：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\φ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\φ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\φ}}\left(t\right)\end{array}\right]$

上式中，Z_p(t)、Z_v(t)、Z_φ(t)分别为位置、速度及姿态量测矢量；H_p(t)、H_v(t)、H_φ(t)分别为位置、速度及姿态量测系数矩阵；V_p(t)、V_v(t)、V_φ(t)分别为位置、速度及姿态观测量噪声矩阵；

步骤3、在步骤2的基础上，对系统状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对IMU安装误差和标度因数误差的标定与补偿。

进一步的，步骤3中所述对IMU安装误差和标度因数误差的标定与补偿，其具体步骤为：

（301）将滤波器状态方程和量测方程离散化处理：

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(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时刻系统的噪声矩阵；Z(k)为t_k时刻系统的位置、速度及姿态观测量矩阵；H(k)为t_k时刻的位置、速度及姿态量测系数矩阵；V(k)为t_k时刻的位置、速度及姿态观测量的噪声矩阵；

（302）按照式(8)、式(9)、式(10)、式(11)对步骤（301）部分的惯性导航系统误差状态量和协方差信息的量测更新：

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1)^T+Γ(k,k-1)Q(k)Γ(k,k-1)^T

K(k)＝P(k,k-1)H(k)^T[H(k)P(k,k-1)H(k)^T+R(k)]^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)

上述公式中，P(k,k-1)为t_k-1时刻至t_k时刻一步预测协方差矩阵；P(k-1,k-1)为t_k-1时刻滤波状态估计协方差矩阵；Q(k)为t_k时刻系统的噪声协方差矩阵；Κ(k)为t_k时刻滤波增益矩阵；R(k)为t_k时刻位置、速度及姿态量测噪声协方差阵；X(k,k-1)为t_k-1时刻至t_k时刻一步预测状态量；P(k,k)为t_k时刻滤波状态估计协方差矩阵；I为与P(k,k)维数相同的单位矩阵；

（303）对滤波输出的状态量X(k,k)进行判断，若状态量X(k,k)的预测值稳定则完成标定，得到对IMU安装误差和标度因数误差的标定结果，进入步骤（304）；若预测值不稳定，则返回步骤（302）继续对状态值进行预测估计；

（304）在步骤（303）得到对IMU安装误差和标度因数误差的标定结果后，暂存标定值，利用标定值对IMU安装误差和标度因数误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\δK}}_A-\mathrm{\δA}]({\tilde{f}}^b-{\▿}^b)\\ {\mathrm{\ω}}_{\mathrm{ib}}^b=[I-{\mathrm{\δK}}_G-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}^b)\end{array}\right.$

上式中，分别为误差的加速度计输出的加速度和陀螺仪输出的角速率；f^b、分别为去除掉误差后的加速度和角速率；▽^b为加速度计的随机漂移误差，ε^b为陀螺的随机漂移误差。

本发明在对惯性传感器误差建模的基础上，能够在飞行器动态飞行过程中实现对惯性传感器固定性误差的实时标定，利用标定得到的结果可以对惯性传感器误差进行实时补偿，采用本发明可以显著减小惯性导航系统误差能够有效提高惯性导航系统精度，适合工程应用。

附图说明

图1为本发明的惯性导航系统惯性传感器固定性误差实时标定方法结构图。

图2为标定所用的飞行轨迹。

图3为X轴、Y轴及Z轴陀螺的安装误差标定结果。

图4为X轴、Y轴及Z轴陀螺的标度因数误差标定结果。

图5为X轴、Y轴及Z轴加速度计的安装误差标定结果。

图6为X轴、Y轴及Z轴加速度计的标度因数误差标定结果。

图7为惯性传感器固定性误差标定补偿后的惯性导航姿态误差。

图8为惯性传感器固定性误差标定补偿后的惯性导航速度误差。

图9为惯性传感器固定性误差标定补偿后的惯性导航位置误差。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：如图1所示，本发明所述的惯性传感器固定性误差实时标定方法的原理是：陀螺和加速度计的测量输出分别为及其中均包含了固定性和随机性误差。及f^b是经过误差补偿修正后的角速率和加速度信息用于惯性导航系统的导航解算模块。全状态误差建模模块在传统误差模型的基础上建立IMU的安装误差和标度因数误差模型，构建惯性导航系统误差的滤波状态量及状态方程；同时，根据地理系下姿态、速度和位置线性化观测原理，建立地理系下速度、位置及姿态的线性化量测方程，实现对惯性传感器安装误差和标度因数误差的实时标定；利用标定的结果在IMU误差修正模块对IMU的固定性误差进行补偿修正，能够有效提高惯性导航系统精度。

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

1.建立惯性传感器固定性误差模型

（1.1）加速度计误差模型

在捷联惯性导航系统中，由于飞行器振动会导致陀螺仪和加速度计都存在安装误差及标度因数误差。忽略高阶非线性误差项，加速度计的误差模型为：

δf^b＝[δK_A+δA]f^b+▽^b   (1)

式(1)中，δK_A＝diag[δK_Ax δK_Ay δK_Az]为加速度计的标度因数误差；

▽＝[▽_x ▽_y ▽_z]^T为加速度计的零偏误差； $\mathrm{\δA}=\left[\begin{array}{ccc}0& {\mathrm{\δA}}_z& -{\mathrm{\δA}}_y\\ -{\mathrm{\δA}}_z& 0& {\mathrm{\δA}}_x\\ {\mathrm{\δA}}_y& -{\mathrm{\δA}}_x& 0\end{array}\right]$ 为加速度计的安装误差角矩阵；

加速度计的标度因数误差δK_A和安装误差δA均取为随机常数，加速度计的零偏误差▽取为一阶马尔可夫过程：且假定三个轴加速度计的误差模型相同：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{K}}_A=0\\ \mathrm{\δ}\stackrel{\·}{A}=0\\ \stackrel{\·}{\▿}=-\frac{1}{T_a}\▿+w_a\end{array}\right.---\left(2\right)$

（1.2）陀螺误差模型

与加速度计误差建模方法类似，陀螺的误差模型为：

${\mathrm{\δ\ω}}_{\mathrm{ib}}^b=[{\mathrm{\δK}}_G+\mathrm{\δG}]{\mathrm{\ω}}_{\mathrm{ib}}^b+{\mathrm{\ϵ}}^b---\left(3\right)$

式(3)中，δK_G＝diag[δK_Gx δK_Gy δK_Gz]为陀螺的标度因数误差矩阵； $\mathrm{\δG}=\left[\begin{array}{ccc}0& {\mathrm{\δG}}_z& -{\mathrm{\δG}}_y\\ -{\mathrm{\δG}}_z& 0& {\mathrm{\δG}}_x\\ {\mathrm{\δG}}_y& -{\mathrm{\δG}}_x& 0\end{array}\right]$ 为陀螺的安装误差角矩阵；陀螺仪的标度因数误差和

安装误差均取为随机常数，假定三个轴陀螺的误差模型相同，得到：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{K}}_G=0\\ \mathrm{\δ}\stackrel{\·}{G}=0\end{array}\right.---\left(4\right)$

陀螺漂移误差ε_b＝[ε_x ε_y ε_z]^T取为

ε＝ε_b+ε_r+w_g (5)

式(5)中，为随机常数，为一阶马尔可夫过程，w_g为白噪声。

2.建立基于全状态误差模型的卡尔曼滤波器模型

（2.1）建立惯性导航系统误差的状态量方程

选取机体坐标系(b系)为机体的“右、前、上”(X，Y，Z)方向，选取导航坐标系(n系)为东北天(ENU)地理坐标系。基于全状态误差建模的系统状态变量为：

X＝[φ_E φ_N φ_U δV_E δV_N δV_U δL δλ δh ε_bx ε_by ε_bz ε_rx ε_ry ε_rz ▽_x ▽_y ▽_zδA_x δA_y δA_z δK_Ax δK_Ay δK_Az δG_x δG_y δG_z δK_Gx δK_Gy δK_Gz]^T

(6)

式(6)中，φ_E,φ_N,φ_U分别为惯性导航系统中的东向、北向及天向平台误差误差角状态量；δV_E,δV_N,δV_U分别为惯性导航系统误差中的东向、北向及天向速度误差状态量；δL,δλ,δh表示惯性导航系统中的纬度误差状态量、经度误差状态量及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中的X轴、Y轴、Z轴方向陀螺的常值漂移误差状态量；ε_rx,ε_ry,ε_rz分别为X轴、Y轴和Z轴方向的陀螺一阶马尔科夫漂移误差状态量；▽_x,▽_y,▽_z分别为惯性导航系统中X轴、Y轴及Z轴方向的加速度计一阶马尔科夫零偏误差状态量；δA_x,δA_y,δA_z为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的安装误差状态量；δK_Ax,δK_Ay,δK_Az为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的标度因数误差状态量；δG_x,δG_y,δG_z为惯性导航系统中X轴、Y轴及Z轴三个方向上陀螺的安装误差状态量；δK_Gx,δK_Gy,δK_Gz为惯性导航系统中X轴、Y轴及Z轴方向上陀螺的标度因数误差状态量；

建立惯性导航系统的误差状态方程，如式(7)所示：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(7\right)$

式(7)中，X(t)为系统状态变量，为状态变量X(t)的一阶导数；F(t)为系统矩阵，G(t)为噪声系数矩阵，W(t)为噪声矩阵。

根据步骤1中所建立的惯性传感器误差模型，与式(6)及式(7)对应的系统矩阵F(t)为：

$F\left(t\right)=\left[\begin{array}{ccc}{\mathrm{FN}\left(t\right)}_{9\×9}& \mathrm{FS}{\left(t\right)}_{9\×9}& {\mathrm{Fs}\left(t\right)}_{9\×12}\\ 0_{9\×9}& {\mathrm{FM}\left(t\right)}_{9\×9}& 0_{9\×12}\\ & 0_{12\×30}& \end{array}\right]---\left(8\right)$

式(8)中，FN(t)_9×9为对应9个惯性导航系统导航参数（姿态、速度及位置）之间的关系矩阵，其非零项元素为：

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{1,2}\right)={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L\\ \mathrm{FN}\left(\mathrm{1,3}\right)=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{1,5}\right)=-\frac{1}{R_M+h}\end{array}\right.$ $\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{2,1}\right)=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\\ \mathrm{FN}\left(\mathrm{2,3}\right)=-\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{2,4}\right)=\frac{1}{R_N+h}\\ \mathrm{FN}\left(\mathrm{2,7}\right)=-{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{3,1}\right)={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}\\ \mathrm{FN}\left(\mathrm{3,2}\right)=\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{3,4}\right)=\frac{\tan L}{R_N+h}\\ \mathrm{FN}\left(\mathrm{3,7}\right)={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{4,2}\right)=-f_u\\ \mathrm{FN}\left(\mathrm{4,3}\right)=f_n\\ \mathrm{FN}\left(\mathrm{4,4}\right)=\frac{V_N\tan L-V_U}{R_N+h}\\ \mathrm{FN}\left(\mathrm{4,5}\right)={2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L\\ \mathrm{FN}\left(\mathrm{4,6}\right)=-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{4,7}\right)={2\mathrm{\ω}}_{\mathrm{ie}}(V_U\sin L+V_N\cos L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{5,1}\right)=f_u\\ \mathrm{FN}\left(\mathrm{5,3}\right)=-f_e\\ \mathrm{FN}\left(\mathrm{5,4}\right)=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)\\ \mathrm{FN}\left(\mathrm{5,5}\right)=-\frac{V_U}{R_M+h}\\ \mathrm{FN}\left(\mathrm{5,6}\right)=-\frac{V_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{5,7}\right)=-({2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E^2}{R_N+h}{\sec }^{2}L)\end{array}\right.$ $\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{6,1}\right)=-f_n\\ \mathrm{FN}\left(\mathrm{6,2}\right)=f_e\\ \mathrm{FN}\left(\mathrm{6,4}\right)=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})\\ \mathrm{FN}\left(\mathrm{6,5}\right)=\frac{{2V}_N}{R_M+h}\\ \mathrm{FN}\left(\mathrm{6,7}\right)=-{2V}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{FN}\left(\mathrm{7,5}\right)=\frac{1}{R_M+h}\\ \mathrm{FN}\left(\mathrm{8,4}\right)=\frac{\sec L}{R_N+h}\\ \mathrm{FN}\left(\mathrm{8,7}\right)=\frac{V_E}{R}\\ \mathrm{FN}\left(\mathrm{9,6}\right)=1\end{array}\right.$

R_M为地球子午圈曲率半径，R_N为地球卯酉圈曲率半径；f_e,f_n,f_u为东向、北向及天向的加速度。

式(8)中， $\mathrm{FS}\left(t\right)=\left[\begin{array}{ccc}{C}_b^n& C_b^n& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& C_b^n\\ & 0_{3\×9}& \end{array}\right],$ 为机体系到地理坐标系的转换矩阵

${\mathrm{Fs}\left(t\right)}_{9\×12}=\left[\begin{array}{ccc}{0}_{3\×6}& F_1{\left(t\right)}_{3\×3}& F_3{\left(t\right)}_{3\×3}\\ F_2{\left(t\right)}_{3\×3}& F_4{\left(t\right)}_{3\×3}& 0_{3\×6}\\ & 0_{3\×12}& \end{array}\right]---\left(9\right)$

式(9)中：

$F_1\left(t\right)=\left[\begin{array}{ccc}-C_{12}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{13}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{11}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{13}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{11}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{12}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -C_{22}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{23}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{21}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{23}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{21}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{22}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ -C_{32}{\mathrm{\ω}}_{\mathrm{ibz}}^b+C_{33}{\mathrm{\ω}}_{\mathrm{iby}}^b& C_{31}{\mathrm{\ω}}_{\mathrm{ibz}}^b-C_{33}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{31}{\mathrm{\ω}}_{\mathrm{iby}}^b+C_{32}{\mathrm{\ω}}_{\mathrm{ibx}}^b\end{array}\right],$

$F_2\left(t\right)=\left[\begin{array}{ccc}{C}_{12}{f}_z^b-C_{13}{f}_y^b& -C_{11}{f}_z^b+C_{13}{f}_x^b& C_{11}{f}_y^b-C_{12}{f}_x^b\\ C_{22}{f}_z^b-C_{23}{f}_y^b& -C_{21}{f}_z^b+C_{23}{f}_x^b& C_{21}{f}_y^b-C_{22}{f}_x^b\\ C_{32}{f}_z^b-C_{33}{f}_y^b& -C_{31}{f}_z^b+C_{33}{f}_x^b& C_{31}{f}_y^b-C_{32}{f}_x^b\end{array}\right]$

$F_3\left(t\right)=\left[\begin{array}{ccc}{-C}_{11}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{12}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{13}{\mathrm{\ω}}_{\mathrm{ibz}}^b\\ {-C}_{21}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{22}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{23}{\mathrm{\ω}}_{\mathrm{ibz}}^b\\ -C_{31}{\mathrm{\ω}}_{\mathrm{ibx}}^b& -C_{32}{\mathrm{\ω}}_{\mathrm{iby}}^b& -C_{33}{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right],$ $F_4\left(t\right)=\left[\begin{array}{ccc}{C}_{11}{f}_x^b& C_{12}{f}_y^b& C_{13}{f}_z^b\\ C_{21}{f}_x^b& C_{22}{f}_y^b& C_{23}{f}_z^b\\ C_{31}{f}_x^b& C_{32}{f}_y^b& C_{33}{f}_z^b\end{array}\right]$

其中：3为机体系到地理坐标系的转换矩阵， $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ 为加速度计在机体系的输出， ${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{ibx}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T$ 为陀螺输出在机体系上的投影。

式(8)中，惯性传感器误差之间的关系矩阵为：

$\mathrm{FM}\left(t\right)=\mathrm{Diag}\left[\begin{array}{ccccccccc}0& 0& 0& -\frac{1}{T_{\mathrm{gx}}}& -\frac{1}{T_{\mathrm{gy}}}& -\frac{1}{T_{\mathrm{gz}}}& -\frac{1}{T_{\mathrm{ax}}}& -\frac{1}{T_{\mathrm{ay}}}& -\frac{1}{T_{\mathrm{az}}}\end{array}\right]---\left(10\right)$

系统噪声矩阵为：

W＝[w_gx w_gy w_gz w_rx w_ry w_rz w_ax w_ay w_az]   (11)

误差系数矩阵：

$G\left(t\right)=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{9\×3}& 0_{9\×3}& 0_{9\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]---\left(12\right)$

（2.2）量测方程的确定

采用地理系下姿态、速度和位置线性化观测原理，建立地理系下卫星导航速度、位置及星敏感器姿态观测量和步骤2.1)中惯性导航系统误差状态量之间的线性化量测方程：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\φ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\φ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\φ}}\left(t\right)\end{array}\right]---\left(13\right)$

式中，Z_p(t)，Z_v(t)以及Z_φ(t)是位置、速度及姿态量测矢量。H_p(t)，H_v(t)，H_φ(t)为位置、速度及姿态量测系数矩阵，位置、速度及姿态观测量噪声矩阵为V_p(t)，V_v(t)及V_φ(t)。3.惯性传感器固定性误差的实时标定补偿

（3.1）将滤波器状态方程和量测方程离散化处理：

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)  (14)

Z(k)＝H(k)X(k)+V(k)  (15)

式(15)中，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时刻系统的噪声矩阵；Z(k)为t_k时刻系统的位置、速度及姿态观测量矩阵；H(k)为t_k时刻的位置、速度及姿态量测系数矩阵；V(k)为t_k时刻的位置、速度及姿态观测量的噪声矩阵。

（3.2）按照式(16)、式(17)、式(18)、式(19)对子步骤3.1)部分的惯导误差状态量和协方差信息的量测更新：

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1)^T+Γ(k,k-1)Q(k)Γ(k,k-1)^T

(16)

K(k)＝P(k,k-1)H(k)^T[H(k)P(k,k-1)H(k)^T+R(k)]^-1 (17)

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)] (18)

P(k,k)＝[I-K(k)H(k)]P(k,k-1) (19)

其中，P(k,k-1)为t_k-1时刻至t_k时刻一步预测协方差矩阵；P(k-1,k-1)为t_k-1时刻滤波状态估计协方差矩阵；Q(k)为t_k时刻系统的噪声协方差矩阵；Κ(k)为t_k时刻滤波增益矩阵；R(k)为t_k时刻位置、速度及姿态量测噪声协方差阵；X(k,k-1)为t_k-1时刻至t_k时刻一步预测状态量；P(k,k)为t_k时刻滤波状态估计协方差矩阵；

I为与P(k,k)维数相同的单位阵；

（3.3）对滤波输出的状态量X(k,k)进行判断，若状态量的预测值稳定，则完成标定进入步骤（3.4）若预测值不稳定，则返回步骤（3.2）继续对状态值进行预测估计。

（3.4）在步骤（3.3）得到对IMU安装误差和标度因数误差的标定结果后，暂存标定值，利用该标定值对IMU安装误差和标度因数误差进行补偿校正，校正在一个导航解算周期内完成。误差补偿校正算法为：

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\δK}}_A-\mathrm{\δA}]({\tilde{f}}^b-{\▿}^b)\\ {\mathrm{\ω}}_{\mathrm{ib}}^b=[I-{\mathrm{\δK}}_G-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}^b)\end{array}\right.---\left(20\right)$

式(20)中，δK_A、δA、▽^b分别为加速度计的标度因数误差、安装误差和零偏，δK_G、δG、ε^b为陀螺的标度因数误差、安装误差及随机零偏，均由标定过程中获得。

（3.5）在步骤（3.4）对IMU的安装误差和标度因数误差校正补偿完成后，不再使用滤波器进入纯惯性导航系统工作模式。

为了验证发明所提出的惯性导航系统惯性传感器的固定性误差建模及标定方法的正确性及有效性，采用本发明方法建立模型，进行matlab仿真验证。图2为验证时采用的飞行航迹，对惯性传感器的安装误差及标度因数误差的标定结果如图3、图4、图5、图6所示。

在飞行器动态飞行过程中利用标定结果对IMU安装误差和标度因数误差进行补偿修正，补偿后的惯性导航结果与未经过补偿的惯性导航导航结果进行对比验证，验证结果如图7、图8、图9所示。

图3、图4、图5、图6中黑色实线代表本发明的标定结果，红色点划线为设定值。从图中可以看出对IMU安装误差和标度因数误差标定所用时间在20s左右，标定结果与设定值一致。图7、图8、图9中黑色实线代表未补偿IMU固定性误差的纯惯导导航结果，红色点划线代表补偿IMU固定性误差后的纯惯导导航结果。从图7、图8、图9中可以看出，利用对IMU安装误差和标度因数误差的标定结果对IMU的安装误差和标度因数误差进行校正后，惯性导航系统误差明显减小，具有有益的工程应用价值。

Claims (2)

1.一种惯性导航系统中惯性传感器固定误差实时标定方法，其特征在于，所述方法具体步骤如下：

步骤1、建立惯性传感器的固定性误差模型，包括加速度计标度因数误差矩阵和陀螺仪的标度因数误差矩阵：

加速度计标度因数误差矩阵为δK_A＝diag[δK_Ax δK_Ay δK_Az]，其中，δK_Ax、δK_Ay、δK_Az分别为X轴、Y轴及Z轴加速度计的标度因数误差；加速度计安装误差矩阵为 $\mathrm{\δA}=\left[\begin{array}{ccc}0& {\mathrm{\δA}}_z& -{\mathrm{\δA}}_y\\ -{\mathrm{\δA}}_z& 0& {\mathrm{\δA}}_x\\ {\mathrm{\δA}}_y& {-\mathrm{\δA}}_x& 0\end{array}\right],$ 其中，δA_x、δA_y、δA_z分别为X轴、Y轴及Z轴加速度计的安装误差角；δK_A和δA均取为随机常数，X、Y、Z三轴的加速度计误差模型相同，如下所示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{K}}_A=0\\ \mathrm{\δ}\stackrel{.}{A}=0\end{array}\right.$

上式中，为标度因数误差矩阵δK_A的一阶导数，为安装误差矩阵δA的一阶导数；

陀螺仪的标度因数误差矩阵为δK_G＝diag[δK_Gx δK_Gy δK_Gz]，δK_Gx、δK_Gy、δK_Gz分别为X轴、Y轴及Z轴陀螺的标度因数误差；陀螺安装误差矩阵为 $\mathrm{\δG}=\left[\begin{array}{ccc}0& {\mathrm{\δG}}_z& -{\mathrm{\δG}}_y\\ -{\mathrm{\δG}}_z& 0& {\mathrm{\δG}}_x\\ {\mathrm{\δG}}_y& {-\mathrm{\δG}}_x& 0\end{array}\right],$ δG_x、δG_y、δG_z分别为X轴、Y轴及Z轴陀螺的安装误差；标度因数误差和安装误差均取为随机常数，X、Y、Z三轴的陀螺仪误差模型相同，如下所示：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{K}}_G=0\\ \mathrm{\δ}\stackrel{.}{G}=0\end{array}\right.$

上式中，为标度因数误差矩阵δK_G的一阶导数，为安装误差矩阵δG的一阶导数；

步骤2、在步骤1对IMU的安装误差和标度因数误差建模的基础上，将IMU的安装误差和标度因数误差作为系统状态变量，构建基于惯性传感器全状态误差模型的滤波器状态方程和位置、速度及姿态线性化量测方程：

基于惯性传感器全状态误差建模的系统状态变量X为：

$\begin{array}{c}X=\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\ϵ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{ry}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right.\\ {\left.\begin{array}{cccccccccccc}{\mathrm{\δA}}_x& {\mathrm{\δA}}_y& {\mathrm{\δA}}_z& {\mathrm{\δK}}_{\mathrm{Ax}}& {\mathrm{\δK}}_{\mathrm{Ax}}& {\mathrm{\δK}}_{\mathrm{Az}}& {\mathrm{\δG}}_x& {\mathrm{\δG}}_y& {\mathrm{\δG}}_z& {\mathrm{\δK}}_{\mathrm{Gx}}& {\mathrm{\δK}}_{\mathrm{Gy}}& {\mathrm{\δK}}_{\mathrm{Gz}}\end{array}\right]}^T\end{array}$

上式中，φ_E,φ_N,φ_U分别为惯性导航系统中的东向、北向及天向平台误差角状态量；δV_E,δV_N,δV_U分别为惯性导航系统误差中的东向、北向及天向速度误差状态量；δL,δλ,δh分别为惯性导航系统中的纬度误差状态量、经度误差状态量及高度误差状态量；ε_bx,ε_by,ε_bz分别为惯性导航系统中的X轴、Y轴、Z轴方向陀螺的常值漂移误差状态量；ε_rx,ε_ry,ε_rz分别为X轴、Y轴和Z轴方向的陀螺一阶马尔科夫漂移误差状态量；分别为惯性导航系统中X轴、Y轴及Z轴方向的加速度计一阶马尔科夫零偏误差状态量；δA_x,δA_y,δA_z分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的安装误差状态量；δK_Ax,δK_Ay,δK_Az分别为惯性导航系统中X轴、Y轴及Z轴方向上加速度计的标度因数误差状态量；δG_x,δG_y,δG_z分别为惯性导航系统中X轴、Y轴及Z轴三个方向上陀螺的安装误差状态量；δK_Gx,δK_Gy,δK_Gz为惯性导航系统中X轴、Y轴及Z轴方向上陀螺的标度因数误差状态量；

建立惯性导航系统的误差状态方程，如下式所示：

$\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

上式中，X(t)为系统状态变量；为状态变量X(t)的一阶导数；F(t)为系统矩阵；G(t)为噪声系数矩阵；W(t)为噪声矩阵；

采用地理系下位置、速度和姿态线性化观测原理，建立地理系下卫星导航位置观测量、速度观测量及星敏感器姿态观测量和惯性导航系统误差状态量之间的线性化量测方程：

$Z\left(t\right)=\left[\begin{array}{c}{Z}_p\left(t\right)\\ Z_v\left(t\right)\\ Z_{\mathrm{\φ}}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{H}_p\left(t\right)\\ H_v\left(t\right)\\ H_{\mathrm{\φ}}\left(t\right)\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_p\left(t\right)\\ V_v\left(t\right)\\ V_{\mathrm{\φ}}\left(t\right)\end{array}\right]$

上式中，Z_p(t)、Z_v(t)、Z_φ(t)分别为位置、速度及姿态量测矢量；H_p(t)、H_v(t)、H_φ(t)分别为位置、速度及姿态量测系数矩阵；V_p(t)、V_v(t)、V_φ(t)分别为位置、速度及姿态观测量噪声矩阵；

步骤3、在步骤2的基础上，对系统状态方程和量测方程进行离散化处理以及状态量、量测量的更新，实现对IMU安装误差和标度因数误差的标定与补偿。

2.如权利要求1所述的一种惯性导航系统中惯性传感器固定误差实时标定方法，其特征在于，步骤3中所述对IMU安装误差和标度因数误差的标定与补偿，其具体步骤为：

(301)将滤波器状态方程和量测方程离散化处理：

X(k)＝Φ(k,k-1)X(k-1)+Γ(k,k-1)W(k-1)

Z(k)＝H(k)X(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时刻系统的噪声矩阵；Z(k)为t_k时刻系统的位置、速度及姿态观测量矩阵；H(k)为t_k时刻的位置、速度及姿态量测系数矩阵；V(k)为t_k时刻的位置、速度及姿态观测量的噪声矩阵；

(302)按照下列公式对步骤(301)部分的惯性导航系统误差状态量和协方差信息的量测更新：

P(k,k-1)＝Φ(k,k-1)P(k-1,k-1)Φ(k,k-1)^T+Γ(k,k-1)Q(k)Γ(k,k-1)^T

K(k)＝P(k,k-1)H(k)^T[H(k)P(k,k-1)H(k)^T+R(k)]^-1

X(k)＝X(k,k-1)+K(k)[Z(k)-H(k)X(k,k-1)]

P(k,k)＝[I-K(k)H(k)]P(k,k-1)

上述公式中，P(k,k-1)为t_k-1时刻至t_k时刻一步预测协方差矩阵；P(k-1,k-1)为t_k-1时刻滤波状态估计协方差矩阵；Q(k)为t_k时刻系统的噪声协方差矩阵；Κ(k)为t_k时刻滤波增益矩阵；R(k)为t_k时刻位置、速度及姿态量测噪声协方差阵；X(k,k-1)为t_k-1时刻至t_k时刻一步预测状态量；P(k,k)为t_k时刻滤波状态估计协方差矩阵；I为与P(k,k)维数相同的单位矩阵；

(303)对滤波输出的状态量X(k,k)进行判断，若状态量X(k,k)的预测值稳定则完成标定，得到对IMU安装误差和标度因数误差的标定结果，进入步骤(304)；若预测值不稳定，则返回步骤(302)继续对状态值进行预测估计；

(304)在步骤(303)得到对IMU安装误差和标度因数误差的标定结果后，暂存标定值，利用标定值对IMU安装误差和标度因数误差进行补偿校正，校正在一个导航解算周期内完成，误差补偿校正算法为：

$\left\{\begin{array}{c}{f}^b=[I-{\mathrm{\δK}}_A-\mathrm{\δA}]({\tilde{f}}^b-{\▿}^b)\\ {\mathrm{\ω}}_{\mathrm{ib}}^b=[I-{\mathrm{\δK}}_G-\mathrm{\δG}]({\widetilde{\mathrm{\ω}}}_{\mathrm{ib}}^b-{\mathrm{\ϵ}}^b)\end{array}\right.$

上式中，分别为误差的加速度计输出的加速度和陀螺仪输出的角速率；f^b、分别为去除掉误差后的加速度和角速率；为加速度计的随机漂移误差，ε^b为陀螺的随机漂移误差。