Path planning method based on information grid division

Abstract

The invention discloses a path planning method based on information grid division, and the method comprises the steps: constructing a grid-based search environment, and initializing the navigation cost of a starting point, an ending point and nodes in a grid; defining a cost function from a node to an end point, starting from the end point, and calculating cost and a path according to a reverse search strategy; environment change is detected, and node cost influenced by the environment change is updated; recursively updating the navigation cost of an adjacent node of the affected node and the cost thereof to an end point; and updating a global optimal path, further optimizing the path in combination with ship navigation characteristics, and generating an executable navigation instruction. According to the method, the incremental updating strategy and the dynamic cost function model are introduced, so that the path planning efficiency of the ship in a dynamic environment is effectively improved, the change of obstacles can be quickly responded, an efficient and safe navigation path is provided for the ship, the execution efficiency of related tasks is remarkably improved, and the implementation cost is reduced. And intelligent ship navigation and sea area management application are realized.

Description

Technical Field

[0001] The present invention relates to the field of software technology, and in particular to a path planning method based on information grid division. Background Art

[0002] With the rapid development of the global shipping industry and the continuous increase in maritime traffic, the importance of ship path planning in the maritime field has become increasingly prominent. Ship path planning is not only related to transportation efficiency, but also involves multiple key issues such as navigation safety, environmental protection, and energy consumption. Effective path planning can help ships choose the best route, reduce fuel consumption, reduce carbon emissions, while avoiding dangerous areas and ensuring navigation safety. However, in practical applications, the complex and changeable marine environment poses huge challenges to path planning. These challenges mainly come from the distribution of marine obstacles, the dynamic changes of weather and tides, and the influence of ocean currents and waves. These factors will significantly increase the uncertainty and risk during navigation.

[0003] Existing path planning technologies are mainly based on graph theory (such as A* algorithm, Dijkstra algorithm) or geometric algorithms. These methods have good applicability in static or idealized environments. However, when faced with complex dynamic marine environments, their limitations gradually become apparent. For example, the real-time changes of marine obstacles (such as other ships, buoys, rocks, and channel restrictions), as well as the dynamic characteristics of weather and tides, often make it difficult for traditional methods to quickly update path planning results. In addition, these methods usually ignore the dynamic properties of the marine environment such as currents, wind and waves, and the dynamic characteristics of ships under different navigation conditions when modeling, resulting in insufficient accuracy and practical feasibility of the planned path. In practical applications, this deficiency may cause path planning to fail to respond to environmental changes in a timely manner, thereby increasing the risk of ship collisions or reducing navigation efficiency.

[0004] In addition, current research on path planning is mostly focused on specific application scenarios, and fails to fully consider the global dynamic characteristics of the marine environment (such as ocean currents, weather changes, etc.) and the dynamic characteristics of ships in maritime path planning, resulting in inaccurate path calculation results and failure to ensure that ships maintain high navigation efficiency and safety in dynamic environments. For example, when dealing with tidal changes or sudden severe weather, traditional path planning technology may take a long time to recalculate the path, making it difficult to meet the ship's needs for dynamic adjustment of routes, thereby affecting overall transportation efficiency and safety. Therefore, how to perform efficient, accurate and real-time path planning in a complex marine environment remains a technical problem that needs to be solved urgently. Summary of the invention

[0005] Purpose of the invention: The purpose of the present invention is to provide a path planning method based on information grid division.

[0006] Technical solution: The path planning method based on information grid division described in the present invention comprises the following steps: (1) Grid division and node cost initialization: Construct a grid-based search environment and initialize the navigation costs of the start point, end point, and nodes in the grid; (2) Cost function definition and path search: Define the cost function from the node to the end point, and start from the end point and calculate the cost and path according to the reverse search strategy; (3) Dynamic environment detection and cost update: Detect environmental changes and update the node costs affected by the environmental changes; (4) Incremental cost update: recursively update the navigation cost of the affected node’s neighboring nodes and the cost to the destination; (5) Path output and navigation optimization: Update the global optimal path, further optimize the path based on the ship’s navigation characteristics, and generate executable navigation instructions.

[0007] Furthermore, the step (1) includes dividing the target navigation area into equilateral square grid nodes, each grid node represents a small area of ​​the actual sea area; marking each grid unit as impassable, and if so, setting its navigation cost to , otherwise the navigation cost of the grid is set according to the environmental factors and safety factors of the grid.

