APSO-MPC and NTSMC Cascade Control of Fully-Actuated Autonomous Underwater Vehicle Trajectory Tracking Based on RBF-NN Compensator

Abstract

In this paper, a model predictive control (MPC) method optimized by an adaptive particle swarm optimization (APSO) algorithm is proposed. Combined with non-singular terminal sliding mode control (NTSMC), the inner and outer double-closed-loop control system is constructed to solve the fully actuated autonomous underwater vehicle (AUV) dynamic trajectory tracking control problem. First, the outer loop controller generates the expected optimal velocity commands and passes them to the inner loop velocity controller, which generates the available control inputs to ensure the entire closed-loop trajectory tracking. In the controller design stage, system input and state constraints are effectively considered. After that, a compensator based on an adaptive radial basis function (RBF) neural network (NN) is designed to compensate for the model error and external sea state disturbances and to improve the control accuracy of the system. Then, the stability of the proposed controller is proved based on Lyapunov analysis. Finally, the dynamic trajectory tracking performance of an AUV with different sea state disturbances is verified by simulation, and the simulation results are compared with double-closed-loop PD control and cascade control of standard MPC based on PSO and SMC. The results show that the designed controller is effective and robust.

1. Introduction

The ocean is the origin of the development of human civilization, and its rich resources of fish and minerals are strong backing for the sustainable development of human beings [1]. Since the beginning of the 21st century, it has become an inevitable trend for all countries to explore and exploit marine resources [2,3]. Although human beings have been able to travel in space, they know very little about the ocean [4]. The main reason is the lack of effective tools for deep-sea scientific research and the exploitation and utilization of marine resources [5,6].

Unmanned underwater vehicles (UUVs) are a kind of underwater vehicle [7]. By carrying a variety of high-precision sensors and different task modules, they can carry out important tasks such as hydrology surveys, energy exploration, and marine science research [8,9]. UUVs also play an important role in the military and are usually used for underwater vigilance, reconnaissance, surveillance, tracking, and so on [10]. UUVs can be divided into two categories based on the different contact modes between the UUV and surface support systems: remote-operated vehicles (ROVs) have cables, and autonomous underwater vehicles (AUVs) do not have cables [11,12]. With their energy, AUVs can control themselves and make decisions independently without manual control; this has many advantages, such as a large range of activities, no fear of cable entanglement, and wide application space [13]. AUVs are suitable for seabed search, identification, and salvage operations [14]. AUV motion control includes stabilization, path tracking, and trajectory tracking. Trajectory tracking is to use a guidance controller and dynamics controller to make the AUV track from the initial state to the endpoint under the constraint of time [15]. Some traditional control methods, such as sliding mode control (SMC), model predictive control (MPC), neural network control (NNC), and proportional differential control (PD), have been applied in the field of underwater trajectory tracking control [16,17].

PID control is a control system based on the proportion, integral, and differential of the error generated by comparing the real-time data collected with regard to the controlled object to the given value [18,19]. It has the advantages of a simple principle, strong robustness, and wide practical applications [20]. It is a mature technology and is the most widely used control system [21,22]. As proposed by Hammad and Elshenawy [23], a nonlinear fuzzy PID controller can be applied for the trajectory tracking control of AUVs using fuzzy logic rules to adjust the parameters of the PID controller in real-time to reduce the overshoot of the system and to improve the system response time; however, the complex fuzzy logic rules need to be set for the actual problem at hand.

The advantage of SMC is that it can overcome the uncertainty of the system and has strong robustness to disturbances and unmodeled dynamics, especially for the control of nonlinear systems [24,25]. Elmokadem et al. applied the traditional SMC method to 3D trajectory tracking control of an AUV. Because the symbol function is introduced into the corresponding degree of freedom of the sliding mode surface, traditional SMC causes chattering problems, which has a great impact on the performance of the controller [26]. At the same time, London and Patre focus on solving this problem and propose an adaptive fuzzy sliding mode controller that improves the chattering problem of SMC by proposing a new Lyapunov energy function and deriving fuzzy logic rules [27]. Non-singular terminal sliding mode control (NTSMC) solves the singularity problem of the existing terminal sliding mode and realizes global nonsingular control of the system. In addition, it inherits the finite time convergence characteristics of the terminal sliding mode and has high steady-state accuracy [28]. However, in practical applications, the control accuracy is also reduced due to the modeling error of the controlled object or external disturbances.

Classic MPC solves for the currently available optimal control sequence within a limited predictive horizon at each pre-set sampling time [29]. Model predictive control not only pursues control but also pursues optimal control, which is the biggest difference between MPC and traditional control methods [30]. MPC is committed to the optimal control problem with a longer period, or even infinite period, which is decomposed into several optimization control problems with a shorter period or a limited period, and it still pursues the optimal solution to a certain extent [31]. The most classical optimal controller is the quadratic programming (QP) controller [32]. In [33], Yan et al. proposed a double-closed-loop model predictive controller in which the outer MPC loop generates the expected velocity command for the system, and the inner MPC loop outputs the system control command. However, the QP method is used to solve the optimal value of the cost function of MPC, and its disadvantage is that the global optimal solution cannot usually be obtained, which leads to a large trajectory tracking error. To solve this problem, a novel genetic–ant colony optimization algorithm was proposed by [34] to replace the QP link in MPC to obtain a higher-quality global optimal solution; it was successfully applied to the dynamic trajectory tracking control of an AUV with dynamic SMC. Although the fusion of the ant colony algorithm can accelerate the convergence speed of the optimization algorithm, the sampling time of MPC is limited after all. Particle swarm optimization (PSO) has the advantages of simple design and fast convergence speed, which is more suitable for the fusion of MPC. The authors of [35] proposed an MPC based on quantum behavior PSO optimization, but it is still difficult to balance the local search ability and global search ability of PSO.

In this paper, an APSO-based MPC and NTSMC cascade control method is proposed and applied for the trajectory tracking control of an AUV. In the design of the controller, the actual constraints of the system are fully considered. When the APSO algorithm is applied to the MPC controller to solve the optimal speed instruction, it can overcome the problem of the standard MPC being unable to obtain the global optimal solution when applying the QP method to solve the problem with constraints. The NTSMC velocity controller allows the AUV to follow the expected optimal velocity commands. The adaptive RBF-NN compensator is designed to compensate for the modeling error of the AUV and the disturbance of external sea conditions to enhance the control accuracy and robustness of the system. The controller recalculates the best input at the next moment based on the system error within each sampling time of the system. The asymptotic stability of the controller is proven using a Lyapunov function with terminal constraints. Finally, a simulation model of AUV trajectory tracking control is built using MATLAB/Simulink. The simulation process includes setting two different expected trajectory cases, each of which includes settings without disturbance, with random disturbance, and with wave and/or current disturbance to verify the effectiveness and robustness of the proposed controller.

The main contents of this paper are as follows: Section 2 gives the kinematics and dynamics models of the fully actuated AUV, as well as the layout and rationality verification of the thrusters. Section 3 introduces the description of the trajectory tracking control problem and the design of the controller and analyzes the stability of the proposed controller. Section 4 gives the simulation results with different external disturbances. Section 5 provides some conclusive comments.

2. Modeling of the AUV System

AUV system modeling includes physical model design, kinematics modeling, dynamics modeling, and thrust distribution setting. The physical model design determines the basic physical parameters of the AUV and the layout of thrusters. The kinematics model of the AUV describes the relationship between its position, attitude, and velocity. The dynamic model describes the relationship between the acceleration and the force of the AUV in the process of motion.

2.1. Frames of Reference

Figure 1 shows the fully actuated AUV design based on SOLIDWORKS proposed in this paper.

Remark 1. 

Generally, under the action of the restoring force and the moment of the AUV, the rolling motion of the AUV is considered to be self-stable, so the roll angle φ and roll angular velocity p of the rolling degrees of freedom are both 0. Similarly, pitch angular θ is bounded and satisfies $-\pi /2\le \theta \le \pi /2$ [31].

Remark 2. 

The approximate solution method for the main inertial and viscous hydrodynamic coefficients of the fully actuated AUV proposed in this paper is based on the CFD simulation method in [34] and includes the simulation of uniform translational motion, uniform rotational motion, accelerated translational motion, and accelerated rotational motion.

Assumptions

1.

The AUV is treated as a rigid body with constant mass, and its gravity is assumed to be approximately equal to its buoyancy.

2.

The streamlined structure of the AUV is approximately symmetrical in the horizontal and vertical planes, and its hydrodynamic coefficient does not change with changes in the environment.

