Path planning method, system, device and medium based on improved a-star algorithm

Abstract

The application discloses a path planning method, system, device and medium based on an improved A-star algorithm, relates to the technical field of path planning, and solves the technical problems of poor path planning safety and low efficiency in a warehouse robot system. Technical solution points estimate the area openness degree through a cost function based on obstacle density, so that the planned path is more inclined to the open area. The node expansion is performed through the optimized node expansion strategy, the nodes adjacent to the current node up and down or left and right are preferentially processed, the expansion of internal obstacle nodes is skipped, the number of expanded nodes is greatly reduced, and the nodes that may cause path collision or insecurity are effectively excluded to avoid meaningless path search. The final expanded node array is subjected to path optimization through a bidirectional path preprocessing optimization algorithm, so that the path is more concise and smooth, and the overall time consumption is reduced.

Description

Technical Field

[0001] This application relates to the field of path planning technology, and in particular to a path planning method, system, device and medium based on an improved A* algorithm. Background Technology

[0002] In warehouse robot systems, path planning and obstacle avoidance are crucial for achieving efficient material handling. Existing path planning algorithms, such as the classic A* algorithm, while possessing global search capabilities, suffer from the following main drawbacks in dynamic and complex environments:

[0003] (1) Insufficient path safety: The path planning of the traditional A* algorithm is easily affected by the distribution of local obstacles, resulting in the path being close to the edge of the obstacle and lacking safety redundancy.

[0004] (2) Node expansion redundancy: Too many node expansions result in longer path generation time, affecting real-time performance.

[0005] (3) Too many path inflection points: The eight-neighborhood expansion strategy generates a large number of redundant inflection points, which significantly increases the complexity and energy consumption of motion control.

[0006] To address the aforementioned issues, this invention proposes a path planning method based on an improved A* algorithm, aiming to enhance the safety and efficiency of path planning. Summary of the Invention

[0007] This application provides a path planning method, system, device, and medium based on an improved A* algorithm, the technical purpose of which is to improve the security and efficiency of path planning.

[0008] The above-mentioned technical objective of this application is achieved through the following technical solution:

[0009] A path planning method based on an improved A* algorithm includes:

[0010] The degree of openness between the starting node and the target node is estimated by using a cost function based on obstacle density;

[0011] Based on the openness of the area, nodes are expanded according to the optimized node expansion strategy to obtain the final expanded node array.

[0012] The final expanded node array is optimized using a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning.

[0013] Furthermore, the cost function based on obstacle density is expressed as:

[0014] f(n)=g(n)+exp(exp(p_obs))*h(n);

[0015] Where h(n) represents the coefficient function, which is a concave curve; g(n) represents the actual cost function from the starting node to the current node; p_obs represents the obstacle density, and p_obs=c1*p_den+c2*std; where c1+c2=1, p_den represents the density value, and std represents the normalized value of the node coordinates.

[0016] Furthermore, the method for calculating the density value includes:

[0017] Construct an expanded rectangle between the starting node and the target node, count the number of obstacle points within the rectangle, calculate the total number of grid cells within the rectangle, and obtain the density value p_den based on the number of obstacle points and the total number of grid cells.

[0018] Furthermore, the optimized node expansion strategy includes:

[0019] When expanding the neighboring nodes of the current node, it checks whether the current node is in the CLOSED list. If it is, the current node is skipped and the neighboring nodes of the next node are expanded. Otherwise, the coordinates of the neighboring nodes are verified. If the neighboring node overlaps with an obstacle, the neighboring node is marked as an invalid node and removed. If the neighboring node does not overlap with an obstacle, the neighboring node is added to the final expanded node array. The CLOSED list is used to store the nodes that have been searched.

[0020] Furthermore, the bidirectional path preprocessing optimization algorithm includes reverse optimization and forward optimization;

[0021] The reverse optimization includes: reversing the final expanded node array from the target node to the starting node, and then traversing each node in the reverse arrangement in sequence to obtain the reverse-optimized path.

[0022] The forward optimization includes: arranging the path nodes of the reverse-optimized path from the starting node to the target node in a forward direction; detecting whether there are obstacles between the current node and the path nodes; if so, the path node is invalid; otherwise, the path node is stored in the new path; and finally, connecting the nodes in the new path to obtain the final path and complete the path planning.

