分享自己的一篇文章，發(fā)布在人工生命與機器人ICAROB2022，歡迎各位引用。

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

文章目錄

• 1 介紹
• 2 預備知識
• • 2.1 圖論知識
  • 2.2 無人機系統(tǒng)描述
• 3 主要結果
• • • • Lemma 1[6]
  • 3.1 編隊控制
  • • • 定理 1
      • 證明
  • 3.2 最優(yōu)控制
  • 3.3 分布式最優(yōu)編隊控制
  • • • 定理 2
      • 證明
• 4 仿真
• • 4.1 數(shù)值仿真
  • 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）的多智能體系統(tǒng)（MAS）編隊控制的分布式優(yōu)化問題。針對單個無人機的內部狀態(tài)可以完全獲得的情況，利用最優(yōu)控制理論設計了單個無人機的內部最優(yōu)控制法。為了應對系統(tǒng)中每個智能體只能與相鄰的智能體通信的障礙，根據(jù)系統(tǒng)的通信拓撲結構引入了系統(tǒng)的分布式編隊控制法，并進一步用圖論分析了系統(tǒng)的穩(wěn)定性。所設計方案的有效性通過數(shù)值模擬和無人機平臺得到了驗證。

1 介紹

隨著機器人能力的不斷提高，機器人的應用領域也在同時擴大。然而，就像人類一樣，單個機器人在很多情況下會表現(xiàn)出較低的能力，這樣一來，就需要多個代理的協(xié)調來發(fā)揮更大的作用。代理人的分布式協(xié)調和合作能力是多代理系統(tǒng)的基礎，是多代理系統(tǒng)發(fā)揮超額優(yōu)勢的關鍵，也是整個多代理系統(tǒng)智能的體現(xiàn)。

多Agent協(xié)調控制的問題包括共識控制[1]、會合控制[2]、編隊控制[3]，等等。然而，MAS的優(yōu)化對系統(tǒng)通信網(wǎng)絡有很大的依賴性。因為優(yōu)化控制法需要能夠 實時獲得系統(tǒng)中集體個體的所有狀態(tài)，否則就不能滿足優(yōu)化控制的條件。為了處理這個問題，我們設計了一種分布式最優(yōu)編隊控制，它對系統(tǒng)網(wǎng)絡結構是不變的。并將這一成果應用于多個無人駕駛飛行器的編隊控制。

本文的主要貢獻主要包括三個方面。1）設計了一種基于靜態(tài)共識協(xié)議的編隊控制協(xié)議。2）研究了單個代理的性能函數(shù)為最優(yōu)時的最優(yōu)控制法。3）結合編隊控制協(xié)議和最優(yōu)控制法，設計了一種分布式優(yōu)化編隊控制協(xié)議。該協(xié)議不會受到MAS內部通信拓撲結構的影響。即使有些代理不能相互通信，系統(tǒng)也能完成編隊任務。

2 預備知識

本節(jié)介紹了研究無人機系統(tǒng)的初步知識，包括使用圖論來描述系統(tǒng)內的通信關系，單一無人機的動態(tài)模型，以及無人機系統(tǒng)的狀態(tài)空間方程。

2.1 圖論知識

為了描述無人機系統(tǒng)的關系，使用了圖論[1]。$A=[a_{ij}\in \{0,1\}]$ 是圖 $G$ 的鄰接矩陣，表示從節(jié)點 $j$ 到 $i$ 是否有信息交流。矩陣 $D$ 是出度矩陣。節(jié)點 $i$ 的鄰居是 $N_i$。 Laplacian矩陣定義為

$$L=D?A\text{\hspace{1em}(1)}$$

2.2 無人機系統(tǒng)描述

如圖 1 所示，無人機模型可以簡化為公式（2）。

$$\begin{array}{rl}\ddot{x}& =g\theta \\ \ddot{y}& =?g?\\ \ddot{z}& =u_h/m?g\\ \ddot{?}& =u_{?}/I_x\\ \ddot{\theta }& =u_{\theta }/I_y\\ \ddot{\psi }& =u_{\psi }/I_z\end{array}\text{\hspace{1em}(2)}$$

其中 $x,y,z$ 是地面坐標系中沿 $X_g,Y_g,Z_g$ 的位置。$?,\theta ,\psi$ 分別為無人機的滾轉角、俯仰角、偏航角。$I_x,I_y,I_z$ 分別是沿 $X_b,Y_b,Z_b$ 在機體坐標系中的慣性矩。而 $u_h$ 是四個螺旋槳的升力。

---

為了便于計算，系統(tǒng)（2）被轉換為狀態(tài)空間格式，列于公式中（3）。
$$\dot{X}=AX+BU\text{\hspace{1em}(3)}$$
其中 $X=[\begin{array}{cccccccccccc}x& y& z& \dot{x}& \dot{y}& \dot{z}& g\theta & ?g?& 0& g\dot{\theta }& ?g\dot{?}& 0\end{array}{]}^{\text{T}}$,
$U=[\begin{array}{ccc}{u}_{?}& u_{\theta }& 0\end{array}{]}^{\text{T}}$,
$A=?\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}??I_3$,
$B=?\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}??I_3$,
$?$ 是克羅尼克積。

