Dual closed loop AUV trajectory tracking control based on finite time and state observer

1.   Introduction

More and more countries have begun to pay attention to the exploration and development of marine resources, as well as to carry out work such as submarine rescue, marine topographic mapping, optical cable and oil and gas pipeline maintenance; Therefore, we pay more attention to low-cost underwater vehicles. The accurate tracking and control ability of underwater autonomous navigation robot is an important technical prerequisite for the successful completion of the operation. AUV is easily disturbed by ocean current and its dynamic parameters when working underwater. When AUV performs three-dimensional motion underwater, it will face the coupling problem of 6-DOF control output and actual control; the reason is that due to the influence of unknown environmental interference, controller output saturation, controller overshoot and other factors, the design of AUV trajectory tracking controller becomes a difficult point. In reference [1], the author proposed an improved integral action LOS steering control algorithm to counteract the lateral drift effect of external environmental disturbances such as ocean currents, wind and waves. The disadvantage is that it can only be used for AUV horizontal plane line tracking control. In reference [2], based on the backstepping method and the Lyapunov stability theory, the author proposed a path tracking control strategy under the disturbance of invariant ocean currents. But this method has high requirements on the performance of the AUV. In reference [3], also based on the backstepping method and the Lyapunov stability theory, the author proposes a three-dimensional path tracking control rate. However, the uncertainty of the AUV model and external interference are not considered, and the actual effect is not large. In reference [4], the author proposes a nonlinear robust control based on the command filter backstepping method. This method is used to simplify the calculation steps of the backstepping method, but also does not consider the uncertainty of the AUV. In reference [5], the author proposes a three-dimensional space path tracking control based on the improved backstepping method, and considers the uncertainty of the AUV model, but the calculation is very complicated. In reference [6], based on the dynamic sliding mode control theory and backstepping method, the author proposes a nonlinear control rate to realize the path tracking of underactuated ships. But this method is only suitable for straight or piecewise straight lines with zero curvature. In reference [7], the author considered the uncertainty of the AUV model and proposed a second-order sliding mode control theory. But its disadvantage is that the disturbance is required to be of known size and bounded, which is difficult to achieve in actual control. In reference [8], the author designed a robust controller based on disturbance compensation, which solved the USV path tracking problem with unknown environmental disturbances and improved the tracking performance. But AUV model uncertainty is not considered. In reference [9], the author proposes a nonlinear robust controller based on the backstepping method to solve the path tracking problem of underactuated AUVs in a 3D environment. However, the model parameters of the AUV it considers are constant, which is not the case in actual control. In reference [10], the authors propose a disturbance observer to estimate unknown time-varying environmental disturbances, and also design a distributed dynamic controller to receive the position and velocity of each device. But the disadvantage is that the device can only run in a two-dimensional plane, and it is only verified in simulation, there is no actual device experiment, and there is a lack of credibility. In reference [11], the author designed a command filtering fuzzy control, which eliminated the filtering error by introducing a compensator for each control signal, but this method could not eliminate the error in a short time. In reference [12], the author proposed a nonlinear robust control strategy based on the command filter inversion technique, which also has the disadvantage of long convergence time. In reference [13], the author considers the angle constraint and line-of-sight range of autonomous surface ship formation, and proposes a fault-tolerant finite-time control, but due to the influence of the initial observation error, its convergence time is long. The fuzzy control method ^[14,15] does not rely on the precise system model, it is suitable for the uncertain system, has strong robustness to process and parameter changes, and has strong anti-interference ability. But its drawback is that it can only be used in specific scenarios. The neural network ^[16,17] method does not require the establishment of an accurate underwater robot model, and has a nonlinear, self-learning function. But the disadvantage is that it is difficult to obtain training samples, and online learning is required for a long time, and the real-time performance is poor. In reference [18,19], the authors applied the improved sliding mode control to the path tracking control of AUV to resist bounded disturbance. However, in practical applications, the use of sliding mode control may generate chattering to excite unmodeled high-frequency dynamics, resulting in degraded controller performance. The above sliding mode control method can only guarantee that the tracking error is linearly converged, and the convergence speed is slow. If you want to increase the convergence speed, you can only increase the control input. However, increasing the control input may lead to saturation of the thruster, which is difficult to achieve in practical applications. In reference [20], the author designed a non-singular terminal sliding mode controller to shorten the convergence speed. But the disadvantage is that it is easily disturbed by the disturbance of the uncertain upper limit of the outside world. In reference [21], the author proposed an adaptive non-singular terminal sliding mode controller. Although the controller can effectively deal with the time-varying external disturbance, its defect is that it can only guarantee the bounded tracking error, which sacrifices the tracking accuracy. In reference [22], the author proposed a fast terminal sliding mode control scheme based on a finite time-expanded state observer, which accelerated the convergence rate of the system. But the disadvantage is that the convergence time of the system depends too much on the selection of the initial state of the system.

