Formation Control for Mixed-Order UAVs–USVs–UUVs Systems under Cooperative and Optimal Control

Abstract

In this paper, cooperative control and optimal control methods are used for the formation control of mixed-order heterogeneous multi-agent systems. The system consists of unmanned aerial vehicles (UAVs), unmanned surface vehicles (USVs), and unmanned underwater vehicles (UUVs). The system is represented in a state space using a block Kronecker product. The static and dynamic formation control protocols are proposed respectively, and the graph theory is used to prove that formation control protocols can realize system formation. Furthermore, the optimal control and cooperative control are introduced into the static and the dynamic formation control protocols, and the static cooperative optimal formation control protocol and the dynamic cooperative optimal formation control protocol are designed. Through MATLAB simulation, the static cooperative optimal control protocol and static formation control protocol are compared, and the dynamic cooperative optimal control protocol and dynamic formation control protocol are compared. By comparison, the state variables of the system can reach convergence quickly, and the system can complete formation in a short time, which verifies the effectiveness of the optimal theory and cooperative control.

1. Introduction

Multi-agent systems (MASs) are widely used in rescue [1,2,3], reconnaissance [4,5,6], exploration [7,8,9], and other fields. They can cooperate to complete complex tasks that a single agent cannot complete.

In recent years, formation control of multi-agent systems has become a scorching research field, and formation control has also been widely applied in many engineering fields, such as robot field [10,11,12], attitude control of multi-satellite systems [13,14], and flight control of UAVs [15,16]. By using an appropriate formation control method, the formation control challenge is to accomplish some difficult global activities. Many formation control methods and techniques have been studied. For example, paper [17] uses A* algorithm combined with an optimization algorithm for formation to realize a collision-free path. In the literature [18], multiple mobile robots adopt the leader-following method for formation control. This paper [19] uses the method of minimizing the Kullback–Leibler divergence to achieve the desired formation of multiple UAVs. To reduce the communication burden of multiple UAVs, this paper [20] designs a distributed frame structure, the control protocol includes event triggering, and the system completes the distributed formation. However, the above results for formation control are applied to isomorphic multi-agent systems. In practical applications, there are many heterogeneous multi-agent systems (HMASs) with different structures, dynamic models, and even information perception and decision-making abilities. Article [21] studies the formation problem of a class of general first/second-order discrete-time heterogeneous multi-agent systems. In article [22], agents from the same group build a time-varying formation in their own dimension, while agents from different groups move cooperatively in different dimensions to accomplish the cross-dimensional formation of heterogeneous multi-agent systems.

When applied in practice, how can the system quickly complete the task? This will consider optimization problems such as the following: In paper [23], distributed optimal control is proposed to optimize the trajectory of many unicycle robots. The optimal collaborative control of linear multi-agent systems is investigated in this paper [24], and the proposed controller can minimize the quadratic global cost function to a specific optimal value, with the optimal solution being independent of uncertainty. In the paper [25], multi-agent systems’ leader–follower formation control problem adopts a stress matrix, which has better formation flexibility. The inverse optimal control theory is used to demonstrate the effectiveness of distributed control protocols in paper [26].

Due to the collaboration of multi-agent systems that should be are autonomous, fault-tolerant, coordinated, adaptable, and scalable, they may also involve a variety of other fields of research, including resource exploration [27,28], target positioning [29,30], environmental monitoring [31,32], and military exercise [33,34]. So far, the collaborative control problem of MASs has attracted wide attention from researchers in physics, autonomous vehicles, industrial engineering, biology, machine intelligence, and other fields. However, there are challenges in collaborative control between multiple agents with different dynamics.

Based on the above results, the literature [21,22] only studies the formation problem of heterogeneous systems, without considering the optimization of system formation. Literature [23,24,25,26] considers the flexibility and optimization of formation based on the same dynamic model. In this paper, we discuss the formation problem of mixed-order multi-agent systems by combining the optimal control theory and cooperative control and solve the formation optimization problem of systems with different dynamics models. At the same time, the system can achieve coordination. The main contributions are as follows:

Firstly, the dynamic model of each agent system is introduced, and the different dynamic model systems are written into a state space by using a block Kronecker product. Secondly, static formation control protocols and dynamic formation control protocols are designed, respectively, and graph theory is used to prove that the control protocols can complete formation. Furthermore, the optimal control law of each agent system is designed by using the optimal control theory, and the problem of dimensionality inconsistency is solved by using cooperative control. The optimal control theory and cooperative control are introduced into the static and dynamic formation control protocols, and the static and dynamic cooperative optimal formation control protocols are designed. Finally, the collaborative optimal formation control protocol and the formation control protocol are compared by simulation, and the system state variables can rapidly converge and realize the cooperative formation, which verifies the effectiveness of the optimal control and the cooperative control.

The rest of this paper is organized as the following. The preparatory knowledge and system model are introduced in Section 2. Section 3 introduces the design of the control protocol. Then in Section 4, simulation experiments verify the effectiveness of the proposed control protocol. Section 5 summarizes the main contents of this article.

2. Preliminaries

2.1. Graph Theory

A graph $G=\left(V,E,A\right)$ is used to represent the topology of the information exchange between agents, where $V=\left(v_1,v_2,\cdots v_n\right)$ is the set of agent nodes, and each node represents an agent. $E\subseteq \left\{\left(i,j\right):i,j\in V\right\}$ is the set of edges about $e_{ij}$, indicating that there is information exchange between agent $i$ and $j$, and the information is from $i$ to $j$. A weighted adjacency matrix $A={\left[a_{ij}\right]}_{n\times n}$ with nonnegative adjacency elements $a_{ij}$, where $a_{ij}$ is the weight of the edge $e_{ij}=\left(v_i,v_j\right)$. For $i,j=\mathrm{1,2},3,\cdots ,n(i\ne j)$, if the agents $v_i$ and $v_j$ can receive information from each other, then the elements in the adjacency matrix are $a_{ij}>0$; otherwise, the element in the adjacency matrix is $0.$ To simplify the calculation, let $a_{ij}\in \left\{\mathrm{0,1}\right\}$. Note that $a_{ij}=a_{ji}$ in the undirected graph, and $a_{ij}\ne a_{ji}$ in the directed graph.

The set of neighbor node $N_i=\left\{j|j\in V:e_{ij}\in E\right\}$, which represents the set composed of all agents that have information exchange with agent 𝑖. In the graph, the degree represents the number of neighbors of a node, that is, the number of edges per node. The node degree is defined as $d_i=\sum _{j=1}^n{a}_{ij}$, and the degree matrix $D$ of the graph is diagonal, as follows: $D=diag(d_1,d_2,\cdots d_n)$. The Laplace matrix L of a multi-agent system is defined as: $L=D-A$.

2.2. Formation Definition

The problem of formation control is to find a control protocol that allows multiple intelligences to form formations or configurations. The desired vector of the system formation is:

$$X_d={\left[d\left(P_A\right)d\left(V_A\right)d\left({\mathsf{\Omega }}_A\right)d\left({\dot{\mathsf{\Omega }}}_A\right)d\left(P_S\right)d\left(V_S\right)d\left(P_U\right)d\left(V_U\right)\right]}^T.$$

The formation error vector can be expressed as:

$$\tilde{X}=X-X_d={\left[{\tilde{P}}_A{\tilde{V}}_A{\tilde{\mathsf{\Omega }}}_A{\widetilde{\dot{\mathsf{\Omega }}}}_A\widetilde{P_S}\widetilde{V_S}{\tilde{P}}_U{\tilde{V}}_U\right]}^T$$

By introducing the error vector, the formation problem is transformed into the error consistency problem about the state variables. When the system error vector reaches consistency, it means that the formation is realized.

 Definition 1.

When all the states in the system meet the definition of Formula (1), it indicates that the system realizes static formation control.