---

當有 $N$ 個無人機時，狀態(tài)空間方程（3）可以轉化為以下形式。

$$\dot{\tilde{X}}=I_N?A\tilde{X}+I_N?B\tilde{U}\text{\hspace{1em}(4)}$$

其中 $\tilde{X}=[X_1^{\text{T}},X_2^{\text{T}},\dots ,X_N^{\text{T}}{]}^{\text{T}}$,
$\tilde{U}=[U_1^{\text{T}},U_2^{\text{T}},\dots ,U_N^{\text{T}}{]}^{\text{T}}$,
$I_N$ 表示 $N$-維單位矩陣。

---

期望編隊狀態(tài) $h$ 定義為
$$h=?\begin{array}{c}{h}^x\\ h^y\\ h^z\end{array}?\in {\mathbb{R}}^3\text{\hspace{1em}(5)}$$

將編隊狀態(tài)（5）轉化為位置狀態(tài)，三個新的誤差位置狀態(tài)自然而然地出現(xiàn)，即（6）。

$$?\begin{array}{c}{\delta }^x\\ {\delta }^y\\ {\delta }^z\end{array}?=?\begin{array}{c}x?h^x\\ y?h^y\\ z?h^z\end{array}?\text{\hspace{1em}(6)}$$

那么編隊控制問題就變成了尋找一個協(xié)議 $\tilde{U}$ 來驅動誤差向量 $\delta$ 為零。這表明
$$\begin{array}{rl}\underset{t\to \infty }{\lim }\Vert {\delta }_i?{\delta }_j\Vert & =0,\underset{t\to \infty }{\lim }\Vert v_i\Vert =0\\ \underset{t\to \infty }{\lim }\Vert {\Omega }_i\Vert & =0,\underset{t\to \infty }{\lim }\Vert {\dot{\Omega }}_i\Vert =0,i=1,2,?,N\end{array}\text{\hspace{1em}(7)}$$

---

3 主要結果

本節(jié)首先介紹了編隊控制協(xié)議的設計。之后，詳細介紹了單個無人機的最優(yōu)控制法。最后，最優(yōu)控制法被擴展到集合的編隊控制協(xié)議中。

Lemma 1[6]

針對一個 $N\times N$ 拉氏矩陣 $L$，$N{e}^{?Lt},t>0$ 是一個具有正對角元素的隨機矩陣。如果 $L$ 有一個唯一的 $0$ 特征值，$\text{Rank}(N)=N?1$，那么它的左特征值有 $v=[\begin{array}{cccc}{v}_1& v_2& ?& v_N\end{array}{]}^{\text{T}}\ge 0$ 和 $1_N^{\text{T}}v=1,L^{\text{T}}v=0$，其中 $t\to \infty ,e^{?Lt}\to 1_N{v}^{\text{T}}$。

3.1 編隊控制

參照以前的工作，一致性協(xié)議可以分為兩種類型：一種是靜態(tài)的，另一種是動態(tài)的。在靜態(tài)一致性協(xié)議的基礎上，這里采用了以下形成控制協(xié)議，即（8）。

$$u_i=\alpha \sum _{j\in N_i}({\delta }_j?{\delta }_i)?\beta v_i?{\gamma }_1{\Omega }_i?{\gamma }_2\dot{\Omega }\text{\hspace{1em}(8)}$$

其中 $\alpha ,\beta ,{\gamma }_1,{\gamma }_2$ 都是正向增益，
${\delta }_i=[{\delta }_i^x,{\delta }_i^y,{\delta }_i^z{]}^{\text{T}}$,
$v_i=[{\dot{x}}_i,{\dot{y}}_i,{\dot{z}}_i{]}^{\text{T}}$,
${\Omega }_i=[g{\theta }_i,?g?,0{]}^{\text{T}}$,
${\dot{\Omega }}_i=[g{\dot{\theta }}_i,?g\dot{?},0{]}^{\text{T}}$.

---

定理 1

假設 $G$ 是連通無向圖。如果協(xié)議（8）滿足以下條件，系統(tǒng)（4）可以實現(xiàn)（7）中定義的編隊。
$$\begin{array}{r}\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{array}$$

證明

請參照參考文獻 [5]。

---

3.2 最優(yōu)控制

最優(yōu)控制的解決方案需要能夠獲得系統(tǒng)中所有的狀態(tài)信息，這在多代理系統(tǒng)中往往無法滿足。然而，可以通過研究單個代理中的最優(yōu)控制法來確定系統(tǒng)的最優(yōu)控制法。

在給出性能函數(shù)之前，令 $\bar{X}=[\begin{array}{cccccccccccc}{\delta }^x& {\delta }^y& {\delta }^z& \dot{x}& \dot{y}& \dot{z}& g\theta & ?g?& 0& g\dot{\theta }& ?g\dot{?}& 0\end{array}{]}^{\text{T}}$，性能函數(shù)定義為
$$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\text{\hspace{1em}(9)}$$

