Collaborative Optimal Formation Control for Heterogeneous Multi-Agent Systems

Abstract

In this paper, the distributed optimal control method is used to study the cooperative formation of heterogeneous multi-agents in the air–ground environment. The considered system consists of an unmanned aerial vehicle (UAV) and an unmanned ground vehicle (UGV). The optimal control theory is introduced into the formation control protocol, the distributed optimal formation control protocol is designed, and the stability is verified by graph theory. Furthermore, the cooperative optimal formation control protocol is designed, and the stability is analyzed using a block Kronecker product and matrix transformation theory. Through the comparison of simulation results, the introduction of optimal control theory shortens the formation time of the system and accelerates the convergence speed of the system.

1. Introduction

With the rapid development of science and technology, the world’s military powers attach great importance to the cooperation capability of an unmanned combat system. In recent decades, air–ground heterogeneous unmanned combat systems, which consist of unmanned aerial vehicles (UAVs) and unmanned ground vehicles (UGVs), have been favored by military powers due to their fast response speed, strong communication capability, strong payload capacity, and high target reconnaissance accuracy [1,2,3].

In the field of multi-agent systems, cooperative control has received extensive attention and research. Examples of such applications include the field of robot collaboration, UAV formation, cooperative transport, and combat reconnaissance [4]. Formation control is an application hotspot in the field of distributed cooperation. In general, formation control can be divided into two categories according to the presence or absence of leader agents: leader and leaderless [5,6,7,8]. Reference [9] used the leader–follower method to complete the trajectory tracking task of the UAV–UGV system but did not cooperate in the process of completing the formation. Furthermore, Reference [10] studied the cooperation problem of the UAV–UGV system by improving the artificial physics approach but did not study the formation problem. Reference [11] studied the time-varying formation control of cooperative heterogeneous multi-agent systems and combined formation with cooperation.

In the cooperative reconnaissance and cooperative strike of the unmanned combat system, based on forming an established formation, the unmanned combat system must also identify the complex and volatile battlefield environments and quickly cross barriers, such as fences and fortifications. Therefore, quickly making the whole formation reach the desired state is also an important concern of the formation problem. Reference [12] used a virtual-structure-based approach and multiple-impedance control to achieve the optimal formation of three mobile robots, and the mobile robots carried out the cooperative formation. However, this study was based on the study of homogeneous multi-agents. In Reference [13], the leader–follower strategy and the virtual leader strategy were integrated into an optimal control framework to study the optimal formation of multiple UAVs. However, this study did not investigate the cooperation of multiple UAVs. For the optimal formation of heterogeneous multi-agent, there are few related research results. Reference [14] is based on using reinforcement learning methods to achieve the optimal formation of heterogeneous multi-agent systems but does not study cooperative control. It should be noted that optimal formation control alone can only solve a relatively limited number of problems, and cooperative optimal formation control is still an open problem.

In addition, most of the existing research results are based on the same dynamic model, namely the homogeneous agent model [15,16,17,18]. Compared with homogeneous multi-agent systems, multi-agent systems composed of heterogeneous dynamic models are more flexible in practical applications. Therefore, it is of great significance to study heterogeneous multi-agent systems. A large number of valuable research results have been obtained for the heterogeneous cooperative problem [19,20,21].

In this paper, the cooperative optimal formation problem of heterogeneous multi-agent systems is studied on the unmanned aerial vehicle and unmanned ground vehicle model. There are threefold main innovations:

Firstly, a heterogeneous modeled UAV–UGV system is proposed, a cooperative architecture of heterogeneous multi-agent systems with equal number is designed, and a Laplacian matrix of communication topology is designed. In addition, a novel block Kronecker product is used to describe the UAV–UGV system. Based on this, distributed formation control is proposed.

The second contribution is to introduce the optimal control method into the formation control protocol, design the distributed formation optimal control protocol, and prove the stability using the method of graph theory.

The third contribution is to design a cooperative formation control protocol for the air–ground system based on the heterogeneous system model so that the UAV–UGV system can achieve the cooperative formation effect. Then, the optimal control is introduced into the cooperative formation control protocol, and the cooperative optimal formation control protocol is designed, which enables the UAV–UGV system formation to quickly achieve the expected effect.

2. Preliminaries

This section mainly introduces the preliminary knowledge of unmanned aerial vehicles and unmanned ground vehicles, including the use of graph theory to describe the internal relationship of the system and the state-space equation of the UAV system and the UGV system.

2.1. Graph Theory

A weighted undirected graph $G=\left(V,E,A\right)$ consists of $n$ vertices, where $V=\left(v_1,v_2,\cdots v_n\right)$ represents the set of all vertices in the undirected graph, and each vertex represents an agent. $E=\left\{e_{ij}=\left(v_i,v_j\right)\right\}\subseteq v\times v$ represents the edge set between vertices, and $e_{ij}=\left(v_i,v_j\right)$ represents the edge from $v_i$ vertex to $v_j$ vertex; an edge connection between two vertices indicates that there is an information interaction between these two vertices. The graph is undirected if it allows two-way communication; otherwise, it is directed.$A={\left[a_{ij}\right]}_{n\times n}$ represents the adjacency matrix indicating the relationship between agents, where $a_{ij}$ is the weight of the side $e_{ij}=\left(v_i,v_j\right)$ and where the diagonal elements of the matrix $A$ are all 0. For $i,j=1,2,3,\cdots ,n\left(i\ne j\right)$, if the agents $v_i$ and $v_j$ can receive information from each other, then the elements in the adjacency matrix are $a_{ij}=a_{ji}>0$; otherwise, the element in the adjacency matrix is $0.$

In an undirected graph, the degree represents the number of neighbors of a node, that is, the number of edges per node. $D=dig\left\{d_1,\dots ,d_n\right\}$ of undirected graph G is a diagonal matrix with $d_i={\displaystyle \sum }_{j=1}^n{a}_{ij}$.Then, the Laplacian matrix of G is defined as L = D $-$ A, which has at least one zero eigenvalue with $1={\left[1,1\dots ,1\right]}^T$ as its corresponding right eigenvector. In addition, L has exactly one zero eigenvalue if and only if the directed graph G contains a directed spanning tree.

2.2. UGV Dynamics Model

Single UGV motion model:

$$\{\begin{array}{l}{\dot{p}}_{gi}=v_{gi}\hfill \\ {\dot{v}}_{gi}=u_{gi}\hfill \end{array}$$

(1)

where $p_{gi}={\left[p_{gi}^x,p_{gi}^y,p_{gi}^z\right]}^T$ stands for the position in ground space, $v_{gi}={\left[v_{gi}^x,v_{gi,}^y,v_{gi}^z\right]}^T$ is the velocity in the direction $p_{gi}$, and $u_{gi}={\left[u_{gi}^x,u_{gi}^y,u_{gi}^z\right]}^T$ represents the input of agent $i$. If there are $\mathrm{k}$ UGVs, the above formula is converted to the states:

$${\dot{X}}_G=A_G{X}_G+B_G{U}_G$$

(2)

where $X_G={\left(P_G,V_G\right)}^T$,$P_G=\left(p_1,p_2,p_3\dots p_k\right)$,$p_i=\left(x_i,y_i,z_i\right),i=1,2,\dots ,k$;

$$V_G=\left(v_1,v_2,v_3\dots v_k\right),v_i=\left(v_i^x,v_i^y,v_i^z\right),i=1,2,\dots ,k$$

$$U_G=\left(u_1,u_2,u_3\dots u_k\right),u_i=\left(u_i^x,u_i^y,u_i^z\right),i=1,2,\dots ,k$$

$$A_G=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\otimes I_{\mathrm{k}},B_G=\left[\begin{array}{c}0\\ 1\end{array}\right]\otimes I_{\mathrm{k}}.$$

Subscript G represents the state variable of the unmanned vehicle.

The expected formation state is $h_g={\left(h_g^x,h_g^y,h_g^z\right)}^T$, the formation state is transformed into the position state, and the new error position state naturally appears, namely:${\delta }_G={\left({\delta }_G^x,{\delta }_G^y,{\delta }_G^Z\right)}^T={\left(P_{gi}^x-h_g^x,P_{gi}^y-h_g^y,P_{gi}^z-h_g^z\right)}^T$.

Therefore, the problem of formation control becomes finding a protocol $U_G$ to drive the error vector ${\delta }_G$ to zero, which means that

$${lim}_{t\to \infty }^{ }\parallel {\delta }_{gi}-{\delta }_{gj}\parallel =0,{lim}_{t\to \infty }^{ }=\parallel v_{gi}\parallel =0.$$

(3)

2.3. UAV Dynamics Model

The motion model of a single UAV is:

$$\{\begin{array}{l}\ddot{x}=g\theta \hfill \\ \ddot{y}=-g\varphi \hfill \\ \ddot{z}=f_z/m-g\hfill \\ \ddot{\varphi }=M_{\varphi }/I_x\hfill \\ \ddot{\theta }=M_{\theta }/I_y\hfill \\ \ddot{\phi }=M_{\phi }/I_z\hfill \end{array}$$

(4)

where $g$ is the acceleration of gravity; $x,y,$ and $z$ are the positions of the UAV in three coordinate systems; $\varphi ,\theta ,$ and $\phi$ are the roll angle, the pitch angle, and the yaw angle of the UAV, respectively; $f_z$ is the lift force in the direction of height; $M_{\varphi },M_{\theta },$ and $M_{\phi }$ are the torques on the three axes of the body coordinate system; $I_x,I_y$, and $I_Z$ are the inertial matrices in the body coordinate system. For $\mathrm{L}$ UAVs, the above equations are converted to the state-space form as follows:

$${\dot{X}}_A=A_A{X}_A+B_A{U}_A$$

(5)

where $X_A={\left(P_A,V_A,{\mathsf{\Omega }}_A,{\dot{\mathsf{\Omega }}}_A\right)}^T$,$P_A=\left(p_1,p_2,p_3\dots p_l\right)$,$p_i=\left(x_i,y_i,z_i\right),i=1,2,\dots ,l$;

$$V_A=\left(v_1,v_2,v_3\dots v_l\right),v_i=\left(v_i^x,v_i^y,v_i^z\right),i=1,2,\dots ,l$$

$${\mathsf{\Omega }}_A=\left({\mathsf{\Omega }}_1,{\mathsf{\Omega }}_2,{\mathsf{\Omega }}_3,\dots {\mathsf{\Omega }}_l\right),{\mathsf{\Omega }}_i=\left(g{\theta }_i,-g{\varphi }_i,0\right),i=1,2,\dots ,l$$

$${\dot{\mathsf{\Omega }}}_A=\left({\dot{\mathsf{\Omega }}}_1,{\dot{\mathsf{\Omega }}}_2,{\dot{\mathsf{\Omega }}}_3,\dots {\dot{\mathsf{\Omega }}}_l\right),{\dot{\mathsf{\Omega }}}_i=\left(g{\dot{\theta }}_i,-g{\dot{\varphi }}_i,0\right),i=1,2,\dots ,l$$

$$A_A=\left(\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right)\otimes I_l,B_A=\left(\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right)\otimes I_l$$

$$U_A={\left(u_1,u_2,u_3\dots u_l\right)}^T,u_i=\left(u_i^x,u_i^y,u_i^z\right),i=1,2,\dots ,l.$$

Subscript A represents the state variable of the unmanned vehicle.

The expected formation state is $h_a={\left(h_a^x,h_a^y,h_a^z\right)}^T$, the formation state is transformed into the position state, and the new error position state naturally appears, namely: ${\delta }_A={\left({\delta }_A^x,{\delta }_A^y,{\delta }_A^z\right)}^T={\left(P_{ai}^x-h_a^x,P_{ai}^y-h_a^y,P_{ai}^z-h_a^z\right)}^T$.

Therefore, the problem of formation control becomes finding a protocol $U_A$ to drive the error vector ${\mathsf{\delta }}_{\mathrm{A}}$ to zero, which means that

$${lim}_{t\to \infty }^{ }\parallel {\delta }_{ai}-{\delta }_{aj}\parallel =0,{lim}_{t\to \infty }^{ }=\parallel v_{ai}\parallel =0,{lim}_{t\to \infty }^{ }\parallel {\mathsf{\Omega }}_i\parallel =0,{lim}_{t\to \infty }^{ }\parallel {\dot{\mathsf{\Omega }}}_i\parallel =0.$$

(6)

2.4. Heterogeneous Multi-Agent System

To analyze heterogeneous multi-agent systems more conveniently, the UAV system and UGV system are written into the same state space and combined with the state-space model of the single agent above; the form of the heterogeneous multi-agent state-space model is defined as:

$$\dot{X}=AX+BU$$

(7)

where $X={\left(X_G^T,X_A^T\right)}^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), and U={\left(U_G^T,U_A^T\right)}^T$. The Laplace matrix is $L=\left(\begin{array}{cc}{L}_{AA}& L_{AG}\\ L_{GA}& L_{GG}\end{array}\right)$, where $L_{AG},L_{GA}$ represents information between heterogeneous agent systems. This paper takes the heterogeneous multi-agent system composed of three UGVs and three UAVs as the research object, and its Laplace matrix relationship is as follows:

$$L=\left(\begin{array}{cc}\begin{array}{c}\underset{{{\displaystyle L}}_{AA}}{\underbrace{\begin{array}{cc}\begin{array}{c}-3\\ \begin{array}{c}1\\ 1\end{array}\end{array}& \begin{array}{cc}\begin{array}{c}1\\ \begin{array}{c}-3\\ 1\end{array}\end{array}& \begin{array}{c}1\\ \begin{array}{c}1\\ -3\end{array}\end{array}\end{array}\end{array}}}\\ \underset{{{\displaystyle L}}_{GA}}{\underbrace{\begin{array}{cc}\begin{array}{c}\begin{array}{c}0\\ 0\end{array}\\ 1\end{array}& \begin{array}{cc}\begin{array}{c}\begin{array}{c}0\\ 1\end{array}\\ 0\end{array}& \begin{array}{c}\begin{array}{c}1\\ 0\end{array}\\ 0\end{array}\end{array}\end{array}}}\end{array}& \begin{array}{c}\underset{{{\displaystyle L}}_{AG}}{\underbrace{\begin{array}{cc}\begin{array}{cc}\begin{array}{c}\begin{array}{c}0\\ 0\end{array}\\ 1\end{array}& \begin{array}{c}\begin{array}{c}0\\ 1\end{array}\\ 0\end{array}\end{array}& \begin{array}{c}\begin{array}{c}1\\ 0\end{array}\\ 0\end{array}\end{array}}}\\ \underset{{{\displaystyle L}}_{GG}}{\underbrace{\begin{array}{cc}\begin{array}{c}\begin{array}{c}-3\\ 1\end{array}\\ 1\end{array}& \begin{array}{cc}\begin{array}{c}\begin{array}{c}1\\ -3\end{array}\\ 1\end{array}& \begin{array}{c}\begin{array}{c}1\\ 1\end{array}\\ -3\end{array}\end{array}\end{array}}}\end{array}\end{array}\right)$$

The expected formation state is $h={\left(h_a^T,h_g^T\right)}^T$, the formation state is transformed into the position state, and the new error position state naturally appears, namely:

$$\delta ={\left({\delta }_A^x,{\delta }_A^y,{\delta }_A^Z,{\delta }_G^x,{\delta }_G^y,{\delta }_G^Z\right)}^T={\left(P_{ai}^x-h_a^x,P_{ai}^y-h_a^y,P_{ai}^z-h_a^z,P_{gi}^x-h_g^x,P_{gi}^y-h_g^y,P_{gi}^z-h_g^z\right)}^T.$$

Therefore, the problem of formation control becomes finding a protocol $U$ to drive the error vector $\delta$ to zero, which means that

$$\underset{t\to \infty }{lim}\parallel {\delta }_i-{\delta }_j\parallel =0,\underset{t\to \infty }{lim}=\parallel v_i\parallel =0,\underset{t\to \infty }{lim}\parallel {\mathsf{\Omega }}_i\parallel =0,\underset{t\to \infty }{lim}\parallel {\dot{\mathsf{\Omega }}}_i\parallel =0$$

(8)

3. Design of Control Protocol

To realize the formation of heterogeneous multi-agent systems of UAVs and UGVs, this section is based on the formation control protocol. Firstly, the optimal control law is applied to the single agent. Then, according to the combination of the optimal control law and the formation control protocol, a heterogeneous multi-agent system with distributed optimal formation control is realized. Finally, according to the motion equation of the heterogeneous multi-agent system, the cooperative formation control and cooperative optimal formation control of the heterogeneous multi-agent system are realized.

Lemma 1.

[22]. For an $N*N$ Laplacian matrix $L,N{e}^{-Lt},t>0$ is a random matrix with positive diagonal elements. If $L$ has a unique zero eigenvalue, Rank $\left(N\right)=N-1$, then its left eigenvector has $v={\left[v_1,v_2,\cdots v_n\right]}^T\ge 0$ and $1_N^{T}v=1, L^{T}v=0$, where, $t\to \infty$,$e^{-Lt}\to 1_N{v}^T$.

3.1. Formation Control

Formation control protocol for the UAVs:

$$u_{ia}=\alpha {{\displaystyle \sum }}_{j\in N_i}^{ }\left({\delta }_{aj}-{\delta }_{ai}\right)-\beta v_{ai}-{\gamma }_1{\mathsf{\Omega }}_i-{\gamma }_2{\dot{\mathsf{\Omega }}}_i.$$

(9)

Formation control protocol for the UGVs:

$$u_{ig}=\alpha {{\displaystyle \sum }}_{j\in N_i}^{ }\left({\delta }_{gj}-{\delta }_{gi}\right)-\beta v_{gi}$$

(10)

where $\alpha ,\beta ,{\gamma }_1,{\gamma }_2$ represent the positive gain coefficients,

$${\delta }_{ai}={\left({\delta }_{ai}^x,{\delta }_{ai}^y,{\delta }_{ai}^z\right)}^T, {\delta }_{gi}={\left({\delta }_{gi}^x,{\delta }_{gi}^y,{\delta }_{gi}^z\right)}^T, v_{gi}={\left({\dot{P}}_{gi}^x,{\dot{P}}_{gi}^y,{\dot{P}}_{gi}^z\right)}^T, v_{ai}={\left({\dot{P}}_{ai}^x,{\dot{P}}_{ai}^y,{\dot{P}}_{ai}^z\right)}^T$$

$${\mathsf{\Omega }}_i={(g\theta ,-g\varphi ,0)}^T,{\dot{\mathsf{\Omega }}}_i={(g\dot{\theta },-g\dot{\varphi },0)}^T$$

Protocols (9) and (10) shall be unified into the same type:

$$U=-L_d\cdot \tilde{X}$$

(11)

Define the state-space form of the multi-agent system formation:

$$\dot{\tilde{X}}=A\tilde{X}+BU$$

(12)

where

$$L_d=\left(\begin{array}{llllll}\alpha L_s\otimes I_3\hfill & -\beta I_l\otimes I_3\hfill & -{\gamma }_1{I}_l\otimes I_3\hfill & -{\gamma }_2{I}_l\otimes I_3\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & \alpha L_s\otimes I_3\hfill & -\beta I_k\otimes I_3\hfill \end{array}\right),$$

$L_S=\left(\begin{array}{lll}-1\hfill & 1\hfill & 0\hfill \\ 0\hfill & -1\hfill & 0\hfill \\ 1\hfill & 0\hfill & -1\hfill \end{array}\right), \tilde{X}={\left({\delta }_{ai}^T,v_{ai}^T,{\mathsf{\Omega }}^T,{\dot{\mathsf{\Omega }}}^T,{\delta }_{gi}^T,v_{gi}^T\right)}^T,A=\left(\begin{array}{cc}{A}_A& 0\\ 0& A_G\end{array}\right), B=\left(\begin{array}{cc}{B}_A& 0\\ 0& B_G\end{array}\right),$$I$ is the identity matrix, and $\otimes$ is the Kronecker product.

Theorem 1.

If Protocols (9) and (10) are satisfied$\alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0$,$\beta \gg \alpha ,{\gamma }_1>\beta ,{\gamma }_2>\beta ,\beta {\gamma }_1{\gamma }_2>{\beta }^2{\gamma }_2\alpha$, Systems (2) and (5) can implement the formation defined in (3) and (6).

Proof of Theorem 1.

Substitute Formula (11) into Formula (12) to obtain: $\dot{\tilde{X}}=-T_d\cdot \tilde{X}$ where $T_d=\left[\begin{array}{llllll}0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 1\hfill & 0\hfill & 0\hfill \\ \alpha L_s\otimes I_3\hfill & -\beta I_l\otimes I_3\hfill & -{\gamma }_1{I}_l\otimes I_3\hfill & -{\gamma }_2{I}_l\otimes I_3\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 1\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & \alpha L_s\otimes I_3\hfill & -\beta I_k\otimes I_3\hfill \end{array}\right]$

According to the linear stability theorem, the parameters $\alpha ,\beta ,{\gamma }_1$, and ${\gamma }_2$ need to be selected so that $T_d$ has a zero eigenvalue and other eigenvalues have genuine negative parts. The parameters $\alpha$ and $\beta$ need to meet the stability of UGV consistency, and the parameters ${\gamma }_1$ and ${\gamma }_2$ need to meet the stability of UAV consistency. After selecting parameters, $T_d$ can be converted to a Jordan standard type: $T_d=PJ{P}^{-1}$. Let $v_1^T$ be the first row of $P^{-1}$ and the left eigenvector have eigenvalue 0. Let $w_1 \mathrm{be}$ the first column of $P$ and the right eigenvector have eigenvalue $0.$ Therefore, $v_1^T{w}_1=1$; as time approaches infinity, the system’s state becomes:. According to Lemma 1, as time approaches infinity, Systems (2) and (5) asymptotically agree and the systems complete formation. □

3.2. Optimal Control

The solution of optimal control requires the states of all multi-agents. Before providing performance indicators, $\overline{X}$ and $\overline{W}$ are defined:

$\overline{X}={\left({\delta }_{ai}^x,{\delta }_{ai}^y,{\delta }_{ai}^z,{\dot{P}}_{ai}^x,{\dot{P}}_{ai}^y,{\dot{P}}_{ai}^z,g\theta ,-g\varphi ,0,g\dot{\theta },-g\dot{\varphi },0\right)}^T,\overline{W}={\left({\delta }_{gi}^x,{\delta }_{gi}^y,{\delta }_{gi}^z,{\dot{P}}_{gi}^x,{\dot{P}}_{gi}^y,{\dot{P}}_{gi}^z\right)}^T$ Then, define the performance indicator function as:

$$J_i={{\displaystyle \int }}_0^{\infty }\left[{\overline{X}}_i^{T}Q{\overline{X}}_i+u_{ia}^{T}R{u}_{ia}\right]dt$$

(13)

$${\omega }_i={{\displaystyle \int }}_0^{\infty }\left[{\overline{w}}_i^{T}Q{\overline{w}}_i+u_{ig}^T\gamma u_{ig}\right]dt$$

(14)

As the UAV and the UGV are independent in different coordinate systems, the UAV weight must be set to $Q=q^{\ast }{I}_{12},R=r*I_3,$ where, $q>0, r>0$. The UGV weight must be set to $T=\lambda *I_6,\gamma =\mu *I_3$, where $\lambda >0,\mu >0$.

According to the optimal control theory, the optimal control law of a single agent UAV is: $u_a^{*}=-R^{-1}{B}_A^T{P}_A\overline{X}$, where $P_A$ is the solution of Riccati’s in Equation (15)

$$A_A^T{P}_A+P_A{A}_A-P_A{B}_A{R}^{-1}{B}_A^T{P}_A+Q=0$$

(15)

The optimal control law for a single agent UGV is: $u_g^{*}=-{\gamma }^{-1}{B}_G^T{P}_G\overline{w}$, where $P_G$ is the solution of Riccati’s in Equation (16)

$$A_G^T{P}_G+P_G{A}_G-P_G{B}_G{\gamma }^{-1}{B}_G^T{P}_G+T=0$$

(16)

Through the above calculation, the optimal control law $u_a^{*}$ can be obtained. Let $K=R^{-1}{B}_A^T{P}_A$, the dimension of matrix $K$ is $3\ast 12,K$ expressed as $K=\left[k_1,k_2,k_3,k_4\right]\otimes I_3.$ Similarly, the optimal control law $u_g^{*}$ can also be solved, let $G={\gamma }^{-1}{B}_G^T{P}_G$ the dimension of matrix $G$ is $3\ast 6,G$ expressed as $G=\left[g_1,g_2\right]\otimes I_3$.

3.3. Distributed Optimal Formation Control

All UAVs have the same dynamics model, so all UAVs are homogeneous multi-agents. Similarly, all UGVs are homogeneous multi-agents. Therefore, optimal control laws can be extended to the formation control of UAV and UGV multi-agent systems.

$$u_{ia}^{*}=k_1{{\displaystyle \sum }}_{j\in N_i}^{ }\left({\delta }_{aj}-{\delta }_{ai}\right)-k_2{v}_{ai}-k_3{\mathsf{\Omega }}_i-k_4{\dot{\mathsf{\Omega }}}_i$$

(17)

$$u_{ig}^{*}=g_1{{\displaystyle \sum }}_{j\in N_i}^{ }\left({\delta }_{gj}-{\delta }_{gi}\right)-g_2{v}_{gi}$$

(18)

Define the multi-agent system to be optimized:

$$\dot{\tilde{X}}=A\tilde{X}+BU$$

(19)

where $\tilde{X}={\left({\delta }_{ai}^T,v_{ai}^T,{\mathsf{\Omega }}^T,{\dot{\mathsf{\Omega }}}^T,{\delta }_{gi}^T,v_{gi}^T\right)}^T,A=\left(\begin{array}{cc}{A}_A& 0\\ 0& A_G\end{array}\right), B=\left(\begin{array}{cc}{B}_A& 0\\ 0& B_G\end{array}\right)$,$k_1,k_2,k_3,$ and $k_4$ are derived from the matrix $K.g_1$ and $g_2$ are derived from the matrix $G$.

Theorem 2.

If the unmanned ground vehicle system in (2) and the unmanned aerial vehicle system in (5) use Protocols (17) and (18), respectively, the formation can be completed, and Performance Functions (13) and (14) can be optimized.

Proof of Theorem 2.

Protocols (17) and (18) shall be unified into the same type:

$$U=-L_l\cdot \tilde{X}$$

(20)

where

$$L_l=\left(\begin{array}{llllll}{k}_1{L}_s\otimes I_3\hfill & -k_2{I}_l\otimes I_3\hfill & -k_3{I}_l\otimes I_3\hfill & -k_4{I}_l\otimes I_3\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & g_1{L}_s\otimes I_3\hfill & -g_2{I}_k\otimes I_3\hfill \end{array}\right),$$

$L_S=\left(\begin{array}{lll}-1\hfill & 1\hfill & 0\hfill \\ 0\hfill & -1\hfill & 0\hfill \\ 1\hfill & 0\hfill & -1\hfill \end{array}\right), \tilde{X}={\left({\delta }_{ai}^T,v_{ai}^T,{\mathsf{\Omega }}^T,{\dot{\mathsf{\Omega }}}^T,{\delta }_{gi}^T,v_{gi}^T\right)}^T,I$ is the identity matrix, and $\otimes$ is the Kronecker product.

Let $U=\left(\begin{array}{cc}{U}_{11}& U_{12}\\ U_{21}& U_{22}\end{array}\right)$=$\left(\begin{array}{llllll}{k}_1{L}_s\hfill & -k_2{I}_l\hfill & -k_3{I}_l\hfill & -k_4{I}_l\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & g_1{L}_s\hfill & -g_2{I}_k\hfill \end{array}\right)$

Substituting $U=\left(\begin{array}{cc}{U}_{11}& U_{12}\\ U_{21}& U_{22}\end{array}\right)\cdot \tilde{X}$ into Equation (19): $\dot{\tilde{X}}=-T_l\cdot \tilde{X}$

where

$$T_l=\left[\begin{array}{llllll}0\hfill & I\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & I\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & I\hfill & 0\hfill & 0\hfill \\ k_1{L}_s\hfill & -k_{2}I\hfill & -k_{3}I\hfill & -k_{4}I\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & I\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & g_1{L}_s\hfill & -g_{2}I\hfill \end{array}\right]$$

Elementary row and column transformation can be taken on $T_l$:

$$T_l=\left[\begin{array}{llllll}I\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & I\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & I\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & I\hfill & 0\hfill \\ 0\hfill & 0\hfill & k_1{L}_s\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & g_1{L}_s\hfill \end{array}\right]=\left(\begin{array}{ccc}{I}_1& 0& 0\\ 0& 0& I_2\\ 0& k_{1}L& g_{1}L\end{array}\right)=E$$

If $\mathrm{Rank}\left(L\right)=N-1,\mathrm{Rank}\left(\left[\begin{array}{cc}{I}_1& \\ & I_2\end{array}\right]\right)=r$, it can be seen that:$\mathrm{Rank}\left(T_l\right)=\mathrm{Rank}\left(E\right)=r+N-1$.

where $T_l$ has only zero eigenvalues. Therefore, we must select the parameters $k_1$,$k_2,k_3,k_4,g_1,\mathrm{and} g_2$ so that $T_l$ has zero eigenvalue, and all other eigenvalues have negative real parts. The parameters $k_1$,$k_2,k_3 \mathrm{and} k_4$ need to meet the stability of UAV consistency, and the parameters $g_1$ and $g_2$ need to meet the stability of UGV consistency. After determining the parameters, $T_l$ can be converted to a Jordan standard type: $T_l=PJ{P}^{-1}$. Let $v_1^T$ be the first row of $P^{-1}$ and the left eigenvector have eigenvalue 0. Let $w_1$ be the first column of $P$ and the right eigenvector with eigenvalue $0.$ Therefore, $v_1^T{w}_1=1$; when the time approaches infinity, the system’s state becomes: ${\lim }_{t\to \infty }^{ }\tilde{X}={\lim }_{t\to \infty }^{ }{e}^{T_{l}t}\tilde{X}\left(0\right)$, $e^{T_{l}t}\tilde{X}\left(0\right)\to \left(w_1{v}_1^T\right)\tilde{X}\left(0\right)\left(t\to \infty \right)$. According to Lemma 1 it is then seen that the system can reach asymptotic consensus in cases where time tends toward infinity. □

3.4. Heterogeneous Cooperative Formation Control

For UAV:

$${\tilde{u}}_{ia}=\alpha {{\displaystyle \sum }}_{j\in N_i}^{ }{a}_{ij}\left({\delta }_j-{\delta }_i\right)-\beta a_{ij}\left(v_j-v_i\right)-{\gamma }_1{\mathsf{\Omega }}_i-{\gamma }_2{\dot{\mathsf{\Omega }}}_i$$

(21)

For UGV:

$${\tilde{u}}_{ig}=\alpha {{\displaystyle \sum }}_{j\in N_i}^{ }{a}_{ij}\left({\delta }_j-{\delta }_i\right)-\beta a_{ij}\left(v_j-v_i\right)$$

(22)

$${\delta }_i={\left({\delta }_{ia}^T,{\delta }_{ig}^T\right)}^T, v_i={\left(v_{ia}^T,v_{ig}^T\right)}^T,{ \mathsf{\Omega }}_i={(g\theta ,-g\varphi ,0)}^T,\dot{\mathsf{\Omega }}={(g\dot{\theta },-g\dot{\varphi },0)}^T$$

In combination with the Laplace matrix, Protocols (21) and (22) are rewritten as:

$$\tilde{U}=H\cdot \tilde{X}$$

(23)

$$H=\left(\begin{array}{cc}\begin{array}{l}\alpha {{\displaystyle L}}_{AA}\otimes {{\displaystyle I}}_3\\ \alpha {{\displaystyle L}}_{GA}\otimes {{\displaystyle I}}_3\end{array}& \begin{array}{cc}\begin{array}{l}-\beta {{\displaystyle L}}_{AA}\otimes {{\displaystyle I}}_3\\ -\beta {{\displaystyle L}}_{GA}\otimes {{\displaystyle I}}_3\end{array}& \begin{array}{cc}\begin{array}{l}-{{\displaystyle \gamma }}_1{{\displaystyle I}}_l\otimes {{\displaystyle I}}_3\\ 0\end{array}& \begin{array}{cc}\begin{array}{l}{{\displaystyle -\gamma }}_2{{\displaystyle I}}_l\otimes {{\displaystyle I}}_3\\ 0\end{array}& \begin{array}{cc}\begin{array}{l}\alpha {{\displaystyle L}}_{AG}\otimes {{\displaystyle I}}_3\\ \alpha {{\displaystyle L}}_{GG}\otimes {{\displaystyle I}}_3\end{array}& \begin{array}{l}-\beta {{\displaystyle L}}_{AG}\otimes {{\displaystyle I}}_3\\ -\beta {{\displaystyle L}}_{GG}\otimes {{\displaystyle I}}_3\end{array}\end{array}\end{array}\end{array}\end{array}\end{array}\right)$$

Define the state-space form of the heterogeneous multi-agent formation:

$$\dot{\tilde{X}}=A\tilde{X}+B\tilde{U}$$

(24)

where $A=\left(\begin{array}{ll}{A}_A\hfill & 0\hfill \\ 0\hfill & A_G\hfill \end{array}\right), B=\left(\begin{array}{cc}{B}_A& 0\\ 0& B_G\end{array}\right),\tilde{X}={\left({\overline{X}}^T,{\overline{W}}^T\right)}^T.$

Theorem 3.

if Protocols (21) and (22) meet$\alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0,\beta \gg \alpha ,{\gamma }_1>\beta$,${\gamma }_2>\beta , \beta {\gamma }_1{\gamma }_2>{\beta }^2{\gamma }_2\alpha$, the heterogeneous system in (7) can be achieved, and (8) is defined in the formation, then formation control is realized.

Proof of Theorem 3.

Substitute Equation (23) into Equation (24) and obtain: $\dot{\tilde{X}}=-T_S\cdot \tilde{X}$

where $T_s=\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ \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& \alpha L_{AG}\otimes I_3& -\beta L_{AG}\otimes I_3\\ 0& 0& 0& 0& 0& 1\\ \alpha L_{GA}\otimes I_3& -\beta L_{GA}\otimes I_3& 0& 0& \alpha L_{GG}\otimes I_3& -\beta L_{GG}\otimes I_3\end{array}\right],$$I$ is the identity matrix and $\otimes$ is the Kronecker product.

