Improving path planning efficiency for underwater gravity-aided navigation based on a new depth sorting fast search algorithm

Xiaocong Zhou , Wei Zheng , Zhaowei Li , Panlong Wu , Yongjin Sun b,c,d

a School of Automation, Nanjing University of Science and Technology, Nanjing, 210094, China

b China Academy of Aerospace Science and Innovation, Beijing,100176, China

c Qian Xuesen Laboratory of Space Technology, China Academy of Space Technology, Beijing,100094, China

d School of Electronics and Information Engineering, Harbin Institute of Technology, Harbin,150001, China

Keywords:Depth Sorting Fast Search algorithm Underwater gravity-aided navigation Path planning efficiency Quick Rapidly-exploring Random Trees*(QRRT*)

ABSTRACT This study focuses on the improvement of path planning efficiency for underwater gravity-aided navigation.Firstly,a Depth Sorting Fast Search(DSFS)algorithm was proposed to improve the planning speed of the Quick Rapidly-exploring Random Trees* (Q-RRT*) algorithm.A cost inequality relationship between an ancestor and its descendants was derived, and the ancestors were filtered accordingly.Secondly, the underwater gravity-aided navigation path planning system was designed based on the DSFS algorithm, taking into account the fitness, safety, and asymptotic optimality of the routes, according to the gravity suitability distribution of the navigation space.Finally, experimental comparisons of the computing performance of the ChooseParent procedure, the Rewire procedure, and the combination of the two procedures for Q-RRT* and DSFS were conducted under the same planning environment and parameter conditions, respectively.The results showed that the computational efficiency of the DSFS algorithm was improved by about 1.2 times compared with the Q-RRT*algorithm while ensuring correct computational results.

1.Introduction

Due to the unique characteristics of the underwater environment, underwater vehicles cannot use signals from satellite navigation systems,astronomical navigation systems,and other similar systems for navigation and positioning [1-3].Inertial navigation system (INS), which does not rely on external information, can provide real-time, continuous, autonomous, and all-weather information on carrier position, velocity, acceleration, attitude, etc.,meeting the requirements of underwater navigation for concealment and reliability, and it is currently the most important navigation method for underwater vehicles [4-7].However, the navigation accuracy of INS gradually decreases with time due to the accumulation of drift errors in the inertial devices, which cannot meet the accuracy requirements of long-duration underwater navigation [8,9].Therefore, other navigation methods are needed for auxiliary correction [10,11].Gravity-aided navigation, which uses the variation of the Earth's gravity field for positioning, does not require the emission and reception of signals,is less susceptible to external interference, and can meet the requirements of underwater navigation for concealment and positioning reliability.It is one of the most suitable navigation methods for assisting inertial navigation in underwater navigation [12,13].

The gravity-aided navigation obtains the gravity characteristic information of the current position through the gravity sensor and compares the real-time measurement value with the pre-stored ocean gravity field reference map.By estimating the position of the carrier through a certain matching criterion, the navigation error of the INS is corrected[14].The matching positioning effect of gravity-aided navigation is closely related to the gravity field characteristics of the area in which it is located.The submersible will pass through the gravity field feature-rich suitability area and the feature-poor non-suitability area during the navigation.In order to guarantee the positioning success rate and positioning accuracy of gravity-aided navigation,the route needs to be reasonably planned based on the suitability evaluation, so that it is always in the gravity suitability area to avoid the non-suitability area [15].Thus, the optimal route planning problem for underwater gravityaided navigation can be converted into a global path planning problem with the gravity unsuitability region as an obstacle.

