CN110426032A - 一种解析式冗余的飞行器容错导航估计方法 - Google Patents

Abstract

本发明公开了一种解析式冗余的飞行器容错导航估计方法。首先，利用旋翼飞行器的动力学模型，对飞行器的加速度、角加速度信息进行估计；其次，构建角加速度计/动力学模型/IMU故障检测滤波器和IMU/磁传感器/GPS/气压计故障检测滤波器；然后，根据两个故障检测滤波器的输出，构建故障定位策略，实现对导航传感器的故障定位；最后，根据故障检测结果，隔离故障传感器，完成导航信息解算。本发明通过引入动力学模型和角加速度计，实现对角加速度计、动力学模型、IMU、量测传感器的故障检测，同时可以用动力学模型完全代替故障IMU进行导航解算，提高了导航系统的容错性能。

Description

一种解析式冗余的飞行器容错导航估计方法

技术领域

本发明属于导航技术领域，特别涉及了一种飞行器容错导航估计方法。

背景技术

机载导航传感器是飞行器进行导航解算的数据来源，解算出的导航信息是飞行器进行稳定飞行的基础。目前常用的机载导航传感器包括惯性传感器——IMU和量测传感器——磁传感器、GPS、气压计等。但在一些特殊的环境中，机载导航传感器性能下降甚至失效，例如声波干扰会破坏IMU的性能，城市遮挡会导致GPS信号丢失等。如果使用故障的传感器参与导航解算，会导致飞行器失控，甚至坠毁。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了一种解析式冗余的飞行器容错导航估计方法，提高飞行器导航系统的容错性能。

为了实现上述技术目的，本发明的技术方案为：

一种解析式冗余的飞行器容错导航估计方法，包括如下步骤：

步骤一：周期读取k时刻载体的旋翼转速量测系统、角加速度计、IMU、磁传感器、GPS和气压计的输出，通过旋翼飞行器的动力学模型，估计旋翼飞行器的运动加速度信息和角加速度信息；

步骤二：利用步骤一中的机载传感器数据输出，构建角加速度计/动力学模型/IMU故障检测滤波器S1和IMU/磁传感器/GPS/气压计故障检测滤波器S2；S1中包括三个并行的子检测器，分别为角加速度计/动力学模型子检测器D1、角加速度计/IMU子检测器D2和动力学模型/IMU子检测器D3；S2中包括三个并行的子检测器，分别为IMU/磁传感器子检测器D4、IMU/GPS子检测器D5和IMU/气压计子检测器D6；

步骤三：根据S1中各个子检测器的故障检测结果，构建故障定位策略，实现对角加速度计、动力学模型和IMU的故障定位，输出定位结果；根据S1的故障定位结果与S2中各个子检测器的故障检测结果，构建全局故障定位策略，定位故障传感器；

步骤四：根据步骤三得到的故障定位结果，制定故障隔离策略，从状态滤波估计中隔离故障传感器，完成导航状态估计；

当机载传感器无故障或动力学模型故障时，使用角加速度计、IMU的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当IMU故障时，使用角加速度计和动力学模型的气动力模型作为状态方程输入，磁传感器、GPS、气压计作为量测传感器；

当角加速度计故障时，使用动力学模型的气动力矩模型和IMU中的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当GPS或气压计或磁传感器故障时，使用角加速度计、IMU的加速度计作为状态方程输入，隔离故障传感器，使之不参与全局融合滤波；

步骤五：根据步骤四得到的状态估计结果，对k时刻的S1和S2中的状态量进行校正。

进一步地，在步骤一中，通过下式估计旋翼飞行器的运动加速度信息和角加速度信息：

f_mz＝k_T0+k_T1(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

其中，f_mx、f_my、f_mz为通过动力学模型计算得到的机体系x、y、z轴的加速度；为通过动力学模型计算得到的机体系x、y、z轴的角加速度，ω_mz为z轴的角速度；k_Hx0、k_Hx1、k_Hx2、k_Hx3、k_Hy0、k_Hy1、k_Hy2、k_Hy3、k_T0、k_T1、k_x0、k_x1、k_x2、k_x3、k_y0、k_y1、k_y2、k_y3、k_z0、k_z1、k_z2、k_z3为旋翼飞行器动力学模型参数，均为常值；为机体系相对于导航系的线速度在机体系x、y轴上的分量；β_x、β_y为机体系x、y轴的角加速度，通过角加速度计获得；ω_i为第i个旋翼的转速，i＝1,2,3,4，为第i个旋翼转速的角加速度；f_ax、f_ay为机体系x、y轴的加速度，通过IMU中的加速度计获得。

进一步地，在步骤二中，构建角加速度计/动力学模型子检测器D1的方法如下：

1、利用角加速度计输出和动力学模型输出，计算z轴角加速度残差：

上式中，r₁(k)为角加速度计和动力学模型计算得到的z轴角加速度残差；β_z(k)为k时刻机体系z轴角加速度，通过z轴角加速度计输出获得；为通过动力学模型计算得到的机体系z轴的角加速度；abs(*)表示取绝对值；

2、判断子检测器D1是否发生故障：

上式中，τ₁为故障判断阈值；T₁为故障检测结果；如果T₁＝1，则角加速度计或动力学模型故障；如果T₁＝0，则角加速度计和动力学模型无故障；

构建角加速度计/IMU子检测器D2的方法如下：

1、利用角加速度计输出计算x、y、z轴角速度：

上式中，ω_x(k)、ω_y(k)、ω_z(k)为k时刻的机体系x、y、z轴角速度，通过角加速度计的输出β_x、β_y、β_z积分获得；ω_x(k-1)、ω_y(k-1)、ω_z(k-1)为k-1时刻的机体系x、y、z轴角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ΔT为离散采样时间；

2、计算子检测器D2的统计参数：

r₂(k)＝[ω_gx(k) ω_gy(k) ω_gz(k)]^T-[ω_x(k) ω_y(k) ω_z(k)]^T

A₂(k)＝H₂(k)P₂(k|k-1)H₂(k)+R₂(k)

λ₂＝r₂(k)A₂(k)^-1r₂(k)

P₂(k|k-1)＝F₂(k)P₂(k-1)F₂ ^T(k)+G₂(k-1)Q₂(k-1)G₂ ^T(k-1)

上式中，r₂(k)为k时刻IMU和角加速度计输出计算得到的x、y、z轴角速度残差；ω_gx(k)、ω_gy(k)、ω_gz(k)为机体系x、y、z轴角速度，通过陀螺输出获得；A₂(k)为k时刻IMU和角加速度计输出计算得到的残差方差；H₂(k)＝[I_3×3]为k时刻量测系数矩阵，I_3×3为3×3的单位矩阵；P₂(k|k-1)为k时刻一步预测均方误差矩阵；R₂(k)＝diag([ε_ωgx(k) ε_ωgy(k) ε_ωgz(k)]²)为k时刻量测噪声矩阵，ε_ωgx(k)、ε_ωgy(k)、ε_ωgz(k)为机体系x、y、z轴陀螺噪声；λ₂(k)为k时刻IMU和角加速度计输出计算得到的统计参数；F₂(k)＝[I_3×3]为k时刻IMU和角加速度计检测系统的状态系数矩阵；P₂(k-1)为k-1时刻的均方误差矩阵；G₂(k-1)＝[ΔT*I_3×3]为k-1时刻IMU和角加速度计检测系统的状态噪声系数矩阵；Q₂(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1)]²)为k-1时刻IMU和角加速度计检测系统的状态噪声，ε_βx(k-1)、ε_βy(k-1)、ε_βz(k-1)为k-1时刻的x、y、z轴角加速度计噪声；上标T表示转置；