[0023] Furthermore, the reverse optimization includes:

[0024] Step 100: Traverse each node in the reverse arrangement;

[0025] Step 101: Determine whether the current node, its parent node, and its grandparent node are collinear. If yes, proceed to step 102; otherwise, proceed to step 103.

[0026] Step 102: Skip the parent node, connect the current node directly to the grandparent node, and then go to step 100;

[0027] Step 103: Check whether the straight line segment between the current node and its grandparent node intersects with an obstacle. If yes, proceed to step 104; otherwise, proceed to step 105.

[0028] Step 104: Keep the parent node, then proceed to step 106;

[0029] Step 105: Skip the parent node, connect the current node directly to the grandparent node, and then proceed to step 106;

[0030] Step 106: Update the path;

[0031] Step 107: Check if all nodes have been traversed. If so, return the optimized path and end the reverse optimization. Otherwise, go to step 100.

[0032] Furthermore, in step 101, the vector angle between the current node, the parent node, and the grandparent node is calculated, and the collinearity of the current node, the parent node, and the grandparent node is determined based on the vector angle.

[0033] A path planning system based on the improved A* algorithm includes:

[0034] The openness estimation unit estimates the openness of the area between the starting node and the target node using a cost function based on obstacle density;

[0035] The node expansion unit expands nodes based on the openness of the area and according to the optimized node expansion strategy to obtain the final expanded node array;

[0036] The path optimization unit optimizes the final expanded node array using a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning.

[0037] An electronic device comprising:

[0038] At least one processor; and,

[0039] A memory that is communicatively connected to the at least one processor;

[0040] The memory stores a computer program that can be executed by the at least one processor, which enables the at least one processor to execute any of the path planning methods based on the improved A* algorithm.

[0041] A computer-readable storage medium storing a computer program that, when executed by a processor, implements any of the path planning methods based on the improved A* algorithm.

[0042] The beneficial effects of this application are as follows: The path planning method, system, device, and medium based on the improved A* algorithm described in this application estimate the openness of the area through a cost function based on obstacle density, making the planned path more inclined towards open areas. Node expansion is performed through an optimized node expansion strategy, prioritizing nodes adjacent vertically or horizontally to the current node and skipping the expansion of internal obstacle nodes, greatly reducing the number of expanded nodes and effectively eliminating nodes that may cause path collisions or unsafety, thus avoiding meaningless path searches. The final expanded node array is optimized through a bidirectional path preprocessing optimization algorithm, resulting in a simpler and smoother path with reduced overall time consumption.

[0043] In summary, the path planning method based on the improved A* algorithm described in this application can ensure the shortest travel time while guaranteeing path safety and improving search speed. Attached Figure Description

[0044] Figure 1 This is a schematic diagram showing two distribution scenarios of equal number of obstacle grids within the same area, obtained based on existing technology.

[0045] Figure 2 This is a flowchart of the reverse optimization process in the embodiments of this application;

[0046] Figure 3 This is a schematic diagram comparing two sets of evaluation function paths based on obstacle density and cost function paths based on distance in an embodiment of this application;

[0047] Figure 4 A diagram comparing the number of nodes and path planning of the A* algorithm and the improved A* algorithm;

[0048] Figure 5 This is a schematic diagram comparing unoptimized paths with paths optimized using the path planning method described in this application.

[0049] Figure 6 This is a schematic diagram of the included angle of vectors in an embodiment of this application. Detailed Implementation

[0050] The technical solution of this application will be described in detail below with reference to the accompanying drawings.

[0051] The path planning method based on the improved A* algorithm described in this application includes:

[0052] 10: Estimate the openness of the area between the starting node and the target node using a cost function based on obstacle density.

