基于观测器的四旋翼无人机容错控制方法.pdf

本发明涉及一种四旋翼无人机容错控制问题，为实现四旋翼无人机执行器故障情况下的姿态稳定。本发明采用的技术方案是，基于观测器的四旋翼无人机容错控制方法，步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{Bd}，通过分析执行器对四旋翼无人机的作用原理，用未知对角矩阵表示执行器故障对其动力学特性的影响，得到四旋翼无人机执行器发生故障时的非线性动力学模型；采用基于单位四元数的姿态表示方法；采用基于浸入‑不变集方法的观测器技术对执行器进行观测。本发明主要应用于四旋翼无人机设计、控制场合。

1.一种基于观测器的四旋翼无人机容错控制方法，其特征是，步骤如下：首先定义惯性
坐标系{I}、机体坐标系{B}和目标坐标系{Bd}，通过分析执行器对四旋翼无人机的作用原
理，用未知对角矩阵表示执行器故障对其动力学特性的影响，得到四旋翼无人机执行器发
生故障时的非线性动力学模型：
$J\stackrel{\·}{\mathrm{\ω}}=-S\left(\mathrm{\ω}\right)J\mathrm{\ω}+LF\mathrm{\λ}---\left(1\right)$
式(1)中各变量定义如下：ω＝[ω1 ω2 ω3]T∈R3×1表示机体坐标系{B}相对于惯性坐
标系{I}的姿态角速度，ω1,ω2,ω3分别表示滚转角速度、俯仰角速度和偏航角速度，[·]T
表示矩阵的转置，∈表示集合间的“属于”关系，R3×1表示3行1列的实数向量，表示求取ω
的一阶时间导数，下同；J∈R3×3为转动惯量，S(ω)表示求取ω对应的反对称矩阵，L∈R3×4
为与机身长度和反扭矩系数相关的常系数矩阵，F＝diag{[f1 f2 f3 f4]T}∈R4×4表示升力
矩阵，f1,f2,f3,f4分别表示四个电机产生的升力，diag{[f1 f2 f3 f4]T}表示向量[f1 f2 f3
f4]张成的对角矩阵，λ＝[λ1 λ2 λ3 λ4]T∈R4×1表示故障向量，λi＝1，i＝1,2,3,4表示第i个
通道执行器正常，λi≠1，i＝1,2,3,4表示第i个通道执行器发生故障，假设执行器故障为常
增益型故障，因此故障向量λ满足：
$\stackrel{\·}{\mathrm{\λ}}={\left[\begin{array}{cccc}0& 0& 0& 0\end{array}\right]}^T---\left(2\right)$
为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系
{B}在惯性坐标系{I}下的表达用“等效轴角坐标系”方法，将{B}和{I}重合，将{B}绕矢量
k∈R3×1按右手定则旋转角，得到当前姿态单位四元数其中
且满足k∈R3×1为定义在坐标系{I}中的
任意单位矢量，为坐标系{B}绕矢量k旋转的任意角度；由机体坐标系{B}到惯性坐标系
{I}的坐标变换矩阵用四元数表示为I3为3×3的
单位矩阵，下同，S(qv)表示求取qv对应的反对称矩阵，同理，目标坐标系{Bd}在惯性坐
标系{I}下的表达也可以用“等效轴角坐标系”方法，将{Bd}和{I}重合，将{Bd}绕矢量kd
∈R3×1按右手定则旋转角，得到目标姿态单位四元数其中
且满足kd∈R3×1为定义在坐标系{I}
中的任意单位矢量，为坐标系{Bd}绕矢量kd旋转的任意角度；由目标坐标系{Bd}到惯性坐
标系{I}的坐标变换矩阵用四元数表示为S
(qvd)表示求取qvd对应的反对称矩阵，为了描述四旋翼无人机当前姿态与目标姿态之间的
差异，定义姿态误差四元数
$\left\{\begin{array}{c}{e}_0=q_0{q}_{0d}+q_v^T{q}_{vd}\\ e_v=q_{0d}{q}_v-q_0{q}_{vd}+S\left(q_v\right)q_{vd}\end{array}\right.---\left(3\right)$
其中e0和ev同样满足由目标坐标系{Bd}到机体坐标系{B}的坐标变换矩阵
示为S(ev)表示求取ev对应的反对称矩阵；
为了对四旋翼无人机执行器故障进行更有针对性的容错控制，采用基于浸入-不变集
方法的观测器技术对执行器进行观测，定义观测器为：
$\stackrel{\·}{\mathrm{\ξ}}=-\left(\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂\mathrm{\ω}}\right(-J^{-1}S\left(\mathrm{\ω}\right)J\mathrm{\ω}+J^{-1}LF\widehat{\mathrm{\λ}})+\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂\widehat{\mathrm{\ω}}}\stackrel{\·}{\widehat{\mathrm{\ω}}}+\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂X}\stackrel{\·}{X})---\left(4\right)$
其中ξ∈R4为观测器状态，表示求取ξ的一阶时间导数，
为待求函数，为方便表示，用X代替ωd,eq，表示求
取对ω的偏导数，表示求取对的偏导数，表
示求取对X的偏导数，J-1表示J的逆矩阵，表示求取的一阶时间导数，表示
求取X的一阶时间导数，表示对λ的估计向量，表示对ω的估计值，且满足：
$\stackrel{\·}{\widehat{\mathrm{\ω}}}=-J^{-1}S\left(\mathrm{\ω}\right)J\mathrm{\ω}+J^{-1}LF\widehat{\mathrm{\λ}}+K(\mathrm{\ω},r,\widehat{\mathrm{\ω}}-\mathrm{\ω})(\widehat{\mathrm{\ω}}-\mathrm{\ω})---\left(5\right)$
其中为正增益函数，定义故障观测误差为z∈R4：
$z=\frac{\widehat{\mathrm{\λ}}-\mathrm{\λ}}{r}=\frac{\mathrm{\ξ}+\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)-\mathrm{\λ}}{r}---\left(6\right)$
其中r∈R为动态增益，对z求一阶时间导数，得
$\stackrel{\·}{z}=-\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂\mathrm{\ω}}{J}^{-1}LFz-\frac{\stackrel{\·}{r}}{r}z---\left(7\right)$
假设存在正常数γ和连续可微矩阵：利用
分别表示
的列向量，使得：
$-\frac{1}{2}{({\[B(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)J^{-1}LF\]}^T+B(\mathrm{\ω},X,\mathrm{\β}\left(\mathrm{\ω},\widehat{\mathrm{\ω}},X\right)\left)J^{-1}LF\right)}_{=}=-\mathrm{\γ}{\[J^{-1}LF\]}^T\[J^{-1}LF\];$
定义其中W1,W2,W3分别为：
$\left\{\begin{array}{c}{W}_1={\∫}_0^{{\mathrm{\ω}}_1}{B}_1(\mathrm{\σ},{\widehat{\mathrm{\ω}}}_2,{\widehat{\mathrm{\ω}}}_3,X,{\mathrm{\β}}_1(\mathrm{\σ},{\widehat{\mathrm{\ω}}}_2,{\widehat{\mathrm{\ω}}}_3,X\left)\right)d\mathrm{\σ}\\ W_2={\∫}_0^{{\mathrm{\ω}}_2}{B}_2({\widehat{\mathrm{\ω}}}_1,\mathrm{\σ},{\widehat{\mathrm{\ω}}}_3,X,{\mathrm{\β}}_2({\widehat{\mathrm{\ω}}}_1,\mathrm{\σ},{\widehat{\mathrm{\ω}}}_3,X\left)\right)d\mathrm{\σ}\\ W_3={\∫}_0^{{\mathrm{\ω}}_3}{B}_3({\widehat{\mathrm{\ω}}}_1,{\widehat{\mathrm{\ω}}}_2,\mathrm{\σ},X,{\mathrm{\β}}_3({\widehat{\mathrm{\ω}}}_1,{\widehat{\mathrm{\ω}}}_2,\mathrm{\σ},X\left)\right)d\mathrm{\σ}\end{array}\right.---\left(8\right)$
其中表示相对于σ从0
到ω1的定积分，下同，式(8)中分别为：
$\left\{\begin{array}{c}{\mathrm{\β}}_1(\mathrm{\σ},{\widehat{\mathrm{\ω}}}_2,{\widehat{\mathrm{\ω}}}_3,X)=\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X){|}_{{\mathrm{\ω}}_1=\mathrm{\σ},{\mathrm{\ω}}_2={\widehat{\mathrm{\ω}}}_2,{\mathrm{\ω}}_3={\widehat{\mathrm{\ω}}}_3}\\ {\mathrm{\β}}_2({\widehat{\mathrm{\ω}}}_1,\mathrm{\σ},{\widehat{\mathrm{\ω}}}_3,X)=\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X){|}_{{\mathrm{\ω}}_2=\mathrm{\σ},{\mathrm{\ω}}_1={\widehat{\mathrm{\ω}}}_1,{\mathrm{\ω}}_3={\widehat{\mathrm{\ω}}}_3}\\ {\mathrm{\β}}_3({\widehat{\mathrm{\ω}}}_1,{\widehat{\mathrm{\ω}}}_2,\mathrm{\σ},X)=\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X){|}_{{\mathrm{\ω}}_3=\mathrm{\σ},{\mathrm{\ω}}_1={\widehat{\mathrm{\ω}}}_1,{\mathrm{\ω}}_2={\widehat{\mathrm{\ω}}}_2}\end{array}\right.---\left(9\right)$
其中表示在ω1＝σ,时的取值，对W1求
ω1的偏导数，整理得
$\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂{\mathrm{\ω}}_1}=\frac{\∂{\mathrm{\β}}_1(\mathrm{\σ},{\widehat{\mathrm{\ω}}}_2,{\widehat{\mathrm{\ω}}}_3,X)}{\∂{\mathrm{\ω}}_1}={\mathrm{\β}}_{\mathrm{\ω}1}---\left(10\right)$
其中表示求取对ω1的偏导数，同理可得，
因此写为：
$\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂\mathrm{\ω}}=\[\begin{array}{ccc}{\mathrm{\β}}_{\mathrm{\ω}1}& {\mathrm{\β}}_{\mathrm{\ω}2}& {\mathrm{\β}}_{\mathrm{\ω}3}\end{array}\]---\left(11\right)$
定义ω的估计误差为：由于连续可
微，因此存在δij∈R4,i,j＝1,2,3满足：
$\left\{\begin{array}{c}{\mathrm{\β}}_{\mathrm{\ω}1}=B_1(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)+e_{\mathrm{\ω}2}{\mathrm{\δ}}_{12}+e_{\mathrm{\ω}3}{\mathrm{\δ}}_{13}\\ {\mathrm{\β}}_{\mathrm{\ω}2}=B_2(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)+e_{\mathrm{\ω}1}{\mathrm{\δ}}_{21}+e_{\mathrm{\ω}3}{\mathrm{\δ}}_{23}\\ {\mathrm{\β}}_{\mathrm{\ω}3}=B_3(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)+e_{\mathrm{\ω}1}{\mathrm{\δ}}_{31}+e_{\mathrm{\ω}2}{\mathrm{\δ}}_{32}\end{array}\right.---\left(12\right)$
因此写为：
$\frac{\∂\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X)}{\∂\mathrm{\ω}}=B(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)+{\mathrm{\Σ}}_{j=1}^3{e}_{\mathrm{\ω}j}{\mathrm{\Δ}}_j---\left(13\right)$
其中表示求取eω1Δ1+eω2Δ2+eω3Δ3的和，Δj＝[δ1j δ2j δ3j]∈R4×3,δjj＝0,j
＝1,2,3，将式(15)代入式(7)，整理得
$\stackrel{\·}{z}=-B(\mathrm{\ω},X,\mathrm{\β}(\mathrm{\ω},\widehat{\mathrm{\ω}},X\left)\right)J^{-1}LFz+{\mathrm{\Σ}}_{j=1}^3{e}_{\mathrm{\ω}j}{\mathrm{\Δ}}_j{J}^{-1}LFz-\frac{\stackrel{\·}{r}}{r}z---\left(14\right)$
对eω求一阶时间导数，整理得
${\stackrel{\·}{e}}_{\mathrm{\ω}}=-K(\mathrm{\ω},r,\widehat{\mathrm{\ω}}-\mathrm{\ω})e_{\mathrm{\ω}}+{\mathrm{rJ}}^{-1}LFz---\left(15\right)$
设计r，分别满足：
$\stackrel{\·}{r}={\mathrm{cr\Σ}}_{j=1}^3{e}_{\mathrm{\ω}j}^2\·\left|\right|{\stackrel{\‾}{\mathrm{\Δ}}}_j(\mathrm{\ω},e_{\mathrm{\ω}},X,\mathrm{\β})|{|}^2,r\left(0\right)\≥1---\left(16\right)$
$K(\mathrm{\ω},r,\widehat{\mathrm{\ω}}-\mathrm{\ω})={\mathrm{mr}}^2{I}_3+{\mathrm{pcr}}^{2}D---\left(17\right)$
其中，r(0)表示r的初值，c,m,p均为正常数，且满足c≥3/(2γ)，表示Δj的上界，|
|·||表示2范数，I3为3×3的单位矩阵，为3×3的对角矩
阵，若式(16)和式(17)成立，则由式(16)和式(17)组成的系统有一个全局稳定的平衡点(z,
eω)＝(0,0)，且z,r,eω均有界。
2.如权利要求1所述的基于观测器的四旋翼无人机容错控制方法，其特征是，由式(16)
和式(17)组成的系统有一个全局稳定的平衡点(z,eω)＝(0,0)，且z(t),r(t),eω(t)均有界
的证明步骤是采用基于Lyapunov的分析方法进行证明，具体地：
定义滑模面其中Ks为一3×3的正常数增益对角矩阵，证明当s渐近收敛到0
时，和ev也渐近收敛到0的过程是：
对s求导，并将代入得
$J\stackrel{\·}{s}=\Π(\mathrm{\ω},{\mathrm{\ω}}_d,{\stackrel{\·}{\mathrm{\ω}}}_d,e_q)+LF\widehat{\mathrm{\λ}}-rLFz---\left(18\right)$
其中：
$\Π(\mathrm{\ω},{\mathrm{\ω}}_d,{\stackrel{\·}{\mathrm{\ω}}}_d,e_q)=-S(\widetilde{\mathrm{\ω}}+{\tilde{R}}^T{\mathrm{\ω}}_d)J(\widetilde{\mathrm{\ω}}+{\tilde{R}}^T{\mathrm{\ω}}_d)+J\left(S\right(\widetilde{\mathrm{\ω}}){\tilde{R}}^T{\mathrm{\ω}}_d-{\tilde{R}}^T{\stackrel{\·}{\mathrm{\ω}}}_d)+\frac{1}{2}{\mathrm{JK}}_s\left(S\right(e_v)+e_0{I}_3)\widetilde{\mathrm{\ω}}$，
rLFz是有界的，因此假设||-rLFz||≤ρ，ρ为正常数，设计控制输入F为
$F=-\left|\right|\widehat{\mathrm{\λ}}|{|}^{-2}\·L^R(\Π(\mathrm{\ω},{\mathrm{\ω}}_d,{\stackrel{\·}{\mathrm{\ω}}}_d,e_q)+\mathrm{\Γ}s+\mathrm{\ρ}sign(s\left)\right)\·{\widehat{\mathrm{\λ}}}^T---\left(19\right)$
其中LR＝LT(LLT)-1表示矩阵L的伪逆矩阵，Γ为一3×3的正常数增益对角矩阵，sign为
符号函数，将式(19)代入式(18)，采用基于Lyapunov的分析方法可以证明闭环系统全局渐
近稳定，即当时间趋于无穷时，滑模面s渐近收敛到0，则和ev也渐近收敛到0。

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

