Distributed Model Predictive Consensus Control of Unmanned Surface Vehicles with Post-Verification

Abstract

In this paper, the consensus control of unmanned surface vehicles (USVs) is investigated by employing a distributed model predictive control approach. A hierarchical control structure is considered during the controller design, where the upper layer determines the reference signals of USV velocities while the lower layer optimizes the control inputs of each USV. The main feature of this work is that a post-verification procedure is proposed to address the failure states caused by local errors or cyberattacks. Each USV compares the actual state and the predicted one obtained at the previous moment. This allows the estimation of local perturbations. In addition, the failure state of the USV can also be determined if a preset condition is satisfied, thus forcing a change in the communication topology and avoiding further impact. Simulations show that the proposed method is effective in USV formation control. Compared with the method without post-verification, the proposed approach is more robust when failure states occur.

1. Introduction

In recent years, the cooperative control of unmanned surface vehicles (USVs) has attracted considerable attention in systems and ocean engineering fields, broadening their applications in territorial surveillance, marine rescue, environmental detection, etc. When USVs cooperate as a formation [1], they can improve efficiency, reduce costs and switch the communication topology to provide redundancy in the event of the failure of any single USV. Formation control mainly solves two problems, namely formation composition and formation maintenance. Formation maintenance is the core feature of formation control, and is the main task of the USV navigation stage. The currently available formation control methods are primarily based on the leader–follower strategy [2,3], behavior-based method [4,5] and virtual structure method [6,7,8,9,10].

Among these control schemes, the leader–follower strategy has garnered more practical attention due to its simplicity and practicality; the collaborative control of USVs has been reported in many excellent works under the framework of a leader–follower strategy. In [11], to address the formation control problem of underactuated USVs, consistency theory and the leader–follower method were used along with the adjustment of the control input such that each USV converged to the reference value, thereby ensuring the expected formation of the USVs. Park and Yoo [12] deduced the performance functions of connection maintenance and collision avoidance for a nonlinear USV model, and completed a robust leader–follower formation tracking design. In [13], under the influences of USV input overload and external interference, the back-stepping method was used to improve formation control accuracy.

Formation control is generally divided into three categories based on the control method under the leader–follower strategy: centralized [14,15,16], decentralized [17,18,19,20,21] and distributed control [22]. In centralized control, there is a need to collect all information from USVs, and the large scale of the system increases the burden of online computing, which makes it more difficult to ensure real-time performance. Different from centralized control, decentralized control has no information exchange between the controllers, and coupling is ignored such that the vehicles may not achieve cooperative control in most cases. Distributed control allows full autonomy of the USVs, in addition to having the characteristics of low computational burden, strong fault tolerance, robustness and scalability; hence, this formation control method is more suitable for large-scale systems with a greater number of nodes, and it has attracted significant attention from scientists and researchers [23,24,25,26,27].

Although great progress has been achieved in the distributed formation control of USVs, the problem involves multi-input, multi-output systems with constraints and coupled tasks, making it inherently difficult [28,29,30,31,32]. Fortunately, distributed model predictive control (DMPC) has significant advantages in handling this class of problems [33,34]. The DMPC method combines model predictive control and distributed control principles organically, and is thus robust and flexible [35,36]. Hence, it is an ideal tool for USV formation control. DMPC has been used to investigate the formation control of vehicles. Zhao et al. [37] proposed a DMPC method for multi-quadrotor unmanned aerial vehicles (UAVs) to address the formation and maintenance problem during a cruise flight. The cost function was designed by introducing the assumed state trajectories of the local vehicle and its neighbors. Zhen et al. [38] presented a DMPC approach for vehicle platoons with one-way topologies. The terminal constraint based on the adjacent average value was introduced into the cost function, and then the closed-loop stability was proved. Zhao et al. [39] proposed a novel distributed coordinated control scheme on the grounds of heterogeneous UAVs to achieve formation control. Further results were reported by Fan et al. [40], who proposed a formation control strategy based on a hierarchical DMPC strategy, in which the upper layer guarantees the leader–follower cooperation between the unmanned vessels while the lower layer allows the unmanned vessels to track optimal instructions. Note that DMPC employs a receding horizon optimization during the implementation, which provides distributed agents with local prediction capabilities; in other words, “assumed” states are available to each agent. The assumed states play important roles in the formation control. They reflect local disturbances, since the actual states usually differ from the assumed ones. Furthermore, some failure states caused by local errors or cyberattacks that are fatal to formation control may also be determined if local predictions can be fully utilized. These have not been well studied in the existing literature, and thus motivated our research. In this paper, a hierarchical DMPC approach with post-verification is proposed for USVs with actuation constraints. Since the inputs of USVs are usually not coupled, the upper layer mainly considers communication interactions between USVs and generates the optimal references for the lower layer according to the kinematic model. The lower layer predictive controller is designed based on the dynamic model to ensure the tracking performance. The local predictions are employed in three aspects. First, the states within the prediction horizon are sent to the neighbors for the purpose of formation control. Second, the assumed states of a USV are incorporated in its local cost function, which improves its control performance when local disturbances exist. Third, the differences between the actual states and the assumed states are sent to the neighbor in the post-verification process, which renders compensations for the local predictions to be used by the neighbors. Moreover, failure states are determined once the differences satisfy a preset condition, in which case the communication topology is forced to change to an alternative one, where the affected USV does not send information to other agents. In this way, the USVs can still maintain the desired formation, avoiding the phenomenon of formation disorder caused by failure states. Under the directed and time-invariant communication topology, the upper-layer controller was considered based on the kinematic model of the USV to generate the optimal references and the lower-layer controller was designed using the dynamic model to ensure the tracking performance. By comparing the actual state with the predicted state obtained at the previous time, the post-verification process can reduce the trajectory deviation caused by external disturbances, which can increase the speed of forming the required formation. In this study, a backup topology is also designed. When the post-verification process meets the preset conditions, it is judged that the communication information between the two USVs (communicating according to communication topology $\mathcal{G}$) is wrong or interrupted. At this time, the topology is forced to change from the normal state to the fault state, so that the USVs can keep the desired formation while tracking their respective expected paths, improving the phenomenon of formation disorder due to communication failures.