3、判断子检测器D2是否发生故障：

上式中，τ₂为故障判断阈值；T₂为故障检测结果；如果T₂＝1，则角加速度计或IMU故障；如果T₃＝0，则角加速度计和IMU无故障；

构建动力学模型/IMU子检测器D3的方法如下：

1、利用动力学模型输出计算z轴角速度：

上式中，ω_mz(k-1)为k-1时刻的角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ω_mz(k)为k时刻的角速度；为k时刻的角加速度，通过动力学模型获得；ΔT为离散采样时间；

2、计算子检测器D3的残差和统计参数：

r_3a(k)＝abs(f_az(k)-f_mz(k))

r_3b(k)＝ω_gz(k)-ω_mz(k)

A_3b(k)＝H_3b(k)P_3b(k|k-1)H_3b(k)+R_3b(k)

λ_3b＝r_3b(k)A_3b(k)^-1r_3b(k)

P_3b(k|k-1)＝F_3b(k)P_3b(k-1)F_3b ^T(k)+G_3b(k-1)Q_3b(k-1)G_3b ^T(k-1)

上式中，r_3a(k)为k时刻IMU和动力学模型计算得到的z轴加速度残差；f_az(k)为k时刻机体系z轴的加速度，通过加速度计获得；r_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差；A_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差方差；H_3b(k)＝1为k时刻量测系数矩阵；P_3b(k|k-1)为k时刻一步预测均方误差矩阵；R_3b(k)＝diag([ε_ωgz(k)]²)为k时刻量测噪声矩阵；λ_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度统计参数；为k时刻z轴陀螺和动力学模型检测系统的状态系数矩阵；P_3b(k-1)为k-1时刻的均方误差矩阵；G_3b(k-1)＝1为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声系数矩阵；Q_3b(k-1)＝diag([ε_ωmz(k-1)]²)为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声，ε_ωmz(k-1)为k-1时刻通过动力学模型获得的机体系z轴角速度噪声；

3、判断子检测器D3是否发生故障：

上式中，τ_3a、τ_3b为故障判断阈值；T_3a、T_3b为故障检测结果；如果T_3a(k)＝1或T_3b(k)＝1，IMU或动力学模型故障；如果T_3a(k)＝0和T_3b(k)＝0，IMU和动力学模型无故障。

进一步地，在步骤二中，计算k时刻子检测器D4、D5、D6的状态预测：

q₀(k)＝q₀(k-1)-0.5ω_gx(k)q₁(k-1)-0.5ω_gy(k)q₂(k-1)-0.5ω_gz(k)q₃(k-1)

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

上式中，q₀(k)、q₁(k)、q₂(k)、q₃(k)为通过状态方程获得的k时刻四元数；q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻四元数，通过k-1时刻联邦卡尔曼滤波器输出获得；为通过状态方程获得的k时刻机体系相对于导航系的线速度在机体系z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系z轴上的分量，通过k-1时刻联邦卡尔曼滤波器输出获得；g为重力加速度；P_N(k)、P_E(k)、P_D(k)为通过状态方程获得的k时刻北向、东向、地向位置；P_N(k-1)、P_E(k-1)、P_D(k-1)为k-1时刻北向、东向、地向位置，通过k-1时刻联邦卡尔曼滤波器输出获得；

构建IMU/磁传感器子检测器D4：

1、计算子检测器D4的统计参数：

使用k时刻的状态预测计算航向：

A₄(k)＝H₄(k)P₄(k|k-1)H₄(k)+R₄(k)

λ₄(k)＝r₄(k)A₄(k)^-1r₄(k)

P₄(k|k-1)＝F₄(k)P₄(k-1)F₄ ^T(k)+G₄(k-1)Q₄(k-1)G₄ ^T(k-1)

