Path planning method fusing improved A*algorithm and DWA algorithm

Abstract

The invention discloses a path planning method integrating an improved A * algorithm and a DWA algorithm, which comprises the following steps of: 1, establishing a grid map for a plane area through which a vehicle needs to pass from a starting point to an end point by adopting a grid method, and presetting the positions of the starting point and the end point in the grid map; 2, operating an improved A * algorithm, and performing global path planning from the starting point of the grid map to obtain a global optimal path from the starting point to the terminal point; and step 3, operating an improved DWA algorithm, taking the global optimal path in the step 2 as a reference path, performing local dynamic obstacle avoidance path planning from the starting point to the end point in the step 1, and planning an optimal dynamic obstacle avoidance path for the vehicle in the step 1. According to the method, an efficient and reliable global optimal path is provided for the movement of the intelligent vehicle, the capability of avoiding dynamic obstacles in real time is given to the intelligent vehicle, remarkable benefits in multiple aspects are achieved, the search efficiency is improved, the path quality is improved, and the method is more suitable for the complex environment.

Description

Technical Field

[0001] The invention belongs to the field of intelligent vehicle path planning, and specifically is a path planning method integrating an improved A* algorithm and a DWA algorithm. Background Art

[0002] The A* algorithm is a graph-based path search algorithm that finds the shortest path by evaluating the path cost from the starting point to the end point, combining the actual cost and the heuristic cost (usually the Euclidean distance or the Manhattan distance). The A* algorithm is used in many fields for its efficiency and accuracy, especially in static environments. However, the A* algorithm may encounter efficiency problems when processing large-scale data or complex environments because it needs to consider many unnecessary nodes, resulting in increased computational complexity. In addition, the A* algorithm has poor adaptability in dynamic environments because it is not convenient to update the path in real time to deal with sudden obstacles.

[0003] The DWA algorithm (Dynamic Window Approach) is a local path planning algorithm that takes into account the robot's dynamic constraints and real-time obstacle avoidance requirements. The DWA algorithm achieves dynamic obstacle avoidance by searching for the optimal control input (speed and angular velocity) within a local window. Although the DWA algorithm performs well in local obstacle avoidance, it lacks global path planning capabilities, which may cause the robot to be unable to find the optimal path from the starting point to the end point in a complex environment. In addition, without global path guidance, the DWA algorithm may wander near obstacles and fail to move forward effectively. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a path planning method that integrates the improved A* algorithm and the DWA algorithm in response to the deficiencies of the above-mentioned prior art. The path planning method that integrates the improved A* algorithm and the DWA algorithm provides an efficient and reliable global optimal path for the movement of intelligent vehicles, and gives them the ability to avoid dynamic obstacles in real time, thereby achieving significant benefits in many aspects, improving search efficiency, improving path quality, and being more adaptable to complex environments.

[0005] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0006] A path planning method integrating the improved A* algorithm and the DWA algorithm comprises the following steps:

[0007] Step 1: Using a grid method to create a grid map of the plane area that the vehicle needs to pass through from the starting point to the end point, and pre-setting the starting point and the end point positions in the grid map;

[0008] Step 2: Run the improved A* algorithm to perform global path planning from the starting point of the grid map to obtain a global optimal path from the starting point to the end point;

[0009] Step 3: Run the improved DWA algorithm, take the global optimal path described in step 2 as the reference path, perform local dynamic obstacle avoidance path planning from the starting point to the end point described in step 1, and plan an optimal dynamic obstacle avoidance path for the vehicle described in step 1.

[0010] As a further improved technical solution of the present invention, the step 2 specifically includes:

[0011] Step 2.1, determine the starting node (x1, y1) and the target node (x n ,y n ), where the starting node (x1, y1) is the starting point and the target node (x n ,y n ) is the end point;

[0012] Step 2.2, initialize the open list Open List and the closed list Closed List, where the Open List is used to store the nodes to be expanded, initially only containing the starting node (x1, y1), and the Closed List has been expanded nodes, initially empty;

[0013] Step 2.3, record the current node as (x s ,y s ), 1≤s≤n, if the next successor node of the current node is the target node (x n ,y n ), the path planning is successful, and the target node (x n ,y n ) to the Closed List and directly execute step 2.10; otherwise, start from the current node (x s ,y s ) expansion, determine five optimal search directions;

[0014] Step 2.4: For the current node (x s ,y s ), generate successor nodes (x s+1 ,y s+1 ); For each direction, calculate the successor node (x s+1 ,y s+1 ) and check the positions of these successor nodes (x s+1 ,y s+1 ) is beyond the map boundary or located on an obstacle. If a successor node (x s+1 ,ys+1 ) exceeds the map boundary or is located on an obstacle, the successor node (x s+1 ,y s+1 );

