CN115877717A - 一种基于自抗扰控制的飞行器容错控制结构与控制方法 - Google Patents

Abstract

本发明提供了一种基于自抗扰控制的飞行器容错控制结构与控制方法，属于飞行器容错控制技术领域。解决了四旋翼无人飞行器由于执行机构故障和传感器测量噪声导致的飞行不稳定或飞行事故的问题。其技术方案为：控制结构包括带执行机构故障描述的动力学模型、改进卡尔曼滤波模型、观测模型以及总控制模型；控制方法为：通过设计改进卡尔曼滤波模型用作预滤波级，实现在测量噪声环境下对执行机构故障系数以及飞行状态的估计。本发明的有益效果为：在有测量噪声情况下保证四旋翼无人飞行器能够快速准确地估计故障系数和实现容错控制，确保飞行任务安全和稳定。

Description

一种基于自抗扰控制的飞行器容错控制结构与控制方法

技术领域

本发明涉及飞行***容错控制领域，尤其涉及一种基于自抗扰控制的飞行器容错控制结构与控制方法。

背景技术

近年来，四旋翼无人飞行器由于结构相对简单、易于控制等特点使其在科学研究与生产生活方面有非常广泛的应用。科研人员对其进行了大量的研究，比如：四旋翼状态估计、四旋翼无人飞行器的姿态控制以及四旋翼无人飞行器的轨迹跟踪控制等。随着四旋翼的大量使用，控制方法的安全性和可靠性引起了极大的关注。

及时对故障进行监测能够帮助飞行器及时调整控制结构和参数，避免飞行器发生故障时造成人员受伤和财产损失。为了能及时发现四旋翼无人机所发生的故障并且保证故障发生后四旋翼无人飞行器的安全性与可靠性，相关专家在四旋翼无人飞行器的故障诊断与容错控制等领域做了大量研究，是飞行控制方向的热点问题。

目前国内部分四旋翼无人飞行器的飞控采用自抗扰控制算法，其优点在于可以有效估计模型的不确定性和外部扰动，调参过程简单且具有较好的飞行效果。自抗扰控制通过扩张状态观测器实现扰动的精确估计。但是，基于观测器的控制方法对测量噪声敏感，扩张状态观测器的带宽会因为测量噪声的大小而受到限制，从而导致扰动估计不准确。因此，如何提供一种对测量噪声不敏感的四旋翼无人飞行器容错控制方法是基于自抗扰控制的飞行控制器亟需解决的问题。

发明内容

本发明提供一种基于自抗扰控制的飞行器容错控制结构与控制方法，旨在对飞行器进行故障诊断与容错控制，并提升飞行控制器在噪声环境下的抗扰能力。

为了实现上述发明目的，本发明采用技术方案具体为：一种基于自抗扰控制的飞行器容错控制结构，包括：

根据所述飞行器构建带执行机构故障描述的动力学模型、改进卡尔曼滤波模型、观测模型以及总控制模型；

根据带执行机构故障描述的四旋翼无人飞行器动力学模型搭建改进卡尔曼滤波模型，改进卡尔曼滤波模型用作预滤波级，计算观测模型中需要使用的状态信号，快速准确地估计出故障系数与***状态；

所述改进卡尔曼滤波模型估计出的故障系数，输入到所述观测模型中，观测器对当前时刻改进卡尔曼滤波模型估计出的故障进行观测，并根据观测结果输出扰动估计值；

用所述改进卡尔曼滤波模型估计出的***状态

和故障系数b_k替换传统扩张状态观测器中的实际输出值和控制输入增益，以此提高观测模型在执行机构故障和测量噪声环境中的估计精度；

根据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

以及观测模型观测输出设计所述总控制模型，所述总控制模型有六个回路：X、Y、Z三个位置回路以及俯仰角、偏航角和横滚角三个姿态回路，对每个回路都采用自抗扰控制器，根据故障系数b_k实时调整自抗扰控制器中的参数b₀；

所述总控制模型根据观测模型输出、改进卡尔曼滤波模型输出的故障系数估计值b_k和***状态估计值

计算出所述飞行控制指令U，并输入给所述带执行机构故障描述的四旋翼无人飞行器动力学模型以实现设定的飞行任务，保证在发生故障后及时进行容错控制，保证四旋翼无人飞行器的稳定飞行。

为了更好地实现上述发明目的，本发明还提供了一种基于自抗扰控制的飞行器容错控制结构的控制方法，具体包括如下步骤：