[0008] Furthermore, the step (2) includes defining a cost function and initializing node costs, starting from the end point, updating the costs of adjacent nodes in sequence, and then starting from the end point, recursively tracing the path with the minimum cost in reverse until reaching the starting point.

[0009] Furthermore, the cost function definition and path search in step (2) include: Define the starting point and end point ; Define the cost function :node To the end The cost is calculated as follows:

[0010] in, Represents a slave node To the end The minimum cost of equal; Represents a slave node To the end The estimated minimum cost of is calculated as follows:

[0011] in, Is a node The adjacent nodes of is the set of adjacent nodes, Is a node To Node The cost of sailing, when When updating, Will be updated and initialized together , initialize the remaining nodes , and the node Join the priority queue; Select the node with the lowest cost from the priority queue To process, if , then update The value of , and then traverse all adjacent nodes , update them according to the following formula :

[0012] And all adjacent nodes Join the priority queue if , then the node Mark as consistent and remove from the priority queue; Repeat the above update process until the priority queue is empty and all nodes reach a consistent state, and then trace back from the starting point to generate the global optimal path.

[0013] Furthermore, step (3) includes using real-time sensors or a maritime dynamic data monitoring system to continuously detect environmental changes in the sea area that affect the navigation of ships. If the environment of some nodes in the grid changes, these nodes are marked as inconsistent, and the navigation costs of these nodes are recalculated, and then the navigation costs of these nodes are recalculated according to formula (2). , and finally add them to the priority queue.

[0014] Furthermore, the inconsistent state means that the current cost value of the node needs to be re-evaluated due to environmental changes, that is, .

[0015] Furthermore, the step (4) includes selecting the node with the lowest cost from the priority queue Update the cost value incrementally, first checking consistency; then propagate the update, if If a change occurs, its impact is propagated to all adjacent nodes. , update according to formula (3) , and all adjacent nodes Add it to the priority queue; finally, repeatedly select the node with the lowest cost from the priority queue and update it until all nodes reach a consistent state, that is, the priority queue is empty.

[0016] Furthermore, the consistency check refers to checking the consistency of the grid nodes. and Are they equal? ​​If not, update for .

[0017] Furthermore, the step (5) includes generating a path with minimum cost according to the calculated cost value; then further optimizing the generated path according to the actual situation of the ship; and finally converting the optimized path into navigation coordinates in the actual sea area to generate executable navigation instructions.

[0018] Furthermore, the path with the minimum cost refers to starting from the starting point and tracing back to the end point according to the final cost value graph to generate a global optimal path.

[0019] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: By introducing an incremental update strategy and a dynamic cost function model, the present invention effectively improves the path planning efficiency of ships in a dynamic environment, especially in a complex sea environment, and can quickly respond to changes in obstacles, provide ships with efficient and safe navigation paths, significantly improve the execution efficiency of related tasks, and realize intelligent ship navigation and sea area management applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a flow chart of the present invention. DETAILED DESCRIPTION

[0021] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0022] The present invention first divides the map into a grid structure, initializes the starting point and the end point, and defines the cost of each node in the grid according to the attributes of each node, such as environmental factors, safety factors, etc. Next, a cost function is defined, and the cost required from each node to the end point is calculated according to the cost function, and the minimum cost path from the starting point to the end point is recorded. Then, environmental changes are detected. When the environment changes, the cost of the affected nodes is updated and the cost from the affected nodes to the end point is recursively recalculated. Finally, the global optimal path is updated and the navigation path is optimized according to the actual situation of the ship.

[0023] In order to better illustrate the technical content of the present invention, the following description is made in conjunction with the accompanying drawings.

[0024] like Figure 1The figure shows the overall flow chart of the present invention. The path planning method based on information grid division of the present invention comprises the following five steps: (1) Environmental grid division and grid navigation cost initialization. The target navigation area is divided into equilateral square grids, and each grid cell represents a small area of ​​the actual sea area. The grid size is determined by the specific application scenario, generally 100m×100m to 1km×1km. Each grid cell is marked as impassable. If so, its navigation cost is set to Otherwise, the navigation cost of the grid is set according to the environmental factors of the grid (such as wind direction and speed, water flow direction and speed, etc.) and safety factors (such as water depth information of the area, obstacle density, etc.), and the starting point is defined. and end point .

