Method for improving underwater navigation path planning efficiency based on fast search of depth ordering

Abstract

A method for improving underwater navigation path planning efficiency based on quick search of depth ordering, comprising: processing gravity anomaly data and seabed topography data in a navigation area to divide into passable area and impassable area; based on the passable area, taking the starting point as the root node of the random tree, gradually expanding the tree by random sampling in the search space, when the leaf node is expanded to the target point or target area, a path from the starting point to the target point composed of the edges of the random tree is obtained; when the first path connecting the starting point and the target point is found, the optimal path is obtained by continuing to expand the node to find the intermediate node with lower path cost. In the process of reselecting the parent node, the ancestor nodes of the adjacent nodes are hierarchically screened in depth order, which can significantly reduce the number of parent node verification and collision detection, and further speed up the expansion speed and convergence speed of the random tree.

Description

Technical Field

[0001] This invention relates to the interdisciplinary fields of underwater navigation and marine surveying, and in particular to a method for improving the efficiency of underwater navigation path planning based on depth sorting fast search. Background Technology

[0002] Underwater gravity-assisted navigation utilizes gravity characteristics to correct the accumulated errors of inertial navigation systems, providing accurate position information. The matching positioning effect of gravity-assisted navigation is closely related to the gravity field characteristics of the area. Properly planning the route to ensure it remains within the gravity adaptation zone plays a crucial role in improving the positioning accuracy of gravity-assisted navigation systems.

[0003] Rapid Exploratory Random Tree (RRT) is a sampling-based path planning algorithm. Its simple structure and probabilistic completeness make it suitable for solving path planning problems in complex environments. The RRT* algorithm adds a process of reselecting parent nodes and rewiring, thus finding optimal path solutions. The Q-RRT* algorithm, building upon the RRT* algorithm, adds ancestor nodes of neighboring nodes in the hypersphere to the parent node selection range, enabling the finding of initial paths with lower costs. However, the solution speed of both the RRT* and Q-RRT* algorithms is affected by the radius of the hypersphere; as the radius increases, the number of nodes in the hypersphere grows exponentially, leading to longer computation times. Furthermore, due to the random sampling principle in the search space, it can result in longer search times and slower convergence speeds when encountering narrow passages with many obstacles. Summary of the Invention

[0004] The technical problem to be solved by this invention is to overcome the shortcomings of the prior art and solve the problem of underwater gravity-assisted navigation path planning efficiency.

[0005] The objective of this invention is achieved through the following technical solutions:

[0006] A method for improving the efficiency of underwater navigation path planning based on depth-ordered fast search includes:

[0007] The gravity anomaly data and seabed topography data within the navigation area are processed to delineate navigable and impassable areas.

[0008] Based on the passable area, the starting point is used as the root node of the random tree. The tree is gradually expanded by randomly sampling in the search space. When the leaf node expands to the target point or target area, a path from the starting point to the target point is obtained, which is composed of the edges of the random tree.

[0009] Once the first path connecting the starting point and the target point is found, the optimal path is obtained by continuing to expand the nodes to find intermediate nodes that reduce the cost of the path.

[0010] Preferably, during sampling, a target bias approach is adopted, using the target point to control the growth direction of the random tree.

[0011] Preferably, the depth-sorting fast search method filters nodes in the Q-RRT* algorithm in layers according to depth order, thereby reducing the number of path cost calculations and collision detections during the selection process.

[0012] Preferably, in the depth-sort fast search method, for a new node, the set of candidate parent nodes of the new node retains only all descendant nodes of nodes that can make the new node less than the current path cost and whose routes with the new node are inaccessible.

[0013] Preferably, in the depth-sorted fast search method, a new parent node with a smaller depth is only explored when the cost of a certain path is less than the current value and the collision detection indicates that the path is impassable.

[0014] Preferably, the gravity-assisted navigation adaptation zone and non-adaptation zone are divided based on a single characteristic parameter, the standard deviation of gravity anomaly, and the planned route can only reach the destination through this zone.

[0015] Preferably, the computational load of nodes is reduced by sorting ancestor nodes by depth, utilizing the path cost relationship between ancestor nodes and their descendant nodes.

[0016] Preferably, when the cost of the path remains constant for M consecutive times, the expansion is stopped and the path planning ends.

[0017] Preferably, depth sorting of the nodes to be selected reduces the number of nodes to be computed during the process of reselecting parent nodes and rewiring, thereby accelerating the expansion speed of the random tree.

[0018] A system for improving underwater navigation path planning efficiency based on depth-sorting fast search includes:

[0019] The gravity adaptation zone delineation module processes gravity anomaly data and seabed topography data within the navigation area to delineate traversable and impassable areas.

[0020] The path generation and optimization module uses the starting point as the root node of a random tree based on the passable area. It gradually expands the tree by randomly sampling in the search space. When the leaf node expands to the target point or target area, a path from the starting point to the target point is obtained, which consists of the edges of the random tree. After finding the first path connecting the starting point and the target point, the module continues to expand the nodes to find intermediate nodes that reduce the path cost and obtain the optimal path.

[0021] Compared with the prior art, the present invention has the following advantages:

[0022] (1) In the process of reselecting the parent node, the present invention can significantly reduce the number of candidate parent node verifications and collision detections by screening the ancestor nodes of neighboring nodes in depth order, thereby accelerating the expansion and convergence speed of the random tree.

[0023] (2) In the rerouting process of this invention, by performing depth sorting on the new extended nodes and their ancestor nodes, the number of path cost calculations and collision detections is greatly reduced, which effectively improves the calculation speed of the rerouting process.

[0024] (3) This invention transforms the optimal route planning problem of underwater gravity-assisted navigation into a global path planning problem that takes gravity misfit areas as obstacles. By combining the gravity adaptability distribution map, the navigation path of the underwater vehicle can be reasonably planned so that it always stays in the gravity adaptability area and avoids the misfit area, thereby ensuring the positioning success rate and positioning accuracy of gravity-assisted navigation.

[0025] (4) This invention reduces the running time of the path planning algorithm and the speed of convergence to the optimal solution, thereby improving the path planning efficiency of underwater gravity-assisted navigation. Attached Figure Description

[0026] Figure 1 A theoretical diagram of gravity adaptability distribution;

[0027] Figure 2 Map showing the distribution of hazardous areas;

[0028] Figure 3 This is a gravity adaptability distribution map;

[0029] Figure 4 The number of path cost calculations for the process of reselecting the parent node;

[0030] Figure 5 A schematic diagram of the nodes involved in path cost calculation; Figure 5 a is a node diagram of the Q-RRT* parent node reselection algorithm; Figure 5 b is a schematic diagram of the DSFS parent node reselection algorithm;

[0031] Figure 6 The number of collision checks during the parent node reselection process;

[0032] Figure 7 The computation time for the process of reselecting the parent node;

[0033] Figure 8 Number of path cost calculations for the rerouting process;

[0034] Figure 9 The number of collision detections during the rewiring process;

[0035] Figure 10Calculate the time required for the rewiring process;

[0036] Figure 11 The number of path cost calculations for the parent node reselection and rerouting processes in the Q-RRT* and DSFS algorithms;

[0037] Figure 12 The total number of path cost calculations for the Q-RRT* algorithm and the DSFS algorithm;

[0038] Figure 13 The number of collision detections during the parent node reselection and rerouting processes;

[0039] Figure 14 This represents the total number of collision detections performed by the Q-RRT* and DSFS algorithms.

[0040] Figure 15 The total computation time for the parent node reselection and rerouting processes;

[0041] Figure 16 The path obtained by the underwater gravity-assisted navigation DSFS path planning system. Detailed Implementation

[0042] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be described in further detail below with reference to the accompanying drawings.

[0043] This invention addresses route planning for underwater gravity-assisted navigation. To further improve the planning efficiency of the Q-RRT* algorithm, a novel depth-sorting fast search method is proposed based on the path cost inequality between ancestor nodes and their descendants during random tree expansion. By performing depth sorting on the nodes to be selected, the number of node calculations in the process of reselecting parent nodes and rewiring is reduced, thereby accelerating the expansion speed of the random tree and effectively reducing the running time of the planning algorithm. The effectiveness of the algorithm in improving planning efficiency is verified by trajectory planning simulation based on a gravity adaptability distribution map.

[0044] The underwater gravity-assisted navigation path planning method based on a novel depth-ranking fast search method includes:

[0045] set up For the planning space, d represents the dimension of the state space. And d≥2. This invention focuses on path planning in a two-dimensional plane for underwater gravity-assisted navigation, therefore d=2 is chosen.

