CN109612459B - 基于动力学模型的四旋翼飞行器惯性传感器容错导航方法 - Google Patents

Abstract

本发明公开了一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，包括以下步骤：步骤1：周期获取四旋翼飞行器机载传感器信息；步骤2：计算加速度、角加速度；步骤3：预测角速度、四元数、速度和位置；步骤4：判断各检测滤波器的故障结果，并对故障进行定位；步骤5：隔离故障传感器信息，通过联邦卡尔曼滤波器对角速度、四元数、速度、位置进行校正；步骤6：对各检测滤波器进行重置更新。本发明提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的优点在于：当惯性传感器故障时，通过动力学模型代替惯性传感器进行导航解算，解决惯性传感器失效时的导航问题，提高了四旋翼飞行器的导航可靠性。

Description

基于动力学模型的四旋翼飞行器惯性传感器容错导航方法

技术领域

本发明涉及容错导航技术领域，尤其涉及基于动力学模型的四旋翼飞行器惯性传感器容错导航方法。

背景技术

四旋翼飞行器具有体积小、结构简单、可悬停和垂直起降等优点，特别适合在近地面环境(如室内、城区和丛林等)中执行监视、侦察等任务，具有广阔的军事和民用前景。导航系统为四旋翼飞行器提供其飞行控制系统所必须的导航信息，是其完成各种复杂飞行任务的必要保障。

目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航系统)、磁传感器、气压高度计，其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。此时，会导致导航系统精度下降，影响飞行安全。目前，尚未有针对惯性传感器的容错导航方案。

发明内容

本发明所要解决的技术问题在于提供一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法。

本发明是通过以下技术方案解决上述技术问题的：

通过对四旋翼飞行器的气动力和气动力矩分析，建立四旋翼飞行器的气动力模型，包括X轴阻力模型、Y轴阻力模型、升力模型，气动力矩模型，包括横滚力矩模型、俯仰力矩模型、扭矩模型，形成惯性传感器的冗余信息，将动力学模型、惯性传感器、量测传感器，三者两两组合，形成一种“互表决式”的故障检测系统，突破惯性传感器无故障的假设。该检测系统中采用的故障检测方法为卡方检测，但卡方检测对于量测传感器故障敏感，对状态输入量故障不敏感，为了实现对状态输入量故障的快速故障检测，提出使用n步预测卡方检测，在该检测方法中，状态预测在n步之内进行累积，不使用量测进行校准，为了避免检测器发散，在第n步时使用联邦滤波器的状态估计结果对检测器进行状态重置更新。在状态输入量发生故障时，在第n步的状态重置时，导致状态输入量故障的重新累积，为了避免累积过程中，检测结果的不断切换和误检测，引入故障隔离周期概念。当状态输入量检测到故障后，故障隔离周期开始计输，在故障隔离周期内认为系统一直存在故障。在隔离周期内再次检测到故障时，隔离周期重置。联邦卡尔曼滤波器根据故障定位结果，隔离故障传感器信息，使用无故障传感器进行状态融合估计。该方案通过故障检测系统及相应的故障隔离和系统重构措施，解决了惯性传感器失效时的导航问题。

本发明提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的优点在于：可以在不假设惯性传感器的无故障条件下，实现对惯性传感器、量测传感器、动力学模型的故障检测，并且当惯性传感器故障时，可以使用动力学模型代替故障的惯性传感器，完成惯性传感器故障情况下四旋翼飞行器角速度、四元数、速度、位置的解算，提高了四旋翼飞行器的导航可靠性。

附图说明

图1是本发明的实施例所提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的流程图；

图2是本发明的实施例所提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的系统结构框图；

图3是本发明的实施例所提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的滤波框图；

图4是本发明的实施例所提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的故障检测框图；

图5是本发明的实施例所提供的基于动力学模型的四旋翼飞行器惯性传感器容错导航方法的四旋翼飞行器轨迹图；

图6是本发明的实施例所提供的在四旋翼飞行器的X、Y、Z轴陀螺中注入故障后的故障检测结果；

图7是本发明的实施例所提供的在四旋翼飞行器的X、Y、Z轴陀螺中注入故障后的，X、Y、Z轴角速度、横滚角估计结果；

图8是本发明的实施例所提供的在四旋翼飞行器的X、Y、Z轴加速度计中注入故障后的故障检测结果；

图9是本发明的实施例所提供的在四旋翼飞行器的横滚力矩模型、俯仰力矩模型、扭矩模型、X轴阻力模型、Y轴阻力模型、升力模型中注入故障后的故障检测结果；

图10是本发明的实施例所提供的在四旋翼飞行器的磁传感器、GPS气压计中注入故障后的故障检测结果。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。

如图1所示，一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，包括以下步骤：

步骤1：周期获取k时刻四旋翼飞行器机载传感器信息；

以当前时刻载体的重心位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的前向、右向和地向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与当地水平面的北向、东向、地向重合；

