A Bounded Near-Bottom Cruise Trajectory Planning Algorithm for Underwater Vehicles

Abstract

The trajectory planning algorithm of underwater vehicle near-bottom cruise is important to scientific investigation, industrial inspection, and military affairs. An autonomous underwater vehicle (AUV) often faces the problems of complex underwater environment and large cruise area in a real environment, and some robots must hide themselves during the cruise. However, to the best of our knowledge, few studies have focused on trajectory planning algorithms for AUVs with multiple constraints on large-scale maps. The currently used algorithms are not effective at solving length-constraint problems, and the mainstream trajectory planning algorithms for robots cannot be directly applied to the needs of underwater vehicle sailing near the bottom. Therefore, we present a bounded ridge-based trajectory planning algorithm (RA*) for an AUV to go on a near-bottom cruise. In the algorithm, we design a safety map based on a spherical structure to ensure the safe operation of the robot. In addressing the length-constraint problem and large-scale map planning problem, this paper proposes a two-stage framework for RA*, which designs map compression and parallel computation using a coarse-fine planning framework to solve the large-scale trajectory planning problem and uses a bounded search method to meet the trajectory planning requirements of length constraint. In this study, experiments based on the virtual ocean ridge are conducted, and the results validate the effectiveness and efficiency of the proposed RA* with MCPC algorithm framework.

1. Introduction

With the rapid development of information technology and artificial intelligence technology, the application scenario of trajectory planning has changed to interpretable, large scale, and practical [1]. Trajectory planning for underwater vehicles, as a classical application scenario in related research fields, has been the classical research direction in related fields [2].

Trajectory planning can be divided into online planning and off-line planning, which correspond to different use scenarios [3]. Before the departure of an autonomous underwater vehicle (AUV), its navigation trajectory must be pre-planned on the basis of the requirements, which is suitable for offline trajectory planning algorithm [4]. In recent years, the off-line trajectory planning algorithm with special practical application requirements has gradually become a research hotspot [5,6]. In the existing research results, some studies aim at the multi-objective optimization objectives in trajectory planning, using the evolutionary algorithm to search a trajectory satisfying Pareto optimum [7]. Some studies focus on improving the efficiency of algorithms, which can deal with large-scale maps by establishing multiple heuristic functions [8,9]. Other studies aim to plan/optimize the trajectory based on the motion ability of objects [10]. However, these works still have some problems when being applied to a real environment. The evolutionary algorithm has strong randomness, and it cannot ensure the stability of trajectory planning results. Moreover, efficiency cannot be ensured in large-scale cases. Although the search-based method can make up for the lack of predictability to a certain extent, the algorithm cannot run under the framework of multi-objective definitions. These limitations lead to the poor predictability of the algorithm, which leads to users’ doubts about the practicability of the algorithm. At present, among the special requirements of many AUV, there is little need for sailing near the bottom of the ocean ridge, and terrain must be considered. Therefore, when facing large-scale maps, for example, the size reaches 10,000×10,000, the trajectory planning algorithm that meets the requirements of concealment and safety and defines the optimization goal has rarely been studied.

Studying the bottom-up path planning of bounded AUV for large maps has many benefits. With regard to quantitative optimization indicators, this planning algorithm can provide stable and convincing trajectory planning results. With regard to safety, it can provide a more accurate and safe distance for the AUV and reduce the probability of collision between the AUV and the ocean ridge during near-bottom navigation. In some special applications, the algorithm can also reduce the detection probability of sonar and camera by terrain which could reduce the cost and production difficulty [11].

In a complex seabed environment, the implementation of the abovementioned trajectory planning scheme primarily faces the following three challenges. First, designing a trajectory planning algorithm that satisfies the optimal distance condition and bound is a challenge. Second, considering the different definitions of safe distance in different scenarios, it has different effects on trajectory planning; thus, considering safety in planning is a challenge. Finally, computational efficiency is important for practicability. Traditional algorithms involve a trajectory planning compression algorithm that considers planning accuracy and computational efficiency. Furthermore, constraining the trajectory and splitting the algorithm into parallel processing methods is a challenge.

In addressing the above-mentioned challenges, we propose a novel trajectory planning framework: ridge-based A* trajectory planning algorithm, abbreviated as RA* algorithm. The algorithm (i) proposes a bounded trajectory planning algorithm, (ii) designs a safety map construction algorithm, and (iii) proposes a parallel bounded map planning framework. In particular, the contributions of this paper are as follows.

