CN113568419A - 一种变载四旋翼无人机容错控制方法 - Google Patents

Abstract

本发明公开了一种变载四旋翼无人机容错控制方法，包括：建立变载四旋翼无人机非线性动力学模型；建立自适应质量估计算法和扰动估计算法；建立位置子系统的非奇异快速终端滑模面，设计位置子系统控制器；建立姿态故障模型；根据姿态故障模型建立自适应非线性故障观测器，得到故障估计值；根据故障估计值，在姿态故障模型基础上利用积分滑模建立姿态子系统控制器。本发明通过自适应质量估计机制，精确计算出四旋翼无人机在整体质量变化瞬间的实际值；通过外部扰动自适应机制，有效降低外部扰动对变载飞控系统的影响；利用自适应非线性故障观测器精确地估计故障的时变信息，有效包容未知执行器故障对飞控系统的影响。

Description

一种变载四旋翼无人机容错控制方法

技术领域

本发明涉及飞行器自动控制领域，特别涉及一种变载四旋翼无人机容错控制方法。

背景技术

四旋翼无人机具有结构简单、垂直起降、悬停精准、灵活性高和操作方便的优点，被广泛应用于空间监视、电力巡检、农业植保、环境监测、空中物流、高空救援等领域。变载四旋翼无人机是一类携带负载工作的无人机，时变的负载会大大降低四旋翼飞控系统的稳定性和可靠性；外部干扰如风力的影响，以及飞行过程中高速旋转的电动机带来的突发性执行器故障，都会导致无人机偏离预定轨道，甚至发生坠毁的事故。内在的负载变化和外在的环境影响之间存在一定的耦合关系，给飞控系统的安全设计带来一定的困难。

在无人机执行器故障容错控制方面，主要分为主动和被动两种容错方式。被动容错能力存在局限性，增加四旋翼控制器的冗余；主动容错控制的研究方法较多，但存在不足：如将系统执行器故障程度分段，分别设计不同的补偿控制律，实时故障检测并调用对应的控制律进行补偿故障损耗，但是，调用不同的控制律会降低控制器对突发状况的响应速度，导致跟踪效果变慢；利用神经网络逼近外部扰动上界，结合自适应估计补偿执行器故障，但应对严重突发性故障时，系统的跟踪误差会有较大的超调量。

发明内容

发明目的：针对以上问题，本发明目的是提供一种变载四旋翼无人机容错控制方法，针对无人机在受到内部质量变化、外部扰动和执行器故障情况下仍能保持位置和姿态的稳定。

技术方案：本发明的一种变载四旋翼无人机容错控制方法，包括如下步骤：

(1)建立变载四旋翼无人机在正常运行状态下的非线性动力学模型，包括位置子系统模型和姿态子系统模型；

(2)考虑无人机负载质量变化和外部扰动的影响，分别建立自适应质量估计算法和扰动估计算法；

(3)建立位置子系统的非奇异快速终端滑模面，考虑自适应质量估计算法和扰动估计算法，对位置子系统模型构建虚拟控制量，根据虚拟控制量与无人机姿态角之间的关系，得到位置子系统的位置控制输入，设计位置子系统控制器；

(4)考虑执行器故障对姿态子系统的影响，在姿态子系统模型的基础上建立姿态故障模型；

(5)根据姿态故障模型建立自适应非线性故障观测器，得到故障估计值；

(6)根据故障估计值，在姿态故障模型基础上利用积分滑模建立姿态子系统控制器。

进一步，步骤1包括：

(101)根据牛顿第二定律，建立变载四旋翼无人机的在地面坐标系下的位置平移动力学方程为：

其中，m为变质量四旋翼无人机总体的质量，

为地面坐标系下的x，y，z方向加速度，[x,y,z]为无人机质心在惯性坐标系中的位置坐标，

分别为x方向、y方向和z方向的加速度，F＝[0 0 U₁]^T为机体坐标系下产生的升力矩阵，U₁为位置的控制输入，R为地面坐标系与机体坐标系之间的转换矩阵，d_F＝[d₁ d₂ d₃]^T代表外部扰动，g为重力系数；

(102)基于牛顿-欧拉方法，建立变载四旋翼无人机的在地面坐标系下的姿态旋转动力学方程为：

其中，τ_B为各电机转子提供的扭矩，τ_B＝[U₂I_x U₃I_y U₄I_z]^T，U₂、U₃、U₄为姿态的控制输入，I_x为x轴的转动惯量，I_y为y轴的转动惯量，I_z为z轴的转动惯量；I_B为系统转动惯量，I_B＝diag(I_x,I_y,I_z)；ω_B为机体坐标系下的姿态角速度，ω_B＝[p,q,r]^T，p,q,r分别代表机体坐标系下的横滚角、俯仰角、偏航角的角速度；τ_d为气动摩擦力矩，τ_d＝diag(d_φ,d_θ,d_ψ)ω_E，ω_E为地面坐标系下的姿态角速度，