步骤S1：对于四旋翼无人飞行器的位置运动和姿态运动***，根据牛顿-欧拉公式，假设四旋翼无人飞行器处于悬停状态且处在无偏航状态，则可以得到其简化的动力学模型为：

公式(1)中，X、Y、Z是飞行器质心在地球框架下的坐标，J₁为沿Y方向的转动惯量，J₂为沿X方向的转动惯量，J₃为沿Z方向的转动惯量，m为飞行器的机身质量；

根据所述简化的动力学模型，构建离散线性时变状态空间方程来描述四旋翼无人飞行器***：

公式(2)中，x_k、u_k、y_k分别为状态变量、控制输入变量、输出变量，A_k、B_k、C_k则为对应的可调系数矩阵，

和v_k+1为互不相关的高斯白噪声；

将执行器故障建模为控制性能损失及故障系数：

公式(3)中，b_k即为故障系数构成的矩阵，当b_ik＝0或b_ik＝1时，代表第i个执行器完全没问题或者发生故障导致完全损坏，为了便于参数估计，上式的状态空间方程可以改写成：

公式(4)中，U_k＝diag[u_1k u_2k u_3k u_4k]；

步骤S2：根据带执行机构故障描述的动力学模型构建改进卡尔曼滤波模型，相比普通卡尔曼滤波模型，所述改进卡尔曼滤波模型采用两级卡尔曼滤波算法，作为预滤波级，用于估计执行机构故障系数及飞行器状态信息；

卡尔曼滤波方程分为两类：时间更新方程与测量更新方程，时间更新方程在时域中将状态和误差协方差提前一步以此来获得先验估计，测量更新方程则将测量反馈到先验估计中来获得后验估计，整个预测校正过程用来估计尽可能接近其真实值的状态，因此构建所述改进卡尔曼滤波模型具体步骤包括：

步骤S21：改进卡尔曼滤波模型为两级卡尔曼滤波模型。在卡尔曼滤波模型的基础上，忽略偏置项，构建一种无偏置滤波器，具体公式如下所示：

公式(5)中，

为忽略偏置时第k时刻的状态估计值，/>

为忽略偏置时第k时刻预测k+1时刻的状态估计值，

为忽略偏置时第k+1时刻的状态估计值，V_k+1|k为第k时刻预测k+1时刻的偏置补偿系数并由耦合方程计算得出，/>

为滤波增益，/>

为第k时刻的故障系数估计值；/>

为忽略偏置时第k时刻预测k+1时刻的误差协方差，/>

为忽略偏置时第k+1时刻的误差协方差；

步骤S22：进一步构造偏置滤波器，计算剩余状态以及无偏置滤波器的输出并重构原始***状态，偏置滤波器的公式如下所示：

公式(6)中，

为第k+1时刻的补偿滤波增益，/>

为第k时刻预测k+1时刻的补偿滤波协方差，H_k+1|k为当前状态到测量的转换矩阵并由耦合方程所计算得到，/>

为第k+1时刻的补偿滤波协方差，R_k+1为噪声协方差，/>

为第k+1时刻的故障系数估计值；

步骤S23：根据所述无偏置滤波器和所述偏置滤波器进行线性组合得到最优状态估计，原始状态估计值和误差协方差矩阵可以通过补偿得到，公式如下所示：

公式(7)中，

为第k+1时刻的状态估计值，V_k+1|k+1为第k+1时刻的偏置补偿系数并由耦合方程计算得出，P_k+1|k+1为第k+1时刻的误差协方差，/>

为第k+1时刻的故障系数估计值，/>

为忽略偏置时第k+1时刻的误差协方差；

步骤S3：所述观测模型根据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

估计总扰动，观测模型如下式所示：

公式(8)中，b_k为改进卡尔曼滤波模型所估计出的故障系数，

为所述改进卡尔曼滤波模型估计出的***状态，β₁为所述观测模型的可调观测增益一、β₂为所述观测模型的可调观测增益二、β₃为所述观测模型的可调观测增益三，z₁为对/>

的观测值，z₂为对/>

导数的观测值，z₃为对总扰动的观测值；

所述观测模型与传统扩张状态观测器的区别在于用所述改进卡尔曼滤波模型估计出的***状态

和故障系数b_k替换传统扩张状态观测器中的实际输出值和控制输入增益，以此提高观测模型在执行机构故障和测量噪声环境中的估计精度；