• To our knowledge, this study is the first to explore the near-bottom trajectory planning of AUV in a large-scale oceanic ridge environment.
• A bounded trajectory planning algorithm is proposed, which can combine the path length with near-bottom navigation based on the suboptimal bound of the path.
• A fuzzy trajectory planning framework is proposed. The algorithm can create a mosaic of regional features based on scene requirements and then carry out distributed trajectory planning for each block through a parallel computing module.
• We propose a general safety map construction algorithm that provides a safe navigation trajectory map for AUV, which can meet the needs of different application scenarios and adapt to complex seabed terrain environment.
• In this paper, a large number of simulation experiments are carried out by using virtual ocean ridge maps, and the results are compared with those of classical trajectory planning algorithms. A case study is conducted for practical multi-attribute detection requirement projects, which further verifies the practicability and effectiveness of the algorithm.

2. Motivation

Our research was based on the experimental scenario of AUV simulation trajectory planning (Figure 1). First, in the typical AUV detection mission, AUV must cross the undulating oceanic ridge to detect the safety of any area in a long distance. The terrain environment to be detected is complex, and it cannot be described by a formula. The area is also large, and can reach 10,000 × 10,000 scale data.

The planned results can cause the AUVs to be in a safe state. In this study, the “safe state” has two meanings. One meaning refers to navigation safety, that is, an AUV must meet a certain safe distance from obstacles when sailing. In general, AUVs will be disturbed by various conditions during driving, including the disturbance of surface ocean currents from the environment. AUVs usually have insufficient power [12,13]. When disturbed by ocean currents, they will continue to consume power to resist ocean currents. The other meaning describes how safe AUVs are for detection. When the AUV performs concealment tasks, it should minimize the exposure rate and avoid the interference and detection from underwater sonar. We hope that the AUV will fit the bottom of the ridge as much as possible during the voyage to avoid sonar irradiation by using undulating landforms. In addition, near-bottom navigation is more convenient for AUV to perform some minor tasks in navigation missions, such as scanning the bottom pipeline or finding other mission targets.

Therefore, we propose a large-scale and fine-grained trajectory planning algorithm and use geographical factors such as ridges to avoid ocean current disturbance and sonar detection.

2.1. Safety Distance Requirement

Making a global route plan under the precondition of security is necessary. In the traditional underwater safety trajectory planning, an AUV must keep a certain underwater cruise depth to avoid collision with the underwater bottom (Figure 2).

As shown in Figure 2, the depth of AUV $A_1$ is $S_1$. A certain safe distance must be kept between AUV and the bottom $B_1$ to ensure safety. An evident deficiency is observed in the related work. In applications requiring near-bottom navigation, the abovementioned strategies cannot meet the requirements of lateral safety trajectory. For example, as shown in Figure 2, the horizontal safety distance ${b^{\prime }}_2$ of AUV $A_2$ is evidently smaller than the vertical safety distance $B_2$. Therefore, considering the vertical safety distance is insufficient.

2.2. Benefits of Large-Scale and Fine-Grained Trajectory Planning

(1) Navigation safety The AUV can also use terrain to avoid specific “dangers” during traveling. For example, in military and other special purposes, if it can sail near the bottom based on terrain, then it can use the occlusion of the ridge to achieve the concealment effect; avoid sonar, photoelectric, and other detection; and increase the success rate of the mission. In addition, in the actual use of medium and low-speed aircraft, certain collision risks will occur when traveling in areas full of obstacles because of different application requirements. Therefore, a navigation track must be planned to meet the safety requirements in advance and achieve higher safety standards.

(2) Planning interpretability During multi-objective trajectory planning, the optimization objectives include distance, safety, and fuel consumption. The commonly used method is heuristic intelligent search such as genetic algorithm or particle swarm optimization. The multi-objective optimization problem is transformed into single-objective optimization by coefficient weighting, and then the optimal solution is searched for or solved. However, during optimization, the abovementioned algorithm cannot limit the specified factors, and the optimization results have certain randomness and poor reproducibility. If the variables can be optimized in a bounded way, then the algorithm results can be reproduced. Considering that the solution is suboptimal, the risk is controllable. These methods are convenient for users to understand and increase the interpretability of the algorithm.

(3) Practicability of the system In the actual underwater exploration mission, multiple AUVs must detect multiple detection points cooperatively. In the framework of the cooperative detection system, the trajectory planning algorithm must be called frequently, and in the context of fine grains, the scale of the planning map is large, which proposes strict requirements for the efficiency and stability of the algorithm. Therefore, a stable, bounded, and efficient algorithm will inevitably increase the practicability of the trajectory planning system.

(4) Diversity of trajectories When performing tasks such as environmental detection, regional patrol, and object recognition in an area full of obstacles, the granularity and scale of environmental modeling are related to the navigation ability of AUV. In the traditional coarse-grained route planning algorithm, the carrier aims to float to the depth without obstacles and then travel in a fixed depth way. The fine-grained trajectory planning algorithm considers the terrain; thus, it can consider the security and terrain based on the experimental requirements. Consequently, the trajectory has more options and diversity.

3. Related Works