[0053] by Figure 1 Taking a grid map with concentrated obstacles in (a) as an example, this illustrates how the prior art calculates obstacle density. Using the red current node and the blue target point as diagonal points, a rectangle with an expansion of 0.7 units is constructed to limit the search area. The number of obstacle points within this rectangle is calculated to be 16. Dividing this by the total number of grid cells within the rectangle (100) yields an obstacle density of 0.16. However, this calculation method cannot accurately reflect the local idle areas within the region. Figure 1 (b) shows another case of dispersed obstacles, with 16 occupying grids distributed within the same defined area, compared to... Figure 1 (a) has the same density. However, the degree of openness in the two is completely different. Figure 1 The path in (a) is safer because it may have fewer inflection points. To address the above shortcomings, the method for calculating the openness of a region needs to be further improved. This application designs a new cost function that estimates the openness of a region by introducing the standard deviation and density normalization coefficient of regional obstacles, thereby dynamically assessing the safety of the path based on the openness of the region.

[0054] In this embodiment, the standard deviation of the occupied grid cells along the x-axis and y-axis within a defined range is calculated, and then the mean of the standard deviation is normalized as a description of the grid cell concentration. The coordinates of the occupied grid cells along the x-axis are CLOSD = {(x1,y1),(x2,y1)...(x...y1)}. n ,y n )}, where n represents the number of obstacles. Calculate the mean μ of the x-axis coordinates of all points. x , is represented as:

[0055]

[0056] The standard deviation σ of the x-axis coordinate x The calculation is performed and expressed as follows:

[0057]

[0058] Normalize the standard deviation to the range [0, 1], denoted as std_x:

[0059]

[0060] Where std_x represents the standard deviation σ x Normalize, N x N y These represent the width and height of the defined area, respectively.

[0061] The normalized standard deviations (std) in the x and y directions are expressed as follows:

[0062]

[0063] Where std_y represents the standard deviation σ of the y-axis coordinate. y Normalize, σ y The calculation method and σ x same.

[0064] Combining the density value p_den with normalization, the obstacle density p_obs can be calculated by selecting different combinations of normalization coefficients c1 and c2, expressed as:

[0065] p_obs = c1 * p_den + c2 * std

[0066] Where c1 + c2 = 1.

[0067] Specifically, the method for calculating the density value p_den includes: constructing an expanded rectangle between the starting node and the target node, counting the number of obstacle points within the rectangle, calculating the total number of grid cells within the rectangle, and obtaining the density value based on the number of obstacle points and the total number of grid cells.

[0068] The improved cost function based on obstacle density is then expressed as:

[0069] f(n)=g(n)+exp(exp(p_obs))*h(n);

[0070] Where h(n) represents the coefficient function, which is a concave curve. The smaller the probability, the smaller the coefficient value, making it easier to plan the path to an open area; g(n) represents the actual cost function from the starting node to the current node.

[0071] 11: Based on the openness of the area, expand the nodes according to the optimized node expansion strategy to obtain the final expanded node array.

[0072] Preferably, to enhance the A* algorithm's resistance to the influence of obstacles, an obstacle detection mechanism is introduced in the node expansion process. Potential obstacle nodes are identified by judging the number of obstacles, ensuring that the generated potential paths are safe and effective.

[0073] The optimized node expansion strategy is as follows:

[0074] (1) Obstacle constraint: Avoid searching for nodes located in obstacle areas.

[0075] (2) Directional constraints: optimize the connectivity and rationality of node expansion.

[0076] Specifically, when expanding the neighboring nodes of the current node, the system checks whether the current node is in the CLOSED list. If it is, the current node is skipped, and the expansion proceeds to the neighboring nodes of the next node. Otherwise, the coordinates of the neighboring nodes are verified: if a neighboring node overlaps with an obstacle, it is marked as an invalid node and removed, effectively eliminating nodes that may cause path collisions or unsafe paths, thus avoiding meaningless path searches; if a neighboring node does not overlap with an obstacle, it is added to the final expanded node array. The CLOSED list stores the nodes that have already been searched.

[0077] The improved node expansion strategy employs more refined expansion logic, prioritizing nodes that are vertically or horizontally adjacent to the current node. This effectively reduces unnecessary computation and skips the expansion of internal obstacle nodes.

[0078] 12: The expanded nodes are optimized by using a bidirectional path preprocessing optimization algorithm. The path is smoothed by detecting forward connectivity and removing collinear nodes in the reverse direction, thus completing the path planning.

