An improved path planning search algorithm based on grid map

Abstract

The present invention relates to the field of data processing technology, and in particular to an improved path planning search algorithm based on a grid map. The algorithm comprises designing a storage data structure for grid map nodes, using bit fields to store characteristic information representing grid map nodes, and obtaining a final grid map; preprocessing the final grid map to obtain a preprocessed grid map; designing a cost function to obtain an improved algorithm; and designing a search process based on the preprocessed grid map and the improved algorithm. The improved search algorithm of the present invention has faster search efficiency in a factory environment with low-density obstacles, thereby solving the problem of low search efficiency of existing search algorithms when facing large maps with obstacles.

Description

Technical Field

[0001] The present invention relates to the technical field of data processing, and in particular to an improved path planning search algorithm based on a grid map. Background Art

[0002] With the rapid development of science and technology in my country in recent years and the continuous upgrading of the manufacturing sector, my country has gradually moved from "Made in China" to "Smart Manufacturing in China". Among them, the vigorous development of AGV (Automated Guided Vehicle) has played a key role in promoting the process of "Smart Manufacturing in China". AGV is a mobile robot that can move and transport materials, products or goods in a manufacturing or warehousing environment autonomously or with limited human supervision, thereby improving production efficiency, reducing labor costs and increasing safety. As one of the core technologies of AGV, the autonomous navigation algorithm aims to provide AGV with navigation planning capabilities based on perception information, and has always been the focus of academia and industry.

[0003] Path planning is divided into global path planning and local path planning; global path planning algorithms are mainly divided into three categories: graph-based search algorithms, mainly including A* algorithm, jumping point search (JPS) algorithm, Dijkstra algorithm, D* algorithm, etc.; sampling-based search algorithms, such as random sampling consensus (RRT) algorithm, rapidly exploring random trees (RRT*) algorithm, etc.; intelligent model-based search algorithms, such as ant colony algorithm, genetic algorithm, reinforcement learning algorithm, etc.

[0004] In manufacturing plants, graph-based search algorithms are commonly used to provide global path planning for automated guided vehicles (AGVs). The A* algorithm, in particular, is widely used due to its simplicity and efficiency. However, the A* algorithm also has its challenges. Because global path planning is only one step in an AGV system, the system typically allocates only a small portion of computing power to path search and planning. However, when faced with large maps with obstacles, the A* algorithm needs to store and maintain a large amount of search information, resulting in significant consumption of memory space, computing power, and time, reducing search efficiency. Summary of the Invention

[0005] The purpose of the present invention is to provide an improved path planning search algorithm based on grid maps, aiming to solve the problem of low search efficiency of existing search algorithms when facing some large maps with obstacles.

[0006] To achieve the above object, the present invention provides an improved path planning search algorithm based on a grid map, comprising the following steps:

[0007] S1 designs the storage data structure of the grid map nodes, uses bit fields to store the characteristic information of the grid map nodes, and obtains the final grid map;

[0008] S2 preprocesses the final grid map to obtain a preprocessed grid map;

[0009] S3 designs the cost function and obtains the improved algorithm;

[0010] S4 designs a search process based on the preprocessed grid map and the improved algorithm.

[0011] The preprocessing of the final grid map to obtain the initialization node information value includes:

[0012] S21 sets the values ​​of the eight directions of each node of the final grid map to -1, and sets the multiplication value k=0;

[0013] S22 starts to traverse all nodes, ignoring all obstacle nodes, and updates the values ​​of the 8 directions of non-obstruction nodes. If there is an adjacent node in the corresponding direction and the node is not an obstacle point, the value in that direction is set to 0;

[0014] S23 traverses all nodes and ignores all obstacle nodes; for the current non-obstructed current node P, update its value in 8 directions; if the value of p in a certain direction is less than k value, ignore it; if it is equal to k value and moves 2 in that direction, k If there is a non-obstruction node q after the step, and the value of q in this direction is not less than k, then the value of p in this direction is updated to k+1;

[0015] S24 increases the value of k by one;

[0016] S25 repeats steps S23-S24 until the value of any node in any direction on the final grid map does not change after one traversal, and the preprocessing is terminated to obtain a preprocessed grid map.

[0017] The design cost function is improved by an algorithm comprising:

[0018] S31 introduces a weight factor into the heuristic cost function to obtain a modified cost function;

