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

权利要求书

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时刻的北向位置、东向位置、地向高度。