Distributed Consensus of USVs under Heterogeneous UAV-USV Multi-Agent Systems Cooperative Control Scheme

Abstract

This paper addresses the formation motion control of heterogeneous multi-agent unmanned systems via a distributed consensus approach. The considered heterogeneous system consisted of unmanned aerial vehicles (UAVs) and unmanned surface vehicles (USVs). A leader-following consensus scheme and APF method are used to construct UAV-USVs Formation task requirements. A fuzzy-based sliding mode control approach is proposed to ensure the formation assembles in a finite time, and the finite-time stability is proved by the Lyapunov stability theorem. To highlight the cooperation within the heterogeneous systems, such as UAV and USV, a novel vision-based path re-planning approach is proposed. Simulation results confirm the efficiency of the proposed approach.

1. Introduction

Compared with conventional manned ships, USVs have many outstanding tactical and technical features, such as small size, good concealment, high speed, flexible maneuverability, and no risk of casualties. An unmanned vehicle is capable of independently completing various maritime tasks in complex marine environments without human manipulation, which is an effective tool for marine development. USVs are playing an increasingly important role in the military fields and scientific research. It is worth noting that as unmanned systems research progresses, the tasks that can be accomplished by a single unmanned vehicle become more and more limited. Motivated by engineering applications including cooperation control of other unmanned vehicles, distributed sensor networks [1], swarm aggregation [2], and consensus has received increasing attention in recent decades. Consensus of a multi-agent system is one of the most fundamental issues in the cooperative control of a group of unmanned vehicles [3]. Consensus is considered as one of the major challenges in distributed cooperative control, and its difficulty lies in how to deal with the information interaction between different agents. USVs move on the surface with environmental awareness capabilities, while the information that USVs can perceive during cooperation tasks is very limited due to environmental constraints. For the reason of the complex application scenarios of unmanned system cooperation and variable tasks, recent efforts have been extended to the consensus of heterogeneous multi-agent systems.

In the related literature, the combined application of UUV and USV is adopted in the ocean area without discussing the formation assembly problem [4]. The formation of UAVs and USVs is investigated in [5], where a combination of the second-order model and Euler–Lagrange model is adopted. Above mentioned studies focus on the heterogeneous multi-agent systems of general dynamics, which, however, did not describe the interaction between different dynamics. Although there are theoretical combinations of UAV and USV models [6,7], there is no prior work that considers heterogeneous systems’ cooperative control under weak communication scenarios. UAV and USV are typically heterogeneous unmanned systems, which can compensate for the lack of environmental awareness of USVs formation cooperation. With the advantage of its “hawk-eye” like view, UAV can quickly and accurately obtain the information of USVs on the surface. Therefore, in this paper, we propose a cooperative path planning and motion control method for USVs based on UAV-USV cooperation.

Consensus of a multi-agent system, where the position relationship between ships was considered to make the group form a specific shape. However, such approaches tend to generate low-level controller commands, with a lack of discussion of the decision level controller. For this issue, some further efforts have been studied more intensively, involving coordination at multiple control levels, from the decision-making layer to the control layer. The purpose of coordinated motion control is to provide practical navigation information for the group to optimize the trajectory of the formation, thereby facilitating the coordination of multiple USVs [8]. There are several common cooperative motion control strategies such as leader-follower architecture [9], behavior-based method [10], artificial potential field method [11], virtual structure [12], and cooperative path-following [13]. Based on these cooperative motion control strategies, some excellent results have been proposed. Ghommem et al. designed the formation controller based on the virtual structure method, divided the controller into two parts: formation shape keeping and formation path tracking, and combined the dynamics of underactuated USVs to realize the formation keeping during the path tracking of USVs formation[14]. Chen et al. proposed an improved leader-follower approach in which the follower tracks the corresponding virtual target to achieve the formation shape and conducted related real-ship verification experiments [15]. Ghabcheloo et al. investigated the problem of steering maritime vehicles with given spatial paths. A path-following control law along with the coordination states was purposed, while communication failure and time delays were involved [16]. Above mentioned studies discuss the cooperative motion control of USVs of general dynamics and topologies, where the position of each USV is known. However, the information of the agents is not easily available.

There is a prominent problem in the USVs formation control method: how to obtain the information of the targets inside and outside the formation? In theoretical research, generally assumes that USVs can communicate freely. As long as an optimized communication topology is designed, information interaction can be easily formed. In the actual scenario, there are static or dynamic obstacles around the ship. In the design of the formation obstacle avoidance strategy, it is usually assumed that the scale and the motion of the target are known, while the controller is designed under this assumption. However, in practical applications, the communication capability between ships is suboptimal and the USVs’ own environmental sensing capability is somewhat deficient. In this paper, we present a formation control approach adapted for onboard visual relative localization of heterogeneous multi-agent systems consisting of UAVs and USVs. The localization is based on the “hawk-eye” view of the UAV in which the information about the movement of USVs on the surface can be obtained under weak communication conditions. UAV/USV cooperation is a new frontier technology field, and the advantages of both complement each other, which can effectively broaden the application scenarios of unmanned systems. The possibility to localize USVs from the “hawk eye” view increases robustness and precision in determining the relative position. Additionally, the “hawk eye” view brings another perspective for human operators supervising the mission [17]. The target recognition and tracking algorithms have been relatively well established. For example, Long P T. et al. used a monocular camera to look down to identify objects with specific color and shape characteristics and estimate the position and rotation to control the motion of a ground robot based on visual information [18]. Zhao et al. used an unmanned airship platform to achieve target recognition in dynamic scenarios by dividing ground target detection into two parts: static and dynamic targets [19]. The above results demonstrated the practical application of top view target identification and tracking without discussing the purpose of target identification and tracking. However, the most direct application is to replace the positioning system using the identified position information. In recent years, further studies focus on target recognition algorithms based on deep learning, improving real-time performance and accuracy compared to conventional methods, while requiring repeated training to build a database. For the problem of sea-level target detection under the “hawk-eye” like view of UAV, there are fewer targets on the sea surface and the target features are prominent, which makes the manual calibration workload of the database greatly reduced.

We can find good guiding significance in the related literature, such as Elfes et al., who developed an air-ground robot cooperative system for inspection, monitoring, and data collection [20]. Not coincidentally, Grocholsky et al. investigated UAVs and UGVs with GPS equipped, using a distributed fusion data-based approach that combines established target characteristics and linearized estimates with detection and localization of ground targets [21]. Based on the above ideas, we propose a UAV-USV heterogeneous unmanned system cooperation method for offshore operations. Unlike the heterogeneous architecture proposed by Shao et al., who uses a hierarchical guidance point generation algorithm to achieve cooperative takeoff and landing control of USV [22], this paper unifies UAV and USV as members in a heterogeneous unmanned system formation, establishes dynamics models, and designs a distributed consensus controller. Due to the limited visual angle of the hawk-eye view of the UAV and the possible motion of the USVs on the sea surface beyond the field of the vision sensor’s view, we use the projection point of the UAV on the sea level as the virtual leader of the USVs formation and employ the leader-following consensus method [23] to achieve the cooperative UAV-USV motion, so that the USVs are always within the field of sensor view.