[0019] S32 increases the weight of the modified cost function to obtain an improved algorithm.

[0020] The heuristic cost function adopts a diagonal distance calculation formula.

[0021] The search process based on the pre-processed grid map and the improved algorithm is designed, including:

[0022] S41 loads the pre-processed grid map into the memory, then determines the starting point and the end point of the path finding, and puts the information of the starting point into the Open list of the improved algorithm to start the path finding;

[0023] S42: If the nodes in the Open list are not empty, then according to the node information in the Open list, take out the current node P with the smallest GVal, put the information of the current node P into the Closed list, and determine the end point and the close point of the end point based on the current node P, and add the close point to the Open list;

[0024] S43 checks whether the end point exists in the Closed list. If so, the path is traced back from the end point. Otherwise, it can be determined that there is no path from the starting point to the end point, and the path finding ends.

[0025] The method of extracting the current node P with the smallest GVal from the node information in the Open list, placing the information of the current node P in the Closed list, and determining the end point and the close point of the end point based on the current node P, and adding the close point to the Open list includes:

[0026] S421 extracts the current node P with the smallest GVal from the node information in the Open list;

[0027] S422 puts the information of the current node P into the Closed list and records the previous node S that reaches the current node P;

[0028] S423 determines whether the current node P is the end point. If the current node P is the end point, execute step S43; otherwise, execute step S423.

[0029] S424 finds out the eight adjacent nodes around the current node P that are not in the Open list and the Closed list and adds them to the Open list;

[0030] S425 finds the relative direction of the current node P to the end point, and then determines the relative direction and two adjacent directions as the search direction;

[0031] S436 finds the end point close to the end point according to the search direction and the multiplication information of the current node P and adds it to the Open list;

[0032] S436 loops steps S421-SS436 until step S423 determines the end point.

[0033] The present invention discloses an improved path planning search algorithm based on a grid map. The algorithm designs a storage data structure for grid map nodes and uses bit fields to store feature information representing grid map nodes to obtain a final grid map. The algorithm preprocesses the final grid map to obtain a preprocessed grid map. A cost function is designed to obtain an improved algorithm. A search process is designed based on the preprocessed grid map and the improved algorithm. The improved search algorithm of the present invention has faster search efficiency in factory environments with low obstacle density, solving the problem of low search efficiency of existing search algorithms when facing large maps with obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0035] Figure 1 It is a schematic diagram of the two-dimensional coordinates of the end point established at the current node P.

[0036] Figure 2 This is a schematic diagram of the two-dimensional coordinates where the end point is no longer at the current node P.

[0037] Figure 3 This is a schematic diagram of situation one.

[0038] Figure 4 This is a schematic diagram of scenario 2.

[0039] Figure 5 This is a schematic diagram of an improved path planning search algorithm based on a grid map provided by the present invention.

[0040] Figure 6 This is a flowchart of an improved path planning search algorithm based on a grid map provided by the present invention. DETAILED DESCRIPTION

[0041] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0042] See also Figures 1 to 6 The present invention provides an improved path planning search algorithm based on a grid map, comprising the following steps:

[0043] S1 designs the storage data structure of the grid map nodes, uses bit fields to store the characteristic information of the grid map nodes, and obtains the final grid map;

[0044] Specifically, a grid map is used to represent the vehicle's operating area environment. The map is divided into several grids, each representing a discrete spatial unit. Binary values ​​are used to represent the specific attributes of the spatial unit, for example, 0 represents a passable grid, while 1 represents an obstacle on the grid that is inaccessible. The improved algorithm requires the design of a node storage data structure and the initialization of map node information through map preprocessing.

[0045] A 64-bit long integer is used to store all the information of a node.

[0046] The storage information is designed as follows:

[0047]

[0048]

[0049] S2 preprocesses the final grid map to obtain a preprocessed grid map;

[0050] The specific method is:

[0051] S21 sets the values ​​of the eight directions of each node of the final grid map to -1, and sets the multiplication value k=0;

[0052] S22 starts to traverse all nodes, ignoring all obstacle nodes, and updates the values ​​of the 8 directions of non-obstruction nodes. If there is an adjacent node in the corresponding direction and the node is not an obstacle point, the value in that direction is set to 0;

