分享自己的一篇文章，发布在中国智能系统会议（CISC2021），欢迎各位引用，引用格式为：

Zhao J, Dai F, Song Y. Consensus of Heterogeneous Mixed-Order Multi-agent Systems Including UGV and UAV. Proceedings of 2021 Chinese Intelligent Systems Conference. Springer, Singapore, 2022: 202-210.

无法下载的也可以联系我分享论文原文。原文中有一些打错字的情况，万分抱歉。

详细版代码地址为：Consensus_of_Heterogeneous_Mixed-order_Multi-agent_Systems_including_UGV_and_UAV

摘要

本文分析了在现有混合阶动态方程情况下，异构多智能体系统的一致性。混合阶智能体主要包括二阶智能体，以无人驾驶地面车辆（UGV）为例，以及高阶智能体，以无人驾驶航空器（UAV）为例。我们定义了系统的两类一致性，包括动态一致性和静态一致性。我们为这两种一致性的定义提出了相应的一致性控制协议。对于异构多智能体系统，我们通过结合两种提议的控制协议分析了系统的稳定性。并通过数值模拟的形式验证了控制协议的有效性。

1 介绍

作为一门新兴的复杂系统科学，多智能体系统在执行既定任务时具有单一智能体无法比拟的优势，如多无人驾驶地面车辆（UGV）系统的协同作战和多无人驾驶航空器（UAV）系统的编队性能。一致性问题是多智能体系统协调运行的基础。只有当智能体的个体状态一致时，他们才能进一步聚集和合作。

多智能体系统的研究通常分为三个部分，包括单个智能体的动力学模型，多个智能体之间的通信关系，以及多代理系统要实现一致时的控制协议。目前，对多智能体的研究多以同构智能体为基础，对异构智能体的研究就更少。同时，异构智能体的顺序也大多是一样的。对混合阶的异构多智能体的研究就更少了。

Olfati-Saber等人 [1] 分析了具有切换拓扑结构和时间延迟的多智能体系统的一致性。Song [2] 分析了多智能体系统中存在一阶和二阶智能体时，异构智能体系统在离散时间内的一致性问题。Dong等人 [3] 研究了时变条件下无人机的系统编队。Yang等人 [4] 研究了关于无人机、无人地面飞行器（USV）和无人水下飞行器（UUV）的分布式控制。Shi等人 [5] 分析了线性和非线性条件下异构二阶多智能体系统的一致性问题。Zhao等人 [6] 分析了异构多智能体系统的一致性。

本文的主要贡献包括：1）设计了一个具有不等数量的混合阶智能体的异构多智能体系统合作架构；2）提出了静态和动态的一致性协议；3）分析了所提一致性协议下系统的稳定性。

2 预备知识

2.1 图论

在本文中，我们使用图论的拓扑结构来表示系统的通信关系。

我们将与无向图 $G$ 相关的图拉普拉斯矩阵定义为
$$L=D-A\text{\hspace{1em}(1)}$$

其中 $D$ 是度矩阵，$A$ 是邻接矩阵。

考虑到实际环境，信息流是双向的。也就是说，当智能体 $i$ 检测到智能体 $j$ 时，$j$ 同时也检测到了 $i$。所以我们假设图 $G$ 是无向的。

根据定义，拉氏矩阵的每一行和都为0。因此，拉氏矩阵总是有一个关于右特征向量 $w_r=(1/\sqrt{n})1_n=(1/\sqrt{n})(1,1,\cdots ,1{)}^{\text{T}}$ 的 0 特征值。

针对异构系统，我们将拉氏矩阵分割为以下四块
$$L=\left[\begin{array}{cc}{L}_{GG}& L_{GA}\\ L_{AG}& L_{AA}\end{array}\right]$$

其中 $L_{GG},L_{AA}$ 分别表示同构无人设备之间的通信。$L_{GA},L_{AG}$ 表示在异构系统 UGVs 和 UAVs 之间的通信。

引理 1 ([7])

