CN106842952A - 针对三旋翼无人机舵机堵塞故障的容错控制方法 - Google Patents

Abstract

本发明涉及三旋翼无人机容错控制技术，为针对三旋翼无人机发生舵机堵塞故障时的姿态控制问题开展研究，本发明采用的技术方案是，针对三旋翼无人机舵机堵塞故障的容错控制方法，步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}，通过分析舵机对三旋翼无人机的作用原理，并考虑外部扰动对其动力学特性的影响，得到三旋翼无人机执行器发生故障时的非线性动力学模型，设计相应的观测器、控制器。本发明主要应用于三旋翼无人机控制场合。

Description

针对三旋翼无人机舵机堵塞故障的容错控制方法

技术领域

本发明涉及三旋翼无人机容错控制技术，具体讲,涉及针对三旋翼无人机舵机堵塞故障的容错控制方法。

背景技术

近年来，多旋翼无人机在高空摄影、灾后救援、环境监测等军事和民用领域得到越来越广泛的应用。与传统四旋翼无人机、六旋翼无人机不同，三旋翼无人机通常由三个电机和一个舵机组成，结构更简单、成本更低、能耗更小、机动性更强。三旋翼无人机依靠三个电机的转动及舵机的偏转实现俯仰、滚转、偏航等动作，受无人机飞行稳定性及自身工艺等影响，舵机极易发生堵塞故障，对无人机的飞行性能产生严重影响。

目前国内外很多研究机构已经开始致力于三旋翼无人机的位姿控制研究，但是针对舵机发生堵塞故障时的位姿控制尚没有相关研究文献。三旋翼无人机作为一个四输入六输出的欠驱动系统，当舵机发生堵塞故障时，输入量减少一个，这与四旋翼无人机执行器发生故障的情况类似。针对四旋翼无人机执行器发生故障时的姿态控制问题，目前采用较为广泛的容错控制策略大致有两种：被动容错和主动容错。被动容错利用控制器的鲁棒性使得控制器对故障信息不敏感，从而达到容错控制的目的；而主动容错则通过故障诊断与故障隔离能够在线检测并分离出所发生故障，再根据故障模式进行故障重构，以此达到容错控制目的。

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

但是，当前各种容错控制方法均有其各自的局限性。比如：在对执行器故障进行动力学建模时，将其视为外部扰动力矩，进行了较大程度的近似，难以反映执行器故障对无人机的真实影响(期刊：控制理论与应用；著者：杨荟憭，姜斌，张柯；出版年月：2014年；文章题目：四旋翼直升机姿态系统的直接自我修复控制；页码：1053-1060)；部分容错控制方法在平衡点处对四旋翼无人机的动力学模型进行了线性化处理，理论证明只能得到平衡点附近的稳定结论，当执行器发生故障时，飞行器姿态会发生突变，且多数情况下飞行器姿态会偏离平衡点较大位置，控制器应用范围难以保证(期刊：IEEE Transactions on ControlSystems Technology；著者：Z.T.Dydek，A.M.Annaswamy，E.Lavretsky；出版年月：2013年7月；文章题目：Adaptive Control of Quadrotor UAVs：a Design Trade Study withFlight Evaluations；页码：1400–1406)；被动容错方法应用范围有限，难以做到对外界扰动和执行器故障鲁棒性的兼容性，控制效果较差，而主动容错控制方法则需要进行故障诊断和故障隔离，并在此基础上进行故障重构，算法复杂，难以实现工程应用(期刊：Proceedings of the Institution of Mechanical Engineers,Part I,Journal ofSystems and Control Engineering；著者：T.Li,Y.M.Zhang,B.W.Gordon；出版年月：2012年1月；文章题目：Passive and Active Nonlinear Fault-Tolerant Control of aQuadrotor 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＝diag{[J₁ J₂ J₃]^T}∈R^3×3为转动惯量矩阵，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]张成的对角矩阵，J₁,J₂,J₃分别表示绕各个坐标轴的转动惯量；S(ω)表示求取ω对应的反对称矩阵；为一系数矩阵，其中l表示前面某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前面两个电机连线与某一电机和无人机轴心连线之间的夹角，k为电机的升力系数，δ_f为舵机发生堵塞时的偏转角度，l,l₃,α,k均为已知常数，δ_f为未知常数，sin(·)和cos(·)分别表示正弦和余弦函数；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障发生后的升力向量，f_δ1,f_δ2,f_δ3分别表示故障发生后三个电机产生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3为外部扰动矩阵，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]张成的对角矩阵，d₁,d₂,d₃分别表示作用于各个通道的外部扰动；

