Enhancing Path Planning Efficiency for Underwater Gravity Matching Navigation with a Novel Three-Dimensional Along-Path Obstacle Profiling Algorithm

Abstract

This paper presents a study on enhancing the efficiency of underwater gravity matching navigation path planning in a three-dimensional environment. Firstly, to address the challenges of the computational complexity and prolonged calculation times associated with the existing three-dimensional path planning algorithms, a novel Three-Dimensional Along-Path Obstacle Profiling (TAOP) algorithm is introduced. The principles of the TAOP algorithm are as follows: (1) unfolding obstacles along the path using the path obtained from two-dimensional planning as an axis, interpolating water depth values based on downloaded terrain data, and subjecting obstacles to dilation treatment to construct a dilated obstacle profile for path segments; (2) conducting height direction course planning and a secondary optimization of the path based on the profile contours of the dilated obstacles; and (3) integrating height planning with the path points from two-dimensional planar planning to obtain a complete path containing all turning points in the three-dimensional space. Secondly, gravity anomaly data are utilized to delineate gravity suitability areas, and a three-dimensional planning environment that is suitable for underwater gravity matching navigation is established by integrating seafloor terrain data. Under identical planning environments and parameter conditions, the performance of the TAOP algorithm is compared to that of the RRT* algorithm, Q-RRT* algorithm, and Depth Sorting Fast Search (DSFS) algorithm. The results show that, compared to the RRT* algorithm, Q-RRT* algorithm, and DSFS algorithm, the TAOP algorithm achieves efficiency improvements of 15.6 times, 5.98 times, and 4.04 times, respectively.

1. Introduction

Currently, inertial navigation systems are the primary navigation tools used for underwater vehicles. They do not rely on external information and can provide real-time, continuous, autonomous, and all-weather data such as position, velocity, acceleration, and attitude, meeting the requirements of underwater navigation for concealment and reliability [1,2,3,4]. However, the drift error of inertial devices accumulates over time, leading to a gradual decrease in the navigation accuracy of the inertial navigation system, which cannot meet the accuracy requirements for long-duration missions. Therefore, regular calibration using other navigation methods is necessary [5,6].

Gravity matching navigation utilizes variations in the Earth’s gravity field for positioning and does not require signal transmission and reception. It possesses strong anti-interference capabilities and can meet the requirements of underwater navigation for concealment and positioning reliability, making it one of the most suitable navigation methods for aiding inertial navigation systems [7,8,9]. Gravity matching navigation acquires the gravity-related information of the current position using a gravity sensor. By comparing the real-time measurements with a prestored gravity field reference map and employing suitable matching and solving algorithms, the position of the vehicle is estimated, thus correcting the navigation errors of the inertial navigation system. The positioning accuracy and success rate of gravity matching navigation are closely related to the distribution of gravity field characteristics in the navigation area. The same matching algorithm exhibits different matching effects in regions with different gravity features, and areas with rich gravity features can significantly improve the matching accuracy [10]. Therefore, to ensure the effectiveness of gravity matching navigation, it is necessary to evaluate the suitability of the navigational area based on gravity features and plan the trajectory in accordance with the distribution of suitability, ensuring that the trajectory remains within the suitable region.

Traditional underwater gravity matching navigation path planning algorithms are typically based on two-dimensional maps and cannot effectively adapt to the complex underwater terrain and obstacles [11,12,13]. Researching and applying path planning algorithms for underwater three-dimensional space is of great significance in advancing underwater autonomous navigation technology and enhancing the safety and reliability of underwater devices. Despite significant achievements in two-dimensional path planning algorithms, extending them directly to three-dimensional space is not a straightforward task. Compared to the two-dimensional plane, a three-dimensional space involves more degrees of freedom and a more complex physical environment, which imposes higher requirements on path planning algorithms. For instance, in terms of computational complexity and storage space, three-dimensional path planning algorithms need to overcome greater challenges. Currently, the commonly used three-dimensional path planning algorithms include the artificial potential field method, A* algorithm, ant colony algorithm, and rapidly exploring random tree (RRT) algorithm. The artificial potential field method is a path planning approach that mimics natural potential fields. It constructs an artificial potential field where the target exerts attraction on objects and obstacles exert repulsion, causing objects to move in the direction of the potential field gradient to reach the destination. However, this method can sometimes result in objects becoming trapped in local minima and being unable to reach the target location, thus not guaranteeing a solution [14,15]. The A* algorithm is a heuristic-based path planning method that utilizes cost functions to obtain information about the distance to the goal, directing the search towards finding the path with the minimum cost and shortest distance. However, this method has limited real-time performance, with large computational requirements and long processing times for each node. As the number of nodes increases, the efficiency of the algorithm decreases [16,17]. The ant colony algorithm is a heuristic path planning method that simulates the foraging behavior of ants, treating the target as a food source and obstacles as impassable regions. It finds the optimal path by updating the pheromone concentration. The ant colony algorithm has strong robustness and can be combined with other algorithms. However, its convergence speed is relatively slow in the initial stage, and it is sensitive to the selection of initial solutions [18,19].

The RRT algorithm is a randomized search-based path planning method that constructs a random tree to explore the state space and quickly find a feasible path from the start point to the goal. The algorithm starts with an initial state as the root node and randomly samples a state at each iteration, attempting to connect it with the nearest state in the tree. If the connection is successful (without intersecting obstacles and satisfying other constraints), the new state is added to the tree [20,21]. Traditional RRT algorithms can find feasible paths, but they are not necessarily optimal and have slow convergence speeds and long search times. The RRT* algorithm improves upon this by introducing the process of reselecting the parent node for new nodes within a certain radius and rewiring nearby nodes. This modification enables the RRT* algorithm to exhibit asymptotic optimality, meaning that as the number of iterations increases indefinitely, the found path tends to be optimal. However, the convergence speed of the algorithm is influenced by the size of the bounding hyperball. Increasing the radius leads to a significant increase in the number of nodes within the hyperball, resulting in longer computation times [22,23,24,25]. To improve the search efficiency of the RRT* algorithm, the Q-RRT* algorithm extends the range of parent node selection for new nodes and nearby nodes based on the triangle inequality, which can generate better initial paths and achieve a faster convergence speed [26,27]. RRT-based algorithms exhibit significantly reduced search efficiency when dealing with high-dimensional spaces and complex obstacles, leading to a substantial increase in the required number of iterations and computation time [28,29,30,31,32]. The Depth Sorting Fast Search (DSFS) algorithm enhances the efficiency of the parent node reselection process and rewiring process by utilizing the inequality relationship between ancestor nodes and their descendants. It achieves this by selecting candidate nodes in depth-first order, thereby reducing the number of nodes that require collision detection and effectively saving computation time [33].