Since the 20th century,scholars both domestic and abroad have proposed various methods for solving path planning problems,including artificial potential field method,A*algorithm,ant colony algorithm, and rapid exploring random tree algorithm, among others.The artificial potential field method models the environment as a virtual force field, treating obstacles as repulsive points and the target point as an attractive point,with the vehicle's motion determined primarily by the resultant force of obstacles and the target in the environment.This method has the advantages of a simple structure and ease of implementation, enabling rapid obstacle avoidance,but it is prone to oscillation in narrow passages and generates oscillations in front of obstacles, leading to the formation of trap regions and difficulties in discovering paths that pass through similar obstacles [16].The A* algorithm is a typical heuristic search algorithm that uses a cost function to obtain heuristic information related to the target point and guide the search direction.This algorithm finds the optimal path while maintaining high efficiency,but the design of the heuristic function can be challenging, and different heuristic functions can affect the algorithm's performance[17].The ant colony algorithm is a swarm intelligence algorithm developed based on the behavior of ants searching for food.By simulating the behavior of ants during the search process and continuously updating the concentration of pheromones and path selection probabilities, the path search gradually tends toward the global optimal solution.The ant colony algorithm has strong robustness and is easy to combine with other algorithms,but it also faces issues of low search efficiency and local optimization [18].Currently, researchers have applied artificial potential field method, A* algorithm, and ant colony algorithm to the path planning problem for underwater gravity-assisted navigation.Shi et al.improved the traditional A* algorithm by incorporating the performance constraints of the vehicle and utilizing Bezier curves to smoothen the path,which effectively enhanced the matching accuracy of the gravity algorithm [19].Feng et al.proposed an enhanced internal path planning method that determines the entropy value based on the importance of gravity parameters for track planning, resulting in more effective and accurate positioning of the submersible during long-endurance navigation[20].Lu et al.employed an ant colony algorithm with gravity correlation coefficients as heuristic factors and conducted path planning simulations based on local gravity anomaly maps,which yielded paths through nodes with relatively low gravity correlation coefficients[21].Zhang et al.reconstructed the heuristic function based on the ant colony algorithm using the artificial potential field method and introduced the maximum-minimum ant colony system to improve the pheromone update rule, leading to the proposed ant colonypotential field algorithm that reduced the chance of falling into local optimal solutions and improved the convergence speed [22].Nonetheless, when solving path planning problems in complex environments, such as narrow passages with numerous obstacle areas, the convergence rate of these algorithms significantly decreases,leading to prolonged path generation time.

Rapidly-exploring Random Trees (RRT) is a sampling-based algorithm that boasts a simple structure, fast search speed, and probabilistic completeness, making it an effective tool for solving path planning problems with complex constraints in multidimensional space[23].RRT-type algorithms have been widely employed in various fields, such as unmanned aerial vehicles (UAVs), autonomous vehicles, and mobile robots [24-28].However, its application in gravity-aided navigation path planning has yet to be explored in literature.While the traditional RRT algorithm can find feasible solutions, it may lack optimality and can suffer from slow convergence and long search time due to its reliance on random sampling in the search space.In contrast, the RRT*algorithm improves on the traditional approach by introducing a procedure for selecting a parent node in a hypersphere with a specific radius,and a rewiring procedure that identifies the lowest-cost path between nearby nodes [29-31].With an increasing number of sampling points, RRT* can achieve asymptotic convergence to the optimal solution.The convergence speed of RRT*is influenced by the radius of the hypersphere.Larger radii accelerate the solution speed but also exponentially increase the number of nodes in the hypersphere,leading to longer computation time.Some researchers have attempted to improve the efficiency of the RRT algorithm by implementing bi-directional expansion, which improves convergence speed to some extent but does not guarantee the optimality of the two paths searched [32,33].Others have decreased the probability of generating invalid nodes and accelerated traversal through narrow and congested areas by adjusting the expansion step size,using larger step sizes in open areas and smaller step sizes in narrow areas[34,35].Jeong et al.proposed the Q-RRT*algorithm by expanding the parent selection range to include the ancestors of nearby nodes within the hypersphere, which can identify initial paths with lower path costs without increasing time and space complexity [36,37].

In contrast to previous studies,this paper proposes a novel DSFS algorithm that significantly improves the efficiency of route planning for underwater gravity-aided navigation, by leveraging the unequal cost relationship between ancestors and descendants.A trajectory planning simulation experiment is conducted,comparing the DSFS algorithm against the Q-RRT* algorithm,which validates that the DSFS algorithm achieves reasonable path planning of underwater vehicles while further enhancing the planning efficiency.Salient contributions are summarized as follows:

(1) The unequal cost relationship between ancestors and their descendants was derived based on the basic rule of parent selection in random tree expansion.

(2) A new DSFS algorithm was proposed based on the cost inequality between ancestors and their descendants.By sorting candidate nodes by depth, node verification during the ChooseParent process and the Rewire process was reduced,effectively reducing the number of function calls for collision detection, further improving the expansion speed and convergence rate of the random tree.Additionally,it has the same optimality as the Q-RRT*algorithm.

(3) A DSFS path planning system was designed in conjunction with a gravity suitability distribution map.Trajectory planning simulation experiments validated that this system can realize the reasonable planning of underwater vehicle navigation paths and greatly enhance planning efficiency.

2.Methods

2.1.Problem definition

Let χ?Rdbe the planning space and d denote the dimension of the state space,d?N and d ≥2.This study is dedicated to the path planning of underwater gravity-aided navigation in two dimensions, so take d = 2.

Let χobs?χ be an impassable region in the planning space,then cl(χχobs)is the passable region χfreeand cl(?)denotes the closure of the set.Given the path planning parameters (χfree,xinit,χgoal),

2.2.The main idea of the DSFS algorithm

Compared with RRT*,the parent choice range for a new node of Q-RRT*adds the ancestors of the nearby nodes in the hypersphere,leading to an increase in the number of nodes to be selected.The DSFS algorithm filters these nodes in a layered order of depth.Layering can largely reduce the number of cost calculations and collision detections in the filtering process, thus shortening the running time of the planning algorithm.

In a random tree expansion,the cost of a node x to reach the root node xinitis related to the cost of its parent xparentto reach the root node xinitas follows：

2.3.Depth-sorting ChooseParent procedure

When choosing a parent for a new node xnew, according to the inequality relationship between the ancestor and its descendants in Eq.(3),if the cost of xnewwhen a node becomes its parent is not less than the current cost cmin,then it is not necessary to compute and verify all descendants of that node to eliminate them from the candidate set.If the cost of xnewwhen a node is its parent is less than the current cost cminand the route between them is passable,then that node will replace the original parent as the new current parent without continuing to verify all the descendants of that node.In summary, the set of parents to be chosen retains only all descendants of nodes whose route to xnewis impassable but can make xnewsmaller than the current cost cmin.This mechanism of layered filtering by the depth of ancestors is used to design a depthsorting ChooseParent procedure, as shown in Algorithm 1.

The primitive procedures involved in Algorithm 1 are described below:

? Steer: Given two states xs, xd?χ, the function Steer(xs,xd)returns the straight path σ from σ(0)=xsto σ(1) = xd.

? CollisionFree: Given a path σ, the function CollisionFree(σ)returns the logical value true if it is passable (i.e., ?τ?[0,1],σ(τ)?χfree), otherwise returns the logical value false.

? Descendant: Given a graph G = (V, E), a vertex x?V and a natural number,the function Descendant(G,x,k)returns all the kth level descendants of x.

2.4.Depth-sorting Rewire procedure

The purpose of the rewiring process is to choose new parents for the nearby nodes in the set Xnearand make their costs lower.In rewiring, the cost relationship between an ancestor and its descendants can also be exploited to reduce node computation by sorting ancestors by depth.The pseudo-code for the depth-sorting Rewire procedure is given in Algorithm 2.

The primitive procedures involved in Algorithm 2 are described below:

? Reconnect:Given a graph G =(V,E),two vertices xp,x?V and a path σ from xpto x, the function Reconnect(G,xp,x,σ) replaces the edge in E that connects x to its parent with the edge representing σ and returns the graph G.

images/BZ_293_973_318_1001_341.pngimages/BZ_293_676_1714_705_1737.pngimages/BZ_293_931_1786_1070_1821.pngimages/BZ_293_728_2720_757_2743.pngimages/BZ_293_984_2792_1123_2827.png

?

Algorithm 2.Rewire- DSFS(T,xnew,Xnear,n)1: for all depth=0 to n do 2: xdepthnewpar←Ancestry(T,{xnew},depth);3: end for 4: for all xnear?Xnear do 5: for all depth=n to 0 (step = - 1) do 6:σ←Steer(xdepthnewpar,xnear);7: if Cost(xdepthnewpar)+c(σ)

