Path planning method, system and equipment based on improved A star algorithm and medium

Abstract

The invention discloses a path planning method, system and equipment based on an improved A star algorithm, and a medium, relates to the technical field of path planning, and solves the technical problems of poor safety and low efficiency of path planning in a storage robot system. According to the technical scheme, the method is characterized in that the area open degree is estimated through a cost function based on the obstacle density, so that the planned path is more inclined to the open area. Node expansion is carried out through the optimized node expansion strategy, nodes adjacent to the current node up and down or left and right are preferentially processed, expansion of internal obstacle nodes is skipped, the number of the expansion nodes is greatly reduced, nodes possibly causing path collision or unsafety are effectively eliminated, and meaningless path search is avoided. And path optimization is carried out on the final expansion node array through a bidirectional path preprocessing optimization algorithm, so that the path is more concise and smoother, and the total time consumption is reduced.

Description

Technical Field

[0001] The present 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-star algorithm. Background Art

[0002] In warehouse robotic systems, path planning and navigation and obstacle avoidance are key to achieving efficient logistics handling. Existing path planning algorithms, such as the classic A-star algorithm, although capable of global search, suffer from the following major drawbacks in dynamic and complex environments:

[0003] (1) Insufficient path security: The path planning of the traditional A-star 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 extension redundancy: Too many node extensions will cause the path generation to take a long time, affecting the real-time performance.

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

[0006] In order to solve the above problems, the present invention proposes a path planning method based on an improved A-star algorithm, aiming to improve 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-star algorithm, the technical purpose of which is to improve the safety and efficiency of path planning.

[0008] The above technical objectives of this application are achieved through the following technical solutions:

[0009] A path planning method based on an improved A-star algorithm, comprising:

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

[0011] Based on the degree of regional openness, node expansion is performed according to the optimized node expansion strategy to obtain the final expanded node array;

[0012] The path of the final extended node array is optimized through 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] Wherein, 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 coordinate.

[0016] Furthermore, the density value calculation method includes:

[0017] An expanded rectangular box is constructed between the starting node and the target node. The number of obstacle points in the rectangular box is counted, and the total number of grids in the rectangular box is calculated. The density value p_den is obtained based on the number of obstacle points and the total number of grids.

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

[0019] When expanding the adjacent nodes of the current node, check whether the current node is in the CLOSED list. If so, skip the current node and expand the adjacent nodes of the next node. If not, verify the coordinates of the adjacent nodes. If the adjacent node overlaps with an obstacle, mark the adjacent node as an invalid node and clear it. If the adjacent node does not overlap with an obstacle, add the adjacent node to the final expanded node array. The CLOSED list is used to store nodes that have been searched.

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

[0021] The reverse optimization includes: reversely arranging the final expanded node array from the target node to the starting node, and then traversing each node in the reverse arrangement in order to obtain a 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 is an obstacle between the current node and the path node, if so, the path node is invalid, if not, the path node is stored in the new path, and finally connecting the nodes in the new path to obtain the final path, thereby completing the path planning.

[0023] Furthermore, the reverse optimization includes:

[0024] Step 100: traverse each node in reverse order;

[0025] Step 101: Determine whether the current node is on the same line with its parent node and grandparent node. If yes, go to step 102; if not, go to step 103.

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

[0027] Step 103: Check whether the straight line segment between the current node and the grandparent node intersects with the obstacle. If so, go to step 104; if not, go to step 105.

[0028] Step 104: retain the parent node, and then go to step 106;

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

[0030] Step 106: Update the path;

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

[0032] Furthermore, in step 101, by calculating the vector angle between the current node, the parent node and the grandparent node, it is determined whether the current node, the parent node and the grandparent node are collinear based on the vector angle.

[0033] A path planning system based on an improved A-star algorithm, comprising:

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

[0035] The node expansion unit, based on the degree of regional openness, performs node expansion according to the optimized node expansion strategy to obtain the final expanded node array;

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

[0037] A device, comprising:

[0038] at least one processor; and,

[0039] a memory communicatively coupled to the at least one processor;

