Path planning method, system, equipment and medium

By building an obstacle rate model and a child node optimization model, combined with the dynamic window algorithm DWA, the problem that the A* algorithm cannot effectively avoid moving obstacles in mobile robot path planning is solved, and the smooth obstacle avoidance and safety improvement of the path is achieved.

CN119022925AInactive Publication Date: 2024-11-26TAIZHOU UNIV (ZHEJIANG)
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202411100772.2
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-12
Publication Date
2024-11-26
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing A* algorithm cannot effectively avoid moving obstacles in mobile robot path planning, resulting in path planning that does not conform to the actual application environment.

Method used

By building a barrier rate model, adjust the heuristic function of the A* algorithm to reduce the number of search nodes, and use the child node optimization model and the dynamic window algorithm DWA to optimize the path to form the final planning path.

Benefits of technology

The path is smoothly avoids moving obstacles, reduces planning costs, and improves the safety and efficiency of path planning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119022925A_ABST
    Figure CN119022925A_ABST
Patent Text Reader

Abstract

The invention discloses a path planning method, system, equipment and medium, and relates to the technical field of path planning and navigation.The path planning method comprises the steps that the number of search nodes is reduced through an obstacle rate model by means of a heuristic function of an A * algorithm, so that a planned path is optimized; a child node optimization model is used for optimizing the adjusted A * algorithm, and the node optimization model takes adjacent nodes around the father node as child nodes, so that the child nodes and the father node jointly form a Sudoku; corresponding processing is carried out according to the child node type of the position where the obstacle is located; generating a global path between the starting node and the target node in the road environment modeling by using the optimized A * algorithm; and performing bidirectional smoothness processing on the global path, and optimizing the processed global path through a dynamic window algorithm (DWA) to form a final planned path. According to the invention, the path planned by the A * algorithm can accord with an actual application environment.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of path planning and navigation technology, and in particular to a path planning method, system, device and medium. Background Art

[0002] Different path planning algorithms are needed for different application scenarios. At present, many path planning algorithms have been applied in practical scenarios, but it is still a hot issue in the field of mobile robots. Path planning can be divided into two categories: one is global path planning based on known or partially known environmental maps, and the other is local path planning based on sensor information. When performing global path planning, we need to find a suitable path in the existing environmental information, and the accuracy of this path is determined by the accuracy of the environmental information.

[0003] In the prior art, the A* algorithm is used for path planning. When an unknown object appears and causes the external environment to change, local path planning is required to provide excellent obstacle avoidance performance.

[0004] However, the high-cost path estimated by the A* algorithm cannot smoothly avoid moving obstacles, making the algorithm unsuitable for practical application environments. Summary of the invention

[0005] The embodiments of the present invention provide a path planning method, system, device and medium, which can solve the problem in the prior art that the path planned by the A* algorithm does not conform to the actual application environment.

[0006] The embodiment of the present invention provides a path planning method, comprising the following steps: using a grid map including a start node and a target node to model a road environment, and constructing an obstacle rate model for reflecting the distribution of obstacles; using the heuristic function of the A* algorithm to reduce the number of search nodes through the obstacle rate model to optimize the planned path, the heuristic function can determine the search node to estimate the estimated cost from the current node to the target node; using a child node optimization model to optimize the adjusted A* algorithm; the child node optimization model is to use the adjacent nodes around the parent node as child nodes, so that multiple child nodes and the parent node form a nine-square grid together; when the position of the obstacle is located at the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the position of the obstacle is located at the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the position of the obstacle is located at the child node in the corner of the nine-square grid, no processing is performed; using the optimized A* algorithm to generate a global path between the start node and the target node in the road environment modeling; and performing bidirectional smoothness processing on the global path, and optimizing the processed global path through a dynamic window algorithm DWA to form a final planned path.

[0007] Furthermore, the obstacle rate model is the ratio of the number of grids where obstacles are located to the total number of grids.

[0008] Furthermore, the heuristic function of the A* algorithm reduces the number of search nodes through the obstacle rate model. The specific steps include: obtaining the total cost f(n) from the starting node to the current node in the A* algorithm, obtaining the actual cost g(n) from the starting node to the current node, obtaining the estimated cost h(n) from the current node to the target node, and obtaining the obstacle rate model P; the estimated cost is the heuristic function of the A* algorithm, and the number of search nodes of the heuristic function is reduced by the obstacle rate model P, so that the weight can be adjusted. The formula is:

[0009] f(n)=g(n)+(1-lnP)h(n).

[0010] Further, the A* algorithm after optimization and adjustment using the child node optimization model specifically comprises the following steps: optimizing the child node selection method of the A* algorithm after optimization and adjustment by the child node optimization model; taking the parent node as the center of the nine-square grid, and the points in the nine-square grid other than the parent node as the child nodes, starting from the first child node in the upper left corner, and sequentially numbering all the child nodes in a clockwise direction; the first category of child nodes are numbered 2 and 6, the second category of child nodes are numbered 4 and 8, and the third category of child nodes are numbered 1, 3, 5 and 7; when the obstacle is located at the first category of positions, that is, the obstacle is located at the upper and lower positions of the parent node, the left and right child nodes of the obstacle, that is, child nodes 1 and 3, or child nodes 5 and 7, should be removed; when the obstacle is located at the second category of positions, that is, the obstacle is located at the left and right positions of the parent node, the upper and lower child nodes of the obstacle, that is, child nodes 1 and 7, or child nodes 3 and 5, should be removed; when the obstacle is located at the third category of positions, no processing is performed.

[0011] Furthermore, the global path after being optimized by the dynamic window algorithm DWA specifically comprises the following steps: using the dynamic window algorithm to plan the local path so that the starting node and the target node are consistent with the starting node and the target node of the optimized A* algorithm, and setting the moving obstacles and the fixed obstacles to generate a new trajectory as the local path.

[0012] The embodiment of the present invention provides a path planning system, including: a model building module, which is used to use a grid map including a start node and a target node to model a road environment, and to build an obstacle rate model for reflecting the distribution of obstacles; an algorithm adjustment module, which is used to reduce the number of search nodes of the heuristic function of the A* algorithm through the obstacle rate model to optimize the planned path, and the heuristic function can determine the search node to estimate the estimated cost from the current node to the target node; an optimization model building module, which is used to optimize the adjusted A* algorithm using a child node optimization model; the child node optimization model is to use the adjacent nodes around the parent node as child nodes, so that multiple child nodes are adjacent to the parent node. The nodes together form a nine-square grid; when the obstacle is located at the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the corner of the nine-square grid, no processing is done; the algorithm fusion module is used to use the optimized A* algorithm to generate a global path between the start node and the target node in road environment modeling; and the global path is processed for bidirectional smoothness, and the global path after optimization is processed by the dynamic window algorithm DWA to form the final planned path.

[0013] An embodiment of the present invention provides a computer device, comprising: a memory and a processor; the memory stores a computer program, and the processor implements the above-mentioned path planning method when executing the computer program.

[0014] An embodiment of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the path planning method described above is implemented.

[0015] The embodiments of the present invention provide a path planning method, system, device and medium. Compared with the prior art, the advantages thereof are as follows:

[0016] A grid map containing the start node and the target node is used to model the road environment, and an obstacle rate model is constructed to reflect the distribution of obstacles. The heuristic function of the A* algorithm is used to reduce the number of search nodes through the obstacle rate model to optimize the planned path. The child node optimization model is used to optimize the adjusted A* algorithm. The child node optimization model takes the adjacent nodes around the parent node as child nodes, so that multiple child nodes and the parent node form a nine-square grid together. When the obstacle is located in the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the obstacle is located in the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the obstacle is located in the child node in the corner of the nine-square grid, no processing is done; using the optimized A* algorithm, a global path between the start node and the target node is generated in the road environment modeling, and the global path is processed for bidirectional smoothness, and the global path after optimization is processed by the dynamic window algorithm DWA to form the final planned path.

[0017] Among them, reducing the number of search nodes through the obstacle rate model can reduce unnecessary node searches. Combined with the processing of the sub-node optimization model around the obstacle location, path planning is more reasonable and the planning cost is reduced. Then, through the bidirectional smoothness processing of the global path and the use of the dynamic window algorithm DWA for avoiding moving obstacles to optimize the global path, the planned path can smoothly avoid moving obstacles, ultimately achieving the effect of smoothly avoiding moving obstacles with low-cost planned paths. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 A flowchart of an AGV path planning method for improving the A* and DWA fusion algorithm provided in an embodiment of the present invention;

