CN108981709B - 基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法 - Google Patents

Abstract

本发明公开一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，步骤是：周期读取k时刻四旋翼飞行器机载传感器信息，执行故障检测滤波器及故障定位策略，判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障；根据故障定位结果，确定各个子滤波器的状态方程，进行各子滤波器的数据融合；根据故障检测结果，执行全局滤波器，对各子滤波器进行故障隔离，对无故障的子滤波器进行数据融合，得到x轴角速度、y轴角速度、横滚角、俯仰角信息；根据全局滤波器结果，进行重置更新，并执行系统重置策略。此种方法通过四旋翼飞行器的动力学模型，形成x、y轴陀螺的冗余，实现x、y轴陀螺的故障检测及系统重置，在陀螺故障时仍能获得姿态角、航向角的准确估计。

Description

基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法

技术领域

本发明属于组合导航及容错导航领域，具体涉及一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法的容错导航方法。

背景技术

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

目前四旋翼飞行器常用的传感器包括惯性传感器、GNSS(卫星导航系统)、磁传感器、气压高度计，其中惯性传感器包括陀螺仪与加速度计。受成本、体积所限，四旋翼飞行器中选用的惯性传感器精度、可靠性较低，易受外界温度、振动干扰而产生性能下降，甚至失效。此时，会导致导航系统精度下降，影响飞行安全。目前，尚未有针对惯性传感器失效情况下的四旋翼飞行器导航方法。本专利针对x轴、y轴陀螺故障的情况，提出了一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，通过力矩模型构建x轴、y轴陀螺的冗余信息，可以实现对x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障检测，在检测到x轴陀螺或y轴陀螺故障时，可以使用横滚力矩模型或俯仰力矩模型代替故障的陀螺进行导航解算，获得准确的姿态角估计结果。

发明内容

本发明的目的，在于提供一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，通过四旋翼飞行器的动力学模型，形成x、y轴陀螺的冗余，实现x、y轴陀螺的故障检测及系统重置，在陀螺故障时仍能获得姿态角、航向角的准确估计。

为了达成上述目的，本发明的解决方案是：

一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度；磁传感器信息ψ_m(k)；陀螺信息

其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量；加计信息

分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

步骤二：执行故障检测滤波器及故障定位策略，判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障；

步骤三：根据步骤二的故障定位结果，确定各个子滤波器的状态方程，进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合；

步骤四：根据故障检测结果，执行全局滤波器，对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离，对无故障的子滤波器进行数据融合，得到x轴角速度、y轴角速度、横滚角、俯仰角信息；

步骤五：根据全局滤波器结果，对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新，并执行系统重置策略。

采用上述方案后，本发明利用四旋翼飞行器的横滚力矩、俯仰力矩模型，与机载陀螺、加速度计、磁传感器、GPS相融合，可实现x轴、y轴陀螺故障情况下对横滚角、俯仰角的准确估计。在该方法中，通过横滚力矩模型、俯仰力矩模型构建x轴、y轴陀螺的冗余信息，建立故障检测函数，实现对x轴、y轴陀螺故障以及力矩故障的检测；同时，在x轴、y轴陀螺故障情况下，通过横滚力矩模型、俯仰力矩对其角速度信号进行重构，代替故障的陀螺进行导航解算，实现容错导航。该方法无需增加额外的惯性传感器，可实现对x轴、y轴陀螺的故障检测、隔离、信号重构，保障陀螺失效情况下姿态角的估计精度。

附图说明

图1为本发明方法的滤波结构图；

图2为本发明方法的故障检测结构图；

图3为本发明方法的流程图；

图4为x轴陀螺故障时，故障检测结果；

图5为x轴陀螺故障时，x轴角速度估计结果；

图6为x轴陀螺故障时，y轴角速度估计结果；

图7为x轴陀螺故障时，横滚角估计结果；

图8为x轴陀螺故障时，俯仰角估计结果；

图9为y轴陀螺故障时，故障检测结果；

图10为y轴陀螺故障时，x轴角速度估计结果；

图11为y轴陀螺故障时，y轴角速度估计结果；

图12为y轴陀螺故障时，横滚角估计结果；

图13为y轴陀螺故障时，俯仰角估计结果；

图14为横滚力矩模型故障时，故障检测结果；

图15为俯仰力矩模型故障时，故障检测结果。

具体实施方式

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

本发明方法的滤波流程如图1所示，故障检测结构如图2所示，其采用半物理仿真的形式，四旋翼无人机连续飞行三个矩形，采集机载传感器实验数据，其包含惯性传感器实验数据、电子调速器数据、GPS数据、磁传感器数据。

本发明提供的一种基于力矩模型的四旋翼横滚角、俯仰角容错估计方法，包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度；磁传感器信息ψ_m(k)；陀螺信息

其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量；加计信息

分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

步骤二：执行故障检测滤波器及故障定位策略，判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障，故障检测滤波器由横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器等6个检测滤波器组成，上述6个检测滤波器的状态更新、故障定位更新步骤如下：

步骤21，横滚力矩模型/x轴陀螺检测滤波器故障检测步骤如下：

步骤211，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的状态估计值及估计均方误差

k_x0(k)＝k_x0(k-1)

式中，k_x0(k)为k时刻横滚力矩模型零偏系数；k_x0(k-1)为k-1时刻横滚力矩模型零偏系数；k_x1、k_x2为横滚力矩模型系数，为常值；ω_mx(k)为k时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量；ω_mx(k-1)为k-1时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量；

为k时刻机体系相对于导航系的加速度在机体系y轴上的分量，通过加计输出获得；ω_i(k)为k时刻第i个旋翼的转速，i＝1,2,3,4；ΔT为离散采样周期；上标T表示转置；

为k时刻的估计均方误差；

为k-1时刻的估计均方误差；

为雅可比矩阵；G_d1(k-1)＝[I_2×2]，为系统噪声系数，I_2×2为2×2的单位矩阵；W_d1(k-1)＝[w_kx0(k-1) w_mx(k-1)]^T，为状态噪声，w_kx0(k-1)为k-1时刻横滚力矩模型零偏白噪声；w_mx(k-1)为k-1时刻横滚力矩模型的白噪声；

步骤212，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的故障统计参数

式中，

为k时刻横滚力矩模型/x轴陀螺检测滤波器的统计参数；

为k时刻的残差；

