Underactuated AUV Nonlinear Finite-Time Tracking Control Based on Command Filter and Disturbance Observer

Abstract

The three-dimensional (3D) path following problem of an underactuated autonomous underwater vehicle with ocean currents disturbances is addressed in this paper. Firstly, the motion equation under the ocean currents disturbance is established, and the dynamic model of 3D tracking error is constructed based on virtual guidance method. Then, a finite-time control scheme based on super-twisting observer and command filtered backstepping technology is proposed. We adopt super-twisting observer based on finite-time theory to observe the ocean currents disturbances for improving the system robust. A command filtered backstepping is proposed to replace the differential process in the conventional backstepping method for avoiding the differential expansion problem. The filter compensation loop is designed to ensure the accuracy of the filtered signal, and the anti-integration saturation link is designed considering the influence of integral saturation. Lyapunov stability theory is used to prove the stability of the underactuated AUV. Simulation studies are conducted to show the effectiveness and robustness of the controller.

1. Introduction

Autonomous underwater vehicles have become more and more practical in many fields, such as civil and military, underwater monitoring, deep sea environmental resource exploration and development, ocean data observation and collection, seabed topographic scanning, submarine pipeline detection and marine mine clearance, etc. [1,2,3]. The completion of related tasks generally requires the AUV to follow an ideal path. Therefore, the path following technology of AUV has attracted the attention of many scholars and has become a hot research topic. AUVs are often designed to be under-actuated, such as the REMUS series [4,5], for reasons of energy savings, weight reduction and increased reliability. Most of underactuated AUVs cannot directly provide lateral and vertical forces, and can only control surge velocity, yaw angle velocity and pitch angle velocity directly. In addition, AUV is also affected by external disturbances such as waves, currents and uncertainties of its own hydrodynamic parameters in the actual environment, which increases the difficulty of controller design [6,7].

In the past few decades, researchers have tried various control methods for path following control of underactuated AUVs [8,9]. A cascade structure consisting of an integral LOS guidance and feedback linearized proportional derivative controller was designed in [9]. A tracking control law based on the extended state observer and the optimal reference signals were proposed in [10]. The control strategy based on the input-output feedback linearization method was designed in [11]. The robust adaptive path following method based on fuzzy position observer was proposed in [12]. The above path following controllers have achieved good control effects. However, most of them focus on two-dimensional path following problems such as horizontal or vertical planes. In the three-dimensional space, due to the strong nonlinearity and strong coupling characteristics of AUV, the path following control of under-actuated AUV is more challenging. The underactuated AUV nonlinear controller of 3D path following was proposed for the first time [13], but there were singularities. For solving the singularity problem, the literature [14] introduced virtual guidance points in the path and firstly proposed the line-of-sight (LOS) method. Subsequently, many researchers followed and improved the line-of-sight method in kinematic and introduced intelligent controller in dynamic. Improved line-of-sight (LOS) and fuzzy controller were designed in [15]. A 3D trajectory tracking controller for underactuated AUV was designed by linear stability theory and backstepping method [16]. However, the impacts of ocean currents were not considered in the above literatures. Designing an observer is one of the common means of nonlinear system control [17,18]. Sliding mode control is also one of the effective control methods [19]. For the ocean currents disturbance, the tracking control scheme combined with the sliding mode control and backstepping method was designed in [20], and adopted the fuzzy logic theory to estimate the nonlinear term and disturbance. In the literature [21], the tracking error equations of horizontal and vertical planes were established based on the line-of-sight method, the controller was designed by cascade theory and backstepping method. The literature [22] improved the LOS guidance law, introduced an integration strategy to eliminate the effects of currents, and designed a dynamic controller based on relative velocity. However, the above research can only achieve tracking of spatial lines and tracking control of curved paths cannot be realized. In addition, the process of calculating the derivative of a virtual control signal in the backstepping method is very complicated and the differential expansion problem may occur. The literature [23] present a command-filtering backstepping controller, by using numerical integration for the derivative of the virtual control signal, for avoiding the problem of differential expansion. But the integral saturation was not considered in this method. And the error between the filtered signal and the virtual control signal cannot guarantee to convergence.

At present, most controllers can only achieve asymptotic convergence, and cannot guarantee convergence in a limited time. The finite-time control method has gradually gained in-depth research in recent years because of its advantages of high accuracy, fast convergence speed and strong robustness [24]. The path following control with finite-time control theory was applied to the horizontal plane tracking, and achieve good control effects [25,26].

Motivated by the above discussions, the command filtering backstepping method, super-twisting observer and finite-time control technology were adopted to propose a three-dimensional path following controller for underactuated AUV in the presence of ocean currents, which has not been proposed in the published literatures. The main contributions of this article are:

(1)

The differential filtering problem caused by the traditional backstepping calculation complexity is avoided by the command filtering backstepping method, and the part of filtering error compensation is proposed to ensure the accordance of virtual control signal and the filtered signal.

(2)

Since the integral action of the filter part, the anti-integration saturation is considered in the control loop to deal with the problem of integral saturation in the control signals.

(3)

A current disturbance observer is presented to reduce the impact of external ocean currents disturbances on the system and increase the tracking controller robustness.

(4)

The 3D path following controller, based on the finite-time control theory, is designed to improve the response speed and control accuracy of the controller.

The rest of this paper is organized as follows. Firstly, the 5-DOF motion model and error model for AUV are established in Section 2. The main results, including the ocean currents disturbance observer and the 3D path following controller, are presented in Section 3. In Section 4, the mathematical proof of the control system is presented. Numerical simulation results, carried in MATLAB-Simulink to demonstrate the controller tracking performance, are given in Section 5. In the end, short conclusions are presented in Section 6.

2. Problem Formulation

2.1. Coordinate System and Parameter Definition

As is depicted in Figure 1, $\left\{I\right\}$ is assumed as the inertial reference frame (earth-fixed frame), its original point can be set at any place in the ocean. $\left\{B\right\}$ is the body-fixed frame, its original point is set at the AUV’s gravity center. $\left\{SF\right\}$ denotes the Serret-Frenet frame, its original point $Q$ is the any point of the desired path. ${\eta }_e=\left(x,y,z\right)$ denotes the position of the AUV’s gravity center within the frame $\left\{I\right\}$. $\left(x_P,y_P,z_P\right)$ denotes the original center of $\left\{SF\right\}$ within the inertial reference frame. $\left(x_e,y_e,z_e\right)$ denotes the position of the AUV’s gravity center within the Serret-Frenet frame $\left\{SF\right\}$.

2.2. AUV Kinematic and Dynamic Equations

According to Reference [27], the nonlinear motion equations of AUV can be described:

$$\dot{\eta }=J(\eta )v,$$

(1)

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

(2)

where $\eta$ denotes the position and orientation vector with coordinates in the earth fixed frame, $J(\eta )$ is the transform matrix between the body-fixed and inertial coordinates, $v$ denotes the linear and angular velocity vector with coordinates in the body-fixed frame, $M$ is the inertial and add inertial matrix, $C(v)$ is the matrix od Coriolis and Centrifugal terms, $D(v)$ is the matrix of hydrodynamics terms, $g(\eta )$ is the vector of gravity and buoyant force, $\tau$ denotes the forces and moments in the body-fixed frame. It must be mentioned that the ocean currents were assumed constant in frame $\left\{B\right\}$ in many earlier researches. A more natural and real assumption are made in this paper: the ocean currents are constant in frame $\left\{I\right\}$. The ocean currents in frame $\left\{I\right\}$ are proposed as [27]

$$v_c^n={[u_c^n,v_c^n,w_c^n]}^T.$$

(3)

With the Euler angle rotation matrix, it can be transformed to the frame $\left\{B\right\}$:

$$\begin{array}{l}{v}_c^b=R_b^n(\psi ,\theta )·v_c^n\\ \hspace{1em}=\left[\begin{array}{ccc}\mathrm{c}\theta \mathrm{c}\psi & -\mathrm{s}\psi & \mathrm{s}\theta \mathrm{c}\psi \\ \mathrm{c}\theta \mathrm{s}\psi & \mathrm{c}\psi & \mathrm{s}\theta \mathrm{s}\psi \\ -\mathrm{s}\theta & 0& \mathrm{c}\theta \end{array}\right]·{[u_c^n,v_c^n,w_c^n]}^T\end{array},$$

(4)

where ${\mathrm{c}}^{\ast }$ and ${\mathrm{s}}^{\ast }$ are abbreviations for ${\cos }^{\ast }$ and ${\sin }^{\ast }$, respectively. Assume the ocean currents change slowly, the acceleration of the current is negligible such that

$${\dot{v}}_c^b\approx 0.$$

(5)

Usually, the roll motion of AUV can be ignored due to its left–right symmetrical structure [28]. Under the assumption, the motion equations can be given as

$$\{\begin{array}{l}\dot{x}=u_r\mathrm{c}\theta \mathrm{c}\psi -v_r\mathrm{s}\psi +w_r\mathrm{s}\theta \mathrm{c}\psi \\ \dot{y}=u_r\mathrm{c}\theta \mathrm{s}\psi +v_r\mathrm{c}\psi +w_r\mathrm{s}\theta \mathrm{s}\psi \\ \dot{z}=-u_r\mathrm{s}\theta +w_r\mathrm{c}\theta \\ \dot{\theta }=\dot{\alpha }+q\\ \dot{\psi }=\dot{\beta }+r/\mathrm{c}\theta \end{array},$$

(6)

$$\{\begin{array}{l}\dot{u}=\frac{1}{m-X_{\dot{u}}}{F}_u+\frac{X_{uu}}{m-X_{\dot{u}}}{u}_r{}^2+\frac{X_{vv}}{m-X_{\dot{u}}}{v}_r{}^2+\frac{X_{ww}}{m-X_{\dot{u}}}{w}_r{}^2+\frac{X_{qq}}{m-X_{\dot{u}}}{q}^2\\ \dot{v}=-\frac{m-Y_{ur}}{m-Y_{\ddot{v}}}{u}_{r}r+\frac{Y_{uv}}{m-Y_{\ddot{v}}}{u}_{r}v+\frac{Y_{v\left|v\right|}{v}_r}{m-Y_{\ddot{v}}}\left|v_r\right|\\ \dot{w}=-\frac{m-Z_{uq}}{m-Z_{\dot{w}}}{u}_{r}q+\frac{Z_{uw}}{m-Z_{\dot{w}}}{u}_r{w}_r+\frac{Z_{w\left|w\right|}}{m-Z_{\dot{w}}}{w}_r\left|w_r\right|+\frac{1}{m-Z_{\dot{w}}}m{z}_g{q}^2\\ \dot{q}=\frac{1}{I_y-M_{\dot{q}}}M+\frac{M_{q\left|q\right|}}{I_y-M_{\dot{q}}}q\left|q\right|-\frac{M_{uq}}{I_y-M_{\dot{q}}}{u}_{r}q-\frac{M_{uw}}{I_y-M_{\dot{q}}}{u}_r{w}_r-\\ \hspace{1em}\frac{1}{I_y-M_{\dot{q}}}(z_{g}w-z_{b}B)\mathrm{s}\theta -\frac{1}{I_y-M_{\dot{q}}}m{z}_g(w_{r}q-v_{r}r)\\ \dot{r}=\frac{1}{I_z-N_{\dot{r}}}N+\frac{N_{uv}}{I_z-N_{\dot{r}}}{u}_r{v}_r+\frac{N_{v\left|v\right|}}{I_z-N_{\dot{r}}}{v}_r\left|v_r\right|+\frac{N_{ur}}{I_z-N_{\dot{r}}}{u}_{r}r\end{array},$$

(7)

where $m$ is the AUV mass, $I_y$ and $I_z$ denote the inertia moments about the pitch and yaw rotation. $u_r$, $v_r$ and $w_r$ denote the real linear velocities, where $u_r=u-u_c^b$, $v_r=v-v_c^b$ and $w_r=w-w_c^b$. $q$, $r$ are the velocities of pitch and yaw. $r$ and $\psi$ present the pitch and yaw angle. The corresponding hydrodynamic derivatives are denoted by $X_{(\ast )}$, $Y_{(\ast )}$, $Z_{(\ast )}$, $M_{(\ast )}$ and $N_{(\ast )}$. $z_g$ and $z_b$ are the vertical positions of the gravity and buoyancy center within body-fixed frame. $X$ denotes the force generated by the stern thruster. $M$ and $N$ are the moments provided by horizontal and vertical rudders.

2.3. AUV Error Systems

The error formulations can be derived by using the Serret-Frenet frame $\left\{SF\right\}$ according to Figure 1 [29]. We define point P that is the origin of $\left\{SF\right\}$ as the virtual moving target AUV that describes the path, and the position in $\left\{I\right\}$ is ${\eta }_d^e={(x_d,y_d,z_d)}^T$. The Serret-Frenet frame is rotated with angle ${\theta }_F$, ${\psi }_F$ relative to the inertial frame.

$$\{\begin{array}{l}{\theta }_F=-\arctan \left(\frac{{\dot{z}}_d}{\sqrt{{\dot{x}}_d^2+{\dot{y}}_d^2}}\right)\\ {\psi }_F=\arctan \left(\frac{{\dot{y}}_d}{{\dot{x}}_d}\right)\end{array}.$$

(8)

The desired path is described by parameter $s$. Rotation angles can be also defined as

$$\{\begin{array}{l}{\dot{\theta }}_F={\mathrm{c}}_1(s)\dot{s}\\ {\dot{\psi }}_F={\mathrm{c}}_2(s)\dot{s}\end{array},$$

(9)

where ${\mathrm{c}}_1(s)$ and ${\mathrm{c}}_2(s)$ are the torsion and curvature of the virtual target point in spatial curve.

The AUV’s position is ${\eta }^e={(x,y,z)}^T$, and represented by point Q. The position of the AUV is usually obtained by GPS, but GPS is not available when the AUV is underwater. At that time, it can only be obtained by dead reckoning or long/short baseline instruments [30]. Hence, the error of path following can be defined in frame $\left\{B\right\}$.

$$\epsilon =R_b^{eT}({\eta }^e-{\eta }_d^e),$$

(10)

$$\dot{\epsilon }={\dot{R}}_b^{eT}({\eta }^e-{\eta }_d^e)+R_b^{eT}({\dot{\eta }}^e-{\dot{\eta }}_d^e),$$

(11)

where $R_b^e$ is the rotation matrix from $\left\{B\right\}$ to $\left\{I\right\}$. ${\dot{R}}_b^e=R_b^e{[\varpi ]}_{qr}$, ${[\varpi ]}_{qr}$ is the vector of angular velocity. ${\dot{\eta }}^e=R_b^e{v}_b$, $v_b={(u,v,w)}^T$ is the vector of velocities in $\left\{B\right\}$. ${\dot{\eta }}_d^e=R_F^e{v}_F$, $v_F={(u_r,0,0)}^T$ is the reference velocities. $R_F^e$ is the rotation matrix from $\left\{SF\right\}$ to $\left\{I\right\}$.

$$\begin{array}{l}\dot{\tau }={\left[\varpi \right]}_{qr}^T{R}_b^{eT}({\eta }^e-{\eta }_d^e)+R_b^{eT}({\dot{\eta }}^e-{\dot{\eta }}_d^e)\\ \hspace{1em}={\left[\varpi \right]}_{qr}^T\tau +R_b^{eT}{R}_b^e{v}_b-R_b^{eT}{R}_F^e{v}_F\\ \hspace{1em}={\left[\varpi \right]}_{qr}^T\tau +v_b-R({\psi }_e,{\theta }_e)v_F\end{array},$$

(12)

where ${\left[\varpi \right]}_{qr}=\left[\begin{array}{ccc}0& r& -q\\ -r& 0& 0\\ q& 0& 0\end{array}\right]$, $R_b^{eT}{R}_F^e=R({\psi }_e,{\theta }_e)$, ${\psi }_e=-{\psi }_F+\psi +\beta$ and ${\theta }_e=\theta +\alpha -{\theta }_F$. $R({\psi }_e,{\theta }_e)=\left(\begin{array}{ccc}\mathrm{c}{\theta }_e\mathrm{c}{\psi }_e& -\mathrm{s}{\psi }_e& \mathrm{s}{\theta }_e\mathrm{c}{\psi }_e\\ \mathrm{c}{\theta }_e\mathrm{s}{\psi }_e& \mathrm{c}{\psi }_e& \mathrm{s}{\theta }_e\mathrm{s}{\psi }_e\\ -\mathrm{s}{\theta }_e& 0& \mathrm{c}{\theta }_e\end{array}\right)$.

$$\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{z}}_e\end{array}\right]=\left[\begin{array}{ccc}0& r& -q\\ -r& 0& 0\\ q& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ y_e\\ z_e\end{array}\right]+\left[\begin{array}{c}u\\ v\\ w\end{array}\right]-R({\psi }_e,{\theta }_e)\left[\begin{array}{c}{u}_r\\ 0\\ 0\end{array}\right].$$

(13)

The angular velocities of virtual target are presented as Equation (9). Then we define the AUV path following error:

$$\{\begin{array}{l}{\dot{x}}_e=r{y}_e-q{z}_e+u-u_r\mathrm{c}{\psi }_e\mathrm{c}{\theta }_e\\ {\dot{y}}_e=-r{x}_e+v-u_r\mathrm{s}{\psi }_e\mathrm{c}{\theta }_e\\ {\dot{z}}_e=q{x}_e+w+u_r\mathrm{s}{\theta }_e\\ {\dot{\psi }}_e=-{\mathrm{c}}_1(s)\dot{s}+r/\mathrm{c}{\theta }_e+\dot{\beta }\\ {\dot{\theta }}_e=-{\mathrm{c}}_2(s)\dot{s}+q+\dot{\alpha }\end{array},$$

(14)

Lemma 1.

For nonlinear systems$\dot{\omega }=g(\omega ,\tau )$, where$\omega$denotes the state vector, and$\tau$is the control input,$g(·)$is continuous and$g(0)=0$, if$\lambda >0$,$0<\alpha <1$and$0<\eta <\infty$, and the continuous function$V(\omega )$meets$\dot{V}(\omega )\le -\lambda V^{\alpha }(\omega )+\eta$, then the system converges in finite-time [31].

Lemma 2.

For the system in lemma 1, if${\lambda }_1>0$,${\lambda }_2>0$and$0<\alpha <1$, and the continuous function$V(\omega )$meets$\dot{V}(\omega )\le -{\lambda }_{1}V(\omega )-{\lambda }_2{V}^{\alpha }(\omega )$, then the system converges in finite-time. The time of convergence is$T\le \frac{1}{{\lambda }_1(1-\alpha )}\ln \frac{({\lambda }_1{V}^{1-\alpha }({\omega }_0)+{\lambda }_2)}{{\lambda }_2}$, where the$V({\omega }_0)$is the initial value of $V(\omega )$ [32].

3. Design of Path Following Control

This section introduces the nonlinear control law to solve the path following control problem of underactuated AUV in the presence of the ocean current. The control flow chart is shown in Figure 2. Controller design is divided into two stages. The first stage handles the design of the ocean current observer based on super-twisting technology. Observer is used to compensate for the effects of currents on the system. The second stage addresses the tracking controller based on command filtered backstepping. Taking the designed virtual control as the input of the command filter, the derivative of the virtual control is obtained through the integration rather than the differential process.

Controller of the AUV is developed in this section such that the real AUV can track the virtual AUV, namely

$$\underset{t\in \left[t_0,\infty \right]}{\sup }(\Vert x_e\Vert ,\Vert y_e\Vert ,\Vert z_e\Vert ,\Vert u-u_d\Vert )\le ({\epsilon }_1,{\epsilon }_2,{\epsilon }_3,{\epsilon }_4),$$

(15)

where ${\epsilon }_i(i=1,2,3,4)$ denote arbitrary small positive numbers.

Referring to the LOS guidance theory, pitch and heading guidance laws are defined as

$$\{\begin{array}{l}{\theta }_{LOS}=-{\theta }_a\frac{e^{2k_{\theta }{z}_e}-1}{e^{2k_{\theta }{z}_e}+1}\\ {\psi }_{LOS}=-{\psi }_a\frac{e^{2k_{\psi }{y}_e}-1}{e^{2k_{\psi }{y}_e}+1}\end{array},$$

(16)

where ${\theta }_a\in \left(0,\frac{\pi }{2}\right)$ and ${\psi }_a\in \left(0,\frac{\pi }{2}\right)$ are chosen to let ${\theta }_{LOS}\in \left(-\frac{\pi }{2},\frac{\pi }{2}\right)$ and ${\psi }_{LOS}\in \left(-\frac{\pi }{2},\frac{\pi }{2}\right)$, $k_{\theta }>0$ and $k_{\psi }>0$ are gains. When the vertical error between AUV and desired path becomes larger, ${\theta }_{LOS}$ also increases, and when AUV is in the desired path, the error becomes zero, and ${\theta }_{LOS}=0$. ${\psi }_{LOS}$ is the same. AUV will have better endurance and the redundant range can be shortened to save energy by introducing the approach angle [29].

In an attempt to eliminate effects of differential expansion in the conventional method, a command filter is considered to add to the backstepping control loop. In this approach, the control signals are passed via filter to get derivative instead of the differentiation process.

$$\{\begin{array}{l}{\dot{z}}_1=z_2\\ {\dot{z}}_2=-2\zeta {\omega }_f{z}_2-{\omega }_f^2(z_1-x_{co})\end{array}$$

(17)

where $x_{co}={\left[\begin{array}{cccc}\begin{array}{ccc}\begin{array}{ccc}{x}_{ce}^o& y_{ce}^o& z_{ce}^o\end{array}& {\psi }_{ce}^o& {\theta }_{ce}^o\end{array}& u_c^o& r_c^o& q_c^o\end{array}\right]}^T$ is the vector of desired control, $z_1=x_c={\left[\begin{array}{cccc}\begin{array}{ccc}\begin{array}{ccc}{x}_{ce}& y_{ce}& z_{ce}\end{array}& {\psi }_{ce}& {\theta }_{ce}\end{array}& u_c& r_c& q_c\end{array}\right]}^T$ and $z_2={\dot{x}}_c={\left[\begin{array}{cccc}\begin{array}{ccc}\begin{array}{ccc}{\dot{x}}_{ce}& {\dot{y}}_{ce}& {\dot{z}}_{ce}\end{array}& {\dot{\psi }}_{ce}& {\dot{\theta }}_{ce}\end{array}& {\dot{u}}_c& {\dot{r}}_c& {\dot{q}}_c\end{array}\right]}^T$ are the filtered control signals vector. $\zeta$ and ω_f are the filter parameters, where 0 < ζ < 1, ω_f > 0. The flow chart of command filter is shown as Figure 3.

3.1. Design of Super-Twisting Disturbance Observer

In this section, an ocean currents observer based on super-twisting technology is designed. Let $\widehat{D}$ and $\widehat{x}$ denote the observation of $D$ and $x$, we can get

$$\dot{\widehat{x}}=A\widehat{x}+Bu+C+\widehat{D},$$

(18)

where, $A=-M^{-1}(D(v)+C(v))$, $B=M^{-1}$, $C=-M^{-1}g(\eta )$, $\widehat{D}=M^{-1}{\widehat{\tau }}_d$. Error vector of state $x$ and observation error $\tilde{D}$ can be present as

$$\begin{array}{l}e=x-\widehat{x}\\ \tilde{D}=D-\widehat{D}\end{array}.$$

(19)

The observer of disturbance was designed as

$${\widehat{D}}_i=k_1\sqrt{\left|e_i\right|}\mathrm{sgn}(e_i)+{\displaystyle \int k_2\mathrm{sgn}(e_i)dt,i=1,2,3},$$

(20)

where $k_1$ and $k_2$ are positive parameters.

Together with Equations (18) and (19), we have

$${\dot{e}}_i={\dot{x}}_i-{\dot{\widehat{x}}}_i=D_i-k_i\sqrt{\left|e_i\right|}\mathrm{sgn}(e_i)-{\displaystyle \int k_2\mathrm{sgn}(e_i)}dt,i=1,2,3.$$

(21)

Obviously, when the observation error of $x$ is finite-time converged, the observation error of disturbance also converges in finite-time. Following is the proof.

Proof. 

For convenience of proof, select the intermediate quantities $M\in R^{3\times 1}$ and $N\in R^{3\times 1}$

$$\begin{array}{l}{M}_i=k_1\sqrt{\left|e_i\right|}\mathrm{sgn}(e_i)\\ N_i=D_i-\frac{1}{2}{\displaystyle \int k_2\mathrm{sgn}(e_i)}\end{array}.$$

(22)

Differentiating the above equation, we can get

$$\begin{array}{l}{\dot{M}}_i=\frac{1}{2\left|M_i\right|}{\dot{e}}_i=\frac{1}{2\left|M_i\right|}(-k_1{M}_i+N_i)\\ {\dot{N}}_i={\dot{D}}_i-\frac{1}{2}{k}_2\mathrm{sgn}(e_i)\end{array}.$$

(23)

Let ${\beta }_i={[M_i{N}_i]}^T$, and differentiate it:

$$\begin{array}{l}{\dot{\beta }}_i=\frac{1}{2\left|M_i\right|}\left[\begin{array}{cc}-k_1& 1\\ -k_2& 0\end{array}\right]\left[\begin{array}{c}{M}_i\\ N_i\end{array}\right]+\\ \frac{1}{2\left|M_i\right|}\left[\begin{array}{cc}0& 0\\ 0& 2\left|M_i\right|\end{array}\right]\left[\begin{array}{c}0\\ {\dot{D}}_i\end{array}\right]\end{array}.$$

(24)

$D_i$ is bounded, $\left|{\dot{D}}_i\right|<\Delta$, let $0<\rho <2\Delta$. Hence, ${\dot{D}}_i=\frac{\rho }{2}\mathrm{sgn}(e_i)=\frac{\rho }{2}\frac{M_i}{\left|M_i\right|}$, we choose $A=\frac{1}{2\left|M_i\right|}\left[\begin{array}{cc}-k_1& 1\\ -k_2+\rho & 0\end{array}\right]$

$${\dot{\beta }}_i=A{\beta }_i.$$

(25)

Consider the Lyapunov function as follows

$$V_{i0}={\beta }_i^{T}P{\beta }_i,$$

(26)

where $P=\left[\begin{array}{cc}\lambda +{\tau }^2& -\tau \\ -\tau & 1\end{array}\right]$ is the symmetric matrix.

Differentiating $V_{i0}$ yields

$${\dot{V}}_{i0}={\beta }_i^T\left[A^{T}P+PA\right]{\beta }_i.$$

(27)

Choose $Q=\left[\begin{array}{cc}2k_1\lambda +2\tau \rho & -\rho -\lambda -{\tau }^2\\ -\rho -\lambda -{\tau }^2& 2\tau \end{array}\right]$, when $k_1>\frac{(\rho +\lambda +{\tau }^2)}{2\lambda \tau }+\frac{\tau }{2\lambda }-\frac{\tau \rho }{\lambda }$ and ${\lambda }_{\min }(Q)\ge \tau$, we get

$${\dot{V}}_{i0}={\beta }_i^T\left[A^{T}P+PA\right]{\beta }_i\le -\frac{1}{2\left|M_i\right|}{\beta }_i^{T}Q{\beta }_i\le -\frac{\tau }{2\left|M_i\right|}{\left|{\beta }_i\right|}^2.$$

(28)

Equation (26) meets the following conditions

$${\lambda }_{\min }(P){\Vert {\beta }_i\Vert }^2\le {\beta }_i^{T}P{\beta }_i\le {\lambda }_{\max }(P){\Vert {\beta }_i\Vert }^2.$$

(29)

Then

$$\{\begin{array}{l}\left|M_i\right|\le \Vert {\beta }_i\Vert \\ \frac{V_{i0}^{1/2}}{{\lambda }_{\max }^{1/2}}\le \Vert {\beta }_i\Vert \end{array}.$$

(30)

According to Equations (28) and (30), we get

$$\{\begin{array}{l}{\dot{V}}_{i0}\le -\mu V_{i0}^{1/2}\\ \mu =\frac{\tau }{2{\lambda }_{\max }^{1/2}(P)}\end{array},$$

(31)

where $\mu >0$, at this moment, Equation (31) meets the lemma 1. Hence, the error of system states and observation can converge in finite-time. The disturbance error converges in a small neighborhood, and defined as $\left|\tilde{D}\right|\le {\zeta }_M$. □

3.2. Position Control

According to Equation (14), we design the Lyapunov function:

$$V_p=\frac{1}{2}(x_e^2+y_e^2+z_e^2)$$

(32)

Then, differentiate $V_p$ along with Equation (14), we can get:

$${\dot{V}}_p=x_e(u-u_{r}c{\psi }_{e}c{\theta }_e)+y_e(-u_{r}s{\psi }_{e}c{\theta }_e+v)+z_e(u_{r}s{\theta }_e+w)$$

(33)

According to References [8], the virtual control signals are designed as:

$$\{\begin{array}{l}{\psi }_{ce}^o=\arcsin \frac{k_{v2}{z}_e}{\sqrt{1+{(k_{v2}{z}_e)}^2}}\\ {\theta }_{ce}^o=-\arcsin \frac{k_{v3}{y}_e}{\sqrt{1+{(k_{v3}{y}_e)}^2}}\\ u_c^o=-k_{v1}+u_{r}c{\psi }_{ce}^{o}c{\theta }_{ce}^o\end{array}$$

(34)

where $k_{v1}>0$, $k_{v2}>0$ and $k_{v3}>0$ are constant gains, and substitute Equation (34) into Equation (33), we get

$$\begin{array}{l}{\dot{V}}_p=-k_{v1}{x}_e^2-k_{v2}{u}_r\frac{1}{\sqrt{1+(k_{v2}{y}_e^2)}}\frac{1}{\sqrt{1+(k_{v3}{z}_e^2)}}{y}_e^2-\\ \hspace{1em}{k}_3{u}_r\frac{1}{\sqrt{1+(k_{v3}{z}_e^2)}}+y_{e}v+z_{e}w\end{array}$$

(35)

For avoiding the differential expansion problem, command filtered backstepping method is used to replace the differential process in the conventional backstepping method.

We define the tracking error of position:

$$\left[\begin{array}{c}{\tilde{x}}_e\\ {\tilde{y}}_e\\ {\tilde{z}}_e\end{array}\right]=\left[\begin{array}{c}{x}_e-x_{ce}\\ y_e-y_{ce}\\ z_e-z_{ce}\end{array}\right].$$

(36)

Then, differentiating the above equation alone with Equation (14) yields following error systems.

$$\begin{array}{l}\left[\begin{array}{c}{\dot{\tilde{x}}}_e\\ {\dot{\tilde{y}}}_e\\ {\dot{\tilde{z}}}_e\end{array}\right]=\left[\begin{array}{c}r{\tilde{y}}_e-q{\tilde{z}}_e\\ -r{\tilde{x}}_e\\ q{\tilde{x}}_e\end{array}\right]+\\ \left[\begin{array}{ccc}A& Bg(\tilde{\psi })u_r& Cg(\tilde{\theta })u_r\end{array}\right]\left[\begin{array}{c}\tilde{u}\\ \tilde{\psi }\\ \tilde{\theta }\end{array}\right]\end{array},$$

(37)

where $\tilde{u}=u-u_{ce}$, $\tilde{\psi }={\psi }_e-{\psi }_{ce}$, $\tilde{\theta }={\theta }_e-{\theta }_{ce}$ are the errors of filter, and

$$\begin{array}{l}A=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right],B=\left[\begin{array}{cc}\begin{array}{c}\mathrm{c}{\psi }_{ce}\mathrm{c}{\theta }_e\\ \mathrm{s}{\psi }_{ce}\mathrm{c}{\theta }_e\\ 0\end{array}& \begin{array}{c}-\mathrm{s}{\psi }_{ce}\mathrm{c}{\theta }_e\\ \mathrm{c}{\psi }_{ce}\mathrm{c}{\theta }_e\\ 0\end{array}\end{array}\right],C=\left[\begin{array}{cc}\begin{array}{c}\mathrm{c}{\theta }_{ce}\mathrm{c}{\psi }_{ce}\\ \mathrm{c}{\theta }_{ce}\mathrm{s}{\psi }_e\\ -\mathrm{s}{\theta }_{ce}\end{array}& \begin{array}{c}-\mathrm{s}{\theta }_{ce}\mathrm{c}{\psi }_{ce}\\ -\mathrm{s}{\theta }_{ce}\mathrm{s}{\psi }_e\\ -\mathrm{c}{\theta }_{ce}\end{array}\end{array}\right]\\ g(\tilde{\psi })={\left[\begin{array}{cc}\frac{\mathrm{c}\tilde{\psi }-1}{\tilde{\psi }}& \frac{\mathrm{s}\tilde{\psi }}{\tilde{\psi }}\end{array}\right]}^T,g(\tilde{\theta })={\left[\begin{array}{cc}\frac{\mathrm{c}\tilde{\theta }-1}{\tilde{\theta }}& \frac{\mathrm{s}\tilde{\theta }}{\tilde{\theta }}\end{array}\right]}^T\end{array},$$

where $\underset{\tilde{\psi }\to 0}{\lim }g(\tilde{\psi })={\left[\begin{array}{cc}0& 1\end{array}\right]}^T$, $\underset{\tilde{\psi }\to 0}{\lim }g(\tilde{\theta })={\left[\begin{array}{cc}0& 1\end{array}\right]}^T$.

Then, by differentiating Equation (36) and using ${\left[\begin{array}{ccc}{\dot{x}}_{ce}^o& {\dot{y}}_{ce}^o& {\dot{z}}_{ce}^o\end{array}\right]}^T$, the following equations are obtained.

$$\left[\begin{array}{c}{\dot{x}}_e\\ {\dot{y}}_e\\ {\dot{z}}_e\end{array}\right]=\left[\begin{array}{c}{\dot{\tilde{x}}}_e\\ {\dot{\tilde{y}}}_e\\ {\dot{\tilde{z}}}_e\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_{ec}-{\dot{x}}_{ce}^o\\ {\dot{y}}_{ec}-{\dot{y}}_{ce}^o\\ {\dot{z}}_{ec}-{\dot{z}}_{ce}^o\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_{ce}^o\\ {\dot{y}}_{ce}^o\\ {\dot{z}}_{ce}^o\end{array}\right].$$

(38)

The desired signal ${\left[\begin{array}{ccc}{\dot{x}}_{ce}^o& {\dot{y}}_{ce}^o& {\dot{z}}_{ce}^o\end{array}\right]}^T$ can be written as:

$$\left[\begin{array}{c}{\dot{x}}_{ce}^o\\ {\dot{y}}_{ce}^o\\ {\dot{z}}_{ce}^o\end{array}\right]=\left[\begin{array}{c}-k_x{\tilde{x}}_e+{\dot{x}}_{ce}\\ -k_y{\tilde{y}}_e+{\dot{y}}_{ce}\\ -k_z{\tilde{z}}_e+{\dot{z}}_{ce}\end{array}\right].$$

(39)

Substitute Equations (37) and (39) into Equation (38), and the errors of position are yielded as

$$\begin{array}{c}\left[\begin{array}{c}{\dot{\tilde{x}}}_e\\ {\dot{\tilde{y}}}_e\\ {\dot{\tilde{z}}}_e\end{array}\right]=\left[\begin{array}{c}r{\tilde{y}}_e-q{\tilde{z}}_e\\ -r{\tilde{x}}_e\\ q{\tilde{x}}_e\end{array}\right]+\left[\begin{array}{c}-k_x{\tilde{x}}_e\\ -k_y{\tilde{y}}_e\\ -k_z{\tilde{z}}_e\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_{ce}-{\dot{x}}_{ce}^o\\ {\dot{y}}_{ce}-{\dot{y}}_{ce}^o\\ {\dot{z}}_{ce}-{\dot{z}}_{ce}^o\end{array}\right]+\\ \left[\begin{array}{ccc}A& Bg(\tilde{\psi })u_r& Cg(\tilde{\theta })u_r\end{array}\right]\left[\begin{array}{c}\tilde{u}\\ \tilde{\psi }\\ \tilde{\theta }\end{array}\right]\end{array}.$$

(40)

3.3. Attitude Control

Differentiate $\tilde{\psi }={\psi }_e-{\psi }_{ce}$ and $\tilde{\theta }={\theta }_e-{\theta }_{ce}$, we get

$$\dot{\tilde{\psi }}=(r_c^o+(r_c-r_c^o)+\tilde{r})/\mathrm{c}\theta +\dot{\beta }-r_F-{\dot{\psi }}_{ce},$$

(41)

$$\dot{\tilde{\theta }}=q_c^o+(q_c-q_c^o)+\tilde{q}-q_F-{\dot{\theta }}_{ce},$$

(42)

where the errors of angular velocities are defined as $\tilde{r}=r-r_c$, $\tilde{q}=q-q_c$.

Based on Equations (41) and (42), $q_c^o$ and $r_c^o$ represent the desired virtual control signals of angular velocities $q$ and $r$, and can be proposed as

$$\begin{array}{l}{r}_c^o=\mathrm{c}\theta (r_F+{\dot{\psi }}_c+\dot{\beta }-k_{\psi }\tilde{\psi })-{\psi }_{bs}\\ q_c^o=q_F+{\dot{\theta }}_c-k_{\theta }\tilde{\theta }-{\theta }_{bs}\end{array},$$

(43)

where $k_{\psi }>0$, $k_{\theta }>0$ are the controller parameters, ${\psi }_{bs}$ and ${\theta }_{bs}$ represent the robust terms. The robust terms are defined in Section 4.

Substitute Equation (43) into Equations (41) and (42), Equations (41) and (42) can be rewritten as

$$\begin{array}{l}\dot{\tilde{\psi }}=-k_{\psi }\tilde{\psi }+\frac{(r_c-r_c^o)+\tilde{r}}{\mathrm{c}\theta }-{\psi }_{bs}\\ \dot{\tilde{\theta }}=-k_{\theta }\tilde{\theta }+(q_c-q_c^o)+\tilde{q}-{\theta }_{bs}\end{array}.$$

(44)

3.4. Velocity and Angular Velocity Control

For improving the robustness of the controller, we design the integral term ${\epsilon }_1$, ${\epsilon }_2$ and ${\epsilon }_3$, where ${\dot{\epsilon }}_1=\tilde{u}$, ${\dot{\epsilon }}_2=\tilde{q}$, ${\dot{\epsilon }}_3=\tilde{r}$. The finite-time control input of AUV 3D path following can be proposed as

$$\{\begin{array}{l}{F}_u=(m-X_{\dot{u}})(-k_u\tilde{u}-k_{fu}{\mathrm{sig}}^{\gamma }(\tilde{u})+{\dot{u}}_c-u_{bs})-f_u\\ M=(I_y-M_{\dot{q}})(-k_q\tilde{q}-k_{fq}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{q}}_c-q_{bs})-f_q\\ N=(I_z-N_{\dot{r}})(-k_r\tilde{r}-k_{fr}{\mathrm{sig}}^{\gamma }(\tilde{r})+{\dot{r}}_c-r_{bs})-f_r\end{array},$$

(45)

where $f_u=X_{uu}{u}_r{}^2+X_{vv}{v}_r{}^2+X_{ww}{w}_r{}^2+X_{qq}{q}^2$, $f_q=-(z_{g}w-z_{b}B)\sin \theta +M_{q\left|q\right|}q\left|q\right|-M_{uw}{u}_r{w}_r-M_{uq}{u}_{r}q-m{z}_g(w_{r}q-v_{r}r)$ and $f_r=N_{uv}{u}_r{v}_r+N_{v\left|v\right|}{v}_r\left|v_r\right|+N_{ur}{u}_{r}r$ are nonlinear dynamic terms. ${\mathrm{sig}}^{\gamma }(x)$ is given as

$${\mathrm{sig}}^{\gamma }(x)=\mathrm{sgn}(x){\left|x\right|}^{\gamma }.$$

(46)

where $x\in ℝ$, $\alpha \in (0,1)$, and ${\mathrm{sig}}^{\alpha }(·)$ is continuous and increasing, and ${\mathrm{sig}}^{\alpha }(0)=0$.

Considering the observer into the Equation (45), the controller of the underactuated AUV proposed in this paper can be obtained:

$$\{\begin{array}{l}{F}_u=(m-X_{\dot{u}})(-k_u\tilde{u}-k_{fu}{\mathrm{sig}}^{\gamma }(\tilde{u})+{\dot{u}}_c-u_{bs}-{\widehat{F}}_u)-f_u\\ M=(I_y-M_{\dot{q}})(-k_q\tilde{q}-k_{fq}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{q}}_c-q_{bs}-\widehat{N})-f_q\\ N=(I_z-N_{\dot{r}})(-k_r\tilde{r}-k_{fr}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{r}}_c-r_{bs}-\widehat{M})-f_r\end{array},$$

(47)

When the command filter controller is not considered, Equation (47) is changed as follows

$$\{\begin{array}{l}{F}_u=(m-X_{\dot{u}})(-k_u\tilde{u}-k_{fu}{\mathrm{sig}}^{\gamma }(\tilde{u})+{\dot{u}}_c-{\widehat{F}}_u)-f_u\\ M=(I_y-M_{\dot{q}})(-k_q\tilde{q}-k_{fq}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{q}}_c-\widehat{N})-f_q\\ N=(I_z-N_{\dot{r}})(-k_r\tilde{r}-k_{fr}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{r}}_c-\widehat{M})-f_r\end{array},$$

(48)

Then we substitute Equation (45) into Equation (7) and get the following error dynamics

$$\{\begin{array}{l}\dot{\tilde{u}}=-k_u\tilde{u}-k_{fu}{\mathrm{sig}}^{\gamma }(\tilde{u})-u_{bs}\\ \dot{\tilde{q}}=-k_q\tilde{q}-k_{fq}{\mathrm{sig}}^{\gamma }(\tilde{q})-q_{bs}\\ \dot{\tilde{r}}=-k_r\tilde{r}-k_{fr}{\mathrm{sig}}^{\gamma }(\tilde{r})-r_{bs}\end{array}.$$

(49)

Let ${\sigma }_u=\tilde{u}$, ${\sigma }_r=\tilde{r}$ and ${\sigma }_q=\tilde{q}$, we get the filter error compensation dynamics

$$\{\begin{array}{l}{\dot{\sigma }}_u=-k_u{\sigma }_u-k_{fu}{\mathrm{sig}}^{\gamma }({\sigma }_u)-u_{bs}\\ {\dot{\sigma }}_q=-k_q{\sigma }_q-k_{fq}{\mathrm{sig}}^{\gamma }({\sigma }_q)-q_{bs}\\ {\dot{\sigma }}_r=-k_r{\sigma }_r-k_{fr}{\mathrm{sig}}^{\gamma }({\sigma }_r)-r_{bs}\end{array}.$$

(50)

The above Equation (50) can be finally rewritten as

$$\dot{E}=GE+HT+Jsi{g}^{\gamma }(E),$$

(51)

where $G=-K_P=diag\{-k_u,-k_q,-k_r\}$, $k_u$, $k_q$ and $k_r$ are positive parameters. $H=I_{3\times 3}$, $J=-K_F=diag\{-k_{fu},-k_{fq},-k_{fr}\}$, $k_{fu}$, $k_{fq}$ and $k_{fr}$ are positive parameters. $T={\left[-u_{bs},-q_{bs},-r_{bs}\right]}^T$, $E={[{\sigma }_u,{\sigma }_q,{\sigma }_r]}^T$.

3.5. Design of Compensation Loop

For guaranteeing that the error between the desired signals and the filter signals can be converged, a compensation of filter error is proposed in the part. The errors of compensation are defined as following

$$\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_e-{\zeta }_x\\ {\tilde{y}}_e-{\zeta }_y\\ {\tilde{z}}_e-{\zeta }_z\end{array}\right],$$

(52)

where ${\zeta }_x$, ${\zeta }_y$, ${\zeta }_z$ denote the filter compensation, according Equation (40), they can be defined

$$\begin{array}{c}\left[\begin{array}{c}{\dot{\zeta }}_x\\ {\dot{\zeta }}_y\\ {\dot{\zeta }}_z\end{array}\right]=\left[\begin{array}{c}r{\zeta }_y-q{\zeta }_z\\ -r{\zeta }_x\\ q{\zeta }_x\end{array}\right]+\left[\begin{array}{c}-k_x{\tilde{x}}_e\\ -k_y{\zeta }_y\\ -k_z{\zeta }_z\end{array}\right]+\left[\begin{array}{c}{\dot{x}}_{ce}^o+{\dot{x}}_{ce}\\ {\dot{y}}_{ce}^o+{\dot{y}}_{ce}\\ {\dot{z}}_{ce}^o+{\dot{z}}_{ce}\end{array}\right]+\\ \left[\begin{array}{ccc}A& Bg(\tilde{\psi })u_r& Cg(\tilde{\theta })u_r\end{array}\right]\left[\begin{array}{c}{\zeta }_u\\ {\zeta }_{\psi }\\ {\zeta }_{\theta }\end{array}\right]\end{array},$$

(53)

where ${\zeta }_{\psi }$ and ${\zeta }_{\theta }$ are designed in section of stability analysis, ${\zeta }_x(0)={\zeta }_y(0)={\zeta }_z(0)=0$.

We choose the following Lyapunov function

$$E_1=\frac{1}{2}({\sigma }_x^2+{\sigma }_y^2+{\sigma }_z^2).$$

(54)

By differentiating Equation (54) along with Equations (53) and (40), we get

$$\begin{array}{l}{\dot{E}}_1=-k_x{\sigma }_x^2-k_y{\sigma }_y^2-k_z{\sigma }_z^2\\ +\left[\begin{array}{ccc}{\sigma }_x& {\sigma }_y& {\sigma }_z\end{array}\right]\times \left[\begin{array}{ccc}A& Bg(\tilde{\psi })u_r& Cg(\tilde{\theta })u_r\end{array}\right]\left[\begin{array}{c}{\zeta }_u\\ {\zeta }_{\psi }\\ {\zeta }_{\theta }\end{array}\right]\\ =-k_x{\sigma }_x^2-k_y{\sigma }_y^2-k_z{\sigma }_z^2+A^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_u\\ +g^T(\tilde{\psi })B^T{u}_r\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_{\psi }+g^T(\tilde{\theta })C^T{u}_r\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_{\theta }\end{array}.$$

(55)

The filter compensation errors of the pitch and yaw angle can be defined as

$$\begin{array}{l}{\sigma }_{\psi }=\tilde{\psi }-{\zeta }_{\psi }\\ {\sigma }_{\theta }=\tilde{\theta }-{\zeta }_{\theta }\end{array}.$$

(56)

According to Equation (44), ${\zeta }_{\psi }$ and ${\zeta }_{\theta }$ can be defined as

$$\begin{array}{l}{\dot{\zeta }}_{\psi }=-k_{\psi }{\zeta }_{\psi }+\frac{(r_{ce}-r_{ce}^o)+{\zeta }_r}{\cos \theta }\\ {\dot{\zeta }}_{\theta }=-k_{\theta }{\zeta }_{\theta }+(q_{ce}-q_{ce}^o)+{\zeta }_q\end{array},$$

(57)

where ${\zeta }_{\psi }(0)={\zeta }_{\theta }(0)={\zeta }_r(0)={\zeta }_q(0)=0$.

Choose the following Lyapunov function as

$$E_2=\frac{1}{2}({\sigma }_{\psi }^2+{\sigma }_{\theta }^2).$$

(58)

Differentiating Equation (58) alone with Equations (44) and (56), we get

$$\begin{array}{l}{\dot{E}}_2={\sigma }_{\psi }{\dot{\sigma }}_{\psi }+{\sigma }_{\theta }{\dot{\sigma }}_{\theta }=(\dot{\tilde{\psi }}-{\dot{\zeta }}_{\psi }){\sigma }_{\psi }+(\dot{\tilde{\theta }}-{\dot{\zeta }}_{\theta }){\sigma }_{\theta }\\ =(\frac{(r_{ce}-r_{ce}^o)+\tilde{r}}{\cos \theta }-k_{\psi }\tilde{\psi }-{\psi }_{bs}+k_{\psi }{\zeta }_{\psi }-\frac{(r_{ce}-r_{ce}^o)+{\zeta }_r}{\cos \theta }){\sigma }_{\psi }\\ +(-k_{\theta }\tilde{\theta }+(q_{ce}-q_{ce}^o)+\tilde{q}-{\theta }_{bs}+k_{\theta }{\zeta }_{\theta }-(q_{ce}-q_{ce}^o)-{\zeta }_q){\sigma }_{\theta }\\ =-k_{\psi }{\sigma }_{\psi }^2-k_{\theta }{\sigma }_{\theta }^2+{\sigma }_q{\sigma }_{\theta }-{\theta }_{bs}{\sigma }_{\theta }-{\psi }_{bs}{\sigma }_{\psi }+\frac{{\sigma }_r}{\cos \theta }{\sigma }_{\psi }\end{array},$$

(59)

where ${\sigma }_q=\tilde{q}$ and ${\sigma }_r=\tilde{r}$.

3.6. Design of Anti-Windup

The signal after the anti-windup scheme is present as

$$u_o=\{\begin{array}{l}{u}_m,u\ge u_m\\ u,-u_m<u<u_m\\ -u_m,u\le -u_m\end{array},$$

(60)

where $u_m$ is the upper limited of control output. We design the anti-windup part as

$$u=u_o-K_o{\displaystyle \int (u-u_o)dt},$$

(61)

where $K_o$ denote the anti-windup gain coefficient.

In summary, the main equations of the proposed path following controller are described as:

$$\{\begin{array}{l}{F}_u=(m-X_{\dot{u}})(-k_u\tilde{u}-k_{fu}{\mathrm{sig}}^{\gamma }(\tilde{u})+{\dot{u}}_c-u_{bs}-{\widehat{F}}_u)-f_u\\ M=(I_y-M_{\dot{q}})(-k_q\tilde{q}-k_{fq}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{q}}_c-q_{bs}-\widehat{N})-f_q\\ N=(I_z-N_{\dot{r}})(-k_r\tilde{r}-k_{fr}{\mathrm{sig}}^{\gamma }(\tilde{q})+{\dot{r}}_c-r_{bs}-\widehat{M})-f_r\end{array}$$

(62)

Figure 4 shows the structure of the path following control system.

4. Stability Analysis

The stability of path following control system is proposed in this section. Firstly, we give the provident of the proposed controller without command filter. Substitute Equation (48) into Equation (7) and yield the following error equations

$$\{\begin{array}{l}{\dot{\sigma }}_u=-k_u{\sigma }_u-k_{fu}{\mathrm{sig}}^{\gamma }({\sigma }_u)-{\widehat{F}}_u\\ {\dot{\sigma }}_q=-k_q{\sigma }_q-k_{fq}{\mathrm{sig}}^{\gamma }({\sigma }_q)-\widehat{N}\\ {\dot{\sigma }}_r=-k_r{\sigma }_r-k_{fr}{\mathrm{sig}}^{\gamma }({\sigma }_r)-\widehat{M}\end{array}.$$

(63)

Then Equation (64) can be present by

$$\dot{\sigma }=-K_p\sigma -K_{f}si{g}^{\gamma }(\sigma )-\tilde{D},$$

(64)

where $K_p=diag(k_u,k_q,k_r)$, $K_f=diag(k_{fu},k_{fq},k_{fr})$ and $\tilde{D}={\left[{\widehat{F}}_u,\widehat{N},\widehat{M}\right]}^T$.

A Lyapunov function candidate:

$$V_1=\frac{1}{2}{\sigma }^T\sigma ,$$

(65)

where $\sigma ={\left[{\sigma }_u,{\sigma }_q,{\sigma }_r\right]}^T$ denote the attitude error, and differentiating Equation (66), we get

$$\begin{array}{l}{\dot{V}}_1={\sigma }^T\dot{\sigma }\\ ={\sigma }^T(-K_p\sigma -K_{f}si{g}^{\gamma }(\sigma )-\tilde{D})\\ =-{\sigma }^T{K}_p\sigma -{\sigma }^T{K}_{f}si{g}^{\gamma }(\sigma )-{\sigma }^T\tilde{D}\end{array},$$

(66)

where $\left|\tilde{D}\right|\le {\zeta }_M$, according to Young’s inequality, it can be changed

$${\sigma }^T\tilde{D}\le \left|{\sigma }^T\right|{\zeta }_M\le {\sigma }^T\delta \sigma +\frac{1}{4}{\zeta }_M^T{\delta }^{-1}{\zeta }_M,$$

(67)

where $\delta =diag({\delta }_1,{\delta }_2,{\delta }_3)$, ${\delta }_i$ are arbitrary positive constant.

Substitute Equation (68) into Equation (67), and we get

$${\dot{V}}_1\le -{\sigma }^T(K_p+\delta )\sigma -{\sigma }^T{K}_{f}si{g}^{\gamma }(\sigma )-\frac{1}{4}{\zeta }_M^T{\delta }^{-1}{\zeta }_M.$$

(68)

Furthermore, Equation (69) can be changed

$$\begin{array}{l}{\dot{V}}_1\le -{\sigma }^T(K_p+\delta )\sigma -{\sigma }^T{K}_{f}si{g}^{\gamma }(\sigma )\\ \le -(K_p+\delta )V_1-2^{(\gamma +1)/2}{K}_f{V}_1^{(\gamma +1)/2}\end{array}.$$

(69)

According to the Lemma 2, the finite-time controller without command filter is finite-time stable. Additionally, convergence time can be expressed as follows: $t_0\le \frac{1}{(K_p+\delta )\left(1-\gamma \right)/2}\ln \frac{(K_p+\delta )V_1^{(1-\gamma )/2}(t_0)+2^{(\gamma +1)/2}{K}_f}{2^{(\gamma +1)/2}{K}_f}$.

Secondly, the stability of the overall control system is proposed. According to Equations (51), (54) and (58), we consider the following Lyapunov function

$$V=E_1+E_2+\frac{1}{2}{E}^{T}KE,$$

(70)

where $K=diag\left\{k_1,k_2,k_3\right\}$ is a positive coefficient matrix. Let $G^{T}K+KG=-2Q$, and differentiating Equation (71), we get

$$\begin{array}{l}\dot{V}={\dot{E}}_1+{\dot{E}}_2+E^{T}K\dot{E}\\ =-k_x{\sigma }_x^2-k_y{\sigma }_y^2-k_z{\sigma }_z^2-k_{\psi }{\sigma }_{\psi }^2-k_{\theta }{\sigma }_{\theta }^2-\frac{1}{2}{E}^{T}QE+A^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_u\\ +g^T(\tilde{\psi })B^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_{\psi }+g^T(\tilde{\theta })C^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]{\sigma }_{\theta }+\frac{{\sigma }_r}{\cos \theta }{\sigma }_{\psi }+{\sigma }_q{\sigma }_{\theta }-{\theta }_{bs}{\sigma }_{\theta }-{\psi }_{bs}{\sigma }_{\psi }\\ -p_{21}{\sigma }_u{u}_{bs}-p_{22}{\sigma }_q{q}_{bs}-p_{23}{\sigma }_r{r}_{bs}+E^{T}KJsi{g}^{\gamma }(E)\end{array}.$$

(71)

Then, to meet $\dot{V}<0$, the robust terms ${\psi }_{bs}$, ${\theta }_{bs}$, $u_{bs}$, $q_{bs}$ and $r_{bs}$ can be design as

$$\begin{array}{l}{\psi }_{bs}=g^T(\tilde{\psi })B^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right],{\theta }_{bs}=g^T(\tilde{\theta })C^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right],u_{bs}=\frac{1}{p_{21}}{A}^T\left[\begin{array}{c}{\sigma }_x\\ {\sigma }_y\\ {\sigma }_z\end{array}\right]\\ q_{bs}=\frac{{\sigma }_{\theta }}{p_{22}},r_{bs}=\frac{{\sigma }_{\psi }}{p_{23}\cos \theta }\end{array}.$$

(72)

Substitute Equation (73) into Equation (72), and we get

$$\begin{array}{l}\dot{V}={\dot{E}}_1+{\dot{E}}_2+E^{T}K\dot{E}\\ =-k_x{\sigma }_x^2-k_y{\sigma }_y^2-k_z{\sigma }_z^2-k_{\psi }{\sigma }_{\psi }^2-k_{\theta }{\sigma }_{\theta }^2-\frac{1}{2}{E}^{T}QE+E^{T}KJsi{g}^{\gamma }(E)\end{array},$$

(73)

where $J$ is a negative matrix. Hence, $\dot{V}<0$, it indicates that path following errors are convergent. The entire control close-loop system is global stable. The proof is completed.

5. Numerical Simulations

In this section, to verify and analyze the proposed controller (FTCFPC) base on the super-twisting observer, finite-time technology and command filtered backstepping methodology of this paper, numerical simulations on an underactuate AUV WL-4 (Figure 5)., which is developed by Harbin Engineering University in China, were conduct in MATLAB-Simulink.

In order to simulate a more realistic environment, the environment disturbances are considered in simulations. The disturbances can be expressed as

$$d+T\dot{d}=K\epsilon ,$$

(74)

where $d={[{\tau }_{ud},{\tau }_{vd},{\tau }_{wd},{\tau }_{qd},{\tau }_{rd}]}^T$ is the external disturbances vector. $\epsilon$ is a vector of white Gaussian noise with the largest amplitude of $\epsilon =1$, and represents the high measurement noise. $K={[2,1,1,2,2]}^T$ represents the gain parameter matrix. $T={[20,20,20,20,20]}^T$ denotes the time constant matrix.

For the underactuated AUV spiral dive operation, the following 3D desired path is design.

$$\{\begin{array}{l}{x}_d=20\cos (\frac{\pi }{10}s)\\ y_d=20\sin (\frac{\pi }{10}s)\\ z_d=s\end{array}.$$

(75)

The initial position of the AUV is $(x_0,y_0,z_0)=(30,0,1)\mathrm{m}$, the initial attitude angle are $({\theta }_0,{\psi }_0)=(0,0)$. The initial velocities of AUV are $v_0=0\mathrm{m}/\mathrm{s}$, $u_0=0\mathrm{m}/\mathrm{s}$, and $w_0=0\mathrm{m}/\mathrm{s}$. The desired velocity is $u_d=1\mathrm{m}/\mathrm{s}$. The constant velocity of ocean currents is $v=(0.2,0.2,0.05)\mathrm{m}/\mathrm{s}$. The path following control simulations results using FTCFPC control are compared with the results of traditional backstepping.

In the numerical simulation, we design AUV controllers according to (47) and the parameters are given by $k_x=5$, $k_y=1$, $k_z=2$, $k_{\psi }=k_{\theta }=2$, $k_u=8$, $k_q=k_r=3$, $k_{fu}=k_{fq}=k_{fr}=1$, $p_{21}=5$, $p_{22}=p_{23}=5$. The parameters of the filter are selected as $w_n=20rad/s$, $\zeta =0.9$.

Finally, the simulation results by using finite-time command filtered backstepping control and the traditional backstepping control are shown in Figure 6, Figure 7, Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12.

The real and desired paths in the spatial space are shown in Figure 6, Figure 7 and Figure 8 respectively show the projection curves of horizontal plane and vertical plane. It can be seen from these figures that the actual path of the AUV deviates from the desired path due to the influence of the ocean current. As can be seen from Figure 6, Figure 7 and Figure 8. The path under FTCFPC control is closer to the desired path than that under traditional backstepping control. The FTCFPC controllers has the smaller offset and achieve significantly higher accuracy than traditional backstepping controllers in the presence of unknown disturbance and ocean currents. Figure 9 shows the tracking errors of 3D path. It can be seen that the error of real and desired path with the FTCFPC controller can converge in a short time. The position error of two controllers has some fluctuations, but the fluctuation of the FTCFPC method is much smaller. These can be proved that the FTCFPC method present in this paper has good robustness and effectiveness. Figure 10 and Figure 11 depict the response of velocity and angle respectively, from which we can see that, FTCFPC method can achieve the desired path more quickly and has good anti-interference ability. The control signals of different channels are shown Figure 12.

In summary, the FTCFPC method proposed in this paper can make the AUV tracking to the desired path smoothly and robustly with the ocean currents. Following are the advantages which can been seen from the above results:

• The controller has strong robustness under the interference of ocean current.
• It has faster convergence, and the AUV can follow the desired path in a short time.
• The control has better following accuracy under current disturbance.

6. Conclusions

This paper proposes a FTCFPC controller to improve an underactuated AUV path following performance in the presence of ocean currents and unknown disturbances. The designed controller is based on command filtered backstepping method, finite-time theory and super-twisting observer techniques. A second-order filter is designed to achieve derivative of virtual control signals such that the computational complexity can be reduced and the differential expansion can be avoided. A filtered error compensation loop is developed to ensure the error between the desired signals and the filter signals converge. A super-twisting observer is proposed to reduce the impact of unknown disturbances and ocean currents. A finite-time controller is introduced to ensure the system can be stable in a short time. The system stability is analyzed based on the Lyapunov stability theory. Simulation results indicates that the 3D path following controller proposed in this paper for underactuated AUVs is more effective and robust than the conventional backstepping method in the presence of ocean currents and unknown disturbances.

For the future works, more real external disturbance will be considered such as impermanent currents and waves near the surface. The cooperative path following of AUVs and the limited performance of actuators should be developed.