定义变量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，则λ₁,λ₂为未知常数，A(δ_f)可写为式(1)可表示为

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{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对应的反对称矩阵，定义角速度误差其中ω_d∈R^3×1表示目标坐标系{B_d}相对于惯性坐标系{I}的姿态角速度；

为了对三旋翼无人机舵机堵塞故障进行更有针对性的容错控制，采用基于自适应滑模方法的观测器技术对故障进行观测，设计观测器为：

其中表示对ω的估计值，表示求取的一阶时间导数，v＝[v₁ v₂ v₃]^T∈R^{3 ×1}，表示求取v的一阶时间导数，定义ω的估计误差为 分别表示对λ₁,λ₂的估计值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2) k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为正常数，e_ω1,e_ω2,e_ω3为e_ω的三个元素，sign(·)表示符号函数，|·|^1/2表示求取绝对值的次方；

定义变量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，设计控制器为：

其中表示求取矩阵的逆矩阵，表示求取ω_d的一阶时间导数，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3为正常数增益矩阵，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]张成的对角矩阵，g₁,g₂,g₃均为正常数。为外部扰动估计矩阵，表示向量张成的对角矩阵，是对d₁,d₂,d₃的估计值；

若设计和的自适应律满足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均为正常数，分别表示滚转角速度误差、俯仰角速度误差和偏航角速度误差，则姿态误差四元数和角速度误差渐近稳定。

验证步骤具体是，采用基于Lyapunov的分析方法可以证明当时间趋于无穷时，和e_v分别渐近收敛到[0 0 0]^T。

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

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

附图说明：

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

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

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

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

c是控制输入变化曲线；

d是电机转速变化曲线；

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

f是舵机堵塞故障估计值变化曲线；

g是外部扰动估计值变化曲线。

具体实施方式

本发明涉及一种三旋翼无人机容错控制问题。针对三旋翼无人机舵机发生堵塞故障时的姿态控制问题，提出一种基于自适应滑模观测器技术的非线性容错控制方法。

为填补现有研究对象的空白，针对三旋翼无人机发生舵机堵塞故障时的姿态控制问题开展研究。本发明采用的技术方案是基于自适应滑模观测器的容错控制方法，利用观测器对故障信息进行观测，控制器的设计采用观测器的观测信息，对执行器故障进行补偿。

具体步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}，通过分析舵机对三旋翼无人机的作用原理，并考虑外部扰动对其动力学特性的影响，得到三旋翼无人机执行器发生故障时的非线性动力学模型：

式(1)中各变量定义如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示机体坐标系{B}相对于惯性坐标系{I}的姿态角速度，ω₁,ω₂,ω₃分别表示滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取ω的一阶时间导数，下同；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3为转动惯量矩阵，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]张成的对角矩阵，J₁,J₂,J₃分别表示绕各个坐标轴的转动惯量；S(ω)表示求取ω对应的反对称矩阵；为一系数矩阵，其中l表示前面某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前面两个电机连线与某一电机和无人机轴心连线之间的夹角，k为电机的升力系数，δ_f为舵机发生堵塞时的偏转角度，l,l₃,α,k均为已知常数，δ_f为未知常数，sin(·)和cos(·)分别表示正弦和余弦函数；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障发生后的升力向量，f_δ1,f_δ2,f_δ3分别表示故障发生后三个电机产生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3为外部扰动矩阵，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]张成的对角矩阵，d₁,d₂,d₃分别表示作用于各个通道的外部扰动。

为方便分析，定义变量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，则λ₁,λ₂为未知常数，A(δ_f)可写为式(1)可表示为

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{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对应的反对称矩阵。定义角速度误差其中ω_d∈R^3×1表示目标坐标系{B_d}相对于惯性坐标系{I}的姿态角速度。

为了对三旋翼无人机舵机堵塞故障进行更有针对性的容错控制，采用基于自适应滑模方法的观测器技术对故障进行观测，设计观测器为：

其中表示对ω的估计值，表示求取的一阶时间导数，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一阶时间导数，定义ω的估计误差为 分别表示对λ₁,λ₂的估计值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2)k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为正常数，e_ω1,e_ω2,e_ω3为e_ω的三个元素，sign(·)表示符号函数，|·|^1/2表示求取绝对值的次方。