$$\left\{\begin{array}{c}\begin{array}{c}\underset{t\to \infty }{\lim }‖P_j-P_i‖=P_{dj}-P_{di} i,j=\mathrm{1,2},3,\cdots ,l\\ \underset{t\to \infty }{\lim }‖v_i‖=0 i=\mathrm{1,2},3,\cdots ,l\end{array}\\ \underset{t\to \infty }{\lim }‖{\mathsf{\Omega }}_i‖=0 i=\mathrm{1,2},3,\cdots ,m\\ \underset{t\to \infty }{\lim }‖{\dot{\mathsf{\Omega }}}_i‖=0 i=\mathrm{1,2},3,\cdots ,m\end{array}\right.$$

(1)

 Definition 2.

When all the states in the system meet the definition of Formula (2), it indicates that the system realizes dynamic formation control.

$$\left\{\begin{array}{c}\underset{t\to \infty }{\lim }‖P_j-P_i‖=P_{dj}-P_{di} i,j=\mathrm{1,2},3,\cdots ,l\\ \underset{t\to \infty }{\lim }‖v_j-v_i‖=0 i,j=\mathrm{1,2},3,\cdots ,l\\ \begin{array}{c}\underset{t\to \infty }{\lim }‖{\mathsf{\Omega }}_i‖=0 i=\mathrm{1,2},3,\cdots ,m\\ \underset{t\to \infty }{\lim }‖{\dot{\mathsf{\Omega }}}_i‖=0 i=\mathrm{1,2},3,\cdots ,m\end{array}\end{array}\right.$$

(2)

2.3. High-Order UAV Dynamics Model

For the dynamics model of the UAV, refer to the literature [35]. Referring to Figure 1, unmanned aerial vehicle attitude angle includes: roll angle $\varphi$, pitching angle $\theta$ and yaw angle $\phi$. The roll angle refers to the rotation angle along the $X_b$ axis, the pitch angle refers to the rotation angle along the $Y_b$ axis, the yaw angle refers to the rotation angle along the $Z_b$ axis. $M_1, M_2, M_3$ and $M_4$ are the torques of the four propellers due to rotation, $F_1, F_2, F_3$ and $F_4$ are the lift forces generated by the four propellers. $I_x, I_y$, and $I_Z$ denote the rotational inertia along the $X_b$, $Y_b$ and $Z_b$ axes, respectively. Combined with the literature [36], the drag coefficients are neglected in this paper, the roll angle and pitch angle only have small variations, and the yaw angle has no variation, that is, $sin\varphi \approx \varphi ,sin\theta \approx \theta$, $cos\varphi \approx 1$, $cos\theta \approx 1$, $\phi =0$, $sin\phi =0$, $cos\phi =1$. The dynamics model of the UAV is simplified and the dynamics model is represented as follows:

$$\left\{\begin{array}{l}{\stackrel{.}{p}}_a^x=g\theta \\ {\stackrel{.}{p}}_a^y=-g\varphi \\ {\stackrel{.}{p}}_a^z=f_z/m-g\\ \stackrel{.}{\varphi }=M_{\varphi }/I_x\\ \stackrel{.}{\theta }=M_{\theta }/I_y\\ \stackrel{.}{\phi }=M_{\phi }/I_z\end{array}\right.$$

(3)

where $p_a^x$, $p_a^y$, $p_a^z$ represent position state, $\varphi , \theta ,$ and $\phi$ represent the attitude state, $f_z$ is the lift force in the height direction, $M_{\varphi }, M_{\theta },$ and $M_{\phi }$ represent moments of the quadrotor, $I_x, I_y$, and $I_Z$ represent inertial moments of the quadrotor.

According to Equation (3), the Equation of state is:

$${\dot{X}}_{A1}=A_A{X}_{A1}+B_A{U}_{A1}$$

(4)

The subscript a represents the UAV state variables, where

$X_{A1}=\left(P_{A1},V_{A1},{\mathsf{\Omega }}_{A1},{\dot{\mathsf{\Omega }}}_{A1}\right)$, $P_{A1}={\left(p_{ai}^x,p_{ai}^y,p_{ai}^z\right)}^T$,

$V_{A1}={\left(v_{ai}^x,v_{ai}^y,v_{ai}^z\right)}^T,{\mathsf{\Omega }}_{A1}={\left(g{\theta }_{ai},-g{\varphi }_{ai},0\right)}^T$,

${\dot{\mathsf{\Omega }}}_{A1}={\left(g{\dot{\theta }}_{ai},-g{\dot{\varphi }}_{ai},0\right)}^T$, $U_{A1}={\left(u_{ai}^x,u_{ai}^y,u_{ai}^z\right)}^T$,

$$A_A=\left(\begin{array}{cccc}{0}_{3\times 3}& I_3& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_3& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& I_3\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right){,B}_A=\left(\begin{array}{c}{0}_{3\times 3}\\ 0_{3\times 3}\\ 0_{3\times 3}\\ I_3\end{array}\right),I_3=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)$$

2.4. Second-Order USV Dynamics Model

The details dynamic equations of USV can be found in [37]. In this article, the USV is limited to the water surface for cross-media relay communication, and the agents taken into consideration is the moving in a planar environment; systems are characterized by a second-order model.

$$\left\{\begin{array}{l}{\dot{p}}_{si}=v_{si}\\ {\dot{v}}_{si}=u_{si}\end{array}\right.$$

(5)

where $p_{si}={\left[p_{si}^x,p_{si}^y\right]}^T$ represents the position state, $v_{si}={\left[v_{si}^x,v_{si}^y\right]}^T$ represents the velocity in the direction of $p_{si}$, and $u_{si}$ represents the input of agent $i$.

According to Equation (5), the Equation of state is:

$${\dot{X}}_{S1}=A_S{X}_{S1}+B_S{U}_{S1}$$

(6)

The subscript s represents the USV state variables, where

$X_{S1}=\left(P_{S1},V_{S1}\right)$, $P_{S1}={\left(p_{si}^x,p_{si}^y\right)}^T$

$V_{S1}={\left(v_{si}^x,v_{si}^y\right)}^T$, $U_{S1}={\left(u_{si}^x,u_{si}^y\right)}^T$

$A_S=\left[\begin{array}{cc}{0}_{2\times 2}& I_2\\ 0_{2\times 2}& 0_{2\times 2}\end{array}\right]$, $B_S=\left[\begin{array}{c}{0}_{2\times 2}\\ I_2\end{array}\right]$, $I_2=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)$.

2.5. Second-Order UUV Dynamics Model

The details kinematic and dynamic equations of UUV can be found in [38]. According to the structure of the UUV studied in the paper, there is no thruster to control the angular velocity in roll, the rolling has little influence on the translational motion, and the dynamics of the actuators and thrusters are reasonably neglected in this paper. It is assumed that the UUV is torpedo-type, such that the system is characterized by a second-order model [39].

To simplify the problem, we present dynamic systems as follows:

$$\left\{\begin{array}{l}{\dot{p}}_{ui}=v_{ui}\\ {\dot{v}}_{ui}=u_{ui}\end{array}\right.$$

(7)

Refer to Figure 2, where $p_{vi}={⌈p_{ui}^x,p_{ui}^y,p_{ui}^z⌉}^T$ represents the position state, $v_{vi}={\left[v_{ui}^x,v_{ui}^y,v_{ui}^z\right]}^T$ represents the velocity in the direction of $p_{ui}$, and $u_{ui}$ represents the input of agent $i$.

According to Equation (7), the Equation of state is:

$${\dot{X}}_{U1}=A_U{X}_{U1}+B_U{U}_{U1}$$

(8)

The subscript u represents the UUV state variables, where

$X_{U1}=\left(P_{U1},V_{U1}\right)$, $P_{U1}={\left(p_{ui}^x,p_{ui}^y,p_{ui}^z\right)}^T$,

$V_{U1}={\left(v_{ui}^x,v_{ui}^y,v_{ui}^z\right)}^T$, $U_{U1}={\left(u_{ui}^x,u_{ui}^y,u_{ui}^z\right)}^T$

$A_U=\left[\begin{array}{cc}{0}_{3\times 3}& I_3\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$, $B_U=\left[\begin{array}{c}{0}_{3\times 3}\\ I_3\end{array}\right]$, $I_3=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right)$.

2.6. Heterogeneous Multi-Agent System

Write the UAV system, UUV system, and USV system as a state-space model:

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

(9)

where $X=[P_A,V_A,{\mathsf{\Omega }}_A,{\dot{\mathsf{\Omega }}}_A,P_S,V_S,P_U,V_U]$

$P_A=[P_1,P_2,P_3,\cdots P_m]$, $p_i={[x_i,y_i,z_i]}^T$, $i=\mathrm{1,2},\cdots ,m$

$V_A=[v_1,v_2,v_3,\cdots v_m]$, $v_i={[v_i^x,v_i^y,v_i^z]}^T$, $i=\mathrm{1,2},\cdots ,m$

${\mathsf{\Omega }}_A=[{\mathsf{\Omega }}_1,{\mathsf{\Omega }}_2,{\mathsf{\Omega }}_3,\cdots {\mathsf{\Omega }}_m]$, ${\mathsf{\Omega }}_i={[g{\theta }_i,-g{\varphi }_i,0]}^T$, $i=\mathrm{1,2},\cdots ,m$

${\dot{\mathsf{\Omega }}}_A=[{\dot{\mathsf{\Omega }}}_1,{\dot{\mathsf{\Omega }}}_2,{\dot{\mathsf{\Omega }}}_3,\cdots {\dot{\mathsf{\Omega }}}_m]$, ${\dot{\mathsf{\Omega }}}_i={[g{\dot{\theta }}_i,-g{\dot{\varphi }}_i,0]}^T$, $i=\mathrm{1,2},\cdots ,m$

$P_S=[P_{m+1},\cdots P_k]$, $p_i={[x_i,y_i]}^T$, $i=m+1,\cdots ,k$

$V_s=[v_{m+1},\cdots v_k]$, $v_i={[v_i^x,v_i^y]}^T$, $i=m+1,\cdots ,k$

$P_U=[P_{k+1},\cdots P_l]$, $p_i={[x_i,y_i,z_i]}^T$, $i=k+1,\cdots ,l$

$V_U=[v_{k+1},\cdots v_l]$, $v_i={[v_i^x,v_i^y,v_i^z]}^T$, $i=k+1,\cdots ,l$.

The UAV, UUV, and USV are grouped into a group, and the formation can be extended to multiple pairs, expressed as: $2 \mathrm{m}=\mathrm{k},3 \mathrm{m}=l$.

Where matrix A is:

$$A=\left(\begin{array}{cccccccc}{{\displaystyle 0}}_{3\times 3}& {{\displaystyle I}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle I}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle I}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 2}& {{\displaystyle I}}_{2\times 2}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}\\ {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 2}& {{\displaystyle 0}}_{2\times 2}& {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle I}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 3}\end{array}\right)\otimes {{\displaystyle I}}_m$$

