CN103278837B - 基于自适应滤波的sins/gnss多级容错组合导航方法 - Google Patents

Abstract

本发明公开了一种基于自适应滤波的SINS/GNSS多级容错组合导航方法，步骤如下：一、建立SINS/GNSS组合导航***数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS的各误差量并对其进行反馈校正；二、子***状态判断：根据SINS的陀螺采样值和加速度计采样值判断SINS子***的工作状态，根据GNSS输出量测值判断GNSS子***的工作状态；三、导航决策匹配：根据SINS子***和GNSS子***的工作状态进行导航决策匹配处理、输出导航信息，并循环执行步骤二、三。该方法在SINS异常、GNSS丢星失锁、噪声突变等动态情况下有效地提高组合***的容错性能、可靠性和导航精度，在地面车辆、飞机、导弹及舰船领域有广泛的应用前景。

Description

基于自适应滤波的SINS/GNSS多级容错组合导航方法

技术领域

本发明涉及一种基于自适应滤波的SINS/GNSS多级容错组合导航方法，可用于提高地面车辆、飞机、导弹或者舰船用SINS/GNSS组合导航***的容错性、可靠性及导航精度。

背景技术

捷联惯性导航***（SINS）是一种不依赖于任何外部信息、也不向外辐射能量的自主式导航***，具有体积小、数据更新率高、短时精度高及不受气候条件限制等优点，被广泛应用于航空航天及民用等领域，然而其误差随工作时间积累，即其精度具有时间相关性，不适于长时间导航；卫星导航定位***具有与时间无关、定位输出稳定等特点，但定位条件苛刻，极易受到遮挡等外部因素的干扰。因此常将两者结合以构成SINS/GNSS组合导航***。

在基于速度、位置组合的SINS/GNSS组合导航***中，通常方法是通过建立组合导航***的数学模型，以SINS的速度、位置、姿态及惯性测量器件的误差量作为状态量，以GNSS输出的位置、速度为观测量，通过卡尔曼滤波最优估计算法将SINS及GNSS子***进行信息融合，估计出SINS误差后进行反馈校正，提高导航定位精度。然而，对于以SINS为主、GNSS辅助定位的组合导航***而言，从本质上SINS及GNSS子***仍然是独立工作的，并且卡尔曼滤波是量测驱动的，在高动态及恶劣环境等情况下各子***出现的极短异常均会降低组合***的导航精度，使***稳定性变差；同时，***数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变差、精度下降，甚至导致滤波发散，同样极大地影响了组合***的导航精度。

发明内容

本发明的目的在于提供一种基于自适应滤波的SINS/GNSS多级容错组合导航方法，该方法在SINS异常、GNSS丢星失锁、噪声突变等动态情况下有效地提高组合***的容错性能、可靠性和导航精度。

本发明的技术解决方案为：一种基于自适应滤波的SINS/GNSS多级容错组合导航方法，包括下列步骤：

步骤1，建立SINS/GNSS组合导航***数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS的各误差量并对其进行反馈校正；

步骤2，子***状态判断：根据SINS的陀螺采样值和加速度计采样值判断SINS子***的工作状态，根据GNSS输出量测值判断GNSS子***的工作状态；

步骤3，导航决策匹配：根据SINS子***和GNSS子***的工作状态进行导航决策匹配处理、输出导航信息，并循环执行步骤2～3。

本发明与现有技术相比其显著效果是：（1）本发明采用的多级容错组合导航方法，对组合***SINS及GNSS子***工作状态进行判断，匹配了不同状态下的导航策略，安全可靠；（2）将自适应滤波作为信息融合算法，可在SINS、GNSS异常及噪声突变、高动态等情况下减小由SINS及GNSS工作异常带来的不利影响；（3）有效提高了地面车辆、飞机、导弹或者舰船用SINS/GNSS组合导航***的容错性、可靠性及导航精度。

附图说明

图1是本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法的原理框图。

图2是本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法的导航流程。

图3是实施例中组合导航***的地面跑车试验轨迹曲线图。

图4是实施例中组合导航***的地面跑车试验位置曲线图。

图5是实施例中组合导航***的飞行试验轨迹曲线图。

图6是实施例中组合导航***的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的速度对比曲线图。

图7是实施例中组合导航***的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的位置对比曲线图。