Differing from previous research, this paper proposes a novel Three-Dimensional Along-Path Obstacle Profiling (TAOP) algorithm aimed at further enhancing the efficiency of underwater gravity-matching navigation in three-dimensional environments. The TAOP algorithm decomposes three-dimensional path planning into two-dimensional planning and a height planning phases. It can be seamlessly integrated with existing planar path planning algorithms, thereby circumventing the low computational efficiency issue of current path planning methods in three-dimensional scenarios. Additionally, by combining gravity anomaly data and seafloor terrain data, a three-dimensional planning environment that is suitable for underwater gravity-matching navigation is established, and performance simulation tests for route planning are conducted within this environment. Through a comparison of the RRT*, Q-RRT*, and DSFS algorithms, various factors including the initial path quality and computation time are evaluated. This validation demonstrates that the TAOP algorithm is capable of achieving rational path planning for underwater gravity-matching navigation and effectively improving planning efficiency.

2. Three-Dimensional Along-Path Obstacle Profiling Algorithm

When performing three-dimensional path planning for underwater gravity-matching navigation, it is necessary to consider both gravity suitability, to ensure the normal operation of the navigation system, and seamount constraints, to ensure safe navigation. When RRT-based algorithms directly sample in three-dimensional space, the collision detection of the edges of the random tree requires both being in a gravity suitability grid and maintaining a safe distance from the seamount in the height direction, which places high demands on the nodes. The number of invalid nodes sampled in a three-dimensional space far exceeds that in the plane, which leads to a very high failure rate of collision detection, an excessive computational load, and a slow convergence of the planning algorithm. The calculation time is too long, and even feasible solutions cannot be found. Moreover, when the sampling range covers the entire three-dimensional planning space, the effective nodes are dispersed, resulting in large path volatility and a high path cost. In addition, each time collision detection is performed, it is necessary to interpolate the water depth at each detection point on the path to ensure a safe distance from obstacles. However, interpolation in large-scale terrain data is a time-consuming computational step, which greatly affects the computation time.

To address these issues, this paper proposes the Three-Dimensional Along-Path Obstacle Profiling (TAOP) algorithm, which transforms three-dimensional path planning into two separate processes: two-dimensional planar planning and height planning. First, based on the gravity suitability distribution map, a two-dimensional planning algorithm is employed to plan the planar path points. Next, obstacles along the path are unfolded along the planar path axis, and interpolation is used to generate a seamount obstacle profile graph with the distance from the starting point along the path as the horizontal axis and the elevation information as the vertical axis. Finally, by utilizing the obstacle profile information, the height of the path points is planned to ensure safety in the height direction. The TAOP algorithm separates height planning from planar planning, and gravity suitability actually only acts on the planar directions of the latitude and longitude, where planar planning is directly related to gravity suitability. Using planar planning to sample in a two-dimensional plane can reduce the number of invalid sampling nodes significantly, and it does not require the consideration of the impact of seamounts on the height, which greatly reduces the amount of collision detection calculations. The interpolation operation is only performed once in the preprocessing stage to generate the profile of the obstacle, effectively shortening the calculation time. The TAOP algorithm unfolds the profile of the obstacle along the path after planar planning, and it generates a three-dimensional path based on the profile contour according to height planning, which can be used in combination with most two-dimensional planning algorithms.

The TAOP algorithm mainly includes four steps, with the basic procedure shown in Algorithm 1 and the schematic diagram shown in Figure 1: planar path point data preprocessing ($\mathrm{Preprocess}-\mathrm{TAOP}$), path coarse planning ($\mathrm{CoarsePlan}-\mathrm{TAOP}$), path secondary optimization ($\mathrm{Optimize}-\mathrm{TAOP}$), and path point integration ($\mathrm{Integrate}-\mathrm{TAOP}$).

Algorithm 1 $\mathrm{TAOP}\left(P_{\mathrm{pre}},d_{\mathrm{obs}},d_{\mathrm{delta}}\right)$

$P_{\mathrm{d}}\leftarrow \mathrm{Preprocess}-\mathrm{TAOP}\left(P_{\mathrm{pre}},d_{\mathrm{obs}},d_{\mathrm{delta}}\right)$; $P_{\mathrm{r}}\leftarrow \mathrm{CoarsePlan}-\mathrm{TAOP}\left(P_{\mathrm{d}}\right)$; $P_{\mathrm{opt}}\leftarrow \mathrm{Optimize}-\mathrm{TAOP}\left(P_{\mathrm{r}}\right)$; $P_{\mathrm{mer}}\leftarrow \mathrm{Integrate}-\mathrm{TAOP}\left(P_{\mathrm{pre}},P_{\mathrm{opt}}\right)$; return $P_{\mathrm{mer}}$;

Explanation of parameters in Algorithm 1:

• $P_{\mathrm{pre}}$: The set of planar path points;
• $d_{\mathrm{obs}}$: The safe distance from terrain obstacles;
• $d_{\mathrm{delta}}$: The interval distance when generating the obstacle profile;
• $P_{\mathrm{d}}$: The set of location points to be verified;
• $P_{\mathrm{r}}$: The set of coarse planned path points;
• $P_{\mathrm{opt}}$: The set of secondary optimization path points;
• $P_{\mathrm{mer}}$: The set of integrated path points.