针对一个 $N\times N$ 的拉氏矩阵， $L$，$e^{-Lt},\forall t\ge 0$ 是一个有正对角元素的随机矩阵。如果 $L$ 有一个唯一的 0 特征值，秩为 $\text{Rank}(L)=N-1$，它的左特征向量有 $v=[\begin{array}{cccc}{v}_1& v_2& \cdots & 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}}$。

$论文中引理写错了，应该是左特征向量，论文中写成了左特征值$

例子 1

存在一个 3 个 UGV，2 个 UAV 的系统。通信关系如图 1 所示。那么它的拉氏矩阵可以写成如下四部分的形式

$$L=\left[\begin{array}{ccc}\underset{L_{GG}}{\underbrace{\begin{array}{ccc}2& -1& -1\\ -1& 3& -1\\ -1& -1& 3\end{array}}}& & \underset{L_{GA}}{\underbrace{\begin{array}{cc}0& 0\\ -1& 0\\ 0& -1\end{array}}}\\ & & \\ \underset{L_{AG}}{\underbrace{\begin{array}{ccc}0& -1& 0\\ 0& 0& -1\end{array}}}& & \underset{L_{AA}}{\underbrace{\begin{array}{cc}2& -1\\ -1& 2\end{array}}}\end{array}\right]\text{\hspace{1em}(3)}$$

2.2 系统描述

二阶 UGV 动力学模型。
单个 UGV 的动力学模型为
$$\begin{array}{rl}{\dot{p}}_i=& v_i\\ {\dot{v}}_i=& u_i\end{array}\text{\hspace{1em}(4)}$$

其中 $p_i=[p_i^x,p_i^y{]}^{\text{T}}$ 表示在 2-D 平面的运动，$v_i=[v_i^x,v_i^y{]}^{\text{T}}$ 表示 $p_i$ 的速度，$u_i=[u_i^x,u_i^y{]}^{\text{T}}$ 表示智能体 $i$ 的输入。

针对 $k$ 个 UGV 时，将（4）转换为状态空间形式
$$\begin{array}{r}{\dot{X}}_G=A_G\cdot X_G+B_G\cdot U_G\end{array}\text{\hspace{1em}(5)}$$

其中 $X_G=[P_G,V_G{]}^{\text{T}}$,
$A_G=\left[\begin{array}{cc}{0}_{2k\times 2k}& 1_{2k\times 2k}\\ 0_{2k\times 2k}& 0_{2k\times 2k}\end{array}\right]$,
$B_G=\left[\begin{array}{c}{0}_{2k\times 2k}\\ 1_{2k\times 2k}\end{array}\right]$,
$U_G={\left[\begin{array}{ccccccc}{u}_1^x& u_1^y& u_2^x& u_2^y& \cdots & u_k^x& u_k^y\end{array}\right]}^{\text{T}}$,
$P_G=\left[\begin{array}{ccccccc}{p}_1^x& p_1^y& p_2^x& p_2^y& \cdots & p_k^x& p_k^y\end{array}\right]$,
$V_G=\left[\begin{array}{ccccccc}{v}_1^x& v_1^y& v_2^x& v_2^y& \cdots & v_k^x& v_k^y\end{array}\right]$.

高阶 UAV 动力学模型。
参考 [4,7]，我们采用如下 UAV 的模型
$$\begin{array}{rl}\ddot{x}& =g\theta \\ \ddot{y}& =-g\varphi \\ \ddot{z}& =f_z/m-g\\ \ddot{\varphi }& =M_{\varphi }/I_x\\ \ddot{\theta }& =M_{\theta }/I_y\\ \ddot{\psi }& =M_{\psi }/I_z\end{array}\text{\hspace{1em}(6)}$$

其中 $g$ 是重力加速度。$x,y,z$ 是三轴位置。$\varphi ,\theta ,\psi$ 分别为无人机的滚转角、俯仰角、偏航角。$f_z$ 是在高度方向上的升力。$M_{\varphi },M_{\theta },M_{\psi }$ 是机体坐标系中三轴方向上的力矩。$I_x,I_y,I_z$ 分别是在机体坐标系中的惯性矩。

针对 $l$ 个 UAVs，转化（6）为状态空间形式
$$\begin{array}{r}{\dot{X}}_A=A_A\cdot X_A+B_A\cdot U_A\end{array}\text{\hspace{1em}(7)}$$