图8是实施例中组合导航***的飞行试验使用常规滤波CKF法与基于自适应滤波AKF的SINS/GNSS多级容错组合导航法的姿态对比曲线图。

具体实施方式

本发明通过建立组合导航***的数学模型，以SINS的速度、位置、姿态及惯性测量器件的误差量作为状态量，以GNSS输出的位置、速度为观测量，通过卡尔曼滤波最优估计算法将SINS及GNSS子***进行信息融合，估计出SINS误差后进行反馈校正，提高导航定位精度。

然而，对于以SINS为主、GNSS辅助定位的组合导航***而言，从本质上SINS及GNSS子***仍然是独立工作的，并且卡尔曼滤波是量测驱动的，在高动态及恶劣环境等情况下各子***出现的极短异常均会降低组合***的导航精度，使***稳定性变差；同时，***数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变差、精度下降，甚至导致滤波发散，同样极大地影响了组合***的导航精度。因此，需要根据SINS和GNSS子***工作状态的不同，采用不同的导航策略，同时采用自适应滤波算法，提高组合导航***的导航精度和***可靠性、容错性能。

因此，所述的一种基于自适应滤波的SINS/GNSS多级容错组合导航方法的基本原理是：首先，通过多级容错组合导航方法对SINS及GNSS子***的工作状态进行判断；然后，根据子***工作状态的不同分别匹配不同的组合导航策略。其中，当SINS及GNSS子***均正常工作时，建立以SINS/GNSS组合***误差为状态量、GNSS提供的速度位置信息为观测量的***状态方程，采用自适应卡尔曼滤波算法进行信息融合，估计出状态误差并进行反馈校正，提高了组合导航***的容错性能、可靠性及导航精度。

下面结合附图与具体实施例对本发明做进一步详细描述。

如图1、2所示，本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法，包括下列步骤：

步骤1，建立SINS/GNSS组合导航***数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS的各误差量并对其进行反馈校正；

（1.1）建立SINS误差方程，具体如下：

(a)***状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}$

其中X为***状态矢量，表示***状态矢量的导数，F为***状态转移矩阵，G为***噪声驱动矩阵，W为***噪声矢量，具体如下：

X＝[ψ_E ψ_N ψ_U δV_E δV_N δV_U δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

$F_{15\×15}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\×9}& F_M\end{array}\right],F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],F_M={\[0}_{6\×6}\],G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

其中ψ_E、ψ_N、ψ_U分别为东、北、天方向的平台失准角误差；δV_E、δV_N、δV_U分别为东、北、天方向的速度误差；δL、δλ、δh分别为纬度、经度及高度误差；ε_x、ε_y、ε_z分别为陀螺常值漂移在x、y、z轴上的分量；▽_x、▽_y、▽_z分别为加速度计常值偏置在x、y、z轴上的分量；F_N为对应9个基本导航参数的***阵；为姿态矩阵； 分别为陀螺噪声在x、y、z轴上的分量；分别为加表噪声在x、y、z轴上的分量；

(b)***量测方程：

Z＝HX+V

其中，Z为观测矢量，H为观测矩阵，V为观测噪声矩阵，具体如下:

Z＝[δL δλ δh δV_E δV_N δV_U]^T，H＝[H_P H_V]^T

H_P＝[0_3×6 diag[R_m R_n cosL 1]0_3×6]

H_V＝[0_3×3 I_3×3 0_3×9]

$V={\left[\begin{array}{cccccc}{V}_L& V_{\mathrm{\λ}}& V_h& V_{V_E}& V_{V_N}& V_{V_U}\end{array}\right]}^T$

其中R_m为椭球子午圈上各点的曲率半径，R_n为卯酉圈（它所在的平面与子午面垂直）上各点的曲率半径，L、λ、h分别表示纬度、经度及高度，V_E、V_N、V_U分别为东、北、天方向的速度，V_L、V_λ、V_h分别表示GNSS的纬度、经度、高度的观测误差，分别表示GNSS的东、北、天方向上的速度观测误差。

（1.2）利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS各误差量并对其进行反馈校正，具体为：

如图2所示，根据***状态方程和***量测方程，采用基于单步量测新息矢量r_k的自适应卡尔曼滤波信息融合算法，其算法公式编排具体如下：

①状态一步预测方程：

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

其中，为k时刻***状态一步预测值，为k-1时刻***状态估计值，Φ_k,k-1为k-1时刻到k时刻的***状态转移矩阵；