定义变量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，设计控制器为：

其中表示求取矩阵的逆矩阵，表示求取ω_d的一阶时间导数，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3为正常数增益矩阵，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]张成的对角矩阵，g₁,g₂,g₃均为正常数。为外部扰动估计矩阵，表示向量张成的对角矩阵，是对d₁,d₂,d₃的估计值。

若设计和的自适应律满足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均为正常数，分别表示滚转角速度误差、俯仰角速度误差和偏航角速度误差，则姿态误差四元数和角速度误差渐近稳定。采用基于Lyapunov的分析方法可以证明当时间趋于无穷时，和e_v分别渐近收敛到[0 0 0]^T。

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

首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}，通过分析舵机对三旋翼无人机的作用原理，并考虑外部扰动对其动力学特性的影响，得到三旋翼无人机执行器发生故障时的非线性动力学模型：

式(1)中各变量定义如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示机体坐标系{B}相对于惯性坐标系{I}的姿态角速度，ω₁,ω₂,ω₃分别表示滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取ω的一阶时间导数，下同；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3为转动惯量矩阵，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]张成的对角矩阵，J₁,J₂,J₃分别表示绕各个坐标轴的转动惯量；S(ω)表示求取ω对应的反对称矩阵；为一系数矩阵，其中l表示前面某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前面两个电机连线与某一电机和无人机轴心连线之间的夹角，k为电机的升力系数，δ_f为舵机发生堵塞时的偏转角度，l,l₃,α,k均为已知常数，δ_f为未知常数，sin(·)和cos(·)分别表示正弦和余弦函数；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障发生后的升力向量，f_δ1,f_δ2,f_δ3分别表示故障发生后三个电机产生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3为外部扰动矩阵，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]张成的对角矩阵，d₁,d₂,d₃分别表示作用于各个通道的外部扰动。

为方便分析，定义变量λ₁＝-l₃cosδ_f，λ₂＝kcosδ_f+l₃sinδ_f，则λ₁,λ₂为未知常数，A(δ_f)可写为式(1)可表示为

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{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对应的反对称矩阵。定义角速度误差其中ω_d∈R^3×1表示目标坐标系{B_d}相对于惯性坐标系{I}的姿态角速度。

为了对三旋翼无人机舵机堵塞故障进行更有针对性的容错控制，采用基于自适应滑模方法的观测器技术对故障进行观测，设计观测器为：

其中表示对ω的估计值，表示求取的一阶时间导数，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一阶时间导数，定义ω的估计误差为 分别表示对λ₁,λ₂的估计值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2)k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2sign(e_ω1) k₁₂|e_ω2|^1/2sign(e_ω2) k₁₃|e_ω3|^1/2sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为正常数，e_ω1,e_ω2,e_ω3为e_ω的三个元素，sign(·)表示符号函数，|·|^1/2表示求取绝对值的次方。

定义变量z_1i＝|e_ωi|^1/2sign(e_ωi),z_2i＝v_i,i＝1,2,3，设计控制器为：

其中表示求取矩阵的逆矩阵，表示求取ω_d的一阶时间导数，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3为正常数增益矩阵，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]张成的对角矩阵，g₁,g₂,g₃均为正常数。为外部扰动估计矩阵，表示向量张成的对角矩阵，是对d₁,d₂,d₃的估计值。

若设计和的自适应律满足：

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均为正常数，分别表示滚转角速度误差、俯仰角速度误差和偏航角速度误差，则姿态误差四元数和角速度误差渐近稳定。采用基于Lyapunov的分析方法可以证明当时间趋于无穷时，和e_v分别渐近收敛到[0 0 0]^T。

一、实验平台简介

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

二、容错控制实验

