Unmanned ship global shortest path planning method based on sparse graph
By constructing a sparse graph structure and a feature point generation method, the problem of high computational complexity in unmanned surface vessel (USV) path planning is solved, achieving efficient, safe, and shortest path planning, which is suitable for autonomous navigation of USVs.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-09-27
- Publication Date
- 2026-03-20
AI Technical Summary
Existing global path planning algorithms for unmanned surface vessels (USVs) struggle to achieve efficient computation while maintaining safe distances, especially in large-scale environments where computational complexity is too high to meet real-time planning requirements.
By constructing a sparse graph structure, using elliptical regions to filter obstacles, adaptively generating feature points, and employing a specific sorting strategy and a greedy optimization algorithm, a safe and shortest global path is generated.
It significantly reduces the size of computing nodes, improves planning efficiency, ensures path safety and global optimality, and generates smooth paths that conform to the dynamics of unmanned surface vessels.
Smart Images
Figure CN121702393A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of autonomous navigation and path planning technology for unmanned surface vessels. Specifically, it relates to an efficient computational method for global, safe, and shortest path planning in a known static polygon obstacle environment by constructing and searching a dedicated sparse graph structure. Background Technology
[0002] Global path planning is one of the core technologies for achieving autonomous navigation of unmanned surface vessels (USVs). Its goal is to find the optimal (usually shortest) path from the starting point to the destination within a known obstacle map, while ensuring a safe distance from all obstacles. Currently, existing global path planning algorithms mainly include: 1) A* algorithm and its variants: These use heuristic functions to guide the search direction and are highly efficient in grid maps, but their path length and smoothness are affected by grid resolution, and the "zigzag" path is unsuitable for USV dynamics models. 2) Quick Randomized Tree Extension (RRT) algorithm: This is suitable for high-dimensional spaces, but the planned path is usually not optimal and has randomness, making it difficult to guarantee global optimality. 3) Visibility Graph method: This method connects the starting point, destination, and all obstacle vertices to construct a network graph, and then uses algorithms such as Dijkstra's to search for the shortest path. This method can obtain the mathematically shortest path, but its drawback is significant: the path is too close to the obstacle vertices, failing to meet the practical requirement of maintaining a safe distance between the USV and obstacles. 4) Voronoi diagram method: generates paths that are as far away from obstacles as possible, which is safe, but the paths are usually much longer than the shortest path and are not optimal.
[0003] The closest technology to this invention is an improved version of the visual graph method. These methods attempt to meet the safety distance requirement by introducing the concept of "expanded obstacles," i.e., pre-expanding the obstacle polygon outwards by a safety distance, and then constructing a visual graph between the vertices of the newly expanded obstacle. While this method solves the safety problem, it leads to a denser graph structure: the expansion operation generates a large number of new vertices, causing a sharp increase in the number of vertices (V) and edges (E) of the graph (E and V). 2 (Proportional). Searching on this dense graph involves extremely high computational complexity, making it difficult to meet the real-time planning requirements of unmanned surface vessels.
[0004] Traditional graph search methods often involve paths that closely follow obstacle vertices, which is unsafe. Improved methods ensure safe distances by expanding obstacles, but this expansion generates a large number of new polygon vertices, leading to a surge in the number of nodes in the graph and creating a highly dense graph structure. Searching on this dense graph incurs enormous time and space overhead, making path planning too time-consuming and unsuitable for large-scale environments or unmanned surface vessels requiring high real-time performance. Traditional graph search algorithms (such as Dijkstra's algorithm) expand uniformly in all directions during the search, lacking directionality. This results in traversing a large number of nodes unrelated to the destination direction, performing a significant amount of unnecessary work and leading to low search efficiency. Summary of the Invention
[0005] By analyzing the shortcomings of the existing technologies, this invention focuses on solving the technical challenge of simultaneously considering path safety, global optimality (shortest path), and planning efficiency. It aims to provide a method that can significantly reduce the number of nodes required for planning, has a clear search direction, and can perform in-depth optimization of the initial path, ultimately generating a safe and shortest global path planning method efficiently.
[0006] This invention can be achieved through the following technical solutions:
[0007] A global shortest path planning method for unmanned surface vessels based on sparse graphs includes the following steps:
[0008] 1) Initialization: Input the starting point S, the ending point G, the vertex coordinates of each obstacle, and the safe distance d. safe ;
[0009] 2) By defining the region by ellipses and using an obstacle filtering mechanism, a large number of irrelevant obstacles are eliminated to reduce the computational scale;
[0010] 3) By adaptively generating feature points based on the included angle of obstacle vertices and the safety distance, a limited, high-quality set of path point candidates, i.e., feature points, is constructed;
[0011] 4) Determine a specific sorting strategy to rearrange the feature point list to guide the search direction of global path planning, significantly reduce the expansion of invalid nodes, and improve search speed;
[0012] 5) By sequentially converting the feature points in the feature point list into nodes with specific structures and adding them to the node list, a sparse path graph is constructed;
[0013] 6) Generate the global shortest path by using a specific greedy optimization algorithm that backtracks from the endpoint to the starting point.
[0014] Further, step 2) specifically involves: constructing an elliptical region with S and G as foci, and filtering out regions whose distance from the elliptical region is less than d. safeThe obstacles are selected, and a Set of obstacles is constructed using these selected obstacles as elements. obstacle Collision detection in subsequent steps only considers the Set. obstacle Obstacles in the middle.
[0015] Further, step 3) specifically involves: for the set Set obstacle For each vertex of each obstacle, one or two feature points are adaptively generated based on the value of the included angle θ between its adjacent sides: ① When θ > 180°, a feature point is precisely calculated on the angle bisector of that vertex, and the distance between this feature point and the vertex is d. safe / sin(θ / 2); ② When 90°≤θ<180°, accurately calculate a feature point on the angle bisector at the vertex, the distance between the feature point and the vertex is d. safe / sin(θ / 2); ③ When θ<90°, a feature point is generated on each side of the angle bisector at the vertex; the distance between these two feature points and the angle bisector is d. safe The expression is given by tan(π / 4 - θ / 4), and the distance from the vertex is d. safe / cos(π / 4-θ / 4).
[0016] Further, step 4) specifically involves: constructing a feature point list (List) using the feature points generated in the preceding steps as elements. point Add S and G to the list List point And remove feature points outside the ellipse range, according to F distance = 2*d1+d2 for the list List point Sort the features from smallest to largest, where d1 is the Euclidean distance from the feature point to the starting point and d2 is the Euclidean distance from the feature point to the ending point.
[0017] Further, step 5) specifically involves: creating a new node list, and sequentially converting the feature points in the feature point list into nodes, then adding them to the node list List based on the collision detection results. node The process continues until the added node becomes the endpoint. Node information includes its own coordinates, parent node coordinates, distance to the parent node, and the cumulative path length from the starting point to the current node. cost Bridge node list; the specific steps are as follows:
[0018] 5.1) Transform the starting point S into a node Node S Its parent node is empty, and the distance to the parent node is 0. The cumulative path length P from the starting point to this node is... cost The value is 0, indicating an empty bridge node list;
[0019] 5.2) Place the Node S Add it as the first node to the node list List. nodeAt the same time, delete the list. point Feature point S in;
[0020] 5.3) Processing Lists point The first feature point in the;
[0021] 5.4) Convert the current feature point P into a new node Node. new ;
[0022] 5.5) Traversing a List node All nodes, with Node new Collision detection is performed on the connection; if the connection intersects with a Set... obstacle The distance to all obstacles is no less than d. safe Then collision detection is performed;
[0023] 5.6) Select from the collision detection results the path can make cost The smallest node is called Node new The parent node;
[0024] 5.7) If Node new If the parent node exists, then generate a list of bridge nodes with a step size of step and set the Node... new Add to List node In the middle, delete the list at the same time. point For the feature point P in the diagram, the bridge node information includes its own coordinates, the coordinates of its parent node, the distance to its parent node, and the path. cost ;
[0025] 5.8) If Node new If the parent node does not exist, update the current feature point to the list List. point Find the next feature point in the sequence and proceed with steps 5.4)-5.8);
[0026] 5.9) Repeat steps 5.3)-5.8) until the node transformed from the endpoint G is reached. G Add to List node This will give you an initial safe path from S to G.
[0027] Further, step 6) specifically involves: from node Node G By backtracking, the total path from the starting point S to the ending point G is shortened by converting bridge nodes into parent nodes; the specific steps are as follows:
[0028] 6.1) Using Node G For the current node current ;
[0029] 6.2) According to Node current Parent node parent traverse the nodes in ascending order of their distances. parent Bridge nodes, until a certain bridge node is connected to Node current The connection failed the collision detection, meaning the connection did not collide with the Set. obstacle The distance to the obstacle is less than d safe Then convert the previous bridge node into a Node. parent ;
[0030] 6.3) According to Node current traverse the nodes in ascending order of their distances. current Bridge nodes, until a certain bridge node can be connected to the list. node Connect other nodes to reduce Node size current Path cost Furthermore, the connection passes collision detection, meaning the connection and the set are in agreement. obstacle The distance to all obstacles is no less than d. safe Then convert the bridge node to a Node. parent ;
[0031] 6.4) Place the Node current Update to Node parent ;
[0032] 6.5) Repeat steps 6.2)-6.4) until Node. current For Node S ;
[0033] 6.6) From Node G Start by recursively visiting the parent node of each node, until Node. S Output the coordinates of each node visited in each step, from the starting point S to the ending point G, in order to generate the optimized global shortest path.
[0034] Beneficial effects
[0035] 1) Significantly improve planning efficiency: By using elliptical regions and safety distances for dual screening, a large number of irrelevant obstacles and feature points are eliminated, which greatly reduces the scale of subsequent graph search and solves the problem of high computational complexity in traditional visual graph methods.
[0036] 2) Ensuring path safety: By generating feature points based on vertex angles and safety distances, the safe distance between path points and obstacles is guaranteed from the source. Furthermore, refined collision detection of bridge nodes ensures the safe distance between path segments and obstacles.
[0037] 3) Guaranteeing global optimality of the path: This invention first generates an initial safe path, and then through a unique backtracking optimization algorithm (bridge node reparenting, cross-node connection), it can break free from the constraints of the initial search node sequence and find possible "shortcuts", thereby obtaining the global shortest path under the current sampling point, which is better than the algorithm that is formed in one step and has no optimization process.
[0038] 4) Strong search directionality: The 2d1+d2 sorting strategy strongly guides the algorithm to explore the region closer to the endpoint first, avoiding the blind uniform search of Dijkstra's algorithm, greatly reducing the expansion of invalid nodes and improving the search speed.
[0039] 5) The path is more in line with the dynamics of the unmanned surface vessel: Due to the introduction of bridge nodes and the existence of optimization process, the final path has fewer broken line segments and is smoother, which is beneficial to the tracking and control of the unmanned surface vessel. Attached Figure Description
[0040] Figure 1 This is a flowchart of the path planning method in this invention;
[0041] Figure 2 This is a schematic diagram of the region focusing and obstacle screening mechanism in this invention;
[0042] Figure 3-1 This is a schematic diagram illustrating the generation of feature points when the included angle θ between the adjacent sides of an obstacle vertex satisfies the condition θ>180° in this invention.
[0043] Figure 3-2 This is a schematic diagram illustrating the generation of feature points when the included angle θ between the adjacent sides of an obstacle vertex satisfies the condition 90°≤θ<180° in this invention.
[0044] Figure 3-3 This is a schematic diagram illustrating the generation of feature points when the included angle θ between the adjacent sides of an obstacle vertex satisfies the condition θ < 90° in this invention.
[0045] Figure 4-1 This is a schematic diagram of the original sparse path graph constructed in this invention;
[0046] Figure 4-2 This is a schematic diagram illustrating how the bridge node of node 2 is converted into the parent node of node 3 in this invention;
[0047] Figure 4-3 This is a schematic diagram illustrating the conversion of the bridge node of node 3 into the parent node of node 3 in this invention;
[0048] Figure 5 This is a diagram illustrating the implementation effect of the path planning method in this invention. Detailed Implementation
[0049] The following specific embodiments illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification.
[0050] A global shortest path planning method for unmanned surface vessels based on sparse graphs is illustrated in the flowchart below. Figure 1 As shown. The specific implementation steps are as follows:
[0051] 1) Initialization. Input the starting point S, the ending point G, the vertex coordinates of each obstacle, and the safety distance d. safe .
[0052] 2) Region Focusing and Obstacle Filtering. An elliptical region is constructed with S and G as foci. Obstacles with a distance less than d from the elliptical region are filtered out. safe Obstacles, such as Figure 2 As shown. A Set of obstacles is constructed using the selected obstacles as elements. obstacle Collision detection in subsequent steps only considers the Set. obstacle Obstacles in the middle.
[0053] 3) Feature point generation. For a set Set obstacle For each vertex of each obstacle, one or two feature points are adaptively generated based on the value of the included angle θ between its adjacent sides:
[0054] 3.1) When θ > 180°, calculate a feature point precisely on the angle bisector at the vertex, the distance between the feature point and the vertex being d. safe / sin(θ / 2), such as Figure 3-1 As shown;
[0055] 3.2) When 90°≤θ<180°, accurately calculate a feature point on the angle bisector at the vertex, the distance between this feature point and the vertex being d. safe / sin(θ / 2), such as Figure 3-2 As shown;
[0056] 3.3) When θ < 90°, a feature point is generated on each side of the angle bisector at the vertex. The distance between these two feature points and the angle bisector is d. safe The expression is given by tan(π / 4 - θ / 4), and the distance from the vertex is d. safe *cos(π / 4-θ / 4), such as Figure 3-3 As shown.
[0057] 4) Feature point list construction. A feature point list (List) is constructed using the feature points generated in the previous steps as elements. point Add S and G to the list List point And remove feature points outside the ellipse range, according to F distance= 2 * d1 + d2 (where d1 is the Euclidean distance from the feature point to the starting point, and d2 is the Euclidean distance from the feature point to the ending point) for a list. point Sort them from smallest to largest.
[0058] 5) Constructing a sparse path graph. Create a new node list, and sequentially convert the feature points in the feature point list into nodes, then add them to the node list List based on the collision detection results. node This continues until the added node becomes the endpoint. Node information includes its own coordinates, the coordinates of its parent node, the distance to its parent node, and the cumulative path length from the starting point to this node (Path). cost Bridge node list. The specific steps are as follows:
[0059] 5.1) Transform the starting point S into a node Node S Its parent node is empty, and the distance to the parent node is 0. The cumulative path length P from the starting point to this node is... cost The value is 0, indicating an empty bridge node list;
[0060] 5.2) Place the Node S Add it as the first node to the node list List. Node At the same time, delete the list. point Feature point S in;
[0061] 5.3) Processing Lists point The first feature point in the;
[0062] 5.4) Convert the current feature point P into a new node Node. new ;
[0063] 5.5) Traversing a List node All nodes, with Node new Perform collision detection on the connection. If the connection intersects with a Set... obstacle The distance to all obstacles is no less than d. safe Then collision detection is performed;
[0064] 5.6) Select from the collision detection results the path can make cost The smallest node is called Node new The parent node;
[0065] 5.7) If Node new If the parent node exists, then generate a list of bridge nodes with a step size of step and set the Node... new Add to List node In the middle, delete the list at the same time. point The feature point P in the diagram. Bridge node information includes its own coordinates, parent node coordinates, distance to the parent node, and path.cost ;
[0066] 5.8) If Node new If the parent node does not exist, update the current feature point to the list List. point Find the next feature point in the sequence and proceed with steps 5.4)-5.8);
[0067] 5.9) Repeat steps 5.3)-5.8) until the node transformed from the endpoint G is reached. G Add to List node This will give you an initial safe path from S to G.
[0068] 6) Path optimization. From node Node G By backtracking, the total path from the starting point S to the ending point G is shortened by converting bridge nodes into parent nodes. The specific steps are as follows:
[0069] 6.1) Using Node G For the current node current ;
[0070] 6.2) According to Node current Parent node parent traverse the nodes in ascending order of their distances. parent Bridge nodes, until a certain bridge node is connected to Node current The connection failed the collision detection (i.e., the connection did not collide with the Set). obstacle The distance to the obstacle is less than d safe If the previous bridge node is converted to a Node, then the previous bridge node will be converted to a Node. parent ,like Figure 4-1 , 4-2 As shown;
[0071] 6.3) According to Node current traverse the nodes in ascending order of their distances. current Bridge nodes, until a certain bridge node can be connected to the list. node Connect other nodes to reduce Node size current Path cost And the connection passes collision detection (i.e., the connection and the set are collided). obstacle The distance to all obstacles is no less than d. safe If the bridge node is converted to a Node, then the bridge node is converted to a Node. parent ,like Figure 4-2 , 4-3 As shown;
[0072] 6.4) Place the Node current Update to Nodeparent ;
[0073] 6.5) Repeat steps 6.2)-6.4) until Node. current For Node S .
[0074] 7) Path output. From Node G Start by recursively visiting the parent node of each node, until Node. S Outputting the coordinates of each node visited in this process, from the starting point S to the ending point G, in order, yields the optimized global shortest path, such as... Figure 5 As shown in the diagram, the inflection points of the shortest path are marked with dashed circles.
[0075] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. A global shortest path planning method for unmanned surface vessels based on sparse graphs, characterized in that, Includes the following steps: 1) Initialization: Input the starting point S, the ending point G, the vertex coordinates of each obstacle, and the safe distance d. safe ; 2) By defining the region by ellipses and using an obstacle filtering mechanism, a large number of irrelevant obstacles are eliminated to reduce the computational scale; 3) By adaptively generating feature points based on the included angle of obstacle vertices and the safety distance, a limited, high-quality set of path point candidates, i.e., feature points, is constructed; 4) Determine a specific sorting strategy to rearrange the feature point list to guide the search direction of global path planning, significantly reduce the expansion of invalid nodes, and improve search speed; 5) By sequentially converting the feature points in the feature point list into nodes with specific structures and adding them to the node list, a sparse path graph is constructed; 6) Generate the global shortest path by using a specific greedy optimization algorithm that backtracks from the endpoint to the starting point.
2. The global shortest path planning method for unmanned surface vessels based on sparse graphs according to claim 1, characterized in that, Step 2) specifically involves: constructing an elliptical region with S and G as foci, and filtering out regions whose distance from the elliptical region is less than d. safe The obstacles are selected, and a Set of obstacles is constructed using these selected obstacles as elements. obstacle Collision detection in subsequent steps only considers the Set. obstacle Obstacles in the middle.
3. The global shortest path planning method for unmanned surface vessels based on sparse graphs according to claim 2, characterized in that, Step 3) specifically refers to: for the set Set obstacle For each vertex of each obstacle, one or two feature points are adaptively generated based on the value of the included angle θ between its adjacent sides: ① When θ > 180°, a feature point is precisely calculated on the angle bisector of that vertex, and the distance between this feature point and the vertex is d. safe / sin(θ / 2); ② When 90°≤θ<180°, accurately calculate a feature point on the angle bisector at the vertex, the distance between the feature point and the vertex is d. safe / sin(θ / 2); ③ When θ<90°, a feature point is generated on each side of the angle bisector at the vertex; the distance between these two feature points and the angle bisector is d. safe The expression is tan(∏ / 4-θ / 4), and its distance from the vertex is d. safe / cos(π / 4-θ / 4).
4. The global shortest path planning method for unmanned surface vessels based on sparse graphs according to claim 3, characterized in that, Step 4) specifically involves constructing a feature point list (List) using the feature points generated in the preceding steps as elements. point Add S and G to the list List point And remove feature points outside the ellipse range, according to F distance = 2*d1+d2 for the list List point Sort the features from smallest to largest, where d1 is the Euclidean distance from the feature point to the starting point and d2 is the Euclidean distance from the feature point to the ending point.
5. The global shortest path planning method for unmanned surface vessels based on sparse graphs according to claim 4, characterized in that, Step 5) specifically involves: creating a new node list, converting the feature points in the feature point list into nodes sequentially, and then adding them to the node list List based on the collision detection results. node The process continues until the added node becomes the endpoint. Node information includes its own coordinates, parent node coordinates, distance to the parent node, and the cumulative path length from the starting point to the current node. cost Bridge node list; The specific steps are as follows: 5.1) Transform the starting point S into a node Node S Its parent node is empty, and the distance to the parent node is 0. The cumulative path length P from the starting point to this node is... cost If the value is 0, the bridge node list is empty; 5.2) Place the Node S Add it as the first node to the node list List. node At the same time, delete the list. point Feature point S in; 5.3) Processing Lists point The first feature point in the; 5.4) Convert the current feature point P into a new node Node. new ; 5.5) Traversing a List node All nodes, with Node new Collision detection is performed on the connection; if the connection intersects with a Set... obstacle The distance to all obstacles is no less than d. safe Then collision detection is performed; 5.6) Select from the collision detection results the path can make cost The smallest node is called Node new The parent node; 5.7) If Node new If the parent node exists, then generate a list of bridge nodes with a step size of step and set the Node... new Add to List node In the middle, delete the list at the same time. point For the feature point P in the diagram, the bridge node information includes its own coordinates, the coordinates of its parent node, the distance to its parent node, and the path. cost ; 5.8) If Node new If the parent node does not exist, update the current feature point to the list List. point Find the next feature point in the sequence and proceed with steps 5.4)-5.8); 5.9) Repeat steps 5.3)-5.8) until the node transformed from the endpoint G is reached. G Add to List node This will give you an initial safe path from S to G.
6. The global shortest path planning method for unmanned surface vessels based on sparse graphs according to claim 5, characterized in that, Step 6) specifically involves: from node Node G By backtracking, the total path from the starting point S to the ending point G is shortened by converting bridge nodes into parent nodes; the specific steps are as follows: 6.1) Using Node G For the current node current ; 6.2) According to Node current Parent node parent traverse the nodes in ascending order of their distances. parent Bridge nodes, until a certain bridge node is connected to Node current The connection failed the collision detection, meaning the connection did not collide with the Set. obstacle The distance to the obstacle is less than d safe Then convert the previous bridge node into a Node. parent ; 6.3) According to Node current traverse the nodes in ascending order of their distances. current Bridge nodes, until a certain bridge node can be connected to the list. node Connect other nodes to reduce Node size current Path cost Furthermore, the connection passes collision detection, meaning the connection and the set are in agreement. obstacle The distance to all obstacles is no less than d. safe Then convert the bridge node to a Node. parent ; 6.4) Place the Node current Update to Node parent ; 6.5) Repeat steps 6.2)-6.4) until Node. current For Node S ; 6.6) From Node G Start by recursively visiting the parent node of each node, until Node. S Output the coordinates of each node visited in each step, from the starting point S to the ending point G, in order to generate the optimized global shortest path.