[0025] The specific steps are as follows: (1.1) Grid division: Divide the target sea area into regular two-dimensional or three-dimensional grids, and record the geographic coordinates, depth information, and navigation conditions of each grid node; (1.2) Node cost initialization: Calculate the navigation cost of the grid node based on the previously recorded grid information. The navigation cost includes the following factors: Distance cost: the sailing distance between nodes; Environmental cost: Consider the impact of environmental factors such as wind and water flow on the path; Safety cost: Set the navigation risk cost according to the navigation depth and obstacle distribution; (1.3) Definition of starting point and end point: Specify the current position of the ship as the starting point and the target position as the end point.

[0026] (2) Define the cost function and initialize the node cost. Starting from the end point, update the cost of adjacent nodes one by one. Then, starting from the end point, recursively trace the path with the minimum cost in reverse until you reach the starting point.

[0027] The specific steps are as follows: (2.1) Define the cost function: Define the cost function f(n) as the cost from node n to the end point G; (2.2) Initialize the nodes and define the initial state: the cost of the end point is set to zero, indicating that the cost of reaching the end point is zero; the initial cost of other nodes is set to infinity, indicating that the cost of reaching the end point has not yet been calculated. Establish a priority queue to store the nodes to be processed, and give priority to the nodes with the lowest cost; (2.3) Node cost update: Starting from the end point, update the cost of adjacent nodes in sequence and determine their predecessor nodes; (2.4) Path tracing: Starting from the starting point, generate the optimal path based on its predecessor node.

[0028] Define the cost function For Node To the end The cost is calculated as follows:

[0029] in, Represents a slave node To the end The minimum cost of equal. Represents a slave node To the end The estimated minimum cost of is calculated as follows:

[0030] in, Is a node The adjacent nodes of is the set of adjacent nodes, Is a node To Node The cost of sailing. When updating, Will be updated together. Initialization , initialize the remaining nodes , and the node Join the priority queue.

[0031] Then calculate the cost. Select the node with the lowest cost from the priority queue If , then update The value of , and then traverse all adjacent nodes , update them according to the following formula :

[0032] And all adjacent nodes Join the priority queue. If , then the node Mark as "consistent" and remove from the priority queue.

[0033] Repeat the above update process until the priority queue is empty and all nodes reach a consistent state, and then trace back from the starting point to generate the global optimal path.

[0034] (3) Detect environmental changes. Use real-time sensors or maritime dynamic data monitoring systems to continuously detect environmental changes in the sea area that may affect the navigation of ships, such as dynamic obstacles (such as the movement of other ships, the appearance of floating objects, etc.) and changes in environmental conditions (such as changes in wind speed, wind direction, water flow intensity and direction, or tidal effects). If the environment of some nodes in the grid changes, these nodes are marked as "inconsistent state", which means that the current cost value of the node needs to be re-evaluated due to environmental changes. Then, the navigation costs of these nodes are redefined in the manner of step (1), and then recalculated according to formula (2), and finally they are added to the priority queue.

[0035] The specific steps are as follows: (3.1) Dynamic environment detection: Detect dynamic environmental changes in the sea area through real-time sensors or maritime data; (3.2) Update of the cost of the affected nodes: Based on the changed environment, redefine the navigation cost of the node and the cost from the node to the destination; (3.3) Priority queue add: Add the affected nodes to the priority queue.

[0036] (4) Incremental update cost. Select the node with the lowest cost from the priority queue. Update the cost value gradually. The specific process is as follows: First, check the consistency. For the selected nodes , detection and Are they consistent? If not, update for . Then propagate the update, if If a change occurs, its impact is propagated to all adjacent nodes. , update according to formula (3) , and all adjacent nodes Add it to the priority queue. Finally, repeatedly select the node with the lowest cost from the priority queue and update it until all nodes reach a consistent state, that is, the priority queue is empty.

[0037] The specific steps are as follows: (4.1) Priority queue management: The nodes with the lowest cost are processed in turn. After processing, their adjacent nodes are added to the priority queue. (4.2) Recursive update of adjacent node costs: Recursively update the cost from the node to the end point based on the adjacent nodes of the affected node.

[0038] (5) Path optimization and output. After completing the cost update, the final cost value graph is used to trace back from the starting point to the target node to generate a global optimal path. The generated path is further optimized based on the actual navigation parameters of the ship. For example, the path is smoothed according to the minimum turning radius of the ship to avoid safety problems caused by sharp turns. The optimized path is converted into navigation coordinates in the actual sea area, and executable navigation instructions are generated, including specific parameters such as heading, speed and turning point. The real-time navigation system is combined to provide continuous guidance for the ship to ensure that the ship follows the optimal path and dynamically adjusts the path when necessary.