[0040] The memory stores a computer program that can be executed by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute any of the path planning methods based on the improved A-star algorithm.

[0041] A computer-readable storage medium stores a computer program, wherein when the computer program is executed by a processor, the computer program implements any of the above-mentioned path planning methods based on the improved A-star algorithm.

[0042] The beneficial effects of the present application are as follows: the path planning method, system, device and medium based on the improved A-star algorithm described in the present application estimate the degree of openness of the area through a cost function based on the density of obstacles, so that the planned path is more inclined to open areas. Node expansion is performed through the optimized node expansion strategy, giving priority to nodes adjacent to the upper and lower or left and right of the current node, skipping the expansion of internal obstacle nodes, greatly reducing the number of expanded nodes, and effectively eliminating nodes that may cause path collisions or unsafe nodes to avoid meaningless path searches. The final expanded node array is optimized through a two-way path preprocessing optimization algorithm, making the path more concise and smooth, and reducing the overall time consumption.

[0043] In summary, the path planning method based on the improved A-star algorithm described in this application can ensure the shortest driving time while ensuring the safety of the path and improving the search speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 Schematic diagram of two distribution situations of equal obstacle grids in the same area obtained based on existing technology;

[0045] Figure 2 This is a flowchart of reverse optimization in an embodiment of the present application;

[0046] Figure 3 Schematic diagram comparing two groups of evaluation function paths based on obstacle density and cost function paths based on distance in an embodiment of the present application;

[0047] Figure 4 A schematic diagram comparing the number of expanded nodes and path planning between the A-star algorithm and the improved A-star algorithm;

[0048] Figure 5 Schematic diagram comparing the paths before and after optimization using the path planning method described in this application;

[0049] Figure 6 Schematic diagram of vector angle in the embodiment of this application. DETAILED DESCRIPTION

