3D Obstacle Avoidance Path Planning Algorithm and Software Design for UUV Based on Improved D* Lite-APF

Abstract

To meet the development requirements of the path planning unit for unmanned underwater vehicles (UUVs), research is conducted on UUV 3D obstacle avoidance path planning algorithms and software design. Firstly, aiming at the problem of underwater 3D obstacle avoidance path planning for UUVs, a global path planning algorithm based on the improved D* Lite is designed, and a local path planning algorithm combining the 3D obstacle avoidance strategy and the improved artificial potential field (APF) algorithm is designed. Secondly, based on the above path planning algorithms, a UUV 3D obstacle avoidance path planning software is developed under the Robot Operating System 2 (ROS2) framework and deployed on an Orange Pi 5B. To test the algorithms and the developed software, a UUV autonomous navigation hardware-in-the-loop (HIL) simulation system is constructed. Finally, based on this system, three types of HIL simulation experiments are conducted, including global path planning, local path planning, and comprehensive obstacle avoidance path planning. The simulation experiments show that the improved D* Lite-APF algorithm has better comprehensive performance than the traditional D* Lite-APF algorithm; the path planning software can guide the UUV to reach the goal point safely and runs stably and reliably. The designed UUV 3D obstacle avoidance path planning algorithm and software exhibit good obstacle avoidance performance and can be applied to the rapid development of actual UUV path planning units.

1. Introduction

UUVs are widely applied in fields such as marine environmental surveys, ocean development, reconnaissance, and combat [1]. Path planning is one of the key technologies for UUVs to achieve autonomous navigation. However, the marine environment is characterized by high uncertainty and dynamic variability, and UUVs themselves have limited capabilities in detection, perception, and positioning. Therefore, designing an obstacle avoidance path planning software that can be practically deployed on hardware platforms is of great significance for realizing UUV autonomous navigation functions.

(1)

Research Overview of Path Planning Algorithms

Path planning algorithms are classified into two categories according to whether they have prior knowledge of environmental information: global path planning algorithms in known environments and local path planning algorithms in unknown environments [2].

Global path planning refers to the process of planning a collision-free path from a start point to an end point that is optimal according to specific performance metrics, based on known environmental information. Graph-search-based global path planning algorithms are relatively mature, straightforward to implement, and widely applied, and their evolution process is illustrated in Figure 1. The A* algorithm, proposed by Hart et al. [3], is a global path planning algorithm that combines the advantage of the Dijkstra algorithm in ensuring that the optimal path can be found and the high efficiency of heuristic search algorithms, and it has been widely applied in various robot navigation frameworks [4]. The D* algorithm, proposed by Stentz et al. [5], is a dynamic variant of the A* algorithm. It conducts a reverse search from the goal towards a moving start point and only updates the affected portion of the path when new obstacles appear, making it applicable to path planning in dynamic environments. The Lifelong Planning A* (LPA*) algorithm, proposed by Koenig et al. [6], is an incremental variant of the A* algorithm. It can perform incremental search by utilizing stored information when the environment changes, but it is not suitable for scenarios where the start point and end point change. Building upon LPA*, Koenig et al. [7] proposed the D* Lite algorithm, which achieves the same dynamic replanning effect as D* but is simpler and more efficient. However, it has the problem that the planned path is too close to obstacles. Aiming at the problem that D* Lite is prone to generating invalid paths that pass through obstacles when dealing with incomplete maps, Karlsson et al. [8] proposed the ${\mathrm{D}}_{+}^{\ast }$ algorithm. By introducing unknown space costs, a proximity risk layer, and a dynamic map update mechanism, it effectively enhances path safety and reliability, albeit with higher memory usage. Based on the D* Lite algorithm, Likhachev et al. [9] proposed the Anytime Dynamic A* (AD*) algorithm. It incorporates an anytime mechanism to quickly provide a feasible solution and continuously optimize it under time-constrained computation, thereby balancing dynamic response speed and solution quality.

Local path planning refers to performing real-time obstacle avoidance and local path adjustment based on real-time sensor perception information in unknown or dynamic environments. The artificial potential field (APF) algorithm, proposed by Khatib et al. [10], utilizes virtual forces for path planning. It features a simple structure and low computational burden, making it commonly used for real-time obstacle avoidance. However, it suffers from issues such as local optima and target unreachability. Gao et al. [11] addressed the target unreachability problem by modifying the repulsive potential field function through the introduction of the m-th power of the relative distance between the robot and the goal point. Additionally, they combined it with a simulated annealing algorithm incorporating angle and safety distance constraints to generate virtual goal points, thereby avoiding the local optima problem. Sun et al. [12] addressed the local minima problem by introducing a virtual goal point mechanism based on the positional relationships of specific obstacles. Szczepanski et al. [13] addressed the local minima problem by introducing virtual obstacles that apply additional repulsive forces when the robot becomes trapped in a local minimum.

(2)

Research Overview of UUV Simulation Platforms

Considering the high costs associated with hardware platforms and the elevated risks of physical testing, robotic simulation technology has gradually become a viable solution for algorithm verification [14]. Fossen et al. [15] provided a marine system simulator (MSS) based on Matlab/Simulink (https://www.mathworks.com (14 February 2026)) that can be used for simulating UUV motion control algorithms. However, it cannot provide simulation data for sensors such as sonar, making it unsuitable for the simulation and verification of path planning algorithms. Nie et al. [16] adopted a strategy combining computational fluid dynamics software with Unity3D (https://unity.com (14 February 2026)) to assist in constructing virtual marine environments and simulating UUV working conditions under various scenarios. Manhães et al. [17] developed a high-fidelity UUV simulation software package named UUV Simulator based on ROS and Gazebo. It can simulate various underwater terrains and ocean current disturbances, and also integrates various plugins for simulating hydrodynamic effects, thrusters, and sensors. UUV Simulator has been widely used in the simulation and verification of algorithms for simultaneous UUV localization and mapping [18,19], path planning [20], and motion control [21].

(3)

Related Work

Global–local coupled path planning algorithms have been widely adopted in robot path planning. However, when applied to 3D underwater navigation for UUVs, problems such as poor engineering adaptability of algorithm combinations and insufficient safety and reliability still exist. Specifically, intelligent algorithms based on approaches like deep reinforcement learning (DRL) possess strong adaptive decision-making capabilities, yet they are still not applicable to unmanned platforms with high safety requirements or limited computational resources at the current stage [22]. Probabilistic-sampling-based algorithms (e.g., the RRT* algorithm) have the advantages of high exploration efficiency and strong probabilistic completeness in high-dimensional spaces, but suffer from limitations such as slow convergence speed and unstable path quality [1]. Traditional graph-search-based algorithms (e.g., the D* Lite algorithm) feature high theoretical completeness and simple implementation, but the generated paths usually lie close to obstacles without fully considering the UUV’s size, positioning error and depth constraints, leading to collision and over-depth risks during actual path tracking [8]. Potential-field-based algorithms (e.g., the APF algorithm) have the merits of high real-time performance and simple implementation, but they have inherent defects such as local minima and goal unreachability, which affect the reliability of obstacle avoidance in practical UUV applications [23].

Firstly, considering factors such as the complex underwater environment and UUV motion constraints, the D* Lite algorithm with higher replanning efficiency and the APF algorithm with favorable real-time obstacle avoidance performance are selected as the basic algorithms. These algorithms are improved and fused to overcome their inherent defects, and a 3D obstacle-avoidance path planning algorithm for UUVs is developed, which presents more prominent advantages in safety, real-time performance and engineering realizability. Secondly, a path planning unit is designed: based on the proposed path planning algorithm, path planning software is developed under the ROS 2 framework and deployed on the Orange Pi 5B hardware platform. A UUV detection–navigation–control simulation platform is designed and connected with the path planning unit, thereby constructing a UUV autonomous navigation HIL simulation system. Finally, HIL simulation experiments are carried out using this platform, verifying that the improved D* Lite-APF algorithm achieves favorable obstacle-avoidance performance and that the designed software can be feasibly deployed on actual UUV hardware.

The main scientific and engineering contributions of this paper can be summarized into the following three aspects:

• A UUV 3D obstacle avoidance path planning algorithm with superior advantages in safety, real-time obstacle avoidance and engineering realizability is designed, namely the global–local fusion path planning algorithm based on the improved D* Lite-APF.
• A deployable software system based on ROS 2 is constructed and verified via HIL: the above algorithm is implemented as a set of modular software running on ROS 2 and deployed on the Orange Pi 5B embedded hardware. The establishment of the HIL simulation platform provides a practical and repeatable engineering verification environment for algorithm validation, going beyond pure numerical simulation.
• A comprehensive evaluation method for path planning is established: a multi-dimensional cost function is proposed to comprehensively evaluate collision risk, depth risk, path length and planning time, providing a comprehensive performance evaluation method for path planning beyond single indicators such as path length.

2. 3D Obstacle Avoidance Path Planning Algorithm Design

2.1. Global Path Planning Algorithm Design

To address the problem that the path planned by the D* Lite algorithm is too close to obstacles, and considering the underwater 3D obstacle avoidance environment of UUVs, an improved D* Lite algorithm is designed to achieve global path planning. As shown in Figure 2, let the start point be ${\mathrm{s}}_{\mathrm{start}}$, the goal point be ${\mathrm{s}}_{\mathrm{goal}}$, and the current point be ${\mathrm{s}}_{\mathrm{current}}$. The D* Lite algorithm starts from the goal point ${\mathrm{s}}_{\mathrm{goal}}$, gradually expanding the optimal points ${\mathrm{s}}_i^{\ast }$ ($i=1,2,\cdots ,m$) towards ${\mathrm{s}}_{\mathrm{current}}$ to generate the optimal path. Here, m is the total number of waypoints between ${\mathrm{s}}_{\mathrm{current}}$ and ${\mathrm{s}}_{\mathrm{goal}}$, and ${\mathrm{s}}_i^{\ast }$ is the point among the candidate expansion points ${\mathrm{s}}_i$ with the smallest priority queue function $\mathit{k}\left({\mathrm{s}}_i\right)$. ${\mathrm{s}}_i^{\ast }$ and $\mathit{k}\left({\mathrm{s}}_i\right)$ can be expressed as follows:

$${\mathrm{s}}_i^{\ast }=arg{min}_{{\mathrm{s}}_i}\mathit{k}\left({\mathrm{s}}_i\right)$$

(1)

$$\mathit{k}({\mathrm{s}}_i)=\left[\begin{array}{c}{k}_1\left({\mathrm{s}}_i\right)\\ k_2\left({\mathrm{s}}_i\right)\end{array}\right]=\left[\begin{array}{c}min\{g\left({\mathrm{s}}_i\right),rhs\left({\mathrm{s}}_i\right)\}+h\left({\mathrm{s}}_i\right)+k_{\mathrm{m}}\hfill \\ min\{g\left({\mathrm{s}}_i\right),rhs\left({\mathrm{s}}_i\right)\}\hfill \end{array}\right]$$

(2)

where the value of $\mathit{k}\left({\mathrm{s}}_i\right)$ consists of two parts: $k_1\left({\mathrm{s}}_i\right)$ and $k_2\left({\mathrm{s}}_i\right)$. The “min” in Equation (1) means that, when prioritizing points for expansion, the point with the smallest $k_1\left({\mathrm{s}}_i\right)$ value is considered first. If $k_1\left({\mathrm{s}}_i\right)$ values are equal, then the point with the smallest $k_2\left({\mathrm{s}}_i\right)$ value is selected. $h\left({\mathrm{s}}_i\right)$ is the heuristic function, representing the estimated cost from ${\mathrm{s}}_i$ to ${\mathrm{s}}_{\mathrm{current}}$. It is typically the Euclidean distance between ${\mathrm{s}}_i$ and ${\mathrm{s}}_{\mathrm{current}}$, denoted as ${‖{\mathrm{s}}_i{\mathrm{s}}_{\mathrm{current}}‖}_2$. $k_{\mathrm{m}}$ is an offset used to adjust the heuristic function, initialized to 0. It accumulates based on the movement cost when ${\mathrm{s}}_{\mathrm{current}}$ is updated to maintain consistency of the heuristic function. $g\left({\mathrm{s}}_i\right)$ represents the actual cost from ${\mathrm{s}}_{\mathrm{goal}}$ to ${\mathrm{s}}_i$. ${\mathrm{s}}_{i+1}$ is the immediate successor of ${\mathrm{s}}_i$, and $rhs\left({\mathrm{s}}_i\right)$ represents the one-step lookahead cost from ${\mathrm{s}}_i$ to ${\mathrm{s}}_{\mathrm{goal}}$, and it can be expressed as:

$$rhs\left({\mathrm{s}}_i\right)=\left\{\begin{array}{cc} 0,\hfill & {\mathrm{s}}_i={\mathrm{s}}_{\mathrm{goal}}\hfill \\ {min}_{{\mathrm{s}}_{i+1}\in \mathrm{Succ}\left({\mathrm{s}}_i\right)}[c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})+g\left({\mathrm{s}}_{i+1}\right)],\hfill & \mathrm{else}\hfill \end{array}\right.$$

