CN104181574B - 一种捷联惯导***/全球导航卫星***组合导航滤波***及方法 - Google Patents

Abstract

本发明公开了一种捷联惯导***/全球导航卫星***组合导航滤波***，由一次装订模块、地球参数解算模块、初始对准模块、捷联惯导解算模块、改进滤波算法参数计算模块、正常模式组合导航滤波模块、故障模式组合导航滤波模块和反馈校正输出模块组成；改进滤波算法参数计算模块根据SINS和GNSS提供的数据，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据结果及状态标志传递至正常模式或故障模式组合导航滤波模块中进行改进滤波计算。本发明将组合导航滤波算法模块化，显著提高了组合导航的精度，加快了组合导航滤波算法的收敛速度，且可有效抑制卡尔曼滤波的发散。

Description

一种捷联惯导***/全球导航卫星***组合导航滤波***及 方法

技术领域

本发明涉及组合导航信息融合技术，特别是涉及一种捷联惯导***/全球导航卫星***组合导航滤波***及方法。

背景技术

捷联惯导***（SINS）能够完全自主并实时地提供载体的姿态、速度和位置信息，广泛应用于航空、航天和航海等军用及民用领域。但其突出的缺点是导航误差会随时间积累，不能独立完成长时间的导航任务。全球导航卫星***（GNSS）具备长时间高精度导航的能力，但其缺点是卫星信号容易受干扰，无法提供连续的导航信息。

SINS/GNSS组合导航***将GNSS与SINS结合起来进行导航，充分利用了各自的优点，并弥补了各自的缺点，可以得到一个既能全天候自主导航又能保证长时间导航精度的***。Kalman滤波技术是目前实现SINS/GNSS组合导航信息融合的有效方法，通过滤波技术实现对姿态、速度和位置误差以及惯性器件误差的最优估计，进而实现误差的反馈校正。这样，在GNSS信号良好时能够输出高精度的导航信息，同时维持SINS***的稳定；在GNSS失锁时，进入纯惯导模式，继续提供缓慢发散的导航信息。

然而，基于Kalman滤波的SINS/GNSS组合导航***在GNSS信号失锁重捕后，由纯惯导模式切换进入组合模式，依靠GNSS信息将纯惯导缓慢发散的导航信息收敛到正确值。该收敛过程的持续时间受纯惯导模式运行时间的影响，纯惯导模式运行时间越长，收敛过程越缓慢，有时甚至伴随着震荡过程，大大降低了组合导航的精度和快速性。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于改进卡尔曼滤波算法的模块化实现捷联惯导***/全球导航卫星***组合导航滤波***及方法，将组合导航滤波算法模块化，显著提高组合导航精度，同时大大加快组合导航滤波算法的收敛速度，并有效抑制卡尔曼滤波的发散。

本发明的目的是通过以下技术方案来实现的：一种捷联惯导***/全球导航卫星***组合导航滤波***，它由一次装订模块、地球参数解算模块、初始对准模块、捷联惯导解算模块、改进滤波算法参数计算模块、正常模式组合导航滤波模块、故障模式组合导航滤波模块和反馈校正输出模块组成；

所述一次装订模块用于根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

所述地球参数解算模块用于根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数；

所述初始对准模块用于根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

所述捷联惯导解算模块用于根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算；

所述改进滤波算法参数计算模块用于根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

所述正常模式组合导航滤波模块包括状态预测子模块、协方差预测子模块、增益计算子模块、状态估计子模块和协方差估计子模块组成；

所述故障模式组合导航滤波模块用于根据***提供的错误状态标志，通过改进卡尔曼滤波计算的状态预测功能，通过后端处理方式排除前端出现的错误；

所述反馈校正输出模块用于根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出。

进一步地，所述的地球参数包括地球自转角速度投影分量、导航坐标系相对地球的旋转角速度和载体所在位置的重力加速度信息。

进一步地，所述的捷联惯导解算模块进行导航解算过程中，采用旋转矢量四元数算法解算载体的姿态角，采用双子样速度算法解算载体的速度，采用速度积分算法解算载体的当前位置。

一种捷联惯导***/全球导航卫星***组合导航滤波方法，它包括以下步骤：

S1：一次装订模块根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

S2：地球参数解算模块根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数；

S3：初始对准模块根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

S4：捷联惯导解算模块根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算；

S5：改进滤波算法参数计算模块根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

S6：反馈校正输出模块根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出。

本发明的有益效果是：

