基于观测器的四旋翼无人机容错控制方法与流程

本发明涉及一种四旋翼无人机容错控制问题。针对四旋翼无人机执行器发生失效故障时的姿态控制问题，提出一种基于浸入-不变集观测器技术的非线性自适应容错控制方法。

背景技术：

四旋翼无人机依靠四个电机的转动带动螺旋桨旋转产生升力，通过改变不同电机的转速实现俯仰、滚转、偏航等动作。受飞行器控制稳定性及自身工艺影响，电机和螺旋桨的持续高速旋转使其发生故障的概率大大提高。此外，在飞行器飞行过程中会不可避免地受到外界干扰(如障碍物撞击、雨水强风、强气流等)、飞行器自身气动特性发生强烈变化等诸多不确定因素，这些都直接关系到飞行器飞行性能和安全性能。由于多旋翼飞行器是一个具有强耦合特性的典型非线性系统，一旦发生上述故障，飞行稳定性就会急剧下降，甚至导致飞行器失控，对飞行器本身及地面人员造成极大的伤害。因此，在飞行器飞行控制中加入容错控制模块以保证飞行器安全稳定的飞行显得尤为重要。如何保证多旋翼飞行器在发生故障的情况下仍能得到有效控制，正成为多旋翼飞行器研究领域的一个热点问题。

针对四旋翼无人机执行器发生故障时的姿态控制问题，目前采用较为广泛的容错控制策略大体可分为两类：被动容错和主动容错。被动容错利用控制器的鲁棒性使得控制器对故障信息不敏感，从而达到容错控制的目的；而主动容错则通过故障诊断与故障隔离能够在线检测并分离出所发生故障，再根据故障模式进行故障重构，以此达到容错控制目的。

对于上述两种容错控制策略，国内外很多研究单位，如麻省理工学院、瑞士联邦理工大学、康考迪亚大学、南京航空航天大学、北京航空航天大学等，基于多种线性或非线性控制方法开展了相关研究，如变增益PID、反步法、滑模控制、模型参考自适应、模型预测控制等方法，并且对这些方法的控制效果具有数值仿真或实际飞行实验的验证(书籍：Automatic Flight Control Systems-Latest Development；著者：Youmin Zhang，Annas Chamseddine；出版年月：2012年；文章题目：Fault Tolerant and Flight Control Techniques with application to a Quadrotor UAV Testbed；页码：119–150)。

但是，当前各种容错控制方法均有其各自的局限性。比如：在对执行器故障进行动力学建模时，将其视为外部扰动力矩，进行了较大程度的近似，难以反映执行器故障对无人机的真实影响(期刊：控制理论与应用；著者：杨荟憭，姜斌，张柯；出版年月：2014年；文章题目：四旋翼直升机姿态系统的直接自我修复控制；页码：1053-1060)；部分容错控制方法在平衡点处对四旋翼无人机的动力学模型进行了线性化处理，理论证明只能得到平衡点附近的稳定结论，当执行器发生故障时，飞行器姿态会发生突变，且多数情况下飞行器姿态会偏离平衡点较大位置，控制器应用范围难以保证(期刊：IEEE Transactions on Control Systems Technology；著者：Z.T.Dydek，A.M.Annaswamy，E.Lavretsky；出版年月：2013年7月；文章题目：Adaptive Control of Quadrotor UAVs：a Design Trade Study with Flight Evaluations；页码：1400–1406)；被动容错方法应用范围有限，难以做到对外界扰动和执行器故障鲁棒性的兼容性，控制效果较差，而主动容错控制方法则需要进行故障诊断和故障隔离，并在此基础上进行故障重构，算法复杂，难以实现工程应用(期刊：Proceedings of the Institution of Mechanical Engineers,Part I,Journal of Systems and Control Engineering；著者：T.Li,Y.M.Zhang,B.W.Gordon；出版年月：2012年1月；文章题目：Passive and Active Nonlinear Fault-Tolerant Control of a Quadrotor UAV Based on Sliding Mode Control Technique；页码：12-23)。

技术实现要素：

为克服现有技术的不足，实现四旋翼无人机执行器故障情况下的姿态稳定。本发明采用的技术方案是，基于观测器的四旋翼无人机容错控制方法，步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}，通过分析执行器对四旋翼无人机的作用原理，用未知对角矩阵表示执行器故障对其动力学特性的影响，得到四旋翼无人机执行器发生故障时的非线性动力学模型：