3.

The origin of the horizontal and vertical thrusters is coplanar with the center of gravity of the AUV, resulting in no corresponding moment disturbance.

The position and attitude information $\mathbf{\eta }={[x,y,z,\theta ,\psi ]}^{\mathrm{T}}$ of the AUV is defined in the inertial coordinate system E [36] corresponding to the information of five freedoms of motion: surge, sway, heave, pitch, and yaw.

The velocity information $\mathit{v}={[u,v,w,q,r]}^{\mathrm{T}}$ of the AUV is defined in the body coordinate system G corresponding to the information of the above five freedoms of motion. Here, E and G are both right-handed Cartesian coordinates.

Remark 3. 

The origin definition of $G(x_G,y_G,z_G)$ coincides with the center of gravity of the AUV proposed in this paper. The buoyancy force of the AUV is defined as B, and the center of buoyancy coordinate is $(x_B,y_B,z_B)$.

2.2. Kinematics and Dynamics

The kinematic model of the fully actuated AUV can be defined:

$$\dot{\mathbf{\eta }}=\mathit{J}(\mathbf{\eta })\mathit{v}$$

(1)

where $\mathit{J}(\mathbf{\eta })$ is defined as the transition matrix between the coordinates of E and G:

$$\mathit{J}(\mathbf{\eta })=\left[\begin{array}{ccccc}cos\psi cos\theta & -sin\psi & sin\theta cos\psi & 0& 0\\ sin\psi cos\theta & cos\psi & sin\theta sin\psi & 0& 0\\ -sin\theta & 0& cos\theta & 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1/cos\theta \end{array}\right]$$

(2)

The general dynamics model of the underwater vehicle proposed by Fossen [33] is also applicable to the fully actuated AUV proposed in this paper and can be expressed in the following form:

$$\mathit{M}\dot{\mathit{v}}+\mathit{C}(\mathit{v})\mathit{v}+\mathit{D}(\mathit{v})\mathit{v}+\mathit{g}(\mathbf{\eta })+{\mathbf{\tau }}_d=\mathbf{\tau }$$

(3)

where $\mathit{M}=diag(m-X_{\dot{u}},m-Y_{\dot{v}},m-Z_{\dot{w}},I_y-M_{\dot{q}},I_z-N_{\dot{r}})$ is defined as the inertia matrix, with m the mass of the AUV and $I_x$, $I_y$, and $I_z$ the inertia tensors of AUV. The values in this paper can be obtained from the SOLIDWORKS structural model of the designed AUV. Here, $-X_{\dot{u}}$, $-Y_{\dot{v}}$, $-Z_{\dot{w}}$, $-M_{\dot{q}}$, and $-N_{\dot{r}}$ are the main inertial hydrodynamic coefficient caused by the AUV performing accelerated motion underwater [37], where $\mathit{C}(\mathit{v})={\mathit{C}}_{\mathit{RB}}(\mathit{v})+{\mathit{C}}_{\mathit{A}}(\mathit{v})$. Because of the Earth’s rotation, an AUV naturally experiences Coriolis and centripetal effects when moving underwater.

Here, ${\mathit{C}}_{\mathit{RB}}(\mathit{v})$ and ${\mathit{C}}_{\mathit{A}}(\mathit{v})$ are the Coriolis and centripetal force matrices caused by the above rigid body inertial mass matrix and the additional mass matrix, respectively.

Specifically,

$${\mathit{C}}_{\mathit{RB}}(\mathit{v})=\left[\begin{array}{ccccc}0& 0& 0& mw& -mv\\ 0& 0& 0& 0& mu\\ 0& 0& 0& -mu& 0\\ -mv& 0& mu& 0& 0\\ mv& -mu& 0& 0& 0\end{array}\right]$$

(4)

$${\mathit{C}}_{\mathit{A}}(\mathit{v})=\left[\begin{array}{ccccc}0& 0& 0& -Z_{\dot{w}}w& Y_{\dot{v}}v\\ 0& 0& 0& 0& -X_{\dot{u}}u\\ 0& 0& 0& X_{\dot{u}}u& 0\\ Z_{\dot{w}}w& 0& -X_{\dot{u}}u& 0& 0\\ -Y_{\dot{v}}v& X_{\dot{u}}u& 0& 0& 0\end{array}\right]$$

(5)

where $\mathit{D}(\mathit{v})=diag(X_u-X_{\left|u\right|u}\left|u\right|,Y_v-Y_{\left|v\right|v}\left|v\right|,Z_w-Z_{\left|w\right|w}\left|w\right|,K_p-K_{\left|p\right|p}\left|p\right|,M_q-M_{\left|q\right|q}\left|q\right|,N_r-N_{\left|r\right|r}\left|r\right|)$ is defined as the viscous damping matrix.

$X_u$, $Y_v$, $Z_w$, $M_q$, $N_r$, $X_{\left|u\right|u}$, $Y_{\left|v\right|v}$, $Z_{\left|w\right|w}$, $M_{\left|q\right|q}$, and $N_{\left|r\right|r}$ are estimable linear and nonlinear viscous hydrodynamic coefficients, respectively.

Here, $\mathit{g}(\mathbf{\eta })$ is the restoring force matrix generated by the static force of the AUV in the underwater environment.

$$\mathit{g}(\mathbf{\eta })=\left[\begin{array}{c}0\\ 0\\ 0\\ y_{B}Bcos\theta \\ -z_{B}Bsin\theta -x_{B}cos\theta \\ y_{B}Bsin\theta \end{array}\right]$$

(6)

${\mathbf{\tau }}_{\mathit{d}}={[{\mathbf{\tau }}_{du},{\tau }_{dv},{\tau }_{dw},{\tau }_{dq},{\tau }_{dr}]}^{\mathrm{T}}$ is the the vector of external unknown ocean disturbance force and moment for each degree of freedom.

$\mathbf{\tau }={[{\mathbf{\tau }}_u,{\tau }_v,{\tau }_w,{\tau }_q,{\tau }_r]}^{\mathrm{T}}$ is the vector of the thruster input force and moment for each degree of freedom.

2.3. Thruster Layout and Rationality Verification

The thruster arrangement of the fully actuated AUV proposed in this paper is shown in Figure 2, where $T_{\mathrm{x}}$ is the main thruster arranged in the surge direction, $T_{\mathrm{y}1}$ and $T_{\mathrm{y}2}$ are the two thrusters arranged in the sway direction, and $T_{\mathrm{z}1}$ and $T_{\mathrm{z}2}$ are the two thrusters arranged in the heave direction. The values $l_{\mathrm{x}}$, $l_{\mathrm{y}}$, and $l_{\mathrm{z}}$ are the moment arms in the corresponding direction.

In order to complete the motion control task, $\mathbf{\tau }={[{\mathbf{\tau }}_u,{\tau }_v,{\tau }_w,{\tau }_q,{\tau }_r]}^{\mathrm{T}}$ should be assigned to the appropriate thrusters. Based on the thruster arrangement in Figure 2, the thrust distribution matrix equation can be easily obtained in the following form:

$$\mathbf{\tau }=\left[\begin{array}{c}{\tau }_u\\ {\tau }_v\\ {\tau }_w\\ {\tau }_q\\ {\tau }_r\end{array}\right]=\left[\begin{array}{ccccc}1& 0& 0& 0& 0\\ 0& 1& 1& 0& 0\\ 0& 0& 0& 1& 1\\ 0& 0& 0& l_{\mathrm{z}}& -l_{\mathrm{z}}\\ 0& l_{\mathrm{y}}& -l_{\mathrm{y}}& 0& 0\end{array}\right]\left[\begin{array}{c}{T}_{\mathrm{x}}\\ T_{\mathrm{y}1}\\ T_{\mathrm{y}2}\\ T_{\mathrm{z}1}\\ T_{\mathrm{z}2}\end{array}\right]$$

(7)

To reasonably choose the type of thruster, this paper used computational fluid dynamics (CFD) software ANSYS-FLUENT to simulate surge, sway, and heave motions of the AUV at different velocities to solve for the maximum drag value. The mesh part of the AUV in the CFD software FLUENT is shown in Figure 3.

Figure 4 shows the relevant process and data of the CFD simulation. It can be seen from the figure that when the velocity is about 3 knots, the maximum drag in the surge, sway, and heave directions is about 46 N, 180 N, and 180 N, respectively.

