CN109324636B - 基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法 - Google Patents

Abstract

本发明公开了一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法。针对多四旋翼编队飞行过程中几何队形生成、稳固保持和协同抗干扰问题，提出了一种可应对外部环境干扰和气动参数不确定性的多四旋翼主从式协同编队控制方法：首先，建立存在外部干扰的四旋翼运动学/动力学模型；其次，设计多四旋翼主从式通讯拓扑和编队样式以及领航者的位置和速度信息；然后，构造多四旋翼分布式位置保持控制器，为后续姿态控制器构造提供必要的期望指令；最后，构造基于自抗扰控制的多四旋翼姿态跟踪控制器。所提编队控制方法可以在局部智能体通信的前提下显著改善四旋翼编队***的抗干扰能力，提升干扰环境下多四旋翼编队几何构型的稳固性。

Description

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

技术领域

本发明涉及导航制导方向，多四旋翼编队领域，具体为一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，主要应用于存在外部干扰以及参数不确定性的多四旋翼编队任务。

背景技术

多四旋翼协同编队指的是多四旋翼通过保持预定的空间几何拓扑形态，借助全局或局部信息交互与共享，实现多旋翼***的群体集聚行为，形成多智能体***的协同超越能力，为执行单体所无法胜任的复杂性任务提供有效的解决方案。例如，多四旋翼可采用合理的编队飞行替代士兵执行恶劣、危险环境下诸如目标侦查、敌情搜集等军事任务，也可用于复杂山区地形环境人员搜救、矿产勘测等民用场合，可极大地弥补单体获取环境信息不完整、时效性不足的弊端，具有重要的军/民两用研究价值与迫切的现实意义。

多四旋翼协同编队控制作为多智能体***协同控制理论的应用实例之一，与无人车、无人船等二自由度运动体编队相比，在控制器构造与综合方面更具挑战性，表现在：(1)四旋翼本身为欠驱动、强非线性的多输入多输出六自由度***，为保证编队几何队形的稳固性，必须使得编队飞行过程中各四旋翼的位置、速度等协调变量趋于一致。然而，目前多智能体一致性理论大多针对低阶、线性等理想质点运动学模型展开，无法直接适用于多四旋翼编队***模型；(2)四旋翼的空气动力学参数受限于缺乏相应测量仪器无法精确获得，给模型带来了较大参数不确定性。此外，四旋翼空间飞行过程中不可避免遭受外部未知风扰的影响，上述因素给四旋翼的编队保持抗干扰控制器构造带来了困难。因而，迫切需要在考虑编队几何构型约束和参数不确定性以及外部干扰的情况下，构造适用于四旋翼典型动力学特征并具有强鲁棒性的多四旋翼协同编队控制方法。

发明内容

本发明为了解决多四旋翼编队飞行过程中几何队形生成、稳固保持和协同抗干扰的问题，基于领航者—跟随者的协同控制策略，提供了一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法。

本发明是通过如下技术方案来实现的：一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，包括如下步骤：

(1)建立存在外部干扰的四旋翼运动学/动力学模型；

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

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

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

本发明所提供的基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，具体为在四旋翼轨迹回路中引入二阶一致性理论，结合代数图论和通信拓扑，以领航者位置作为动态编队的几何中心，将多四旋翼期望几何队形生成和保持问题转化为从机位置一致性跟踪与领航者的相对位置偏差控制问题，从而构造具有渐近收敛能力的多四旋翼分布式位置保持控制器；对于姿态回路的参数不确定性和外部干扰，借鉴自抗扰控制思想，将参数不确定性和外部干扰视为集总扰动，采用扩张状态观测器对其进行在线观测与补偿，实现对于给定姿态的高精度跟踪控制。

上述步骤中：步骤(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)的多四旋翼分布式位置保持控制器具体如下：

根据多四旋翼主从式编队通讯拓扑和期望领航者轨迹信息，结合多智能体二阶一致性原理，构造如下虚拟控制输入(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)多四旋翼姿态跟踪控制器具体如下：

其中，

为正的控制器比例增益，k_d＝2w_c为正的控制器微分增益，w_c为姿态回路的控制器带宽；

为姿态回路集总干扰的估计值，可由如下仅依赖于控制输入力矩和姿态角量测输出的模型辅助扩张状态观测器给出：

其中，z_1,i是对x_3,i的估计，z_2,i是对x_4,i的估计，z_3,i是对

的估计，w_o为姿态回路观测器带宽。

本发明与现有技术相比本发明具有以下有益效果：与目前大多数的多智能体一致性理论仅考虑低阶线性的模型不同，本发明充分考虑四旋翼的六自由度非线性、强耦合运动学/动力学模型；针对编队几何构型约束和参数不确定性以及外部干扰，构造了用于四旋翼姿态回路的抗干扰控制器，解决了多四旋翼编队的位置保持控制以及姿态跟踪控制问题，可以极大改善四旋翼编队***的抗干扰能力，提升干扰环境下多四旋翼编队几何构型的稳固性，对于丰富和发展多智能体协同控制理论具有重要意义。

附图说明

图1是本发明一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法的控制框图。

图2是以领航者为坐标原点的多四旋翼编队通信拓扑和编队样式。