[0015] Step 2.5: If a successor node (x s+1 ,y s+1 ) is not in the Open List, add it to the Open List;

[0016] If the successor node (x s+1 ,y s+1 ) is the current node, and it is moved from the Open List to the Closed List, and the process returns to step 2.3. Otherwise, the process goes to step 2.6.

[0017] Step 2.6, calculate the distance from the starting node (x1, y1) to each successor node (x s+1 ,y s+1 )’s cost g(s+1):

[0018] g(s+1)=g(s)+cost(s);

[0019] Where cost(s) is the distance from the current node (x s ,y s ) to the successor node (x s+1 ,y s+1 ) cost, if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), the cost(s) is 1 if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), cost(s) is g(1)=0;

[0020] Step 2.7, calculate the value used to estimate the value from the successor node (x s+1 ,y s+1 ) to the target node (x n ,y n ) is the heuristic function h(s+1):

[0021] h(s+1)=sqrt((x s+1 -x n ) 2 +(y s+1 -y n) 2 );

[0022] Among them, sqrt is to find the square root, (x s+1 ,y s+1 ) is the coordinate of the successor node, (x n ,y n ) are the coordinates of the target node;

[0023] Step 2.8. Calculate the total cost f(s+1) of the successor node based on the heuristic functions h(s+1) and g(s+1):

[0024] f(s+1)=g(s+1)+(1-log(P))*h(s+1);

[0025] Among them, P represents the obstacle rate between the starting node and the target node;

[0026] Step 2.9, select the successor node with the smallest total cost f(s+1) from the Open List and update it as the current node, and move all the successor nodes processed in step 2.8 from the Open List to the Closed List; return to execute step 2.3;

[0027] Step 2.10, start path backtracking, starting from the target node (x n ,y n ) to the starting node (x1, y1), and then trace back the path through the parent node pointer. That is, all points corresponding to the minimum total cost f(s+1) will be recorded in an array separately, and trace back to the previous node one by one until the starting node (x1, y1) is reached, and then the target node (x n ,y n ), each node in the array and the starting node (x1, y1) to form the final path;

[0028] Step 2.11, optimize the final path, delete the redundant nodes in the middle, reduce the turning points, increase the smoothness of the path, and output the path from the starting node (x1, y1) to the target node (x n ,y n )’s optimal path.

[0029] As a further improved technical solution of the present invention, in step 2.3, from the current node (x s ,y s ) expansion, and determine the five optimal search directions, specifically:

[0030] Calculate the current node (x s ,y s ) and the target node (x n ,y n) and the vertical direction, and determine the five optimal search directions based on the range of the angle.

[0031] As a further improved technical solution of the present invention, the expression of the improved DWA algorithm in step 3 is:

[0032] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·vel(v,ω));

[0033] Wherein, v is the linear velocity of the moving vehicle, ω is the angular velocity of the moving vehicle, heading(v,ω) is the azimuth evaluation factor, which is used to evaluate the angle θ between the position direction and the target point when the vehicle moves at the current linear velocity v and angular velocity ω; dist(v,ω) is the obstacle distance evaluation factor, which is used to evaluate the distance between the end of the trajectory and the obstacle when the vehicle moves at the current linear velocity v and angular velocity ω; vel(v,ω) is the current velocity evaluation factor, which is the absolute value of the linear velocity v, σ is the normalization factor of the function, and α, β, and γ are the weight parameters of the three evaluation factors in the trajectory evaluation function G(v,ω) of the improved DWA algorithm.

[0034] As a further improved technical solution of the present invention, the step 3 is specifically:

[0035] Step 3.1: Use the improved DWA algorithm to perform local path planning. First, calculate the vehicle's running trajectory within the simulation cycle. Then, the vehicle's position expression at time k in the world coordinate system is:

[0036]

[0037] Wherein, k and k-1 represent the time nodes of the vehicle in the world coordinate system, x(k) and y(k) represent the coordinate positions of the vehicle at the kth moment in the world coordinate system, x(k-1) and y(k-1) represent the coordinate positions of the vehicle at the k-1th moment in the world coordinate system, v and ω represent the linear velocity and angular velocity of the vehicle, θ(k) and θ(k-1) represent the attitude angles of the vehicle at the kth and k-1th moments, respectively, and the time interval is t;

[0038] Step 3.2: Sample the velocity vector of the vehicle's forward velocity space, and obtain the most suitable sets of linear velocities v and angular velocities ω for the vehicle during movement under the constraints of velocity boundary constraints, acceleration boundary constraints, and obstacle avoidance velocity constraints, i.e., velocity vector Vr, which is expressed as follows:

[0039] Vr=Vs∩Va∩Vd;