[φ,θ,ψ]为无人机三个姿态的欧拉角度，分别代表滚转角、俯仰角和偏航角；

(103)结合步骤(101)和步骤(102)，得到变载四旋翼无人机的非线性动力学模型如下：

式中前三项为变载四旋翼无人机的位置子系统模型，后三项为变载四旋翼无人机的姿态子系统模型；

分别为滚转角、俯仰角和偏航角的角速度，

分别为滚转角、俯仰角和偏航角的角加速度，

分别为x方向、y方向和z方向的加速度，a_i为常数并且i＝1,...,6，S_(*)表示sin(*)，C_(*)表示cos(*)，U₁为位置系统的控制输入，U₂为滚转角的控制输入，U₃为俯仰角的控制输入，U₄为偏航角的控制输入，d_j为外界干扰项，j＝1,...,6，d_j满足|d_j|≤D，D为未知扰动上界，且D＞0，a_i的表达式为：

其中d_φ、d_θ、d_ψ均为阻力系数；

U₁、U₂、U₃、U₄满足以下条件：

其中，Ω₁,Ω₂,Ω₃,Ω₄为电机的转速，l为电机到变质量机体重心的距离，κ为拉力系数，

为扭矩系数。

进一步，步骤2中自适应质量估计算法和扰动估计算法表达式分别为：

其中，

表示的是四旋翼无人机质量变化的自适应律；

表示的是外部干扰的自适应律；γ₁,γ₂为待设计参数；s_p是位置子系统的非奇异快速终端滑模面；

是位置子系统控制输出的中间变量。

进一步，步骤3包括：

(301)将步骤103中位置子系统模型写成如下矩阵形式：

其中U_p＝[u_x u_y u_z]^T为位置子系统的虚拟控制量，u_x＝U₁(C_φS_θC_ψ+S_φS_ψ)，u_y＝U₁(C_φS_θS_ψ-S_φC_ψ)，u_z＝U₁(C_φC_θ)；d_p＝[d₁ d₂ d₃]^T为位置子系统三个方向的外部扰动，G＝[0 0 g]^T为位置子系统的重力系数；

(302)根据非奇异终端滑模面公式建立位置子系统非奇异快速终端滑模面s_p，表达式为：

e_p＝P-P_d是位置跟踪误差，P＝[x y z]^T为实际值，P_d＝[x_d y_d z_d]^T为目标值；α,β,h均为滑模参数，且满足α＞0,β＞0,h≥0；

对滑模面s_p求导：

定义

则上式改写为：

(303)根据下式设计趋近律

其中，趋近律的各参数满足k₁＞0，k₂＞0，0＜ε＜1；

(304)结合滑模面s_p和趋近律

根据下式设计中间变量

(305)根据下式设计位置子系统的虚拟控制量U_p＝[u_x u_y u_z]^T：

(306)给定偏航角的期望信号ψ_d，根据步骤301中虚拟控制量u_x,u_y,u_z与三个姿态角的关系，得到滚转角和俯仰角的期望信号φ_d、θ_d以及位置子系统的控制输入U₁：

φ_d和θ_d作为期望信号用于姿态角控制器的设计，ψ_d取值范围为

根据控制输入U₁设计变载无人机的位置控制器；

(307)确保位置控制器能保证位置子系统的稳定性，定义李雅普诺夫函数，表达式为：

其中，

为质量估计误差，

为外部扰动估计误差，且质量及外部扰动均变化缓慢，即它们的导数为

对上式进行求导数，将控制输入U₁代入到

得到：

因为k₁、k₂均大于零，0＜ε＜1，故

所以位置控制器能保证位置子系统存在李雅普诺夫稳定。

进一步，步骤4中故障模型表达式为：

其中，

是姿态系统状态量，

为系统不确定因素，u_i＝[U₂ U₃ U₄]^T为姿态系统控制输入，

是故障的估计值，Δf_i＝[Δf₁ Δf₂ Δf₃]^T是故障估计误差值，i＝1,2,3。

进一步，步骤5自适应非线性故障观测器的表达式为：

其中，u＝[U₂ U₃ U₄]^T为姿态子系统控制输入，

是三个姿态角及角速度的观测量，

是观测器输出矩阵，

为系统不确定性观测矩阵，A,B,C为已知参数矩阵，K是观测器增益矩阵，