For the global path planning algorithm applied to AUV, researchers have proposed many commonly used algorithms, such as A* algorithm [14], genetic algorithm [15], particle swarm optimization algorithm [16], and other algorithms [3,17]. With the wide application of AUV in marine pollutant monitoring, pipeline detection, anti-submarine warfare, and other fields in recent years, the requirements for submarine path planning are increasing [18]. Yao et al. [19] proposed an AUV continuous-direction path planning method based on edge search algorithm. Considering energy consumption, their algorithm considers the influence of a strong current marine environment on AUV power while planning the path, thereby maximizing the use of energy. Sun et al. [20] abstracted the flow field as a static non-uniform vector field, combined directed graph with k-means method, and proposed GK-OPM algorithm to generate an AUV path with optimal time. Sang et al. [21] proposed a new MTAPF algorithm based on the improved APF and used the improved A* algorithm to obtain the global path. Yonetani et al. [22] proposed a neural A* algorithm that can be trained end to end. Yu et al. [23] proposed the Smooth-RRT algorithm to solve the problems of a complex working environment and high real-time requirements in AUV application scenarios, which meets the special requirements of AUV such as the shortest distance and maneuverability. Siddharth [8] used several different heuristic functions to improve the calculation speed of the path planning algorithm from the perspective of pure algorithm principle. Du et al. [9] proposed the simultaneous construction of multiple state spaces with different resolutions, which can improve the search speed while ensuring bounded sub-optimality. Ma et al. [24] proposed an alarm pheromone-assisted ant colony system (AP-ACS) that incorporates the alarm pheromone in addition to the traditional guiding pheromone. Roberge et al. [25] used a genetic algorithm and particle swarm optimization to generate UAV trajectory and achieved real-time path planning of UAV by parallel computing. Oral et al. [26] proposed a multi-objective path planning algorithm based on multi-objective genetic algorithm, which improved the quality of path generation and replaced the traditional weighted optimal solution with Pareto optimal solution. Xi et al. [27] introduced real ocean current, weather, temperature, and other related information and used reinforcement learning to plan AUV in combination with the environment. Ru et al. [28] proposed an improved multi-objective genetic algorithm, which was combined with its three-dimensional omni-directional sensor, and designed an AUV planning method that can sense the surrounding environment to the greatest extent. Li et al. [29] proposed an improved DQN path planning algorithm to solve the problem of model tracking error and over-dependence in the planning process.

4. Model Design

In this paper, a new single-objective optimal path planning method is proposed, which can improve the traversal of map feature points while ensuring the bounded suboptimal total path length.

4.1. Problem Definitions and Concepts

4.1.1. Inconsistent State

In the following description, S represents all discrete states, including a starting state $s_{start}$ and a target state $s_{goal}$. $g\left(s\right)$ represents the cost from the starting point to the state s, whereas $h\left(s\right)$ represents the expected cost from the state s to the end point. In this paper, the Euclidean distance is used to calculate the expected distance. In the A* algorithm, there is $f\left(s\right)=g\left(s\right)+h\left(s\right)$, where $f\left(s\right)$ represents the total generation value. The optimal cost from $s_{start}$ to state s is represented as $g^{\ast }\left(s\right)$, and the function $c(s,s^{\prime })$ represents the actual cost of the edge between states s and $c(s,s^{\prime })$ (in the absence of an edge, $c(s,s^{\prime })=\infty$). Assuming $c(s,s^{\prime })\ge 0$, for all s and $s^{\prime }$, $SUCCS\left(s\right)=\{s^{\prime }\in S|c(s,s^{\prime })\ne \infty \}$, representing all successors of s. $pred\left(s\right)$ represents all the predecessors of s, and $bp\left(s\right)$ represents the best predecessor of s.

In this paper, we introduce the concept of local inconsistency. In the common A* algorithm, all the extended states are in the local consistent state. For the state s, its $g\left(s^{\prime }\right)$ is calculated as follows:

$$g\left(s^{\prime }\right)=mi{n}_{s^{″}\in pred\left(s^{\prime }\right)}(g\left(s^{″}\right)+c(s^{\prime },s^{″}))=g\left(s\right)+c(s,s^{\prime })$$

(1)

where $g\left(s\right)=g^{\ast }\left(s\right)$, and it will not be updated later. Therefore, this node will not be updated after joining the CLOSE table. The concept of local inconsistency indicates that the value of expanded nodes is not optimal, and it can be updated to a better value later, which is shown as follows:

$$g\left(s^{\prime }\right)>mi{n}_{s^{″}\in pred\left(s^{\prime }\right)}(g\left(s^{″}\right)+c(s^{\prime },s^{″}))$$

(2)

Therefore, in the common A* algorithm, except for the state with the smallest $f\left(s\right)$ value in the priority queue OPEN, other states may be locally inconsistent.

4.1.2. A* Algorithm Based on the Inconsistent State