Internal stability parameters ${\gamma }_1$ and ${\gamma }_2$ should guarantee the stability of a single UAV agent. Therefore, the UAV is written as:

$\left(\begin{array}{c}{\dot{P}}_A\\ {\dot{V}}_A\\ {\dot{\mathsf{\Omega }}}_A\\ {\ddot{\mathsf{\Omega }}}_A\end{array}\right)=\mathsf{\Gamma }\left(\begin{array}{c}{P}_A\\ V_A\\ {\mathsf{\Omega }}_A\\ {\dot{\mathsf{\Omega }}}_A\end{array}\right)$, where $\mathsf{\Gamma }=\left(\begin{array}{cccc}0& I& 0& 0\\ 0& 0& I& 0\\ 0& 0& 0& I\\ -\alpha I& -\beta I& -{\gamma }_{1}I& -{\gamma }_{2}I\end{array}\right)$.

The characteristic polynomial is: $\det \left(SI-\mathsf{\Gamma }\right)=\left|\begin{array}{cccc}S& -I& 0& 0\\ 0& S& -I& 0\\ 0& 0& S& -I\\ \alpha I& \beta I& {\gamma }_{1}I& S+{\gamma }_{2}I\end{array}\right|$

where $I$ is the identity matrix. For a single UAV:

$$\left|SI-\mathsf{\Gamma }\right|=s^4+{\gamma }_2{s}^3+{\gamma }_1{s}^2+\beta s+\alpha$$

According to the Routh–Hurwitz stability criterion:

$$\begin{array}{c}\alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0,{\gamma }_1{\gamma }_2>\beta ,{\gamma }_1{\gamma }_2\beta >{\beta }^2+{\gamma }_2^2\end{array}$$

According to the linear stability theorem, the parameter $\alpha ,\beta ,{\gamma }_1$, and ${\gamma }_2$ should be selected so that there is a zero eigenvalue and other eigenvalues have negative genuine part parameters. The parameters $\alpha$ and $\beta$ must meet the stability of UGV consistency, and the parameters ${\gamma }_1$ and ${\gamma }_2$ must meet the stability of UAV consistency. After selecting the parameters, $T_s$ can be converted to a Jordan standard type: $T_s=PJ{P}^{-1}$. Let $v_1^T$ be the first row of $P^{-1}$ and the left eigenvector have eigenvalue 0. Let $w_1 \mathrm{be}$ the first column of $P$ and the right eigenvector have eigenvalue $0.$ Therefore, $v_1^T{w}_1=1$; when the time approaches infinity, the system’s state becomes: $\underset{t\to \infty }{\lim }\tilde{X}=\underset{t\to \infty }{\lim }{e}^{T_{s}t}\tilde{X}\left(0\right)$, $e^{T_{s}t}\tilde{X}\left(0\right)\to \left(w_1{v}_1^T\right)\tilde{X}\left(0\right)\left(t\to \infty \right)$. According to Lemma 1, when the time approaches infinity, the system in (7) asymptotically agrees; that is, the system achieves cooperative formation.□