The first step is planar path point data preprocessing. After obtaining the planar polyline path from two-dimensional planning, the TAOP algorithm performs a ‘straightening’ operation on the path. ‘Straightening’ is equivalent to coordinate transformation. The points on the planar path are no longer represented by the latitude and longitude or the coordinates in the two-dimensional planning space, but by the two-dimensional path distance $s$ (not Euclidean distance) between them and the starting point of the path. Specifically, let $P_{\mathrm{pre}}\left(p_{\mathrm{pre}}^1,p_{\mathrm{pre}}^2,\cdots ,p_{\mathrm{pre}}^m\right)$ be the set of planar path points generated by the two-dimensional planning algorithm, let $m$ be the total number of path points obtained via two-dimensional planning, and let each point $p_{\mathrm{pre}}$ contain position information $\left(x,y\right)$ in the two-dimensional space. In each vector segment formed by adjacent path points (i.e., $\overrightarrow{p_{\mathrm{pre}}^1{p}_{\mathrm{pre}}^2}$,$\overrightarrow{p_{\mathrm{pre}}^2{p}_{\mathrm{pre}}^3}$,…,$\overrightarrow{p_{\mathrm{pre}}^{m-1}{p}_{\mathrm{pre}}^m}$), divide the vector into equal intervals of given interval distance $d_{\mathrm{delta}}$ and uniformly insert interval points. Calculate the positions of each interval point, as well as its distance $s$ from the starting point $p_{\mathrm{init}}$, in the two-dimensional plane. Include the $\left(x,y,s\right)$ information for each point $p_{\mathrm{e}}$ and add it to the interval point set $P_{\mathrm{e}}$. Then, unfold the obstacles along the path onto a plane, which is equivalent to ‘slicing’ the obstacles in a three-dimensional space. To obtain the height information of the obstacles along the path, obtain the depth $h_0$ at the location corresponding to each interval point by interpolating the downloaded terrain data based on the latitude and longitude. Expand the information of point $p_{\mathrm{e}}$ to $\left(x,y,s,h_0\right)$. Then, set the distance of each point on the ‘straightened’ path from the starting point as the horizontal axis, and set the depth value of the path points as the vertical axis, as shown in Figure 2. Point A and point B represent the starting point and target point of the path planning, respectively. The solid line contour in the figure represents the profiles of obstacles along a path generated via two-dimensional planning.

To ensure the safety of navigation, the planned path needs to maintain a certain safe distance $d_{\mathrm{obs}}$ from the obstacles. Therefore, the obstacle profile is dilated. The original depth $h_0$ of each interval point is extended by a distance $d_{\mathrm{obs}}$ along the outer normal direction of the profile edge to obtain the dilated depth $h$, and the information for the point $p_{\mathrm{e}}$ is updated to $\left(x,y,s,h\right)$. The dashed line contour in Figure 2 represents the dilated profiles of the obstacles.

In the subsequent coarse planning phase, the TAOP algorithm will continuously adjust the height of the path by comparing the obstacle’s dilation depth at interval points with the height of the current planned path at that location. Therefore, the smaller value between the starting point depth and the target point depth is set as $h_{\min }$, and the intervals with a puffed depth value of less than $h_{\min }$ in the interval point set $P_{\mathrm{e}}$ are deleted, as they do not affect the path direction. The remaining interval points are sorted in descending order based on their corresponding puffed water depth values, and these interval points are used as the set of points $P_{\mathrm{d}}$ for verification. The pseudocode for the preprocessing process of the planar path point data is described in detail in Algorithm 2, and the schematic diagram is shown in Figure 3.

Algorithm 2 $\mathrm{Preprocess}-\mathrm{TAOP}\left(P_{\mathrm{pre}},d_{\mathrm{obs}},d_{\mathrm{delta}}\right)$

$P_{\mathrm{e}}\leftarrow \varnothing$; $P_{\mathrm{d}}\leftarrow \varnothing$; $d_{\mathrm{toi}}\leftarrow 0$; for $i=1$ to $m-1$ do $P_{\mathrm{e}}^i\leftarrow \mathrm{Segment}\left(p_{\mathrm{pre}}^i,p_{\mathrm{pre}}^{i+1},d_{\mathrm{delta}}\right)$; $P_{\mathrm{e}}^i.s\leftarrow P_{\mathrm{e}}^i.s+d_{\mathrm{toi}}$; $P_{\mathrm{e}}\leftarrow P_{\mathrm{e}}\cup P_{\mathrm{e}}^i$; $d_{\mathrm{toi}}\leftarrow d_{\mathrm{toi}}+d\left(p_{\mathrm{pre}}^i,p_{\mathrm{pre}}^{i+1}\right)$; end for $P_{\mathrm{e}}.h\leftarrow \mathrm{Gridalt}\left(P_{\mathrm{e}},Ar{r}_{\mathrm{terrain}}\right)$; $P_{\mathrm{e}}.h\leftarrow P_{\mathrm{e}}.h+d_{\mathrm{obs}}$; for all $p_{\mathrm{e}}\in P_{\mathrm{e}}$ do       if $p_{\mathrm{e}}.h\ge \min \left(p_{\mathrm{init}}.h,p_{\mathrm{goal}}.h\right)$ then $P_{\mathrm{d}}\leftarrow P_{\mathrm{d}}\cup \left\{p_e\right\}$;       end if end for return $P_{\mathrm{d}}$;

The second step is the coarse planning of the path. After establishing a profile map of the obstacles along the path segment, connect the path starting point $p_{\mathrm{init}}$ with the target point $p_{\mathrm{goal}}$ as the highly planned initial path, and initialize the planned path point set $P_{\mathrm{r}}$ to $\left\{p_{\mathrm{init}},p_{\mathrm{goal}}\right\}$. For each position point $p_{\mathrm{d}}$ in the set $P_{\mathrm{d}}$ to be verified, locate the minimum interval $\left(p_{\mathrm{r}}^{i-1},p_{\mathrm{r}}^i\right)$ of the planned path based on its two-dimensional path distance $s$ from the path starting point. Here, $i-1$ and $i$ are, respectively, the serial numbers of the front and rear ends of the path segment. Based on the two-dimensional path distance and the height between the path points at both ends of the interval and the starting point, as shown in Figure 4, calculate the height $h_{\mathrm{r}}$ (the height of intersection in Figure 4) of the path at the horizontal coordinate of the position to be verified according to the principle of similar triangles:

$$\frac{p_{\mathrm{r}}^{i-1}.h-h_{\mathrm{r}}}{p_{\mathrm{r}}^{i-1}.h-p_{\mathrm{r}}^i.h}=\frac{p_{\mathrm{d}}.s-p_{\mathrm{r}}^{i-1}.s}{p_{\mathrm{r}}^i.s-p_{\mathrm{r}}^{i-1}.s}$$

(1)

$$h_{\mathrm{r}}=p_{\mathrm{r}}^{i-1}.h+\frac{\left(p_{\mathrm{r}}^i.h-p_{\mathrm{r}}^{i-1}.h\right)\times \left(p_{\mathrm{d}}.s-p_{\mathrm{r}}^{i-1}.s\right)}{p_{\mathrm{r}}^i.s-p_{\mathrm{r}}^{i-1}.s}$$

(2)

where $p_{\mathrm{d}}$ represents the location point to be verified, $p_{\mathrm{r}}^{i-1}$ and $p_{\mathrm{r}}^i$ represent the path points before and after the smallest interval of the planned path that has been located, $s$ and $h$ represent the path distance and water depth from the starting point, respectively, and $h_{\mathrm{r}}$ represents the height of the planned path at the abscissa of the location point to be verified.

Compare the calculated path height $h_{\mathrm{r}}$ with the $p_{\mathrm{d}}.h$ value of the position to be verified. If $h_{\mathrm{r}}<p_{\mathrm{d}}.h$, this indicates that the current planned path passes through obstacles or cannot maintain a safe distance from the obstacles at this position, and the path needs to be corrected. Take $p_{\mathrm{d}}$ as a new path point and insert it into the middle of the front and rear end points of the located minimum interval, $p_{\mathrm{r}}^{i-1}$ and $p_{\mathrm{r}}^i$. The $p_{\mathrm{d}}.h$ value at this position is the new path height, as shown in Figure 5.

If $h_{\mathrm{r}}\ge p_{\mathrm{d}}.h$, this indicates that the current planned path can maintain a safe distance from the obstacles at this position and no path correction is necessary. The pseudo code of the path coarse planning process is shown in Algorithm 3.

Algorithm 3 $\mathrm{CoarsePlan}-\mathrm{TAOP}\left(P_{\mathrm{d}}\right)$

$P_{\mathrm{r}}\leftarrow \left\{p_{\mathrm{init}},p_{\mathrm{goal}}\right\}$; for all $p_{\mathrm{d}}\in P_{\mathrm{d}}$do       for $i=\mathrm{length}\left(P_{\mathrm{r}}\right)$ to $2$ ($step=-1$) do               if $p_{\mathrm{d}}.s<p_{\mathrm{r}}^i.s$ then $h_{\mathrm{r}}\leftarrow p_{\mathrm{r}}^{i-1}.h+\left(p_{\mathrm{r}}^i.h-p_{\mathrm{r}}^{i-1}.h\right)\times \left(p_{\mathrm{d}}.s-p_{\mathrm{r}}^{i-1}.s\right)/\left(p_{\mathrm{r}}^i.s-p_{\mathrm{r}}^{i-1}.s\right)$;                        if $h_{\mathrm{r}}<p_{\mathrm{d}}.h$ then $P_{\mathrm{r}}\leftarrow \mathrm{Insert}\left(P_{\mathrm{r}},p_{\mathrm{d}},i\right)$;                             break                   end if             end if        end for end for return $P_{\mathrm{r}}$;

The third step is the secondary optimization of the path. After traversing the point set $P_{\mathrm{d}}$ to be verified, a set of path points that can safely pass through obstacles is obtained. However, this path may contain a certain number of redundant points, resulting in an increase in the total length of the path and the number of turns, which affects the efficiency of path planning. The optimization process can be used to identify and remove redundant points in a path. If there are no obstacles on the line between two path points, one of the points can be directly removed, thereby reducing unnecessary points on the path and making the path smoother, simpler, and more efficient.

For each path point $p_{\mathrm{r}}$ in the path point set $P_{\mathrm{r}}$, check whether each point can directly connect with all non-adjacent points behind it. Suppose that the path points passed by the non-adjacent path points, $p_{\mathrm{r}}^i$ and $p_{\mathrm{r}}^j$, are $p_{\mathrm{r}}^k\in P_{\mathrm{r}}$ (where $k=i+1,\cdots ,j-1$), and that the corresponding planning height is $p_{\mathrm{r}}^k.h$. After connecting $p_{\mathrm{r}}^i$ and $p_{\mathrm{r}}^j$, the height $h_{\mathrm{c}}^k$ (where $k=i+1,\cdots ,j-1$) at the intersection point of the intermediate path point’s horizontal coordinate on this line can be calculated according to the similarity triangle principle, as shown in Figure 6. Therefore, the intermediate path point’s horizontal coordinate corresponding to the height $h_{\mathrm{c}}^k$ (the height of intersection in Figure 6) on this line can be obtained according to the similarity triangle principle:

$$\frac{p_{\mathrm{r}}^i.h-h_{\mathrm{c}}^k}{p_{\mathrm{r}}^i.h-p_{\mathrm{r}}^j.h}=\frac{p_{\mathrm{r}}^k.s-p_{\mathrm{r}}^i.s}{p_{\mathrm{r}}^j.s-p_{\mathrm{r}}^i.s}$$

(3)

$$h_{\mathrm{c}}^k=p_{\mathrm{r}}^i.h+\frac{\left(p_{\mathrm{r}}^j.h-p_{\mathrm{r}}^i.h\right)\times \left(p_{\mathrm{r}}^k.s-p_{\mathrm{r}}^i.s\right)}{p_{\mathrm{r}}^j.s-p_{\mathrm{r}}^i.s}$$

(4)