[0079] In this application, the goal of improving the cost function and node expansion is to appropriately guide the path to a safe region and increase the planning speed. Although the safety and planning efficiency of the path are improved, problems such as non-shortest paths and too many inflection points still exist. The bidirectional path preprocessing optimization algorithm effectively solves the problem of too many inflection points in the A* algorithm, improving the continuity and feasibility of the path.

[0080] Preferably, the bidirectional path preprocessing optimization algorithm includes reverse optimization and forward optimization.

[0081] The reverse optimization is as follows Figure 2 As shown, this includes: reversing the order of the expanded nodes from the target node to the starting node, and then traversing each node in the reversed order to obtain the reverse-optimized path, specifically including:

[0082] Step 100: Traverse each node in the reverse arrangement.

[0083] Step 101: Determine whether the current node, its parent node, and its grandparent node are collinear. If yes, proceed to step 102; otherwise, proceed to step 103.

[0084] Specifically, the angle between the vectors of the current node, its parent node, and its grandparent node is calculated, and the collinearity of these three nodes is determined based on this angle. Figure 6 As shown.

[0085] Step 102: Skip the parent node, connect the current node directly to the grandparent node, and then go to step 100.

[0086] Step 103: Check whether the straight line segment between the current node and its grandparent node intersects with an obstacle. If yes, proceed to step 104; otherwise, proceed to step 105.

[0087] Specifically, obstacle detection is achieved by calculating the equation of the straight line segment formed by the current node and the grandparent node, that is, by calculating the minimum distance between the obstacle and the straight line segment to determine whether there is an intersection.

[0088] Step 104: Keep the parent node, then proceed to step 106.

[0089] Step 105: Skip the parent node, connect the current node directly to the grandparent node, and then proceed to step 106.

[0090] Step 106: Update the path.

[0091] Step 107: Check if all nodes have been traversed. If so, return the optimized path and end the reverse optimization. Otherwise, go to step 100.

[0092] The forward optimization includes: arranging the path nodes of the reverse-optimized path from the starting node to the target node in a forward direction; detecting whether there are obstacles between the current node and the path nodes; if so, the path node is invalid; otherwise, the path node is stored in the new path; and finally, connecting the nodes in the new path to obtain the final path and complete the path planning.

[0093] In summary, forward optimization focuses more on the connection between path nodes and obstacle detection, making it suitable for handling path connectivity and safety issues. Backward optimization, on the other hand, focuses more on path smoothness and straightening.

[0094] The path planning system based on the improved A* algorithm described in this application includes an openness estimation unit, a node expansion unit, and a path optimization unit. The openness estimation unit estimates the openness of the area between the starting node and the target node using a cost function based on obstacle density. The node expansion unit expands nodes according to an optimized node expansion strategy based on the openness estimation, resulting in expanded nodes. The path optimization unit optimizes the expanded nodes using a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete the path planning.

[0095] The path planning method based on the improved A* algorithm described in this application will be verified through specific embodiments below.

[0096] First, verify whether the path planned by the method in this application tends to choose open areas. For example... Figure 3As shown, the green path between start point 1 and end point 1 represents the path planned by the existing A* algorithm, while the blue path is the path planned using the improved A* algorithm described in this application. Within the range of 3 to 30 on the horizontal coordinate, compared to the existing A* algorithm, the path based on the improved cost function selects a more open area around the shelves. However, the subsequent path, while minimizing the total length, does not pass through any significant empty areas; therefore, there is no significant difference between the two in this segment. For the path between start point 2 and end point 2, the black path is the result planned by the existing A* algorithm, and the red path is the result of the improved A* algorithm described in this application. Within the area between the start point and coordinates (50, 50), the path planned by the existing A* algorithm directly heads towards the end point and traverses the shelf area, failing to adequately consider the distribution of obstacles. The improved A* algorithm of this application, however, selects a relatively open area on the left, performing particularly well in the latter half of the path, successfully avoiding the checkerboard-shaped shelf layout. Overall, the path planned using the improved cost function based on obstacle density tends to favor open areas, rather than solely considering the straight-line distance to the target point.