[0040] Among them, Vs represents the speed boundary constraint when the vehicle is moving, Va represents the speed constraint when the vehicle considers acceleration during movement, and Vd represents the obstacle avoidance speed constraint when the vehicle is moving;

[0041] Step 3.3: Each set of linear velocity v and angular velocity ω in the velocity vector space Vr described in step 3.2 corresponds to a motion trajectory. Substitute multiple trajectories into the improved DWA algorithm trajectory evaluation function G(v,ω), and then select the optimal trajectory based on the obtained values. Finally, drive the vehicle to move with its corresponding linear velocity v and angular velocity ω, that is, obtain the local optimal dynamic obstacle avoidance path planned with reference to the global optimal path in the current operation cycle.

[0042] The beneficial effects of the present invention are:

[0043] The present invention integrates the path planning method of the improved A* algorithm and the DWA algorithm, provides an efficient and reliable global optimal path for the movement of intelligent vehicles, and gives them the ability to avoid dynamic obstacles in real time, achieving significant benefits in many aspects.

[0044] First, in terms of path planning efficiency, the improved A* algorithm optimizes the search direction and accurately discards three unnecessary search directions based on the relative position of the target point and the current point, retaining only the five optimal search directions. This strategy significantly reduces the number of neighborhood nodes when the algorithm expands each node, effectively reducing the computational complexity and search space. Compared with the traditional A algorithm, this optimization makes the path planning process faster and can generate the global optimal path from the starting point to the end point for the intelligent vehicle more quickly, significantly improving the planning efficiency and making its application in complex environments more efficient.

[0045] Secondly, this method also achieved excellent performance in terms of path smoothness. By removing redundant nodes in the middle and reducing turns, the path generated by the improved A* algorithm is smoother and more fluid. Such a path not only conforms to the driving characteristics of the vehicle, reduces energy consumption and wear during driving, but also improves passenger comfort. A smooth path also helps the vehicle maintain stable acceleration and speed during driving, reducing safety hazards caused by frequent turns and speed changes.

[0046] The improved DWA algorithm plays a key role in dynamic obstacle avoidance capabilities. It uses the global optimal path as a reference, combined with the azimuth, obstacle distance and current speed evaluation factors, to evaluate the vehicle's motion trajectory in real time. During the simulation cycle, the DWA algorithm can quickly calculate the vehicle's trajectory, and under the conditions of speed boundary constraints, acceleration boundary constraints, obstacle avoidance speed constraints, etc., select the optimal linear speed and angular velocity to drive the vehicle along the local optimal dynamic obstacle avoidance path. This enables intelligent vehicles to effectively respond to sudden dynamic obstacles and ensure safety and flexibility during driving.

[0047] In addition, the method of the present invention has good adaptability. It is not only suitable for global path planning in static environments, but also can adjust the path in real time in dynamic environments to cope with various emergencies. This path planning strategy that combines global and local aspects enables intelligent vehicles to achieve efficient and safe driving in complex and changeable road environments, such as urban streets, parking lots, industrial parks and other scenarios.

[0048] In summary, the path planning method implemented by the present invention has achieved remarkable beneficial effects in improving planning efficiency, optimizing path smoothness, enhancing dynamic obstacle avoidance capability, and improving adaptability, providing strong technical support for the autonomous navigation and safe driving of intelligent vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Schematic diagram of the search direction of the traditional A* algorithm.

[0050] Figure 2 This is a comparison chart of the retrieval range of the improved A* algorithm and the traditional A* algorithm.

[0051] Figure 2 (a) is the path planning graph based on the traditional A* algorithm.

[0052] Figure 2 (b) is the path planning graph based on the improved A* algorithm.

[0053] Figure 3 This is a comparison chart of the global planning effects between the improved A* algorithm and the traditional A* algorithm.

[0054] Figure 4 Graph of the static global planning and local dynamic obstacle avoidance process. DETAILED DESCRIPTION

[0055] The specific embodiments of the present invention are further described below according to the accompanying drawings:

[0056] A path planning method integrating the improved A* algorithm and the DWA algorithm comprises the following steps:

[0057] Step 1: Use the grid method to create a grid map of the plane area that the vehicle needs to pass through from the starting point to the end point, and pre-set the starting point and the end point in the grid map, such as Figure 2 The grid map shown, black indicates obstacles;

[0058] Step 2: Run the improved A* algorithm to perform global path planning from the starting point of the grid map to obtain a global optimal path from the starting point to the end point, such as Figure 2 As shown in (b);

[0059] Step 3: Run the improved DWA algorithm, take the global optimal path described in step 2 as the reference path, perform local dynamic obstacle avoidance path planning from the starting point to the end point described in step 1, and plan an optimal dynamic obstacle avoidance path for the vehicle described in step 1.

[0060] Wherein, the step 2 specifically includes:

[0061] Step 2.1, determine the starting node (x1, y1) and the target node (x n ,y n ), where the starting node (x1, y1) is the starting point and the target node (x n ,y n ) is the end point.

[0062] Step 2.2, initialize the open list Open List and the closed list Closed List, where the Open List is used to store the nodes to be expanded, initially containing only the starting node (x1, y1), and the Closed List contains the nodes that have been expanded, and is initially empty.

[0063] Step 2.3, record the current node as (x s ,y s ), 1≤s≤n. If the next successor node of the current node is the target node (x n ,y n ), the path planning is successful, and the target node (x n ,y n ) to the Closed List and proceed directly to step 2.10;

[0064] Otherwise, from the current node (x s ,y s ) expansion, and determine the five optimal search directions, specifically:

[0065] Calculate the current node (x s ,y s ) and the target node (x n ,y n ) and the vertical direction, and determine the five optimal search directions according to the range of angles. The traditional A* algorithm will expand the 8-neighborhood grid of the current node when performing path planning, such as Figure 1As shown, the dark grid is the current point, and n1 to n8 are the eight directions in which the current grid can move. The restriction of the target point orientation will cause unnecessary grids in the 8-neighborhood grid, resulting in a waste of computing time and storage space. In order to further improve the search efficiency, this embodiment proposes an optimized search point selection strategy, which discards three search directions and retains five search directions according to the relative position of the target node and the current node. The angle between the line connecting the target node and the current node and the n1 direction is set to α, and the five retained search directions are determined according to the angle α, and three directions are discarded. The corresponding relationship between the angle α and the three discarded directions is shown in Table 1.

[0066] Table 1. Correspondence between angle α and three discard directions:

[0067] α 5 directions of retention Three directions of abandonment [22.5°,67.5°) <![CDATA[n1、n2、n3、n4、n8]]> <![CDATA[n5、n6、n7]]> [67.5°,112.5°) <![CDATA[n1、n2、n3、n4、n5]]> <![CDATA[n6、n7、n8]]> [112.5°,157.5°) <![CDATA[n2、n3、n4、n5、n6]]> <![CDATA[n1、n7、n8]]> [157.5°,202.5°) <![CDATA[n3、n4、n5、n6、n7]]> <![CDATA[n1、n2、n8]]> [202.5°,247.5°) <![CDATA[n4、n5、n6、n7、n8]]> <![CDATA[n1、n2、n3]]> [247.5°,292.5°) <![CDATA[n1、n5、n6、n7、n8]]> <![CDATA[n2、n3、n4]]> [292.5°,337.5°) <![CDATA[n1、n2、n6、n7、n8]]> <![CDATA[n3、n4、n5]]> [337.5°,360°)∪[0°,22.5°) <![CDATA[n1、n2、n3、n7、n8]]> <![CDATA[n4、n5、n6]]>

[0068] Step 2.4: For the current node (x s ,y s ), generate successor nodes (x s+1 ,y s+1 ); For each direction, calculate the successor node (x s+1 ,y s+1 ) and check the positions of these successor nodes (x s+1 ,y s+1 ) is beyond the map boundary or located on an obstacle. If a successor node (x s+1 ,y s+1 ) exceeds the map boundary or is located on an obstacle, the successor node (x s+1 ,y s+1 );

[0069] Step 2.5: If a successor node (x s+1 ,y s+1 ) is not in the Open List, add it to the Open List;

[0070] If the successor node (x s+1 ,y s+1 ) is the current node, and it is moved from the Open List to the Closed List, and the process returns to step 2.3. Otherwise, the process goes to step 2.6.

[0071] Step 2.6: If a successor node (x s+1 ,y s+1 ) is already in the Closed List, skip steps 2.6-2.8;

[0072] Otherwise, calculate the distance from the starting node (x1, y1) to each successor node (x s+1 ,y s+1 )’s cost g(s+1):

[0073] g(s+1)=g(s)+cost(s);

[0074] Where cost(s) is the distance from the current node (x s ,y s ) to the successor node (x s+1 ,y s+1 ) cost, if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), the cost(s) is 1 if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), cost(s) is g(1)=0;

[0075] Step 2.7, calculate the value used to estimate the value from the successor node (x s+1 ,y s+1 ) to the target node (x n ,y n ) is the heuristic function h(s+1):

[0076] h(s+1)=sqrt((x s+1 -x n ) 2 +(y s+1 -y n ) 2 );

[0077] Among them, sqrt is to find the square root, (x s+1 ,y s+1 ) is the coordinate of the successor node, (x n ,y n ) is the coordinate of the target node; the heuristic function is used to estimate the cost from the successor node to the target node; the Euclidean distance is applicable to the case where movement in any direction is possible, which is consistent with the vehicle path planning research conducted by the present invention, so the present invention selects the Euclidean distance as the heuristic function;

