分享自己的一篇文章，发布在人工生命与机器人ICAROB2022，欢迎各位引用。

A Distributed Optimal Formation Control for Multi-Agent System of UAVs

文章目录

  • 1 介绍
  • 2 预备知识
    • 2.1 图论知识
    • 2.2 无人机系统描述
  • 3 主要结果
      • Lemma 1[6]
    • 3.1 编队控制
      • 定理 1
      • 证明
    • 3.2 最优控制
    • 3.3 分布式最优编队控制
      • 定理 2
      • 证明
  • 4 仿真
    • 4.1 数值仿真
    • 4.2 平台仿真
  • 5 结论
  • 附录：程序
    • A.1 main1ConsensusDynamic
    • A.2 main1ConsensusStatic
    • A.3 main2FormationDynamic
    • A.4 main2FormationStatic
    • A.5 main2FormationStatic2
    • A.6 main3OptimalRiccati
    • A.7 main4OptimalFormation
    • A.8 plotResults

本文在此提出了无人机（UAVs）的多智能体系统（MAS）编队控制的分布式优化问题。针对单个无人机的内部状态可以完全获得的情况，利用最优控制理论设计了单个无人机的内部最优控制法。为了应对系统中每个智能体只能与相邻的智能体通信的障碍，根据系统的通信拓扑结构引入了系统的分布式编队控制法，并进一步用图论分析了系统的稳定性。所设计方案的有效性通过数值模拟和无人机平台得到了验证。

1 介绍

随着机器人能力的不断提高，机器人的应用领域也在同时扩大。然而，就像人类一样，单个机器人在很多情况下会表现出较低的能力，这样一来，就需要多个代理的协调来发挥更大的作用。代理人的分布式协调和合作能力是多代理系统的基础，是多代理系统发挥超额优势的关键，也是整个多代理系统智能的体现。

多Agent协调控制的问题包括共识控制[1]、会合控制[2]、编队控制[3]，等等。然而，MAS的优化对系统通信网络有很大的依赖性。因为优化控制法需要能够 实时获得系统中集体个体的所有状态，否则就不能满足优化控制的条件。为了处理这个问题，我们设计了一种分布式最优编队控制，它对系统网络结构是不变的。并将这一成果应用于多个无人驾驶飞行器的编队控制。

本文的主要贡献主要包括三个方面。1）设计了一种基于静态共识协议的编队控制协议。2）研究了单个代理的性能函数为最优时的最优控制法。3）结合编队控制协议和最优控制法，设计了一种分布式优化编队控制协议。该协议不会受到MAS内部通信拓扑结构的影响。即使有些代理不能相互通信，系统也能完成编队任务。

2 预备知识

本节介绍了研究无人机系统的初步知识，包括使用图论来描述系统内的通信关系，单一无人机的动态模型，以及无人机系统的状态空间方程。

2.1 图论知识

为了描述无人机系统的关系，使用了图论[1]。A=[aij∈{0,1}]A=[a_{ij} \in \{0, 1\}]A=[aij​∈{0,1}] 是图 GGG 的邻接矩阵，表示从节点 jjj 到 iii 是否有信息交流。矩阵 DDD 是出度矩阵。节点 iii 的邻居是 NiN_iNi​。 Laplacian矩阵定义为

L=D−A(1)L = D - A \tag{1}L=D−A(1)

2.2 无人机系统描述

如图 1 所示，无人机模型可以简化为公式（2）。

x¨=gθy¨=−gϕz¨=uh/m−gϕ¨=uϕ/Ixθ¨=uθ/Iyψ¨=uψ/Iz(2)\begin{aligned} \ddot{x} &= g \theta \\ \ddot{y} &= -g \phi \\ \ddot{z} &= u_h / m - g \\ \ddot{\phi} &= u_\phi / I_x \\ \ddot{\theta} &= u_\theta / I_y \\ \ddot{\psi} &= u_\psi / I_z \\ \end{aligned} \tag{2}x¨y¨​z¨ϕ¨​θ¨ψ¨​​=gθ=−gϕ=uh​/m−g=uϕ​/Ix​=uθ​/Iy​=uψ​/Iz​​(2)

其中 x,y,zx,y,zx,y,z 是地面坐标系中沿 Xg,Yg,ZgX_g, Y_g, Z_gXg​,Yg​,Zg​ 的位置。ϕ,θ,ψ\phi, \theta, \psiϕ,θ,ψ 分别为无人机的滚转角、俯仰角、偏航角。Ix,Iy,IzI_x, I_y, I_zIx​,Iy​,Iz​ 分别是沿 Xb,Yb,ZbX_b, Y_b, Z_bXb​,Yb​,Zb​ 在机体坐标系中的惯性矩。而 uhu_huh​ 是四个螺旋桨的升力。

为了便于计算，系统（2）被转换为状态空间格式，列于公式中（3）。
X˙=AX+BU(3)\dot{X} = A X + B U \tag{3}X˙=AX+BU(3)
其中 X=[xyzx˙y˙z˙gθ−gϕ0gθ˙−gϕ˙0]TX = [\begin{matrix} x & y & z & \dot{x} & \dot{y} & \dot{z} & g\theta & -g\phi & 0 & g\dot{\theta} & -g \dot{\phi} & 0 \end{matrix}]^\text{T}X=[x​y​z​x˙​y˙​​z˙​gθ​−gϕ​0​gθ˙​−gϕ˙​​0​]T,
U=[uϕuθ0]TU=[\begin{matrix} u_\phi & u_\theta & 0 \end{matrix}]^\text{T}U=[uϕ​​uθ​​0​]T,
A=[0100001000010000]⊗I3A=\left[\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ \end{matrix}\right] \otimes I_3A=⎣⎢⎢⎡​0000​1000​0100​0010​⎦⎥⎥⎤​⊗I3​,
B=[0001]⊗I3B=\left[\begin{matrix} 0 \\ 0 \\ 0 \\ 1 \\ \end{matrix}\right] \otimes I_3B=⎣⎢⎢⎡​0001​⎦⎥⎥⎤​⊗I3​,
⊗\otimes⊗ 是克罗尼克积。

当有 NNN 个无人机时，状态空间方程（3）可以转化为以下形式。

X~˙=IN⊗AX~+IN⊗BU~(4)\dot{\tilde{X}} = I_N \otimes A \tilde{X} + I_N \otimes B \tilde{U} \tag{4}X~˙=IN​⊗AX~+IN​⊗BU~(4)

其中 X~=[X1T,X2T,…,XNT]T\tilde{X} = [X_1^\text{T}, X_2^\text{T}, \dots, X_N^\text{T}]^\text{T}X~=[X1T​,X2T​,…,XNT​]T,
U~=[U1T,U2T,…,UNT]T\tilde{U} = [U_1^\text{T}, U_2^\text{T}, \dots, U_N^\text{T}]^\text{T}U~=[U1T​,U2T​,…,UNT​]T,
INI_NIN​ 表示 NNN-维单位矩阵。