机载传感器信息包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；惯性传感器信息ω_gx(k)、ω_gy(k)、ω_gz(k)、f_ax(k)、f_ay(k)、f_az(k)，其分别为机体坐标系下X、Y、Z轴陀螺角速度输出和加速度计输出；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度，P_NG(k)、P_EG(k)，其分别为北向位置、东向位置；气压高度计信息h_b(k)；磁传感器信息ψ_m(k)。

步骤2：计算k时刻四旋翼飞行器的加速度、角加速度；

加速度信息通过下式进行计算：

式中，

为k时刻机体系相对于导航系的加速度在机体系X、Y、Z轴上的分量；k_Hx0、k_Hx1、k_Hx2、k_Hy0、k_Hy1、k_Hy2、k_T0、k_T1为模型参数；

为k-1时刻机体系相对于导航系的线速度在机体系X、Y轴上的分量；

为k-1时刻机体系相对于导航系的角加速度在机体系X、Y轴上的分量；

角加速度信息通过下式计算：

式中，

为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，

分别是

的微分，即角加速度；k_R0、k_R1、k_R2、k_P0、k_P1、k_P2、k_Q0、k_Q1、k_Q2为模型参数，所述模型参数均为常数，通过离线辨识方法获得。

步骤3：预测k时刻四旋翼飞行器的角速度、四元数、速度和位置；

1)角速度预测：

式中，

为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；ΔT为离散采样周期；

2)四元数预测：

式中，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的四元数；

3)速度预测：

式中，

为k时刻机体系相对于导航系的线速度在机体系X、Y、Z轴上的分量；g为重力加速度；

4)位置预测：

式中，p_n(k)、p_e(k)、p_d(k)分别为k时刻的北向位置、东向位置、地向高度。

步骤4：判断k时刻各检测滤波器的故障结果，并对故障进行定位；

故障检测的方法如下：

4.1模型/惯性传感器检测滤波器

4.1.1计算X轴阻力模型/X轴加速度计检测滤波器检测结果：

①计算统计参数λ_d11(k)：

式中，λ_d11(k)为k时刻的统计参数；

②计算检测函数J₁₁(k)：

式中，T₁₁是阈值，当J₁₁(k)＝1时，X轴阻力模型或X轴加速度计故障，当J₁₁(k)＝0时，X轴阻力模型或X轴加速度计均无故障；

4.1.2计算Y轴阻力模型/Y轴加速度计检测滤波器检测结果：

①计算统计参数λ_d12(k)：

式中，λ_d12(k)为k时刻的统计参数；

②计算检测函数J₁₂(k)：

式中，T₁₂是阈值，当J₁₂(k)＝1时，Y轴阻力模型或Y轴加速度计故障，当J₁₂(k)＝0时，Y轴阻力模型或Y轴加速度计均无故障；

4.1.3计算升力模型/Z轴加速度计检测滤波器检测结果：

①计算统计参数λ_d13(k)：

式中，λ_d13(k)为k时刻的统计参数，

②计算检测函数J₁₃(k)：

式中，T₁₃是阈值，当J₁₃(k)＝1时，升力模型或Z轴加速度计故障，当J₁₃(k)＝0时，升力模型或Z轴加速度计均无故障；

4.1.4计算横滚力矩模型/X轴陀螺检测滤波器检测结果：

①计算统计参数λ_d14(k)：

λ_d14(k)＝e_d14(k)P_dr14(k)^-1e_d14(k)^T

式中，λ_d14(k)是k时刻的统计参数，e_d14(k)为k时刻残差，P_dr14为k时刻残差方差；

Y_d14(k)＝ω_gx(k)

H_d14(k)＝1

为k时刻X轴陀螺噪声；

A_d14(k)＝1

G_d14(k-1)＝ΔT

式中，R₁₄(k)为k时刻量测噪声，

为k时刻X轴陀螺噪声；W_d14(k-1)为k-1时刻状态噪声，

为k-1时刻横滚力矩模型噪声；

②计算检测函数J₁₄(k)：

式中，T₁₄是阈值，当J₁₄(k)＝1时，横滚力矩模型或X轴陀螺故障，当J₁₄(k)＝0时，横滚力矩模型或X轴陀螺均无故障；

4.1.5计算俯仰力矩模型/Y轴陀螺检测滤波器检测结果：

①计算统计参数λ_d15(k)：

λ_d15(k)＝e_d15(k)P_dr15(k)^-1e_d15(k)^T

式中，λ_d15(k)为k时刻的统计参数，e_d15(k)为k时刻残差，P_dr15(k)为k时刻残差方差；

Y_d15(k)＝ω_gy(k)

H_d15(k)＝1

A_d15(k)＝1

G_d15(k-1)＝ΔT

式中，R₁₅(k)为k时刻量测噪声，

为k时刻Y轴陀螺噪声；W_d15(k-1)为k-1时刻状态噪声，