[0078] Step 2.8. Calculate the total cost f(s+1) of the successor node based on the heuristic functions h(s+1) and g(s+1):

[0079] f(s+1)=g(s+1)+(1-log(P))*h(s+1);

[0080] Among them, P represents the obstacle rate between the starting node and the target node;

[0081] Step 2.9, select the successor node with the smallest total cost f(s+1) from the Open List and update it as the current node, and move all retrieved points (including the starting node (x1, y1), the successor node after the operation in step 2.8, etc.) from the Open List to the Closed List; return to execute step 2.3; note that if the starting node (x1, y1) has been moved from the Open List to the Closed List, ignore the operation of moving the starting node (x1, y1) from the Open List to the Closed List;

[0082] Step 2.10, start path backtracking, starting from the target node (x n ,y n ) starts to backtrack to the starting node (x1, y1), and traces back the path through the parent node pointer, that is, all points corresponding to the minimum total cost f(s+1) will be recorded separately into an array (if the successor node (x s+1 ,y s+1 ) has only one successor node, then the successor node (x s+1 ,y s+1 ) is recorded in the array), and the previous node is traced back one by one until the starting node (x1, y1) is reached, and then the target node (x n ,y n ), each node in the array and the starting node (x1, y1) to form the final path;

[0083] Step 2.11, optimize the final path, delete the redundant nodes in the middle, reduce the turning points, increase the smoothness of the path, and output the path from the starting node (x1, y1) to the target node (x n ,y n )’s optimal path.

[0084] The expression of the improved DWA algorithm in step 3 is:

[0085] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·vel(v,ω));

[0086] Wherein, v is the linear velocity of the moving vehicle; ω is the angular velocity of the moving vehicle; heading(v,ω) is the azimuth evaluation factor, which is used to evaluate the angle θ between the position direction and the target point when the vehicle moves at the current linear velocity v and angular velocity ω; dist(v,ω) is the obstacle distance evaluation factor, which is used to evaluate the distance between the end of the trajectory and the obstacle when the vehicle moves at the current linear velocity v and angular velocity ω; vel(v,ω) is the current speed evaluation factor, which is the absolute value of the linear velocity v, ensuring that the vehicle selects the optimal current linear velocity v and angular velocity ω for movement; σ is the normalization factor of the function; α, β, and γ are the weight parameters of the three evaluation factors in the trajectory evaluation function G(v,ω) of the improved DWA algorithm.

[0087] The step 3 is specifically as follows:

[0088] Step 3.1: Use the improved DWA algorithm to perform local path planning. First, calculate the vehicle's running trajectory within the simulation cycle. Then, the vehicle's position expression at time k in the world coordinate system is:

[0089]

[0090] Wherein, k and k-1 represent the time nodes of the vehicle in the world coordinate system, x(k) and y(k) represent the coordinate positions of the vehicle at the kth moment in the world coordinate system, x(k-1) and y(k-1) represent the coordinate positions of the vehicle at the k-1th moment in the world coordinate system, v and ω represent the linear velocity and angular velocity of the vehicle, θ(k) and θ(k-1) represent the attitude angles of the vehicle at the kth and k-1th moments, respectively, and the time interval is t;

[0091] Step 3.2: Sample the velocity vector of the vehicle's forward velocity space, and obtain the most suitable sets of linear velocities v and angular velocities ω for the vehicle during movement under the constraints of velocity boundary constraints, acceleration boundary constraints, and obstacle avoidance velocity constraints, i.e., velocity vector Vr, which is expressed as follows:

[0092] Vr=Vs∩Va∩Vd;

[0093] Among them, Vs represents the speed boundary constraint when the vehicle is moving, Va represents the speed constraint when the vehicle considers acceleration during movement, and Vd represents the obstacle avoidance speed constraint when the vehicle is moving;

[0094] Step 3.3: Each set of linear velocity v and angular velocity ω in the velocity vector space Vr described in step 3.2 corresponds to a motion trajectory. Substitute multiple trajectories into the improved DWA algorithm trajectory evaluation function G(v,ω), and then select the optimal trajectory based on the obtained values. Finally, drive the vehicle to move with its corresponding linear velocity v and angular velocity ω, that is, obtain the local optimal dynamic obstacle avoidance path planned with reference to the global optimal path in the current operation cycle.

[0095] The present invention combines the improved A* algorithm with the dynamic window method to ensure the global path is optimal while having the ability to avoid obstacles randomly. The specific steps are as follows: first, the improved A* algorithm is used for global path planning to obtain the globally optimal node sequence; then, the dynamic window method is used for local path planning between every two adjacent nodes.