其中 $X_A=[P_A,V_A,{\Omega }_A,{\dot{\Omega }}_A{]}^{\text{T}}$,
$A_A=\left[\begin{array}{ccc}{0}_{3l\times 3l}& 1_{3l\times 3l}{0}_{3l\times 3l}& 0_{3l\times 3l}\\ 0_{3l\times 3l}& 0_{3l\times 3l}{1}_{3l\times 3l}& 0_{3l\times 3l}\\ 0_{3l\times 3l}& 0_{3l\times 3l}{0}_{3l\times 3l}& 1_{3l\times 3l}\\ 0_{3l\times 3l}& 0_{3l\times 3l}{0}_{3l\times 3l}& 0_{3l\times 3l}\end{array}\right]$,
$B_A=\left[\begin{array}{c}{0}_{3l\times 3l}\\ 0_{3l\times 3l}\\ 0_{3l\times 3l}\\ 1_{3l\times 3l}\end{array}\right]$,
$U_A={\left[\begin{array}{cccccccccc}{u}_1^x& u_1^y& u_1^z& u_2^x& u_2^y& u_2^z& \cdots & u_l^x& u_l^y& u_l^z\end{array}\right]}^{\text{T}}$.

为了简化计算，我们假设 UAV 飞行在固定高度。也就是说，在 Z-轴方向上的控制输入为 0。于是

$P_A=\left[\begin{array}{cccccccccc}{p}_1^x& p_1^y& p_1^z& p_2^x& p_2^y& p_2^z& \cdots & p_l^x& p_l^y& p_l^z\end{array}\right]$,
$V_A=\left[\begin{array}{cccccccccc}{v}_1^x& v_1^y& v_1^z& v_2^x& v_2^y& v_2^z& \cdots & v_l^x& v_l^y& v_l^z\end{array}\right]$,
${\Omega }_A=\left[\begin{array}{cccccccccc}g{\theta }_1& -g{\varphi }_1& 0& g{\theta }_2& -g{\varphi }_2& 0& \cdots & g{\theta }_l& -g{\varphi }_l& 0\end{array}\right]$,
${\dot{\Omega }}_A=\left[\begin{array}{cccccccccc}g{\dot{\theta }}_1& -g\dot{{\varphi }_1}& 0& g{\dot{\theta }}_2& -g{\dot{\varphi }}_2& 0& \cdots & g{\dot{\theta }}_l& -g{\dot{\varphi }}_l& 0\end{array}\right]$.

异构多智能体系统。
为了更方便地分析异构多智能体系统，我们将UGV模型（5）和UAV模型（7）整合为一个状态空间模型，以便清晰描述。结合前面提到的，异构多智能体系统的状态空间公式可以表示为
$$\begin{array}{r}\dot{X}=A\cdot X+B\cdot U\end{array}\text{\hspace{1em}(8)}$$

其中$X=[X_G^{\text{T}},X_A^{\text{T}}{]}^{\text{T}}$,
$A=\left[\begin{array}{cc}{A}_G& 0\\ 0& A_A\end{array}\right]$, $B=\left[\begin{array}{cc}{B}_G& 0\\ 0& B_A\end{array}\right]$, $U=\left[\begin{array}{cc}{U}_G^{\text{T}}& U_A^{\text{T}}\end{array}\right]$.

Remark 1

在本节中，我们首先描述了UGV和UAV的单智能体模型。其次，用状态空间方程来描述多个的UGV和UAV。最后，我们提出UGV和UAV的合作结构。

关于多智能体系统的一致性，我们提出以下两种类型：

Definition 1 （动态一致性）