为k-1时刻俯仰力矩模型噪声；

②计算检测函数J₁₅(k)：

式中，T₁₅是阈值，当J₁₅(k)＝1时，俯仰力矩模型或Y轴陀螺故障，当J₁₅(k)＝0时，俯仰力矩模型或Y轴陀螺均无故障；

4.1.6计算扭矩模型/Z轴陀螺检测滤波器检测结果：

①计算统计参数λ_d16(k)：

λ_d16(k)＝e_d16(k)P_dr16(k)^-1e_d16(k)^T

式中，λ_d16(k)为k时刻的统计参数，e_d16(k)为k时刻残差，P_dr16为k时刻残差方差；

Y_d16(k)＝ω_gz(k)

H_d16(k)＝1

A_d16(k)＝1

G_d16(k-1)＝ΔT

式中，R₁₆(k)为k时刻量测噪声，

为k时刻Z轴陀螺噪声；W_d16(k-1)为k-1时刻状态噪声，

为k-1时刻扭矩模型噪声；

②计算检测函数J₁₆(k)：

式中，T₁₆是阈值，当J₁₆(k)＝1时扭矩模型或Z轴陀螺故障，当J₁₆(k)＝0时，扭矩模型或Z轴陀螺均无故障；

4.2模型/量测传感器检测滤波器

4.2.1计算动力学模型/磁传感器检测滤波器检测结果：

①计算统计参数λ_d21(k)：

λ_d21(k)＝e_d21(k)P_dr21(k)^-1e_d21(k)^T

式中，λ_d21(k)为k时刻的统计参数，e_d21(k)为k时刻残差；P_dr21为k时刻残差方差；

Y_d21(k)＝ψ_m(k)

R₂₁(k)＝w_ψm(k)

式中，W_d21(k-1)为k-1时刻状态噪声；R₂₁(k)为k时刻量测噪声；

为k时刻磁传感器噪声；

②计算检测函数J₂₁(k)：

式中，T₂₁是阈值，当J₂₁(k)＝1时动力学模型或磁传感器故障，当J₂₁(k)＝0时，动力学模型或磁传感器均无故障；

4.2.2计算动力学模型/GPS检测滤波器检测结果：

①计算统计参数λ_d22(k)：

λ_d22(k)＝e_d22(k)P_dr22(k)^-1e_d22(k)^T

式中，λ_d22(k)为k时刻的统计函数，e_d22(k)为k时刻残差；P_dr22(k)为时刻残差方差；

Y_d22(k)＝[V_NG(k) V_EG(k)]^T

H_d22(k)＝[0_2×7 ξ_2×3]

式中，W_d22(k-1)为k-1时刻状态噪声，

分别为k-1时刻机体系X、Y、Z轴速度噪声；R₂₂(k)为k时刻量测噪声，

分为k时刻GPS北向速度、东向速度噪声；

②计算检测函数J₂₂(k)：

式中，T₂₂是阈值，当J₂₂(k)＝1时动力学模型或GPS故障，当J₂₂(k)＝0时，动力学模型或GPS均无故障；

4.2.3计算动力学模型/气压计检测滤波器检测结果：

①计算统计参数λ_d23(k)：

λ_d23(k)＝e_d23(k)P_dr23(k)^-1e_d23(k)^T

式中，λ_d23(k)为k时刻的统计参数，e_d23(k)为k时刻残差；P_dr23为k时刻残差方差；

Z_1×3＝[2ΔT(q₁(k)q₃(k)-q₀(k)q₂(k))2ΔT(q₂(k)q₃(k)+q₀(k)q₁(k))ΔT(q₀ ²(k)-q₁ ²(k)-q₂ ²(k)+q₃ ²(k))]

式中，W_d23(k-1)为k-1时刻状态噪声，

为k-1时刻地向高度噪声；

为k时刻量测噪声，

为k时刻气压计噪声。

②计算检测函数J₂₃(k)：

式中，T₂₃是阈值，当J₂₃(k)＝1时，动力学模型或气压计故障，当J₂₃(k)＝0时，动力学模型或气压计均无故障；

4.3惯性传感器/量测传感器检测滤波器

4.3.1计算惯性传感器/磁传感器检测滤波器检测结果：

①计算统计参数λ_d31(k)：

λ_d31(k)＝e_d31(k)P_dr31(k)^-1e_d31(k)^T

式中，λ_d31(k)为k时刻的统计参数，e_d31(k)为k时刻残差；P_dr31为k时刻残差方差；

Y_d31(k)＝ψ_m(k)

A_d31(k)＝Τ

G_d31(k-1)＝Θ_4×3

式中，W_d31(k-1)为k-1时刻的状态噪声；R₃₁(k)为k时刻的量测噪声；

②计算检测函数J₃₁(k)：

式中，T₃₁是阈值，当J₃₁(k)＝1时，惯性传感器或磁传感器故障，当J₃₁(k)＝0时，惯性传感器或磁传感器均无故障；