②一步预测均方误差方程:

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

$S_k=\frac{\mathrm{Trace}[r_k{r}_k^T-H_k{Q}_{k-1}{H}_k^T{-R}_k]}{\mathrm{Trace}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k,k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}$

其中，P_k,k-1为k-1时刻到k时刻的***状态协方差阵，P_k-1为k-1时刻的***状态协方差阵，S_k为自适应因子，Q_k-1为k-1时刻***噪声矩阵、Γ_k-1为k-1时刻***噪声驱动矩阵；

③卡尔曼滤波增益方程:

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

其中，K_k为k时刻***增益矩阵，H_k为k时刻***量测矩阵，R_k为k时刻***量测噪声矩阵；

④状态估计值更新方程:

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{r}_k$

$r_k=Z_k-H_k{\hat{X}}_{k,k-1}$

为k时刻***状态估计值，r_k为新息矢量，K_k为k时刻***增益矩阵，Z_k为k时刻量测向量；

⑤估计均方误差方程：

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

P_k为k时刻的***状态协方差阵，I是单位阵；

基于单步量测新息r_k的滤波发散判据为：

$r_k^T{r}_k>\mathrm{\λTrace}[H_k{P}_{k,k-1}{H}_k^T+R_k]$

其中λ为常值系数，取1～5，若该式成立，则滤波发散；反之不发散。在滤波发散情况下，组合导航***的各误差增大，降低了滤波精度。所以根据滤波发散判据，如果判断滤波发散，则通过求出自适应权重因子S_k以扩大P_k,k-1的作用，增大滤波增益，从而增强新量测值的修正作用，进而抑制滤波发散；如果判断滤波不发散，即S_k＝1，则可以直接使用常规的卡尔曼滤波算法进行修正。

最后，通过状态误差估计量对SINS/GNSS组合导航***中的速度误差、位置误差、姿态误差以及惯导内部的加速度零偏和陀螺漂移进行反馈校正。

步骤2，子***状态判断：根据SINS的陀螺采样值和加速度计采样值判断SINS子***的工作状态，根据GNSS输出量测值判断GNSS子***的工作状态；

子***状态判断的具体步骤为：

(2.1)根据SINS的陀螺仪采样值和加速度计采样值判断SINS子***的工作状态，设A_(axis)max为加速度阈值、ω_(axis)max为角速度阈值(通常根据惯性测量器件或载体的动态极限设定)，判断加速度计采样值A_axis和陀螺采样值ω_axis是否满足以下条件：

|A_axis|＜A_(axis)max

|ω_axis|＜ω_(axis)max

当满足上式时，则SINS子***的工作状态正常，否则SINS子***的工作状态异常；

(2.2)根据GNSS输出量测值判断GNSS子***的工作状态，先后进行外层判断和内层判断：

(a)外层判断为收星条件判断：设定n为最小收星数、dop为精度因子门限，判断收星数N_sats和精度因子xDOP是否满足以下条件：

N_sats≥n

xDOP≤dop

当满足上式时，继续内层判断，否则认为GNSS子***的工作状态异常；

(b)内层判断主要依靠SINS数据短时间内具有较高的稳定性和精度这一特性，内层判断对GNSS量测粗大误差进行判断，分为位置粗大误差判断、速度粗大误差判断以及GNSS误差带判断；设定下标GNSS、SINS分别代表GNSS接收机输出值和惯导输出值，下标1、0分别代表当前时刻值和前一时刻值，具体判断方法如下：

①位置粗大误差判断：

位置粗大误差d_err判断方法具体公式如下，其中，L、λ、H分别代表载体在地理系下的纬度、经度及高度，R_m为椭球子午圈上各点的曲率半径，R_n为卯酉圈（它所在的平面与子午面垂直）上各点的曲率半径：

d_err＝{[R_m((L_GNSS1-L_GNSS0)-(L_SINS1-L_SINS0))]²

+[R_ncos(L_SINS1)·((λ_GNSS1-λ_GNSS0)-(λ_SINS1-λ_SINS0))]²

+[(H_GNSS1-H_GNSS0)-(H_SINS1-H_SINS0)]²}^1/2

当d_err大于所设定的位置误差阀值（如100m）时，则GNSS定位信息受到干扰，位置数据异常；否则，GNSS位置数据正常；

②速度粗大误差判断：