[0019] Figure 2 A* algorithm node diagram provided by an embodiment of the present invention;

[0020] Figure 3 A bidirectional smoothing optimization strategy diagram for a path provided by an embodiment of the present invention;

[0021] Figure 4 A traditional A* algorithm planning trajectory diagram provided by an embodiment of the present invention;

[0022] Figure 5 The improved A* algorithm planning trajectory diagram provided by the embodiment of the present invention;

[0023] Figure 6 A conventional DWA algorithm planning trajectory diagram provided in an embodiment of the present invention;

[0024] Figure 7This is a trajectory diagram planned by the improved A* and DWA fusion algorithm provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0025] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the present invention, so the present invention is not limited by the specific embodiments disclosed below.

[0026] See also Figure 1 to Figure 7 , an embodiment of the present invention provides a path planning method, comprising the following steps:

[0027] Step 1: Use a grid map containing start nodes and target nodes to model the road environment, and build an obstacle rate model to reflect the distribution of obstacles.

[0028] Step 2: Use the heuristic function of the A* algorithm to reduce the number of search nodes through the obstacle rate model to optimize the planned path.

[0029] Step 3: Use the child node optimization model to optimize the adjusted A* algorithm; the child node optimization model is to take the current node as the parent node and the adjacent nodes around the parent node as child nodes; when the child node where the obstacle is located is located in the upper and lower directions of the parent node, remove the child nodes on the left and right directions of the child node where the obstacle is located; when the child node where the obstacle is located is located in the left and right directions of the parent node, remove the child nodes on the upper and lower directions of the child node where the obstacle is located; when the child node where the obstacle is located is located in the remaining position of the adjacent node, no processing is done.

[0030] Step 4: Use the optimized A* algorithm to generate a global path between the start node and the target node in the road environment modeling; perform bidirectional smoothness processing on the global path, and optimize the processed global path through the dynamic window algorithm DWA to form the final planned path.

[0031] The specific contents are as follows:

[0032] 1. The improved algorithm uses a grid map for environment modeling, sets the start node and target node on the grid map, and constructs an OPEN and CLOSE list.

[0033] The improved algorithm introduces the concept of obstacle rate:

[0034] In the grid map, the obstacle rate P is introduced to abstractly represent the distribution of obstacles. The obstacle rate can be defined as the ratio of the number of obstacle grids to the total number of grids. The starting node coordinates are set to (x s,y s ), the target node coordinates are (x g ,y g ), the number of barrier grids is N, and its expression is as follows:

[0035]

[0036] In the formula, P represents the obstacle rate and N represents the number of obstacle grids.

[0037] By quantifying the obstacle rate in the map information, the distribution of obstacles in the environment can be more accurately reflected, and the heuristic function in the A* algorithm can be dynamically adjusted accordingly, thereby reducing the number of unnecessary search nodes and improving the effectiveness of path planning.

[0038] For different obstacle distributions, different heuristic function strategies need to be adopted, that is, the weight of the heuristic function is dynamically adjusted according to the change of the obstacle rate P to ensure the accuracy and efficiency of path planning.

[0039] The optimized A* algorithm incorporates the concept of obstacle rate P into the evaluation function, which is described as follows:

[0040] f(n)=g(n)+(1-lnP)h(n) (2)

[0041]

[0042] Where f(n) represents the starting node (x s ,y s ) to the current node (x g ,y g ), g(n) represents the total cost of the starting node (x s ,y s ) to the current node (x n ,y n ), h(n) represents the actual cost of the current node (x n ,y n ) to the target node (x g ,y g ), and 1-lnP represents the way to adjust the heuristic function.

[0043] In this way, by dynamically adjusting the weight of the heuristic function, the evaluation function can be made more adaptable to the path planning requirements under different obstacle distribution conditions, ensuring that the planned path is globally optimal.

[0044] 2. The algorithm starts to traverse the nodes near the starting node and optimizes the method of selecting child nodes. Finally, it selects the node with the smallest evaluation function value f(n) as the next path node and adds it to the CLOSE list.