4.3.2计算惯性传感器/GPS检测滤波器检测结果：

①计算统计参数λ_d32(k)：

λ_d32(k)＝e_d32(k)P_dr32(k)^-1e_d32(k)^T

式中，λ_d32(k)为k时刻检测滤波器统计参数；

为k时刻残差；

为k时刻残差方差；

Y_d32(k)＝[V_NG(k) V_EG(k)]^T

H_d32(k)＝[0_2×3 ξ_2×3]

式中，W_d32(k-1)为k-1时刻的状态噪声；R₃₂(k)为k时刻的量测噪声；

②计算检测函数J₃₂(k)：

式中，T₃₂是阈值，当J₃₂(k)＝1时，惯性传感器或GPS故障，当J₃₂(k)＝0时，惯性传感器或GPS均无故障；

4.3.3计算惯性传感器/气压计检测滤波器检测结果：

①计算统计参数λ_d33(k)：

λ_d33(k)＝e_d33(k)P_dr33(k)^-1e_d33(k)^T

式中，λ_d33(k)为k时刻的统计参数；e_d33(k)为k时刻残差；P_dr33(k)为k时刻残差方差；

Y_d33(k)＝h_b(k)

H_d33(k)＝-1

式中，R₃₃(k)为k时刻量测噪声；W_d33(k-1)为k-1时刻状态噪声；

②计算检测函数J₃₃(k)：

式中，T₃₃是阈值，当J₃₃(k)＝1时，惯性传感器或气压计故障，当J₃₃(k)＝0时，惯性传感器或气压计均无故障。

对故障位置进行定位的函数如下：

步骤5：根据步骤4的故障检测结果，隔离故障传感器信息，通过联邦卡尔曼滤波器对k时刻四旋翼飞行器的角速度、四元数、速度、位置进行校正，校正方法如下：

无故障情况下，采用如下过程对k时刻四旋翼飞行器的角速度、四元素、速度、位置进行校正：

1)计算滤波器的一步预测均方误差P_C(k|k-1)：

P_C(k|k-1)＝A_C(k|k-1)P_C(k-1)A_C(k|k-1)^T+G_C(k-1)W_C(k-1)G_C(k-1)^T

式中，A_C(k|k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示转置；P_C(k-1)为k-1时刻的状态估计均方差，P_C(k|k-1)为k-1时刻到k时刻的一步预测均方差；

式中，G_C(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵；W_C(k-1)为k-1时刻状态噪声；

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

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

3)计算k时刻第i个子滤波器扩展卡尔曼滤波器状态估计值X_Ci(k)：

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

式中，X_Ci(k)为k时刻状态量的估计值，

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

4)计算k时刻第i个子滤波器估计均方误差P_Ci(k)：

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

式中，P_Ci(k)为k时刻估计均方误差，I为单位矩阵。

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

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

+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

情况1：状态预测输入量发生故障，采用如下方法进行系统重构：

①横滚力矩模或俯仰力矩模型或扭矩模型故障，修改角速度一步预测方程如下：

修改横滚力矩模型、俯仰力矩模型、扭矩模型状态噪声；子滤波器量测更新及主滤波器融合滤波过程与无故障情况下的滤波融合过程相同；

②加速度计故障，修改速度一步预测方程如下：

修改速度预测状态噪声，子滤波器量测更新及主滤波器融合滤波过程与无故障情况下的滤波融合过程相同；

情况2：量测传感器发生故障，采用如下方法进行系统重构：

①X轴陀螺故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：

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

+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

②Y轴陀螺故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：

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

+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

③Z轴陀螺故障时，主滤波器的全局融合状态估计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_C4(k)^-1X_C4(k)

+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

④磁传感器故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：使用冗余的磁传感器代替故障磁传感器进行主滤波器的融合滤波；

⑤：GPS故障，使用加速计代替GPS进行量测更新

a)计算k时刻第7个子滤波器扩展卡尔曼滤波器滤波增益K_C7(k)：

K_C7(k)＝P_C(k|k-1)H_C7(k)^T[H_C7(k)P_C(k|k-1)H_C7(k)^T+R_C7(k)]^-1

其中，

Y_C7(k)＝[f_ax(k) f_ay(k)]^T

H_C7(k)＝[k_Hx1 k_Hy1]^T

R_C7(k)为量测噪声；

b)计算k时刻第7个子滤波器扩展卡尔曼滤波器状态估计值X_C7(k)：

X_C7(k)＝X_C7(k|k-1)+K_C7(k)[Y_C7(k)-h_C7(X_C7(k|k-1))]

式中，

c)计算k时刻第7个子滤波器估计均方误差P_C7(k)：

P_C7(k)＝[I-K_C7(k)H_C7(k)]P_C7(k|k-1)

d)计算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)