[0046] set up To define inaccessible areas within the planned space, then cl(χ\χ) obs ) represents the passable area χ free cl(·) denotes the closure of the set. Given path planning parameters (χ...free ,x start ,χ goal ), x start ∈χ free Indicates the starting point of the plan. Indicates the target region, typically χ goal ={x∈χ|d(x,x goal )<R}, where x goal ∈χ free Let represent the target point, and d(·,·) represent the Euclidean distance between the two points. Bounded variation continuous function. A path can be represented, and the planned path σ must satisfy... σ(τ)∈χ free And σ(0)=x start , σ(1)=cl(χ goal Optimal path planning is the process of finding a feasible path σ. * This path has the lowest cost, i.e., c(σ). * )=min{c(σ):σ∈∑ f}, where c(·) is the path cost calculation function, ∑ f It is the set of all feasible paths.

[0047] This invention proposes an underwater gravity-assisted navigation (DSFS) path planning method and system based on the concept of depth sorting. The DSFS path planning system can be broadly divided into two parts: a gravity adaptation zone delineation module and a path generation and optimization module. The gravity adaptation zone delineation module processes gravity anomaly data and seabed topography data within the navigation area to determine gravity adaptation characteristics and delineate traversable and impassable areas, serving as a crucial prerequisite for route planning. The path generation and optimization module, after acquiring the starting and target point location information, uses a planning algorithm to obtain a safe, collision-free path that meets the optimization objectives.

[0048] Obtain the initial parameters required for planning. Let... For the planning space, d represents the dimension of the state space. And d≥2. This invention focuses on path planning in a two-dimensional plane for underwater gravity-assisted navigation, therefore d=2 is chosen. Let... To define inaccessible areas within the planned space, then cl(χ\χ) obs ) represents the passable area χ free cl(·) denotes the closure of the set. Given path planning parameters (χ... free ,x start ,χ goal ), x start ∈χ free Indicates the starting point of the plan. Indicates the target region, typically χ goal ={x∈χd(x,x goal )<R}, where x goal ∈χ fre Let represent the target point, and d(·,·) represent the Euclidean distance between the two points. Bounded variation continuous function. A path can be represented, and the planned path σ must satisfy... σ(τ)∈χ free And σ(0)=x start , σ(1)=cl(χ goal Optimal path planning is the process of finding a feasible path σ. * This path has the lowest cost, i.e., c(σ). * )=min{c(σ):σ∈∑ f}, where c(·) is the path cost calculation function, ∑ f It is the set of all feasible paths.

