Optimization of Trajectory Generation and Tracking Control Method for Autonomous Underwater Docking

Abstract

This study proposes a receding horizon optimization-based docking control method to address the autonomy and safety challenge of underwater docking between manned submersibles and unmanned vehicles, facilitating the integration of docking trajectory generation and tracking control. A novel approach for optimizing and generating reference trajectory is proposed to construct a docking corridor that satisfies safe collision-free and visual guidance effective regions. It generates dynamically feasible and continuously smooth docking trajectories by rolling optimization. Subsequently, a docking trajectory tracking control method based on nonlinear model predictive control (NMPC) is designed, which is specifically tailored to address thruster saturation and system state constraints while ensuring the feasibility and stability of the control system. The control performance and robustness of underwater docking were validated through simulation experiments. The optimized trajectory generated is continuous, smooth, and complies with the docking constraints. The control system demonstrates superior tracking accuracy than backstepping control, even under conditions where the model has a 40% error and bounded disturbances from currents are present. The research findings presented in this study contribute significantly to enhancing safety and efficiency in deep-sea development.

1. Introduction

In deep-sea development, manned underwater submersibles are stationed on the seabed for extended durations to facilitate the deployment and retrieval of diverse types of unmanned underwater vehicles for operational purposes. The efficient and safe execution of exploration operations relies heavily on the prompt resolution of docking control for manned submersibles and remotely operated vehicles (ROVs), which presents a critical challenge. Docking control comprises two closely interconnected components: the planning and tracking control of the docking trajectory. A docking control system must exhibit exceptional robustness and adaptability to satisfy the high demands of this task. The perturbation of the current surrounding a manned submersible influences ROV control. The presence of highly turbid seawater close to the seabed impedes visual recognition and positioning, consequently leading to diminished pose feedback accuracy and stability. To ensure the safety of both the submersible and ROV, it is crucial for docking control to meet designated precision standards to minimize collision risks and potential damage.

Various control methods have been proposed to solve the tracking control problem of ROVs, such as proportional–integral–derivative (PID) control, backstepping control (BSC), sliding mode control (SMC), and fuzzy logic control (FLC) [1,2]. The PID controller has a simple design and low complexity. Cowen et al. proposed an optical quadrant tracker-based guidance scheme for underwater vehicles and conducted sea trials with a control system implemented using a PID controller [3]. However, PID control performance is suboptimal owing to nonlinearity and model uncertainty [4]. The Lyapunov-based BSC is a mainstream and robust tracking control method [5]. System control is obtained through a linear combination of the control of the reference system and the deviation of the system [6]. However, BSC cannot mitigate the impact of external interference or the uncertainty associated with the system parameters. SMC is another mainstream nonlinear method used for trajectory tracking control. Zuo et al. designed a sliding mode control method based on power approaching and the saturated boundary layer for autonomous underwater vehicle (AUV) docking control [7]. Vu et al. proposed a station-keeping control algorithm based on sliding mode control (SMC) for hovering an over-actuated autonomous underwater vehicle (HAUV), taking into account the propeller saturation constraint [8]. However, SMC generally presents high-frequency oscillations on the sliding surface, commonly referred to as “chattering,” thereby compromising control precision and increasing energy consumption [9]. FLC typically involves converting expert knowledge-based control strategies into automated control strategies, characterized by robustness and resistance to process and parameter variations. Petritoli et al. developed an innovative method to enhance vertical stability based on the Takagi–Sugeno (T-S) fuzzy inference system [10]. Establishing fuzzy rules relies on experiential knowledge, which inherently entails subjectivity.

However, a prevailing deficiency is evident in these control techniques in that they fail to directly address the system state constraints and the saturation constraints of the thrusters, resulting in suboptimal tracking performance. The presence of state constraints and control constraints in the docking control problem is crucial for determining its success or failure and must be thoroughly considered during controller design.

Model predictive control (MPC) is a receding horizon control approach that employs multi-objective rolling optimization to effectively handle both the multi-input, multi-output (MIMO) model and explicit system constraints [11,12]. Compared to alternative approaches, the distinguishing characteristic of MPC lies in its systematic handling of system constraints during controller design. Shen et al. [13] proposed a model predictive control (LMPC) method based on the Lyapunov method. The framework for multi-objective control is based on a multi-objective model predictive control (MOMPC) approach, wherein introducing a logistic function and utilizing the weighted sum method enable the selection of an appropriate weight in accordance with the system state [14]. However, the aforementioned research lacks a comprehensive discussion of the reference trajectory, merely assuming that the trajectory and its higher-order derivative exist and are bounded. Nevertheless, it should be noted that this trajectory may not always be present or satisfy the constraints of the docking state. Gong et al. [15] introduced a novel MPC framework that incorporated dual closed loops, wherein the optimization problem was redefined as an outer-loop position controller and an inner-loop speed controller. Li et al. proposed a parallel model predictive control (PMPC) law comprising a soft-constrained model predictive controller (SMPC) and a hard-constrained model predictive controller (HMPC) [16]. However, in the aforementioned research work, the decoupling of horizontal and vertical control for trajectory tracking is not achieved. Moreover, employing either MPC parallel optimization calculation or multiple MPC controllers can result in excessive computational burden and potentially hinder the identification of an optimal or suboptimal solution within a specified timeframe. Vu et al. proposed a novel algorithm for underwater docking assessment, comprising three stages: depth tracking, analysis of the docking-feasibility area, and evaluation of the probability of successful docking [17]. However, addressing the intricate challenges of cross-coupling in the context of multiple constraints, such as underwater docking, remains a complex task.

Therefore, the motivation for the research work in this paper is to present a control scheme for the integration of trajectory optimization and control for docking. Instead of simply assuming that the docking trajectory and its higher-order derivatives exist and are bounded, constraints such as docking safety, dynamics feasibility, and visual guidance effectiveness are directly addressed in trajectory optimization. In trajectory tracking control, constraints such as thruster limitations, system states, etc., can be addressed to find an optimized control solution with limited resources and time.

The key contributions of this study are summarized as follows:

• The architecture for an underwater autonomous docking control system is designed, and a receding horizon optimization method is proposed to integrate docking trajectory generation and tracking control.
• The docking corridor that satisfies docking safety and the effective region of visual guidance is constructed. The dynamic state constraints, higher-order differential boundedness, and docking corridor are hard constrained, and a smooth and flat docking trajectory with segmented Bessel curve characteristics is generated by solving the minimum snap optimization problem.
• The docking control problem is decoupled and decomposed into two subproblems: horizontal and vertical controls. An enhanced tracking control algorithm based on NMPC is developed to achieve horizontal docking control, considering thrust saturation and system state as constraints. Vertical control is accomplished through adaptive PID control utilizing the whale optimization algorithm.

The remainder of this paper is organized as follows: Section 2 presents the kinematics, dynamic model, and docking control system architecture. Section 3 presents a trajectory generation and optimization method that effectively fulfills the multiple docking constraints. Section 4 introduces the docking control method based on NMPC in detail. The effectiveness of the proposed method is validated through a simulation in Section 5. Finally, Section 6 summarizes the conclusions of this study.

2. Model and Docking Control System

2.1. Docking Model

As shown in Figure 1, the origin of the north-east-down (NED) frame was fixed to the docking station, assuming that point E is set as the origin of the NED frame. The reference body frame (RBF) was set on the ROV, and the origin was selected as the center of gravity (G). When the ROV is recovered to the docking station, the origin G of the RBF will coincide at point E.

The control object investigated in this study is an observation-class ROV. By pre-configuring the ROV, it achieves an underwater state of gravity and buoyancy balance, exhibiting minimal roll and pitch angles at the initial static state. Considering safety precautions, the docking motion speed is intentionally kept very low to minimize variations in both roll and pitch angles during movement. To simplify controller design and conserve computational resources, we suppose simplifying the kinematic model of the ROV by disregarding any changes in roll and pitch angles during docking.

The kinematic model of the docking ROV can be formulated as follows [18]:

$$\dot{\mathsf{\eta }}=\mathit{\varkappa }\left(\mathsf{\eta }\right)\mathbf{v}$$

(1)

where $\mathsf{\eta }={[x,y,z,\psi ]}^T$ indicates the position and heading of the ROV in the NED frame; $\mathbf{v}={[u,v,w,r]}^T$ represents the linear velocity and yaw rate of the ROV in the RBF; and $\mathit{\varkappa }\left(\mathsf{\eta }\right)$ is an invertible rotation matrix, expressed as

