An Autonomous Driving Obstacle Avoidance Trajectory Planning Method Based on Improved A* Algorithm
By improving the A* algorithm and combining it with multi-sensor technology and distance weight optimization, the problem of rapid obstacle avoidance for autonomous vehicles in complex environments was solved, and efficient and real-time trajectory planning was achieved.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- NANJING UNIV OF AERONAUTICS & ASTRONAUTICS
- Filing Date
- 2023-06-07
- Publication Date
- 2026-05-26
Smart Images
Figure CN116700256B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of vehicle global planning and local planning technology, and particularly relates to an autonomous driving obstacle avoidance trajectory planning method based on an improved A* algorithm. Background Technology
[0002] With the rapid development of artificial intelligence and autonomous driving, multi-sensor fusion technology, navigation and positioning technology, motion control and decision-making technology, and multi-target cooperative technology are widely used in various fields of autonomous driving. Among these, trajectory planning is a crucial part to meet the needs of unmanned vehicle navigation tasks. Accurate, safe, and rapid obstacle avoidance during autonomous vehicle operation not only improves driving safety but also enhances travel efficiency. Its research is of great significance for improving the intelligent driving level of vehicles and increasing road capacity.
[0003] Automobiles have become indispensable tools in people's daily lives and production. In the closed-loop driving system composed of people, vehicles, and the environment, differences in driver skills, physiological state, and physical strength are the weakest link in the system and a major cause of frequent traffic accidents. Autonomous vehicles are considered capable of solving problems such as traffic safety, congestion, and vehicle scheduling because they eliminate some or all human factors. Autonomous driving technology has become the future direction of automotive development. The core technologies of autonomous driving mainly include four modules: perception, decision-making, planning, and control. Path planning, after obtaining a model of the vehicle's surrounding environment, determines the optimal driving path while satisfying obstacle avoidance and feasible path requirements. Current main methods of path planning include:
[0004] (1) The path planning algorithm based on artificial potential field is based on the idea of concretizing the influence of the target and obstacles on the robot's motion into an artificial potential field. The potential energy is low at the target and high at the obstacle. The artificial potential field method is easy to calculate and the resulting path is safe and smooth. However, the complex potential field environment may produce local minima outside the target point, which may prevent the target from being reached.
[0005] (2) Path planning algorithms based on graph search: Graph search methods rely on known environmental maps and obstacle information in the maps to construct feasible paths from the starting point to the destination. However, as the resolution of raster maps increases, the time cost of such algorithms will increase dramatically, making them unsuitable for complex environments.
[0006] (3) Path planning algorithms based on random sampling, the main method of which is the Rapid Expanding Random Tree (RRT). The RRT algorithm is a relatively efficient algorithm that can handle path planning problems with nonholonomic constraints well, and has great advantages in many aspects. However, the RRT algorithm cannot guarantee that the feasible path obtained is relatively optimized.
[0007] However, most existing algorithms are only applicable to specific situations, and there is no standard and fast planning method for vehicle trajectory planning in complex environments. Summary of the Invention
[0008] Purpose of the invention: The technical problem to be solved by the present invention is to address the shortcomings of the existing technology by providing an autonomous driving obstacle avoidance trajectory planning method based on an improved A* algorithm. This method takes into account both global optimality and real-time performance, improves the efficiency of the algorithm and its adaptability to the environment, thereby realizing online planning of autonomous driving reference paths and dynamic global obstacle avoidance, and completing the task of efficient optimal path search.
[0009] The present invention provides an autonomous driving obstacle avoidance trajectory planning method based on an improved A* algorithm, which specifically includes the following steps:
[0010] Step 1: Obtain real-time information about the vehicle itself and surrounding obstacles using multi-sensor fusion technology;
[0011] Step 2: Create a grid map of the surrounding environment and obstacles to simplify the vehicle model;
[0012] Step 3: Based on the grid map and simplified vehicle model generated in real time in Step 2, establish the cost function F. ij (k) and calculate the cost of each path using the cost function.
[0013] Step 4: Determine the start point, end point, and obstacle locations in the m*n grid map generated in Step 2.
[0014] Step 5: Set the OPEN, CLOSE, and ROUTE sets, and initialize them to empty sets.
[0015] Step 6: Place the starting point in the ROUTE set and place all obstacle point sets in the CLOSE set.
[0016] Step 7: Using the current node in ROUTE as the base point, search for all its expandable node sets and place them in the OPEN set. At the same time, remove all sets in CLOSE from OPEN; and also put the current node into CLOSE.
[0017] Step 8: Determine if the OPEN set is empty. If it is, return to step 7; otherwise, calculate the cost function F for all nodes in OPEN. ij (k); and F ij The node with the smallest (k) value is placed in ROUTE, that is, it is used as the next expansion point.
[0018] Step 9: Determine if this node is the endpoint. If not, return to step 7.
[0019] Step 10, record the number of all nodes in ROUTE as num; at the same time, initialize the recursive starting point route(i), i = 0,
[0020] and i < num - 1; the end point route(j), j = 1, and i < j < num;
[0021] Step 11, calculate the total length dis of the planned route F (i,j) and the total length dis planned by the original A* algorithm A (i,j); if dis F (i,j) ≤ dis A (i,j), then replace the original sequence in the path; otherwise, continue to search for the shortest path node from the current node to the end point. Until all nodes in ROUTE are traversed.
[0022] Step 12, output the optimal path node sequence.
[0023] Further, in Step 1, information about the vehicle itself and surrounding obstacles is obtained through millimeter-wave radar, lidar, camera, inertial sensor IMU, GPS, and vehicle sensors.
[0024] In Step 1, the information about the vehicle itself and surrounding obstacles includes the vehicle's own state and position information, the state, type, and position information of obstacles, and the basic information of the road (such as lane lines, road boundaries, etc.).
[0025] Further, the vehicle simplified model in Step 2 is established as follows:
[0026] Vehicle left front corner point coordinates:
[0027]
[0028] Vehicle right front corner point coordinates:
[0029]
[0030] Vehicle left rear corner point coordinates:
[0031]
[0032] Vehicle right rear corner point coordinates:
[0033]
[0034] In the formula, (X r , Y r ) is the coordinate of the center point of the vehicle's rear axle; is the angle between the center line of the vehicle and the X-axis; S is the width of the vehicle; L is the total length of the vehicle; l rThe distance from the center of the rear axle of the car to the rear of the car; l f This is the distance from the center of the front axle of the car to the front of the car.
[0035] Furthermore, step 3 includes:
[0036] By calculating the distance weights between any pair of points, a path search is performed on a known graph to find the shortest path between any two points. The state transition formula is as follows:
[0037] Map[i][j]=min{Map[i][j],Map[i][k]+Map[k][j]} (5)
[0038] Map[i][j] represents the shortest path value from point i to point j; Map[i][k] represents the shortest path value from point i to point k; Map[k][j] represents the shortest path value from point k to point j; n is the total number of nodes; 1 <k<n;
[0039] Euclidean distance is chosen as the heuristic function. A bias adjustment factor α is set for H(k,j) to improve road network guidance based on high-precision maps. The improved A* algorithm cost function is used to represent the estimated cost from i to j after passing point k, and the cost function F... ij (k) is:
[0040] F ij (k)=G(i,k)+αH(k,j)(7)
[0041] Where F ij (k) represents the estimated cost of traveling from point i to point j via point k; G(i,k) represents the cost already paid for traveling from point i to point k; H(k,j) represents the predicted cost for traveling from k to j; and α is the bias adjustment factor.
[0042] The distance weight is expressed as:
[0043]
[0044] Where Map[μ][ν] is the distance weight from point μ to point ν, and ω(μ,ν) is the weight exponent.
[0045] Based on the obtained distance weights, find the shortest feasible path between the start and end points in the raster map, which meets the following requirements:
[0046]
[0047] (i,j)∈P represents the set of all feasible routes from point i to point j.
[0048] In step 3, G(i,k) = ξMap[i][k], where ξ represents the step size cost.
[0049] In step 3, H(k,j) is the Euclidean distance from k to j.
[0050] In step 3, dis F (i,j)=G(i,j).
[0051] dis A (i,j)=α×S ij (n)+β×D ij (n); where α is the cost of moving a single cell in a straight line, and β is the cost of moving a single cell in a diagonal. The relationship between α and β is: S ij (n) represents the number of steps taken along a straight line from point i to point j, D ij (n) represents the number of steps taken along the diagonal from point i to point j.
[0052] Furthermore, in step 5, the OPEN set is used to store all expandable node sets, the CLOSE set is used to store obstacle points and expanded node sets, and the ROUTE set is used to store the node with the minimum cost function value.
[0053] Beneficial Effects: The purpose of this invention is to provide an autonomous driving trajectory planning method based on an improved A* algorithm. A deviation adjustment factor is added to reduce the number of expanded nodes in the A* algorithm, thereby improving the algorithm's computational efficiency. Simultaneously, distance weights are used instead of the cost function, improving efficiency while also ensuring global optimality. This invention improves the algorithm's adaptability to complex environments and meets the real-time requirements of application scenarios. Attached Figure Description
[0054] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments, and the advantages of the present invention in the above and / or other aspects will become clearer.
[0055] Figure 1 This is a flowchart of the method of the present invention.
[0056] Figure 2 A simplified diagram of a car model.
[0057] Figure 3 This is a simplified version of the raster map.
[0058] Figure 4a , Figure 4b , Figure 4c , Figure 4d This is a schematic diagram of the algorithm steps.
[0059] Figure 5 This is a distance-weighted graph.
[0060] Figure 6 This is a schematic diagram of the adjacency relationship between two points. Detailed Implementation
[0061] like Figure 1 As shown, this invention provides an autonomous driving obstacle avoidance trajectory planning method based on an improved A* algorithm, specifically including:
[0062] Step 1: The vehicle uses multi-sensor fusion technology to obtain its own status information as well as information about obstacles and the surrounding environment.
[0063] In step 1, information required for trajectory planning is collected through information sensing modules such as millimeter-wave radar, lidar, camera, IMU, GPS, and vehicle sensors.
[0064] The data collected in step 1 includes the vehicle's own motion state and pose information; the state, type, location, and length and width information of obstacles;
[0065] Step 2: Create a high-precision map based on the collected information. The specific steps are as follows: Import the map into the vehicle's computer and rasterize the map; generate an m*n grid map, such as... Figure 3 As shown, m represents the number of grid cells in each row, and n represents the number of grid cells in each column.
[0066] refer to Figure 2 Establish a simplified vehicle dynamics model:
[0067] Coordinates of the vehicle's left front turning point:
[0068]
[0069] Coordinates of the vehicle's right front turning point:
[0070]
[0071] Coordinates of the vehicle's left rear corner:
[0072]
[0073] Coordinates of the vehicle's right rear corner:
[0074]
[0075] In the formula, (X r ,Y r ( ) represents the coordinates of the center point of the rear axle of the car; S is the angle between the car's centerline and the X-axis; S is the car's width; L is the car's total length; l r The distance from the center of the rear axle of the car to the rear of the car; l f This is the distance from the center of the front axle of the car to the front of the car.
[0076] Step 3: Based on the grid map and the simplified vehicle model, establish the cost function F. ij (k), and calculate the cost of each path using the cost function;
[0077] Step 3, establishing the cost function, includes the following steps:
[0078] By calculating the distance weights between any pair of points, a path search is implemented in a known graph to find the shortest path between any two points. The state transition formula is as follows:
[0079] Map[i][j]=min{Map[i][j],Map[i][k]+Map[k][j]},(1 <k<n) (5)
[0080] In the formula, Map[i][j] is the shortest path from point i to point j; Map[i][k] is the shortest path from point i to point k; Map[k][j] is the shortest path from point k to point j; and n is the total number of nodes.
[0081] Furthermore, the distance weights are established as follows:
[0082]
[0083] Where Map[μ][ν] represents the distance weight from point μ to point ν, and ω(μ,ν) is the weight exponent. For example... Figure 5 The table shown is a distance weight table, where INF indicates that the weight is infinite.
[0084] Furthermore, such as Figure 6 As shown, thick solid lines represent obstacle points, thin solid lines connecting two points indicate that they are adjacent and the connecting line does not pass through an obstacle point, indicating that a straight line can pass between the two points; dashed lines connecting two points indicate that they are not adjacent and since the connecting line passes through an obstacle point, a straight line cannot pass between the two points, and the distance between the two points has an infinite weight.
[0085] Euclidean distance was chosen as the heuristic function. A bias adjustment factor α was set for H(k,j) to improve road network guidance based on high-precision maps. The improved A* algorithm cost function is used to represent the estimated cost from i to j after passing point k, as follows:
[0086] F ij (k)=G(i,k)+αH(k,j)(7)
[0087] In the formula: F ij (k) represents the estimated cost of traveling from point i to point j via point k; G(i,k) represents the cost already paid for traveling from point i to point k; H(k,j) represents the predicted cost for traveling from k to j; and α is the bias adjustment factor.
[0088] Furthermore, G(i,k) = ξMap[i][k], where ξ represents the step size cost.
[0089] Furthermore, H(k,j) is the Euclidean distance from k to j.
[0090] The obtained distance weights are used to find the shortest feasible paths between the start and end nodes in the map. These paths must meet the following requirements:
[0091]
[0092] In the formula: dis F (i,j) represents the total length of the planned route; dis A (i,j) represents the total length planned by the original A* algorithm; (i,j)∈P represents the set of all feasible routes from point i to point j.
[0093] Furthermore, dis F (i,j)=G(i,j).
[0094] Furthermore, dis A (i,j)=α×S ij (n)+β×D ij (n); where α is the cost of moving a single cell in a straight line, and β is the cost of moving a single cell in a diagonal. The relationship between α and β is: S ij (n) represents the number of steps taken along a straight line from point i to point j, D ij (n) represents the number of steps taken along the diagonal from point i to point j.
[0095] Step 4, in such Figure 4a The grid map shown identifies the endpoint and obstacle points, with the car's current position as the starting point. The black areas represent drivable areas, while the white areas represent drivable areas.
[0096] Step 5: Set the OPEN, CLOSE, and ROUTE sets, and initialize them to empty sets.
[0097] Step 6, refer to Figure 4a Place the starting point k0 in the ROUTE set, and place the obstacle point k9 in the CLOSE set.
[0098] Step 7, refer to Figure 4a Using the current node k0 in ROUTE as the base point, search for all its expandable nodes k1, k2, and k3 and place them in the OPEN set. At the same time, remove all sets in CLOSE from OPEN; and also put the current node k0 into CLOSE.
[0099] Step 8, refer to Figure 4b, the OPEN set is not an empty set, calculate F ij (k1), F ij (k2), F ij (k3). Place the node k1 with the smallest F ij (k) value in ROUTE, that is, as the next expansion point.
[0100] Step 9, refer to Figure 4c , the node k1 is not the end point, return to Step 7;
[0101] Step 10, refer to Figure 4d , the node k17 is the end point, the number of all nodes in ROUTE is 9; at the same time, initialize the recursive starting point route(i), i = 0, and i < num - 1; the end point route(j), j = 1, and i < j < num;
[0102] Step 11, calculate the total length dis of the planned route F (i, j) and the total length dis of the original A* algorithm planning A (i, j); if dis F (i, j) ≤ dis A (i, j), then replace the original sequence in the path; otherwise, continue to search for the shortest path node from the current node to the end point.
[0103] Furthermore, the original sequence in the path is {k0, k1, k6, k7, k8, k 11 , k 12 , k 15 , k 17}}.
[0104] Furthermore, when i = 0, j = 1: disF(0, 1) = 3, disA(0, 1) = 3; there is no need to replace the (i, j) value.
[0105] When i = 0, j = 2: disF(0, 2) = 6, disA(0, 2) = 6; there is no need to replace the (i, j) value.
[0106] When i = 0, j = 3: disF(0, 3) = 8, disA(0, 3) = 9 replace the (0, 3) value with 8.
[0107] Traverse all nodes in ROUTE in turn.
[0108] Step 12, the final distance table obtained from Step 11 is as Figure 5 shown, calculate the optimal distance node and output the optimal path node sequence as (k0, k8, k17).
[0109] In its specific implementation, this application provides a computer storage medium and a corresponding data processing unit. The computer storage medium is capable of storing a computer program, which, when executed by the data processing unit, can run the invention's content regarding an autonomous driving obstacle avoidance trajectory planning method based on an improved A* algorithm, as well as some or all of the steps in various embodiments. The storage medium can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc.
[0110] Those skilled in the art will clearly understand that the technical solutions in the embodiments of the present invention can be implemented using computer programs and their corresponding general-purpose hardware platforms. Based on this understanding, the technical solutions in the embodiments of the present invention, or the parts that contribute to the prior art, can be embodied in the form of computer programs, i.e., software products. These computer program software products can be stored in a storage medium and include several instructions to cause a device containing a data processing unit (which may be a personal computer, server, microcontroller, MUU, or network device, etc.) to execute the methods described in various embodiments or certain parts of the embodiments of the present invention.
[0111] This invention provides an obstacle avoidance trajectory planning method for autonomous driving based on an improved A* algorithm. Many methods and approaches exist for implementing this technical solution; the above description is merely a preferred embodiment of the invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications should also be considered within the scope of protection of this invention. All components not explicitly stated in this embodiment can be implemented using existing technologies.
Claims
1. A method for planning obstacle avoidance trajectory for autonomous driving based on an improved A* algorithm, characterized in that, Includes the following steps: Step 1: Obtain real-time information about the vehicle itself and surrounding obstacles using multi-sensor fusion technology; Step 2: Create an m*n grid map of the surrounding environment and obstacles, where m represents the number of grid cells in each row and n represents the number of grid cells in each column, and create a simplified vehicle model. Step 3: Establish the cost function based on the grid map and the simplified vehicle model. And calculate the cost of each path using a cost function; Step 3 includes: By calculating the distance weights between any pair of points, a path search is performed on a known graph to find the shortest path between any two points. The state transition formula is as follows: (5)， For point Time The shortest path value; For point Time The shortest path value; For point Time The shortest path value; The total number of nodes; ; Cost function for: （7）， in For the process Point, from point Time Estimated costs; For point Time The value already paid; for arrive The predicted value; This is the deviation adjustment factor; in The total length of the planned route; The total length is planned using the original A* algorithm; Represents from point Time The set of all feasible routes, The set of all feasible path points; In step 3, the distance weight is expressed as: ， in For point Time Distance weights, For weighted index; Based on the obtained distance weights, find the shortest feasible path between the start and end points in the raster map, which meets the following requirements: （8）； In step 3, ,in Indicates the step size cost; In step 3, for arrive Euclidean distance; In step 3, ; ; where settings To determine the cost of moving a single grid line, set The cost of moving a single cell diagonally. and The relationship is ; From point Time The number of steps moved along a straight line. From point Time The number of steps moved along the diagonal; Step 4: Determine the start point, end point, and obstacle locations in the m*n grid map generated in Step 2; Step 5: Set the OPEN, CLOSE, and ROUTE sets and initialize them to empty sets; Step 6: Place the starting point in the ROUTE set and place all obstacle points in the CLOSE set; Step 7: Using the current node in the ROUTE set as the base point, search for all expandable node sets of the base point and place them in the OPEN set. At the same time, remove all points in the CLOSE set from the OPEN set; and also put the current node into the CLOSE set. Step 8: Determine if the OPEN set is empty. If it is, return to step 7; otherwise, calculate the cost function for all nodes in the OPEN set. ; and will The node with the smallest value is placed in the ROUTE set and used as the next expansion point; Step 9, determine the... Is the node with the smallest value the endpoint? If not, return to step 7; if so, sort the node set in the ROUTE set by cost function value from smallest to largest to obtain the original sequence. Step 10: Denote the number of all nodes in the ROUTE set as num; simultaneously initialize the recursion starting point. ,and ;end ,and ; Step 11, calculate the total length of the planned route. The total length is planned using the original A* algorithm. Size; if If the original sequence in the path is found to be correct, then replace it; otherwise, continue searching for the shortest path node from the current node to the destination until all nodes in the ROUTE set have been traversed. Step 12: Output the optimal path node sequence.
2. The method according to claim 1, characterized in that, In step 1, information about the vehicle itself and surrounding obstacles is obtained through millimeter-wave radar, lidar, cameras, inertial measurement units (IMUs), GPS, and vehicle sensors.
3. The method according to claim 2, characterized in that, In step 1, the information about the vehicle itself and surrounding obstacles includes the vehicle's own status and location information, the status, type and location information of the obstacles, and basic road information.
4. The method according to claim 3, characterized in that, In step 2, the simplified vehicle model is established as follows: Coordinates of the vehicle's left front turning point: （1）， Coordinates of the vehicle's right front turning point: （2）， Coordinates of the vehicle's left rear corner: （3）， Coordinates of the vehicle's right rear corner: （4）， in, These are the x-coordinate and y-coordinate of the center point of the rear axle of the car, respectively. , These are the x and y coordinates of the vehicle's left front turning point, respectively. , These are the x and y coordinates of the vehicle's right front turning point, respectively. , These are the x and y coordinates of the vehicle's left rear corner point, respectively. , These are the x and y coordinates of the vehicle's right rear corner point, respectively. The angle between the car's centerline and the X-axis; S is the width of the car; L is the total length of the car; This is the distance from the center of the rear axle of the car to the rear of the car. This is the distance from the center of the front axle of the car to the front of the car.
5. The method according to claim 4, characterized in that, In step 5, the OPEN set is used to store all sets of nodes that can be expanded, the CLOSE set is used to store obstacle points and expanded sets of nodes, and the ROUTE set is used to store the node with the minimum cost function value.