速度粗大误差V_err判断方法具体公式如下，其中V表示速度数据，下标E、N、U代表东、北、天方向：

$V_{\mathrm{err}}=\left\{\right[(V_{\mathrm{GNSS}1}^N-V_{\mathrm{GNSS}0}^N)-(V_{\mathrm{SINS}1}^N-V_{\mathrm{SINS}0}^N){]}^2+[(V_{\mathrm{GNSS}1}^E-V_{\mathrm{GNSS}0}^E)-(V_{\mathrm{SINS}1}^E-V_{\mathrm{SINS}0}^E){]}^2$

$+[(V_{\mathrm{GNSS}1}^U-V_{\mathrm{GNSS}0}^U)-(V_{\mathrm{SINS}1}^U-V_{\mathrm{SINS}0}^U){]}^2{\}}^{1/2}$

当V_err大于设定的速度误差阈值（如10m/s）时，则认为GNSS测速信息受到干扰，速度数据异常；否则，GNSS速度数据正常；

③GNSS误差带判断：

采用下列公式判断当前时刻GNSS输出的位置数据是否处于SINS误差范围内，具体公式如下：

d_perr＝{[R_m(L_GNSS1-L_SINS1)]²+[R_ncos(L_SINS1)·(λ_GNSS1-λ_SINS1)]²+(H_GNSS1-H_SINS1)²}^1/2

设e_GNSS为GNSS位置误差阈值、e_SINS为SINS位置误差阈值（如均设定为50m）；若d_perr≤e_GNSS+e_SINS成立，GNSS数据处于SINS误差范围内，GNSS数据有效；否则，GNSS数据无效；

根据上述判断方法，当位置、速度数据均正常并且GNSS数据处于SINS误差范围内时，GNSS子***正常；否则，GNSS子***工作异常，数据存在突变。

步骤3，导航决策匹配：根据SINS子***和GNSS子***的工作状态进行导航决策匹配处理，并返回步骤2，根据GNSS、IMU工作状态的不同匹配相应导航策略，以增强***适应性和鲁棒性，具体如下：

(3.1)当SINS子***、GNSS子***工作状态均正常时，采用SINS/GNSS组合导航：将SINS和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正，其中滤波算法采用自适应滤波，滤波周期为1s；

(3.2)当SINS子***工作状态异常、GNSS子***工作状态正常时，放弃当前时刻SINS中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：

ω(k)_axis＝ω(k-1)_axis

A(k)_axis＝A(k-1)_axis

其中ω(k)_axis为k时刻的角速度，ω(k-1)_axis为k-1时刻的角速度，A(k)_axis为k时刻的加速度，A(k-1)_axis为k-1时刻的加速度；

(3.3)当SINS子***工作状态正常、GNSS子***工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并对导航输出进行修正。

（a）在捷联解算后，采用照常进行状态及均方误差的时间更新，省略量测更新的方法处理短时间丢星情况，方法如下：

