Distributed Model Predictive Consensus Control of Unmanned Surface Vehicles with Post-Veriﬁcation

: 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-veriﬁcation 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 satisﬁed, 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-veriﬁcation, the proposed approach is more robust when failure states occur.


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 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 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. 1.
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.

2.
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 MPCbased 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.

Modeling of USV
Consider a group of multiple USVs, labeled 1 to 6. For each USV, the kinematic model is expressed as follows:ẋ where [x i , y i ] T is the position and ϕ i is the heading angle of each USV. The vector ν is defined is the linear velocity on the X-and Y-axes, and r i is the angular velocity of the rudder. M = M T > 0 is the inertia matrix including added mass; the matrix C(ν) is the Coriolis and centripetal forces and moments; the matrix D is the fluid damping matrix. The vector u = [τ u τ r ] T is the control input corresponding to propeller thrust and the rudder deflection angle; the matrix G is a 3 × 2 matrix. The dynamic model of each USV is stated as follows: where (·) h are the hydrodynamic forces and moment; λ and γ are rudder coefficients connected with speed and yaw moment, respectively; m and E z are the vehicle mass and mass moment of inertia, respectively; τ r and τ 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(1) + cos(1 + π/2)), f r = 2(sin(1) + cos(1 + π/2)).

Communication Topology
The directed graph G = {V, E } is used to describe the communication topology of the formation system, where V = {1, . . . , N v } represents the set of nodes in the graph G(t); E ⊆ V × V represent the set of edges. A = a ij ∈ R N×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) ∈ E , then a ij = 1; otherwise, a ij = 0. Using N i = {j | (j, i) ∈ E } to represent the neighbor set of node i, O i = {j | (i, j) ∈ E } is used to represent the collection of nodes that can obtain the information about node i. The Laplacian matrix L ∈ R N×N is defined as L = D − A, where D ∈ R N×N is named the degree matrix, and defined as D = diag{d 1 , . . . , d N } with d i = ∑ N j=1 a ij , i = 1, . . . , N.

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;

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.

Upper-Layer Distributed MPC
The same length of predictive horizon N P is used in all local problems. Over the prediction horizon t, t + N p , the used states and input symbols are listed in Table 1, where k = 0, 1, . . . N P and the assumed state trajectory z a i (t) 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 ∈ {1, 2, . . . , N} at time t.
subject to: where u p i (0 | t), . . . , u p i N p − 1 | t denotes the unknown variables to be optimized; z −ĩ = z −ĩ | (j, i) ∈ E 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 U i = {u min,i ≤ u i ≤ u max,i }, 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: where z M z T Mz 1/2 , and Q i , R i , F i and G i are the weighting matrices. In problem 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; 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. V des,i = [u des,i, , v des,i, , r des,i, ] 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 isd 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.

Lower-Layer MPC
After calculating by the DMPC formation controller, the optimal control input u * i (k | 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 = u * i (k | t). So, we define the lower-layer optimal control problem : subject to: where w p i (0 | t), . . . , w p i N p − 1 | t denotes the unknown variables to be optimized; w P i (k | t) and v P i (k | t) are the predictive control input and predictive state trajectory of the lowerlayer controller, respectively; constraint (8a) shows the constraint from USV dynamics; constraint (8b) represent the control input set and Ω i = {w min,i ≤ w i ≤ w max,i }, where w min,i and w max,i are the bounds.
The function l in (7) is designed as: where α > 0 represents the intensity to penalize the output error from the desired state and β > 0 represents the intensity to penalize the input error deviated from the desired state.

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 G, all USVs are given the expected relative state informationd 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 p i (0 | 0) = z i (0), the assumed control inputs and assumed state are defined as where 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 informationd i,j , solve the desired state trajectory Z des,i (t) = {z des,i (k | t) | k = 0, . . . , N p }.
(2) Optimize the problem F i according to its current state x i (t), self assumed state and neighbor assumed state trajectory, and obtain the optimal control input sequence u * i (k | t), k = 0, 1, . . . , N p − 1.
(3) Calculate the optimal state trajectory within the prediction range using the optimal control sequence: (4) Compute the assumed control u a i (k | 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.
the corresponding assumed output is also calculated as: i.e., (5) According to the communication topology G, the assumed state trajectory Z a i (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 a −ĩ (k | t + 1) is received. (6) The first element of the optimal control sequence is used to implement the control effort, i.e., u i (t) = u * i (0 | t), conveying u * i (0 | 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 a i (t − 1) − y i (t) 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 = |e i |, if E i σ, take σ = 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 | t).
(3) Apply the first element of the optimal control sequence w * i (0 | t) to the lower-layer control system. Calculate the optimal state trajectory within the prediction range using the optimal control sequence v * i (k + 1 | t) = Gv * i (k | t) + Hw * i (k | t), k = 0, 1, . . . , N p − 1.
Step 5: At time t + 1, repeat the above steps.

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 cm · s −1 , X t = 0, Y t = 0, N t = 0, λ = 0.02, γ = −0.01 and E z = 0.3 kgm 2 . Table 2 lists the the initial state of USVs in the inertial system as well as the expected relative state between USVs, whered i,j = z des,i − z des,j , the first and second items of matrixd 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 byd i,j , it is not listed.; the remaining simulation parameters are shown in Table 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 cm · s −1 and the minimum value is set to −2 cm · s −1 ; similarly, the maximum and minimum values of the control input of the USV in the dynamic model are set to 1 cm · s −1 and −1 cm/s, respectively.

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 Figures 5-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 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 8 shows the control output of the lower layer. By comparing Figures 8 and 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 | 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  At time t = 0 upper-layer and lower-layer system initialization: At time t (t 1) 1: Obtain upper-layer system state: z i (k | t), k = 0, 1, . . . , N P − 1 2: Accept neighbor state:z a −ĩ (k | t + 1), k = 0, 1, . . . , N P − 1 3: Optimize cost function: and then pass to the lower-layer system 5: . Make up this error in the next time 6: If E i ≥ σ, then switch topology 7: Obtain lower-layer system state: 6: Obtain w * i (k | t) and apply to system Remark 1. The condition for switching topology is E i σ, 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 ≥ σ 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.

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 postverification 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.

Data Availability Statement:
The data used to support the findings of this study are available from the corresponding author upon request.

Conflicts of Interest:
The authors declare that there are no conflicts of interest regarding the publication of this paper.