2.5.DSFS path planning system for underwater gravity-aided navigation

The positioning success rate and positioning accuracy of gravityaided navigation system are closely related to the gravity characteristics of the region.When the gravity field characteristics are rich, i.e., the gravity characteristic values vary widely, the positioning points are easy to identify and match, and thus the positioning effect is excellent; on the contrary, when the gravity characteristics are not obvious, it is easy to mismatch, resulting in poor positioning effect.In order to improve the matching efficiency of underwater gravity-aided navigation,it is necessary to combine the gravity field distribution characteristics of the navigation area to select the region favorable for gravity matching before the path generation.This paper designed a DSFS path planning system for underwater gravity-aided navigation based on the depth sorting principle with the acquired gravity anomaly data of the navigation area,as shown in Fig.1.

The DSFS path planning system consists of two parts:the gravity suitability division module and the path generation and optimization module.The gravity suitability division module processes gravity anomaly data and submarine terrain data in the navigation area, determines gravity suitability and divides the passable and impassable areas, which is an important prerequisite for route planning; the path generation and optimization module obtains a safe and collision-free path that satisfies the optimization objective through the planning algorithm with the acquisition of the location information of the starting point and the goal point.

Fig.1.Schematic diagram of the DSFS path planning system.

2.5.1.Gravity suitability division

