Quasi-Infinite Horizon Nonlinear Model Predictive Control for Cooperative Formation Tracking of Underactuated USVs with Four Degrees of Freedom

Abstract

To address the issues of external unknown disturbances and roll motion in the tracking control of underactuated unmanned surface vehicle (USV) formation, a cooperative formation control method based on nonlinear model predictive control (NMPC) algorithm and finite-time disturbance observer is proposed. Initially, a tracking error model for the USV formation is established within a leader–follower framework, utilizing a four-degree-of-freedom (4-DOF) dynamic model to simultaneously account for roll motion and trajectory tracking. This error model is then approximately linearized and discretized. To mitigate the initial non-smoothness in the desired trajectories of the follower USVs, a tracking differentiator is designed to smooth the heading angle of the leader USV. Thereafter, a quasi-infinite horizon NMPC algorithm is developed, in which a terminal penalty function is constructed based on quasi-infinite horizon theory. Furthermore, a finite-time disturbance observer is developed to facilitate real-time estimation and compensation for unknown marine disturbances. The proposed method’s effectiveness is validated both mathematically and in simulation. Mathematically, closed-loop stability is rigorously guaranteed via a Lyapunov-based proof of the quasi-infinite horizon NMPC design. In simulations, the algorithm demonstrates superior performance, reducing steady-state tracking errors by over 80% and shortening convergence times by up to 75% compared to a conventional PID controller. These results confirm the method’s robustness and high performance for complex USV formation tasks.

1. Introduction

Unmanned surface vehicles (USVs) have been playing an increasingly important role in a wide range of maritime activities, including marine resource exploration, waterborne rescue, and military operations. Compared to a single USV, multi-USV formations possess greater capability and flexibility for carrying out complex marine tasks and missions [1,2,3]. Cooperative formation control of USVs generally refers to the regulation of USV formations to maintain a prescribed configuration while accurately tracking a desired trajectory, which serves as a fundamental prerequisite for multi-USV systems to accomplish various complex formation-based operations [4,5]. Extensive research has been conducted on this subject, leading to significant advancements in the field.

A novel sliding surface-based distributed control law for USV formation was proposed by Jiang et al. [6], ensuring finite-time convergence. To address parameter uncertainties and complex disturbances, an accurate disturbance observer (ADO) along with a novel fixed-time fast terminal sliding mode (FTFTSM) controller was designed by Shen et al. [7]. For the formation tracking control of USVs under directed graphs, Sui et al. [8] provided a prescribed-time cooperative tracking control scheme, in which a state observer-based prescribed-time sliding surface was developed. A sliding mode observer to obtain the desired trajectory for each USV was developed by Duan et al. [9]. Dong et al. [10] effectively suppressed high-frequency oscillations in the controller caused by complex marine disturbances through the coordinated design of dynamic surface sliding mode control and a lateral velocity tracking differentiator.

A double closed-loop fast integral terminal sliding mode control method was proposed by Xia et al. [11], which not only improved the stability of the leader–follower formation strategy, but also significantly overcame the issue of non-convergence of formation tracking errors within a finite time, commonly observed in traditional approaches. To achieve trajectory tracking control of USV formations with unknown model parameters and environmental disturbances, Dong et al. [12] proposed a data-driven adaptive fuzzy sliding mode control scheme. An online identification algorithm based on navigation data feedback was designed to address the problem of unknown model parameters, thereby enabling real-time acquisition of USV model parameters for the construction of a real-time synchronous formation model. For the underactuated USV formation control problem in environments with static obstacles, model uncertainties, and time-varying disturbances, Huang et al. [13] developed a self-structuring fuzzy Mexican hat wavelet cerebellar model articulation controller based on the Mexican hat wavelet function, thereby achieving model-free control.

The nonlinear model predictive control (NMPC) algorithm achieves control of complex nonlinear systems through online receding-horizon optimization. This approach features advantages such as predictive capability, rolling optimization, and feedback correction [14,15,16]. By incorporating a model predictive mechanism and an online optimization framework, the NMPC algorithm decomposes a complex infinite-horizon optimization problem into finite-horizon subproblems that can be solved in real time. In this manner, an optimal control sequence for the system is generated within each sampling interval. With the rapid advancement of computational capabilities in recent years, the application of NMPC in USV formation control has been increasingly investigated. Zhang et al. [17] proposed a dual closed-loop MPC scheme enhanced by a finite-time extended state observer (ESO) to achieve AUV formation tracking under compound disturbances. A formation tracking error model for USVs was constructed, and a virtual transition trajectory was introduced to guide the design of the tracking controller, particularly to address large initial tracking errors. A dual closed-loop distributed robust MPC algorithm was proposed in [18] to address formation control problems characterized by time-varying communication delays, current disturbances, and model uncertainties.

To address the challenges associated with formation tracking and obstacle avoidance, an event-triggered adaptive NMPC method was developed by Li et al. [19]. Zhu et al. [20] formulated a distributed event-triggered adaptive model predictive control (DEAMPC) approach for formation control, leveraging information from neighboring agents to address trajectory tracking issues in both UAVs and USVs. A distributed formation control strategy for USVs based on NMPC was designed by Liu et al. [21], through which connectivity maintenance and collision avoidance in multi-USV systems were achieved. A path following and collision avoidance system for autonomous surface vehicles based on NMPC was proposed by Gonzalez-Garci et al. [22]. effectively maintained the vehicle’s controlled-speed path tracking, while deviations from the reference trajectory were permitted to safely maneuver around unexpected obstacles.

Based on the analysis of existing research, it has been identified that studies [6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21] predominantly design control algorithms based on three-degree-of-freedom (3-DOF) models of USVs, with insufficient consideration given to the effects of roll motion on tracking control. In works [6,7,8], cooperative formation control algorithms are developed under the assumption of fully actuated USVs. However, in practical engineering applications, most USVs are underactuated as a result of inherent nonlinear dynamics and actuator limitations. Studies [10,11,12,13,18,19] achieve the generation and maintenance of USV formations based on a leader–follower approach, yet they do not address the pronounced fluctuations in the desired trajectories of follower USVs during the initial tracking phase, which is influenced by the leader’s heading angle. Furthermore, the stability analysis is not provided in [21,22].

Based on the aforementioned analysis, a control approach is presented in this paper, in which a finite-time disturbance observer and a quasi-infinite horizon NMPC algorithm are employed to address the tracking control challenges encountered by underactuated USV formations under marine disturbances and roll motion. Unlike the cited works, which are based on the conventional 3-DOF model (surge, sway, yaw), this study develops its control algorithm based on a 4-DOF model. We explicitly account for and control the USV’s roll motion, which is a significant factor for stability and performance in realistic marine environments. A tracking differentiator is designed to process the heading angle of the leader USV, so that the fluctuations in the desired trajectories of the follower USVs during the initial stage are mitigated. To simultaneously address the roll motion and trajectory tracking control problems of USV formation, a tracking error model is constructed. This model is subsequently subjected to approximate linearization and discretization. A quasi-infinite horizon NMPC algorithm is then proposed, in which a terminal penalty function is designed to ensure the stability of the algorithm. Finally, a finite-time disturbance observer is designed to estimate and compensate for unknown marine disturbances, thereby enhancing the robustness and tracking accuracy of the proposed control scheme.

Compared with existing research, the innovations of this paper are summarized as: (1) The effect of roll motion in underactuated USVs is taken into account, and a trajectory tracking control algorithm is developed based on a 4-DOF mathematical model of the USV. (2) To address the issue of pronounced fluctuations in the desired trajectory of the follower USVs during the initial stage of the tracking task, a tracking differentiator is constructed to smooth the attitude angle of the leader USV, thereby enhancing tracking control performance. (3) A terminal penalty function is designed based on quasi-infinite horizon theory, and terminal inequality constraints are constructed, leading to the development of a quasi-infinite horizon NMPC algorithm. This ensures the stability of the algorithm within the finite prediction horizon. (4) A finite-time disturbance observer is designed and applied to the 4-DOF model of the USV to enable real-time estimation and compensation of unknown marine disturbances.

The remainder of this paper is organized as follows: In Section 2, the USV model and the formation error model are established. In Section 3, a tracking control algorithm for USV formation is designed utilizing the NMPC algorithm in conjunction with a finite-time disturbance observer. Section 4 is dedicated to the stability analysis of the proposed algorithm. In Section 5, the effectiveness and stability of the algorithm are validated through simulation experiments conducted under three different cases.

2. System Model

2.1. Mathematical Model of USV

The development of an accurate mathematical model is fundamental to the design of effective motion control algorithms for USVs. While the majority of research simplifies the control problem by considering only the horizontal motions—specifically, the surge, sway, and yaw degrees of freedom (DOF)—this approach often neglects the critical influence of roll motion.