$$\mathit{\varkappa }(\mathsf{\eta })=\left[\begin{array}{cccc}\mathit{cos}\psi & -\mathit{sin}\psi & 0& 0\\ \mathit{sin}\psi & \mathit{cos}\psi & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(2)

The inverse of the rotation matrix satisfies ${\mathit{\varkappa }}^{-1}\left(\mathsf{\eta }\right)={\mathit{\varkappa }}^T\left(\mathsf{\eta }\right)$ and retains the length.

Further, the dynamic model of the docking ROV is expressed as

$$\mathbf{M}\dot{\mathbf{v}}+\mathbf{C}\left(\mathbf{v}\right)\mathbf{v}+\mathbf{D}\left(\mathbf{v}\right)\mathbf{v}+\mathbf{g}\left(\mathsf{\eta }\right)=\mathsf{\tau }$$

(3)

where $\mathbf{M}$ is the inertia and damping matrix; $\mathbf{C}\left(\mathbf{v}\right)$ is the centripetal force and coriolis matrix; and $\mathbf{D}\left(\mathbf{v}\right)$ is the hydrodynamic damping matrix.

To facilitate the analysis, the following mild assumptions are given.

Assumption 1. 

It is assumed that the ROV system can be considered a rigid body with constant mass and moment of inertia. The ROV is assumed to possess three planes of symmetry.

Assumption 2. 

The mass distribution of the ROV is uniform, and the non-diagonal term of matrix $\mathbf{M}$ is neglected. In matrix $\mathbf{C}\left(\mathbf{v}\right)$, hydrodynamic coefficients of quadratic terms are disregarded. In matrix $\mathbf{D}\left(\mathbf{v}\right)$, only hydrodynamic damping coefficients of linear and quadratic terms on the diagonal term are considered.

Thus, these matrices can be obtained $\mathbf{M}=\mathit{diag}\left(m-X_{\dot{u}},m-Y_{\dot{v}},m-Z_{\dot{w}},I_z-N_{\dot{r}}\right)$.

$$\mathbf{C}(\mathbf{v})=\left[\begin{array}{cccc}0& 0& 0& -M_{\dot{v}}v\\ 0& 0& 0& M_{\dot{u}}u\\ 0& 0& 0& 0\\ M_{\dot{v}}v& -M_{\dot{u}}u& 0& 0\end{array}\right]$$

(4)

$$\mathbf{D}\left(\mathbf{v}\right)=diag(X_u,Y_v,Z_w,N_r)+diag\left(X_{\left|u\right|u|}\right|u|,Y_{\left|v\right|v}|v|,Z_{\left|w\right|w}\left|w\right|,N_{\left|r\right|r}|r\left|\right)$$

(5)

where $\mathbf{g}\left(\mathsf{\eta }\right)$ is the gravity and buoyancy term, which presents the vector of restoring forces and moments; $\mathbf{g}\left(\mathsf{\eta }\right)$ has an upper bound $\overline{g}$ satisfying ${\Vert \mathbf{g}\left(\mathsf{\eta }\right)\Vert }_{\infty }\le \overline{g}$; and $\mathsf{\tau }={[{\tau }_u,{\tau }_v,{\tau }_w,{\tau }_r]}^T$ are the forces and torques acting on the gravity center.

The ROV in this paper is an underactuated system featuring a thruster layout, as depicted in Figure 2. The ROV comprises six sets of thrusters, encompassing four sets of horizontal thrusters T₁~T₄ for controlling longitudinal and lateral movements as well as yaw control. Additionally, it incorporates two sets of vertical thrusters T₅~T₆ to facilitate vertical submersion control.

The angle between the thrust direction of T₁ and the u-axis is $\chi$. The angle between the thrust direction of T₅ and the w-axis is $\zeta$. Therefore, τ can be expressed as follows:

$$\mathsf{\tau }=\mathsf{\Theta }\cdot \mathbf{u}$$

(6)

where $\mathsf{\Theta }$ is the thrust control matrix and ${\mathbf{u}=[u_1,u_2,u_3,u_4,u_5,u_6]}^T$.

$$\mathsf{\Theta }=\left[\begin{array}{cccccc}\mathit{cos}\chi & \mathit{cos}\chi & -\mathit{cos}\chi & -\mathit{cos}\chi & 0& 0\\ \mathit{sin}\chi & -\mathit{sin}\chi & \mathit{sin}\chi & -\mathit{sin}\chi & 0& 0\\ 0& 0& 0& 0& \mathit{cos}\zeta & \mathit{cos}\zeta \\ A& -A& -A& A& 0& 0\end{array}\right]$$

(7)

where $A=(b/2)·sin\chi +(a/2)·cos\chi$; $a$ and $b$ are the equivalent spacing of the thrusters in the width and length directions of ROV, respectively.

By integrating Equations (1), (3), and (6), the docking model was established as follows:

$$\dot{\mathsf{\xi }}=\left[\begin{array}{c}\mathit{\varkappa }\left(\mathsf{\eta }\right)\mathbf{v}\\ {\mathbf{M}}^{-1}(\mathsf{\Theta }\mathbf{u}-\mathbf{C}\mathbf{v}-\mathbf{D}\mathbf{v}-\mathbf{g})\end{array}\right]=f(\mathsf{\xi },\mathbf{u})$$

(8)

where the state is defined as $\mathsf{\xi }={[x,y,z,\psi ,u,v,w,r]}^T$.

Assumption 3. 

All thrusters have thrust saturation and the same capability $\left|u_i\right|\le u_{max}$. Assuming a clear upper bound of τ exists, ${\tau }_{max}={\Vert {\mathit{\tau }}_{max}\Vert }_{\infty }$ and ${\mathit{\tau }}_{max}={[{\tau }_{u,max},{\tau }_{v,max},{\tau }_{w,max},{\tau }_{r,max}]}^T$ are satisfied.

Assumption 4. 

Suppose that the gravity and buoyancy of ROV are in balance, $\mathbf{g}\left(\mathsf{\eta }\right)=\mathbf{0}$.

2.2. Autonomous Docking Control System

For docking applications at low speeds, the six-degrees-of-freedom (DOFs) kinetics of an underwater vehicle with an open-frame structure can be divided into two subsystems with minimal interaction: maneuvering and diving [19]. Therefore, to minimize the computational burden of NMPC optimization, the docking control problem can be decomposed into two distinct subproblems: horizontal tracking control and vertical control. A block diagram of the control strategy is presented in Figure 3. The docking controller comprised two sub-controllers.

The vertical controller employed a self-tuning PID controller based on a whale optimization algorithm (WOA) for depth control, generating two sets of commands T₅–T₆ for controlling the vertical thrusters. The WOA optimization algorithm was employed to optimize the PID control parameters.

The primary objective of the horizontal controller was to achieve precise tracking control of ${\mathsf{\eta }}_d={[x_d,y_d,{\psi }_d]}^T$. The horizontal controller employed an enhanced NMPC to determine the optimal control sequence for the four sets of thrusters (T₁–T₄).

The proposed docking control procedure delineated in this study is as follows:

• Docking trajectory generation. The relative position and attitude ${\mathsf{\eta }}_i={[x_i,y_i,z_i,{\psi }_i]}^T$ of ROV were measured online by visual guidance, and the appropriate docking trajectory ${\mathsf{\rho }}_d\left(t\right)={[x_d,y_d,z_d,{\psi }_d]}^T$ was generated by rolling. The reference state ${\mathsf{\xi }}_d={[x_d,y_d,{\psi }_d,u_d,v_d,r_d]}^T$ was calculated with the horizontal reference trajectory.
• Status feedback $\mathsf{\xi }\left(t\right)$ calculation. The Kalman filter was used to fuse the data measured by the visual guidance ${\mathsf{\eta }}_i$ and navigation sensors ${\mathsf{\xi }}_{\mathbf{m}\mathbf{e}\mathbf{a}}={[x,y,\psi ,u,v,r]}^T$, and also to estimate the relative position and attitude of the ROV and docking station [20].
• Docking control. The docking trajectory tracking controller, based on NMPC, generated the control force τ, while the thrust allocation (TA) algorithm was employed to compute the control of thrusters T₁–T₆.
• During the docking movement of the ROV, iterating through the aforementioned steps was necessary, beginning from Step 1 until the docking process was completed.

2.3. Tuning of PID Controller Using WOA

Due to the strong nonlinearity of both the control object and the desired trajectory, it is very difficult to tune the PID controller. The WOA has the advantages of fast convergence, strong global search ability, simple control parameters, and easy implementation [21]. Therefore, WOA can be used for the optimal tuning of PID parameters. The WOA consists of three steps: initializing the control parameters, calculating the fitness function, and updating the position.

The objective of WOA is to minimize the integral of squared error (ISE), and the fitness function utilized was

$$I_{ISE}={\int }_0^T{\left[e\left(t\right)\right]}^{2}dt$$

(9)

where $e\left(t\right)=z\left(t\right)-z_d$ indicates position error.

When encircling the prey, the process of position updating is represented as follows [22]:

$$\begin{gathered} \overrightarrow{U}=‖\overrightarrow{\gamma }·{\overrightarrow{p}}_{best}\left(t\right)-\overrightarrow{p}\left(t\right)‖ \\ \overrightarrow{p}\left(t+1\right)={\overrightarrow{p}}_{best}\left(t\right)-\overrightarrow{\omega }·\overrightarrow{U} \end{gathered}$$

(10)

where $\overrightarrow{p}\left(t\right)={\left[K_p,K_i,K_d\right]}^T$ and ${\overrightarrow{p}}_{best}\left(t\right)$ represent the position vector and the finest position vector. $\overrightarrow{\omega }$ and $\overrightarrow{\gamma }$ are coefficient vectors which are calculated as follows:

$$\begin{gathered} \overrightarrow{\omega }=2\overrightarrow{\mu }·\overrightarrow{r}-\overrightarrow{\mu } \\ \overrightarrow{\gamma }=2\overrightarrow{r} \\ \overrightarrow{\mu }=2-2·t/t_{max} \end{gathered}$$

(11)

where $\overrightarrow{\mu }$ is linearly reduced from 2 to 0 and $\overrightarrow{r}$ is an unsystematic vector in [0, 1].

When bubble net attacking, the updated position is

$$\overrightarrow{p}\left(t+1\right)=\left\{\begin{array}{cc}‖{\overrightarrow{p}}_{best}\left(t\right)-\overrightarrow{p}\left(t\right)‖·e^{dk}·\mathrm{c}\mathrm{o}\mathrm{s}\left(2\pi k\right)+{\overrightarrow{p}}_{best}\left(t\right)& ifq\ge 0.5\\ {\overrightarrow{p}}_{best}\left(t\right)-\overrightarrow{\omega }·\overrightarrow{U}& ifq<0.5\end{array}\right.$$

(12)

where $k$ is a random number in [−1, 1] and $d$ is a constant defining the shape of the logarithmic spiral. $q$ is a random number in the range of [0, 1].

When searching the prey, the whales move towards a global optimum value.

$$\begin{gathered} \overrightarrow{U}=‖\begin{array}{c}\overrightarrow{\gamma }.{\overrightarrow{p}}_{rand}-\overrightarrow{p}\left(t\right)\end{array}‖ \\ \overrightarrow{p}\left(t+1\right)={\overrightarrow{p}}_{rand}-\overrightarrow{\omega }·\overrightarrow{U} \end{gathered}$$

(13)

where ${\overrightarrow{p}}_{rand}$ is a random position vector.

In each iteration, the WOA adjusts the PID parameters in an attempt to minimize the fitness function, thus gradually finding the optimal parameter settings $K_p$, $K_i$, and $K_d$.

3. Docking Reference Trajectory Generation

This study proposed an optimization method for generating docking trajectory, which can generate a differentially flat and smooth curve ${\mathsf{\rho }}_d\left(t\right)$. The position and heading set of a series of key route-control points were determined using typical convex properties based on the piecewise Bezier curve. ROV must pass the docking corridors within the area of effective visual guidance with no obstacles at the specified time $T_g$. Satisfying the continuous equality constraints of the position, velocity, acceleration, and jerk, along with the inequality constraints imposed by the upper and lower limits, is imperative. Finally, the trajectory was solved by optimization.

3.1. Piecewise Formulation and Cost Function

Trajectory planning for each dimension was conducted independently. The μth dimension of the trajectory was divided into m segments and can be expressed as a piecewise Bezier curve [23], as follows:

$$f_{\mu }(t)=\left\{\begin{array}{l}\begin{array}{cc}{s}_1\cdot B_1\left({\displaystyle \frac{t-t_0}{s_1}}\right)& t\in \left[t_0,t_1\right]\end{array}\\ \begin{array}{cc}{s}_2\cdot B_2\left({\displaystyle \frac{t-t_1}{s_2}}\right)& t\in \left[t_1,t_2\right]\end{array}\\ \begin{array}{cc}\dots & \dots \end{array}\\ \begin{array}{cc}{s}_m\cdot B_m\left({\displaystyle \frac{t-t_{m-1}}{s_m}}\right)& t\in \left[t_{m-1},t_m\right]\end{array}\end{array}\right.$$

(14)

where each segment $B_j\left(t\right)$ of the curve employs an n-order Bernstein polynomial instead of a traditional monomial polynomial.

The Bezier curve was defined within a predetermined range of values from 0 to 1. For the jth segment $B_j\left(t\right)$ of the trajectory, the time interval must be scaled proportionally from [0, 1] to the allocated time $[t_{j-1},t_j]$ of each segment using the time scale factor $s_j$, as follows:

$$\begin{gathered} \begin{array}{c}{B}_j\left(t\right)={\displaystyle \sum _{i=0}^n}{c}_j^i{b}_n^i\left(t\right)\end{array} \\ {b}_n^i(t)=C_n^i\cdot t^i\cdot (1-t{)}^{n-i} \end{gathered}$$

(15)

where n represents the rank of the polynomial, ${\mathbf{c}}_j=[c_j^0,c_j^1,\dots ,c_j^n]$ is represented as a set of points on the jth segment of the Bezier curve, and $b_n^i(t)$ is a Bernstein polynomial.

The objective function is computed using the integral of the square of the derivative up to the kth-order. The minimum snap objective function [24] is used to optimize the docking trajectory, i.e., $k=4$, to minimize the thrust differential as follows:

$$J_1=\sum _{\mu \in \{x,y,z,\psi \}}{\int }_0^T{\left({\displaystyle \frac{d^k{f}_{\mu }(t)}{d{t}^k}}\right)}^{2}dt$$

(16)

where $J_1$ is a quadratic formulation. Suppose ${\mathbf{Q}}_0$ represents the Hessian matrix of the objective function.

The objective function in the jth segment of the dimension μ is further expressed as follows:

$$\begin{array}{cc}\hfill J_{\mu j}& ={\int }_0^{s_j} ({\displaystyle \frac{d^k{f}_{\mu j}(t)}{d{t}^k}}{)}^{2}dt={\int }_0^1 s_j({\displaystyle \frac{s_j\cdot d^k{g}_{\mu j}(\tau )}{d(s_j\cdot \tau {)}^k}}{)}^{2}d\tau \hfill \\ & ={\displaystyle \frac{1}{s_j^{2k-3}}}\cdot {\int }_0^1 ({\displaystyle \frac{d^k{g}_{\mu j}(t)}{d{\tau }^k}}{)}^{2}d\tau \hfill \end{array}$$

(17)

where the jth segment of the μ dimension of the trajectory is $f_{\mu j}(t)$, and the unscaled Bezier curve of the interval [0, 1] is $g_{\mu j}(\tau )$, and $\tau =(t-t_j)/s_{j.}$

3.2. Docking Trajectory Constraints

For each Bezier curve, the derivative of order l can be represented as a linear combination of the corresponding points with lower orders, formulated as follows:

$$\left\{\begin{array}{l}{a}_{\mu j}^{0,i}=c_{\mu j}^i\\ \begin{array}{cc}{a}_{\mu j}^{l,i}={\displaystyle \frac{n!}{(n-l)!}}\cdot \left(a_{\mu j}^{l-1,i+1}-a_{\mu j}^{l-1,i}\right)& ,l\ge 1\end{array}\end{array}\right.$$

(18)

• Waypoint Constraints: The constraints of the position, velocity, and heading angle must be satisfied at the start point, midpoint, and end point. All waypoints are under the NED frame. During the docking process, several waypoints can be preset as needed, and these waypoints are the states that the ROV must pass through. The lth-order derivative $d_{\mu j}^{\left(l\right)}$ at the start of the jth segment is expressed as

  $$a_{\mu j}^{l,0}\cdot s_j^{(1-l)}=d_{\mu j}^{\left(l\right)}$$

  (19)
• Continuity Constraints: The pth-order derivative of the trajectory at the junction of two consecutive segments is continuous ($0\le p\le k-1$). The position, velocity, acceleration, and jerk are continuous to ensure that the trajectory is continuous, smooth, and flat. For the jth segment curve,

  $$\left\{\begin{array}{l}{a}_{\mu j}^{p,n}\cdot s_j^{(1-p)}=a_{\mu ,j+1}^{p,0}\cdot s_{j+1}^{(1-p)}\\ a_{\mu j}^{0,i}=c_{\mu j}^i\end{array}\right.$$

  (20)
• Dynamic Feasibility Constraints: The velocity and acceleration of the trajectory in the jth segment must meet limiting conditions to satisfy the feasible constraints of higher-order dynamics, as follows:

  $$\left\{\begin{array}{l}{\nu }_{min}^m\le n\cdot \left(c_{\mu j}^i-c_{\mu j}^{i-1}\right)\le {\nu }_{max}^m\\ a_{min}^m\le n\cdot (n-1)\cdot \left(c_{\mu j}^i-2c_{\mu j}^{i-1}+c_{\mu j}^{i-2}\right)/s_j\le a_{max}^m\end{array}\right.$$

  (21)

  where $v_{max}^m$ and $v_{min}^m$ represent the maximum and minimum velocity thresholds, respectively, and $a_{max}^m$ and $a_{min}^m$ represent the maximum and minimum acceleration thresholds, respectively.
• Docking Corridor Constraints: Each segment must be within the docking corridor to satisfy the effective visual guidance and safe docking constraints. Therefore, the docking corridor ${\Omega }_j$ is the intersection of the obstruction-free safe area $W_j$ and the visual guidance effective area $G_j$, i.e., ${\Omega }_j=W_j\cap G_j$.

3.3. Docking Corridors

The algorithms for solving the safe area, the visual guidance effective area, and the docking corridors are presented herein, as shown in Figure 4.

3.3.1. Docking Safe Area

The safe area $W_j$ must be free of obstacles to ensure docking safety. The distance from each point in the docking path to the nearest obstacle was obtained using the Euclidean signed distance field (ESDF). A sphere corresponding to the safe space of the nearest obstacle was obtained at each point. The corridors were initialized as inscribed cubes of a sphere. Each cube was then expanded by querying the adjacent meshes in the direction of the principal axis of each dimension until it reached the maximum free volume. Finally, all repeated parts of the corridors were trimmed to obtain a safe collision-free area [25].

3.3.2. Effective Area of Visual Guidance

Because visual guidance and positioning are used during docking, the lights at the entrance of the docking station must always be within the visual area of the camera so that docking control can provide stable and continuous information on the position and attitude.

For each point in the docking path, the visual guidance area $G_j$ is a cube with customizable length, and the width and height can be defined as

$$\left\{\begin{array}{c}{H}_y=\mathit{tan}\left(\alpha /2\right)·\left|x\right|-L_h/2\\ H_z=\mathit{tan}\left(\beta /2\right)·\left|x\right|-L_v/2\end{array}\right.$$

(22)

where $\alpha$ is the horizontal angle of view, $\beta$ is the vertical angle of view, $L_h$ is the horizontal distance between the lights, and $L_v$ is the vertical distance between the lights.

3.3.3. Docking Corridor Constraints

The trajectory was limited to the boundaries defined by its control points because of the convex hull characteristics exhibited by the Bezier curve. Therefore, integrating security restrictions to ensure that all control points were included within the respective cube of ${\Omega }_j$ was possible. For a portion of a path, given that the expanded cube formed a convex polygon and all the control points were contained within it, the convex hull formed by these control points remained within the confines of the cube. Consequently, the trajectory was constrained to a generalized cube.

Boundary limits were implemented to enforce the security constraints on the control points, specifically for the control points of the jth segment.

$${\xi }_{\mu j}^{-}\le c_{\mu j}^i\le {\xi }_{\mu j}^{+},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\begin{array}{cc}\mu \in \{x,y,z,\psi \},& i=0,1,2,\dots ,n\end{array}$$

(23)

where ${\xi }_{\mu j}^{+}$ and ${\xi }_{\mu j}^{-}$ represent the upper and lower limits of their respective jth cube counterparts.

3.4. Quadratic Formulation

The optimization variables in each segment were directly defined within the feasible domain, incorporating constraints related to waypoints and corridors (${\mathbf{c}}_j\in {\Omega }_j$). The linear equality and inequality constraints were derived from the continuity and higher-order dynamic constraints. Subsequently, the trajectory planning problem was converted into an optimization problem using quadratic programming (QP) with imposed constraints.

We can obtain a set of control points for each trajectory [26], as follows:

$$\begin{array}{cc}& min{\mathbf{c}}^T{\mathbf{Q}}_o\mathbf{c}\hfill \\ \hfill s.t.& {\mathbf{A}}_{eq}\mathbf{c}={\mathbf{b}}_{eq},\hfill \\ & {\mathbf{A}}_{ie}\mathbf{c}\le {\mathbf{b}}_{ie},\hfill \\ & {\mathbf{c}}_j\in {\mathsf{\Omega }}_j, j=1,2,\dots ,m\hfill \end{array}$$

(24)

where $\mathbf{c}=[{\mathbf{c}}_1,{\mathbf{c}}_2,\dots ,{\mathbf{c}}_m]$. The analytical solution for the trajectory can be obtained by solving the coefficient of the nth-order polynomial for each trajectory. Using Bezier’s hodograph property, the velocity and acceleration of each trajectory can then be calculated.

4. Docking Model Predictive Control

4.1. NMPC Algorithm

The reference trajectory ${\mathsf{\eta }}_d={[x_d,y_d,{\psi }_d]}^T$(${\mathsf{\eta }}_d\left(t\right)\in {\mathbb{R}}^3$) of the horizontal plane and derivatives is smooth and bounded, and ${\mathsf{\eta }}_d$, ${\dot{\mathsf{\eta }}}_d$, and ${\ddot{\mathsf{\eta }}}_d$ have the upper limits of $\Vert {\mathsf{\eta }}_d{\Vert }_{\infty }\le {\overline{\eta }}_d$, $\Vert {\dot{\mathsf{\eta }}}_d{\Vert }_{\infty }\le {\overline{\eta }}_{d1}$, and $\Vert {\ddot{\mathsf{\eta }}}_d{\Vert }_{\infty }\le {\overline{\eta }}_{d2}$, respectively.

We extend the augmented ${\mathsf{\eta }}_d\left(t\right)$ to the reference system so that the control system has a feasible reference state ${\mathsf{\xi }}_d(t)={[x_d,y_d,{\psi }_d,u_d,v_d,r_d]}^T$ (${\mathsf{\xi }}_d\left(t\right)\in {\mathbb{R}}^6$).

Therefore, as derived from Equation (1),

$${\mathbf{v}}_d={\mathfrak{R}}^{-1}\left({\mathsf{\eta }}_d\right){\dot{\mathsf{\eta }}}_d$$

(25)

where ${\mathbf{v}}_d={[u_d,v_d,r_d]}^T$ and $\mathfrak{R}(\mathsf{\eta })=\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]$.

The following can be derived from (3):

$${\mathsf{\tau }}_d=\mathbf{M}{\dot{\mathbf{v}}}_d+\mathbf{C}{\mathbf{v}}_d+\mathbf{D}{\mathbf{v}}_d$$

(26)

where ${{\mathsf{\tau }}_d=[{\tau }_{ud},{\tau }_{vd},{\tau }_{rd}]}^T$, and ${\dot{\mathbf{v}}}_d$ can be solved by the differential operation of ${\mathbf{v}}_d$.

Under the condition that ${\mathsf{\Theta }}^T\mathsf{\Theta }$ is not singular, the Moore–Penrose pseudo-inverse approach is applied to address the thrust allocation issue and acquire control for each thruster, as follows:

$${\mathbf{u}}_d=({\mathsf{\Theta }}^T\mathsf{\Theta }{)}^{-1}{\mathsf{\Theta }}^T{\mathsf{\tau }}_d={\mathsf{\Theta }}^{\prime }{\mathsf{\tau }}_d$$

(27)

The cost function integrates the incremental control input and measures the discrepancy between the predicted and anticipated trajectories. The control law of the NMPC can be established [27] as follows:

$$\begin{array}{c}\underset{\widehat{\mathbf{u}}\in S\left(\delta \right)}{\mathrm{m}\mathrm{i}\mathrm{n}}{J}_2={\int }_0^{T_p} \left({\parallel \tilde{\mathsf{\xi }}(s)\parallel }_{\mathbf{Q}}^2+{\parallel \widehat{\mathbf{u}}(s)\parallel }_{\mathbf{R}}^2\right)ds+{\parallel \tilde{\mathsf{\xi }}(T_p)\parallel }_{\mathbf{P}}^2\\ s.\mathrm{t}. \dot{\widehat{\mathsf{\xi }}}(s)=f(\widehat{\mathsf{\xi }}(s),\widehat{\mathbf{u}}(s))\\ \widehat{\mathsf{\xi }}(0)=\mathsf{\xi }(t_0)\\ \widehat{\mathsf{\xi }}(s)\in {\Omega }_j,j=1,2,\dots ,m\\ \left|\widehat{\mathbf{u}}(s)\right|\le {\mathbf{u}}_{max}\\ {\displaystyle \frac{\partial V}{\partial \mathsf{\xi }}}f(\widehat{\mathsf{\xi }}(0),\widehat{\mathbf{u}}(0))\le {\displaystyle \frac{\partial V}{\partial \mathsf{\xi }}}f(\widehat{\mathsf{\xi }}(0),l(\widehat{\mathsf{\xi }}(0)))\end{array}$$

(28)

where $\widehat{\mathbf{u}}(s)$ is the predictive optimal control; $\widehat{\mathsf{\xi }}(s)$ is the corresponding predictive state, which can be deduced from $\mathsf{\xi }\left(t_0\right)$ using the system model; $\tilde{\mathsf{\xi }}=\widehat{\mathsf{\xi }}-{\mathsf{\xi }}_d$ is the state error; $\tilde{\mathbf{u}}=\widehat{\mathbf{u}}-{\mathbf{u}}_d$ is the control error; $\delta$ is the sampling period; and $T_p=N·\delta$ is the prediction horizon. The family of functions denoted as $S\left(\delta \right)$ is defined by the sampling period δ. The weighting matrices Q, R, and P are positive-definite diagonal matrices.

However, resolving the optimization problem in Equation (28) is a general nonlinear programming (NLP) problem. Given the nonlinearity of the equality constraints, the problem transforms into a nonconvex problem. We adopted the finite receding horizon approach to implement sequential quadratic programming (SQP).

Remark 1. 

To guarantee precise tracking and seamless control, the first component of the cost function pertains to penalizing tracking errors. This measure ensures trajectory tracking accuracy while also reflecting its dynamic capability. The second control input penalty reflects the requirement for a steady change in the control quantity and makes the trajectory tracking more robust. The third component is the error penalty in the terminal state. Adjusting the weight matrices R, Q, and P should account for both state error and dynamic performance. For instance, increasing matrix Q can effectively reduce the steady-state error, while increasing matrix R will inhibit significant changes in the control quantity.

Remark 2. 

The control of the thruster must be bounded owing to thrust saturation, and the following constraints should be applied:

$$\left|\widehat{\mathbf{u}}\left(s\right)\right|\le {\mathbf{u}}_{max}$$

(29)

where ${\mathrm{u}}_{max}$ is the upper limit of the preset control. By imposing the constraints on the control inputs, this allows the NMPC controller to search for an optimal solution only within the set of control inputs that satisfy the saturation constraints of the thrusters, thus being able to ensure that the obtained control inputs satisfy the saturation constraints of the thrusters.

Remark 3. 

The following constraints should be imposed on the state vector $\widehat{\mathsf{\xi }}$ within the docking corridors ${\Omega }_j$ in the receding horizon to ensure successful and safe docking:

$${\zeta }_{\mu j}^{-}\le {\widehat{\mu }}_j\le {\zeta }_{\mu j}^{+},\hspace{0.33em}\hspace{0.33em}\hspace{0.33em}\begin{array}{cc}\mu \in \{x,y,\psi \}& \end{array}$$

(30)

where ${\widehat{\mu }}_j$ has the lower limit ${\zeta }_{\mu j}^{-}$ and the upper limit ${\zeta }_{\mu j}^{+}$.

Remark 4. 

We propose incorporating a Lyapunov-based nonlinear tracking control auxiliary law and introducing an additional convergence constraint to guarantee the stability of NMPC. In Equation (28), $l(\cdot )$ is an auxiliary tracking control law based on Lyapunov $l\left(\mathsf{\xi }\right)={\mathsf{\Theta }}^{\prime }\overline{l}\left(\mathsf{\xi }\right)$, and $V(\cdot )$ is the Lyapunov function.

Let $\tilde{\mathsf{\eta }}=\mathsf{\eta }-{\mathsf{\eta }}_d$, ${\dot{\mathsf{\eta }}}_r={\dot{\mathsf{\eta }}}_d-\tilde{\mathsf{\eta }}$, and $\mathbf{s}=\dot{\mathsf{\eta }}-{\dot{\mathsf{\eta }}}_r$. Using Equation (1), the following can be derived:

$${\mathbf{v}}_r={\mathfrak{R}}^T\left(\psi \right){\dot{\mathsf{\eta }}}_r$$

(31)

Suppose that the auxiliary control law is $\overline{l}\left(\mathsf{\xi }\right)$, which satisfies ${‖\overline{l}\left(\mathsf{\xi }\right)‖}_{\infty }\le {\tau }_{max}$ to ensure the recursive feasibility and closed-loop stability of NMPC [28], the Lyapunov function $V\left(\mathsf{\xi }\right)$ corresponding to $\overline{l}\left(\mathsf{\xi }\right)$ is

$$V(\mathsf{\xi })={\displaystyle \frac{1}{2}}{\mathbf{s}}^T{\mathbf{M}}^{\prime }(\psi )\mathbf{s}+{\displaystyle \frac{1}{2}}{\tilde{\mathsf{\eta }}}^T{\mathbf{G}}_p\tilde{\mathsf{\eta }}$$

(32)

where ${\mathbf{M}}^{\prime }\left(\psi \right)=\mathfrak{R}\left(\psi \right)\mathbf{M}{\mathfrak{R}}^T\left(\psi \right)$. ${\mathbf{G}}_p$ denotes positive-definite gain matrices.

Take the derivative of $V\left(\mathsf{\xi }\right)$

$$\begin{gathered} \dot{V}\left(\mathsf{\xi }\right)={\mathbf{s}}^T{\mathbf{M}}^{\prime }\left(\mathsf{\psi }\right)\dot{\mathbf{s}}+{\displaystyle \frac{1}{2}}{\mathbf{s}}^T{\dot{\mathbf{M}}}^{\prime }\left(\mathsf{\psi }\right)\mathbf{s}+{\tilde{\mathsf{\eta }}}^{\mathit{T}}{\mathbf{G}}_{\mathbf{p}}\dot{\tilde{\mathsf{\eta }}} \\ =-{\mathbf{s}}^{\mathrm{T}}\left[{\mathbf{C}}^{\prime }+{\mathbf{D}}^{\prime }\right]\mathbf{s}+{\mathbf{s}}^T\mathfrak{R}\left(\mathsf{\psi }\right)\left[\mathsf{\tau }-\mathbf{M}{\dot{\mathbf{v}}}_r-\mathbf{C}{\mathbf{v}}_r-\mathbf{D}{\mathbf{v}}_r\right] \\ +{\displaystyle \frac{1}{2}}{\mathbf{s}}^{\mathrm{T}}{\dot{\mathbf{M}}}^{\prime }\left(\mathsf{\psi }\right)\mathbf{s}-{\tilde{\mathsf{\eta }}}^{\mathrm{T}}{\mathbf{G}}_{\mathrm{p}}\tilde{\mathsf{\eta }}+{\mathbf{s}}^{\mathrm{T}}{\mathbf{G}}_{\mathrm{p}}\tilde{\mathsf{\eta }} \end{gathered}$$

(33)

where ${\mathbf{C}}^{\prime }=\mathfrak{R}\left(\psi \right)\left[\mathbf{C}-\mathbf{M}{\mathfrak{R}}^{\mathrm{T}}\left(\psi \right)\dot{\mathfrak{R}}\left(\psi \right)\right]{\mathfrak{R}}^{\mathrm{T}}\left(\psi \right)$; ${\mathbf{D}}^{\prime }=\mathfrak{R}\left(\psi \right)\mathbf{D}{\mathfrak{R}}^T\left(\psi \right)$ is positive-definite.

Therefore, if we choose the following control law:

$$\overline{l}\left(\mathsf{\xi }\right)=\mathbf{M}{\dot{\mathbf{v}}}_r+\mathbf{C}{\mathbf{v}}_r+\mathbf{D}{\mathbf{v}}_r-{\mathfrak{R}}^T{\mathbf{G}}_p\tilde{\mathsf{\eta }}-{\mathfrak{R}}^T{\mathbf{G}}_d\mathbf{s}$$

(34)

where ${\mathbf{G}}_d$ denotes positive-definite gain matrices.

The derivative of the Lyapunov function $\dot{V}\left(\mathsf{\xi }\right)$ is

$$\dot{V}\left(\mathsf{\xi }\right)=-{\mathbf{s}}^T\left[{\mathbf{D}}^{\prime }+{\mathbf{G}}_d\right]\mathbf{s}-{\tilde{\mathsf{\eta }}}^T{\mathbf{G}}_p\tilde{\mathsf{\eta }}$$

As $\dot{V}\left(\mathsf{\xi }\right)\le 0$, it can be inferred that the equilibrium points ${[\tilde{\mathsf{\eta }},\mathbf{s}]}^T={[\mathbf{0},\mathbf{0}]}^T$ of the system remain globally asymptotically stable when subjected to the control law in Equation (34).

4.2. Integrating Trajectory Generation and Control

Tracking control based on NMPC was implemented in the receding horizon, combined with a trajectory generation and optimization algorithm. The prediction horizon is $T_p=N·\delta$, where $N$ is fixed and $\delta$ is the sample time. If $T_g$ is set as $T_g=M·T_p$, each $M$ prediction period $T_p$ serves a trajectory optimization period $T_g$, enabling the synchronous execution of trajectory optimization calculations.

The initial state ${\mathsf{\eta }}_0={[x_0,y_0,{\psi }_0]}^T$ at the current time was obtained through visual positioning. The reference trajectory ${\mathsf{\rho }}_{di}$ for the subsequent period $T_g$ was generated using Equation (19), and the reference state ${\mathsf{\xi }}_d\left(t\right)$ was calculated. The current system state $\mathsf{\xi }\left(t\right)$ was reevaluated from visual guidance and sensors to solve the NMPC problem and obtain the optimal control sequence ${\mathbf{U}}^{\ast }$. Taking only the first control ${\mathbf{u}}_0^{\ast }$ for the current cycle δ, the trajectory optimization and tracking control processes were iterated until the docking procedure was successfully accomplished.

The integrated docking algorithm for trajectory generation and tracking control is summarized in Algorithm 1.

Algorithm 1: Docking Trajectory Generation and Control Algorithm

1: The initial state of the ROV is ${\mathsf{\eta }}_0$

2: For ($i=0;i\le M;i++$)

3:  At this moment the state of the ROV is ${\mathsf{\eta }}_i$.

4:  Calculate the reference trajectory using Equation (24). Let ${\mathsf{\rho }}_{di}$ be the solution.

5:  For ($k=0;k\le N;k++$)

6:    Calculate the reference state ${\mathsf{\xi }}_d\left(t\right)$ from Equations (25)–(27) with ${\mathsf{\eta }}_{di}$.

7:    Re-evaluate the present state $\mathsf{\xi }\left(t\right)$ from visual guidance and sensors;

8:    Recalculate the objective function, as stated in Equation (28);

9:    Solve the NMPC problem in Equation (28) with $\mathsf{\xi }\left(t_0\right)=\mathsf{\xi }\left(t\right)$;

10:     Obtain the optimal solution sequence ${\mathbf{U}}^{\ast }=[{\mathbf{u}}_0^{\ast },\dots ,{\mathbf{u}}_{N-1}^{\ast }{]}^T$;

11:     Perform the execution of ${\mathbf{u}}_0^{\ast }$ for only one sampling period: $\mathbf{u}\left(t\right)={\mathbf{u}}_0^{\ast }$;

12:     Set $t=t+\delta$ at the next time instant and repeat from Step 6 onwards.

13:  end

14: end

15: Repeat the above process until $\mathsf{\eta }\left(t\right)=\mathit{E}$.

Remark 5. 

Notably, for the NMPC, only N steps of the reference state are utilized to formulate the optimization problem at each sampling time $\delta$. According to Algorithm 1, each reference trajectory is generated from the previous M∙N step. The reference track serves as a predefined trajectory, albeit dynamically generated as the ROV progresses forward. The closed-loop stability of the NMPC remains unaffected by the receding trajectory generation.

4.3. Stability Analysis

Next, the recursive feasibility of Equation (28) is analyzed, and the closed-loop stability is investigated using Algorithm 1.

Lemma 1. 

The bounded Coriolis and centripetal matrix $\mathbf{C}\left(\mathbf{v}\right)$ and the damping matrix $\mathbf{D}\left(\mathbf{v}\right)$ fulfill the following conditions: $\Vert \mathbf{C}\left(\mathbf{v}\right){\Vert }_{\infty }\le \overline{C}$ and $\Vert \mathbf{D}\left(\mathbf{v}\right){\Vert }_{\infty }\le \overline{D}$. Where $\overline{C}=2\overline{M}(\sqrt{2}{\overline{\eta }}_{d1}+2\sqrt{2}\Vert \mathit{\lambda }(0\left){\Vert }_2\right)$, $\overline{D}={\overline{D}}_1+{\overline{D}}_2\left(\sqrt{2}{\overline{\eta }}_{d1}+2\sqrt{2}\Vert \mathit{\lambda }\left(0\right){\Vert }_2\right)$, $\mathit{\lambda }\left(t\right)={\left(\stackrel{\mathrm{~}}{\mathsf{\eta }},\mathbf{s}\right)}^T$, $\overline{M}=\mathit{max}\left\{M_{\dot{u}},M_{\dot{v}},M_{\dot{r}}\right\}$, ${\overline{D}}_1=\mathit{max}\left\{\left|X_u\right|,\left|Y_v\right|,\left|N_r\right|\right\}$, and ${\overline{D}}_2=\mathit{max}\left\{X_{\left|u\right|u|},Y_{\left|v\right|v},N_{\left|r\right|r}\right\}$.

The proof of Lemma 1 is presented in Appendix A.

Theorem 1. 

For $l\left(\mathsf{\xi }\right)={\mathsf{\Theta }}^{\prime }\overline{l}\left(\mathsf{\xi }\right)$, if Equation (35) holds, then NMPC is recursively feasible, i.e., $\left|l\left(\widehat{\mathsf{\xi }}\right)\right|\le {\mathbf{u}}_{max}$ and ${{\mathbf{u}}_{max}=u}_{max}\mathbf{I}$. Suppose the control gain matrices ${\mathbf{G}}_p=\mathit{diag}\left\{g_{pi}\right\}$ and ${\mathbf{G}}_d=\mathit{diag}\left\{g_{di}\right\}$ are positive-definite. Denote ${\overline{g}}_p=\mathit{max}\left\{g_{pi}\right\}$ and ${\overline{g}}_d=\mathit{max}\left\{g_{di}\right\}$.

$$\overline{M}{\overline{v}}_{r1}+(\overline{C}+\overline{D}){\overline{v}}_r+\sqrt{2}\left({\overline{g}}_p+{\overline{g}}_d\right){‖\mathsf{\lambda }\left(0\right)‖}_2\le {\tau }_{max}$$

(35)

where ${\overline{v}}_r=\sqrt{2}\left({\overline{\eta }}_{d1}+\Vert \mathit{\lambda }\left(0\right){\Vert }_2\right)$; ${\overline{v}}_{r1}=\sqrt{2}{\overline{\eta }}_{d2}+2{\overline{\eta }}_{d1}^2+\left(2\sqrt{2}+6{\overline{\eta }}_{d1}\right)\parallel \mathit{\lambda }\left(0\right){\parallel }_2+4\parallel \mathit{\lambda }\left(0\right){\parallel }_2^2$.

Proof of Theorem 1. 

Take the infinite norm of Equation (31) combined with $\Vert {\dot{\mathsf{\eta }}}_d{\Vert }_{\infty }\le {\overline{\eta }}_{d1}$ and $\Vert \widehat{\tilde{\mathsf{\eta }}}{\Vert }_{\infty }\le \Vert \widehat{\mathsf{\lambda }}{\Vert }_{\infty }\le \Vert \mathsf{\lambda }\left(0\right){\Vert }_2$ to achieve

$$\begin{gathered} ‖{\widehat{\mathbf{v}}}_r{\Vert }_{\infty }\le ‖{\mathfrak{R}}^T\left(\psi \right){\Vert }_{\infty }‖{\dot{\widehat{\mathsf{\eta }}}}_r{\Vert }_{\infty }\le \sqrt{2}‖{\dot{\mathsf{\eta }}}_d-\widehat{\tilde{\mathsf{\eta }}}{\Vert }_{\infty } \\ \le \sqrt{2}\left(\Vert {\dot{\mathsf{\eta }}}_d{\Vert }_{\infty }+\Vert \widehat{\tilde{\mathsf{\eta }}}{\Vert }_{\infty }\right)\le \sqrt{2}\left({\overline{\eta }}_{d1}+\Vert \mathsf{\lambda }\left(0\right){\Vert }_2\right) \end{gathered}$$

(36)

The derivative of ${\mathbf{v}}_r$ in Equation (31) is obtained as follows:

$${\dot{\mathbf{v}}}_r={\mathfrak{R}}^T\left(\psi \right){\ddot{\mathsf{\eta }}}_r-r\mathsf{H}\left(\psi \right){\dot{\mathsf{\eta }}}_r$$

(37)

where $\mathsf{H}(\psi )={\displaystyle \frac{\partial {\mathfrak{R}}^T\left(\psi \right)}{\partial \psi }}$ and $\Vert \mathsf{H}\left(\psi \right){\Vert }_{\infty }\le \sqrt{2}$.

Take the infinite norm of Equation (37) combined with $\Vert {\ddot{\mathsf{\eta }}}_d\left(t\right){\Vert }_{\infty }\le {\overline{\eta }}_{d2}$ to give

$$\begin{array}{l}\Vert {\dot{\widehat{\mathbf{v}}}}_r{\Vert }_{\infty }\le \Vert {\mathfrak{R}}^T(\psi )({\ddot{\mathsf{\eta }}}_d-\dot{\widehat{\tilde{\mathsf{\eta }}}}){\Vert }_{\infty }+\Vert r\mathsf{H}(\psi )({\dot{\mathsf{\eta }}}_d-\widehat{\tilde{\mathsf{\eta }}}){\Vert }_{\infty }\\ \le \sqrt{2}(\Vert {\ddot{\mathsf{\eta }}}_d{\Vert }_{\infty }+\Vert \dot{\widehat{\tilde{\mathsf{\eta }}}}){\Vert }_{\infty })+\sqrt{2}\Vert r{\Vert }_{\infty }(\Vert {\dot{\mathsf{\eta }}}_d{\Vert }_{\infty }+\Vert \widehat{\tilde{\mathsf{\eta }}}){\Vert }_{\infty })\\ \le \sqrt{2}({\overline{\eta }}_{d2}+2\Vert \mathsf{\lambda }(0){\Vert }_2)+\sqrt{2}(\sqrt{2}{\overline{\eta }}_{d1}+2\sqrt{2}\Vert \mathsf{\lambda }(0){\Vert }_2)({\overline{\eta }}_{d1}+\Vert \mathsf{\lambda }(0){\Vert }_2)\\ \le \sqrt{2}{\overline{\eta }}_{d2}+2{\overline{\eta }}_{d1}^2+\left(2\sqrt{2}+6{\overline{\eta }}_{d1}\right)\Vert \mathsf{\lambda }(0){\Vert }_2+4\Vert \mathsf{\lambda }(0){\Vert }_2^2\end{array}$$

(38)

Taking the infinite norm of Equation (34) with Lemma 1 gives

$$\Vert \overline{l}\left(\widehat{\mathsf{\xi }}\right){\Vert }_{\infty }\le \overline{M}{\overline{v}}_{r1}+(\overline{C}+\overline{D}){\overline{v}}_r+\sqrt{2}\left({\overline{g}}_p+{\overline{g}}_d\right)\Vert \mathsf{\lambda }\left(0\right){\Vert }_2$$

(39)

Satisfying Equation (35) leads to

$$\Vert l\left(\widehat{\mathsf{\xi }}\right){\Vert }_{\infty }\le \Vert {\mathsf{\Theta }}^{\prime }{\Vert }_{\infty }\Vert \overline{l}\left(\widehat{\mathsf{\xi }}\right){\Vert }_{\infty }\le \Vert {\mathsf{\Theta }}^{\prime }{\Vert }_{\infty }{\tau }_{max}$$

(40)

If the thrust allocation algorithm is always feasible, Equation (41) must be satisfied as such

$${\tau }_{max}\le {\displaystyle \frac{u_{max}}{\Vert {\mathsf{\Theta }}^{\prime }{\Vert }_{\infty }}}$$

(41)

When $\Vert l\left(\widehat{\mathsf{\xi }}\right){\Vert }_{\infty }\le u_{max}$ will always be satisfied. We complete the proof. □

The auxiliary controller serves as the initial estimate for the optimization problem in Equation (28); however, it has not yet been implemented in the control system. The results demonstrate that the auxiliary controller ensures control feasibility, whereas NMPC enhances control performance.

Theorem 2. 

Assume the existence of an auxiliary control law $\overline{l}\left(\mathsf{\xi }\right)$. When $l\left(\mathsf{\xi }\right)={\mathsf{\Theta }}^{\prime }\overline{\mathit{l}}\left(\mathsf{\xi }\right)$, the closed-loop system exhibits asymptotic stability at the equilibrium point $\stackrel{\mathrm{~}}{\mathsf{\xi }}=0$, and $V\left(\mathsf{\xi }\right)$ is the corresponding Lyapunov function. The closed-loop system employing Algorithm 1 is asymptotically stable, ensuring convergence of the docking system to the desired trajectory while maintaining recursive feasibility.

Proof of Theorem 2. 

Given the existence of a Lyapunov function $V\left(\mathsf{\xi }\right)$ that is continuously differentiable and radically unbounded, as stated in Equation (33), according to the converse Lyapunov theorems [29], functions ${\kappa }_i(\cdot ),i=1,2,3$ exist, belonging to the ${\mathcal{K}}_{\infty }$ class, and satisfy the following inequalities: ${\kappa }_1\left(‖\mathsf{\xi }‖\right)\le V\left(\mathsf{\xi }\right)\le {\kappa }_2\left(‖\mathsf{\xi }‖\right)$ and ${\displaystyle \frac{\partial V}{\partial \mathsf{\xi }}}f(\mathsf{\xi },l(\mathsf{\xi }\left)\right)\le -{\kappa }_3\left(‖\mathsf{\xi }‖\right)$.

The optimal solution is achieved within a single sampling period owing to the convergence constraints of Equation (28); thus,

$${\displaystyle \frac{\partial V}{\partial \mathsf{\xi }}}f\left(\mathsf{\xi },\mathbf{u}\left(\mathsf{\xi }\right)\right)\le {\displaystyle \frac{\partial V}{\partial \mathsf{\xi }}}f\left(\mathsf{\xi },l\left(\mathsf{\xi }\right)\right)\le -{\kappa }_3\left(‖\mathsf{\xi }‖\right)$$

(42)

According to the Lyapunov arguments [29], the closed-loop system controlled by Algorithm 1 exhibits asymptotic stability when $\tilde{\mathsf{\xi }}=0$ and convergence to the desired trajectory and region of attraction (ROA) $\mathsf{\Lambda }=\left\{\mathsf{\xi }\in {\mathbb{R}}^n\left|\left(\Vert l\left(\widehat{\mathsf{\xi }}\right){\Vert }_{\infty }\le \Vert {\mathsf{\Theta }}^{\prime }{\Vert }_{\infty }{\tau }_{max}\right)\right.\right\}$ is guaranteed. □

5. Results and Discussion

The proposed docking control system was simulated to validate the feasibility and robustness of the autonomous docking strategy and control method presented in this study. It was constructed based on the ROV dynamics model and incorporated the parameters listed in Appendix B.

5.1. Trajectory Generation Test

The docking simulation time was $T=40$ s. The initial position of the ROV relative to the docking station was ${[10,4,2,\pi /3]}^T$. The initial and docking endpoints exhibited zero velocity and acceleration, respectively. The maximum speed was set to $v_{max}=\pm 0.5\mathrm{m}/\mathrm{s}$, and the maximum acceleration was set to $a_{max}={\pm 0.1\mathrm{m}/\mathrm{s}}^2$. The horizontal and vertical angles of view for visual guidance are α and β, respectively, and both are $80°$.

The displacement curves of y, z and the heading angle ψ with respect to x are depicted in Figure 5. The blue boxes represent the designated docking corridor. The size of the blue box diminished as the displacement x between the ROV and docking station decreased, owing to a reduced effective visual positioning area. The circles of different colors represent the waypoints of each segment of the trajectories. The position and higher-order kinetic variables of the trajectory generated and optimized using this method were strictly contained within the designated safety region.

The simulated position, velocity, acceleration, and jerk curves in the x, y, z, and ψ directions exhibited temporal variations, as depicted in Figure 6. The generated trajectory curves exhibited continuity and smoothness while satisfying the constraints of equal amplitude for both velocity and acceleration. When $t=32\mathrm{s}$, the velocity of the x-direction was limited to a maximum of −0.5 m/s.

5.2. Docking Performance Test

The following parameters were used in the simulation. The sampling period was $\delta =0.01\mathrm{s}$, the prediction horizon was $N=5$, the control horizon was $C=5$, and the frequency of trajectory generation was $M=5$. The weight coefficient matrices were $\mathbf{Q}=diag(1\times {10}^3,2\times {10}^3,5\times {10}^3,1\times {10}^2,1\times {10}^2,1\times {10}^2)$, $\mathbf{R}=diag(1\times {10}^{-4},1\times {10}^{-4},1\times {10}^{-4},1\times {10}^{-4})$, and $\mathbf{P}=diag(2\times {10}^6,1\times {10}^7,3\times {10}^4,1\times {10}^1,1\times {10}^1,1\times {10}^1)$. The control gain matrices were ${\mathbf{G}}_p={\mathbf{G}}_d=diag(1,1,1)$.

The initial position of the ROV relative to the docking station was ${[8,1,2,\pi /3]}^T$ and the initial velocity was zero. The docking simulation time was set to $T=60\mathrm{s}$. The maximum thrust amplitude of each propeller was 500 N.

In order to facilitate the simulation comparison and analysis with the current common methods, we adopt the BSC controller from [30,31,32]. The design of the BSC controller is based on Lyapunov theory and backstepping technology.

$${\mathsf{\tau }}_{BSC}\left(\mathbf{x}\right)=\mathbf{M}{\dot{\mathbf{v}}}_r+\mathbf{C}{\mathbf{v}}_r+\mathbf{D}{\mathbf{v}}_r+\mathbf{g}-{\mathbf{R}}^{\mathrm{T}}{\mathbf{K}}_p\tilde{\mathit{\eta }}-{\mathbf{R}}^{\mathrm{T}}{\mathbf{K}}_d\mathbf{s}$$

(43)

where ${\mathbf{K}}_p$ and ${\mathbf{K}}_d$ are user-specified control gain matrices. ${\mathbf{K}}_p={\mathbf{K}}_d=diag(1\times {10}^3,1\times {10}^3,1\times {10}^3,1\times {10}^3)$. This equation is basically the same as the Lyapunov-based auxiliary controller above, but the auxiliary controller is only used to construct the convergence constraints, and the performance of the controller mainly relies on NMPC for the optimization calculation.

The 3D trajectory tracking performance without disturbances is shown in Figure 7. The simulated docking trajectory curves show that the ROV is able to accurately track the docking trajectory and recover autonomously at the docking station. This result validates the proposed method. Figure 8 and Figure 9 depict the position and velocity trajectory comparison curves using different control methods, respectively. In Figure 8 and Figure 9, the red curves represent the expected docking trajectories generated by trajectory planning, while the blue curves correspond to the trajectories controlled by the NMPC. The green curve represents the trajectory controlled by the BSC. In the absence of perturbations, both the NMPC and BSC controllers can effectively drive the ROV along the desired trajectory. Based on the comparison of the trajectory curves obtained from the different control methods, the designed controller is faster than the BSC controller. The NMPC controller demonstrated rapid convergence.

The mean square errors (MSEs) value is used as a performance comparison metric for trajectory tracking control by different algorithms.

$$I_{MSE}={\displaystyle \frac{1}{t}}{\int }_0^T{\left[e\left(t\right)\right]}^{2}dt$$

(44)

The MSEs of x, y, z, and $\psi$ were 4.6562 × 10⁻⁸, 2.8527 × 10⁻⁹, 1.8372 × 10⁻⁸, and 5.6762 × 10⁻⁸, respectively, with the designed NMPC controller. The MSEs of x, y, z, and $\psi$ were 1.6870 × 10⁻⁵, 6.5458 × 10⁻⁵, 5.1530 × 10⁻⁶, and 1.4623 × 10⁻⁶, respectively, with the BSC controller. The proposed NMPC controller exhibited a reduced tracking error. Figure 10 illustrates the controller output regulation for the thrusters.

5.3. Robustness Test

The robustness of docking control against disturbances caused by ocean currents was verified through simulation tests in which errors and disturbances were intentionally introduced. The kinematic and dynamic docking model parameters exhibited a 40% error. Simultaneously, both fixed and sinusoidal current disturbances were introduced. The disturbance of ocean currents was ${[100+30sin(t),100+30sin(t),0,0]}^T$.

The 3D trajectory tracking performance with disturbances is shown in Figure 11. Figure 12 and Figure 13 show the simulation results for the presence of disturbances. When current disturbances are present, the proposed controller demonstrated the ability to effectively guide the ROV towards the desired trajectory, surpassing the tracking control based on the BSC, which exhibited a larger tracking error. Furthermore, minimal errors and fluctuations were observed throughout the tracking process. The MSEs of x, y, z, and $\psi$ were 1.9512 × 10⁻⁵, 1.7129 × 10⁻⁶, 2.5056 × 10⁻⁸, and 9.0077 × 10⁻⁶, respectively, with the designed controller. The MSEs of x, y, z, and $\psi$ were 2.3 × 10⁻², 2.6 × 10⁻², 5.1530 × 10⁻⁶, and 2.7460 × 10⁻⁶, respectively, with the BSC controller. The controller proposed in this study exhibited a significantly reduced tracking error. The experimental results show that the proposed tracking algorithm exhibited rapid and stable convergence compared with the BSC. The reason for this lies in the ability of the NMPC to employ receding optimization, which enables it to dynamically adjust the control gain and compensate for the interference, whereas the BSC operates with a fixed gain controller. The controller outputs of the thrusters in response to the disturbance are shown in Figure 14. The designed controller significantly enhanced the robustness of the trajectory tracking control, thereby demonstrating the effectiveness and flexibility of the docking control algorithm.

Despite the progress achieved on underwater autonomous docking control methods, there are still some limitations in this study which will be addressed in subsequent work.

• The computational cost is relatively high, and multiple optimization calculations for online trajectory generation and trajectory tracking control are required during the operation of the control system, which may face the challenge of insufficient computational capacity for the implementation of the actual ROV embedded computer.
• It is assumed that the system state feedback is accurate and timely in trajectory tracking control. In actual docking, the measurement of system position and velocity relies on the integrated navigation of visual guidance and navigation sensors, especially the low update rate of visual guidance. As a result, the system state feedback suffers from errors, hysteresis, and a low update rate, which are not addressed in this paper.
• Limited by the existing experimental conditions, our research methodology is currently validated mainly by simulation. Subsequently, pool experiments will be conducted to validate our proposed method.

6. Conclusions

We proposed an integrated receding execution approach for trajectory generation, optimization, and tracking control for underwater docking. Trajectory generation was transformed into a rolling optimization problem by incorporating multiple docking constraints and formulating an objective function to minimize snap. The proposed approach enhanced the feasibility, smoothness, and safety of the docking trajectory. An NMPC docking control algorithm was proposed, and the closed-loop stability of the system was demonstrated. The optimization approach employed in this study enhances tracking control performance while effectively addressing docking constraints. The simulation results demonstrate the performance and robustness of the proposed method in the presence of uncertain disturbances. In future research, we will further investigate safe obstacle avoidance control during docking. In addition, we aim to validate the proposed methodology through pooling experiments using real systems. The research findings will contribute to enhancing the safety and overall efficiency of deep-sea exploration and development.