To sum up, in recent years, more advanced algorithms such as neural network, adaptive sliding mode control and other algorithms have been used in AUV trajectory tracking, but these algorithms have slow response speed, high requirements on hardware equipment, slow convergence speed, and learning long time etc. In order to improve the convergence speed of the double closed-loop system, a finite-time control method is introduced in the position controller. In the attitude controller, the controller is designed based on the reduced-order extended state observer and integral sliding mode control, which solves the problems of unknown environmental disturbance and model uncertainty. An auxiliary dynamic compensation system is introduced to solve the problem of controller input saturation. At the same time, based on the "virtual guidance", the AUV three-dimensional trajectory error model is established in the Serret-Frenet coordinate system.

2.   AUV mathematical model and trajectory tracking error model establishment

In order to study the trajectory tracking control method of "sea exploration Ⅱ" AUV, the dynamic model and tracking error model of AUV need to be constructed, which is the basis of control method research and simulation. The mathematical basis of the AUV model described in this paper is mainly from the "sea exploration Ⅱ" AUV. In order to describe the hydrodynamic model of the "sea exploration Ⅱ" AUV more vividly, first, we need to establish two coordinate systems: the geodetic coordinate system ($E-\xi \; \eta \; \zeta$ static coordinate system) and the body coordinate system ($O-xyz$ dynamic coordinate system). As shown in Figure 1. Each coordinate system is determined according to the right-hand system, and the symbol system adopts the symbols recommended by the International Towing Tank Conference (ITTC). $\theta, \psi, \varphi \;$ are the attitude angles, $u, v, w, q, p, r$ are the linear velocities and angular velocities of the robot in the body coordinate system.

2.1.   AUV kinematics model

The attitude angle is determined by the relationship between the body coordinate system and the geodetic coordinate system. It is represented by three euler angles of heading, pitch and heel: the heading angle $\psi$ is the X-axis in the body coordinate system on the horizontal plane. The angle between the projection on the $\xi$ axis and the $\eta$ axis in the geodetic coordinate system. The vertical tilt angle $\theta$ is the angle between the Y-axis in the body coordinate system and the $\eta$ axis in the horizontal plane geodetic coordinate system. The heel angle $\varphi$ is the body coordinate system.The angle between the middle Z-axis plane and the $\zeta$-axis in the geodetic coordinate system.

Assuming that the origin of the geodetic coordinate system coincides with the origin of the body coordinate system, the transfrmation martix S can be expressed as:

First, the coordinates are converted to:

According to the above principles, it can be concluded that the derivative of displacement $(x, y, z)^T$ in the carrier coordinate system has the following relationship with the AUV linear velocity $(u, v, w)^T$ :

The relationship between the derivative $(\varphi, \theta, \psi)^T$ and angular velocity $(p, q, r)^T$ of the AUV's attitude angle in the carrier coordinate system is as follows:

The transformation matrix T is as follows:

Expanding the above Eqs (2.3) and (2.4), the six-degree-of-freedom kinematics equation of AUV can be expressed as:

2.2.   AUV kinetic model

The underwater robot's underwater motion state can be understood as the motion of a rigid body in a fluid, which is mainly subject to rigid body and rigid body moments, hydrodynamic and hydrodynamic moments, and therefore subject to the basic Newton's laws, momentum conservation and energy conservation laws. Based on this, we can express the space dynamics equation of the AUV in the carrier coordinate system as:

where, $M = M_{RB}+M_A$, A is the generalized mass matrix, $C(v) = C_{RB}(v)+C_A(v)$ is the sum of Coriolis force matrix and Coriolis force, $D\left(v \right)$ is the nonlinear fluid hydrodynamic damping matrix, $g\left(\eta \right)$ is the restoring force vector generated by buoyancy and gravity, $\tau _{env}$ is environmental interference and $\tau$ is the thrust and torque output of the propeller.

The rigid body inertia matrix $M_{RB}$ is a symmetric positive definite matrix. The robot is symmetric on the x = 0 and y = 0 planes, so the main components are distributed on the diagonal. Matrix $M_{RB}$ can be expressed as:

Among them, $m$ is the quality of the AUV, $x_G, y_G, z_G$ are the coordinates of the center of gravity of the AUV, and $I_x, \; I_y, I_z$ are the moments of inertia of the AUV on the three coordinate axes.

When the underwater robot is in motion, it will be affected by factors such as hydrodynamic force and torque. Assuming that the hydrodynamic force and hydrodynamic torque received by the underwater robot can be linearly superimposed, the radiation force $\tau _h$ exerted by the hydrodynamic force can be expressed as:

Among them: $M_A$ additional mass matrix, $C_A$ Coriolis force matrix, $D\left(v \right)$ damping matrix, $g\left(\eta \right)$ restoring force matrix. The above matrix can be expressed as:

Among them, $\; \frac{\partial Y}{\partial \dot{u}} = Y_u, \frac{\partial Y}{\partial \dot{v}} = Y_{\dot{v}}$, the other symbols are the same.

The expression form of the hydrodynamic Coriolis and centripetal force matrix $C_A$ can be expressed as:

The damping parameter matrix $D\left(v \right)$ can be expressed as:

The magnitude and direction of the AUV's gravity and buoyancy do not vary with depth. Define gravity: $W = mg$ and buoyancy: $B = \rho gV$. Where $g$ is the acceleration due to gravity, $\rho$ is the density of seawater, and $V$ is the volume of the AUV. Generally taking the center of buoyancy as the origin, the restoring force can be expressed as:

In the carrier coordinate system, the center of gravity and the center of buoyancy are defined as:

When the center of buoyancy and center of gravity of the robot satisfy $W = B$, $X_G = x_b, y_G = y_b, Z_G = z_b$, it can be simplified to:

In order to study the motion law of AUV better, on the basis of the space dynamics in Chapter 2, the influence of rolling motion of AUV and nonlinear hydrodynamic parameters higher than the second order is ignored, and the 5-DOF dynamic model of AUV is established as shown in formula (2.10).

where, $m_{ii}\left(1, 2, 3, 5, 6 \right)$ is denoted as the inertial hydrodynamic force of AUV, which is the force generated by the inertia of the surrounding water flow in the acceleration process, it can be expressed as:

$\; d_{11} = X_u+X_{u|u|}\left| u \right|; d_{22} = Y_v+Y_{v\left| v \right|}\left| v \right|;$ $d_{33} = Z_w+Z_{w\left| w \right|}\left| w \right|; d_{55} = M_q+M_{q\left| q \right|}\left| q \right|; d_{66} = N_r+N_{r|r|}\left| r \right|.$ $\; X_u, X_{u|\psi |}, Y_v, Y_{v|v|}, Z_w, Z_{w|\psi |}, \; M_q, M_{q|q|}, N_r, N_{r|r|}$ are hydrodynamic parameter and damping term, $B$ is the buoyancy of the AUV in the water, $\overline{BG_Z}$ is the longitudinal stability of the AUV. $\omega = \left[\omega _u, \omega _v, \omega _w, \omega _q, \omega _r \right] ^T$ is constant external interference, which is specifically expressed as:

2.3.   Trajectory tracking error model based on virtual guide

When using the virtual guidance method, the underwater movement of the AUV satisfies three assumptions. First, the expected trajectory is the determined parameter curve and the related virtual guidance parameters are always positive and bounded. Second, the heel angle and angular velocity of the AUV affect the underwater robot. It is small and can be ignored. At the same time, the six-degree-of-freedom mathematical model of the AUV is simplified to a five-degree-of-freedom mathematical model. Finally, the longitudinal speed control performance of the AUV is good, and it will not reverse when moving.