${\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$

P_k＝P_k,k-1

其中X_k为k时刻的n维状态向量，X_k-1为k-1时刻的n维状态向量，为状态X_k的卡尔曼滤波估计值，为状态X_k-1的卡尔曼滤波估计值，为利用计算得到的对X_k的一步预测；Φ_k,k-1为k-1到k时刻的***一步转移矩阵（n×n阶），为Φ_k,k-1的转置矩阵；P_k为估值的均方误差阵，P_k-1为估值的均方误差阵，P_k,k-1为估值的均方误差阵；Γ_k-1为***噪声矩阵（n×r阶），它表征由k-1到k时刻的各个***噪声分别影响k时刻各个状态的程度，为Γ_k-1的转置矩阵；Q_k-1为k-1时刻的***噪声的方差矩阵。

(b)但是当量测缺失时间过长或者由此导致的状态估计均方误差过大时，应当否定滤波结果的有效性，甚至重置卡尔曼滤波器。

当GNSS从长时间异常恢复时，用GNSS给出的位置、速度作为当前时刻组合导航的位置、速度值；同时当采用的惯性器件稳定性较好时，可采用误差状态转移阵估计出导航误差并进行修正。首先，在丢星期间计算与滤波周期相应的Φ_k,k-1并连乘，得到丢星前一时刻t₀到当前时刻t₁状态转移矩阵下式所示：

${\mathrm{\Φ}}_{t_1,t_0}={\mathrm{\Φ}}_{t_1,t_1-T}\·\·\·{\mathrm{\Φ}}_{t_0+2T,t_0+T}{\mathrm{\Φ}}_{t_0+T,t_0}$

其中，T为滤波周期；然后由此及t₀时刻的误差状态量推算出当前t₁时刻的误差状态量，并修正导航输出，如下式所示:

$\left[\begin{array}{c}{\mathrm{\Ψ}}_{t_1}\\ {\mathrm{\δV}}_{t_1}\\ {\mathrm{\δP}}_{t_1}\\ \mathrm{\ϵ}\\ \▿\end{array}\right]={\mathrm{\Φ}}_{t_1,t_0}\left[\begin{array}{c}{\mathrm{\Ψ}}_{t_0}\\ {\mathrm{\δV}}_{t_0}\\ {\mathrm{\δP}}_{t_0}\\ \mathrm{\ϵ}\\ \▿\end{array}\right]$

其中为t₀到t₁时刻的***一步转移矩阵（n×n阶）， ε、▽分别为t₀时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏， ε、▽分别为t₁时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏。

(3.4)当SINS子***、GNSS子***工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计，对载体当前时刻运动状态进行估计，削弱***无法正常工作带来的不利影响。

实施例1

本发明基于自适应滤波的SINS/GNSS多级容错组合导航方法，已应用于SINS/GNSS组合导航***，并先后经过多次试验，主要包括地面跑车试验及飞行试验：

(1)地面跑车试验主要验证低动态下组合导航的位置重复性，轨迹曲线及位置曲线分别如图3～4所示，其位置均方差结果如表1所示:

表1位置均方差结果

从场地跑车试验结果看出，其位置重复性较好，表明该组合导航***在低动态下具有良好的稳定性和较高的导航定位精度。

(2)飞行试验进一步验证动态环境下组合导航的导航精度，图5为飞行轨迹曲线，图6～8分别为常规滤波(CKF)与采用本发明的容错导航方法及自适应滤波(AKF)时的速度、位置、姿态对比曲线。从试验结果看出，在飞行环境恶劣的情况下，CKF由于不具备发散在线判断、发散抑制措施以及缺乏噪声变化时的自适应能力等，其滤波效果和收敛性均较差，并且出现了滤波发散，导致滤波结果偏差过大，速度、姿态误差均超出指标，无法满足组合导航的要求；当使用AKF并采用多级容错组合导航方法时，效果较好，无论滤波收敛性、滤波精度还是稳定性均要优于CKF。

上述试验表明，本发明的一种基于自适应滤波的SINS/GNSS多级容错组合导航方法可以有效提高SINS/GNSS组合导航***的导航精度及容错性、可靠性。

Claims (4)

1.一种基于自适应滤波的SINS/GNSS多级容错组合导航方法，其特征在于，包括下列步骤：

步骤1，建立SINS/GNSS组合导航***数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS的各误差量并对其进行反馈校正；

步骤2，子***状态判断：根据SINS的陀螺采样值和加速度计采样值判断SINS子***的工作状态，根据GNSS输出量测值判断GNSS子***的工作状态；步骤2中所述子***状态判断的具体步骤为：

(2.1)根据SINS的陀螺仪采样值和加速度计采样值判断SINS子***的工作状态，设A_(axis)max为加速度阈值、ω_(axis)max为角速度阈值，角速度阈值根据惯性测量器件或载体的动态极限设定，判断加速度计采样值A_axis和陀螺采样值ω_axis是否满足以下条件：

|A_axis|＜A_(axis)max

|ω_axis|＜ω_(axis)max

当满足上式时，则SINS子***的工作状态正常，否则SINS子***的工作状态异常；

(2.2)根据GNSS输出量测值判断GNSS子***的工作状态，先后进行外层判断和内层判断：

(a)外层判断为收星条件判断：设定n为最小收星数、dop为精度因子门限，判断收星数N_sats和精度因子xDOP是否满足以下条件：

N_sats≥n

xDOP≤dop

当满足上式时，继续内层判断，否则认为GNSS子***的工作状态异常；

(b)内层判断对GNSS量测粗大误差进行判断，分为位置粗大误差判断、速度粗大误差判断以及GNSS误差带判断；设定下标GNSS、SINS分别代表GNSS接收机输出值和惯导输出值，下标1、0分别代表当前时刻值和前一时刻值，具体判断方法如下：

①位置粗大误差判断：

位置粗大误差d_err判断方法具体公式如下，其中，L、λ、H分别代表载体在地理系下的纬度、经度及高度，R_m为椭球子午圈上各点的曲率半径，R_n为卯酉圈上各点的曲率半径：

$\begin{array}{c}{d}_{\mathrm{err}}=\{{\left[R_m((L_{\mathrm{GNSS}1}-L_{\mathrm{GNSS}0})-(L_{\mathrm{SINS}1}-L_{\mathrm{SINS}0}))\right]}^2\\ +{[R_n\cos \left(L_{\mathrm{SINS}1}\right)\·(({\mathrm{\λ}}_{\mathrm{GNSS}1}-{\mathrm{\λ}}_{\mathrm{GNSS}0})-({\mathrm{\λ}}_{\mathrm{SINS}1}-{\mathrm{\λ}}_{\mathrm{SINS}0}))]}^2\\ +{{[(H_{\mathrm{GNSS}1}-H_{\mathrm{GHSS}0})-(H_{\mathrm{SINS}1}-H_{\mathrm{SINS}0})]}^2\}}^{1/2}\end{array}$

当d_err大于所设定的位置误差阀值时，则GNSS位置数据异常；否则，GNSS位置数据正常；

②速度粗大误差判断：

速度粗大误差V_err判断方法具体公式如下，其中V表示速度数据，下标E、N、U代表东、北、天方向：

$\begin{array}{c}{V}_{\mathrm{err}}=\{{[(V_{\mathrm{GNSS}1}^N-V_{\mathrm{GNSS}0}^N)-(V_{\mathrm{SINS}1}^N-V_{\mathrm{SINS}0}^N)]}^2+{[(V_{\mathrm{GNSS}1}^E-V_{\mathrm{GNSS}0}^E)-(V_{\mathrm{SINS}1}^E-V_{\mathrm{SINS}0}^E)]}^2\\ +{{[(V_{\mathrm{GNSS}1}^U-V_{\mathrm{GNSS}0}^U)-(V_{\mathrm{SINS}1}^U-V_{\mathrm{SNS}0}^U)]}^2\}}^{1/2}\end{array}$

当V_err大于设定的速度误差阈值时，GNSS速度数据异常；否则，GNSS速度数据正常；

③GNSS误差带判断：

采用下列公式判断当前时刻GNSS输出的位置数据是否处于SINS误差范围内，具体公式如下：

d_perr＝{[R_m(L_GNSS1-L_SINS1)]²+[R_ncos(L_SINS1)·(λ_GNSS1-λ_SINS1)]²+(H_GNSS1-H_SINS1)²}^1/2

设e_GNSS为GNSS位置误差阈值、e_SINS为SINS位置误差阈值，若d_perr≤e_GNSS+e_SINS成立，GNSS数据处于SINS误差范围内，GNSS数据有效；否则，GNSS数据无效；

根据上述判断方法，当位置、速度数据均正常并且GNSS数据处于SINS误差范围内时，GNSS子***正常；否则，GNSS子***工作异常；

步骤3，导航决策匹配：根据SINS子***和GNSS子***的工作状态进行导航决策匹配处理、输出导航信息，并循环执行步骤2～3。

2.根据权利要求1所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法，其特征在于，步骤1所述的建立SINS误差方程，具体如下：

(a)***状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}$