+P_C6(k)^-1X_C6(k)+P_C7(k)^-1X_C7(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝6P_g(k)

步骤6：根据步骤4的检测结果和步骤5的滤波估计结果对各检测滤波器进行重置更新：1)无故障重置方法，检测滤波器每运算n步执行一次重置：

2)有故障重置，检测滤波器的检测结果为“1”时，检测滤波器的状态及均方误差立即步骤5的方法重置，其余检测器采用n步重置法。

图6为在四旋翼飞行器的X、Y、Z轴陀螺中注入故障后的故障检测结果，故障类型为“0”，表示无故障，故障类型为“1”，表示检测到故障，可以看出故障检测系统可以检测到故障发生，在注入故障后约0.5s后检测到故障，在故障消失后，检测系统存在一个10s的延迟。

图7为在四旋翼飞行器的X、Y、Z轴陀螺中注入故障后的X、Y、Z轴角速度的参考曲线、估计曲线和横滚角、俯仰角、航向角的参考曲线、估计曲线，可以看出角速度估计和横滚角、俯仰角、航向角估计结果基本上不受影响，满足导航需求。

图8为在四旋翼飞行器的X、Y、Z轴加速度计中注入故障后的故障检测结果，故障类型为“0”，表示无故障，故障类型为“1”，表示检测到故障，可以看出故障检测系统可以检测到故障发生，在注入故障后约0.5后检测到故障，在故障消失后，检测系统存在一个10s的延迟。

图9为在四旋翼飞行器的横滚力矩模型、俯仰力矩模型、扭矩模型、X轴阻力模型、Y轴阻力模型、升力模型中注入故障后的故障检测结果，故障类型为“0”，表示无故障，故障类型为“1”，表示检测到故障，可以看出故障检测系统可以检测到故障发生。

图10为在四旋翼飞行器的磁传感器、GPS、气压计中注入故障后的故障检测结果，故障类型为“0”，表示无故障，故障类型为“1”，表示检测到故障，可以看出故障检测系统可以检测到故障发生。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，在不脱离本发明的精神和原则的前提下，本领域普通技术人员对本发明所做的任何修改、等同替换、改进等，均应落入本发明权利要求书确定的保护范围之内。

Claims (6)

1.一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：包括以下步骤：

步骤1：周期获取k时刻四旋翼飞行器机载传感器信息；

步骤2：计算k时刻四旋翼飞行器的加速度、角加速度；

步骤3：预测k时刻四旋翼飞行器的角速度、四元数、速度和位置；

步骤4：判断k时刻各检测滤波器的故障结果，并对故障进行定位；

步骤5：根据步骤4的故障检测结果，隔离故障传感器信息，通过联邦卡尔曼滤波器对k时刻四旋翼飞行器的角速度、四元数、速度、位置进行校正；

步骤6：根据步骤4的检测结果和步骤5的滤波估计结果对各检测滤波器进行重置更新；

以当前时刻载体的重心位置为原点构建机体坐标系，其中X轴、Y轴与Z轴分别与当前时刻载体的前向、右向和地向重合；以初始时刻载体的位置为原点构建导航坐标系，其中X轴、Y轴与Z轴分别与当地水平面的北向、东向、地向重合；

步骤1中获取的机载传感器信息包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；惯性传感器信息ω_gx(k)、ω_gy(k)、ω_gz(k)、f_ax(k)、f_ay(k)、f_az(k)，其分别为机体坐标系下X、Y、Z轴陀螺角速度输出和加速度计输出；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度，P_NG(k)、P_EG(k)，其分别为北向位置、东向位置；气压高度计信息h_b(k)；磁传感器信息ψ_m(k)；

步骤2中，加速度信息通过下式进行计算：

式中，

为k时刻机体系相对于导航系的加速度在机体

系X、Y、Z轴上的分量；k_Hx0、k_Hx1、k_Hx2、k_Hy0、k_Hy1、k_Hy2、k_T0、k_T1为模型参数；

为k-1时刻机体系相对于导航系的线速度在机体系X、Y轴上的分量；

为k-1时刻机体系相对于导航系的角加速度在机体系X、Y轴上的分量；

角加速度信息通过下式计算：

式中，

为k时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量，

分别是

的微分，即角加速度；k_R0、k_R1、k_R2、k_P0、k_P1、k_P2、k_Q0、k_Q1、k_Q2为模型参数。

2.根据权利要求1所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤3中预测k时刻四旋翼飞行器的角速度、四元数、速度和位置的方法如下：

1)角速度预测：

式中，

为k-1时刻机体系相对于导航系的角速度在机体系X、Y、Z轴上的分量；ΔT为离散采样周期；

2)四元数预测：

式中，q₀(k)、q₁(k)、q₂(k)、q₃(k)为k时刻的四元数；

3)速度预测：

式中，

为k时刻机体系相对于导航系的线速度在机体系X、Y、Z轴上的分量；g为重力加速度；

4)位置预测：

式中，p_n(k)、p_e(k)、p_d(k)分别为k时刻的北向位置、东向位置、地向高度。