期望编队状态 hhh 定义为
h=[hxhyhz]∈R3(5)h = \left[\begin{matrix} h^x \\ h^y \\ h^z \\ \end{matrix}\right] \in \R^3 \tag{5}h=⎣⎡​hxhyhz​⎦⎤​∈R3(5)

将编队状态（5）转化为位置状态，三个新的误差位置状态自然而然地出现，即（6）。

[δxδyδz]=[x−hxy−hyz−hz](6)\left[\begin{matrix} \delta^x \\ \delta^y \\ \delta^z \\ \end{matrix}\right]= \left[\begin{matrix} x - h^x \\ y - h^y \\ z - h^z \\ \end{matrix}\right] \tag{6}⎣⎡​δxδyδz​⎦⎤​=⎣⎡​x−hxy−hyz−hz​⎦⎤​(6)

那么编队控制问题就变成了寻找一个协议 U~\tilde{U}U~ 来驱动误差向量 δ\deltaδ 为零。这表明
lim⁡t→∞∥δi−δj∥=0,lim⁡t→∞∥vi∥=0lim⁡t→∞∥Ωi∥=0,lim⁡t→∞∥Ω˙i∥=0,i=1,2,⋯,N(7)\begin{aligned} \lim_{t \rightarrow \infty} \| \delta_i - \delta_j \| &= 0, ~~~ \lim_{t \rightarrow \infty} \| v_i \| = 0 \\ \lim_{t \rightarrow \infty} \| \Omega_i \| &= 0, ~~~ \lim_{t \rightarrow \infty} \| \dot{\Omega}_i \| = 0, ~~~ i = 1,2,\cdots, N \\ \end{aligned} \tag{7}t→∞lim​∥δi​−δj​∥t→∞lim​∥Ωi​∥​=0,   t→∞lim​∥vi​∥=0=0,   t→∞lim​∥Ω˙i​∥=0,   i=1,2,⋯,N​(7)

3 主要结果

本节首先介绍了编队控制协议的设计。之后，详细介绍了单个无人机的最优控制法。最后，最优控制法被扩展到集合的编队控制协议中。

Lemma 1[6]

针对一个 N×NN \times NN×N 拉氏矩阵 LLL，Ne−Lt,t>0N e^{-Lt}, t>0Ne−Lt,t>0 是一个具有正对角元素的随机矩阵。如果 LLL 有一个唯一的 000 特征值，Rank(N)=N−1\text{Rank}(N) = N-1Rank(N)=N−1，那么它的左特征值有 v=[v1v2⋯vN]T≥0v = [\begin{matrix} v_1 & v_2 & \cdots & v_N \end{matrix}]^\text{T} \ge 0v=[v1​​v2​​⋯​vN​​]T≥0 和 1NTv=1,LTv=01_N^\text{T} v = 1, L^\text{T} v = 01NT​v=1,LTv=0，其中 t→∞,e−Lt→1NvTt \rightarrow \infty, e^{-L t} \rightarrow 1_N v^\text{T}t→∞,e−Lt→1N​vT。

3.1 编队控制

参照以前的工作，一致性协议可以分为两种类型：一种是静态的，另一种是动态的。在静态一致性协议的基础上，这里采用了以下形成控制协议，即（8）。

ui=α∑j∈Ni(δj−δi)−βvi−γ1Ωi−γ2Ω˙(8)u_i = \alpha \sum_{j\in N_i} (\delta_j - \delta_i) - \beta v_i - \gamma_1 \Omega_i - \gamma_2 \dot{\Omega} \tag{8}ui​=αj∈Ni​∑​(δj​−δi​)−βvi​−γ1​Ωi​−γ2​Ω˙(8)

其中 α,β,γ1,γ2\alpha, \beta, \gamma_1, \gamma_2α,β,γ1​,γ2​ 都是正向增益，
δi=[δix,δiy,δiz]T\delta_i = [\delta_i^x, \delta_i^y, \delta_i^z]^\text{T}δi​=[δix​,δiy​,δiz​]T,
vi=[x˙i,y˙i,z˙i]Tv_i = [\dot{x}_i, \dot{y}_i, \dot{z}_i]^\text{T}vi​=[x˙i​,y˙​i​,z˙i​]T,
Ωi=[gθi,−gϕ,0]T\Omega_i = [g \theta_i, -g \phi, 0]^\text{T}Ωi​=[gθi​,−gϕ,0]T,
Ω˙i=[gθ˙i,−gϕ˙,0]T\dot{\Omega}_i = [g \dot{\theta}_i, -g \dot{\phi}, 0]^\text{T}Ω˙i​=[gθ˙i​,−gϕ˙​,0]T.

定理 1

假设 GGG 是连通无向图。如果协议（8）满足以下条件，系统（4）可以实现（7）中定义的编队。
α>0,β>0,γ1>0,γ2>0,β>>α,γ1,γ2>β>>α,βγ1γ2>β2+γ22α\begin{aligned} \alpha > 0, ~~~\beta>0, ~~~\gamma_1 > 0, ~~~\gamma_2 > 0, ~~~\beta >> \alpha, \\ \gamma_1, \gamma_2 > \beta >> \alpha, ~~~\beta \gamma_1 \gamma_2 > \beta^2 + \gamma_2^2 \alpha \end{aligned}α>0,   β>0,   γ1​>0,   γ2​>0,   β>>α,γ1​,γ2​>β>>α,   βγ1​γ2​>β2+γ22​α​

证明

请参照参考文献 [5]。

3.2 最优控制

最优控制的解决方案需要能够获得系统中所有的状态信息，这在多代理系统中往往无法满足。然而，可以通过研究单个代理中的最优控制法来确定系统的最优控制法。

在给出性能函数之前，令 Xˉ=[δxδyδzx˙y˙z˙gθ−gϕ0gθ˙−gϕ˙0]T\bar{X} = [\begin{matrix} \delta^x & \delta^y & \delta^z & \dot{x} & \dot{y} & \dot{z} & g\theta & -g\phi & 0 & g\dot{\theta} & -g\dot{\phi} & 0 \end{matrix}]^\text{T}Xˉ=[δx​δy​δz​x˙​y˙​​z˙​gθ​−gϕ​0​gθ˙​−gϕ˙​​0​]T，性能函数定义为
Ji=∫0∞[XˉiT(t)QXˉi(t)+uiT(t)Rui(t)]dt(9)J_i = \int_0^\infty [\bar{X}_i^\text{T}(t) Q \bar{X}_i(t) + u_i^\text{T}(t) R u_i(t)] dt \tag{9}Ji​=∫0∞​[XˉiT​(t)QXˉi​(t)+uiT​(t)Rui​(t)]dt(9)

由于无人机在不同坐标轴上的状态是独立的，我们需要设置权重矩阵 Q=q∗I12,R=r∗I3Q = q * I_{12}, R = r * I_{3}Q=q∗I12​,R=r∗I3​，其中 q>0,r>0q>0, r>0q>0,r>0。

通过最优控制理论，单个智能体的最优控制法则是
ui∗=−R−1BTPXˉ(10)u_i^* = - R^{-1} B^\text{T} P \bar{X} \tag{10}ui∗​=−R−1BTPXˉ(10)