其中X为***状态矢量，表示***状态矢量的导数，F为***状态转移矩阵，G为***噪声驱动矩阵，W为***噪声矢量，具体如下：

X＝[ψ_E ψ_N ψ_U δV_E δV_N δV_U δL δλ δh ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

$F_{15\×15}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\×9}& F_M\end{array}\right],F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_n^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],F_W=\left[0_{6\×6}\right],G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

其中ψ_E、ψ_N、ψ_U分别为东、北、天方向的平台失准角误差；δV_E、δV_N、δV_U分别为东、北、天方向的速度误差；δL、δλ、δh分别为纬度、经度及高度误差；ε_x、ε_y、ε_z分别为陀螺常值漂移在x、y、z轴上的分量；▽_x、▽_y、▽_z分别为加速度计常值偏置在x、y、z轴上的分量；F_N为对应9个基本导航参数的***阵；为姿态矩阵； 分别为陀螺噪声在x、y、z轴上的分量；分别为加表噪声在x、y、z轴上的分量；

(b)***量测方程：

Z＝HX+V

其中，Z为观测矢量，H为观测矩阵，V为观测噪声矩阵，具体如下:

Z＝[δL δλ δh δV_E δV_N δV_U]^T，H＝[H_P H_V]^T

H_P＝[0_3×6 diag[R_m R_ncosL 1]0_3×6]