As shown in Figure 3, from the beginning to the end, the expansion order of ordinary A* algorithm should be $A_3\to B_2\to C_1$, and its generation value $f\left(s_{goal}\right)=2\sqrt{2}$. All the expansion states in this process are locally consistent. If the feature points must be passed during path planning, then the expansion order is $A_3\to B_3\to C_3\to C_2\to C_1$. At this time, $f\left(C_2\right)=3$, $f\left(s_{goal}\right)=4$, the states of $C_2$ and $C_1$ are locally inconsistent because the shortest path to $C_2$ is $A_3\to B_3\to C_2$ or $A_3\to B_2\to C_2$, $f\left(C_2\right)=1+\sqrt{2}$. This inconsistency will continue to pass backward with the expansion, resulting in $g\left(s\right)\ne g^{\ast }\left(s\right)$, and the length of the final path cannot be controlled. Therefore, if the number of feature points in the path is considered while planning the path, then the expanded state is locally inconsistent. Moreover, we cannot ensure the bounded sub-optimality of the path.

In this paper, an improved A* algorithm is proposed, which improves the traversal of feature points in the bounded suboptimal case. Compared with the traditional A* algorithm, this algorithm uses an extra priority queue and adopts two sets of cost functions $f\left(s\right)=g\left(s\right)+h\left(s\right)$ and $f_{incons}\left(s\right)=g_{incons}\left(s\right)+h\left(s\right)$ for all states, where $f\left(s\right)$ represents the cost of ordinary A* calculation, and $f_{incons}\left(s\right)$ represents the total cost of state s when considering characteristic points. For the state s$f\left(s\right)\le f_{incons}\left(s\right)$, $f\left(s\right)$ is used to calculate the boundary problem in theory, and $f_{incons}\left(s\right)$ is used to calculate the actual distance when considering feature points, thereby considering the path length and the number of feature points in the path.

4.2. Construction of Safety Ridge Map

4.2.1. Safety Map

Constructing a safety map is the foundation of the system. Underwater navigation has many safety requirements, such as the vertical distance, horizontal distance, and vertical–horizontal mixed distance from terrain, which are all longer than the safety threshold. If the map data (point, edge, or surface) are expanded along the normal vector direction of the terrain, then it will bring a huge amount of computation, although the accurate expanded map can be obtained. Considering that all the safety maps constructed are grid data, this paper aims to construct a new map based on the original map and ensure that the shortest distance between every grid point on the new map and the seabed is longer than the safety threshold. Furthermore, the space above the level of the safety map is the safety space that meets the safety threshold requirements.

Each coordinate point s must be processed to generate a new safe coordinate point $s_f$. This paper summarizes three methods of building security maps to meet different kinds of security requirements, as shown in the following figures.

(1) Vertical safety map

A vertical safety map is the simplest and most commonly used safety map construction method, which is only suitable for scenes that are sensitive to vertical distance. All map points must increase the safety threshold $d_s$ along the z axis, that is, $s_f\left(z\right)=s\left(z\right)+d_s$ (Figure 4a).

(2) Horizontal safety map

A horizontal safety map aims to add a horizontal safety criterion (horizontal safety threshold ${d^{\prime }}_s$) based on the vertical safety map. This method is suitable for scenes that are sensitive to horizontal distance, and it has different requirements for horizontal and vertical safety distance. All grid points must find the highest point of elevation data within the range of their horizontal distance of ${d^{\prime }}_s$, which is defined as $s_d$, and then calculate $s_f\left(z\right)=s_d\left(z\right)+d_s$ where $d_s$ is the vertical safe distance, which is the vertical safe range of such problems. In addition, AUV must increase a pie-shaped space with a thickness of $d_s$ along the ridge (Figure 4b). Thus, the safe distance directly below the AUV is longer than $d_s$, and the safe distance from the horizontal area is longer than ${d^{\prime }}_s$.

(3) Spherical safety map

When the straight-line distance is used as the safe distance, the spherical safety map construction method must be used. This method is widely applicable, which is primarily considered in this paper, and the default method is considered for subsequent experiments. All grid points must be traversed in the range of horizontal distance ${d^{\prime }}_s$. In addition, the highest point and the intersection point of vertical lines on the ground from $s_d$ to s point must be defined as $s_d$ and o, respectively, and then calculated to obtain $s_f\left(z\right)=\sqrt{d_s^2-o{s}_d^2}+os$ (Figure 4c).

4.2.2. Construction of Oceanic Ridge Map

In this paper, the hydrological analysis method in ArcGIS toolbox is used to extract the ridge line from the digital elevation model (DEM) data. After binarization, a new map $M_r$ with the same size as the original map is generated. In this map, $M_r\left(s\right)=1$ indicates that the state s is located in the ridge part, and $M_r\left(s\right)=0$ indicates that the state s is located in the non-ridge part. The effect of the algorithm is shown as follows.