Q₄(k-1)＝diag([ε_ωgx(k-1) ε_ωgy(k-1) ε_ωgz(k-1) ε_vbx(k-1) ε_vby(k-1) ε_vbz(k-1)ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)

上式中，为通过状态预测计算得到的k时刻航向角；r₄(k)为k时刻D4检测器的航向角残差；ψ_m(k)为k时刻磁传感器输出的航向；A₄(k)为k时刻D4检测器的残差方差；H₄(k)为k时刻D4检测器的量测系数矩阵；P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₄(k)＝diag([ε_ψm(k)]²)为k时刻的噪声矩阵，ε_ψm(k)为k时刻磁传感器的高度噪声；λ₄(k)为k时刻的统计参数；0_i×j为i×j的零矩阵；I_i×j为i×j的单位矩阵；F₄(k)为k时刻IMU和磁传感器检测器的状态系数矩阵；P₄(k-1)为k-1时刻的均方误差矩阵；G₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声系数矩阵；Q₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声，ε_vbx(k-1)、ε_vby(k-1)、ε_vbz(k-1)为k时刻的x、y、z轴线速度噪声；ε_pn(k-1)、ε_pe(k-1)、ε_pd(k-1)为k-1时刻的北向、东向、地向位置噪声；

3、判断子检测器D4是否发生故障

上式中，τ₄为故障判断阈值；T₄为故障检测结果；如果T₄＝1，IMU或磁传感器故障；如果T₄＝0，IMU和磁传感器无故障；

构建IMU/GPS子检测器D5：

1、计算子检测器D5的统计参数：

使用k时刻的状态预测计算东向速度：

使用k时刻的状态预测计算北向速度：

A₅(k)＝H₅(k)P₅(k|k-1)H₅(k)+R₅(k)

λ₅(k)＝r₅(k)A₅(k)^-1r₅(k)

H₅(k)＝[Υ_2×4 Ω_2×3 0_2×3]

上式中，为通过状态预测计算得到的k时刻东向速度预测、北向速度；r₅(k)为k时刻IMU和GPS检测系统的北向速度、东向速度残差；V_NG(k)、V_EG(k)为k时刻GPS输出的北向速度、东向速度；A₅(k)为k时刻IMU和GPS检测系统的残差方差；H₅(k)为k时刻IMU和GPS检测系统的量测系数矩阵；P₅(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；为k时刻的噪声矩阵，为k时刻的GPS北向、东向速度噪声；λ₅(k)为k时刻的统计参数；

3、判断检测器D5是否发生故障：

上式中，τ₅为故障判断阈值；T₅为故障检测结果；如果T₅＝1，IMU或GPS故障；如果T₅＝0，IMU和GPS无故障；

构建IMU/气压计子检测器D6：

1、计算检测器D6的统计参数：

使用k时刻的状态预测计算高度：

A₆(k)＝H₆(k)P₆(k|k-1)H₆(k)+R₆(k)

λ₆(k)＝r₆(k)A₆(k)^-1r₆(k)

上式中，为通过状态预测计算得到的k时刻高度；r₆(k)为k时刻IMU和气压计检测系统的高度残差；h_b(k)为k时刻气压计输出的高度；A₆(k)为k时刻IMU和气压计检测系统的残差方差；H₆(k)＝-1为k时刻IMU和气压计检测系统的量测系数矩阵；P₆(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₆(k)＝diag([ε_hb(k)]²)为k时刻的噪声矩阵，ε_hb(k)为k时刻的气压计的高度噪声；λ₆(k)为k时刻的统计参数；

3、判断子检测器D6是否发生故障：

上式中，τ₆为故障判断阈值；T₆为故障检测结果；如果T₆＝1，IMU或气压计故障；如果T₆＝0，IMU和气压计无故障。

进一步地，在步骤三中，首先根据S1构建故障定位策略：

然后根据S1和S2构建全局的故障定位策略：

上式中，F_GACC、F_DM、F_IMU、F_mag、F_gps、F_baro分别为角加速度计、动力学模型、IMU、磁传感器、GPS、气压计的故障定位函数，故障定位函数等于“1”，表示对应的传感器发生故障，故障定位函数等于“0”，表示对应的传感器未发生故障；“||”表示逻辑运算符“或”；“&”表示逻辑运算符“与”；“-”表示逻辑运算符“非”。

进一步地，在步骤四中，将故障情况分为以下7种情况：

情况1：机载传感器无故障：

(1)计算k时刻的状态和均方误差预测值：

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

P(k|k-1)＝F(k)P(k-1)F^T(k)+G(k)Q(k)G^T(k)

Q(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1) ε_ωx(k-1) ε_ωy(k-1) ε_ωz(k-1) ε_vbx(k-1) ε_vby(k-1) ε_vbz(k-1) ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)

上式中，为k时刻机体系相对于导航系的角速度在机体系x、y、z轴上的分量；P(k|k-1)为k-1时刻到k时刻的一步预测均方误差；P(k-1)为k-1时刻的均方误差；F(k)为k时刻的状态系数矩阵；G(k-1)为k-1时刻的噪声系数矩阵；Q(k-1)为k-1时刻的噪声系数矩阵；ε_ωx(k-1)、ε_ωy(k-1)、ε_ωz(k-1)为k-1时刻的x、y、z轴角速度噪声；

(2)计算k时刻第i个子滤波器的扩展卡尔曼滤波器的增益K_Ci：

K_Ci(k)＝P(k|k-1)H_Ci(k)^T[H_Ci(k)P(k|k-1)H_Ci(k)^T+R_Ci(k)]^-1

上式中，为k时刻的GPS地向速度噪声；为k时刻的GPS地向速度噪声；为k时刻的GPS东向位置、北向位置噪声；

(3)计算k时刻第i个子滤波器的扩展卡尔曼滤波器状态估计值X_Ci(k)及均方误差P_Ci(k)：

X_Ci(k)＝X(k|k-1)+K_Ci(k)[Y_Ci(k)-h_Ci(X_Ci(k|k-1))]

P_Ci(k)＝[I-K_Ci(k)H_Ci(k)]P(k|k-1)

Y_Ci(k)为第i个子滤波器量测量：

上式中，V_DG(k)、P_NG(k)、P_EG(k)为k时刻GPS地向速度、北向位置、东向位置；h_baro(k)为k时刻气压计高度；

(4)计算k时刻子滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)：

X_g(k)＝P_g(k)[P_C1(k)^-1X_C1(k)+P_C2(k)^-1X_C2(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)]^-1

P_g(k)＝[P_C1(k)^-1+P_C2(k)^-1+P_C3(k)^-1+P_C4(k)^-1]^-1

X(k)＝X_g(k)

P(k)＝4P_g(k)

上式中，X(k)、P(k)为k时刻各子滤波器的状态重置及均方误差重置；

情况2：IMU故障时，IMU被隔离，使用IMU中加速度计作为输入的速度微分方程修改如下：

使用IMU中的陀螺作为量测信息的子滤波器被切断，不参与融合滤波；根据传感器隔离情况在情况1的基础上修改状态估计相关矩阵；

情况3：动力学模型故障时，滤波更新过程与情况1相同；

情况4：角加速度计故障时，角加速度计被隔离，使用角加速度计作为输入的角速度微分方程修改如下：

量测更新过程与情况1相同；

情况5：磁传感器故障时，磁传感器被隔离，使用冗余的航向信息作为量测量；量测更新过程中，将情况1中的步骤(3)中的Y_C2(k)＝ψ_m(k)，修改为使用冗余航向传感器输出的数据；

情况6：GPS故障时，GPS被隔离，使用冗余的速度位置信息作为量测量；量测更新过程中，将情况1中步骤(3)中的Y_C3(k)＝[V_NG(k) V_EG(k) V_DG(k) P_NG(k) P_EG(k)]^T，修改为使用冗余速度量测传感器输出的数据；

情况7：气压计故障时，气压计被隔离，使用冗余的高度信息作为量测量；量测更新过程中，将情况1中步骤(3)中的Y_C4(k)＝h_baro(k)，修改为使用冗余高度量测传感器输出的数据。

采用上述技术方案带来的有益效果：

本发明通过引入动力学模型和角加速度计，可以实现对角加速度计、动力学模型、IMU、量测传感器的故障检测，同时可以使用动力学模型完全代替故障IMU进行导航解算，提高导航系统的容错性能。并且在IMU故障时，可以使用角加速度计输出作为动力学模型输入，提高大姿态角变化下的动力学模型估计精度。

附图说明

图1为本发明的整体容错框图；

图2为本发明中故障诊断框图；

图3为本发明中滤波框图；

图4为本发明的流程图；

图5为本发明在IMU故障时的故障检测结果图；

图6为本发明在IMU故障时的x轴角速度估计结果图；

图7为本发明在IMU故障时的y轴角速度估计结果图；

图8为本发明在IMU故障时的z轴角速度估计结果图；

图9为本发明在IMU故障时的横滚角估计结果图；

图10为本发明在IMU故障时的俯仰角估计结果图；

图11为本发明在IMU故障时的航向角估计结果图；

图12为本发明在IMU故障时的x轴线速度估计结果图；

图13为本发明在IMU故障时的y轴线速度估计结果图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明设计了一种解析式冗余的飞行器容错导航估计方法，如图1所示，包括如下步骤：

步骤一：周期读取k时刻载体的旋翼转速量测系统、角加速度计、IMU、磁传感器、GPS和气压计的输出，通过旋翼飞行器的动力学模型，估计旋翼飞行器的运动加速度信息和角加速度信息；

步骤二：利用步骤一中的机载传感器数据输出，构建角加速度计/动力学模型/IMU故障检测滤波器S1和IMU/磁传感器/GPS/气压计故障检测滤波器S2；S1中包括三个并行的子检测器，分别为角加速度计/动力学模型子检测器D1、角加速度计/IMU子检测器D2和动力学模型/IMU子检测器D3；S2中包括三个并行的子检测器，分别为IMU/磁传感器子检测器D4、IMU/GPS子检测器D5和IMU/气压计子检测器D6；

步骤三：根据S1中各个子检测器的故障检测结果，构建故障定位策略，实现对角加速度计、动力学模型和IMU的故障定位，输出定位结果；根据S1的故障定位结果与S2中各个子检测器的故障检测结果，构建全局故障定位策略，定位故障传感器；

步骤四：根据步骤三得到的故障定位结果，制定故障隔离策略，从状态滤波估计中隔离故障传感器，完成导航状态估计；

当机载传感器无故障或动力学模型故障时，使用角加速度计、IMU的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当IMU故障时，使用角加速度计和动力学模型的气动力模型作为状态方程输入，磁传感器、GPS、气压计作为量测传感器；

当角加速度计故障时，使用动力学模型的气动力矩模型和IMU中的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当GPS或气压计或磁传感器故障时，使用角加速度计、IMU的加速度计作为状态方程输入，隔离故障传感器，使之不参与全局融合滤波；

步骤五：根据步骤四得到的状态估计结果，对k时刻的S1和S2中的状态量进行校正。

在本实施例中，步骤一采用如下优选方案实现：

在步骤一中，通过下式估计旋翼飞行器的运动加速度信息和角加速度信息：

f_mz＝k_T0+k_T1(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

其中，f_mx、f_my、f_mz为通过动力学模型计算得到的机体系x、y、z轴的加速度；为通过动力学模型计算得到的机体系x、y、z轴的角加速度，ω_mz为z轴的角速度；k_Hx0、k_Hx1、k_Hx2、k_Hx3、k_Hy0、k_Hy1、k_Hy2、k_Hy3、k_T0、k_T1、k_x0、k_x1、k_x2、k_x3、k_y0、k_y1、k_y2、k_y3、k_z0、k_z1、k_z2、k_z3为旋翼飞行器动力学模型参数，均为常值；为机体系相对于导航系的线速度在机体系x、y轴上的分量；β_x、β_y为机体系x、y轴的角加速度，通过角加速度计获得；ω_i为第i个旋翼的转速，i＝1,2,3,4，为第i个旋翼转速的角加速度；f_ax、f_ay为机体系x、y轴的加速度，通过IMU中的加速度计获得。

在本实施例中，步骤二采用如下优选方案实现：

在步骤二中，构建角加速度计/动力学模型子检测器D1的方法如下：

1、利用角加速度计输出和动力学模型输出，计算z轴角加速度残差：

上式中，r₁(k)为角加速度计和动力学模型计算得到的z轴角加速度残差；β_z(k)为k时刻机体系z轴角加速度，通过z轴角加速度计输出获得；为通过动力学模型计算得到的机体系z轴的角加速度；abs(*)表示取绝对值；

2、判断子检测器D1是否发生故障：

上式中，τ₁为故障判断阈值；T₁为故障检测结果；如果T₁＝1，则角加速度计或动力学模型故障；如果T₁＝0，则角加速度计和动力学模型无故障；

构建角加速度计/IMU子检测器D2的方法如下：

1、利用角加速度计输出计算x、y、z轴角速度：

上式中，ω_x(k)、ω_y(k)、ω_z(k)为k时刻的机体系x、y、z轴角速度，通过角加速度计的输出β_x、β_y、β_z积分获得；ω_x(k-1)、ω_y(k-1)、ω_z(k-1)为k-1时刻的机体系x、y、z轴角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ΔT为离散采样时间；

2、计算子检测器D2的统计参数：

r₂(k)＝[ω_gx(k) ω_gy(k) ω_gz(k)]^T-[ω_x(k) ω_y(k) ω_z(k)]^T

A₂(k)＝H₂(k)P₂(k|k-1)H₂(k)+R₂(k)

λ₂＝r₂(k)A₂(k)^-1r₂(k)

P₂(k|k-1)＝F₂(k)P₂(k-1)F₂ ^T(k)+G₂(k-1)Q₂(k-1)G₂ ^T(k-1)

上式中，r₂(k)为k时刻IMU和角加速度计输出计算得到的x、y、z轴角速度残差；ω_gx(k)、ω_gy(k)、ω_gz(k)为机体系x、y、z轴角速度，通过陀螺输出获得；A₂(k)为k时刻IMU和角加速度计输出计算得到的残差方差；H₂(k)＝[I_3×3]为k时刻量测系数矩阵，I_3×3为3×3的单位矩阵；P₂(k|k-1)为k时刻一步预测均方误差矩阵；R₂(k)＝diag([ε_ωgx(k) ε_ωgy(k) ε_ωgz(k)]²)为k时刻量测噪声矩阵，ε_ωgx(k)、ε_ωgy(k)、ε_ωgz(k)为机体系x、y、z轴陀螺噪声；λ₂(k)为k时刻IMU和角加速度计输出计算得到的统计参数；F₂(k)＝[I_3×3]为k时刻IMU和角加速度计检测系统的状态系数矩阵；P₂(k-1)为k-1时刻的均方误差矩阵；G₂(k-1)＝[ΔT*I_3×3]为k-1时刻IMU和角加速度计检测系统的状态噪声系数矩阵；Q₂(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1)]²)为k-1时刻IMU和角加速度计检测系统的状态噪声，ε_βx(k-1)、ε_βy(k-1)、ε_βz(k-1)为k-1时刻的x、y、z轴角加速度计噪声；上标T表示转置；