3.根据权利要求2所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤4中对各检测滤波器进行故障检测的方法如下：

4.1模型/惯性传感器检测滤波器

4.1.1计算X轴阻力模型/X轴加速度计检测滤波器检测结果：

①计算统计参数λ_d11(k)：

式中，λ_d11(k)为k时刻的统计参数；

②计算检测函数J₁₁(k)：

式中，T₁₁是阈值，当J₁₁(k)＝1时，X轴阻力模型或X轴加速度计故障，当J₁₁(k)＝0时，X轴阻力模型或X轴加速度计均无故障；

4.1.2计算Y轴阻力模型/Y轴加速度计检测滤波器检测结果：

①计算统计参数λ_d12(k)：

式中，λ_d12(k)为k时刻的统计参数；

②计算检测函数J₁₂(k)：

式中，T₁₂是阈值，当J₁₂(k)＝1时，Y轴阻力模型或Y轴加速度计故障，当J₁₂(k)＝0时，Y轴阻力模型或Y轴加速度计均无故障；

4.1.3计算升力模型/Z轴加速度计检测滤波器检测结果：

①计算统计参数λ_d13(k)：

式中，λ_d13(k)为k时刻的统计参数，

②计算检测函数J₁₃(k)：

式中，T₁₃是阈值，当J₁₃(k)＝1时，升力模型或Z轴加速度计故障，当J₁₃(k)＝0时，升力模型或Z轴加速度计均无故障；

4.1.4计算横滚力矩模型/X轴陀螺检测滤波器检测结果：

①计算统计参数λ_d14(k)：

λ_d14(k)＝e_d14(k)P_dr14(k)^-1e_d14(k)^T

式中，λ_d14(k)是k时刻的统计参数，e_d14(k)为k时刻残差，P_dr14为k时刻残差方差；

Y_d14(k)＝ω_gx(k)

H_d14(k)＝1

A_d14(k)＝1

G_d14(k-1)＝ΔT

式中，R₁₄(k)为k时刻量测噪声；

为k时刻X轴陀螺噪声；W_d14(k-1)为k时刻状态噪声；

为k-1时刻横滚力矩模型噪声；

②计算检测函数J₁₄(k)：

式中，T₁₄是阈值，当J₁₄(k)＝1时，横滚力矩模型或X轴陀螺故障，当J₁₄(k)＝0时，横滚力矩模型或X轴陀螺均无故障；

4.1.5计算俯仰力矩模型/Y轴陀螺检测滤波器检测结果：

①计算统计参数λ_d15(k)：

λ_d15(k)＝e_d15(k)P_dr15(k)^-1e_d15(k)^T

式中，λ_d15(k)为k时刻的统计参数，e_d15(k)为k时刻残差，P_dr15(k)为k时刻残差方差；

Y_d15(k)＝ω_gy(k)

H_d15(k)＝1

A_d15(k)＝1

G_d15(k-1)＝ΔT

式中，R₁₅(k)为k时刻量测噪声；

为k时刻Y轴陀螺噪声；W_d15(k-1)为k时刻状态噪声；

为k-1时刻俯仰力矩模型噪声；

②计算检测函数J₁₅(k)：

式中，T₁₅是阈值，当J₁₅(k)＝1时，俯仰力矩模型或Y轴陀螺故障，当J₁₅(k)＝0时，俯仰力矩模型或Y轴陀螺均无故障；

4.1.6计算扭矩模型/Z轴陀螺检测滤波器检测结果：

①计算统计参数λ_d16(k)：

λ_d16(k)＝e_d16(k)P_dr16(k)^-1e_d16(k)^T

式中，λ_d16(k)为k时刻的统计参数，e_d16(k)为k时刻残差，P_dr16为k时刻残差方差；

Y_d16(k)＝ω_gz(k)

H_d16(k)＝1

A_d16(k)＝1

G_d16(k-1)＝ΔT

式中，R₁₆(k)为k时刻量测噪声；

为k时刻Z轴陀螺噪声；W_d16(k-1)为k时刻状态噪声；

为k-1时刻扭矩模型噪声；

②计算检测函数J₁₆(k)：

式中，T₁₆是阈值，当J₁₆(k)＝1时扭矩模型或Z轴陀螺故障，当J₁₆(k)＝0时，扭矩模型或Z轴陀螺均无故障；

4.2模型/量测传感器检测滤波器

4.2.1计算动力学模型/磁传感器检测滤波器检测结果：

①计算统计参数λ_d21(k)：

λ_d21(k)＝e_d21(k)P_dr21(k)^-1e_d21(k)^T

式中，λ_d21(k)为k时刻的统计参数，e_d21(k)为k时刻残差；P_dr21为k时刻残差方差；

Y_d21(k)＝ψ_m(k)

式中，W_d21(k-1)为k-1时刻状态噪声；R₂₁(k)为k时刻量测噪声；

为k时刻磁传感器噪声；