The main features of the proposed approach are summarized as follows.

• The USV formation control method has strong robustness with respect to external disturbances. This is because the local predictions are used by both the single USV and its neighbors in constructing cost functions. Therefore, it is suitable for USVs that operate in extreme conditions, such as strong winds.
• The proposed approach can deal with fatal errors that exist in a single USV or at the communication level, since the post-verification process compares the actual states with the assumed ones, which enable it to determine the failure states in time, reducing the formation error and maintaining the formation shape. Although USVs can be equipped with advanced communication technologies [41,42], it is still important to adapt active fault-tolerant control strategies.

The remainder of this manuscript is organized as follows. In Section 2, the mathematical models of a single USV and the formation system are established; in Section 3, the predictive formation control strategies of the upper DMPC-based and lower MPC-based controllers are introduced; in Section 4, the algorithmic flow of the control strategy is presented; Section 5 shows the simulation examples; in Section 6, a summary of this work is provided.

2. Problem Statement

2.1. Modeling of USV

Consider a group of multiple USVs, labeled 1 to 6. For each USV, the kinematic model is expressed as follows:

$$\begin{array}{c}{\dot{x}}_i=u_{i}cos{\left(\phi \right)}_i-v_{i}sin{\left(\phi \right)}_i,\hfill \\ {\dot{y}}_i=u_{i}sin{\left(\phi \right)}_i+v_{i}cos{\left(\phi \right)}_i,\hfill \\ {\dot{\phi }}_i=r_i,\hfill \end{array}$$

(1)

$$\mathit{M}\dot{\nu }+\mathit{C}\left(\nu \right)\nu +\mathit{D}\nu =\mathit{G}\mathit{u},$$

(2)

where ${[x_i,y_i]}^T$ is the position and ${\phi }_i$ is the heading angle of each USV. The vector $\nu$ is defined as $\nu =[u_i,v_i,{\mathrm{r}}_i]$, where ${[u_i,v_i]}^T$ is the linear velocity on the X- and Y-axes, and $r_i$ is the angular velocity of the rudder. $\mathit{M}={\mathit{M}}^T>0$ is the inertia matrix including added mass; the matrix $\mathit{C}\left(\nu \right)$ is the Coriolis and centripetal forces and moments; the matrix $\mathit{D}$ is the fluid damping matrix. The vector $\mathit{u}={\left[{\tau }_u{\tau }_r\right]}^T$ is the control input corresponding to propeller thrust and the rudder deflection angle; the matrix $\mathit{G}$ is a $3\times 2$ matrix. The dynamic model of each USV is stated as follows:

$$\begin{array}{c}\begin{array}{c}m(\dot{v}-ur)=X_h+{\tau }_u+f_u,\hfill \\ m(\dot{u}+vr)=Y_h+\lambda {\tau }_r,\hfill \\ E_z\dot{r}=Z_h+\gamma {\tau }_r+f_r,\hfill \end{array}\hfill \end{array}$$

(3)