当所有智能体的状态都满足以下条件时，系统就会达成动态一致性：
$$\begin{array}{rl}& \underset{t\to \infty }{\lim }\Vert p_i(t)-p_j(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert v_i(t)-v_j(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert {\omega }_i(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert {\dot{\omega }}_i(t)\Vert =0,i,j=1,2,\cdots ,n\end{array}\text{\hspace{1em}(9)}$$

Definition 2 （静态一致性）

当所有智能体的状态都满足以下条件时，系统就会达成静态一致性：
$$\begin{array}{rl}& \underset{t\to \infty }{\lim }\Vert p_i(t)-p_j(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert v_i(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert {\omega }_i(t)\Vert =0,\\ & \underset{t\to \infty }{\lim }\Vert {\dot{\omega }}_i(t)\Vert =0,i,j=1,2,\cdots ,n\end{array}\text{\hspace{1em}(10)}$$

3 主要结果

3.1 一致性协议

针对上述的定义 1 和定义 2，我们提出下述两种类型一致性协议。

针对定义 1 的动态一致性协议分别为
$$\begin{array}{rl}（\text{UGV}）u_i=& \alpha \sum _{j\in N_i}{a}_{ij}(P_j-P_i)+\beta \sum _{j\in N_i}{a}_{ij}(V_j-V_i),i=1,2,\cdots ,k.\\ （\text{UAV}）u_i=& \alpha \sum _{j\in N_i}{a}_{ij}(P_j-P_i)+\beta \sum _{j\in N_i}{a}_{ij}(V_j-V_i)-{\gamma }_1{\Omega }_i-{\gamma }_2{\dot{\Omega }}_i,i=1,2,\cdots ,l.\end{array}\text{\hspace{1em}(11)}$$

其中 $\alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0$ 是控制因子。

结合拉氏矩阵（2），一致性协议可以表示为
$$U=-L_d\cdot X\text{\hspace{1em}(12)}$$

其中
$$L_d=\left[\begin{array}{cccccc}\alpha L_{GG}\otimes I_2& \beta L_{GG}\otimes I_2& \alpha L_{GA}\otimes T_1& \beta L_{GA}\otimes T_1& 0& 0\\ \alpha L_{AG}\otimes T_2& \beta L_{AG}\otimes T_2& \alpha L_{AA}\otimes I_3& \beta L_{AA}\otimes I_3& {\gamma }_1{I}_l\otimes I_3& {\gamma }_2{I}_l\otimes I_3\end{array}\right]\text{\hspace{1em}(13)}$$

$I$ 是单位矩阵，$\otimes$ 表示克罗尼克积，$T_1=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right],T_2=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\end{array}\right]$。

针对定义 2 的静态一致性协议分别为
$$\begin{array}{rl}（\text{UGV}）u_i=& \alpha \sum _{j\in N_i}{a}_{ij}(P_j-P_i)-\beta V_i,i=1,2,\cdots ,k.\\ （\text{UAV}）u_i=& \alpha \sum _{j\in N_i}{a}_{ij}(P_j-P_i)-\beta V_i-{\gamma }_1{\Omega }_i-{\gamma }_2{\dot{\Omega }}_i,i=1,2,\cdots ,l.\end{array}\text{\hspace{1em}(14)}$$

矩阵形式为
$$U=-L_s\cdot X\text{\hspace{1em}(15)}$$

其中
$$L_s=\left[\begin{array}{cccccc}\alpha L_{GG}\otimes I_2& \beta L_k\otimes I_2& \alpha L_{GA}\otimes T_1& 0& 0& 0\\ \alpha L_{AG}\otimes T_2& 0& \alpha L_{AA}\otimes I_3& \beta L_l\otimes I_3& {\gamma }_1{I}_l\otimes I_3& {\gamma }_2{I}_l\otimes I_3\end{array}\right]\text{\hspace{1em}(16)}$$

3.2 稳定性分析

定理 1

假设 $G=(V,E,A)$ 是连通无向图。异构混合阶系统（8）使用协议（11）且满足如下条件时，系统能达到动态一致。

$$\begin{array}{rl}& \alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0,\\ & \beta >>\alpha ,\text{and}{\gamma }_1,{\gamma }_2>\beta >>\alpha ,\\ & \beta {\gamma }_1{\gamma }_2>{\beta }^2+{\gamma }_2^2\alpha \end{array}$$

证明

将（12）代入（8）
$$\dot{X}={\Gamma }_d\cdot X\text{\hspace{1em}(17)}$$

其中
$${\Gamma }_d=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ \alpha L_{GG}\otimes I_2& \beta L_{GG}\otimes I_2& \alpha L_{GA}\otimes T_1& \beta L_{GA}\otimes T_1& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ \alpha L_{AG}\otimes T_2& \beta L_{AG}\otimes T_2& \alpha L_{AA}\otimes I_3& \beta L_{AA}\otimes I_3& {\gamma }_1{I}_l\otimes I_3& {\gamma }_2{I}_l\otimes I_3\end{array}\right]$$

根据线性系统稳定性定理。我们需要选择参数 $\alpha ,\beta ,{\gamma }_1,{\gamma }_2$ 使 ${\Gamma }_d$ 有一个零特征值，其他特征值均有负实部。参数 $\alpha ,\beta$ 需要满足 UGV 一致性的稳定。参数 ${\gamma }_1,{\gamma }_2$ 需要满足 UAV 一致性的稳定性。更多细节可参考文献 [4]。

选定参数之后， ${\Gamma }_d$ 可以转换为一个约当标准型 [1]：
$${\Gamma }_d=PJ{P}^{-1}$$

令 $v_1^{\text{T}}$ 是 $P^{-1}$ 的第一行，

4 仿真

我们对上述两种协议进行了数值模拟。假设一个由 3 个 UGV 和 2 个 UAV 组成的异构多智能体系统，其通信关系如图 1 所示。初始状态请参考原论文。

图 2 显示了系统在控制法则（11）下达到动态一致性。我们可以看到，当时间趋于无穷时，系统的速度是一致的。由于速度是一致而不是0，所以位置处于不断增加的状态。在系统达到动态一致后，位置增加的速度与一致后的速度相等。

当使用静态一致性协议（14）时，系统的状态如图 3 所示。静态一致性的定义是位置达到一致，速度变为零。因此，当时间趋于无穷时，我们可以看到系统的速度变为0，而位置保持不变。

5 结论

本文以UGV为例，分析了二阶智能体的动力学模型；以UAV为例，分析了高阶智能体的动力学模型。通过Kronecker乘积，将两种不同阶数的智能体组合成一个混合阶异构多智能体系统。混合阶智能体的数量可以是任意数量。针对异构多智能体系统提出了两种共识协议，包括动态一致性和静态一致性。分析了一致性协议融合后系统的稳定性。该协议通过数值模拟进行了验证。

至于未来的工作，我们打算致力于研究异构多智能体系统合作架构下的异常检测和识别问题。当然，还将增加无人机在高度方向的控制，这将扩大该系统的空中工作范围。

程序代码

% UGV/UAV 异构控制一致性控制
% Author: Zhao-Jichao
% Date: 2021-05-14
clear
clc


%% Laplacian Matrix
L = [2 -1 -1  0  0
    -1  3 -1 -1  0
    -1 -1  3  0 -1
     0 -1  0  2 -1
     0  0 -1 -1  2];


%% Initial States
% PositionX, PositionY, VeloctiyX, VelocityY (4)
% UGV1 = [90, 50,  10, 10]';
% UGV2 = [65, 10,  10, 10]';
% UGV3 = [55, 20,  10, 10]';
% PositionX, PositionY, PositionZ, VeloctiyX, VelocityY, VelocityZ,
% Pitch, Phi, Psi, dPitch, dPhi, dPsi (12)
% UAV4 = [25, 10, 10,  10, 10, 00,  10, 10, 00,  10, 10, 00]';
% UAV5 = [10, 30, 10,  10, 10, 00,  10, 10, 00,  10, 10, 00]';

% X0 = [UGV1; UGV2; UGV3; UAV4; UAV5];

P1X(1,1) = 90; P1Y(1,1) = 50; V1X(1,1) = 0; V1Y(1,1) = 15;
P2X(1,1) = 65; P2Y(1,1) = 10; V2X(1,1) = 10; V2Y(1,1) = 0;
P3X(1,1) = 55; P3Y(1,1) = 20; V3X(1,1) = 20; V3Y(1,1) = 15;

P4X(1,1) = 30;       P4Y(1,1) = 50;     P4Z(1,1) = 50;
V4X(1,1) = 00;       V4Y(1,1) = 10;     V4Z(1,1) = 0;
gtheta4(1,1) = 10;   gphi4(1,1) = 30;   gpsi4(1,1) = 0;
gdtheta4(1,1) = 00;  gdphi4(1,1) = 00;  gdpsi4(1,1) = 0;

P5X(1,1) = 90;       P5Y(1,1) = 30;     P5Z(1,1) = 50;
V5X(1,1) = 00;       V5Y(1,1) = 10;     V5Z(1,1) = 0;
gtheta5(1,1) = 20;   gphi5(1,1) = 15;   gpsi5(1,1) = 00;
gdtheta5(1,1) = 00;  gdphi5(1,1) = 00;  gdpsi5(1,1) = 00;

% Time parameters
tBegin = 0;
tFinal = 50;
dT = 0.01;
times = (tFinal-tBegin)/dT;
time(1,1) = 0;


%% Calculate
r1 = 0.1;
r2 = 0.8;
r3 = 4.0;
r4 = 1.5;
global protocol
protocol = 1;   % (1:Dynamic/2:Static)

for i=1:times
    time(i+1,1) = time(i,1) + dT;
    if protocol == 1
    u1x = r1*( (P2X(i,:)-P1X(i,:))+(P3X(i,:)-P1X(i,:)) ) + r2*( (V2X(i,:)-V1X(i,:))+(V3X(i,:)-V1X(i,:)) );
    u2x = r1*( (P1X(i,:)-P2X(i,:))+(P3X(i,:)-P2X(i,:)+(P4X(i,:)-P2X(i,:))) ) + r2*( (V1X(i,:)-V2X(i,:))+(V3X(i,:)-V2X(i,:))+(V4X(i,:)-V2X(i,:)) );
    u3x = r1*( (P1X(i,:)-P3X(i,:))+(P2X(i,:)-P3X(i,:)+(P5X(i,:)-P3X(i,:))) ) + r2*( (V1X(i,:)-V3X(i,:))+(V2X(i,:)-V3X(i,:))+(V5X(i,:)-V3X(i,:)) );
    u4x = r1*( (P2X(i,:)-P4X(i,:))+(P5X(i,:)-P4X(i,:)) ) + r2*( (V2X(i,:)-V4X(i,:))+(V5X(i,:)-V4X(i,:)) ) + r3*( -gtheta4(i,:)) + r4*( -gdtheta4(i,:));
    u5x = r1*( (P3X(i,:)-P5X(i,:))+(P4X(i,:)-P5X(i,:)) ) + r2*( (V3X(i,:)-V5X(i,:))+(V4X(i,:)-V5X(i,:)) ) + r3*( -gtheta5(i,:)) + r4*( -gdtheta5(i,:));
    u1y = r1*( (P2Y(i,:)-P1Y(i,:))+(P3Y(i,:)-P1Y(i,:)) ) + r2*( (V2Y(i,:)-V1Y(i,:))+(V3Y(i,:)-V1Y(i,:)) );
    u2y = r1*( (P1Y(i,:)-P2Y(i,:))+(P3Y(i,:)-P2Y(i,:)+(P4Y(i,:)-P2Y(i,:))) ) + r2*( (V1Y(i,:)-V2Y(i,:))+(V3Y(i,:)-V2Y(i,:))+(V4Y(i,:)-V2Y(i,:)) );
    u3y = r1*( (P1Y(i,:)-P3Y(i,:))+(P2Y(i,:)-P3Y(i,:)+(P5Y(i,:)-P3Y(i,:))) ) + r2*( (V1Y(i,:)-V3Y(i,:))+(V2Y(i,:)-V3Y(i,:))+(V5Y(i,:)-V3Y(i,:)) );
    u4y = r1*( (P2Y(i,:)-P4Y(i,:))+(P5Y(i,:)-P4Y(i,:)) ) + r2*( (V2Y(i,:)-V4Y(i,:))+(V5Y(i,:)-V4Y(i,:)) ) + r3*( -gphi4(i,:)) + r4*( -gdphi4(i,:));
    u5y = r1*( (P3Y(i,:)-P5Y(i,:))+(P4Y(i,:)-P5Y(i,:)) ) + r2*( (V3Y(i,:)-V5Y(i,:))+(V4Y(i,:)-V5Y(i,:)) ) + r3*( -gphi5(i,:)) + r4*( -gdphi5(i,:));
    end
    if protocol == 2
    u1x = r1*( (P2X(i,:)-P1X(i,:))+(P3X(i,:)-P1X(i,:)) ) + r2*( -V1X(i,:) );
    u2x = r1*( (P1X(i,:)-P2X(i,:))+(P3X(i,:)-P2X(i,:)+(P4X(i,:)-P2X(i,:))) ) + r2*( -V2X(i,:) );
    u3x = r1*( (P1X(i,:)-P3X(i,:))+(P2X(i,:)-P3X(i,:)+(P5X(i,:)-P3X(i,:))) ) + r2*( -V3X(i,:) );
    u4x = r1*( (P2X(i,:)-P4X(i,:))+(P5X(i,:)-P4X(i,:)) ) + r2*( (V2X(i,:)-V4X(i,:))+(V5X(i,:)-V4X(i,:)) ) + r3*( -gtheta4(i,:)) + r4*( -gdtheta4(i,:));
    u5x = r1*( (P3X(i,:)-P5X(i,:))+(P4X(i,:)-P5X(i,:)) ) + r2*( (V3X(i,:)-V5X(i,:))+(V4X(i,:)-V5X(i,:)) ) + r3*( -gtheta5(i,:)) + r4*( -gdtheta5(i,:));
    u1y = r1*( (P2Y(i,:)-P1Y(i,:))+(P3Y(i,:)-P1Y(i,:)) ) + r2*( -V1Y(i,:) );
    u2y = r1*( (P1Y(i,:)-P2Y(i,:))+(P3Y(i,:)-P2Y(i,:)+(P4Y(i,:)-P2Y(i,:))) ) + r2*( -V2Y(i,:) );
    u3y = r1*( (P1Y(i,:)-P3Y(i,:))+(P2Y(i,:)-P3Y(i,:)+(P5Y(i,:)-P3Y(i,:))) ) + r2*( -V3Y(i,:) );
    u4y = r1*( (P2Y(i,:)-P4Y(i,:))+(P5Y(i,:)-P4Y(i,:)) ) + r2*( (V2Y(i,:)-V4Y(i,:))+(V5Y(i,:)-V4Y(i,:)) ) + r3*( -gphi4(i,:)) + r4*( -gdphi4(i,:));
    u5y = r1*( (P3Y(i,:)-P5Y(i,:))+(P4Y(i,:)-P5Y(i,:)) ) + r2*( (V3Y(i,:)-V5Y(i,:))+(V4Y(i,:)-V5Y(i,:)) ) + r3*( -gphi5(i,:)) + r4*( -gdphi5(i,:));
    end

    
    V1X(i+1,1) = V1X(i,1)+dT*u1x; P1X(i+1,1) = P1X(i,1)+dT*V1X(i+1,1);
    V2X(i+1,1) = V2X(i,1)+dT*u2x; P2X(i+1,1) = P2X(i,1)+dT*V2X(i+1,1);
    V3X(i+1,1) = V3X(i,1)+dT*u3x; P3X(i+1,1) = P3X(i,1)+dT*V3X(i+1,1);
    
    gdtheta4(i+1,1) = gdtheta4(i,1)+dT*u4x; 
    gtheta4(i+1,1) = gtheta4(i,1)+dT*gdtheta4(i+1,1); 
    V4X(i+1,1) = V4X(i,1)+dT*gtheta4(i+1,1); 
    P4X(i+1,1) = P4X(i,1)+dT*V4X(i+1,1);
    
    gdtheta5(i+1,1) = gdtheta5(i,1)+dT*u5x; 
    gtheta5(i+1,1) = gtheta5(i,1)+dT*gdtheta5(i+1,1); 
    V5X(i+1,1) = V5X(i,1)+dT*gtheta5(i+1,1); 
    P5X(i+1,1) = P5X(i,1)+dT*V5X(i+1,1);
    
    V1Y(i+1,1) = V1Y(i,1)+dT*u1y; P1Y(i+1,1) = P1Y(i,1)+dT*V1Y(i+1,1);
    V2Y(i+1,1) = V2Y(i,1)+dT*u2y; P2Y(i+1,1) = P2Y(i,1)+dT*V2Y(i+1,1);
    V3Y(i+1,1) = V3Y(i,1)+dT*u3y; P3Y(i+1,1) = P3Y(i,1)+dT*V3Y(i+1,1);
    
    gdphi4(i+1,1) = gdphi4(i,1)+dT*u4y; 
    gphi4(i+1,1) = gphi4(i,1)+dT*gdphi4(i+1,1); 
    V4Y(i+1,1) = V4Y(i,1)+dT*gphi4(i+1,1); 
    P4Y(i+1,1) = P4Y(i,1)+dT*V4Y(i+1,1);
    
    gdphi5(i+1,1) = gdphi5(i,1)+dT*u5y; 
    gphi5(i+1,1) = gphi5(i,1)+dT*gdphi5(i+1,1); 
    V5Y(i+1,1) = V5Y(i,1)+dT*gphi5(i+1,1); 
    P5Y(i+1,1) = P5Y(i,1)+dT*V5Y(i+1,1);
end


%% Draw graphs
subplot(4,2,1, 'Position',[0.02 0.775 0.45 0.20])
plot(time,P1X, time,P2X, time,P3X, time,P4X, time,P5X, 'linewidth',1.5);
legend('P^X_{G1}','P^X_{G2}','P^X_{G3}','P^X_{A1}','P^X_{A2}'); grid on
title("X Positions")

subplot(4,2,3, 'Position',[0.02 0.525 0.45 0.20])
plot(time,V1X, time,V2X, time,V3X, time,V4X, time,V5X, 'linewidth',1.5);
legend('V^X_{G1}','V^X_{G2}','V^X_{G3}','V^X_{A1}','V^X_{A2}'); grid on
title("X Velocities")

subplot(4,2,5, 'Position',[0.02 0.275 0.45 0.20])
plot(time,gtheta4, time,gtheta5, 'linewidth',1.5);
legend('theta_{A1}','theta_{A2}'); grid on
title("X Omega")

subplot(4,2,7, 'Position',[0.02 0.025 0.45 0.20])
plot(time,gdtheta4, time,gdtheta5, 'linewidth',1.5);
legend('dtheta_{A1}','dtheta_{A2}'); grid on
title("X dotOmega")


subplot(4,2,2, 'Position',[0.53 0.775 0.45 0.20])
plot(time,P1Y, time,P2Y, time,P3Y, time,P4Y, time,P5Y, 'linewidth',1.5);
legend('P^Y_{G1}','P^Y_{G2}','P^Y_{G3}','P^Y_{A1}','P^Y_{A2}'); grid on
title("Y Positions")

subplot(4,2,4, 'Position',[0.53 0.525 0.45 0.20])
plot(time,V1Y, time,V2Y, time,V3Y, time,V4Y, time,V5Y, 'linewidth',1.5);
legend('V^Y_{G1}','V^Y_{G2}','V^Y_{G3}','V^Y_{A1}','V^Y_{A2}'); grid on
title("Y Velocities")

subplot(4,2,6, 'Position',[0.53 0.275 0.45 0.20])
plot(time,gphi4, time,gphi5, 'linewidth',1.5);
legend('phi_{A1}','phi_{A2}'); grid on
title("Y Omega")

subplot(4,2,8, 'Position',[0.53 0.025 0.45 0.20])
plot(time,gdphi4, time,gdphi5, 'linewidth',1.5);
legend('dphi_{A1}','dphi_{A2}'); grid on
title("Y dotOmega")

最后

以上就是自信信封为你收集整理的【Paper】2021_使用UGV和UAV的异构混合阶多智能体系统一致性摘要1 介绍2 预备知识3 主要结果4 仿真5 结论程序代码的全部内容，希望文章能够帮你解决【Paper】2021_使用UGV和UAV的异构混合阶多智能体系统一致性摘要1 介绍2 预备知识3 主要结果4 仿真5 结论程序代码所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。