[0096] Figure 2 The comparison between the improved A algorithm and the traditional A algorithm in terms of search range is shown. By optimizing the search direction, the improved A* algorithm significantly reduces unnecessary node retrieval, thereby improving the efficiency and speed of path planning.

[0097] Traditional A* algorithm: When each node is expanded, the traditional A* algorithm considers 8 neighboring nodes (as shown in the dark grid in the figure). Although this method is comprehensive, it will lead to a lot of computational and storage overhead in complex environments, especially under the constraints of the target point orientation, many neighboring nodes are unnecessary.

[0098] Improved A* algorithm: The improved A* algorithm proposed in this paper discards 3 unnecessary search directions and retains only 5 optimal search directions based on the relative position of the target point and the current point. This optimization strategy enables the algorithm to focus on the possible optimal path more efficiently, reducing the amount of calculation and storage requirements while maintaining the accuracy of path planning.

[0099] pass Figure 2 By comparing the results, we can see the advantage of the improved A* algorithm in retrieval range, which not only improves the operation efficiency of the algorithm, but also provides strong support for the rapid path planning of intelligent vehicles in complex environments.

[0100] Figure 3 The comparison between the improved A algorithm and the traditional A algorithm in global path planning is shown. By optimizing the search strategy and heuristic function, the improved A* algorithm can generate a more efficient and reasonable global optimal path.

[0101] Traditional A* algorithm path: The path generated by the traditional A* algorithm may have many turns and unnecessary nodes, resulting in an unsmooth path and a long path length. This is because in complex grid maps, the traditional algorithm may be limited by the layout of obstacles and the search direction, resulting in less than ideal path planning.

[0102] Improved A* algorithm path: The improved A* algorithm proposed in this paper generates a smoother and shorter path. By optimizing the search direction and heuristic function, the algorithm can more accurately evaluate the priority of nodes and thus select better path nodes. In addition, the path optimization step further removes redundant nodes in the middle, reduces turns, and improves the smoothness and drivability of the path.

[0103] pass Figure 3 From the comparison, we can see the advantages of the improved A* algorithm in global path planning. It can provide a more efficient, reasonable and smooth path for intelligent vehicles, which helps to improve the driving efficiency and safety of vehicles.

[0104] Figure 4 It shows the combination of static global planning and local dynamic obstacle avoidance process, reflecting the overall workflow of the path planning method of the present invention that integrates the improved A* algorithm and DWA algorithm. Among them, the gray one is a static obstacle, which is placed after the static global planning. The overlap of the static obstacle with the dotted line means that the static obstacle blocks the original road, and then the vehicle takes the solid line, which means that the vehicle successfully avoids the obstacle through the local dynamic obstacle avoidance method. The yellow one is a dynamic obstacle, and there is a short dotted line below it, which is its starting point to the end point. Figure 4 The dynamic obstacle position means that the vehicle has reached the end point. Green indicates the speed and direction that can be selected.

[0105] Static global planning: First, the improved A* algorithm is used for static global path planning to generate a global optimal path from the starting point to the end point (as shown by the dotted line in the figure). This path takes into account the static obstacle layout of the entire environment and ensures the optimality and feasibility of the global path. Global path planning provides reference and guidance for subsequent local dynamic obstacle avoidance.

[0106] Local dynamic obstacle avoidance: Based on the global path, the improved DWA algorithm is used for local dynamic obstacle avoidance path planning. Between every two adjacent global path nodes, the DWA algorithm calculates the optimal linear velocity and angular velocity based on real-time dynamic obstacle information and the robot's dynamic constraints, thereby generating a local dynamic obstacle avoidance path (as shown by the solid line in the figure). The local dynamic obstacle avoidance process can effectively deal with sudden dynamic obstacles and ensure the safety and flexibility of the robot during driving.

[0107] Through this diagram, we can clearly understand how the present invention combines static global planning with local dynamic obstacle avoidance to achieve efficient and safe path planning for intelligent vehicles in complex dynamic environments. The global optimal path provides the robot with a clear driving direction and goal, while the local dynamic obstacle avoidance gives the robot the ability to respond to environmental changes in real time. The two work together to ensure the comprehensiveness and reliability of path planning.

[0108] The present invention integrates the path planning method of the improved A* algorithm and the DWA algorithm, provides an efficient and reliable global optimal path for the movement of intelligent vehicles, and gives them the ability to avoid dynamic obstacles in real time, achieving significant benefits in many aspects.

[0109] First, in terms of path planning efficiency, the improved A algorithm optimizes the search direction and accurately discards three unnecessary search directions based on the relative position of the target point and the current point, retaining only the five optimal search directions. This strategy significantly reduces the number of neighborhood nodes when the algorithm expands each node, effectively reducing the computational complexity and search space. Compared with the traditional A algorithm, this optimization makes the path planning process faster and can generate the global optimal path from the starting point to the end point for the intelligent vehicle more quickly, significantly improving the planning efficiency and making its application in complex environments more efficient.