where ${(·)}_h$ are the hydrodynamic forces and moment; $\lambda$ and $\gamma$ are rudder coefficients connected with speed and yaw moment, respectively; m and $E_z$ are the vehicle mass and mass moment of inertia, respectively; ${\tau }_r$ and ${\tau }_u$ are the rudder deflection angle and propeller thrust, respectively; u and v are the linear velocities (X-axis and Y-axis), and r is the angular velocity; and $f_u$ and $f_r$ are time-invariant current disturbances: $f_u=5(sin\left(1\right)+cos(1+\pi /2))$, $f_r=2(sin\left(1\right)+cos(1+\pi /2))$.

2.2. Communication Topology

The directed graph $\mathcal{G}=\{\mathcal{V},\mathcal{E}\}$ is used to describe the communication topology of the formation system, where $\mathcal{V}=\left\{1,\dots ,N_v\right\}$ represents the set of nodes in the graph $\mathcal{G}\left(t\right)$; $\mathcal{E}\subseteq \mathcal{V}\times \mathcal{V}$ represent the set of edges. $\mathcal{A}=\left[a_{ij}\right]\in {\mathbb{R}}^{N\times N}$ is the set of time-varying adjacency matrix, which describes the communication among the followers. Edge $(i,j)$ represents that node i is able to obtain the status information of node j, and we say that node j is the neighbor of node i. If $(j,i)\in \mathcal{E}$, then $a_{ij}=1$; otherwise, $a_{ij}=0$. Using ${\mathbb{N}}_i=\{j\mid (j,i)\in \mathcal{E}\}$ to represent the neighbor set of node i, ${\mathbb{O}}_i=\{j\mid (i,j)\in \mathcal{E}\}$ is used to represent the collection of nodes that can obtain the information about node i. The Laplacian matrix $\mathcal{L}\in {\mathbb{R}}^{N\times N}$ is defined as $\mathcal{L}=\mathcal{D}-\mathcal{A}$, where $\mathcal{D}\in {\mathbb{R}}^{N\times N}$ is named the degree matrix, and defined as $\mathcal{D}=diag\left\{d_1,\dots ,d_N\right\}$ with $d_i={\sum }_{j=1}^N{a}_{ij},i=1,\dots ,N$.

2.3. Control Objective of Multi-USV Systems

Considering the desired formation control problem as demonstrated in Figure 1, USV${}_1$ is the leader ship, USV${}_2$− USV${}_6$ are the follower ships and each USV follows a preset formation. The formation requirements are as follows: (1) each USV tracks a provided straight trajectory; (2) according to the formation matrix, the distance between USVs can be calculated, and the specific formation matrix is defined in the simulation example; (3) each USV can exchange position information with the adjacent USVs via communication networks; (4) if any communication networks fail, standby communication networks can be quickly switched to maintain the desired formation.

3. Design of Distributed Model Predictive Controller

This section first introduces the controllers used by the upper and lower structures of the formation system; the upper layer has a DMPC-based controller designed using the kinematic model, and the lower layer has an MPC-based controller designed using the dynamic model for tracking the optimal control input of the upper layer, as shown in Figure 2. To facilitate the demonstration, six USVs are considered. The upper layer obtains the formation (i.e., triangle formation) through the communication topology. The distributed MPC scheme is used to solve for the optimal solution and provides formation commands to each USV${}_i$. The task of the lower controller is to track formation commands and achieve optimal output.

3.1. Upper-Layer Distributed MPC

The same length of predictive horizon $N_P$ is used in all local problems. Over the prediction horizon $\left[t,t+N_p\right]$, the used states and input symbols are listed in

Table 1. States and input symbols.

Symbol	Variable Name
$u_i^P$	Predicted control input
$u_i^{*}$	Optimal output input
$u_i^a$	Assumed control input
$Z_i^p$	Predicted state trajectory
$Z_i^{*}$	Optimal state trajectory
$Z_i^a$	Assumed state trajectory
$Z_{-\tilde{i}}^a$	Assumed neighbor state trajectory

, where $k=0,1,\dots N_P$ and the assumed state trajectory $z_i^a\left(t\right)$ is essentially the optimal state trajectory $z_i^{*}(t-1)$ obtained by solving the optimization problem at time $t-1$.

Now we define the optimal control problem for each node $i\in \{1,2,\dots ,N\}$ at time t.

Problem ${\mathcal{F}}_i$:

$$\begin{array}{cc}\hfill & \underset{u_i^p(0\mid t),\dots ,u_i^p\left(N_p-1\mid t\right)}{min}{J}_i\left(z_i^p,u_i^p,z_i^a,z_{-\tilde{i}}^a\right)\hfill \\ \hfill =& \sum _{k=0}^{N_p-1}{l}_i\left(z_i^p(k\mid t),u_i^p(k\mid t),z_i^a(k\mid t),z_{-\tilde{i}}^a(k\mid t)\right),\hfill \end{array}$$