4.3. Ridge-Based A* Algorithm

The process of the improved A* algorithm based on the oceanic ridge is shown as follows.

Steps of RA*

Step 1. ArcGIS is used to generate a matching ridge map based on the original DEM data.

Step 2. A priority queue $OPE{N}_{cost}$ and an array $OPE{N}_{nor}$ for storing all the explored states are constructed. The $OPE{N}_{cost}$ is sorted on the basis of $f\left(s\right)$, and the optimal state of $f\left(s\right)$ is $s_0$. $\omega$ is a weighted hyper-parameter that controls the boundary, and $Nor\left(s\right)$ represents the ridge point contained from the starting point to the state s. Let $s_i$ represent the state in $OPE{N}_{nor}$ that conforms to $f_{incons}\left(s\right)<limit$ and $Nor\left(s\right)$ maximum. Among them, the $limit$ is defined as follows.

$$limit=\omega ·f\left(s_{cost}^{top}\right)$$

(3)

Step 3.$s_{nor}^{top}$ is extended. Similarly, $s_{cost}^{top}$ is extended when none of the states in $OPE{N}_{nor}$ meet the conditions.

Step 4.$SUCCS\left(s\right)$ returns all subsequent states of the extended state s and updates the subsequent states in accordance with the rules. The updated rules of $f\left(s\right)$ are the same as those of ordinary A* algorithm, and $f_{incons}\left(s\right)$ is updated in accordance with the condition of satisfying the descending order of $Nor$, which must satisfy $f_{incons}\left(s\right)<limit$.

Step 5. If the target point is not found, then the abovementioned steps 3 and 4 are repeated.

4.4. RA* Details

Algorithm 1 is the main function of RA* In the main function, lines 2 to 6 are initialization steps. First, a priority queue $OPE{N}_{cost}$ and an ordinary array $OPE{N}_{nor}$ are constructed, and the starting state $s_{start}$ is assigned and added to the two priority queues. First, each round of the loop must update $limit$, which is used as a constraint for subsequent extensions. In the selection of the current state, the top element of $OPE{N}_{nor}$ is selected by default. Lines 11–13 determine whether $s_{nor}^{top}$ exists. If $OPE{N}_{nor}$ is empty or $f_{incons}$ of all elements of $OPE{N}_{nor}$ is greater than $limit$, then $s_{nor}^{top}$ does not exist, that is, degenerates into the original A* search algorithm. In addition, the current state is replaced by $s_{cost}^{top}$, and $limit$ is continuously increased through subsequent updates until $s_{nor}^{top}$ exists. Lines 14–15 indicate that if $s_{cost}^{top}$ and $s_{nor}^{top}$ are the same, then the extension of $OPE{N}_{cost}$ and $OPE{N}_{nor}$ is controlled simultaneously. Moreover, the top elements are replaced by other elements. For $OPE{N}_{cost}$, the cost of the top element is bound to increase, and subsequent extensions of the state in $OPE{N}_{nor}$ result in a larger $limit$. Consequently, we can update $limit$ and extend the state in $OPE{N}_{nor}$ with just one extension, reducing the number of secondary extensions to the same state. Line 17 indicates that if $s_{nor}^{top}$ exists, and $s_{cost}^{top}$ is not the same as $s_{nor}^{top}$, then $s_{nor}^{top}$ is extended separately to extend as many feature points as possible under constraints.

Algorithm 1:MAIN FUNCTION OF RA*

Algorithm 2 describes the details of the exploration state. Line 3 determines whether the current extension node is the top element of $OPE{N}_{cost}$. If the result is true, then the operation is performed in accordance with the A* algorithm. Lines 4–12 are responsible for inserting the new state $s^{\prime }$ and updating $OPE{N}_{cost}$. Line 14 determines whether the current extension node is the top element of $OPE{N}_{nor}$. If the judgment is true, then the extension will maximize the number of feature points. As long as it meets $g_{incons}\left(s\right)+c(s,s^{\prime })<limit$, it can be extended arbitrarily with the increased direction of $Nor$. The state s can serve as the predecessor node of $s^{\prime }$, and $g_{incons}\left(s^{\prime }\right)$ and $Nor\left(s^{\prime }\right)$ can also be updated.    

Algorithm 2:EXPLORATION STATE

    Figure 5 provides a simple 2D illustration of the RA* algorithm. In this case, $\omega =1.1$.

4.5. Map Compression and Parallel Computing

In this section, we introduce the algorithmic details of map compression and parallel computing (abbreviated as MCPC).

4.5.1. Map Compression

First, the surface is meshed and compressed, as shown in Figure 6.

