Trajectory Tracking Control of Unmanned Underwater Vehicle Based on Projected Perpendicular Guidance Method with Disturbance Observer

Abstract

Unmanned underwater vehicles (UUVs) possess impressive maneuverability and versatility, but controlling them during trajectory tracking can be challenging due to their susceptibility to external disturbances and perturbations in their model parameters. Additionally, the UUV has four degrees of freedom underwater, but only three control inputs, making it a typical underactuated system. To address these issues, this paper introduces a novel optimize sliding mode control (OPSMC) algorithm grounded in projected perpendicular guidance (PPG). The PPG algorithm transforms the three-degree-of-freedom path trajectory control into two-degree-of-freedom heading tracking control and surge velocity tracking control by designing virtual posture angles. Optimized sliding mode control, based on sliding mode control, improves control precision and reduces control input chattering by constructing optimization functions for control inputs. During trajectory tracking, UUVs are susceptible to external environmental disturbances and perturbations in system model parameters. Disturbance observers are employed to estimate these disturbances and perturbations. Finally, MATLAB/Simulink is used for numerical simulation experiments. The simulation results demonstrate that the PPG algorithm effectively enables underactuated UUVs to achieve trajectory tracking control. The designed optimized sliding mode controller and disturbance observer enhance the control precision and robustness of the system.

1. Introduction

Background and motivation: Autonomous underwater vehicles have emerged as indispensable high-tech tools for delving into and comprehending the depths of the ocean. They are extensively used in a variety of underwater tasks, including seafloor mapping [1], underwater structure inspections [2], military operations [3], and pipeline tracking [4]. Their significance has garnered extensive recognition on a global scale. As a principal avenue of underwater vehicle research, trajectory tracking has garnered escalating interest among scholars.

1.1. Related Work

Underwater vehicles display distinctive traits including robust coupling, nonlinearity, susceptibility to multiple external disturbances, and vulnerability to internal parameter perturbations. In practical engineering scenarios, achieving superior control performance necessitates controllers endowed with rapid convergence rates, formidable resistance to interference, and robust adaptability. For instance, ref. [5] introduced a learning-based optimal control technique, merging optimal backstepping (OB) norms with reinforcement learning (RL) technology to solve the optimal control quandary within transformation systems. Their approach ensures uniform ultimate boundedness (UUB) while optimizing costs. Ref. [6] devised an intelligent control strategy to address actuator saturation issues during trajectory tracking of unmanned underwater vehicles. In [7], a control method based on deep reinforcement learning (DRL) had been proposed for the path-following control of UUVs to enhance control accuracy. Ref. [8] proposed a three-dimensional formation control scheme for torpedo-type UUVs. Meanwhile, Ref. [9] innovatively presented asynchronous multithreading near-end strategy optimization algorithms for path planning and trajectory tracking, effectively adapting to varying mission scenarios of unmanned submersibles. In a different vein, Ref. [10] advanced a robust nonlinear model predictive control (NMPC) approach for UUVs. Ref. [11] introduced a model-free high-order sliding mode controller, achieving finite-time convergence. In [12], a novel global sliding mode controller had been proposed for underwater vehicles with bounded parameter uncertainties and disturbances within limited control inputs. Furthermore, Ref. [13] proposed a flexible dynamic positioning strategy and control method to extend unmanned submersible aircraft’s operational duration. Ref. [14] designed a biomimetic neural dynamics model and a chattering-free sliding mode technique for horizontal trajectory tracking control. Ref. [15] laid out a motion control algorithm founded on deep simulation reinforcement learning for unmanned underwater vehicles. Ref. [16] offered two adaptive integration terminal sliding mode control schemes for unmanned submersibles, the integrating terminal sliding mode (ITSM) and fast ITSM. Ref. [17] crafted a motion control system steering underactuated UUVs along spiral orbits. Lastly, Ref. [18] delved into dynamic modeling and motion simulation research for unmanned ocean platforms, surmounting limitations encountered in the realm of ocean exploration by existing platforms. Ref. [19] designed a robust station-keeping control algorithm based on the sliding mode control (SMC) theory to ensure stability and improved performance of a hovering over-actuated autonomous underwater vehicle despite the presence of model uncertainties and ocean current disturbances in the horizontal plane. In [20], the least squares method and quadratic programming method were used to allocate appropriate thrust to each thruster of the underactuated AUV, ensuring that it maintained the desired static position of UVs permanently under fixed conditions. In [21], the problem of depth controller design for UUVs had been addressed, considering ocean currents and communication delays. A Takagi–Sugeno (T-S) fuzzy modeling method and an event-triggered integral sliding mode control strategy have been proposed.

Navigating the high-speed motion state of underactuated UUV poses challenges stemming from system model parameter uncertainty and disturbances. In practical scenarios, the hydrodynamic coefficients of a UUV is intricately intertwined with their intrinsic properties, motion state characteristics, and the surrounding fluid environment. Ascertaining these coefficients with precision proves arduous, compounded by their susceptibility to external environmental influences during operational phases. In [22], an adaptive fuzzy nonlinear integral sliding mode control strategy based on extended state observer (ESO) was proposed to solve the trajectory tracking problem of a UUV with unknown external disturbance and model parameter uncertainty. In [23], the trajectory tracking control of underwater vehicles in three-dimensional space was studied, which can make the position and direction of the system usable in the presence of disturbances. In [24], a dynamic output feedback fault-tolerant control scheme was proposed to achieve trajectory tracking of a UUV in the event of actuator failure, actuator saturation, external disturbances, and other system uncertainties. In [25], in the presence of output constraints and environmental disturbances, Barrier Lyapunov functions are employed to design the controller, addressing control input constraints, and radial basis function neural networks are utilized to mitigate external disturbances.