In this paper, we consider the formation control problem of a heterogeneous unmanned system consisting of a UAV and USVs based on the leader-following consensus method. In a heterogeneous unmanned system operating mission, the agents form a fixed shape formation. We assume that the USVs on the surface are not equipped with any onboard sensors and that the UAVs (quadrotors) are equipped with a vision sensor of a “hawk-eye” like view to obtain relative position information of the USVs with different feature markers, and that the UAVs are equipped with a positioning system and an altitude sensor. We use the RGB color space features of the images captured by the vision sensor to identify the locations of USVs in the field of view. In addition to this, a fixed formation shape was designed to ensure that all USVs are always within the field of view. When designing the controller, it is usually divided into ship course control [24,25,26] and ship trajectory control [27,28] based on the control output. Among them, the control output of the ship course controller acts directly on the rudder angle, and the agents constantly change the heading angle to complete the formation task under the condition of fixed speed, while the control output of the ship trajectory controller is a set of desired velocities, including linear and angular velocities, and the formation motion is solved by the low-level controller of the agent. To ensure the generality of the method, the ship trajectory controller is designed so that it can be applied to unmanned systems with different dynamics. For this purpose, we employed the sliding mode control method to make the assembly of USVs quickly and steadily and constructed the APF (artificial potential field) function so that the USVs could be assembled without collision. Currently, there are some results related to the cooperative formation of UAV-USV heterogeneous unmanned systems in [5,29,30,31]. However, the heterogeneous properties are expressed in the variation of model parameters, while the cooperative relationship between UAV and USV is less described. To the best of our knowledge, results on distributed consensus control of heterogeneous UAV-USVs systems are scarce, which deserves more attention.

2. Materials and Methods

A heterogeneous unmanned system contains several unmanned vehicles with different dynamics, and the relative motion relationships between the different vehicles are considered when designing the controller. In the case of this paper, the UAV-USV heterogeneous unmanned system consists of a quadrotor UAV and four USVs, where the motion of the USVs on the surface is approximated as a motion on a two-dimensional plane while the UAV is moving in the three-dimensional space above the plane. The heterogeneous unmanned system coordinate system relationship is shown in Figure 1.

$O_W{X}_W{Y}_W{Z}_W$ represents the world coordinate system, $O_{Bi}{X}_{Bi}{Y}_{Bi}{Z}_{Bi}$ represents the body coordinate system of the i-th USV and $O_C{X}_C{Y}_C{Z}_C$ represents the camera coordinate system. In addition, the body coordinate system of the UAV is defined as $O_A{X}_A{Y}_A{Z}_A$, the center of mass of the camera coincides with the UAV, and the following transformation converting relationship exists between the world coordinate system and the camera coordinate system,

$$H_W^C=\left[\begin{array}{cc}{R}_W^C& d_W^C\\ 0& 1\end{array}\right]$$

(1)

where,

$$R_W^C=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],d_W^C=\left[\begin{array}{l}{d}_x\\ d_y\\ d_z\end{array}\right]$$

(2)

$R_W^C$ represents the rotation matrix associated with the camera attitude, $d_W^C$ represents the position vector of the origin of the camera coordinate system in the world coordinate system. In general, the camera is fixed to the UAV and the elements in the rotation matrix are related to the roll, pitch, and yaw of the UAV in the world coordinate system. In the case of this study, to stabilize the hawk-eye view of the UAV, we assume that the camera is mounted on a self-stabilizing platform so that the coordinate system of the UAV and the camera conforms to the North East Earth (NED) coordinate system, i.e., the axis $Z_W$ remains perpendicular to the water surface downward. When the UAV is moving, the plane $O_C{X}_C{Y}_C$ of the camera coordinate system is always parallel to the plane $O_W{X}_W{Y}_W$ of the world coordinate system, and the height of the camera is acquired by the UAV’s sensors.

In the world coordinate system, the position of the i-th USV is $P_i=(x_i,y_i,z_i)$, while the relative position of the camera coordinate system is $P_i^C=(x_i^C,y_i^C,z_i^C)$. The converting relationship between them is,

$$[x_i^C,y_i^C,z_i^C,1]=H_W^C[x_i,y_i,z_i,1]$$

(3)

Define the pixel coordinate system of the image captured by the camera as $OUV$, so that the coordinate of $P_i$ in $OUV$ is $P_i^{\prime }=({\mu }_i,{\nu }_i)$, and the center of the image is $P_0^{\prime }=({\mu }_0,{\nu }_0)$. Assuming that the focal length of the camera is f, and the horizontal/vertical dimensions of the pixel are equal to s, we have $f_s=f/s$. In this case, the projection points coordinate of the USV in $O_C{X}_C{Y}_C{Z}_C$ and its relative coordinates in $OUV$ have the following converting relationship,

$$\left[\begin{array}{c}{\mu }_i\\ v_i\\ 1\end{array}\right]=\frac{1}{z_i^C}\left[\begin{array}{cccc}{f}_s& 0& {\mu }_0& 0\\ 0& f_s& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_i^C\\ y_i^C\\ z_i^C\\ 1\end{array}\right]$$

(4)

substitute Equation (3) into Equation (4), we can obtain the rotating matrix for converting the coordinates in the pixel coordinate system to the world coordinate system,

$$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\\ 1\end{array}\right]=z_i^C\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ h_{21}& h_{22}& h_{23}\\ h_{31}& h_{32}& h_{33}\\ h_{41}& h_{42}& h_{43}\end{array}\right]\left[\begin{array}{c}{\mu }_i\\ v_i\\ 1\end{array}\right]$$

(5)

2.1. Heterogeneous Unmanned System Model

To describe the motion of the USV formation in the plane, we use the converting relationship between the world coordinate system and the body coordinate system to build the kinematics and the dynamics. $O_W{X}_W{Y}_W{Z}_W$ is also called inertial coordinate system, while the USV is moving in the $O_W{X}_W{Y}_W$ plane. The origin of the body coordinate system coincides with the center of gravity of USV. Ignore the effects of pitch, roll, and heave, the USV plane three-degree-of-freedom nonlinear mathematical model is built under the following assumption.

Assumption 1. 

USV is symmetrical about its central axis and only moves at low speed in the horizontal plane, ignoring the effect of higher-order hydrodynamic damping coefficients. The center of gravity, the center of mass, and the center of USV coincide and are coincident with the origin of the body coordinate system.

Considering a heterogeneous unmanned system containing N USVs, the plane three-degree-of-freedom nonlinear mathematical model for the i-th ($i\in N,N=\{1,\dots ,N\}$). USV is derived based on the USV nonlinear mathematical model proposed by Fossen [32].