技术领域

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

背景技术

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

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

对于上述两种容错控制策略，国内外很多研究单位，如麻省理工学院、瑞士联邦理
工大学、康考迪亚大学、南京航空航天大学、北京航空航天大学等，基于多种线性或非线性
控制方法开展了相关研究，如变增益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}和目标坐标系{Bd}，通过分析执行器对四旋翼无人机的作用原
理，用未知对角矩阵表示执行器故障对其动力学特性的影响，得到四旋翼无人机执行器发
生故障时的非线性动力学模型：

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

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

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

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

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

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

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

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

定义其中W1,W2,W3分别为：

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

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

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

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

因此写为：

其中表示求取eω1Δ1+eω2Δ2+eω3Δ3的和，Δj＝[δ1j δ2j δ3j]∈R4×3,δjj＝
0,j＝1,2,3，将式(15)代入式(7)，整理得

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

设计r，分别满足：

其中，r(0)表示r的初值，c,m,p均为正常数，且满足c≥3/(2γ)，表示Δj的上
界，||·||表示2范数，I3为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的分析方法进行证明，具体地：

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

对s求导，并将代入得

其中：

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

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

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

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

附图说明：

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

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

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

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

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

d是控制输入变化曲线；

e是电机转速变化曲线；

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

g是故障估计变化曲线；

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

具体实施方式

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

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

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

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

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

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

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

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

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

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

定义其中W1,W2,W3分别为：

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

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

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

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

因此写为：

其中表示求取eω1Δ1+eω2Δ2+eω3Δ3的和，Δj＝[δ1j δ2jδ3j]∈R4×3,δjj＝
0,j＝1,2,3，将式(15)代入式(7)，整理得

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

设计r，分别满足：

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

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

对s求导，并将代入可得

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

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

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

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

一、实验平台简介

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

二、容错控制实验

本发明所采用方法中涉及的各参数取值如下：J＝diag{[1.25 1.25 2.5]T}kg·
m2，l＝0.225m，ε＝0.25，c＝2.5，γ＝1，p＝1，m＝50，Ks＝diag{[2. 22.5 4]T}，ρ＝0.40，Γ
＝diag{[1.86 1.86 3.05]T}。初始姿态角和角速度分别为qd＝[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)分别表示对故
障的估计值和估计误差的变化曲线，均为稳定状态，与理论计算结果相符。

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