(3)

where $\mathrm{Succ}({\mathrm{s}}_i)$ denotes the set of successor points ${\mathrm{s}}_{i+1}$ of ${\mathrm{s}}_i$, and $c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})$ represents the edge cost from ${\mathrm{s}}_i$ to ${\mathrm{s}}_{i+1}$, which can be expressed as

$$c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})=\left\{\begin{array}{cc}{C}_{\mathrm{occ}},\hfill & {\mathrm{s}}_{i+1}\mathrm{is}\mathrm{occupied}\hfill \\ C_{\mathrm{free}},\hfill & {\mathrm{s}}_{i+1}\mathrm{is}\mathrm{free}\hfill \end{array}\right.$$

(4)

where $C_{\mathrm{free}}$ is generally taken as ${‖{\mathrm{s}}_i{\mathrm{s}}_{i+1}‖}_2$, and $C_{\mathrm{occ}}$ is a constant value with $C_{\mathrm{occ}}\gg C_{\mathrm{free}}$. When $g\left({\mathrm{s}}_i\right)=rhs\left({\mathrm{s}}_i\right)$, it indicates that ${\mathrm{s}}_i$ is locally consistent. When $g\left({\mathrm{s}}_i\right)\ne rhs\left({\mathrm{s}}_i\right)$, the point ${\mathrm{s}}_i$ is locally inconsistent, which means that obstacles have emerged or been removed on the path. In this case, it is necessary to initiate local updates from the affected nodes and expand gradually outward until all nodes regain local consistency. The relationships among the cost functions including $k_{\mathrm{m}}$, $h\left({\mathrm{s}}_i\right)$, $c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})$ and $g\left({\mathrm{s}}_i\right)$ are illustrated in Figure 2.

Considering the constraint on the maximum pressure-resistant depth of UUVs, points with depth greater than the maximum pressure-resistant depth $D_{\max }$ are set to the occupied state, i.e., equivalent to obstacles, so as to avoid waypoints that do not satisfy the depth constraint.

To address the problem that the path planned by the D* Lite algorithm is too close to obstacles, and considering factors such as UUV dimensions, path tracking errors and positioning errors, an inflation layer cost $C_{\exp }$ is introduced into the calculation formula of $c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})$, which is expressed as follows:

$$c({\mathrm{s}}_i,{\mathrm{s}}_{i+1})=\left\{\begin{array}{cc}{C}_{\mathrm{occ}},\hfill & {\mathrm{s}}_{i+1}\mathrm{is}\mathrm{occupied}\hfill \\ C_{\mathrm{free}}+C_{exp},\hfill & {\mathrm{s}}_{i+1}\mathrm{is}\mathrm{free}\hfill \end{array}\right.$$

(5)

$$C_{\exp }=\left\{\begin{array}{cc}{\displaystyle \frac{{\rho }_{\exp 0}}{{\rho }_{\mathrm{occ}}}}-1,\hfill & {\rho }_{\mathrm{occ}}<{\rho }_{\exp 0}\hfill \\ 0,\hfill & \mathrm{else}\hfill \end{array}\right.$$

(6)

where ${\rho }_{\mathrm{occ}}$ is the Euclidean distance from ${\mathrm{s}}_i$ to the occupied points, and ${\rho }_{exp0}$ is the inflation layer thickness threshold. In fact, the traditional D* Lite algorithm is equivalent to the case where ${\rho }_{exp0}=0$.

Under the finite state space represented by a 3D discrete voxel map, the conventional D* Lite algorithm possesses completeness, convergence and optimality [7]. The improvements proposed in this paper mainly include setting voxels exceeding the limit depth to the occupied state to constrain the maximum navigation depth and introducing an obstacle expansion layer cost into the edge cost to form a composite cost model, as shown in Equation (5). Both improvements are constraints and modifications to the input layer of the algorithm, which do not change the finiteness of the state space nor alter the core search procedure, vertex update rules and priority key value calculation logic of the D* Lite algorithm. Therefore, the improved algorithm inherits the completeness and convergence of the D* Lite algorithm. In addition, the composite edge cost model defined by the improved algorithm is composed of the actual geometric cost and the expansion layer cost, both of which satisfy non-negativity. Meanwhile, the heuristic function h(s) is defined as the Euclidean distance, i.e., the straight-line distance between two points, whose value is necessarily no greater than the cost of the actual shortest feasible path between any two points under the composite cost model. Thus, the heuristic function still maintains admissibility and consistency [7]. Accordingly, the improved algorithm still retains optimality under the composite cost model defined in this paper.

2.2. Local Path Planning Algorithm Design

For local path planning, a 3D obstacle avoidance strategy based on the decoupling of horizontal and vertical planes is proposed in this paper. Additionally, an improved APF algorithm is designed for application in UUV 3D obstacle avoidance.

2.2.1. 3D Obstacle Avoidance Strategy

The 3D motion of the UUV is decomposed into horizontal and vertical planes, and a 3D obstacle avoidance strategy based on horizontal/vertical plane decoupling is designed. It is assumed that the geographic coordinate system $x_{\mathrm{n}}-y_{\mathrm{n}}-z_{\mathrm{n}}$ is oriented east–north–up, and the body coordinate system $x_{\mathrm{b}}-y_{\mathrm{b}}-z_{\mathrm{b}}$ is oriented forward–left–up. As shown in Figure 3, when an obstacle is detected by the sonar, the obstacle avoidance mode M is determined as the horizontal obstacle avoidance mode ${\mathrm{M}}_{\mathrm{h}}$ or the vertical obstacle avoidance mode ${\mathrm{M}}_{\mathrm{v}}$ based on the coordinates $(x_i,y_i,z_i)$ ($i=1,2,\cdots ,n$, where n is the number of point clouds) of the obstacle point clouds in the body coordinate system. Considering safety factors such as UUV maneuvering energy consumption and spatial constraints of underwater vertical motion, horizontal obstacle avoidance is set as the priority mode, which is a reasonable decision based on engineering practical experience and has sound justifications. In terms of energy consumption, the energy consumption of vertical maneuvering is usually significantly higher than that of horizontal maneuvering. Engineering practice shows that in order to achieve tracking control with the same variation in heading angle and pitch angle, the required pitching moment is considerably larger than the steering moment. In terms of spatial constraints, the navigable space in the underwater vertical direction is usually strictly limited by seabed topography and maximum safe depth, resulting in a limited depth range available for safe obstacle avoidance. In contrast, the maneuverable area on the horizontal plane is generally wider, which is conducive to planning obstacle-avoidance paths with higher redundancy.

Assume that the horizontal and vertical obstacle avoidance distance thresholds are $y_0$ and $z_0$ respectively, which satisfy $z_0<y_0<{\rho }_{rep0}$ (${\rho }_{rep0}$ is the repulsive force action distance threshold in the APF algorithm, as described in Section 2.2.2). The vertical plane obstacle avoidance mode is activated only when there exists at least one obstacle point satisfying $\left|y_i\right|>y_0$ and all points satisfying $\left|z_j\right|\le z_0$($j=1,2,\cdots ,n$); otherwise, the horizontal plane obstacle avoidance mode is adopted. The above judgment condition is equivalent to:

$$\mathrm{M}=\left\{\begin{array}{cc}{\mathrm{M}}_{\mathrm{v}},\hfill & \max \left\{\left|y_i\right|\right\}>y_0\mathrm{and}\max \left\{\left|z_j\right|\right\}\le z_0\hfill \\ {\mathrm{M}}_{\mathrm{h}},\hfill & \mathrm{else}\hfill \end{array}\right.$$

(7)

2.2.2. Improved APF Algorithm

(1)

Traditional APF Algorithm

The APF algorithm guides an agent to avoid obstacles by constructing a virtual potential field comprising an attractive field and a repulsive field. The attractive field $U_{\mathrm{att}}$ and the repulsive field $U_{\mathrm{rep},i}$ ($i=1,2,\cdots ,n$) are calculated as follows:

$$U_{\mathrm{att}}={\displaystyle \frac{1}{2}}{k}_{\mathrm{att}}{\rho }_{\mathrm{goal}}^2$$

(8)

$$U_{\mathrm{rep},i}=\left\{\begin{array}{cc}{\displaystyle \frac{1}{2}}{k}_{\mathrm{rep}}{\left({\displaystyle \frac{1}{{\rho }_{\mathrm{obs},i}}}-{\displaystyle \frac{1}{{\rho }_{\mathrm{rep}0}}}\right)}^2,\hfill & {\rho }_{\mathrm{obs},i}\le {\rho }_{\mathrm{rep}0}\\ 0,\hfill & {\rho }_{\mathrm{obs},i}>{\rho }_{\mathrm{rep}0}\end{array}\right.$$

(9)

where $k_{\mathrm{att}}$ is the attractive coefficient of the gravitational field generated by the goal point and $k_{\mathrm{rep}}$ is the repulsive coefficient of the repulsive field generated by obstacles. ${\rho }_{\mathrm{goal}}$ and ${\rho }_{\mathrm{obs},i}$ refer to ${‖{\mathrm{s}}_{\mathrm{current}}{\mathrm{s}}_{\mathrm{goal}}‖}_2$ and ${‖{\mathrm{s}}_{\mathrm{current}}{\mathrm{s}}_{\mathrm{obs},i}‖}_2$ respectively. ${\rho }_{\mathrm{rep}0}$ is the repulsive force acting distance threshold, which should be less than the maximum effective detection distance. The attractive force ${\mathit{F}}_{\mathrm{att}}$ and repulsive force ${\mathit{F}}_{\mathrm{rep},i}$ are derived from the negative gradient of the corresponding potential functions, and can be expressed as

$${\mathit{F}}_{\mathrm{att}}=-\nabla U_{\mathrm{att}}=k_{\mathrm{att}}{\rho }_{\mathrm{goal}}·{\mathit{e}}_{\mathrm{att}}$$

(10)

$${\mathit{F}}_{\mathrm{rep},i}=-\nabla U_{\mathrm{rep},i}=\left\{\begin{array}{cc}{k}_{\mathrm{rep}}\left({\displaystyle \frac{1}{{\rho }_{\mathrm{obs},i}}}-{\displaystyle \frac{1}{{\rho }_{\mathrm{rep}0}}}\right){\displaystyle \frac{1}{{\rho }_{\mathrm{obs},i}^2}}·{\mathit{e}}_{\mathrm{rep},i},\hfill & {\rho }_{\mathrm{obs},i}\le {\rho }_{\mathrm{rep}0}\\ 0,\hfill & {\rho }_{\mathrm{obs},i}>{\rho }_{\mathrm{rep}0}\end{array}\right.$$

(11)

where ${\mathit{e}}_{\mathrm{att}}$ is the unit vector pointing from ${\mathrm{s}}_{\mathrm{current}}$ to ${\mathrm{s}}_{\mathrm{goal}}$ and ${\mathit{e}}_{\mathrm{rep},i}$ is the unit vector pointing from ${\mathrm{s}}_{\mathrm{obs},i}$ to ${\mathrm{s}}_{\mathrm{current}}$. The formula of the resultant force ${\mathit{F}}_{\mathrm{total}}$ is as follows:

$${\mathit{F}}_{\mathrm{total}}={\mathit{F}}_{\mathrm{att}}+\sum _i{\mathit{F}}_{\mathrm{rep},i}$$

(12)

(2)

Improvement for the Local Minima Problem

The traditional APF algorithm suffers from the local minima problem. As shown in Figure 4, when the UUV navigates to a certain point ${\mathrm{s}}_{\min }$, the attractive force generated by the goal point ${\mathrm{s}}_{\mathrm{goal}}$ and the repulsive force generated by the obstacle at point ${\mathrm{s}}_{\min }$ are equal in magnitude but opposite in direction. The resultant force is exactly zero, causing the UUV to stop advancing. This point is a local minimum point. However, in actual navigation, it is rare for the UUV to remain stationary exactly at a local minimum point. More commonly, the UUV oscillates repeatedly within the vicinity of the local minimum, unable to escape the local region. Therefore, it is necessary to determine whether the UUV is trapped at a local minimum. A detection period T and a detection distance threshold ${\rho }_{\det 0}$ are set. Let ${\mathrm{s}}_t$ represent the UUV’s position at time t. Every period T, the UUV’s displacement ${‖{\mathrm{s}}_t{\mathrm{s}}_{t+T}‖}_2$ is checked. If ${‖{\mathrm{s}}_t{\mathrm{s}}_{t+T}‖}_2\le {\rho }_{\det 0}$, the UUV is determined to be trapped at a local minimum point.

To avoid the problem of local minima, a virtual goal point ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$ is introduced to break the force state when the UUV is trapped in a local minimum point. The selection of the position of ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$ is illustrated in Figure 5. Let the length of ${\mathrm{s}}_{\min }{\mathrm{s}}_{\mathrm{goal}}^{\prime }$ be L and the angles between ${\mathrm{s}}_{\min }{\mathrm{s}}_{\mathrm{goal}}^{\prime }$ and the $x_{\mathrm{b}}$-axis in the horizontal plane and vertical plane be ${\alpha }_h$ and ${\alpha }_{\mathrm{v}}$ respectively. When the UUV is in the horizontal obstacle avoidance mode, the coordinates of ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$ in the body coordinate system are $(Lcos{\alpha }_h,Lsin{\alpha }_h,0)$; when in the vertical obstacle avoidance mode, the coordinates are $(Lcos{\alpha }_{\mathrm{v}},0,Lsin{\alpha }_{\mathrm{v}})$. The formula of the attractive force ${\mathit{F}}_{\mathrm{att}}^{\prime }$ generated by ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$ is as follows:

$${\mathit{F}}_{\mathrm{att}}^{\prime }=k_{\mathrm{att}}^{\prime }L·{\mathit{e}}_{\mathrm{att}}^{\prime }$$

(13)

where $k_{\mathrm{att}}^{\prime }$ is the attractive coefficient of the gravitational force generated by ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$, and ${\mathit{e}}_{\mathrm{att}}^{\prime }$ is the unit vector pointing from ${\mathrm{s}}_{\min }$ to ${\mathrm{s}}_{\mathrm{goal}}^{\prime }$.

(3)

Improvement for the Target Unreachability Problem

The traditional APF algorithm also suffers from the target unreachability problem. As shown in Figure 6, when the UUV navigates to the vicinity of the goal point ${\mathrm{s}}_{\mathrm{goal}}$ and ${\mathrm{s}}_{\mathrm{goal}}$ is within the repulsive force acting distance ${\rho }_{\mathrm{rep}0}$, the repulsive force ${\mathit{F}}_{\mathrm{rep},i}$ acting on the UUV is greater than the attractive force ${\mathit{F}}_{\mathrm{att}}$, resulting in the UUV being unable to reach the goal point.

To avoid the target unreachability problem, the target distance ${\rho }_{\mathrm{goal}}$ is introduced into the repulsive force calculation Equation (11), and the improved repulsive force calculation formula is as follows:

$${\mathit{F}}_{\mathrm{rep},i}=\left\{\begin{array}{cc}{k}_{\mathrm{rep}}\left(1-e^{-{\rho }_{\mathrm{goal}}}\right)\left({\displaystyle \frac{1}{{\rho }_{\mathrm{obs},i}}}-{\displaystyle \frac{1}{{\rho }_{\mathrm{rep}0}}}\right){\displaystyle \frac{1}{{\rho }_{\mathrm{obs},i}^2}}·{\mathit{e}}_{\mathrm{rep},i},\hfill & {\rho }_{\mathrm{obs},i}\le {\rho }_{\mathrm{rep}0}\\ 0,\hfill & {\rho }_{\mathrm{obs},i}>{\rho }_{\mathrm{rep}0}\end{array}\right.$$

(14)

After the improvement, the repulsive force acting on the UUV decreases significantly as ${\rho }_{\mathrm{goal}}$ decreases. When ${\rho }_{\mathrm{goal}}=0$, i.e., when the UUV reaches the goal point, ${\mathit{F}}_{\mathrm{rep}}=0$, thus avoiding the target unreachability problem.

(4)

Calculation of Commanded Speed, Yaw Angle and Pitch Angle

After the resultant force ${\mathit{F}}_{\mathrm{total}}$ is obtained by Equation (12), the obstacle avoidance mode M is determined as the horizontal obstacle avoidance mode ${\mathrm{M}}_{\mathrm{h}}$ or the vertical obstacle avoidance mode ${\mathrm{M}}_{\mathrm{v}}$ according to the 3D obstacle avoidance strategy. If $\mathrm{M}={\mathrm{M}}_{\mathrm{h}}$, the horizontal component ${\mathit{F}}_{\mathrm{h}}$ of ${\mathit{F}}_{\mathrm{total}}$ is calculated, and the commanded speed $u_{\mathrm{c}}$ and commanded yaw angle ${\psi }_{\mathrm{c}}$ are calculated and published based on the magnitude and direction of ${\mathit{F}}_{\mathrm{h}}$. If $\mathrm{M}={\mathrm{M}}_{\mathrm{v}}$, the vertical component ${\mathit{F}}_{\mathrm{v}}$ of ${\mathit{F}}_{\mathrm{total}}$ is calculated, and the commanded speed $u_{\mathrm{c}}$ and commanded pitch angle ${\theta }_{\mathrm{c}}$ are calculated and published based on the magnitude and direction of ${\mathit{F}}_{\mathrm{v}}$. Assume that the current yaw angle and pitch angle of the UUV are $\psi$ and $\theta$ respectively, and the expressions of ${\mathit{F}}_{\mathrm{h}}$ and ${\mathit{F}}_{\mathrm{v}}$ in the body coordinate system are ${\mathit{F}}_{\mathrm{h}}=(f_{\mathrm{h}x},f_{\mathrm{h}y},0)$ and ${\mathit{F}}_{\mathrm{v}}=(f_{\mathrm{v}x},0,f_{\mathrm{v}y})$ respectively. Then the formulas of $u_{\mathrm{c}}$, ${\psi }_{\mathrm{c}}$ and ${\theta }_{\mathrm{c}}$ can be expressed as follows:

$$u_{\mathrm{c}}=\left\{\begin{array}{cc}{k}_{\mathrm{h}}{f}_{\mathrm{h}x},& \mathrm{M}={\mathrm{M}}_{\mathrm{h}}\\ k_{\mathrm{v}}{f}_{\mathrm{v}x},& \mathrm{M}={\mathrm{M}}_{\mathrm{v}}\end{array}\right.$$

(15)

$$\left\{\begin{array}{cc}{\psi }_{\mathrm{c}}={\psi }_{\mathrm{c}}+\mathsf{\Delta }{\psi }_{\mathrm{c}}=\psi +arctan{\displaystyle \frac{f_{\mathrm{h}y}}{f_{\mathrm{h}x}}},& \mathrm{M}={\mathrm{M}}_{\mathrm{h}}\\ {\theta }_{\mathrm{c}}={\theta }_{\mathrm{c}}+\mathsf{\Delta }{\theta }_{\mathrm{c}}=\theta +arctan{\displaystyle \frac{f_{\mathrm{v}z}}{f_{\mathrm{v}x}}},& \mathrm{M}={\mathrm{M}}_{\mathrm{v}}\end{array}\right.$$

(16)

where $\mathsf{\Delta }{\psi }_{\mathrm{c}}$ and $\mathsf{\Delta }{\theta }_{\mathrm{c}}$ are the yaw angle correction command and pitch angle correction command respectively; $k_{\mathrm{h}}$ and $k_{\mathrm{v}}$ are proportional constants, which are both set to 1 for simplicity.

Furthermore, considering the constraints of actuators and kinematics, the maximum yaw angle correction $\mathsf{\Delta }{\psi }_{\mathrm{cmax}}$ and maximum pitch angle correction $\mathsf{\Delta }{\theta }_{\mathrm{cmax}}$ are set, and the calculation formulas of ${\psi }_{\mathrm{c}}$ and ${\theta }_{\mathrm{c}}$ are rewritten as follows:

$$\left\{\begin{array}{cc}{\psi }_{\mathrm{c}}=\psi +{\displaystyle \frac{\mathsf{\Delta }{\psi }_{\mathrm{cmax}}}{\pi /2}}arctan{\displaystyle \frac{f_{\mathrm{h}y}}{f_{\mathrm{h}x}}},& \mathrm{M}={\mathrm{M}}_{\mathrm{h}}\\ {\theta }_{\mathrm{c}}=\theta +{\displaystyle \frac{\mathsf{\Delta }{\theta }_{\mathrm{cmax}}}{\pi /2}}arctan{\displaystyle \frac{f_{\mathrm{v}z}}{f_{\mathrm{v}x}}},& \mathrm{M}={\mathrm{M}}_{\mathrm{v}}\end{array}\right.$$

(17)

(5)

Convergence and Completeness Analysis

Within the limited 3D discrete voxel state space for UUV navigation, the improved APF algorithm does not change the core gradient-descent-based solution framework of the artificial potential field, and realizes targeted optimization for the core defects of the conventional algorithm, thus possessing conditional convergence and conditional completeness, which are analyzed respectively as follows.

In terms of conditional convergence, the improved algorithm effectively escapes from the local optimum state by introducing virtual target points, avoiding the convergence failure caused by zero resultant force in the traditional algorithm. In addition, by integrating the target distance into the repulsive force function, the unreachable target problem caused by excessive repulsive force near the target, namely the convergence failure at the end of the path, is avoided. Under the condition that the limit of the maximum attitude angle correction of the UUV is satisfied and there are no extremely dense obstacle clusters in the underwater environment, the algorithm can continuously approach the target point iteratively along the potential field gradient, and the iteration process will inevitably terminate due to the finite state space. Therefore, the improved algorithm has conditional convergence under the above conditions.

In terms of conditional completeness, if there exists an obstacle avoidance path satisfying the UUV depth constraint and collision safety distance constraint in the space, the algorithm can search for the path within finite iterations and converge to the target point; if there is no feasible path satisfying the constraints in the space, the algorithm will judge the path as infeasible due to the continuous failure to move effectively towards the target point. Therefore, the improved algorithm has conditional completeness within the limited 3D discrete environment and UUV motion constraints.

2.3. Comprehensive Path Planning Cost Function Design

To evaluate the performance of UUV 3D path planning under complex obstacle conditions such as static and dynamic obstacles in known and unknown underwater environments, a comprehensive path planning cost function J is designed mainly considering four aspects: collision risk degree, diving depth, navigation distance and planning time consumption. The smaller the value of J, the better the comprehensive performance of path planning. The formulas of J is as follows:

$$J={\omega }_{\mathrm{obs}}{J}_{\mathrm{obs}}+{\omega }_{\mathrm{dep}}{J}_{\mathrm{dep}}+{\omega }_{\mathrm{len}}{J}_{\mathrm{len}}+{\omega }_{\mathrm{time}}{J}_{\mathrm{time}}$$

(18)

where $J_{\mathrm{obs}}$, $J_{\mathrm{dep}}$, $J_{\mathrm{len}}$ and $J_{\mathrm{time}}$ are the collision risk function, depth risk function, navigation distance cost function and planning time cost function respectively. ${\omega }_{\mathrm{obs}}$, ${\omega }_{\mathrm{dep}}$, ${\omega }_{\mathrm{len}}$ and ${\omega }_{\mathrm{time}}$ are the weight coefficients of each cost function, which satisfy the condition of ${\omega }_{\mathrm{obs}}+{\omega }_{\mathrm{dep}}+{\omega }_{\mathrm{len}}+{\omega }_{\mathrm{time}}=1$. Among the above weight coefficients, ${\omega }_{\mathrm{obs}}$ and ${\omega }_{\mathrm{dep}}$ are related to navigation safety with the highest priority and shall be assigned larger values, which means that, in the path optimization process, the algorithm will significantly prefer paths that keep away from obstacles and lie within the safe depth, even at the cost of increased path length or computation time; ${\omega }_{\mathrm{len}}$ is related to navigation efficiency and energy consumption, and on the premise of ensuring safety, shortening the navigation distance helps to save energy, so it takes the second largest value; ${\omega }_{\mathrm{time}}$ mainly affects the real-time response capability of the algorithm, and in the hybrid architecture proposed in this paper, global planning is executed only at the beginning of the task or in the event of drastic environmental changes, so it takes the smallest value. Based on the above analysis, the proportional relationship of each weight is set as ${\omega }_{\mathrm{obs}}:{\omega }_{\mathrm{dep}}:{\omega }_{\mathrm{len}}:{\omega }_{\mathrm{time}}=3.5:3.5:2:1$. All the above cost functions are reasonably normalized.

Among the above weight coefficients, ${\omega }_{\mathrm{obs}}$ and ${\omega }_{\mathrm{dep}}$ are related to navigation safety and should be assigned larger values; ${\omega }_{\mathrm{len}}$ is associated with navigation efficiency and energy consumption, so a second-larger value should be taken; ${\omega }_{\mathrm{time}}$ is only related to the efficiency of planning calculation itself, thus a minimum value is appropriate. All the above cost functions are reasonably normalized.

(1)

Collision Risk Function $J_{\mathrm{obs}}$

The expression of collision risk function $J_{\mathrm{obs}}$ is defined as follows:

$$J_{\mathrm{obs}}=\left\{\begin{array}{cc}\infty ,\hfill & {\rho }_{min}<{\rho }_{\mathrm{crit}}\\ {\displaystyle \frac{l_{\mathrm{risk}\_\mathrm{o}}}{l}}\hfill & {\rho }_{min}\ge {\rho }_{\mathrm{crit}}\end{array}\right.$$

(19)

where ${\rho }_{min}$ is the minimum distance from the UUV to the obstacle, ${\rho }_{\mathrm{crit}}$ is the critical collision distance, which should be greater than or equal to the sum of the UUV size, maximum path tracking error and positioning error, and ${\rho }_{\mathrm{risk}}$ is the collision risk distance with ${\rho }_{\mathrm{risk}}>{\rho }_{\mathrm{crit}}$. A collision risk sphere is defined, with the obstacle center as the sphere center and ${\rho }_{\mathrm{risk}}$ as the radius. $l_{\mathrm{risk}\_\mathrm{o}}$ is the trajectory length within the collision risk sphere, and l is the total length of the UUV’s trajectory from the start point to the end point. The meanings of the various distance variables are shown in Figure 7.

(2)

Depth Risk Function $J_{\mathrm{dep}}$

The expression of depth risk function $J_{\mathrm{dep}}$ is defined as follows:

$$J_{\mathrm{dep}}=\left\{\begin{array}{cc}\infty \hfill & d_{\max }\ge D_{\max }\\ {\displaystyle \frac{l_{\mathrm{risk}\_\mathrm{d}}}{l}}\hfill & d_{\max }<D_{\max }\end{array}\right.$$

(20)

where $D_{\max }$ is the maximum pressure-resistant depth of the UUV, which is a constant; $d_{\mathrm{risk}}$ is the set risk depth, which should satisfy $d_{\mathrm{risk}}<D_{\max }$; $d_{\max }$ is the maximum trajectory depth of the UUV; $l_{\mathrm{risk}\_\mathrm{d}}$ is the trajectory length of the UUV navigating at depths greater than $d_{\mathrm{risk}}$; and l has the same meaning as in Equation (19). The meanings of the various distance variables are shown in Figure 8.

(3)

Navigation Distance Cost Function $J_{\mathrm{len}}$

The expression of navigation distance cost function $J_{\mathrm{len}}$ is defined as follows:

$$J_{\mathrm{len}}={\displaystyle \frac{l}{2l_{\mathrm{total}}}}$$

(21)

where l has the same meaning as in Equation (19) and $l_{\mathrm{total}}$ is the straight-line distance from the start point to the end point.

(4)

Planning Time Cost Function $J_{\mathrm{time}}$

The expression of planning time cost function $J_{\mathrm{time}}$ is defined as follows:

$$J_{\mathrm{time}}={\displaystyle \frac{t}{t_{\max }}}$$

(22)

where t is the planning time consumption, $t_{\max }$ is the maximum allowable planning time consumption, which satisfies $t_{\max }\le {\rho }_{\mathrm{crit}}/U_{\max }$, and $U_{\max }$ is the maximum speed of the UUV.

3. Path Planning Software Design and UUV Autonomous Navigation HIL Simulation System Construction

3.1. Overall Design

3.1.1. System Composition and Functions

The UUV autonomous navigation HIL simulation system consists of a path planning unit and a UUV detection–navigation–control simulation platform (hereinafter referred to as UUV simulation platform). The UUV simulation platform is used to simulate functions such as target detection, navigation, and motion control, and to monitor the navigation status. The architecture of the UUV autonomous navigation HIL simulation system is shown in Figure 9. The composition and functions of the path planning unit and the UUV simulation platform will be introduced separately below.

The path planning unit includes nodes such as the mapping node octomap_server, the global path planning node global_planner, and the local path planning node local_planner. The mapping node converts sonar point cloud data (PCD) into a voxel map M. The global path planning node subscribes M, along with the start point ${\mathrm{s}}_{\mathrm{start}}$ and goal point ${\mathrm{s}}_{\mathrm{goal}}$, and plans a collision-free global path P. The local path planning node subscribes sonar point cloud data. When dynamic obstacles are detected, it calculates commanded motion parameters (including commanded speed $u_{\mathrm{c}}$, yaw angle ${\psi }_{\mathrm{c}}$, and pitch angle ${\theta }_{\mathrm{c}}$) for obstacle avoidance in real time and sends them to the motion controller.

The UUV simulation platform mainly consists of the motion control unit, navigation unit, UUV Simulator, and ground station unit. The motion control unit runs the motion control node pid_controller. It subscribes P and commanded motion parameters from the path planning unit, combines them with the estimated attitude, velocity, and position (AVP) information from the navigation unit, and publishes commanded thrust $T_{\mathrm{c}}$ to the UUV’s thrusters through path guidance and motion control processes. The navigation unit, composed of the state estimation node ekf_localization, feeds back the estimated AVP to the path planning and motion control units. UUV Simulator is primarily used to simulate the forward-looking sonar, navigation sensors, actuators, and the underwater simulation environment. The ground station unit runs the set command node set_cmd to publish ${\mathrm{s}}_{\mathrm{start}}$ and ${\mathrm{s}}_{\mathrm{goal}}$ and runs the monitoring and visualization tool RViz to monitor the UUV’s motion status.

In the HIL simulation system, the Orange Pi 5B is used as the hardware for the path planning unit, while a portable computer serves as the hardware for the UUV simulation platform. The configuration of the Orange Pi 5B is: Rockchip RK3588S 2.4 GHz CPU, 16 GB RAM. The configuration of the portable computer is: Intel Core 2.4 GHz CPU, 16 GB RAM. The portable computer and the Orange Pi 5B are connected via an Ethernet cable. The hardware and operating scenario of the UUV path planning HIL simulation platform are shown in Figure 10.

3.1.2. Software Architecture

The software architecture of the UUV autonomous navigation HIL simulation system built on ROS2 is illustrated in Figure 11.

Compared to ROS, ROS2 offers significant improvements in real-time performance and reliability. Therefore, the path planning unit software is developed based on the ROS2 framework, utilizing the ROS2 Humble distribution and Ubuntu 22.04. However, the current UUV Simulator is only compatible with ROS. Considering optimal compatibility, the software environment for the UUV simulation platform employs ROS Melodic and Ubuntu 18.04. To enable cross-platform node communication between the path planning software and UUV simulation platform, ros1_bridge [24] is used to facilitate bidirectional communication between ROS and ROS2. The simulation platform uses ROS2 messages for external communication. Since Ubuntu 18.04 officially supports the ROS2 Dashing distribution, ROS2 Dashing is also installed on the simulation platform.

The connection relationships between nodes and topics in the HIL simulation platform are shown in Figure 12. Among these, the “/gazebo” node is responsible for bidirectional communication of topics and services between ROS and the Gazebo simulation world. Actuators, sonar, navigation sensors, etc., defined by UUV Simulator all run within Gazebo and interact with other ROS nodes through “/gazebo” node. The monitoring and visualization tool RViz can subscribe to all topics.

3.2. Path Planning Software Design

The path planning software consists of a mapping node, as well as global and local path planning nodes.

3.2.1. Mapping Node Design

The mapping node octomap_server is implemented based on the OctoMap framework [25]. As an efficient probabilistic 3D mapping framework, the core of OctoMap lies in organizing 3D space with an octree data structure, which recursively divides the 3D space into eight subspaces until the preset resolution requirement is satisfied, as shown in Figure 13. In this design, under the OctoMap framework, the octomap_server node performs coordinate transformation on the input 3D sonar point cloud data from the sonar coordinate system to the geographic coordinate system, filters out unreliable, erroneous, or redundant measurement points in the point cloud, then updates the occupancy probability of the corresponding voxels, and finally generates a probabilistic voxel map M for the D* Lite global path planning node to use.

3.2.2. Global Path Planning Node Design

Considering safety factors such as known underwater static obstacles and diving depth, a global path planning node, global_planner, is designed to generate the globally optimal obstacle avoidance path. This node subscribes to the 3D voxel map M constructed by the mapping node, as well as to the start point ${\mathrm{s}}_{\mathrm{start}}$, goal point ${\mathrm{s}}_{\mathrm{goal}}$, and current point ${\mathrm{s}}_{\mathrm{current}}$. Based on the improved D* Lite algorithm, it progressively expands optimal points from ${\mathrm{s}}_{\mathrm{goal}}$ towards ${\mathrm{s}}_{\mathrm{current}}$, ultimately generating the globally optimal path. Upon receiving a replanning command from the local path planning node, it updates M and ${\mathrm{s}}_{\mathrm{current}}$ to perform replanning. The operational flowchart of the global path planning node is shown in Figure 14.

3.2.3. Local Path Planning Node Design

Considering static and dynamic obstacles in unknown underwater environments, a local path planning node local_planner is designed to generate obstacle avoidance commands. First, it subscribes to sonar point cloud data and the global path. Then, the sub-goal point ${\mathrm{s}}_{\mathrm{sub}}$ of the APF algorithm is selected from the global waypoints. ${\mathrm{s}}_{\mathrm{sub}}$ satisfies the following two conditions:

• ${\mathrm{s}}_{\mathrm{sub}}$ is a global path point that the UUV has not yet reached;
• For all $i\in \left[1,n\right]$, ${‖{\mathrm{s}}_{\mathrm{sub}}{\mathrm{s}}_{\mathrm{obs},i}‖}_2>{\rho }_0$ is always satisfied, where n is the number of obstacles’ point clouds and ${\mathrm{s}}_{\mathrm{obs},i}$ is the center point of the i-th obstacle.

After the sub-goal point is determined, the resultant force ${\mathit{F}}_{\mathrm{total}}$ is calculated according to the improved APF algorithm. The commanded speed $u_{\mathrm{c}}$, commanded yaw angle ${\psi }_{\mathrm{c}}$ and commanded pitch angle ${\theta }_{\mathrm{c}}$ are calculated by Equations (15) and (17). The motion controller subscribes $u_{\mathrm{c}}$, ${\psi }_{\mathrm{c}}$ and ${\theta }_{\mathrm{c}}$ to control the motion of the UUV. When it is detected that the UUV is trapped at a local minimum point, a virtual sub-goal point ${\mathrm{s}}_{\mathrm{sub}}^{\prime }$ is introduced to recalculate ${\mathit{F}}_{\mathrm{total}}$. If the trapping time exceeds five detection cycles T, a replanning command is published to make global_planner node replan the global path. When the UUV escapes from the local minimum point, ${\mathrm{s}}_{\mathrm{sub}}^{\prime }$ is removed and the path planning continues until the UUV reaches the sub-goal point ${\mathrm{s}}_{\mathrm{sub}}$. The operation flow of the local path planning node is shown in Figure 15.

3.3. UUV Simulation Platform Software Design

The UUV simulation platform software is composed of functional modules including UUV Simulator, navigation, motion control, and a ground station.

(1)

UUV Simulator Configuration

Within UUV Simulator, the UUV type selected is the Sperre SF 30k ROV [26], named RexROV. Its relevant specifications are listed in

Table 1. Relevant specifications of RexROV.

Name	Symbol [Unit]	Value
Dimensions	$L\times W\times H$ [m]	2.6 × 1.5 × 1.6
Number of horizontal thrusters	$n_{\mathrm{h}}$ [−]	4
Number of vertical thrusters	$n_{\mathrm{v}}$ [−]	4
Maximum speed	$U_{\max }$ [$\mathrm{m}·{\mathrm{s}}^{-1}$]	1.5
Economic speed	$U_{\mathrm{eco}}$ [$\mathrm{m}·{\mathrm{s}}^{-1}$]	1
Maximum depth	$D_{\max }$ [m]	25

. To verify the effectiveness of the improved algorithm when considering maximum depth constraints, $D_{\max }$ is artificially modified from 1000 m to 12 m during simulation. RexROV is equipped with sensors including a forward-looking sonar, and navigation sensors including a Doppler velocity log (DVL), IMU, manometer, magnetometer, and acoustic positioning device (named relative position transducer, RPT). The parameter configurations for these sensors are detailed in

Table 2. Performance parameters of RexROV.

Senseors	Parameter [Unit]	Value
Forward-looking sonar	Maximum detection range [m]	16
	Horizontal field of view [°]	$\pm 45$
	Vertical field of view [°]	$\pm 20$
RPT	Noise standard deviation [m]	0.1
DVL	Noise standard deviation [m]	0.05
IMU	Gyroscope dynamic bias stability [$°·{\mathrm{h}}^{-1}$]	0.133
	Accelerometer dynamic bias stability [mg]	0.408
Manometer	Measurement range [kPa]	30,000
	Noise standard deviation [kPa]	3
Magnetometer	Noise standard deviation [$\mathsf{\mu }\mathrm{T}$]	1

. It is assumed that each sensor of the navigation unit can stably and continuously obtain the attitude, velocity and position of the UUV by means of acoustics, inertial measurement and water pressure detection. It is assumed that the sonar can accurately and effectively detect obstacles and stably output point cloud data containing the position and contour information of obstacles. RexROV and its configured sensors are depicted in Figure 16. The simulation environment is a 140 m × 140 m lake, as shown in Figure 17, with a maximum depth of 30 m, and no environmental disturbances such as current, wind and waves are assumed.

(2)

Software Design of the Navigation Unit

The navigation unit software is implemented using the node ekf_localization. A message conversion function module is designed to convert data from the DVL, manometer, and magnetometer into standard ROS message formats. The open-source package robot_localization [27] subscribes to DVL velocity messages /rexrov/sensors/dvl (for brevity, “rexrov/sensors” is omitted hereafter), manometer underwater pressure messages /…/pressure, magnetometer magnetic induction intensity messages /…/magnet, RPT relative position messages /…/rpt, and IMU attitude messages /…/imu. Based on the extended Kalman filter (EKF) algorithm, it estimates UUV motion parameters such as attitude, velocity, and position.

(3)

Software Design of the Motion Control Unit

The motion control unit software is implemented using the node pid_controller, primarily consisting of two functional modules: path guidance and motion control. Path guidance employs the integral line-of-sight guidance law [28] to generate commanded motion parameters for tracking the global path. Motion control uses PID algorithms to generate thrust commands for tracking the commanded motion parameters.

(4)

Software Design of the Ground Station Unit

The ground station unit software primarily utilizes the open-source visualization tool RViz [29]. By subscribing to all topics within the ROS/ROS2 network, it enables real-time monitoring of the UUV’s motion status. A command publishing node, set_cmd, is designed to publish position commands for the start and goal points.

4. HIL Simulation Experiments and Analysis

To test the performance of the UUV 3D obstacle avoidance path planning algorithm and the developed software, experiments are conducted on the established HIL simulation platform. These included global obstacle avoidance path planning in known environments, local path planning in unknown environments, and comprehensive obstacle avoidance path planning experiments involving both aforementioned scenarios.

4.1. Global Obstacle Avoidance Path Planning Experiment

Considering static obstacles in a known environment, a global path planning experiment was conducted. Prior to global path planning, a 3D voxel map needs to be generated by the mapping node, with the map resolution set to 1m. After launching the octomap_server node and UUV simulation platform, the RexROV is navigated via keyboard control. The octomap_server node subscribes sonar data to construct a 3D voxel map of the underwater environment, as shown in Figure 18. The experimental scene displayed in Gazebo is shown in Figure 19. A comparison between Figure 18 and Figure 19 shows that the generated map M contains relatively complete topographic information required for global path planning.

To quantitatively analyze the performance of the improved D* Lite algorithm under different values, the octomap_server, global_planner node and UUV simulation platform are launched, and 10 independent and repeated path planning experiments are conducted with ${\rho }_{exp0}$ set to 5 m, 8 m, 11 m, 14 m, 17 m and 20 m respectively. The coordinates of the start point and end point are set to (−60, 40, −7) and (20, −40, −15) respectively in the ground station unit software. On UUV Simulator, the initial yaw angle is set to 0, and the initial speed command is set to the economic speed (1 m/s). The parameter settings of the improved D* Lite algorithm are shown in

Table 3. Parameters of improved D* Lite algorithm.

Name	Symbol [Unit]	Value
Cost of occupied voxel	$C_{\mathrm{occ}}$ [m]	10,000
Cost of free voxel	$C_{\mathrm{free}}$ [m]	1
Thickness threshold of the inflation layer	${\rho }_{\exp 0}$ [m]	5, 8, 11, 14, 17, 20

. The parameter settings of the comprehensive cost function for path planning, which is used to evaluate the planning effect, are shown in

Table 4. Parameters of comprehensive cost function.

Name	Symbol [Unit]	Value
Weight coefficient of $J_{\mathrm{obs}}$	${\omega }_{\mathrm{obs}}$ [−]	0.35
Weight coefficient of $J_{\mathrm{dep}}$	${\omega }_{\mathrm{dep}}$ [−]	0.35
Weight coefficient of $J_{\mathrm{len}}$	${\omega }_{\mathrm{len}}$ [−]	0.2
Weight coefficient of $J_{\mathrm{time}}$	${\omega }_{\mathrm{time}}$ [−]	0.1
Critical collision distance	${\rho }_{\mathrm{crit}}$ [m]	2
Collision risk distance	${\rho }_{\mathrm{risk}}$ [m]	12
Risk depth	$d_{\mathrm{risk}}$ [m]	12
Maximum allowable planning time	$t_{\max }$ [s]	1

.

The mean values and standard deviations of the cost function under different values of ${\rho }_{exp0}$ are shown in

Table 5. Mean values and standard deviations of the cost function under different values of ${\rho }_{\exp 0}$.

${\mathit{\rho }}_{\mathbf{exp}\mathbf{0}}$ [m]	${\mathit{J}}_{\mathbf{obs}}$	${\mathit{J}}_{\mathbf{dep}}$	${\mathit{J}}_{\mathbf{len}}$	${\mathit{J}}_{\mathbf{time}}$	J
5	0.384 ± 0.014	0.302 ± 0.012	0.564 ± 0.008	0.268 ± 0.015	0.380 ± 0.007
8	0.288 ± 0.017	0.272 ± 0.015	0.571 ± 0.007	0.280 ± 0.019	0.338 ± 0.008
11	0.095 ± 0.018	0.190 ± 0.014	0.582 ± 0.009	0.298 ± 0.021	0.246 ± 0.008
14	0.000 ± 0.000	0.033 ± 0.015	0.593 ± 0.007	0.302 ± 0.013	0.160 ± 0.006
17	0.000 ± 0.000	0.036 ± 0.017	0.633 ± 0.008	0.304 ± 0.018	0.170 ± 0.006
20	0.000 ± 0.000	0.045 ± 0.019	0.643 ± 0.011	0.306 ± 0.019	0.175 ± 0.007

, and the variation curves of the mean values of each cost function with ${\rho }_{exp0}$ are presented in Figure 20. Under the improved D* Lite algorithm, as ${\rho }_{exp0}$ increases, the collision risk function $J_{\mathrm{obs}}$ and the depth risk function $J_{\mathrm{dep}}$ decrease significantly, which indicates the improvement in safety level. Meanwhile, the voyage distance cost function $J_{\mathrm{len}}$ increases gradually, and the planning time cost function $J_{\mathrm{time}}$ increases slowly and then stabilizes. When ${\rho }_{exp0}=14 \mathrm{m}$, the comprehensive cost function J reaches the minimum value, which means that the comprehensive performance of global path planning is optimal. For all subsequent simulations, ${\rho }_{exp0}$ is set to 14 m. The relative errors of the comprehensive cost function in the 10 repeated experiments are all less than 5%, indicating that the results of each experiment are basically stable.

To validate the effectiveness of the improved D* Lite algorithm, a comparative experiment on global path planning was conducted between the traditional D* Lite algorithm and the improved D* Lite algorithm under static known obstacle conditions. Figure 21 shows the planned paths (blue dashed lines) and actual trajectories (red solid lines), where Figure 21a corresponds to the traditional D* Lite algorithm and Figure 21b corresponds to the improved D* Lite algorithm. Figure 22 displays the corresponding 3D planned paths and trajectories for the experiment.

As shown in Figure 21 and Figure 22, the designed global path planning node can plan a 3D global path based on the voxel map M. Although the path planned by the traditional D* Lite algorithm avoids the obstacle terrain, it remains excessively close to the obstacles, resulting in a collision for the RexROV. In contrast, the path planned by the improved D* Lite algorithm maintains a safe distance from the obstacle terrain, allowing the RexROV to successfully track the global path and reach the goal point.

4.2. Local Obstacle Avoidance Path Planning Experiment

Considering static/dynamic obstacles in an unknown environment, local path planning experiments were conducted. To verify the obstacle avoidance effectiveness of the 3D obstacle avoidance strategy and the improved APF algorithm, dynamic obstacle avoidance experiments, obstacle avoidance experiments involving local minima points, and obstacle avoidance experiments near the goal point were carried out respectively.

4.2.1. Dynamic Obstacle Avoidance Experiment

The RexROV tracked a given route. Two obstacles, ObsROV1 and ObsROV2, were set to move within the horizontal and vertical planes respectively, with their trajectories intersecting the given route. The local_planner node and the UUV simulation platform were launched to conduct 10 independent and repeated local path planning experiments, with the algorithm parameters set as shown in

Table 6. Parameters of local path planning algorithm.

Classification	Name	Symbol [Unit]	Value
Improved APF	Repulsive coefficient	$k_{\mathrm{rep}}$ [$\mathrm{J}·{\mathrm{m}}^2$]	16
	Attractive coefficient	$k_{\mathrm{att}}$ [$\mathrm{J}·{\mathrm{m}}^{-2}$]	0.5
	Coefficient of virtual force	$k_{\mathrm{att}}^{\prime }$ [$\mathrm{J}·{\mathrm{m}}^{-2}$]	0.25
	Distance of virtual goal point	L [m]	15
	Horizontal angle of virtual goal point	${\alpha }_{\mathrm{h}}$ [°]	30
	Vertical angle of virtual goal point	${\alpha }_{\mathrm{v}}$ [°]	25
	Repulsive force threshold	${\rho }_{\exp 0}$ [m]	12
	Max pitch angle correction	$\mathsf{\Delta }{\theta }_{\mathrm{c}\max }$ [°]	15
	Max yaw angle correction	$\mathsf{\Delta }{\psi }_{\mathrm{c}\max }$ [°]	40
Local minimum detection	Detection period	T [s]	2
	Detection distance threshold	${\rho }_{\det 0}$ [m]	3
3D obstacle avoidance strategy	Horizontal thresholds	$y_0$ [m]	7
	Vertical thresholds	$z_0$ [m]	5

. The results of one of these experiments are presented below. Figure 23a–c show the visualization effects when the RexROV encounters ObsROV1, encounters ObsROV2, and reaches the goal point, respectively. In the figures, the blue dashed line represents the given route, the red solid line represents the actual navigation trajectory of the RexROV, and the black solid lines represent the motion trajectories of ObsROV1 and ObsROV2. Figure 23d displays both the given route and the actual trajectory. Figure 24 shows the curves of speed and pitch angle changes during vertical obstacle avoidance. Figure 25 shows the curves of speed and yaw angle changes during horizontal obstacle avoidance.

As shown in Figure 23a,b, when the RexROV encounters ObsROV1, since $y_{\mathrm{obs}1}>y_0$ and $z_{\mathrm{obs}1}<z_0$, the system enters vertical plane obstacle avoidance mode according to the obstacle avoidance strategy. From Figure 24, it can be seen that the APF algorithm calculates the speed and pitch angle commands, with the pitch angle variation kept within the range of [−15°, 15°], meeting the kinematic constraints. Ultimately, the RexROV successfully avoids the obstacle.

As shown in Figure 23b,c, when the RexROV encounters ObsROV2, since $y_{\mathrm{obs}2}<y_0$ and $z_{\mathrm{obs}2}<z_0$, the system enters horizontal plane obstacle avoidance mode according to the obstacle avoidance strategy. From Figure 25, it can be seen that the APF algorithm calculates the speed and yaw angle commands, ultimately enabling the RexROV to successfully avoid the obstacle.

Ten independent and repeated dynamic obstacle avoidance experiments show that the improved APF algorithm combined with the 3D obstacle avoidance strategy achieves a 100% success rate of obstacle avoidance.

Next, the sensitivity of the parameters of the improved APF algorithm, including those related to the APF algorithm and virtual target points, is analyzed. The method is to perturb each parameter value separately based on the set values in Table 6. This section analyzes the sensitivity of the APF algorithm parameters $k_{\mathrm{att}}$, $k_{\mathrm{rep}}$ and ${\rho }_{\mathrm{rep}0}$. From the perspective of ensuring the obstacle avoidance success rate, the range of positive perturbation of $k_{\mathrm{att}}$ and the ranges of negative perturbation of $k_{\mathrm{rep}}$ and ${\rho }_{\mathrm{rep}0}$ should be restricted; from the perspective of improving navigation efficiency, the ranges of reverse perturbation of the above parameters should be restricted. For the perturbation value of each parameter, 10 independent and repeated dynamic obstacle avoidance experiments are conducted, and the parameter ranges with a 100% obstacle avoidance success rate are obtained as follows: the positive perturbation of $k_{\mathrm{att}}$ is less than 20%, and the negative perturbations of $k_{\mathrm{rep}}$ and ${\rho }_{\mathrm{rep}0}$ are less than 18% and 33%, respectively. If the perturbation of each parameter exceeds the above ranges, collisions are likely to occur due to insufficient steering or insufficient obstacle avoidance distance. If the negative perturbation of $k_{\mathrm{att}}$ is greater than 40%, the attractive force toward the target will be too small, resulting in a 50% reduction in speed. If the positive perturbation of $k_{\mathrm{rep}}$ is greater than 25%, the UUV will be excessively sensitive to changes in the distance to obstacles, leading to oscillation of the resultant force and ultimately causing oscillations in speed and attitude. The corresponding parameter values in Table 6 are the optimal values obtained based on multiple experiments, and this parameter combination can ensure the stability of the results of the improved APF algorithm within the above perturbation ranges.

4.2.2. Obstacle Avoidance Experiment with Local Minimum Point

To verify the obstacle avoidance effectiveness of the algorithm when trapped in a local minimum point, an unknown static obstacle (8 m × 8 m × 8 m) was placed along the given path, as shown in Figure 26. A comparison of the obstacle avoidance paths is presented in Figure 27.

To verify the obstacle avoidance performance of the algorithm when trapped in local minima, 10 independent and repeated obstacle avoidance experiments were conducted. An unknown static obstacle (8 m × 8 m × 8 m) was set on the given path, with its position randomly offset within 1 m of the preset position, as shown in Figure 26. The comparison of obstacle avoidance paths from one typical experiment is presented in Figure 27. When navigating to the point where attractive and repulsive forces are balanced (i.e., the local minimum point), under the traditional APF algorithm, the RexROV oscillates repeatedly near the local minimum area and fails to reach the goal point. To escape the local minimum point in this scenario, the global path planning node would need to be restarted, significantly increasing computational load and affecting navigation efficiency. In contrast, the improved APF algorithm guides the RexROV to promptly escape the local minimum point by introducing a virtual goal point, allowing it to return to the original given path and successfully reach the goal point. In the 10 independent and repeated obstacle avoidance experiments, the obstacle avoidance success rates of the traditional APF algorithm and the improved APF algorithm were 20% and 100%, respectively.

Next, the sensitivity of the improved APF algorithm with respect to the virtual target point-related parameters $k_{\mathrm{att}}^{\prime }$, L, ${\alpha }_{\mathrm{h}}$ and ${\alpha }_{\mathrm{v}}$ was analyzed. Based on the set values in Table 6, perturbations were applied to each of the four parameters separately. Ten independent and repeated experiments were carried out for the perturbed value of each parameter, and the parameter ranges corresponding to a 100% obstacle avoidance success rate were obtained as follows: the positive perturbation of $k_{\mathrm{att}}^{\prime }$ was less than 40%, the bidirectional perturbation of L was less than 26%, and the negative perturbations of ${\alpha }_{\mathrm{h}}$ and ${\alpha }_{\mathrm{v}}$ were less than 33% and 20%, respectively. If the perturbation of each parameter exceeded the above ranges, obstacle avoidance failure would easily occur due to insufficient obstacle avoidance distance or inability to escape from local minima. If the negative perturbation of $k_{\mathrm{att}}^{\prime }$ exceeded 40%, a 50% speed reduction would be induced. The corresponding parameter values in Table 6 were the optimal values obtained from multiple experiments, and this parameter combination could ensure the reliability and stability of the experimental results within the above perturbation ranges.

4.2.3. Obstacle Avoidance Experiment near the Goal Point

To verify the effectiveness of the improved APF algorithm in addressing the target unreachable problem, 10 independent and repeated obstacle avoidance experiments were conducted. A static obstacle was placed near the end point of the given route, with a distance of 5 m between them, and the obstacle was randomly offset within 1 m of the preset position, as shown in Figure 28. The comparison of obstacle avoidance paths under the two algorithms in one experiment is presented in Figure 29. Under the traditional APF algorithm, the UUV oscillates repeatedly near the goal point and fails to reach it. The improved APF algorithm addresses this by modifying the repulsive force calculation formula, which reduces the repulsive force near the goal point and successfully guides the UUV to the goal point. In the 10 independent and repeated obstacle avoidance experiments, the success rates of reaching the end point for the traditional APF algorithm and the improved APF algorithm were 0% and 100%, respectively.

4.3. Comprehensive Obstacle Avoidance Experiment

To comprehensively verify the performance of the improved D* Lite-APF obstacle avoidance path planning algorithm and the corresponding software, 10 independent and repeated comprehensive obstacle avoidance comparison experiments were conducted among the proposed algorithm, the traditional D* Lite-APF algorithm and the A*-APF algorithm. As shown in Figure 30, an unmapped shipwreck was set as a static obstacle, and the trajectory of a dynamic obstacle, ObsROV, intersected with the globally planned path of RexROV. The algorithm parameters are set according to Table 3 and Table 6. The results of one typical experiment are presented below. Figure 31 shows the planned path (blue dashed line), the replanned path (yellow dashed line), the actual trajectory of RexROV (red solid line) and the motion trajectory of ObsROV (black solid line), where Figure 31a corresponds to the A*-APF algorithm, Figure 31b to the traditional D* Lite-APF algorithm, and Figure 31c to the improved D* Lite-APF algorithm. The 3D paths and trajectories of the comprehensive obstacle avoidance experiments under the three algorithms are shown in Figure 32. The mean values and standard deviations of each cost function are presented in

Table 7. Mean values and standard deviations of each cost function.

Algorithm Type	${\mathit{J}}_{\mathbf{obs}}$	${\mathit{J}}_{\mathbf{dep}}$	${\mathit{J}}_{\mathbf{len}}$	${\mathit{J}}_{\mathbf{time}}$	J
A*-APF	0.668 ± 0.031	0.245 ± 0.025	0.668 ± 0.024	0.731 ± 0.017	0.526 ± 0.015
Traditional D* Lite-APF	0.637 ± 0.035	0.290 ± 0.030	0.635 ± 0.023	0.461 ± 0.011	0.498 ± 0.017
Improved D* Lite-APF	0.297 ± 0.032	0.024 ± 0.028	0.604 ± 0.024	0.302 ± 0.014	0.263 ± 0.016

.

All three algorithms can guide the RexROV to reach the end point collision-free. The path planning performance of the A*-APF algorithm is shown in Figure 31a, and its main shortcomings are as follows: although the A*-APF algorithm can plan a global path to avoid the known obstacle terrain, it lacks the incremental replanning capability of the A* algorithm; when the RexROV encounters an unmodeled static obstacle, the algorithm has to re-invoke the A* algorithm to perform a full global replanning, which results in a significantly high planning time cost, as indicated by $J_{\mathrm{time}}$ in Table 7.

The path planning performance of the traditional D* Lite-APF algorithm is shown in Figure 31b. Compared with the improved D* Lite-APF algorithm, its main disadvantages are: the planned global path is too close to the obstacle terrain, leading to a remarkably high risk level (as indicated by the comparative values of $J_{\mathrm{obs}}$ and $J_{\mathrm{dep}}$ in Table 7); when encountering static obstacles, it falls into a local minimum during obstacle avoidance and can only escape after the global planning node is activated for replanning, resulting in a large navigation distance and planning time consumption (as indicated by the comparative values of $J_{\mathrm{len}}$ and $J_{\mathrm{time}}$ in Table 7).

In contrast, the improved D* Lite-APF algorithm exhibits the optimal comprehensive performance, and its path planning effect is shown in Figure 31c. The improved D* Lite algorithm plans a global path that maintains a certain safe distance from the obstacle terrain, significantly reducing collision and depth risks. When the RexROV encounters an unmodeled static obstacle, it successfully performs horizontal obstacle avoidance based on the improved APF algorithm and the 3D obstacle avoidance strategy, and continues to track the remaining global path after obstacle avoidance. When encountering dynamic obstacles, the algorithm can also respond quickly, complete obstacle avoidance successfully, and continue to track the global path to reach the end point.

In summary, in complex underwater 3D obstacle avoidance tasks, the comprehensive cost function value of the improved D* Lite-APF algorithm is reduced by 50% and 47.2% compared with the A*-APF algorithm and the traditional D* Lite-APF algorithm, respectively, indicating better comprehensive obstacle avoidance performance.

In addition, during the HIL simulation experiments, under all the scenarios set in this paper, the CPU usage of the Orange Pi 5B did not exceed 30%, and the memory usage did not exceed 60%. All kinds of computing and communication tasks were completed correctly and efficiently, with stable and reliable operation.

5. Discussion and Limitations

Although the improved D* Lite-APF algorithm and the corresponding software and hardware systems proposed in this study demonstrate favorable comprehensive obstacle avoidance performance in simulation experiments, they still present several limitations:

(1)

Neither the algorithm nor the simulation environment takes into account dynamic disturbances in the marine environment, especially the influence of ocean currents. Ocean currents can significantly alter the position and energy consumption of UUVs: sailing downstream can greatly reduce propulsion energy consumption, whereas upstream or cross-stream sailing requires extra energy to overcome resistance and maintain the trajectory, and may even render the planned path untrackable. Neglecting ocean current effects restricts the path optimality to disturbance-free environments, creating a discrepancy with practical applications.

(2)

The algorithm performance is sensitive to parameters including inflation layer thickness, attractive/repulsive force coefficients, and virtual target point position. Parameter tuning relies on experience and prior scene knowledge, which may lead to degraded obstacle avoidance performance when insufficient calibration is conducted or the environment changes drastically.

(3)

Gaps still exist between HIL simulation results and actual lake or sea trial results. Although the HIL simulation platform integrates high-fidelity sensors and dynamic models, it cannot fully replicate the full complexity of the real marine environment. The real underwater environment involves higher uncertainties: for instance, acoustic sensors are vulnerable to reverberation and noise, causing false alarms or missed detections; underwater positioning suffers from errors; and environmental disturbances such as ocean currents are present, all of which may degrade safety. Therefore, HIL simulation experiments can only validate the obstacle avoidance performance of the algorithm and software under disturbance-free conditions.

To further improve the performance and practicality of the algorithm, the main work to be carried out in the future includes:

(1)

UUV path planning considering ocean current disturbances: Rational utilization of ocean currents can effectively reduce the energy consumption of UUVs. Cui et al. [30] considered the influence of ocean currents on AUV energy consumption, and introduced an energy consumption cost based on current direction into the cost function of the D* algorithm, enabling the AUV to actively avoid upstream and cross-stream currents in global path planning and achieve energy-efficient navigation. Sun et al. [31] proposed the QuatAPF method, which guides the UUV to sail downstream by introducing an ocean current potential field, thus significantly reducing energy consumption. Therefore, in future work, an energy consumption cost will be introduced into the cost function of the improved D* Lite algorithm, and an ocean current potential field term will be added to the improved APF algorithm so that the UUV can take the influence of ocean currents into account in both global path planning and real-time obstacle avoidance, achieving safe and energy-efficient path planning.

(2)

Adaptive Parameter Optimization: Intelligent optimization algorithms demonstrate favorable performance in the automatic optimization of algorithm parameters. El Moutaouakil et al. [32] utilized a genetic algorithm to automatically optimize the key parameters of Fuzzy-C-Means SMOTE, with entropy minimization as the core objective to suppress noise generation during the oversampling process, thereby significantly improving the generalization ability of classifiers on imbalanced data. For the iterative solution process of the fractional-order continuous Hopfield network (FRAC-CHN), El Moutaouakil et al. [33] proposed an adaptive time step selection method based on the analytical solution of the gradient information of the energy function, achieving the dynamic optimal configuration of iterative step sizes. Therefore, in future work, intelligent optimization algorithms such as genetic algorithms, particle swarm optimization, or reinforcement learning will be introduced to perform online or offline optimization on the sensitive parameters in the obstacle avoidance algorithm, so as to reduce the dependence on manual parameter tuning and enhance the adaptive ability.

(3)

Practical UUV system integration and verification by lake or sea trials: Engineering implementation and verification from HIL simulation to the actual UUV system will be carried out, including the development of software and hardware interfaces, system integration, onboard debugging, and lake or sea trials, so as to verify the practicality and engineering feasibility of the algorithm.

6. Conclusions

This paper designed an improved D* Lite-APF algorithm that includes an improved D* Lite global path planning algorithm and a local path planning algorithm integrating a 3D obstacle avoidance strategy with an improved APF algorithm. A comprehensive cost function for evaluating global and local path planning capabilities was also proposed. Based on this improved algorithm, UUV 3D obstacle avoidance path planning software was developed and deployed on an Orange Pi 5B. By interfacing with the UUV simulation platform, three types of HIL simulation experiments were conducted: global obstacle avoidance path planning, local obstacle avoidance path planning, and comprehensive obstacle avoidance path planning. These experiments verified that the improved D* Lite-APF algorithm offers better comprehensive obstacle avoidance performance compared to the traditional algorithm. This is specifically manifested in: (1) the improved D* Lite algorithm, by considering depth constraints and introducing an obstacle inflation layer, achieves lower collision risk and depth risk; (2) the improved APF algorithm avoids the problem of easily falling into local minima by introducing a virtual goal point, and prevents the target unreachability problem by modifying the repulsive force calculation formula. Additionally, the path planning software deployed on the hardware platform runs stably and reliably. This study can provide a directly deployable path planning unit for practical UUV autonomous navigation systems. The designed improved D*Lite-APF algorithm balances obstacle avoidance safety and real-time performance. The developed software has been adapted to the embedded hardware platform (Orange Pi 5B) and verified through HIL simulation, laying a technical foundation for transplantation to practical UUV development.