步骤S4：据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

以及观测模型观测出的z₁，z₂和z₃设计所述总控制模型，所述总控制模型有六个回路：X、Y、Z三个位置回路以及俯仰角、偏航角和横滚角三个姿态回路，对每个回路都采用自抗扰控制器，根据故障系数b_k实时调整自抗扰控制器中的参数b₀；

步骤S5：根据观测模型输出的z₁，z₂和z₃，以及改进卡尔曼滤波模型估计出的故障系数b_k计算出飞行控制指令U，具体公式如下所示：

公式(9)中，u₀为总扰动补偿前的飞行控制指令，v为设定的飞行轨迹信号，k_p为比例系数，k_d为微分系数，k_p、k_d分别可以由以下算式计算而出：k_p＝ω_c ²，k_d＝2ω_c；

飞行控制指令U输入给带执行机构故障描述的四旋翼无人飞行器动力学模型以实现设定的飞行任务，保证在发生故障后及时进行容错控制，保证四旋翼无人飞行器的稳定飞行。

与现有技术相比，本发明的有益效果为：当无人机执行机构存在故障且***存在测量噪声时，本发明所提供的控制结构与控制方法利用改进卡尔曼滤波模型可准确地估计执行机构的故障系数和无人机当前飞行状态，降低故障和测量噪声对于观测模型的影响。进一步地，根据故障系数和估计的当前飞行状态，观测模型和总控制模型可以精确估计***的总扰动并实时补偿，确保飞行器在执行机构发生故障时能实现稳定飞行，同时仍能有效抵抗外部扰动。

附图说明

为了更加清楚的说明本申请实施中的方法，也为了方便理解，下面将对本实施中的一些附图作一些简单的介绍，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

图1是本发明实施例1提供的一种四旋翼无人飞行器容错控制结构框图；

图2是本发明实施例1提供的一种四旋翼无人飞行器容错控制方法的流程示意图；

图3是本发明实施例1提供的一种四旋翼无人飞行器容错控制方法中的改进卡尔曼滤波模型结构性框图；

图4是本发明实施例1提供的一种四旋翼无人飞行器容错控制方法中的总控制模型结构框图；

图5是本发明实施例2提供的改进卡尔曼滤波模型故障检测结果示意图；

图6是本发明实施例3提供的四旋翼无人飞行器轨迹跟踪效果示意图。

其中，附图标记为：11、动力学模型；12、改进卡尔曼滤波模型；13、观测模型；14、总控制模型。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

附图中所示的流程图仅是示例说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作或者步骤还可以分解、组合或部分合并，因此实际执行的顺序有可能根据实际情况改变。

应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。下面结合附图，对本申请的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互组合。

实施例1

本实施例的提供了一种四旋翼无人飞行器容错控制结构与控制方法，本结构与方法旨在及时检测飞行器故障并进行容错控制，保证飞行器能够保持相对稳定的飞行，提高飞行器的安全性与可靠性。

请参阅图1，图1是本发明实施例1提供的一种四旋翼无人飞行器容错控制结构框图；

如图1所示，根据四旋翼无人飞行器构建带执行机构故障描述的动力学模型11、改进卡尔曼滤波模型12、观测模型13、及总控制模型14；

所述改进卡尔曼滤波模型12根据带执行机构故障描述的动力学模型11搭建而成，用作预滤波级，计算观测模型13中需要使用的状态信号。四旋翼无人飞行器GPS中得到的飞行器位置信号、飞控陀螺仪中得到的飞行器三个姿态角的θ、φ、ψ信号作为改进卡尔曼滤波模型12的输入。改进卡尔曼滤波模型12估计出故障系数b_k与***状态

并输入到观测模型13，***状态/>

替换观测模型13中原本的实际测量信号，观测模型13根据观测结果输出z₁，z₂和z₃。根据改进卡尔曼滤波算法估计出的故障系数b_k与观测模型13的输出设计反馈控制器，并计算出飞行控制指令U输入给带执行机构故障描述的四旋翼无人飞行器动力学模型11以实现设定的飞行任务，保证在发生故障后及时进行容错控制，保证四旋翼无人飞行器的稳定飞行。

如图2所示，图2是本发明实施例1提供的一种四旋翼无人飞行器容错控制方法的流程示意图；

所述四旋翼无人飞行器容错控制方法具体包括步骤S1至步骤S5。

根据所述四旋翼无人飞行器构建带执行机构故障描述的动力学模型、改进卡尔曼滤波模型、观测模型及总控制模型；