For USVs operating in realistic wave environments, roll motion is readily excited by wave disturbances and can severely compromise vehicle stability and performance. To enhance the fidelity of our model and the robustness of the subsequent control strategy, this paper incorporates roll dynamics, constructing a comprehensive four-degree-of-freedom (4-DOF) model that captures the coupled motions of surge, sway, roll, and yaw.

According to the literature [23], we consider a formation system comprising N USVs. The kinematic and dynamic equations for the $i^{th}(i=1,2,\dots ,N)$ USV are presented. The model accounts for rigid-body dynamics, hydrodynamic effects, and environmental disturbances.

As shown in Figure 1, the kinematic equations describe the geometry of motion, relating the time derivatives of position and orientation in the earth-fixed inertial frame to the linear and angular velocities in the body-fixed frame.

And the dynamic equations describe the relationship between forces/moments acting on the vehicle and the resulting changes in its linear and angular velocities, following Newtonian and Eulerian mechanics.

$$\left\{\begin{array}{l}{\dot{x}}_i=u_i\cos {\psi }_i-v_i\sin {\psi }_i\cos {\phi }_i\\ {\dot{y}}_i=u_i\sin {\psi }_i+v_i\cos {\psi }_i\cos {\phi }_i\\ {\dot{\phi }}_i=p_i\\ {\dot{\psi }}_i=r_i\cos {\phi }_i\\ (m-X_{\dot{u}}){\dot{u}}_i=(m-Y_{\dot{v}})v_i{r}_i-(X_u+X_{u\left|u\right|}\left|u_i\right|)u_i+{\tau }_{ui}+d{\tau }_{ui}\\ (m-Y_{\dot{v}}){\dot{v}}_i=-(m-X_{\dot{u}})u_i{r}_i-(Y_v+Y_{v\left|v\right|}\left|v_i\right|)v_i+d{\tau }_{vi}\\ (I_x-K_{\dot{p}}){\dot{p}}_i=-(K_p+K_{p\left|p\right|}\left|p_i\right|)p_i+(z_g-z_b)W\sin ({\phi }_i)+{\tau }_{pi}+d{\tau }_{pi}\\ (I_z-N_{\dot{r}}){\dot{r}}_i=-(X_{\dot{u}}-Y_{\dot{v}})u_i{v}_i-(N_r+N_{r\left|r\right|}\left|r_i\right|)r_i+{\tau }_{ri}+d{\tau }_{ri}\end{array}\right.,$$

(1)

where $x_i,y_i$ denote the longitudinal and lateral position of the $i^{th}$ USV. ${\phi }_i,{\psi }_i$ are the roll angle and the yaw angle of the $i^{th}$ USV, respectively. $u_i,v_i,p_i,r_i$ represent the surge velocity, sway velocity, roll angular velocity, and yaw angular velocity of the $i^{th}$ USV, respectively. $X_{\ast },Y_{\ast },K_{\ast },N_{\ast }$ correspond to the hydrodynamic coefficients acting on the four degrees of freedom. $m$ denotes the $i^{th}$ USV’s mass. $z_{gi},z_{bi},W_i$ represent the vertical positions of the center of gravity, the center of buoyancy, and the weight of the $i^{th}$ USV, respectively. ${\tau }_{ui},{\tau }_{pi},{\tau }_{ri}$ are the control inputs. $d{\tau }_{ui},d{\tau }_{vi},d{\tau }_{pi},d{\tau }_{ri}$ represent the environmental disturbances acting on the surge, sway, roll, and yaw motions, respectively.

2.2. Formation Tracking Error System

In this paper, a leader–follower scheme is employed to enable the USV formation. The formation is assumed to comprise a single leader USV and $N-1$ follower USVs. The overall structure of the leader–follower framework is depicted in Figure 2.

As shown in Figure 2, the $j^{th}$ USV functions as a follower vessel relative to the leader USV. Let $Y_l={\left[x_l,y_l\right]}^T$ denote the position of the leader USV. $Y_j={\left[x_j,y_j\right]}^T$ represent the position of the $j^{th}$ follower USV, and $Y_{jd}={\left[x_{jd},y_{jd}\right]}^T$ denote its desired position vector, which can be derived from the leader’s positional information, as detailed below:

$$Y_{jd}=Y_l+\left[\begin{array}{cc}\cos {\psi }_l& -\sin {\psi }_l\\ \sin {\psi }_l& \cos {\psi }_l\end{array}\right]L_j,$$

(2)

where ${\psi }_l$ represents the heading angle of the leader, while $L_j={\left[l_{xj},l_{yj}\right]}^T$ denotes the relative position of the $j^{th}$ follower. Specifically, $l_{xj}$ and $l_{yj}$ refer to the desired relative longitudinal and lateral positions, respectively, in the coordinate frame of the leader USV.

Differentiating Equation (2) and combining with Equation (1), we have [24]:

$$\left\{\begin{array}{l}{\dot{x}}_{id}=-r_l\sin {\phi }_l{l}_{xj}-r_l\cos {\phi }_l{l}_{yj}+u_l\cos {\phi }_l-v_l\sin {\psi }_l\cos {\phi }_l\\ {\dot{y}}_{id}=r_l\cos {\phi }_l{l}_{xj}-r_l\sin {\phi }_l{l}_{yj}+u_l\sin {\phi }_l+v_l\cos {\psi }_l\cos {\phi }_l\end{array}\right..$$

(3)

where $u_l,v_l,r_l$ represent the surge velocity, sway velocity, and yaw angular velocity of the leader USV, respectively.

Considering the drift angle ${\varsigma }_{jd}=\arctan 2\left(-v_{jd},u_{jd}\right)$ of the $j^{th}$ follower USV, and in conjunction with Equation (1), we can design the remaining desired state variables for the $j^{th}$ follower USV as follows:

$$\left\{\begin{array}{l}{\psi }_{jd}=\arctan 2\left({\dot{y}}_{jd},{\dot{x}}_{jd}\right)+{\varsigma }_{jd}\\ {\phi }_{jd}=0, p_{jd}=0\\ u_{jd}={\dot{x}}_{jd}\cos {\psi }_{jd}+{\dot{y}}_{jd}\sin {\psi }_{jd}\\ v_{jd}=\left(-{\dot{x}}_{jd}\sin {\psi }_{jd}+{\dot{y}}_{jd}\cos {\psi }_{jd}\right)/\cos {\phi }_{jd}\\ r_{jd}=\left({\dot{x}}_{jd}{\ddot{y}}_{jd}-{\dot{y}}_{jd}{\ddot{x}}_{jd}\right)/\left({\dot{x}}_{jd}^2+{\dot{y}}_{jd}^2\right)\end{array}\right.,$$

(4)

where ${\phi }_{jd},{\psi }_{jd}$ are the desired roll angle and heading angle of the $j^{th}$ USV. $u_{jd},v_{jd}$ represent the desired longitudinal and lateral velocity of the $j^{th}$ UMV, respectively. $p_{jd},r_{jd}$ denote the desired roll and yaw angular velocity of the $j^{th}$ UMV, respectively. The method for calculating the desired state variables of the leader USV is consistent with that of the follower USVs.

To facilitate the following discussion, let ${\chi }_{id}={\left[x_{id},y_{id},{\phi }_{id},{\psi }_{id},u_{id},v_{id},p_{id},r_{id}\right]}^T$ denotes the desired state vector of the $i^{th}$ USV and ${\chi }_i={\left[x_i,y_i,{\phi }_i,{\psi }_i,u_i,v_i,p_i,r_i\right]}^T$ denotes its actual state vector.

The tracking error model is constructed as follows: First, predefined desired positions are used to compute additional desired state variables, which are tracked by the leader USV. The desired states of follower USVs are generated within the leader-follower frame-work. Subsequently, the tracking error model is formulated by subtracting the desired state from the actual state of each USV. The tracking error model translates formation-level goals into individual USV trajectory tracking tasks. Under the leader-follower framework, the leader USV tracks a predefined global trajectory and each follower USV maintains a relative position relative to the leader. Errors are computed relative to time-varying desired states, enabling dynamic formation reconfiguration.

Consequently, the tracking control problem for the $i^{th}$ USV is transformed into designing an appropriate control input ${\tau }_i={[{\tau }_{ui},{\tau }_{pi},{\tau }_{qi}]}^T$ such that the tracking error ${\tilde{\chi }}_i={\chi }_i-{\chi }_{id}$ approaches zero.

3. Control Algorithm Design

This paper addresses the aforementioned control problem using the NMPC algorithm to achieve coordinated tracking control of USV formation under the influence of marine disturbances. The detailed formation control process is illustrated in the flowchart shown in Figure 3.

As shown in Figure 3, the control algorithm design for both the leader USV and the follower USVs is consistent. Therefore, only the design for the $j^{th}$ USV will be described in detail. Specifically, Section 3.1 outlines the design of a tracking differentiator to smooth the desired trajectory of the $j^{th}$ USV, Section 3.2 details the design of the NMPC algorithm, and the design of a finite-time disturbance observer is presented in Section 3.3.

3.1. Desired Trajectory Processing

In the context of the NMPC framework, the design of the tracking control algorithm for the $j^{th}$ USV requires the desired state variables within the prediction horizon $N_p$. Therefore, this section introduces the assumed state variables of the leader USV within the prediction horizon $N_p$ at time step $k$, as well as the assumed control increments within the control horizon $N_c$. The details are as follows:

$$\Delta {\widehat{\tau }}_l\left(s|k\right)=\left\{\begin{array}{l}\Delta {\tau }_l^{*}\left(s|k-1\right),s\in \left[k,k+N_c-2\right]\\ K_l{\xi }_l\left(k+N_c-1|k-1\right),s=k+N_c-1\end{array}\right.,$$

(5)

$${\widehat{\chi }}_l\left(s|k\right)=\left\{\begin{array}{l}{\chi }_l^{*}\left(s|k-1\right) ,s\in \left[k,k+N_p-1\right]\\ {\chi }_l^K\left(k+N_p|k-1\right),s=k+N_p\end{array}\right.,$$

(6)

where $\Delta {\widehat{\tau }}_l\left(s|k\right)$ and ${\widehat{\chi }}_l\left(s|k\right)$ represent the assumed control increment sequence and assumed state sequence of the leader USV at time step $k$. $\Delta {\tau }_l^{*}\left(s|k-1\right)$ and ${\chi }_l^{*}\left(s|k-1\right)$ refer to the optimal predicted control increment sequence and optimal state sequence obtained for the leader USV at time step $k-1$ through the NMPC algorithm. $K_l{\xi }_l\left(k+N_c-1|k-1\right)$ denotes the terminal controller of the leader USV at time step $k-1$, with its specific design detailed in Section 3.2. Finally, ${\chi }_l^K\left(k+N_p|k-1\right)$ represents the terminal state calculated based on the USV model and the terminal controller.

It should be noted that, according to Equation (2), the desired position of the $j^{th}$ follower USV is highly sensitive to the heading angle of the leader USV. During the initial stages of the tracking task, the discrepancy between the leader’s actual and desired positions is considerable, resulting in significant fluctuations in its actual trajectory. Consequently, the desired trajectory of the follower USV also exhibits pronounced variability.

To mitigate this issue, a tracking differentiator is developed in this section to smooth the leader USV’s heading angle $\widehat{\psi }\left(s|k\right),s=k,\dots ,k+N_p$ over the prediction horizon $N_p$ at time step $k$. This smoothing process ensures that the resulting desired trajectory for the $j^{th}$ follower USV is both smooth and well-aligned. The inputs and outputs of the tracking differentiator at time step $k$ are illustrated in Figure 4.

In Figure 4, the output signal ${\overline{\psi }}_l\left(s|k\right)$ represents the smoothed hypothetical heading angle of the leader USV, which is used to track the input signal ${\widehat{\psi }}_l\left(s|k\right)$. The derivative of the output signal ${\overline{\psi }}_l\left(s|k\right)$ is denoted as ${\overline{r}}_l\left(s|k\right)$, representing the smoothed heading angular velocity.

The continuous-time tracking differentiator is typically expressed in the following form:

$$\left\{\begin{array}{l}{\dot{\overline{\psi }}}_l\left(s|k\right)={\overline{r}}_l\left(s|k\right)\\ {\dot{\overline{r}}}_l\left(s|k\right)=-{\alpha }^2\left(s|k\right)\left({\overline{\psi }}_l\left(s|k\right)-{\widehat{\psi }}_l\left(s|k\right)\right)-2\alpha \left(s|k\right){\overline{r}}_l\left(s|k\right)\end{array}\right.,$$

(7)

where $\alpha \left(s|k\right)$ represents the speed factor, by which the tracking speed of the output signal is determined.

Due to the requirements of simulation and computation, the discretized form of the following tracking differentiator has been established:

$$\left\{\begin{array}{l}{\overline{\psi }}_l\left(s+1|k\right)={\overline{\psi }}_l\left(s|k\right)+{\overline{r}}_l\left(s|k\right)h\\ {\overline{r}}_l\left(s+1|k\right)={\overline{r}}_l\left(s|k\right)+h\left\{-{\alpha }^2\left(s|k\right)\left({\overline{\psi }}_l\left(s|k\right)-{\widehat{\psi }}_l\left(s|k\right)\right)-2\alpha \left(s|k\right){\overline{r}}_l\left(s|k\right)\right\}\end{array}\right.,$$

(8)

where $h$ is sampling time.

The tracking differentiator is essential for smoothing the heading angle of the leader USV during initial phase. However, our analysis revealed that persistent use of the tracking differentiator beyond necessary introduces steady-state errors in the follower USVs’ tracking precision. In order to ensure that the tracking differentiator meets the tracking accuracy requirements of the $j^{th}$ follower USV, the following criterion has been established:

$${\widehat{\psi }}_l\left(s|k\right)=\left\{\begin{array}{l}{\overline{\psi }}_l\left(s|k\right), ‖{\widehat{Y}}_l\left(s|k\right)-Y_{ld}\left(s|k\right)‖>\beta \\ {\widehat{\psi }}_l\left(s|k\right), ‖{\widehat{Y}}_l\left(s|k\right)-Y_{ld}\left(s|k\right)‖\le \beta \end{array}\right..$$

(9)

Here ${\widehat{Y}}_l\left(s|k\right)$ and $Y_{ld}\left(s|k\right)$ denote the assumed and desired position coordinates, respectively, of the leader USV at the time step k within the prediction horizon $N_p$. The parameter β is a positive constant, representing the triggering threshold of the tracking differentiator. By combining Equations (2)–(4) with Equations (5) and (6), the desired state variable, ${\mathit{\chi }}_{jd}\left(s|k\right),s=k,\dots ,k+N_p$ for the j^th follower USV within the prediction horizon N_p, as processed by the tracking differentiator, can thus be obtained.

3.2. Quasi-Infinite Horizon NMPC

For the convenience of subsequent control algorithm design, the mathematical model of the $j^{th}$ follower USV at time step $k$ in Equation (1) is reformulated as follows:

$${\dot{\chi }}_j\left(k\right)=f\left({\chi }_j\left(k\right),{\tau }_j\left(k\right),{\tau }_{wj}\left(k\right)\right),$$

(10)

where ${\tau }_{wj}={\left[d{\tau }_{uj},d{\tau }_{vj},d{\tau }_{pj},d{\tau }_{rj}\right]}^T$ indicates the ocean disturbance imposed on the $j^{th}$ follower USV. ${\chi }_j$ denotes the state vector of the $j^{th}$ follower USV, and ${\tau }_j$ is the control input.

A first-order Taylor expansion is performed on system (10) at the desired variable $\left({\chi }_{jd}\left(k\right),{\tau }_{jd}\left(k\right)\right)$, and a linearized model is established to describe the deviation between the actual and desired state variables of the $j^{th}$ follower USV at time step $k$.

$$\left\{\begin{array}{l}{\dot{\tilde{\chi }}}_j\left(k\right)={\Phi }_{jk}{\tilde{\chi }}_j\left(k\right)+M_{jk}{\tilde{\tau }}_j\left(k\right)\\ {\tilde{\chi }}_j\left(k\right)={\chi }_j\left(k\right)-{\chi }_{jd}\left(k\right)\\ {\tilde{\tau }}_j\left(k\right)={\tau }_j\left(k\right)-{\tau }_{jd}\left(k\right)\end{array}\right.,$$

(11)

where ${\tilde{\chi }}_j\left(k\right)$ and ${\tilde{\tau }}_j\left(k\right)$ denotes the deviations of the state variable and the control input, respectively, for the $j^{th}$ follower USV. ${\Phi }_{jk}={\left.\frac{\partial f}{\partial {\chi }_j}\right|}_{\begin{array}{l}{\chi }_j={\chi }_{jd}\\ {\tau }_j={\tau }_{jd}\end{array}}$ and $M_{jk}={\left.\frac{\partial f}{\partial {\tau }_j}\right|}_{\begin{array}{l}{\chi }_j={\chi }_{jd}\\ {\tau }_j={\tau }_{jd}\end{array}}$ are the Jacobian matrices.

In this work, the Jacobian matrices are held constant throughout the prediction horizon at each optimization step. While this introduces a local approximation error, its effect is mitigated by the receding-horizon nature of the MPC framework. By relinearizing the system dynamics at the current operating point in every sampling step, the error remains bounded and does not accumulate significantly in closed-loop operation. This approach represents a well-established trade-off between computational tractability and control performance.

System (11) is discretized by the forward Euler method, and the incremental state-space equation at time step $k+1$ can be expressed as follows:

$$\left\{\begin{array}{l}{\xi }_j\left(k+1\right)={\tilde{\Phi }}_{jk}{\xi }_j\left(k\right)+{\tilde{M}}_{jk}\Delta {\tau }_j\left(k\right)\\ {\xi }_j\left(k\right)={\left[{\tilde{\chi }}_j^T\left(k\right),{\tilde{\tau }}_j^T\left(k-1\right)\right]}^T\\ \Delta {\tau }_j\left(k\right)={\tau }_j\left(k\right)-{\tau }_j\left(k-1\right)\end{array}\right.,$$

(12)

where $\Delta {\tau }_j\left(k\right)$ is control input increment, ${\tilde{\Phi }}_{jk}=\left[\begin{array}{cc}{\Phi }_{jk}h+I& M_{jk}h\\ 0_{3\times 8}& I\end{array}\right]$ and ${\tilde{M}}_{jk}=\left[\begin{array}{c}{M}_{jk}h\\ I\end{array}\right]$ are coefficient matrices.

According to the description in Section 2, each USV in the formation is required to accomplish its individual tracking task. The tracking control problem for each USV is converted into an optimization problem of a cost function through the NMPC algorithm. The cost function $J_j\left(k\right)$ for the $j^{th}$ follower USV at time step $k$ is formulated as follows:

$$J_j\left(k\right)={\displaystyle \sum _{s=k+1}^{k+N_p}{‖{\chi }_j\left(s|k\right)-{\chi }_{jd}\left(s|k\right)‖}_{Q_j}^2+}{\displaystyle \sum _{s=k}^{k+N_c-1}{‖\Delta {\tau }_j\left(s|k\right)‖}_{R_j}^2}.$$

(13)

The first term of Equation (13) is the tracking capability of the $j^{th}$ follower USV with respect to the desired trajectory; the second term corresponds to the control input cost, which reflects the smoothness of the control signals. The matrices $Q_j,R_j$ are specified as positive definite diagonal matrices.

The traditional MPC operates through receding-horizon optimization (solving finite-horizon problems at each step), a fundamental challenge exists: finite horizons cannot inherently guarantee closed-loop stability, as truncating predictions risks terminal state deviations. To resolve this, we adopt quasi-infinite horizon theory. This method ensures stability by terminal penalty function and terminal region constraint.

Therefore, in this section, based on the quasi-infinite horizon theory, a terminal inequality constraint ${\xi }_j\left(k+N_p|k\right)\in \Omega$ is incorporated into the optimization problem, where $\Omega$ denotes a neighborhood of the origin. Since the USV system is controllable, a linear state feedback law $\Delta {\tau }_j\left(k\right)=K_j{\xi }_j\left(k\right)$ exists for system (12), such that ${\tilde{\Phi }}_{jk}={\tilde{\Phi }}_{jk}+{\tilde{M}}_{jk}\cdot K_i$ is rendered asymptotically stable, where $K_i$ represents the gain matrix, which can be obtained via the LQR algorithm. For any given positive definite diagonal matrices $Q_j^{\prime }=\mathrm{diag}(Q_j,I_{3\times 3})$ and $R_j$, as well as a gain coefficient ${\mu }_j>0$, the following equality is satisfied:

$${\mathit{\Phi }}_{jk}^T{P}_j{\Phi }_{jk}-P_j+{\mu }_j{Q}_j^{*}=0,$$

(14)

where $Q_j^{*}=Q_j^{\prime }+K_j^T{R}_j{K}_j$. By solving Equation (14), a unique positive definite matrix $P_j$ can be obtained.

Therefore, a constant ${\mathit{\partial }}_j\in (0,\infty )$ can be found such that a neighborhood $\Omega =\left\{{\xi }_j\left(k\right)|{\xi }_j{\left(k\right)}^T{P}_j{\xi }_j\left(k\right)\le {\partial }_j\right\}$ of the origin is established as a terminal region for system (12). $\Delta {\tau }_j\left(k\right)=K_j{\xi }_j\left(k\right)$ and ${\xi }_j{\left(k\right)}^T{P}_j{\xi }_j\left(k\right)$ are the terminal controller and the terminal penalty function, respectively. Equation (13) can be reformulated as follows:

$$J_j\left(k\right)={\displaystyle \sum _{s=k+1}^{k+N_p-1}{‖{\chi }_j\left(s|k\right)-{\chi }_{jd}\left(s|k\right)‖}_{Q_j}^2+}{\displaystyle \sum _{s=k}^{k+N_c-1}{‖\Delta {\tau }_j\left(s|k\right)‖}_{R_j}^2+{‖{\xi }_j\left(k+N_p|k\right)‖}_{P_j}^2}.$$

(15)

And the constraints for the system of the $j^{th}$ USV are designed as follows:

$$\left\{\begin{array}{l}\Delta {\tau }_{j\min }\le \Delta {\tau }_j\left(s|k\right)\le \Delta {\tau }_{j\max }\\ {\tau }_{j\min }\le {\tau }_j\left(s|k\right)\le {\tau }_{j\max }\\ {\chi }_{j\min }\le {\chi }_j\left(s|k\right)\le {\chi }_{j\max }\end{array}\right..$$

(16)

Here $\Delta {\tau }_{j\min }$ and $\Delta {\tau }_{j\max }$ are the minimum and maximum control input increments. ${\tau }_{j\min }$ and ${\tau }_{j\max }$ are the minimum and maximum control inputs. ${\chi }_{j\min }$ and ${\chi }_{j\max }$ are the constraints of the state variables.

By incorporating Equation (16), the problem of solving the cost function can be reformulated as:

$$\begin{array}{l}\Delta {\tau }_j^{*}\left(s|k\right)=\underset{\Delta {\tau }_j(s|k),s=k,\dots ,k+N_c}{\arg \min }{J}_j\left(k\right).\\ \mathrm{s}.\mathrm{t}. \mathrm{constraints} \left(16\right) \\ \left\{\begin{array}{l}{\chi }_j^{*}\left(k|k\right)={\chi }_j\left(k\right)\\ {\tau }_j^{*}\left(k|k\right)=\Delta {\tau }_j^{*}\left(k|k\right)+{\tau }_j\left(k-1\right)\\ {\tau }_j^{*}\left(s+1|k\right)=\Delta {\tau }_j^{*}\left(s+1|k\right)+{\tau }_j^{*}\left(s|k\right)\\ {\dot{\chi }}_j^{*}\left(s|k\right)=f\left({\chi }_j^{*}\left(s|k\right),{\tau }_j^{*}\left(s|k\right),{\widehat{\tau }}_{wj}\left(k\right)\right)\end{array}\right.\end{array}$$

(17)

Here, ${\widehat{\tau }}_{wj}\left(k\right)$ denotes the estimated ocean disturbance for the $j^{th}$ USV at time step $k$. It is assumed that ${\widehat{\tau }}_{wj}\left(k\right)$ remains constant within the prediction horizon $N_p$.

By solving Equation (17), the optimal control increment sequence $\Delta U_j^{*}\left(k\right)={\left[\begin{array}{ccc}\Delta {\tau }_j^{*}\left(k|k\right)& ,\cdots ,& \Delta {\tau }_j^{*}\left(k+N_c-1|k\right)\end{array}\right]}^T$ for the $j^{th}$ USV at time step $k$ can be obtained. Consequently, the optimal control input sequence within the control horizon of $N_c$ at time step $k$ can be computed as follows:

$$\left\{\begin{array}{l}{\tau }_j^{*}\left(k|k\right)={\tau }_j\left(k-1\right)+\Delta {\tau }_j^{*}\left(k|k\right)\\ \dots \\ {\tau }_j^{*}\left(k+s|k\right)={\tau }_j^{*}\left(k+s-1|k\right)+\Delta {\tau }_j^{*}\left(k+s|k\right)\\ \dots \\ {\tau }_j^{*}\left(k+N_c-1|k\right)={\tau }_j^{*}\left(k+N_c-2|k\right)+\Delta {\tau }_j^{*}\left(k+N_c-1|k\right)\end{array}\right..$$

(18)

The first component of Equation (18) is utilized as the control input at time step $k$:

$${\tau }_j\left(k\right)={\tau }_j^{*}\left(k|k\right)={\tau }_j\left(k-1\right)+\Delta {\tau }_j^{*}\left(k|k\right).$$

(19)

The pseudocode of the quasi-infinite horizon NMPC is shown in Algorithm 1.

Algorithm 1 Quasi-Infinite Horizon NMPC

1: Initialization: Initialize the USV state ${\chi }_i$, control input ${\tau }_i$, relative position $L_j$ of the follower USV, and suitable controller parameters;

2: for k ← 1 to max k do

3: calculate the desired state ${\chi }_{id}$;

4: if $‖{\widehat{Y}}_l\left(s|k\right)-Y_{ld}\left(s|k\right)‖>\beta$ then

5: smooth desired state for the follower USVs via tracking differentiator;

6: end if

7: calculate the Jacobian matrices;

8: solve (14) to get $P_j$;

9: solve optimization problem (17) to get $\Delta U_j^{*}\left(k\right)$;

10: apply the control input ${\tau }_i(k)$;

11: update USV state ${\chi }_i$;

12: end for

3.3. Design of Finite-Time Disturbance Observer

In this section, a finite-time disturbance observer is designed to estimate and compensate for ocean disturbances, thereby enhancing the robustness of the control algorithm. Let ${\widehat{\tau }}_{wj}={[{\widehat{\tau }}_{uj},{\widehat{\tau }}_{vj},{\widehat{\tau }}_{pj},{\widehat{\tau }}_{rj}]}^T$ denote the vector of ocean disturbances estimated by the observer. Based on the USV mathematical model (1), the finite-time disturbance dynamics model for the $j^{th}$ USV is established as follows:

$$\left\{\begin{array}{l}(m-X_{\dot{u}}){\dot{\widehat{u}}}_j=(m-Y_{\dot{v}})v_j{r}_j-(X_u+X_{u\left|u\right|}\left|u_j\right|)u_j+{\tau }_{uj}+{\widehat{\tau }}_{uj}\\ (m-Y_{\dot{v}}){\dot{\widehat{v}}}_j=-(m-X_{\dot{u}})u_j{r}_j-(Y_v+Y_{v\left|v\right|}\left|v_j\right|)v_j+{\widehat{\tau }}_{vj}\\ (I_x-K_{\dot{p}}){\dot{\widehat{p}}}_j=-(K_p+K_{p\left|p\right|}\left|p_j\right|)p_j+{\tau }_{pj}+{\widehat{\tau }}_{pj}\\ (I_z-N_{\dot{r}}){\dot{\widehat{r}}}_j=-(X_{\dot{u}}-Y_{\dot{v}})u_j{v}_j-(N_r+N_{r\left|r\right|}\left|r_j\right|)r_j+{\tau }_{rj}+{\widehat{\tau }}_{rj}\end{array}\right..$$

(20)

The disturbance observation error vector ${\omega }_j$ can be expressed as:

$${\omega }_j=\left[\begin{array}{c}(m-X_{\dot{u}})\left(u_j-{\widehat{u}}_j\right)\\ \begin{array}{l}(m-Y_{\dot{v}})\left(v_j-{\widehat{v}}_j\right)\\ (I_x-K_{\dot{p}})\left(p_j-{\widehat{p}}_j\right)\end{array}\\ (I_z-N_{\dot{r}})\left(r_j-{\widehat{r}}_j\right)\end{array}\right].$$

(21)

The finite-time formulation for the observed disturbance is established as follows:

$${\widehat{\tau }}_{jw}={\lambda }_{i1}{\mathrm{sig}}^{{\delta }_{j1}}\left({\omega }_j\right)+{\lambda }_{j2}{\displaystyle \int {\mathrm{sig}}^{{\delta }_{j2}}\left({\omega }_2\right)dt}.$$

(22)

Here ${\mathrm{sig}}^{{\delta }_{j1}}\left({\omega }_j\right)={\left|{\omega }_j\right|}^{{\delta }_{j1}}\mathrm{sgn}\left({\omega }_j\right)$, while ${\lambda }_{j1},{\lambda }_{j2}$ are designated as positive definite diagonal matrices. Additionally, ${\delta }_{j1},{\delta }_{j2}$ are defined as positive constants that satisfy the following relation:

$$0.5\le {\delta }_{j1}<1,{\delta }_{j2}=2{\delta }_{j1}-1.$$

(23)

4. Stability Analysis

4.1. Stability of Finite-Time Disturbance Observer

The following lemma is introduced:

Lemma 1 

([25]). Consider a continuous system:

$$\dot{x}=f\left(x\right),f\left(0\right)=0,x\in ℝ.$$

(24)

Suppose there exists a continuous positive definite function $V$, positive real numbers $\rho$ and $b\in \left(0,1\right)$, as well as an open neighborhood $U_0\in ℝ$ of the origin, such that:

$$\dot{V}+\rho V^b\le 0,x\in U_0/\left\{0\right\}.$$

(25)

Then, system (24) is finite time stable at the origin.

The differential form of Equation (21) can be derived based on Equations (20) and (22):

$${\dot{\omega }}_j=-{\lambda }_{j1}{\mathrm{sig}}^{{\delta }_{j1}}\left({\omega }_j\right)-{\lambda }_{j2}{\displaystyle \int {\mathrm{sig}}^{{\delta }_{j2}}\left({\omega }_j\right)+{\tau }_{wj}}.$$

(26)

Define new variables as:

$${\left[x_{j1},x_{j2}\right]}^T={\left[{\omega }_j,-{\lambda }_{j2}{\displaystyle \int {\mathrm{sig}}^{{\delta }_{j2}}\left({\omega }_j\right)+{\tau }_{wj}}\right]}^T,$$

(27)

Based on Equation (27), Equation (26) can be rewritten as follows:

$$\left\{\begin{array}{l}{\dot{x}}_{j1}=-{\lambda }_{j1}{\mathrm{sig}}^{{\delta }_{j1}}\left(x_{j1}\right)+x_{j2}\\ {\dot{x}}_{j2}=-{\lambda }_{j2}{\mathrm{sig}}^{{\delta }_{j2}}\left(x_{j1}\right)+{\dot{\tau }}_{wj}\end{array}\right..$$

(28)

Considering that the ocean disturbance is bounded, i.e., there exist positive number $q$ such that $‖{\dot{\tau }}_{wj}‖\le q$. According to Equation (28), if $x_{j1},x_{j2}$ can converge to zero within a finite time, then it follows that ${\dot{x}}_{j1},{\dot{x}}_{j2}\to 0$ in finite time is achieved. Therefore, it can be demonstrated that the observation error approaches zero within a finite time. The following Lyapunov function $V_{oj}$ is constructed:

$$V_{0j}=\frac{1}{2}{\mathit{\varsigma }}_j^T{F}_j{\varsigma }_j$$

(29)

The specific forms of the vector ${\varsigma }_j$ and the positive definite matrix $F_j$ are given as follows:

$$\left\{\begin{array}{l}{\varsigma }_j={\left[{\mathrm{sig}}^{{\delta }_{j1}}(x_{j1}),x_{j2}\right]}^T\\ F_j=\left[\begin{array}{cc}2{\lambda }_{j2}/{\delta }_{j1}& -{\lambda }_{j1}\\ -{\lambda }_{j1}& 2I_{4\times 4}\end{array}\right]\end{array}\right..$$

(30)

Theorem 1. 

Consider the Lyapunov function (29), The following inequality hold:

$${\dot{V}}_{0j}\le -\rho V_{0j}^b,$$

(31)

where $b,\rho$ are tunable parameters.

The following proves Theorem 1. It is noted that Equation (29) is continuous and differentiable everywhere except at $\left\{\left(x_{j1},x_{j2}\right)|x_{j1}=0_{4\times 1}\right\}$. Therefore, the $V_{oj}$ satisfies the following inequality:

$${\lambda }_{\min }\left(F_j\right){‖{\varsigma }_j‖}^2/2\le V_{0j}\le {\lambda }_{\max }\left(F_j\right){‖{\varsigma }_j‖}^2/2,$$

(32)

where ${\lambda }_{\min }\left(·\right)$ represents minimum eigenvalue of a square matrix, and

$${‖{\varsigma }_j‖}^2={\left|x_{j1}\right|}^{2{\delta }_{j1}}+x_{j2}^T{x}_{j2}.$$

(33)

Then we get the following:

$${\left|x_{j1}\right|}^{{\delta }_{j1}-1}\ge {‖{\varsigma }_j‖}^{\left({\delta }_{j1}-1\right)/{\delta }_{j1}}.$$

(34)

Derive the derivative of the Lyapunov function $V_{oj}$ based on Equations (30)–(33):

$$\begin{array}{c}{\dot{V}}_{0j}=-{\left|x_{j1}\right|}^{{\delta }_{j1}-1}{\varsigma }_j^T{H}_{0j}{\varsigma }_j+{\varsigma }_j^T{h}_{0j}{\tau }_{wj}\\ \le -{\lambda }_{\min }\left(H_{0j}\right){\left|x_{j1}\right|}^{{\delta }_{j1}-1}{‖{\varsigma }_j‖}^2+q_j‖{\varsigma }_j‖‖h_0^j‖,\end{array}$$

(35)

where $H_{0j}={\lambda }_{j1}\left[\begin{array}{cc}{\lambda }_{j2}+{\delta }_{j1}{\mathbf{\lambda }}_{j1}^2& -{\delta }_{j1}{\mathbf{\lambda }}_{j1}\\ -{\delta }_{j1}{\mathbf{\lambda }}_{j1}& -{\delta }_{j1}\end{array}\right]$, and $h_{0j}=\left[\begin{array}{c}-{\lambda }_{j1}\\ 2I_{4\times 4}\end{array}\right]$.

Substituting (33) into (34), it leads to:

$${\dot{V}}_{0j}\le -{\iota }_j‖{\varsigma }_j‖{V_{0j}}^{\left({\delta }_{j1}+{\delta }_{j2}\right)/{\delta }_{j1}},$$

(36)

where ${\iota }_j={\lambda }_{\min }\left(H_{0j}\right)-q_j‖h_{0j}‖/{‖{\varsigma }_j‖}^{{\delta }_{j2}/{\delta }_{j1}}$.

To guarantee that ${\iota }_j$ remains strictly positive, the following condition is imposed:

$$‖{\varsigma }_j‖>{\left(q_j‖h_{0j}‖/{\lambda }_{\min }\left(H_{0j}\right)\right)}^{{\delta }_{j1}/{\delta }_{j2}}.$$

(37)

Employing Equations (31) and (36), inequality (35) can be rewritten in the following form:

$${\dot{V}}_{0j}\le -\rho V_{0j}^b.$$

(38)

Here $b=\left({\delta }_{j1}+{\delta }_{j2}\right)/\left(2{\delta }_{j1}\right),\rho =2{\iota }_j\left({\lambda }_{\min }{\left(F_j\right)}^{-{\delta }_{j2}/\left(2{\delta }_{j1}\right)}/\sqrt{{\lambda }_{\min }\left(F_j\right)}\right)$, and the conditions $0<b<1,\rho >0$ are satisfied.

According to Lemma 1, the system (29) is finite-time stable at the origin. Consequently, the deviation between the ${\tau }_{wj}$ and ${\widehat{\tau }}_{wj}$ can be driven to zero within a finite time.

4.2. Stability of the Controller

In Section 4.1, it was demonstrated that the disturbance observation can achieve finite-time stability. This section aims to establish the asymptotic stability of the control algorithm for the USV formation system. By solving Equation (17), the optimal control increment sequence for the $j^{th}$ follower USV at time step $k$ can be expressed as follows:

$$\Delta U_j^{*}\left(k\right)={\left[\begin{array}{ccc}\Delta {\tau }_j^{*}\left(k|k\right)& ,\cdots ,& \Delta {\tau }_j^{*}\left(k+N_c-1|k\right)\end{array}\right]}^T.$$

(39)

The optimal state sequence corresponding to time step $k$ can be represented as follows:

$$X_j^{*}\left(k\right)={\left[\begin{array}{ccc}{x}_j^{*}\left(k+1|k\right)& ,\cdots ,& x_j^{*}\left(k+N_p|k\right)\end{array}\right]}^T.$$

(40)

It can be observed from Equation (15) that the minimum value of cost function at time step $k$ is given by:

$$J_j^{*}\left(k\right)={\displaystyle \sum _{s=k}^{k+N_p-1}{‖{\tilde{\chi }}_j^{*}\left(s|k\right)‖}_{Q_j}^2+}{\displaystyle \sum _{s=k}^{k+N_c-1}{‖\Delta {\tau }_j^{*}\left(s|k\right)‖}_{R_j}^2}+{‖{\xi }_j^{*}\left(k+N_p|k\right)‖}_{P_j}^2.$$

(41)

Based on Equation (39) and the terminal controller at time step $k$, the feasible control increment sequence at time step $k+1$ is proposed as:

$$\Delta U_j\left(k+1\right)=\left[\begin{array}{c}\Delta {\tau }_j\left(k+1|k+1\right)\\ \Delta {\tau }_j\left(k+2|k+1\right)\\ ⋮\\ \Delta {\tau }_j\left(k+N_c-1|k+1\right)\\ \Delta {\tau }_j\left(k+N_c|k+1\right)\end{array}\right]=\left[\begin{array}{c}\Delta {\tau }_j^{*}\left(k+1|k\right)\\ \Delta {\tau }_j^{*}\left(k+2|k\right)\\ ⋮\\ \Delta {\tau }_j^{*}\left(k+N_c-1|k\right)\\ K_j{\xi }_j\left(k+N_c|k\right)\end{array}\right].$$

(42)

The corresponding sequence of state variables can be expressed as follows:

$${\tilde{\chi }}_j^{*}\left(k+1+i|k+1\right)=\left\{\begin{array}{c}{\tilde{\chi }}_j^{*}\left(k+1+i|k\right),i=0,1,\dots ,N_p-1\\ {\Psi }_j\left(\left({\tilde{\Phi }}_{jk}+{\tilde{M}}_{jk}{K}_j\right){\xi }_j\left(k+N_p|k\right)\right),i=N_p\end{array}\right.,$$

(43)

where ${\Psi }_j=\left[\begin{array}{cc}{I}_{8\times 8}& 0\\ 0& 0_{3\times 3}\end{array}\right]$.

Consequently, the cost function at time step $k+1$ is given by:

$$\begin{array}{c}{J}_j\left(k+1\right) ={\displaystyle \sum _{s=k+1}^{k+N_p}{‖{\tilde{\chi }}_j\left(s|k+1\right)‖}_{Q_j}^2}+{\displaystyle \sum _{s=k+1}^{k+N_c}{‖\Delta {\tau }_j\left(s|k+1\right)‖}_{R_j}^2}+{‖{\xi }_j\left(k+1+N_P|k+1\right)‖}_{P_j}^2\\ ={\displaystyle \sum _{s=k+1}^{k+N_p-1}{‖{\tilde{\chi }}_j^{*}\left(s|k\right)‖}_{Q_j}^2}+{\displaystyle \sum _{s=k+1}^{k+N_c-1}{‖\Delta {\tau }_j^{*}\left(s|k\right)‖}_{R_j}^2}+{‖{\tilde{\chi }}_j^{*}\left(k+N_p|k\right)‖}_{Q_j}^2\\ +{‖K_j{\xi }_j\left(k+N_c|k\right)‖}_{R_j}^2+{‖\left({\tilde{\Phi }}_{jk}+{\tilde{M}}_{jk}{K}_j\right){\xi }_j\left(k+N_p|k\right)‖}_{P_j}^2.\end{array}$$

(44)

Combining Equation (14) and assuming $N_p=N_c=N^{\prime }$, Equation (43) can be rewritten as follows:

$$\begin{array}{ll}{J}_j\left(k+1\right)& \le {\displaystyle \sum _{s=k+1}^{k+N^{\prime }-1}({‖{\tilde{\chi }}_j^{*}\left(s|k\right)‖}_{Q_j}^2}+{‖\Delta {\tau }_j^{*}\left(s|k\right)‖}_{R_j}^2)+{‖{\xi }_j\left(k+N^{\prime }|k\right)‖}_{Q_j^{\prime }}^2\\ & +{‖K_j{\xi }_j^{*}\left(k+N^{\prime }|k\right)‖}_{R_j}^2+{‖\left({\tilde{\Phi }}_{jk}+{\tilde{M}}_{jk}{K}_j\right){\xi }_j\left(k+N^{\prime }|k\right)‖}_{P_j}^2\\ & ={\displaystyle \sum _{s=k+1}^{k+N^{\prime }-1}({‖{\tilde{\chi }}_j^{*}\left(s|k\right)‖}_{Q_j}^2+}{‖\Delta {\tau }_j^{*}\left(s|k\right)‖}_{R_j}^2)+{‖{\xi }_j\left(k+N^{\prime }|k\right)‖}_{P_j}^2\\ & ={\displaystyle \sum _{s=k}^{k+N^{\prime }-1}({‖{\tilde{\chi }}_j^{*}\left(s|k\right)‖}_{Q_j}^2+}{‖\Delta {\tau }_j^{*}\left(s|k\right)‖}_{R_j}^2)-{‖{\tilde{\chi }}_j^{*}\left(k|k\right)‖}_{Q_j}^2-{‖\Delta {\tau }_j^{*}\left(k|k\right)‖}_{R_j}^2\\ & +{‖{\xi }_j\left(k+N^{\prime }|k\right)‖}_{P_j}^2\\ & =J_j^{*}\left(k\right)-{‖{\tilde{\chi }}_j^{*}\left(k|k\right)‖}_{Q_j}^2-{‖\Delta {\tau }_j^{*}\left(k|k\right)‖}_{R_j}^2.\end{array}$$

(45)

Here, $P_j={\left({\tilde{\Phi }}_{jk}+{\tilde{M}}_{jk}{K}_j\right)}^T{P}_j\left({\tilde{\Phi }}_{jk}+M_{jk}{K}_j\right)+{\mu }_j\left(Q_j^{\prime }+K_j^T{R}_j{K}_j\right)$.

According to the principles of MPC, if an optimal solution exists for Equation (17) at time step $k$, it is less than or equal to any feasible solution. Therefore, by combining this result with Equation (44), the following can be obtained:

$$J_j^{*}\left(k+1\right)\le J_j\left(k+1\right)\le J_j^{*}\left(k\right).$$

(46)

It can be observed from Equation (40) that $J_j^{*}\left(k\right)\ge 0$ holds at any time step. According to Lyapunov stability theory [26] and Equation (45), the terminal penalty function based on the quasi-infinite horizon approach ensures that the designed controller achieves convergence within a finite prediction horizon.

Integrating the stability analysis of the finite-time disturbance observer, the following global Lyapunov function is constructed:

$$V_j\left(k\right)=J_j^{\ast }\left(k\right)+V_{0j}\left(k\right)$$

(47)

Based on Equations (38) and (47), the following result is obtained:

$$V_j\left(k+1\right)=J_j^{\ast }\left(k+1\right)+V_{0j}\left(k+1\right)\le J_j^{\ast }\left(k\right)+V_{0j}\left(k\right)=V_j\left(k\right)$$

(48)

Furthermore, since $V_j\left(k\right)\ge 0$ holds at any time, Lyapunov stability theory [26] confirms that the proposed control algorithm guarantees closed-loop stability.

5. Computer Simulation

To validate the effectiveness of the NMPC algorithm proposed in this paper, simulation experiments have been conducted involving one leader USV and two follower USVs under three different cases: straight-line, circular, and sinusoidal trajectories. To further highlight the superiority of the NMPC approach, a comparative analysis has been performed between the proposed method and a PID-based method. Specifically, Method I is defined as the NMPC algorithm integrated with a tracking differentiator, whereas Method II is defined as the PID algorithm combined with a tracking differentiator. To facilitate a quantitative evaluation of the controller’s performance, four metrics were adopted in this study: the mean steady-state error (MSSE), the root mean square error (RMSE) of tracking error, the convergence time, and the overshoot.

The model parameters are shown in

Table 1. Model parameters.

Parameter	Value (Unit)	Parameter	Value (Unit)	Parameter	Value (Unit)
$m$	$30.48 \left(\mathrm{kg}\right)$	$Y_{\dot{v}}$	$-35.3 (\mathrm{kg})$	$N_r$	$6.87 (\mathrm{kg}\cdot {\mathrm{m}}^2/\mathrm{s})$
$X_u$	$13.5 (\mathrm{kg}/\mathrm{s})$	$Y_{v\left|v\right|}$	$131 (\mathrm{kg}/\mathrm{m})$	$N_{\dot{r}}$	$-4.88 (\mathrm{kg}\cdot {\mathrm{m}}^2)$
$X_{\dot{u}}$	$-0.93 (\mathrm{kg})$	$K_p$	$0.0006 (\mathrm{kg}\cdot {\mathrm{m}}^2/\mathrm{s})$	$N_{r\left|r\right|}$	$9.4 (\mathrm{kg}\cdot {\mathrm{m}}^2)$
$X_{u\left|u\right|}$	$3.9 (\mathrm{kg}/\mathrm{m})$	$K_{\dot{p}}$	$-0.00141 (\mathrm{kg}\cdot {\mathrm{m}}^2)$	$W$	$306 (\mathrm{N})$
$Y_v$	$66.6 (\mathrm{kg}/\mathrm{s})$	$K_{p\left|p\right|}$	$0.0013 (\mathrm{kg}\cdot {\mathrm{m}}^2)$	$z_b$	$-0.0196 (\mathrm{m})$
$z_g$	0	$I_x$	$0.177 (\mathrm{kg}\cdot {\mathrm{m}}^2)$	$I_z$	$3.45 (\mathrm{kg}\cdot {\mathrm{m}}^2)$

.

Several controller parameters common to all three simulation experiments are listed in

Table 2. Controller parameters.

Parameter	Value	Parameter	Value	Parameter	Value (Unit)
$N_p$	$7$	${\delta }_{i1}$	$0.9$	$\Delta {\mathit{\tau }}_{i\min }$	${[-10 \left(\mathrm{N}\right),-1 (\mathrm{N}\cdot \mathrm{m}),-4 (\mathrm{N}\cdot \mathrm{m})]}^T$
$N_c$	$7$	${\lambda }_{i1}$	$\mathrm{diag}(1,0.8,1)$	$\Delta {\mathit{\tau }}_{i\max }$	${[10 \left(\mathrm{N}\right),1 (\mathrm{N}\cdot \mathrm{m}),4 (\mathrm{N}\cdot \mathrm{m})]}^T$
${\mu }_i$	$1$	${\lambda }_{i2}$	$\mathrm{diag}(1,0.8,1)$	${\mathit{\tau }}_{i\min }$	${[0,-10 (\mathrm{N}\cdot \mathrm{m}),-10 (\mathrm{N}\cdot \mathrm{m})]}^T$
$\beta$	$0.1$	${\mathit{R}}_i$	$\mathrm{diag}(1,1,1)$	${\mathit{\tau }}_{i\max }$	${[50 \left(\mathrm{N}\right),10 (\mathrm{N}\cdot \mathrm{m}),10 (\mathrm{N}\cdot \mathrm{m})]}^T$

.

The ocean disturbances are set as follows:

$$\left\{\begin{array}{l}d{\tau }_{ui}=4\sin (0.5t)+\cos (0.2t)\sin (0.1t)\\ d{\tau }_{vi}=3\sin (0.2t)\sin (0.15t)\\ d{\tau }_{pi}=\sin (0.1t)\sin (0.05t)\\ d{\tau }_{ri}=2\sin (0.4t)+\sin (0.3t)\end{array}\right..$$

• Case 1. Straight line trajectory

The desired trajectory of the leader USV is described as: $x_d=0.8t,y_d=0.4x_d+4$. The desired relative positions of the follower USVs with respect to the leader USV are specified as: $L_1={[-3(\mathrm{m}),6(\mathrm{m})]}^T,L_2={[-3(\mathrm{m}),-6(\mathrm{m})]}^T$. The weighting coefficient matrix Q is selected as: $Q_i=100\mathrm{diag}(10,10,10,10,1,1,1,1)$, and the initial conditions of USVs are given by:

$$\left\{\begin{array}{l}{Y}_l={\left[-3(\mathrm{m}),10(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_1={\left[-4(\mathrm{m}),14(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_2={\left[-6(\mathrm{m}),3(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\end{array}\right..$$

The simulation results of Case 1 are shown in Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11, Figure 12, Figure 13, Figure 14, Figure 15 and Figure 16.

• Case 2. Circular trajectory

The desired trajectory of the leader USV is described as: $x_d=25(\sin (0.04t)+1),y_d=25(1-\cos (0.04t))$. The desired relative positions of the follower USVs with respect to the leader USV are specified as: $L_1={[-5(\mathrm{m}),5(\mathrm{m})]}^T,L_2={[-5(\mathrm{m}),-5(\mathrm{m})]}^T$. The weighting coefficient matrix Q is selected as: $Q_i=100\mathrm{diag}(5,5,100,5,1,1,1,1)$, and the initial conditions of USVs are given by:

$$\left\{\begin{array}{l}{Y}_l={\left[22(\mathrm{m}),-5(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_1={\left[10(\mathrm{m}),0,0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_2={\left[15(\mathrm{m}),-18(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\end{array}\right..$$

The simulation results of Case 2 are shown in Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22, Figure 23, Figure 24, Figure 25, Figure 26 and Figure 27.

• Case 3. Sinusoidal trajectory

The desired trajectory of the leader USV is described as: $x_d=0.75t,y_d=10+5\sin (0.075t)$. The desired relative positions of the follower USVs with respect to the leader USV are specified as: $L_1={[0,5(\mathrm{m})]}^T,L_2={[0,-5(\mathrm{m})]}^T$. The weighting coefficient matrix Q is selected as: $Q_i=100\mathrm{diag}(10,10,20,10,1,1,1,1)$, and the initial conditions of USVs are given by:

$$\left\{\begin{array}{l}{Y}_l={\left[-3(\mathrm{m}),10(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_1={\left[-4(\mathrm{m}),14(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\\ Y_2={\left[-6(\mathrm{m}),3(\mathrm{m}),0,0,0,0,0,0,0,0,0,0\right]}^T\end{array}\right..$$

The simulation results of Case 3 are shown in Figure 28, Figure 29, Figure 30, Figure 31, Figure 32, Figure 33, Figure 34, Figure 35, Figure 36, Figure 37 and Figure 38.

In the simulation results for the three cases, Figure 5a, Figure 17a and Figure 28a depict the trajectories of the USV formation under Method 1. These results demonstrate that the NMPC algorithm enables the USV formation to accurately track the desired trajectory, thereby verifying the stability and effectiveness of the proposed algorithm. In contrast, Figure 5b, Figure 17b and Figure 28b illustrate the trajectories under Method 2. Compared with Method 1, the USV formation under Method 2 fails to follow the desired trajectory as closely and exhibits more pronounced tracking errors, highlighting the limitations of the PID algorithm in USV tracking control.

Furthermore, Figure 6, Figure 18 and Figure 29 show the evolution of position errors for the USV formation under both control algorithms across the three cases. It is evident that Method 1 consistently yields significantly smaller tracking errors, further confirming the effectiveness and accuracy of the proposed NMPC algorithm for underactuated USV formation tracking control. Additionally, in the aforementioned trajectory Figures, the desired trajectories of the follower USVs remain smooth during the initial tracking phase, reflecting the effectiveness of the tracking differentiator.

Figure 7, Figure 8, Figure 9, Figure 19, Figure 20, Figure 21, Figure 30, Figure 31 and Figure 32 depict the variations in roll and heading angles for the leader USV and two follower USVs across three different cases. It can be observed that, under Method 2, significant periodic errors are present in the roll and heading angles of all three USVs, indicating a lack of convergence in these states when the PID algorithm is applied. In contrast, under Method 1, both the roll and heading angles are accurately tracked to their desired values, further validating that the NMPC algorithm, designed based on the quasi-infinite horizon theory, ensures superior stability and performance compared to the PID algorithm.

This performance gap is further substantiated by the velocity profiles presented in Figure 10, Figure 11, Figure 12, Figure 22, Figure 23, Figure 24, Figure 33, Figure 34 and Figure 35. Compared to Method 1, substantially larger fluctuations in surge, sway, roll, and yaw velocities are observed under Method 2 for all three USVs, which further highlights the limitations of the PID algorithm in maintaining velocity stability. The variations in control inputs for the three USVs are presented in Figure 13, Figure 14, Figure 15, Figure 25, Figure 26, Figure 27, Figure 36, Figure 37 and Figure 38. It can be observed that, under the NMPC-based control, larger control inputs are applied during the initial tracking phase, allowing the USV formation to be driven to the desired trajectory with a faster response speed.

To facilitate a quantitative evaluation of the controller’s performance, four metrics were adopted in this study: the mean steady-state error (MSSE), the root mean square error (RMSE) of tracking error, the convergence time, and the overshoot. The comparative results, summarized in

Table 3. Performance metrics comparison in Case 1.

Method	USV	MSSE (m)	RMSE (m)	Convergence (s)	Overshoot (m)
Method 1	Leader	0.022	2.290	10	0.124
	Follower 1	0.020	0.486	11.5	0.169
	Follower 2	0.027	1.192	14	0.135
Method 2	Leader	0.244	3.043	41	0.506
	Follower 1	0.305	0.583	19	0.687
	Follower 2	0.473	1.371	17	0.684

,

Table 4. Performance metrics comparison in Case 2.

Method	USV	MSSE (m)	RMSE (m)	Convergence (s)	Overshoot (m)
Method 1	Leader	0.022	1.240	9	0.020
	Follower 1	0.044	1.294	11	0.099
	Follower 2	0.041	2.269	15	0.180
Method 2	Leader	0.292	2.131	43	0.634
	Follower 1	0.536	1.569	17.5	0.861
	Follower 2	0.399	2.398	58.5	0.819

and

Table 5. Performance metrics comparison in Case 3.

Method	USV	MSSE (m)	RMSE (m)	Convergence (s)	Overshoot (m)
Method 1	Leader	0.023	0.600	5	0.167
	Follower 1	0.016	0.281	6.5	0.030
	Follower 2	0.033	0.847	8.5	0.112
Method 2	Leader	0.142	0.713	12	0.248
	Follower 1	0.190	0.526	13.5	0.350
	Follower 2	0.224	0.889	14	0.371

, provide clear and compelling evidence of the superiority of the proposed NMPC-based method (Method 1) over the PID-based approach (Method 2).

In terms of tracking accuracy, Method 1 consistently achieves significantly lower MSSE and RMSE across all three test cases. For instance, in the Case 3, the leader USV’s MSSE was reduced by over 80% (from 0.142 m to 0.023 m), a trend that holds for the follower USVs and for the Case 1 and Case 2. This demonstrates a marked improvement in the formation’s ability to precisely maintain its desired position. Furthermore, the proposed method exhibits a faster and more stable dynamic response. Convergence times under Method 1 are drastically shorter, with reductions of up to 75% (e.g., from 41 s to 10 s for the leader in Case 1). This faster convergence indicates a more stable and efficient control action.

The observed quantitative improvements can be directly linked to the theory of the proposed control architecture. The exceptional robustness and significantly lower mean steady-state errors (MSSE) are a direct consequence of the finite-time disturbance observer. As shown in Figure 16, the observer accurately estimates and compensates for the unknown marine disturbances in real-time, fulfilling its theoretical purpose of enhancing disturbance rejection. This active compensation allows the USVs to maintain their desired trajectories with high precision, even in the presence of external forces.

Furthermore, the superior dynamic response is attributable to the predictive nature of the quasi-infinite horizon NMPC. Unlike a reactive controller, the NMPC optimizes the control inputs over a future horizon, anticipating the desired trajectory. This proactive approach allows for aggressive yet smooth control actions that guide the USVs to the reference trajectory quickly without the significant overshoot. The stability guarantees from the quasi-infinite horizon design ensure this aggressive response does not lead to instability. Finally, the effectiveness of the tracking differentiator is evident in the smooth initial trajectories of the follower USVs, which helps prevent large initial tracking errors and contributes to the overall stability and reduced overshoot of the formation. Collectively, the simulation results provide concrete validation of the expected theoretical benefits of each component of our integrated control strategy.

6. Conclusions

This paper successfully developed and validated a robust cooperative formation control strategy for underactuated USVs by explicitly addressing the critical effects of roll motion. The main contribution lies in the design and rigorous stability analysis of a quasi-infinite horizon Nonlinear Model Predictive Control (NMPC) framework for a four-degree-of-freedom (4-DOF) USV model. The mathematical core of this contribution is the construction of a terminal penalty function and terminal inequality constraints based on quasi-infinite horizon theory, which, combined with a Lyapunov-based analysis, formally guarantees the stability of the closed-loop system within a finite prediction horizon. The integration of a finite-time disturbance observer further enhances the system’s robustness against unknown marine disturbances. To further enhance the tracking performance of the followers, a tracking differentiator is designed to smooth the heading angle of the leader USV when generating the desired trajectories for the followers.

The superiority of the proposed method was demonstrated through comparative simulations against a conventional PID controller. The quantitative results were compelling: our approach reduced the mean steady-state error (MSSE) by over 80% in some cases (e.g., from 0.142 m to 0.023 m for the leader USV in the Case 3) and shortened convergence times by as much as 75% (e.g., from 41 s to 10 s). Quantitative analysis of performance metrics, including tracking error, convergence time, and overshoot, confirms that the proposed algorithm offers substantial improvements over traditional control methods, ensuring more precise and stable formation control.

Building upon the theoretical framework and simulation results presented in this paper, several promising avenues for future research emerge. A critical next step involves conducting comprehensive experimental validation of the proposed control algorithms on a physical multi-USV testbed. Such experiments will be crucial for verifying the real-world robustness and practical applicability of our method. Furthermore, we intend to extend our investigation to address the challenges of formation control in low-communication environments, considering aspects such as limited bandwidth, intermittent connectivity, and communication delays. Finally, exploring the complexities of heterogeneous formation control, where the USV team comprises vessels with diverse dynamic characteristics, sizes, and operational capabilities, represents another significant direction to broaden the applicability and impact of this research.