[发明专利]针对无人机舵机故障的姿态鲁棒自适应容错控制方法

权利要求书

1.一种针对无人机舵机故障的姿态鲁棒自适应容错控制方法，适用于三旋翼无人机，其特征在于，包括：

当无人机舵机故障时，确定当前电机升力向量F、当前姿态四元数q、当前姿态角速度Ω以及目标姿态四元数q_d；

根据所述当前电机升力向量F、所述当前姿态角速度Ω以及故障估计值和按照预先设计的滑模自适应观测器对故障进行观测，确定并更新姿态角速度估计值

根据所述姿态角速度估计值和所述当前姿态角速度Ω，计算并更新姿态角速度估计误差e_Ω；

根据所述当前姿态四元数q和所述目标姿态四元数q_d，确定姿态误差四元数e_q；

根据所述姿态角速度估计误差e_Ω、所述姿态误差四元数e_q，按照预先设计的容错控制器，计算电机升力向量，以按照所述电机升力向量控制调整无人机姿态；

根据所述当前电机升力向量F、所述姿态角速度估计误差e_Ω和姿态误差四元数e_q，按照预先设计的故障估计值和自适应律，估算更新故障估计值和

定义惯性坐标系I、机体坐标系B和目标坐标系B_d，预先构建无人机舵机故障时的第一非线性动力学模型，

其中，Ω＝[Ω₁ Ω₂ Ω₃]^T∈R^3×1，表征为机体坐标系B相对于惯性坐标系I的姿态角速度，Ω₁,Ω₂,Ω₃分别表征为滚转角速度、俯仰角速度和偏航角速度，[·]^T表示矩阵的转置，∈表示集合间的“属于”关系，R^3×1表示3行1列的实数向量，表示求取Ω的一阶时间导数；J＝[J₁ J₂ J₃]∈R^3×3表征为转动惯量矩阵，J₁,J₂,J₃分别表征为三旋翼无人机绕三个轴的转动惯量；×表示叉乘，Ω×JΩ表示求取Ω和JΩ的向量积；

为一系数矩阵，其中l表示前面某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前面两个电机连线与某一电机和无人机轴心连线之间的夹角，k为电机的升力系数，δ为舵机发生堵塞时的偏转角度，l,l₃,α,k均为已知常数，δ为未知常数；F＝[F₁ F₂ F₃]^T∈R^3×1表征为电机升力向量，F₁,F₂,F₃分别表征为故障发生后三个电机产生的升力，·表示点乘，Δ(δ)·F表示求取Δ(δ)和F的内积；D＝[d₁ d₂ d₃]^T∈R^3×1为角速度变化引起的扰动系数；

定义故障变量β₁，β₂，β₁＝l₃cosδ，β₂＝kcosδ+l₃sinδ，变换所述第一非线性动力学模型的变量，得到含有所述故障变量β₁，β₂的第二非线性动力学模型，

根据所述第二非线性动力学模型，预先设计所述滑模自适应观测器，以对故障进行观测，

所述滑模自适应观测器为：

其中，表征为姿态角速度估计值，也即表示对Ω的估计值，表示求取的一阶时间导数，v＝[v₁ v₂ v₃]^T∈R^3×1，∫vdt表示求取v关于时间的积分，定义Ω的估计误差，也即姿态角速度估计误差故障估计值分别表示故障变量β₁，β₂的估计值，SIG1＝[k₁₁|e_Ω1|^1/2sign(e_Ω1)k₁₂|e_Ω2|^1/2sign(e_Ω2)k₁₃|e_Ω3|^1/2sign(e_Ω3)]^T，v＝[k₂₁sign(e_Ω1)k₂₂sign(e_Ω2)k₂₃sign(e_Ω3)]^T，其中k₁₁,k₁₂,k₁₃,k₂₁,k₂₂,k₂₃均为正常数，e_Ω1,e_Ω2,e_Ω3为姿态角速度估计误差e_Ω的三个元素，sign表示符号函数，|·|^1/2表示求取绝对值的次方，

所述根据所述当前姿态四元数q和目标姿态四元数q_d，确定姿态误差四元数e_q，包括：

采用基于单位四元数的姿态表示方法，机体坐标系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的目标姿态角速度，

还包括：

定义误差信号变量s，r，预先设计容错控制器，

s＝e_Ω+k_se_v，其中，k_s∈R^3×3，均为常系数矩阵，

所述容错控制器为：

其中，为控制器增益矩阵，diag{[σ₁ σ₂ σ₃]}表示求取以σ₁,σ₂,σ₃为对角线元素的对角矩阵；k_f∈R^3×3为常系数矩阵，I₃为3行3列的单位矩阵，r(t)为r在t时刻的取值，r(0)为r在0时刻的参数，表示对(k_f+I₃)r(τ)+σsign(s(τ))关于τ从0到t积分；表示求取矩阵的逆矩阵。