[0053] S23 traverses all nodes and ignores all obstacle nodes; for the current non-obstructed current node P, update its value in 8 directions; if the value of p in a certain direction is less than k value, ignore it; if it is equal to k value and moves 2 in that direction, k If there is a non-obstruction node q after the step, and the value of q in this direction is not less than k, then the value of p in this direction is updated to k+1;

[0054] S24 increases the value of k by one;

[0055] S25 repeats steps S23-S24 until the value of any node in any direction on the final grid map does not change after one traversal, and the preprocessing is terminated to obtain a preprocessed grid map.

[0056] S3 designs the cost function and obtains the improved algorithm;

[0057] The A* algorithm is a commonly used heuristic path search algorithm, typically used for path planning in grid maps. It combines the advantages of both the Dijkstra algorithm and the DFS (depth-first search) algorithm. Building on the Dijkstra algorithm, it prioritizes the search direction using a heuristic evaluation function h, eliminating the blindness of the BFS (breadth-first search) algorithm and reducing the number of irrelevant nodes searched, resulting in a faster path to the target.

[0058] The evaluation function f(n) of the A* algorithm can be expressed as

[0059] f(n)=g(n)+h(n)

[0060] In the above formula, n represents the current search point position, g(n) represents the actual cost of reaching the current node from the starting point, which is usually expressed as the actual path length; h(n) represents the heuristic cost function of reaching the target node from the current node, which is usually expressed as the Euclidean distance between the current node and the end point, that is:

[0061]

[0062] Among them, (x n ,y n ) represents the coordinates of the current point, (x e ,y e ) represents the coordinates of the end point.

[0063] Therefore, f(n) which combines g(n) and h(n) represents the cost estimation function of reaching the end point from the starting point through n points.

[0064] h(n) is the most distinctive heuristic function in the A* algorithm and its primary feature that distinguishes it from other algorithms. For the A* algorithm, the value of the heuristic evaluation function h(n) determines search speed and accuracy. When h(n) equals 0, the A* algorithm is equivalent to the Dijkstra algorithm. When h(n) is strictly equal to the distance from the current node to the target node, the A* algorithm achieves maximum search efficiency in an unobstructed map.

[0065] The implementation process of the A* algorithm to search for a better path is as follows:

[0066] 1) Create an Open list and a Closed list to store information about nodes to be visited and nodes that have been visited, respectively.

[0067] 2) Set the starting point and end point, calculate the cost f of the starting point, the heuristic cost h and the cost g to reach the current node, and then store the starting point and cost information in the Open list.

[0068] 3) Select the node with the smallest f value from the Open list, remove it from the Open list and add it to the Closed list.

[0069] 4) Expand the current node, add the adjacent nodes of the current node to the Open list, and calculate the values ​​of f, h and g for the nodes newly added to the Open list.

[0070] 5) If the newly added node already exists in the Open list, compare their g values. If the new path cost is better, update the node's g value and f value.

[0071] 6) If the newly added node already exists in the Closed list, it will be ignored.

[0072] 7) Repeat steps 3-6 until the destination is added to the Closed list, or the Open list is empty (indicating that the destination cannot be reached).

[0073] 8) If the end point is added to the Closed list, find a better path by backtracking from the end point to the starting point.

[0074] The specific method is:

[0075] S31 introduces a weight factor into the heuristic cost function to obtain a modified cost function;

[0076] Specifically, the heuristic cost function adopts a diagonal distance calculation formula.

[0077] S32 increases the weight of the modified cost function to obtain an improved algorithm.

[0078] Specifically, the cost function plays a very important role in the A* algorithm and is a reference indicator for the node traversal priority in the A* algorithm.

[0079] The heuristic cost function h(n) of the improved algorithm will use the calculation formula of diagonal distance:

[0080] dx=abs(x n -x e ) (1)

[0081] dy=abs(y n -y e ) (2)

[0082] h(n)=D1*min(dx,dy)+D2*abs(dx-dy) (3)

[0083] In the above three formulas, (x n ,y n ) represents the coordinates of the current point, (xe ,y e ) represents the coordinates of the endpoint, dx represents the distance between the current point and the endpoint on the x-axis, dy represents the distance on the y-axis, D1 represents the cost of moving one square in the diagonal direction, and D2 represents the cost of moving one square in the straight line. We then introduce the weight factor k and change the cost function to f(n) = g(n) + k·h(n). By modifying the calculation formula for h(n) and increasing its weight, the A* algorithm is made closer to the Dijkstra algorithm, reducing the number of searches in blind spots.