式(1)中各变量定义如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示机体坐标系{B}相对于惯性坐标系{I}的姿态角速度，ω₁,ω₂,ω₃分别表示滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取ω的一阶时间导数，下同；J∈R^3×3为转动惯量，S(ω)表示求取ω对应的反对称矩阵，L∈R^3×4为与机身长度和反扭矩系数相关的常系数矩阵，F＝diag{[f₁ f₂ f₃ f₄]^T}∈R^4×4表示升力矩阵，f₁,f₂,f₃,f₄分别表示四个电机产生的升力，diag{[f₁ f₂ f₃ f₄]^T}表示向量[f₁ f₂f₃ f₄]张成的对角矩阵，λ＝[λ₁ λ₂ λ₃ λ₄]^T∈R^4×1表示故障向量，λ_i＝1，i＝1,2,3,4表示第i个通道执行器正常，λ_i≠1，i＝1,2,3,4表示第i个通道执行器发生故障，假设执行器故障为常增益型故障，因此故障向量λ满足：

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{B}在惯性坐标系{I}下的表达用“等效轴角坐标系”方法，将{B}和{I}重合，将{B}绕矢量k∈R^3×1按右手定则旋转角，得到当前姿态单位四元数其中且满足k∈R^3×1为定义在坐标系{I}中的任意单位矢量，为坐标系{B}绕矢量k旋转的任意角度；由机体坐标系{B}到惯性坐标系{I}的坐标变换矩阵用四元数表示为I₃为3×3的单位矩阵，下同，S(q_v)表示求取q_v对应的反对称矩阵，同理，目标坐标系{B_d}在惯性坐标系{I}下的表达也可以用“等效轴角坐标系”方法，将{B_d}和{I}重合，将{B_d}绕矢量k_d∈R^3×1按右手定则旋转角，得到目标姿态单位四元数其中且满足k_d∈R^3×1为定义在坐标系{I}中的任意单位矢量，为坐标系{B_d}绕矢量k_d旋转的任意角度；由目标坐标系{B_d}到惯性坐标系{I}的坐标变换矩阵用四元数表示为S(q_vd)表示求取q_vd对应的反对称矩阵，为了描述四旋翼无人机当前姿态与目标姿态之间的差异，定义姿态误差四元数

其中e₀和e_v同样满足由目标坐标系{B_d}到机体坐标系{B}的坐标变换矩阵示为S(e_v)表示求取e_v对应的反对称矩阵；

为了对四旋翼无人机执行器故障进行更有针对性的容错控制，采用基于浸入-不变集方法的观测器技术对执行器进行观测，定义观测器为：

其中ξ∈R⁴为观测器状态，表示求取ξ的一阶时间导数，R³×R³×R³×R³×R⁴→R⁴为待求函数，为方便表示，用X代替ω_d,e_q，表示求取对ω的偏导数，表示求取对的偏导数，表示求取对X的偏导数，J^-1表示J的逆矩阵，表示求取的一阶时间导数，表示求取X的一阶时间导数，表示对λ的估计向量，表示对ω的估计值，且满足：

其中为正增益函数，定义故障观测误差为z∈R⁴：

其中r∈R为动态增益，对z求一阶时间导数，得

假设存在正常数γ和连续可微矩阵：利用分别表示的列向量，使得

定义其中W₁,W₂,W₃分别为：

其中表示相对于σ从0到ω₁的定积分，下同，式(8)中分别为：

其中表示在ω₁＝σ,时的取值，对W₁求ω₁的偏导数，整理得

其中表示求取对ω₁的偏导数，同理可得，因此写为：

定义ω的估计误差为：由于连续可微，因此存在δ_ij∈R⁴,i,j＝1,2,3满足：

因此写为：

其中表示求取e_ω1Δ₁+e_ω2Δ₂+e_ω3Δ₃的和，Δ_j＝[δ_1j δ_2j δ_3j]∈R^4×3,δ_jj＝0,j＝1,2,3，将式(15)代入式(7)，整理得

对e_ω求一阶时间导数，整理得

设计r，分别满足：

其中，r(0)表示r的初值，c,m,p均为正常数，且满足c≥3/(2γ)，表示Δ_j的上界，||·||表示2范数，I₃为3×3的单位矩阵，为3×3的对角矩阵，若式(16)和式(17)成立，则由式(16)和式(17)组成的系统有一个全局稳定的平衡点(z,e_ω)＝(0,0)，且z,r,e_ω均有界。

由式(16)和式(17)组成的系统有一个全局稳定的平衡点(z,e_ω)＝(0,0)，且z(t),r(t),e_ω(t)均有界的证明步骤是采用基于Lyapunov的分析方法进行证明，具体地：

定义滑模面其中K_s为一3×3的正常数增益对角矩阵，证明当s渐近收敛到0时，和e_v也渐近收敛到0的过程是：

对s求导，并将代入得

其中：

rLFz是有界的，因此假设||-rLFz||≤ρ，ρ为正常数，设计控制输入F为

