[发明专利]针对三旋翼无人机舵机堵塞故障的容错控制方法

权利要求书

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为未知常数，和分别表示正弦和余弦函数；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)可表示为

为避免姿态表示奇异性问题，采用基于单位四元数的姿态表示方法，机体坐标系{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_ω的三个元素，表示符号函数，表示求取绝对值的次方；

定义变量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,γ₁,γ₂,γ₃均为正常数，分别表示滚转角速度误差、俯仰角速度误差和偏航角速度误差，则姿态误差四元数和角速度误差渐近稳定。

2.如权利要求1所述的针对三旋翼无人机舵机堵塞故障的容错控制方法，其特征是，还包括验证步骤，验证步骤具体是，采用基于Lyapunov的分析方法可以证明当时间趋于无穷时，和e_v分别渐近收敛到[0 0 0]^T。