[0110] Secondly, this method also achieved excellent performance in terms of path smoothness. By removing redundant nodes in the middle and reducing turns, the path generated by the improved A* algorithm is smoother and more fluid. Such a path not only conforms to the driving characteristics of the vehicle, reduces energy consumption and wear during driving, but also improves passenger comfort. A smooth path also helps the vehicle maintain stable acceleration and speed during driving, reducing safety hazards caused by frequent turns and speed changes.

[0111] The improved DWA algorithm plays a key role in dynamic obstacle avoidance capabilities. It uses the global optimal path as a reference, combined with the azimuth, obstacle distance and current speed evaluation factors, to evaluate the vehicle's motion trajectory in real time. During the simulation cycle, the DWA algorithm can quickly calculate the vehicle's trajectory, and under the conditions of speed boundary constraints, acceleration boundary constraints, obstacle avoidance speed constraints, etc., select the optimal linear speed and angular velocity to drive the vehicle along the local optimal dynamic obstacle avoidance path. This enables intelligent vehicles to effectively respond to sudden dynamic obstacles and ensure safety and flexibility during driving.

[0112] In addition, the method of the present invention has good adaptability. It is not only suitable for global path planning in static environments, but also can adjust the path in real time in dynamic environments to cope with various emergencies. This path planning strategy that combines global and local aspects enables intelligent vehicles to achieve efficient and safe driving in complex and changeable road environments, such as urban streets, parking lots, industrial parks and other scenarios.

[0113] In summary, the path planning method implemented by the present invention has achieved remarkable beneficial effects in improving planning efficiency, optimizing path smoothness, enhancing dynamic obstacle avoidance capability, and improving adaptability, providing strong technical support for the autonomous navigation and safe driving of intelligent vehicles.

[0114] The protection scope of the present invention includes but is not limited to the above embodiments. The protection scope of the present invention shall be based on the claims. Any replacement, deformation, and improvement of the technology that can be easily thought of by technicians in this field shall fall within the protection scope of the present invention.

Claims

1. A path planning method integrating improved A* algorithm and DWA algorithm, characterized in that: The following steps are involved: Step 1: Using a grid method to create a grid map of the plane area that the vehicle needs to pass through from the starting point to the end point, and pre-setting the starting point and the end point positions in the grid map; Step 2: Run the improved A* algorithm to perform global path planning from the starting point of the grid map to obtain a global optimal path from the starting point to the end point; Step 3: Run the improved DWA algorithm, take the global optimal path described in step 2 as the reference path, perform local dynamic obstacle avoidance path planning from the starting point to the end point described in step 1, and plan an optimal dynamic obstacle avoidance path for the vehicle described in step 1.

2. The path planning method integrating the improved A* algorithm and the DWA algorithm according to claim 1 is characterized in that: The step 2 specifically includes: Step 2.1, determine the starting node (x1, y1) and the target node (x n ,y n ), where the starting node (x1, y1) is the starting point and the target node (x n ,y n ) is the end point; Step 2.2, initialize the open list Open List and the closed list Closed List, where the Open List is used to store the nodes to be expanded, initially only containing the starting node (x1, y1), and the Closed List has been expanded nodes, initially empty; Step 2.3, record the current node as (x s ,y s ), 1≤s≤n, if the next successor node of the current node is the target node (x n ,y n ), the path planning is successful, and the target node (x n ,y n ) to the Closed List and directly execute step 2.10; otherwise, start from the current node (x s ,y s ) expansion, determine five optimal search directions; Step 2.4: For the current node (x s ,y s ), generate successor nodes (x s+1 ,y s+1 ); For each direction, calculate the successor node (x s+1 ,y s+1 ) and check the positions of these successor nodes (x s+1 ,y s+1 ) is beyond the map boundary or located on an obstacle. If a successor node (x s+1 ,y s+1 ) exceeds the map boundary or is located on an obstacle, the successor node (x s+1 ,y s+1 ); Step 2.5: If a successor node (x s+1 ,y s+1 ) is not in the Open List, add it to the Open List; If the successor node (x s+1 ,y s+1 ) If there is only one node, update it as the current node and move it from OpenList to Closed List, and return to step 2.