是待设计参数，更新率为

ι_k＞0；

是故障分布矩阵，E为单位矩阵；

是故障向量估计矩阵；

为了估计执行器故障f(t)，假设待估计的故障及故障导数有界，则有：

||f(t)||≤v₁

其中上界v₁,v₂均为正数；

故障的自适应律设计为：

其中，观测器输出误差为

待设计参数矩阵为η＝η^T＞0，N为待设计参数矩阵；

在确定观测器增益矩阵K的情况下，若存在矩阵Υ＝Υ^T＞0，Q＝Q^T＞0，参数σ＞0，则待设计参数矩阵N由下式求出：

进一步，考虑自适应非线性故障观测器的稳定性，建立李雅普诺夫函数，包括：

(501)建立误差方程：

为三个姿态角及角速度的观测量，

为姿态子系统的观测输出量，

为故障估计值，

为参数k的估计值；

(502)对e_x的进行求导数：

(503)定义李雅普诺夫函数表达式为：

对

求导数：

其中，

λ_max()表示相应矩阵的最大特征值，λ_min()表示相应矩阵的最小特征值，

σ＝λ_min(-Θ)；当Θ＜0时，若

可得

自适应非线性故障观测器存在李雅普诺夫稳定。

进一步，步骤6包括：

(601)定义滚转角φ、俯仰角θ、偏航角ψ的跟踪误差，分别为：

e_φ＝φ-φ_d

e_θ＝θ-θ_d

e_ψ＝ψ-ψ_d

φ,θ,ψ表示三个姿态角的实际值，φ_d,θ_d,ψ_d表示三个姿态角的期望值；

(602)建立滚转角φ通道控制器第一个滑模面为：

s_1φ＝e_φ+k_φ∫e_φdt

其中参数k_φ＞0；

(603)根据下式设计滚转角φ通道的虚拟控制输入ρ_φ：

其中参数A_φ＞0；

(604)建立滚转角φ通道控制器第二个滑模面s_2φ为：

并对s_2φ求导数得：

(605)结合步骤5自适应非线性故障观测器对故障的估计，为姿态故障模型中的滚转角通道设计控制输入U₂，表达式为：

考虑到系统不确定性上界的未知性因素，会对输入控制律产生限制影响，利用可调增益参数

进行调节，可调增益参数的更新率设计为：

δ_φ＞0；

(606)建立俯仰角θ和偏航角ψ的第一个滑模面，分别表示为：

s_1θ＝e_θ+k_θ∫e_θdt

s_1ψ＝e_ψ+k_ψ∫e_ψdt

其中参数k_θ＞0，k_ψ＞0；

(607)定义俯仰角θ通道和偏航角ψ通道的虚拟控制输入ρ_θ、ρ_ψ，分别表示为：

其中参数A_θ＞0，A_ψ＞0；

(608)建立俯仰角θ通道和偏航角ψ通道的控制输入U₃，U₄分别为，

根据控制输入U₂、U₃、U₄完成姿态子系统控制器建立。

进一步，考虑姿态控制器能保证姿态子系统的稳定性，建立李雅普诺夫函数，表达式为：

可调增益的更新误差为：

将控制输入U₂代入

得：

若存在Γ_φ＞0且Γ_φ≥|Δf₁|，则

姿态控制器能保证姿态子系统存在李雅普诺夫稳定。

有益效果：本发明与现有技术相比，其显著优点是：

1、本发明通过自适应质量估计机制，精确计算出四旋翼无人机在整体质量变化瞬间的实际值；通过外部扰动自适应机制，有效降低外部扰动对变载飞控系统的影响；

2、利用自适应非线性故障观测器精确地估计故障的时变信息，有效包容未知执行器故障对飞控系统的影响；

3、利用非奇异快速终端滑模设计的位置控制器和利用积分滑模方法设计的姿态控制器，避免了奇异性问题，实现有限时间收敛，减小稳态误差，保证整体系统在时变质量情况下的稳定性。

附图说明

图1为本发明各系统之间关系图；

图2为存在质量变化的情况下，系统对变化质量的真实值与估计值的对比仿真图；

图3为存在质量变化的情况下，系统对变化质量的估计误差曲线图；

图4为存在多通道执行器故障情况下，系统对故障1的真实值与估计值的对比仿真图；

图5为存在多通道执行器故障情况下，系统对故障2的真实值与估计值的对比仿真图；

图6为存在多通道执行器故障情况下，系统对故障3的真实值与估计值的对比仿真图；

图7为存在质量变化和外部扰动的条件下，系统位置x、y、z跟踪曲线图；