其中L^R＝L^T(LL^T)-¹表示矩阵L的伪逆矩阵，Γ为一3×3的正常数增益对角矩阵，sign为符号函数，将式(19)代入式(18)，采用基于Lyapunov的分析方法可以证明闭环系统全局渐近稳定，即当时间趋于无穷时，滑模面s渐近收敛到0，则和e_v也渐近收敛到0。

本发明的特点及有益效果是：

本发明采用基于观测器技术的方法针对四旋翼无人机执行器故障的容错控制进行研究。该方法既能对故障信息进行有效估计，进行很好的抑制，又不需要主动容错控制所需要的故障隔离，大大地减少了计算量，提高的控制效率。实验表明，该方法对四旋翼无人机执行器故障具有较好的鲁棒性，当四旋翼无人机执行器发生故障时，无人机能够迅速克服故障影响，保持姿态稳定。

附图说明：

图1是本发明所用实验平台。

图2是容错控制实验效果图，图中：

a是欧拉角形式的姿态角变化曲线；

b是姿态误差四元数变化曲线；

c是角速度误差变化曲线；

d是控制输入变化曲线；

e是电机转速变化曲线；

f是角速度估计误差变化曲线；

g是故障估计变化曲线；

h是故障估计误差变化曲线。

具体实施方式

可实现四旋翼无人机执行器发生故障时的姿态控制，包括下列步骤：

为了进行四旋翼无人机动力学和运动学特性分析，首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}。通过分析执行器对四旋翼无人机的作用原理，用未知对角矩阵表示执行器故障对其动力学特性的影响，得到四旋翼无人机执行器发生故障时的非线性动力学模型：

式(1)中各变量定义如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示机体坐标系{B}相对于惯性坐标系{I}的姿态角速度，ω₁,ω₂,ω₃分别表示滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取ω的一阶时间导数，下同；J∈R^3×3为转动惯量，S(ω)表示求取ω对应的反对称矩阵，L∈R^3×4为与机身长度和反扭矩系数相关的常系数矩阵，F＝diag{[f₁ f₂ f₃ f₄]^T}∈R^4×4表示升力矩阵，f₁,f₂,f₃,f₄分别表示四个电机产生的升力，diag{[f₁ f₂ f₃ f₄]^T}表示向量[f₁ f₂f₃ f₄]张成的对角矩阵，λ＝[λ₁ λ₂ λ₃ λ₄]^T∈R^4×1表示故障向量，λ_i＝1，i＝1,2,3,4表示第i个通道执行器正常，λ_i≠1，i＝1,2,3,4表示第i个通道执行器发生故障，假设执行器故障为常增益型故障，因此故障向量λ满足：

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{B}在惯性坐标系{I}下的表达用“等效轴角坐标系”方法，将{B}和{I}重合，将{B}绕矢量k∈R^3×1按右手定则旋转角，得到当前姿态单位四元数其中且满足k∈R^3×1为定义在坐标系{I}中的任意单位矢量，为坐标系{B}绕矢量k旋转的任意角度；由机体坐标系{B}到惯性坐标系{I}的坐标变换矩阵用四元数表示为I₃为3×3的单位矩阵，下同，S(q_v)表示求取q_v对应的反对称矩阵，同理，目标坐标系{B_d}在惯性坐标系{I}下的表达也可以用“等效轴角坐标系”方法，将{B_d}和{I}重合，将{B_d}绕矢量k_d∈R^3×1按右手定则旋转角，得到目标姿态单位四元数其中且满足k_d∈R^3×1同样为定义在坐标系{I}中的任意单位矢量，为坐标系{B_d}绕矢量k_d旋转的任意角度；由目标坐标系{B_d}到惯性坐标系{I}的坐标变换矩阵用四元数表示为S(q_vd)表示求取q_vd对应的反对称矩阵。为了描述四旋翼无人机当前姿态与目标姿态之间的差异，定义姿态误差四元数

其中e₀和e_v同样满足由目标坐标系{B_d}到机体坐标系{B}的坐标变换矩阵可表示为S(e_v)表示求取e_v对应的反对称矩阵。

为了对四旋翼无人机执行器故障进行更有针对性的容错控制，采用基于浸入-不变集方法的观测器技术对执行器进行观测，定义观测器为：

其中ξ∈R⁴为观测器状态，表示求取ξ的一阶时间导数，R³×R³×R³×R³×R⁴→R⁴为待求函数，为方便表示，用X代替ω_d,e_q。表示求取对ω的偏导数，表示求取对的偏导数，表示求取对X的偏导数，J^-1表示J的逆矩阵，表示求取的一阶时间导数，表示求取X的一阶时间导数，表示对λ的估计向量，表示对ω的估计值，且满足：

其中为正增益函数，定义故障观测误差为z∈R⁴：