(4)

subject to:

$$\begin{array}{cc}\hfill z_i^p(k+1\mid t)& ={\varphi }_i{z}_i^p(k\mid t)+{\psi }_i{u}_i^p(k\mid t),\hfill \end{array}$$

(5a)

$$\begin{array}{cc}\hfill z_i^p(0\mid t)& =z_i\left(t\right),\hfill \end{array}$$

(5b)

$$\begin{array}{cc}\hfill u_i^p(k\mid t)& \in {\mathcal{U}}_i,\hfill \end{array}$$

(5c)

$$\begin{array}{cc}\hfill Z_i^p(Np\mid t)& =Z_{des,i}^p(Np\mid t),\hfill \end{array}$$

(5d)

where $u_i^p(0\mid t),\dots ,u_i^p\left(N_p-1\mid t\right)$ denotes the unknown variables to be optimized; ${\mathit{z}}_{-\tilde{i}}=\left\{{\mathit{z}}_{-\tilde{i}}\mid (j,i)\in \mathcal{E}\right\}$ represents the set of neighbor states of the USV${}_i$; constraint (5a) shows the constraint from USV kinematics; constraint (5b) indicates that the current state at time t is taken as the time initial state of the optimization problem; constraint (5c) denotes the control input set and ${\mathcal{U}}_i=\left\{u_{min,i}\le u_i\le u_{max,i}\right\}$, where $u_{min,i}$ and $u_{max,i}$ are the bounds; terminal equality constraint (5d) represents the terminal equality constraint of USV${}_i$; and the state predicted by the last step at time t is equal to the desired state, which is mainly used to ensure the gradual stability of the system.

The function $l_i$ in (4) is the cost bounded with node i, defined as:

$$\begin{array}{cc}\hfill & l_i\left(z_i^p(k\mid t),u_i^p(k\mid t),z_i^a(k\mid t),z_{\tilde{i}}^a(k\mid t)\right)\hfill \\ \hfill & ={∥z_i^p(k\mid t)-z_{\mathrm{des},i}(k\mid t)∥}_{Q_i}+{∥u_i^p(k\mid t)-{\mathcal{V}}_{\mathrm{des},i}∥}_{R_i}\hfill \\ \hfill & +{∥z_i^p(k\mid t)-z_i^a(k\mid t)∥}_{F_i}+\sum _{j\in {\mathbb{N}}_i}{∥z_i^p(k\mid t)-z_{-\tilde{i}}^a(k\mid t)-{\tilde{d}}_{i,j}∥}_{G_i},\hfill \end{array}$$

(6)

where ${\parallel \mathit{z}\parallel }_{\mathit{M}}\triangleq {\left({\mathit{z}}^{\mathrm{T}}\mathit{M}\mathit{z}\right)}^{1/2}$, and $Q_i,R_i,F_i$ and $G_i$ are the weighting matrices. In problem ${\mathcal{F}}_i$, the cost function (6) contains four items: (1) The first item corresponds to the penalty of the weight matrix $Q_i$ for the deviation between the predicted state and the target state of USV${}_i$, which represents the expectation of reaching the target state as soon as possible; $z_{\mathrm{des},i}(k\mid t)={\left[x_{\mathrm{des},i}(k\mid t),y_{\mathrm{des},i}(k\mid t),{\phi }_{\mathrm{des},i}(k\mid t)\right]}^{\mathrm{T}}$ present the target status of USV${}_i$. (2) The second item corresponds to the penalty of the control input with the weight matrix $R_i$, which represents that USV${}_i$ prefers uniform motion. ${\mathcal{V}}_{\mathrm{des},i}={\left[u_{\mathrm{des},i,},v_{\mathrm{des},i,},r_{\mathrm{des},i,}\right]}^{\mathrm{T}}$ is the desired speed, which is a constant. (3) The third item corresponds to the penalty of the weight matrix $F_i$ for the deviation (or self deviation for short) between the predicted track and the assumed track of USV${}_i$. (4) The fourth item corresponds to the penalty for the deviation between the predicted track of USV${}_i$ and its neighbor’s assumed track., The offset is ${\tilde{d}}_{i,j}$ (or referred to as neighbor bias for short), and the weight matrix is $G_i$, which indicates that USV${}_i$ should keep the expected bias running with the assumption state of neighbor nodes as much as possible. The third and fourth terms are called the self-bias penalty and neighbor-bias penalty, respectively, while $F_i$ and $G_i$ are called self-bias weight and neighbor-bias weight, respectively.