[0097] The simulation results of the optimized node expansion strategy are as follows: Figure 3 In this algorithm, the number of extended nodes between start point 1 and end point 1 is 142, with almost no redundant nodes. This represents a 96.4% reduction compared to the 3935 nodes extended by the existing A* algorithm with the same start and end points. In more complex cases, such as... Figure 4 As shown in (a), in order to increase the complexity of the search, the starting point is set in the concave area in the lower left and the target point is located in the upper right. Figure 4 The medium gray grid represents expanded nodes, the black path represents the path planned by the existing A* algorithm, and the blue path is planned by the A* algorithm with improved cost function and node search optimization in this application. The existing A* algorithm expands by 3379 nodes, while the improved A* algorithm expands by only 355 nodes, a reduction of 89.5%. Except for a slight redundancy in the number of exit nodes in the concave region, the number of additional nodes for subsequent path expansion is significantly reduced. Figure 4 The path comparison in (c) and (d) shows that the existing A* algorithm expands the number of nodes by 1922, while the improved A* algorithm in this application expands by 416, a reduction of 78.4%. The improved A* algorithm in this application significantly reduces the number of nodes while ensuring the quality of path planning.

[0098] Figure 5 The results demonstrate the path optimization achieved using the method described in this application. The black path represents the existing A* algorithm, the red path has undergone node search optimization but not path smoothing, and the blue path represents the path optimized using the method described in this application. For ease of comparison, two sets of path points are provided, with their start and end coordinates compared to... Figure 3The results are consistent. Taking the path between start point 1 and end point 1 as an example, although the red path after improving the cost function and node search strategy has certain advantages, it is not superior to the original A* algorithm in terms of the number of path inflection points and the distance to obstacles. Prioritizing adjacent points (up, down, left, right) during node search leads to an increase in the number of inflection points in some areas, such as the path between x-coordinate 66 and end point 1. It is evident that after path preprocessing optimization, many polyline segments are integrated into a simpler path in the comparison from 66 to end point 1, and the distance between the path and obstacles is also improved. For the path between start point 2 and end point 2, the blue optimized path between start point 2 and x-coordinate 31 only requires three path segments to replace the complex red path. The results show that path optimization has a significant effect on improving path efficiency.

[0099] The performance of different path planning algorithms is compared and analyzed using specific data, and the effectiveness of path optimization is evaluated. Figure 5The path from start point 1 to end point 1, planned using the existing A* algorithm, has a total length of 110.81m. However, the path length of A_dai_jie, optimized with improved cost function and node search, slightly increases to 111.63m, and the number of path segments increases to 23, an increase of 35.3%. Simultaneously, the total turning angle increases to 1035°, an increase of 43.75%, indicating increased path complexity. Applying the path optimization method of this application to A_dai_jie_lu reduces the path length to 106.99m, a 4.16% reduction compared to A_dai_jie; the number of path segments decreases to 7, a 58.82% reduction compared to the existing A* algorithm and a 69.57% reduction compared to the A_dai_jie algorithm; the total turning angle significantly decreases to 218.15°, only 21.08% of A_dai_jie and 30.3% of the existing A* algorithm; the theoretical total time is 116.73s, a 14.64% reduction compared to the existing A* algorithm. For the path from start point 2 to end point 2, the existing A* algorithm has a path length of 103.86m, while the optimized path length using A_dai_jie increases to 121.18m, indicating increased path complexity. After A_dai_jie_lu polyline optimization, the path length is reduced to 116.48m, the number of path segments is reduced to 6, the total turning angle is optimized to 255.5°, and the time taken is 125.23s. Although the final path length is 12.15% longer than the existing A* algorithm, the total time is reduced by 16.56%. The data shows that the path planned using the improved cost function based on obstacle density, node search optimization, and path optimization method can ensure the shortest travel time while guaranteeing path safety and improving search speed. Here, A_dai_jie represents the improved cost function and node search optimization method, and A_dai_jie_lu represents the improved cost function, node search optimization, and path optimization method, i.e., the path planning method based on the improved A* algorithm described in this application.

[0100] Table 1 Comparison of Path Data

[0101]

[0102] In summary, simulation experiments have verified the effectiveness of the improved A* algorithm described in this application. The results show that the algorithm effectively reduces the total path travel time by 14.64% and significantly improves path safety. Experimental results also demonstrate that the improved A* algorithm exhibits stronger adaptability and robustness in complex warehousing environments.