Schematic diagram of AUV space trajectory tracking, Geodetic coordinate system $\left\{ I \right\}$, serret-Frenet coordinate system $\left\{ F \right\}$, and body coordinate system $\left\{ B \right\}$. As shown in Figure 2.

Among them: $O$ represents the center of gravity of the AUV, $O = [\xi, \eta, \zeta]^T$, the velocity at point $O$ can be expressed as: $\; V = \parallel O\parallel = \sqrt{O^TO}$, the center of gravity of the AUV coincides with the origin of the motion coordinate system $O$, the origin of the $\{F\}$ coordinate system is any virtual reference point in the desired trajectory of the AUV, the speed relative to the AUV is $v_p$, and the origin $P$ is the "virtual guide" on the expected underwater robot trajectory. The Serret-Frenet coordinate system with point $P$ as the origin can be understood as the coordinate system $\; \{I\}\;$ first rotates $\psi _F$ degrees around the $\eta$ axis, and then rotates $\theta _F$ degrees around $\zeta$, and translates within the coordinate system $\{I\}$. So that the origin $E$ and point $P$ coincidence, the rotation angle can be expressed as: $\psi _F = {\mathrm{arc}}\tan \!\:\left(\frac{y_{F}^{{\mathrm{'}}}}{x_{F}^{{\mathrm{'}}}} \right)$, and the displacement $P$ is the point $p$. The coordinate vector in the coordinate system; the displacement $q$ is the displacement variable of the point $O$ in the coordinate system $\{I\}$, the displacement $d$ is the coordinate variable between the point $p$ and the point $O$ in the coordinate system $\{I\}$, $U = \sqrt{u^2+v^2+w^2}$ is the synthetic velocity vector of the AUV.

The heading angle $\gamma _w$ and the pitch angle $\chi _w$ can be expressed as:

When the AUV is affected by the underwater environment, the angle of attack $\alpha$ and the angle of drift $\beta$ always exist. The heading angle $\psi$ and the pitch angle $\theta$ of the existing AUV can be expressed as:

where $\alpha = {\mathrm{arc}}\tan \!\:\left(\frac{w}{u} \right), u\ge 0, \beta = {\mathrm{arc}}\tan \!\:\left(\frac{v}{\sqrt{u^2+w^2}} \right)$.

In the AUV kinematics equation under the geodetic coordinate system, the velocity vector $U$, the heading angle $\gamma _w$ and the pitch angle $\chi _w$ can be expressed as:

Rotate the motion coordinate system $\{B\}$ along the $y_{B\; }\;$axis by an angle of $\gamma _w\;$ then rotate $\chi _w\;$ angle along the $z_B$ axis. Finally, the fluid coordinate system $\{W\}$ can be obtained, and the $\chi _w$ direction of the fluid coordinate system $\{W\}$ is consistent with the direction of the composite velocity vector $U$ of the AUV. Define $W_{BW}\left(\chi _w, \gamma _w \right)$ as the rotation matrix from the motion coordinate system $\{B\}$ to the fluid coordinate system $\{W\}$. Assuming that the desired path is a continuous smooth curve, represented by the parameter $s$, any point $P$ on the desired path can indicate that it is located on the AUV path. The virtual reference point $V_P$ has a certain speed, which is the so-called "virtual guide". For a virtual guide at any point on the desired path, there must be a unit vector $T$ in the tangent direction of the desired path and a unit vector $N$ along the normal direction of the desired path, and there must be $B = T*N$, the curve described by the expected path parameter $s$, according to the corresponding relationship of the instantaneous speed of the origin of the Serret-Frenet coordinate system, the moving speed of the virtual guide is: $V_P = s$, $c_1(s)$ and $c_2(s)$ represent the curvature and torsion of the virtual guide on the desired path, the two are continuously derivable and bounded with respect to the desired path parameter $s$. The angular velocity of the AUV in the fixed coordinate system $\{I\}$ is expressed in the fluid coordinate system $\{F\}$ as : $W_F(\chi _w, \gamma _w) = (c_2\left(s \right)s, c_1(s)s)$.