$${\dot{P}}_i=T(P_i)u_{\mathrm{i}}$$

(6)

where $P_i{=[\mathrm{x}}_i{,\mathrm{y}}_i{]}^T$ represents the position of the i-th USV in the world coordinate system, while $u_i={[v_i,w_i]}^T$ represents the linear and angular velocities, respectively. For a general under-actuated USV, there is no lateral propeller, so that the relationship between the yaw angle and the angular velocity is ${\dot{\theta }}_i=w_i$. $T(P_{\mathrm{i}})$ denotes the transformation matrix from the body coordinate system to the world coordinate system, which satisfies $T^{-1}(P)=T^{\mathrm{T}}(P)$.

$$T(P_i)=\left[\begin{array}{cc}\cos {\theta }_i& -\sin {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\end{array}\right]$$

(7)

A typical UAV model is shown in Figure 2.

The Euclidean position expression of UAV in the inertial coordinate system is unified with USV, which is $P_i=(x_i,y_i,z_i)$. The Euler angle is expressed as ${\Phi }_i=({\varphi }_i,{\theta }_i,{\psi }_i)$. A high-order kinematic model of the UAV can be derived through the Newton-Euler equation [33].

$$\left\{\begin{array}{l}\ddot{x}=(\cos \varphi \sin \theta \cos \psi +\sin \varphi \sin \psi )F_1/m-K_x\dot{x}/m\\ \ddot{y}=(\sin \varphi \sin \theta \cos \psi -\sin \varphi \sin \psi )F_1/m-K_y\dot{y}/m\\ \ddot{z}=(\cos \theta \cos \varphi )F_z/m-K_z\dot{z}/m-g\\ \ddot{\varphi }=F_3/I_x-K_{\varphi }\dot{\varphi }/I_x\\ \ddot{\theta }=F_2/I_y-K_{\theta }\dot{\theta }/I_y\\ \ddot{\psi }=F_4/I_z-K_{\psi }\dot{\psi }/I_z\end{array}\right.$$

(8)

where $F_i$ denotes the lift force generated by each rotor motor, K is the aerodynamically relevant drag coefficient, I denotes the moment of inertia, and g is the gravitational acceleration constant. For the UAV, we only consider the motion of the UAV at a certain altitude, which means that the flight altitude of the UAV remains unchanged, so there is the following assumption.

Assumption 2. 

Assuming that the UAV is flying at a constant height and low speed in three dimensions space, the total lift generated by the rotor of the UAV is equal to its gravity, and the variation of the pitch and roll angles is minimal, so that$\sin \varphi \approx \varphi$, $\sin \theta \approx \theta$, and the yaw angle is$\psi =0$.

Based on the assumption above, Equation (8) can be simplified and has the following form,

$$\left\{\begin{array}{l}\ddot{x}=g\theta \\ \ddot{y}=-\varphi g\\ \ddot{z}=F_z/m-g\\ \ddot{\varphi }=M_{\varphi }/I_x\\ \ddot{\theta }=M_{\theta }/I_y\\ \ddot{\psi }=M_{\psi }/I_z\end{array}\right.$$

(9)

where $F_z$ denotes the total lift of the UAV in $Z_W$ direction. $M_{\varphi }$, $M_{\theta }$, $M_{\psi }$ denote the torque of roll, yaw, and pitch in the body coordinate system, respectively. Define the control input as:

$$u_z=F_z-g,u_{\varphi }=M_{\varphi }/I_x,u_{\theta }=M_{\theta }/I_y, u_{\psi }=M_{\psi }/I_z$$

(10)

In this paper, we consider a unified scheme of UAV and USV for formation motion design, that is, to design controllers for mixed-order heterogeneous multi-agent unmanned systems. To achieve this purpose, we use the state-space model for controller design. The heterogeneous multi-agent unmanned system state space is defined as,

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

(11)

where $X=[\begin{array}{ccc}{P}_A& V_A& \begin{array}{ccc}{\alpha }_A& {\beta }_A& \begin{array}{cc}{P}_U& V_U\end{array}\end{array}\end{array}]$. The subscript _A represents the UAV state variables, and the subscript _U represents the state variables of USV. $P_A=[\begin{array}{ccc}{x}_A& y_A& z_A\end{array}]$ and $V_A=[\begin{array}{ccc}{v}_{xA}& v_{yA}& v_{zA}\end{array}]$ denote the position vector and the velocity vector, respectively. ${\alpha }_A=[\begin{array}{ccc}g\theta & -g\varphi & 0]\end{array}$ represents the second-order derivative vector at the current position of UAV, ${\beta }_A=[\begin{array}{ccc}g{\omega }_x& -g{\omega }_y& 0]\end{array}$ is the variation vector of ${\alpha }_A$. ${\omega }_x \mathrm{and} {\omega }_y$ represent the angular velocity in the x-axis and y-axis, respectively. $P_U={[\begin{array}{ccc}{P}_1& P_2& \begin{array}{cc}{P}_3& P_4\end{array}\end{array}]}^T$, $V_U={[\begin{array}{ccc}{V}_1& V_2& \begin{array}{cc}{V}_3& V_4\end{array}\end{array}]}^T$, where $P_i=[\begin{array}{cc}{x}_i& y_i\end{array}]$, $V_i=[\begin{array}{cc}{v}_{xi}& v_{yi}\end{array}]$($i=1,2,3,4$) denote the position vector and the velocity vector of i-th USV, respectively. At this point, the state space system matrixes A and B have the following form:

$$\mathit{A}=\left[\begin{array}{cccccc}{0}_{3\times 3}& {\mathit{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2}& 0_{3\times 2}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathit{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 2}& 0_{3\times 2}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\mathit{I}}_{3\times 3}& 0_{3\times 2}& 0_{3\times 2}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 2}& 0_{3\times 2}\\ 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 2}& {\mathit{I}}_{2\times 2}\\ 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 2}& 0_{2\times 2}\end{array}\right], B=\left[\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 2}\\ 0_{3\times 3}& 0_{3\times 2}\\ 0_{3\times 3}& 0_{3\times 2}\\ I_{3\times 3}& 0_{3\times 2}\\ 0_{2\times 3}& 0_{2\times 2}\\ 0_{2\times 3}& I_{2\times 2}\end{array}\right],$$

define the status feedback input U as:

$$U=\left[\begin{array}{c}{U}_A\\ U_U\end{array}\right]=\left[\begin{array}{cc}{U}_{11}\otimes I_{3\times 3}& U_{12}\otimes I_{3\times 2}\\ U_{21}\otimes I_{2\times 3}& U_{22}\otimes I_{2\times 2}\end{array}\right]\times E$$

where E denotes the formation error vector of the heterogeneous unmanned system.

The controller in this paper will be designed based on the above model.

2.2. Graph Theory

