A Trajectory Tracking and Local Path Planning Control Strategy for Unmanned Underwater Vehicles

Abstract

The control strategy of an underdriven unmanned underwater vehicle (UUV) equipped with front sonar and actuator faults in a continuous task environment is investigated. Considering trajectory tracking and local path planning in complex-obstacle environments, we propose a task transition strategy under the event-triggered mechanism and design the corresponding state space and action space for the trajectory tracking task under the deep reinforcement learning framework. Meanwhile, a feed-forward compensation mechanism is designed to counteract the effects of external disturbances and actuator faults in combination with a reduced-order extended state observer. For the path planning task under the rapidly exploring random tree (RRT) framework, a reward component and angular factors are introduced to optimize the growth and exploration points of the extended tree under the consideration of the shortest distance, optimal energy consumption, and steering angle constraints. The effectiveness of the proposed method was verified through continuous task simulations of trajectory tracking and local path planning.

1. Introduction

With the emergence of unmanned underwater vehicles (UUVs) demonstrating high maneuverability, stealth, and intelligence in the tasks of detecting underwater pipelines, surveying underwater topography, exploring oil and gas resources, and undertaking military surveillance and patrolling activities, their control strategies in harsh ocean environments with poor communication conditions and low visibility have become a hot research topic for many scholars [1]. Since the underwater trajectory tracking and local path planning of a single UUV are the basis and prerequisite for accomplishing various complex tasks, granting UUVs these two capabilities simultaneously in complex underwater environments has become a top priority.

There have been numerous attempts at trajectory tracking with classical control methods. A fault-tolerant controller using feedback-linearized control to solve the consensus control problem for multi-UV systems was presented in [2]. Additionally, in [3], the target tracking control issue of underactuated UUVs was effectively tackled through the implementation of line-of-sight (LOS) and backstepping methods. In [4], a novel Lyapunov-based MPC framework was proposed to improve UUV tracking performance, and trajectory tracking control was explored using the model predictive control (MPC) approach. In order to attenuate the effect of the accuracy of the UUV mathematical model on the actual UUV control, many scholars have made improvements to the robustness and control algorithms of UUVs. In [5], an improved self-resistant (ADRC) control strategy was proposed, which estimated unknown disturbances by designing a generalized expanded state observer (ESO). Also, in terms of resistance to underwater disturbances, the motion control of an overactuated UUV based on dynamic sliding mode control (DSMC) theory was presented in [6] to improve the system robustness under the effects of the ocean current and model uncertainties. On this basis, in [7], an optimal allocation control (AC) algorithm was designed to maintain the linear position and Euler angles of the UUV in the presence of model uncertainties as well as ocean currents and to minimize the energy consumption of the system. To solve the UUV bottom-following control accuracy problem, in [8], a model-free controller based on a fuzzy approximator was provided, which significantly improved the robustness and control accuracy. Although the proposals in [4,8] had significant advantages in terms of control accuracy, their handling of external perturbations was poorly considered in comparison to the solutions in [5,7], resulting in possible poor adaptation to real environments. In contrast to the traditional methods described above, reinforcement learning learns directly by interacting with the environment, and the control accuracy does not depend on the accuracy of the model [9,10].

Introducing reinforcement learning into the field of UUV control is an exciting direction. In [11], a deep deterministic policy gradient (DDPG)-based algorithm was implemented to control the velocity and attitude angle of a UUV; similarly, in [12], the DDPG algorithm was used to control the isotropic linear motion of a three-degrees-of-freedom UUV. At a high level of motion control, in [13], the trajectory tracking control task of a UUV was decomposed into multiple position tracking tasks, and the DDPG algorithm was used to realize the 3D trajectory tracking control of the UUV. In [14], pictures were innovatively used as inputs to the policy network, and the proximal policy optimization algorithm was employed to train the UUV to track underwater pipes. However, the above methods tended to ignore the actuator failure and perturbation states of UUVs, and the simplified few-degrees-of-freedom UUV model is simpler to control; thus, the application scenarios are limited.

In terms of obstacle avoidance and path planning, the mainstream methods can be roughly divided into three categories: environment modeling, traditional search, and meta-heuristic search. Among them, meta-heuristic search algorithms have become the current research hotspot in this direction due to their powerful exploration and adaptation capabilities. Environmental modeling methods include visual graphs, Voronoi diagrams, and grid-based techniques, while traditional search methods are mainly used for local path planning, including artificial potential field methods, dynamic window methods, and rapidly exploring random tree methods [15]. For global path planning, the rise of meta-heuristic algorithms has led to much more efficient and successful planning. These methods include the water wave optimization algorithm (WWO) [16], gray wolf optimization algorithm (GWO) [17], artificial fish swarm algorithm (AFSA) [18], and whale optimization algorithm (WOA) [19]. However, these algorithms have fewer applications in real UUV mission environments and are difficult to integrate with trajectory tracking tasks in the temporal and spatial domains. On the contrary, the RRT algorithm is widely used in global path planning and local replanning due to its strong environmental adaptability and search efficiency.

In [20], an improved RRT algorithm was presented to solve the problem of target search and interception in an unknown three-dimensional (3D) environment. In [21], an improved AAF-RRT algorithm that combined the artificial potential field method (APF) with the RRT algorithm was proposed, and a two-layer path planner that could pass through narrow channels was designed. Additionally, in [22], to tackle the smoothness problem of the paths planned by RRT, the planned paths were smoothed by combining greedy algorithms in order to satisfy the special requirements of UUVs for the shortest distance and maneuverability. The methods in both [21,22] were optimized by optimizing the process of generating new path nodes using the RRT algorithm, which is the current direction of RRT algorithm development. The difference is that the combination with the APF algorithm directly changed the position of the newly generated node, while the greedy algorithm was optimized in terms of path smoothness. The problem with the greedy algorithm is that the planned paths tend to lose some of their optimal characteristics, which is why we chose to incorporate angular constraints.

In our task environment, the advantage of RRT is that its search approach is based on a tree structure with random sampling, and routes from points to be extended with new points can generate kinematics and dynamics simulations of the robot, which makes its combination with the tracking task in the same temporal and spatial domains more efficient [21]. Secondly, the search process of RRT is more concise compared to other metaheuristic algorithms and does not require precise spatial information about the obstacles.