1）将组合导航滤波算法模块化，并在单片DSP上设计实现，根据不同的状态标志执行不同的模块，消除了组合导航滤波算法所受卫星状态的限制，显著提高了组合导航的精度，同时大大加快了组合导航滤波算法的收敛速度，还能有效地抑制卡尔曼滤波的发散；

2）故障模式组合导航滤波模块根据***提供的错误状态标志，利用改进卡尔曼滤波的状态预测功能，通过后端处理方式排除前端出现的错误，提高了组合导航***的故障诊断和容错能力。

附图说明

图1为本发明组合导航滤波方法流程图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

一种捷联惯导***/全球导航卫星***组合导航滤波***，它由一次装订模块、地球参数解算模块、初始对准模块、捷联惯导解算模块、改进滤波算法参数计算模块、正常模式组合导航滤波模块、故障模式组合导航滤波模块和反馈校正输出模块组成；

所述一次装订模块用于根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

所述地球参数解算模块用于根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数，地球参数包括地球自转角速度投影分量、导航坐标系相对地球的旋转角速度和载体所在位置的重力加速度等信息；

所述初始对准模块用于根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

所述捷联惯导解算模块用于根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算；其中，采用旋转矢量四元数算法解算载体的姿态角，采用双子样速度算法解算载体的速度，采用速度积分算法解算载体的当前位置；

所述改进滤波算法参数计算模块用于根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

所述正常模式组合导航滤波模块包括状态预测子模块、协方差预测子模块、增益计算子模块、状态估计子模块和协方差估计子模块组成；

所述故障模式组合导航滤波模块用于根据***提供的错误状态标志，通过改进卡尔曼滤波计算的状态预测功能，通过后端处理方式排除前端出现的错误，由此提高组合导航***的故障诊断和容错能力；

所述反馈校正输出模块用于根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出，由此提高组合导航***的精度，大大加快卡尔曼滤波的收敛速度，并有效地抑制卡尔曼滤波的发散。

一种捷联惯导***/全球导航卫星***组合导航滤波方法，它包括以下步骤：

S1：一次装订模块根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

S2：地球参数解算模块根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数；

S3：初始对准模块根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

S4：捷联惯导解算模块根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算；

S5：改进滤波算法参数计算模块根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

S6：反馈校正输出模块根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出。

如图1所示，以北斗/捷联惯导***组合导航滤波方法为例，其步骤为：

SS1：BD/INS组合导航开始；

SS2：初始对准：初始对准模块根据一次装订模块和北斗全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息；

SS3：捷联惯导解算模块根据地球参数解算模块计算得到的地球相关参数进行组合导航滤波参数计算；

SS4：判断BD是否有效：无效则结合经验数据进行改进的组合导航滤波处理；有效则结合量测数据进行组合导航滤波处理；

SS5：校正输出。

组合导航***的状态方程：

滤波器取导航***各状态的误差量作为滤波器的状态变量，包括：平台误差角 导航***的速度误差δV_E、δV_N、δV_U，位置误差δL、δλ、δH，陀螺的随机常值漂移ε_bE、ε_bN、ε_bU，陀螺的相关漂移ε_rE、ε_rN、ε_rU及加速度计零偏▽_E、▽_N、▽_U。

姿态误差角方程：

当考虑飞行器高度和地球为圆球体时，姿态误差角方程为：

式中：R_M和R_N分别表示地球参考椭球子午圈和卯酉圈上各点的曲率半径，表达式如下：

R_M＝R_e(1-2f+3fsin²L);

R_N＝R_e(1+fsin²L);

R_e＝6378137m;

f＝1/298.257。

速度误差方程：

当考虑飞行高度为h和地球为圆球体时，速度误差方程为：

位置误差方程为：

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}\\ \mathrm{\δ}\stackrel{\·}{h}\end{array}\right]\left[\begin{array}{c}{\mathrm{\δv}}_N/(R_N+h)\\ {\mathrm{\δv}}_E/(R_N+h)\sec L+v_E/(R_N+h)\mathrm{\δ}L\sec L\tan L\\ {\mathrm{\δv}}_U\end{array}\right].$

惯性仪表误差：

为了简单起见，惯性仪表误差只考虑随机误差。

1）陀螺随机漂移误差模型

根据捷联惯导随机误差分析的结果可知：陀螺随机漂移的误差模型一般取为随机常数、一阶马尔科夫过程和白噪声的组合，即：ε＝ε_b+ε_r+ω_g。式中，ε_b为随机常值漂移，ε_r为一阶马尔柯夫过程，ω_g为白噪声。