Compare the planning height $p_{\mathrm{r}}^k.h$ of all intermediate path points with the height $h_{\mathrm{c}}^k$ at their horizontal coordinates on the connecting line. If $\forall p_{\mathrm{r}}^k.h\le h_{\mathrm{c}}^k$, this indicates that the path between the path points $p_{\mathrm{r}}^i$ and $p_{\mathrm{r}}^j$ can be optimized, so delete the intermediate path points and directly connect $p_{\mathrm{r}}^i$ and $p_{\mathrm{r}}^j$; if $\exists p_{\mathrm{r}}^k.h>h_{\mathrm{c}}^k$, the path between $p_{\mathrm{r}}^i$ and $p_{\mathrm{r}}^j$ remains unchanged. The optimized path is $P_{\mathrm{opt}}$, and the pseudo code of the secondary optimization is shown in Algorithm 4.

Algorithm 4 $\mathrm{Optimize}-\mathrm{TAOP}\left(P_{\mathrm{r}}\right)$

while $i<\mathrm{length}\left(P_{\mathrm{r}}\right)-1$ do $reconnect\_status\leftarrow \mathrm{false}$;       for $j=2$ to $\mathrm{length}\left(P_{\mathrm{r}}\right)$ do $nu{m}_{\mathrm{pass}}\leftarrow 0$;             for $k=i+1$ to $j-1$ do $h_{\mathrm{c}}^k\leftarrow p_{\mathrm{r}}^i.h+\left(p_{\mathrm{r}}^j.h-p_{\mathrm{r}}^i.h\right)\times \left(p_{\mathrm{r}}^k.s-p_{\mathrm{r}}^i.s\right)/\left(p_{\mathrm{r}}^j.s-p_{\mathrm{r}}^i.s\right)$;                   if $h_{\mathrm{c}}^k<p_{\mathrm{r}}^k.h$ then                         break                   else $nu{m}_{\mathrm{pass}}\leftarrow nu{m}_{\mathrm{pass}}+1$;                   end if             end for             if $nu{m}_{\mathrm{pass}}=j-i-1$ then $P_{\mathrm{r}}\leftarrow P_{\mathrm{r}}\backslash {\left\{p_{\mathrm{r}}^k\right\}}_{k=i+1,\cdots ,j-1}$; $reconnect\_status\leftarrow \mathrm{ture}$;                   break             end if       end for       if $reconnect\_status=\mathrm{false}$ then $i\leftarrow i+1$;       end if end while $P_{\mathrm{opt}}\leftarrow P_{\mathrm{r}}$; return $P_{\mathrm{opt}}$;

The fourth step is path point integration. Due to the fact that the obstacle profile along the route is obtained based on the ‘straightened’ planar planning path points, which is equivalent to the artificial omission of the turning points in the $x-y$ plane, the path point set $P_{\mathrm{opt}}$ after the second optimization only contains the turning points in the $s-h$ plane. It is necessary to integrate the path points from the height planning and the two-dimensional planar planning to obtain a complete path $P_{\mathrm{mer}}$ that includes all of the turning points in the three-dimensional space. The specific implementation of the path point integration algorithm is given in Algorithm 5.

Algorithm 5 $\mathrm{Integrate}-\mathrm{TAOP}\left(P_{\mathrm{pre}},P_{\mathrm{opt}}\right)$

$P_{\mathrm{pre}}.h\leftarrow \mathrm{Interp}(P_{\mathrm{opt}}.s,P_{\mathrm{opt}}.h,P_{\mathrm{pre}}.s)$; $P_{\mathrm{mer}}\leftarrow \mathrm{Sortrows}(P_{\mathrm{pre}},s)$; for all $p_{\mathrm{opt}}\in P_{\mathrm{opt}}$ do       if not $\mathrm{Ismember}\left(p_{\mathrm{opt}}.s,P_{\mathrm{mer}}.s\right)$ then             for $i=2$ to $\mathrm{length}\left(P_{\mathrm{mer}}\right)$ do                   if $p_{\mathrm{opt}}.s<p_{\mathrm{mer}}^i.s$ then $P_{\mathrm{mer}}\leftarrow \mathrm{Insert}\left(P_{\mathrm{mer}},p_{\mathrm{opt}},i\right)$;                         break                   end if             end for       end if end for return $P_{\mathrm{mer}}$;

The TAOP algorithm involves the following function explanations:

$\mathrm{Segment}$: Given two points, $p_{\mathrm{s}}$ and $p_{\mathrm{d}}$, and a distance, $d$, divide the vector $\overrightarrow{p_{\mathrm{s}}{p}_{\mathrm{d}}}$ with equal intervals $d$ starting from $p_{\mathrm{s}}$, calculate the positions $\left(x,y\right)$ of all the interval points and their distances $s$ from $p_{\mathrm{s}}$, and return the set $P\left(x,y,s\right)$ of all the interval points that contain the point $p_{\mathrm{s}}$.

$\mathrm{Gridalt}$: Given a struct array $P$ containing position information $\left(x,y\right)$ and a terrain array $Ar{r}_{\mathrm{terrain}}$ containing latitude, longitude, and altitude information, convert the position coordinates $\left(x,y\right)$ in $P$ into latitude, longitude, and altitude $\left(x_{\mathrm{lon}},y_{\mathrm{lat}}\right)$$\left(x_{\mathrm{lon}},y_{\mathrm{lat}}\right)$, interpolate the latitude and longitude values into the terrain array $Ar{r}_{\mathrm{terrain}}$$Ar{r}_{\mathrm{terrain}}$ to obtain the height values $h$ for all of the coordinates in $P$ and return it.

$\mathrm{length}$: Given an array $P$, return the length of $P$.

$\mathrm{Insert}$: Given a set $P_{\mathrm{r}}$, a point $p_{\mathrm{d}}$, and a positive integer $i$, insert the point $p_{\mathrm{d}}$ into the $i$th position of the set $P_{\mathrm{r}}$.

$\mathrm{Interp}$: Given an array $Ar{r}_x$ containing the values of $x$, an array $Ar{r}_y$ containing the values of $y$, and an array $Ar{r}_x{}^{\prime }$ containing the values of $x$ to be queried, use linear interpolation to calculate and return the corresponding $y$ values for all elements in $Ar{r}_x{}^{\prime }$.

