[发明专利]基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法

权利要求书

1.一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，其特征在于：包括如下步骤：

(1)建立存在外部干扰的四旋翼运动学/动力学模型，具体如下：

定义四旋翼编队数量集合Γ＝(1,2,...,n),i∈Γ，给出n架四旋翼组成编队中的第i架四旋翼运动学/动力学模型：

其中：m_i为第i架四旋翼的质量，t为时间，G_i＝[0,0,m_ig]^T，g为重力加速度，J_i＝diag(J_i,1,J_i,2,J_i,3)∈R^3×3表示一个正定的对角惯性矩阵，J_i,1、J_i,2、J_i,3分别是第i架四旋翼在机体坐标系下沿x,y,z轴的转动惯量；g_1,i＝[c(ψ_i)s(θ_i)c(φ_i)+s(ψ_i)s(φ_i),s(ψ_i)s(θ_i)c(φ_i)-c(ψ_i)s(φ_i),c(θ_i)c(φ_i)]^T表示与姿态相关的位置回路输入矩阵，s(·)与c(·)分别表示正弦函数和余弦函数；

p_i＝[x_i,y_i,z_i]^T,Θ_i＝[φ_i,θ_i,ψ_i]^T分别表示第i个四旋翼在惯性坐标系下的位置矢量和在机体坐标系下的姿态角；Π_1,i＝diag(k_x,i,k_y,i,k_z,i),Π_2,i＝diag(k_φ,i,k_θ,i,k_ψ,i)分别是第i个四旋翼位置和姿态回路的空气阻尼矩阵，k_r,i∈R,是第i架四旋翼的空气阻尼系数；τ_i＝[τ_x,iτ_y,iτ_z,i]^T为绕机体x,y,z轴的三个控制力矩，控制输入u_i∈R为第i架四旋翼的拉力；g_2,i＝diag(l_i,l_i,c_i)∈R^3×3，其中l_i是螺旋桨到四旋翼质心的几何距离，c_i是力矩系数，d_Θ,i(t)＝[d_φ,i,d_θ,i,d_ψ,i]^T表示姿态回路中的有界外部干扰；

为便于后续位置控制器和姿态控制器的构造，引入如下符号定义：

其中，F_i＝[F_i,x,F_i,y,F_i,z]^T∈R^3×1表示虚拟控制输入量，d_4,i为姿态回路的集总扰动，包含外部扰动和参数不确定性的综合影响，δ_1,i、δ_2,i是第i架四旋翼的参数化不确定性矩阵，分别是Π_2,i、g_2,i的标称值；

借助上述中间变量，将四旋翼运动/动力学模型(1)改写为如下严格反馈形式：

(2)利用代数图论知识，设计多四旋翼主从式通讯拓扑和编队样式以及领航者的位置和速度信息，具体如下：

四旋翼编队采用主从式结构，定义领航者是一个编号为0的节点，每一个从机依次被编号为1，…，n；四旋翼之间的通信拓扑可用无向图G＝{V,E,A}表示；V为节点集，E为边集，A＝[a_ij]∈R^n×n是邻接权重矩阵；如果四旋翼i和四旋翼j相连，则a_ij＝a_ji＞0，否则a_ij＝a_ji＝0，此外，定义a_ii＝0；领航者和从机i之间的连通权重用b_i表示，如果第i架从机可以直接获取领航者信息，则b_i＞0，否则b_i＝0；

定义领航者为多四旋翼编队样式的几何中心，即编队样式坐标原点，根据期望的编队几何形状，设计第i架和第j架从机相对于领航者的位置矢量Δ_i＝[Δ_i,x,Δ_i,y,Δ_i,z]^T和Δ_j＝[Δ_j,x,Δ_j,y,Δ_j,z]^T，第i架和第j架从机间的相对位置偏差可由Δ_ij＝Δ_i-Δ_j＝[Δ_i,x,Δ_i,y,Δ_i,z]^T-[Δ_j,x,Δ_j,y,Δ_j,z]^T＝[Δ_ij,x,Δ_ij,y,Δ_ij,z]^T描述；

领航者轨迹运动信息可由下式产生：

其中，x^d、y^d、z^d分别为领航者在惯性坐标系下沿x,y,z轴的位置分量，分别为领航者在惯性坐标系下沿x,y,z轴的速度分量；

(3)针对步骤(1)中建立的四旋翼运动学模型和步骤(2)中的多四旋翼通讯拓扑和编队样式以及领航者的位置和速度信息，构造适用于主从式编队任务且具有渐近收敛能力的多四旋翼分布式位置保持控制器，为后续姿态控制器构造提供期望指令，多四旋翼分布式位置保持控制器具体如下：

根据多四旋翼主从式编队通讯拓扑和期望领航者轨迹信息，结合多智能体二阶一致性原理，构造如下虚拟控制输入(F_i,x,F_i,y,F_i,z)^T：

其中，k₁、k₂表示待设计的控制器参数，且N_i表示与第i架四旋翼具有直接通信连接的四旋翼集合，x_i为第i架四旋翼在惯性坐标系下沿x轴的位置分量，x_j为第j架四旋翼沿x轴的位置分量，v_i,x为第i架四旋翼在惯性坐标系下沿x轴的速度分量，v_j,x为第j架四旋翼惯性坐标系下沿x轴的速度分量；

在获得上述虚拟控制输入(F_i,x,F_i,y,F_i,z)^T的基础上，结合公式(2)进行逆动力学解算，可得到如下的期望姿态角指令

其中，u_i为四旋翼无人机的期望拉力，分别为机体坐标系下的期望滚转角、俯仰角以及偏航角；

(4)针对步骤(1)中建立的四旋翼动力学模型和步骤(3)生成的姿态期望指令，构造基于自抗扰控制的多四旋翼姿态跟踪控制器，具体如下：

其中，为正的控制器比例增益，k_d＝2w_c为正的控制器微分增益，w_c为姿态回路的控制器带宽；为姿态回路集总干扰的估计值，可由如下仅依赖于控制输入力矩和姿态角量测输出的模型辅助扩张状态观测器给出：

其中，z_1,i是对x_3,i的估计，z_2,i是对x_4,i的估计，z_3,i是对的估计，w_o为姿态回路观测器带宽。