3.5. Heterogeneous Cooperative Optimal Formation Control

UAV systems and UGV systems are heterogeneous systems. Applying the optimal control law to the heterogeneous system can be expressed as:

$${\tilde{u}}_{ia}=k_1{{\displaystyle \sum }}_{j\in N_i}^{ }{a}_{ij}\left({\delta }_j-{\delta }_i\right)-k_2{a}_{ij}\left(v_j-v_i\right)-k_3{\Omega }_i-k_4{\dot{\Omega }}_i$$

(25)

$${\tilde{u}}_{ig}=g_1{{\displaystyle \sum }}_{j\in N_i}^{ }{a}_{ij}\left({\delta }_j-{\delta }_i\right)-g_2{a}_{ij}\left(v_j-v_i\right)$$

(26)

where $k_1,k_2,k_3$,and $k_4$ are derived from the matrix $K.g_1$ and $g_2$ are derived from the matrix $G$.

In combination with the Laplace matrix, Protocols $\left(25\right)$ and $\left(26\right)$ are rewritten as:

$$\tilde{U}=S\cdot \tilde{X}$$

(27)

$$S=\left(\begin{array}{llllll}{k}_1{L}_{AA}\otimes I_3\hfill & -k_2{L}_{AA}\otimes I_3\hfill & -k_3{I}_l\otimes I_3\hfill & -k_4{I}_l\otimes I_3\hfill & k_1{L}_{AG}\otimes I_3\hfill & -k_2{L}_{AG}\otimes I_3\hfill \\ g_1{L}_{GA}\otimes I_3\hfill & -g_2{L}_{GA}\otimes I_3\hfill & 0\hfill & 0\hfill & g_1{L}_{GG}\otimes I_3\hfill & -g_2{L}_{GG}\otimes I_3\hfill \end{array}\right)$$