3、判断子检测器D2是否发生故障：

上式中，τ₂为故障判断阈值；T₂为故障检测结果；如果T₂＝1，则角加速度计或IMU故障；如果T₃＝0，则角加速度计和IMU无故障；

构建动力学模型/IMU子检测器D3的方法如下：

1、利用动力学模型输出计算z轴角速度：

上式中，ω_mz(k-1)为k-1时刻的角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ω_mz(k)为k时刻的角速度；为k时刻的角加速度，通过动力学模型获得；ΔT为离散采样时间；

2、计算子检测器D3的残差和统计参数：

r_3a(k)＝abs(f_az(k)-f_mz(k))

r_3b(k)＝ω_gz(k)-ω_mz(k)

A_3b(k)＝H_3b(k)P_3b(k|k-1)H_3b(k)+R_3b(k)

λ_3b＝r_3b(k)A_3b(k)^-1r_3b(k)

P_3b(k|k-1)＝F_3b(k)P_3b(k-1)F_3b ^T(k)+G_3b(k-1)Q_3b(k-1)G_3b ^T(k-1)

上式中，r_3a(k)为k时刻IMU和动力学模型计算得到的z轴加速度残差；f_az(k)为k时刻机体系z轴的加速度，通过加速度计获得；r_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差；A_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差方差；H_3b(k)＝1为k时刻量测系数矩阵；P_3b(k|k-1)为k时刻一步预测均方误差矩阵；R_3b(k)＝diag([ε_ωgz(k)]²)为k时刻量测噪声矩阵；λ_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度统计参数；为k时刻z轴陀螺和动力学模型检测系统的状态系数矩阵；P_3b(k-1)为k-1时刻的均方误差矩阵；G_3b(k-1)＝1为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声系数矩阵；Q_3b(k-1)＝diag([ε_ωmz(k-1)]²)为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声，ε_ωmz(k-1)为k-1时刻通过动力学模型获得的机体系z轴角速度噪声；

3、判断子检测器D3是否发生故障：

上式中，τ_3a、τ_3b为故障判断阈值；T_3a、T_3b为故障检测结果；如果T_3a(k)＝1或T_3b(k)＝1，IMU或动力学模型故障；如果T_3a(k)＝0和T_3b(k)＝0，IMU和动力学模型无故障。

在步骤二中，计算k时刻子检测器D4、D5、D6的状态预测：

q₀(k)＝q₀(k-1)-0.5ω_gx(k)q₁(k-1)-0.5ω_gy(k)q₂(k-1)-0.5ω_gz(k)q₃(k-1)

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

上式中，q₀(k)、q₁(k)、q₂(k)、q₃(k)为通过状态方程获得的k时刻四元数；q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻四元数，通过k-1时刻联邦卡尔曼滤波器输出获得；为通过状态方程获得的k时刻机体系相对于导航系的线速度在机体系z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系z轴上的分量，通过k-1时刻联邦卡尔曼滤波器输出获得；g为重力加速度；P_N(k)、P_E(k)、P_D(k)为通过状态方程获得的k时刻北向、东向、地向位置；P_N(k-1)、P_E(k-1)、P_D(k-1)为k-1时刻北向、东向、地向位置，通过k-1时刻联邦卡尔曼滤波器输出获得；

构建IMU/磁传感器子检测器D4：

1、计算子检测器D4的统计参数：

使用k时刻的状态预测计算航向：

A₄(k)＝H₄(k)P₄(k|k-1)H₄(k)+R₄(k)

λ₄(k)＝r₄(k)A₄(k)^-1r₄(k)

P₄(k|k-1)＝F₄(k)P₄(k-1)F₄ ^T(k)+G₄(k-1)Q₄(k-1)G₄ ^T(k-1)