[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-star 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 The example of a grid map with concentrated obstacles in (a) illustrates how obstacle density is calculated in the prior art. Using the red current node and the blue target point as diagonal points, a rectangular box with a 0.7-unit expansion is constructed to define the search range. The number of obstacles within this box is calculated to be 16. Dividing this by the total number of grid cells within the box, 100, yields an obstacle density of 0.16. However, this calculation method does not accurately reflect the local availability of the area. Figure 1 (b) shows another case of scattered obstacles, with 16 occupied grids distributed within the same limited range, which is the same as Figure 1 The density of (a) is the same. However, the degree of emptiness of the two is completely different. Figure 1 The path in (a) is safer because it may have fewer turning points. To address the above shortcomings, the method for calculating regional openness needs to be further improved. This application designs a new cost function that estimates regional openness by introducing the standard deviation of regional obstacles and a density normalization coefficient, thereby dynamically evaluating path safety based on regional openness.

[0054] In the embodiment of the present application, the standard deviation of the grids occupied in the limited range on the x-axis and y-axis is calculated, and then the standard deviation is normalized to the mean to describe the concentration of the grid. The coordinate point of the grid occupied in the x-axis direction is CLOSD = {(x1, y1), (x2, y1) ... (x n ,y n )}, where n represents the number of obstacles. Calculate the mean x-axis coordinates of all coordinate points μ x , expressed as:

[0055]

[0056] The standard deviation σ of the x-axis coordinate x Calculate and express it as:

[0057]

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

[0059]

[0060] Among them, std_x represents the standard deviation σ x Normalize, N x ,N y Respectively represent the width and height of the limited area.

[0061] The standard deviation normalized value std in the x-direction and y-direction is expressed as:

[0062]

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

[0064] Combined with the density value p_den, after normalization, different combinations of normalization coefficients c1 and c2 can be selected to calculate the obstacle density p_obs, expressed as:

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

[0066] Among them, c1+c2=1.

[0067] Specifically, the density value p_den is calculated by constructing an expanded rectangular box between the starting node and the target node, counting the number of obstacle points within the rectangular box, and calculating the total number of grids within the rectangular box, and obtaining a density value based on the number of obstacle points and the total number of grids.

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

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

[0070] Among them, h(n) represents the coefficient function, which is a concave curve. The smaller the probability, the smaller the coefficient value, and it is 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 degree of regional openness, node expansion is performed according to the optimized node expansion strategy to obtain the final expanded node array.

[0072] Preferably, in order to enhance the resistance of the A-star algorithm to the influence of obstacles, an obstacle detection mechanism is introduced in the node expansion, and potential obstacle nodes are identified by judging the number of obstacles to ensure that the generated potential paths are safe and effective.

[0073] The optimized node expansion strategy is specifically optimized 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 adjacent nodes of the current node, the system checks whether the current node is in the CLOSED list. If so, the current node is skipped and the adjacent nodes of the next node are expanded. Otherwise, the coordinates of the adjacent nodes are verified. If the adjacent node overlaps with an obstacle, the adjacent node is marked as invalid and cleared, effectively eliminating nodes that may cause path collisions or are unsafe, thus avoiding meaningless path searches. If the adjacent node does not overlap with an obstacle, the adjacent node is added to the final expanded node array. The CLOSED list is used to store nodes that have already been searched.

[0077] The improved node expansion strategy adopts more sophisticated expansion logic, giving priority to nodes adjacent to the current node above, below, or to the left and right, which can effectively reduce unnecessary calculations and skip the expansion of internal obstacle nodes.

[0078] 12: The path of the expanded nodes is optimized through the bidirectional path preprocessing optimization algorithm, and the forward connectivity detection and reverse collinear node elimination are carried out to achieve path smooth optimization and complete 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 area and improve planning speed. Although this improves path safety and planning efficiency, issues such as non-shortest paths and excessive inflection points still exist. The bidirectional path preprocessing optimization algorithm effectively addresses the excessive inflection points in the A-star algorithm, improving path continuity and feasibility.

[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, it includes: rearranging the expanded nodes from the target node to the starting node in reverse order, and then traversing each node in the reverse arrangement in order to obtain the reverse optimized path, specifically including:

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

[0083] Step 101: Determine whether the current node is on the same line with its parent node and grandparent node. If yes, go to step 102; if not, go to step 103.

[0084] Specifically, by calculating the vector angle between the current node, the parent node, and the grandparent node, it is determined whether the current node, the parent node, and the grandparent node are collinear based on the vector angle, such as Figure 6 shown.

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

[0086] Step 103: Check whether the straight line segment between the current node and the grandparent node intersects with the obstacle. If so, go to step 104; if not, go 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 : retain the parent node, and then go to step 106 .

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

[0090] Step 106: Update the path.

[0091] Step 107: Check whether all nodes have been traversed. If so, return to the optimized path and end the reverse optimization. If not, 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 is an obstacle between the current node and the path node, if so, the path node is invalid, if not, the path node is stored in the new path, and finally connecting the nodes in the new path to obtain the final path, thereby completing the path planning.

[0093] In summary, forward optimization focuses more on the connection between path nodes and obstacle detection, and is suitable for dealing with path connectivity and safety issues. Reverse optimization focuses more on the smoothness and straightness of the path.

[0094] The path planning system based on the improved A-star algorithm described in this application includes an openness estimation unit, a node expansion unit, and a path optimization unit. The openness estimation unit is used to estimate 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 is used to expand nodes based on the openness of the area according to an optimized node expansion strategy to obtain expanded nodes; and the path optimization unit is used to optimize the path of the expanded nodes using a bidirectional path preprocessing optimization algorithm to achieve path smoothing optimization and complete path planning.

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

[0096] First, verify whether the path planned by this application method is more inclined to choose open areas. Figure 3As shown, the green path between starting point 1 and end point 1 represents the path planned by the existing A-star algorithm, and the blue path is the path obtained based on the path planning method of the improved A-star algorithm described in this application. In the interval between the horizontal coordinates of 3 and 30, compared with the existing A-star algorithm, the path based on the improved cost function chooses a more open area outside the shelf, but the subsequent path does not pass through obvious idle areas while ensuring the total length is as short as possible. There is no significant difference between the two in this section. For the path between starting point 2 and end point 2, the black path is the result of planning by the existing A-star algorithm, and the red path is the result of planning by the improved A-star algorithm of this application. In the area between the starting point and the coordinates (50, 50), the path planned by the existing A-star algorithm directly heads towards the end point and crosses the shelf area, failing to reasonably consider the distribution of obstacles. However, the path planned by the improved A-star algorithm of this application chooses a relatively open area on the left, especially in the second half of the path, successfully avoiding the checkerboard-shaped shelves. Overall, the path planned by the improved cost function based on obstacle density is more inclined to open areas, rather than just 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 the example, the number of nodes expanded between starting point 1 and end point 1 is 142, and there are almost no redundant nodes. This is 96.4% less than the 3935 nodes expanded by the existing A-star algorithm under the same starting point and end point. 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 corner and the target point is located in the upper right corner. Figure 4 The medium-gray grid represents expanded nodes. The black path represents the path planned by the existing A-star algorithm. The blue path is the path planned by the A-star algorithm with an improved cost function and node search optimization in this application. The existing A-star algorithm expanded 3379 nodes, while the improved A-star algorithm only expanded 355 nodes, a reduction of 89.5%. Except for the slightly redundant number of exit nodes in the concave area, the number of additional nodes expanded in the subsequent path is significantly reduced. Figure 4 Comparison of paths in (c) and (d) shows that the existing A-star algorithm expands the number of nodes to 1922, while the improved A-star algorithm in this application expands the number to 416, a reduction of 78.4%. The improved A-star algorithm in this application significantly reduces the number of nodes while maintaining the quality of path planning.

[0098] Figure 5 The black path is planned by the existing A-star algorithm, the red path is optimized by node search but not smoothed, and the blue path is the path optimized by the method described in this application. For the convenience of comparison, two groups of path points are set, and their starting and ending coordinates are the same as Figure 3. For example, the path between start point 1 and end point 1 shows some advantages. The improved cost function and node search strategy yield a red path with some advantages, but it does not outperform the original A-star algorithm in terms of the number of inflection points and distance to obstacles. The algorithm prioritizes adjacent points above, below, left, and right during node search, resulting in an increase in the number of inflection points in some areas, such as the path between horizontal coordinate 66 and end point 1. It is clear that after path preprocessing and optimization, many broken line segments are integrated into a simpler path, while the distance to obstacles is also improved. For the path between start point 2 and end point 2, the optimized blue path between start point 2 and horizontal coordinate 31 replaces the complex red path with only three segments. These results demonstrate that path optimization significantly improves path efficiency.

[0099] Compare and analyze the performance of different path planning algorithms based on specific data, and evaluate the effect of path optimization. Figure 5For the path from start point 1 to end point 1, the total length of the path planned using the existing A-star algorithm is 110.81m. However, the path length of A_dai_jie with improved cost function and node search optimization increased slightly to 111.63m, and the number of path segments increased to 23, an increase of 35.3%; at the same time, the total turning angle rose to 1035°, an increase of 43.75%, indicating that the complexity of the planned path increased. After the application of the path optimization method of this application, the path length of A_dai_jie_lu was reduced to 106.99m, which was 4.16% less than that of A_dai_jie; the number of path segments was reduced to 7, which was 58.82% less than that of the existing A-star algorithm and 69.57% less than that of A_dai_jie algorithm; the total turning angle was significantly reduced to 218.15°, which was only 21.08% of A_dai_jie and 30.3% of the existing A-star algorithm; the theoretical total time was 116.73s, which was 14.64% less than that of the existing A-star algorithm. For the path from starting point 2 to end point 2, the path length of the existing A-star algorithm is 103.86m, while the length after A_dai_jie optimization increases to 121.18m, indicating an increase in path complexity. After A_dai_jie_lu broken line optimization, the path length is reduced to 116.48m, the number of path segments is reduced to 6 segments, the total turning angle is optimized to 255.5°, and the time consumption is 125.23s; although the final path length is 12.15% longer than that of the existing A-star algorithm, the total time consumption is reduced by 16.56%. The data show that the path planned by the improved cost function, node search optimization and path optimization method based on obstacle density can ensure the shortest driving time while ensuring the safety of the path and improving the search speed. Among them, A_dai_jie represents the method of improving cost function and node search optimization, and A_dai_jie_lu represents the method of improving cost function, node search optimization and path optimization, that is, the path planning method based on the improved A-star algorithm described in this application.

[0100] Table 1 Path data comparison

[0101]

[0102] In summary, simulation experiments validated the effectiveness of the improved A-star algorithm described in this application. Results showed that the algorithm effectively reduced the total route travel time by 14.64% and significantly improved route safety. Experimental results demonstrate that the improved A-star algorithm exhibits greater adaptability and robustness in complex warehouse environments.

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

Claims

1. A path planning method based on an improved A-star algorithm, characterized in that: include: The degree of openness of the area between the starting node and the target node is estimated using a cost function based on obstacle density; Based on the degree of regional openness, node expansion is performed according to the optimized node expansion strategy to obtain the final expanded node array; The path of the final extended node array is optimized through a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning.

2. The method according to claim 1, wherein The cost function based on obstacle density is expressed as: f(n)=g(n)+exp(exp(p_obs))*h(n); Wherein, 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 coordinate.

3. The method according to claim 2, wherein The calculation method of the density value includes: An expanded rectangular box is constructed between the starting node and the target node. The number of obstacle points in the rectangular box is counted, and the total number of grids in the rectangular box is calculated. The density value p_den is obtained based on the number of obstacle points and the total number of grids.

4. The method according to claim 1, wherein The optimized node expansion strategy includes: When expanding the adjacent nodes of the current node, check whether the current node is in the CLOSED list. If so, skip the current node and expand the adjacent nodes of the next node. If not, verify the coordinates of the adjacent nodes. If the adjacent node overlaps with an obstacle, mark the adjacent node as an invalid node and clear it. If the adjacent node does not overlap with an obstacle, add the adjacent node to the final expanded node array. The CLOSED list is used to store nodes that have been searched.

5. The method according to claim 1, wherein The bidirectional path preprocessing optimization algorithm includes reverse optimization and forward optimization; The reverse optimization includes: reversely arranging the final expanded node array from the target node to the starting node, and then traversing each node in the reverse arrangement in order to obtain a 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 is an obstacle between the current node and the path node, if so, the path node is invalid, if not, the path node is stored in the new path, and finally connecting the nodes in the new path to obtain the final path, thereby completing the path planning.

6. The method according to claim 5, wherein The reverse optimization includes: Step 100: traverse each node in reverse order; Step 101: Determine whether the current node is on the same line with its parent node and grandparent node. If yes, go to step 102; if not, go to step 103. Step 102: Skip the parent node, connect the current node and the grandparent node directly, and then go to step 100; Step 103: Check whether the straight line segment between the current node and the grandparent node intersects with the obstacle. If so, go to step 104; if not, go to step 105. Step 104: retain the parent node, and then go to step 106; Step 105: Skip the parent node, connect the current node directly to the grandparent node, and then go to step 106; Step 106: Update the path; Step 107: Check whether all nodes have been traversed. If so, return to the optimized path and end the reverse optimization. If not, go to step 100.

7. The method according to claim 6, wherein In step 101, the vector angle between the current node, the parent node and the grandparent node is calculated, and whether the current node, the parent node and the grandparent node are collinear is determined based on the vector angle.

8. A path planning system based on an improved A-star algorithm, characterized in that: include: The empty space estimation unit estimates the empty space between the starting node and the target node using a cost function based on obstacle density; The node expansion unit, based on the degree of regional openness, performs node expansion according to the optimized node expansion strategy to obtain the final expanded node array; The path optimization unit optimizes the path of the final extended node array through a bidirectional path preprocessing optimization algorithm to achieve smooth path optimization and complete path planning.

9. A device, characterized in that The electronic device comprises: at least one processor; and, a memory communicatively coupled to the at least one processor; The memory stores a computer program that can be executed by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the path planning method based on the improved A-star algorithm as described in any one of claims 1 to 7.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the path planning method based on the improved A-star algorithm as claimed in any one of claims 1 to 7 is implemented.

Images