H_V＝[0_3×3 I_3×3 0_3×9]

$V={\left[\begin{array}{cccccc}{V}_L& V_{\mathrm{\λ}}& V_h& V_{V_E}& V_{V_N}& V_{V_U}\end{array}\right]}^T$

其中R_m为椭球子午圈上各点的曲率半径，R_n为卯酉圈上各点的曲率半径，卯酉圈所在的平面与子午面垂直，L、λ、h分别代表载体在地理系下的纬度、经度及高度，V_E、V_N、V_U分别为东、北、天方向的速度，V_L、V_λ、V_h分别表示GNSS的纬度、经度、高度的观测误差，分别表示GNSS的东、北、天方向上的速度观测误差。

3.根据权利要求1或2所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法，其特征在于，步骤1中所述利用自适应卡尔曼滤波方法对SINS子***和GNSS子***进行信息融合，估计SINS各误差量并对其进行反馈校正，具体为：

根据***状态方程和***量测方程，采用基于单步量测新息矢量r_k的自适应卡尔曼滤波信息融合算法，其算法公式编排具体如下：

①状态一步预测方程：

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

其中，为k时刻***状态一步预测值，为k-1时刻***状态估计值，Φ_k,k-1为k-1时刻到k时刻的***状态转移矩阵；

②一步预测均方误差方程:

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

$S_k=\frac{\mathrm{Trace}[r_k{r}_k^T-H_k{Q}_{k-1}{H}_k^T-R_k]}{\mathrm{Trace}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k,k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}$

其中，P_k,k-1为k-1时刻到k时刻的***状态协方差阵，P_k-1为k-1时刻的***状态协方差阵，S_k为自适应权重因子，Q_k-1为k-1时刻***噪声矩阵，Γ_k-1为k-1时刻***噪声驱动矩阵；

③卡尔曼滤波增益方程:

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

其中，K_k为k时刻***增益矩阵，H_k为k时刻***量测矩阵，R_k为k时刻***量测噪声矩阵；

④状态估计值更新方程:

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{r}_k$

$r_k=Z_k-H_k{\hat{X}}_{k,k-1}$

为k时刻***状态估计值，r_k为新息矢量，K_k为k时刻***增益矩阵，Z_k为k时刻量测向量；

⑤估计均方误差方程：

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

P_k为k时刻的***状态协方差阵，I是单位阵；

基于单步量测新息矢量r_k的滤波发散判据为：

$r_k^T{r}_k>\mathrm{\λTrace}[H_k{P}_{k,k-1}{H}_k^T+R_k]$

其中λ为常值系数，取1～5，若该式成立，则滤波发散；否则滤波不发散：

如果判断滤波发散，则通过求出自适应权重因子S_k以扩大P_k,k-1的作用，增大滤波增益，从而增强新量测值的修正作用，进而抑制滤波发散；如果判断滤波不发散，即S_k＝1，则可以直接使用常规的卡尔曼滤波算法进行修正；

最后，通过状态误差估计量对SINS的速度、位置、横滚角和俯仰角进行反馈校正。

4.根据权利要求1所述的基于自适应滤波的SINS/GNSS多级容错组合导航方法，其特征在于，步骤3中所述导航决策匹配的具体方法如下：

(3.1)当SINS子***、GNSS子***工作状态均正常时，采用SINS/GNSS组合导航：将SINS和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正；

(3.2)当SINS子***工作状态异常、GNSS子***工作状态正常时，放弃当前时刻SINS中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：

ω(k)_axis＝ω(k-1)_axis

A(k)_axis＝A(k-1)_axis

其中ω(k)_axis为k时刻的角速度，ω(k-1)_axis为k-1时刻的角速度，A(k)_axis为k时刻的加速度，A(k-1)_axis为k-1时刻的加速度；

(3.3)当SINS子***工作状态正常、GNSS子***工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并对导航输出进行修正；

(3.4)当SINS子***、GNSS子***工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计。