图8为存在质量变化和执行器故障的条件下，系统滚转角、俯仰角、偏航角跟踪曲线图；

图9为变载飞行器的三维跟踪轨迹图。

具体实施方式

本实施例所述的一种变载四旋翼无人机容错控制方法，包括如下步骤：

(1)建立变载四旋翼无人机在正常运行状态下的非线性动力学模型，包括位置子系统模型和姿态子系统模型；

(101)根据牛顿第二定律，建立变载四旋翼无人机的在地面坐标系下的位置平移动力学方程为：

其中，m为变质量四旋翼无人机总体的质量，

为地面坐标系下的x，y，z方向加速度，[x,y,z]为无人机质心在惯性坐标系中的位置坐标，

分别为x方向、y方向和z方向的加速度，F＝[0 0 U₁]^T为机体坐标系下产生的升力矩阵，U₁为位置的控制输入，R为地面坐标系与机体坐标系之间的转换矩阵，d_F＝[d₁ d₂ d₃]^T代表外部扰动，g为重力系数；

(102)基于牛顿-欧拉方法，建立变载四旋翼无人机的在地面坐标系下的姿态旋转动力学方程为：

其中，τ_B为各电机转子提供的扭矩，τ_B＝[U₂I_x U₃I_y U₄I_z]^T，U₂、U₃、U₄为姿态的控制输入，I_x为x轴的转动惯量，I_y为y轴的转动惯量，I_z为z轴的转动惯量；I_B为系统转动惯量，I_B＝diag(I_x,I_y,I_z)；ω_B为机体坐标系下的姿态角速度，ω_B＝[p,q,r]^T，p,q,r分别代表机体坐标系下的横滚角、俯仰角、偏航角的角速度；τ_d为气动摩擦力矩，τ_d＝diag(d_φ,d_θ,d_ψ)ω_E，ω_E为地面坐标系下的姿态角速度，

[φ,θ,ψ]为无人机三个姿态的欧拉角度，分别代表滚转角、俯仰角和偏航角；

(103)结合步骤(101)和步骤(102)，得到变载四旋翼无人机的非线性动力学模型如下：

式中前三项为变载四旋翼无人机的位置子系统模型，后三项为变载四旋翼无人机的姿态子系统模型；

分别为滚转角、俯仰角和偏航角的角速度，

分别为滚转角、俯仰角和偏航角的角加速度，

分别为x方向、y方向和z方向的加速度，a_i为常数并且i＝1,...,6，S_(*)表示sin(*)，C_(*)表示cos(*)，U₁为位置系统的控制输入，U₂为滚转角的控制输入，U₃为俯仰角的控制输入，U₄为偏航角的控制输入，d_j为外界干扰项，j＝1,...,6，d_j满足|d_j|≤D，D为未知扰动上界，且D＞0，a_i的表达式为：

其中d_φ、d_θ、d_ψ均为阻力系数；

U₁、U₂、U₃、U₄满足以下条件：

其中，Ω₁,Ω₂,Ω₃,Ω₄为电机的转速，l为电机到变质量机体重心的距离，κ为拉力系数，

为扭矩系数。

(2)考虑无人机负载质量变化和外部扰动的影响，分别建立自适应质量估计算法和扰动估计算法：

其中，

表示的是四旋翼无人机质量变化的自适应律；

表示的是外部干扰的自适应律；γ₁,γ₂为待设计参数；s_p是位置子系统的非奇异快速终端滑模面；

是位置子系统控制输出的中间变量。

(3)建立位置子系统的非奇异快速终端滑模面，考虑自适应质量估计算法和扰动估计算法，对位置子系统模型构建虚拟控制量，根据虚拟控制量与无人机姿态角之间的关系，得到位置子系统的位置控制输入，设计位置子系统控制器；

(301)将步骤103中位置子系统模型写成如下矩阵形式：

其中U_p＝[u_x u_y u_z]^T为位置子系统的虚拟控制量，u_x＝U₁(C_φS_θC_ψ+S_φS_ψ)，u_y＝U₁(C_φS_θS_ψ-S_φC_ψ)，u_z＝U₁(C_φC_θ)。d_p＝[d₁ d₂ d₃]^T为位置子系统三个方向的外部扰动，G＝[0 0 g]^T为位置子系统的重力系数；

(302)根据非奇异终端滑模面公式建立位置子系统非奇异快速终端滑模面s_p，表达式为：

e_p＝P-P_d是位置跟踪误差，P＝[x y z]^T为实际值，P_d＝[x_d y_d z_d]^T为目标值；α,β,h均为滑模参数，且满足α＞0,β＞0,h≥0；