[0049] A specific sea area is selected as the navigation space for the underwater vehicle. The point with the smallest latitude and longitude within this planned space is set as the origin O. The x-axis is defined along the direction of increasing longitude, and the y-axis along the direction of increasing latitude, thus establishing a planned spatial coordinate system. The geographical coordinates of any point ξ within the navigation planning space are denoted as follows: The planning coordinates relative to the planning spatial coordinate system are denoted as ξ. 2 (x ξ ,y ξ If ξ is the spatial coordinate, then the spatial coordinates are planned. 2 With geographical coordinates ξ 1 The conversion relationship is as follows:

[0050]

[0051] In the formula, R = 6371393m, which is the average radius of the Earth; The coordinates of the origin O are the geographical coordinates.

[0052] The performance of gravity-assisted navigation is closely related to its adaptability to the working area. The adaptability of the gravity field can be measured by various indicators, and different indicators can reflect different aspects of the characteristics of the gravity field. By fusing information from multiple gravity characteristic parameters, the working area of ​​gravity-assisted navigation can be evaluated more comprehensively and effectively. However, since the method of dividing the adaptation zone is not the focus of this invention, this invention divides the adaptation zone and the non-adaptation zone of gravity-assisted navigation based on a single characteristic parameter, the standard deviation of gravity anomalies.

[0053] Set a moving calculation window with a resolution of 10′×10′, and determine the gravity adaptation zone based on the standard deviation s of the gravity anomaly. The selection criteria for the adaptation zone are as follows:

[0054] s>s0

[0055] In the formula, s0 is the standard deviation threshold for dividing the fit region into the non-fit region.

[0056] To ensure navigational safety, the planned route needs to avoid areas with dense distribution of shoals and reefs. Therefore, by using seabed topographic data within the planning area and setting safe water depths, a raster map of hazardous areas is extracted. Hazardous areas are then removed from the planning area, resulting in a gravity compatibility distribution map. Black represents gravity-misfit areas with poor positioning matching; gray represents hazardous areas unsuitable for navigation; and white represents gravity-assisted navigation compatible areas where safe navigation is possible, and the planned route can only reach the destination through these areas.

[0057] This invention utilizes the depth-first search (DSFS) path planning algorithm to plan and optimize navigation routes for underwater gravity-assisted navigation. The algorithm uses the starting point as the root node of a random tree and gradually expands the tree by randomly sampling in the search space. When a leaf node expands to the target point or target region, a path from the starting point to the target point, composed of the edges of the random tree, is obtained. The specific steps of the DSFS path planning algorithm are as follows:

[0058] Step 1: Initialize parameters: Random tree T = (V, E), root node is the planning starting point (i.e., x). init =x start The pathfinding success identifier FindPath = 0.

[0059] Step 2: Random sampling is performed within the planned space χ using a target bias approach. Specifically, a probability threshold λ is set, and the rand function is used to generate random values ​​in the interval (0,1). If the random value is less than λ, then the sample is taken within the passable area χ. free A sampling point x is randomly selected from the data. random Otherwise, directly set the target point x goal As sampling points, i.e.

[0060]

[0061] Step 3: Traverse all nodes in the random tree T and find the node that corresponds to the sampling point x. rand The nearest node x nearest .

[0062] Step 4: with x nearest With the origin as the starting point and the step size ρ, along... Directional expansion yields a new node x new If x nearest With x rand If the distance is less than the step size ρ, then the random sampling point x will be... rand Let x be the extended node new .

[0063] Step 5: Perform x nearest and x new Collision detection for straight paths between them (CollisionFree(x)) nearest ,x new During collision detection, this invention divides the path σ to be tested into L equal parts. Based on latitude and longitude coordinates, the adaptation characteristics of the regions where the endpoints and the division points are located are detected sequentially. If the position coordinates of the two endpoints and all division points are within the adaptation zone where safe navigation is possible, it indicates that the route σ is safe to navigate and suitable for matching, and is determined to be passable; otherwise, it is determined to be impassable. If x nearest and x new If the straight path between them is passable, proceed to step 6 to reselect the parent node; if it is not passable, return to step 2 to resample.

[0064] Step 6: Reselect the parent node. For the new node x new When reselecting a parent node, the Q-RRT* algorithm expands the selection range to include the ancestor nodes of neighboring nodes in the hypersphere, thus increasing the number of candidate nodes. This invention uses a depth-ordered layered selection method to significantly reduce the number of path cost calculations and collision detections during the selection process, thereby shortening the algorithm's runtime.

[0065] During the expansion of a random tree, a node x reaches the root node x. init The path cost is related to its parent node x parent Reaching the root node x init The path costs have the following relationship:

[0066] Cost(x) = d(x, x parent )+Cost(x parent (1)

[0067] Where Cost(·) is the path cost from a node to the root node.

[0068] Let the ancestor nodes of node x with depths of 1, ..., n be respectively And the ancestor node with depth 0 is itself, that is

[0069] Based on the basic rules for selecting parent nodes, we can obtain:

[0070] The path cost of x from its ancestor node of depth p to the root node is no greater than the path cost of x from its ancestor node of depth q to the root node if and only if p > q (p = 0, ..., n, q = 0, ..., n).

[0071]

[0072] Therefore, it can be concluded that a certain node The path cost of the parent node of node x is no greater than that of all its descendant nodes. Path cost when x is the parent node:

[0073]

[0074] Where i = 0, ..., p-1.

[0075] According to the inequality relationship between ancestor nodes and their descendant nodes in equation (3), if a certain node is a parent node, x new The path cost is not less than the current path cost c min Then, it is not necessary to calculate and verify all descendant nodes of the node to remove them from the set of candidate parent nodes. If a node is a parent node, x new The path cost is less than the current path cost c min If the paths between them are passable, then that node will replace the original parent node as the new current parent node, and it is unnecessary to continue verifying all of its descendant nodes. In summary, the set of candidate parent nodes only retains those that can make x... new Less than the current path cost c min But with x new All descendant nodes of nodes whose routes between them are impassable. The specific operation for reselecting a parent node is as follows:

[0076] 1) Traverse the random tree T and find the relationship with x. new Add all nodes whose distance is less than r to the set X. near Find set X near All ancestor nodes of depth 1, ..., n (usually n = 2) are placed into a set. The nodes in each set are denoted as follows:

[0077] 2) Let x neares t is the initial parent node x min , that is, x min =x neares Let t be the straight-line path between the two points, and let σ be the distance between them. min Then x new Reaching the root node x init The initial path cost c min =Cost(x min )+c(σ min ).

[0078] 3) Determine the root node x init Can it be used as x? new Parent node: If the initial parent node x nearest That is, the root node xinit If the result is positive, the process of reselecting the parent node ends; otherwise, proceed to the root node x. init With x new Collision detection is performed on the paths between them; if a path is passable, then x is set to [a certain value]. init As x new If the parent node is selected, the entire process of reselecting the parent node ends; otherwise, hierarchical filtering begins.