Q₄(k-1)＝diag([ε_ωgx(k-1) ε_ωgy(k-1) ε_ωgz(k-1) ε_vbx(k-1)ε_vby(k-1) ε_vbz(k-1)ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)

上式中，为通过状态预测计算得到的k时刻航向角；r₄(k)为k时刻D4检测器的航向角残差；ψ_m(k)为k时刻磁传感器输出的航向；A₄(k)为k时刻D4检测器的残差方差；H₄(k)为k时刻D4检测器的量测系数矩阵；P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₄(k)＝diag([ε_ψm(k)]²)为k时刻的噪声矩阵，ε_ψm(k)为k时刻磁传感器的高度噪声；λ₄(k)为k时刻的统计参数；0_i×j为i×j的零矩阵；I_i×j为i×j的单位矩阵；F₄(k)为k时刻IMU和磁传感器检测器的状态系数矩阵；P₄(k-1)为k-1时刻的均方误差矩阵；G₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声系数矩阵；Q₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声，ε_vbx(k-1)、ε_vby(k-1)、ε_vbz(k-1)为k时刻的x、y、z轴线速度噪声；ε_pn(k-1)、ε_pe(k-1)、ε_pd(k-1)为k-1时刻的北向、东向、地向位置噪声；

4、判断子检测器D4是否发生故障

上式中，τ₄为故障判断阈值；T₄为故障检测结果；如果T₄＝1，IMU或磁传感器故障；如果T₄＝0，IMU和磁传感器无故障；

构建IMU/GPS子检测器D5：

1、计算子检测器D5的统计参数：

使用k时刻的状态预测计算东向速度：

使用k时刻的状态预测计算北向速度：

A₅(k)＝H₅(k)P₅(k|k-1)H₅(k)+R₅(k)

λ₅(k)＝r₅(k)A₅(k)^-1r₅(k)

H₅(k)＝[Υ_2×4 Ω_2×3 0_2×3]

上式中，为通过状态预测计算得到的k时刻东向速度预测、北向速度；r₅(k)为k时刻IMU和GPS检测系统的北向速度、东向速度残差；V_NG(k)、V_EG(k)为k时刻GPS输出的北向速度、东向速度；A₅(k)为k时刻IMU和GPS检测系统的残差方差；H₅(k)为k时刻IMU和GPS检测系统的量测系数矩阵；P₅(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；为k时刻的噪声矩阵，为k时刻的GPS北向、东向速度噪声；λ₅(k)为k时刻的统计参数；

4、判断检测器D5是否发生故障：

上式中，τ₅为故障判断阈值；T₅为故障检测结果；如果T₅＝1，IMU或GPS故障；如果T₅＝0，IMU和GPS无故障；

构建IMU/气压计子检测器D6：

1、计算检测器D6的统计参数：

使用k时刻的状态预测计算高度：

A₆(k)＝H₆(k)P₆(k|k-1)H₆(k)+R₆(k)

λ₆(k)＝r₆(k)A₆(k)^-1r₆(k)

上式中，为通过状态预测计算得到的k时刻高度；r₆(k)为k时刻IMU和气压计检测系统的高度残差；h_b(k)为k时刻气压计输出的高度；A₆(k)为k时刻IMU和气压计检测系统的残差方差；H₆(k)＝-1为k时刻IMU和气压计检测系统的量测系数矩阵；P₆(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₆(k)＝diag([ε_hb(k)]²)为k时刻的噪声矩阵，ε_hb(k)为k时刻的气压计的高度噪声；λ₆(k)为k时刻的统计参数；

4、判断子检测器D6是否发生故障：

上式中，τ₆为故障判断阈值；T₆为故障检测结果；如果T₆＝1，IMU或气压计故障；如果T₆＝0，IMU和气压计无故障。

在本实施例中，步骤三采用如下优选方案实现：

在步骤三中，首先根据S1构建故障定位策略：

然后根据S1和S2构建全局的故障定位策略：

上式中，F_GACC、F_DM、F_IMU、F_mag、F_gps、F_baro分别为角加速度计、动力学模型、IMU、磁传感器、GPS、气压计的故障定位函数，故障定位函数等于“1”，表示对应的传感器发生故障，故障定位函数等于“0”，表示对应的传感器未发生故障；“||”表示逻辑运算符“或”；“&”表示逻辑运算符“与”；“-”表示逻辑运算符“非”。

上述故障诊断过程如图2所示。

在本实施例中，步骤四采用如下优选方案实现：

在步骤四中，将故障情况分为以下7种情况：

情况1：机载传感器无故障：

(1)计算k时刻的状态和均方误差预测值：

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

P(k|k-1)＝F(k)P(k-1)F^T(k)+G(k)Q(k)G^T(k)

Q(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1) ε_ωx(k-1) ε_ωy(k-1) ε_ωz(k-1) ε_vbx(k-1) ε_vby(k-1) ε_vbz(k-1) ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)上式中，为k时刻机体系相对于导航系的角速度在机体系x、y、z轴上的分量；P(k|k-1)为k-1时刻到k时刻的一步预测均方误差；P(k-1)为k-1时刻的均方误差；F(k)为k时刻的状态系数矩阵；G(k-1)为k-1时刻的噪声系数矩阵；Q(k-1)为k-1时刻的噪声系数矩阵；ε_ωx(k-1)、ε_ωy(k-1)、ε_ωz(k-1)为k-1时刻的x、y、z轴角速度噪声；

(2)计算k时刻第i个子滤波器的扩展卡尔曼滤波器的增益K_Ci：

K_Ci(k)＝P(k|k-1)H_Ci(k)^T[H_Ci(k)P(k|k-1)H_Ci(k)^T+R_Ci(k)]^-1

上式中，为k时刻的GPS地向速度噪声；为k时刻的GPS地向速度噪声；为k时刻的GPS东向位置、北向位置噪声；

(3)计算k时刻第i个子滤波器的扩展卡尔曼滤波器状态估计值X_Ci(k)及均方误差P_Ci(k)：

X_Ci(k)＝X(k|k-1)+K_Ci(k)[Y_Ci(k)-h_Ci(X_Ci(k|k-1))]

P_Ci(k)＝[I-K_Ci(k)H_Ci(k)]P(k|k-1)

Y_Ci(k)为第i个子滤波器量测量：

上式中，V_DG(k)、P_NG(k)、P_EG(k)为k时刻GPS地向速度、北向位置、东向位置；h_baro(k)为k时刻气压计高度；

(4)计算k时刻子滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)：

X_g(k)＝P_g(k)[P_C1(k)^-1X_C1(k)+P_C2(k)^-1X_C2(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)]^-1

P_g(k)＝[P_C1(k)^-1+P_C2(k)^-1+P_C3(k)^-1+P_C4(k)^-1]^-1

X(k)＝X_g(k)

P(k)＝4P_g(k)

上式中，X(k)、P(k)为k时刻各子滤波器的状态重置及均方误差重置；

情况2：IMU故障时，IMU被隔离，使用IMU加速度计作为输入的速度微分方程修改如下：

使用IMU中的陀螺作为量测信息的子滤波器被切断，不参与融合滤波；根据传感器隔离情况在情况1的基础上修改状态估计相关矩阵；

情况3：动力学模型故障时，滤波更新过程与情况1相同；

情况4：角加速度计故障时，角加速度计被隔离，使用角加速度计作为输入的角速度微分方程修改如下：

量测更新过程与情况1相同；

情况5：磁传感器故障时，磁传感器被隔离，使用冗余的航向信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中的步骤(3)中的Y_C2(k)＝ψ_m(k)，修改为使用冗余航向传感器输出的数据；

情况6：GPS故障时，GPS被隔离，使用冗余的速度位置信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中步骤(3)中的Y_C3(k)＝[V_NG(k) V_EG(k) V_DG(k) P_NG(k) P_EG(k)]^T，修改为使用冗余速度量测传感器输出的数据；

情况7：气压计故障时，气压计被隔离，使用冗余的高度信息作为量测量；量测更新过程根据量测方程在情况1的基础上进行修改，将情况1中步骤(3)中的Y_C4(k)＝h_baro(k)，修改为使用冗余高度量测传感器输出的数据。

上述滤波过程如图3所示。

在本实施例中，步骤五采用如下优选方案实现：

在步骤五中，使用步骤四中状态估计结果，对角加速度计/IMU子检测器D2的角速度、动力学模型/IMU子检测器D3的角速度以及IMU/磁传感器/GPS/气压计故障检测滤波器S2的四元数、线速度、高度进行校正更新。

上述整个过程如图4所示。

下文通过具体仿真实例来说明本发明的效果：

采用实际的飞行数据进行半物理仿真验证的方式，对使用本发明后的容错估计性能进行验证。对载体在的故障检测性能进行验证，采集飞行器在不同机动运动下的数据：悬停、横滚运动、沿着俯仰轴的侧飞运动、悬停、俯仰运动、沿着俯仰轴的侧飞运动、慢速航向轴的转动、快速航向轴的转动。由于所使用的无人机缺少角加速度计传感器，所以通过使用陀螺数据微分进行模拟角加速度计的输出，在不同的传感器数据中注入常值仿真故障进行故障检测验证，本实施例仅给出IMU故障时的故障检测及状态估计结果，其余传感器故障结果类似。

图5为在IMU数据中注入零偏故障时，采用本发明方法后的故障检测结果。由图中可以看出，检测系统能够及时准确的检测出IMU发生故障。

图6为在IMU数据中注入零偏故障时，采用本发明方法后的x轴角速度估计结果。由图中可以看出，角速度估计误差小于10度/秒。

图7为在IMU数据中注入零偏故障时，采用本发明方法后的y轴角速度估计结果。由图中可以看出，角速度估计误差小于10度/秒。

图8为在IMU数据中注入零偏故障时，采用本发明方法后的z轴角速度估计结果。由图中可以看出，角速度估计误差小于2度/秒。

图9为在IMU数据中注入零偏故障时，采用本发明方法后的横滚角估计结果。由图中可以看出，横滚角估计误差小于5度。

图10为在IMU数据中注入零偏故障时，采用本发明方法后的俯仰角估计结果。由图中可以看出，俯仰角估计误差小于5度。

图11为在IMU数据中注入零偏故障时，采用本发明方法后的航向角估计结果。由图中可以看出，航向角估计误差小于1度。

图12为在IMU数据中注入零偏故障时，采用本发明方法后的x轴线速度估计结果。由图中可以看出，x轴线速度估计误差小于0.2m/s。

图13为在IMU数据中注入零偏故障时，采用本发明方法后的y轴线速度估计结果。由图中可以看出，y轴线速度估计误差小于0.2m/s。

Claims (6)

1.一种解析式冗余的飞行器容错导航估计方法，其特征在于，包括如下步骤：

步骤一：周期读取k时刻载体的旋翼转速量测系统、角加速度计、IMU、磁传感器、GPS和气压计的输出，通过旋翼飞行器的动力学模型，估计旋翼飞行器的运动加速度信息和角加速度信息；

步骤二：利用步骤一中的机载传感器数据输出，构建角加速度计/动力学模型/IMU故障检测滤波器S1和IMU/磁传感器/GPS/气压计故障检测滤波器S2；S1中包括三个并行的子检测器，分别为角加速度计/动力学模型子检测器D1、角加速度计/IMU子检测器D2和动力学模型/IMU子检测器D3；S2中包括三个并行的子检测器，分别为IMU/磁传感器子检测器D4、IMU/GPS子检测器D5和IMU/气压计子检测器D6；

步骤三：根据S1中各个子检测器的故障检测结果，构建故障定位策略，实现对角加速度计、动力学模型和IMU的故障定位，输出定位结果；根据S1的故障定位结果与S2中各个子检测器的故障检测结果，构建全局故障定位策略，定位故障传感器；

步骤四：根据步骤三得到的故障定位结果，制定故障隔离策略，从状态滤波估计中隔离故障传感器，完成导航状态估计；

当机载传感器无故障或动力学模型故障时，使用角加速度计、IMU的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当IMU故障时，使用角加速度计和动力学模型的气动力模型作为状态方程输入，磁传感器、GPS、气压计作为量测传感器；

当角加速度计故障时，使用动力学模型的气动力矩模型和IMU中的加速度计作为状态方程输入，IMU中的陀螺、磁传感器、GPS、气压计作为量测传感器；

当GPS或气压计或磁传感器故障时，使用角加速度计、IMU的加速度计作为状态方程输入，隔离故障传感器，使之不参与全局融合滤波；

步骤五：根据步骤四得到的状态估计结果，对k时刻的S1和S2中的状态量进行校正。

2.根据权利要求1所述解析式冗余的飞行器容错导航估计方法，其特征在于，在步骤一中，通过下式估计旋翼飞行器的运动加速度信息和角加速度信息：

f_mz＝k_T0+k_T1(ω₁ ²+ω₂ ²+ω₃ ²+ω₄ ²)

其中，f_mx、f_my、f_mz为通过动力学模型计算得到的机体系x、y、z轴的加速度；为通过动力学模型计算得到的机体系x、y、z轴的角加速度，ω_mz为z轴的角速度；k_Hx0、k_Hx1、k_Hx2、k_Hx3、k_Hy0、k_Hy1、k_Hy2、k_Hy3、k_T0、k_T1、k_x0、k_x1、k_x2、k_x3、k_y0、k_y1、k_y2、k_y3、k_z0、k_z1、k_z2、k_z3为旋翼飞行器动力学模型参数，均为常值；为机体系相对于导航系的线速度在机体系x、y轴上的分量；β_x、β_y为机体系x、y轴的角加速度，通过角加速度计获得；ω_i为第i个旋翼的转速，i＝1,2,3,4，为第i个旋翼转速的角加速度；f_ax、f_ay为机体系x、y轴的加速度，通过IMU中的加速度计获得。

3.根据权利要求1所述解析式冗余的飞行器容错导航估计方法，其特征在于，在步骤二中，构建角加速度计/动力学模型子检测器D1的方法如下：

1、利用角加速度计输出和动力学模型输出，计算z轴角加速度残差：

上式中，r₁(k)为角加速度计和动力学模型计算得到的z轴角加速度残差；β_z(k)为k时刻机体系z轴角加速度，通过z轴角加速度计输出获得；为通过动力学模型计算得到的机体系z轴的角加速度；abs(*)表示取绝对值；

2、判断子检测器D1是否发生故障：

上式中，τ₁为故障判断阈值；T₁为故障检测结果；如果T₁＝1，则角加速度计或动力学模型故障；如果T₁＝0，则角加速度计和动力学模型无故障；

构建角加速度计/IMU子检测器D2的方法如下：

1、利用角加速度计输出计算x、y、z轴角速度：

上式中，ω_x(k)、ω_y(k)、ω_z(k)为k时刻的机体系x、y、z轴角速度，通过角加速度计的输出β_x、β_y、β_z积分获得；ω_x(k-1)、ω_y(k-1)、ω_z(k-1)为k-1时刻的机体系x、y、z轴角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ΔT为离散采样时间；

2、计算子检测器D2的统计参数：

r₂(k)＝[ω_gx(k) ω_gy(k) ω_gz(k)]^T-[ω_x(k) ω_y(k) ω_z(k)]^T

A₂(k)＝H₂(k)P₂(k|k-1)H₂(k)+R₂(k)

λ₂＝r₂(k)A₂(k)^-1r₂(k)

P₂(k|k-1)＝F₂(k)P₂(k-1)F₂ ^T(k)+G₂(k-1)Q₂(k-1)G₂ ^T(k-1)

上式中，r₂(k)为k时刻IMU和角加速度计输出计算得到的x、y、z轴角速度残差；ω_gx(k)、ω_gy(k)、ω_gz(k)为机体系x、y、z轴角速度，通过陀螺输出获得；A₂(k)为k时刻IMU和角加速度计输出计算得到的残差方差；H₂(k)＝[I_3×3]为k时刻量测系数矩阵，I_3×3为3×3的单位矩阵；P₂(k|k-1)为k时刻一步预测均方误差矩阵；R₂(k)＝diag([ε_ωgx(k) ε_ωgy(k) ε_ωgz(k)]²)为k时刻量测噪声矩阵，ε_ωgx(k)、ε_ωgy(k)、ε_ωgz(k)为机体系x、y、z轴陀螺噪声；λ₂(k)为k时刻IMU和角加速度计输出计算得到的统计参数；F₂(k)＝[I_3×3]为k时刻IMU和角加速度计检测系统的状态系数矩阵；P₂(k-1)为k-1时刻的均方误差矩阵；G₂(k-1)＝[ΔT*I_3×3]为k-1时刻IMU和角加速度计检测系统的状态噪声系数矩阵；Q₂(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1)]²)为k-1时刻IMU和角加速度计检测系统的状态噪声，ε_βx(k-1)、ε_βy(k-1)、ε_βz(k-1)为k-1时刻的x、y、z轴角加速度计噪声；上标T表示转置；