由于無人機在不同坐標軸上的狀態(tài)是獨立的，我們需要設置權重矩陣 $Q=q?I_{12},R=r?I_3$，其中 $q>0,r>0$。

---

通過最優(yōu)控制理論，單個智能體的最優(yōu)控制法則是
$$u_i^{?}=?R^{?1}{B}^{\text{T}}P\bar{X}\text{\hspace{1em}(10)}$$

其中 $P$ 是黎卡提方程（11）的解。
$$A^{\text{T}}P+PA?PB{R}^{?1}{B}^{\text{T}}P+Q=0\text{\hspace{1em}(11)}$$

3.3 分布式最優(yōu)編隊控制

通過上述計算，可以得到最優(yōu)控制法（10）。令 $K=R^{?1}{B}^{\text{T}}P$，$K$ 的維度一定是 $3\times 12$，同時也具有如下形式

$$\begin{array}{rl}K=& [\begin{array}{cccc}{k}_1& k_2& k_3& k_4\end{array}]?I_3\\ =& ?\begin{array}{cccccccccccc}{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{array}?\end{array}$$

無人機的多代理系統(tǒng)是同構的，即所有代理的動態(tài)性能都是一樣的，所以最優(yōu)控制法可以直接應用于無人機系統(tǒng)。因此最優(yōu)編隊控制法則為

$$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\text{\hspace{1em}(12)}$$

其中 $k_1,k_2,k_3,k_4$ 來源于矩陣 $K$。

---

定理 2

假設圖 $G$ 是連通無向圖。無人機系統(tǒng)（4）如果使用協(xié)議（12），可以完成編隊，同時使性能函數(shù)（9）最優(yōu)。

證明

將（1）代入（2），我們可以得到
$$\begin{array}{rl}U=& \Gamma ?I_3\bar{X}\\ \Gamma =& L?[\begin{array}{cccc}{k}_1& 0& 0& 0\end{array}]+I_3?[\begin{array}{cccc}0& k_2& k_3& k_4\end{array}]\end{array}$$

那么
$$\dot{\bar{X}}=A\bar{X}+B\Gamma ?I_3\bar{X}=\bar{\Gamma }\bar{X}$$

結合定理 1 的證明，$\bar{\Gamma }$ 可以變成一個約旦標準：
$$\bar{\Gamma }=PJ{P}^{?1}$$

令 $v_1^{\text{T}}$

---

4 仿真

在本節(jié)中，進行了數(shù)值模擬和虛擬平臺模擬，以驗證所設計的編隊控制協(xié)議的有效性。在本節(jié)中，進行了數(shù)值模擬和虛擬平臺模擬，以驗證所設計的編隊控制協(xié)議的有效性。

4.1 數(shù)值仿真

數(shù)值實驗分別驗證了編隊控制和分布式優(yōu)化編隊控制，如圖3和圖4所示。

在編隊控制中，可以觀察到系統(tǒng)可以完成三角形編隊，但在編隊完成過程中與設定位置有較大誤差，在第5秒時仍有誤差。當使用分布式最優(yōu)編隊控制時，可以觀察到系統(tǒng)可以快速完成三角形編隊，而且實際位置與設定值之間的誤差很小。

此外，令人驚訝的是，最優(yōu)控制不僅減少了系統(tǒng)的損失，還加快了系統(tǒng)的收斂速度，這對減少系統(tǒng)形成陣型的時間有很大幫助。

4.2 平臺仿真

仿真軟件CoppeliaSim/Vrep也被用來驗證無人機編隊的飛行情況。為了在操作過程中保持系統(tǒng)的視野，我們假設無人機保持靜止狀態(tài)。觀察系統(tǒng)在不同時間段的位置，如圖5所示，可以看出，系統(tǒng)最終可以完成三角形編隊。完整版的實驗視頻在https://www.bilibili.com/video/BV1Br4y1D7VJ/。

5 結論

本文通過分析無人機的動態(tài)模型，建立了一個多無人機系統(tǒng)。在靜態(tài)共識協(xié)議的基礎上，設計了編隊控制協(xié)議。針對系統(tǒng)中部分無人機無法獲得其他無人機狀態(tài)信息的問題，設計了單個無人機的最優(yōu)控制法。最后，結合編隊控制協(xié)議和最優(yōu)控制法，設計了一個多無人機系統(tǒng)的分布式優(yōu)化編隊控制協(xié)議。該協(xié)議不受多代理系統(tǒng)通信拓撲結構的干擾，并能在系統(tǒng)完成編隊任務的同時優(yōu)化性能函數(shù)。

下一步，我們計劃結合工程應用，將理論研究成果應用于實踐。

附錄：程序

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_多无人机系统的分布式最优编队控制的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內容還不錯，歡迎將生活随笔推薦給好友。