[0079] 4) During hierarchical filtering, the set of ancestor nodes to be selected. The depth is selected from n by layer, with each layer being 0. For the set... For each node in the list, if that node is the root node x init If a node is a first-level descendant, then all its first-level descendant nodes are selected for the next level of selection; otherwise, calculate x when the node is a parent node. new The path cost to the root node. If the path cost is less than c. min Then, perform a cross-section between that point and x. new Collision detection between points; if passage is possible, then that point is designated as x. new If the current parent node and its descendant nodes are not selected for the next level of selection, and if the path is impassable, then all its first-level descendant nodes are selected for the next level of selection. If the path cost is not less than c... min If a node fails to pass the screening, its descendant nodes will not be selected for the next level of screening. After each level of screening, the number of nodes to be verified in the ancestor set of the next level will be significantly reduced.

[0080] Step 7: Rewire.

[0081] The purpose of the rewiring process is to provide set X near In the process of rerouting, a new parent node is selected for each node to reduce its cost. Similarly, the path cost relationship between ancestor and descendant nodes can be utilized, and the computational cost of nodes can be reduced by sorting ancestor nodes by depth. The depth-sorting rerouting algorithm improves computational speed by removing unnecessary node verifications. The specific rerouting operation is as follows:

[0082] Find x new Ancestor nodes of depth 1,…,n (usually n=2) are denoted as follows: For X near Each node x in near Calculate sequentially in descending order of depth from n to 0. When x is the parent node near The path cost. If the cost of a certain path is not less than the current value, then this path x is terminated directly. near The rerouting process does not need to consider smaller depths. If the path cost is less than the current path cost and collision detection allows passage, then disconnect x. near The connection with the original parent node will now... As the new parent node and end this x near Rerouting. In other words, smaller paths are only considered if the cost of a given path is less than the current value and the collision detection indicates that the path is impassable.

[0083] Step 8: Determine whether the random tree has been expanded to the target region χ goal If d(x) new ,x goal If x < R and the pathfinding success identifier FindPath = 0, then x at this time... new As x goal The parent node, connected to x new With x goal At this point, the first path has been found successfully, and the FindPath identifier is set to 1; otherwise, proceed to step 2 to continue expanding the nodes.

[0084] Step 9: When the first connection starting point x is found start and target point x goal After finding the optimal path (i.e., when FindPath = 1), we can continue to expand the tree to find intermediate nodes that reduce the path cost. As the number of sampling points increases, the planned path will get closer and closer to the optimal path. We can set the current optimal path sequence to be updated every K nodes added to the random tree, and stop expanding when the path cost remains a fixed value for M consecutive times, thus ending the path planning.

[0085] Example:

[0086] To verify the effectiveness of the DSFS trajectory planning algorithm, a comparative simulation experiment between the Q-RRT* algorithm and the DSFS algorithm was conducted in the MATLAB environment.

[0087] The selected sea area's latitude and longitude range is: longitude 112°E–115°E, latitude 10°–11°N. The gravity anomaly data and seafloor topography data used in this invention are sourced from the Scripps Institution of Oceanography website at the University of California, San Diego (https: / / topex.ucsd.edu / ), with a resolution of 1′×1′. Specifically, the maximum gravity anomaly is 133.4 mGal, the minimum is -32.4 mGal, the average is 14.8 mGal, and the standard deviation is 27.2 mGal; the maximum water depth is 0 m, the minimum is -4391 m, the average is -2381.6 m, and the standard deviation is 1118.2 m.

[0088] After numerous matching experiments, regions with a gravity anomaly standard deviation greater than 5 showed good fit. Therefore, a threshold of s0 = 5 was selected, and the established two-dimensional gravity fitting grid is as follows: Figure 1As shown, the black areas represent non-adapted grid areas, and the white areas represent adapted grid areas. With a safe water depth set at 100m, the resulting hazardous area grid map is shown below. Figure 2 As shown. Figure 2 Dangerous navigation areas in China Figure 1 After removing the middle, the resulting gravity fit distribution map is as follows: Figure 3 As shown, the black area is the non-adaptation area, where the positioning matching effect is poor; the gray area is the danger zone, which is not suitable for navigation; the white area is the gravity-assisted navigation adaptation area where navigation is safe.