3、判断子检测器D2是否发生故障：

上式中，τ₂为故障判断阈值；T₂为故障检测结果；如果T₂＝1，则角加速度计或IMU故障；如果T₃＝0，则角加速度计和IMU无故障；

构建动力学模型/IMU子检测器D3的方法如下：

1、利用动力学模型输出计算z轴角速度：

上式中，ω_mz(k-1)为k-1时刻的角速度，通过k-1时刻联邦卡尔曼滤波器输出获得；ω_mz(k)为k时刻的角速度；为k时刻的角加速度，通过动力学模型获得；ΔT为离散采样时间；

2、计算子检测器D3的残差和统计参数：

r_3a(k)＝abs(f_az(k)-f_mz(k))

r_3b(k)＝ω_gz(k)-ω_mz(k)

A_3b(k)＝H_3b(k)P_3b(k|k-1)H_3b(k)+R_3b(k)

λ_3b＝r_3b(k)A_3b(k)^-1r_3b(k)

P_3b(k|k-1)＝F_3b(k)P_3b(k-1)F_3b ^T(k)+G_3b(k-1)Q_3b(k-1)G_3b ^T(k-1)

上式中，r_3a(k)为k时刻IMU和动力学模型计算得到的z轴加速度残差；f_az(k)为k时刻机体系z轴的加速度，通过加速度计获得；r_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差；A_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度残差方差；H_3b(k)＝1为k时刻量测系数矩阵；P_3b(k|k-1)为k时刻一步预测均方误差矩阵；R_3b(k)＝diag([ε_ωgz(k)]²)为k时刻量测噪声矩阵；λ_3b(k)为k时刻IMU和动力学模型计算得到的z轴角速度统计参数；为k时刻z轴陀螺和动力学模型检测系统的状态系数矩阵；P_3b(k-1)为k-1时刻的均方误差矩阵；G_3b(k-1)＝1为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声系数矩阵；Q_3b(k-1)＝diag([ε_ωmz(k-1)]²)为k-1时刻z轴陀螺和动力学模型检测系统的状态噪声，ε_ωmz(k-1)为k-1时刻通过动力学模型获得的机体系z轴角速度噪声；