[0084] S4 designs a search process based on the preprocessed grid map and the improved algorithm.

[0085] The specific method is:

[0086] S41 loads the pre-processed grid map into the memory, then determines the starting point and the end point of the path finding, and puts the information of the starting point into the Open list of the improved algorithm to start the path finding;

[0087] S42: If the nodes in the Open list are not empty, then according to the node information in the Open list, take out the current node P with the smallest GVal, put the information of the current node P into the Closed list, and determine the end point and the close point of the end point based on the current node P, and add the close point to the Open list;

[0088] Specifically, the method of extracting the current node P with the smallest GVal from the node information in the Open list, placing the information of the current node P in the Closed list, determining the end point and the close point of the end point based on the current node P, and adding the close point to the Open list includes:

[0089] S421 extracts the current node P with the smallest GVal from the node information in the Open list;

[0090] Specifically, the information of node P is first placed in the Closed list, indicating that a better path from the starting point to node P has been found and node P will not be visited again. At the same time, the previous node S before reaching node P is recorded for subsequent path backtracking.

[0091] S422 puts the information of the current node P into the Closed list and records the previous node S that reaches the current node P;

[0092] S423 determines whether the current node P is the end point. If the current node P is the end point, execute step S43; otherwise, execute step S423.

[0093] S424 finds out the eight adjacent nodes around the current node P that are not in the Open list and the Closed list and adds them to the Open list;

[0094] S425 finds the relative direction of the current node P to the end point, and then determines the relative direction and two adjacent directions as the search direction;

[0095] Specifically, the end point is on the coordinate axis of the two-dimensional coordinates established by the current node P. As shown in the figure below, the end point is directly to the right of the current node P, so the three search directions are the upper right, the right, and the lower right. Figure 1 Indicated by the large arrow.

[0096] The end point is no longer on the coordinate axis of the two-dimensional coordinates established by the current node P, such as Figure 2 As shown, the end point is above and to the right of the current node P, so the three search directions are directly above, directly to the right, and directly to the right, as shown in Figure 2 Indicated by the large blue arrow.

[0097] S436 finds the end point close to the end point according to the search direction and the multiplication information of the current node P and adds it to the Open list;

[0098] Specifically, there are two situations, each with different processing:

[0099] Scenario 1: Figure 3 As shown, the end point is at the upper right of the current point P. The search direction of point P is to the right, and the value S of point P in the right direction is 右 (P)=5, which means it can go right at least 2 5 When the grid reaches the brown dot, it also means that the range of the next point to the right of point P cannot exceed 32 grids, and the coordinates of the end point and the current point P are only 19 grids apart on the X axis, so we can directly add the point 19 grids to the right of the current point P to the Open list in one step.

[0100] Scenario 2: Figure 4 As shown. Due to the obstruction of the obstacle, point P cannot go all the way to the right to reach 32 squares to the right of P, so the value S of point P in the right direction 右 (P)=4, which means you can move right at least 2 steps. 4 The next step is to reach the point 16 squares to the right of the current point P. The range of the next point to the right of point P cannot exceed 16 squares, and the X coordinate of the end point is more than 16 squares larger than the X coordinate of point P. Therefore, the next step can only add the point 16 squares to the right of the current point P to the Open list.

[0101] S436 loops steps S421-SS436 until step S423 determines the end point.

[0102] S43 checks whether the end point exists in the Closed list. If so, the path is traced back from the end point. Otherwise, it can be determined that there is no path from the starting point to the end point, and the path finding ends.

[0103] Search time comparison experiment

[0104] 1) Simulate low-density obstacle factory scene

[0105] 100 randomly generated low-density grid maps simulating factory scenarios (30 small maps with a length and width of less than 500, and 70 large maps with a length and width between 1000 and 2000). 500 sets of random starting and ending points were generated for each map to simulate path planning. The final experimental data statistics are shown in the following table:

[0106]

[0107] As can be seen from the table above, in the low-obstacle factory scenario, the improved algorithm significantly improves path search efficiency compared to the traditional A* algorithm and the jump-point search algorithm. The length of the reachable path searched is, on average, approximately 10% longer than that of the A* algorithm and the jump-point search algorithm.

[0108] 2) Simulating a high-density obstacle factory scene