对滑模面s_p求导：

定义

则上式改写为：

(303)根据下式设计趋近律

其中，趋近律的各参数满足k₁＞0，k₂＞0，0＜ε＜1；

(304)结合滑模面s_p和趋近律

根据下式设计中间变量

(305)根据下式设计位置子系统的虚拟控制量U_p＝[u_x u_y u_z]^T：

(306)给定偏航角的期望信号ψ_d，根据步骤301中虚拟控制量u_x,u_y,u_z与三个姿态角的关系，得到滚转角和俯仰角的期望信号φ_d、θ_d以及位置子系统的控制输入U₁：

φ_d和θ_d作为期望信号用于姿态角控制器的设计，ψ_d取值范围为

根据控制输入U₁设计变载无人机的位置控制器；

(307)确保位置控制器能保证位置子系统的稳定性，定义李雅普诺夫函数，表达式为：

其中，

为质量估计误差，

为外部扰动估计误差，且质量及外部扰动均变化缓慢，即它们的导数为

对上式进行求导数，将控制输入U₁代入到

得到：

因为k₁、k₂均大于零，0＜ε＜1，故

所以位置控制器能保证位置子系统存在李雅普诺夫稳定。

(4)考虑执行器故障对姿态子系统的影响，在姿态子系统模型的基础上建立姿态故障模型，表达式为：

其中，

是姿态系统状态量，

为系统不确定因素，u_i＝[U₂ U₃ U₄]^T为姿态系统控制输入，

是故障的估计值，Δf_i＝[Δf₁ Δf₂ Δf₃]^T是故障估计误差值，i＝1,2,3。

(5)根据姿态故障模型建立自适应非线性故障观测器，得到故障估计值；

自适应非线性故障观测器的表达式为：

其中，u＝[U₂ U₃ U₄]^T为姿态子系统控制输入，

是三个姿态角及角速度的观测量，

是观测器输出矩阵，

为系统不确定性观测矩阵，A,B,C为已知参数矩阵，K是观测器增益矩阵，

是待设计参数，更新率为

ι_k＞0；

是故障分布矩阵，E为单位矩阵；

是故障向量估计矩阵；

为了估计执行器故障f(t)，假设待估计的故障及故障导数有界，则有：

||f(t)||≤v₁

其中上界v₁,v₂均为正数；

故障的自适应律设计为：

其中，观测器输出误差为

待设计参数矩阵为η＝η^T＞0，N为待设计参数矩阵；

在确定观测器增益矩阵K的情况下，若存在矩阵Υ＝Υ^T＞0，Q＝Q^T＞0，参数σ＞0，则待设计参数矩阵N由下式求出：

考虑自适应非线性故障观测器的稳定性，建立李雅普诺夫函数，包括：

(501)建立误差方程：

为三个姿态角及角速度的观测量，

为姿态子系统的观测输出量，

为故障估计值，

为参数k的估计值；

(502)对e_x的进行求导数：

(503)定义李雅普诺夫函数表达式为：

对

求导数：

其中，

λ_max()表示相应矩阵的最大特征值，λ_min()表示相应矩阵的最小特征值，

σ＝λ_min(-Θ)。当Θ＜0时，若

可得

自适应非线性故障观测器存在李雅普诺夫稳定。

(6)根据故障估计值，在姿态故障模型基础上利用积分滑模建立姿态子系统控制器。

(601)定义滚转角φ、俯仰角θ、偏航角ψ的跟踪误差，分别为：

e_φ＝φ-φ_d

e_θ＝θ-θ_d

e_ψ＝ψ-ψ_d

φ,θ,ψ表示三个姿态角的实际值，φ_d,θ_d,ψ_d表示三个姿态角的期望值；

(602)建立滚转角φ通道控制器第一个滑模面为：

s_1φ＝e_φ+k_φ∫e_φdt

其中参数k_φ＞0；

(603)根据下式设计滚转角φ通道的虚拟控制输入ρ_φ：

其中参数A_φ＞0；

(604)建立滚转角φ通道控制器第二个滑模面s_2φ为：

并对s_2φ求导数得：

(605)结合步骤5自适应非线性故障观测器对故障的估计，为姿态故障模型中的滚转角通道设计控制输入U₂，表达式为：

考虑到系统不确定性上界的未知性因素，会对输入控制律产生限制影响，利用可调增益参数

进行调节，可调增益参数的更新率设计为：

(606)建立俯仰角θ和偏航角ψ的第一个滑模面，分别表示为：

s_1θ＝e_θ+k_θ∫e_θdt

s_1ψ＝e_ψ+k_ψ∫e_ψdt