In [26], the trajectory tracking control problem of an underactuated UUV with model parameter perturbations and constant unknown current on the horizontal plane was proposed. In [27], the trajectory tracking control of an underactuated UUV was proposed, and the radial basis function neural network was used to approximate the nonlinear uncertainty. The effectiveness and robustness of the proposed method were verified through simulation examples. In [28], a MPC method based on quantum behavior particle swarm optimization and SMC was proposed to solve the dynamic trajectory tracking problem of UUVs in a three-dimensional underwater environment.

1.2. Contribution and Article Structure

Referring to the literature reviewed above, this paper proposes a trajectory tracking control of UUVs based on the PPG method with disturbance observer, with the following key innovations:

• We propose a novel Projected Perpendicular guidance method that converts trajectory tracking information into virtual heading and virtual velocity control.
• We propose a novel optimization sliding mode control algorithm based on a disturbance observer is proposed, which not only reduces the impact of external environmental disturbances on UUV but also enhances control precision.

The paper is structured as follows: In Section 2, the UUV model is introduced. Section 3 provides detailed insights into the Trajectory Tracking guidance loop, succeeded by the unveiling of the Trajectory Tracking control loop in Section 4. Section 5 provides simulation validation, supporting the proposed methodology. Finally, Section 6 summarizes conclusions and subsequent discussions.

2. Nonlinear UUV Model

2.1. UUV Kinematics Model

The motion control problem of the underwater unmanned vehicle can be divided into two parts:

(1)

The kinematics part, which simply describes the geometric relationship of the underwater unmanned vehicle motion, including its position, speed, acceleration, and the change in attitude, angular velocity, and angular acceleration with time;

(2)

The dynamics section mainly analyzes the impact of force and torque on the motion status information of a UUV. For the three-dimensional motion of a UUV, in order to systematically and accurately describe their position and attitude information in three-dimensional space, they can be represented by the coordinate values of the origin $O_b$ of the motion coordinate system on a fixed coordinate system (x, y, z), as well as the three attitude angles ($\varphi$, $\theta$, $\psi$) of the motion coordinate system relative to the fixed coordinate system, as specifically shown in Figure 1 [29]. The specific model is as follows:

$$\left\{\begin{array}{c}\dot{\varphi }=p+qsin\varphi tan\theta +rcos\varphi tan\theta \hfill \\ \dot{\theta }=qcos\varphi -rsin\varphi \hfill \\ \dot{\psi }=\left(qsin\varphi +rcos\varphi \right)/cos\theta \hfill \\ \dot{x}=ucos\theta cos\psi +v\left(sin\varphi sin\theta cos\psi -cos\varphi sin\psi \right)+w\left(sin\varphi sin\theta cos\psi +sin\varphi sin\psi \right)\hfill \\ \dot{y}=ucos\theta sin\psi +v\left(sin\varphi sin\theta sin\psi +cos\varphi cos\psi \right)+w\left(\sin \varphi sin\theta sin\psi -sin\varphi cos\psi \right)\hfill \\ \dot{z}=-usin\theta +vsin\psi cos\theta +wcos\theta sin\psi \hfill \end{array}\right.$$

(1)

In Equation (1), the variables x, y, and z correspond to the northern, eastern, and depth coordinates, respectively. The symbols $\varphi$, $\theta$, and $\psi$ represent the roll, pitch, and heading angles, respectively. The quantities u, v, and w denote the longitudinal, lateral, and vertical velocities, respectively. The parameters p, q, and r, respectively, signify the roll angular velocity, pitch angular velocity, and heading angular velocity.

2.2. UUV Simplified Model

Since rolling and pitching degrees of freedom are not required for UUV trajectory tracking, they are omitted from the original six dgrees [29,30] of freedom model. Torque is used as control inputs, resulting in a simplified four degrees of freedom UUV model.

$$\left\{\begin{array}{c}\dot{x}=ucos\left(\psi \right)-vsin\left(\psi \right)\hfill \\ \dot{y}=usin\left(\psi \right)+vcos\left(\psi \right)\hfill \\ \dot{\psi }=r\hfill \\ \dot{z}=w\hfill \\ \dot{u}=\left(mvr+\frac{\rho }{2}{L}^4{X}_{rr}{r}^2+\frac{\rho }{2}{L}^3{X}_{vr}vr+\frac{\rho }{2}{L}^2{X}_{vv}{v}^2+\frac{\rho }{2}{L}^2{X}_{ww}{w}^2+\frac{\rho }{2}{C_d}_0{L}^{2}u+{\tau }_u+f_u\right)/m_{11}\hfill \\ \dot{v}=\left(\frac{\rho }{2}{L}^3{Y}_{r}ur+\frac{\rho }{2}{L}^3{Y}_{wr}wr-mur+\frac{\rho }{2}{L}^2{Y}_{v}uv+\frac{\rho }{2}{L}^2{Y}_{v\mathrm{w}}vw+f_v\right)/m_{22}\hfill \\ \dot{w}=\left(\frac{\rho }{2}{L}^4{Z}_{rr}{r}^2+\frac{\rho }{2}{L}^3{Z}_{vr}vr+\frac{\rho }{2}{L}^2{Z}_{w}uw+\frac{\rho }{2}{L}^2{Z}_{vv}{v}^2+\frac{\rho }{2}{L}^2{Z}_{wn}uw+{\tau }_w+f_w\right)/m_{33}\hfill \\ \dot{r}=\left(\frac{\rho }{2}{L}^4\left[N_{r}ur+N_{wr}wr\right]+\frac{\rho }{2}{L}^3{N}_{v}uv+\frac{\rho }{2}{L}^3{N}_{vw}vw+\frac{\rho }{2}{L}^3{u}^2{N}_{\mathrm{prop}}+{\tau }_r+f_r\right)/I_{11}\hfill \end{array}\right.$$

(2)

In Equation (2), x, y, and z represent the location of the UUV, $\rho$ is the water density, and the UUV length of UUV is denoted as L. m is the quality of the UUV. $m_{11}=m-X_{\dot{u}}\frac{\rho }{2}{L}^3$, $m_{22}=m-\frac{\rho }{2}{L}^3{Y}_{\dot{v}}$, $m_{33}=m-\frac{\rho }{2}{L}^3{Z}_{\dot{w}}$, $I_{11}=I_z-\frac{\rho }{2}{L}^5{N}_{\dot{r}}$, $\psi$ is the heading angle of the submarine, and $X_{\dot{u}}$,$X_{rr}$, $X_{vr}$, $X_{vv}$, $X_{ww}$, $X_{prop}$, $Y_{\dot{v}}$, $Y_{\dot{v}}$, $Y_r$, $Y_{wr}$, $Y_v$, $Y_{vw}$, $Z_{\dot{W}}$, $Z_{\dot{r}r}$, $Z_{\dot{v}r}$, $Z_w$, $Z_{vv}$, $Z_{wn}$, $N_{\dot{r}}$, $N_r$, $N_{wr}$, $N_v$, $N_{vw}$, and $N_{\mathrm{prop}}$ represent the hydrodynamic coefficients of each degree of freedom. The specific values can be obtained from [29]. ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ symbolize control inputs with varying degrees of freedom. $f_u$, $f_v$, $f_w$, and $f_r$ represent external disturbances with different degrees of freedom.

2.3. Control Objectives and Paper Assumptions

The control objective of this article is to design a control law ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ to enable a UUV to track the three-dimensional reference trajectory. Figure 2 is the overall control structure diagram.The assumptions required for this article are as follows:

(1)

The external disturbance f is bounded, i.e., $\left|f_u\right|\le f_{umax}$, $\left|f_v\right|\le f_{vmax}$, $\left|f_w\right|\le f_{wmax}$, $\left|f_r\right|\le f_{rmax}$.

(2)

The states x, y, $\psi$, z, u, v, r, w of the UUV are known.

(3)

The change rate of external disturbances values $f_u$, $f_v$, $f_r$, $f_w$ are bounded, and their values are very small, i.e., ${\dot{f}}_u\le \left|{\dot{f}}_u\right|\le \sigma$, ${\dot{f}}_v\le \left|{\dot{f}}_v\right|\le \sigma$, ${\dot{f}}_r\le \left|{\dot{f}}_r\right|\le \sigma$, ${\dot{f}}_w\le \left|{\dot{f}}_w\right|\le \sigma$.

Among them, Assumption 1 is a necessary condition for system controllability, if external disturbances are unbounded, and no matter how you design the controller, it cannot effectively accomplish the control task [31,32]. Assumption 2 can be obtained through electronic devices in the ship. In Assumption 3, $\sigma \in R^{+}$, since the external environment does not undergo sudden changes, it is reasonable to assume that the rate of change in the external environment is less than a very small value.

3. UUV Guidance Loop

3.1. Virtual Speed and Heading Angle

This section introduces a pioneering approach known as the PPG method. This method involves devising a virtual control law to transform the task of trajectory tracking control into separate speed and heading control aspects [33].

Based on the coordinate relationship, we can infer that

$$\left\{\begin{array}{c}{\vartheta }_k=arctan\left({\dot{y}}_d/{\dot{x}}_d\right)\hfill \\ b_1=y_d-k_1{x}_d\hfill \\ b_2=y_d-k_2{x}_d\hfill \\ k_1=tan{\vartheta }_k\hfill \\ k_2=-1/k_1\hfill \end{array}\right.$$

(3)

As depicted in Figure 3, by considering the separation between a point and a straight line, we can deduce the following equation:

$$\left\{\begin{array}{c}{x}_e=\left({A_{\kappa }}_{1}x+{B_{\kappa }}_{1}y+{C_{\kappa }}_1\right)/\sqrt{{A_{\kappa }}_1^2+{B_{\kappa }}_1^2}\hfill \\ y_e=\left({A_{\kappa }}_{2}x+{B_{\kappa }}_{2}y+{C_{\kappa }}_2\right)/\sqrt{{A_{\kappa }}_2^2+{B_{\kappa }}_2^2}\hfill \end{array}\right.$$

(4)

In Equation (4), by taking ${A_{\kappa }}_1=k_1$, ${B_{\kappa }}_1=-1$, ${C_{\kappa }}_1=b_1$, ${A_{\kappa }}_2=k_2$, ${B_{\kappa }}_2=-1$, ${C_{\kappa }}_1=b_2$, the equation can be simplified.

$$\left\{\begin{array}{c}{x}_e=-xcos{\vartheta }_k-ysin{\vartheta }_k+x_{\mathrm{d}}cos{\vartheta }_k+y_{\mathrm{d}}sin{\vartheta }_k\hfill \\ y_e=xsin{\vartheta }_k-ycos{\vartheta }_k-x_{\mathrm{d}}sin{\vartheta }_k+y_{\mathrm{d}}cos{\vartheta }_k\hfill \end{array}\right.$$

(5)

By taking the derivative of time from Equation (5), it can be obtained that

$$\left\{\begin{array}{c}{\dot{x}}_e=-\sqrt{u^2+v^2}\cos \left(\psi +\beta -{\vartheta }_k\right)+{\xi }_x\hfill \\ {\dot{y}}_e=-\sqrt{u^2+v^2}sin\left(\psi +\beta -{\vartheta }_k\right)+{\xi }_y\hfill \\ {\xi }_x={\dot{\vartheta }}_k\left(xsin{\vartheta }_k-ycos{\vartheta }_k-x_{\mathrm{d}}sin{\vartheta }_k+y_{\mathrm{d}}cos{\vartheta }_k\right)+{\dot{x}}_{\mathrm{d}}cos{\vartheta }_k+{\dot{y}}_{\mathrm{d}}\sin {\vartheta }_k\hfill \\ {\xi }_y={\dot{\vartheta }}_k\left(xcos{\vartheta }_k+ysin{\vartheta }_k-x_{\mathrm{d}}cos{\vartheta }_k-y_{\mathrm{d}}sin{\vartheta }_k\right)-{\dot{x}}_{\mathrm{d}}sin{\vartheta }_k+{\dot{y}}_{\mathrm{d}}cos{\vartheta }_k\hfill \end{array}\right.$$

(6)

According to Equation (6), setting the virtual control law can obtain

$$\left\{\begin{array}{c}{u}_{\mathrm{d}}=\sqrt{\left(\frac{{k_x}_1\left(tanh{k_x}_2{x}_e\right)+{\xi }_x}{cos\left(\psi +\beta -{\vartheta }_k\right)}\right)-v^2}\hfill \\ {\psi }_{\mathrm{d}}=arcsin\left({k_y}_{1}tanh\left({k_y}_2{y}_{\mathrm{e}}\right)+{\xi }_y/\sqrt{u^2+v^2}\right)\hfill \end{array}\right.$$

(7)

In Equation (7), $k_{x1}$ is used to compress the coordinates to prevent excessive weighting due to a large $x_e$; $k_{x2}$ is used to amplify the gain of s, ensuring that even when s is small, the weight can still have a significant response, thereby improving approximation accuracy. Similarly, the functions of parameters $k_{y1}$ and $k_{y2}$ are analogous to those of $k_{x1}$ and $k_{x2}$.

Remark 1.

Constructing path deviation $x_e=x_{\mathrm{d}}-x$, $y_e=y_{\mathrm{d}}-y$, $z_e=z_{\mathrm{d}}-z$ through reference [6], it is difficult to track certain paths, such as when the reference trajectory is parallel to the x-axis, and the deviation of $y_e$ is always not zero, so it is not possible to track reference trajectories parallel to x. Compared to [6], this paper constructs the path deviation by constructing the distance from a point to a straight line, which is applicable to any reference path.

3.2. Guidance Stability Proof

To ascertain the stability of the derived virtual velocity and heading angle, the Lyapunov function $V_1$ is introduced, defined as follows:

$$V_1=\frac{1}{2}{x}_e^2+\frac{1}{2}{y}_e^2$$

(8)

By taking its derivative and incorporating Equations (6) and (7), we can obtain

$$\begin{array}{c}{\dot{V}}_1=x_e{\dot{x}}_e+y_e{\dot{y}}_e\hfill \\ =-{k_x}_1{x}_e\left(tanh{k_x}_2{x}_e\right)-{k_y}_1{y}_{\mathrm{e}}tanh\left({k_y}_2{y}_{\mathrm{e}}\right)\hfill \end{array}$$

(9)

Due to the fact that the hyperbolic tangent function is an odd function, so $x_e\left(tanh{x}_e\right)\ge$ 0 and $y_{\mathrm{e}}tanh\left(y_{\mathrm{e}}\right)\ge 0$, therefore ${\dot{V}}_1\le 0$, the system is stable, the designed virtual control law can make the error and approach zero.

4. UUV Controller Loop

4.1. Controller Design

In this section, a sliding mode controller [34,35] will be developed to regulate the speed, heading angle, and depth of the UUV.

4.1.1. Speed Tracking Control Law

To control the UUV’s surge speed, one commence by defining the error between the actual velocity and the virtual velocity. The error is defined as depicted in Equation (10).

$$\left\{\begin{array}{c}{u}_e=u-u_{\mathrm{d}}\hfill \\ {\dot{u}}_e=\dot{u}-{\dot{u}}_{\mathrm{d}}\hfill \end{array}\right.$$

(10)

In order to stabilize $u_e$, the error towards zero, the convergence law described by Equation (11) is employed.

$${\dot{u}}_e=-k_u{u}_e$$

(11)

The surge speed control law obtained is

$${\tau }_u=m_{11}\left(-k_u{u}_e+{\dot{u}}_{\mathrm{d}}\right)-mvr-\frac{\rho }{2}{L}^4{X}_{rr}{r}^2-\frac{\rho }{2}{L}^3{X}_{vr}vr-\frac{\rho }{2}{L}^2{X}_{vv}{v}^2-\frac{\rho }{2}{L}^2{X}_{ww}{w}^2-\frac{\rho }{2}{L}^2{u}^2{X}_{\mathrm{prop}}-{\widehat{f}}_u$$

(12)

In Equation (12), ${\tau }_u$ is the control law of UUVs on the virtual velocity, ${\widehat{f}}_u$ is the $f_u$ observed value through the disturbance observer.

4.1.2. Heading Tracking Control Law

To control the UUV’s heading angle, we commence by defining the error between the actual heading angle and the virtual heading angle. The error is defined as depicted in Equation (13).

$$\left\{\begin{array}{c}{\psi }_e=\psi -{\psi }_{\mathrm{d}}\hfill \\ {\dot{\psi }}_e=\dot{\psi }-{\dot{\psi }}_{\mathrm{d}}\hfill \\ {\ddot{\psi }}_e=\ddot{\psi }-{\ddot{\psi }}_{\mathrm{d}}\hfill \end{array}\right.$$

(13)

Since the control input ${\tau }_r$ exists in $\ddot{\psi }$, we define a sliding mode surface $s_{\psi }$

$$\left\{\begin{array}{c}{s}_{\psi }=k_{\psi }{\psi }_e+{\dot{\psi }}_e\hfill \\ {\dot{s}}_{\psi }=k_{\psi }{\dot{\psi }}_e+{\ddot{\psi }}_e\hfill \end{array}\right.$$

(14)

In order to stabilize the sliding surface,

$${\dot{s}}_{\psi }=k_{s1}{s}_{\psi }$$

(15)

Then the control law is obtained as follows:

$${\tau }_r=I_{11}\left(k_{s}s+{\ddot{\psi }}_{\mathrm{d}}-k_{\psi }{\dot{\psi }}_e\right)-\frac{\rho }{2}{L}^4\left[N_{r}ur+N_{wr}wr\right]-\frac{\rho }{2}{L}^3{N}_{v}uv-\frac{\rho }{2}{L}^3{N}_{vw}vw-\frac{\rho }{2}{L}^3{u}^2{N}_{\mathrm{prop}}-{\widehat{f}}_r$$

(16)

In Equation (16), ${\tau }_r$ is the design of a control law to control the virtual heading, ${\widehat{f}}_r$ is the observed value obtained through a disturbance observer.

4.1.3. UUV Depth Control Law

The depth of a UUV is generally determined by adjusting the buoyancy adjustment system. The buoyancy regulation system is a reversible ballast system that can cause changes in its own buoyancy by changing the drainage volume of underwater submarines. It can autonomously adjust according to the water depth, achieving a balance between buoyancy and gravity. Firstly, we define the depth error of the UUV:

$$\left\{\begin{array}{c}{z}_e=z-z_{\mathrm{d}}\hfill \\ {\dot{z}}_e=\dot{z}-{\dot{z}}_{\mathrm{d}}\hfill \\ {\ddot{z}}_e=\ddot{z}-{\ddot{z}}_{\mathrm{d}}\hfill \end{array}\right.$$

(17)

Similarly, since it is also a second-order system, a sliding surface $s_z$ is defined as

$$\left\{\begin{array}{c}{s}_z=k_z{z}_e+{\dot{z}}_e\hfill \\ {\dot{s}}_z=k_z{\dot{z}}_e+{\ddot{z}}_e\hfill \end{array}\right.$$

(18)

In order to stabilize the sliding surface,

$${\dot{s}}_z=-k_{s2}{s}_z$$

(19)

So the UUV depth control law is

$${\tau }_w=m_{33}\left(-k_{s2}{s}_z+{\ddot{z}}_{\mathrm{d}}-k_z{\dot{z}}_e\right)-\frac{\rho }{2}{L}^4{Z}_{rr}{r}^2-\frac{\rho }{2}{L}^3{Z}_{vr}vr-\frac{\rho }{2}{L}^2{Z}_{w}uw-\frac{\rho }{2}{L}^2{Z}_{vv}{v}^2-\frac{\rho }{2}{L}^2{Z}_{wn}uw-{\widehat{f}}_w$$

(20)

In Equation (20), ${\tau }_w$ is designed to control the depth of a UUV using a control law, and ${\widehat{f}}_w$ is the observed value $f_w$ obtained by perturbing the observer.

4.2. Input Optimization

Given the necessity of factoring in the UUV’s maneuverability, this section focuses on optimizing control inputs to alleviate issues related to input constraints and rudder angle fluctuations during path following [36].

$$\left\{\begin{array}{c}{F}_{op}={F_{op}}_1+F_{op2}+F_{op3}\hfill \\ {F_{op}}_1={\vartheta }_1{\left({\tau }_u-{\tau }_u\left(k\right)\right)}^2+{\vartheta }_2{\left({\tau }_u-{\tau }_u\left(k-1\right)\right)}^2+{\vartheta }_3{\tau }_u^2\hfill \\ F_{op2}={\gamma }_1{\left({\tau }_r-{\tau }_r\left(k\right)\right)}^2+{\gamma }_2{\left({\tau }_r-{\tau }_r\left(k-1\right)\right)}^2+{\gamma }_3{\tau }_r^2\hfill \\ F_{op3}={\varsigma }_1{\left({\tau }_w-{\tau }_w\left(k\right)\right)}^2+{\varsigma }_2{\left({\tau }_w-{\tau }_w\left(k-1\right)\right)}^2+{\varsigma }_3{{\tau }_w}^2\hfill \end{array}\right.$$

(21)

In Equation (21), ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ represent the current control inputs, while ${\tau }_u(k-1)$, ${\tau }_r(k-1)$, and ${\tau }_w(k-1)$ denote the control inputs from the previous time step. ${\vartheta }_{1-3}$, ${\gamma }_{1-3}$, and ${\varsigma }_{1-3}$ serve as the designated weight parameters. Balancing the control and input performance is achieved by adjusting these design weight parameters. Subsequently, Equation (21) is optimized.

$$\left\{\begin{array}{c}\partial F_{op}/\partial {\tau }_u=2\left({\vartheta }_1+{\vartheta }_2+{\vartheta }_3\right){\tau }_u-2\left({\vartheta }_1{\tau }_u\left(k\right)+{\vartheta }_2{\tau }_u\left(k-1\right)\right)\hfill \\ \partial F_{op}/\partial {\tau }_r=2\left({\gamma }_1+{\gamma }_2+{\gamma }_3\right){\tau }_r-2\left({\gamma }_1{\tau }_r\left(k\right)+{\gamma }_2{\tau }_r\left(k-1\right)\right)\hfill \\ \partial F_{op}/\partial {\tau }_w=2\left({\varsigma }_1+{\varsigma }_2+{\varsigma }_3\right){\tau }_w-2\left({\varsigma }_1{\tau }_w\left(k\right)+{\varsigma }_2{\tau }_w\left(k-1\right)\right)\hfill \end{array}\right.$$

(22)

To seek the optimal solution, we compute its derivative as depicted in Equation (22).

$$\left\{\begin{array}{c}\partial F_{op}/\partial {\tau }_u=0\hfill \\ \partial F_{op}/\partial {\tau }_r=0\hfill \\ \partial F_{op}/\partial {\tau }_w=0\hfill \end{array}\right.$$

(23)

The final optimization function obtained is as follows:

$$\left\{\begin{array}{c}{\tau }_u={\vartheta }_{i1}{\tau }_u\left(k\right)+{\vartheta }_{i2}{\tau }_u\left(k-1\right)/\left({\vartheta }_1+{\vartheta }_2+{\vartheta }_3\right)\hfill \\ {\tau }_r={\gamma }_{i1}{\tau }_r\left(k\right)+{\vartheta }_{i2}{\tau }_r\left(k-1\right)/\left({\gamma }_1+{\gamma }_2+{\gamma }_3\right)\hfill \\ {\tau }_w={\varsigma }_1{\tau }_w\left(k\right)+{\varsigma }_2{\tau }_w\left(k-1\right)/\left({\varsigma }_1+{\varsigma }_2+{\varsigma }_3\right)\hfill \end{array}\right.$$

(24)

Remark 2.

By adjusting the parameters ${\vartheta }_1$>${\vartheta }_2$>${\vartheta }_3$, ${\gamma }_1$>${\gamma }_2$>${\gamma }_3$ and ${\varsigma }_1$ >${\varsigma }_2$ > ${\varsigma }_3$, the stability of ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ could be the same as ${\tau }_u\left(k\right)$, ${\tau }_r\left(k\right)$, and ${\tau }_w\left(k\right)$.

4.3. Design of Disturbance Observer

Given the susceptibility of UUVs to external disturbances during navigation, along with the possibility of perturbations in their inherent model parameters, which can substantially affect control effectiveness, a disturbance observer is carefully designed to precisely estimate external environmental disturbances [37,38].

$$\left\{\begin{array}{c}\dot{\widehat{u}}=\left(mvr+\frac{\rho }{2}{L}^4{X}_{rr}{r}^2+\frac{\rho }{2}{L}^3{X}_{vr}vr+\frac{\rho }{2}{L}^2{X}_{vv}{v}^2+\frac{\rho }{2}{L}^2{X}_{ww}{w}^2+\frac{\rho }{2}{C_d}_0{L}^2\widehat{u}+{\tau }_u+{\widehat{f}}_u\right)/m_{11}-l_u\left(\widehat{u}-u\right)\hfill \\ {\dot{\widehat{f}}}_u=-l_{fu}\left(\widehat{u}-u\right)\hfill \\ \dot{\widehat{v}}=\left(\frac{\rho }{2}{L}^3{Y}_{r}ur+\frac{\rho }{2}{L}^3{Y}_{wr}wr-mur+\frac{\rho }{2}{L}^2{Y}_{v}u\widehat{v}+\frac{\rho }{2}{L}^2{Y}_{v\mathrm{w}}\widehat{v}w+{\widehat{f}}_v\right)/m_{22}-l_v\left(\widehat{v}-v\right)\hfill \\ {\dot{\widehat{f}}}_v=-l_{fv}\left(\widehat{v}-v\right)\hfill \\ \dot{\widehat{w}}=\left(\frac{\rho }{2}{L}^4{Z}_{rr}{r}^2+\frac{\rho }{2}{L}^3{Z}_{vr}vr+\frac{\rho }{2}{L}^2{Z}_{w}u\widehat{w}+\frac{\rho }{2}{L}^2{Z}_{vv}{v}^2+\frac{\rho }{2}{L}^2{Z}_{wn}u\widehat{w}+{\tau }_w+{\widehat{f}}_w\right)/m_{33}-l_w\left(\widehat{w}-w\right)\hfill \\ {\dot{\widehat{f}}}_w=-l_{fw}\left(\widehat{w}-w\right)\hfill \\ \dot{\widehat{r}}=\left[\frac{\rho }{2}{L}^4\left(N_{r}u\widehat{r}+N_{wr}w\widehat{r}\right)+\frac{\rho }{2}{L}^3{N}_{v}uv+\frac{\rho }{2}{L}^3{N}_{vw}vw+\frac{\rho }{2}{L}^3{u}^2{N}_{\mathrm{prop}}+{\tau }_r+{\widehat{f}}_r\right]/I_{11}-l_r\left(\widehat{r}-r\right)\hfill \\ {\dot{\widehat{f}}}_r=-l_{fr}\left(\widehat{r}-r\right)\hfill \end{array}\right.$$

(25)

The observation error is

$$\left\{\begin{array}{c}\dot{\tilde{u}}=\left(\frac{\rho }{2}{C_d}_0{L}^2\tilde{u}+{\tilde{f}}_u\right)/m_{11}-l_u\tilde{u}\hfill \\ {\dot{\tilde{f}}}_u=-l_{fu}\tilde{u}-{\dot{f}}_u\hfill \\ \dot{\tilde{v}}=\left(\frac{\rho }{2}{L}^2{Y}_{v}u\tilde{v}+\frac{\rho }{2}{L}^2{Y}_{v\mathrm{w}}\tilde{v}w+{\tilde{f}}_v\right)/m_{22}-l_v\tilde{v}\hfill \\ {\dot{\tilde{f}}}_v=-l_{fv}\tilde{v}-\dot{f_v}\hfill \\ \dot{\tilde{w}}=\left(\frac{\rho }{2}{L}^2{Z}_{w}u\tilde{w}+\frac{\rho }{2}{L}^2{Z}_{wn}u\tilde{w}+{\tilde{f}}_w\right)/m_{33}-l_w\tilde{w}\hfill \\ {\dot{\tilde{f}}}_w=-l_{fw}\tilde{w}-{\dot{f}}_w\hfill \\ \dot{\tilde{r}}=\left[\frac{\rho }{2}{L}^4\left(N_{r}u\tilde{r}+N_{wr}w\tilde{r}\right)+{\tilde{f}}_r\right]/I_{11}-l_r\tilde{r}\hfill \\ {\dot{\tilde{f}}}_r=-l_{fr}\tilde{r}-{\dot{f}}_r\hfill \end{array}\right.$$

(26)

In Equation (26), $\tilde{A}=A-\widehat{A}$ represents the error of the estimated value.

Remark 3.

Because the primary aim of constructing a disturbance observer is to estimate unknown disturbances, the design proposed in this article operates under the assumption that u, v, w, and r are known.

5. Simulation Verification

A trajectory tracking control of a UUV based on the PPG method and comparative examples have been carried out on an industrial computer (Intel(R) Core(TM) i7-10510U CPU @ 1.80 GHz 2.30 GHz). The simulation platform used is MATLAB/Simulink. The UUV used in this paper is from reference [29], and the parameters of the controller were obtained through tuning, with specific values as follows: $k_u=0.15$, $k_{\psi }=0.2$, $k_{s1}=\pi /4$, $k_z=0.2$, $k_{Ls2}=0.3$, $k_{Fx1}=1.2$, $k_{x2}=0.005$, $k_{y1}=0.62$, $k_{y2}=0.05$. The observer parameters are $l_u=1.0$, $l_{fu}=1.8$, $l_v=1.0$, $l_{fv}=1.8$, $l_r=1.0$, $l_{fr}=1.8$, $l_w=1.0$, $l_{fw}=1.8$. The simulation of external disturbances involves the integration of sine and cosine functions, because according to Fourier series, any nonlinear function can be represented as a sum of sine and cosine functions, and using a combination of sine and cosine functions to simulate underwater disturbance is meaningful to some extent. The specific formulation provided in Equation (27).

$$\left\{\begin{array}{c}{f}_u=4\times {10}^3\left(sin0.02t+cos0.05t\right)\hfill \\ f_v=1\times {10}^3\left(sin0.01t+cos0.04t\right)\hfill \\ f_w=2\times {10}^3\left(sin0.01t+cos0.04t\right)\hfill \\ f_r=3\times {10}^3\left(sin0.05t+cos0.03t\right)\hfill \end{array}\right.$$

(27)

The initial state of the UUV is [u, v, r, w, x, y, z, $\psi$] = [ 7, 0, 0, 0, 50, 200, 0, 0]. This paper employs a set of experiments and two sets of comparative experiments to validate the proposed algorithm. These include OPSMC based on disturbance observer, SMC based on a disturbance observer, and SMC alone to achieve UUV trajectory tracking. Here, the SMC algorithm is consistent with the design method of SMC in [37]. We simulate it using straight and curved paths, respectively. For a better result comparison and quantitative analysis, quantitative metrics are defined as the Integral of the Absolute Error $\left(IAE\right)$ and the Integral of the Square Value $\left(ISV\right)$ of the control input [39,40]. The specific formulas for these quantitative metrics are shown in Equation (28), and the quantified results are presented in

Table 1. Comparison of $IAE$ for different control algorithms.

Straight line	Controller type	OPSMC + DO	SMC + DO	SMC
	$x_e$(m)	3.05 $\times {10}^4$	3.63$\times {10}^4$	3.15$\times {10}^5$
IAE	$y_e$ (m)	2.11$\times {10}^5$	2.88$\times {10}^5$	7.86$\times {10}^5$
	$z_e$(m)	1.74$\times {10}^3$	5.56$\times {10}^3$	1.71$\times {10}^4$
Curve	Controller type	OPSMC + DO	SMC + DO	SMC
	$x_e$ (m)	5.38$\times {10}^4$	6.89$\times {10}^4$	2.18$\times {10}^5$
IAE	$y_e$ (m)	1.89$\times {10}^5$	2.71$\times {10}^5$	5.77$\times {10}^5$
	$z_e$ (m)	1.70$\times {10}^3$	3.84$\times {10}^3$	1.53$\times {10}^4$

and

Table 2. Comparison of $ISV$ for different control algorithms.

Straight line	Controller type	OPSMC + DO	SMC + DO
	${\tau }_u$ (N)	2.74 $\times {10}^9$	2.75 $\times {10}^9$
ISV	${\tau }_r$ (N.m)	6.75 $\times {10}^{10}$	6.77 $\times {10}^{10}$
	${\tau }_w$ (N)	1.36 $\times {10}^{11}$	1.37 $\times {10}^{11}$
Curve	Controller type	OPSMC + DO	SMC + DO
	${\tau }_u$ (N)	2.90 $\times {10}^9$	2.91 $\times {10}^9$
ISV	${\tau }_r$ (N.m)	6.04 $\times {10}^9$	6.04 $\times {10}^9$
	${\tau }_w$ (N)	1.56 $\times {10}^8$	1.57 $\times {10}^8$

.

$$\left\{\begin{array}{c}IAE={\int }_0^{tf}\left|e_i\right|dt\hfill \\ ISV={\int }_0^{tf}\left|{\tau }_i\right|dt\hfill \end{array}\right.$$

(28)

Case 1: The reference trajectory of a straight line is

$$\left\{\begin{array}{llll}{x}_{\mathrm{d}}=5t& y_{\mathrm{d}}=0& z_{\mathrm{d}}=-20& 0\le t\le 1000\\ x_{\mathrm{d}}=4t+1000& y_{\mathrm{d}}=t-1000& z_{\mathrm{d}}=-30& 1000\le t\le 2000\\ x_{\mathrm{d}}=3t+3000& y_{\mathrm{d}}=2t-3000& z_{\mathrm{d}}=-30& 2000\le t\le 3000\end{array}\right.$$

(29)

Figure 4, Figure 5, Figure 6, Figure 7, Figure 8, Figure 9, Figure 10 and Figure 11 illustrate the trajectory tracking performance of a UUV on straight paths. Figure 4 demonstrates the effect of 3D trajectory tracking. It is evident from Figure 4 that UUVs exhibit effective trajectory tracking with the employment of the PPG method, whereby 3D position control is effectively translated into control of virtual heading, virtual speed, and depth. In Figure 4, the red line represents the trajectory tracking effect based on the OPSMC and disturbance observer, the blue line depicts the trajectory tracking effect based on SMC and disturbance observer, the green line denotes the SMC without the utilization of a disturbance observer, and the black line signifies the reference trajectory. Notably, the SMC controller without a disturbance observer manifests pronounced oscillations during the trajectory tracking process. Both SMC and disturbance observer-based trajectory tracking yield higher accuracy. As can be seen from Table 1, for the OPSMC with disturbance observer compared to SMC with disturbance observer, the path deviation $x_e$ decreased by 15.98%. Similarly, the longitudinal deviation $y_e$ decreased 26.74%, and the depth deviation $z_e$ decreased 68.71%.

Figure 5 presents the trajectory tracking effect of the three-dimensional trajectory projected onto the x–y coordinate plane. From Figure 5, it can be discerned that OPSMC outperforms SMC in terms of accuracy. Figure 6 portrays the deviations $x_e$, $y_e$, and $z_e$ in trajectory tracking, with all deviations converging towards zero. This indicates that the designed controller boasts robustness, effectively managing the UUV in the presence of external interference. Trajectory tracking based on disturbance observation showcases smaller errors. Figure 7 showcases the tracking of the preset virtual speed and virtual heading angle through the sliding mode controller. The controller’s robustness translates into exceptional control effectiveness. Figure 8 exhibits the temporal curves of UUV’s lateral velocity u, longitudinal velocity v, vertical velocity w, and yaw rate r. Figure 9 illustrates the temporal curve of control torque inputs, including ${\tau }_u$,${\tau }_r$, and ${\tau }_w$. As show in Table 2, compared to the SMC, the control inputs ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ of the OPSMC decreased 0.36%, 0.296%, and 0.730%, respectively.

Figure 10 represents the temporal curve of the disturbance observer for time-varying disturbance estimation, wherein the red line represents actual disturbance values, and the blue line signifies estimated disturbance values. From Figure 10, it is evident that the designed disturbance observer offers commendable estimation performance. Finally, Figure 11 depicts the temporal curve of disturbance estimation errors.

Case 2: Select the curve reference path as

$$\left\{\begin{array}{c}{x}_{\mathrm{d}}=4.5t \hfill \\ y_{\mathrm{d}}=250\sin \left(0.0025t\right)\hfill \\ z_{\mathrm{d}}=-0.1t\hfill \end{array}\right.$$

(30)

Figure 12, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17, Figure 18 and Figure 19 depict the tracking performance of UUV on curved paths. Specifically, Figure 12 illustrates the tracking effect of curved trajectories in a three-dimensional space. It is evident from Figure 12 that UUVs exhibit robust tracking performance on curved paths, underscoring the applicability of the guidance methodology and control strategy proposed in this study for curved trajectory tracking. In comparison to trajectory tracking without disturbance observation, trajectory tracking utilizing the disturbance observer-based SMC demonstrates improved tracking performance. Moreover, the OPSMC exhibits higher accuracy than SMC. As can be seen from Table 1, for the OPSMC with disturbance observer compared to SMC with disturbance observer, the path deviation $x_e$ decreased by 21.79%. Similarly, the longitudinal deviation $y_e$ decreased by 30.26%, and the depth deviation $z_e$ decreased by 55.21%. Figure 13 showcases the trajectory tracking outcome as the three-dimensional trajectory is projected onto the $x-y$ coordinate plane. Figure 14 presents the deviations $x_e$, $y_e$, and $z_e$ in curve-track tracking. These trajectory tracking deviations ultimately converge to zero, underscoring the controller’s ability to robustly track the curve trajectory of UUVs in the presence of external interference. Notably, trajectory tracking based on the disturbance observer exhibits smaller tracking errors when compared to trajectory tracking without the disturbance observer. OPSMC achieves more accurate trajectory tracking than SMC. Figure 15 portrays the successful tracking of the set virtual speed and virtual heading angle through Sliding Mode Control. The controller’s robustness contributes to its exceptional control effectiveness. Additionally, Figure 16 displays the temporal curves of UUVs’ surge velocity u, sway velocity v, vertical velocity w, and yaw rate r. In Figure 17, the duration curve of the control torque input ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ is presented. As show in Table 2, compared to the SMC, the control inputs ${\tau }_u$, ${\tau }_r$, and ${\tau }_w$ of the OPSMC decreased by 0.344%, 0.0496%, and 0.636% respectively. Meanwhile, Figure 18 depicts the duration curve of the disturbance observer for time-varying disturbance estimation. This graph indicates that the designed disturbance observer exhibits commendable estimation performance, accurately estimating disturbances. Lastly, Figure 19 represents the duration curve of disturbance estimation error.

6. Conclusions

This paper proposes a novel approach that introduces vertical projection guidance, an optimized sliding mode algorithm, and disturbance observer for achieving trajectory tracking control of UUVs. Its main conclusions are as follows:

• The proposed PPG algorithm successfully converts trajectory control into virtual heading and virtual longitudinal control, thus addressing the issue of insufficient control inputs in underactuated systems.
• With the use of disturbance observers, the optimized sliding mode control reduces the average path deviation by 36% and the average control input by 0.315% compared to conventional sliding mode control.
• The employed disturbance observer effectively estimates disturbances.

The control algorithm proposed in this paper has only been tested through simulation experiments and has not been validated through real UUV experiments. In future work, consideration will be given to conducting real UUV experiments. more data can be checked in Appendix A.