3、判断子检测器D3是否发生故障：

上式中，τ_3a、τ_3b为故障判断阈值；T_3a、T_3b为故障检测结果；如果T_3a(k)＝1或T_3b(k)＝1，IMU或动力学模型故障；如果T_3a(k)＝0和T_3b(k)＝0，IMU和动力学模型无故障。

4.根据权利要求3所述解析式冗余的飞行器容错导航估计方法，其特征在于，在步骤二中，计算k时刻子检测器D4、D5、D6的状态预测：

q₀(k)＝q₀(k-1)-0.5ω_gx(k)q₁(k-1)-0.5ω_gy(k)q₂(k-1)-0.5ω_gz(k)q₃(k-1)

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

上式中，q₀(k)、q₁(k)、q₂(k)、q₃(k)为通过状态方程获得的k时刻四元数；q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻四元数，通过k-1时刻联邦卡尔曼滤波器输出获得；为通过状态方程获得的k时刻机体系相对于导航系的线速度在机体系z轴上的分量；为k-1时刻机体系相对于导航系的线速度在机体系z轴上的分量，通过k-1时刻联邦卡尔曼滤波器输出获得；g为重力加速度；P_N(k)、P_E(k)、P_D(k)为通过状态方程获得的k时刻北向、东向、地向位置；P_N(k-1)、P_E(k-1)、P_D(k-1)为k-1时刻北向、东向、地向位置，通过k-1时刻联邦卡尔曼滤波器输出获得；

构建IMU/磁传感器子检测器D4：

1、计算子检测器D4的统计参数：

使用k时刻的状态预测计算航向：

A₄(k)＝H₄(k)P₄(k|k-1)H₄(k)+R₄(k)

λ₄(k)＝r₄(k)A₄(k)^-1r₄(k)

P₄(k|k-1)＝F₄(k)P₄(k-1)F₄ ^T(k)+G₄(k-1)Q₄(k-1)G₄ ^T(k-1)