为k时刻机体系相对于导航系的角速度在机体系x轴上的分量，通过x轴陀螺输出获得；

为k时刻的残差方差；H_d1(k)＝[01]；R_d1(k)＝diag([w_gx(k)]²)，w_gx为机体系x轴陀螺的白噪声；

步骤213，根据故障统计参数结果，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的检测函数，判断横滚力矩模型和x轴陀螺是否发生故障

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

步骤22，俯仰力矩模型/y轴陀螺检测滤波器故障检测步骤如下：

步骤221，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的状态估计值及估计均方误差

k_y0(k)＝k_y0(k-1)

式中，k_y0(k)为k时刻俯仰力矩模型零偏系数；k_y0(k-1)为k-1时刻俯仰力矩模型零偏系数；k_y1、k_y2为俯仰力矩模型系数；ω_my(k)为k时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量；ω_my(k-1)为k-1时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量；

为雅可比矩阵；G_d2(k-1)＝[I_2×2]，为系统噪声系数；W_d2(k-1)＝[w_ky0(k-1) w_my(k-1)]^T，为状态噪声，w_ky0(k-1)为k-1时刻俯仰力矩模型零偏白噪声，w_my(k-1)为k-1时刻俯仰力矩模型的白噪声；

步骤222，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的故障统计参数

式中，

为k时刻俯仰力矩模型/y轴陀螺检测器的统计参数；

为k时刻的残差；

为k时刻机体系相对于导航系的角速度在机体系y轴上的分量，通过陀螺输出获得；

为k时刻的残差方差；H_d2(k)＝[0 1]；R_d2(k)＝diag([w_gy(k)]²)，w_gy为机体系y轴陀螺的白噪声；

步骤223，根据故障统计参数结果，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的检测函数，判断俯仰力矩模型和y轴陀螺是否发生故障

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

步骤23，横滚力矩模型/加速度计/GPS检测滤波器故障检测步骤如下：

步骤231，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差