步骤S1：具体地，对于四旋翼无人飞行器的位置运动和姿态运动***，根据牛顿-欧拉公式，假设四旋翼无人飞行器处于悬停状态且无处在无偏航状态，则可以得到其简化的动力学模型为：

公式(1)中，X、Y、Z是飞行器质心在地球框架下的坐标，J₁为沿Y方向的转动惯量，J₂为沿X方向的转动惯量，J₃为沿Z方向的转动惯量，m为飞行器的机身质量；

根据所述简化的动力学模型，构建离散线性时变状态空间方程来描述四旋翼无人飞行器***：

上式(2)中，x_k、u_k、y_k分别为状态变量、控制输入变量、输出变量，A_k、B_k、C_k则为对应的可调系数矩阵，

和v_k+1为互不相关的高斯白噪声；

将执行器故障建模为控制性能损失及故障系数：

公式(3)中，b_k即为故障系数构成的矩阵，当b_ik＝0或b_ik＝1时，代表第i个执行器完全没问题或者发生故障导致完全损坏，为了便于参数估计，可以很明显表示四旋翼无人飞行器的执行器故障情况，上式的状态空间方程可以改写成：

公式(4)中，U_k＝diag[u_1k u_2k u_3k u_4k]，

和v_k+1为互不相关的高斯白噪声；/>

如图3所示，构建改进卡尔曼滤波模型，包括：

步骤S2：根据带执行机构故障描述的动力学模型构建改进卡尔曼滤波模型，相比普通卡尔曼滤波模型，所述改进卡尔曼滤波模型采用两级卡尔曼滤波算法，作为预滤波级，用于估计执行机构故障系数及飞行器状态信息；

卡尔曼滤波方程分为两类：时间更新方程与测量更新方程，时间更新方程在时域中将状态和误差协方差提前一步以此来获得先验估计，测量更新方程则将测量反馈到先验估计中来获得后验估计，整个预测校正过程用来估计尽可能接近其真实值的状态，步骤S2具体包括：

步骤S21：改进卡尔曼滤波模型为两级卡尔曼滤波模型。在卡尔曼滤波模型的基础上，忽略偏置项，构建一种无偏置滤波器，具体公式如下所示：

公式(5)中，

为忽略偏置时第k时刻的状态估计值，/>

为忽略偏置时第k时刻预测k+1时刻的状态估计值，

为忽略偏置时第k+1时刻的状态估计值，V_k+1|k为第k时刻预测k+1时刻的偏置补偿系数并由耦合方程计算得出，/>

为滤波增益，/>

为第k时刻的故障系数估计值；/>

为忽略偏置时第k时刻预测k+1时刻的误差协方差，/>

为忽略偏置时第k+1时刻的误差协方差；

步骤S22：构造偏置滤波器，产生剩余状态以及无偏置滤波器的输出并重构原始***状态，偏置滤波器公式如下所示：

公式(6)中，

为第k+1时刻的补偿滤波增益，/>

为第k时刻预测k+1时刻的补偿滤波协方差，/>

为第k+1时刻的补偿滤波协方差，H_k+1|k为当前状态到测量的转换矩阵并由耦合方程所计算得到，R_k+1为噪声协方差，/>

为第k+1时刻的故障系数估计值；

步骤S23：根据所述无偏置滤波器和所述偏置滤波器进行线性组合以此得到最优状态估计，原始状态估计值和误差协方差矩阵可以通过补偿得到，公式如下所示：

公式(7)中，

为第k+1时刻的状态估计值，V_k+1|k+1为第k+1时刻的偏置补偿系数并由耦合方程计算得出，P_k+1|k+1为第k+1时刻的误差协方差，/>

为第k+1时刻的故障系数估计值，/>

为忽略偏置时第k+1时刻的误差协方差；

步骤S3：所述观测模型根据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

估计总扰动，观测模型如下式所示：

公式(8)中，b_k为改进卡尔曼滤波模型所估计出的故障系数，

为所述改进卡尔曼滤波模型估计出的***状态，β₁为所述观测模型的可调观测增益一、β₂为所述观测模型的可调观测增益二、β₃为所述观测模型的可调观测增益三，z₁为/>

的观测值，z₂为/>

导数的观测值，z₃为总扰动的观测值；

所述观测模型与传统扩张状态观测器的区别在于用所述改进卡尔曼滤波模型估计出的***状态