图3是惯性坐标系下多四旋翼编队运动轨迹在x-y平面的投影。

图4是每架四旋翼的位置响应及相邻距离变化曲线。

图5是每架四旋翼的姿态角响应曲线。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细说明。

一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，如图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)生成的姿态期望指令，构造基于自抗扰控制的多四旋翼姿态跟踪控制器：步骤(4)多四旋翼姿态跟踪控制器具体如下：

其中，

为正的控制器比例增益，k_d＝2w_c为正的控制器微分增益，w_c为姿态回路的控制器带宽；

为姿态回路集总干扰的估计值，可由如下仅依赖于控制输入力矩和姿态角量测输出的模型辅助扩张状态观测器给出：

其中，z_1,i是对x_3,i的估计，z_2,i是对x_4,i的估计，z_3,i是对

的估计，w_o为姿态回路观测器带宽。

实施例：

为考核所构造的控制器性能，采用式(1)描述的四旋翼运动学/动力学模型进行仿真。四旋翼运动学/动力学模型参数如表1所示。

用于控制器构造的模型参数选择如下：四旋翼质量m_i为2kg，重力加速度g为9.8m/s²，惯性矩阵J_i＝diag(J_i,1,J_i,2,J_i,3)＝diag(0.16,0.16,0.32)kgm²，空气阻尼矩阵Π_1,i＝diag(k_x,i,k_y,i,k_z,i)＝diag(0.01,0.01,0.01)Nms²，空气阻尼矩阵Π_2,i＝diag(k_φ,i,k_θ,i,k_ψ,i)＝diag(0.01,0.01,0.01)Nms²，螺旋桨到四旋翼质心的几何距离l_i为0.4米，力矩系数c_i为0.05，参数不确定性δ_1,i、δ_2,i分别为

和

表1四旋翼运动学/动力学模型参数

本发明所考虑的多四旋翼编队的通讯拓扑和编队样式如图2所示，显然，参与编队的四旋翼数量为5，即n＝5。无向图的邻接权重系数如下：a₁₂＝a₂₁＝1,a₂₃＝a₃₂＝1,a₃₄＝a₄₃＝1,b₁＝1。为了便于分析，选择在惯性坐标系x-y平面中确定的正方形作为编队样式，且边长为2米，以领航者为编队样式几何中心，故从机相对于领航者的位置矢量为：Δ₁＝[1,1,0]^T,Δ₂＝[-1,1,0]^T,Δ₃＝[-1,-1,0]^T,Δ₄＝[1,-1,0]^T。领航者的轨迹设置为：(x^d,y^d,z^d)^T＝(0.5t,5cos(0.2t),0.5t)^T。各从机的姿态角及角速度初始状态为零，位置和速度初始状态如下设计：

[x₁(0),y₁(0),z₁(0),v_1,x(0),v_1,y(0),v_1,z(0),]＝[0.8,0,2,0,0,0]

[x₂(0),y₂(0),z₂(0),v_2,x(0),v_2,y(0),v_2,z(0),]＝[0,0.8,5,0,0,0]

[x₃(0),y₃(0),z₃(0),v_3,x(0),v_3,y(0),v_3,z(0),]＝[-0.5,0.8,0,0,0,0]

[x₄(0),y₄(0),z₄(0),v_4,x(0),v_4,y(0),v_4,z(0),]＝[-0.5,0,0.8,0,0,0]

为调整控制器的性能，对控制器参数进行选取。控制器参数如表2所示。

控制器参数选择如下：位置保持控制器参数k₁为1.7，位置保持控制器参数k₂为1.7，姿态控制器比例增益k_p为25，姿态控制器微分增益k_d为10。

表2控制器参数

为了考验控制器的抗干扰性，在四旋翼姿态回路中设置了如下外部扰动：

惯性坐标系下多四旋翼编队运动轨迹在x-y平面的投影如图3所示，可以看出，在所提供控制方法的作用下，即使存在未知的外部干扰和参数不确定性，多四旋翼依然可以保持期望的正方形编队构型。

每架四旋翼的位置响应曲线如图4所示，由图中可以看出，各个四旋翼轨迹状态在有限时间内保持较好的一致性；此外，从机之间的相邻距离可以快速、稳定收敛，最终与期望的几何构型边长2米保持一致。

每架四旋翼的姿态角响应曲线如图5所示，由图中可以看出，尽管存在外部干扰以及参数不确定性，四条响应曲线较快的趋于平稳一致，表示各四旋翼姿态角响应平稳光滑、最终趋于一致，控制效果达到预期。

综上所述，从本实施例中可以得出如下结论：本发明涉及的一种基于二阶一致性和自抗扰的多四旋翼主从式协同编队控制方法，在外部干扰以及参数不确定性的影响下，可以提升多四旋翼编队队形的稳固性，提高多四旋翼编队***的抗干扰能力。

本发明要求保护的范围不限于以上具体实施方式，而且对于本领域技术人员而言，本发明可以有多种变形和更改，凡在本发明的构思与原则之内所作的任何修改、改进和等同替换都应包含在本发明的保护范围之内。

Claims (1)

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为姿态回路观测器带宽。

Images