[0045] In order to improve the safety of path planning, the child node selection method of the A* algorithm is optimized. In the process of generating child nodes, the relative position of the child nodes and obstacles is divided into three different categories, which are displayed in the form of a nine-square grid. The center point is the parent node, and the surrounding nodes are arranged clockwise from the upper left corner, namely child nodes 1, 2, 3, 4, 5, 6, 7, and 8. The first category is child nodes 2 and 6, the second category is child nodes 4 and 8, and the third category is child nodes 1, 3, 5, and 7.

[0046] (1) When the obstacle is located at the position of category I, that is, the obstacle is located between the upper and lower child nodes of the parent node, the left and right optional child nodes of the obstacle should be removed, which are child nodes 1 and 3 or child nodes 5 and 7.

[0047] (2) When the obstacle is located at the position of Category II, that is, the obstacle is located between the left and right child nodes of the parent node, the upper and lower optional child nodes of the obstacle should be removed, which are child nodes 1 and 7 or child nodes 3 and 5.

[0048] (3) When the obstacle is located in the position of Category III, that is, the obstacle is not located between the upper and lower child nodes or the left and right child nodes of the parent node, no processing is performed.

[0049] The use of this optimized sub-node selection strategy can significantly reduce the collision risk between the generated path and obstacles, thereby enhancing the overall safety of path planning.

[0050] 3. The algorithm continues to execute until the OPEN list is empty, that is, the predetermined target node is reached, and the global path is output.

[0051] A formula for determining whether a path collides with an obstacle, where line segment CD represents the longitudinal distance from point C to line segment AB, denoted as l; line segment CE represents the vertical distance from point C to line segment AB, denoted as d; let α be the angle between line segment AB and the X-axis, and r be the radius of the circumscribed circle of point C, then the safety distance D satisfies D>r. When D≥d, a collision with an obstacle may occur, so this path cannot be selected; if d>D, then this path can be selected.

[0052] According to the above conditions, the relevant expressions of l, α, d, and r can be obtained as follows:

[0053]

[0054] d=lcosα (7)

[0055]

[0056] 4. Perform bidirectional smoothness optimization on the generated path and output the optimized global path.

[0057] 5. After obtaining the global path, the DWA algorithm is used to plan the local path. The starting node and the target node are consistent with the improved A* algorithm. Moving obstacles and fixed obstacles are set to generate a new trajectory to become the local path.

[0058] DWA algorithm speed sampling:

[0059] (1) Maximum and minimum speed limits. In actual use scenarios, given the limitations of the mobile robot’s hardware performance and the speed limit, it is defined as the maximum and minimum speed limit set of the mobile robot. Then v m It is expressed as:

[0060] v m ={(v,ω)|v∈[v min ,v max ],ω∈[ω min ,ω max ]} (9)

[0061] In the formula, v min Indicates the minimum linear velocity, v max represents the maximum linear velocity, ω min represents the minimum angular velocity, ω max Indicates the maximum angular velocity.

[0062] (2) Motor acceleration and deceleration limits. Since the torque of the motor and other related tools is limited, there are limits on motor acceleration and deceleration. Within the predicted time range, if a combination of maximum and minimum speeds can be generated, the speed set v d It is expressed as:

[0063]

[0064] In the formula, v t represents the linear velocity of the mobile robot at time t, ω t represents the angular velocity of the mobile robot at time t, represents the maximum linear acceleration of the mobile robot, represents the maximum angular acceleration of the mobile robot, and Δt represents the simulation prediction time interval.

[0065] (3) Braking distance constraint. During path planning, the mobile robot must ensure that it does not collide with any obstacle and, when reaching the maximum deceleration, ensure that its speed drops to zero before a collision occurs. When the speed set is restricted by the braking distance, it can be expressed as a speed set constrained by the braking distance. Then the speed set v a It is expressed as:

[0066]

[0067] Where dist(v,ω) represents the shortest distance from the mobile robot to the obstacle.

[0068] DWA algorithm evaluation function:

[0069] First, calculate the three indicators corresponding to each speed group, and then divide them by the sum of the indicators of all speed groups in the time period. The calculation of the evaluation function is as follows:

[0070] G(v,ω)=α·head(v,ω)+β·vel(v,ω)+γ·dist(v,ω) (12)

[0071] In the formula, α represents the weighting coefficient, β represents the weighting coefficient, γ represents the weighting coefficient, head(v,ω) represents the azimuth, vel(v,ω) represents the current velocity value, and dist(v,ω) represents the closest distance to the obstacle.