其中 PPP 是黎卡提方程（11）的解。
ATP+PA−PBR−1BTP+Q=0(11)A^\text{T} P + P A - P B R^{-1} B^\text{T} P + Q = 0 \tag{11}ATP+PA−PBR−1BTP+Q=0(11)

3.3 分布式最优编队控制

通过上述计算，可以得到最优控制法（10）。令 K=R−1BTPK = R^{-1} B^\text{T} PK=R−1BTP，KKK 的维度一定是 3×123 \times 123×12，同时也具有如下形式

K=[k1k2k3k4]⊗I3=[k100k200k300k4000k100k200k300k4000k100k200k300k4]\begin{aligned} K =& [\begin{matrix} k_1 & k_2 & k_3 & k_4 \end{matrix}] \otimes I_3 \\ =& \left[\begin{matrix} k_1 & 0 & 0 & k_2 & 0 & 0 & k_3 & 0 & 0 & k_4 & 0 & 0 \\ 0 & k_1 & 0 & 0 & k_2 & 0 & 0 & k_3 & 0 & 0 & k_4 & 0 \\ 0 & 0 & k_1 & 0 & 0 & k_2 & 0 & 0 & k_3 & 0 & 0 & k_4 \\ \end{matrix}\right] \end{aligned}K==​[k1​​k2​​k3​​k4​​]⊗I3​⎣⎡​k1​00​0k1​0​00k1​​k2​00​0k2​0​00k2​​k3​00​0k3​0​00k3​​k4​00​0k4​0​00k4​​⎦⎤​​

无人机的多代理系统是同构的，即所有代理的动态性能都是一样的，所以最优控制法可以直接应用于无人机系统。因此最优编队控制法则为

u∗=k1∑j∈Ni(δj−δi)−k2vi−k3Ωi−k4Ω˙i(12)u^* = k_1 \sum_{j \in N_i} (\delta_j-\delta_i) - k_2 v_i - k_3 \Omega_i - k_4 \dot{\Omega}_i \tag{12}u∗=k1​j∈Ni​∑​(δj​−δi​)−k2​vi​−k3​Ωi​−k4​Ω˙i​(12)

其中 k1,k2,k3,k4k_1, k_2, k_3, k_4k1​,k2​,k3​,k4​ 来源于矩阵 KKK。

定理 2

假设图 GGG 是连通无向图。无人机系统（4）如果使用协议（12），可以完成编队，同时使性能函数（9）最优。

证明

将（1）代入（2），我们可以得到
U=Γ⊗I3XˉΓ=L⊗[k1000]+I3⊗[0k2k3k4]\begin{aligned} U =& \Gamma \otimes I_3 \bar{X} \\ \Gamma =& L \otimes [\begin{matrix} k_1 & 0 & 0 & 0 \end{matrix}] + I_3 \otimes [\begin{matrix} 0 & k_2 & k_3 & k_4 \end{matrix}] \end{aligned}U=Γ=​Γ⊗I3​XˉL⊗[k1​​0​0​0​]+I3​⊗[0​k2​​k3​​k4​​]​

那么
Xˉ˙=AXˉ+BΓ⊗I3Xˉ=ΓˉXˉ\dot{\bar{X}} = A \bar{X} + B \Gamma \otimes I_3 \bar{X} = \bar{\Gamma} \bar{X}Xˉ˙=AXˉ+BΓ⊗I3​Xˉ=ΓˉXˉ

结合定理 1 的证明，Γˉ\bar{\Gamma}Γˉ 可以变成一个约旦标准：
Γˉ=PJP−1\bar{\Gamma} = P J P^{-1}Γˉ=PJP−1

令 v1Tv_1^\text{T}v1T​

4 仿真

在本节中，进行了数值模拟和虚拟平台模拟，以验证所设计的编队控制协议的有效性。在本节中，进行了数值模拟和虚拟平台模拟，以验证所设计的编队控制协议的有效性。

4.1 数值仿真

数值实验分别验证了编队控制和分布式优化编队控制，如图3和图4所示。

在编队控制中，可以观察到系统可以完成三角形编队，但在编队完成过程中与设定位置有较大误差，在第5秒时仍有误差。当使用分布式最优编队控制时，可以观察到系统可以快速完成三角形编队，而且实际位置与设定值之间的误差很小。

此外，令人惊讶的是，最优控制不仅减少了系统的损失，还加快了系统的收敛速度，这对减少系统形成阵型的时间有很大帮助。

4.2 平台仿真

仿真软件CoppeliaSim/Vrep也被用来验证无人机编队的飞行情况。为了在操作过程中保持系统的视野，我们假设无人机保持静止状态。观察系统在不同时间段的位置，如图5所示，可以看出，系统最终可以完成三角形编队。完整版的实验视频在https://www.bilibili.com/video/BV1Br4y1D7VJ/。

5 结论

本文通过分析无人机的动态模型，建立了一个多无人机系统。在静态共识协议的基础上，设计了编队控制协议。针对系统中部分无人机无法获得其他无人机状态信息的问题，设计了单个无人机的最优控制法。最后，结合编队控制协议和最优控制法，设计了一个多无人机系统的分布式优化编队控制协议。该协议不受多代理系统通信拓扑结构的干扰，并能在系统完成编队任务的同时优化性能函数。

下一步，我们计划结合工程应用，将理论研究成果应用于实践。

附录：程序

