[发明专利]三旋翼无人机舵机堵塞故障下位姿系统的容错控制方法

权利要求书

1.一种三旋翼无人机舵机堵塞故障下位姿系统的容错控制方法，其特征是，步骤如下：首先定义惯性坐标系{I}、机体坐标系{B}，通过分析三旋翼无人机的动力学特性，并同时考虑外部扰动以及未知空气阻尼的影响，得到三旋翼无人机发生舵机堵塞故障时的非线性动力学模型：采用浸入-不变集方法进行高度通道控制器设计；采用浸入-不变集方法结合RISE算法进行平动系统控制器设计，实现三旋翼无人机容错控制，具体步骤细化如下，得到三旋翼无人机发生舵机堵塞故障时的非线性动力学模型：

Π₁:

Π₂:

式(1)中各变量定义如下：参数Π₁表示高度通道动力学模型，参数Π₂表示平动系统动力学特性，包括横向位置通道、纵向位置通道、滚转通道和俯仰通道,在系统Π₁中，参数m表示三旋翼无人机的质量，z为其高度，k_z表示高度通道的空气阻尼系数，g为重力加速度，T为各电机产生的总升力，φ和θ分别为滚转角和俯仰角，和分别表示z的一阶时间导数和二阶时间导数，在系统Π₂中，J₁和J₂分别表示滚转通道和俯仰通道的转动惯量，k_φ和k_θ分别表示滚转通道和俯仰通道的空气阻尼系数，d_φ和d_θ分别为滚转通道和俯仰通道的外部扰动力矩，τ_φ和τ_θ分别为滚转通道和俯仰通道的控制输入力矩，x和y分别为三旋翼无人机水平横向和纵向的位置，k_x和k_y为水平方向的空气阻尼系数，ψ表示偏航角，在后续讨论中，参数k_x,k_y,k_z,k_θ,d_θ均假设为未知；

滚转通道、俯仰通道输入力矩及总升力与各电机升力及舵机角度之间的关系表示为：

其中f₁,f₂,f₃分别为三个电机产生的升力，δ为舵机偏转的角度，l表示无人机前方某一电机中心到无人机轴心的距离，l₃表示舵机中心到无人机轴心的距离，α表示前方两个电机连线与任一电机和无人机轴心连线之间的夹角；

为后续分析设计方便，作如下变量替换：

将式(2)和式(3)代入式(1)，整理可得：

Π₁:

Π₂:

在参数k_x,k_y,k_z,k_θ,d_θ和舵机故障λ(t)未知的情况下设计u₁,u₂,T使得三旋翼无人机跟踪目标轨迹[x_d y_d z_d]^T：

高度通道控制器设计

定义高度跟踪误差e_z和滤波误差信号r_z分别为e_z＝z_d-z和其中α_z为一正常数，基于浸入-不变集方法得到未知参数自适应律和总升力分别为：

式(6)中，表示对k_z的估计值，表示对的估计律，β_z为一辅助函数，设计为：

其中γ_z为一正常数，表示对从0到关于r_z求定积分，σ为自变量，表示β_z关于e_z的偏导数，表示β_z关于r_z的偏导数，式(6)中，k_r和k_ez均为正常数；

平动系统控制器设计

为后续分析设计方便，对式(4)中的系统Π₂写为：

Π₂:

其中，各变量定义如下：

对于横向位置和纵向位置通道，定义向量X₁＝[x y]^T∈R²和其中∈R²表示二维向量，定义目标跟踪位置X_1d＝[x_d y_d]^T∈R²，则位置跟踪误差表示为：E₁＝X_1d-X₁，E₂＝X_2d-X₂，其中X_2d为一虚拟控制输入，定义空气阻尼系数向量为K_p＝[K_x K_y]^T，采用浸入-不变集方法得到空气阻尼系数的估计律表示为：

其中表示对K_p的估计值，表示对的估计律，K₁为一2行2列的正常数对角矩阵，v_pd＝[v_xd v_yd]^T为外环虚拟控制输入量，v_xd和v_yd分别为横向通道和纵向通道的虚拟输入，△v_p为虚拟输入与实际输入间的偏差，β_p为一辅助函数，设计为：

其中K₁为一2行2列的正常数对角矩阵，表示对从0到关于E₂求定积分，σ为自变量，表示β_p关于的偏导数，表示β_p关于的偏导数；

对于滚转通道和俯仰通道，由前述虚拟控制输入求解得到目标滚转角和俯仰角分别表示为：

其中arcsin和arccos分别表示反正弦和反余弦函数，其余变量前文均已定义；

定义滚转通道跟踪误差φ_e以及滤波误差信号s_φ，r_φ分别为：

定义俯仰通道跟踪误差θ_e以及滤波误差信号s_θ，r_θ分别为：

θ_e＝θ_d-θ,

其中α_φ,α_θ,β_φ,β_θ均为正常数增益；

采用RISE算法得到滚转通道和俯仰通道的控制输入分别表示如下：

在式(15)中，g_φ,h_φ为正常数增益，r_φ(t),r_φ(0)分别表示在t时刻和0时刻参数r_φ的取值，表示对(g_φ+1)(r_φ(τ)+h_φsign(s_φ(τ))从0到关于t求定积分，τ为自变量，sign表示符号函数；在式(16)中，g_θ,h_θ,K_T为正常数增益，r_θ(t),r_θ(0)分别表示在t时刻和0时刻参数r_θ的取值，表示对从0到关于t求定积分，τ为自变量，表示的T的一阶时间导数，表示的倒数，为λ的估计值，其满足如下自适应律：

其中为的导数，γ_θ为正常数，为U₂的导数；

经过上述分析，滚转角和俯仰角的跟踪误差渐近稳定，进而得到高度以及水平横向位置和纵向位置的跟踪误差也渐近稳定。