[0039] The specific steps are as follows: (5.1) Path output: According to the updated cost value, trace back from the starting point to the end point and output the global optimal path; (5.2) Navigation characteristics optimization: The route is further optimized based on the ship’s speed, turning radius and other parameters; (5.3) Generate navigation instructions: Convert the optimized path into executable navigation instructions for the ship, including specific parameters such as heading, speed and turning point.

Claims

1. A path planning method based on information grid division, characterized in that: The steps include: (1) Grid division and node cost initialization: Construct a grid-based search environment and initialize the navigation costs of the start point, end point, and nodes in the grid; (2) Cost function definition and path search: Define the cost function from the node to the end point, and start from the end point and calculate the cost and path according to the reverse search strategy; (3) Dynamic environment detection and cost update: Detect environmental changes and update the node costs affected by the environmental changes; (4) Incremental cost update: recursively update the navigation cost of the affected node’s neighboring nodes and the cost to the destination; (5) Path output and navigation optimization: Update the global optimal path, further optimize the path based on the ship’s navigation characteristics, and generate executable navigation instructions.

2. The path planning method based on information grid division according to claim 1 is characterized in that: The step (1) includes dividing the target navigation area into equilateral square grid nodes, each grid node represents a small area of ​​the actual sea area; marking each grid unit as impassable, and if so, setting its navigation cost to , otherwise the navigation cost of the grid is set according to the environmental factors and safety factors of the grid.

3. The path planning method based on information grid division according to claim 1 is characterized in that: The step (2) includes defining a cost function and initializing node costs, starting from the end point, updating the costs of adjacent nodes in sequence, and then starting from the end point, recursively tracing the path with the minimum cost in reverse until reaching the starting point.

4. The path planning method based on information grid division according to claim 1 is characterized in that: The cost function definition and path search in step (2) include: Define the starting point and end point ; Define the cost function :node To the end The cost is calculated as follows: ， in, Represents a slave node The minimum cost to the end point is equal; Represents a slave node To the end The estimated minimum cost of is calculated as follows: ， in, Is a node The adjacent nodes of is the set of adjacent nodes, Is a node To Node The cost of sailing, when When updating, Will be updated and initialized together , initialize the remaining nodes , and the node Join the priority queue; Select the node with the lowest cost from the priority queue for processing if , then update The value of , and then traverse all adjacent nodes , update them according to the following formula : ， And all adjacent nodes Join the priority queue if , then the node Mark as consistent and remove from the priority queue; Repeat the above update process until the priority queue is empty and all nodes reach a consistent state, and then trace back from the starting point to generate the global optimal path.

5. The path planning method based on information grid division according to claim 1 is characterized in that: The step (3) includes using real-time sensors or a maritime dynamic data monitoring system to continuously detect environmental changes in the sea area that affect the navigation of ships. If the environment of some nodes in the grid changes, these nodes are marked as inconsistent, and the navigation costs of these nodes are recalculated. Then, the navigation costs of these nodes are recalculated according to formula (2). , and finally add them to the priority queue.

6. The path planning method based on information grid division according to claim 5 is characterized in that: The inconsistent state means that the current cost value of the node needs to be re-evaluated due to environmental changes, that is, .

7. The path planning method based on information grid division according to claim 1 is characterized in that: The step (4) includes selecting the node with the lowest cost from the priority queue Update the cost value incrementally, first checking consistency; then propagate the update, if If a change occurs, its impact is propagated to all adjacent nodes. , update according to formula (3) , and all adjacent nodes Add it to the priority queue; finally, repeatedly select the node with the lowest cost from the priority queue and update it until all nodes reach a consistent state, that is, the priority queue is empty.

8. The path planning method based on information grid division according to claim 7 is characterized in that: The consistency check refers to checking the consistency of the grid nodes. and Are they equal? ​​If not, update for .

9. The path planning method based on information grid division according to claim 1, characterized in that: The step (5) includes generating a path with the minimum cost according to the calculated cost value; then further optimizing the generated path according to the actual situation of the ship; and finally converting the optimized path into navigation coordinates in the actual sea area to generate executable navigation instructions.

10. The path planning method based on information grid division according to claim 9, characterized in that: The path with the minimum cost refers to starting from the starting point and tracing back to the end point according to the final cost value graph to generate a global optimal path.

Images