其中参数k_θ＞0，k_ψ＞0；

(607)定义俯仰角θ通道和偏航角ψ通道的虚拟控制输入ρ_θ、ρ_ψ，分别表示为：

其中参数A_θ＞0，A_ψ＞0；

(608)建立俯仰角θ通道和偏航角ψ通道的控制输入U₃，U₄分别为，

根据控制输入U₂、U₃、U₄完成姿态子系统控制器建立。

进一步，考虑姿态控制器能保证姿态子系统的稳定性，建立李雅普诺夫函数，表达式为：

可调增益的更新误差为：

将控制输入U₂代入

得：

若存在Γ_φ＞0且Γ_φ≥|Δf₁|，则

姿态控制器能保证姿态子系统存在李雅普诺夫稳定。

下面利用Matlab2017b软件，对本实施例无人机容错控制方法进行了仿真验证。

四旋翼无人机控制系统的参数选取如下：

其中t代表时间，单位为秒。g＝9.80m/s²，l＝0.2m，κ＝1.15×10^-7N·s²·rad^-2，

I_y＝I_y＝1.25N·s²·rad^-1，I_z＝2.5N·s²·rad^-1，d_ψ＝d_φ＝d_θ＝0.012/N·s·rad^-1；

位置控制器参数选取如下：

α＝0.5，β＝0.5，h＝3，γ₁＝100，γ₂＝1.1，k₁＝4，k₂＝4，ε＝0.6；

姿态控制器参数选取如下：

k_φ＝k_θ＝k_ψ＝0.9,A_φ＝A_θ＝A_ψ＝10,δ_φ＝δ_θ＝δ_ψ＝200；

观测器参数选取如下：

B＝[0_3×3 E_3×3]^T，C＝E_6×6，F＝[0_3×3 E_3×3]^T，ι_k＝1，η＝diag(4,4,4)，

L＝E_3×3，Z＝diag(200,200,200)；

期望信号为：

[x_d,y_d,z_d]＝[sin(t),cos(t),3t]m，ψ_d＝sin(t)rad；

状态初始值为：

x₀＝y₀＝z₀＝0m,φ₀＝θ₀＝ψ₀＝0rad；

外部扰动取值为：

d_i＝0.2sin(t)N,(i＝1,2,3),d_j＝0.2sin(t)N·m,(j＝4,5,6)；

执行器多通道故障设计如下：

结果说明：

如图2和图3所示，当四旋翼无人机受到质量变化的影响时，自适应质量估计机制能够在3s以内精确地估计出变化质量瞬间的实际值，并将估计的误差有效地控制在0.1KG左右，极大地降低了质量变化对系统的影响。

如图4-6所示，当变载四旋翼无人机的姿态系统受到执行器多通道故障，横滚角、俯仰角、偏航角通道同时发生不同的执行器故障时，自适应非线性故障观测器能够快速准确地估计出发生在不同姿态角通道的实时故障值。

如图7和图9所示，当位置子系统受到内部质量变化以及外部扰动影响时，采用本实施例中设计的位置子系统控制器可以使系统分别在3s、4s和1s内，跟踪上x,y,z方向上的位置目标，保证了位置子系统的稳定性。

如图8所示，当姿态子系统受到内部质量变化及执行器多通道故障时，采用本发明中设计的姿态子系统控制器可以使系统在分别1s、0.5s和0.1s内，跟踪上φ,θ,ψ通道上的姿态目标，保证了姿态子系统的稳定性。

Claims (9)

1.一种变载四旋翼无人机容错控制方法，其特征在于，包括如下步骤：

(1)建立变载四旋翼无人机在正常运行状态下的非线性动力学模型，包括位置子系统模型和姿态子系统模型；

(2)考虑无人机负载质量变化和外部扰动的影响，分别建立自适应质量估计算法和扰动估计算法；

(3)建立位置子系统的非奇异快速终端滑模面，考虑自适应质量估计算法和扰动估计算法，对位置子系统模型构建虚拟控制量，根据虚拟控制量与无人机姿态角之间的关系，得到位置子系统的位置控制输入，设计位置子系统控制器；

(4)考虑执行器故障对姿态子系统的影响，在姿态子系统模型的基础上建立姿态故障模型；

(5)根据姿态故障模型建立自适应非线性故障观测器，得到故障估计值；

(6)根据故障估计值，在姿态故障模型基础上利用积分滑模建立姿态子系统控制器。

2.根据权利要求1所述的容错控制方法，其特征在于，步骤1包括：

(101)根据牛顿第二定律，建立变载四旋翼无人机的在地面坐标系下的位置平移动力学方程为：