Q₄(k-1)＝diag([ε_ωgx(k-1) ε_ωgy(k-1) ε_ωgz(k-1) ε_vbx(k-1) ε_vby(k-1) ε_vbz(k-1) ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)

上式中，为通过状态预测计算得到的k时刻航向角；r₄(k)为k时刻D4检测器的航向角残差；ψ_m(k)为k时刻磁传感器输出的航向；A₄(k)为k时刻D4检测器的残差方差；H₄(k)为k时刻D4检测器的量测系数矩阵；P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₄(k)＝diag([ε_ψm(k)]²)为k时刻的噪声矩阵，ε_ψm(k)为k时刻磁传感器的高度噪声；λ₄(k)为k时刻的统计参数；0_i×j为i×j的零矩阵；I_i×j为i×j的单位矩阵；F₄(k)为k时刻IMU和磁传感器检测器的状态系数矩阵；P₄(k-1)为k-1时刻的均方误差矩阵；G₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声系数矩阵；Q₄(k-1)为k-1时刻IMU和磁传感器检测器的状态噪声，ε_vbx(k-1)、ε_vby(k-1)、ε_vbz(k-1)为k时刻的x、y、z轴线速度噪声；ε_pn(k-1)、ε_pe(k-1)、ε_pd(k-1)为k-1时刻的北向、东向、地向位置噪声；

2、判断子检测器D4是否发生故障

上式中，τ₄为故障判断阈值；T₄为故障检测结果；如果T₄＝1，IMU或磁传感器故障；如果T₄＝0，IMU和磁传感器无故障；

构建IMU/GPS子检测器D5：

1、计算子检测器D5的统计参数：

使用k时刻的状态预测计算东向速度：

使用k时刻的状态预测计算北向速度：

A₅(k)＝H₅(k)P₅(k|k-1)H₅(k)+R₅(k)

λ₅(k)＝r₅(k)A₅(k)^-1r₅(k)

H₅(k)＝[γ_2×4 Ω_2×3 0_2×3]

上式中，为通过状态预测计算得到的k时刻东向速度预测、北向速度；r₅(k)为k时刻IMU和GPS检测系统的北向速度、东向速度残差；V_NG(k)、V_EG(k)为k时刻GPS输出的北向速度、东向速度；A₅(k)为k时刻IMU和GPS检测系统的残差方差；H₅(k)为k时刻IMU和GPS检测系统的量测系数矩阵；P₅(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；为k时刻的噪声矩阵，为k时刻的GPS北向、东向速度噪声；λ₅(k)为k时刻的统计参数；

2、判断检测器D5是否发生故障：

上式中，τ₅为故障判断阈值；T₅为故障检测结果；如果T₅＝1，IMU或GPS故障；如果T₅＝0，IMU和GPS无故障；

构建IMU/气压计子检测器D6：

1、计算检测器D6的统计参数：

使用k时刻的状态预测计算高度：

A₆(k)＝H₆(k)P₆(k|k-1)H₆(k)+R₆(k)

λ₆(k)＝r₆(k)A₆(k)^-1r₆(k)

上式中，为通过状态预测计算得到的k时刻高度；r₆(k)为k时刻IMU和气压计检测系统的高度残差；h_b(k)为k时刻气压计输出的高度；A₆(k)为k时刻IMU和气压计检测系统的残差方差；H₆(k)＝-1为k时刻IMU和气压计检测系统的量测系数矩阵；P₆(k|k-1)＝P₄(k|k-1)为k-1时刻到k时刻的一步预测均方误差；R₆(k)＝diag([ε_hb(k)]²)为k时刻的噪声矩阵，ε_hb(k)为k时刻的气压计的高度噪声；λ₆(k)为k时刻的统计参数；

2、判断子检测器D6是否发生故障：

上式中，τ₆为故障判断阈值；T₆为故障检测结果；如果T₆＝1，IMU或气压计故障；如果T₆＝0，IMU和气压计无故障。

5.根据权利要求4所述解析式冗余的飞行器容错导航估计方法，其特征在于，在步骤三中，首先根据S1构建故障定位策略：

然后根据S1和S2构建全局的故障定位策略：

上式中，F_GACC、F_DM、F_IMU、F_mag、F_gps、F_baro分别为角加速度计、动力学模型、IMU、磁传感器、GPS、气压计的故障定位函数，故障定位函数等于“1”，表示对应的传感器发生故障，故障定位函数等于“0”，表示对应的传感器未发生故障；“||”表示逻辑运算符“或”；“&”表示逻辑运算符“与”；“-”表示逻辑运算符“非”。

6.根据权利要求4所述解析式冗余的飞行器容错导航估计方法，其特征在于，在步骤四中，将故障情况分为以下7种情况：

情况1：机载传感器无故障：

(1)计算k时刻的状态和均方误差预测值：

q₁(k)＝0.5ω_gx(k)q₀(k-1)+q₁(k-1)+0.5ω_gz(k)q₂(k-1)-0.5ω_gy(k)q₃(k-1)

q₂(k)＝0.5ω_gy(k)q₀(k-1)-0.5ω_gz(k)q₁(k-1)+q₂(k-1)+0.5ω_gx(k)q₃(k-1)

q₃(k)＝0.5ω_gz(k)q₀(k-1)+0.5ω_gy(k)q₁(k-1)-0.5ω_gx(k)q₂(k-1)+q₃(k-1)

P(k|k-1)＝F(k)P(k-1)F^T(k)+G(k)Q(k)G^T(k)

Q(k-1)＝diag([ε_βx(k-1) ε_βy(k-1) ε_βz(k-1) ε_ωx(k-1) ε_ωy(k-1) ε_ωz(k-1) ε_vbx(k-1)ε_vby(k-1) ε_vbz(k-1) ε_pn(k-1) ε_pe(k-1) ε_pd(k-1)]²)

上式中，为k时刻机体系相对于导航系的角速度在机体系x、y、z轴上的分量；P(k|k-1)为k-1时刻到k时刻的一步预测均方误差；P(k-1)为k-1时刻的均方误差；F(k)为k时刻的状态系数矩阵；G(k-1)为k-1时刻的噪声系数矩阵；Q(k-1)为k-1时刻的噪声系数矩阵；ε_ωx(k-1)、ε_ωy(k-1)、ε_ωz(k-1)为k-1时刻的x、y、z轴角速度噪声；

(2)计算k时刻第i个子滤波器的扩展卡尔曼滤波器的增益K_Ci：

K_Ci(k)＝P(k|k-1)H_Ci(k)^T[H_Ci(k)P(k|k-1)H_Ci(k)^T+R_Ci(k)]^-1

上式中，为k时刻的GPS地向速度噪声；为k时刻的GPS地向速度噪声；为k时刻的GPS东向位置、北向位置噪声；

(3)计算k时刻第i个子滤波器的扩展卡尔曼滤波器状态估计值X_Ci(k)及均方误差P_Ci(k)：

X_Ci(k)＝X(k|k-1)+K_Ci(k)[Y_Ci(k)-h_Ci(X_Ci(k|k-1))]

P_Ci(k)＝[I-K_Ci(k)H_Ci(k)]P(k|k-1)

Y_Ci(k)为第i个子滤波器量测量：

上式中，V_DG(k)、P_NG(k)、P_EG(k)为k时刻GPS地向速度、北向位置、东向位置；h_baro(k)为k时刻气压计高度；

(4)计算k时刻子滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)：

X_g(k)＝P_g(k)[P_C1(k)^-1X_C1(k)+P_C2(k)^-1X_C2(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)]^-1

P_g(k)＝[P_C1(k)^-1+P_C2(k)^-1+P_C3(k)^-1+P_C4(k)^-1]^-1

X(k)＝X_g(k)

P(k)＝4P_g(k)

上式中，X(k)、P(k)为k时刻各子滤波器的状态重置及均方误差重置；

情况2：IMU故障时，IMU被隔离，使用IMU中加速度计作为输入的速度微分方程修改如下：

使用IMU中的陀螺作为量测信息的子滤波器被切断，不参与融合滤波；根据传感器隔离情况在情况1的基础上修改状态估计相关矩阵；

情况3：动力学模型故障时，滤波更新过程与情况1相同；

情况4：角加速度计故障时，角加速度计被隔离，使用角加速度计作为输入的角速度微分方程修改如下：

量测更新过程与情况1相同；

情况5：磁传感器故障时，磁传感器被隔离，使用冗余的航向信息作为量测量；量测更新过程中，将情况1中的步骤(3)中的Y_C2(k)＝ψ_m(k)，修改为使用冗余航向传感器输出的数据；

情况6：GPS故障时，GPS被隔离，使用冗余的速度位置信息作为量测量；量测更新过程中，将情况1中步骤(3)中的Y_C3(k)＝[V_NG(k) V_EG(k) V_DG(k) P_NG(k) P_EG(k)]^T，修改为使用冗余速度量测传感器输出的数据；

情况7：气压计故障时，气压计被隔离，使用冗余的高度信息作为量测量；量测更新过程中，将情况1中步骤(3)中的Y_C4(k)＝h_baro(k)，修改为使用冗余高度量测传感器输出的数据。