②计算检测函数J₂₁(k)：

式中，T₂₁是阈值，当J₂₁(k)＝1时动力学模型或磁传感器故障，当J₂₁(k)＝0时，动力学模型或磁传感器均无故障；

4.2.2计算动力学模型/GPS检测滤波器检测结果：

①计算统计参数λ_d22(k)：

λ_d22(k)＝e_d22(k)P_dr22(k)^-1e_d22(k)^T

式中，λ_d22(k)为k时刻的统计函数，e_d22(k)为k时刻残差；P_dr22(k)为时刻残差方差；

Y_d22(k)＝[V_NG(k) V_EG(k)]^T

H_d22(k)＝[0_2×7 ξ_2×3]

式中，W_d22(k-1)为k-1时刻状态噪声，

分别为k-1时刻机体系X、Y、Z轴速度噪声；R₂₂(k)为k时刻量测噪声，

分为k时刻GPS北向速度、东向速度噪声；

②计算检测函数J₂₂(k)：

式中，T₂₂是阈值，当J₂₂(k)＝1时动力学模型或GPS故障，当J₂₂(k)＝0时，动力学模型或GPS均无故障；

4.2.3计算动力学模型/气压计检测滤波器检测结果：

①计算统计参数λ_d23(k)：

λ_d23(k)＝e_d23(k)P_dr23(k)^-1e_d23(k)^T

式中，λ_d23(k)为k时刻的统计参数，e_d23(k)为k时刻残差；P_dr23为k时刻残差方差；

Z_1×3＝[2ΔT(q₁(k)q₃(k)-q₀(k)q₂(k)) 2ΔT(q₂(k)q₃(k)+q₀(k)q₁(k)) ΔT(q₀ ²(k)-q₁ ²(k)-q₂ ²(k)+q₃ ²(k))]

式中，W_d23(k-1)为k-1时刻状态噪声，

为k-1时刻地向高度噪声；

为k时刻量测噪声，

为k时刻气压计噪声；

②计算检测函数J₂₃(k)：

式中，T₂₃是阈值，当J₂₃(k)＝1时，动力学模型或气压计故障，当J₂₃(k)＝0时，动力学模型或气压计均无故障；

4.3惯性传感器/量测传感器检测滤波器

4.3.1计算惯性传感器/磁传感器检测滤波器检测结果：

①计算统计参数λ_d31(k)：

λ_d31(k)＝e_d31(k)P_dr31(k)^-1e_d31(k)^T

式中，λ_d31(k)为k时刻的统计参数，e_d31(k)为k时刻残差；P_dr31为k时刻残差方差；

Y_d31(k)＝ψ_m(k)

A_d31(k)＝T

G_d31(k-1)＝Θ_4×3

式中，W_d31(k-1)为k-1时刻的状态噪声；R₃₁(k)为k时刻的量测噪声；

②计算检测函数J₃₁(k)：

式中，T₃₁是阈值，当J₃₁(k)＝1时，惯性传感器或磁传感器故障，当J₃₁(k)＝0时，惯性传感器或磁传感器均无故障；

4.3.2计算惯性传感器/GPS检测滤波器检测结果：

①计算统计参数λ_d32(k)：

λ_d32(k)＝e_d32(k)P_dr32(k)^-1e_d32(k)^T

式中，λ_d32(k)为k时刻检测滤波器统计参数；

为k时刻残差；

为k时刻残差方差；

Y_d32(k)＝[V_NG(k) V_EG(k)]^T

H_d32(k)＝[0_2×3 ξ_2×3]

式中，W_d32(k-1)为k-1时刻的状态噪声；R₃₂(k)为k时刻的量测噪声；

②计算检测函数J₃₂(k)：

式中，T₃₂是阈值，当J₃₂(k)＝1时，惯性传感器或GPS故障，当J₃₂(k)＝0时，惯性传感器或GPS均无故障；

4.3.3计算惯性传感器/气压计检测滤波器检测结果：

①计算统计参数λ_d33(k)：

λ_d33(k)＝e_d33(k)P_dr33(k)^-1e_d33(k)^T

式中，λ_d33(k)为k时刻的统计参数；e_d33(k)为k时刻残差；P_dr33(k)为k时刻残差方差；

Y_d33(k)＝h_b(k)

H_d33(k)＝-1

式中，R₃₃(k)为k时刻量测噪声；W_d33(k-1)为k-1时刻状态噪声；

②计算检测函数J₃₃(k)：

式中，T₃₃是阈值，当J₃₃(k)＝1时，惯性传感器或气压计故障，当J₃₃(k)＝0时，惯性传感器或气压计均无故障。

4.根据权利要求3所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤4中对故障进行定位的函数如下：

5.根据权利要求4所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤5中对四旋翼飞行器的角速度、四元数、速度和位置进行校正的方法如下：

无故障情况下，采用如下过程对k时刻四旋翼飞行器的角速度、四元素、速度、位置进行校正：