Define the state space of heterogeneous multi-agent formation:

$$\dot{\tilde{X}}=A\tilde{X}+B\tilde{U}$$

(28)

where $A=\left(\begin{array}{cc}{A}_A& 0\\ 0& A_G\end{array}\right), B=\left(\begin{array}{cc}{B}_A& 0\\ 0& B_G\end{array}\right), \tilde{X}={\left({\overline{X}}^T,{\overline{W}}^T\right)}^T$

Theorem 4.

Heterogeneous systems can complete formation and optimize Performance Functions (13) and (14) if they use Protocols (25) and (26), respectively.

Proof of Theorem 4.

Each UAV and UGV is defined as a group of formation units, so their number is made the same, that is, L = K; the block Laplacian matrix of the system has the same number of rows and columns.

Then, the heterogeneous system in (7) becomes:

$$\dot{X}=\widehat{A}X+\widehat{B}U$$

(29)

where $\widehat{A}=\left(\begin{array}{cccccc}0& I& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\\ 0& 0& 0& 0& 0& 0\end{array}\right)$,$\widehat{B}=\left(\begin{array}{c}0----0\\ I----0\\ 0----0\\ 0----0\\ 0----0\\ 0----I\end{array}\right)$.

According to State Equation (29):

$$\left(\begin{array}{c}SI-\widehat{A}----B\end{array}\right)=\left(\begin{array}{cccccccc}S& -I& 0& 0& 0& 0& 0& 0\\ 0& S& 0& 0& 0& 0& I& 0\\ 0& 0& S& -I& 0& 0& 0& 0\\ 0& 0& 0& S& -I& 0& 0& 0\\ 0& 0& 0& 0& S& -I& 0& 0\\ 0& 0& 0& 0& 0& S& 0& I\end{array}\right)$$

It is easy to discover the eigenvalues of the matrix $\widehat{A}$: ${\lambda }_1={\lambda }_2=\cdots ={\lambda }_6=0$.

Verify the rank of the matrix $\left(\begin{array}{c}SI-\widehat{A}----B\end{array}\right)$ with the above eigenvalues. Let $S={\lambda }_1={\lambda }_2=\dots ={\lambda }_6=0$:

Rank $\left(\begin{array}{c}SI-\widehat{A}----B\end{array}\right)$ = Rank $\left(\begin{array}{cccccccc}0& -I& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& I& 0\\ 0& 0& 0& -I& 0& 0& 0& 0\\ 0& 0& 0& 0& -I& 0& 0& 0\\ 0& 0& 0& 0& 0& -I& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& I\end{array}\right)$= 6, according to the rank criterion of PBH, the system state space is controllable.

Let $\tilde{U}=\left(\begin{array}{cc}{U}_{11}& U_{12}\\ U_{21}& U_{22}\end{array}\right)$=$\left(\begin{array}{llllll}{k}_1{L}_{AA}\hfill & -k_2{L}_{AA}\hfill & -k_{3}I\hfill & -k_{4}I\hfill & k_1{L}_{AG}\hfill & -k_2{L}_{AG}\hfill \\ g_1{L}_{GA}\hfill & -g_2{L}_{GA}\hfill & 0\hfill & 0\hfill & g_1{L}_{GG}\hfill & -g_2{L}_{GG}\hfill \end{array}\right)$

Substituting $\tilde{U}=\left(\begin{array}{cc}{U}_{11}& U_{12}\\ U_{21}& U_{22}\end{array}\right)\cdot \tilde{X}$ into Equation (28):$\dot{\tilde{X}}=-T*\cdot \tilde{X}$

where

$$T*=\left[\begin{array}{llllll}0\hfill & I\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & I\hfill & 0\hfill & 0\hfill & 0\hfill \\ 0\hfill & 0\hfill & 0\hfill & I\hfill & 0\hfill & 0\hfill \\ k_1{L}_{AA}\hfill & -k_2{L}_{AA}\hfill & -k_{3}I\hfill & -k_{4}I\hfill & g_1{L}_{AG}\hfill & -g_2{L}_{AG}\hfill \\ 0\hfill & 0\hfill & 0\hfill & 0\hfill & 0\hfill & I\hfill \\ k_1{L}_{GA}\hfill & -k_2{L}_{GA}\hfill & 0\hfill & 0\hfill & g_1{L}_{GG}\hfill & -g_2{L}_{GG}\hfill \end{array}\right]$$

The $T*$ basic determinant change:

$$\left(\begin{array}{cccccc}\lambda +I& -I& 0& 0& 0& 0\\ I& \lambda & -I& 0& 0& 0\\ 2I& 0& \lambda -I& -I& 0& 0\\ C_1& C_2& k_{3}I& \lambda +k_{4}I& -g_1{L}_{AG}& g_2{L}_{AG}\\ I& 0& 0& 0& \lambda & -I\\ C_3& k_2{L}_{GA}-I& 0& 0& -g_1{L}_{GG}& g_2{L}_{GG}\end{array}\right)=\lambda I-\mathsf{\Lambda },$$

where

$$\mathsf{\Lambda }=\left(\begin{array}{cccccc}-I& I& 0& 0& 0& 0\\ -I& 0& I& 0& 0& 0\\ -2I& 0& I& I& 0& 0\\ -C_1& -C_2& -k_{3}I& -k_{4}I& g_1{L}_{AG}& -g_2{L}_{AG}\\ -I& 0& 0& 0& 0& I\\ -C_3& I-k_2{L}_{GA}& 0& 0& g_1{L}_{GG}& -g_2{L}_{GG}\end{array}\right)$$

$$C_1=I-k_1{L}_{AA}-g_2{L}_{AG}-k_2{L}_{AA}-k_{4}I.$$

$$C_2=k_2{L}_{AA}-k_{3}I-I$$

$$C_3=I-k_1{L}_{GA}-g_2{L}_{GG}$$

Now $\lambda I-T*\cong \lambda I-\mathsf{\Lambda }$, so matrix $T*$ is similar to Λ. Therefore, there is a nonsingular transformation matrix Q, making $\mathsf{\Lambda }=QT*Q^{-1}$,$\dot{\tilde{X}}=\mathsf{\Lambda }\cdot \tilde{X}; \mathsf{\Lambda }$ is the matrix for which the sum of each row is zero. Therefore, there is at least one zero eigenvalue. Elementary row and column transformation can be taken on $T*$:

$$T*=\left(\begin{array}{cccccc}I& 0& 0& 0& 0& 0\\ 0& I& 0& 0& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I\\ 0& 0& k_1{L}_{AA}& -k_2{L}_{AA}& g_1{L}_{AG}& -g_2{L}_{AG}\\ 0& 0& k_1{L}_{GA}& -k_2{L}_{GA}& g_1{L}_{GG}& -g_2{L}_{GG}\end{array}\right)=\left(\begin{array}{ccc}{I}_1& 0& 0\\ 0& 0& I_2\\ 0& kL& gL\end{array}\right)=E$$

If $\mathrm{Rank}\left(L\right)=N-1$,$\mathrm{Rank}\left(\left[\begin{array}{cc}{I}_1& \\ & I_2\end{array}\right]\right)=r$, it can be seen that:

$$\mathrm{Rank}\left(T^{*}\right)=\mathrm{Rank}\left(\mathsf{\Lambda }\right)=\mathrm{Rank}\left(E\right) =r +N-1$$

where the $\mathsf{\Lambda }$ and $T*$ have only zero eigenvalues. Therefore, we must select the parameters $k_1$,$k_2,k_3,k_4,g_1,\mathrm{and} g_2$ so that $T*$ has zero eigenvalue and all other eigenvalues have negative real parts. The parameters $k_1$,$k_2,k_3, \mathrm{and} k_4$ must meet the stability of UAV consistency, and the parameters $g_1$ and $g_2$ must meet the stability of UGV consistency. After determining the parameters, $T*$ can be converted to a Jordan standard type: $T*=PJ{P}^{-1}$. Let $v_1^T$ be the first row of $P^{-1}$ and the left eigenvector have eigenvalue 0. Let $w_1$ the first column of $P$ and the right eigenvector have eigenvalue $0.$ Therefore, $v_1^T{w}_1=1$; when the time approaches infinity, the system’s state becomes: $\underset{t\to \infty }{\lim }\tilde{X}=\underset{t\to \infty }{\lim }{e}^{T^{*}t}\tilde{X}\left(0\right)$, $e^{T^{*}t}\tilde{X}\left(0\right)\to \left(w_1{v}_1^T\right)\tilde{X}\left(0\right)\left(t\to \infty \right)$. According to Lemma 1, it is then seen that the system can reach asymptotic consensus in cases where time tends toward infinity.□

4. Simulations

The formation protocol, distributed optimal formation protocol, cooperative formation protocol, and cooperative optimal formation protocol individually designed in this paper are simulated and analyzed using Matlab2016a. The effectiveness of the designed control protocol is verified via simulation. To achieve a better cooperative formation task effect in the system, the speed values set by the UAV system and the UGV system are similar.

The initial state is as follows: the position of UAV1 is $\left(30,50,50\right)\mathrm{m}$, the position of UAV2 is $\left(90,30,50\right)\mathrm{m}$, and the position of UAV3 is $\left(60,30,15\right)\mathrm{m}$. The speed of UAV1 is $\left(1,1,-2\right)\mathrm{m}/\mathrm{s}$, the speed of UAV2 is $\left(1,-2,1\right)\mathrm{m}/\mathrm{s}$, and the speed of UAV3 is $\left(-2,1,1\right)\mathrm{m}/\mathrm{s}$. The attitude angle of UAV1 is ${(0,0,0)}^{\circ }$, the attitude angle of UAV2 is ${(0,0,0)}^{\circ }$, and the attitude angle of UAV3 is ${(0,0,0)}^{\circ }$

The rate of the attitude angle of UAV1 is ${(0,0,0)}^{\circ }/\mathrm{s}$, the rate of the attitude angle of UAV2 is ${(0,0,0)}^{\circ }/\mathrm{s}$, and the rate of the attitude angle of UAV3 is ${(0,0,0)}^{\circ }/\mathrm{s}$. The setting position of UAV1 is $\left(10,10,30\right)\mathrm{m}$, the setting position of UAV2 is $\left(0,5,30\right)\mathrm{m}$, and the setting position of UAV3 is $\left(0,15,30\right)\mathrm{m}$. The UAV and UGV systems have a set speed of $\left(1,0,0\right)\mathrm{m}/\mathrm{s}$. The position of UGV1 is $\left(90,50,0\right)\mathrm{m}$, the position of UGV2 is $\left(65,10,0\right)\mathrm{m}$, and the position of UGV3 is $\left(30,20,0\right)\mathrm{m}$. The speed of UGV1 is $\left(1,1,0\right)\mathrm{m}/\mathrm{s}$, the speed of UGV2 is $\left(2,-2,0\right)\mathrm{m}/\mathrm{s}$, and the speed of UGV3 is $\left(-1,1,0\right)\mathrm{m}/\mathrm{s}$. The setting position of UGV1 is $\left(10,10,0\right)\mathrm{m}$, the setting position of UGV2 is $\left(0,5,0\right)\mathrm{m}$, and the setting position of UGV3 is $\left(0,15,0\right)\mathrm{m}$.

The parameters are $\mathsf{\alpha }=0.2,\beta =1.5,{\gamma }_1=5,{\gamma }_2=2,k_1=2.3452,k_2=6.4707,k_3=7.7541,k_4=4.5835,g_1=0.4472,\mathrm{and} g_2=1.0461$.

Simulations of the UAV system and the UGV system using formation control and distributed optimal formation control are shown in Figure 1 and Figure 2. By comparing the two figures in Figure 1 and Figure 2, it can be found that both protocols can be used to complete triangular formation at the same time, but when distributed optimal formation control is used, it can be observed that the error between the actual position and the set value is significantly reduced in a short period of time.

When using the formation control protocol, with parameters $\alpha =0.2,\beta =1.5,{\gamma }_1=5,\mathrm{and} {\gamma }_2=2$. Figure 1 shows the change of position coordinates of the multi-agent system and shows the actual formation position of the UAV and UGV when $t=10 \mathrm{s}$. It can be seen that there is a significant error with the set value.

When using the distributed optimal formation control protocol, with parameters $k_1=2.3452,k_2=6.4707,k_3=7.7541, k_4=4.5835,g_1=0.4472,\mathrm{and} g_2=1.0461$. Figure 2 shows the position coordinate changes of the system and shows the actual formation status of the UAV and UGV when $t=10$ s. It can be observed that the system can quickly complete the triangular formation and that the error between the actual position and the set value is small.

It can be seen from Figure 3 and Figure 4 that the state of each system variable changes with time when the formation control protocol is used. It can be seen from the figures that the state of each variable in the system is stable from 40 s to 50 s.

From Figure 5 and Figure 6, it can be seen that the system uses the distributed optimal formation control protocol to change the state of each variable with time. It is observed that the system reaches stability between 30 and 40 s.

The simulation of cooperative formation protocol and cooperative optimal formation protocol of heterogeneous multi-agents is shown in Figure 7 and Figure 8.

When using the cooperative formation control protocol, with parameters $\alpha =0.2,\beta =1.5,{\gamma }_1=5,\mathrm{and} {\gamma }_2=2$. Figure 7 shows the change of position coordinates of the system and the expected formation state of the system at the 10th second. It can be seen that the actual formation position of the system has a significant error with the set value.

When using the cooperative optimal formation control protocol, with parameter $k_1=2.3452,k_2=6.4707,k_3=7.7541,k_4=4.5835,g_1=0.4472,\mathrm{and} g_2=1.0461$.