[0072] head(v,ω) is evaluated by π-Δθ, where Δθ is the deviation angle between the end point posture direction of the simulated trajectory and the destination position. When the forward path is closer to the target point, Δθ becomes smaller, π-Δθ becomes larger, the proportion of head(v,ω) is higher, and the score is higher.

[0073]

[0074] Where N represents the total number of simulated trajectories, i represents the i-th simulated trajectory, Δθ i Represents the endpoint attitude and target azimuth angle of the i-th simulation trajectory.

[0075] vel(v,ω) is the current speed value. The faster the mobile robot is, the higher the proportion of vel(v,ω) and the higher the score.

[0076]

[0077] In the formula, v i Represents the absolute value of the velocity at the end point of the i-th simulation trajectory.

[0078] dist(v,ω) is the shortest distance to the obstacle. When the distance between the mobile robot and the obstacle is closer, the value of d will be lower and the proportion of dist(v,ω) will also decrease, resulting in a lower score.

[0079]

[0080] Where, d iIt represents the shortest distance between the end point of the i-th simulation trajectory and the obstacle, and D0 represents the set constant.

[0081] 6. When the target node of the local path is the same as the global path, the algorithm ends and the final planned path is output.

[0082] The technical solutions adopted by the present invention are summarized as follows:

[0083] In the process of using the traditional A* algorithm for path planning, due to the presence of a large number of obstacles in the environment, the algorithm may generate a large number of unnecessary search nodes, resulting in an increase in the search space and a decrease in efficiency. In order to improve the search efficiency, the present invention first integrates the information of obstacles into the evaluation function, and dynamically adjusts the heuristic function according to the specific distribution of obstacles. Secondly, this optimized sub-node selection strategy is adopted to reduce the collision risk between the generated path and obstacles, and enhance the overall safety of path planning. At the same time, the path is optimized for bidirectional smoothness, unnecessary path nodes are removed, and the judgment criteria of obstacle distance are added to generate the optimal global path. Finally, it is merged with the DWA algorithm, and the nodes of the global path are used as references to generate the optimal local path and realize the dynamic obstacle avoidance function.

[0084] The beneficial effects of the present invention also include:

[0085] 1. By quantifying the obstacle rate in the map information, the distribution of obstacles in the environment can be more accurately reflected, and the heuristic function in the A* algorithm can be dynamically adjusted accordingly, thereby reducing the number of unnecessary search nodes and improving the effectiveness of path planning.

[0086] 2. The present invention adopts an optimized sub-node selection strategy, which can significantly reduce the collision risk between the generated path and obstacles, enhance the overall safety of path planning, and avoid making the generated path obliquely pass through the top of the obstacle and collide with the obstacle, posing a threat to the safety of the robot.

[0087] 3. In view of a series of problems faced in the path planning process, such as multiple turns, non-shortest path and poor smoothness, the present invention implements bidirectional smoothness optimization for path nodes and sets a safety distance for them to prevent collision.

[0088] 4. In order to solve the problem that local path planning easily falls into the local optimal solution, the present invention proposes a path planning idea of ​​hybrid A* algorithm and DWA algorithm.

[0089] An embodiment of the present invention provides a path planning system, including:

[0090] The model building module is used to use a grid map containing a start node and a target node to model the road environment and to build an obstacle rate model for reflecting the distribution of obstacles.

[0091] The algorithm adjustment module is used to reduce the number of search nodes of the heuristic function of the A* algorithm through the obstacle rate model to optimize the planning path. The heuristic function can determine the search node to estimate the estimated cost from the current node to the target node.

[0092] The optimization model building module is used to optimize the adjusted A* algorithm using the child node optimization model; the child node optimization model is to use the adjacent nodes around the parent node as child nodes, so that multiple child nodes and the parent node together form a nine-square grid; when the obstacle is located at the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the corner of the nine-square grid, no processing is done.

[0093] The algorithm fusion module is used to use the optimized A* algorithm to generate a global path between the start node and the target node in the road environment modeling; and to process the bidirectional smoothness of the global path, and to optimize the processed global path through the dynamic window algorithm DWA to form the final planned path.