Graph theory can be used to describe the formation communication topology of heterogeneous multi-agent unmanned systems, where each unmanned system can be regarded as a node of the graph and the information interaction between unmanned systems can be regarded as the edges connecting the nodes. For a formation system consisting of $N$ USVs, let $G=G(V,\epsilon )$ denote a graph, where $V=\{v_i,i=1,\dots ,N\}$, named as the node set, denotes the set of elements, and $\epsilon =\{{(v_i,v_j)}_k,k=1,\dots ,M\}\subseteq V\times V$, named as the edge set, denotes the set of directed edges between the nodes of the set $V$. If $(v_i,v_j)\in \epsilon$ then $v_i$ can communicate to $v_j$. If $\exists (v_i,v_j)$, such that $(v_i,v_j)\in \epsilon \iff (v_j,v_i)\in \epsilon$, the edge is traversed positively, otherwise it is traversed negatively. If any edge in the graph is traversed positively, the graph is an undigraph; if not, the graph is a digraph. Define the adjacency matrix $\Lambda =({\mathrm{a}}_{ij})\in {ℝ}^{N\times N}$ to describe the connectivity between the nodes of the graph.

$$a_{ij}=\left\{\begin{array}{ll}0& \mathrm{otherwise}\\ 1& \mathrm{if} \left({\mathrm{v}}_i,v_j\right)\in \epsilon ,i\ne j\end{array}\right.$$

(12)

Define the indegree matrix $D=diag\{d_1,\dots ,d_N\}\in {\mathrm{R}}^{N\times N}$, where $d_i={\displaystyle {\sum }_{j\in N_i}{a}_{ij}}$.

Thus, the Laplacian matrix of graph G can be expressed as $L=D-\Lambda =[l_{ij}]\in {\mathrm{R}}^{N\times N}$.

In the case of this paper, the projection point of the UAV on the horizontal plane is considered as a virtual leader. A digraph with 5 nodes is proposed as shown in Figure 3.

The communication between heterogeneous multi-agent unmanned systems is complex, and if the communication topology of UAVs and USVs is defined as a simple connectivity relationship, it does not reflect the special characteristics of their communication. To better describe the communication between heterogeneous systems, the following definition is made.

Definition 1. 

To describe heterogeneous unmanned system communication, define the block Laplacian matrix:

$$L=\left[\begin{array}{cc}{L}_{AA}& L_{AU}\\ L_{UA}& L_{UU}\end{array}\right]$$

(13)

where$L_{AA}$and$L_{UU}$represent the Laplacian matrix corresponding to UAV and USV homogeneous unmanned systems, while$L_{AU}$and$L_{UA}$represent the Laplacian block matrix of the graph between heterogeneous unmanned systems.

2.3. Distribute Leader-Following Consensus with APF Collision Avoidance

Among the many formation control strategies, the leader-follower strategy is the most widely used. The leader-follower architecture describes the relative position between the follower and the leader and converts the formation control problem to a trajectory tracking problem. However, the drawback of the leader-follower strategy is obvious, which is the leader and only the leader gets the highest priority at the control level and can obtain all navigation information. Once the leader fails, the entire formation collapses. In response to the above problem, we employed the concept of a virtual leader, that is, the horizontal projection of UAV as the virtual leader of USVs formation. The APF method is widely used to deal with the relative distance between agents and can perform collision avoidance of the formation.

Define the desired state vector of the formation as $X^d=[\begin{array}{ccc}{P}_A^d& V_A^d& \begin{array}{ccc}{\alpha }_A^d& {\beta }_A^d& \begin{array}{cc}{P}_U^d& V_U^d\end{array}\end{array}\end{array}]$, then the formation error vector is

$$E=X-X^d$$

(14)

The state-space of the formation desired vector can be expressed as

$${\dot{X}}^d=A{X}^d+B{U}^d$$

(15)

When the formation satisfies the desired state, the control input $U^d=0$, we can obtain the state space representation of the formation error vector:

$$\begin{array}{lll}\dot{E}& =& \dot{X}-{\dot{X}}^d\\ & =& AX+BU-A{X}^d\\ & =& AE+BU\end{array}$$

(16)

For the heterogeneous unmanned system formation, the ultimate control goal is to achieve a fixed shape formation consensus of the heterogeneous multi-agent unmanned systems, where the vector $E$ is only related to the position vector $P_u$. Since we only focus on the position error vector in the formation error vector $E$, giving

$$E=[\begin{array}{cccccc}{P}_A-P_A^d& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& P_U-P_U^d& 0_{2\times 3}]\end{array}$$

(17)

USVs follow a virtual leader to achieve consensus. Since the projection point of the UAV serves as the virtual leader, USVs in the formation construct the leader-follower formation scheme based on the reference position of the virtual leader. We employed the APF path planning strategy to constrain the distance between the desired position of the i-th USV $P_U^d$ and the position of the virtual leader $P_A$ at the same time. The motion dynamics can be represented as

$${\dot{P}}_U^d={\dot{P}}_A+g(P_i-P_A)+{\displaystyle \sum _{j=1,j\ne i}^{N}g(P_i-P_j)}$$

(18)

where $g(P)$ represents the attraction/repulsion function within the formation and has the following form [34]:

$$\begin{array}{lll}g(P)& =& -P[g_a(‖P‖)-g_r(‖P‖)]\\ & =& -P[a-b\exp (-{‖P‖}^2/c)]\end{array}$$

(19)

where a, b, c are positive constants, Equation (17) can be expressed as

$${\dot{P}}_U^d-{\dot{P}}_A=V_U^d-V_A=-{\nabla }_{P_i}J\left(P\right)$$

(20)

$J(P)$ is the potential function that determines the interactions between the unmanned systems and has the following form,

$$J(P)=J_1(P)+J_2(P)$$

(21)

$$J_1(P)={\displaystyle \sum _{i=1}^{N-1}{\displaystyle \sum _{j=i+1}^N{J}_a\left(‖P_i-P_j‖\right)}}-J_r\left(‖P_i-P_j‖\right)$$

(22)

$$J_2(P)={\displaystyle \sum _{i=1}^N{J}_a}\left(‖P_i-P_A‖\right)-J_r\left(‖P_i-P_A‖\right)$$

(23)

where $J_a$ represents the attractive potential that dominates on long distances and $J_r$ represents the repulsive potential that dominates on short distances. The motion of each unmanned system is restricted along the combined gradient of these potentials. There exist corresponding functions $J_a$ and $J_r$ such that $J=J_a-J_r$, ${\nabla }_P{J}_a(‖P‖)=P{g}_a(‖P‖)$ and ${\nabla }_P{J}_r(‖P‖)=P{g}_r(‖P‖)$. For the attraction/repulsion function, we have the following form:

$$J_a\left(p\right)=\frac{1}{2}a{‖p‖}^2,J_r\left(p\right)=-\frac{1}{2}bc\exp \left(-\frac{{‖p‖}^2}{c}\right)$$

(24)

For any two of the heterogeneous unmanned system, there exists a distance $R_{ij}$ such that the formation achieves the desired shape. It means that the control goal is $‖P_i^d-P_j^d‖=R_{ij}$, at this point the formation is stable and the attraction force is equal to the repulsion force. For the case discussed in this paper, consider the distance between any two unmanned systems, including the virtual leader, reaching $R_{ij}$ and ${\nabla }_{P_i}J(‖P_i-P_j‖)=0$, $J_1(P)$ and $J_2(P)$ have the unique minimum value.

In the heterogeneous multi-agent unmanned system formation control problem, the control objectives are divided into swarm aggregation and formation control. For some specific formation tasks, heterogeneous unmanned systems are required a certain relative position during the movement, which cannot be satisfied by the swarm aggregation algorithm. The fixed-shape formation control problem is to make a group of multi-agent unmanned systems with random initial positions and headings to achieve a stable form in a finite time. To achieve this goal, we employed the potential function to make the relative distance reach the desired value. The motion of each unmanned system is restricted along the combined gradient of the potentials, which has the following form,

$$\begin{array}{lll}{\nabla }_{P_i}J& =& {\nabla }_{P_i}{J}_a+{\nabla }_{P_i}{J}_r\\ {\nabla }_{P_i}{J}_a& =& \Lambda [a(P_i-P_A)/‖P_i-P_A‖]\\ {\nabla }_{P_i}{J}_r& =& -[b(P_i-P_j)\times e^{-R_{ij}^2/c}]\end{array}$$

(25)

where ${\nabla }_{P_i}{J}_a$ represents the formation attraction with the projection point of the UAV as the virtual leader, while ${\nabla }_{P_i}{J}_r$ denotes the repulsion within the formation.

2.4. Heterogeneous Muti-Agent Unmanned System Formation Assemble

The key to the formation assembly of heterogeneous multi-agent unmanned systems is the cooperative task allocation. The information flow in the unmanned system determines the controller frame. We assume that the UAV can measure its own position, attitude, and altitude. The position of each USV in the images taken by the vision sensor is served as the point of interest, which can be converted into the world coordinate system along with the center of the pixel coordinate system via the coordinate conversion. As mentioned above, since we assumed that the vision sensor mounted on the UAV will not respond to the roll and pitch of the UAV, the center of the image coincides with the projection point on the surface of the UAV. Figure 4 illustrates the information flow of a heterogeneous multi-agent unmanned system.

Based on the leader-follower formation control strategy, the formation shape is represented by the desired status according to the formation task requirements, and a set of desired coordinates is generated and transmitted to the APF path planning module to impose constraints on the USVs in the formation. The calculation of the gradients in the APF path planning module is transmitted to the sliding mode control algorithm module, which generates a series of available velocity control commands. Each unmanned system will utilize its low-level controller to implement motion control based on desired velocities.

As we assumed a vision sensor mounted on the UAV with a self-stabilizing PTZ, in other words, the camera coordinate system plane $O_C{X}_C{Y}_C$ remains parallel to the world coordinate system plane $O_W{X}_W{Y}_W$. Therefore, the coordinate of each USV in the pixel coordinate system is proportional to the actual coordinate in the world coordinate system, so that the formation assembly and the motion control will be discussed in the pixel coordinate system.

The APF method was previously described as a path planning module in heterogeneous multi-agent unmanned system formation control with the function of providing goal-oriented navigation information in optimal time. In the APF approach, we use the potential function to provide the dynamical environment in which the USVs are located. The formation has a minimum potential field when it reaches the steady desired status. As the desired shape of the formation is determined, distributed control laws are designed for each agent in the USVs formation for a given dynamics. The control law is designed to satisfy the conditions of Equation (19), which means that the formation control input has to satisfy the following condition: The velocity increases along with the negative net gradient potential, and the velocity is bounded, with the boundary depending on the USV dynamics. To satisfy the above conditions, we employed the sliding mode control method. USV is a typical class of nonlinear systems, and the sliding mode control method is one of the most effective and stable nonlinear control techniques applied to nonlinear systems. The sliding mode control method has good robustness to model uncertainties of nonholonomic robots and external disturbances so that the sliding mode control method is widely used in the controller design of USVs.

The conventional sliding mode control methods generally make a smooth transition of the control input, while a nonlinear sliding surface is used to construct the terminal sliding mode control to speed up the response and decrease the convergence time. In the case of this paper, the desired formation of a heterogeneous multi-agent unmanned system is a fixed shape formation, and the parameter that determines the final effect of formation assembly is the formation error vector. Making the smooth convergence of the formation error vector is equivalent to indirectly improving the control effect of the controller. Therefore, we choose to construct the sliding surface based on the formation error vector.

Remark 1.

Make${\mathrm{sgn}}^{\sigma }(P)={\left|P\right|}^{\sigma }sign(P),(\sigma >0)$, if$P$is an n-dimensional vector, then$sg{n}^{\sigma }(P)=[sg{n}^{\sigma }(P_1),\dots ,sg{n}^{\sigma }(P_n)]$.

The system sliding surface $S$ has the following form:

$$S=E_i+{\displaystyle {\int }_0^t({\lambda }_{1}sg{n}^{1+\frac{1}{\delta }}(E_i)+{\lambda }_{2}sg{n}^{1-\frac{1}{\delta }}(E_i))dt}$$

(26)

where $E_i$ denotes the position error of the i-th USV, $i=1,2,3,4$, ${\lambda }_i=diag\{{\lambda }_{11},\dots ,{\lambda }_{nn}\}$ is a diagonal matrix, the element ${\lambda }_{ij}>0$, and $\delta >1$. The derivative of Equation (25) yields,

$$\dot{S}={\dot{E}}_i+{\lambda }_{1}sg{n}^{1+\frac{1}{\delta }}(E_i)+{\lambda }_{2}sg{n}^{1-\frac{1}{\delta }}(E_i)$$

(27)

When the formation error reaches the sliding mode surface, as well as $\dot{S}=0$, then,

$${\dot{E}}_i=-{\lambda }_{1}sg{n}^{1+\frac{1}{\delta }}(E_i)-{\lambda }_{2}sg{n}^{1-\frac{1}{\delta }}(E_i)$$

(28)

Combining Equations (6), (13), (15), and (19), we can obtain:

$$\dot{S}=T(P_i)u_i-{\dot{P}}_A+{\nabla }_{P_i}J(P_i)+{\lambda }_{1}sg{n}^{1+\frac{1}{\delta }}(E_i)+{\lambda }_{2}sg{n}^{1-\frac{1}{\delta }}(E_i)$$

(29)

Designing fast terminal sliding mode control reaching law for sliding surface $S$ so that the system reaches the sliding surface in finite time. We fuzzified the parameter of terminal sliding mode control sliding surface [35], and a fuzzy inference mechanism(FIM) [36] is employed to simulate the sliding surface reaching law. In the case of this paper, let the sliding surface $S$ be the input linguistic variable of the FIM, $u_f$ be the output linguistic variable, we have utilized triangular input membership functions and singleton output membership function as shown in Figure 5. Since the fuzzy subset range is $S\in (-1,1)$, then the domain of the fuzzy subset $S=\{PE,ZE,NE\}$ is $S=\{-1,0,1\}$, which corresponds to the following

Table 1. Fuzzy rules base.

Fuzzy Rules	S	${\mathit{u}}_{\mathit{f}}$
rule 1	P	PE
rule 2	Z	ZE
rule 3	N	NE

of fuzzy rules base, where P, Z, and N denotes positive, zero, and negative, respectively.

For the calculation of the relevant parameters of the fuzzy output $u_f$, we use the center of gravity-based defuzzification method. Define the fire strength of the three fuzzy rules $0\le w_1,w_2,w_3\le 1$, and $w_1+w_2+w_3=1$. Then the fuzzy output $u_f$ is

$$u_f=\frac{w_1{r}_1+w_2{r}_2+w_3{r}_3}{w_1+w_2+w_3}$$

(30)

The centers of the output membership functions PE, ZE, and NE are $r_1=r_i$, $r_2=0$ and $r_3=-r_i$, respectively. The parameter $r_i$ to be tuned is the width of the output membership function. Substituting the above centers into Equation (30), we can obtain:

$$u_f=r_i\left(w_1-w_3\right)$$

(31)

Remark 2. 

For all possible values of S, there are four cases, which are$S>1$, $0<S<1$, $-1<S<0$and$S<-1$. Correspondingly, only rule 1 is activated in the first case,$w_1=1$, $w_2=w_3=0$and$u_f=r_i$; In the second case, rule 1 and rule 2 are simultaneously activated,$w_3=0$, $0<w_1,w_2<1$and$u_f=r_i{w}_1$; In the third case, rule 2 and 3 are simultaneously activated,$w_1=0$, $0<w_2,w_3<1$and$u_f=-r_i{w}_3$; In the fourth case only rule 4 is activated,$w_3=1$, $w_1=w_2=0$and$u_f=-r_i$. In summary, we can obtain:

$$S(w_1-w_3)=\left|S\right|\left|(w_1-w_3)\right|\ge 0$$

(32)

We adopted the fast terminal sliding mode control reaching law in [35]:

$$\dot{S}=-{{\rm K}}_{i}tanh(S)-{\Pi }_{i}S+D$$

(33)

where ${\rm K}$ and $\Pi$ are reaching law parameter matrixes, $D$ denotes the total external disturbance. Using parameter fuzzification to simulate the reaching law, we can finally obtain the fuzzy reaching law:

$$\dot{S}=-R_i{(w_1-w_3)}_i-{\Pi }_{i}S+D$$

(34)

where $R_i=r_{i}I$ represented the fuzzy parameter matrix. Substitute Equation (34) into Equation (28), we can obtain the control law:

$$u_i=T{(P_i)}^{-1}\left({\dot{P}}_A-{\nabla }_{P_i}J(P_i)-{\lambda }_{1}sg{n}^{1+1/\delta }(E_i)\right.\left.-{\lambda }_{2}sg{n}^{1-1/\delta }(E_i)-R_i{(w_1-w_3)}_i-{\Pi }_{i}S+D\right)$$

(35)

We reconstructed the sliding surface for the heterogeneous multi-agent unmanned system controller, and the corresponding fuzzy sliding mode control law is given by Equation (35). For the fast terminal sliding mode control law, there exist ${\mathit{R}}_i$ and ${\mathit{\Pi }}_i$ that allows the system to reach a steady status in finite time. We employed the adaptive self-tuning algorithm to tune the parameter matrixes. Define the estimate values of the parameter matrixes ${\mathit{R}}_i$ and ${\mathit{\Pi }}_i$ are ${\overline{\mathit{R}}}_i$ and ${\overline{\mathit{\Pi }}}_i$, respectively, then we can obtain the adaption errors ${\tilde{\mathit{R}}}_i={\overline{\mathit{R}}}_i-{\mathit{R}}_i$ and ${\tilde{\mathit{\Pi }}}_i={\overline{\mathit{\Pi }}}_i-{\mathit{\Pi }}_i$. To prove the finite-time stability of the system, consider the following Lyapunov candidate function:

$$V=S^{T}S$$

(36)

Taking the derivate of Equation (36), and substituting for $\dot{S}$, we can obtain

$$\dot{V}=S^T(-R_i{(w_1-w_3)}_i-{\Pi }_{i}S+D)$$

(37)

If the gains of parameter matrixes $R_i$ and ${\Pi }_i$ are updated by the adaptive law, which are ${\dot{\overline{R}}}_i={\alpha }^{-1}{(w_1-w_3)}_i{S}^T$ and ${\dot{\overline{\Pi }}}_i={\mathsf{\Omega }}^{-1}S{S}^T$, where $\alpha$ and $\mathsf{\Omega }$ are positive constants. If the sliding mode $S\dot{S}\le 0$ is to be satisfied, the values of the parameter matrixes are taken to satisfy the following inequality:

$$\begin{array}{l}{R}_i\ge \frac{D}{{(w_1-w_3)}_i}+P_i\\ {\Pi }_i\ge -\frac{{(w_1-w_3)}_i{P}_i}{S}+\frac{{[S^{T}S+n{\lambda }_{\max }(\alpha {\overline{R}}_i{\overline{R}}_i)+n{\lambda }_{\max }(\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i)]}^{\varsigma }}{S^{T}S}\end{array}$$

(38)

where n denotes the order of the parameter matrix, ${\lambda }_{\max }$ represents the maximum eigenvalue of the matrix, $\varsigma \in (0,1)$. Modify the Lyapunov candidate function as:

$$V\left(S,{\overline{R}}_i,{\overline{\Pi }}_i\right)=S{S}^T+tr\left\{\alpha {\overline{R}}_i{\overline{{\rm P}}}_i\right\}+tr\left\{\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i\right\}$$

(39)

where $tr\left\{\right\}$ denotes the trace of the matrix, take the derivate of Equation (39) and substitute for Equation (34), we can obtain:

$$\dot{V}\left(S,{\overline{R}}_i,{\overline{\Pi }}_i\right)=S^T\left(-{\overline{R}}_i{(w_1-w_3)}_i-{\overline{\Pi }}_{i}S-R_i{(w_1-w_3)}_i-{\Pi }_{i}S+D\right)+tr\left\{\alpha {\overline{R}}_i{\dot{\overline{R}}}_i\right\}+tr\left\{\mathsf{\Omega }{\overline{\Pi }}_i{\dot{\overline{\Pi }}}_i\right\}$$

Substituting the inequality (37) into the above equation:

$$\begin{array}{lll}\dot{V}\left(S,{\overline{R}}_i,{\overline{\Pi }}_i\right)& \le & S^T\left(-{\overline{R}}_i{(w_1-w_3)}_i-{\overline{\Pi }}_{i}S\right)\\ & -& {\left(S{S}^T+n\left({\lambda }_{max}\left(\alpha {\overline{R}}_i{\overline{R}}_i\right)+{\lambda }_{max}\left(\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i\right)\right)\right)}^{\varsigma }\\ & +& tr\left\{\alpha {\overline{R}}_i{\dot{\overline{R}}}_i\right\}+tr\left\{\mathsf{\Omega }{\overline{\Pi }}_i{\dot{\overline{\Pi }}}_i\right\}\end{array}$$

(40)

According to the properties of the trace of the matrix and substituting the adaptive law, the following inequality can be obtained.

$$\begin{array}{lll}\dot{V}\left(S,{\overline{R}}_i,{\overline{\Pi }}_i\right)& \le & -{\left(S{S}^T+n\left({\lambda }_{max}\left(\alpha {\overline{R}}_i{\overline{R}}_i\right)+{\lambda }_{max}\left(\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i\right)\right)\right)}^{\varsigma }\\ & +& tr\left\{\alpha {\overline{R}}_i{\dot{\overline{R}}}_i-{\overline{R}}_i{(w_1-w_3)}_i{S}^T\right\}+tr\left\{\mathsf{\Omega }{\overline{\Pi }}_i{\dot{\overline{\Pi }}}_i-{\overline{\Pi }}_{i}S{S}^T\right\}\end{array}$$

$$\dot{V}\left(S,{\overline{R}}_i,{\overline{\Pi }}_i\right)\le -{\left(S{S}^T+n\left({\lambda }_{max}\left(\alpha {\overline{R}}_i{\overline{R}}_i\right)+{\lambda }_{max}\left(\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i\right)\right)\right)}^{\varsigma }$$

(41)

It is obvious that $tr\left\{\right\}\le n{\lambda }_{\max }\left\{\right\}$, so, $tr\left\{\alpha {\overline{R}}_i{\dot{\overline{R}}}_i\right\}+tr\left\{\mathsf{\Omega }{\overline{\Pi }}_i{\dot{\overline{\Pi }}}_i\right\}\le n({\lambda }_{max}(\alpha {\overline{R}}_i{\overline{R}}_i)+{\lambda }_{max}(\mathsf{\Omega }{\overline{\Pi }}_i{\overline{\Pi }}_i))$, then $\dot{V}(S,{\overline{R}}_i,{\overline{\Pi }}_i)\le -V{(S,{\overline{R}}_i,{\overline{\Pi }}_i)}^{\varsigma }$.

Lemma 1. 

Suppose that there exists a positive-definite continuous function$V(P),P\in U_0\in R$, real numbers$k>0$, and$\alpha \in (0,1)$, so that$\dot{V}(P)+k{V}^{\varsigma }\le 0$; Then, there exists$U_0\in R$so that any$V(P)$in$U_0\in R$can reach$V(P)=0$in finite time, which is$T\le V^{1-\varsigma }(P_0)/k(1-\varsigma )$.

As per Lemma 1, the Lyapunov candidate function $V(S,{\overline{R}}_i,{\overline{\Pi }}_i)$ satisfies the inequality, then this adaptive sliding mode control law is finite-time stable. With the distributed sliding mode controller for heterogeneous multi-agent unmanned systems, the UAV-USVs heterogeneous unmanned system can converge and reach the consensus in a finite time.

3. Simulation Results and Discussion

In this part, the above-mentioned heterogeneous multi-agent unmanned system formation assembles method is simulated and verified. We used the robotic simulator CoppeliaSim to simulate the motion of heterogeneous unmanned systems in a realistic environment while using the MATLAB/Simulink platform for the design of the controller and the calculation of relevant parameters. CoppeliaSim provides a series of API functions that can be called by MATLAB to facilitate the joint simulation.

First, we build heterogeneous multi-agent unmanned system models in the CoppeliaSim platform, including dynamics models of UAVs, USVs, water surfaces, etc. Among them, the UAV carries a “hawk-eye” like view vision sensor and the USVs have different color features. The composition of the whole heterogeneous unmanned system is shown in Figure 6.

3.1. Simulation Validation

When the simulation starts running, the vision sensor starts working as shown in Figure 7. The acquired image information is called by the MATLAB platform via the API. The position of each USV in the pixel coordinate system is identified in the MATLAB platform according to the algorithm, and at this time, the actual position and the desired position are handed over to the APF function to calculate the combined gradient according to the formation shape requirement. The desired velocity of each USV is obtained using the sliding mode control method and the gradient descent method. The desired velocity is returned to the CoppeliaSim platform via an API and solved by the low-level controller for inverse kinematics, which converts the control inputs into thruster speed and rudder angle to control the model’s motion.

Then the adjacency matrix $\Lambda$ of this heterogeneous unmanned system communication topology and the corresponding Laplacian matrix L are

$$\Lambda =\left[\begin{array}{ccccc}0& 1& 1& 0& 0\\ 0& 0& 1& 1& 0\\ 0& 1& 0& 0& 1\\ 0& 1& 0& 0& 1\\ 0& 0& 1& 1& 0\end{array}\right], L=\left[\begin{array}{ccccc}2& -1& -1& 0& 0\\ 0& 2& -1& -1& 0\\ 0& -1& 2& 0& -1\\ 0& -1& 0& 2& -1\\ 0& 0& -1& -1& 2\end{array}\right]$$

The main parameters of the vision sensor include: resolution of 512 × 512 pixels, covering a two-dimensional plane of 106 m × 106 m; Near/far clipping plane is 15 m/35 m; The perspective angle is $\mathsf{\Theta }=100°$. The camera internal reference matrix is:

$$k=\left(\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right)$$

where the UAV flight altitude is 30 m, i.e., $Z_C=30\mathrm{m}$. The camera field of view is $l_x=l_y=106\mathrm{m}$, therefore, $\frac{l_x/2}{Z_C}=\tan \frac{\mathsf{\Theta }}{2}$, the internal reference $f_x=f_y=\frac{Z_C}{l_x/\mathrm{Res}}=\frac{\mathrm{Res}}{2\tan \frac{\mathsf{\Theta }}{2}}$, we can obtain the vision sensor internal reference matrix $k=\left(\begin{array}{ccc}214.81& 0& 256\\ 0& 214.81& 256\\ 0& 0& 1\end{array}\right)$.

The parameters of the UAV feedback control law are $r_1=0.1, r_2=0.6, r_3=3 \mathrm{and} r_4=2$, respectively. The APF function parameters are set as: $a=0.5, b=5, c=0.15$. The parameters of the sliding mode control law are $\alpha =10, \mathsf{\Omega }=50,000 \mathrm{and} \delta =3$. To verify the effectiveness of the heterogeneous multi-agent unmanned system formation control law, we built physical models of UAV and USVs in the CoppeliaSim platform to verify the formation assemble process under a realistic dynamics environment.

To simplify the simulation process, we set the origin $O_W$ of the world coordinate system, along with the origin $P_0^{\prime }$ of the pixel coordinate system, are coincident. The coordinate of $P_0^{\prime }$ in the pixel coordinate system is $P_0^{\prime }=(256,256)$, Located in the world coordinate system coordinates $P_0=(0,0)$. The initial positions of the USVs on the water surface are $P_1=(-20,-2.5,0)$, $P_2=(-20,7.5,0)$, $P_3=(-20,-7.5,0)$ and $P_4=(-20,2.5,0)$, respectively. UAV is directly above $P_0$, with the world coordinate system coordinate $P_A=(0,0,30)$, and the initial posture are $\alpha =0,\beta =0$. The spatial layout of the heterogeneous multi-agent unmanned system is shown in Figure 8.

The projection point of the initial position of the UAV on the horizontal plane is $P_0$, which is served as the virtual leader of the USVs. The projection point is involved in the potential field calculation. The MATLAB simulation results are shown in Figure 9.

During the simulation time, the reference trajectory of the UAV is known and the controller commands the UAV to move in a fixed altitude trajectory. The projection point of the UAV on the surface serves as the virtual leader of the formation with a reference trajectory of $P_A=(t,\sin t,0)$. The task objective of the USVs is to reach a leader-following consensus along with the virtual leader in a finite time. The formation shape is designed as an isosceles right triangle with the projection point as the vertex. The formation position error is shown in Figure 10.

When the formation position error is within the interval of [−0.005 m, 0.005 m], we consider that the formation assemble is completed and reaches the consensus. We can easily obtain the convergence time of 32.59 s from Figure 10.

In the MATLAB platform, the controller can easily get the current position information of each unmanned system for the calculation of trajectory points, which means the controller makes a collision-free path planning according to the task requirements and generates a set of reference trajectory points consisting of coordinates and velocities. We transmit such information to the heterogeneous multi-agent unmanned system dynamics models in the CoppeliaSim platform through a remote API and use the inverse kinematics solution to obtain the speed, rudder angle, and other parameters needed by the low-level controller. During the movement of the model, the images acquired by the vision sensor are returned to the MATLAB platform for processing via the remote API, and the actual current position of each USV is calculated based on the RGB information of the returned images, which is updated to the controller for path re-planning. The simulation process is shown in Figure 11 and Figure 12.

The image acquired by the vision sensor is transmitted to MATLAB through the API, and the coordinates of the USVs in the pixel coordinate system are obtained in real-time based on the RGB information of the image. Then the actual coordinates in the world coordinate system are obtained through coordinate converting in Equation (5) and transferred to the controller for motion planning. The controller calculates the angular and linear velocities of each USV as the control output, as shown in Figure 13.

It can be seen from Figure 13. that the control output varies by a large amount at the beginning of the simulation, while the USV has a minimum turning radius and a maximum linear velocity due to the dynamics. So, we set the range of the angular velocity control output as $w_i\in (-10,10)\mathrm{rad}/\mathrm{s}$. and the range of the linear velocity control output as $v_i\in (0,1.5)\mathrm{m}/\mathrm{s}$ to prevent the low-level controller from failing due to the overflow of the control input. The USVs formation reaches a leader-following consensus after a period of simulation. We introduce external disturbances caused by random waves on the surface, and we do not require the a priori knowledge of disturbance bounds because of the FIM used to tune the sliding mode control reaching law parameter matrixes. The formation position tracking error of USVs under the effect of external disturbance is shown in Figure 14.

3.2. Discussion

In the MATLAB platform, the process of UAV-USV heterogeneous unmanned system cooperative formation assemble is shown in Figure 9. In this process, the projection point of the UAV on the surface serves as the virtual leader, and the controller calculates the reference trajectory of each USV based on the reference trajectory of the projection point to reach the leader-following consensus. the controller generates a set of desired velocities and solves the coordinates in the world coordinate system according to the kinematic model.

Figure 10 shows the USVs formation position error in the world coordinate system. From the figure, it can be seen that the position error converges to zero after a period, which in this case is 32.59 s. This means that the USV formation can be formed in a finite time. Previous studies on USV formation control have focused on the asymptotic convergence of the formation as time tends to infinity. When the finite-time theory was proposed, further studies focused on how to optimize the system convergence time. In the related literature, the prescribed performance method [37,38], the dynamic surface control method [39], and the disturbance observer method [40] have been used to reduce the convergence time. For nonlinear dynamical systems, which usually have model uncertainty and time-varying external disturbances, the sliding mode control method exhibits extremely high robustness in navigation and control. When the non-singular terminal sliding mode control method is employed, the system becomes more insensitive to model uncertainty and external disturbances, allowing the system gain to converge fast and stably.

Figure 11 shows a “hawk eye” like view of 7 discrete moments, with 4 USVs in the field of view having different color characteristics. From the RGB information, we first calculate the pixel coordinates at each moment and then transform the coordinates into the world coordinate system, at which point we obtain the actual positions of the USVs at each moment. Finally, the actual positions of the USVs are updated to the APF path planning module for path replanning. Based on this, the number of onboard sensors, such as positioning systems, rangefinders, and vision sensors, can be greatly reduced. This is beneficial for the design and manufacturing of USVs, as the complexity of the system is reduced, with the consequent increase in robustness and cost reduction.

We can see the optimized simulation results in Figure 12, which is due to the idealized modification of the system scheme, such as the UAV moving at a fixed altitude, the vision sensor always perpendicular to the horizontal plane, and the absence of other disturbing objects on the water surface. The above-mentioned features have some limitations in practical applications, such as uncertain external disturbances that cause pose changes of the vision sensor, and the accuracy of RGB color space feature recognition under different water environments and lighting conditions, which deserve further investigation.

4. Conclusions

In this paper, we proposed a distributed consensus method for heterogeneous multi-agent unmanned systems consisting of UAVs and USVs. A leader-following consensus scheme and APF method are used to construct UAV-USVs heterogeneous unmanned system formation shape. We employed the sliding mode control method to make the formation assemble in a finite time. The effectiveness of the proposed approach has been validated through simulation studies. By appropriately setting the parameters, each USV of the group is collision-free and move in consensus with the virtual leader. A novel vision-based controller parameter update method is proposed such that heterogeneous unmanned system formation can perform formation assemble tasks under weak communication conditions. The novel FIM scheme is designed in such a way that the finite-time convergence property of sliding mode control has been retained.

The approach proposed in this paper does not require the support of a large number of sensors for USVs, which can greatly reduce the design and manufacturing costs of USVs. The algorithm will be extended in a more cluttered environment such as static obstacles and other moving ships conditions.