假定三个轴向的陀螺漂移误差模型相同，则表达式为：

${\stackrel{\·}{\mathrm{\ϵ}}}_b=0$

${\stackrel{\·}{\mathrm{\ϵ}}}_r={\mathrm{\ϵ}}_r/T_g+{\mathrm{\ω}}_r;$

其中，T_g为相关时间。

2）加速度计误差模型

加速度计的误差模型考虑为一阶马尔科夫过程，且假定三个轴的加速度计的误差模型相同，均为零偏和随机噪声，可表示为：

$\stackrel{\·}{\▿=-\▿/T_a+{\mathrm{\ω}}_a,}$ 其中，T_a为相关时间。

SINS误差状态方程：

将平台误差角方程、速度误差方程、位置误差方程和惯性仪表方程综合在一起，得出SINS误差状态方程的一般表达式为：

${\stackrel{\·}{X}}_I\left(t\right)=F_I\left(t\right)X_I\left(t\right)+G_I\left(t\right)W_I\left(t\right);$

式中：

$F_I=\left[\begin{array}{cc}{F}_N& F_S\\ 0& F_M\end{array}\right];$

F_N为对应于SINS***9个误差参数（3个姿态误差、3个速度误差、3个位置误差）的***动态矩阵。

F_S在捷联惯导中为：

$F_S=\left[\begin{array}{ccc}{C}_b^t& C_b^t& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& C_b^t\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right];$

F_M表达式为：

$F_M=-\mathrm{diag}[\mathrm{0,0,0},T_{\mathrm{gx}}^{-1},T_{\mathrm{gy}}^{-1},T_{\mathrm{gz}}^{-1},T_{\mathrm{ax}}^{-1},T_{\mathrm{ay}}^{-1},T_{\mathrm{az}}^{-1}];$

W_I表达式为：

W_I＝[ω_gx,ω_gy,ω_gz,ω_bx,ω_by,ω_bz,ω_ax,ω_ay,ω_az]^T；

G_I在捷联惯导***中的表达式为：

$G_I=\left[\begin{array}{ccc}{C}_b^t& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right].$

GPS/SINS组合***状态方程：

在组合导航过程中，对于分布式组合方式，有两个卡尔曼滤波器分别处理INS和GPS数据。两个滤波器的误差状态方程单独建立，选取相应参数进行滤波。而对于全组合式方法，只有一个卡尔曼滤波器。组合***的误差状态方程中，除了有INS的18个误差状态变量，是否还需要增加与GPS相关的误差状态变量，由观测修正信息的选择而决定。如果选择位置、速度为观测信息，则不需要在组合***中增加GPS的状态变量。当采用GPS基本观测量作为量测修正信息时，一般是利用状态扩充法将GPS接收机的钟差及钟漂扩充到组合导航***的状态向量中去，使符合卡尔曼滤波器的要求。

在本方案中，仅选择INS和GPS的位置、速度观测信息来组成量测方程，且讨论全组合的组合方式。因此，GPS/INS组合导航***的误差状态变量，仍取INS的误差状态变量，无需GPS的误差状态变量，表达式如下：

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

式中：

组合***的量测方程：

在位置、速度组合模式中，其量测值有两组：一组为位置量测差值，另一组为速度量测差值。所谓位置量测差值是由惯导***给出的经度、纬度、高度信息和GPS接收机给出的相应的信息求差，作为一组量测值；所谓速度量测差值是由惯导***给出的速度信息和GPS接收机给出的速度信息求差，作为一组量测值。

惯导***的位置量测信息可表示为地理系下的真值与相应误差之和：

$\left[\begin{array}{c}{L}_I\\ {\mathrm{\λ}}_I\\ h_I\end{array}\right]=\left[\begin{array}{c}{L}_t+\mathrm{\δL}\\ {\mathrm{\λ}}_t+\mathrm{\δ\λ}\\ h_t+\mathrm{\δh}\end{array}\right];$

GPS接收机给出的位置信息可表示为地理系下的真值与相应误差之差，由下式表示：

$\left[\begin{array}{c}{L}_G\\ {\mathrm{\λ}}_G\\ h_G\end{array}\right]=\left[\begin{array}{c}{L}_t-N_N/R_M\\ {\mathrm{\λ}}_t-N_E/R_N\cos L\\ h_t+\mathrm{Nh}\end{array}\right];$