Define the displacement $\varepsilon$ from point $P$ to point $O$ in the fixed coordinate system $\{I\}$ as the trajectory tracking error of AUV. Its projection in the $\; T, \; N$ and $B$ directions can be represented by $x_e, y_e, z_e$. Then the relative speed between point $P$ and point $O$ can be expressed as follows in the coordinate system $\{F\}$: $\frac{d\varepsilon}{dt} = \left(x_e, y_e, z_e \right) ^T$, the velocity of point $P$ in the coordinate system $\{I\}$ can be expressed in the coordinate system $\{F\}$ as: $(\frac{dp}{dt})_F = [{\mathrm{s}}, 0, 0]^T$. Define $R$ to represent the rotation matrix between the coordinate system $\{W\}$ and the coordinate system $\{F\}$, which can be expressed as:

Among them: $W_{IF} = (U, 0, 0)^T$ is the vector represented by the synthetic velocity of the AUV in the $\{I\}$ coordinate system in the $\{F\}$ coordinate system. $V_F = (s, 0, 0)^T$ is the vector represented by the speed of the virtual guide AUV in the $\{I\}$ coordinate system in the $\{F\}$ coordinate system. $\psi _e$ and $\theta _e$ are the heading angle error and the pitch angle error between the coordinate system $\{W\}$ and the coordinate system $\{F\}$.

Therefore, formula (2.23) can be rewritten as:

In the definition of the coordinate system $\{W\}$, $\frac{r}{\cos \!\:\theta}+\beta$ and $q+\alpha$ can be used to represent the lateral and vertical angular velocities of the AUV. In the coordinate system $\{F\}$, $c_1(s)s$ and $c_2\left(s \right) s$ can be used to express the desired angular velocity of the AUV, which can be expressed as:

Based on the above formulae (2.24) and (2.25), the AUV trajectory tracking error model can be expressed as:

3.   Design of AUV trajectory tracking controller system

According to the time scale of control, it is divided into position controller and attitude controller. The position controller is designed using adaptive control law and finite time method, and the controller is designed for the depth and longitudinal direction of the AUV respectively. The attitude controller uses dynamic surface control idea, integral sliding mode, reduced order state observer and new reaching law to design the controller with other methods.

3.1.   Position controller design

Compared with infinite time asymptotically stable control, finite time control requires the system state to be stable within a specified time. From the perspective of control time, finite time control is the optimal control method for its convergence time, and finite time control has a fast convergence speed. The control accuracy is higher, and it has good robustness to the control performance of the AUV itself and the disturbance of the external environment.

According to the AUV kinematic equation and dynamic equation, assuming that the AUV heave velocity satisfies $w\approx 0$. The trim angle $\theta$ is within a small range of change. Basically satisfy $sin\theta \approx 0, cos\theta \approx 1$. Regardless of the small terms of the second order and above, the dynamic equation of the vertical plane is as follows:

Set the desired depth of the AUV to $z_d$ and the actual depth to $z$, and the depth error can be re-expressed as: $\; z_{e1} = z-z_d$.

The first step is to define the Lyapunov function $\; V_{z1}$ as follows:

Derivatives on both sides are available:

Define a virtual control quantity:

The coefficients $k_{z1}, k_{z2}, k_{z3}$ are all constants greater than zero, then:

The second setp is to define the Lyapunov function $V_{z2}$ as follows:

Derivatives on both sides are available:

The third setp is to define the Lyapunov function $V_{z3}$ as follows:

Derivatives on both sides are available:

Define $z_{e3} = q-m_{55}\delta _2$, the state variable $z_{e3}$ is designed with a finite time control method, the rudder angle controller is as follows:

The error derivative calculation of the state quantity $q$ can be obtained:

Assuming that the equilibrium point of the system is the origin, the state of the system converges to $\Omega _z$ in a finite time, where the finite extreme value of the vertical interference function $\omega _q(t)$ is $l, l\ge 0, \Omega _z = \{z_{e3}:\left| z_{e3} \right|\le \left(\frac{k_{z4}}{K} \right) ^{\frac{1}{a}}, K > 0, 0 < a < 1$.

Proof: Choose the Lyapunov function $V\left(z_{e3} \right) = \frac{1}{2}z_{e3}^{2}$.

Substituting Eqs (3.10)–(3.12) into Eq (3.9), the following expressions can be obtained:

The proof of system stability is completed. According to the finite-time controller design decision theorem and the proof of inequality (3.12). It can be seen that the designed finite-time position controller (3.9) is finite-time stable, and the vertical error in the system can effectively converge to the equilibrium point. The robustness and anti-interference of the system are excellent.

3.2.   Design of AUV attitude controller

The controller will use the integral sliding mode control method to design the longitudinal control moment $\tau _u$ the pitch control force $\tau _q$ and the heading control moment $\tau _r$. Use the reduced-order expansion state observer to approximate the disturbance, and introduce the dynamic surface technology and new reaching law to avoid the design explosion of virtual control rate calculation and the lack of system compensation.

Compared with the nonlinear state-expanded state observer, the linear reduced-order expanded state observer has a simpler structure and higher practicability when certain quantities can be measured. Among them, the parameters of the linear reduced-order state observer are related to the concept of bandwidth in practical engineering applications, and the engineering practicability value is higher.

where, $w_i(i = 1, 2, 3, 4, 5)$ is the gain of the reduced-order extended observer, $p_i(i = 1, 2, 3, 4, 5)$ is the auxiliary state of the extended observer, $\hat{w}_i(i = u, v, w, q, r)$ is the estimated value of the mixed uncertainty. The initialization value is set: $p_1\left(t_0 \right) = -w_1u, p_2\left(t_0 \right) = -w_2v, p_3\left(t_0 \right) = -w_3w, p_4\left(t_0 \right) = -w_4q$. Make the mixed uncertainty estimate $\hat{D}_i$ always equal to zero. Appropriately increasing the observer's $w_1, w_2, w_3, w_4, w_5$ can reduce the actual error.

The idea of introducing the dynamic surface simplifies the controller, where the error variable is defined as follows:

where, $u_{ed}, q_{ed}$ and $r_{ed}$ are the desired velocities on the three degrees of freedom on the desired trajectory of the AUV respectively.

In order to obtain high-quality tracking response and ideal virtual control input, obtain the derivative information of $u_{ed}, q_{ed}, r_{ed}$, thereby simplifying the calculation of the controller, new state variables $\gamma _q, \gamma _r, \gamma _u$ are introduced. Their mathematical expressions can be expressed as:

where, $T$ is the filter time constant, $\; \gamma _q = \dot{\theta}_{ed}-k_{q1}\left(\theta -\theta _{ed} \right)$, $\gamma _r = \dot{\psi}_{ed}-k_{{\mathrm{r}}1}\left(\psi -\psi _{ed} \right) \cos \theta$, $\; \gamma _u = Ucos\theta _{ed}cos\psi _{ed}$, $\dot{q}_{ed} = \frac{\gamma _q-q_{ed}}{T_q}$, $\dot{r}_{ed} = \frac{\gamma _r-r_{ed}}{T_r}, \dot{u}_{ed} = \frac{\gamma _u-u_{ed}}{T_u}$.

The error integral sliding mode surface designed for stabilizing longitudinal, pitch and heading velocities can be expressed as:

Taking the derivation of both sides of Eq (3.17) without considering the saturation effect and combining with Eq (2.17), the equation can be obtained as:

Setting Eq (3.18) equal to 0, the equivalent control rate can be obtained as:

where, $\hat{w}_I(I = u, q, r)$ is the estimate of the uncertainty in the system by the reduced-order extended state observer.

In the case of parameter perturbation and external interference in the system, the designed equivalent control law cannot guarantee the control effect of the AUV. It is necessary to further introduce a specific reaching law to compensate for the disturbance of the external AUV itself. At the same time, the AUV will generate discontinuous signals when turning. The introduction of the reaching law can guide the AUV to drive along the desired path. The reaching law is selected as follows:

where, $0 < \rho _i < 1, {\mathrm{sig}}^{\rho _i}\!\:\left(S_i \right) = \left| S_i \right|^{\rho _i}{\mathrm{sign}}\!\:\left(S_i \right)$ parallel and $a_i > 0$.

So the control law we designed can be expressed as:

When the AUV travels along the desired trajectory underwater, the controller we designed may experience saturation of the controller output during the steering of the AUV. In order to eliminate the influence of the saturation value on the control performance, the following auxiliary systems can be expressed as:

where, $g_u = \frac{1}{m_{11}}, g_q = \frac{1}{m_{44}}, g_r = \frac{1}{m_{55}}, b_i > 0\left(i = u, q, r \right)$, $\Delta \sigma _i = \sigma _i-\tau _i$ $\left(i = u, q, r \right), \chi _i(i = u, q, r)$ is the state variable of the auxiliary dynamic system.

The control input we actually designed can be expressed as:

Proof: In the case of considering input saturation, formula (3.18) can be rewritten as:

Constructing the Lyapunov preselection function:

Taking the derivative of both sides of Eq (3.25), the equation can be obtained as:

Simultaneous formulae (3.16), (3.18) and (3.22) can be obtained as:

Substituting Eq (3.21) into Eq (3.27) yields:

Further simplification can be obtained as:

According to Young's inequality, the equation can be obtained as:

where, $\chi _i\left(i = u, q, r \right), \tau _i\left(i = u, q, r \right)$.

Substituting Eqs (3.30) and (3.31) into Eq (3.29), then the equation can be obtained as:

By selecting appropriate controller parameters, namely $2\ge k_u\ge 0$, $2\ge k_q\ge 0$, $2\ge k_r\ge 0$, $b_u\ge 1$, $b_q\ge 1$, $b_r\ge 1\;$. $\dot{V}_1$ can be kept less than or equal to zero, and the closed-loop controller is asymptotically stable, so that the control speed and angle of the AUV can keep up with the desired trajectory.

4.   Simulation experiment

First, the performance of the reduced-order state observer designed in this paper is verified by simulation. Figure 3 shows the simulation diagram of the estimated value and actual value of the mixed uncertainty item. The simulation results show that the reduced-order state observer designed in this paper can effectively estimate the mixed uncertainty disturbance in the trajectory tracking process quickly and accurately.

In order to verify the effectiveness and feasibility of the control algorithm, the three-dimensional trajectory synchronous tracking and stabilization control of AUV are numerically simulated based on the mathematical model of "sea exploration Ⅱ" AUV.

Figure 4 shows the simulation results of AUV 3D trajectory tracking. The model uncertainty and complex disturbances in the ocean are simultaneously simulated in the simulation environment. It can be seen from the simulation results that the ESO state observer and finite-time controller can reach the desired path quickly and stably. Compared with the adaptive sliding mode algorithm, its diving and inflection point fluctuations are smaller, and the robustness is better.

Figure 5 shows the output response of the controller for the heading thrust, vertical thrust, and longitudinal thrust. It can be concluded that in the process of eliminating the initial position error of the adaptive sliding mode controller, there is a large output overshoot oscillation in the pitch angle of the AUV at the beginning of the control. However, under the action of the finite-time controller, the algorithm in this paper has no obvious overshoot oscillation, and the output of the AUV vertical thrust is stable.

Figure 6 shows the change of maneuvering angle of AUV along the desired trajectory. Compared with the adaptive sliding mode algorithm, the controller designed in this question is significantly better than the comparison algorithm in lateral stability, the angle transition is smooth, and it also reflects that the designed saturation function plays a role. During the initial dive and steering, the designed controller does not have too much oscillation, the contrast algorithm has serious oscillation and poor stability and robustness.

As shown in Figure 7, when adding random interference and model uncertainty, combined with the position controller and attitude controller designed in this paper, there is no large depth fluctuation when diving and reaching the desired depth, and the error is within 5%, which can be adjusted to the expected value in a short time and remain stable. The adaptive sliding mode algorithm has large overshoot and is easy to shake. From the curve in the figure, it can be concluded that compared with the algorithm in this paper, the adaptive sliding mode algorithm has a larger overshoot and a larger jitter amplitude. The large disturbance in the simulated environment is the water flow rate. The simulated water flow rate is shown in Table 2. Under the large disturbance of water flow in the simulated environment, the algorithm in this paper reaches a stable state in about 31 s. Compared with the adaptive sliding mode control algorithm, the time required by the proposed algorithm to control to a stable expected state is shorter, and the maximum relative error of the two algorithms can reach 10% after reaching the expected depth.

Comprehensive analysis shows that the controller designed in this paper has relatively high trajectory tracking control accuracy, strong robustness to the model uncertainty of AUV itself and the composite disturbance of ocean current. At the same time, the output of the controller is relatively smooth and stable, has low requirements for AUV itself, and has good adaptability and feasibility.

5.   Experiments conducted by the "ocean exploration Ⅱ" AUV in the lake

In order to verify the operation stability of "sea exploration Ⅱ" AUV equipment and the effectiveness of the designed three-dimensional trajectory tracking controller, a lake test was carried out in a lake in Suzhou. The water surface temperature was 29.5℃, and the wind speed was almost no wind. The lake water velocity is about 12 cm/s. Before the lake test, the relevant equipment was debugged and the AUV self-test was carried out. The "sea exploration Ⅱ" AUV is an all-drive AUV equipped with two lateral thrusters, two vertical thrusters and one main thruster. Table 3 lists the appearance and other technical parameters of AUV. Tables 4 and 5 list the technical parameters of the thrusters. Figure 8 shows the side view of AUV.

The "sea exploration Ⅱ" AUV uses fiber inertial navigation, ultra-short baseline(USBL), Doppler log and depth sensor for combined navigation, enabling the AUV to navigate accurately in the water. The technical parameters of the navigation equipments used are shown in Tables 6–9.

The lake test site is a test site in Jiangsu province. The test water depth is up to 40 m, and the lake bottom is flat and the water quality is clear, which is conducive to the AUV movement experiment. The test was carried out in September 2021. The underwater trajectory was set by the upper computer, and the underwater trajectory was tracked by dead reckoning and trajectory path tracking control algorithm. First, make mission planning: the target depth is -1m, the AUV speed is 0.5 knots, the initial point coordinates are (120.317750, 31.109933), the target point coordinates are (120.319405, 31.109317), and the mission duration is 25 minutes. AUV path planning is shown in Figure 9. Figure 10 shows the experiment of AUV in Suzhou lake.

As shown in Figure 11, the AUV gradually approaches the preset track point according to the combined navigation information. The specific implementation method is to obtain the desired heading angle through dead reckoning and use it as the input of closed-loop control for track tracking control. Due to the poor navigation accuracy of GPS in combined navigation under water, the trajectory has a small fluctuation, and AUV cannot move completely along the desired trajectory. However, the trajectory tracking algorithm adopted by this AUV completes the underwater trajectory tracking task better.

As shown in Table 10, the statistics of AUV trajectory error are derived from the 200–1000 s control data in the trajectory tracking control experiment. The statistics of AUV and control error after reaching the specified depth. When the AUV moves according to the expected hexagonal path, the heading angle will change, and the AUV will also change when it is disturbed by the direction of the water flow. The interference of water flow in different directions has a greater impact on the roll angle of the AUV, so the error of the roll angle is slightly larger than that of the heading angle, pitch angle and depth. The experimental data show that, the trajectory tracking controller has excellent control performance in the depth and attitude of AUV. Compared with the expected value, the average value of the vertical thrust and heading thrust error is less than 8 degrees, and the average value of the depth error is less than 0.1 M.

After entering the underwater, the position calibration is carried out through combined navigation, which can better track the hexagonal trajectory, and the trajectory curve has high identification degree. The course curve and depth curve are shown in Figures 12–15. It can be seen from the figure that there is a certain overshoot in the course angle of AUV during path tracking, which is caused by large angle bow turning and certain water flow interference when AUV switches the course. In terms of depth curve, there is a certain fluctuation, which is due to the shaking caused by poor metacentric stability during AUV path tracking. Generally, AUV can track the target depth well.

6.   Conclusions

Considering the unknown environmental disturbance and the control overshoot and slow convergence of conventional sliding mode controller, finite time control and reduced order ESO dynamic integral sliding mode control are proposed for three-dimensional trajectory tracking control. Dynamic assistant system and virtual wizard with adaptive law are introduced. The simulation results show that the controller can better combine the advantages of reduced order ESO and finite time control, suppress chattering and weaken external interference. At the same time, the controller has the advantages of fast response and good robustness. It overcomes the problem of controller overshoot at the inflection point.

Acknowledgments

This work was supported by the Zhoushan Science and Technology Project(2022C13034) and Jiangsu Key R & D plan (BE2018007).

Conflict of interest

On behalf of all authors, the corresponding author states that there is no conflict of interest.