k_x0(k)＝k_x0(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时刻四元素；

为机体系相对于导航系的角速度在机体系z轴上的分量，通过z轴陀螺输出获得；

为雅可比矩阵；

为k-1时刻系统噪声系数；

I_2×2为2×2的单位矩阵；0_2×3为2×3的零矩阵；0_4×2为4×2的零矩阵；W_d3(k-1)＝[w_kx0(k-1)w_mx(k-1) w_ωx(k-1) w_ωy(k-1) w_gz(k-1)]^T，为k-1时刻状态噪声，w_ωx(k-1)为k-1时刻机体系x轴角速度白噪声；w_gz(k-1)为k-1时刻机体系z轴陀螺白噪声；

步骤232，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻横滚力矩模型/加速度计/GPS检测滤波器的统计参数；

为k时刻的残差；

V_DG(k)分别为导航系下北向、东向、地向速度，通过GPS获得，

为k-1时刻系统滤波四元素估计；

分别为k时刻机体系相对于导航系的线速度在机体系y轴的分量；

g为重力加速度；

为k时刻的残差方差，H_d3(k)＝[0_1×2 Ω_1×4 ^d3],Ω_1×4 ^d3＝[2gq₁(k) 2gq₀(k) 2gq₃(k)2gq₂(k)]，

为加速度计和GPS的组合白噪声在机体系y轴上的分量；

步骤233，根据故障统计参数结果，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的检测函数，判断横滚力矩模型是否发生故障

式中，T₃是阈值，当J₃(k)＝1时，横滚力矩模型故障；当J₃(k)＝0时，横滚力矩模型无故障；

步骤24，俯仰力矩模型/加速度计/GPS检测滤波器故障检测步骤如下：

步骤241，计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差

k_y0(k)＝k_y0(k-1)

式中，

为雅可比矩阵；G_d4(k-1)＝G_d3(k-1)，为系统噪声系数矩阵；W_d4(k-1)＝W_d3(k-1)，为状态噪声；

步骤242，计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻俯仰力矩模型/加速度计/GPS检测滤波器的统计参数；

为k时刻的残差；

为k时刻的残差方差；H_d4(k)＝[0_1×2 Ω_1×4 ^d4]；

Ω_1×4 ^d4＝[-2gq₂(k) 2gq₃(k) -2gq₀(k)2gq₁(k)]；

为加速度计和GPS的组合白噪声在机体系x轴上的分量；

步骤243，根据故障统计参数结果，计算k时刻检测滤波器的检测函数，判断俯仰力矩模型是否发生故障

式中，T₄是阈值，当J₄(k)＝1时，俯仰力矩模型故障；当J₄(k)＝0时，俯仰力矩模型无故障；

步骤25，x轴陀螺/加速度计/GPS检测滤波器检测步骤如下：

步骤251，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差

式中，

为雅可比矩阵；

为系统噪声系数；W_d5(k-1)＝[w_gx(k-1) w_gy(k-1) w_gz(k-1)]^T，为k-1时刻状态噪声；

步骤252，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻x轴陀螺/加速度计/GPS检测滤波器的统计参数，

为k时刻的残差，

为k时刻的残差方差，H_d5(k)＝Ω_1×4 ^d5，Ω_1×4 ^d5＝Ω_1×4 ^d3，

步骤253，根据故障统计参数结果，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的检测函数，判断x轴陀螺是否发生故障

式中，T₅是阈值，当J₅(k)＝1时，x轴陀螺故障；当J₅(k)＝0时，无故障；

步骤26，y轴陀螺/加速度计/GPS检测滤波器检测步骤如下：

步骤261，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差

式中，Φ_d6(k)＝Φ_d5(k)，为雅可比矩阵；G_d6(k-1)＝G_d5(k-1)，为系统噪声系数；W_d6(k-1)＝W_d5(k-1)，为状态噪声；

步骤262，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻y轴陀螺/加速度计/GPS检测滤波器的统计参数，

为k时刻的残差，

为k时刻的残差方差，H_d6(k)＝Ω_1×4 ^d6，

Ω_1×4 ^d6＝Ω_1×4 ^d4；

步骤263，根据故障统计参数结果，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的检测函数，判断y轴陀螺故障是否发生故障

式中，T₆是阈值，当J₆(k)＝1时，y轴陀螺故障；当J₆(k)＝0时，无故障；

步骤27，根据各个检测滤波器的检测函数结果，计算k时刻故障定位函数及根据故障定位函数计算结果，进行故障定位，步骤如下：

步骤271，计算k时刻预发生故障定位函数，判断系统是否进入预发生故障

式中，“∨”表示逻辑运算中“或”运算符，“∧”表示逻辑运算中“与”运算符；当F_pre(k)＝1时，进入预发生故障阶段，当F_pre(k)＝0时，未进入预发生故障；

步骤272，计算k时刻x轴陀螺故障定位函数，判断x轴陀螺是否发生故障：

当F_GX(k)＝1时，x轴陀螺故障；当F_GX(k)＝0时，x轴陀螺无故障；

步骤273，计算k时刻y轴陀螺故障定位函数，判断y轴陀螺是否发生故障：

当F_GY(k)＝1时，y轴陀螺故障；当F_GY(k)＝0时，y轴陀螺无故障；

步骤274，计算横滚力矩模型故障定位函数，判断横滚力矩模型是否发生故障：

当F_MX(k)＝1时，横滚力矩模型故障；当F_MX(k)＝0时，横滚力矩模型无故障；

步骤275，计算俯仰力矩模型故障定位函数，判断俯仰力矩模型是否发生故障：

当F_MY(k)＝1时，俯仰力矩模型故障；当F_MY(k)＝0时，俯仰力矩模型无故障；

步骤276，计算无故障定位函数，判断系统是否发生故障：

当F_no(k)＝1时，无故障。

步骤三：根据步骤二的故障定位结果，确定各个子滤波器的状态方程，进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合：

步骤31，计算k时刻四个子滤波器的状态预测及预测均方误差

情况311，无故障或x轴陀螺或y轴陀螺故障时，四个子滤波器均按照如下步骤进行状态预测及均方误差预测：

步骤3111，计算k时刻横滚力矩模型零偏及俯仰力矩模型零偏预测

k_x0(k|k-1)＝k_x0(k-1)

k_y0(k|k-1)＝k_y0(k-1)

式中，k_x0(k|k-1)、k_y0(k|k-1)分别为横滚力矩模型、俯仰力矩模型零偏项在k-1时刻到k时刻的状态一步预测；

步骤3112，计算k时刻x轴角速度预测及y轴角速度预测

式中，ω_mx(k|k-1)、ω_my(k|k-1)分别为x、y轴角速度在k-1时刻到k时刻的状态一步预测；

步骤3113，计算k时刻四元素预测

式中，q₀(k|k-1)、q₁(k|k-1)、q₂(k|k-1)、q₃(k|k-1)为四元素在k-1时刻到k时刻的状态一步预测；

步骤3114，计算一步预测均方误差P_C(k|k-1)

P_C(k|k-1)＝Φ_C(k|k-1)P_C(k-1)Φ_C(k|k-1)^T+G_C(k-1)Q_C(k-1)G_C(k-1)^T式中，P_C(k|k-1)为k-1到k时刻的一步预测均方误差；P_C(k-1)为k-1时刻估计均方误差；

为雅可比矩阵，

为非线性状态方程，

为k-1时刻状态量，

Q_C(k-1)＝diag(W_C(k-1)²)；

W_C(k-1)＝[w_kx0(k-1) w_ky0(k-1) w_mx(k-1) w_my(k-1) w_ωx(k-1) w_ωy(k-1) w_gz(k-1)]^T为k-1时刻的状态噪声；

情况312，横滚力矩模型/x轴陀螺检测滤波器预发生故障或横滚力矩模型故障时，四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测：

3个子滤波器的状态方程中关于x轴角速度的预测方程修改为ω_mx(k|k-1)＝ω_mx(k-1)，雅可比矩阵修改为：

其余同无故障情况；

情况313，俯仰力矩模型/y轴陀螺检测滤波器预发生故障或俯仰力矩模型故障时，四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测：

3个子滤波器的状态方程中关于y轴角速度的预测方程修改为ω_my(k|k-1)＝ω_my(k-1)，雅可比矩阵修改为：

其余同无故障情况；

步骤32，力矩模型/x轴陀螺子滤波器量测更新步骤如下：

步骤321，计算k时刻的力矩模型/x轴陀螺子滤波器的滤波增益

K_C1(k)＝P_C(k|k-1)H_C1(k)^T[H_C1(k)P_C(k|k-1)H_C1(k)^T+R_C1(k)]^-1

式中，H_C1(k)＝[0_1×2 1 0_1×5]^T，0_1×2为1×2的零矩阵；0_1×5为1×5的零矩阵；K_C1(k)为k时刻的滤波增益，R_C1(k)＝diag([w_gx(k)]²)，为k时刻的量测噪声；

步骤322，计算k时刻力矩模型/x轴陀螺子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

步骤323，计算k时刻力矩模型/x轴陀螺子滤波器估计均方误差P_C1(k)

P_C1(k)＝[I-K_C1(k)H_C1(k)]P_C(k|k-1)

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

步骤33，力矩模型/y轴陀螺子滤波器量测更新步骤如下：

步骤331，计算k时刻的力矩模型/y轴陀螺子滤波器的滤波增益

K_C2(k)＝P_C(k|k-1)H_C2(k)^T[H_C2(k)P_C(k|k-1)H_C2(k)^T+R_C2(k)]^-1

式中，H_C2(k)＝[0_1×3 1 0_1×4]^T，0_1×3为1×3的零矩阵，0_1×4为1×4的零矩阵；K_C2(k)为k时刻的滤波增益，R_C2(k)＝diag([w_gy(k)]²)，为k时刻的量测噪声；

步骤332，计算k时刻力矩模型/y轴陀螺子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

步骤333，计算k时刻力矩模型/y轴陀螺子滤波器估计均方误差P_C2(k)

P_C2(k)＝[I-K_C2(k)H_C2(k)]P_C(k|k-1)

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

步骤34，力矩模型/加速度计/GPS子滤波器量测更新步骤如下：

步骤341，计算k时刻的力矩模型/加速度计/GPS子滤波器的滤波增益

K_C3(k)＝P_C(k|k-1)H_C3(k)^T[H_C3(k)P_C(k|k-1)H_C3(k)^T+R_C3(k)]^-1

式中，K_C3(k)为k时刻的滤波增益；

0_1×4为1×4的零矩阵，g为重力加速度；

为k时刻的量测噪声；

步骤342，计算k时刻力矩模型/加速度计/GPS子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

步骤343，计算k时刻力矩模型/加速度计/GPS子滤波器估计均方误差P_C3(k)

P_C3(k)＝[I-K_C3(k)H_C3(k)]P_C(k|k-1)

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

步骤35，力矩模型/磁传感器子滤波器更新步骤如下：

步骤351，计算k时刻的力矩模型/磁传感器子滤波器的滤波增益

K_C4(k)＝P_C(k|k-1)H_C4(k)^T[H_C4(k)P_C(k|k-1)H_C4(k)^T+R_C4(k)]^-1

式中，H_C4(k)＝[0_1×4 N_1×4]，0_1×4为1×4的零矩阵，

K_C4(k)为k时刻的滤波增益，

为k时刻的量测噪声，

为k时刻磁航向角白噪声；

步骤352，计算k时刻力矩模型/磁传感器子滤波器状态估计值

式中，

为k时刻状态量的估计值；Y_C4(k)＝ψ_m(k)为k时刻磁航向角；

步骤353，计算k时刻力矩模型/磁传感器子滤波器估计均方误差P_C4(k)

P_C4(k)＝[I-K_C4(k)H_C4(k)]P_C(k|k-1)

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

步骤四：根据步骤二的故障定位结果和步骤三的子滤波器滤波结果，执行全局滤波器，对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离，对无故障的子滤波器进行数据融合，得到x轴角速度、y轴角速度、姿态角信息：

情况41，无故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波：

步骤411，根据故障定位结果，隔离故障子滤波器

四个子滤波器均参与全局滤波，无子滤波器被隔离；

步骤412，计算k时刻的全局滤波器的估计均方误差

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

步骤413，计算k时刻的全局滤波器的状态估计值

情况42，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波：

步骤421，根据故障定位结果，隔离故障子滤波器

采用横滚力矩模型故障时的状态方程，隔离横滚力矩模型/x轴陀螺检测滤波器；

步骤422，计算k时刻的全局滤波器的估计均方误差

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

步骤423，计算k时刻的全局滤波器的状态估计值

情况43，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，按照如下步骤进行自滤波器的故障隔离和全局滤波：

步骤431，根据故障定位结果，隔离故障子滤波器

采用俯仰力矩模型故障时的状态方程，隔离俯仰力矩模型/y轴陀螺检测滤波器；

步骤432，计算k时刻的全局滤波器的估计均方误差

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

步骤433，计算k时刻的全局滤波器的状态估计值

情况44，x轴陀螺故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波：

步骤441，根据故障定位结果，隔离故障子滤波器

横滚力矩模型/x轴陀螺子滤波器被隔离，其余三个子滤波器参与全局滤波；

步骤442，计算k时刻的全局滤波器的估计均方误差

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

步骤443，计算k时刻的全局滤波器的状态估计值

情况45，y轴陀螺故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤451，根据故障定位结果，隔离故障子滤波器

俯仰力矩模型/y轴陀螺子滤波器被隔离，其余三个子滤波器参与全局滤波；

步骤452，计算k时刻的全局滤波器的估计均方误差

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

步骤453，计算k时刻的全局滤波器的状态估计值

情况46，横滚力矩模型故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波：

步骤461，根据故障定位结果，隔离故障子滤波器

采用横滚力矩模型故障时的状态方程，无子滤波器被隔离，其余四个子滤波器参与全局滤波；

步骤462，计算k时刻的全局滤波器的估计均方误差

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

步骤463，计算k时刻的全局滤波器的状态估计值

情况47，俯仰力矩模型故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤471，根据故障定位结果，隔离故障子滤波器

采用俯仰力矩模型故障时的状态方程，无子滤波器被隔离，其余四个子滤波器参与全局滤波；

步骤472，计算k时刻的全局滤波器的估计均方误差

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

步骤473，计算k时刻的全局滤波器的状态估计值

步骤五：根据全局滤波器结果，对各个子滤波器、故障检测滤波器状态量、误差估计进行重置，执行系统重置策略：

步骤51，根据故障检测结果及全局滤波结果对各个子滤波器进行状态量及均方误差重置

情况511，无故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

情况512，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况513，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况514，x轴陀螺故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况515，y轴陀螺故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况516，横滚力矩模型故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

情况517，俯仰力矩模型故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

步骤52，根据故障检测结果及全局滤波结果对各个检测滤波器进行状态量及均方误差重置

检测滤波器为n步预测卡方检测器，设置n步重置周期，在n步之内不进行重置，使用自身的状态估计结果进行状态递推，在第n步时使用全局滤波结果进行状态量重置，状态及均方误差重置根据滤波结果采用如下方式：

情况521，无故障时，第n步时，检测滤波器状态及均方误差重置为如下：

式中，

表示使用状态向量中的第1列中，第i到第j行元素进行重置；P_g(k)[i:j；i:j]表示使用误差估计矩阵中的第i到第j列、第i到第j行元素进行重置；

情况522，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，检测滤波器状态及均方误差重置为如下：

横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器不进行重置更新，只进行累积直至确定故障位置；俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器采用n步重置规则，采用如下方法进行重置：

情况523，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，检测滤波器状态及均方误差重置为如下：

俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器不进行重置更新，只进行累积直至确定故障位置；横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器采用n步重置规则，采用如下方法进行重置：

情况524，x轴陀螺故障时，检测滤波器状态及均方误差重置为如下：

当检测到x轴陀螺故障时，对横滚力矩模型/x轴陀螺检测滤波器、x轴陀螺/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况525，y轴陀螺故障时，检测滤波器状态及均方误差重置为如下：

当检测到y轴陀螺故障时，对俯仰力矩模型/y轴陀螺检测滤波器、y轴陀螺/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况526，横滚力矩模型故障时，检测滤波器状态及均方误差重置

当检测到横滚力矩模型故障时，对横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况527，俯仰力矩模型故障时，检测滤波器状态及均方误差重置

当检测到俯仰力矩模型故障时，对俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

步骤53，当检测到故障后，基于故障特性，执行以下系统重置策略

设置x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障隔离周期，当检测到x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障后，进入故障隔离周期，在隔离周期内认为一直存在故障，在隔离周期内若再次检测到故障，重新开始隔离周期。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，其特征在于包括如下步骤：

步骤一：周期读取k时刻四旋翼飞行器机载传感器信息，包括旋翼转速传感器信息ω₁(k)、ω₂(k)、ω₃(k)、ω₄(k)，其分别为四个旋翼的转速；GPS信息V_NG(k)、V_EG(k)、V_DG(k)，其分别为北向速度、东向速度、地向速度；磁传感器信息ψ_m(k)；陀螺信息

其分别为机体系相对于导航系的角速度在机体系x、y、z轴上的分量；加计信息

分别为机体系相对于导航系的加速度在机体系x、y、z轴上的分量；

步骤二：执行故障检测滤波器及故障定位策略，判断x轴陀螺、y轴陀螺、横滚力矩模型、俯仰力矩模型的故障；

步骤三：根据步骤二的故障定位结果，确定各个子滤波器的状态方程，进行力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器的数据融合；

步骤四：根据故障检测结果，执行全局滤波器，对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离，对无故障的子滤波器进行数据融合，得到x轴角速度、y轴角速度、横滚角、俯仰角信息；

步骤五：根据全局滤波器结果，对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新，并执行系统重置策略。

2.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，其特征在于：所述步骤二中，故障检测滤波器由横滚力矩模型/x轴陀螺检测滤波器、俯仰力矩模型/y轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器组成，上述6个检测滤波器的状态更新、故障定位步骤如下：

步骤21，横滚力矩模型/x轴陀螺检测滤波器故障检测步骤如下：

步骤211，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的状态估计值及估计均方误差

k_x0(k)＝k_x0(k-1)

式中，k_x0(k)为k时刻横滚力矩模型零偏系数；k_x0(k-1)为k-1时刻横滚力矩模型零偏系数；k_x1、k_x2为横滚力矩模型系数，为常值；ω_mx(k)为k时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量；ω_mx(k-1)为k-1时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量；

为k时刻机体系相对于导航系的加速度在机体系y轴上的分量，通过加计输出获得；ω_i(k)为k时刻第i个旋翼的转速，i＝1,2,3,4；ΔT为离散采样周期；上标T表示转置；

为k时刻的估计均方误差；

为k-1时刻的估计均方误差；

为雅可比矩阵；G_d1(k-1)＝[I_2×2]，为系统噪声系数，I_2×2为2×2的单位矩阵；W_d1(k-1)＝[w_kx0(k-1) w_mx(k-1)]^T，为状态噪声，w_kx0(k-1)为k-1时刻横滚力矩模型零偏白噪声；w_mx(k-1)为k-1时刻横滚力矩模型的白噪声；

步骤212，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的故障统计参数

式中，

为k时刻横滚力矩模型/x轴陀螺检测滤波器的统计参数；

为k时刻的残差；

为k时刻机体系相对于导航系的角速度在机体系x轴上的分量，通过x轴陀螺输出获得；

为k时刻的残差方差；H_d1(k)＝[0 1]；R_d1(k)＝diag([w_gx(k)]²)，w_gx为机体系x轴陀螺的白噪声；

步骤213，根据故障统计参数结果，计算k时刻横滚力矩模型/x轴陀螺检测滤波器的检测函数，判断横滚力矩模型和x轴陀螺是否发生故障

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

步骤22，俯仰力矩模型/y轴陀螺检测滤波器故障检测步骤如下：

步骤221，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的状态估计值及估计均方误差

k_y0(k)＝k_y0(k-1)

式中，k_y0(k)为k时刻俯仰力矩模型零偏系数；k_y0(k-1)为k-1时刻俯仰力矩模型零偏系数；k_y1、k_y2为俯仰力矩模型系数；ω_my(k)为k时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量；ω_my(k-1)为k-1时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量；

为雅可比矩阵；G_d2(k-1)＝[I_2×2]，为系统噪声系数；W_d2(k-1)＝[w_ky0(k-1) w_my(k-1)]^T，为状态噪声，w_ky0(k-1)为k-1时刻俯仰力矩模型零偏白噪声，w_my(k-1)为k-1时刻俯仰力矩模型的白噪声；

步骤222，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的故障统计参数

式中，

为k时刻俯仰力矩模型/y轴陀螺检测器的统计参数；

为k时刻的残差；

为k时刻机体系相对于导航系的角速度在机体系y轴上的分量，通过陀螺输出获得；

为k时刻的残差方差；H_d2(k)＝[0 1]；R_d2(k)＝diag([w_gy(k)]²)，w_gy为机体系y轴陀螺的白噪声；

步骤223，根据故障统计参数结果，计算k时刻俯仰力矩模型/y轴陀螺检测滤波器的检测函数，判断俯仰力矩模型和y轴陀螺是否发生故障

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

步骤23，横滚力矩模型/加速度计/GPS检测滤波器故障检测步骤如下：

步骤231，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差

k_x0(k)＝k_x0(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时刻四元素；

为机体系相对于导航系的角速度在机体系z轴上的分量，通过z轴陀螺输出获得；

为雅可比矩阵；

为k-1时刻系统噪声系数；I_2×2为2×2的单位矩阵；0_2×3为2×3的零矩阵；0_4×2为4×2的零矩阵；W_d3(k-1)＝[w_kx0(k-1) w_mx(k-1) w_ωx(k-1) w_ωy(k-1) w_gz(k-1)]^T，为k-1时刻状态噪声，w_ωx(k-1)为k-1时刻机体系_x轴角速度白噪声；w_gz(k-1)为k-1时刻机体系z轴陀螺白噪声；

步骤232，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻横滚力矩模型/加速度计/GPS检测滤波器的统计参数；

为k时刻的残差；

V_NG(k)、V_EG(k)、V_DG(k)分别为导航系下北向、东向、地向速度，通过GPS获得，

为k-1时刻系统滤波四元素估计；

分别为k时刻机体系相对于导航系的线速度在机体系y轴的分量；

g为重力加速度；

为k时刻的残差方差，H_d3(k)＝[0_1×2 Ω_1×4 ^d3],Ω_1×4 ^d3＝[2gq₁(k) 2gq₀(k) 2gq₃(k) 2gq₂(k)]，

为加速度计和GPS的组合白噪声在机体系y轴上的分量；

步骤233，根据故障统计参数结果，计算k时刻横滚力矩模型/加速度计/GPS检测滤波器的检测函数，判断横滚力矩模型是否发生故障

式中，T₃是阈值，当J₃(k)＝1时，横滚力矩模型故障；当J₃(k)＝0时，横滚力矩模型无故障；

步骤24，俯仰力矩模型/加速度计/GPS检测滤波器故障检测步骤如下：

步骤241，计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的状态估计值及估计均方误差

k_y0(k)＝k_y0(k-1)

式中，

为雅可比矩阵；G_d4(k-1)＝G_d3(k-1)，为系统噪声系数矩阵；W_d4(k-1)＝W_d3(k-1)，为状态噪声；

步骤242，计算k时刻俯仰力矩模型/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻俯仰力矩模型/加速度计/GPS检测滤波器的统计参数；

为k时刻的残差；

为k时刻的残差方差；H_d4(k)＝[0_1×2 Ω_1×4 ^d4]；

Ω_1×4 ^d4＝[-2gq₂(k) 2gq₃(k) -2gq₀(k) 2gq₁(k)]；

为加速度计和GPS的组合白噪声在机体系x轴上的分量；

步骤243，根据故障统计参数结果，计算k时刻检测滤波器的检测函数，判断俯仰力矩模型是否发生故障

式中，T₄是阈值，当J₄(k)＝1时，俯仰力矩模型故障；当J₄(k)＝0时，俯仰力矩模型无故障；

步骤25，x轴陀螺/加速度计/GPS检测滤波器检测步骤如下：

步骤251，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差

式中，

为雅可比矩阵；

为系统噪声系数；W_d5(k-1)＝[w_gx(k-1) w_gy(k-1) w_gz(k-1)]^T，为k-1时刻状态噪声；

步骤252，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻x轴陀螺/加速度计/GPS检测滤波器的统计参数，

为k时刻的残差，

为k时刻的残差方差，H_d5(k)＝Ω_1×4 ^d5，Ω_1×4 ^d5＝Ω_1×4 ^d3，

步骤253，根据故障统计参数结果，计算k时刻x轴陀螺/加速度计/GPS检测滤波器的检测函数，判断x轴陀螺是否发生故障

式中，T₅是阈值，当J₅(k)＝1时，x轴陀螺故障；当J₅(k)＝0时，无故障；

步骤26，y轴陀螺/加速度计/GPS检测滤波器检测步骤如下：

步骤261，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的状态估计值及估计均方误差

式中，Φ_d6(k)＝Φ_d5(k)，为雅可比矩阵；G_d6(k-1)＝G_d5(k-1)，为系统噪声系数；W_d6(k-1)＝W_d5(k-1)，为状态噪声；

步骤262，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的故障统计参数

式中，

为k时刻y轴陀螺/加速度计/GPS检测滤波器的统计参数，

为k时刻的残差，

为k时刻的残差方差，H_d6(k)＝Ω_1×4 ^d6，

Ω_1×4 ^d6＝Ω_1×4 ^d4；

步骤263，根据故障统计参数结果，计算k时刻y轴陀螺/加速度计/GPS检测滤波器的检测函数，判断y轴陀螺故障是否发生故障

式中，T₆是阈值，当J₆(k)＝1时，y轴陀螺故障；当J₆(k)＝0时，无故障；

步骤27，根据各个检测滤波器的检测函数结果，计算k时刻故障定位函数及根据故障定位函数计算结果，进行故障定位，步骤如下：

步骤271，计算k时刻预发生故障定位函数，判断系统是否进入预发生故障：

式中，“∨”表示逻辑运算中“或”运算符，“∧”表示逻辑运算中“与”运算符；

当F_pre(k)＝1时，进入预发生故障阶段，当F_pre(k)＝0时，未进入预发生故障；

步骤272，计算k时刻x轴陀螺故障定位函数，判断x轴陀螺是否发生故障：

当F_GX(k)＝1时，x轴陀螺故障；当F_GX(k)＝0时，x轴陀螺无故障；

步骤273，计算k时刻y轴陀螺故障定位函数，判断y轴陀螺是否发生故障：

当F_GY(k)＝1时，y轴陀螺故障；当F_GY(k)＝0时，y轴陀螺无故障；

步骤274，计算横滚力矩模型故障定位函数，判断横滚力矩模型是否发生故障：

当F_MX(k)＝1时，横滚力矩模型故障；当F_MX(k)＝0时，横滚力矩模型无故障；

步骤275，计算俯仰力矩模型故障定位函数，判断俯仰力矩模型是否发生故障：

当F_MY(k)＝1时，俯仰力矩模型故障；当F_MY(k)＝0时，俯仰力矩模型无故障；

步骤276，计算无故障定位函数，判断系统是否发生故障：

当F_no(k)＝1时，无故障。

3.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，其特征在于：所述步骤三中，根据故障定位结果，确定子滤波器的状态方程，进行子滤波器的状态递推，按照如下步骤执行四个子滤波器：

步骤31，计算k时刻四个子滤波器的状态预测及预测均方误差

情况311，无故障或x轴陀螺或y轴陀螺故障时，四个子滤波器均按照如下步骤进行状态预测及均方误差预测：

步骤3111，计算k时刻横滚力矩模型零偏及俯仰力矩模型零偏预测

k_x0(k|k-1)＝k_x0(k-1)

k_y0(k|k-1)＝k_y0(k-1)

式中，k_x0(k|k-1)、k_y0(k|k-1)分别为横滚力矩模型、俯仰力矩模型零偏项在k-1时刻到k时刻的状态一步预测，k_x0(k-1)为k-1时刻横滚力矩模型零偏系数，k_y0(k-1)为k-1时刻俯仰力矩模型零偏系数；

步骤3112，计算k时刻x轴角速度预测及y轴角速度预测

式中，ω_mx(k|k-1)、ω_my(k|k-1)分别为x、y轴角速度在k-1时刻到k时刻的状态一步预测，ω_mx(k-1)为k-1时刻通过横滚力矩模型计算得到的机体系相对于导航系的角速度在机体系x轴上的分量，ω_my(k-1)为k-1时刻通过俯仰力矩模型计算得到的机体系相对于导航系的角速度在机体系y轴上的分量；

步骤3113，计算k时刻四元素预测

式中，q₀(k|k-1)、q₁(k|k-1)、q₂(k|k-1)、q₃(k|k-1)为四元素在k-1时刻到k时刻的状态一步预测，q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为k-1时刻四元素，ΔT为离散采样周期；

步骤3114，计算一步预测均方误差P_C(k|k-1)

P_C(k|k-1)＝Φ_C(k|k-1)P_C(k-1)Φ_C(k|k-1)^T+G_C(k-1)Q_C(k-1)G_C(k-1)^T

式中，P_C(k|k-1)为k-1到k时刻的一步预测均方误差；P_C(k-1)为k-1时刻估计均方误差；

为雅可比矩阵，

为非线性状态方程，

为k-1时刻状态量，

W_C(k-1)＝[w_kx0(k-1) w_ky0(k-1) w_mx(k-1) w_my(k-1) w_ωx(k-1) w_ωy(k-1) w_gz(k-1)]^T为k-1时刻的状态噪声，w_kx0(k-1)为k-1时刻横滚力矩模型零偏白噪声，w_ky0(k-1)为k-1时刻俯仰力矩模型零偏白噪声，w_mx(k-1)为k-1时刻横滚力矩模型的白噪声，w_my(k-1)为k-1时刻俯仰力矩模型的白噪声，w_ωx(k-1)为k-1时刻机体系_x轴角速度白噪声，w_gz(k-1)为k-1时刻机体系z轴陀螺白噪声；Q_C(k-1)＝diag(W_C(k-1)²)，

情况312，横滚力矩模型/x轴陀螺检测滤波器预发生故障或横滚力矩模型故障时，四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测:

3个子滤波器的状态方程中关于x轴角速度的预测方程修改为ω_mx(k|k-1)＝ω_mx(k-1)，雅可比矩阵修改为：

除了上述3个子滤波器之外的力矩模型/x轴陀螺子滤波器与无故障情况相同，按照步骤3111～步骤3114进行预测；

情况313，俯仰力矩模型/y轴陀螺检测滤波器预发生故障或俯仰力矩模型故障时，四个子滤波器状态预测均按照如下步骤进行状态预测及均方误差预测：

3个子滤波器的状态方程中关于y轴角速度的预测方程修改为ω_my(k|k-1)＝ω_my(k-1)，雅可比矩阵修改为：

除了上述3个子滤波器之外的力矩模型/y轴陀螺子滤波器与无故障情况相同，按照步骤3111～步骤3114进行预测；

步骤32，力矩模型/x轴陀螺子滤波器量测更新步骤如下：

步骤321，计算k时刻的力矩模型/x轴陀螺子滤波器的滤波增益

K_C1(k)＝P_C(k|k-1)H_C1(k)^T[H_C1(k)P_C(k|k-1)H_C1(k)^T+R_C1(k)]^-1

式中，H_C1(k)＝[0_1×2 1 0_1×5]^T，0_1×2为1×2的零矩阵；0_1×5为1×5的零矩阵；K_C1(k)为k时刻的滤波增益，R_C1(k)＝diag([w_gx(k)]²)，为k时刻的量测噪声；

步骤322，计算k时刻力矩模型/x轴陀螺子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

步骤323，计算k时刻力矩模型/x轴陀螺子滤波器估计均方误差P_C1(k)

P_C1(k)＝[I-K_C1(k)H_C1(k)]P_C(k|k-1)

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

步骤33，力矩模型/y轴陀螺子滤波器量测更新步骤如下：

步骤331，计算k时刻的力矩模型/y轴陀螺子滤波器的滤波增益

K_C2(k)＝P_C(k|k-1)H_C2(k)^T[H_C2(k)P_C(k|k-1)H_C2(k)^T+R_C2(k)]^-1

式中，H_C2(k)＝[0_1×3 1 0_1×4]^T，0_1×3为1×3的零矩阵，0_1×4为1×4的零矩阵；K_C2(k)为k时刻的滤波增益，R_C2(k)＝diag([w_gy(k)]²)，为k时刻的量测噪声；

步骤332，计算k时刻力矩模型/y轴陀螺子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

步骤333，计算k时刻力矩模型/y轴陀螺子滤波器估计均方误差P_C2(k)

P_C2(k)＝[I-K_C2(k)H_C2(k)]P_C(k|k-1)

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

步骤34，力矩模型/加速度计/GPS子滤波器量测更新步骤如下：

步骤341，计算k时刻的力矩模型/加速度计/GPS子滤波器的滤波增益

K_C3(k)＝P_C(k|k-1)H_C3(k)^T[H_C3(k)P_C(k|k-1)H_C3(k)^T+R_C3(k)]^-1

式中，

为1×4的零矩阵；g为重力加速度；K_C3(k)为k时刻的滤波增益，

为k时刻的量测噪声，

为加速度计和GPS的组合白噪声在机体系x轴上的分量，

为加速度计和GPS的组合白噪声在机体系y轴上的分量；

步骤342，计算k时刻力矩模型/加速度计/GPS子滤波器状态估计值

式中，

为k时刻状态量的估计值，

为k时刻的量测量，

为k时刻机体系相对于导航系的加速度在机体系x轴上的分量,

为k时刻机体系相对于导航系的线速度在机体系y轴的分量，

为k时刻机体系相对于导航系的加速度在机体系y轴上的分量，

步骤343，计算k时刻力矩模型/加速度计/GPS子滤波器估计均方误差P_C3(k)：

P_C3(k)＝[I-K_C3(k)H_C3(k)]P_C(k|k-1)

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

步骤35，力矩模型/磁传感器子滤波器更新步骤如下：

步骤351，计算k时刻的力矩模型/磁传感器子滤波器的滤波增益

K_C4(k)＝P_C(k|k-1)H_C4(k)^T[H_C4(k)P_C(k|k-1)H_C4(k)^T+R_C4(k)]^-1

式中，H_C4(k)＝[0_1×4 N_1×4]，0_1×4为1×4的零矩阵，

K_C4(k)为k时刻的滤波增益，

为k时刻的量测噪声，

为k时刻磁航向角白噪声；

步骤352，计算k时刻力矩模型/磁传感器子滤波器状态估计值

式中，

为k时刻状态量的估计值；Y_C4(k)＝ψ_m(k)为k时刻磁航向角；

其中，q₀＝q₀(k|k-1)、q₁＝q₁(k|k-1)、q₂＝q₂(k|k-1)、q₃＝q₃(k|k-1)；

步骤353，计算k时刻力矩模型/磁传感器子滤波器估计均方误差P_C4(k)

P_C4(k)＝[I-K_C4(k)H_C4(k)]P_C(k|k-1)

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

4.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，其特征在于：所述步骤四的具体内容是：根据故障检测结果，执行全局滤波器，对力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器进行故障隔离，对无故障的子滤波器进行数据融合，得到x轴角速度、y轴角速度、横滚角、俯仰角信息：

情况41，无故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤411，根据故障定位结果，隔离故障子滤波器

四个子滤波器均参与全局滤波，无子滤波器被隔离；

步骤412，计算k时刻的全局滤波器的估计均方误差

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

式中，P_C1(k)、P_C2(k)、P_C3(k)、P_C4(k)分别为k时刻力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器估计均方误差；

步骤413，计算k时刻的全局滤波器的状态估计值：

式中，

分别为k时刻力矩模型/x轴陀螺子滤波器、力矩模型/y轴陀螺子滤波器、力矩模型/加速度计/GPS子滤波器、力矩模型/磁传感器子滤波器状态估计值；

情况42，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤421，根据故障定位结果，隔离故障子滤波器

采用横滚力矩模型故障时的状态方程，隔离横滚力矩模型/x轴陀螺检测滤波器；

步骤422，计算k时刻的全局滤波器的估计均方误差

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

步骤423，计算k时刻的全局滤波器的状态估计值

情况43，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤431，根据故障定位结果，隔离故障子滤波器

采用俯仰力矩模型故障时的状态方程，隔离俯仰力矩模型/y轴陀螺检测滤波器；

步骤432，计算k时刻的全局滤波器的估计均方误差

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

步骤433，计算k时刻的全局滤波器的状态估计值

情况44，x轴陀螺故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤441，根据故障定位结果，隔离故障子滤波器

横滚力矩模型/x轴陀螺子滤波器被隔离，其余三个子滤波器参与全局滤波；

步骤442，计算k时刻的全局滤波器的估计均方误差

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

步骤443，计算k时刻的全局滤波器的状态估计值

情况45，y轴陀螺故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波：

步骤451，根据故障定位结果，隔离故障子滤波器

俯仰力矩模型/y轴陀螺子滤波器被隔离，其余三个子滤波器参与全局滤波；

步骤452，计算k时刻的全局滤波器的估计均方误差

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

步骤453，计算k时刻的全局滤波器的状态估计值

情况46，横滚力矩模型故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤461，根据故障定位结果，隔离故障子滤波器

采用横滚力矩模型故障时的状态方程，无子滤波器被隔离，其余四个子滤波器参与全局滤波；

步骤462，计算k时刻的全局滤波器的估计均方误差

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

步骤463，计算k时刻的全局滤波器的状态估计值

情况47，俯仰力矩模型故障时，按照如下步骤进行子滤波器的故障隔离和全局滤波

步骤471，根据故障定位结果，隔离故障子滤波器

采用俯仰力矩模型故障时的状态方程，无子滤波器被隔离，其余四个子滤波器参与全局滤波；

步骤472，计算k时刻的全局滤波器的估计均方误差

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

步骤473，计算k时刻的全局滤波器的状态估计值

5.如权利要求1所述的基于力矩模型辅助的四旋翼横滚角、俯仰角容错估计方法，其特征在于：所述步骤五包含如下内容，根据全局滤波器结果，对各个子滤波器、故障检测滤波器状态量、均方误差进行重置更新，并执行系统重置策略：

步骤51，根据故障检测结果及全局滤波结果对各个子滤波器进行状态量及均方误差重置

情况511，无故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

情况512，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况513，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况514，x轴陀螺故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况515，y轴陀螺故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝3P_g(k)

情况516，横滚力矩模型故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

情况517，俯仰力矩模型故障时，四个子滤波器状态量及均方误差重置

P_C(k)＝4P_g(k)

步骤52，根据故障检测结果及全局滤波结果对各个检测滤波器进行状态量及均方误差重置

检测滤波器为n步预测卡方检测器，设置n步重置周期，在n步之内不进行重置，使用自身的状态估计结果进行状态递推，在第n步时使用全局滤波结果进行状态量重置，状态及均方误差重置根据滤波结果采用如下方式：

情况521，无故障时，第n步时，检测滤波器状态及均方误差重置为如下：

式中，

表示使用状态向量中的第1列中，第i到第j行元素进行重置；P_g(k)[i:j；i:j]表示使用误差估计矩阵中的第i到第j列、第i到第j行元素进行重置；

情况522，横滚力矩模型/x轴陀螺检测滤波器预发生故障时，检测滤波器状态及均方误差重置为如下：

横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器不进行重置更新，只进行累积直至确定故障位置；俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器采用n步重置规则，采用如下方法进行重置：

情况523，俯仰力矩模型/y轴陀螺检测滤波器预发生故障时，检测滤波器状态及均方误差重置为如下：

俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器、y轴陀螺/加速度计/GPS检测滤波器不进行重置更新，只进行累积直至确定故障位置；横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器、x轴陀螺/加速度计/GPS检测滤波器采用n步重置规则，采用如下方法进行重置：

情况524，x轴陀螺故障时，检测滤波器状态及均方误差重置为如下：

当检测到x轴陀螺故障时，对横滚力矩模型/x轴陀螺检测滤波器、x轴陀螺/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况525，y轴陀螺故障时，检测滤波器状态及均方误差重置为如下：

当检测到y轴陀螺故障时，对俯仰力矩模型/y轴陀螺检测滤波器、y轴陀螺/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况526，横滚力矩模型故障时，检测滤波器状态及均方误差重置

当检测到横滚力矩模型故障时，对横滚力矩模型/x轴陀螺检测滤波器、横滚力矩模型/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

情况527，俯仰力矩模型故障时，检测滤波器状态及均方误差重置

当检测到俯仰力矩模型故障时，对俯仰力矩模型/y轴陀螺检测滤波器、俯仰力矩模型/加速度计/GPS检测滤波器进行立即重置，其他的检测器按照n步重置规则，采用如下方法进行重置：

步骤53，当检测到故障后，基于故障特性，执行以下系统重置策略

设置x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障隔离周期，当检测到x陀螺、y轴、横滚力矩模型、俯仰力矩模型故障后，进入故障隔离周期，在隔离周期内认为一直存在故障，在隔离周期内若再次检测到故障，重新开始隔离周期。

Images