[0089] The starting point of the path planning is set to (112.4°E, 10.8°N) and the target point to (115.5°E, 10.9°N). The maximum number of iterations N is 5000, the sampling probability threshold λ is 0.5, the expansion step size ρ is 2000m, and the set X... near The selection radius r is 6000m, the maximum depth n is 2, the number of equal parts for path collision detection L is 200, the number of nodes for path sequence update K is 300, and the path planning ends when the updated path cost is a fixed value for 3 consecutive times or the number of algorithm iterations reaches the maximum number N.

[0090] In the process of reselecting parent nodes and rerouting, the number of calls to the path cost function c(·) directly reflects the number of nodes participating in the verification calculation. The collision detection function CollisionFree is the main time-consuming calculation step, and the number of calls to this function is a key factor affecting the computation time and planning efficiency. Therefore, in order to test the effectiveness of the new DSFS algorithm in reducing computational load during the reselection of parent nodes and rerouting process, the number of path cost calculations and the number of collision detection function calls are used as metrics.

[0091] 1. The effectiveness of depth-based sorting for reselecting parent nodes

[0092] In the planning environment of this invention, the Q-RRT* parent node reselection algorithm and the DSFS parent node reselection algorithm are compared and tested. Specifically, whenever a new node x is obtained from the random tree... new Then, two parent node reselection algorithms were used for node x. newEach method performs a parent node selection calculation once, recording the number of path cost calculations, collision detection function calls, and the computation time for the parent node selection process for both methods. After the selection process, the parent node results from the two methods are compared. If the results are inconsistent, the situation is recorded, and the parent node with the lower path cost is selected to continue the planning process. After path planning is completed, the total number of path cost calculations, the total number of collision detection function calls, and the total time for the parent node selection calculation process are statistically analyzed for both methods throughout the entire planning process. Because the node selection in RRT-type algorithms is random, 50 simulation comparison experiments were conducted to ensure the fairness of the verification.