本发明所采用方法中涉及的各参数取值如下：J＝diag{[1 1 2]^T}kg·m²，l＝0.16m，l₃＝0.25m，α＝26°，k＝0.05，D＝diag{[0.1 0.1 0.1]^T},ε₁＝ε₂＝10,m＝2，n＝1，k₁₁＝k₁₂＝5，k₁₃＝4,k₂₁＝k₂₂＝8,k₂₃＝3.5,γ₁＝γ₂＝0.1,γ₃＝0.2。初始姿态四元数和角速度分别为q_d＝[1 0 0 0]^T，ω_d＝[0 0 0]^Trad/s,三旋翼无人机的舵机在第30s发生堵塞故障，舵机堵塞角约为2.5°。实验结果分别如图2(a)、图2(b)、图2(c)、图2(d)、图2(e)、图2(f)、图2(g)所示。

图2(a)表示三旋翼无人机姿态四元数变化曲线，在前30s，三旋翼无人机保持稳定飞行，姿态四元数误差小于0.05，在第30s，舵机发生堵塞故障，无人机姿态发生变化，并在5s内迅速收敛到0。图2(b)为角速度误差的变化曲线，故障发生后，其在2s之内迅速收敛到0。由此可见，控制目标得到很好的实现。图2(c)和图2(d)分别表示控制输入曲线变化和电机转速变化曲线，均在合理变化范围内。图2(e)表示角速度估计误差曲线，故障发生后迅速收敛到0。图2(f)和图2(g)分别表示对故障的估计值和对外部扰动的估计值，均为稳定状态，与理论计算结果相符。

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

Claims (1)

1.一种针对三旋翼无人机舵机堵塞故障的容错控制方法，其特征是，步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}和目标坐标系{B_d}，通过分析舵机对三旋翼无人机的作用原理，并考虑外部扰动对其动力学特性的影响，得到三旋翼无人机执行器发生故障时的非线性动力学模型：

式(1)中各变量定义如下：ω＝[ω₁ ω₂ ω₃]^T∈R^3×1表示机体坐标系{B}相对于惯性坐标系{I}的姿态角速度，ω₁,ω₂,ω₃分别表示滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取ω的一阶时间导数；J＝diag{[J₁ J₂ J₃]^T}∈R^3×3为转动惯量矩阵，diag{[J₁ J₂ J₃]^T}表示向量[J₁ J₂ J₃]张成的对角矩阵，J₁,J₂,J₃分别表示绕各个坐标轴的转动惯量；S(ω)表示求取ω对应的反对称矩阵；为一系数矩阵，其中l表示前面某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前面两个电机连线与某一电机和无人机轴心连线之间的夹角，k为电机的升力系数，δ_f为舵机发生堵塞时的偏转角度，l,l₃,α,k均为已知常数，δ_f为未知常数，sin(·)和cos(·)分别表示正弦和余弦函数；f_δ＝[f_δ1 f_δ2 f_δ3]^T∈R^3×1表示故障发生后的升力向量，f_δ1,f_δ2,f_δ3分别表示故障发生后三个电机产生的升力，D＝diag{[d₁ d₂ d₃]^T}∈R^3×3为外部扰动矩阵，diag{[d₁ d₂ d₃]^T}表示向量[d₁ d₂ d₃]张成的对角矩阵，d₁,d₂,d₃分别表示作用于各个通道的外部扰动；

定义变量λ₁＝-l₃ cosδ_f，λ₂＝k cosδ_f+l₃ sinδ_f，则λ₁,λ₂为未知常数，A(δ_f)可写为式(1)可表示为

$J\stackrel{\·}{\mathrm{\ω}}=-S\left(\mathrm{\ω}\right)J\mathrm{\ω}+C({\mathrm{\λ}}_1,{\mathrm{\λ}}_2)f_{\mathrm{\δ}}+D\·\mathrm{\ω},---\left(2\right)$

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{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对应的反对称矩阵，为了描述三旋翼无人机当前姿态与目标姿态之间的差异，定义姿态误差四元数

$\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)$

其中e₀和e_v同样满足由目标坐标系{B_d}到机体坐标系{B}的坐标变换矩阵示为S(e_v)表示求取e_v对应的反对称矩阵，定义角速度误差其中ω_d∈R^3×1表示目标坐标系{B_d}相对于惯性坐标系{I}的姿态角速度；

