【Paper】2022_多无人机系统的分布式最优编队控制|电子爱好者

admin管理员组
文章数量:1530244

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

J. Zhao, F. Dai, Y. Song, A Distributed Optimal Formation Control for Multi-Agent System of UAVs, in: 2022 Proceedings of International Conference on Artificial Life and Robotics (ICAROB), 2022, pp. 803-807.

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 = [ a i j ∈ { 0 , 1 } ] A=[a_{ij} \in \{0, 1\}] A=[aij∈{0,1}] 是图 G G G 的邻接矩阵，表示从节点 j j j 到 i i i 是否有信息交流。矩阵 D D D 是出度矩阵。节点 i i i 的邻居是 N i N_i Ni。 Laplacian矩阵定义为

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

2.2 无人机系统描述

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

x ¨ = g θ y ¨ = − g ϕ z ¨ = u h / m − g ϕ ¨ = u ϕ / I x θ ¨ = u θ / I y ψ ¨ = u ψ / I z (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 , z x,y,z x,y,z 是地面坐标系中沿 X g , Y g , Z g X_g, Y_g, Z_g Xg,Yg,Zg 的位置。 ϕ , θ , ψ \phi, \theta, \psi ϕ,θ,ψ 分别为无人机的滚转角、俯仰角、偏航角。 I x , I y , I z I_x, I_y, I_z Ix,Iy,Iz 分别是沿 X b , Y b , Z b X_b, Y_b, Z_b Xb,Yb,Zb 在机体坐标系中的惯性矩。而 u h u_h uh 是四个螺旋桨的升力。

为了便于计算，系统（2）被转换为状态空间格式，列于公式中（3）。
X ˙ = A X + B U (3) \dot{X} = A X + B U \tag{3} X˙=AX+BU(3)
其中 X = [ x y z x ˙ y ˙ z ˙ g θ − g ϕ 0 g θ ˙ − g ϕ ˙ 0 ] T X = [\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=[xyzx˙y˙z˙gθ−gϕ0gθ˙−gϕ˙0]T,
U = [ u ϕ u θ 0 ] T U=[\begin{matrix} u_\phi & u_\theta & 0 \end{matrix}]^\text{T} U=[uϕuθ0]T,
A = [ 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 ] ⊗ I 3 A=\left[\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ \end{matrix}\right] \otimes I_3 A=⎣ ⎡0000100001000010⎦ ⎤⊗I3,
B = [ 0 0 0 1 ] ⊗ I 3 B=\left[\begin{matrix} 0 \\ 0 \\ 0 \\ 1 \\ \end{matrix}\right] \otimes I_3 B=⎣ ⎡0001⎦ ⎤⊗I3,
⊗ \otimes ⊗ 是克罗尼克积。

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

X ~ ˙ = I N ⊗ A X ~ + I N ⊗ B U ~ (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 ~ = [ X 1 T , X 2 T , … , X N T ] T \tilde{X} = [X_1^\text{T}, X_2^\text{T}, \dots, X_N^\text{T}]^\text{T} X~=[X1T,X2T,…,XNT]T,
U ~ = [ U 1 T , U 2 T , … , U N T ] T \tilde{U} = [U_1^\text{T}, U_2^\text{T}, \dots, U_N^\text{T}]^\text{T} U~=[U1T,U2T,…,UNT]T,
I N I_N IN 表示 N N N-维单位矩阵。

期望编队状态 h h h 定义为
h = [ h x h y h z ] ∈ R 3 (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 − h x y − h y z − h z ] (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 → ∞ ∥ v i ∥ = 0 lim ⁡ 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 × N N \times N N×N 拉氏矩阵 L L L， N e − L t , t > 0 N e^{-Lt}, t>0 Ne−Lt,t>0 是一个具有正对角元素的随机矩阵。如果 L L L 有一个唯一的 0 0 0 特征值， Rank ( N ) = N − 1 \text{Rank}(N) = N-1 Rank(N)=N−1，那么它的左特征值有 v = [ v 1 v 2 ⋯ v N ] T ≥ 0 v = [\begin{matrix} v_1 & v_2 & \cdots & v_N \end{matrix}]^\text{T} \ge 0 v=[v1v2⋯vN]T≥0 和 1 N T v = 1 , L T v = 0 1_N^\text{T} v = 1, L^\text{T} v = 0 1NTv=1,LTv=0，其中 t → ∞ , e − L t → 1 N v T t \rightarrow \infty, e^{-L t} \rightarrow 1_N v^\text{T} t→∞,e−Lt→1NvT。

3.1 编队控制

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

u i = α ∑ j ∈ N i ( δ j − δ i ) − β v i − γ 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 = [ δ i x , δ i y , δ i z ] T \delta_i = [\delta_i^x, \delta_i^y, \delta_i^z]^\text{T} δi=[δix,δiy,δiz]T,
v i = [ x ˙ i , y ˙ i , z ˙ i ] T v_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

假设 G G G 是连通无向图。如果协议（8）满足以下条件，系统（4）可以实现（7）中定义的编队。
α > 0 , β > 0 , γ 1 > 0 , γ 2 > 0 , β > > α , γ 1 , γ 2 > β > > α , β γ 1 γ 2 > β 2 + γ 2 2 α \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 δ z x ˙ y ˙ z ˙ g θ − g ϕ 0 g θ ˙ − 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δzx˙y˙z˙gθ−gϕ0gθ˙−gϕ˙0]T，性能函数定义为
J i = ∫ 0 ∞ [ X ˉ i T ( t ) Q X ˉ i ( t ) + u i T ( t ) R u i ( t ) ] d t (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 ∗ I 12 , R = r ∗ I 3 Q = q * I_{12}, R = r * I_{3} Q=q∗I12,R=r∗I3，其中 q > 0 , r > 0 q>0, r>0 q>0,r>0。

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

其中 P P P 是黎卡提方程（11）的解。
A T P + P A − P B R − 1 B T P + 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 − 1 B T P K = R^{-1} B^\text{T} P K=R−1BTP， K K K 的维度一定是 3 × 12 3 \times 12 3×12，同时也具有如下形式

K = [ k 1 k 2 k 3 k 4 ] ⊗ I 3 = [ 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 ] \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==[k1k2k3k4]⊗I3⎣ ⎡k1000k1000k1k2000k2000k2k3000k3000k3k4000k4000k4⎦ ⎤

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

u ∗ = k 1 ∑ j ∈ N i ( δ j − δ i ) − k 2 v i − k 3 Ω i − k 4 Ω ˙ 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∗=k1j∈Ni∑(δj−δi)−k2vi−k3Ωi−k4Ω˙i(12)

其中 k 1 , k 2 , k 3 , k 4 k_1, k_2, k_3, k_4 k1,k2,k3,k4 来源于矩阵 K K K。

定理 2

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

证明

将（1）代入（2），我们可以得到
U = Γ ⊗ I 3 X ˉ Γ = L ⊗ [ k 1 0 0 0 ] + I 3 ⊗ [ 0 k 2 k 3 k 4 ] \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=Γ=Γ⊗I3XˉL⊗[k1000]+I3⊗[0k2k3k4]

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

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

令 v 1 T v_1^\text{T} v1T

4 仿真

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

4.1 数值仿真

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

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

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

4.2 平台仿真

仿真软件CoppeliaSim/Vrep也被用来验证无人机编队的飞行情况。为了在操作过程中保持系统的视野，我们假设无人机保持静止状态。观察系统在不同时间段的位置，如图5所示，可以看出，系统最终可以完成三角形编队。完整版的实验视频在https://www.bilibili/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 input
    u1(:,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 states
    domega1(:,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 time
    t(:,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 input
    u1(:,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 states
    domega1(:,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 time
    t(:,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 input
    u1(:,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 states
    domega1(:,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 time
    t(:,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 input
    u1(:,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 states
    domega1(:,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 time
    t(:,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 error
    delta1 = p1(:,i) - p1h;
    delta2 = p2(:,i) - p2h;
    delta3 = p3(:,i) - p3h;
    % Calculate control input
    u1(:,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 states
    domega1(:,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 time
    t(:,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 on


tt1 = 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 on

legend("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  0
          0  0  1  0
          0  0  0  1
          0  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         0
         0   -0.6325   -0.0000         0   -2.0990   -0.0000         0   -3.1667   -0.0000         0   -2.5949   -0.0000
         0   -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  0
          0  0  1  0
          0  0  0  1
          0  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 time
    t(:,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

版权声明：本文标题：【Paper】2022_多无人机系统的分布式最优编队控制内容由热心网友自发贡献，该文观点仅代表作者本人，转载请联系作者并注明出处：https://m.elefans.com/xitong/1725767230a1041271.html，本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。

电子爱好者 - 最新技术资讯及电子产品介绍！

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

文章目录

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

更多相关文章

服务器重装ubuntu-16.04.7-server-amd64系统

Ubuntu系统切换五笔输入法

万能五笔输入法支持linux系统,Ubuntu安装万能五笔输入法

[Windows系统]查看电脑开关机时间

计算机系统不升级设置,笔记本电脑怎么设置不更新系统？

Win10系统打开什么都是反应比平时慢，转圈等待1分钟如何解决？

crtlc不能复制文件_win10系统按ctrl+c快捷键无法复制文件的操作方法

UEFI模式下用DISM抓取WIN 10 2019 LTSC系统镜像并还原

Win10使用VMware虚拟机使用MacOS10.14系统

微星Z390主板双SSD+HDD，win10系统下安装Ubuntu18.04系统借鉴参考

win系统排坑记

服务器2008系统如何设置休眠时间,Win7休眠和睡眠怎么开启（Win2008）

手机换android版本,安卓手机怎么换系统（最好能自己换）

微星z370安装linux系统,微星主板Z370、Z270、H110、H170设置U盘启动,U盘启动快捷键...

各个版本 Windows 10 系统中自带的 .NET Framework 版本

Windows10系统飞秋软件不信任防火墙

Windows删除EFI系统分区

Win11系统Windows.old能删除吗？Windows.old怎么删？

整活小能手！Windows系统滑动关机！一行脚本实现炫酷效果

麒麟系统挂载文件、麒麟系统挂载windows共享文件夹

发表评论

推荐文章

关于安卓以及微软用户chatgpt上一篇文章如今第五点无法正常进入更新解决方法以及附加本地部署

iPad 升级失败后重新恢复出厂设置

ARM和AMD区别

CentOS桥接模式下设置静态IP并解决java.net.ConnectException: Connection timed out: connect

智能手机防病毒软件作用有限，你会为此而担心吗？

热门文章

CHAPTER 5 Jenkins &amp; SonarQube

用Termux给手机杀毒

Educational Codeforces Round 55 (Rated for Div. 2)C. Multi-Subject Competition

(Talk-Bot,ichat助手,ChatK,DGAL,NextChat,FreeGPT,动点原版chatgpt)分享7个好用ChatGPT

无限注册谷歌Gmail邮箱账号的方法，可跳过手机验证

win7中计算机被改为了句号,WIN7中中文输入法快捷键无法修改的解决方法.doc

《winodws核心编程》随笔

AMD 7950x核显驱动报错The version of AMD Software that

win10系统，mysql-installer-community-5.7.19.0.msi安装

技嘉主板U盘启动UEFI引导模式

最新文章

Windows10 安装kali linux子系统

惠普服务器DL380 Gen10 安装Windows Server 2019

win 10计算机服务,win10 怎么打开服务_win10打开系统服务的3种方法

windows系统电脑如何启用或禁用smb功能

玩转 Windows 自带的 Linux 子系统 WSL（图文指南）

Windows系统删除大量文件导致系统卡顿解决方案

Windows10怎么设置双屏？双屏显示设置有哪些？

Windows10家庭版 SMB共享文件 方法

win10+Ubuntu双系统,设置默认启动项为win10

【windows10】使用宽带拨号即PPPoE拨号上网

Windows10创建工作组、加入工作组、查看工作组包含的所有电脑

CHAPTER 5 Jenkins & SonarQube

Windows10家庭版 SMB共享文件方法

【鬼泣5（Devil May Cry V）v1.0十四项修改】鬼泣5（Devil May Cry V）v1.0十四项修改官方免费下载