Figure 8 shows the change in position coordinates of the system and the expected formation state of the system at t=10 s. It can be observed that the system can quickly complete the triangular formation and that the error between the actual position and the set value is small. In addition, the cooperative optimal formation protocol speeds up the convergence rate of the system, which is of great help to the formation time of the system.

When using the formation control and the distributed optimal formation control, the formation states of the UAV system at different times are shown in Figure 9 and Figure 10.

Figure 9 shows the formation status of the UAV system at different times under the use of the formation control protocol. It can be observed that the system can complete the triangle formation but also that there is a relative reach error with the set position in the formation completion process and that there is still an error at the fiftieth second.

Figure 10 shows the formation status of the UAV system at different times under the use of the distributed optimal formation control protocol. It can be observed that the system can quickly complete the triangle formation and that the error between the actual position and the set value is very small at the thirtieth second.

When using the formation control and the distributed optimal formation control, the formation states of the UGV system at different times are shown in Figure 11 and Figure 12.

Figure 11 shows the formation status of the UGV system at different moments when the formation control protocol is used. It can be observed that the UGV system can complete the triangle formation, but there is still a certain error with the set value in the formation completion process. At the thirtieth second, the difference between the actual position and the set value gradually decreases.

Figure 12 shows the change in the formation shape of the UGV system at different moments when the distributed optimal formation control protocol is used; it can be observed that the UGV system can quickly complete the triangle formation and that the error between the actual position and the set value is small.

When using the heterogeneous cooperative formation control and the heterogeneous cooperative optimal formation control, the formation states of the UAV system at different times are shown in Figure 13 and Figure 14.

Figure 13 shows the formation status of the UAV system at different times under the use of the heterogeneous cooperative formation control protocol. It is observed that the system can complete the triangle formation. At the thirtieth second, the difference between the actual position and the set value gradually decreases.

Figure 14 shows the formation status of the UAV system at different times under the use of the heterogeneous cooperative optimal formation control. It can be observed that the system can quickly complete the triangle formation and that the error between the actual position and the set value is very small at the tenth second.

When using the heterogeneous cooperative formation control and the heterogeneous cooperative optimal formation control, the formation states of the UGV system at different times are shown in Figure 15 and Figure 16.

Figure 15 shows the formation status of the UGV system at different times under the use of the heterogeneous cooperative formation control protocol. It can be observed that the system can complete the triangle formation. At the thirtieth second, the difference between the actual position and the set value gradually decreases.

Figure 16 shows the formation status of the UGV system at different times under the use of the heterogeneous cooperative optimal formation control. It can be observed that the system can quickly complete the triangle formation and that the error between the actual position and the set value is very small at the tenth second.

In order to fully verify the theoretical results, the structure is complicated, and the communication Laplacian matrix is as follows:

$$L=\left(\begin{array}{ccccccccc}-3& 1& 1& 0& 0& 1& 0& 0& 0\\ 1& -3& 1& 0& 1& 0& 0& 0& 0\\ 1& 1& -3& 0& 0& 1& 0& 0& 0\\ 0& 0& 1& -3& 1& 1& 0& 0& 0\\ 0& 1& 0& 1& -3& 1& 0& 0& 0\\ 1& 0& 0& 1& 1& -3& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& -3& 1& 1\\ 0& 0& 0& 0& 1& 0& 1& -3& 1\\ 0& 0& 0& 1& 0& 0& 1& 1& -3\end{array}\right)$$

The added set of UAV states is as follows: the position of UAV4 is $\left(40,50,10\right)\mathrm{m}$, the position of UAV5 is $\left(60,30,20\right)\mathrm{m}$, and the position of UAV $6 \mathrm{is} \left(30,20,10\right)\mathrm{m}$. The speed of UAV4$\mathrm{is} \left(1,1,1\right)\mathrm{m}/\mathrm{s}$, the speed of UAV5$\mathrm{is} \left(2,2,1\right)\mathrm{m}/\mathrm{s}$, and the speed of UAV6 is $\left(3,1,1\right)\mathrm{m}/\mathrm{s}$.The attitude angle of UAV4 is ${(0,0,0)}^{\circ }$, the attitude angle of UAV5$\mathrm{is} {(0,0,0)}^{\circ }$, and the attitude angle of UAV6 is ${(0,0,0)}^{\circ }$. The setting position of UAV4 is $\left(10,10,10\right)\mathrm{m}$, the setting position of UAV5 is $\left(0,5,10\right)\mathrm{m}$, and the setting position of UAV6 is $\left(0,15,10\right)\mathrm{m}$. The UAV systems have a set speed of $\left(1,0,0\right)\mathrm{m}/\mathrm{s}$. The simulation results are shown in Figure 17, Figure 18, Figure 19 and Figure 20.

The experiments have verified the formation control and distributed optimization formation control, as shown in Figure 17 and Figure 18, respectively. By comparing Figure 17 and Figure 18, it can be found that both protocols can be used to complete triangular formation at the same time, but when distributed optimal formation control is used, it can be observed that the error between the actual position and the set value is significantly reduced in a short period of time.

When using the formation control protocol, with parameters $\alpha =0.2,\beta =1.5,{\gamma }_1=5,{\gamma }_2=2$, Figure 17 shows the change of position coordinates of the complex system and the expected formation state of the complex system. However, it can be seen that there is a significant error with the set value.

When using the distributed optimal formation control protocol, with parameters $k_1=2.3452,k_2=6.4707,k_3=7.7541, k_4=4.5835,g_1=0.4472,\mathrm{and} g_2=1.0461$. Figure 18 shows the change of position coordinates of the complex system and the expected formation state of the complex system. It can be observed that the complex system can quickly complete the triangular formation and that the error between the actual position and the set value is small.

The experiments have verified the cooperative formation control and the cooperative optimal formation control, as shown in Figure 19 and Figure 20, respectively.

When using the cooperative formation control protocol, with parameters $\alpha =0.2,\beta =1.5,{\gamma }_1=5,\mathrm{and} {\gamma }_2=2$, Figure 19 shows the change in position coordinates of the complex system and the expected formation state of the complex system. However, it can be seen that there is a significant error with the set value.

When using the cooperative optimal formation control protocol, with parameters $k_1=2.3452,k_2=6.4707,k_3=7.7541, k_4=4.5835,g_1=0.4472,\mathrm{and} g_2=1.0461$, Figure 20 shows the change in position coordinates of the complex system and the expected formation state of the complex system. It can be observed that the complex system can quickly complete the triangular formation and that the error between the actual position and the set value is small. In addition, the cooperative optimal formation protocol speeds up the convergence rate of the complex system, which is of great help to the formation time of the complex system.

5. Conclusions

In this paper, a heterogeneous multi-agent system has been established by analyzing the dynamics model of the unmanned ground vehicle and the unmanned aerial vehicle. Firstly, the formation control protocol is proposed based on the communication topology of a multi-agent system. Then, according to the internal state of a single agent, the optimal control law of a single agent system is designed using the optimal control theory, and the optimal control law is introduced into the system to achieve the distributed optimal formation. Finally, based on the cooperative architecture of the heterogeneous multi-agent system, the cooperative formation design of the heterogeneous multi-agent system is carried out, and the optimal control theory is introduced into the heterogeneous multi-agent system to realize the optimal cooperative formation of the heterogeneous system. The stability of the system is further analyzed by graph theory. The communication topology of the multi-agent system does not interfere with the protocol and the protocol can optimize the performance function while the system completes the formation task. The simulation results show that the optimal control can accelerate the convergence speed of the system and greatly help the system to quickly reach the desired formation state. In the next step, we plan to investigate the anomaly detection and recognition problems under heterogeneous multi-agent cooperation architecture, and we plan to apply the theoretical research results in practice to engineering applications.