Each complex surface consists of a basic mesh surface G, which consists of $n\times m$ sets $s_{1,1},s_{1,2}\dots s_{n,m}$, with $n\times m$ vertices. The height value of each surface is determined by its four vertices $g(i,j)=(s_{i,j},s_{i+1,j},s_{i,j+1},s_{i+1,j+1})/4$. These points are compressed $\rho$ times, and the compressed map becomes $g_p(1,1),g_p(1,2)\dots g_p(n^{\prime },m^{\prime })$, where $g_{\rho }(i^{\prime },j^{\prime })=\gamma \left(g(i,j)\right)/\rho$, and $\gamma (·)$ is the average of the data for the range satisfying $\mathrm{ceil}(i/\rho )\le i^{\prime }\le \mathrm{ceil}(i/\rho +1)$ and $\mathrm{ceil}(j/\rho )\le j^{\prime }\le \mathrm{ceil}(j/\rho +1)$. In addition, each grid has a ridge point attribute $ridge(i,j)$, which is calculated in the same way as the elevation data. The compressed ridge point attribute is defined as $ridg{e}_{\rho }(i^{\prime },j^{\prime })$, where it is the average of the data for the range satisfying $\mathrm{ceil}(i/\rho )\le i^{\prime }\le \mathrm{ceil}(i/\rho +1)$ and $\mathrm{ceil}(j/\rho )\le j^{\prime }\le \mathrm{ceil}(j/\rho +1)$. In addition, considering that each face has the ridge point attribute $ridge(i,j)$, which is similar to elevation data, the compressed ridge point attribute is correspondingly defined as $ridg{e}_{\rho }(i^{\prime },j^{\prime })$.

The starting point and end point of trajectory planning are also compressed in equal proportion. The compressed start and end points are as follows.

$$\begin{array}{c}\hfill {s^{\prime }}_{start}(x,y)=s_{start}(x,y)/\rho \\ \hfill {s^{\prime }}_{goal}(x,y)=s_{goal}(x,y)/\rho \end{array}$$

(4)

First, use the A* algorithm to complete the rough planning trajectory from ${s^{\prime }}_{start}(x,y)$ to ${s^{\prime }}_{goal}(x,y)$, as shown by the red trajectory in Figure 6, which is represented by $\overrightarrow{{s^{\prime }}_{start},{s^{\prime }}_{goal}}=\{{s^{\prime }}_1,{s^{\prime }}_2,\dots ,{s^{\prime }}_k\}$. Then, we must cut the original map into pieces through the $\overrightarrow{{s^{\prime }}_{start},{s^{\prime }}_{goal}}$.

4.5.2. Map Segmentation and Computing

Using the segmentation algorithm, the map can be segmented equally along the trajectory obtained by rough planning, and several block maps with the same size can be obtained, in which the starting point and ending point of the segmented trajectory are at the edge of the sub-map. The specific algorithm of the map segmentation algorithm is as follows.

First, the trajectory must be restored proportionally $\{{\overline{s}}_1,{\overline{s}}_2,\dots ,{\overline{s}}_k\}=\overrightarrow{{s^{\prime }}_{start},{s^{\prime }}_{goal}}·\rho =\{{s^{\prime }}_1,{s^{\prime }}_2,\dots {s^{\prime }}_k\}·\rho$. The schematic diagram of the algorithm is shown in Figure 7a. First, based on the previous compression ratio $\rho$, the trajectory is reverse restored, and $\{{\overline{s}}_1,{\overline{s}}_2,\dots ,{\overline{s}}_k\}$ is obtained. Then, we use the idea of a sliding window to traverse all coordinate points and pause when the maximum length interval $\Delta l$ or the maximum width interval $\Delta w$ is larger than the threshold value $T_s$. If ${\overline{s}}_k$ satisfies the condition, then the point is saved from the starting time to the previous time ${\overline{s}}_{k-1}$ into $t_{temp}$. Moreover, the center point of $t_{temp}$ is determined and defined as $c_p$, as shown by black dots in the figure.

Then, the map and ith group data are clipped and saved into $Section\left[i\right]$, with $c_p$ as the center and $L_s=max(\Delta l,\Delta w)$ as the side length, as shown in the red box in the figure. The four vertices of the square region are $\{c_p+\left(-L_s/2,-L_s/2\right),c_p+\left(-L_s/2,L_s/2\right),c_p+\left(L_s/2,-L_s/2\right),c_p+\left(L_s/2,L_s/2\right)\}$. Finally, the current vertex ${\overline{s}}_k$ is saved as the starting point of the next $t_{temp}$ and iterated until all the points in $\{{\overline{s}}_1,{\overline{s}}_2,\dots ,{\overline{s}}_k\}$ are traversed. In addition, the starting point and ending point ${\overline{s}}_k$ in $t_{temp}$ are recorded as the starting point and ending point $[{sec}_{start}\left(i\right),{sec}_{goal}\left(i\right)]$ of $Section\left[i\right]$ (Figure 7b).