At the same time, all USVs in the formation system solve and update the optimization problems synchronously. From the above analysis, it is seen that the single-node optimization problem only uses the assumptions of neighboring node state information and that there is no global state information; thus, this is an innate problem of the distributed model predictive control scheme.

3.2. Lower-Layer MPC

After calculating by the DMPC formation controller, the optimal control input $u_i^{*}(k\mid t)$ at time t for each USV${}_i$ will be achieved, and the lower-layer MPC controller will take optimal control input as the tracking reference $U_r={\mathbf{u}}_i^{*}(k\mid t)$. So, we define the lower-layer optimal control problem:

Problem ${\mathcal{P}}_i$:

$$\begin{array}{ccc}\hfill & \underset{w_i^p(0\mid t),\dots ,w_i^p\left(N_p-1\mid t\right)}{min}J\left(v_i^p,w_i^p\right)=\hfill & \hfill \sum _{k=0}^{N_p-1}l\left(v_i^p(k\mid t),w_i^p(k\mid t)\right),\end{array}$$

(7)

subject to:

$$\begin{array}{cc}\hfill {\mathbf{v}}_i(k+1\mid t)=& G{\mathbf{v}}_i(k\mid t)+H{\mathbf{w}}_i(k\mid t),\hfill \end{array}$$

(8a)

$$\begin{array}{cc}\hfill w_i^p(k\mid t)& \in {\Omega }_i,\hfill \end{array}$$

(8b)

where $w_i^p(0\mid t),\dots ,w_i^p\left(N_p-1\mid t\right)$ denotes the unknown variables to be optimized; $w_i^P(k\mid t)$ and $v_i^P(k\mid t)$ are the predictive control input and predictive state trajectory of the lower-layer controller, respectively; constraint (8a) shows the constraint from USV dynamics; constraint (8b) represent the control input set and ${\Omega }_i=\left\{w_{min,i}\le w_i\le w_{max,i}\right\}$, where $w_{min,i}$ and $w_{max,i}$ are the bounds.

The function l in (7) is designed as:

$$l\left({\mathbf{v}}_i^p(k\mid t),{\mathbf{w}}_i^a(k\mid t)\right)=\alpha {∥{\mathbf{v}}_i^p(k\mid t)∥}^2+\beta {∥{\mathbf{w}}_i^p(k\mid t)∥}^2,$$

(9)

where $\alpha >0$ represents the intensity to penalize the output error from the desired state and $\beta >0$ represents the intensity to penalize the input error deviated from the desired state.

4. Implementation of the Algorithm

Based on the control strategy proposed above, the algorithm flow of the two-layer distributed model predictive control is as described in the following steps 1–5:

Step 1: According to the communication topology $\mathcal{G}$, all USVs are given the expected relative state information ${\tilde{d}}_{i,j}$ of other USVs.

Step 2: Initialization—at time $t=0$, assuming that all USVs are in uniform motion during operation, for each USV take its current state as the initial value of the prediction state in the prediction time domain at this time, that is, $z_i^p(0\mid 0)=z_i\left(0\right)$, the assumed control inputs and assumed state are defined as

