分享自己的一篇文章，发布在人工生命与机器人ICAROB2022，欢迎各位引用。
A Distributed Optimal Formation Control for Multi-Agent System of UAVs
本文在此提出了无人机（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.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 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;
扫一扫
1308
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