[0109] 100 randomly generated high-density grid maps simulating factory scenarios (30 small maps with a length and width of less than 500, and 70 large maps with a length and width between 1000 and 2000). 500 sets of random starting and ending points were generated for each map to simulate path planning. The final experimental data statistics are shown in the following table:

[0110]

[0111] As can be seen from the table above, as the obstacle density increases, the search efficiency advantage of the improved algorithm over the A* algorithm and the jump point search algorithm gradually decreases, and the disadvantage in path length compared to the jump point search algorithm becomes more and more obvious.

[0112] The simulation experimental data shows that the improved search algorithm has faster search efficiency in a factory environment with low-density obstacles, and the search path length is not much different from that of the traditional A* algorithm and the jump point search algorithm.

[0113] The above disclosure is merely a preferred embodiment of an improved path planning search algorithm based on a grid map of the present invention. It is certainly not intended to limit the scope of the present invention. A person skilled in the art will understand that implementing all or part of the processes of the above embodiment and making equivalent changes in accordance with the claims of the present invention still fall within the scope of the invention.

Claims

1. An improved path planning search algorithm based on a grid map, characterized in that: The following steps are involved: S1 designs the storage data structure of the grid map nodes, uses bit fields to store the characteristic information of the grid map nodes, and obtains the final grid map; S2 preprocesses the final grid map to obtain a preprocessed grid map; S3 designs the cost function and obtains the improved algorithm; S4 designs a search process based on the preprocessed grid map and the improved algorithm; The preprocessing of the final grid map to obtain the initialization node information value includes: S21 sets the values ​​of the eight directions of each node in the final grid map to -1, and sets the multiplication value k=0; S22 starts to traverse all nodes, ignoring all obstacle nodes, and updates the values ​​of the 8 directions of non-obstruction nodes. If there is an adjacent node in the corresponding direction and the node is not an obstacle point, the value in that direction is set to 0; S23 traverses all nodes and ignores all obstacle nodes; for the current non-obstructed current node P, update its value in 8 directions; if the value of p in a certain direction is less than k value, ignore it; if it is equal to k value and moves in that direction If there is a non-obstruction node q after the step, and the value of q in this direction is not less than k, then the value of p in this direction is updated to k+1; S24 increases the value of k by one; S25 repeats steps S23-S24 until the value of any node in any direction on the final grid map does not change after one traversal, and the preprocessing is terminated to obtain a preprocessed grid map; The search process is designed based on the pre-processed grid map and the improved algorithm, including: S41 loads the pre-processed grid map into the memory, then determines the starting point and the end point of the path finding, and puts the information of the starting point into the Open list of the improved algorithm to start the path finding; S42: If the nodes in the Open list are not empty, then according to the node information in the Open list, take out the current node P with the smallest GVal, put the information of the current node P into the Closed list, and determine the end point and the close point of the end point based on the current node P, and add the close point to the Open list; S43 checks whether the end point exists in the Closed list. If so, the path is backtracked from the end point. Otherwise, it is determined that there is no path from the starting point to the end point, and the path finding ends.

2. The improved path planning search algorithm based on a grid map as claimed in claim 1, characterized in that: The design cost function obtains an improved algorithm, including: S31 introduces a weight factor into the heuristic cost function to obtain a modified cost function; S32 increases the weight of the modified cost function to obtain an improved algorithm.

3. The improved path planning search algorithm based on a grid map as claimed in claim 2, characterized in that: The heuristic cost function adopts a diagonal distance calculation formula.

4. The improved path planning search algorithm based on a grid map as claimed in claim 1, characterized in that: The process of extracting the current node P with the smallest GVal from the node information in the Open list, placing the information of the current node P in the Closed list, determining the end point and a close point to the end point based on the current node P, and adding the close point to the Open list includes: S421 extracts the current node P with the smallest GVal from the node information in the Open list; S422 puts the information of the current node P into the Closed list and records the previous node S that reaches the current node P; S423 determines whether the current node P is the end point. If the current node P is the end point, execute step S43; otherwise, execute step S423. S424 finds out the eight adjacent nodes around the current node P that are not in the Open list and the Closed list and adds them to the Open list; S425 finds the relative direction of the current node P to the end point, and then determines the relative direction and two adjacent directions as the search direction; S436 finds the end point close to the end point according to the search direction and the multiplication information of the current node P and adds it to the Open list; S436 loops steps S421-SS436 until step S423 determines the end point.

Images