和故障系数b_k替换传统扩张状态观测器中的实际输出值和控制输入增益，以此提高观测模型在执行机构故障和测量噪声环境中的估计精度；

如图4所示，构建总控制模型，具体步骤包括：

步骤S4：据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

以及观测模型观测出的z₁，z₂和z₃设计所述总控制模型，所述总控制模型有六个回路：X、Y、Z三个位置回路以及俯仰角、偏航角和横滚角三个姿态回路，对每个回路都采用自抗扰控制器，根据故障系数b_k实时调整自抗扰控制器中的参数b₀；

步骤S5：根据观测模型输出的z₁，z₂和z₃与改进卡尔曼滤波模型估计出的故障系数b_k计算出飞行控制指令U，具体公式如下所示：

公式(9)中，u₀为总扰动补偿前的飞行控制指令，v为设定的飞行轨迹信号，k_p为比例系数，k_d为微分系数，k_p、k_d分别可以由以下算式计算而出：k_p＝ω_c ²，k_d＝2ω_c；

飞行控制指令U输入给带执行机构故障描述的四旋翼无人飞行器动力学模型以实现设定的飞行任务，保证在发生故障后及时进行容错控制，保证四旋翼无人飞行器的稳定飞行。通过上述方法确定总控制模型，提高了总控制模型的响应速度、鲁棒性以及稳定性。

实施例2

如图5所示，为了验证所述改进卡尔曼滤波模型估计出的故障系数b_k的准确性，在仿真实验中设置四个执行机构的故障系数真实值均为b_k＝0.2，采用实施例1所述的改进卡尔曼滤波模型进行估计。由图5可以看出，所述改进卡尔曼滤波模型计算出的四个执行机构故障估计值均值0.2附近，估计出故障系数与设定的真实故障值基本一致，证明所述改进卡尔曼滤波模型能够准确地估计出执行机构故障系数，为下一步观测模型精确估计总扰动做好准备。

实施例3

如图6所示，采用实施例1所述的一种四旋翼无人飞行器容错控制结构与控制方法对带执行机构故障的四旋翼无人飞行器轨迹跟踪任务进行仿真实验。四旋翼无人飞行器三个位置输出：X、Y、Z的参考值均为1，俯仰角、横滚角的参考值为0，偏航角的参考值为1，四旋翼无人飞行器四个执行机构故障系数均设为0.2。根据图6所示，四旋翼无人飞行器在执行机构故障的情况下实现良好的轨迹跟踪效果，在t＝10s时均实现设定值的精确跟踪，说明本实施例1所提供的四旋翼无人飞行器容错控制结构与控制方法具有良好的实际效果。

综上，本实施例提供的一种四旋翼无人飞行器容错控制结构与控制方法通过构建带执行机构故障描述的动力模型、观测模型、改进卡尔曼滤波模型及总控制模型实现飞行器容错控制。具体地，当四旋翼无人飞行器执行机构发生故障后，改进卡尔曼滤波模型快速准确的估计出故障系数大小并输入给观测模型，观测模型根据故障系数精确估计***状态和总扰动。根据改进卡尔曼滤波模型估计出的故障系数调整总控制模型的参数，提升***的稳定性与抗干扰性，保证在故障发生飞行器能够保持相对稳定的飞行，从而达到容错控制的效果。

本实施例本实施例以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内本实施例本实施例本实施例本实施例本实施例本实施例。

Claims (2)

1.一种基于自抗扰控制的飞行器容错控制结构，其特征在于，所述结构包括：

带执行机构故障描述的动力学模型、改进卡尔曼滤波模型、观测模型以及总控制模型；

所述带执行机构故障描述的动力学模型的输入为飞行控制指令U，输出为当前时刻的飞行姿态y；

所述改进卡尔曼滤波模型根据带执行机构故障描述的动力学模型搭建而成，用作预滤波级，计算观测模型中需要使用的状态信号，所述改进卡尔曼滤波模型输入包括：飞行器GPS中得到的飞行器位置信号、飞控陀螺仪中得到的飞行器三个姿态角的θ、φ、ψ信号，所述改进卡尔曼滤波模型的输出为故障系数估计值b_k与***状态估计值

所述观测模型的输入包括：改进卡尔曼滤波模型计算出的故障系数估计值b_k与***状态估计值

所述观测模型的输出包括z₁，z₂和z₃，其中：z₁为/>

的观测值，z₂为/>