其中r∈R为动态增益。对z求一阶时间导数，得

假设存在正常数γ和连续可微矩阵：利用分别表示的列向量，使得

定义其中W₁,W₂,W₃分别为：

其中表示相对于σ从0到ω₁的定积分，下同。式(8)中分别为：

其中表示在ω₁＝σ,时的取值，下同。对W₁求ω₁的偏导数，整理得

其中表示求取对ω₁的偏导数，下同。同理可得，因此写为：

定义ω的估计误差为：由于连续可微，因此存在δ_ij∈R⁴,i,j＝1,2,3满足：

因此写为：

其中表示求取e_ω1Δ₁+e_ω2Δ₂+e_ω3Δ₃的和，Δ_j＝[δ_1j δ_2jδ_3j]∈R^4×3,δ_jj＝0,j＝1,2,3，将式(15)代入式(7)，整理得

对e_ω求一阶时间导数，整理得

设计r，分别满足：

其中，r(0)表示r的初值，c,m,p均为正常数，且满足c≥3/(2γ)，表示Δ_j的上界，||·||表示2范数，I₃为3×3的单位矩阵，为3×3的对角矩阵，若式(16)和式(17)成立，则由式(16)和式(17)组成的系统有一个全局稳定的平衡点(z,e_ω)＝(0,0)，且z,r,e_ω均有界。该结论可采用基于Lyapunov的分析方法进行证明。

定义滑模面其中K_s为一3×3的常数增益对角矩阵。可以证明，当s渐近收敛到0时，和e_v也渐近收敛到0。

对s求导，并将代入可得

其中可以证明rLFz是有界的，因此假设||-rLFz||≤ρ，ρ为正常数。设计控制输入F为

其中L^R＝L^T(LL^T)^-1表示矩阵L的伪逆矩阵，Γ为一3×3的常数增益对角矩阵，sign为符号函数。将式(19)代入式(18),采用基于Lyapunov的分析方法可以证明闭环系统全局渐近稳定，即当时间趋于无穷时，滑模面s渐近收敛到0，则和e_v也渐近收敛到0。

为验证本发明所设计容错控制方法的有效性，利用课题组自主研发的四旋翼无人机平台进行了实验验证。下面结合实验和附图对本发明针对四旋翼无人机执行器故障的容错控制方法作出详细说明。

本发明针对四旋翼无人机执行器发生故障时的姿态控制问题，基于浸入-不变集方法设计了观测器对执行器故障进行观测，然后根据观测结果设计了滑模控制器对故障进行补偿，实现了四旋翼无人机执行器故障情况下的姿态稳定。

一、实验平台简介

实验平台如图1所示。该实验平台采用PC/104嵌入式计算机作为仿真控制器，基于Matlab RTW工具箱的xPC目标作为实时仿真环境，采用自主设计的惯性测量单元作为姿态传感器，俯仰角、滚转角测量精度为±0.2°。偏航角测量精度为±0.5°。整个系统控制频率为500Hz。

二、容错控制实验

本发明所采用方法中涉及的各参数取值如下：J＝diag{[1.25 1.25 2.5]^T}kg·m²，l＝0.225m，ε＝0.25，c＝2.5，γ＝1，p＝1，m＝50，K_s＝diag{[2. 22.5 4]^T}，ρ＝0.40，Γ＝diag{[1.86 1.86 3.05]^T}。初始姿态角和角速度分别为q_d＝[1 0 0 0]^T，ω_d＝[0 0 0]^Trad/s。保持上述各参数不变，令故障矩阵为：

即在前80s(‘s’为秒，下同)，四旋翼无人机四个电机正常运行。在第80s时，第3个电机只能提供正常升力的90％。在第125s时，第2个电机只能提供正常升力的80％。实验结果分别如图2(a)、图2(b)、图2(c)、图2(d)、图2(e)、图2(f)、图2(g)、图2(h)所示。图2(a)和图2(b)表示姿态变化曲线，分别用欧拉角和四元数表示，在第80s，第三个电机发生故障，滚转角和俯仰角发生8°左右的波动而偏航角为3°。在第125s，第二个电机发生了更为剧烈的故障，此时滚转角变化约为12°，俯仰角为20°，偏航角为4°。故障发生后，姿态误差均在5s之内迅速收敛到0。图2(c)为角速度误差的变化曲线，故障发生后，其在2s之内迅速收敛到0。图2(d)和图2(e)分别表示控制输入曲线变化和电机转速变化曲线，均在合理变化范围内。图2(f)表示角速度估计误差曲线，故障发生后迅速收敛到0。图2(g)和图2(h)分别表示对故障的估计值和估计误差的变化曲线，均为稳定状态，与理论计算结果相符。

经过上述分析，证明了本发明所提算法的有效性。