式中，L_t,λ_t,h_t表示真实位置；N_E,N_N,Nh为接收机沿东、北、天方向的位置误差。则位置量测误差矢量为：

$Z_p\left(t\right)=\left[\begin{array}{c}(L_I-L_G)R_M\\ ({\mathrm{\λ}}_I-{\mathrm{\λ}}_G)R_N\cos L\\ h_I-h_G\end{array}\right]$

$=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_N\\ R_N\mathrm{\δ}L\cos L+N_E\\ \mathrm{\δh}+N_U\end{array}\right]$

$=H_p\left(t\right)X\left(t\right)+V_p\left(t\right).$

类似位置误差矢量，定义速度量测误差矢量为：

$Z_v\left(t\right)=\left[\begin{array}{c}{v}_{\mathrm{IN}}-v_{\mathrm{GN}}\\ v_{\mathrm{IE}}-v_{\mathrm{GE}}\\ v_{\mathrm{IU}}-v_{\mathrm{GU}}\end{array}\right]$

$=\left[\begin{array}{c}{\mathrm{\δv}}_N+M_N\\ {\mathrm{\δv}}_E+M_E\\ {\mathrm{\δv}}_U+M_U\end{array}\right]$

$=H_v\left(t\right)X\left(t\right)+V_v\left(t\right).$

将位置量测矢量和速度量测矢量合并到一起，得到GPS/INS位置、速度组合***量测方程，如下所示：

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

$=H\left(t\right)X\left(t\right)+V\left(t\right).$

Claims (3)

1.一种捷联惯导***/全球导航卫星***组合导航滤波***，其特征在于：它由一次装订模块、地球参数解算模块、初始对准模块、捷联惯导解算模块、改进滤波算法参数计算模块、正常模式组合导航滤波模块、故障模式组合导航滤波模块和反馈校正输出模块组成；

所述一次装订模块用于根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

所述地球参数解算模块用于根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数；

所述初始对准模块用于根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

所述捷联惯导解算模块用于根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算，所述的捷联惯导解算模块进行导航解算过程中，采用旋转矢量四元数算法解算载体的姿态角，采用双子样速度算法解算载体的速度，采用速度积分算法解算载体的当前位置；

所述改进滤波算法参数计算模块用于根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

所述正常模式组合导航滤波模块包括状态预测子模块、协方差预测子模块、增益计算子模块、状态估计子模块和协方差估计子模块组成；

所述故障模式组合导航滤波模块用于根据***提供的错误状态标志，通过改进卡尔曼滤波计算的状态预测功能，通过后端处理方式排除前端出现的错误；

所述反馈校正输出模块用于根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出。

2.根据权利要求1所述的一种捷联惯导***/全球导航卫星***组合导航滤波***，其特征在于：所述的地球参数包括地球自转角速度投影分量、导航坐标系相对地球的旋转角速度和载体所在位置的重力加速度信息。

3.一种捷联惯导***/全球导航卫星***组合导航滤波方法，其特征在于：它包括以下步骤：

S1：一次装订模块根据本地的先验温度补偿参数、陀螺仪和加速度计的***误差预标定结果，将其作为初始对准模块以及捷联惯导解算模块的固有参数进行装订；

S2：地球参数解算模块根据GNSS全球导航卫星***及SINS捷联惯导***解算的位置、速度信息计算地球参数；

S3：初始对准模块根据一次装订模块和GNSS全球导航卫星***提供的位置、速度、姿态信息解算得到SINS捷联惯导***的初始时间、位置、速度和姿态角信息，并作为捷联惯导解算模块的初始值；

S4：捷联惯导解算模块根据捷联惯导设备提供的角速度和比力信息，并利用一次装订模块和初始对准模块提供的参数进行导航解算；

S5：改进滤波算法参数计算模块根据SINS捷联惯导***和GNSS全球导航卫星***提供的数据信息，对改进卡尔曼滤波算法所需的状态转移矩阵、***驱动噪声协方差矩阵、量测噪声协方差矩阵进行计算，并根据参数计算结果及状态标志选择性地传递至正常模式组合导航滤波模块或故障模式组合导航滤波模块中进行改进卡尔曼滤波计算；

S6：反馈校正输出模块根据正常模式组合导航滤波模块或故障模式组合导航滤波模块估计得到的姿态误差、速度误差和位置误差分别对捷联惯导解算模块解算的姿态角、速度以及位置进行一次修正，并将修正后的结果作为组合导航***的输出。