[0103] The above are exemplary embodiments of this application, and the scope of protection of this application is defined by the claims and their equivalents.

Claims

1. A path planning method based on an improved A* algorithm, characterized in that, include: The degree of openness between the starting node and the target node is estimated by using a cost function based on obstacle density; Based on the openness of the area, nodes are expanded according to the optimized node expansion strategy to obtain the final expanded node array. The final expanded node array is optimized using a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning. The cost function based on obstacle density is expressed as: f(n)=g(n)+exp(exp(p_obs))*h(n); Where h(n) represents the coefficient function, which is a concave curve; g(n) represents the actual cost function from the starting node to the current node; p_obs represents the obstacle density, and p_obs=c1*p_den+c1*std; where c1+c2=1, p_den represents the density value, and std represents the normalized value of the node coordinates. The optimized node expansion strategy includes: When expanding the neighboring nodes of the current node, it checks whether the current node is in the CLOSED list. If it is, the current node is skipped and the neighboring nodes of the next node are expanded. Otherwise, the coordinates of the neighboring nodes are verified. If the neighboring node overlaps with an obstacle, the neighboring node is marked as an invalid node and removed. If the neighboring node does not overlap with an obstacle, the neighboring node is added to the final expanded node array. The CLOSED list is used to store the nodes that have been searched. The bidirectional path preprocessing optimization algorithm includes reverse optimization and forward optimization; The reverse optimization includes: reversing the final expanded node array from the target node to the starting node, and then traversing each node in the reverse arrangement in sequence to obtain the reverse-optimized path. The forward optimization includes: arranging the path nodes of the reverse-optimized path from the starting node to the target node in a forward direction; detecting whether there are obstacles between the current node and the path nodes; if so, the path node is invalid; otherwise, the path node is stored in the new path; and finally, connecting the nodes in the new path to obtain the final path and complete the path planning.

2. The method as described in claim 1, characterized in that, The method for calculating the density value includes: Construct an expanded rectangle between the starting node and the target node, count the number of obstacle points within the rectangle, calculate the total number of grid cells within the rectangle, and obtain the density value p_den based on the number of obstacle points and the total number of grid cells.

3. The method as described in claim 2, characterized in that, The reverse optimization includes: Step 100: Traverse each node in the reverse arrangement; Step 101: Determine whether the current node, its parent node, and its grandparent node are collinear. If yes, proceed to step 102; otherwise, proceed to step 103. Step 102: Skip the parent node, connect the current node directly to the grandparent node, and then go to step 100; Step 103: Check whether the straight line segment between the current node and its grandparent node intersects with an obstacle. If yes, proceed to step 104; otherwise, proceed to step 105. Step 104: Keep the parent node, then proceed to step 106; Step 105: Skip the parent node, connect the current node directly to the grandparent node, and then proceed to step 106; Step 106: Update the path; Step 107: Check if all nodes have been traversed. If so, return the optimized path and end the reverse optimization. Otherwise, go to step 100.

4. The method as described in claim 3, characterized in that, In step 101, the vector angle between the current node, the parent node, and the grandparent node is calculated, and the collinearity of the current node, the parent node, and the grandparent node is determined based on the vector angle.

5. A path planning system based on an improved A* algorithm, wherein the path planning system is used to implement the path planning method based on the improved A* algorithm as described in any one of claims 1-4, characterized in that, The route planning system includes: The openness estimation unit estimates the openness of the area between the starting node and the target node using a cost function based on obstacle density; The node expansion unit expands nodes based on the openness of the area and according to the optimized node expansion strategy to obtain the final expanded node array; The path optimization unit optimizes the final expanded node array using a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning.

6. A device, characterized in that, The electronic device includes: At least one processor; and, A memory that is communicatively connected to the at least one processor; The memory stores a computer program that can be executed by the at least one processor, which enables the at least one processor to perform the path planning method based on the improved A* algorithm as described in any one of claims 1 to 4.

7. A computer-readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the path planning method based on the improved A* algorithm as described in any one of claims 1 to 4.

Images