A sea area is selected as the planning space for navigation,with a longitude range of 112°E-115°E and a latitude range of 10°N-11°N.The gravity anomaly data and seafloor topography data used in this paper were obtained from the website of Scripps Institution of Oceanography, University of California, San Diego (https://topex.ucsd.edu/), both with a resolution of 1′× 1′.The maximum value of gravity anomaly is 133.4 mGal,the minimum value is-32.4 mGal,the average value is 14.8 mGal, and the standard deviation is 27.2 mGal;the maximum value of depth is 0 m,the minimum value is -4391 m, the average value is -2381.6 m, and the standard deviation is 1118.2 m.

The suitability of the gravity field can be measured by a variety of metrics, and different metrics reflect different aspects of the properties[38,39].The gravity-aided navigation working area could be evaluated more comprehensively and effectively by fusing the information of multiple gravity characteristic parameters.However,as this paper does not focus on the method of dividing the suitability, we only use a single characteristic parameter, the gravity anomaly standard deviation, to divide the area into suitable and non-suitable regions for gravity-aided navigation.

Set the mobile computing window with a resolution of 10′×10′,determine the gravity suitability area according to the gravity anomaly standard deviation s[40],and the selection criteria for the suitability area is

where s0is the threshold value for dividing the suitability area and non-suitability area.

For each gravity observation point, the standard deviation of gravity anomalies of its surrounding points is calculated.Then,the standard deviation threshold is set to different values (this paper sets the threshold to 1, 2, …, 10, and 10 groups of matching experiments are conducted)to test the position estimation of gravitymatching regions using the gravity matching algorithm.Through a large number of matching experiments and statistical analysis of the positioning error and matching success rate under different thresholds, the region with a standard deviation of gravity anomalies greater than 5 has better suitability.Therefore, the threshold of s0=5 is selected.The established two-dimensional grids of gravity suitability are shown in Fig.2.The black part is the grid of the non-suitability area, and the white part is the grid of the suitability area.

In addition,to ensure navigation safety,the planned path needs to avoid regions with dense distribution of shoals and reefs.Therefore,the topographic data of the seafloor was used to extract the distribution map of the dangerous region (e.g., the safe-depth was set to 100 m), as shown in Fig.3.The navigation dangerous region in Fig.3 is removed from Fig.2, and the gravity suitability distribution obtained is shown in Fig.4.Among them,the black part is the non-suitability area with a poor positioning matching effect;the gray part is the dangerous area, which is not suitable for navigation; the white part is the suitability area capable of safe navigation, and the planned route can only reach the destination via this area.For collision detection,the path σ to be tested is evenly divided into L equal parts, and the suitability characteristics of the area in which each endpoint and equal point is located are detected in turn according to the latitude and longitude coordinates.If the position coordinates of both endpoints and all equal points are in the suitability area(white part in Fig.4),it means that the route σ is safe to navigate and suitable for matching, and is judged to be passable, otherwise it is judged to be impassable.

Fig.2.Suitability division of gravity matching area (theory).

Fig.3.Distribution of unsafety area.

Fig.4.Suitability division of gravity matching area.

2.5.2.Path generation and optimization

The DSFS path planning algorithm is designed based on the idea of depth ordering to generate and optimize the navigation route for underwater gravity-aided navigation.The DSFS planning algorithm takes the starting point as the root node of the random tree and fast expands the tree by sampling randomly in the search space.Since the non-suitability areas that are not rich in gravity characteristics have been classified as impassable areas, the positions of the new nodes acquired at sampling are those with high positioning success rate and accuracy.By this way,the planned paths circumvent those regions with poor gravity suitability,so that the navigation always maintains well matching performance.When the leaf node is expanded to the goal point or the goal region, a path between the root node and the goal point consisting of edges of the random tree is obtained.Algorithm 3 details the DSFS path planning algorithm.

Algorithm 3.DSFS(V,E)1: T←(V,E);2: for i=0 to N do 3: if rand()<λ then 4: xrand←Sample();5: else 6: xrand←xgoal;7: end if 8: xnearest←Nearest(T,xrand);9: xnew←Extend(xnearest,xrand,ρ);10: σ←Steer(xnearest,xnew);11: if CollisionFree(σ) then 12: Xnear←Near(T,xnew,r);13: (xparent,σparent)←ChooseParent-DSFS(T,Xnear,xnearest,xnew,σ,n) ;14: T←Connect(T,xparent,xnew,σparent);15: T←Rewire- DSFS(T,xnew,Xnear,n);16: end if 17: end for 18: return T;

The primitive procedures involved in Algorithm 3 are described below.

? rand: The function rand() returns a random number in the interval (0,1).

? Sample: The function Sample() returns a randomly sampled state from χfree.

? Nearest: Given a graph G=(V,E) and a state, the function Nearest(G,x) returns the nearest vertex to x in V.

? Extend:Given two states x,x′?χ and a step size ρ,if d(x,x′)< ρ,the function Extend(x,x′,ρ) returns x′,otherwise it returns the state at a distance ρ from x in the direction of -xx′→.

? Near: Given a graph G = (V,E), a state x, and a radius r, the function Near(G,x,r)returns all vertices in V that are contained in the hypersphere centered at x and radius r.

? Connect:Given a graph G =(V,E),a vertex xp?V ,a state x?χ and a path σ from xpto x,the function Connect(G,xp,x,σ)adds x to V,adds the edge representing σ to E,and returns the graph G.In the DSFS path planning algorithm, to avoid the failure of“hitting the wall” of the tree due to the existence of obstacles, the Bias-goal idea is adopted[41,42].Specifically,the growth direction of the random tree is controlled by the goal point, so that the tree has a certain probability of extending towards the goal point each time it chooses a growth direction, and a certain probability of randomly selecting a direction in the map to extend a certain distance,thus ensuring both the randomness of the algorithm and the efficiency of expansion.The specific method is to set a probability threshold,λ.If the generated random value is less than λ,a random sampling point is selected in the χfree-space, otherwise the goal point xgoalis directly used as the sampling point.i.e.

Once the first path connecting the start point to the goal point is found, intermediate nodes that make the path less costly could be found through the continued expansion of nodes.As the number of sampling points increases, the planned path will gradually approach the optimal path.It can be set to update the current optimal path sequence every K nodes added in the random tree,and stop the expansion to end the path planning when the total cost remains fixed for Q consecutive times.

2.6.The complexity analysis of the DSFS algorithm

Table 1 Time complexity of the certain primitive procedures of existing RRT-type algorithms.

The space complexity is defined as the amount of memory space used by a given algorithm.Like the RRT, RRT*, and Q-RRT* algorithms, the DSFS algorithm maintains a tree structure of Gm=(Vm,Em),where the size of Gmdetermines the memory space used, hence the space complexity of the DSFS algorithm is O(m).

3.Results and discussion

In this section, we conducted comparative simulation experiments between the Q-RRT*algorithm and the DSFS algorithm using MATLAB R2018b as the software platform to verify the effectiveness of the proposed route planning algorithm.The starting point was set to (112.4°E,10.8°N) and the target point was set to(115.5°E,10.9°N)based on their respective latitudes and longitudes.The path planning process was terminated either when the updated path cost remained fixed for Q consecutive times or when the maximum number of iterations N was reached.Table 2 presents the parameters involved in the path planning experiments conducted in this paper.

Moreover, Table 1 reveals that collision detection during the ChooseParent process and the Rewire process is the primary computational bottleneck.In the ChooseParent process, the number of cost calculations for xnewreflects the number of candidate parents, while the number of cost calculations for nearby nodes during the rewiring process represents the number of nodesvalidated.Therefore, to evaluate the effectiveness of reducing computational burden during the ChooseParent process and the Rewire process using the proposed DSFS algorithm,the number of cost calculations and the number of collision detections are utilized as evaluation metrics.

3.1.Validation of depth-sorting ChooseParent procedure

In the planning environment above, the ChooseParent procedure of DSFS was tested in comparison with the ChooseParent procedure of Q-RRT*.Each time when a new node xnewis obtained forthe random tree, the two ChooseParent procedures are used to compute once for node xnewrespectively.The number of cost calculations, the number of collision detections and the computation time of the process were recorded separately for the two methods,and the two parent results were compared at the end of the process.If the two results do not agree, the case is recorded and the parent with the lower cost is chosen to continue the planning process.After the whole planning computation,the total number of cost computations and the total number of collision detections and the total time spent on the two ChooseParent procedures were counted for the whole planning process.Due to the randomness of node acquisition of the RRT-type algorithm, 50 simulation comparison experiments were conducted to ensure the fairness of the test.

Table 2 Parameters involved in path planning experiments.

Fig.5 shows the number of cost calculations for both methods during the process of choosing a parent,and the broken line of DSFS is located below Q-RRT* indicating that the DSFS algorithm can significantly reduce the number of cost calculations in the planning process.The schematics of the nodes involved in cost calculation at a certain xnewwhen completing the choosing of the parent for both methods were drawn to more visually demonstrate the effect of the reduction in the number of nodes involved in the calculation, as shown in Fig.6,and the upper image is a partial enlargement of the dotted box part of the lower image.Point A denotes the starting point xinit, point B denotes the goal point xgoal, the pentagram denotes the node xnew,the green circle denotes the selection range of the set Xnear,and the nodes in the circle(nodes with serial number 4 to number 12 in Fig.6(a))are the nearby nodes of xnew.The nodes in the random tree that are involved in the cost calculation are marked with a red plus sign.It can be found that the Q-RRT* algorithm needs to verify that the number of nodes computed is twelve(Fig.6(a)),while the DSFS algorithm only needs to compute two nodes (Fig.6(b)) to obtain the same result as the Q-RRT*algorithm.

Fig.7 shows the number of collision detections for the ChooseParent procedure,and the blue bar graph shows the difference in the number of collision detections.Statistically,it can be seen that the number of collision detections for the ChooseParent procedure of DSFS is less than that of Q-RRT* (40/50).In addition, observing the blue histogram,we can find that when the number of collision detections is smaller for Q-RRT* than for DSFS, the difference is small (below the zero-value line), while when the number of collision detections for DSFS is smaller than for Q-RRT*, the difference is larger (above the zero-value line) several times, which indicates that the ChooseParent procedure of DSFS can reduce the number of collision detections to a greater extent in most cases.

Fig.8 shows the computation time of the ChooseParent procedure for both methods.It can be seen that the computation time of DSFS is significantly reduced compared to that of the Q-RRT*

Fig.5.Number of cost calculations in the ChooseParent procedure.

Fig.6.Schematics of the nodes involved in cost calculation at a new node when completing choosing a parent: (a) Using the ChooseParent procedure of Q-RRT*; (b)Using the ChooseParent procedure of DSFS.

algorithm,indicating that the hierarchical filtering has a significant improvement in the computation speed of the ChooseParent procedure.The number of cost calculations, the number of collision detections and the time spent on the choosing process for the 50 experiments counted are averaged to statistically reflect the computing performance improvement effect of hierarchical filtering on the ChooseParent procedure, as shown in Table 3.The ChooseParent procedure of DSFS reduces the average number of cost calculations by about 63.6%, the average number of collision detections by about 7.8%,and the average computation time of the process from 12.6 to 7.4 s (a reduction of about 41.3%) compared with Q-RRT*.In addition, no inconsistent results occurred during the simulation experiments, indicating that the depth ordering of ancestors can effectively improve the computational performance while ensuring correct computational results.

Fig.7.Number of collision detections in the ChooseParent procedure.

Fig.8.Computing time of the ChooseParent procedure.

Table 3 Computing performance of the ChooseParent procedure.

3.2.Validation of depth-sorting Rewire procedure

To verify the effectiveness of depth sorting for improving the speed of the Rewire procedure, the Rewire procedure of Q-RRT*and that of DSFS were used to perform one rewiring for each node xnearin the set Xnear, respectively.Record the number of cost calculations, the number of collision detections, and the calculation time of the rewiring process, and compare the parents of the two methods at the end of the rewiring for each node xnear.In case of inconsistency, the case was recorded and the node with the lower cost was selected as the parent to continue the planning process.After the planning, the total number of cost calculations, the total number of collision detections and the total duration of the rewiring process were counted in the whole process,and a total of 50 simulations of the Rewire procedure were performed.

Fig.9.Number of cost calculations in the Rewire procedure.

Fig.9 shows the number of cost calculations for the two Rewire procedures, from which it can be seen that DSFS has significantly fewer calculations than Q-RRT*, indicating that the Rewire procedure of DSFS can reduce a large number of useless node calculations to improve computational efficiency.Fig.10 shows the number of collision detections for the two Rewire procedures,and the blue bar graph shows their difference.The number of collision detections for the Rewire procedure of DSFS is reduced in most cases (42/50), and in a few cases is equal to that of Q-RRT* (8/50),but there is no increase.Due to the inclusion of the depth sorting mechanism,the number of collision detections is reduced when the final parent node of xnearis in the set xdepthnewparand nodes with a depth less than its final parent can pass the cost detection.

Fig.10.Number of collision detections in the Rewire procedure.

Fig.11.Computing time of the Rewire procedure.

Table 4 Computing performance of the Rewire procedure.

Fig.11 shows the computation time of the rewiring process.It can be seen that the computation time of the Rewire procedure of DSFS is significantly reduced compared to that of Q-RRT* in all 50 experiments.Table 4 numerically reflects the computing performance of the Rewire procedure for Q-RRT* and DSFS.The Rewire procedure of DSFS reduces the average number of cost calculations by about 64.6%, the average number of collision detections by about 0.6%, and the average computation time of the rewiring process from 10.1 to 4.3 s (a reduction of about 57.4%)compared to that of Q-RRT*.In addition, no inconsistent rewiring results were found throughout the test,indicating that the Rewire procedure of DSFS can ensure the correctness of the rewiring results while reducing the computational effort and effectively improving the computation speed.

3.3.Verification of DSFS path planning algorithm

After each acquisition of a new node, the ChooseParent procedure and the Rewire procedure were performed once each using Q-RRT*and DSFS to test the effectiveness of the DSFS algorithm in improving the efficiency of underwater gravity-aided navigation path planning.The number of cost calculations, the number of collision detections, and the total computation time of both procedures were counted corresponding to the ChooseParent procedure and the Rewire procedure for both algorithms.After each completion of the ChooseParent procedure or the Rewire procedure of the two methods, compare their results, record the situation if they do not agree and choose the parent with a lower cost to continue the planning process.A total of 50 path planning simulation tests were performed.

Fig.12 shows the number of cost calculations in the Choose-Parent procedure and the Rewire procedure.The statistical results show that the number of cost calculations for both processes of DSFS is significantly reduced compared to Q-RRT*.Fig.13 shows the total number of cost calculations for both processes,indicating that the total number of cost calculations for DSFS is smaller than that of Q-RRT*.

Fig.14 shows the number of collision detections in the ChooseParent procedure and the Rewire procedure for both algorithms.It can be seen that the number of collision detections of DSFS is significantly reduced overall in the ChooseParent procedure.Fig.15 shows the total number of collision detections for both processes of Q-RRT* and DSFS, and the blue bar shows the difference in the number of collision detections.The results show that the DSFS algorithm can reduce the number of collision detections in most cases.

Fig.12.Number of cost calculations in the ChooseParent procedure and the Rewire procedure.

Fig.13.Total number of cost calculations.

Fig.14.Number of collision detections in the ChooseParent procedure and the Rewire procedure.

Fig.15.Total number of collision detections.

Fig.16 shows the total time duration of the ChooseParent procedure and the Rewire procedure for Q-RRT* and DSFS.It is seen that DSFS took less time than Q-RRT*for each experiment.Table 5 reflects the computing performance of Q-RRT*and DSFS.Compared with Q-RRT*,DSFS reduces the total number of cost calculations by about 65.2%on average,the total number of collision detections by about 3.8% on average, and the total computation time for both processes is reduced from 25 to 11.4 s, an average reduction of about 54.4%(a 1.2-fold improvement in efficiency).And there were no inconsistent results between the two methods throughout the testing process.

Fig.17 shows a path obtained by the DSFS path planning system for underwater gravity-aided navigation.Point A denotes the starting point xinit,point B denotes the goal point xgoal,and the path(red line segment)follows the edges of the random tree from xinitto xgoal.

Fig.16.Computing time of the ChooseParent procedure and the Rewire procedure.

Table 5 Total computing performance of ChooseParent procedure and the Rewire procedure.

Fig.17.A path obtained by the DSFS path planning system for underwater gravityaided navigation.

In summary, with the parameters set in this paper, the DSFS algorithm proposed can reduce the total computation time of the ChooseParent procedure and the Rewire procedure by about 54.4%compared with the Q-RRT* algorithm, effectively improving the efficiency of underwater gravity-aided navigation planning.

4.Conclusions

This paper proposed a Depth Sorting Fast Search (DSFS) algorithm with the aim of enhancing the efficiency of path planning for underwater gravity-aided navigation.

(1) The DSFS algorithm was developed to address the computational burden caused by adding the ancestors of nearby nodes to the range of nodes in the ChooseParent and Rewire procedures of the Q-RRT* algorithm.To achieve this, the DSFS algorithm employs a cost inequality relationship between an ancestor and its descendants to filter nodes,resulting in a reduction of computation by minimizing the number of node verifications and collision detections.

(2) A DSFS path planning system for underwater gravity-aided navigation was designed based on the proposed algorithm.This system consists of a gravity suitability division module and a path generation and optimization module.It satisfies the safety and asymptotic optimality requirements and provides a reasonable route planning solution for underwater vehicles.

(3) The effectiveness of the DSFS algorithm was verified through 50 experiments on the ChooseParent procedure, the Rewire procedure,and the combination of both.Results showed that the DSFS algorithm reduced the computation time by approximately 54.4% and improved the planning efficiency by about 1.2 times compared to the Q-RRT*algorithm.Thus,the DSFS algorithm is a valuable tool for enhancing the planning efficiency of underwater gravity-aided navigation.

Declaration of competing interest

The authors declare that they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper.

Acknowledgments

This work was supported in part by the National Natural Science Foundation of China (Grant No.42274119), in part by the Liaoning Revitalization Talents Program(Grant No.XLYC2002082),in part by National Key Research and Development Plan Key Special Projects of Science and Technology Military Civil Integration (Grant No.2022YFF1400500), and in part by the Key Project of Science and Technology Commission of the Central Military Commission.

The authors are thankful to the University of California San Diego website for providing the gravity anomaly data and seafloor topography data.