Matrix B is:

$$B=\left(\begin{array}{ccc}{{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle I}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{2\times 3}& {{\displaystyle 0}}_{2\times 2}& {{\displaystyle 0}}_{2\times 3}\\ {{\displaystyle 0}}_{2\times 3}& {{\displaystyle I}}_{2\times 2}& {{\displaystyle 0}}_{2\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle 0}}_{3\times 3}\\ {{\displaystyle 0}}_{3\times 3}& {{\displaystyle 0}}_{3\times 2}& {{\displaystyle I}}_{3\times 3}\end{array}\right)\otimes {{\displaystyle I}}_m$$

Input U is:

$$U=\left(\begin{array}{c}{U}_{A1}\\ U_{S1}\\ U_{U1}\end{array}\right)$$

$I_m$ represent an m-dimensional identity matrix, $\otimes$ is Kronecker product.

3. Design of Control Protocol

3.1. Formation Control Protocol

When the system formation reaches the desired state, the expected control input is 0, and the formation state equation of system (9) can be expressed as:

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

(10)

Based on literature [40], a static formation control protocol is proposed:

$$\left\{\begin{array}{c}{u}_{ia}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)-\beta {\tilde{v}}_i-{\gamma }_1{\mathsf{\Omega }}_i-{\gamma }_2{\dot{\mathsf{\Omega }}}_i i=\mathrm{1,2},3,\cdots ,m\\ u_{is}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)-\beta {\tilde{v}}_i i=m+1,\cdots ,k\\ u_{iu}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)-\beta {\tilde{v}}_i i=k+1,\cdots ,l\end{array}\right.$$

(11)

Dynamic formation control protocol is proposed:

$$\left\{\begin{array}{c}{u}_{ia}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\beta {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right)-{\gamma }_1{\mathsf{\Omega }}_i-{\gamma }_2{\dot{\mathsf{\Omega }}}_i i=\mathrm{1,2},3,\cdots ,m\\ u_{is}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\beta {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right) i=m+1,\cdots ,k\\ u_{iu}=\alpha {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\beta {\displaystyle \sum _{j=1}^l}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right) i=k+1,\cdots ,l\end{array}\right.$$

(12)

 Lemma 1

[41]. 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 $P_l={\left[P_{l1}{P}_{l2}\dots P_{ln}\right]}^T\ge 0$ and $1_n^T{P}_l=1,L^T{P}_l=0$, where, $t\to \infty$, $e^{-Lt}\to 1_n{P}_l^T$.

 Theorem 1.

For the formation equation of state (8), when the communication topology 𝐺 = (𝑉 𝐸, 𝐴) is connected undirected graph or directed graph containing spanning tree, if the gain parameter satisfies the conditions:$\alpha >0$, $\beta >0$, ${\gamma }_1>0$, ${\gamma }_2>0$, $\beta \gg \alpha$, ${\gamma }_1>\beta \gg \alpha$, ${\gamma }_2>\beta \gg \alpha$, ${\gamma }_2{\gamma }_1\beta >{\beta }^2+{{\gamma }_2}^2\alpha$, using static formation control protocol can achieve formation and the system state variables to achieve convergence.

 Proof of Theorem 1.

To explain the system (9) communication relationship, Laplacian matrix relations are described as follows: $\mathrm{L}=\left(\begin{array}{ccc}\begin{array}{c}{L}_{AA}\\ L_{SA}\\ 0\end{array}& \begin{array}{c}{L}_{AS}\\ L_{SS}\\ L_{US}\end{array}& \begin{array}{c}0\\ L_{SU}\\ L_{UU}\end{array}\end{array}\right)$.

Where, $L_{ii}$ represents the Laplacian matrix relationship between isomorphic agents, and the Laplacian matrix $L_{ij}$ represents the ith agent system pointing at the jth agent system, namely, the communication connection of heterogeneous agent system.

The static formation control protocol is written in a unified form:

$$\mathrm{U}=T_d\times \tilde{X}$$

where

$$T_d=\left(\begin{array}{ccc}\begin{array}{c}\alpha L_{AA}\otimes I_3\\ \alpha L_{SA}\otimes T_2\\ 0\end{array}& \begin{array}{c}-\beta I_m\otimes I_3\\ 0\\ 0\end{array}& \begin{array}{ccc}\begin{array}{c}-{\gamma }_1{I}_m\otimes I_3\\ 0\\ 0\end{array}& \begin{array}{c}-{\gamma }_2{I}_m\otimes I_3\\ 0\\ 0\end{array}& \begin{array}{ccc}\begin{array}{c}\alpha L_{AS}\otimes T_1\\ \alpha L_{SS}\otimes I_2\\ \alpha L_{US}\otimes T_1\end{array}& -\begin{array}{c}0\\ \beta I_m\otimes I_2\\ 0\end{array}& \begin{array}{cc}\begin{array}{c}0\\ \alpha L_{SU}\otimes T_2\\ \alpha L_{UU}\otimes I_3\end{array}& \begin{array}{c}0\\ 0\\ -\beta I_m\otimes I_3\end{array}\end{array}\end{array}\end{array}\end{array}\right)$$

$$T_1=\left(\begin{array}{cc}\begin{array}{c}1\\ \begin{array}{c}0\\ 0\end{array}\end{array}& \begin{array}{c}0\\ \begin{array}{c}1\\ 0\end{array}\end{array}\end{array}\right),T_2=\left(\begin{array}{cc}\begin{array}{cc}\begin{array}{c}1\\ 0\end{array}& \begin{array}{c}0\\ 1\end{array}\end{array}& \begin{array}{c}0\\ 0\end{array}\end{array}\right)$$

Substituting $\mathrm{U}$ into Equation (10) yields: $\dot{\tilde{X}}=A\tilde{X}+BU=A\tilde{X}+B{T}_d\tilde{X}=T\tilde{X}$

where

$$T=\left(\begin{array}{ccc}\begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ \alpha L_{AA}\otimes I_3\\ \begin{array}{c}0\\ \alpha L_{SA}\otimes T_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array}\end{array}& \begin{array}{c}{I}_m\otimes I_3\\ 0\\ \begin{array}{c}0\\ -\beta I_m\otimes I_3\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array}\end{array}& \begin{array}{ccc}\begin{array}{c}0\\ I_m\otimes I_3\\ \begin{array}{c}0\\ -{\gamma }_1{I}_m\otimes I_3\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array}\end{array}& \begin{array}{c}0\\ 0\\ \begin{array}{c}{I}_m\otimes I_3\\ {-\gamma }_2{I}_m\otimes I_3\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array}\end{array}& \begin{array}{ccc}\begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ \alpha L_{AS}\otimes T_1\\ \begin{array}{c}0\\ \alpha L_{SS}\otimes I_2\\ \begin{array}{c}0\\ \alpha L_{US}\otimes T_1\end{array}\end{array}\end{array}\end{array}& \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}{I}_m\otimes I_2\\ -\beta I_m\otimes I_2\\ \begin{array}{c}0\\ 0\end{array}\end{array}\end{array}\end{array}& \begin{array}{cc}\begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ \alpha L_{SU}\otimes T_2\\ \begin{array}{c}0\\ \alpha L_{UU}\otimes I_3\end{array}\end{array}\end{array}\end{array}& \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}0\\ 0\\ \begin{array}{c}{I}_m\otimes I_3\\ -\beta I_m\otimes I_3\end{array}\end{array}\end{array}\end{array}\end{array}\end{array}\end{array}\end{array}\right)$$