The design velocity of the fully actuated AUV proposed in this paper is 2 knots. Based on the CFD simulation data, the velocity values of the AUV’s surge, sway, and heave freedom and the corresponding drag values are respectively fitted. The fitting results are shown in Figure 5. Based on the fitting results, an S100-CR-IC electric thruster is selected for the AUV.

As can be can clearly seen from Figure 6, the maximum forward thrust of a single thruster is 13.5 $\mathrm{kg}·\mathrm{f}$, which is about 135 N. The maximum reverse thrust is about 9 $\mathrm{kg}·\mathrm{f}$, or about 90 N. Based on the above data, the arrangement of thrusters in this paper is reasonable.

3. Design of the Controller

This paper focuses on the trajectory tracking control of an AUV with external disturbances. The main control objective is to ensure that the position and velocity of the AUV remain consistent with time. The closed-loop control system consists of an MPC position controller optimized based on the APSO algorithm and a sliding mode velocity controller. The former uses the system output feedback to design the expected velocity to ensure convergence of the AUV position tracking error. The latter achieves gradual convergence of the AUV drive velocity to the expected command, i.e., it ensures convergence of the AUV velocity tracking error. The optimal control solution can be found more effectively by considering the corresponding constraints when designing each controller. The block diagram of the cascade control strategy based on the RBF-NN compensator proposed in this paper is shown in Figure 7.

3.1. Design of MPC-APSO Controller

The trajectories are assumed to be pre-given functions of time that satisfy smooth, bounded, and second-order differentiable functions. We set the expected trajectory to be tracked by the fully actuated AUV as follows:

$${\mathit{Y}}_{\mathit{d}}={[x_d,y_d,z_d,{\theta }_d,{\psi }_d]}^{\mathrm{T}}$$

(8)

The MPC-APSO controller is the guidance controller of the AUV trajectory tracking control; that is, it is the kinematics controller. We set the sampling time $T_s$ of the MPC rolling optimization. The discrete motion model of the AUV can be defined as follows:

$$\mathbf{\eta }(k+1)=\mathbf{\eta }(k)+\mathit{J}(k)\mathit{v}(k)T_s$$

(9)

The velocity increment of an AUV is defined as the input of the guidance controller to ensure the stability of its motion. Based on the MPC principle, the state model of an AUV can be defined as follows:

$$\mathit{X}(k+1)={[\mathbf{\eta }(k+1),\mathit{v}(k)]}^{\mathrm{T}}$$

(10)

Therefore, the input of the system is defined as:

$$\Delta \mathit{v}(k)=\mathit{v}(k)-\mathit{v}(k-1)$$

(11)

By combining Equations (9)–(11), the state space model can be rewritten in the following form:

$$\begin{array}{cc}\hfill \mathit{X}(k+1)& =\left[\begin{array}{c}\mathbf{\eta }(k+1)=\mathbf{\eta }(k)+\mathit{J}(k)\mathit{v}(k)T_s+\mathit{J}(k)\mathit{v}(k-1)T_s\\ -\mathit{J}(k)\mathit{v}(k-1)T_s\\ \mathit{v}(k)+\mathit{v}(k-1)-\mathit{v}(k-1)\end{array}\right]\hfill \\ & =\left[\begin{array}{c}\mathbf{\eta }(k)+\mathit{J}(k)\mathit{v}(k-1)T_s\\ \mathit{v}(k-1)\end{array}\right]+\left[\begin{array}{c}\mathit{J}(k)\mathit{v}(k)T_s+\mathit{J}(k)\mathit{v}(k-1)T_s\\ \mathit{v}(k)-\mathit{v}(k-1)\end{array}\right]\hfill \\ & =\left[\begin{array}{cc}{\mathit{I}}_5& \mathit{J}(k)T_s\\ {\mathbf{0}}_{5\times 5}& {\mathit{I}}_5\end{array}\right]\left[\begin{array}{c}\mathbf{\eta }(k)\\ \mathit{v}(k-1)\end{array}\right]+\left[\begin{array}{c}\mathit{J}(k)T_s\\ {\mathit{I}}_5\end{array}\right]\left[\begin{array}{c}\mathit{v}(k)-\mathit{v}(k-1)\end{array}\right]\hfill \end{array}$$

(12)

where $\mathit{Y}(k)$ is the output. By introducing the two matrices ${\mathsf{\Gamma }}_A(k)$ and ${\mathsf{\Gamma }}_B(k)$, we can get the following form:

$$\mathit{X}(k+1)={\mathsf{\Gamma }}_A(k)X(k)+{\mathsf{\Gamma }}_B(k)\Delta \mathit{v}(k), \mathit{Y}(k)=\mathsf{\Lambda }\mathit{X}(k)$$

(13)

with

$${\mathsf{\Gamma }}_A(k)=\left[\begin{array}{c}{\mathit{I}}_5\mathit{J}(k)T_s\\ {\mathbf{0}}_{5\times 5}{\mathit{I}}_5\end{array}\right], {\mathsf{\Gamma }}_B(k)=\left[\begin{array}{c}\mathit{J}(k)T_s\\ {\mathit{I}}_5\end{array}\right], \mathsf{\Lambda }=\left[\begin{array}{c}{\mathit{I}}_5{\mathbf{0}}_{5\times 5}\end{array}\right]$$

(14)

where ${\mathit{I}}_5$ is a $5\times 5$ identity matrix, and ${\mathbf{0}}_{5\times 5}$ is a $5\times 5$ zero matrix.

Definitions $N_p$ and $N_c$ are the prediction horizon and control horizon, respectively, and satisfy $N_c\le N_p$. The predicted sequence of the state and the output can be expressed in the following form:

$${\mathit{X}}_{N_p}=\left[\begin{array}{c}X(k+1|k)\\ X(k+2|k)\\ ⋮\\ X(k+N_p|k)\end{array}\right],{\mathit{Y}}_{N_p}=\left[\begin{array}{c}Y(k+1|k)\\ Y(k+2|k)\\ ⋮\\ Y(k+N_p|k)\end{array}\right]$$

(15)

The control sequence of the velocity increment as an input to the MPC controller is as follows:

$$\Delta \mathit{v}(k)=\left[\begin{array}{c}\Delta v(k+1|k)\\ \Delta v(k+2|k)\\ ⋮\\ \Delta v(k+N_p-1|k)\end{array}\right]$$

(16)

Based on MPC control theory, the predictive control system model of the AUV can be rewritten as follows:

$$\overline{\mathit{X}}(k+1)={\overline{\mathsf{\Gamma }}}_A(k)\mathit{X}(k)+{\overline{\mathsf{\Gamma }}}_B(k)\Delta \mathit{v}(k)$$

(17)

Here,

$${\overline{\mathsf{\Gamma }}}_A(k)={[{\mathsf{\Gamma }}_A(k),{\mathsf{\Gamma }}_A{(k)}^2,\cdots ,{\mathsf{\Gamma }}_A{(k)}^{N_p}]}^{\mathrm{T}}$$

(18)

$${\overline{\mathsf{\Gamma }}}_B(k)=\left[\begin{array}{cccc}{\mathsf{\Gamma }}_B(k)& 0& \cdots & 0\\ {\mathsf{\Gamma }}_A(k){\mathsf{\Gamma }}_B(k)& {\mathsf{\Gamma }}_B(k)& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\mathsf{\Gamma }}_A{(k)}^{N_p-1}{\mathsf{\Gamma }}_B(k)& {\mathsf{\Gamma }}_A{(k)}^{N_p-2}{\mathsf{\Gamma }}_B(k)& \cdots & {\mathsf{\Gamma }}_B(k)\end{array}\right]$$

(19)

The advantage of MPC is that it can solve practical problems with constraints. Here, the constraints of the control input and the AUV state are given as follows:

$$\Delta {\mathit{v}}_{min}\le \Delta \mathit{v}(k)\le \Delta {\mathit{v}}_{max}$$

(20)

$$X_{min}\le \mathit{X}(k)\le X_{max}$$

(21)

Based on the current state and predicted future state, the cost function of MPC rolling optimization is:

$$H(t)={\int }_0^T[\parallel \mathit{Y}(t)-{\mathit{Y}}_d{(t)\parallel }_{{\mathsf{\Gamma }}_y}^2+{\parallel \Delta \mathit{v}(t)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]$$

(22)

where

$${\parallel y\parallel }_{{\mathsf{\Gamma }}_y}^2=y^{\mathrm{T}}{\mathsf{\Gamma }}_{y}y,{\parallel \Delta \mathit{v}\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2=\Delta {\mathit{v}}^{\mathrm{T}}{\mathsf{\Gamma }}_{\Delta v}\Delta \mathit{v}$$

(23)

The symmetrical positive definite weight matrices ${\mathsf{\Gamma }}_y$ and ${\mathsf{\Gamma }}_{\Delta v}$ are introduced. The discretized form of $E(t)$ is as follows:

$$H(k)=\sum _{i=1}^{N_p}[\parallel \mathit{Y}(k+i|k)-{\mathit{Y}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_y}^2+{\parallel \Delta \mathit{v}(k+i-1|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]$$

(24)

Based on Equation (13), Equation (24) can be rewritten as follows:

$$H(k)=\sum _{i=1}^{N_p}[\parallel \mathit{X}(k+i|k)-{\mathit{X}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_x}^2+{\parallel \Delta \mathit{v}(k+i-1|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]$$

(25)

where

$$\mathit{X}(k+i)={\mathsf{\Lambda }}^{†}\mathit{Y}(k+i), {\mathsf{\Gamma }}_x={\mathsf{\Lambda }}^{†}{\mathsf{\Gamma }}_y\mathsf{\Lambda }$$

(26)

The pseudo inverse matrix of ${\mathsf{\Lambda }}^{†}$ represents $\mathsf{\Lambda }$. Based on Equation (17), a simplification of Equation (25) can be written as:

$$H(k)=\parallel \overline{\mathit{X}}(k)-{\overline{\mathit{X}}}_d{(k)\parallel }_{{\overline{\mathsf{\Gamma }}}_x}^2+{\parallel \Delta \mathit{v}(k)\parallel }_{{\overline{\mathsf{\Gamma }}}_{\Delta v}}^2$$

(27)

where ${\overline{\mathsf{\Gamma }}}_x=diag({\mathsf{\Gamma }}_x,{\mathsf{\Gamma }}_x,{\mathsf{\Gamma }}_x,{\mathsf{\Gamma }}_x,{\mathsf{\Gamma }}_x)$, ${\overline{\mathsf{\Gamma }}}_{\Delta v}=diag({\mathsf{\Gamma }}_{\Delta v},{\mathsf{\Gamma }}_{\Delta v},{\mathsf{\Gamma }}_{\Delta v},{\mathsf{\Gamma }}_{\Delta v},{\mathsf{\Gamma }}_{\Delta v})$.

It can be clearly concluded from Equation (27) that the only independent variable of $E(k)$ is $\Delta \mathit{v}(k)$, so the problem of obtaining the MPC optimal control sequence of the AUV is equivalent to the control input sequence of solving the minimum value of $E(k)$ with Equations (20) and (21) with constraints. The expression is as follows:

$$\Delta {\mathit{v}}^{*}(k)=\underset{\Delta \mathit{v}(k)}{argmin}E(k)$$

(28)

3.2. APSO for Solving the Cost Function

In this part, the APSO method is introduced in place of the QP method to obtain the minimum value of $E(k)$. The adaptive inertia weight is introduced to improve the optimization effect and convergence velocity of the traditional PSO algorithm. The prediction input $\Delta v(k)$ is used as the optimization variable. In the MPC process, the dimensionality D of each particle in the particle swarm is the control horizon $N_c$.

(1)

Initialize the particle swarm.

Set the population size $N_{ps}$ and constrict the position of each particle in the search area. The position of the ith particle is expressed as $x_i=(x_{i1},x_{i2},\cdots ,x_{iD})$, $i=(1,2,\cdots ,N_{ps})$. The fitness function for MPC is the objective function given in Equation (24). The best position experienced by the ith particle, that is, the position with the smallest fitness function value, is expressed as $p_i=(p_{i1},p_{i2},\cdots ,p_{iD})$, $i=(1,2,\cdots ,N_{ps})$. The best position experienced among all particles in the population is expressed as $p_g=(p_{g1},p_{g2},\cdots ,p_{gD})$, $g=(1,2,\cdots ,N_{ps})$.

(2)

Adaptive inertia weight.

To effectively control the flight velocity of particles and make the algorithm achieve an effective balance between local search and global search, we constructed the PSO model with the introduction of an adaptive inertia weight ${\rm Y}$. This adjustment method can ensure the convergence of the PSO algorithm by appropriately selecting parameters and can remove the boundary restriction on velocity. In the application of solving the optimal value, a large inertia weight should be applied at the beginning of the APSO algorithm to increase the searchability of the algorithm and avoid obtaining a local optimal solution. As the algorithm progresses, the inertia weight should be gradually reduced to enhance the local searchability of the algorithm and accelerate the convergence of the algorithm to obtain the global optimal solution. Adaptive inertia weight ${\rm Y}$ is defined as:

$${{\rm Y}}_{id}^k=\left\{\begin{array}{cc}\hfill {{\rm Y}}_{min}+({{\rm Y}}_{max}-{{\rm Y}}_{min})\frac{E(x_{id}^k)-E_{min}^k}{E_{av}^k-E_{min}^k}& , E(x_{id}^k)\le E_{min}^k\hfill \\ \hfill {{\rm Y}}_{max}& , E(x_{id}^k)>E_{min}^k\hfill \end{array}\right.$$

(29)

The particle velocity update function is

$$v_{id}^{k+1}={{\rm Y}}_{id}^k{v}_{id}^k+c_1{r}_1(p_{id}^k-x_{id}^k)+c_2{r}_2(p_{gd}^k-x_{gd}^k)$$

(30)

where $c_1$ and $c_2$ are learning factors, which are normal numbers; and $r_1$ and $r_2$ are random numbers between 0 and 1. The velocity of the ith particle is expressed as $v_i=(v_{i1},v_{i2},\cdots ,v_{iD})$, $i=(1,2,\cdots ,N_{ps})$. The particle position update function is as follows:

$$x_{id}^{k+1}=x_{id}^k+v_{id}^{k+1}$$

(31)

(3)

Operation process of APSO algorithm.

We use the fitness function given in Equation (24) to calculate the historical optimal value of each individual particle and the historical optimal value of the entire particle range (global). We update the current state of each particle using the position and velocity update functions in Equations (30) and (31). We repeat the above calculation process.

The flow of the particle swarm optimization algorithm with the adaptive compression factor is shown in Algorithm 1.

Algorithm 1 Pseudocode of particle swarm optimization algorithm with adaptive inertia weight.

Start of Algorithm; Input: Maximum number of iterations: $N_{max}$; current iteration number: $N_{current}$; number of particles: $N_{ps}$; APSO initialization: Initialize the particle swarm (position and velocity). Equation (22) is used to evaluate the initial fitness value of each particle; while$(N_{current}\le N_{max})$; Step 1: Rank the fitness values and find the current best ${\mathit{X}}_{best}^{N_{current}}$ and worst individual ${\mathit{X}}_{worst}^{N_{current}}$; Step 2: The adaptive inertia weight ${\rm Y}$ of Equation (29) is introduced into the position update equation of the algorithm; Step 3: Use Equation (30) to update the velocity of the particles; Step 4: Use Equation (31) to update the position of the particles; Step 5: Get the current new position of all particles; Step 6: If the new best particle location is better than the previous one, update it; $N_{current}=N_{current}+1$; end while. Return Step 1. Stop iterating when the maximum number of iterations is reached or the optimal solution is converged on. Output: ${\mathit{X}}_{best}^{N_{max}}$. End of Algorithm.

We set a maximum number of iterations. The output optimal solution ${\mathit{X}}_{best}^{N_{max}}$ is the optimal control sequence $\Delta {\mathit{v}}^{*}(k)$ of MPC. We feedback the first component $\Delta {\mathit{v}}^{*}(k+1|k)$ of the optimal control sequence $\Delta {\mathit{v}}^{*}(k)$ to the control system. The optimal velocity command transmitted to the NTSMC velocity controller is:

$${\mathit{v}}_d(k)=\mathit{v}(k)+\Delta {\mathit{v}}^{*}(k+1|k)$$

(32)

3.3. Stability Analysis of MPC-APSO Controller

In this part, we introduce terminal constraints to analyze the stability of the MPC-APSO controller.

Lemma 1. 

By using the traditional Lyapunov stability proof method in control theory, which finds a positive definite Lyapunov function whose inverse is negative definite, the stability of the controller is proved.

Proof. 

At current moment k, the optimal velocity increment sequence obtained by MPC-APSO can be defined as:

$$\Delta {\mathit{v}}^{*}(k)=\left[\begin{array}{c}\Delta {\mathit{v}}^{*}(k|k)\\ \Delta {\mathit{v}}^{*}(k+1|k)\\ ⋮\\ \Delta {\mathit{v}}^{*}(k+N_p-1|k)\end{array}\right]$$

(33)

After the terminal constraint is introduced, the optimal AUV state sequence and output sequence can be defined as:

$$\begin{array}{cc}\hfill & \left\{{\mathit{X}}_k^{*}(k+1|k),{\mathit{X}}_k^{*}(k+2|k),\cdots ,{\mathit{X}}_k^{*}(k+N_p-1|k),0\right\}\hfill \\ & \left\{{\mathit{Y}}_k^{*}(k+1|k),{\mathit{Y}}_k^{*}(k+2|k),\cdots ,{\mathit{Y}}_k^{*}(k+N_p-1|k),0\right\}\hfill \end{array}$$

(34)

The optimal cost function can be expressed as follows:

$$H^{*}(k)=\sum _{i=0}^{N_p-1}[\parallel {\mathit{X}}^{*}(k+i|k)-{\mathit{X}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_y}^2+\parallel \Delta {\mathit{v}}^{*}(k+i|k){\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]$$

(35)

For the MPC-APSO controller, $H^{*}(k)$ is defined as Lyapunov function $H_m^{*}(k)$ according to the Lyapunov stability theorem.

$$H_m^{*}(k)=\sum _{i=0}^{N_p-1}[\parallel {\mathit{X}}^{*}(k+i|k)-{\mathit{X}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_y}^2+\parallel \Delta {\mathit{v}}^{*}(k+i|k){\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]$$

(36)

Clearly, Equation (36) satisfies $H_m^{*}(0)=0$ with $k=0$ and $H_m^{*}(k)>0$ with arbitrary $k\ne 0$.

In Equations (33) and (34), ${\mathit{X}}^{*}(k|k)=\mathit{X}(k)$ and $\Delta {\mathit{v}}^{*}(k|k)=\Delta \mathit{v}(k)$. By combining these with Equation (13), we can easily obtain:

$$\mathit{X}(k+1)={\mathsf{\Gamma }}_A(k)\mathit{X}(k)+{\mathsf{\Gamma }}_B(k)\Delta {\mathit{v}}^{*}(k|k)={\mathit{X}}^{*}(k+1)$$

(37)

When time $k+1$ is reached, this control sequence of MPC-APSO is in the following form:

$$\Delta \mathit{v}(k+1)=\left[\begin{array}{c}\Delta {\mathit{v}}^{*}(k+1|k)\\ \Delta \mathit{v}(k+2|k)\\ ⋮\\ \Delta \mathit{v}(k+N_p-1|k)\\ 0\end{array}\right]$$

(38)

With the conditions of satisfying terminal constraints and system constraints, the status sequence of the AUV is:

$$\mathit{X}(k+i+1|k+1)=\left\{\begin{array}{cc}\hfill {\mathit{X}}^{*}(k+i+1|k),& i=0,\cdots ,N_p-1\hfill \\ \hfill 0,& i=N_p\hfill \end{array}\right.$$

(39)

By substituting Equations (38) and (39) into Equation (25), we obtain

$$\begin{array}{cc}\hfill H(k+1)& =\sum _{i=0}^{N_p-1}[\parallel \mathit{X}(k+i+1|k+1)-{\mathit{X}}_d{(k+i+1|k+1)\parallel }_{{\mathsf{\Gamma }}_x}^2+{\parallel \Delta \mathit{v}(k+i+1|k+1)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]\hfill \\ & =\sum _{i=0}^{N_p-2}[\parallel {\mathit{X}}^{*}(k+i+1|k)-{\mathit{X}}_d{(k+i+1|k)\parallel }_{{\mathsf{\Gamma }}_x}^2+\parallel \Delta {\mathit{v}}^{*}(k+i+1|k){\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]\hfill \\ & =\sum _{i=1}^{N_p-1}[\parallel {\mathit{X}}^{*}(k+i|k)-{\mathit{X}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_x}^2+\parallel \Delta {\mathit{v}}^{*}(k+i|k){\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]\hfill \\ & =\sum _{i=0}^{N_p-1}[\parallel {\mathit{X}}^{*}(k+i|k)-{\mathit{X}}_d{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_x}^2+\parallel \Delta {\mathit{v}}^{*}{(k+i|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2]-\parallel {\mathit{X}}^{*}(k|k)-{\mathit{X}}_d{(k|k)\parallel }_{{\mathsf{\Gamma }}_x}^2-{\parallel \Delta {\mathit{v}}^{*}(k|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2\hfill \\ & =H^{*}(k)-\parallel {\mathit{X}}^{*}(k|k)-{\mathit{X}}_d{(k|k)\parallel }_{{\mathsf{\Gamma }}_x}^2-{\parallel \Delta {\mathit{v}}^{*}(k|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2\hfill \end{array}$$

(40)

Obviously,

$$H^{*}(k+1)\le H(k+1)=H^{*}(k)-\parallel {\mathit{X}}^{*}(k|k)-{\mathit{X}}_d{(k|k)\parallel }_{{\mathsf{\Gamma }}_x}^2-{\parallel \Delta {\mathit{v}}^{*}(k|k)\parallel }_{{\mathsf{\Gamma }}_{\Delta v}}^2$$

(41)

$$H_m^{*}(k+1)\le H_m^{*}(k)$$

(42)

Here, the contents of Equation (36) conform to Lemma 1. Thus, Equation (17) is nominally stable. □

3.4. Design of NN-NTSMC Velocity Controller

In this part, the NTSMC velocity controller is designed to enhance the control accuracy and robustness of the AUV control system. The task of the controller is to determine the forces and moments that drive the AUV based on the input velocity errors. At the same time, the RBF-NN compensator is designed to compensate for the dynamic modeling error of the AUV and the influence of external interference on the control system.

Obviously, it is difficult to obtain the complete model information of the AUV in practice. Equation (3) is only the nominal model of the AUV, and when the AUV works in an underwater environment, the real-time disturbances of the external environment to the AUV are also unknown. Therefore, the dynamic modeling error of the AUV can be expressed as the following unknown function:

$$\mathsf{\Theta }(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})=\Delta M\dot{\mathit{v}}+\Delta C\mathit{v}+\Delta D\mathit{v}+\Delta g(\mathbf{\eta })+{\mathbf{\tau }}_d$$

(43)

Here, $\Delta M$, $\Delta C$, $\Delta D$, and $\Delta g$ are the error matrices corresponding to the nominal model and the actual model of the dynamics, respectively. Therefore, Equation (3) should be rewritten as follows:

$$\mathit{M}\dot{\mathit{v}}+\mathit{C}(\mathit{v})\mathit{v}+\mathit{D}(\mathit{v})\mathit{v}+\mathit{g}(\mathbf{\eta })+\mathsf{\Theta }(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})=\mathbf{\tau }$$

(44)

Remark 4. 

An RBF neural network can approximate any smooth unknown function $\mathsf{\Theta }(·)$ in the following form [38]:

$$\mathsf{\Theta }(·)=\sum _{i=1}^n{\omega }_i^{*}{h}_i(·)+\epsilon (·)$$

(45)

where n is the number of neurons in the hidden layer, and $\epsilon (·)$ is the steady-state error of the NN. The value ${\omega }_i^{*}$ is the optimal connection weight between the ith neuron in the hidden layer and the output layer, which is defined as:

$$\sum _{i=1}^n{\omega }_i^{*}=argmin[\sum _{i=1}^n{\omega }_i^{*}{h}_i(·)-f(·)]$$

(46)

Here, $h_i(·)$ is the activation function of the neurons in the hidden layer. This paper selects the radial basis function–Gaussian function, which is defined as follows:

$$h_i(\varphi (k-1))=e^{\frac{\parallel \mathit{h}(·)-{\mu }_i{\parallel }^2}{2{\sigma }_i^2}}$$

(47)

where ${\mu }_i$ is the center vector of the hidden layer neuron, and ${\sigma }_i$ is the width of the ith neuron in the hidden layer. The function $\parallel \mathit{h}(·)-{\mu }_i\parallel$ is the euclidean distance between the input matrix and the center of the radial basis.

In this paper, a three-layer feed-forward RBF NN is used to construct the unknown item $\mathsf{\Theta }(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})$ in Equation (44). The proposed RBF-NN compensator uses k-means to determine the cluster center and width, and it uses the gradient-descent-based optimization method to update the network weight. The actual estimate of the unknown item is as follows:

$$\widehat{\mathsf{\Theta }}(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})=\sum _{i=1}^n{\widehat{\omega }}_i{\widehat{h}}_i(·)+\epsilon (·)$$

(48)

The difference between the optimal expected velocity of the system obtained by MPC-APSO and the actual velocity of the AUV is used as the input to the NTSMC dynamics controller and is defined as follows:

$${\mathit{e}}_v={\mathit{v}}_d-\mathit{v}={[e_u,e_v,e_w,e_q,e_r]}^{\mathrm{T}} {\dot{\mathit{e}}}_v={\dot{\mathit{v}}}_d-\dot{\mathit{v}}={[{\dot{e}}_u,{\dot{e}}_v,{\dot{e}}_w,{\dot{e}}_q,{\dot{e}}_r]}^{\mathrm{T}}$$

(49)

Here, ${\mathit{v}}_d={[v_{du},v_{dv},v_{dw},v_{dq},v_{dr}]}^{\mathrm{T}}$. The non-singular terminal sliding mode surface function is designed as follows:

$$\mathit{s}={\dot{\mathit{e}}}_v+\frac{1}{\delta }{{\mathit{e}}_v}^{\alpha /\beta }$$

(50)

Here, $\delta >0$, $\alpha$, and $\beta$ are odd numbers and satisfy $\alpha >\beta >0$. A hyperbolic tangent function is used instead of a sign function to reduce the jump at 0, and its exponential approach law is used:

$$\begin{array}{cc}\hfill \dot{\mathit{s}}& ={\ddot{\mathit{e}}}_v+\frac{\alpha }{\delta \beta }{{\mathit{e}}_v}^{(\frac{\alpha -\beta }{\beta })}{\dot{\mathit{e}}}_v\hfill \\ & =-\mathit{tanh}(\frac{\mathit{s}}{\mathbf{\gamma }})\hfill \\ & =-{[tanh{(\frac{s}{\gamma })}_u,tanh{(\frac{s}{\gamma })}_v,tanh{(\frac{s}{\gamma })}_w,tanh{(\frac{s}{\gamma })}_q,tanh{(\frac{s}{\gamma })}_r]}^{\mathrm{T}}\hfill \\ & =({\ddot{\mathit{v}}}_d-\ddot{\mathit{v}})+\frac{\alpha }{\delta \beta }{({\mathit{v}}_d-\mathit{v})}^{(\frac{\alpha -\beta }{\beta })}({\dot{\mathit{v}}}_d-\dot{\mathit{v}})\hfill \end{array}$$

(51)

Here, $\gamma >0$ denotes the velocity at which the moving point of the system approaches the switching surface $s=0$. Obviously, Equation (51) can be transformed into the following form:

$$\dot{\mathit{v}}={\dot{\mathit{v}}}_d-{(\mathit{v}-{\mathit{v}}_d)}^{-(\frac{\alpha -\beta }{\beta })}\frac{\alpha }{\delta \beta }[\ddot{\mathit{v}}-{\ddot{\mathit{v}}}_d-\mathit{tanh}(\frac{\mathit{s}}{\mathbf{\gamma }})]$$

(52)

According to Equations (44) and (48), we can obtain:

$$\dot{\mathit{v}}={\mathit{M}}^{-1}[\mathbf{\tau }-\mathit{C}(\mathit{v})\mathit{v}-\mathit{D}(\mathit{v})\mathit{v}-\mathit{g}(\mathbf{\eta })-\widehat{\mathsf{\Theta }}(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})]$$

(53)

According to Equations (3) to (5), by combining Equations (52) and (53), the amount of control for each degree of freedom of the AUV can be obtained:

$${\tau }_x=(m-X_{\dot{u}})({\dot{v}}_{du}-{(u-v_{du})}^{-(\frac{\alpha -\beta }{\beta })}\frac{\delta \beta }{\alpha }[\ddot{u}-{\ddot{u}}_d-tanh(\frac{s}{\gamma })])+m(wq-vr)-Z_{\dot{w}}wq+Y_{\dot{v}}vr+(X_{\dot{u}}u+X_{\left|u\right|u}\left|u\right|)u+\widehat{\mathsf{\Theta }}{(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})}_x$$

(54)

$${\tau }_y=(m-Y_{\dot{v}})({\dot{v}}_{dv}-{(v-v_{dv})}^{-(\frac{\alpha -\beta }{\beta })}\frac{\delta \beta }{\alpha }[\ddot{v}-{\ddot{v}}_d-tanh(\frac{s}{\gamma })])+ur(m-X_{\dot{u}})+(Y_{\dot{v}}u+Y_{\left|v\right|v}\left|v\right|)v+\widehat{\mathsf{\Theta }}{(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})}_y$$

(55)

$${\tau }_z=(m-Z_{\dot{w}})({\dot{v}}_{dw}-{(w-v_{dw})}^{-(\frac{\alpha -\beta }{\beta })}\frac{\delta \beta }{\alpha }[\ddot{w}-{\ddot{w}}_d-tanh(\frac{s}{\gamma })])-uq(m-X_{\dot{u}})+(Z_{\dot{w}}w+Z_{\left|w\right|w}\left|w\right|)w+\widehat{\mathsf{\Theta }}{(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})}_z$$

(56)

$$\begin{array}{cc}\hfill {\tau }_{\theta }=& (I_y-M_{\dot{q}})({\dot{v}}_{dq}-{(q-v_{dq})}^{-(\frac{\alpha -\beta }{\beta })}\frac{\delta \beta }{\alpha }[\ddot{q}-{\ddot{q}}_d-tanh(\frac{s}{\gamma })])\hfill \\ & +mu(w-v)+wu(Z_{\dot{w}}-X_{\dot{u}})+(M_{\dot{q}}q+M_{\left|q\right|q}\left|q\right|)q-z_{B}Bsin\theta -x_{B}cos\theta +\widehat{\mathsf{\Theta }}{(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})}_{\theta }\hfill \end{array}$$

(57)

$${\tau }_{\psi }=(I_z-N_{\dot{r}})({\dot{v}}_{dr}-{(r-v_{dr})}^{-(\frac{\alpha -\beta }{\beta })}\frac{\delta \beta }{\alpha }[\ddot{r}-{\ddot{r}}_d-tanh(\frac{s}{\gamma })])+uv(X_{\dot{u}}-Y_{\dot{v}})+(N_{\dot{r}}r+N_{\left|r\right|r}\left|r\right|)r+y_{B}Bsin\theta +\widehat{\mathsf{\Theta }}{(\mathbf{\eta },\mathit{v},\dot{\mathit{v}})}_{\psi }$$

(58)

3.5. Stability Analysis of RBF-NTSMC Controller

For the NTSMC velocity controller, the Lyapunov function $H_s^{*}(k)$ is defined as follows:

$$H_s^{*}(k)=\left(\frac{1}{2}\right){\mathit{s}}^2$$

(59)

$${\dot{H}}_s^{*}(k)=\dot{\mathit{s}}\mathit{s}=-\mathit{s}\mathit{tanh}(\frac{\mathit{s}}{\mathbf{\gamma }})=-\left|\mathit{s}\right|$$

(60)

It can be readily derived that $L_s^{*}(k)>0$ and ${\dot{L}}_s^{*}(k)<0$, and that this also satisfies Lemma 1. Thus, the system is stable at the equilibrium point $\mathit{s}=0$, satisfying the conditions of the Lyapunov function, and $\mathit{s}$ eventually stabilizes the NTSMC surface.

4. Simulation Results

In this part, MATLAB/Simulink are used to verify the effectiveness of the proposed cascade controller (MPC-APSO-NTSMC) in AUV 3D trajectory tracking control. The simulation results are compared with the simulation results of the position and velocity double-closed-loop proportional differential (PD) controller (PD-PD) and the traditional MPC and linear SMC cascade controller (MPC-PSO-SMC).

The simulation and verification process is divided into two trajectory tracking cases: spiral trajectory tracking and complex curve tracking. The Simulink simulation model is shown in Figure 8. The basic physics and main hydrodynamic coefficients of the AUV are shown in

Table 1. Basic parameters and hydrodynamic coefficients of AUV.

Parameter	Value	Parameter	Value
Mass	183 kg	Diameter	0.4 m
Length	2.5 m	$I_y$	56.1 ${\mathrm{kgm}}^2$
$X_{\dot{u}}$	35.78 kg	$I_z$	54.5 ${\mathrm{kgm}}^2$
$Y_{\dot{v}}$	159.26 kg	$X_u$	5.77 $\mathrm{kg}/\mathrm{s}$
$Z_{\dot{w}}$	151.31 kg	$Y_v$	37.13 $\mathrm{kg}/\mathrm{s}$
$M_{\dot{q}}$	39.29 ${\mathrm{kgm}}^2$	$Z_w$	35.94 $\mathrm{kg}/\mathrm{s}$
$N_{\dot{r}}$	41.71 ${\mathrm{kgm}}^2$	$M_q$	10.71 ${\mathrm{kgm}}^2/(\mathrm{s}·\mathrm{rad})$
$X_{\left|u\right|u}$	17.77 $\mathrm{kg}/\mathrm{m}$	$N_r$	9.96 ${\mathrm{kgm}}^2/(\mathrm{s}·\mathrm{rad})$
$Y_{\left|v\right|v}$	150.23 $\mathrm{kg}/\mathrm{m}$	$M_{\left|q\right|q}$	26.55 ${\mathrm{kgm}}^2/{\mathrm{rad}}^2$
$Z_{\left|w\right|w}$	147.76 $\mathrm{kg}/\mathrm{m}$	$N_{\left|r\right|r}$	26.71 ${\mathrm{kgm}}^2/{\mathrm{rad}}^2$

. The simulation parameters are shown in

Table 2. Simulation parameters.

Parameter	Value	Parameter	Value
T	100 $\mathrm{s}$	$T_s$	100
$Y_{max}$	${[\infty ,\infty ,\infty ,\pi /2,\pi ]}^{\mathrm{T}}$	$N_{max}$	50
$Y_{min}$	${[-\infty ,-\infty ,-\infty ,-\pi /2,-\pi ]}^{\mathrm{T}}$	$c_1$	2.05
${\mathit{v}}_{max}$	${[2,2,2,0.5,0.5]}^{\mathrm{T}}$	$c_2$	2.05
${\mathit{v}}_{min}$	${[-2,-2,-2,-0.5,-0.5]}^{\mathrm{T}}$	$N_{ps}$	100
${\mathbf{\tau }}_{max}$	${[135,270,270,20,20]}^{\mathrm{T}}$	$N_p$	10
${\mathbf{\tau }}_{min}$	${[-90,-180,-180,-20,-20]}^{\mathrm{T}}$	$N_c$	4
${\mathsf{\Gamma }}_{\Delta \mathit{v}}$	$diag(1,1,1,1,1)$	${{\rm Y}}_{max}$	0.9
${\mathsf{\Gamma }}_x$	$diag(0.05,0.05,0.05,0.05,0.05)$	${{\rm Y}}_{min}$	0.4
$\Delta {\mathit{v}}_{max}$	${[0.15,0.1,0.1,0.05,0.05]}^{\mathrm{T}}$	n	15
$\Delta {\mathit{v}}_{min}$	${[-0.15,-0.1,-0.1,-0.05,-0.05]}^{\mathrm{T}}$

.

The random disturbance in the simulation is defined as follows:

$$\left\{\begin{array}{c}\hfill ran{d}_u=0.1\times \mathrm{rand}(1),(\mathrm{m}/\mathrm{s})\\ \hfill ran{d}_v=0.1\times \mathrm{rand}(1),(\mathrm{m}/\mathrm{s})\\ \hfill ran{d}_w=0.1\times \mathrm{rand}(1),(\mathrm{m}/\mathrm{s})\end{array}\right.$$

(61)

Here, rand(1) represents a random number between 0 and 1. The wave and current disturbance in the simulation is defined as follows:

$$\left\{\begin{array}{c}\hfill wav{e}_u=0.1\sin (\pi /8·\mathrm{t})+0.1,(\mathrm{m}/\mathrm{s})\\ \hfill wav{e}_v=0.1\sin (\pi /8·\mathrm{t})+0.1,(\mathrm{m}/\mathrm{s})\\ \hfill wac{e}_w=0.1\sin (\pi /8·\mathrm{t})+0.1,(\mathrm{m}/\mathrm{s})\end{array}\right.$$

(62)

Case 1: We set the initial position of the fully actuated AUV in the underwater simulation environment to be $\eta (0)={[0,10,0,0,0]}^{\mathrm{T}}$, and the function of the spiral trajectory with time is defined as follows:

$$\left\{\begin{array}{cc}\hfill x_d& =10\sin (0.1\mathrm{t})\hfill \\ \hfill y_d& =10-10\cos (0.1\mathrm{t})\hfill \\ \hfill z_d& =-0.25\mathrm{t}\hfill \\ \hfill {\theta }_d& =0.002\mathrm{t}\hfill \\ \hfill {\psi }_d& =0.1\mathrm{t}\hfill \end{array}\right.$$

(63)

It can be seen from Figure 9 and Figure 10 that all three controllers can provide better trajectory tracking performance for the AUV. However, since the AUV is a system with large inertia, the PD-PD controller (green curve) cannot track in time when the expected trajectory curvature of the AUV is large, resulting in relatively large tracking errors. In addition, the controller designed in this paper (red curve) has better final tracking performance than the MPC-PSO-SMC controller (blue curve) in terms of initial tracking speed and error control.

Figure 11 shows the thrust output of each thruster of the AUV during trajectory tracking. The figure shows that although the thrust output of the PD-PD controller (green curve) is not the maximum at the initial moment, the overall output fluctuates greatly, which is related to the difficulty of setting controller parameters. The controller proposed in this paper (red curve), under the action of the MPC-APSO kinematic controller and the NTSMC dynamics controller, has a relatively stable overall thrust output that is the best among the three.

Figure 12 shows the effect of the NTSMC velocity controller tracking the optimal expected velocity command (blue curve) based on the RBF-NN compensator designed in this paper. Obviously, the MPC-APSO method proposed in this paper can obtain the globally optimal expected velocity, and the proposed velocity controller has a good tracking effect (red curve).

Table 3. Position MAE without disturbance after 20 s for Case 1.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.2897	0.2445	0.2917	0.1547	0.1529
MPC-PSO-SMC	0.1570	0.1317	0.2184	0.1092	0.1291
MPC-APSO-NTSMC	0.1165	0.2013	0.1727	0.0815	0.0721

shows the mean absolute error (MAE) data obtained by simulation when the AUV tracks the spiral trajectory proposed in Case 1 without disturbance. We can clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of the AUV have the minimum position MAE. MAE data of pitch, roll, heave, pitch, and yaw positions were 0.1165 m, 0.2013 m, 0.1727 m, 0.0815 $\mathrm{rad}$, and 0.0721 $\mathrm{rad}$, respectively.

The simulation results of the three controllers for Case 1 with random disturbances are shown in Figure 13, Figure 14, Figure 15 and Figure 16.

Figure 13 shows 3D AUV tracking of a spiral trajectory with the influence of random disturbance. Obviously, compared with Case 1 without external disturbance, the tracking performance of the three control methods has declined, and the corresponding fluctuations are generated under the influence of random disturbance, which is reflected in the large changes to the trajectory curvature. However, compared with PD-PD (green curve) and MPC-PSO-SMC (blue curve), the proposed controller (red curve) still has the best tracking performance.

Figure 14 shows the trajectory tracking performance comparison of the three controllers for each degree of freedom. The PD-PD controller (green curve) has the largest error fluctuation. The proposed controller (red curve) under the action of the RBF-NN compensator not only improves the influence of model errors on the controller performance but also effectively compensates for external disturbance to minimize tracking error fluctuation and improve the robustness of the controller.

Figure 15 shows the comparison of simulation results of thruster thrust output in the case of random interference. Although the thrust output of the controller proposed in this paper has a small-amplitude oscillation with random interference, compared with the other two methods, the overall thrust output is relatively stable under the action of the MPC-APSO kinematic controller with the NTSMC dynamic controller. It is the best of the three.

Figure 16 shows the tracking of the velocity controller designed in this paper. The simulation results show that the velocity tracking performance of each degree of freedom of the AUV decreases slightly with random disturbance, but the overall tracking effect is good. At the same time, it can be proved that compared with the standard PSO, the APSO algorithm can obtain a global optimal value superior to the standard PSO in each sampling period of the MPC, and the proposed NTSMC velocity controller based on the RBF-NN compensator can effectively suppress the influence of external disturbances.

Table 4. Position MAE with random disturbance after 20 s for Case 1.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.3312	0.3239	0.3007	0.1835	0.2025
MPC-PSO-SMC	0.2018	0.1917	0.2354	0.1124	0.1301
MPC-APSO-NTSMC	0.1678	0.1913	0.1427	0.0775	0.0903

shows the MAE data obtained by simulation when the AUV tracks the spiral trajectory proposed in Case 1 with random disturbances. We can also clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of the AUV have the minimum position MAE. MAE data of surge, sway, heave, pitch, and yaw positions are 0.1678 m, 0.1913 m, 0.1427 m, 0.0775 $\mathrm{rad}$, and 0.0903 $\mathrm{rad}$, respectively.

In order to more fully consider the actual situation of underwater trajectory tracking of a fully actuated AUV, the wave and current interference is added in the simulation process, and the trajectory tracking control performance of PD-PD, MPC-PSO-NTSMC, and the proposed controller are again verified by simulation. The simulation results are shown in Figure 17, Figure 18, Figure 19 and Figure 20.

The results show that the proposed controller still has the best trajectory tracking performance and robustness, and all types of trajectory errors are the smallest among the three. The thrust output response speed is fast, the thrust output is smooth, and the speed tracking is correspondingly fast with the condition of wave and ocean current interference at the same time, which, again, proves the effectiveness and robustness of the controller (red curve).

Table 5. Position MAE with wave and current disturbance after 20 s for Case 1.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.3578	0.2789	0.2219	0.1949	0.1825
MPC-PSO-SMC	0.1965	0.2117	0.2784	0.1299	0.1791
MPC-APSO-NTSMC	0.1981	0.1699	0.1827	0.0916	0.0928

shows the MAE data obtained by simulation when the AUV tracks the spiral trajectory proposed in Case 1 with wave and current disturbance. We can clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of the AUV have the minimum position MAE. MAE data of surge, roll, heave, pitch, and yaw are 0.1981 m, 0.1699 m, 0.1827 m, 0.0915 $\mathrm{rad}$, and 0.0928 $\mathrm{rad}$, respectively.

Case 2: We set the initial position of the fully actuated AUV in the underwater simulation environment to be $\eta (0)={[0,10,0,0,0]}^{\mathrm{T}}$, and the function of the complex curve trajectory with time is defined as follows:

$$\left\{\begin{array}{cc}\hfill x_d& =2\sin (0.1\mathrm{t})+2\sin (0.3\mathrm{t})\hfill \\ \hfill y_d& =2\sin (0.05\mathrm{t})+2\sin (0.15\mathrm{t})\hfill \\ \hfill z_d& =-0.1\mathrm{t}\hfill \\ \hfill {\theta }_d& =0.001\mathrm{t}\hfill \\ \hfill {\psi }_d& =\sin (0.015\pi \mathrm{t})\hfill \end{array}\right.$$

(64)

Figure 21 and Figure 22 show the tracking performance of the AUV when tracking complex curves. It can be easily observed that the tracking errors of the three control methods under each degree of freedom increase significantly when tracking the trajectory of complex curves with multiple curvations, especially the position with large curvature changes, compared with tracking the spiral trajectory. However, the position tracking errors of the three control methods are all bounded. The overall tracking error result (red curve) of the proposed controller is better than that of the other two controllers.

Figure 23 shows the output of each thruster when the AUV tracks a complex curve. The PD-PD controller (green curve) has a large change rate of thruster output at the initial stage. Although the change rate of the MPC-PSO-SMC (blue curve) controller’s thruster output during the initial tracking process is better than that of the PD-PD controller, the output value is large. The proposed controller (red curve) has a minimum charge rate of thruster output during the entire tracking process, and it is relatively stable.

Figure 24 shows the effect of the NTSMC velocity controller based on the RBF-NN compensator designed in this paper to track the expected velocity command (blue curve). Obviously, the MPC-APSO method proposed in Case 2 can also obtain the globally optimal expected velocity, and the proposed velocity controller has a great tracking effect (red curve).

Table 6. Position MAE without disturbance after 20 s for Case 2.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.3112	0.2322	0.2001	0.1748	0.1698
MPC-PSO-SMC	0.2457	0.2234	0.2531	0.1406	0.1818
MPC-APSO-NTSMC	0.1778	0.1845	0.1025	0.1134	0.1219

shows the MAE data obtained by simulation when the AUV tracks the complex curve trajectory proposed in Case 2 without disturbance. We can also clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of the AUV have the minimum position MAE. MAE data of surge, roll, heave, pitch, and yaw are 0.1778 m, 0.1845 m, 0.1025 m, 0.1134 $\mathrm{rad}$, and 0.1219 $\mathrm{rad}$, respectively.

Figure 25, Figure 26, Figure 27 and Figure 28 show the trajectory tracking performance of the AUV when tracking complex curves with random disturbance. Similar to tracking the spiral track, with the effect of the random disturbance, the controller proposed in this paper (red curve) can overcome the influence of tracking fluctuation caused by multi-curvature changes and random disturbance regardless of the overall tracking effect, the output stability of thrusters, and the velocity tracking effect of NTSMC based on the NN compensator. The effectiveness and robustness of the controller are the best of the three.

Table 7. Position MAE with random disturbance after 20 s for Case 2.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.3457	0.2812	0.2321	0.1904	0.1717
MPC-PSO-SMC	0.2512	0.2331	0.2583	0.1567	0.1896
MPC-APSO-NTSMC	0.1812	0.1909	0.1215	0.1167	0.1439

shows the MAE data obtained by simulation when the AUV tracks the complex curve trajectory proposed in Case 2 with random disturbance. We can also clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of the AUV have the minimum position MAE. MAE data of surge, roll, heave, pitch, and yaw are 0.1812 m, 0.1909 m, 0.1215 m, 0.1167 $\mathrm{rad}$, and 0.1439 $\mathrm{rad}$, respectively.

Figure 29, Figure 30, Figure 31 and Figure 32 show the trajectory tracking performance of the AUV when tracking complex curves with wave and current disturbance. Similar to spiral tracking, with the effect of the wave and current disturbance, the controller proposed in this paper (red curve) can overcome the influence of tracking fluctuation caused by multi-curvature changes and wave and current disturbance regardless of the overall tracking effect, the output stability of thrusters, and the velocity tracking effect of NTSMC based on the NN compensator. The effectiveness and robustness of the controller are still the best of the three.

Table 8. Position MAE with wave and current disturbance after 20 s for Case 2.

Methods	x (m)	y (m)	z (m)	$\mathit{\theta }$ (rad)	$\mathit{\psi }$ (rad)
PD-PD	0.3549	0.2689	0.2339	0.1549	0.1634
MPC-PSO-SMC	0.1874	0.2201	0.2401	0.1337	0.1814
MPC-APSO-NTSMC	0.1843	0.1712	0.1901	0.1004	0.1115

shows the MAE data obtained by simulation when the AUV tracks the complex curve trajectory proposed in Case 2 with wave and current disturbance. We can also clearly draw from the data in the table that the MPC-APSO-NTSMC controller designed in this paper can make the trajectory tracking task of AUV have the minimum position MAE. MAE data of surge, roll, heave, pitch, and yaw are 0.1843 m, 0.1712 m, 0.1901 m, 0.1004 $\mathrm{rad}$, and 0.1115 $\mathrm{rad}$, respectively.

5. Conclusions

Aiming at the problem of underwater 3D dynamic trajectory tracking control of fully actuated AUVs, a novel cascade controller using MPC and NTSMC is proposed in this paper. Firstly, the five-degree-of-freedom kinematics and dynamics model of the fully actuated AUV is established in the corresponding coordinate system. Secondly, a novel PSO method with adaptive inertia weight is proposed to replace the QP method for solving the optimal control sequence in standard MPC. Combined with the state and control input constraints of the AUV, MPC-APSO obtains the optimal velocity control sequence, generates the expected velocity command, and transmits it to the NTSMC velocity controller. The introduction of adaptive inertia weight can balance and improve the local search and global search capabilities of the optimization algorithm, which overcomes the problem of the traditional MPC control being unable to find the global optimal solution when using QP to solve for the optimal solution. After that, considering the modeling error of the dynamic model of AUV and the unknown disturbance of the underwater environment when the AUV performs underwater trajectory tracking control, this paper improves the response speed and control accuracy of NTSMC by designing an RBF-NN compensator. Moreover, stability analysis based on the Lyapunov method proves the stability of the two controllers. Finally, two different expected 3D trajectories are designed using MATLAB/Simulink to verify the dynamic trajectory tracking performance of the AUV with different external disturbances, and the robustness and effectiveness of the designed controller are verified.

In future research, we will focus on practical problems of AUVs in trajectory tracking control, such as the rationality of the expected trajectory, the integration of the constraints with the underwater environment and AUV dynamic constraints, controller input saturation, multi-AUV cooperative formation control, etc.