其中，m为变质量四旋翼无人机总体的质量，

为地面坐标系下的x，y，z方向加速度，[x,y,z]为无人机质心在惯性坐标系中的位置坐标，

分别为x方向、y方向和z方向的加速度，F＝[0 0 U₁]^T为机体坐标系下产生的升力矩阵，U₁为位置的控制输入，R为地面坐标系与机体坐标系之间的转换矩阵，d_F＝[d₁ d₂ d₃]^T代表外部扰动，g为重力系数；

(102)基于牛顿-欧拉方法，建立变载四旋翼无人机的在地面坐标系下的姿态旋转动力学方程为：

其中，τ_B为各电机转子提供的扭矩，τ_B＝[U₂I_x U₃I_y U₄I_z]^T，U₂为滚转角的控制输入，U₃为俯仰角的控制输入，U₄为偏航角的控制输入，I_x为x轴的转动惯量，I_y为y轴的转动惯量，I_z为z轴的转动惯量；I_B为系统转动惯量，I_B＝diag(I_x,I_y,I_z)；ω_B为机体坐标系下的姿态角速度，ω_B＝[p,q,r]^T，p,q,r分别代表机体坐标系下的横滚角、俯仰角、偏航角的角速度；τ_d为气动摩擦力矩，τ_d＝diag(d_φ,d_θ,d_ψ)ω_E，ω_E为地面坐标系下的姿态角速度，

[φ,θ,ψ]为无人机三个姿态的欧拉角度，分别代表滚转角、俯仰角和偏航角；

(103)结合步骤(101)和步骤(102)，得到变载四旋翼无人机的非线性动力学模型如下：

式中前三项为变载四旋翼无人机的位置子系统模型，后三项为变载四旋翼无人机的姿态子系统模型；

分别为滚转角、俯仰角和偏航角的角速度，

分别为滚转角、俯仰角和偏航角的角加速度，

分别为x方向、y方向和z方向的加速度，a_i为常数并且i＝1,...,6，S_(*)表示sin(*)，C_(*)表示cos(*)，d_j为外界干扰项，j＝1,...,6，d_j满足|d_j|≤D，D为未知扰动上界，且D＞0，a_i的表达式为：

其中d_φ、d_θ、d_ψ均为阻力系数；

U₁、U₂、U₃、U₄满足以下条件：

其中，Ω₁,Ω₂,Ω₃,Ω₄为电机的转速，l为电机到变质量机体重心的距离，κ为拉力系数，

为扭矩系数。

3.根据权利要求2所述的容错控制方法，其特征在于，步骤2中自适应质量估计算法和扰动估计算法表达式分别为：

其中，

表示的是四旋翼无人机质量变化的自适应律；

表示的是外部干扰的自适应律；γ₁,γ₂为待设计参数；s_p是位置子系统的非奇异快速终端滑模面；

是位置子系统控制输出的中间变量。

4.根据权利要求3所述的容错控制方法，其特征在于，步骤3包括：

(301)将步骤103中位置子系统模型写成如下矩阵形式：

其中U_p＝[u_x u_y u_z]^T为位置子系统的虚拟控制量，u_x＝U₁(C_φS_θC_ψ+S_φS_ψ)，u_y＝U₁(C_φS_θS_ψ-S_φC_ψ)，u_z＝U₁(C_φC_θ)；d_p＝[d₁ d₂ d₃]^T为位置子系统三个方向的外部扰动，G＝[0 0 g]^T为位置子系统的重力系数；

(302)根据非奇异终端滑模面公式建立位置子系统非奇异快速终端滑模面s_p，表达式为：

e_p＝P-P_d是位置跟踪误差，P＝[x y z]^T为实际值，P_d＝[x_d y_d z_d]^T为目标值；α,β,h均为滑模参数，且满足α＞0,β＞0,h≥0；

对滑模面s_p求导：

定义

则上式改写为：

(303)根据下式设计趋近律

其中，趋近律的各参数满足k₁＞0，k₂＞0，0＜ε＜1；

(304)结合滑模面s_p和趋近律

根据下式设计中间变量

(305)根据下式设计位置子系统的虚拟控制量U_p＝[u_x u_y u_z]^T：

(306)给定偏航角的期望信号ψ_d，根据步骤301中虚拟控制量u_x,u_y,u_z与三个姿态角的关系，得到滚转角和俯仰角的期望信号φ_d、θ_d以及位置子系统的控制输入U₁：

φ_d和θ_d作为期望信号用于姿态角控制器的设计，ψ_d取值范围为

根据U₁设计变载无人机的位置控制器；