3. Otherwise, execute step 2.6; Step 2.6, calculate the distance from the starting node (x1, y1) to each successor node (x s+1 ,y s+1 )’s cost g(s+1): g(s+1)=g(s)+cost(s); Where cost(s) is the distance from the current node (x s ,y s ) to the successor node (x s+1 ,y s+1 ) cost, if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), the cost(s) is 1 if the successor node (x s+1 ,y s+1 ) is located at the current node (x s ,y s ), cost(s) is g(1)=0; Step 2.7, calculate the value used to estimate the value from the successor node (x s+1 ,y s+1 ) to the target node (x n ,y n ) is the heuristic function h(s+1): h(s+1)=sqrt((x s+1 -x n ) 2 +(y s+1 -y n ) 2 ); Among them, sqrt is to find the square root, (x s+1 ,y s+1 ) is the coordinate of the successor node, (x n ,y n ) are the coordinates of the target node; Step 2.

8. Calculate the total cost f(s+1) of the successor node based on the heuristic functions h(s+1) and g(s+1): f(s+1)=g(s+1)+(1-log(P))*h(s+1); Among them, P represents the obstacle rate between the starting node and the target node; Step 2.9, select the successor node with the smallest total cost f(s+1) from the Open List and update it as the current node, and move all the successor nodes processed in step 2.8 from the Open List to the Closed List; return to execute step 2.3; Step 2.10, start path backtracking, starting from the target node (x n ,y n ) to the starting node (x1, y1), and then trace back the path through the parent node pointer. That is, all points corresponding to the minimum total cost f(s+1) will be recorded in an array separately, and trace back to the previous node one by one until the starting node (x1, y1) is reached, and then the target node (x n ,y n ), each node in the array and the starting node (x1, y1) to form the final path; Step 2.11, optimize the final path, delete the redundant nodes in the middle, reduce the turning points, increase the smoothness of the path, and output the path from the starting node (x1, y1) to the target node (x n ,y n )’s optimal path.

3. The path planning method integrating the improved A* algorithm and the DWA algorithm according to claim 2 is characterized in that: In step 2.3, from the current node (x s ,y s ) expansion, and determine the five optimal search directions, specifically: Calculate the current node (x s ,y s ) and the target node (x n ,y n ) and the vertical direction, and determine the five optimal search directions based on the range of the angle.

4. The path planning method integrating the improved A* algorithm and the DWA algorithm according to claim 1 is characterized in that: The expression of the improved DWA algorithm in step 3 is: G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·vel(v,ω)); Wherein, v is the linear velocity of the moving vehicle, ω is the angular velocity of the moving vehicle, heading(v,ω) is the azimuth evaluation factor, which is used to evaluate the angle θ between the position direction and the target point when the vehicle moves at the current linear velocity v and angular velocity ω; dist(v,ω) is the obstacle distance evaluation factor, which is used to evaluate the distance between the end of the trajectory and the obstacle when the vehicle moves at the current linear velocity v and angular velocity ω; vel(v,ω) is the current velocity evaluation factor, which is the absolute value of the linear velocity v, σ is the normalization factor of the function, and α, β, and γ are the weight parameters of the three evaluation factors in the trajectory evaluation function G(v,ω) of the improved DWA algorithm.

5. The path planning method integrating the improved A* algorithm and the DWA algorithm according to claim 1 is characterized in that: The step 3 is specifically as follows: Step 3.1: Use the improved DWA algorithm to perform local path planning. First, calculate the vehicle's running trajectory within the simulation cycle. Then, the vehicle's position expression at time k in the world coordinate system is: Wherein, k and k-1 represent the time nodes of the vehicle in the world coordinate system, x(k) and y(k) represent the coordinate positions of the vehicle at the kth moment in the world coordinate system, x(k-1) and y(k-1) represent the coordinate positions of the vehicle at the k-1th moment in the world coordinate system, v and ω represent the linear velocity and angular velocity of the vehicle, θ(k) and θ(k-1) represent the attitude angles of the vehicle at the kth and k-1th moments, respectively, and the time interval is t; Step 3.2: Sample the velocity vector of the vehicle's forward velocity space, and obtain the most suitable sets of linear velocities v and angular velocities ω for the vehicle during movement under the constraints of velocity boundary constraints, acceleration boundary constraints, and obstacle avoidance velocity constraints, i.e., velocity vector Vr, which is expressed as follows: Vr=Vs∩Va∩Vd; Among them, Vs represents the speed boundary constraint when the vehicle is moving, Va represents the speed constraint when the vehicle considers acceleration during movement, and Vd represents the obstacle avoidance speed constraint when the vehicle is moving; Step 3.3: Each set of linear velocity v and angular velocity ω in the velocity vector space Vr described in step 3.2 corresponds to a motion trajectory. Substitute multiple trajectories into the improved DWA algorithm trajectory evaluation function G(v,ω), and then select the optimal trajectory based on the obtained values. Finally, drive the vehicle to move with its corresponding linear velocity v and angular velocity ω, that is, obtain the local optimal dynamic obstacle avoidance path planned with reference to the global optimal path in the current operation cycle.

Images