$\mathrm{Sortrows}$: Given a struct array $P$ and a field in it, sort the elements of the array $P$ in ascending order by the given field.

$\mathrm{Ismember}$: Given a number $x$ and an array $Arr$, return a logical value as true if $x$ is present in $Arr$, and return a logical value as false otherwise.

3. Simulation

3.1. Simulation Environment

In this article, a latitude range of 7°N to 10°N and a longitude range of 112°E to 116°E are selected as the navigation space for the underwater vehicle. The source of the gravity anomaly data (version V29.1) and submarine topographic data (version V19.1) is the website of the Scripps Institution of Oceanography at the University of California, San Diego (https://topex.ucsd.edu/, accessed on 7 February 2023), with a resolution of 1′ × 1′. The gravity anomaly data in the area are a maximum of 131.9 mGal, a minimum of −60.3 mGal, an average of 17.1 mGal, and a standard deviation of 26.1 mGal. The maximum depth of the seabed data is 0 m, the minimum is −3408 m, the average is −1769.9 m, and the standard deviation is 659 m. The three-dimensional gravity anomaly and three-dimensional submarine topographic datum maps of the experimental area are shown in Figure 7 and Figure 8.

One prerequisite for gravity matching navigation to work effectively is that the gravity features of the area to be matched need to have good suitability. The characteristic parameters of the gravity field matching area are important factors that affect the positioning accuracy and matching probability. During navigation, gravity matching should be performed in areas with obvious gravity field features. The suitability of the gravity field can be measured using various indicators, and different indicators can reflect different characteristics of the gravity field. By fusing information from multiple gravity feature parameters, a more comprehensive and effective evaluation of the working area of gravity matching navigation can be made. However, the division method of the suitability area is not the focus of this article, so in this article, a single characteristic parameter, the standard deviation of gravity anomaly, is used to divide the suitability area and non-suitability area of gravity matching navigation.

The standard deviation of the gravity field can reflect the degree of dispersion and overall undulation of the gravity anomaly sequence. The larger the standard deviation, the richer the gravity information. The calculation formula for the standard deviation is [34]

$$\delta =\sqrt{\frac{1}{ab-1}{\displaystyle \sum _{i=1}^a{\displaystyle \sum _{j=1}^b{\left[g\left(i,j\right)-\overline{g}\right]}^2}}}$$

(5)

$$\overline{g}=\frac{1}{ab}{\displaystyle \sum _{i=1}^a{\displaystyle \sum _{j=1}^{b}g\left(i,j\right)}}$$

(6)

where $\delta$ represents the standard deviation of gravity anomalies, $a$ and $b$ represent the number of grids in the $x$ and $y$ directions within the moving window, $g\left(i,j\right)$ represents the gravity anomaly at the calculation point $\left(i,j\right)$, and $\overline{g}$ represents the average value of the gravity anomalies. When $g\left(i,j\right)$ is divergent, the value of $\delta$ is large; when $g\left(i,j\right)$ is concentrated, the value of $\delta$ is small. The selection criterion for the suitability area is

$$\delta >{\delta }_0$$

(7)

where ${\delta }_0$ is the threshold for dividing the suitability area and non-suitability area.

Using a moving computational window with a resolution of 10′ × 10′, the local gravity field standard deviation was calculated as described above. After extensive matching experiments and a statistical analysis of the positioning errors and matching success rates with different thresholds, regions with a gravity anomaly standard deviation greater than 5 were found to have better suitability. Therefore, a threshold value of ${\delta }_0=5$ was selected, and a two-dimensional gravity suitability grid was established, as shown in Figure 9. The black portion represents the non-suitability grid, while the white portion represents the suitability grid.

To ensure the safety of navigation, it is usually necessary to avoid dangerous areas such as shoals and reefs when planning a route. Using the seabed topographic data within the planning area, a safe depth of 100 m is set to extract the distribution of dangerous areas, as shown in Figure 10. The dangerous areas in Figure 10 are removed from Figure 9 to obtain the gravity suitability distribution map, as shown in Figure 11. The seabed digital model within the planning area is divided into three regions: a non-suitability area, a dangerous area, and a suitability area. The black portion is the non-suitability area with poor positioning and matching effects; the gray portion is the dangerous area, which is not suitable for navigation; and the white portion is the suitability area, and the planned route can only pass through this area to reach the destination.

3.2. Evaluation Method

This section compares the performance of the TAOP algorithm with those of the RRT* algorithm, Q-RRT* algorithm, and DSFS algorithm for three-dimensional path planning in the environment set forth above, and it evaluates the improvement of the TAOP algorithm in planning efficiency. The $\eta$ symbol represents the path planning efficiency of the algorithm, which is the amount of work completed per unit time by the algorithm. The workload of the path planning algorithm is set as the ratio of the shortest cost $c_{\min }$ to the cost $c$ obtained from the planning, which is called the cost optimization ratio. When the cost generated by the path planning algorithm approaches the shortest cost, the value of the cost optimization ratio approaches 1. Therefore, the calculation formula for path planning efficiency is

$$\eta =\frac{c_{\min }}{ct}$$

(8)

In addition, due to the presence of terrain obstacles in the area studied in this paper, the actual shortest cost is difficult to calculate directly, so the shortest cost $c_{\min }$ is set to the shortest distance between the starting point and the target point when no obstacles are present. The coordinates of the starting point of the path planning are set as (112.7°E, 9.4°N, −300 m), and the coordinates of the target point are set as (115.3°E, 7.9°N, −200 m). According to the Haversine formula, the shortest cost is approximately 3.31 × 10⁵ m.

Three sets of comparative simulation experiments for path planning were conducted: (1) the RRT* algorithm versus the TAOP algorithm; (2) the Q-RRT* algorithm versus the TAOP algorithm; and (3) the DSFS algorithm versus the TAOP algorithm. To ensure the fairness of the experiments, the TAOP algorithm’s planar planning path was generated using the two-dimensional form of the counterpart algorithm’s initial path. In the simulation results, ‘R’ and ‘T_R’ denote the ‘Three-Dimensional RRT* algorithm’ and ‘TAOP algorithm (using two-dimensional RRT* algorithm for planar planning)’, ‘Q’ and ‘T_Q’ denote the ‘Three-Dimensional Q-RRT* algorithm’ and ‘TAOP algorithm (using two-dimensional Q-RRT* algorithm for planar planning)’, and ‘D’ and ‘T_D’ denote the ‘Three-Dimensional DSFS algorithm’ and ‘TAOP algorithm (using two-dimensional DSFS algorithm for planar planning)’. To validate the time-saving effect of the TAOP algorithm in collision detection without the need for depth interpolation, the total collision detection time during the planning process, denoted as ‘$t_{\mathrm{coll}}$’, was recorded. Additionally, the initial cost $c_{\mathrm{init}}$ and the computation time $t_{\mathrm{init}}$ were calculated to evaluate the efficiency of the algorithms in planning the initial path. During testing, each algorithm was independently run 50 times under identical simulation parameters. The specific experimental parameter settings are detailed in

Table 1. Parameter settings for path planning simulation.

Parameter Name	Symbol	Value	Unit
Threshold radius of the target area	$r_{\mathrm{thre}}$	100	m
Maximum number of iterations	$N$	10,000
Extended step size	$\rho$	5000	m
Selection radius for ChooseParent and Rewire procedures	$r$	10,000	m
Maximum depth of ancestors	$n$	2

.

4. Results

Table 2. Statistics of total collision detection time $t_{\mathrm{coll}}$.

Algorithm	Q1	Q2	Q3	Mean	Average Percentage Reduction
R	0.50 s	0.76 s	1.24 s	0.97 s	94.85%
TR	0.04 s	0.04 s	0.05 s	0.05 s
Q	1.94 s	2.79 s	4.66 s	4.07 s	83.78%
TQ	0.44 s	0.55 s	0.72 s	0.66 s
D	1.56 s	2.48 s	3.34 s	2.78 s	80.93%
TD	0.43 s	0.49 s	0.57 s	0.53 s

presents the experimental statistical results of the six algorithms in terms of the total collision detection time $t_{\mathrm{coll}}$. In this table, Q1, Q2, and Q3 represent the first quartile, median, and third quartile of the data, respectively. Figure 12 provides a visual representation of the distribution of the total collision detection time $t_{\mathrm{coll}}$ in the form of a box plot. From the statistical information, it is evident that the TAOP algorithm requires significantly less computation time for collision detection when compared to directly using the RRT*, Q-RRT*, and DSFS algorithms, with average reduction percentages of approximately 94.85%, 83.78%, and 80.93%, respectively. In Figure 12, the data points for the TAOP algorithm consistently appear at lower positions, significantly below their corresponding counterparts in the control algorithms, indicating a faster collision detection process for the TAOP algorithm.

Table 3. Statistics of initial cost $c_{\mathrm{init}}$.

Algorithm	Q1	Q2	Q3	Mean
R	4.38 × 105 m	4.73 × 105 m	4.99 × 105 m	4.66 × 105 m
TR	3.71 × 105 m	3.86 × 105 m	3.99 × 105 m	3.94 × 105 m
Q	3.80 × 105 m	3.89 × 105 m	4.05 × 105 m	3.94 × 105 m
TQ	3.46 × 105 m	3.53 × 105 m	3.71 × 105 m	3.68 × 105 m
D	3.76 × 105 m	3.92 × 105 m	4.11 × 105 m	3.93 × 105 m
TD	3.45 × 105 m	3.48 × 105 m	3.50 × 105 m	3.48 × 105 m

and

Table 4. Statistics of initial path computation time $t_{\mathrm{init}}$.

Algorithm	Q1	Q2	Q3	Mean
R	1.28 s	2.76 s	4.11 s	3.61 s
TR	0.12 s	0.14 s	0.22 s	0.19 s
Q	2.72 s	3.81 s	7.53 s	6.05 s
TQ	0.48 s	0.62 s	0.83 s	0.81 s
D	1.86 s	2.83 s	4.15 s	3.64 s
TD	0.48 s	0.59 s	0.72 s	0.64 s

present the statistical results for the initial cost $c_{\mathrm{init}}$ and initial path computation time $t_{\mathrm{init}}$ for six different algorithms. Figure 13 and Figure 14 display box plots for the initial cost and initial path computation time, respectively. When observing the statistical results, it is evident that the TAOP algorithm exhibits significant reductions in both the initial cost and initial path computation time. In Figure 13, the data points for the TAOP algorithm are noticeably lower, with the upper and lower bounds of the box plot being lower than their corresponding parts for the control algorithms. This indicates that the TAOP algorithm generates paths with smaller costs compared to its control algorithms, effectively optimizing the planning results. In Figure 14, fewer data points for the TAOP algorithm fall within higher time ranges, suggesting that the TAOP algorithm is more efficient in computing the initial path.

In the simulation results, there is a significant deviation between the mean and median of various statistical measures, indicating that the data do not follow a normal distribution. Therefore, we employed the Mann–Whitney U test to further validate the performance advantage of the TAOP algorithm statistically. In this test, the collision detection total time $t_{\mathrm{coll}}$, initial cost $c_{\mathrm{init}}$, and initial path computation time $t_{\mathrm{init}}$ were used as evaluation metrics, and the corresponding p-values were calculated, as shown in

Table 5. Hypothesis test results.

H0: M(ALG1) = M(ALG2) H1: M(ALG1) > M(ALG2)
ALG1	ALG2	M	p-Value	Reject
R	TR	$t_{\mathrm{coll}}$	9.91 × 10−30	Yes
		$c_{\mathrm{init}}$	3.88 × 10−15	Yes
		$t_{\mathrm{init}}$	9.91 × 10−30	Yes
Q	TQ	$t_{\mathrm{coll}}$	1.16 × 10−25	Yes
		$c_{\mathrm{init}}$	1.06 × 10−9	Yes
		$t_{\mathrm{init}}$	1.80 × 10−23	Yes
D	TD	$t_{\mathrm{coll}}$	2.97 × 10−28	Yes
		$c_{\mathrm{init}}$	6.58 × 10−23	Yes
		$t_{\mathrm{init}}$	9.91 × 10−30	Yes

. From Table 5, it can be observed that the p-values for all of test metrics are much smaller than the significance level of 0.05, indicating that the TAOP algorithm outperforms the RRT*, Q-RRT*, and DSFS algorithms in planning simulation experiments.

In addition, based on the mean initial cost and mean initial path computation time in Table 3 and Table 4, the average path planning efficiency for the six algorithms was calculated using Equation (8), as shown in

Table 6. Average path planning efficiency $\overline{\eta }$.

Algorithm	$\overline{\mathit{\eta }}$	Improvement Multiple
R	0.39	15.06
TR	6.26
Q	0.20	5.98
TQ	1.37
D	0.33	4.04
TD	1.64

. It can be observed that, compared to the RRT*, Q-RRT*, and DSFS algorithms, the TAOP algorithm has improved the path planning efficiency by 15.6 times, 5.98 times, and 4.04 times, respectively.

Figure 15, Figure 16 and Figure 17, respectively, present the initial path graphs generated using the R algorithm compared with the T_R algorithm, the Q algorithm compared with the T_Q algorithm, and the D algorithm compared with the T_D algorithm in a specific test scenario. Here, points A and B denote the starting and target points, respectively. Upon examination, it is evident that the paths generated using all six algorithms lie within the predefined suitable area, meeting the requirements for safe navigation. Additionally, in contrast to the RRT*, Q-RRT*, and DSFS algorithms, the TAOP algorithm produces initial paths with a lower cost. To offer a more intuitive portrayal of the variations in the planned paths in the vertical direction, the paths are projected onto the ‘longitude–depth’ plane, as depicted in Figure 15c, Figure 16c and Figure 17c. From the perspective of the ‘longitude–depth’ plane, it is readily apparent that the initial paths generated using the TAOP algorithm exhibit greater smoothness, whereas the paths generated using the corresponding RRT*, Q-RRT*, and DSFS algorithms manifest pronounced undulations in the vertical direction, characterized by one or more conspicuous fluctuations. This divergence arises because the RRT*, Q-RRT*, and DSFS algorithms all engage in random sampling across the entire three-dimensional planning space, with sample points displaying significant randomness in the vertical dimension, thereby rendering them more likely to generate higher-cost paths. Conversely, the TAOP algorithm engages in judicious planning in the vertical dimension based on obstacle profiles, consequently optimizing the cost.

5. Discussion

The underwater gravity-matching navigation employing the TAOP algorithm for three-dimensional path planning offers several prominent advantages. It demonstrates significant improvements in the collision detection time, initial cost, and initial path computation time, effectively reducing the path oscillations and achieving smoother initial paths. Compared to directly using the RRT*, Q-RRT*, and DSFS algorithms, the TAOP algorithm has improved the path planning efficiency by factors of 15.6, 5.98, and 4.04, respectively. It can be seen that the TAOP algorithm has significant advantages, and it can use dimensional reduction methods to solve the problem of high computational costs and even the problem of the inability to find feasible solutions in complex three-dimensional environments due to node diffusion. At the same time, the TAOP algorithm avoids the time-consuming problem of interpolating the terrain data required for three-dimensional collision detection, and it significantly reduces the fluctuation of the path by directly referencing the obstacle contour to plan the height of the path. Similarly, other traditional path planning algorithms for three-dimensional planning (such as artificial potential field method, a* algorithm, ant colony algorithm, etc.) also directly obtain nodes in three-dimensional space, thus also suffering from node diffusion issues and multiple interpolation problems required for three-dimensional collision detection to obtain terrain data. Therefore, the results can be generalized, and the TAOP algorithm can be used to combine height planning with the two-dimensional planar planning forms of these algorithms to reduce the complexity of the problem, thereby effectively saving planning time and improving the path planning efficiency. In addition, the TAOP algorithm is also suitable for situations where the two-dimensional path cannot be changed, and only the height needs to be planned in some special cases.

6. Conclusions

This paper introduces a novel TAOP algorithm aimed at enhancing the path planning efficiency of underwater gravity-matching navigation in three-dimensional environments.

• A novel TAOP algorithm is introduced. Given the existing challenges of high computational complexity and lengthy computation times associated with three-dimensional path planning algorithms, the innovative TAOP algorithm is proposed. It decomposes three-dimensional path planning into two components: two-dimensional planar planning and height planning. The TAOP algorithm straightens the paths obtained from planar planning through coordinate transformations, performs height planning based on obstacle profile contours along the path, and integrates the points from both planar and height planning to obtain the final path in a three-dimensional space.
• A three-dimensional planning environment for underwater gravity-matching navigation is constructed. To accurately represent the terrain features and gravity field distribution in underwater environments, gravity suitability areas are defined based on the characteristics of gravity anomaly data. Combined with seabed topographical data, obstacle constraints in the underwater three-dimensional space are established, making the planned paths more adaptable to the complex terrain and gravity conditions encountered in practical underwater gravity-matching navigation.
• The effectiveness of the TAOP algorithm is validated. In the defined three-dimensional path planning environment, performance comparison simulations are conducted between the TAOP algorithm and the RRT*, Q-RRT*, and DSFS algorithms. The results demonstrate that the TAOP algorithm reduces the collision detection time, lowers the cost of the initial path solutions, shortens the initial planning time, and reduces path oscillation, significantly improving the planning efficiency. Compared to directly employing the RRT*, Q-RRT*, and DSFS algorithms, the TAOP algorithm improves the path planning efficiency by factors of 15.6, 5.98, and 4.04, respectively.
• Future research directions are identified. This paper has not addressed the issue of dynamic environments, which can significantly affect the efficiency and accuracy of path planning. Incorporating dynamic obstacles into the planning algorithm remains a challenge that requires further research. Additionally, while the TAOP algorithm has shown promise in improving the planning efficiency, it may still encounter performance issues when encountering complex, dynamic environments. Future work should focus on enhancing the robustness and suitability of the algorithm to such environments.