[0093] Figure 4 The graph shows the number of path cost calculations for both methods during parent node reselection. The broken line for the DSFS algorithm is below that of the Q-RRT* algorithm, indicating that the DSFS algorithm significantly reduces the number of path cost calculations during the planning process. To more intuitively demonstrate the effect of the DSFS parent node reselection algorithm on reducing the number of node calculations, a plot of a certain x-axis is provided. new A schematic diagram showing the nodes involved in path cost calculation using the two methods during a parent node reselection, as shown below. Figure 5 a and Figure 5 As shown in b, the upper image is a magnified view of the area within the dashed box in the lower image. Points A and B represent the starting points x for path planning. start With target point x goal A pentagram represents node x. new The circle represents set X. near The selection range, the nodes in the circle ( Figure 5 (a) Nodes numbered 4 to 12 in the middle are x new The neighboring nodes. When reselecting the parent node, nodes in the random tree that participated in the path cost verification calculation are marked with a plus sign. It can be seen that... Figure 5 In algorithm a, the Q-RRT* algorithm requires 12 nodes for path cost verification calculation, while Figure 5 (b) The DSFS algorithm only needs to compute one node to obtain the same result as the Q-RRT* algorithm.

[0094] Figure 6The bar chart shows the number of collision checks in the parent node reselection process for the two methods. The histogram represents the difference in the number of collision checks between the Q-RRT* parent node reselection algorithm and the DSFS parent node reselection algorithm. Statistical analysis shows that the DSFS parent node reselection algorithm has fewer collision checks than the Q-RRT* algorithm (40 / 50). Furthermore, observing the histogram reveals that when the Q-RRT* algorithm has fewer collision checks than the DSFS algorithm, the difference is very small (below the zero line). However, when the DSFS algorithm has fewer collision checks than the Q-RRT* algorithm, there are several instances where the difference is larger (above the zero line). This indicates that the DSFS parent node reselection algorithm can significantly reduce the number of collision checks in most cases.

[0095] Figure 7 The computation time of the parent node reselection process for the two methods is shown. It can be seen that the computation time of the DSFS algorithm is significantly reduced compared to the Q-RRT* algorithm, demonstrating a significant improvement in the computation speed of the parent node reselection process through hierarchical sorting. To reflect the performance improvement of hierarchical sorting on the parent node reselection step, the average number of path cost calculations, collision detections, and selection process times from 50 experiments were taken. The average number of path cost calculations decreased by approximately 63.6%, the average number of collision detections decreased by approximately 7.8%, and the average computation time of the selection process decreased from 12.6s to 7.4s, a reduction of approximately 41.3% compared to the Q-RRT* parent node reselection algorithm. Furthermore, no inconsistencies in the parent node reselection results were observed during the simulation experiments, indicating that depth sorting of ancestor nodes can effectively improve the computational performance of the parent node reselection process while ensuring correct calculation results.

[0096] 2. Effectiveness of depth-sorted rerouting

[0097] To verify the effectiveness of depth sorting in improving the speed of the rerouting algorithm, the Q-RRT* rerouting algorithm and the DSFS rerouting algorithm were used for set X. near Each node x in near Perform a rerouting. Record the number of path cost calculations, collision detection function calls, and rerouting computation time for both methods, and record the time taken for each x-axis. near After the rerouting is completed, the parent nodes of the two reroutings are compared. If they are inconsistent, the situation is recorded, and the node with the lower path cost is selected as the parent node to continue the planning process. After the planning is completed, the total number of path cost calculations, the total number of collision detection function calls, and the total time of the rerouting process are counted. A total of 50 rerouting simulation verification experiments are conducted.

[0098] Figure 8The graph shows the number of path cost calculations for the two rerouting algorithms. As can be seen from the graph, the DSFS algorithm has significantly fewer calculations than the Q-RRT* algorithm, indicating that the DSFS rerouting algorithm can improve computational efficiency by reducing a large number of useless node calculations. Figure 9 The graph shows the number of collision detections for the two rerouting methods, with the bar chart representing the difference in collision detection counts between the Q-RRT* algorithm and the DSFS rerouting algorithm. As can be seen from the graph, the DSFS rerouting algorithm reduces the number of collision detections in most cases (42 / 50), and in a few cases, it matches the Q-RRT* algorithm (8 / 50), but there are no instances where the number increases. Due to the inclusion of a depth-based sorting mechanism, when x... near The final parent node is In a network, if a node with a depth less than its final parent node can pass path cost detection, the number of collision detections will be reduced.

[0099] Figure 10 The comparison shows the computation time of the rerouting process for the two methods. It is evident that in 50 experiments, the DSFS rerouting algorithm significantly reduced the computation time compared to the Q-RRT* algorithm. The DSFS rerouting algorithm reduced the average number of path cost calculations by approximately 64.6%, the average number of collision detections by approximately 0.6%, and the computation time of the rerouting process from 10.1s to 4.3s, a reduction of approximately 57.4% compared to the Q-RRT* rerouting algorithm. Furthermore, no inconsistencies in the rerouting results were observed throughout the testing process, indicating that the DSFS rerouting algorithm can reduce computational load while maintaining the correctness of the rerouting results, effectively improving the computation speed of the rerouting process.

[0100] 3. Effectiveness of the DSFS path planning algorithm

[0101] To test the effectiveness of the novel depth-sorting fast search method in improving the path planning efficiency of underwater gravity-assisted navigation, after each new node was acquired, both the Q-RRT* algorithm and the DSFS algorithm were used to perform a reselection of the parent node and a rewiring calculation. The number of path cost calculations, collision detection function calls, and total computation time for each step were statistically analyzed. After each reselection and rewiring calculation, the results of the two methods were compared, and cases of inconsistency were recorded. The parent node with the lower path cost was selected to continue the planning process. A total of 50 path planning simulation tests were conducted.

[0102] Figure 11 The results show that the number of path cost calculations in the parent node reselection and rerouting processes of the DSFS algorithm is significantly reduced compared to the Q-RRT* algorithm in both processes. Figure 12The diagram shows the total number of path cost calculations for the parent node reselection and rerouting processes of the Q-RRT* algorithm and the DSFS algorithm. Clearly, the DSFS algorithm has fewer path cost calculations than the Q-RRT* algorithm.

[0103] Figure 13 The numbers represent the number of collision detections during the parent node reselection and rewiring processes of the two algorithms. It can be seen that the number of collision detections in the DSFS algorithm is significantly reduced overall during the parent node reselection process. Figure 14 The total number of collision detections is the sum of the two processes of the Q-RRT* algorithm and the DSFS algorithm. The bar chart shows the difference in the number of collision detections between the two algorithms. The results show that the DSFS algorithm can reduce the number of collision detections in the planning process in most cases.

[0104] Figure 15 The total computation time for the parent node reselection and rerouting processes in the Q-RRT* and DSFS algorithms is shown. Compared to the Q-RRT* algorithm, the computation time of the DSFS algorithm is significantly reduced in every experiment. Compared to the Q-RRT* algorithm, the DSFS algorithm reduces the average total number of path cost calculations by approximately 65.2%, the average total number of collision detections by approximately 3.8%, and the total computation time for both processes is reduced from 25s to 11.4s, an average reduction of approximately 54.4% (efficiency improved to 2.2 times that of the Q-RRT* algorithm). Furthermore, no inconsistencies were observed between the two methods throughout the testing process.

[0105] Figure 16 The path is planned using the Underwater Gravity-Assisted Navigation (DSFS) path planning system. Points A and B are the starting points x of the path planning. start and target point x goal Path from x init Start by reaching x along the edges of the random tree. goal .

[0106] In summary, taking the parameter settings of this invention as an example, the novel depth sorting fast search algorithm proposed in this invention can shorten the calculation time of the reselection of parent nodes and rewiring process by about 54.4% compared with the Q-RRT* algorithm, effectively improving the efficiency of underwater gravity-assisted navigation planning.

[0107] The contents not described in detail in this specification are common knowledge to those skilled in the art.

[0108] Although the present invention has been disclosed above with reference to preferred embodiments, it is not intended to limit the present invention. Any person skilled in the art can make possible changes and modifications to the technical solutions of the present invention by utilizing the methods and techniques disclosed above without departing from the spirit and scope of the present invention. Therefore, any simple modifications, equivalent changes and alterations made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solutions of the present invention shall fall within the protection scope of the technical solutions of the present invention.

Claims

1. A method for improving the efficiency of underwater navigation path planning based on depth sorting fast search, characterized in that, include: The gravity anomaly data and seabed topography data within the navigation area are processed to delineate navigable and impassable areas. Based on the passable area, the starting point is used as the root node of the random tree. The tree is gradually expanded by randomly sampling in the search space. When the leaf node expands to the target point or target area, a path from the starting point to the target point is obtained, which is composed of the edges of the random tree. Once the first path connecting the starting point and the target point is found, the optimal path is obtained by continuing to expand the nodes to find intermediate nodes that reduce the cost of the path. Depth-sorting fast search reduces the number of path cost calculations and collision detections during the selection process by filtering nodes in the Q-RRT* algorithm in depth order. In depth-sorting fast search, for a new node, only the descendant nodes of nodes whose path cost is less than the current cost and whose routes to the new node are impassable are retained in the set of candidate parent nodes. In depth-sorting fast search, new parent nodes with even lower depths are only explored when the path cost is less than the current value and the collision detection shows that the path is impassable.

2. The method for improving underwater navigation path planning efficiency according to claim 1, characterized in that, During sampling, a target bias approach was adopted, using the target point to control the growth direction of the random tree.

3. The method for improving the efficiency of underwater navigation path planning according to claim 1 or 2, characterized in that, The gravity-assisted navigation adaptation zone and non-adaptation zone are divided based on the standard deviation of gravity anomaly, a single characteristic parameter. The planned route can only reach the destination through this zone.

4. The method for improving the efficiency of underwater navigation path planning according to claim 1 or 2, characterized in that, By leveraging the path cost relationship between ancestor nodes and their descendant nodes, the computational cost of nodes can be reduced by sorting ancestor nodes by depth.

5. The method for improving the efficiency of underwater navigation path planning according to claim 1 or 2, characterized in that, When continuous When the cost of the secondary path remains constant, the expansion stops and the path planning ends.

6. The method for improving the efficiency of underwater navigation path planning according to claim 1 or 2, characterized in that, By performing depth sorting on the nodes to be selected, the number of nodes to be calculated in the process of reselecting parent nodes and rewiring is reduced, thereby accelerating the expansion speed of the random tree.

7. A system for improving the efficiency of underwater navigation path planning based on depth sorting fast search, characterized in that, include: The gravity adaptation zone delineation module processes gravity anomaly data and seabed topography data within the navigation area to delineate traversable and impassable areas. The path generation and optimization module uses the starting point as the root node of a random tree based on the passable area. It gradually expands the tree by randomly sampling in the search space. When the leaf node expands to the target point or target area, a path from the starting point to the target point is obtained, which is composed of the edges of the random tree. After finding the first path connecting the starting point and the target point, the module continues to expand the nodes to find intermediate nodes that reduce the cost of the path and obtain the optimal path. Depth-sorting fast search reduces the number of path cost calculations and collision detections during the selection process by filtering nodes in the Q-RRT* algorithm in depth order. In depth-sorting fast search, for a new node, only the descendant nodes of nodes whose path cost is less than the current cost and whose routes to the new node are impassable are retained in the set of candidate parent nodes. In depth-sorting fast search, new parent nodes with even lower depths are only explored when the path cost is less than the current value and the collision detection shows that the path is impassable.

Images