(307)确保位置控制器能保证位置子系统的稳定性，定义李雅普诺夫函数，表达式为：

其中，

为质量估计误差，

为外部扰动估计误差，且质量及外部扰动均变化缓慢，即它们的导数为

对上式进行求导数，将控制输入U₁代入到

得到：

因为k₁、k₂均大于零，0＜ε＜1，故

所以位置控制器能保证位置子系统存在李雅普诺夫稳定。

5.根据权利要求1所述的容错控制方法，其特征在于，步骤4中故障模型表达式为：

其中，

是姿态系统状态量，

为系统不确定因素，u_i＝[U₂ U₃ U₄]^T为姿态系统控制输入，

是故障的估计值，Δf_i＝[Δf₁ Δf₂ Δf₃]^T是故障估计误差值，i＝1,2,3。

6.根据权利要求5所述的容错控制方法，其特征在于，步骤5自适应非线性故障观测器的表达式为：

其中，u＝[U₂ U₃ U₄]^T为姿态子系统控制输入，

是三个姿态角及角速度的观测量，

是观测器输出矩阵，

为系统不确定性观测矩阵，A,B,C为已知参数矩阵，K是观测器增益矩阵，

是待设计参数，更新率为

ι_k＞0；

是故障分布矩阵，E为单位矩阵；

是故障向量估计矩阵；

为了估计执行器故障f(t)，假设待估计的故障及故障导数有界，则有：

||f(t)||≤v₁

其中上界v₁,v₂均为正数；

故障的自适应律设计为：

其中，观测器输出误差为

待设计参数矩阵为η＝η^T＞0，N为待设计参数矩阵；

在确定观测器增益矩阵K的情况下，若存在矩阵Υ＝Υ^T＞0，Q＝Q^T＞0，参数σ＞0，则待设计参数矩阵N由下式求出：

7.根据权利要求6所述的容错控制方法，其特征在于，考虑自适应非线性故障观测器的稳定性，建立李雅普诺夫函数，包括：

(501)建立误差方程：

为故障估计值，

为参数k的估计值；

(502)对e_x的进行求导数：

(503)定义李雅普诺夫函数表达式为：

对

求导数：

其中，

λ_max()表示相应矩阵的最大特征值，λ_min()表示相应矩阵的最小特征值，

σ＝λ_min(-Θ)；当Θ＜0时，若

可得

自适应非线性故障观测器存在李雅普诺夫稳定。

8.根据权利要求7所述的容错控制方法，其特征在于，步骤6包括：

(601)定义滚转角φ、俯仰角θ、偏航角ψ的跟踪误差，分别为：

e_φ＝φ-φ_d

e_θ＝θ-θ_d

e_ψ＝ψ-ψ_d

φ,θ,ψ表示三个姿态角的实际值，φ_d,θ_d,ψ_d表示三个姿态角的期望值；

(602)建立滚转角φ通道控制器第一个滑模面为：

s_1φ＝e_φ+k_φ∫e_φdt

其中参数k_φ＞0；

(603)根据下式设计滚转角φ通道的虚拟控制输入ρ_φ：

其中参数A_φ＞0；

(604)建立滚转角φ通道控制器第二个滑模面s_2φ为：

并对s_2φ求导数得：

(605)结合步骤5自适应非线性故障观测器对故障的估计，为姿态故障模型中的滚转角通道设计控制输入U₂，表达式为：

考虑到系统不确定性上界的未知性因素，会对输入控制律产生限制影响，利用可调增益参数

进行调节，可调增益参数的更新率设计为：

δ_φ＞0；

(606)建立俯仰角θ和偏航角ψ的第一个滑模面，分别表示为：

s_1θ＝e_θ+k_θ∫e_θdt

s_1ψ＝e_ψ+k_ψ∫e_ψdt

其中参数k_θ＞0，k_ψ＞0；

(607)定义俯仰角θ通道和偏航角ψ通道的虚拟控制输入ρ_θ、ρ_ψ，分别表示为：

其中参数A_θ＞0，A_ψ＞0；

(608)建立俯仰角θ通道和偏航角ψ通道的控制输入U₃，U₄分别为，

根据控制输入U₂、U₃、U₄完成姿态子系统控制器建立。

9.根据权利要求8所述的容错控制方法，其特征在于，考虑姿态控制器能保证姿态子系统的稳定性，建立李雅普诺夫函数，表达式为：

可调增益的更新误差为：

将控制输入U₂代入

得：

若存在Γ_φ＞0且Γ_φ≥|Δf₁|，则

姿态控制器能保证姿态子系统存在李雅普诺夫稳定。

Images