Finally, different $Section\left[i\right]$ are sent to different CPU cores, and the results are calculated by the MRA* algorithm proposed in this paper with their respective starting points and ending points $[{sec}_{start}\left(i\right),{sec}_{goal}\left(i\right)]$. Finally, the sub-trajectories are connected end to end in the order of $Section\left[i\right]$ and spliced into the final trajectory. The split sections are sent to the cores of different CPUs, and the results are obtained by using the RA* algorithm mentioned in the previous section at their respective starting points and ending points. Algorithm 3 describes the details of the map segmentation and computation. Finally, the results are connected end to end and spliced into the final trajectory.

Algorithm 3:MAP SEGMENTATION AND COMPUTATION.

5. Evaluation

In this section, all simulation experiments are performed with MATLAB 2020A. The experiments were executed on a desktop with an AMD Ryzen 9-3950X central processing unit (CPU) @ 3.50 GHz and with a 64 GB memory.

5.1. Generation of Ridge Map

Figure 8a shows the topographic thermal map generated by MATLAB, with yellow representing shallow areas and blue representing deep areas. Figure 8b is the map generated by ArcGIS toolbox, which is formed by overlapping two layers, in which red represents ridge points ($M_r\left(s\right)=1$), which are obtained by binary classification. Moreover, the remaining gray protrusions represent ordinary depressed areas, which are determined as non-ridge areas ($M_r\left(s\right)=0$), because they are not enough to meet the criteria for judging ridges. The scale of the following experimental map is 1:30, that is, each pixel or grid represents 30 m.

5.2. Evaluation of Safety Map

Figure 9 shows the comparison effect of the spherical safety map. The original map includes (200 × 200) DEM data, with a safety threshold of 300 m and a distance of 100 m between two points. As shown in the map, the basic topography can be preserved after processing. After calculation, the average height of the processed safety map increases by 338 m, and any distance between two points in the safety map and the ground are higher than 300 m.

Table 1. Comparison of safety maps.

Safety Distance (m)	Spherical Safety Map (m)	Horizontal Safety Map (m)	Vertical Safety Map (m)
300	+338	+442	+300
200	+222	+342	+200

shows the average fluctuation of the three safety maps when the safety distance is 200 and 300 m. As shown in the table, the horizontal safety consideration has the highest raising distance, whereas the shortest distance consideration primarily used in this paper has a moderate raising distance, which is higher than the vertical safety consideration and can meet the safety distance requirements of various application scenarios.

5.3. Evaluation of RA*

This section describes the experiment on DEM data of 200×200. Figure 10a,b shows that when $\omega =2.5$, the agent will advance along the ridge point as much as possible. A large number of ridge points are observed in the trajectory, which can achieve a good effect of traversing ridge feature points. However, the trajectory does not completely coincide with ridge points, which is limited by the overall cost, and it will make a compromise between them. When $\omega =2$, the boundary constraint of the overall cost is greater. Based on the performance of the RA* algorithm, when $\omega$ decreases greatly, the overall trajectory cost will suddenly decrease, and the trajectory shape will change greatly. When $\omega$ decreases slightly, the overall trajectory does not change much, but some details are slightly adjusted. The abovementioned results show that this algorithm is sensitive to the change of $\omega$, which can adapt to the change of boundary, utilize boundary constraints, and improve the traversal of ridge points as much as possible. When $\omega =1$, RA* is equivalent to the original A* algorithm, which is consistent with the same planning result of Dijkstra.

Three indexes are shown in

Table 2. Data comparison of RA* without safety map.

	$\mathit{\omega }=2.5$	$\mathit{\omega }=2.0$	$\mathit{\omega }=1.8$	$\mathit{\omega }=1.0$
Total distance (km)	8.194	8.001	7.867	5.250
Length of ridge (km)	6.423	5.753	5.325	1.954
Ridge ratio (%)	78.4	71.9	67.7	37.2

, including the total path length, the length of ridge, and the ridge ratio, where the ridge ratio represents the ratio of the ridge length to the total length of the path.

The following part uses the same starting point, end point, and $\omega$ value as the above mentioned experiment and analyzes the trajectory planning results when only the spherical safety map is used. As shown in Figure 11 and

Table 3. Data comparison of RA* with safety map.

	$\mathit{\omega }=2.5$	$\mathit{\omega }=2.0$	$\mathit{\omega }=1.8$	$\mathit{\omega }=1.0$
Total distance (km)	8.389	8.129	7.963	5.463
Length of ridge (km)	6.563	5.646	5.268	2.047
Ridge ratio (%)	78.2	69.5	66.2	37.5

, slight changes are observed in the total length of the path, the length of ridge, and ridge ratio by using the safety map, which can ensure that the path keeps the same safe distance from the terrain and will not have a great effect on the overall trend of trajectory planning.