[0094] An embodiment of the present invention provides a computer device, comprising: a memory and a processor; the memory stores a computer program, and the processor implements the steps of a path planning method when executing the computer program.

[0095] An embodiment of the present invention provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the steps of a path planning method are implemented.

[0096] A specific embodiment is as follows:

[0097] like Figure 1 The overall process diagram of the present invention is shown, which is specifically divided into the following six parts:

[0098] Step 1: The improved algorithm uses a grid map to model the environment, sets the start node and target node on the grid map, and constructs the OPEN and CLOSE lists.

[0099] In step 2, the algorithm starts to traverse the nodes near the starting node and optimizes the method of selecting child nodes. Finally, the node with the smallest evaluation function value f(n) is selected as the next path node and added to the CLOSE list.

[0100] Step 3: The algorithm continues to execute until the OPEN list is empty, that is, the predetermined target node is reached, and the global path is output.

[0101] Step 4: Optimize the bidirectional smoothness of the generated path and output the optimized global path.

[0102] Step 5: After obtaining the global path, the DWA algorithm is used to plan the local path. The starting node and the target node are consistent with the improved A* algorithm. Moving obstacles and fixed obstacles are set to generate a new trajectory to become the local path.

[0103] Step 6: When the target node of the local path is the same as the global path, the algorithm ends and the final planned path is output.

[0104] like Figure 2 The diagram shows the child node selection optimization strategy of the improved A* algorithm of the present invention. When the obstacle is located in the purple area of ​​Class I, that is, the obstacle is located between the upper and lower child nodes of the parent node, the left and right optional child nodes of the obstacle should be removed, which are child nodes 1 and 3 or child nodes 5 and 7. When the obstacle is located in the green area of ​​Class II, that is, the obstacle is located between the left and right child nodes of the parent node, the upper and lower optional child nodes of the obstacle should be removed, which are child nodes 1 and 7 or child nodes 3 and 5. When the obstacle is located in the gray area of ​​Class III, that is, the obstacle is not located between the upper and lower or left and right child nodes of the parent node, no processing is performed.

[0105] like Figure 3 The figure shows the bidirectional path smoothness optimization strategy diagram of the present invention, and the specific optimization process is as follows:

[0106] (1) Remove unnecessary nodes from the path, leaving only the initial node, turning point, and the predetermined target node. The optimized route is S→n7→n8→n9→G.

[0107] (2) Starting from the initial node, sample every fixed distance k between S, n7, n8, n9, and G to generate a series of candidate nodes. Then, connect the adjacent candidate nodes and evaluate whether the connection successfully crosses the obstacle. If it crosses the obstacle, the node is discarded; if not, the vertical distance d from the obstacle next to the connection line to the connection line can be calculated according to formula (7) and compared with the safety distance D. If d>D, then select this node as the path node; if not, select according to the established rules. Figure 3 As shown, the optimized route is S→n 11 →G.

[0108] (3) To ensure the smoothness of the path, we reversely execute the same node selection process from the target node G. Figure 3 As shown, the final optimized path is S→n 12 →G.

[0109] like Figure 4 and 5 The figures show the global planning trajectory of the traditional A* algorithm and the improved A* algorithm. The experimental map is a 30×30 grid map, where black grids represent obstacles and white grids represent passable areas. The improved A* algorithm not only successfully avoids the vertices of obstacles, but also sets a safe distance, making the planned path safer and more reasonable. It also achieves significant results in optimizing the selection strategy of subnodes and improving the smoothness of the path.

[0110] like Figure 4 and 5 The following are the trajectory diagrams of the traditional DWA algorithm and the improved A* and DWA fusion algorithm, where the yellow grid represents moving obstacles and the gray grid represents random unknown obstacles. The improved A* and DWA fusion algorithm realizes the real-time obstacle avoidance function, and the planned path is more reasonable and more in line with actual needs, avoiding the problem that local path planning is prone to fall into the local optimal solution.

[0111] Simulation experiments have proved that this algorithm not only inherits the advantages of the A* algorithm in considering the global path, but also inherits the advantages of the dynamic window algorithm in avoiding unknown obstacles. The planned path is safer and more in line with the actual situation.