为了对三旋翼无人机舵机堵塞故障进行更有针对性的容错控制，采用基于自适应滑模方法的观测器技术对故障进行观测，设计观测器为：

$\left\{\begin{array}{c}J\stackrel{\·}{\widehat{\mathrm{\ω}}}=-S\left(\mathrm{\ω}\right)J\mathrm{\ω}+E\left({\widehat{\mathrm{\λ}}}_1,{\widehat{\mathrm{\λ}}}_2\right)f_{\mathrm{\δ}}-SIG1+v\\ \stackrel{\·}{v}=-\frac{1}{2}SIG2\end{array}\right.,---\left(4\right)$

其中表示对ω的估计值，表示求取的一阶时间导数，v＝[v₁ v₂ v₃]^T∈R^3×1，表示求取v的一阶时间导数，定义ω的估计误差为 分别表示对λ₁,λ₂的估计值，SIG2＝[k₂₁sign(e_ω1) k₂₂sign(e_ω2) k₂₃sign(e_ω3)]^T，SIG1＝[k₁₁|e_ω1|^1/2 sign(e_ω1) k₁₂|e_ω2|^1/2 sign(e_ω2) k₁₃|e_ω3|^1/2 sign(e_ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为正常数，e_ω1,e_ω2,e_ω3为e_ω的三个元素，sign(·)表示符号函数，|·|^1/2表示求取绝对值的次方；

定义变量z_1i＝|e_ωi|^1/2 sign(e_ωi),z_2i＝v_i,i＝1,2,3，设计控制器为：

$f_{\mathrm{\δ}}=E^{-1}({\widehat{\mathrm{\λ}}}_1,{\stackrel{2}{\mathrm{\λ}}}_2)(S\left(\mathrm{\ω}\right)\mathrm{J\ω}-J({\tilde{R}}^{T}S\left(\widetilde{\mathrm{\ω}}\right){\mathrm{\ω}}_d-{\tilde{R}}^T{\stackrel{.}{\mathrm{\ω}}}_d)-G\widetilde{\mathrm{\ω}}-e_v-\hat{D}\·\mathrm{\ω}),---\left(5\right)$

其中表示求取矩阵的逆矩阵，表示求取ω_d的一阶时间导数，G＝diag{[g₁ g₂ g₃]^T}∈R^3×3为正常数增益矩阵，diag{[g₁ g₂ g₃]^T}表示向量[g₁ g₂ g₃]张成的对角矩阵，g₁,g₂,g₃均为正常数。为外部扰动估计矩阵，表示向量张成的对角矩阵，是对d₁,d₂,d₃的估计值；

若设计和的自适应律满足：

$\left\{\begin{array}{c}{\stackrel{\·}{\widehat{\mathrm{\λ}}}}_1=\frac{{\widetilde{\mathrm{\ω}}}_2{f}_{\mathrm{\δ}3}}{2{\mathrm{\ϵ}}_1}-\frac{(m+n^2)z_{12}{f}_{\mathrm{\δ}3}-{\mathrm{nz}}_{22}{f}_{\mathrm{\δ}3}}{2{\mathrm{\ϵ}}_1{J}_2\left|z_{12}\right|}\\ {\stackrel{\·}{\widehat{\mathrm{\λ}}}}_2=\frac{{\widetilde{\mathrm{\ω}}}_3{f}_{\mathrm{\δ}3}}{2{\mathrm{\ϵ}}_2}-\frac{(m+n^2)z_{13}{f}_{\mathrm{\δ}3}-{\mathrm{nz}}_{23}{f}_{\mathrm{\δ}3}}{2{\mathrm{\ϵ}}_2{J}_3\left|z_{13}\right|}\end{array}\right.,---\left(6\right)$

${\stackrel{\·}{\hat{d}}}_i={\mathrm{\γ}}_i{\widetilde{\mathrm{\ω}}}_i{\mathrm{\ω}}_i,i=1,2,3,---\left(7\right)$

其中ε₁,ε₂,m,n,γ₁,γ₂,γ₃均为正常数，分别表示滚转角速度误差、俯仰角速度误差和偏航角速度误差，则姿态误差四元数和角速度误差渐近稳定。

验证步骤具体是，采用基于Lyapunov的分析方法可以证明当时间趋于无穷时，和e_v分别渐近收敛到[0 0 0]^T。