5.4. Evaluation of Map Compression and Parallel Computing

Based on the $5000\times 5000$ large-scale map, this part compares the calculation results of RA* and original A* and analyzes the efficiency of the two methods with and without MCPC module. Figure 12 is an algorithm result based on RA*+MCPC, from which it can be seen that the algorithm in this paper greatly improves the calculation speed and path quality under the condition, that is, the total length of the ridge is constrained ($\omega =3$), and more than half of the whole path is covered by ridge points, which improves the detectability of the feature points. Compared with the original A* algorithm, this algorithm sacrifices part of the optimality, but the calculation speed range is exponentially improved. The trajectory is only using the RA* algorithm. Although the quality of the path is the highest among the four paths, the search speed is too slow, which is not applicable to the planning application of robots in real working environment. However, compared with the original A*, the coverage ability of RA* to ridge points is greatly improved.

Four indexes are shown in

Table 4. Data comparison of different algorithms.

	RA* with MCPC	A* with MCPC	RA* without MCPC	Original A*
Total path length (km)	342.5	293.6	257.6	163.7
Length of ridge (km)	227.6	113.2	176	39.8
Ridge ratio (%)	66.45	38.54	68.32	24.31
Calculation time (s)	2.58	2.03	20.54	15.36

, including the total path length, the length of ridge, the ridge ratio, and the running time of four different algorithms.

5.5. Evaluation of Computational Efficiency

This section verifies how the computational efficiency of the proposed algorithm is affected by the size of the map. As shown in

Table 5. Comparison of computational efficiency.

Map size	$500\times 500$	$1000\times 1000$	$1500\times 1500$	$2000\times 2000$	$2500\times 2500$
Calculation time (s)	0.262	0.475	0.896	0.91	1.07
Map size	$3000\times 3000$	$3500\times 3500$	$4000\times 4000$	$4500\times 4500$	$5000\times 5000$
Calculation time (s)	1.35	1.48	1.53	1.76	1.89
Map size	$5500\times 5500$	$6000\times 6000$	$6500\times 6500$	$7000\times 7000$	$7500\times 7500$
Calculation time (s)	2.14	2.39	2.42	2.65	2.78
Map size	$8000\times 8000$	$8500\times 8500$	$9000\times 9000$	$9500\times 9500$	10,000 × 10,000
Calculation time (s)	2.83	2.91	3.02	3.04	3.12

, the average time spent on map sizes in 20 scales of $500\times 500,1000\times 1000,\dots$ 10,000 × 10,000 when $\omega =2$ is shown. The map of each scale randomly selects 20 starting points and ending points for planning. As shown in the table, when the size of the map increases, the time spent by the planning algorithm increases linearly with the increase of the map size, reaching the size of 10,000 × 10,000. Using the experimental equipment in this paper, no dimension of disaster occurs.

As can be seen from the Figure 13, the calculation time increases linearly with the size of the map.

In this paper, the MRA* algorithm, RRT* algorithm, and WA* algorithm are selected and compared with the RA* algorithm ($\omega =2$) proposed in this paper (

Table 6. Comparison with different algorithms.

	RA*($\mathit{\omega }=2$)	MRA*	RRT*	WA*
Calculation time (s)	3.12	8.46	20.54	25.36
Cost (km)	356.4	232.3	351.5	281.4

). All the experiments are calculated on the map of 10,000 × 10,000 scale, and the average number of calculations is 20 times. All the algorithms have the same starting point and ending point at each calculation. Therefore, the trajectory generation method in this paper is faster than the current mainstream trajectory planning algorithm, but it has the characteristic of bounded detection of feature points, which is not available in traditional algorithms.

Case Study

Finally, we show a specific case study on the 2000 × 2000 map (RA*+MCPC with $\omega =3$). Figure 14a on the left shows a planned trajectory. The calculation time is 0.94 s, and the average safety distance is 341 m. Its starting point and ending point are the relative vertices of the square area. Figure 14b on the right shows the characteristic map of ridge points in this area. The yellow area in the map is ridge points, and the black solid line is the corresponding planning trajectory. Based on the two figures, the algorithm proposed in this paper can effectively and simultaneously complete the trajectory planning task and detect ridge points.

6. Conclusions

In addressing the problem of trajectory planning for AUV near bottom cruise, this paper presents an improved heuristic algorithm and parallel computing scheme by combining global and local planning with a safety map to ensure the effectiveness of the algorithm considering safety factors. Extensive experiments have been performed to study the effectivity of our algorithm. The simulation results show that the trajectory generation method (RA*) in this paper can fully traverse task feature points under bounded suboptimal conditions while ensuring computational efficiency compared with other mainstream trajectory planning, which is essential in practical applications. This work considers both security and boundedness, and is suitable for functional operation, providing a feasible scheme for researchers in need.