[0112] The above-mentioned embodiments only express several implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A path planning method, characterized in that: The following steps are involved: A grid map containing start nodes and target nodes is used to model the road environment, and an obstacle rate model is constructed to reflect the distribution of obstacles. The heuristic function of the A* algorithm is used to reduce the number of search nodes through an obstacle rate model to optimize the planning path. The heuristic function can determine the search nodes to estimate the estimated cost from the current node to the target node; The A* algorithm after optimization and adjustment is optimized using a child node optimization model; the child node optimization model is to use the adjacent nodes around the parent node as child nodes, so that multiple child nodes and the parent node together form a nine-square grid; when the obstacle is located at the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the corner of the nine-square grid, no processing is performed; The optimized A* algorithm is used to generate a global path between the start node and the target node in the road environment modeling; the global path is processed for bidirectional smoothness, and the processed global path is optimized by the dynamic window algorithm DWA to form the final planned path.

2. A path planning method according to claim 1, characterized in that: The obstacle rate model is the ratio of the number of grids where obstacles are located to the total number of grids.

3. A path planning method according to claim 1, characterized in that: The heuristic function of the A* algorithm is used to reduce the number of search nodes through an obstacle rate model, and the specific steps include: Get the total cost f(n) from the starting node to the current node in the A* algorithm, get the actual cost g(n) from the starting node to the current node, get the estimated cost h(n) from the current node to the target node, and get the obstacle rate model P; The estimated cost is the heuristic function of the A* algorithm. The obstacle rate model P is used to reduce the number of search nodes of the heuristic function, so that the weight can be adjusted. The formula is: f(n)=g(n)+(1-lnP)h(n).

4. A path planning method according to claim 1, characterized in that: The method of using the sub-node optimization model to optimize the adjusted A* algorithm specifically comprises the following steps: The sub-node selection method of the A* algorithm after optimization and adjustment through the sub-node optimization model; The parent node is the center of the nine-square grid, and the points in the nine-square grid other than the parent node are child nodes. Starting from the first child node in the upper left corner, all child nodes are numbered in sequence in a clockwise direction; The first type of child nodes are numbered 2 and 6, the second type of child nodes are numbered 4 and 8, and the third type of child nodes are numbered 1, 3, 5, and 7; When the obstacle is located in the first category, that is, the obstacle is located at the upper and lower positions of the parent node, the left and right child nodes of the obstacle should be removed, that is, child nodes 1 and 3, or child nodes 5 and 7; When the obstacle is located in the second category, that is, the obstacle is located at the left and right positions of the parent node, the upper and lower child nodes of the obstacle should be removed, that is, child nodes 1 and 7, or child nodes 3 and 5; When the obstacle is located in the third category, no processing is performed.

5. A path planning method according to claim 1, characterized in that: The global path after the optimization process by the dynamic window algorithm DWA specifically includes the following steps: The dynamic window algorithm is used to plan the local path so that the starting node and the target node are consistent with the starting node and the target node of the optimized A* algorithm, and the moving obstacles and the fixed obstacles are set to generate a new trajectory as the local path.

6. A path planning system, characterized in that: include: A model building module, for modeling a road environment using a grid map containing a start node and a target node, and building an obstacle rate model for reflecting the distribution of obstacles; An algorithm adjustment module, used to reduce the number of search nodes of the heuristic function of the A* algorithm through an obstacle rate model to optimize the planning path, wherein the heuristic function can determine the search node to estimate the estimated cost from the current node to the target node; The optimization model building module is used to optimize the adjusted A* algorithm using the child node optimization model; the child node optimization model is to use the adjacent nodes around the parent node as child nodes, so that multiple child nodes and the parent node together form a nine-square grid; when the obstacle is located at the child node in the upper and lower directions of the parent node, the child nodes in the left and right directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the left and right directions of the parent node, the child nodes in the upper and lower directions of the child node where the obstacle is located are removed; when the obstacle is located at the child node in the corner of the nine-square grid, no processing is performed; The algorithm fusion module is used to use the optimized A* algorithm to generate a global path between the start node and the target node in the road environment modeling; and to process the bidirectional smoothness of the global path, and to optimize the processed global path through the dynamic window algorithm DWA to form the final planned path.

7. A computer device comprising: Memory and processor; The memory stores a computer program, wherein the processor implements a path planning method according to any one of claims 1 to 5 when executing the computer program.

8. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, a path planning method according to any one of claims 1 to 5 is implemented.