A.1 main1ConsensusDynamic

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc% Notes:
% 1. The state is randomly.
% 2. Final states is consensus.%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [10 15 20 2.0 1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [15 10 15 1.0 -2.0 1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 15 10 -2.0 1.0 2.0 0 0 0 0 0 0]';
% UAV1(:,1) = rand(12,1);
% UAV2(:,1) = rand(12,1);
% UAV3(:,1) = rand(12,1);p1      = [UAV1(1,1)  UAV1(2,1)  UAV1(3,1)]';
v1      = [UAV1(4,1)  UAV1(5,1)  UAV1(6,1)]';
omega1  = [UAV1(7,1)  UAV1(8,1)  UAV1(9,1)]';
domega1 = [UAV1(10,1) UAV1(11,1) UAV1(12,1)]';
p2      = [UAV2(1,1)  UAV2(2,1)  UAV2(3,1)]';
v2      = [UAV2(4,1)  UAV2(5,1)  UAV2(6,1)]';
omega2  = [UAV2(7,1)  UAV2(8,1)  UAV2(9,1)]';
domega2 = [UAV2(10,1) UAV2(11,1) UAV2(12,1)]';
p3      = [UAV3(1,1)  UAV3(2,1)  UAV3(3,1)]';
v3      = [UAV3(4,1)  UAV3(5,1)  UAV3(6,1)]';
omega3  = [UAV3(7,1)  UAV3(8,1)  UAV3(9,1)]';
domega3 = [UAV3(10,1) UAV3(11,1) UAV3(12,1)]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Positive gain
alpha = 0.1;
beta = 0.6;
gamma1 =  3;
gamma2 =  2;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 50;
dT = 0.1;
times = (tFinal-tBegin)/dT;% Iterations
for i=1:times% Calculate control inputu1(:,i+1) = alpha*(p2(:,i)-p1(:,i)) + beta*(v2(:,i)-v1(:,i)) + gamma1*(-omega1(:,i)) + gamma2*(-domega1(:,i));u2(:,i+1) = alpha*(p3(:,i)-p2(:,i)) + beta*(v3(:,i)-v2(:,i)) + gamma1*(-omega2(:,i)) + gamma2*(-domega2(:,i));u3(:,i+1) = alpha*(p1(:,i)-p3(:,i)) + beta*(v1(:,i)-v3(:,i)) + gamma1*(-omega3(:,i)) + gamma2*(-domega3(:,i));% Update statesdomega1(:,i+1) = domega1(:,i) + dT * u1(:,i+1);omega1(:,i+1)  = omega1(:,i)  + dT * domega1(:,i+1);v1(:,i+1)      = v1(:,i)      + dT * omega1(:,i+1);p1(:,i+1)      = p1(:,i)      + dT * v1(:,i+1);domega2(:,i+1) = domega2(:,i) + dT * u2(:,i+1);omega2(:,i+1)  = omega2(:,i)  + dT * domega2(:,i+1);v2(:,i+1)      = v2(:,i)      + dT * omega2(:,i+1);p2(:,i+1)      = p2(:,i)      + dT * v2(:,i+1);domega3(:,i+1) = domega3(:,i) + dT * u3(:,i+1);omega3(:,i+1)  = omega3(:,i)  + dT * domega3(:,i+1);v3(:,i+1)      = v3(:,i)      + dT * omega3(:,i+1);p3(:,i+1)      = p3(:,i)      + dT * v3(:,i+1);% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
% X-axis states
subplot(5,2,1)
plot(t,u1(1,:), t,u2(1,:), t,u3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X control inputs");
subplot(5,2,3)
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(5,2,5)
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(5,2,7)
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(5,2,9)
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\theta} states");% Y-axis states
subplot(5,2,2)
plot(t,u1(2,:), t,u2(2,:), t,u3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y control inputs");
subplot(5,2,4)
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(5,2,6)
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(5,2,8)
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(5,2,10)
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\phi} states");

A.2 main1ConsensusStatic

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc% Notes:
% 1. The state is randomly.
% 2. Final states is consensus.%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [10 15 20 1.0 1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [15 10 15 1.0 -2.0 1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 15 10 -2.0 1.0 1.0 0 0 0 0 0 0]';
% UAV1(:,1) = rand(12,1);
% UAV2(:,1) = rand(12,1);
% UAV3(:,1) = rand(12,1);p1      = [UAV1(1,1)  UAV1(2,1)  UAV1(3,1)]';
v1      = [UAV1(4,1)  UAV1(5,1)  UAV1(6,1)]';
omega1  = [UAV1(7,1)  UAV1(8,1)  UAV1(9,1)]';
domega1 = [UAV1(10,1) UAV1(11,1) UAV1(12,1)]';
p2      = [UAV2(1,1)  UAV2(2,1)  UAV2(3,1)]';
v2      = [UAV2(4,1)  UAV2(5,1)  UAV2(6,1)]';
omega2  = [UAV2(7,1)  UAV2(8,1)  UAV2(9,1)]';
domega2 = [UAV2(10,1) UAV2(11,1) UAV2(12,1)]';
p3      = [UAV3(1,1)  UAV3(2,1)  UAV3(3,1)]';
v3      = [UAV3(4,1)  UAV3(5,1)  UAV3(6,1)]';
omega3  = [UAV3(7,1)  UAV3(8,1)  UAV3(9,1)]';
domega3 = [UAV3(10,1) UAV3(11,1) UAV3(12,1)]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Positive gain
alpha = 0.1;
beta = 0.6;
gamma1 =  3;
gamma2 =  2;% alpha = 1;
% beta = 3.0777;
% gamma1 = 4.2361;
% gamma2 =  3.0777;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 300;
dT = 0.1;
times = (tFinal-tBegin)/dT;% Iterations
for i=1:times% Calculate control inputu1(:,i+1) = alpha*(p2(:,i)-p1(:,i)) + beta*(-v1(:,i)) + gamma1*(-omega1(:,i)) + gamma2*(-domega1(:,i));u2(:,i+1) = alpha*(p3(:,i)-p2(:,i)) + beta*(-v2(:,i)) + gamma1*(-omega2(:,i)) + gamma2*(-domega2(:,i));u3(:,i+1) = alpha*(p1(:,i)-p3(:,i)) + beta*(-v3(:,i)) + gamma1*(-omega3(:,i)) + gamma2*(-domega3(:,i));% Update statesdomega1(:,i+1) = domega1(:,i) + dT * u1(:,i+1);omega1(:,i+1)  = omega1(:,i)  + dT * domega1(:,i+1);v1(:,i+1)      = v1(:,i)      + dT * omega1(:,i+1);p1(:,i+1)      = p1(:,i)      + dT * v1(:,i+1);domega2(:,i+1) = domega2(:,i) + dT * u2(:,i+1);omega2(:,i+1)  = omega2(:,i)  + dT * domega2(:,i+1);v2(:,i+1)      = v2(:,i)      + dT * omega2(:,i+1);p2(:,i+1)      = p2(:,i)      + dT * v2(:,i+1);domega3(:,i+1) = domega3(:,i) + dT * u3(:,i+1);omega3(:,i+1)  = omega3(:,i)  + dT * domega3(:,i+1);v3(:,i+1)      = v3(:,i)      + dT * omega3(:,i+1);p3(:,i+1)      = p3(:,i)      + dT * v3(:,i+1);% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
% X-axis states
subplot(5,2,1)
plot(t,u1(1,:), t,u2(1,:), t,u3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X control inputs");
subplot(5,2,3)
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(5,2,5)
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(5,2,7)
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(5,2,9)
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\theta} states");% Y-axis states
subplot(5,2,2)
plot(t,u1(2,:), t,u2(2,:), t,u3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y control inputs");
subplot(5,2,4)
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(5,2,6)
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(5,2,8)
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(5,2,10)
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\phi} states");

A.3 main2FormationDynamic

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc% Notes:
% 1. In order to facilitate the observation of formation information, the initial state is set in advance.
% 2. Final states is formation.%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [10 15 20  1.0  1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [15 10 15  1.0 -2.0  1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 15 10 -2.0  1.0  1.0 0 0 0 0 0 0]';p1      = [UAV1(1,1)  UAV1(2,1)  UAV1(3,1)]';
v1      = [UAV1(4,1)  UAV1(5,1)  UAV1(6,1)]';
omega1  = [UAV1(7,1)  UAV1(8,1)  UAV1(9,1)]';
domega1 = [UAV1(10,1) UAV1(11,1) UAV1(12,1)]';
p2      = [UAV2(1,1)  UAV2(2,1)  UAV2(3,1)]';
v2      = [UAV2(4,1)  UAV2(5,1)  UAV2(6,1)]';
omega2  = [UAV2(7,1)  UAV2(8,1)  UAV2(9,1)]';
domega2 = [UAV2(10,1) UAV2(11,1) UAV2(12,1)]';
p3      = [UAV3(1,1)  UAV3(2,1)  UAV3(3,1)]';
v3      = [UAV3(4,1)  UAV3(5,1)  UAV3(6,1)]';
omega3  = [UAV3(7,1)  UAV3(8,1)  UAV3(9,1)]';
domega3 = [UAV3(10,1) UAV3(11,1) UAV3(12,1)]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Formation states
p1h      = [10 0  0]';
v1h      = [0  0  0]';
omega1h  = [0  0  0]';
domega1h = [0  0  0]';p2h      = [15 0  0]';
v2h      = [0  0  0]';
omega2h  = [0  0  0]';
domega2h = [0  0  0]';p3h      = [05 0  0]';
v3h      = [0  0  0]';
omega3h  = [0  0  0]';
domega3h = [0  0  0]';% Positive gain
alpha = 0.1;
beta = 0.6;
gamma1 =  3;
gamma2 =  2;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 300;
dT = 0.5;
times = (tFinal-tBegin)/dT;% Iterations
for i=1:times% Calculate control inputu1(:,i+1) = alpha*((p2(:,i)-p2h)-(p1(:,i)-p1h)) + beta*(-v1(:,i)) + gamma1*(-omega1(:,i)) + gamma2*(-domega1(:,i));u2(:,i+1) = alpha*((p3(:,i)-p3h)-(p2(:,i)-p2h)) + beta*(-v2(:,i)) + gamma1*(-omega2(:,i)) + gamma2*(-domega2(:,i));u3(:,i+1) = alpha*((p1(:,i)-p1h)-(p3(:,i)-p3h)) + beta*(-v3(:,i)) + gamma1*(-omega3(:,i)) + gamma2*(-domega3(:,i));% Update statesdomega1(:,i+1) = domega1(:,i) + dT * u1(:,i+1);omega1(:,i+1)  = omega1(:,i)  + dT * domega1(:,i+1);v1(:,i+1)      = v1(:,i)      + dT * omega1(:,i+1);p1(:,i+1)      = p1(:,i)      + dT * v1(:,i+1);domega2(:,i+1) = domega2(:,i) + dT * u2(:,i+1);omega2(:,i+1)  = omega2(:,i)  + dT * domega2(:,i+1);v2(:,i+1)      = v2(:,i)      + dT * omega2(:,i+1);p2(:,i+1)      = p2(:,i)      + dT * v2(:,i+1);domega3(:,i+1) = domega3(:,i) + dT * u3(:,i+1);omega3(:,i+1)  = omega3(:,i)  + dT * domega3(:,i+1);v3(:,i+1)      = v3(:,i)      + dT * omega3(:,i+1);p3(:,i+1)      = p3(:,i)      + dT * v3(:,i+1);% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
figure(1)
% X-axis states
subplot(5,2,1)
plot(t,u1(1,:), t,u2(1,:), t,u3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X control inputs");
subplot(5,2,3)
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(5,2,5)
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(5,2,7)
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(5,2,9)
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\theta} states");% Y-axis states
subplot(5,2,2)
plot(t,u1(2,:), t,u2(2,:), t,u3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y control inputs");
subplot(5,2,4)
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(5,2,6)
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(5,2,8)
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(5,2,10)
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\phi} states");figure(2)
plot3(p1(1,:),p1(2,:),t); hold on
plot3(p2(1,:),p2(2,:),t); hold on
plot3(p3(1,:),p3(2,:),t); hold on
grid on

A.4 main2FormationStatic

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc% Notes:
% 1. In order to facilitate the observation of formation information, the initial state is set in advance.
% 2. Final states is formation.%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [20 15 20  1.0  1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [20 10 15  1.0 -2.0  1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 20 10 -2.0  1.0  1.0 0 0 0 0 0 0]';p1      = [UAV1(1,1)  UAV1(2,1)  UAV1(3,1)]';
v1      = [UAV1(4,1)  UAV1(5,1)  UAV1(6,1)]';
omega1  = [UAV1(7,1)  UAV1(8,1)  UAV1(9,1)]';
domega1 = [UAV1(10,1) UAV1(11,1) UAV1(12,1)]';
p2      = [UAV2(1,1)  UAV2(2,1)  UAV2(3,1)]';
v2      = [UAV2(4,1)  UAV2(5,1)  UAV2(6,1)]';
omega2  = [UAV2(7,1)  UAV2(8,1)  UAV2(9,1)]';
domega2 = [UAV2(10,1) UAV2(11,1) UAV2(12,1)]';
p3      = [UAV3(1,1)  UAV3(2,1)  UAV3(3,1)]';
v3      = [UAV3(4,1)  UAV3(5,1)  UAV3(6,1)]';
omega3  = [UAV3(7,1)  UAV3(8,1)  UAV3(9,1)]';
domega3 = [UAV3(10,1) UAV3(11,1) UAV3(12,1)]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Formation states
p1h      = [10 10  0]';p2h      = [00 05  0]';p3h      = [00 15  0]';% Positive gain
alpha = 0.2;
beta = 1.5;
gamma1 =  5;
gamma2 =  2;% alpha = 1;
% beta = 3.0777;
% gamma1 = 4.2361;
% gamma2 =  3.0777;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 100;
dT = 0.5;
times = (tFinal-tBegin)/dT;% Iterations
for i=1:times% Calculate control inputu1(:,i+1) = alpha*((p2(:,i)-p2h)-(p1(:,i)-p1h)) + beta*(-v1(:,i)) + gamma1*(-omega1(:,i)) + gamma2*(-domega1(:,i));u2(:,i+1) = alpha*((p3(:,i)-p3h)-(p2(:,i)-p2h)) + beta*(-v2(:,i)) + gamma1*(-omega2(:,i)) + gamma2*(-domega2(:,i));u3(:,i+1) = alpha*((p1(:,i)-p1h)-(p3(:,i)-p3h)) + beta*(-v3(:,i)) + gamma1*(-omega3(:,i)) + gamma2*(-domega3(:,i));% Update statesdomega1(:,i+1) = domega1(:,i) + dT * u1(:,i+1);omega1(:,i+1)  = omega1(:,i)  + dT * domega1(:,i+1);v1(:,i+1)      = v1(:,i)      + dT * omega1(:,i+1);p1(:,i+1)      = p1(:,i)      + dT * v1(:,i+1);domega2(:,i+1) = domega2(:,i) + dT * u2(:,i+1);omega2(:,i+1)  = omega2(:,i)  + dT * domega2(:,i+1);v2(:,i+1)      = v2(:,i)      + dT * omega2(:,i+1);p2(:,i+1)      = p2(:,i)      + dT * v2(:,i+1);domega3(:,i+1) = domega3(:,i) + dT * u3(:,i+1);omega3(:,i+1)  = omega3(:,i)  + dT * domega3(:,i+1);v3(:,i+1)      = v3(:,i)      + dT * omega3(:,i+1);p3(:,i+1)      = p3(:,i)      + dT * v3(:,i+1);% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
figure(1)
% X-axis states
% subplot(4,2,1)
% plot(t,u1(1,:), t,u2(1,:), t,u3(1,:), 'linewidth',1.5)
% legend("UAV1", "UAV2", "UAV3"); grid on
% title("Time - X control inputs");
subplot(4,2,1)
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(4,2,3)
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(4,2,5)
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(4,2,7)
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - dot \theta states");% Y-axis states
% subplot(5,2,2)
% plot(t,u1(2,:), t,u2(2,:), t,u3(2,:), 'linewidth',1.5)
% legend("UAV1", "UAV2", "UAV3"); grid on
% title("Time - Y control inputs");
subplot(4,2,2)
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(4,2,4)
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(4,2,6)
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(4,2,8)
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - $\dot \phi$ \phi states");figure(2)
plot3(p1(1,:),p1(2,:),t); hold on
plot3(p2(1,:),p2(2,:),t); hold on
plot3(p3(1,:),p3(2,:),t); hold on
grid on

A.5 main2FormationStatic2

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-13
clear
clc% Notes:
% 1. In order to facilitate the observation of formation information, the initial state is set in advance.
% 2. Final states is formation.%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [20 15 20  1.0  1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [20 10 15  1.0 -2.0  1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 20 10 -2.0  1.0  1.0 0 0 0 0 0 0]';p1      = [UAV1(1,1)  UAV1(2,1)  UAV1(3,1)]';
v1      = [UAV1(4,1)  UAV1(5,1)  UAV1(6,1)]';
omega1  = [UAV1(7,1)  UAV1(8,1)  UAV1(9,1)]';
domega1 = [UAV1(10,1) UAV1(11,1) UAV1(12,1)]';
p2      = [UAV2(1,1)  UAV2(2,1)  UAV2(3,1)]';
v2      = [UAV2(4,1)  UAV2(5,1)  UAV2(6,1)]';
omega2  = [UAV2(7,1)  UAV2(8,1)  UAV2(9,1)]';
domega2 = [UAV2(10,1) UAV2(11,1) UAV2(12,1)]';
p3      = [UAV3(1,1)  UAV3(2,1)  UAV3(3,1)]';
v3      = [UAV3(4,1)  UAV3(5,1)  UAV3(6,1)]';
omega3  = [UAV3(7,1)  UAV3(8,1)  UAV3(9,1)]';
domega3 = [UAV3(10,1) UAV3(11,1) UAV3(12,1)]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Formation states
p1h      = [10 10  0]';p2h      = [00 05  0]';p3h      = [00 15  0]';% Positive gain
alpha = 0.2;
beta = 1.5;
gamma1 =  5;
gamma2 =  2;alpha = 0.5;
beta = 3.0777;
gamma1 = 4.2361;
gamma2 =  3.0777;
%
% alpha = 0.6325;
% beta = 2.0990;
% gamma1 = 3.1667;
% gamma2 =  2.5949;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 70;
dT = 0.05;
times = (tFinal-tBegin)/dT;% Iterations
for i=1:times% Calculate position errordelta1 = p1(:,i) - p1h;delta2 = p2(:,i) - p2h;delta3 = p3(:,i) - p3h;% Calculate control inputu1(:,i+1) = alpha*(delta2-delta1) + beta*(-v1(:,i)) + gamma1*(-omega1(:,i)) + gamma2*(-domega1(:,i));u2(:,i+1) = alpha*(delta3-delta2) + beta*(-v2(:,i)) + gamma1*(-omega2(:,i)) + gamma2*(-domega2(:,i));u3(:,i+1) = alpha*(delta1-delta3) + beta*(-v3(:,i)) + gamma1*(-omega3(:,i)) + gamma2*(-domega3(:,i));% Update statesdomega1(:,i+1) = domega1(:,i) + dT * u1(:,i+1);omega1(:,i+1)  = omega1(:,i)  + dT * domega1(:,i+1);v1(:,i+1)      = v1(:,i)      + dT * omega1(:,i+1);p1(:,i+1)      = p1(:,i)      + dT * v1(:,i+1);domega2(:,i+1) = domega2(:,i) + dT * u2(:,i+1);omega2(:,i+1)  = omega2(:,i)  + dT * domega2(:,i+1);v2(:,i+1)      = v2(:,i)      + dT * omega2(:,i+1);p2(:,i+1)      = p2(:,i)      + dT * v2(:,i+1);domega3(:,i+1) = domega3(:,i) + dT * u3(:,i+1);omega3(:,i+1)  = omega3(:,i)  + dT * domega3(:,i+1);v3(:,i+1)      = v3(:,i)      + dT * omega3(:,i+1);p3(:,i+1)      = p3(:,i)      + dT * v3(:,i+1);% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
figure(1)
% X-axis states
% subplot(4,2,1)
% plot(t,u1(1,:), t,u2(1,:), t,u3(1,:), 'linewidth',1.5)
% legend("UAV1", "UAV2", "UAV3"); grid on
% title("Time - X control inputs");
subplot(4,2,1)
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(4,2,3)
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(4,2,5)
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(4,2,7)
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - dot \theta states");% Y-axis states
% subplot(5,2,2)
% plot(t,u1(2,:), t,u2(2,:), t,u3(2,:), 'linewidth',1.5)
% legend("UAV1", "UAV2", "UAV3"); grid on
% title("Time - Y control inputs");
subplot(4,2,2)
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(4,2,4)
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(4,2,6)
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(4,2,8)
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - $\dot \phi$ \phi states");figure(2)
plot3(p1(1,:),p1(2,:),t, '--r', 'linewidth',1.5); hold on
plot3(p2(1,:),p2(2,:),t, '--g', 'linewidth',1.5); hold on
plot3(p3(1,:),p3(2,:),t, '--b', 'linewidth',1.5); hold on
xlabel("$x$ (m)",'Interpreter','latex'); ylabel("$y$ (m)",'Interpreter','latex'); zlabel("t (s)",'Interpreter','latex');
% title("Formation state at different time instants (formation control)",'Interpreter','latex');
title("Formation state at different time instants (distributed optimal formation control)",'Interpreter','latex');xlim([10,40])
ylim([5,35])
grid ontt1 = 10/0.05;
scatter3(p1(1,tt1),p1(2,tt1),tt1*dT,100,'r'); hold on
scatter3(p2(1,tt1),p2(2,tt1),tt1*dT,100,'g'); hold on
scatter3(p3(1,tt1),p3(2,tt1),tt1*dT,100,'b'); hold on
line([p1(1,tt1),p2(1,tt1)],[p1(2,tt1),p2(2,tt1)],[tt1*dT,tt1*dT])
line([p2(1,tt1),p3(1,tt1)],[p2(2,tt1),p3(2,tt1)],[tt1*dT,tt1*dT])
line([p3(1,tt1),p1(1,tt1)],[p3(2,tt1),p1(2,tt1)],[tt1*dT,tt1*dT])
text(p1(1,tt1)+1,p1(2,tt1)-1,tt1*dT,'t = 1s','Interpreter','latex')tt2 = 30/0.05;
scatter3(p1(1,tt2),p1(2,tt2),tt2*dT,100,'r'); hold on
scatter3(p2(1,tt2),p2(2,tt2),tt2*dT,100,'g'); hold on
scatter3(p3(1,tt2),p3(2,tt2),tt2*dT,100,'b'); hold on
line([p1(1,tt2),p2(1,tt2)],[p1(2,tt2),p2(2,tt2)],[tt2*dT,tt2*dT])
line([p2(1,tt2),p3(1,tt2)],[p2(2,tt2),p3(2,tt2)],[tt2*dT,tt2*dT])
line([p3(1,tt2),p1(1,tt2)],[p3(2,tt2),p1(2,tt2)],[tt2*dT,tt2*dT])
text(p1(1,tt2)+1,p1(2,tt2)-1,tt2*dT,'t = 3s','Interpreter','latex')tt3 = 50/0.05;
scatter3(p1(1,tt3),p1(2,tt3),tt3*dT,100,'r'); hold on
scatter3(p2(1,tt3),p2(2,tt3),tt3*dT,100,'g'); hold on
scatter3(p3(1,tt3),p3(2,tt3),tt3*dT,100,'b'); hold on
line([p1(1,tt3),p2(1,tt3)],[p1(2,tt3),p2(2,tt3)],[tt3*dT,tt3*dT])
line([p2(1,tt3),p3(1,tt3)],[p2(2,tt3),p3(2,tt3)],[tt3*dT,tt3*dT])
line([p3(1,tt3),p1(1,tt3)],[p3(2,tt3),p1(2,tt3)],[tt3*dT,tt3*dT])
text(p1(1,tt3)+1,p1(2,tt3)-1,tt3*dT,'t = 5s','Interpreter','latex')
grid onlegend("UAV1", "UAV2", "UAV3",'Interpreter','latex');

A.6 main3OptimalRiccati

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc% Notes:
% 1. In%%
% System matrices
A = kron([0  1  0  00  0  1  00  0  0  10  0  0  0], eye(3));
B = kron([0  0  0  1]',eye(3));% Weight matrices
Q = 2*eye(12);
R = 5*eye(3);% Solve Riccati equation
[P, L, G] = care(A, B, Q, R);K = -inv(R)*B'*P;disp(K)

>> disp(K)-0.6325         0         0   -2.0990         0         0   -3.1667         0         0   -2.5949         0         00   -0.6325   -0.0000         0   -2.0990   -0.0000         0   -3.1667   -0.0000         0   -2.5949   -0.00000   -0.0000   -0.6325         0   -0.0000   -2.0990         0   -0.0000   -3.1667         0   -0.0000   -2.5949

A.7 main4OptimalFormation

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-10
clear
clc%%
% UAV system states
% UAVi = [x y z dx dy dz theta phi 0 dtheta dphi 0]';
UAV1(:,1) = [10 15 20  1.0  1.0 -2.0 0 0 0 0 0 0]';
UAV2(:,1) = [15 10 15  1.0 -2.0  1.0 0 0 0 0 0 0]';
UAV3(:,1) = [20 15 10 -2.0  1.0  1.0 0 0 0 0 0 0]';% Control inputs
u1(:,1) = [0  0  0]';
u2(:,1) = [0  0  0]';
u3(:,1) = [0  0  0]';% Formation states
p1h      = [10 10  0]';
p2h      = [05 0  0]';
p3h      = [15 0  0]';% Positive gain
alpha = 0.1;
beta = 0.6;
gamma1 =  3;
gamma2 =  2;% Time state
t(1,1) = 0;
tBegin = 0;
tFinal = 300;
dT = 0.5;
times = (tFinal-tBegin)/dT;% System matrices
A = kron([0  1  0  00  0  1  00  0  0  10  0  0  0], eye(3));
B = kron([0  0  0  1]',eye(3));X(:,1) = [UAV1' UAV2' UAV3']';
U(:,1) = [u1' u2' u3']';L = [2 -1 -1-1  2 -1-1 -1  2];K = [alpha beta gamma1 gamma2];% Iterations
for i=1:times
%     U(:,i+1) = kron(kron(-L,K),eye(3))*X(:,i);U(:,i+1) = (kron(kron(-L,[alpha 0 0 0]),eye(3)) +... kron(kron(-eye(3),[0 beta 0 0]),eye(3)) +... kron(kron(-eye(3),[0 0 gamma1 0]),eye(3)) +... kron(kron(-eye(3),[0 0 0 gamma2]),eye(3)) ) * X(:,i);dX = kron(eye(3),A) * X(:,i) + kron(eye(3),B) * U(:,i+1);X(:,i+1) = X(:,i) + dT * dX;% record timet(:,i+1) = t(:,i) + dT;
end%% Plot results
figure(1)
% X-axis states
subplot(5,2,1)
plot(t,U(1,:), t,U(4,:), t,U(7,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X control inputs");
subplot(5,2,3)
plot(t,X(1,:), t,X(13,:), t,X(25,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X position states");
subplot(5,2,5)
plot(t,X(4,:), t,X(16,:), t,X(28,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - X velocity states");
subplot(5,2,7)
plot(t,X(7,:), t,X(19,:), t,X(31,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \theta states");
subplot(5,2,9)
plot(t,X(10,:), t,X(22,:), t,X(34,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\theta} states");% Y-axis states
subplot(5,2,2)
plot(t,U(2,:), t,U(5,:), t,U(8,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y control inputs");
subplot(5,2,4)
plot(t,X(2,:), t,X(14,:), t,X(26,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y position states");
subplot(5,2,6)
plot(t,X(5,:), t,X(17,:), t,X(29,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - Y velocity states");
subplot(5,2,8)
plot(t,X(8,:), t,X(20,:), t,X(32,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \phi states");
subplot(5,2,10)
plot(t,X(11,:), t,X(23,:), t,X(35,:), 'linewidth',1.5)
legend("UAV1", "UAV2", "UAV3"); grid on
title("Time - \dot{\phi} states");

A.8 plotResults

% Paper: A Distributed Optimal Formation Control for Multi-Agent System Based on UAVs
% Author: Z-JC
% Data: 2021-12-11%%
subplot(4,2,1)
set(gca,'position',[0.05 0.79 0.43 0.19]);
plot(t,p1(1,:), t,p2(1,:), t,p3(1,:), 'linewidth',1.5)
title("X position error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $x$ (m)",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,2)
set(gca,'position',[0.55 0.79 0.43 0.19]);
plot(t,p1(2,:), t,p2(2,:), t,p3(2,:), 'linewidth',1.5)
title("Y position error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $y$ (m)",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,3)
set(gca,'position',[0.05 0.54 0.43 0.19]);
plot(t,v1(1,:), t,v2(1,:), t,v3(1,:), 'linewidth',1.5)
title("X velocity error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $\dot x$ (m/s)",'Interpreter','latex');
% ylabel('$\dot t_2 $','Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,4)
set(gca,'position',[0.55 0.54 0.43 0.19]);
plot(t,v1(2,:), t,v2(2,:), t,v3(2,:), 'linewidth',1.5)
title("Y velocity error",'Interpreter','latex');
xlabel("t (s)",'Interpreter','latex'); ylabel("Error $\dot y$ (m/s)",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,5)
set(gca,'position',[0.05 0.29 0.43 0.19]);
plot(t,omega1(1,:), t,omega2(1,:), t,omega3(1,:), 'linewidth',1.5)
title("Pitch angles error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\theta ~ (^{\circ})$",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,6)
set(gca,'position',[0.55 0.29 0.43 0.19]);
plot(t,omega1(2,:), t,omega2(2,:), t,omega3(2,:), 'linewidth',1.5)
title("Roll angles error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\phi ~ (^{\circ})$",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,7)
set(gca,'position',[0.05 0.04 0.43 0.19]);
plot(t,domega1(1,:), t,domega2(1,:), t,domega3(1,:), 'linewidth',1.5)
title("Pitch rates error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\dot \theta ~ (^{\circ}/s)$",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;subplot(4,2,8)
set(gca,'position',[0.55 0.04 0.43 0.19]);
plot(t,domega1(2,:), t,domega2(2,:), t,domega3(2,:), 'linewidth',1.5)
title("Roll rates error",'Interpreter','latex');
xlabel("t(s)",'Interpreter','latex'); ylabel("Error $\dot \phi ~ (^{\circ}/s)$",'Interpreter','latex');
legend("UAV1", "UAV2", "UAV3",'Interpreter','latex');
grid on; box on;

【Paper】2022_多无人机系统的分布式最优编队控制相关推荐

  1. 【Paper】2020_多智能体系统的分布式故障估计方法研究

    原文地址:[1]陈帅奇. 多智能体系统的分布式故障估计方法研究[D].长春工业大学,2020. 2020_多智能体系统的分布式故障估计方法研究_陈帅奇 4 基于中间估计器的多智能体系统分布式故障估计 ...

  2. 【Paper】2015_多智能体系统的事件驱动一致性控制与多 Lagrangian 系统的分布式协同

    [2015_多智能体系统的事件驱动一致性控制 与多 Lagrangian 系统的分布式协同] 文章目录 第6章 多 Lagrangian 系统的事件驱动一致性控制 第6章 多 Lagrangian 系 ...

  3. 在OpenShift平台上验证NVIDIA DGX系统的分布式多节点自动驾驶AI训练

    在OpenShift平台上验证NVIDIA DGX系统的分布式多节点自动驾驶AI训练 自动驾驶汽车的深度神经网络(DNN)开发是一项艰巨的工作.本文验证了DGX多节点,多GPU,分布式训练在DXC机器 ...

  4. 【控制】《复杂运动体系统的分布式协同控制与优化》-方浩老师-目录

    无 回到目录 第1章 跳转链接 章节 跳转链接 第1章 绪论 <复杂运动体系统的分布式协同控制与优化>-方浩老师-目录 跳转链接 第1章 绪论 第2章 分布式跟踪控制器设计 分布式控制器 ...

  5. 无人机在计算机专业的应用,嵌入式计算机在无人机系统的应用

    伴随着人工智能.物联网等行业应用的不断成熟,无人机系统正在发生变革.传感系统.AI识别.视频监控等先进技术不断地应用到无人机系统中,促使民用无人机在飞行控制.精准导航.图像传输等诸多技术领域的自主创新 ...

  6. 开源四轴无人机系统迈入新的阶段！

    对开源无人机来说,无人机机架的稳定性,困扰着很多人,尤其是前几年没有好的电机.电调,机架和开源无人机入手十分困难.Ardupilot/Pixhawk这套无人机系统备受诟病,稳定性很差,各种炸机不断.随 ...

  7. 从构建分布式秒杀系统聊聊分布式锁

    从构建分布式秒杀系统聊聊分布式锁 1.案例介绍 在尝试了解分布式锁之前,大家可以想象一下,什么场景下会使用分布式锁? 单机应用架构中,秒杀案例使用ReentrantLcok或者synchronized ...

  8. 无人机基础知识：多旋翼无人机系统基本组成

    无人机基础知识:多旋翼无人机系统基本组成 多旋翼无人机基本组成 机械系统 动力系统 直流无刷电机 电子调速器 Li-Po电池 螺旋桨 飞行控制系统 无人机(Unmanned Aerial Vehicl ...

  9. 【无人机】【2017.11】工程检查和地理空间制图用小型无人机系统

    本文为美国俄勒冈州立大学(作者:Farid Javadnejad)的博士论文,共168页. 携带消费级非测量相机的小型无人机系统(UAS)越来越多地被用来生成高分辨率的三维地理空间数据.无人机的低成本 ...

最新文章

  1. 常用MySQL函数存储过程_解析MySQL存储过程、常用函数代码
  2. Java实现数据序列化工具Avro的例子
  3. JAVA高并发秒杀系统构建之——高并发优化分析
  4. 【转】oracle数据库中varchar2陷阱
  5. 一键生成安卓证书_【带壳截图+电影台词 生成器】
  6. JSON Editor for Mac(JSON编辑器)
  7. 华为鸿蒙系统老手机能用吗_华为发布鸿蒙2.0手机开发者测试版！华为老手机可申请公测...
  8. C语言深度解剖读书笔记(6.函数的核心)
  9. 导入了jar包但是无法import方法
  10. Geohot使用绿雨的BETA4越狱iPhone4 4.1固件详细教程
  11. 爱立信实习总结之面试心得
  12. 网页游戏外挂的设计与编写：QQ摩天大楼【二】（登陆准备-信息处理方式）
  13. 【云原生-白皮书】简章2：深入理解DevOps+微服务
  14. [转] ReactNative Animated动画详解
  15. python 打卡记录代码_利用Python实现对考勤打卡数据处理的总结
  16. xp无线网卡开启的服务器,笔记本xp系统开启无线网卡的方法
  17. 【独家】MobaXterm v22.1 全能终端连接工具中文版最新版
  18. 各种不等式的解法收集【初级辅导和中级辅导】
  19. Lichee（二） 在sun4i_crane平台下的编译
  20. 【老孙点评】古人读书十二法

热门文章

  1. C#学习之Reflection
  2. socket套接字选项
  3. 开始阅读 深入理解计算机系统
  4. Silverlight 2 相关文章汇总
  5. 矩阵分析与多元统计12 0-1矩阵 交换矩阵与Kronecker乘积
  6. Linux下第一个java程序没有成功
  7. tomcat基本使用和超图基本jsp例子
  8. Win32 API CreateCompatibleDC 函数的相关应用
  9. Windows内核系统调用分析
  10. Windows下使用MinGw和gcc构建第一个C程序、g++构建第一个C++程序