Vehicle path planning method and system based on improved A* algorithm, and storage medium
The modified A* algorithm addresses inefficiencies in traditional A* by using search weights and bidirectional search to optimize path planning, improving convergence speed and accuracy in complex environments for autonomous vehicles.
Patent Information
- Application Number
- CN202510315750.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-03-18
- Publication Date
- 2025-07-15
AI Technical Summary
The existing A* algorithm has poor timeliness in path planning, long search steps, difficult to comply with real-time search tasks, and unfriendly computing resources.
The improved A* algorithm is adopted to select points close to the target point by introducing search weights, perform two-way searches, and combine the Floyd algorithm to remove redundant nodes to optimize node selection and processing.
It improves the convergence speed and path length of path planning, reduces redundant search, and improves search efficiency and computing resource utilization.
Smart Images
Figure CN120313622A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the technical field of intelligent vehicle autonomous driving, and more specifically, to a vehicle path planning method, system, and storage medium. Background Art
[0002] Path planning, as a key technology for realizing intelligent vehicle autonomous driving, is mainly divided into global path planning and local trajectory planning. Among them, global planning plays a leading role. A more efficient global planning can make the driving of intelligent vehicles more stable and be more competent in complex driving environments. Therefore, researching more optimized global algorithms is of great significance for the autonomous driving tasks of intelligent vehicles.
[0003] Currently, there are already various path planning algorithms for global search, such as algorithms based on graph search, bionic algorithms, potential field algorithms, etc. Among them, the A* algorithm is a heuristic search algorithm that combines the Breadth-First Search (BFS) algorithm and the Dijkstra algorithm. In global search algorithms, the A* algorithm has the advantages of small size, accurate search, and wide application range.
[0004] Although the A* algorithm has high applicability in search algorithms, this algorithm has the following problems: (1) Poor timeliness. The A* algorithm takes a long time in the search process and is difficult to be competent for real-time search tasks; (2) Long search step length. In the search of the A* algorithm, as the map increases, its search step length will also increase exponentially, which is not friendly to computing resources. Summary of the Invention
[0005] The technical problem to be solved by this application is to provide a vehicle path planning method, system, and storage medium based on an improved A* algorithm, which have significantly improved convergence speed and path length compared with before improvement in complex environments, and can well adapt to real-time search tasks and optimize computing resources.
[0006] The embodiments of this application for solving the above technical problems are implemented as follows: A vehicle path planning method based on an improved A* algorithm, characterized by including: Obtain the driving state of the vehicle, vehicle positioning information, the destination of the vehicle, information on obstacles in front of the vehicle, and road environment information, establish a path planning space and discretize it to obtain sampling points; Use the improved A* algorithm for path planning search: add search weights and perform bidirectional search, and preferentially search several candidate points close to the target point direction. If there are no qualified ones among several preferred candidate points, then search the remaining candidate points; In the above technical solution, vehicle positioning information, information about obstacles in front of the vehicle, and road environment information are projected onto a path planning space to establish the path planning space; the road environment information includes road boundaries and lane lines.
[0007] In the above technical solution, at least 3 candidate points are set.
[0008] In the above technical solution, the current position of the vehicle or the starting point of the vehicle path planning is used as the coordinate origin (0, 0), and the destination of the vehicle is used as the end point. A Cartesian coordinate system is established.
[0009] In the above technical solution, starting from the starting point of the intelligent vehicle path planning, the path planning space is discretely sampled.
[0010] The discrete sampling includes sampling along the x - direction of the Cartesian coordinate system at a sampling interval Δx, and sampling along the y - direction of the Cartesian coordinate system at a sampling interval Δy. For the x - direction, m points are sampled, and for the y - direction, n points are sampled, and finally m n sampling points are obtained within the entire planning space.
[0011] In the above technical solution, in the improved A* algorithm, the weight cost function is expressed as: ; In the formula, is the weight cost function from the starting point of the intelligent vehicle to the end point; is the cumulative value of the weight cost of the path from the starting point of the intelligent vehicle to the current candidate point; is the estimated weight cost from the current candidate point to the end point of the path planning.
[0012] In the above technical solution, calculating the weight cost of the candidate point using the weight cost function also includes: Judging whether there is an obstacle interference situation for three priority search nodes. If there is no such situation, the candidate point with the smallest value is added to the search list and the adjacent points of this candidate point are searched; if there is an obstacle interference for all of them, then a selection is made among the remaining candidate points with lower priority.
[0013] In the above technical solution, the estimated weight cost from the current candidate point to the end point of the path planning, expressed using the Manhattan distance from the current candidate point to the end point of the planning, is: ; Where is the geometric coordinate of the target point, is the geometric coordinate of the current node.
[0014] In the above technical solution, the relative position between the current node and the search target point is represented by an angle as: ; where is the geometric coordinate of the target point, is the geometric coordinate of the current node; According to different angles, three nodes are selected as high-priority search nodes, and the remaining five points are low-priority search nodes.
[0015] In the above technical solution, the specific steps of using the improved A* algorithm for path planning search are as follows: First, initialize the starting point and the ending point, and assign initial weight cost function values to the starting point and the ending point respectively; Initialize the priority queues of the starting point and the ending point, which are used to store the nodes to be expanded respectively; Calculate the relative position between the search starting point and the search ending point, and check whether there are expansion points for the high-priority search nodes; if there are compliant nodes among the high-priority search nodes, directly expand them. If there are no compliant nodes, search other nodes to obtain the expansion nodes; For the currently selected node, calculate and update its weight cost function, cost value, and total estimated value; After each node expansion, check whether there is the same node in the search queue in another search direction. If there is an intersection node, find a path and return; If it is always in another search node, continue the search until a path is found or all nodes are searched. If there is still no intersection node after all nodes are searched, it proves that there is no path between the starting point and the ending point.
[0016] According to another aspect of the present invention, there is also provided a vehicle path planning system using the improved A* algorithm, including: A navigation module for obtaining navigation data including the global navigation path of the vehicle and vehicle positioning information; A map discrete space generation module for discretizing the navigation data into a grid map; A relative position detection module for detecting the relative position angle between the current node and the search ending point and determining the high-priority search node list; A path search module; for searching for the shortest path based on the A* algorithm through the search node list.
[0017] According to another aspect of the present invention, there is also provided a storage medium storing a computer program, and when the processor executes the computer program, it implements the vehicle path planning method based on the improved A* algorithm.
[0018] The beneficial effects of this application are as follows: This application can efficiently plan a global path for intelligent vehicles under relatively complex road conditions, improving the efficiency and accuracy of the planned path of intelligent vehicles in complex road environments. By adding search weights and performing bidirectional search, the problems of slow convergence speed and excessive search nodes in the traditional A* algorithm have been greatly improved.
[0019] Priority search for the target direction: When the traditional A algorithm searches, it expands nodes in all directions without distinction, which may lead to low search efficiency. The improved A algorithm introduces search weights and preferentially searches several candidate points in the direction close to the target point. If there are no candidate points that meet the requirements among several priority candidate points, then the remaining candidate points are searched. This can more efficiently guide the search process towards the target direction.
[0020] Introduction of special dynamic weights: The heuristic function of the traditional A algorithm is usually fixed, such as the Manhattan distance or the Euclidean distance. The improved A algorithm introduces special dynamic weights for the evaluation function to dynamically adjust the search accuracy and breadth of the algorithm, improving the algorithm efficiency.
[0021] Bidirectional search: The traditional A algorithm is a unidirectional search, searching from the starting point to the end point, with a large search range. The improved A algorithm uses bidirectional search, that is, it starts searching from both the starting point and the end point simultaneously until the search paths in both directions meet. This method can reduce the search range, avoid useless searches, and improve the search efficiency.
[0022] By changing the convergence condition of bidirectional search, the number of redundant searches is reduced, and the number of required nodes is reduced.
[0023] By adding search priorities, the number of turns during the search process is reduced, and the vehicle travels more smoothly.
[0024] Combining distance metrics and dynamically assigning weights to them to achieve the shortest global path and reduce the pathfinding time.
[0025] Node selection and processing optimization: Adding rule judgment: Adding rule judgment in the process of selecting child nodes of the A* algorithm to solve the problem of the route contacting the vertices of obstacles and avoid the generation of high-risk paths.
[0026] Removing redundant nodes: For the problem of redundant useless nodes existing in the path, use algorithms such as the Floyd algorithm to remove redundant nodes, reduce the number of turns, and shorten the path length. Description of the Drawings
[0027] To more clearly illustrate the technical solutions of the embodiments of the present application, the accompanying drawings required for the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present application and should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related drawings can also be obtained based on these drawings.
[0028] Figure 1 This is a flowchart of the vehicle path planning method based on the improved A* algorithm in the embodiments of the present application.
[0029] Figure 2 This is a two-way search logic diagram in the vehicle path planning method based on the improved A* algorithm in the embodiments of the present application.
[0030] Figure 3 This is a schematic diagram of the node number and the relative position of the child nodes in the embodiments of the present application.
[0031] Figure 4 This is a schematic diagram of the search process in the improved search node based on the improved A* algorithm in the embodiments of the present application.
[0032] Figure 5 This is a schematic diagram of the grid map used in the first example of the present invention.
[0033] Figure 6 This is a comparison diagram of the search process between the present invention and the traditional A* algorithm in the first example.
[0034] Figure 7 This is a comparison diagram of the convergence speed between the present invention and the traditional A* algorithm in maps of different sizes.
[0035] Figure 8 This is a comparison diagram of the search step between the present invention and the traditional A* algorithm in maps of different sizes. Detailed implementation manners
[0036] To make the objectives, technical solutions, and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are some, but not all, of the embodiments of the present application. Usually, the components of the embodiments of the present application described and illustrated herein can be arranged and designed in various different configurations.
[0037] Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the present application claimed, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of the present application.
[0038] The features and performance of the present application will be further described in detail below in conjunction with embodiments.
[0039] Embodiment 1 The technical solution adopted by the present invention is as follows: A vehicle path planning method based on an improved A* algorithm, comprising the following steps: Obtain the driving state of the vehicle, obtain the path planning space, and perform discrete sampling on the path planning space to obtain m columns and n rows, a total of m n sampling points; Use the improved A* algorithm to perform path planning search. Add search weights, and preferentially search for three candidate points in the direction close to the target point. If there are no qualified ones among the three preferred candidate points, then search the remaining candidate points. While adding search permissions, perform bidirectional search to reduce the search duration. After meeting the convergence condition, merge the final search closed lists in two directions and eliminate overlapping points, and connect them in sequence to finally form the intelligent vehicle planning path.
[0040] According to the vehicle path planning algorithm based on the improved A* algorithm provided by the present invention, it includes: Taking the current position of the vehicle as the coordinate origin (0, 0) and the destination of the vehicle as the end point Establish a Cartesian coordinate system; project the vehicle positioning information, the information of obstacles in front of the vehicle, and the road environment information onto the path planning space, so as to complete the establishment of the path planning space. The road environment information includes road boundaries and lane lines.
[0041] According to the vehicle path planning algorithm based on the improved A* algorithm provided by the present invention, it includes: Start sampling the path planning space from the starting point of the intelligent vehicle path planning; the discrete sampling includes sampling along the x direction of the Cartesian coordinate system at a sampling interval Δx, and sampling along the y direction of the Cartesian coordinate system at a sampling interval Δy. Sample m points in the x direction and n points in the y direction, and finally obtain m n sampling points within the entire planning space.
[0042] According to the vehicle path planning algorithm based on the improved A* algorithm provided by the present invention, it includes: Initialize the starting point and the end point, and respectively assign initial weight cost function values to the starting point and the end point; Initialize the priority queues of the starting point and the end point, which are respectively used to store the nodes to be expanded; Calculate the relative position between the search start point and the search end point, and search for whether there are expandable points in the high-priority nodes; if there are compliant nodes in the high-priority search nodes, directly expand them. If there are no compliant nodes, search other nodes to obtain expandable nodes. For the currently selected node, calculate and update its weight cost function, cost value, and total estimated value.
[0043] After each node expansion, check whether there is the same node in the search queue in another search direction. If there is an intersection node, find a path and return. If it is always in another search node, continue the search until a path is found or all nodes are searched. If there is still no intersection node after all nodes are searched, it proves that there is no path between the start point and the end point.
[0044] The vehicle path planning algorithm based on the improved A* algorithm provided by the present invention includes: Weight cost function It is expressed as: ; In the formula, is the weight cost function from the starting point to the ending point of the intelligent vehicle; is the cumulative value of the weight cost of the path from the starting point of the intelligent vehicle to the currently selected point; is the estimated weight cost from the currently selected point to the ending point of the path planning.
[0045] The vehicle path planning algorithm based on the improved A* algorithm provided by the present invention includes: The estimated weight cost from the currently selected point to the ending point of the path planning , uses the Manhattan distance from the currently selected point to the ending point of the planning as the weight cost , which is expressed as: ; Where is the geometric coordinate of the target point, is the geometric coordinate of the current node.
[0046] The vehicle path planning algorithm based on the improved A* algorithm provided by the present invention includes: The relative position between the current node and the search target point is expressed by an angle as: It is expressed as: ; According to different angles, three nodes are selected as high-priority search nodes, and the remaining five points are low-priority search nodes.
[0047] The corresponding relationship between the angle and the priority search nodes is shown in Table 1 below.
[0048] Table 1 Relationship Table between Relative Angle and High-Priority Search Nodes
[0049] The vehicle path planning algorithm based on the improved A* algorithm provided by the present invention includes: The utilization of the weight cost function Calculating the weight cost of the candidate points further includes: Determine whether there is an obstacle interference among the three priority search nodes. If not, add The candidate point with the smallest value to the search list and search for the adjacent points of this candidate point. If there is obstacle interference in all of them, select from the remaining low-priority candidate points.
[0050] Embodiment 2 The vehicle path planning method based on the improved A* algorithm in the example provided by this method, as Figure 1-4 shown, the steps are as follows: Step1. Through the vehicle navigation module, obtain the vehicle position, map information and vehicle destination, discretize the map, and generate a grid map.
[0051] Step2. Establish two new Openlist sets and Closelist sets, and put the starting points of the bidirectional search, that is, the starting point and the ending point of the vehicle, into the two Openlist sets respectively.
[0052] Step3. Detect the current comprehensive priority function value in the open set Openlist set The smallest best node, and find the node where the search starts currently.
[0053] Calculate the relative angle between the current node to be searched and the end point, specifically: ; Find the high-priority search node corresponding to the angle according to Table 1 below: Table 1 Relationship Table between Relative Angle and High-Priority Search Nodes
[0054] See the schematic diagram of the child node number and the specific position of the child node Figure 3 .
[0055] Step4. Calculate whether all the high-priority child nodes do not meet the requirements. If not, preferentially select from the high-priority child nodes; if all do not meet the requirements, search for the five candidate nodes.
[0056] Step 5: Search the two lists in turn, and after the search is completed, detect whether there are the same nodes in the two lists. If there are the same nodes, merge the two lists, connect the roads and complete the search. If there are no same nodes, continue the search until the condition is met.
[0057] The A* algorithm in the example provided by this method is the most effective direct search method for solving the shortest path in a static environment. The A* algorithm is a heuristic search algorithm that combines the Breadth-First Search (BFS) algorithm and the Dijkstra algorithm. By changing the search performance through the heuristic function, it can find the shortest path faster. Its comprehensive priority function is as follows: ; In the formula, is the weight cost function from the starting point to the end point of the intelligent vehicle; is the cumulative value of the weight cost of the path experienced from the starting point of the intelligent vehicle to the current candidate point; is the estimated weight cost from the current candidate point to the end point of the path planning.
[0058] For the comprehensive priority when the estimated cost from the current point to the end point tends to zero, the A* algorithm degenerates into the Dijkstra algorithm, adopting a greedy mode. Each iteration selects the next node with the minimum cost to the current node until reaching the end point. In this process, all nodes from the starting point to the end point are traversed, and the search speed is slow but the path is the shortest; when tends to zero, the A* algorithm degenerates into the breadth-first search algorithm, and the search speed becomes faster but the shortest path cannot be guaranteed. See the flowchart in Figure 4 as shown.
[0059] In Step 1, the map discrete space generation module is used to discretize the real navigation data into a grid map. Figure 5 is a schematic diagram of the grid map used in this example.
[0060] Refer to Figure 6 (d), which is an embodiment of the present invention, provides a vehicle path planning algorithm based on an improved A* algorithm. In order to verify the beneficial effects of the present invention, scientific demonstration is carried out through simulation experiments. Among them, Figure 6 (a) Traditional A* algorithm, Figure 6 (b) is the A* algorithm with an improved search node strategy, Figure 6 (c) is the bidirectional A* search algorithm.
[0061] The simulation has been run in an environment with an AMD processor and 16GB of memory. The operating system used is Windows 11 Home Edition, and the programming language used is Python.
[0062] Running the algorithm of the present invention in multiple instances, the experimental results of the search step size and convergence time can finally be obtained as Figure 7 、 Figure 8 shown.
[0063] It can be clearly seen that, whether it is the search step size or the convergence time, compared with the traditional A* algorithm, the A* algorithm with improved search node strategy, the bidirectional A* search algorithm, and the improved A* algorithm (improved search node strategy + bidirectional A* search algorithm) of the present invention are all optimal in terms of search step size and convergence time.
[0064] Especially the improved A* algorithm (improved search node strategy + bidirectional A* search algorithm) has the shortest step size and the shortest convergence time, and is significantly superior to the traditional A* algorithm.
[0065] Embodiment 3 According to another aspect of the present invention, there is also provided a computer device, including a memory and a processor. The memory stores a computer program, and when the processor executes the computer program, it implements a vehicle path planning method based on the improved A* algorithm.
[0066] It also includes a navigation module, a map discrete space generation module, a relative position detection module, and a path search module. Among them, the navigation module is used to obtain the global navigation path and vehicle positioning information of the intelligent vehicle; The map discrete space generation module is used to discretize the real navigation data into a grid map; the relative position detection module is used to detect the relative position angle between the current node and the search end point and determine the high-priority search node list. The path search module is used to search for the shortest path based on the A* algorithm during use to implement path planning.
[0067] The above-described embodiments are some embodiments of the present application, rather than all embodiments. The detailed description of the embodiments of the present application is not intended to limit the scope of the present application to be protected, but merely represents the selected embodiments of the present application. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present application without creative efforts belong to the scope of protection of the present application.
Claims
1. A vehicle path planning method based on an improved A* algorithm, characterized in that Including: Obtain the driving state of the vehicle, vehicle positioning information, the destination of the vehicle, information on obstacles in front of the vehicle, and road environment information, establish a path planning space and discretize it to obtain sampling points; Use an improved A* algorithm for path planning search: add search weights and perform bidirectional search, preferentially search for several candidate points in the direction close to the target point. If there are no qualified points among several preferred candidate points, then search the remaining candidate points; After meeting the convergence condition, merge the closed lists of the last searches in the two directions of the bidirectional search, eliminate overlapping points, and connect them in sequence to finally form the planned path of the intelligent vehicle.
2. The vehicle path planning method based on the improved A* algorithm according to claim 1, wherein Project the vehicle positioning information, information on obstacles in front of the vehicle, and road environment information onto the path planning space to establish the path planning space; the road environment information includes road boundaries and lane lines.
3. The vehicle path planning method based on the improved A* algorithm according to claim 1, characterized in that Set at least 3 candidate points.
4. The vehicle path planning method based on the improved A* algorithm according to claim 1, characterized in that Take the current position of the vehicle or the starting point of the vehicle path planning as the coordinate origin, and the destination of the vehicle as the end point to establish a path planning space coordinate system.
5. The vehicle path planning method based on the improved A* algorithm according to claim 1, characterized in that Start from the starting point of the intelligent vehicle path planning and perform discretized sampling on the path planning space.
6. The vehicle path planning method based on the improved A* algorithm according to claim 1, wherein In the improved A* algorithm, the weighted cost function is expressed as: ; In the formula, is the weight cost function from the starting point to the ending point of the intelligent vehicle; is the cumulative value of the weight cost of the path experienced from the starting point of the intelligent vehicle to the current candidate point; is the estimated weight cost from the current candidate point to the ending point of the path planning.
7. The vehicle path planning method based on the improved A* algorithm according to claim 1, characterized in that Said utilization of the weighted cost function Calculating the weighted cost of the candidate points further includes: Determine whether there is an obstacle interference for the three priority search nodes. If none exists, add the candidate point with the smallest value to the search list and search for the adjacent points of this candidate point; if there is obstacle interference for all of them, then make a selection among the remaining candidate points with lower priority.
8. The vehicle path planning method based on the improved A* algorithm according to claim 1, wherein The relative position between the current node and the search target point is represented by an angle as: ; where are the geometric coordinates of the target point, are the geometric coordinates of the current node; According to different angles, select three nodes as high-priority search nodes, and the remaining five points as low-priority search nodes.
9. A vehicle path planning system using an improved A* algorithm, characterized in that Including: A navigation module for obtaining navigation data including the global navigation path of the vehicle and vehicle positioning information; A map discrete space generation module for discretizing the navigation data into a grid map; A relative position detection module for detecting the relative position angle between the current node and the search end point and determining the list of high-priority search nodes; A path search module for searching for the shortest path based on the A* algorithm by searching the list of nodes.
10. A storage medium stores a computer program, characterized in that When the processor executes the computer program, it implements the vehicle path planning method based on the improved A* algorithm according to any one of claims 1-8.