导数的观测值，z₃为总扰动的观测值；

所述总控制模型根据所述观测模型的输出z₁，z₂和z₃、所述改进卡尔曼滤波模型输出的故障系数估计值b_k和***状态估计值

计算所述飞行控制指令U，并输入给所述带执行机构故障描述的四旋翼无人飞行器动力学模型以实现设定的飞行任务，保证在发生故障后及时进行容错控制，实现四旋翼无人飞行器的稳定飞行。

2.根据权利要求1所述的基于自抗扰控制的飞行器容错控制结构的控制方法，其特征在于，包括以下步骤：

步骤S1：构建带执行机构故障描述的动力学模型，具体公式如下所示：

公式(1)中，U_k＝diag[u_1k u_2k u_3k u_4k]，x_k、u_k、y_k分别为状态变量、控制输入变量、输出变量，A_k、B_k、C_k则为对应的系数矩阵系数，

和v_k+1为互不相关的高斯白噪声；

步骤S2：根据所述带执行机构故障描述的动力学模型搭建所述改进卡尔曼滤波模型，改进卡尔曼滤波模型采用两级卡尔曼滤波算法，作为预滤波级，用于估计执行机构故障系数及飞行器状态信息；

所述两级卡尔曼滤波算法具体包括以下步骤：

步骤S21：首先，忽略偏置项，构建一种无偏置滤波器，如下公式所示：

公式(2)中：

为忽略偏置时第k时刻的状态估计值，/>

为忽略偏置时第k时刻预测k+1时刻的状态估计值，/>

为第k+1时刻忽略偏置的状态估计值，V_k+1|k为第k时刻预测k+1时刻的偏置补偿系数，由耦合方程计算得出，/>

为滤波增益，/>

为第k时刻的故障系数估计值；

步骤S22：考虑偏置，构造一种偏置滤波器来补偿所述无偏置滤波器，从而重建原始的***状态，如下公式所示：

公式(3)中，

为第k+1时刻的补偿滤波增益，/>

为第k时刻预测k+1时刻的补偿滤波协方差，H_k+1|k为当前状态到测量的转换矩阵，由耦合方程所计算得到，/>

为第k+1时刻的补偿滤波协方差，/>

为忽略偏置时第k时刻预测k+1时刻的误差协方差，R_k+1为噪声协方差；

步骤S23：根据所述无偏置滤波器和所述偏置滤波器进行线性组合得到最优状态估计，原始状态估计值和误差协方差矩阵通过补偿得到，如下公式所示：

公式(4)中，

为第k+1时刻的状态估计值，V_k+1|k+1为第k+1时刻的偏置补偿系数，由耦合方程计算得出，P_k+1|k+1为第k+1时刻的误差协方差，/>

为第k+1时刻的故障系数估计值，/>

为忽略偏置时第k+1时刻的误差协方差；

步骤S3：所述观测模型根据所述改进卡尔曼滤波模型估计出的故障系数b_k与***状态

估计***总扰动，所述观测模型根据如下公式描述：

公式(5)中，b_k为所述改进卡尔曼滤波模型估计的故障系数，

为所述改进卡尔曼滤波模型估计的***状态，β₁为所述观测模型的可调观测增益一、β₂为所述观测模型的可调观测增益二、β₃为所述观测模型的可调观测增益三，z₁为/>

的观测值，z₂为/>

导数的观测值，z₃为总扰动的观测值；

步骤S4：根据所述改进卡尔曼滤波模型估计出的故障系数b_k以及所述观测模型观测出的z₁，z₂和z₃设计所述总控制模型，所述总控制模型有六个回路：X、Y、Z三个位置回路以及俯仰角、偏航角和横滚角三个姿态回路，对每个回路都采用自抗扰控制器，根据故障系数b_k实时调整自抗扰控制器中的参数b₀；

步骤S5：根据观测模型输出的z₁，z₂和z₃与改进卡尔曼滤波模型估计出的故障系数b_k设计所述自抗扰控制器，用于计算所述飞行控制指令U，具体公式如下所示：

公式(6)中，u₀为总扰动补偿前的飞行控制指令，v为设定的飞行轨迹信号，k_p为可调比例系数，k_d为可调微分系数；

所述飞行控制指令U输入给带执行机构故障描述的四旋翼无人飞行器动力学模型以实现设定的飞行任务，保证在发生故障后及时进行容错控制，实现四旋翼无人飞行器的稳定飞行。

Images