The parameters $\alpha$, $\beta$, ${\gamma }_1$, ${\gamma }_2$ must be chosen so that $T$ has a zero eigenvalue and all the other eigenvalues have negative genuine parts. The gain parameter can be determined using the Routh–Hurwitz stability criterion [42] as follows:

$$\alpha >0,\beta >0,{\gamma }_1>0,{\gamma }_2>0,\beta \gg \alpha ,{\gamma }_1>\beta \gg \alpha ,{\gamma }_2>\beta \gg \alpha ,{\gamma }_2{\gamma }_1\beta >{\beta }^2+{{\gamma }_2}^2\alpha$$

After selecting parameters to stabilize the system, $T$ can be transformed into Jordan standard type as follows:

$$T=PJ{P}^{-1}.$$

Let $P_{dl}^T$ be the first row of $P^{-1}$ and the left eigenvector of 0 eigenvalue, and $P_{dr}$ be the first column of $P$ and the right eigenvector of 0 eigenvalue. Therefore, $P_{dl}^T{P}_{dr}=1$, as time approaches infinity, the system’s state becomes: $\underset{t\to \mathrm{\infty }}{\lim }\tilde{X}=\underset{t\to \mathrm{\infty }}{\lim }{e}^{Tt}\tilde{X}\left(0\right)$, $e^{Tt}\tilde{X}\left(0\right)\to \left(P_{dr}{P}_{dl}^T\right)\tilde{X}\left(0\right), \mathrm{t}\to \mathrm{\infty }$. Lemma 1 states that systems are asymptotically convergent and complete formation as the time approaches infinity. □

 Theorem 2.

For the formation equation of state (10), when the communication topology G = (V E, A) is connected undirected graph or directed graph containing a spanning tree, dynamic formation control protocol can achieve formation and the system state variables to achieve convergence.

 Proof of Theorem 2.

Refer to the proof of Theorem 1. □

3.2. Optimal Control

The main problem of optimal control research is to determine an optimal control law in the allowable control domain according to the mathematical model of the controlled object, so that the performance index of the system reaches the extreme value, that is, the optimal control law is determined when the performance index reaches the extreme value. The controlled objects in this paper are UAVs, USVs, and UUVs. In practical control problems, most of the control quantity is limited by objective conditions and can only be taken within a certain range, which is called admissible control. The performance index is a measure of system performance, and its content and form depend on the task to be completed by the optimal control problem. Based on the literature [43,44], the optimal control law is solved as follows:

Consider the equation of state of the system as:

$$\dot{x}=f\left(x,u,t\right)$$

The control vector $u$ can minimize the following performance index:

$$J={\int }_0^{\mathrm{\infty }}F\left(x,u,t\right)dt$$

Let the Hamilton function is:

$$H\left(x,u,\lambda ,t\right)=F\left(x,u,t\right)+{\lambda }^{T}f\left(x,u,t\right)$$

If the Hamilton function satisfies the following conditions:

$$\dot{\lambda }=-\frac{\partial H}{\partial x}.$$

$$\dot{x}=-\frac{\partial H}{\partial \lambda }$$

$$\frac{\partial H}{\partial u}=0$$

When the above conditions are satisfied, the optimal control law is obtained.

For the UAV system, let ${\tilde{X}}_{A1}={\left[{\tilde{P}}_{A1}{\tilde{V}}_{A1}{\tilde{\mathsf{\Omega }}}_{A1}{\widetilde{\dot{\mathsf{\Omega }}}}_{A1}\right]}^T$, an integral performance index composed of error variables and control variables is constructed as follows:

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

(13)

where $Q\ge 0$ is the symmetric non-negative definite matrix of the appropriate dimension, and $R>0$ is the symmetric positive definite matrix of suitable dimension.

In order to facilitate engineering application, $Q$ and $R$ in the performance index are taken as a diagonal linear matrix. When $Q=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[q_1{q}_2\dots \right]$ is taken, the first part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[{{\tilde{X}}_{A1}}^{T}Q{\tilde{X}}_{A1}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{q}_i{{\tilde{X}}_{A1}}^2\right]dt$, which is the total measurement of tracking error in the process of movement of the system. When $R=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[r_1{r}_2\dots \right]$ is taken, the second part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[u_{ia}^{T}R{u}_{ia}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{r}_i{u_{ia}}^2\right]dt$, which is the full measure of the system’s energy consumption.

Through the aforementioned analysis, the physical meaning of the quadratic performance index is to make the system’s dynamic error and energy consumption in the control process optimal. The optimal control law is as follows: $u_a^{\mathrm{*}}=-R^{-1}{B}_A^T{P}_{A1}{\tilde{X}}_{A1}$, where $P_{A1}$ is the solution of the Riccati equation:

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

(14)

The control parameter equation of UAV is: $K_a=R^{-1}{B}_A^T{P}_{A1}$, the dimension of $K_a$ is 3 × 12, and it also has the following form $K_a=\left[\begin{array}{ccc}{k}_{a1}& k_{a1}& \begin{array}{cc}{k}_{a3}& k_{a4}\end{array}\end{array}\right]\otimes I_3$.

For the USV system, let ${\tilde{X}}_{S1}={\left[{\tilde{P}}_{S1}{\tilde{V}}_{S1}\right]}^T$, an integral performance index composed of error variables and control variables is constructed as follows:

$$S_i={\int }_0^{\mathrm{\infty }}\left[{{\tilde{X}}_{S1}}^{T}G{\tilde{X}}_{S1}+u_{is}^{T}T{u}_{is}\right]dt$$

(15)

where $\mathrm{G}\ge 0$ is the symmetric non-negative definite matrix of appropriate dimension, and $\mathrm{T}>0$ is the symmetric positive definite matrix of suitable dimension.

In order to facilitate engineering application, $\mathrm{G}$ and $\mathrm{T}$ in the performance index are taken as a diagonal linear matrix. When $G=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[g_1{g}_2\dots \right]$ is taken, the first part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[{{\tilde{X}}_{S1}}^{T}G{\tilde{X}}_{S1}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{g}_i{{\tilde{X}}_{S1}}^2\right]dt$, which is the total measurement of tracking error in the process of movement of the system. When $T=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[t_1{t}_2\dots \right]$ is taken, the second part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[u_{is}^{T}T{u}_{is}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{t}_i{u_{is}}^2\right]dt$, which is the full measure of the system’s energy consumption.

Through the aforementioned analysis, the physical meaning of the quadratic performance index is to make the system’s dynamic error and energy consumption in the control process optimal. The optimal control law is as follows: $u_s^{\mathrm{*}}=-T^{-1}{B_S}^T{P}_{S1}{\tilde{X}}_{S1}$, where $P_{S1}$ is the solution of the Riccati equation:

$$A_S^T{P}_{S1}+P_{S1}{A}_S-P_{S1}{B}_S{T}^{-1}{B}_S^T{P}_{S1}+G=0$$

(16)

The control parameter equation of USV is: $K_s=T^{-1}{B_S}^T{P}_{S1}$, the dimension of $K_s$ is 2 × 4, and it also has the following form $K_s=\left[\begin{array}{cc}{k}_{\mathrm{s}1}& k_{\mathrm{s}2}\end{array}\right]\otimes I_2$.

For the UUV system, let ${\tilde{X}}_{U1}={\left[{\tilde{P}}_{U1}{\tilde{V}}_{U1}\right]}^T$, an integral performance index composed of error variables and control variables is constructed as follows:

$$w_i={\int }_0^{\mathrm{\infty }}\left[{{\tilde{X}}_{U1}}^{T}F{\tilde{X}}_{U1}+u_{iu}^{T}Y{u}_{iu}\right]dt$$

(17)

where $F\ge 0$ is the symmetric non-negative definite matrix of the appropriate dimension, and $Y>0$ is the symmetric positive definite matrix of the appropriate dimension.

In order to facilitate engineering application, $F$ and $Y$ in the performance index are taken as a diagonal linear matrix. When $F=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[f_1{f}_2\dots \right]$ is taken, the first part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[{{\tilde{X}}_{U1}}^{T}F{\tilde{X}}_{U1}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{f}_i{{\tilde{X}}_{U1}}^2\right]dt$, which is the total measurement of tracking error in the process of movement of the system. When $Y=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left[y_1{y}_2\dots \right]$ is taken, the second part of the performance index can be expressed as ${\int }_0^{\mathrm{\infty }}\left[u_{iu}^{T}Y{u}_{iu}\right]dt={\int }_0^{\mathrm{\infty }}\left[{\sum }_{i=1}{y}_i{u_{iu}}^2\right]dt$, which is the full measure of the system’s energy consumption.

Through the aforementioned analysis, the physical meaning of the quadratic performance index is to make the system’s dynamic error and energy consumption in the control process optimal. The optimal control law is as follows: $u_u^{\mathrm{*}}=-F^{-1}{B_U}^T{P}_{U1}{\tilde{X}}_{U1}$, where $P_{U1}$ is the solution of the Riccati equation:

$${A_U^{T}P}_{U1}+P_{U1}{A}_U-{P_{U1}B}_U{Y}^{-1}{B}_U^T{P}_{U1}+F=0$$

(18)

The control parameter equation of UUV is: $K_u=F^{-1}{B_U}^T{P}_{U1}$, the dimension of $K_u$ is 3 × 6, and it also has the following form $K_u=\left[\begin{array}{cc}{k}_{\mathrm{u}1}& k_{\mathrm{u}2}\end{array}\right]\otimes I_3$.

 Theorem 3.

For UAV state Equation (2), the optimal control law corresponding to performance indicator Equation (13) is $u_a^{*}$.

 Proof of Theorem 3.

Suppose $u_a^{\mathrm{*}}$ is the optimal control satisfying the performance index Equation (13), then the minimum principle must be satisfied, and the Hamilton function is constructed according to Equation (13):

$$\mathsf{H}=\frac{1}{2}{{\tilde{X}}_{A1}}^{T}Q{\tilde{X}}_{A1}+\frac{1}{2}{u}_{ia}^{T}R{u}_{ia}+{\lambda }^T{A}_A{\tilde{X}}_{A1}+{\lambda }^T{B}_A{u}_{ia}$$

(19)

where $\mathsf{\lambda }$ is a covariate, and since $u_a^{\mathrm{*}}$ is unconstrained, the minimum condition is Hamilton Function (19) with the control input $u_{ia}$ taking an unconditional minimum. Compute the derivative of the Hamilton function for the control input $u_{ia}$: $\frac{\partial \mathrm{H}}{\partial u_{ia}}=R{u}_{ia}+\mathsf{\lambda }{B}_A$, let $\frac{\partial \mathrm{H}}{\partial u_{ia}}=0$:

$$u_{ia}=-R^{-1}{B_A}^T\mathsf{\lambda }$$

(20)

Due to $\frac{{\partial }^{2}H}{{\partial }^2{u}_{ia}}=R>0$; therefore, Formula (20) uses Hamilton Formula (19) to obtain the minimum control, that is, the optimal control.

According to the regular Equation:

$$\left\{\begin{array}{c}{\dot{\tilde{X}}}_{A1}=\frac{\partial H}{\partial \mathsf{\lambda }}=A_A{\tilde{X}}_{A1}+B_A{u}_{ia}\\ \dot{\mathsf{\lambda }}=-\frac{\partial H}{\partial {\tilde{X}}_{A1}}=-Q{\tilde{X}}_{A1}-{A_A}^T\lambda \end{array}\right.$$

(21)

Let $\mathsf{\lambda }=P_{A1}{\tilde{X}}_{A1}$, the matrix $P_{A1}$ is undetermined, so, $\dot{\mathsf{\lambda }}=P_{A1}{\dot{\tilde{X}}}_{A1}$ is substituted into Equation (21):

$$P_{A1}{\dot{\tilde{X}}}_{A1}=-Q{\tilde{X}}_{A1}-{A_A}^T{P}_{A1}{\tilde{X}}_{A1}$$

(22)

Substituting regular Equation (21) into Equation (22), we can achieve:

$$P_{A1}{A}_A{\tilde{X}}_{A1}+P_{A1}{B}_A{u}_{ia}=-Q{\tilde{X}}_{A1}-{A_A}^T{P}_{A1}{\tilde{X}}_{A1}$$

(23)

Equation (20) is substituted into the Riccati equation in (23):

$${A_A}^T{P}_{A1}+P_{A1}{A}_A-P_{A1}{B}_A{R}^{-1}{B_A}^T{P}_{A1}+Q=0$$

(24)

Let $Q>0$, $R>0$, then the solution of $P_{A1}$ is positive definite, select Lyapunov functions:$\mathrm{V}\left({\tilde{X}}_{A1}\right)={\tilde{X}}_{A1}^T{P}_{A1}{\tilde{X}}_{A1}\ge 0$.

On the $\mathrm{V}\left({\tilde{X}}_{A1}\right)$ derivation: $\dot{\mathrm{V}}\left({\dot{\tilde{X}}}_{A1}\right)={{\tilde{X}}_{A1}}^T{P}_{A1}{\tilde{X}}_{A1}+{{\tilde{X}}_{A1}}^T{P}_{A1}{\dot{\tilde{X}}}_{A1}=-{{\tilde{X}}_{A1}}^T\left(P_{A1}{B}_A{R}^{-1}{B_A}^T{P}_{A1}+Q\right){\tilde{X}}_{A1}$. Because $Q>0$, $R>0$, there must be: $\left(P_{A1}{B}_A{R}^{-1}{B_A}^T{P}_{A1}+Q\right)>0$, so there are: $\dot{\mathrm{V}}\left({\dot{\tilde{X}}}_{A1}\right)\le 0$. According to Lyapunov stability theorem, the system with optimal control $u_a^{\mathrm{*}}$ is asymptotically stable. □

3.3. Cooperative Control

Since the models of agents in heterogeneous systems are not the same, the key to collaboration is to find the common part between the models. The heterogeneous system studied consists of unmanned aerial vehicles (UAVs), unmanned surface vehicles (USVs), and unmanned underwater vehicles (UUVs). UAVs contain position state, velocity state, attitude angle, and attitude angle change rate state; USVs contain position state and velocity state; UUVs contain position state and velocity state. Therefore, position state and velocity state are the common domain of the three, unmanned aerial vehicles (UAVs) and unmanned underwater vehicles (UUVs) are three-dimensional space, and unmanned surface vehicles (USVs) are two-dimensional space. Transformation matrix $m_{SA}$, $m_{AS}$, $m_{SU}$, $m_{US}$ are proposed.

Where,

$$m_{AS}=\left[\begin{array}{c}\begin{array}{cc}1& 0\end{array}\\ \begin{array}{cc}0& 1\end{array}\\ \begin{array}{cc}0& 0\end{array}\end{array}\right],m_{SA}=\left[\begin{array}{c}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right],m_{US}=\left[\begin{array}{c}\begin{array}{cc}1& 0\end{array}\\ \begin{array}{cc}0& 1\end{array}\\ \begin{array}{cc}0& 0\end{array}\end{array}\right],m_{SU}=\left[\begin{array}{c}\begin{array}{ccc}1& 0& 0\end{array}\\ \begin{array}{ccc}0& 1& 0\end{array}\end{array}\right]$$

3.4. Collaborative Optimal Control for Mixed-Order Heterogeneous Systems

By changing only the gain parameters, the collaborative optimal control of the system is created without altering the formation control protocol’s structure, which preserves the distribution of the formation control protocol. $\left|N_{Ai}\right|$ represents the number of neighbors of agent $i$ in the UAVs, $\left|N_{Si}\right|$ represents the number of neighbors of agent $i$ in the USVs, and $\left|N_{Ui}\right|$ represents the number of neighbors of agent $i$ in the UUVs. Therefore, the optimal control theory and cooperative control are added into the static formation control protocol can be obtained:

$$\begin{gathered} u_{ia}=\frac{k_{a1}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{a1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{AS}{\tilde{p}}_j-{\tilde{p}}_i\right)-k_{a2}{\tilde{v}}_i-k_{a3}{\mathsf{\Omega }}_i-k_{a4}{\dot{\mathsf{\Omega }}}_i \\ {u}_{is}=\frac{k_{\mathrm{s}1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{s}1}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left(m_{SA}{\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{s}1}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left(m_{SU}{\tilde{p}}_j-{\tilde{p}}_i\right)-k_{\mathrm{s}2}{\tilde{v}}_i \\ {u}_{iu}=\frac{k_{\mathrm{u}1}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{u}1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{US}{\tilde{p}}_j-{\tilde{p}}_i\right)-k_{\mathrm{u}2}{\tilde{v}}_i \end{gathered}$$

(25)

Cooperative optimal dynamic formation control protocol:

$$\begin{gathered} u_{ia}=\frac{k_{a1}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{a1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{AS}{\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{a2}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right)+\frac{k_{a2}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{AS}{\tilde{v}}_j-{\tilde{v}}_i\right)-k_{a3}{\mathsf{\Omega }}_i-k_{a4}{\dot{\mathsf{\Omega }}}_i \\ {u}_{is}=\frac{k_{\mathrm{s}1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{s}1}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left(m_{SA}{\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{s}1}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left(m_{SU}{\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{s}2}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right)+\frac{k_{\mathrm{s}2}}{\left|N_{Ai}\right|}\sum _{j\in N_{Ai}}{a}_{ij}\left(m_{SA}{\tilde{v}}_j-{\tilde{v}}_i\right)+\frac{k_{\mathrm{s}2}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left(m_{SU}{\tilde{v}}_j-{\tilde{v}}_i\right) \\ {u}_{iu}=\frac{k_{\mathrm{u}1}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left({\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{u}1}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{US}{\tilde{p}}_j-{\tilde{p}}_i\right)+\frac{k_{\mathrm{u}2}}{\left|N_{Ui}\right|}\sum _{j\in N_{Ui}}{a}_{ij}\left({\tilde{v}}_j-{\tilde{v}}_i\right)+\frac{k_{\mathrm{u}2}}{\left|N_{Si}\right|}\sum _{j\in N_{Si}}{a}_{ij}\left(m_{US}{\tilde{v}}_j-{\tilde{v}}_i\right) \end{gathered}$$

(26)

By adopting the above cooperative optimal control protocol, the system’s communication topology is not required to be a complete graph but only a connected graph. The above cooperative optimal control protocol takes into account the advantages of optimal control and cooperative control, which do not affect the cooperative control of the multi-agent system and can realize different kinds of task requirements according to the set performance indicators.

 Theorem 4.

For the formation equation of state (10), when the communication topology 𝐺 = (𝑉 𝐸, 𝐴) is connected undirected graph or directed graph containing a spanning tree, using the optimal control theory and cooperative control is added into the static formation control protocol can achieve optimal cooperative formation and the system state variables to achieve convergence.

 Proof of Theorem 4.

As mentioned above, the UAV, USV, and UUV are grouped into a group, the system matrix has the same number of rows and columns, and it can take the following forms:

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

(27)

where

$$\overline{A}=\left(\begin{array}{cccccccc}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& 0& 0& 0\\ 0& 0& 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& I\\ 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right)$$

$$\overline{B}=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ I& 0& 0\\ 0& 0& 0\\ 0& I& 0\\ 0& 0& 0\\ 0& 0& I\end{array}\right)$$

The matrix is constructed according to the PBH criterion as follows:

$$\left(\begin{array}{cc}SI-\overline{A}& \overline{B}\end{array}\right)=\left(\begin{array}{ccccccccccc}S& -I& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& S& -I& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& S& -I& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& S& 0& 0& 0& 0& I& 0& 0\\ 0& 0& 0& 0& S& -I& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& S& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& 0& S& -I& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& S& 0& 0& I\end{array}\right)$$

Through matrix $\overline{A}$, its eigenvalue can be obtained as: ${\lambda }_1={\lambda }_2=\cdots {\lambda }_8=0$. Let $\mathrm{S}={\lambda }_1={\lambda }_2=\cdots {\lambda }_8=0$, the rank of the matrix can be obtained as $\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}=\left(\begin{array}{cc}SI-\stackrel{-}{A}& B\end{array}\right)=8$, and according to the PBH criterion, the heterogeneous system is controllable.

The equation of a state for the cooperative optimal static formation control protocol is expressed as follows:

$$\left(\begin{array}{c}{U}_A\\ U_S\\ U_U\end{array}\right)=\left(\begin{array}{ccc}{U}_{a1}\otimes I_3& U_{a2}\otimes m_{AS}& U_{a3}\otimes I_3\\ U_{s1}\otimes m_{SA}& U_{s2}\otimes I_2& U_{s3}\otimes m_{SU}\\ U_{u1}\otimes I_3& U_{u2}\otimes m_{US}& U_{u3}\otimes I_3\end{array}\right)\times \tilde{X}$$

where

$$\left(\begin{array}{ccc}{{\displaystyle U}}_{a1}& {{\displaystyle U}}_{a2}& {{\displaystyle U}}_{a3}\\ {{\displaystyle U}}_{s1}& {{\displaystyle U}}_{s2}& {{\displaystyle U}}_{s3}\\ {{\displaystyle U}}_{u1}& {{\displaystyle U}}_{u2}& {{\displaystyle U}}_{u3}\end{array}\right)=\left(\begin{array}{cccccccc}\frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{AA}& {{\displaystyle -k}}_{a2}{{\displaystyle I}}_m& -{{\displaystyle k}}_{a3}{{\displaystyle I}}_m& -{{\displaystyle k}}_{a4}{{\displaystyle I}}_m& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{AS}& 0& 0& 0\\ \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{SA}& 0& 0& 0& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{SS}& {{\displaystyle -k}}_{s2}{{\displaystyle I}}_m& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{SU}& 0\\ 0& 0& 0& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{US}& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{UU}& -{{\displaystyle k}}_{u2}{{\displaystyle I}}_m\end{array}\right)$$

Substituting $\left(\begin{array}{ccc}{U}_{a1}& U_{a2}& U_{a3}\\ U_{s1}& U_{s2}& U_{s3}\\ U_{u1}& U_{u2}& U_{u3}\end{array}\right)\times \tilde{X}$ into Equation (27), we can get:

$$\dot{\tilde{X}}=\stackrel{-}{T}\tilde{X}$$

(28)

where

$$\overline{T}=\left(\begin{array}{cccccccc}0& I& 0& 0& 0& 0& 0& 0\\ 0& 0& I& 0& 0& 0& 0& 0\\ 0& 0& 0& I& 0& 0& 0& 0\\ \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{AA}& -{{\displaystyle k}}_{a2}I& -{{\displaystyle k}}_{a3}I& -{{\displaystyle k}}_{a4}I& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{AS}& 0& 0& 0\\ 0& 0& 0& 0& 0& I& 0& 0\\ \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{SA}& 0& 0& 0& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{SS}& {{\displaystyle -k}}_{s2}I& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{SU}& 0\\ 0& 0& 0& 0& 0& 0& 0& I\\ 0& 0& 0& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{US}& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{UU}& -{{\displaystyle k}}_{u2}I\end{array}\right)$$

By applying elementary row and row transformations, this matrix becomes:

$$\mathsf{\Lambda }=\left(\begin{array}{cccccccc}-I& I& 0& 0& 0& 0& 0& 0\\ -I& 0& I& 0& 0& 0& 0& 0\\ -2I& 0& I& I& 0& 0& 0& 0\\ {{\displaystyle r}}_1& {{\displaystyle r}}_2& -{{\displaystyle k}}_{a3}I& -{{\displaystyle k}}_{a4}I& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{AS}& 0& 0& 0\\ -I& 0& 0& 0& 0& I& 0& 0\\ {{\displaystyle r}}_3& I& 0& 0& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{SS}& {{\displaystyle -k}}_{s2}I& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{SU}& 0\\ -I& 0& 0& 0& 0& 0& 0& I\\ {{\displaystyle r}}_4& I& 0& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{US}& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{UU}& -{{\displaystyle k}}_{u2}I\end{array}\right)$$

where

$$\begin{gathered} r_1=k_{a2}I+k_{a4}I+\frac{k_{a1}}{\left|N_{Ai}\right|}{L}_{AA}-I \\ {r}_2=k_{a3}I-k_{a2}I+I \\ {r}_3=k_{s2}I-I \\ {r}_4=k_{u2}I-I \end{gathered}$$

$$\lambda {\rm I}-\mathsf{\Lambda }=\left(\begin{array}{cccccccc}\lambda +I& -I& 0& 0& 0& 0& 0& 0\\ I& \lambda & -I& 0& 0& 0& 0& 0\\ 2I& 0& \lambda -I& -I& 0& 0& 0& 0\\ {{\displaystyle -r}}_1& {{\displaystyle -r}}_2& {{\displaystyle k}}_{a3}I& \lambda +{{\displaystyle k}}_{a4}I& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{AS}& 0& 0& 0\\ I& 0& 0& 0& \lambda & -I& 0& 0\\ {{\displaystyle r}}_3& I& 0& 0& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{SS}& {{\displaystyle -k}}_{s2}I& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{SU}& 0\\ -I& 0& 0& 0& 0& 0& 0& I\\ {{\displaystyle r}}_4& I& 0& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{US}& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{UU}& -{{\displaystyle k}}_{u2}I\end{array}\right)$$

From the matrix transformation described above, it can be inferred that $\mathsf{\lambda }\mathrm{I}-\mathsf{\Lambda }\cong \mathsf{\lambda }\mathrm{I}-\stackrel{-}{T}$, and the matrix $\mathsf{\Lambda }$ is similar to the matrix $\overline{T}$. A non-singular transformation matrix $\overline{\mathrm{Q}}$ exists, resulting in $\mathsf{\Lambda }=\overline{\mathrm{Q}}\overline{T}{\overline{Q}}^{-1}$, $\dot{\tilde{X}}=\mathsf{\Lambda }\tilde{X}$, and $\mathsf{\Lambda }$ is the matrix where the sum of each row is zero.

As a result, at least one eigenvalue is zero. The primary column and row transformation can be performed in $\overline{T}$:

$$\overline{T}=\left(\begin{array}{cccccccc}I& 0& 0& 0& 0& 0& 0& 0\\ 0& I& 0& 0& 0& 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& I\\ 0& 0& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{AA}& \frac{{{\displaystyle k}}_{a1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{AS}& 0& 0& 0& 0\\ 0& 0& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ai}\right|}{{\displaystyle L}}_{SA}& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{SS}& \frac{{{\displaystyle k}}_{s1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{SU}& 0& 0& 0\\ 0& 0& 0& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Si}\right|}{{\displaystyle L}}_{US}& \frac{{{\displaystyle k}}_{u1}}{\left|{{\displaystyle N}}_{Ui}\right|}{{\displaystyle L}}_{UU}& 0& 0& 0\end{array}\right)=\left(\begin{array}{ccc}I& 0& 0\\ 0& 0& I\\ 0& kL& 0\end{array}\right)=E$$

If $\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}\left(\mathrm{L}\right)=\mathrm{N}-1$, $\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}\left(\begin{array}{cc}{I}_1& \\ & I_2\end{array}\right)=r$, then: $\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}\left(\overline{T}\right)=\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}\left(\mathsf{\Lambda }\right)=\mathrm{R}\mathrm{a}\mathrm{n}\mathrm{k}\left(E\right)=r+\mathrm{N}-1$. Combined with the proof of Theorem 1, $\overline{T}$ can be converted to Jordan standard form:

$$\stackrel{-}{T}=PJ{P}^{-1}$$

Let $P_{dl}^T$ be the first row of $P^{-1}$ and the left eigenvector of zero eigenvalue, and $P_{dr}$ be the first column of $P$ and the right eigenvector of zero eigenvalue. Therefore, $P_{dl}^T{P}_{dr}=1$. The system state as time reaches infinity is:

$$\begin{gathered} \underset{t\to \mathrm{\infty }}{\lim }\tilde{X}=\underset{t\to \mathrm{\infty }}{\lim }{e}^{Tt}\tilde{X}\left(0\right) \\ {e}^{Tt}\tilde{X}\left(0\right)\to \left(P_{dr}{P}_{dl}^T\right)\tilde{X}\left(0\right),\mathrm{t}\to \mathrm{\infty } \end{gathered}$$

Lemma 1 states that the system (27) can achieve convergence and the error vector is zero as the time tends to infinity, completing the cooperatively optimal formation. □

4. Simulation

This paper comprises three UAVs, three USVs, and three UUVs. Communication topology’s Laplacian matrix is as follows:

$$L=\left(\begin{array}{ccccccccc}3& -1& -1& -1& 0& 0& 0& 0& 0\\ -1& 3& -1& 0& -1& 0& 0& 0& 0\\ -1& -1& 3& 0& 0& -1& 0& 0& 0\\ -1& 0& 0& 4& -1& -1& -1& 0& 0\\ 0& -1& 0& -1& 4& -1& 0& -1& 0\\ 0& 0& -1& -1& -1& 4& 0& 0& -1\\ 0& 0& 0& -1& 0& 0& 3& -1& -1\\ 0& 0& 0& 0& -1& 0& -1& 3& -1\\ 0& 0& 0& 0& 0& -1& -1& -1& 3\end{array}\right)=\left(\begin{array}{ccc}{{\displaystyle L}}_{AA}& {{\displaystyle L}}_{AS}& 0\\ {{\displaystyle L}}_{SA}& {{\displaystyle L}}_{SS}& {{\displaystyle L}}_{SU}\\ 0& {{\displaystyle L}}_{US}& {{\displaystyle L}}_{UU}\end{array}\right)$$

Table 1. Status variables of the UAV.

UAVs	$\mathbf{Initial}\mathbf{Position}\left(\mathbf{m}\right)$	$\mathbf{Initial}\mathbf{Seed}\left(\mathbf{m}/\mathbf{s}\right)$	Initial Attitude $\mathbf{Angle}\left(°\right)$	Initial Attitude $\mathbf{Angle}\mathbf{Rate}\left(°/\mathbf{s}\right)$	$\mathbf{Expected}\mathbf{Position}\left(\mathbf{m}\right)$	$\mathbf{Expected}\mathbf{Seed}\left(\mathbf{m}/\mathbf{s}\right)$	Expected Attitude $\mathbf{Angle}\left(°\right)$	Expected Attitude $\mathbf{Angle}\mathbf{Rate}\left(°/\mathbf{s}\right)$
UAV1	(30,50,50)	(1,1,1)	(3,2,0)	(0,0,0)	(60,70,30)	(0,0,0)	(0,0,0)	(0,0,0)
UAV2	(90,30,50)	(1,−2,1)	(4,1,0)	(0,0,0)	(80,100,30)	(0,0,0)	(0,0,0)	(0,0,0)
UAV3	(60,30,15)	(−2,1,1)	(2,2,0)	(0,0,0)	(80,50,30)	(0,0,0)	(0,0,0)	(0,0,0)

lists the system state variables of the UAVs, and

Table 2. Status variables of the USV and UUV.

USVs and UUVs	$\mathbf{Initial}\mathbf{Position}\left(\mathbf{m}\right)$	$\mathbf{Initial}\mathbf{Seed}\left(\mathbf{m}/\mathbf{s}\right)$	$\mathbf{Expected}\mathbf{Position}\left(\mathbf{m}\right)$	$\mathbf{Expected}\mathbf{Seed}\left(\mathbf{m}/\mathbf{s}\right)$
USV1	(90,50)	(0,−1)	(60,70)	(0,0)
USV2	(65,10)	(0,1)	(80,100)	(0,0)
USV3	(30,20)	(1,1)	(80,50)	(0,0)
UUV1	(40,50,−10)	(−1,−1,−1)	(50,60,−30)	(0,0,0)
UUV2	(60,30,−30)	(2,−2,−1)	(70,90,−30)	(0,0,0)
UUV3	(30,20,−20)	(−1,1,−1)	(60,50,−30)	(0,0,0)

lists the state variables of the USVs and UUVs systems. The control parameters of the formation control protocol are: $\mathsf{\alpha }=0.2,\beta =1.5,{\gamma }_1=5,{\gamma }_2=2$. The control parameters of cooperative optimal formation control protocol are:$\mathrm{Q}=13\mathrm{\ast }{I}_{12}, R=2\mathrm{\ast }{I}_3{, G=1\mathrm{\ast }{I}_4, T=5\mathrm{\ast }{I}_2, F=2\mathrm{\ast }{I}_6, Y=4\mathrm{\ast }{I}_3, k}_{a1}=2.5495, k_{a2}=6.9756, k_{a3}=8.2681, k_{a4}=4.7996, k_{s1}=0.4472, k_{s2}=1.0461, k_{u1}=0.7071, k_{u2}=1.3836$.

4.1. Simulation of Static and Dynamic Formation Control Protocol

Figure 3, Figure 4 and Figure 5 are the simulation under the static formation control protocol, and Figure 3 is the actual position trajectory of each agent system. It can be seen from the figure that each agent system finally completes the formation with a triangle.

Figure 4 and Figure 5 show each agent system’s convergence of state variables. Figure 4 shows that each agent system realizes position state convergence after 30 s. Figure 5 shows velocity state consistency after 30 s.

It can be seen from Figure 4 and Figure 5 the observation that when the time goes to infinity, the system’s speed reaches the same, and the position converges gradually. Since the speed of the static formation control protocol is 0, the slope of the position change is 0.

Figure 6, Figure 7 and Figure 8 are the simulation under the dynamic formation control protocol, and Figure 6 is the actual position trajectory of each agent system. It can be seen from the figure that each agent system finally completes the formation with a triangle.

Figure 7 and Figure 8 show each agent system’s convergence of state variables. Figure 7 shows that each agent system realizes position state convergence after 25 s. Figure 8 shows velocity state consistency after 25 s.

It can be seen from Figure 7 and Figure 8 the observation that when the time goes to infinity, the system’s velocity reaches the same, and the position converges gradually. Since the speed of the dynamic formation control protocol is not zero, the velocity is the differential of the position for a time, so the position is constantly changing.

4.2. Simulation of Cooperative Optimum Formation Control Protocol

Figure 9, Figure 10 and Figure 11 show the simulation of introducing optimal control and cooperative control to the static formation control protocol, and Figure 9 shows the actual position trajectory of each agent system. It can be seen from the figure that each agent system completes the cooperative optimum formation with a triangle.

Figure 10 and Figure 11 show each agent system’s convergence of state variables. Figure 10 shows that position-state convergence is achieved after 20 s. Figure 11 shows that velocity state consistency is achieved after 20 s. Compared to Figure 10 and Figure 11 with Figure 4 and Figure 5, the system can quickly reach the expected value and complete the formation.

Figure 12, Figure 13 and Figure 14 show the simulation of introducing optimal control and cooperative control to the dynamic formation control protocol, and Figure 12 shows the actual position trajectory of each agent system. It can be seen from the figure that each agent system completes the cooperative optimum formation with a triangle.

Figure 13 and Figure 14 show each agent system’s convergence of state variables. Figure 13 shows that position-state convergence is achieved after 20 s. Figure 14 shows that velocity state consistency is achieved after 15 s. Compared to Figure 13 and Figure 14 with Figure 5 and Figure 6, the system can achieve convergence and cooperative formation quickly.

5. Conclusions

This paper proposes a cooperative optimal formation control strategy for mixed-order heterogeneous multi-agent systems based on optimal control theory and cooperative control.

Firstly, for heterogeneous multi-agent systems with different dimensions and models written as a state space, the block Kronecker product is used to write the system in space. Secondly, the graph theory matrix proves the effectiveness of the proposed dynamic and static formation control protocols. Further, the optimal control theory and cooperative control are proposed, and the dimensional inconsistency problem is solved by using the cooperative control, the optimal gain parameters are obtained by the optimal control, then the cooperative formation control is presented. Finally, the effectiveness of the cooperative optimal formation control protocol is verified by simulation, and it can be verified that the incorporation of optimal control theory and cooperative control can hasten the system’s convergence and complete the cooperative formation. In future work, the system will be affected by the environment, the USV and UUV will be affected by the ocean, the UAV will be affected by the wind speed, and the environmental effects will be the system obstacles, such obstacles will be taken into consideration in the system formation.