The main contributions of our work are summarized below:

• A trajectory tracking controller is designed by using the event triggering mechanism and combining the fault state and disturbance state of the UUV actuator, which realizes the classification and collection of the relevant states in the model training and simulation and the seamless connection of the successive tasks.
• Combining the deep reinforcement learning framework with meta-heuristic algorithms in the same task space, the conditions of distance, energy consumption, and steering angle constraints required by UUVs in the task environment are considered from various perspectives, and reward component and angular factors are introduced into the RRT algorithm to locally plan the path.
• In response to the effects of external perturbations and actuator faults, a joint reduced-order extended state observer and feed-forward compensation mechanism is designed to enable the UUV to counteract the unfavorable effects by automatically adjusting its own output.

2. Preliminaries

In this section, we will model the mission environment, starting from a mathematical model of an underactuated UUV with thruster faults, and combine our knowledge of deep reinforcement learning and RRT to model the environment for the tracking task and local planning task.

2.1. Underactuated UUV Model Description

A torpedo-shaped underactuated UUV, which employs a cross-rudder and single-propeller actuator layout, was chosen as the research object of this paper [23]. We define the position, attitude, and velocity of the underdriven UUV in the Earth-fixed coordinate system $(O_E-X_E{Y}_E{Z}_E)$ and body-axis coordinate system $(O_B-X_B{Y}_B{Z}_B)$, respectively. As shown in Figure 1, (x, y, z) denote the position of the UUV in the Earth-fixed coordinate system. $\theta$ and $\phi$ represent the pitch angle and yaw angle of the UUV in the Earth-fixed coordinate system, respectively, and their positive direction follows the right-hand rule. u, v, and w are the velocities in surge, sway, and heave, respectively. Finally, q and r refer to pitch angular velocity and yaw angular velocity in the body-axis coordinate system, respectively, and their positive direction is the same as that of $\theta$ and $\phi$.

Based on the description of Fossen’s equations [24] associated with UUV thruster failure [25], we can obtain the five-degrees-of-freedom UUV dynamics equations with unknown perturbations:

$$\left\{\begin{array}{c}\dot{u}=\frac{m_{22}}{m_{11}}vr-\frac{m_{33}}{m_{11}}wq-\frac{1}{m_{11}}{f}_u+\frac{1}{m_{11}}\left({\lambda }_{uh}{\tau }_u+{\overline{\omega }}_{uh}\right)-\frac{1}{m_{11}}{\tau }_{du}\hfill \\ \\ \dot{v}=-\frac{m_{11}}{m_{22}}ur-f_v-\frac{1}{m_{22}}{\tau }_{dv}\left(t\right)\hfill \\ \\ \dot{w}=\frac{m_{11}}{m_{22}}uq-f_w-\frac{1}{m_{33}}{\tau }_{dv}\left(t\right)\hfill \\ \\ \dot{q}=\frac{m_{33}-m_{11}}{m_{55}}uw-f_q-\frac{{\rho }_{e}g\nabla G{M}_{L}sin\left(\theta \right)}{m_{55}}+\frac{1}{m_{55}}\left({\lambda }_{qh}{\tau }_q+{\overline{\omega }}_{qh}\right)-\frac{1}{m_{55}}{\tau }_{dq}\hfill \\ \\ \dot{r}=\frac{m_{11}-m_{22}}{m_{66}}uv-f_r+\frac{1}{m_{66}}\left({\lambda }_{rh}{\tau }_r+{\overline{\omega }}_{rh}\right)-\frac{1}{m_{66}}{\tau }_{dr}\hfill \end{array}\right.$$

(1)

where $m_{11}$, $m_{22}$, $m_{33}$, $m_{55}$, and $m_{66}$ represent the mass and inertia parameters of the UUV, and ${\tau }_u$, ${\tau }_q$, and ${\tau }_r$ represent the control force and moment generated by the tail propeller and rudder. $f_u$, $f_v$, $f_w$, $f_q$, and $f_r$ represent hydrodynamic damping and friction items. ${\tau }_{du}\left(t\right)$, ${\tau }_{dv}\left(t\right)$, ${\tau }_{dw}\left(t\right)$, ${\tau }_{dq}\left(t\right)$, and ${\tau }_{dr}\left(t\right)$ represent time-varying bounded environmental disturbances caused by factors such as wind and ocean currents. $G{M}_L$, ∇, g, and ${\rho }_e$ represent the longitudinal metacentric height, displacement, gravity acceleration, and water density, respectively.

In order to more realistically simulate the impact of the UUV’s thruster faults on its drive accuracy and drive efficiency in underwater environments, we simulate the loss of control signals for UUVs when they are subjected to rudder vibration and propeller entanglement by a combination of multiplicative and additive faults. In Equation (1), we set ${\tau }_i(i=u,q,r)$ as the input of the actuator; ${\lambda }_{uh}$, ${\lambda }_{qh}$, and ${\lambda }_{rh}$ are set at a fixed value; and additive faults ${\overline{\omega }}_{uh}$, ${\overline{\omega }}_{qh}$, and ${\overline{\omega }}_{rh}$ are time-varying variables.

2.2. Tracking the State Model of the Mission Environment

In the tracking task, we select the environment state as the relative distance $\rho$, relative pitch angle $\beta$, and relative yaw angle $\alpha$ between the UUV and the desired trajectory in each time step, and obtain it by the LOS method [3]. As shown in Figure 2, $(x_m,y_m,z_m)$ are the coordinates of the desired trajectory point in the Earth’s fixed coordinate system, and $\Delta x=x_m-x$, $\Delta y=y_m-y$, and $\Delta z=z_m-z$ denote the three-dimensional coordinate difference of the trajectory point with respect to the UUV. $(x_r,y_r,z_r)$ are the relative errors and can be expressed as

$$\left[\begin{array}{c}{x}_r\\ y_r\\ z_r\end{array}\right]=R_1^T(\theta ,\phi )\left[\begin{array}{c}\Delta x\\ \Delta y\\ \Delta z\end{array}\right]$$

(2)

where $R_1$ can be expressed as

$$\begin{array}{c}{R}_1(\theta ,\phi )=\left[\begin{array}{ccc}cos\left(\theta \right)cos\left(\phi \right)& -sin\left(\phi \right)& sin\left(\theta \right)cos\left(\phi \right)\\ cos\left(\theta \right)sin\left(\phi \right)& cos\left(\phi \right)& sin\left(\theta \right)sin\left(\phi \right)\\ -sin\left(\theta \right)& 0& cos\left(\theta \right)\end{array}\right]\hfill \end{array}$$

(3)

Finally, we can obtain the relative distance $\rho$, relative pitch angle $\beta$, and relative yaw angle $\alpha$ by

$$\begin{array}{c}\rho =\sqrt{x_r^2+y_r^2+z_r^2}\hfill \\ \\ \beta =arctan2(z_r,\sqrt{x_r^2+y_r^2})\hfill \\ \\ \alpha =arctan2(y_r,x_r)\hfill \end{array}$$

(4)

where $arctan2(a,b)$ indicates the inverse tangent value $a/b$.

Remark 1. 

Some UUV physical constraints and assumptions are as follows:

• The motion of the UUV in the roll direction is neglected.
• The UUV has neutral buoyancy, and the origin of the body-fixed coordinates is located at the center of mass.
• The yaw angle is limited to $(-\pi ,\pi ]$, and the pitch angle is limited to $(-6/\pi ,6/\pi ]$.

To achieve the ability to train UUVs to track a target through deep reinforcement learning, the policy network input states, output actions, and reward functions need to be formulated in conjunction with the states obtained from Equation (4) relative to the desired trajectory.

1.

Trajectory Tracking Task Status

We selected the velocity state, actuator failure state, and perturbation state, which are directly related to the update of the UUV position, velocity, and acceleration states, as the base states for the policy network inputs. The velocity state can be expressed as

$$g_s\dot{=}v={\left[uvwqr\right]}^T$$

(5)

Since multiplicative faults are approximated as fixed values, so that they have no effect on the learning update process of the policy network, the fault state should be set as additive faults, denoted as follows:

$$f_s\dot{=}{\left[{\overline{\omega }}_{uh}{\overline{\omega }}_{qh}{\overline{\omega }}_{rh}\right]}^T$$

(6)

The five-degrees-of-freedom UUV perturbation state can be defined as

$${\mathit{h}}_s=\left[\begin{array}{ccccc}{\tau }_{du}\hfill & {\tau }_{dv}\hfill & {\tau }_{dw}\hfill & {\tau }_{dq}\hfill & {\tau }_{dr}\hfill \end{array}\right]$$

(7)

In addition, combining the relative distance, relative pitch angle, and relative yaw angle obtained from Equation (4), we set the error state of the trajectory tracking task as follows:

$$e_s={[{\rho }_d-\rho ,{\beta }_d-\beta ,{\alpha }_d-\alpha ]}^T$$

(8)

where ${\rho }_d$, ${\beta }_d$, and ${\alpha }_d$ denote the desired relative distance, relative pitch angle, and relative error, respectively.

Finally, we can obtain the input state of the trajectory tracking task in the deep reinforcement learning framework:

$$s=\left[g_s{f}_s{h}_s{e}_s\right]$$

(9)

2.

Action space

The UUV studied in this paper is powered by a propeller in the tail and rudders on both sides, with control forces and moments applied to the UUV to update its state over time. Therefore, we directly chose the control force and moment as the output of the deep reinforcement learning strategy network, which can be expressed as

$$a={[{\tau }_u,{\tau }_q,{\tau }_r]}^T$$

(10)

3.

Reward function

The trajectory tracking task expects the UUV to track the target quickly and maintain a fixed tracking state in a limited time, so the reward function should be selected by first considering the error between the UUV and the desired trajectory, based on the approximation that the larger the error, the smaller the reward. Combining this with Equation (8), one obtains

$$r_s=-{\zeta }_s{∥e_s∥}_1$$

(11)

In addition, we expect the outputs of the UUV propeller and rudder to be maintained in a more stable and energy-efficient manner, which means that under the condition of achieving the same control accuracy, the smaller the output value, the better the corresponding action. Thus, we define the reward function related to the controller output as

$$r_p=-{\zeta }_p{a}^{T}a$$

(12)

where ${\zeta }_s$ and ${\zeta }_p$ denote the weight parameters of the two-part reward function.

2.3. Reduced-Order Extended State Observer

To deal with the unknown perturbations and actuator faults of underdriven UUVs, we design a reduced-order extended observer (RESO) to estimate the perturbation terms and additive fault terms of UUVs in real environments. First, we define the combination term of external perturbations and actuator additive faults as

$$\chi =\left[\begin{array}{c}{\chi }_u\hfill \\ {\chi }_v\hfill \\ {\chi }_w\hfill \\ {\chi }_q\hfill \\ {\chi }_r\hfill \end{array}\right]=\left[\begin{array}{c}{\tau }_{du}\hfill \\ {\tau }_{dv}\hfill \\ {\tau }_{dw}\hfill \\ {\tau }_{dq}\hfill \\ {\tau }_{dr}\hfill \end{array}\right]+\left[\begin{array}{c}{\overline{\omega }}_{uh}\hfill \\ {\overline{\omega }}_{vh}\hfill \\ {\overline{\omega }}_{wh}\hfill \\ {\overline{\omega }}_{qh}\hfill \\ {\overline{\omega }}_{rh}\hfill \end{array}\right]$$

(13)

Assuming that the external perturbation and the actuator fault are both continuously minimizable, then $\chi$ is also continuously minimizable. We can rewrite Equation (1) in matrix form:

$$M\dot{v}+C\left(v\right)v+D\left(v\right)v+g\left(\eta \right)=\lambda a+\chi$$

(14)

where M, C, and D represent the inertia matrix, the Coriolis force, and the centrifugal terms matrix, respectively. The multiplicative faults $\lambda$ of the three actuators are set to 0.8. $v={\left[u,v,w,q,r\right]}^T$ represents the velocity vector, and $\eta ={\left[x,y,z,\theta ,\phi \right]}^T$ represents the position and attitude vectors. a denotes the same action space as in Equation (10). Then, this is reduced to the standard form of a reduced-dimensional observation:

$$\dot{v}=-M^{-1}\left(C\left(v\right)v+D\left(v\right)v+g\left(\eta \right)\right)+\alpha u+\alpha \chi$$

(15)

where u represents $\lambda a$ in Equation (14). For a detailed derivation of the dimensionality-reducing dilation observer, please refer to [26]. Ultimately, the obtained reduced-dimensional extended observer is

$$\left\{\begin{array}{c}\delta =\dot{v}-v\hfill \\ \dot{\widehat{}}v=-M^{-1}\left(C\left(v\right)v+D\left(v\right)v+g\left(\eta \right)\right)-{\beta }_1{f}_1\left(\delta \right)\hfill \\ \dot{\widehat{}}\chi =-\frac{{\beta }_2}{\chi }{f}_2\left(\delta \right)\hfill \end{array}\right.+\alpha \left(\widehat{\chi }+u\right)$$

(16)

Here, $f_1$ and $f_2$ denote the parametric functions, which can be expressed as

$$\left\{\begin{array}{c}{f}_1\left(\delta \right)=k_1\delta \hfill \\ f_2\left(\delta \right)=k_2{\left|\delta \right|}^{\partial }\mathrm{sgn}\left(\delta \right)\hfill \end{array}\right.$$

(17)

where $k_1$, $k_2$ and ∂ are set to constant values. For the proof of Lyapunov stability of this observer, please refer to Appendix A.

2.4. Localized Planning Mission Environment Model

To realize omnidirectional detection centered on the UUV, a 360-degree scanning sonar was selected to obtain scanning data [27]. As shown in Figure 3, an obstacle is considered detected if its relative distance to the UUV is less than or equal to ${\rho }_{max}$.

At the same time, we introduce the event trigger flag $T_d$. When the UUV detects an obstacle, $T_d$ becomes 1, indicating that the trajectory tracking task is temporarily stopped and the local planning task is started; when the UUV travels along the planned collision-free path to the point of the path where the obstacle is not detected, $T_d$ turns back to 0, indicating that the local planning task is completed, and then the UUV continues to follow the desired trajectory.

Since the RRT-planned path is a time series consisting of the coordinates of the UUV corresponding to the time steps in the Earth’s fixed coordinate system, the planned path can be represented by the following mathematical expression:

$$p\left(s\right)=\left\{\begin{array}{c}\overline{x}\left(s\right)\hfill \\ \overline{y}\left(s\right)\hfill \\ \overline{z}\left(s\right)\hfill \end{array}\right.s\in [0,s_n]$$

(18)

where s denotes the path length corresponding to the unit time step, and $s_n$ denotes the distance between the planning start and end points. At the same time, we introduce the concepts of the relative pitch angle and relative yaw angle from Equation (4) into the two neighboring nodes of the planning path and discretize them as

$$\begin{array}{cc}\hfill & \overline{\beta }\left(n\right)=arctan2\left({\overline{z}}_r\left(n\right),\sqrt{{\overline{x}}_r\left(n\right)+{\overline{y}}_r\left(n\right)}\right)\hfill \\ \hfill & \overline{\alpha }\left(n\right)=arctan2\left({\overline{y}}_r\left(n\right),{\overline{x}}_r\left(n\right)\right)\hfill \end{array}$$

(19)

2.5. The Principles of the Basic RRT Algorithm

The concept of the RRT algorithm can be briefly described as follows: First, the random tree T is initialized with the start node of the UUV; then, a random point is evenly sampled in the barrier-free space, and a nearby node in the random tree is traversed by the distance evaluation function. Secondly, a new node is generated by advancing with a unit step from the nearby node to a random node. If there is no collision between the nearby node and the new node, the new node is discarded without any modification to the random tree T [28]. The iterative process above is repeated until the target node becomes a leaf node, or the search ends when the set number of iterations is exceeded. Returning from the target node to the starting point, the planned path can be obtained. Figure 4 shows the node expansion process of basic RRT, as described above.

3. Improved RRT Algorithm with Reinforcement Learning Applications

In this section, a steering angle optimization RRT algorithm based on a reward mechanism is proposed. The logic of deep reinforcement learning applied in this paper is also introduced. Finally, the structural framework of complete trajectory tracking combined with local planning is obtained.

3.1. Improved RRT Algorithm

In order to simulate the real underwater motion of the UUV, we limit the generation of new nodes in the RRT algorithm by using the maximum rotation angles ${\theta }_{max}$ and ${\phi }_{max}$ of the UUV rotating around the y-axis and z-axis of the body-axis coordinate system as the constraints.

The starting point $q_{rand}$ in the state space is used as the root node, and a point $q_{rand}$ is randomly selected from the search space. If this point falls in the non-obstacle interval, it will traverse T to find the nearest node n to this random point [29]. Then, by applying Equation (19), we find the relative pitch angle $\overline{\beta }\left(s\right)$ and relative yaw angle $\overline{\alpha }\left(s\right)$ between $q_{rand}$ and $q_{nearest}$. Finally, we calculate $q_{new}$ under the condition of maximizing angle ${\overline{\beta }}_{max}$ and ${\overline{\alpha }}_{max}$:

$$q_{\mathrm{new}}=\left\{\begin{array}{c}\left(q_{\mathrm{rand}}-q_{\mathrm{near}}\right)·{\overline{\beta }}_{max},\overline{\beta }\left(n\right)>{\overline{\beta }}_{max},\overline{\alpha }\left(n\right)<={\overline{\alpha }}_{max}\hfill \\ \left(q_{\mathrm{rand}}-q_{\mathrm{near}}\right)·{\overline{\alpha }}_{max},\overline{\alpha }\left(n\right)>{\overline{\alpha }}_{max},\overline{\beta }\left(n\right)<={\overline{\beta }}_{max}\hfill \\ \left(q_{\mathrm{rand}}-q_{\mathrm{near}}\right)·min\left({\overline{\beta }}_{max},{\overline{\alpha }}_{max}\right),\overline{\alpha }\left(n\right)>{\overline{\alpha }}_{max},\overline{\beta }\left(n\right)>{\overline{\beta }}_{max}\hfill \end{array}\right.$$

(20)

In addition, for the case where $\overline{\beta }\left(n\right)$ and $\overline{\alpha }\left(n\right)$ are both within the angular constraints, the generation of their new nodes follows the rules of the basic RRT.

In fact, even though we limit the maximum steering angle of the planned path, we still cannot avoid the search instability and hysteresis caused by the strong randomness of the RRT algorithm. In this case, we introduce a reward function for the behavior of RRT to generate new node $q_{new}$ in order to reduce its unnecessary exploration. The setting of the reward weight should consider the following three aspects:

1.

Rewarding the generation of new nodes that are close to the goal point $q_{goal}$.

According to the basic RRT algorithm, the extension vector of the nearest point $q_{near}\left(n\right)$ can be expressed by the following mathematical formula [21]:

$$R\left(n\right)=\tau \times \frac{q_{rand}\left(n\right)-q_{near}\left(n\right)}{∥q_{rand}\left(n\right)-q_{near}\left(n\right)∥}$$

(21)

where $R\left(n\right)$ denotes the expansion vector from the nth nearest point $q_{near}\left(n\right)$ to the nth random point $q_{rand}\left(n\right)$, and $\tau$ represents the step size in the RRT algorithm. Then, the reward component from $q_{goal}$ to $q_{near}\left(n\right)$ can be defined as

$$G_1\left(n\right)=g_1\times \tau \times \frac{q_{goal}-q_{near}\left(n\right)}{∥q_{goal}-q_{near}\left(n\right)∥}$$

(22)

where $g_1$ denotes the reward gain coefficient that varies the target bias of the generated random vector.

2.

Rewarding the generation of new nodes that are close to the desired trajectory.

The continuous task environment of trajectory tracking and localized obstacle avoidance requires the UUV to complete the switch between the two task states with the smallest possible energy consumption. Therefore, it requires us to keep the error with the desired trajectory small during the planning of the paths. As shown in Figure 3, we plan two actual paths, and only that which is closer to the desired trajectory is selected. Emulating the form of the reward component with respect to $q_{goal}$, we obtain the reward component with the desired trajectory point of the next time step as the goal:

$$G_2\left(n\right)=g_2\times \tau \times \frac{q_{\mathrm{track}}(n+1)-q_{near}\left(n\right)}{∥q_{\mathrm{track}}(n+1)-q_{near}\left(n\right)∥}$$

(23)

3.

Rewarding the generation of new nodes that stay away from obstacles.

The new node $q_{new}$ is affected by the reward component $G_3\left(n\right)$ introduced between the barrier and $q_{near}\left(n\right)$ as it is generated, which can be expressed as

$$G_3\left(n\right)=\left\{\begin{array}{c}0,p\left(n\right)>p_0\hfill \\ k\times \tau \left(\frac{1}{p\left(n\right)}-\frac{1}{p_0}\right)\frac{1}{p{\left(n\right)}^2}\frac{\partial \left(q_{near}-x_{obstacle}\right)}{\partial q_{near}},p\left(n\right)<=p_0\hfill \end{array}\right.$$

(24)

Summarizing the above analysis, we can visualize the generation process of new node $q_{new}$ by Figure 5.

3.2. Applications of Deep Reinforcement Learning

To counteract the effects of external perturbations and actuator failures on UUV operation, we apply UUV dynamics model training that excludes external perturbations and actuator additive faults when training the network model using deep reinforcement learning. In the actual test environment, we introduced feed-forward compensation to compensate for the estimated perturbation and additive fault terms before the UUV actuators executed the vehicle’s action. We rewrite Equation (1) as

$$\left\{\begin{array}{c}\dot{u}=\frac{m_{22}}{m_{11}}vr-\frac{m_{33}}{m_{11}}wq-\frac{1}{m_{11}}{f}_u-\frac{{\lambda }_{uh}}{m_{11}}{\tau }_u\hfill \\ \\ \dot{v}=-\frac{m_{11}}{m_{22}}ur-f_v-\frac{1}{m_{22}}{\tau }_{dv}\left(t\right)\hfill \\ \\ \dot{w}=\frac{m_{11}}{m_{22}}uq-f_w-\frac{1}{m_{33}}{\tau }_{dv}\left(t\right)\hfill \\ \\ \dot{q}=\frac{m_{33}-m_{11}}{m_{55}}uw-f_q-\frac{{\rho }_{e}g\nabla G{M}_{L}sin\left(\theta \right)}{m_{55}}-\frac{{\lambda }_{qh}}{m_{55}}{\tau }_q\hfill \\ \\ \dot{r}=\frac{m_{11}-m_{22}}{m_{66}}uv-f_r-\frac{{\lambda }_{rh}}{m_{66}}{\tau }_r\hfill \end{array}\right.$$

(25)

Equation (25) is used for network model training, in conjunction with the state space in Section 2.2. The state we select for training is:

$$s=\left[g_s,{\tau }_{dv},{\tau }_{dw},e_s\right]$$

(26)

where $g_s$ and $e_s$ are consistent with the meanings expressed in Equation (9). Since the UUV does not have actuators in the v and w velocity directions, it cannot offset the effects of perturbations and faults in the form of feed-forward compensation, so we incorporate the perturbations ${\tau }_{dv}$ and ${\tau }_{dw}$ in these two directions into the state space during the training of the network model and train the UUV to output appropriate actions to resist the effects of the perturbations in these two directions.

Then, the feed-forward compensation acts directly on the output action of the UUV actuator, which can be represented as

$$a^{\prime }=\left[\begin{array}{c}{{\tau }^{\prime }}_u\hfill \\ {{\tau }^{\prime }}_q\hfill \\ {{\tau }^{\prime }}_r\hfill \end{array}\right]=\left[\begin{array}{c}{\tau }_u\hfill \\ {\tau }_q\hfill \\ {\tau }_r\hfill \end{array}\right]-\left[\begin{array}{c}{\widehat{\chi }}_u\hfill \\ {\widehat{\chi }}_q\hfill \\ {\widehat{\chi }}_r\hfill \end{array}\right]$$

(27)

The compensated force and torque output of the actuator $a^{\prime }$ ensures that the UUV can safely navigate without disturbances and actuator faults.

Based on the state space of the task environment established in Section 2.2, the architecture of the trajectory tracking task can be represented by Figure 6.

Given the powerful performance of the TD3 algorithm, we embed it in the proposed deep reinforcement learning-based task framework. In the TD3 algorithm, the state-action value function is mainly used to evaluate the current strategy, which we define as

$$Q_{{\theta }_k}(s_t,a_t)\dot{=}r(s_t,a_t)+\gamma E_{s_{t+1}\sim p}\left[V\left(s_{t+1}\right)\right],k=1,2$$

(28)

where $V\left(s_t\right)$ is the state value function, and $\gamma$ is the discount factor. In the state-action value network updating process, a reverse gradient strategy is used, and its objective function can be expressed as follows:

$$y=r(s_t,a_t)+\gamma \left(\underset{k=1,2}{min}{Q}_{{\overline{\theta }}_k}(s_{t+1},a_{t+1}+\epsilon \left|{{\overline{\theta }}_k}^Q\right.)\right)$$

(29)

where $\epsilon$ is a random noise obeying a normal distribution, and the TD3 algorithm introduces two target state-action value networks $Q_{{\overline{\theta }}_k}(s_t,a_t),k=1,2$ and chooses the smaller value of the computation result to be the state-action value of the next state action [30]. Then, its update objective can be expressed as

$$J_Q\left({\theta }_k\right)=E_{(s_t,a_t)\sim \mho }\left[\frac{1}{2}{\left(Q_{{\theta }_k}(s_t,a_t)-y\right)}^2\right]$$

(30)

where ℧ is the replay buffer used to store $s_{i-1}$, $a_{i-1}$, and $s_i$, which is shown in Figure 6. Unlike the state-action value function update approach described above, we update the parameters $\omega$ of the strategy network ${\pi }_{\omega }(a_t\left|s_t)\right.$ by gradient ascent to make the policy network converge to the vicinity of the maximum value of the Q-values. Its update goal can be expressed as follows:

$$J_{\pi }\left(\omega \right)=E_{s_t\sim \mho }\left[\underset{k=1,2}{max}{Q}_{{\theta }_k}(s_t,a_t)\right]$$

(31)

In addition, to improve the generalization ability of the policy network, we include the reference value in the training, which can be denoted as

$$l_d=f\left({\varpi }_i,t\right),i=x_m,y_m,z_m$$

(32)

where ${\varpi }_i$ is a random variable obeying a uniform distribution $U\left({\epsilon }_{min}^d,{\epsilon }_{max}^d\right)$.

At this point, the task strategies of trajectory tracking and local planning have been established. Next, incorporating the description of the event triggering mechanism in Section 2.3, we can obtain the execution rules of these two task strategies in the continuous task space, which are represented by the following pseudo-code (Algorithm 1).

Algorithm 1 Pseudo-code of TD3 for UUV path tracking with local path planning

1: Initialize the interactive environment   2: Randomly initialize state-action value networks $Q_{{\mathit{\theta }}_1}$ and $Q_{{\mathit{\theta }}_2}$ and policy network ${\pi }_{\mathit{\omega }}$ with parameter vectors ${\mathit{\theta }}_1$, ${\mathit{\theta }}_2$, and $\mathit{\omega }$   3: Initialize target state-action value networks $Q_{{\overline{\mathit{\theta }}}_1}$ and $Q_{{\overline{\mathit{\theta }}}_2}$ with parameter vectors ${\overline{\mathit{\theta }}}_1\leftarrow {\mathit{\theta }}_1$ and ${\overline{\mathit{\theta }}}_2\leftarrow {\mathit{\theta }}_2$   4: Initialize replay buffer ${{\mathbb{D}}_1}_{}$ to capacity $\mathit{B}$ and empty it   5: Initialize global shared step counter $C\leftarrow 0$ and maximum time steps of each episode T   6: Set the maximum number of iterations $C_{max}$ and some other necessary parameters (refer to Section 4.1)   7: for $C\in \left\{0,1,2,\cdots ,C_{max}\right\}$ do   8:       Reset step counter for each episode $t\leftarrow 0$   9:       Reset the environment, $E_d\leftarrow 0$, $s_t\leftarrow s_0$, and so on 10:       repeat 11:             Perform action with exploration noise $a\leftarrow {\pi }_{\omega }\left(s\right)+\chi$,  $\chi \sim \mathcal{N}(0,\sigma )$ 12:             Obtain partial states $s_{(t+1)}$ and partial reward r 13:             Store samples in the replay buffer ${\mathbb{D}}_{}\leftarrow {\mathbb{D}}_{}\cup \left\{\left({\mathit{s}}_{\mathit{t}},\mathit{a},{\mathit{s}}_{(\mathit{t}+\mathbf{1})},r\right)\right\}$ 14:             if $|{\mathbb{D}}_{}|==\mathit{B}$ then 15:                   Sample $b_s$ samples $\left({\mathit{s}}_{\mathit{t}},\mathit{a},{\mathit{s}}_{(\mathit{t}+\mathbf{1})},r\right)$ from ${\mathbb{D}}_{}$ 16:                   Update ${\mathit{\theta }}_k\leftarrow {\mathit{\theta }}_k-{\lambda }_{\mathit{\theta }}{\widehat{\nabla }}_{{\mathit{\theta }}_k}{J}_Q\left({\mathit{\theta }}_k\right)$, for $k=1,2$ 17:                   Update $\mathit{\omega }\leftarrow \mathit{\omega }-{\lambda }_{\mathit{\omega }}{\widehat{\nabla }}_{\mathit{\omega }}{J}_{\pi }\left(\mathit{\omega }\right)$ 18:                   Update ${\overline{\mathit{\theta }}}_k\leftarrow \varrho {\mathit{\theta }}_k+\left(1-\varrho \right){\overline{\mathit{\theta }}}_k$, for $k=1,2$ 19:             end if 20:             ${\mathit{s}}_{\mathit{t}}\leftarrow {\mathit{s}}_{(\mathit{t}+\mathbf{1})}$ 21:             $t\leftarrow t+1$ 22:       until $t==T$ 23: end for 24: Start continuous task testing 25: Initialize environment, including desired trajectory, obstacle states, and RRT R 26: repeat 27:       Execute the action a output from the trained policy network ${\pi }_{\mathit{\omega }}$, tracking the desired trajectory 28:       if $T_d==1$ then 29:             for $i\in \left\{1,2,\cdots ,n\right\}$ do 30:                   $q_{rand}\leftarrow Sample\left(i\right)$ 31:                   $q_{near}\leftarrow Nearest(tree,q_{rand})$ 32:                   $\left(q_{new},U_{new}\right)\leftarrow Steer\left(q_{near},q_{rand}\right)$ refer to Section 3.1 33:                   Judge the relationship between $\overline{\partial }\left(n\right)$ and ${\overline{\partial }}_{max}$, $\overline{\beta }\left(n\right)$ and ${\overline{\beta }}_{max}$ 34:                   Update $q_{new}$ by Equation (20) 35:                   if $Obstaclefree\left(q_{new}\right)$ then $R\leftarrow q_{new}$ 36:                   end if 37:                   $T_d==0$ 38:             end for 39:       end if 40: until    41: Continue to complete the trajectory tracking task

Remark 2. 

Please refer to [30] for a detailed description of the TD3 algorithm, which is not developed in this paper. The relevant parameters of the algorithm during the simulation will be given in Section 4.1.

4. Simulation

In this section, the UUV’s trajectory tracking and local planning capabilities and continuous task completion capabilities will be tested in the framework of the above tasks.

4.1. Trajectory Tracking Capability

The parameter settings for the deep reinforcement learning algorithm were consistent before the simulation began. As shown in

Table 1. The values of the shared parameters.

Parameter	Value and Description
$m_{11}$, $m_{22}$, $m_{33}$, $m_{55}$, $m_{66}$	$25.0$, $17.5$, $30.0$, $22.5$, $15.0$
$\rho g▿G{M}_L$	$9.7$
${\lambda }_{uh}$	$0.8$
${\lambda }_{qh}$	$0.8$
${\lambda }_{rh}$	$0.8$
${\overline{\omega }}_{uh}$	$0.5sin\left(0.1t\right)$
${\overline{\omega }}_{qh}$	$0.4cos\left(0.1t\right)$
${\overline{\omega }}_{rh}$	$0.3sin\left(0.1t\right)$
Policy network (full connection layer)	$\to 128\to 64\to 32\to$
State-action value network	$\to 320\to 320\to 160\to 80\to$
${\epsilon }_{min}^d,{\epsilon }_{max}^d$	$0.0$, $1.0$
${\vartheta }_{j=u,v,w,q,r}$	$3.0$, $1.5$, $2.5$, $1.5$, $2.0$
${\gamma }_{j=u,v,w,q,r}$	$0.0$
${\omega }_{j=u,v,w,q,r}$	$0.9$, $0.5$, $0.4$, $0.5$, $0.6$
${\xi }_{j=u,v,w,q,r}$	$0.0$
${\kappa }_{j=u,v,w,q,r}$	$1.0$
$k_{1j=u,v,w,q,r}$	$2.4$, $4.2$, $1.4$, $2.2$, $3.1$
$k_{2j=u,v,w,q,r}$	$1.1$, $0.8$, $0.5$, $1.3$, $0.8$
${\partial }_{j=u,v,w,q,r}$	$0.5$, $0.5$, $0.5$, $0.5$, $0.4$
${\zeta }_s$, ${\zeta }_p$	$0.01$, $0.001$
$\gamma$	$0.99$
$\sigma$	$1.0$
$\mathit{B}$	100,000
$b_s$	128

, for the 3D trajectory tracking task, we set the capacity of replay memory B to 100,000 and the batch size $b_s$ to 128. The learning rate was then set to 0.0003. Next, we selected the desired trajectory for model training as follows:

$$\left\{\begin{array}{c}{x}_d=8sin\left(\left(0.10+0.1{\varpi }_{x_m}\right)t\right)\hfill \\ \\ y_d=10cos\left(\left(0.10+0.15{\varpi }_{y_m}\right)t\right)\hfill \\ \\ z_d=-\left(0.15+0.1{\varpi }_{z_m}\right)t-3\hfill \end{array}\right.$$

(33)

where ${\varpi }_{x_m}$, ${\varpi }_{y_m}$, and ${\varpi }_{z_m}$ are random variables with the same meaning as ${\varpi }_i$ in Equation (32). At the same time, the perturbations in both directions v and w required for the state space during training were set as follows:

$${\tau }_{dj}={\vartheta }_j\left(1-{\gamma }_j\right)cos\left(\left({\omega }_j+{\xi }_j\right)t+{\kappa }_j\right)j=u,v,w,q,r$$

(34)

For training, we only needed the perturbations in the direction of the two velocities u and v as input states, whose parameters were set as ${\gamma }_v,{\gamma }_w,{\xi }_v,{\xi }_w,{\kappa }_v,{\kappa }_w\sim U\left({\epsilon }_{min}^d,{\epsilon }_{max}^d\right)$, with the values updated each training episode. Meanwhile, we set ${\vartheta }_v=1.5,{\vartheta }_w=1.6,{\omega }_v=0.8,{\omega }_w=0.5$. In the actual test, we set the error state based on Equation (8): $e_s={\left[{\rho }_d-\rho ,{\beta }_d-\beta ,{\alpha }_d-\alpha \right]}^T$, where ${\rho }_d$ is 1.0, ${\beta }_d$ and ${\alpha }_d$ are both 0, and the desired trajectory is set to

$$\left\{\begin{array}{c}{x}_d=10sin\left(0.1t\right)\hfill \\ \\ y_d=10cos\left(0.10t\right)\hfill \\ \\ z_d=-0.1t-5\hfill \end{array}\right.$$

(35)

In addition, the parameters related to the amount of disturbance in the actual test could all be set to constant values; please refer to the data in Table 1.

The training process is illustrated by Figure 7, where the average reward was calculated by evaluating the learned action strategy every 100 time steps, such that in each episode we evaluated the action strategy 10 times and finally calculated the average reward for each episode. In Figure 7, the top and bottom of the shaded area indicate the maximum and minimum of the average reward over the 10 assessments, respectively. The end of the 100th episode was taken as the dividing line, before which the process of data collection for the stochastic control strategy was carried out, and after which the action strategy started to be optimized iteratively by gradient descent until the optimal control strategy was obtained. The entire training process took 8.3 h on a computer with a CPU (Inter(R) Core(TM) i5-12400h @2.5GHz made in Portland, OR, United States) and a GPU (NVIDIA GeForce GTX 3070ti 8GB made in Santa Clara, CA, United States). Comparing the deep reinforcement learning algorithms, it is easy to see that the TD3 algorithm rewarded faster convergence and better training stability than the DDPG algorithm (the DDPG algorithm showed individually larger fluctuations near the end of training).

The results of the test are shown in Figure 8, Figure 9, Figure 10 and Figure 11. For comparison with the classical control methods mentioned in Section 1, we reproduced the trajectory tracking controller based on the LOS method in [3]. Additionally, in the context of deep reinforcement learning, in order to verify the convergence and stability performance of the TD3 algorithm in this task, the DDPG algorithm was used as the benchmark algorithm in the same task environment for comparison. In Figure 8 and Figure 9, it is clear that the TD3 algorithm approached the desired trajectory faster and demonstrated better convergence performance and less fluctuation relative to the classical and DDPG algorithms. The problem with the classical algorithms was that they showed poor convergence performance and took a long time to track the target, while the DDPG algorithm, although equal to the TD3 algorithm in terms of tracking speed, had poor stability. In addition, we show the compensated action output values in Figure 10. The DDPG algorithm presented a large-amplitude action output, which would cause dysfunction in the UUV propulsion structure. The estimations of the designed reduced-order extended state observer are shown in Figure 11: the first graph shows the velocity estimates for the UUV with five degrees of freedom, subsequent graphs show the estimates of the combined perturbation and additive fault terms under the five degrees of freedom condition, where the red dashed lines all indicate actual values. It is easy to see that the observer quickly converged to the target value after a short period of oscillation.

4.2. Localized Path Planning Capability

To test the local path planning ability of the UUV, we designed a multi-obstacle environment and compared the pre- and post-improvement RRT algorithms. We assumed that the simulation environment was a space of 100 m × 100 m; the detection radius of the UUV was 10 m; and the starting and ending coordinates were (0, 0) and (99, 99), respectively. The results are shown in Figure 12, where RRT* denotes the improved RRT algorithm obtained in Section 3.1. It is easy to see that the convergence of the spanning tree toward the goal point was greatly improved after the introduction of the gravitational component, and the path smoothness was guaranteed after the addition of the angle restriction. Figure 13 demonstrates the comparison of the above algorithms in terms of search speed and exploration degree. The exploration efficiency and success rate for finding the shortest collision-free path of the RRT* algorithm proposed in this paper were greatly improved in the multi-obstacle environment.

4.3. Trajectory Tracking Combined with Localized Path Planning Capabilities

Based on the event triggering mechanism in Section 2.3, we completed a joint test of the UUV trajectory tracking and local planning capabilities in a 3D environment. We set the desired trajectories as follows:

$$\left\{\begin{array}{c}{x}_d=\left\{\begin{array}{c}2.0+0.7t,0\le \mathrm{t}<20\hfill \\ -7.0+1.3t,20\le \mathrm{t}<60\hfill \end{array}\right.\hfill \\ y_d=\left\{\begin{array}{c}9.0+1.3t,0\le \mathrm{t}<20\hfill \\ -6.7+2.0t,20\le \mathrm{t}<60\hfill \end{array}\right.\hfill \\ z_d=\left\{\begin{array}{c}-3.0-0.4t,0\le \mathrm{t}<20\hfill \\ 6.3-0.9t,20\le \mathrm{t}<60\hfill \end{array}\right.\hfill \end{array}\right.$$

(36)

For the desired trajectory described in Equation (36), the tracking results of the three algorithms (TD3, DDPG, and the classical algorithm) are represented in Figure 14, Figure 15 and Figure 16 and were basically the same as the results in Section 4.1. However, the classical algorithm deviated at about the 210th time step and moved away from the desired trajectory. The TD3 algorithm outperformed the DDPG algorithm in terms of tracking speed, tracking stability, and the magnitude of the output force and moment. In order to fully test the performance of the UUV in the joint task of trajectory tracking and local planning, we established continuous obstacle avoidance and complex obstacle environments, respectively, on the basis of the above tracking trajectories, and the experimental results are analyzed below.

As shown in Figure 17 and Figure 18, in the continuous obstacle environment, the four RRT-based algorithms realized the switching of task states and completed the planning of local collision-free paths through the event triggering mechanism. Due to some randomness in the RRT algorithm, when the reward component was not added, the local path was generated away from the desired direction at a distance larger than half of the sphere. However, after the addition of the reward component, the local path was generated closer to the desired trajectory and the target point. Meanwhile, the smoothness of the planned paths was improved after angle optimization was implemented. Figure 19 and Figure 20 show the joint experiment in a complex-obstacle environment, where the local planning task was turned on after the UUV detected an obstacle ($T_d==1$). The four algorithms under the RRT framework planned different collision-free paths before $T_d$ became 0. It is easy to see in the side view of Figure 19 that the paths planned by the basic RRT and RRT with angular constraints algorithms were relatively long and did not find narrow passages between obstacles. Conversely, after adding the reward component, as shown in the top view of Figure 20, the generation of the localized path moved towards the desired trajectory and successfully searched for the narrow passage. The effectiveness of the proposed algorithm was confirmed.

5. Conclusions

In this paper, a framework for the joint task of UUV underwater trajectory tracking and local path planning was proposed. In this framework, the event triggering mechanism was utilized to realize the switching of task states, and a trajectory tracking controller under a deep reinforcement learning framework was combined with an RRT algorithm to realize local planning in multi-obstacle environments. We introduced two kinds of reward components and angular constraints into the RRT algorithm, which directed the spanning tree towards the target point and deferred the application of the desired trajectory, respectively, preventing the UUV from encountering steering difficulties in the real environment. In order to resist the effects of external perturbations, we proposed a way to observe the perturbations using a reduced-order extended observer and incorporate reinforcement learning features for targeted compensation. Finally, simulation experiments on the UUV’s trajectory tracking ability, local planning ability, and continuous task completion ability were conducted under our proposed framework, which was compared with the classical control algorithm and DDPG algorithm described in Section 1. The results showed that our method achieved better convergence to the desired trajectories and better search performance during planning.

Although this paper demonstrates the feasibility and superiority of our methodology in the theoretical and simulation domains, and the treatment of actuator faults and external perturbations gives the methodology the potential to be applied in a practical environment, the engineering application domain remains the focus of our next work. In addition, the incorporation of the RRT method is not efficient for underwater exploration. In the future, we will use deep reinforcement learning as a framework to explore more efficient and safer methods to tackle real problems in engineering.