$$\left\{\begin{array}{c}{u}_i^a(k\mid 0)=v_i\left(0\right)\hfill \\ z_i^a(k\mid 0)=z_i^p(k\mid 0)\hfill \end{array},k=0,1,\dots ,N_p-1.\right.$$

(10)

where $z_i^p(k+1\mid 0)={\varphi }_i{z}_i^p(k\mid 0)+{\psi }_i·u_i^p(k\mid 0)$, $k=0,1,\dots ,N_p-1.$

Step 3: At any time t$(t⩾1)$, for each USV${}_i$ in the upper layer:

(1) Obtain the expected state trajectory of USV${}_i$ (leader ship) directly or indirectly according to the communication topology, and according to the pre-installed expected relative state information ${\tilde{d}}_{i,j}$, solve the desired state trajectory $Z_{\mathrm{des},i}\left(t\right)=\{z_{\mathrm{des},i}(k\mid t)\mid k=0,\dots ,N_p\}$.

(2) Optimize the problem ${\mathcal{F}}_i$ according to its current state $x_i\left(t\right)$, self assumed state and neighbor assumed state trajectory, and obtain the optimal control input sequence $u_i^{*}(k\mid t),k=0,1,\dots ,N_p-1$.

(3) Calculate the optimal state trajectory within the prediction range using the optimal control sequence:

$$\begin{array}{ccc}\hfill z_i^{*}(k+1\mid t)& ={\varphi }_i\left(z_i^{*}(k\mid t)\right)+{\psi }_i·u_i^{*}(k\mid t),k\hfill & \hfill =0,1,\dots ,N_p-1,\\ \hfill z_i^{*}(0\mid t)& =z_i\left(t\right).\hfill \end{array}$$

(11)

(4) Compute the assumed control $u_i^a(k\mid t+1)$ for the next step by discarding the first term and adding one expected input term; the diagram of the synchronously updating algorithm is shown in Figure 3.

$$\begin{array}{ccc}\hfill & u_i^a(k\mid t+1)\hfill & \hfill =\left\{\begin{array}{c}{u}_i^{*}(k+1\mid t),k=0,1,\dots ,N_p-2\hfill \\ v_{\mathrm{des},i},k=N_p-1\hfill \end{array}\right.,\end{array}$$

(12)

the corresponding assumed output is also calculated as:

$$\begin{array}{cc}\hfill z_i^a(k+1\mid t+1)& ={\varphi }_i\left(z_i^a(k\mid t+1)\right)+{\psi }_i{u}_i^a(k\mid t+1),\hfill \\ \hfill z_i^a(0\mid t+1)& =z_i^{*}(1\mid t).\hfill \end{array}$$

(13)

i.e.,

$$\begin{array}{ccc}\hfill & z_i^a(k\mid t+1)\hfill & \hfill =\left\{\begin{array}{c}{z}_i^{*}(k+1\mid t),k=0,1,\dots ,N_p-1\hfill \\ z_{\mathrm{des},i}(k\mid t+1),k=N_p\hfill \end{array}\right..\end{array}$$

(14)

(5) According to the communication topology $\mathcal{G}$, the assumed state trajectory $Z_i^a(t+1)$ is sent to the USV that can receive its information, and at the same time, the assumed state trajectory of its neighbors $z_{-\tilde{i}}^a(k\mid t+1)$ is received.

(6) The first element of the optimal control sequence is used to implement the control effort, i.e., $u_i\left(t\right)=u_i^{*}(0\mid t)$, conveying $u_i^{*}(0\mid t)$ to its lower-layer control system as reference.

(7) Post-verification process: The error between the Y-axis outputs of the assumed and actual output states $e_i=y_i^a(t-1)-y_i\left(t\right)$ are obtained. Then, this error is compensated for the actual output $y_i(t+1)$ of the system at the next moment. Considering the absolute value of the error $E_i=\left|e_i\right|$, if $E_i⩾\sigma$, take $\sigma =0.5$ cm in the simulation, it is considered that the communication information between USVs is determined to be wrong or interrupted. At this time, the communication topology changes from normal to error states.

Step 4: At any time t$(t⩾1)$, for each USV${}_i$ in the lower-layer:

(1) Obtain the reference $U_r=u_i^{*}(0\mid t)$.

(2) Solve the optimization problem ${\mathcal{P}}_i$, yielding the optimal control sequence $w_i^{*}(k\mid t),k=0,1,\dots ,N_p-1$.

(3) Apply the first element of the optimal control sequence $w_i^{*}(0\mid t)$ to the lower-layer control system. Calculate the optimal state trajectory within the prediction range using the optimal control sequence ${\mathbf{v}}_i^{*}(k+1\mid t)=G{\mathbf{v}}_i^{*}(k\mid t)+H{\mathbf{w}}_i^{*}(k\mid t),k=0,1,\dots ,N_p-1$.

Step 5: At time $t+1$, repeat the above steps.

5. Results and Discussion

5.1. Simulation Setup

The adopted communication topology is shown in Figure 4a. USV${}_1$ is selected as the leader, and the arrow indicates the signal transmission direction, that is, the information of the front-end USV (expected state trajectory, assumed state trajectory) can be obtained by the back-end USV. As shown in Figure 4b, when the communication between USV${}_2$ and USV${}_4$ fails, USV${}_4$ can receive signals from USV${}_5$. As shown in Figure 4c, when the communication between USV${}_3$ and USV${}_6$ fails, USV${}_6$ can receive signals from USV${}_5$ and continue to move on in the desired formation.

In the simulation, the system composed of six USV${}_s$ is equivalent to a homogeneous multi-agent system, so each USV has the same parameters, as follows: $m=30$ kg, $u=1\mathrm{cm}·{\mathrm{s}}^{-1}$, $X_t=0$, $Y_t=0$, $N_t=0$, $\lambda =0.02$, $\gamma =-0.01$ and $E_z=0.3{\mathrm{kgm}}^2$.

Table 2. Initial and expected relative states.

Number	Initial State	Expected Relative State ${\tilde{\mathit{d}}}_{\mathit{i},\mathit{j}}$
USV${}_1$	${(-2,8,1)}^{\mathrm{T}}$	(0, 0,−1,1,−1,−1,−2, 2,−2, 0,−2,−2)
USV${}_2$	${(-2,9,1)}^{\mathrm{T}}$	(1,−1, 0, 0, 0,−2,−1, 1,−1,−1,−1,−3)
USV${}_3$	${(-2,7,1)}^{\mathrm{T}}$	(1, 1, 0, 2, 0, 0,−1, 3,−1, 1,−1,−1)
USV${}_4$	${(-2,10,1)}^{\mathrm{T}}$	(−2, 2,−1, 1,−1, 3, 0, 0, 0, 2, 0, 4)
USV${}_5$	${(-2,8,1)}^{\mathrm{T}}$	(−2, 0,−1,−1,−1, 1, 0,−2, 0, 0, 0, 2)
USV${}_6$	${(-2,6,1)}^{\mathrm{T}}$	(−2,−2,−1,−3,−1,−1,0,−4,0,−2, 0, 0)

lists the the initial state of USVs in the inertial system as well as the expected relative state between USVs, where ${\tilde{d}}_{i,j}=z_{des,i}-z_{des,j}$, the first and second items of matrix ${\tilde{d}}_{1,j}$ mean the expected relative state information between USV${}_1$ and itself; similarly, the third and fourth items mean the expected relative state information between USV${}_1$ and USV${}_2$, and so on. As the expected relative state between two UAVs can be obtained by ${\tilde{d}}_{i,j}$, it is not listed.; the remaining simulation parameters are shown in

Table 3. Parameters used in the USV DMPC simulation.

Parameter	Symbol	Numerical Value	Weight	Symbol	Numerical Value
Upper Layer
Number of USVs	$N_s$	6	State	$Q_i$	1$I_3$
Sampling time	$T_u$	2s	Control input	$R_i$	1$I_3$
Prediction horizon	$N_p$	5	Assumed state	$F_i$	1$I_3$
Control horizon	$N_c$	4	Formation	$G_i$	1$I_3$
Lower Layer
Simulation steps	$N_r$	50	State	$down{Q}_i$	1$I_3$
Sampling time	$T_d$	2s	Control input	$down{R}_i$	1$I_3$

. Theoretically, each USV can obtain its own expected state trajectory based on the expected relative state with other USVs. The maximum value of the control input of a USV in the kinematic model is $2\mathrm{cm}·{\mathrm{s}}^{-1}$ and the minimum value is set to $-2\mathrm{cm}·{\mathrm{s}}^{-1}$; similarly, the maximum and minimum values of the control input of the USV in the dynamic model are set to $1\mathrm{cm}·{\mathrm{s}}^{-1}$ and $-1\mathrm{cm}/\mathrm{s}$, respectively.

5.2. Result Analysis

In order to verify that the algorithm can complete the first three control objectives proposed in Section 1, we ran the control algorithm designed in this paper to obtain Figure 5, Figure 6, Figure 7, Figure 8 and Figure 9. From these figures, it can be seen that the algorithm designed in this paper could achieve the desired control objectives, which verifies the effectiveness of the algorithm.

Figure 5 shows the trajectory in plane coordinates. Scatter points are drawn once every 14 steps. From the trajectory, six USVs could reduce the formation spacing to achieve triangle formation flying with good formation maintenance effect.

Figure 6 shows the output error of the upper-layer control system. It can be seen from the figure that the algorithm could quickly eliminate the upper-level output error. At about step 15, the error in the three degrees of freedom directions was basically zero.

Figure 7 shows the control input of the upper layer, which is the first term of the optimal control sequence $u_i^{*}(0\mid t)$ obtained by solving the DMPC-based controller every time. From the simulation results, the control input met various constraints, and all six USV${}_S$ converged their speed to the expected value at about 15 steps.

Figure 8 shows the control output of the lower layer. By comparing Figure 8 and Figure 9, it can be seen that the speed response had a certain time delay, but it was within an acceptable range.

Figure 9 shows the control input of the lower layer, which is the first term of the optimal control sequence $w_i^{*}(0\mid t)$ obtained by solving the MPC-based controller each time.

To verify that the algorithm can achieve the fourth point of the control goal, the following simulation was conducted. In the 17th to 30th steps of the simulation, we manually disrupted communication between USV${}_2$ and USV${}_4$ and used the communication topology in Figure 4b. Similarly, in steps 30 to 45, we manually removed the communication between USV${}_3$ and USV${}_6$ and used the communication topology in Figure 4c. The system was observed to switch the topology immediately to maintain the original formation without the influence of time delay.

The algorithm proposed by Equation (6) is shown in Figure 10, which we call Algorithm 1. Figure 11 does not depict the hypothetical state in the objective function, that is, it does not include the cost of the predicted state deviation from the hypothetical state (item 3 of Equation (6)) or the formation cost (item 4 of Equation (6)); we name this as Algorithm 2. Algorithm 3 was used in [40] and is shown in Figure 12. The difference between Algorithms 2 and 3 is the cost function of upper controller $J_i\left(z_i^p,u_i^p,z_{\tilde{i}}^a\right)$, so for convenience of explanation, we give only the flow of Algorithm 2. It can be seen from a comparison of these three figures that the algorithm with post-verification proposed in this work met the fourth point of the control objectives.

Algorithm 1: Distributed MPC with post-verification.

At timet = 0 upper-layer and lower-layer system initialization: At time t$(t⩾1)$ 1: Obtain upper-layer system state: $z_i(k\mid t),k=0,1,\dots ,N_P-1$ 2: Accept neighbor state:$z_{-\tilde{i}}^a(k\mid t+1),k=0,1,\dots ,N_P-1$ 3: Optimize cost function: $J_i\left(z_i^p,u_i^p,z_i^a,z_{-\tilde{i}}^a\right)$ 4: Obtain: $U_r=u_i^{*}(k\mid t)$ and then pass to the lower-layer system 5: $E_i=\left|e_i\right|=y_i^a(t-1)-y_i\left(t\right).$ Make up this error in the next time 6: If $E_i\ge \sigma$, then switch topology 7: Obtain lower-layer system state: $v_i(k\mid t),k=0,1,\dots ,N_p-1$ 8: Optimize cost function: $J\left(v_i^p,w_i^p\right)$ 9: Obtain $w_i^{*}(k\mid t)$ and apply to system

Algorithm 2: Decentralized MPC.

At timet = 0 upper-layer and lower-layer system initialization: At time t$(t⩾1)$ 1: Obtain upper-layer system state: $z_i(k\mid t),k=0,1,\dots ,N_P-1$ 2: Optimize cost function: $J_i\left(z_i^p,u_i^p\right)$ 3: Obtain: $U_r=u_i^{*}(k\mid t)$ and then pass to the lower-layer system 4: Obtain lower-layer system state: $v_i(k\mid t),k=0,1,\dots ,N_p-1$ 5: Optimize cost function: $J\left(v_i^p,w_i^p\right)$ 6: Obtain $w_i^{*}(k\mid t)$ and apply to system

Remark 1.

The condition for switching topology is $E_i⩾\sigma$, after switching the trajectory. If the communication is no longer interrupted or wrong, the switched topology is always used. This work assumes that there is always a communication topology that is connected normally, and that there will be no case where a USV cannot accept the formation information from other USVs.

Remark 2.

The stability proof in this work can be found in [26,38].

Figure 10 shows the trajectory evolution using Algorithm 1. We consider that failure occurred in the communication between USV${}_2$ and USV${}_4$. The failure state condition $E_i\ge \sigma$ was satisfied. Consequently, Algorithm 1 switched the current topology (Topology 1 in Figure 4) to an alternative one (Topology 2 in Figure 4). The results show that the method could effectively reduce the formation error. The maximum error was about 0.1 cm and it took about three steps to completely restore the original formation. The same condition was considered in simulations using Algorithms 2 and 3. The results are shown in Figure 11 and Figure 12, respectively. Note that Algorithms 2 and 3 do not adopt the post-verification procedure, and failed to maintain the original USV formation. The difference between Algorithms 2 and 3 lies in the cost function. Specifically, Algorithm 3 employs assumed states of the neighbors in the cost function, which is ignored in Algorithm 2. Simulations revealed that Algorithm 3 had a faster convergence speed compared with Algorithm 2 as failures occurred. Hence, the assumed states of neighbors should be taken into account in the cost function in order to achieve better closed-loop control performance. Similar results were observed when the communication between USV${}_3$ and USV${}_6$ was interrupted. Relevant simulations and analysis are omitted for brevity.

6. Conclusions

In this work, a hierarchical DMPC approach is developed for the formation control of USVs. The upper layer determines the optimal references for the lower layer based on the USV kinematic model and the information received from neighbors. It is shown that the assumed state of the local USV and those of neighbors play important roles in the cost function, which reduces the influence of external disturbances. In addition, a post-verification procedure is adopted which compensates the difference between the assumed states and the actual ones. It is further shown that post-verification can effectively detect the failure state, in which case an alternative communication topology is employed to maintain the USV formation. Simulations reveal the effectiveness of the proposed approach. Our future research will extend the results to multi-agent systems with time delays and unmeasured states.