1)计算滤波器的一步预测均方误差P_C(k|k-1)：

P_C(k|k-1)＝A_C(k|k-1)P_C(k-1)A_C(k|k-1)^T+G_C(k-1)W_C(k-1)G_C(k-1)^T

式中，A_C(k|k-1)为滤波器k-1时刻到k时刻的滤波器一步转移矩阵，上标T表示转置；P_C(k-1)为k-1时刻的状态估计均方差，P_C(k|k-1)为k-1时刻到k时刻的一步预测均方差；

式中，G_C(k-1)为滤波器k-1时刻的滤波器噪声系数矩阵；W_C(k-1)为k-1时刻状态噪声；

和

分别为k-1时刻东向位置噪声、北向位置噪声和地向高度噪声；

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

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

3)计算k时刻第i个子滤波器扩展卡尔曼滤波器状态估计值X_Ci(k)：

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

式中，X_Ci(k)为k时刻状态量的估计值，

p_n(k)、p_e(k)、p_d(k)分别为k时刻的北向位置、东向位置、地向高度；

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

4)计算k时刻第i个子滤波器估计均方误差P_Ci(k)：

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

式中，P_Ci(k)为k时刻估计均方误差，I为单位矩阵；

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

X_g(k)＝P_g(k)[P_C2(k)^-1X_C2(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

情况1：状态预测输入量发生故障，采用如下方法进行系统重构：

①横滚力矩模或俯仰力矩模型或扭矩模型故障，修改角速度一步预测方程如下：

修改横滚力矩模型、俯仰力矩模型、扭矩模型状态噪声；子滤波器量测更新及主滤波器融合滤波过程与无故障情况下的滤波融合过程相同；

②加速度计故障，修改速度一步预测方程如下：

修改速度预测状态噪声，子滤波器量测更新及主滤波器融合滤波过程与无故障情况下的滤波融合过程相同；

情况2：量测传感器发生故障，采用如下方法进行系统重构：

①X轴陀螺故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：

X_g(k)＝P_g(k)[P_C2(k)^-1X_C2(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

P_g(k)＝sP_C2(k)^-1+P_C3(k)^-1+P_C4(k)^-1+P_C5(k)^-1+P_C6(k)^-1]^-1

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

②Y轴陀螺故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：

X_g(k)＝P_g(k)[P_C1(k)^-1X_C1(k)+P_C3(k)^-1X_C3(k)+P_C4(k)^-1X_C4(k)+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

P_g(k)＝sP_C1(k)^-1+P_C3(k)^-1+P_C4(k)^-1+P_C5(k)^-1+P_C6(k)^-1]^-1

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

③Z轴陀螺故障时，主滤波器的全局融合状态估计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_C4(k)^-1X_C4(k)+P_C5(k)^-1X_C5(k)+P_C6(k)^-1X_C6(k)]^-1

P_g(k)＝sP_C1(k)^-1+P_C2(k)^-1+P_C4(k)^-1+P_C5(k)^-1+P_C6(k)^-1]^-1

X_C(k)＝X_g(k)

P_C(k)＝5P_g(k)

④磁传感器故障时，主滤波器的全局融合状态估计X_g(k)及均方误差估计P_g(k)如下：使用冗余的磁传感器代替故障磁传感器进行主滤波器的融合滤波；

⑤：GPS故障，使用加速计代替GPS进行量测更新

a)计算k时刻第7个子滤波器扩展卡尔曼滤波器滤波增益K_C7(k)：

K_C7(k)＝P_C(k|k-1)H_C7(k)^T[H_C7(k)P_C(k|k-1)H_C7(k)^T+R_C7(k)]^-1

其中，

Y_C7(k)＝[f_ax(k) f_ay(k)]^T

H_C7(k)＝[k_Hx1 k_Hy1]^T

R_C7(k)为量测噪声；

b)计算k时刻第7个子滤波器扩展卡尔曼滤波器状态估计值X_C7(k)：

X_C7(k)＝X_C7(k|k-1)+K_C7(k)[Y_C7(k)-h_C7(X_C7(k|k-1))]

式中，

c)计算k时刻第7个子滤波器估计均方误差P_C7(k)：

P_C7(k)＝[I-K_C7(k)H_C7(k)]P_C7(k|k-1)

d)计算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)+P_C6(k)^-1X_C6(k)+P_C7(k)^-1X_C7(k)]^-1

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

X_C(k)＝X_g(k)

P_C(k)＝6P_g(k)。

6.根据权利要求5所述的一种基于动力学模型的四旋翼飞行器惯性传感器容错导航方法，其特征在于：步骤6中对各检测滤波器进行重置更新的方法如下：

1)无故障重置方法，检测滤波器每运算n步执行一次重置：

2)有故障重置，检测滤波器的检测结果为“1”时，检测滤波器的状态及均方误差立即步骤5的方法重置，其余检测器采用n步重置法。

Images