[发明专利]一种解析式冗余的飞行器容错导航估计方法

权利要求书

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

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

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

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

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

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

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

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

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

利用角加速度计输出计算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为离散采样时间；

计算子检测器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)

λ₂(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表示转置；

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

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

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

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

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

计算子检测器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轴的加速度，通过加速度计获得；f_mz(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轴角速度噪声；

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

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

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

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

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

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

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

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

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