Intelligent vehicle path planning method based on improved Astar algorithm

Abstract

The invention discloses an intelligent vehicle path planning method based on an improved Astar algorithm, and the method comprises the following steps: S1, enabling an intelligent vehicle to obtain environment information around a current map, building a grid map, and building a barrier-free state region and a barrier state region in each grid; s2, creating binary heap tables OpenList and CloseList according to the established grating map, defining coordinates of a starting point and an end point, and storing the coordinates of the starting point into the OpenList table; s3, selecting a distance calculation function, improving a heuristic function, introducing an evaluation function weight coefficient, and dynamically adjusting the weight of the heuristic function; s4, improving an Astar algorithm to expand a child node selection rule, and adjusting 8 neighborhood diagonal movement cost; and S5, deleting redundant collinear nodes and turning points on the basis of a Ramer-Douglas-Peucker (Ramer-Douglas-Peucker) algorithm, and obtaining an optimized path. According to the method, the global path planning speed and safety of the Astar algorithm are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of global path planning of autonomous driving vehicles, and in particular to a smart vehicle path planning method based on an improved A_star algorithm. Background Art

[0002] The path planning of an autonomous vehicle is a partial behavioral decision based on the vehicle's real-time surrounding environment and the driving target position. It is a process of providing autonomous vehicles with information on drivable path areas and assisted driving guidance. The path planning of autonomous vehicles can be divided into two categories according to their ability to grasp the surrounding environment. The first category is that the intelligent vehicle plans the path based on the verified surrounding environment information, and the planned path is a global path; the second category is that the intelligent vehicle plans the path based on sensor information, and the planned path is a local path. The premise of global path planning is that the intelligent vehicle map database is known, and the drivable area and the optimal path are planned by feedback, optimization, etc. in combination with the vehicle's driving target position. According to the different principles of global path planning, it can be divided into A* algorithm based on graph search and its variant algorithm D* algorithm, Dijkstra algorithm, etc.; algorithms based on random sampling, such as Rapid-exploration Random Tree (RRT) algorithm and its variant algorithm RRT*, Monte Carlo random sampling algorithm, etc., as well as intelligent algorithms such as deep learning algorithm and ant colony algorithm. Although graph search-based algorithms can find relatively good paths, their high computational complexity reduces search efficiency, the heuristic function settings are affected and they are prone to falling into local minimum states, and the generated paths have path breakpoint problems, which often fail to provide vehicles with paths that meet turning requirements. Although intelligent algorithms such as deep learning algorithms can obtain the optimal path well, due to their complex calculation methods, they often take too long to solve and are not necessarily suitable for global path planning algorithms. The A* algorithm has the advantages of high computational efficiency and short planning paths, and is therefore widely used in the field of unmanned vehicle path planning. Summary of the invention

[0003] The purpose of the present invention is to solve one of the technical problems existing in the prior art. To this end, the present invention provides a smart car path planning method based on an improved A_star algorithm, which can enable the smart car to find a safe and collision-free optimal path from a starting point to a target point in an environment with obstacles.

[0004] To achieve the above design objectives, the present invention provides a smart car path planning method based on an improved A_star algorithm, comprising the following steps:

[0005] Step 1: The smart car obtains the environmental information around the current map and creates a grid map, and establishes an obstacle-free state area and an obstacle state area in each grid;

[0006] Step 2: Create binary heap tables OpenList and CloseList according to the established grid map, define the starting point and end point coordinates, and store the starting point coordinates in the OpenList table.

[0007] Step 3: Select the distance calculation function, improve the heuristic function, introduce the evaluation function weight coefficient, and dynamically adjust the weight of the heuristic function;

[0008] Step 4: Improve the A_star algorithm to expand the child node selection rules and adjust the 8-neighborhood diagonal movement cost;

[0009] Step 5: Based on the RDP (Ramer-Douglas-Peucker) algorithm, redundant collinear nodes and turning points are deleted to obtain the optimized path.

[0010] Furthermore, in the process of establishing the grid map in step 1, it is assumed that the working environment of the smart car is a two-dimensional plane area A, which is evenly divided into a grid of m rows and n columns, and each grid can be represented as a coordinate point (i, j), where i = 1, 2, ..., m and j = 1, 2, ..., n.

[0011] Define a two-dimensional matrix a ij To indicate the state of each unit grid in the map, if the grid is occupied, it is represented by black, and the value in the corresponding numerical matrix is ​​a ij =1; the grid in idle state is represented by white, and the value in the corresponding numerical matrix is ​​a ij = 0. Therefore, the mathematical model of the grid map can be expressed as:

[0012]

[0013] Furthermore, the step 3 selects a distance calculation function, improves the heuristic function, introduces a weight coefficient of the evaluation function, and dynamically adjusts the weight of the heuristic function, specifically including:

[0014] Step 3.1: Select the Euclidean distance formula to calculate the node cost. The formula is:

[0015] f(n)=g(n)+h(n)

[0016]

[0017] Where: n is the current node, f(n) is the cost function of the current node n, g(n) is the actual cost from the path starting point S to the current node n, and h(n) is the heuristic function, which represents the estimated cost from the current node n to the path target point G.

[0018] Step 3.2, introduce weight coefficients ωg, ωh. Assume that the coordinates of the starting point S are (x s ,y s ), the coordinates of the current node n are (x n ,y n ), the coordinates of the target point G are (x g ,y g ), then ω g ,ω h It can be expressed as:

[0019]

[0020] As the current node n continues to change, the values ​​and the values ​​also change continuously, thus realizing the dynamic change of the evaluation function coefficient weights, thereby realizing the adaptive adjustment of the evaluation function f(n).

[0021] The improved evaluation function f(n) is shown in the following formula:

[0022] f(n)=ω g g(n)+ω h ·h(n)

[0023] Furthermore, the specific method of improving the A_star algorithm and expanding the sub-node selection rule in step 4 is:

[0024] Step 4.1: According to the positional relationship between the parent node and the 8 child nodes, the 8 child nodes are divided into 2 categories, namely, category I (orthogonal) child nodes and category II (oblique) child nodes.

[0025] Step 4.2: Set the movement cost of type I child nodes to 1, and the movement cost of type II child nodes to 1.414.

[0026] Step 4.3: When the parent node expands outward to a type II child node, check whether its neighbor's type I child node is an obstacle. If it is an obstacle, set the cost of moving the type II child node to 2 or a higher value; if it is not an obstacle, do nothing.

[0027] Step 4.4: Traverse the adjacent points of the starting node, and determine whether the adjacent point being traversed is passable based on the initialization parameter information. If it is not passable, add the current adjacent point to the CloseList list, continue to traverse the next adjacent point, and add the passable adjacent point to the OpenList list.

[0028] Furthermore, the specific method of deleting redundant collinear nodes and turning points based on the RDP (Ramer-Douglas-Peucker) algorithm in step 5 is as follows:

[0029] Step 5.1: Set the tolerance value to 0, traverse all nodes, delete the intermediate nodes on the same line of the path nodes, and only keep the starting point, turning point and end point.

[0030] Step 5.2: Set the tolerance value, perform equidistant sampling between two adjacent path nodes from the starting point to the target point, and then test several equidistant sampling points in turn, taking each node and the previous end path node to determine whether there is an obstacle on the path between the two points, and calculate the distance from the obstacle next to the path to the path, and compare the size of the tolerance value. If it is satisfied, it means that the current node is a non-essential turning point and should be deleted; otherwise, it means that the current node constitutes a dangerous path and is a key turning point, which should be retained.

[0031] Step 5.3: Repeat step 5.2 from the target point to the starting point, which reduces the turning degree and path length, and is conducive to the safe driving of the smart car.

[0032] The present invention provides a smart car path planning method based on an improved A_star algorithm, by acquiring the environmental information around the current map of the smart car and establishing a grid map, establishing an obstacle-free state area and an obstacle state area in each grid; creating binary heap tables OpenList and CloseList according to the established grid map, defining the starting point and end point coordinates, and storing the starting point coordinates in the OpenList table. Selecting a distance calculation function, improving the heuristic function, introducing an evaluation function weight coefficient, and dynamically adjusting the weight of the heuristic function; improving the A_star algorithm to expand the child node selection rules, adjusting the 8-neighborhood diagonal moving cost; finally, based on the RDP (Ramer-Douglas-Peucker) algorithm, deleting redundant collinear nodes and turning points, a safe and collision-free optimal path from the starting point to the target point is obtained, thereby improving the search efficiency of the algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0034] Figure 1 It is a flow chart of a smart car path planning method based on an improved A_star algorithm of the present invention.

[0035] Figure 2 It is a schematic diagram of a grid map of the present invention.

[0036] Figure 3 It is a schematic diagram of the relationship between the parent node and the child node of the present invention.

[0037] Figure 4 It is a schematic diagram of the barrier isolation analysis of the present invention.

[0038] Figure 5 It is a schematic diagram of deleting redundant nodes in the path planning of the present invention.

[0039] Figure 6 It is a schematic diagram of the path planning simulation of the present invention. DETAILED DESCRIPTION

[0040] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and should not be construed as limiting the present invention.

[0041] See also Figure 1 The present invention provides a smart car path planning method based on an improved A_star algorithm, comprising the following steps:

[0042] S1: The smart car obtains the environmental information around the current map and builds a grid map, and establishes an obstacle-free state area and an obstacle state area in each grid;

[0043] S2: Create binary heap tables OpenList and CloseList according to the established grid map, define the start and end point coordinates, and store the start point coordinates in the OpenList table;

[0044] S3: Select the distance calculation function, improve the heuristic function, introduce the evaluation function weight coefficient, and dynamically adjust the weight of the heuristic function;

[0045] S4: Improve the A_star algorithm to expand the child node selection rules and adjust the 8-neighborhood diagonal movement cost;

[0046] S5: Based on the RDP (Ramer-Douglas-Peucker) algorithm, redundant collinear nodes and turning points are deleted to obtain the optimized path.

[0047] The following is a further explanation based on the specific implementation steps:

[0048] In step S1, please refer to Figure 2The grid map diagram provided, in the grid map, assume that the working environment of the smart car is a two-dimensional plane area A, which is evenly divided into a grid of m rows and n columns. Each grid can be represented as a coordinate point (i, j), where i = 1, 2, ..., m and j = 1, 2, ..., n. Define a two-dimensional matrix a ij To indicate the state of each unit grid in the map, if the grid is occupied, it is represented by black, and the value in the corresponding numerical matrix is ​​a ij =1; the grid in idle state is represented by white, and the value in the corresponding numerical matrix is ​​a ij = 0. Therefore, the mathematical model of the grid map can be expressed as:

[0049]

[0050] In step S2, specifically, create the binary heap tables OpenList and CloseList of the A_star algorithm, add the starting node in the initialization parameter to the CloseList list, and use the starting node as the parent node of its adjacent point;

[0051] In step S3, a distance calculation function is selected, the heuristic function is improved, the evaluation function weight coefficient is introduced, and the weight of the heuristic function is dynamically adjusted. Specifically,

[0052] Step 3.1: Select the Euclidean distance formula to calculate the node cost. The formula is:

[0053] f(n)=g(n)+h(n)

[0054]

[0055] Where: n is the current node, f(n) is the cost function of the current node n, g(n) is the actual cost from the path starting point S to the current node n, and h(n) is the heuristic function, which represents the estimated cost from the current node n to the path target point G.

[0056] Step 3.2, introduce weight coefficients ωg, ωh. Assume that the coordinates of the starting point S are (x s ,y s ), the coordinates of the current node n are (x n ,y n ), the coordinates of the target point G are (x g ,y g ), then ω g ,ω h It can be expressed as:

[0057]

[0058] As the current node n continues to change, the values ​​and the values ​​also change continuously, thus realizing the dynamic change of the evaluation function coefficient weights, thereby realizing the adaptive adjustment of the evaluation function f(n).

[0059] The improved evaluation function f(n) is shown in the following formula:

[0060] f(n)=ω g g(n)+ω h ·h(n)

[0061] In step S4, the A_star algorithm is improved to expand the child node selection rule and adjust the 8-neighborhood diagonal movement cost. For details, please refer to Figure 3 The provided parent node and child node relationship diagram,

[0062] Step 4.1: According to the positional relationship between the parent node and the 8 child nodes, the 8 child nodes are divided into 2 categories, namely, category I (orthogonal) child nodes and category II (oblique) child nodes.

[0063] Step 4.2: Set the movement cost of type I child nodes to 1, and the movement cost of type II child nodes to 1.414.

[0064] Step 4.3: When the parent node expands outward to a type II child node, check whether its neighbor's type I child node is an obstacle. If it is an obstacle, set the cost of moving the type II child node to 2 or a higher value; if it is not an obstacle, do nothing.

[0065] Step 4.4: Traverse the adjacent points of the starting node, and determine whether the adjacent point being traversed is passable based on the initialization parameter information. If it is not passable, add the current adjacent point to the CloseList list, continue to traverse the next adjacent point, and add the passable adjacent point to the OpenList list.

[0066] In step S5, redundant collinear nodes and turning points are deleted based on the RDP (Ramer-Douglas-Peucker) algorithm to obtain an optimized path. For details, please refer to Figure 4 The provided barrier isolation analysis diagram, Figure 5 The provided path planning diagram for deleting redundant nodes.

[0067] Step 5.1: Set the tolerance value to 0, traverse all nodes, delete the intermediate nodes on the same line of the path nodes, and only keep the starting point, turning point and end point.

[0068] Step 5.2: Set the tolerance value, perform equidistant sampling between two adjacent path nodes from the starting point to the target point, and then test several equidistant sampling points in turn, taking each node and the previous end path node to determine whether there is an obstacle on the path between the two points, and calculate the distance from the obstacle next to the path to the path, and compare the size of the tolerance value. If it is satisfied, it means that the current node is a non-essential turning point and should be deleted; otherwise, it means that the current node constitutes a dangerous path and is a key turning point, which should be retained.

[0069] Step 5.3: Repeat step 5.2 again from the target point to the starting point to obtain the optimized path.

[0070] After the above process and method, the intelligent car finally finds a path with fast search speed, few traversal nodes and safe and collision-free. For the complete simulation process, please refer to Figure 6 Schematic diagram of the path planning simulation provided.

[0071] What is disclosed above is only a preferred embodiment of the present invention, and it certainly cannot be used to limit the scope of rights of the present invention. A person skilled in the art can understand that all or part of the processes of the above embodiment and equivalent changes made according to the claims of the present invention still fall within the scope of the invention.

Claims

1. A smart car path planning method based on improved A_star algorithm, characterized in that: The following steps are involved: Step 1: The smart car obtains the environmental information around the current map and creates a grid map, and establishes an obstacle-free state area and an obstacle state area in each grid; Step 2: Create binary heap tables OpenList and CloseList according to the established grid map, define the starting point and end point coordinates, and store the starting point coordinates in the OpenList table. Step 3: Select the distance calculation function, improve the heuristic function, introduce the evaluation function weight coefficient, and dynamically adjust the weight of the heuristic function; Step 4: Improve the A_star algorithm to expand the child node selection rules and adjust the 8-neighborhood diagonal movement cost; Step 5: Based on the RDP (Ramer-Douglas-Peucker) algorithm, redundant collinear nodes and turning points are deleted to obtain the optimized path.

2. According to claim 1, a smart car path planning method based on improved A_star algorithm is characterized in that: In the process of building a grid map in step 1, it is assumed that the working environment of the smart car is a two-dimensional plane area A, which is evenly divided into a grid of m rows and n columns. Each grid can be represented as a coordinate point (i, j), where i = 1, 2, ..., m and j = 1, 2, ..., n. Define a two-dimensional matrix a ij To indicate the state of each unit grid in the map, if the grid is occupied, it is represented by black, and the value in the corresponding numerical matrix is ​​a ij =1; the grid in idle state is represented by white, and the value in the corresponding numerical matrix is ​​a ij = 0. Therefore, the mathematical model of the grid map can be expressed as:

3. The intelligent vehicle path planning method based on the improved A_star algorithm according to claim 1 is characterized in that: The specific method of selecting the distance calculation function, improving the heuristic function, introducing the evaluation function weight coefficient, and dynamically adjusting the weight of the heuristic function in step 3 is: Step 3.1: Select the Euclidean distance formula to calculate the node cost. The formula is: f(n)=g(n)+h(n) Where: n is the current node, f(n) is the cost function of the current node n, g(n) is the actual cost from the path starting point S to the current node n, and h(n) is the heuristic function, which represents the estimated cost from the current node n to the path target point G. Step 3.2, introduce weight coefficients ωg, ωh. Assume that the coordinates of the starting point S are (x s ,y s ), the coordinates of the current node n are (x n ,y n ), the coordinates of the target point G are (x g ,y g ), then ω g ,ω h It can be expressed as: As the current node n continues to change, the values ​​and the values ​​also change continuously, thus realizing the dynamic change of the evaluation function coefficient weights, thereby realizing the adaptive adjustment of the evaluation function f(n). The improved evaluation function f(n) is shown in the following formula: f(n)=ω g ·g(n)+ω h ·h(n) 4. The intelligent vehicle path planning method based on the improved A_star algorithm according to claim 1 is characterized in that: The specific method of improving the A_star algorithm and expanding the sub-node selection rule in step 4 is: Step 4.1: According to the positional relationship between the parent node and the 8 child nodes, the 8 child nodes are divided into 2 categories, namely, category I (orthogonal) child nodes and category II (oblique) child nodes. Step 4.2: Set the movement cost of type I child nodes to 1, and the movement cost of type II child nodes to 1.

414. Step 4.3: When the parent node expands outward to a type II child node, check whether its neighbor's type I child node is an obstacle. If it is an obstacle, set the cost of moving the type II child node to 2 or a higher value; if it is not an obstacle, do nothing. Step 4.4: Traverse the adjacent points of the starting node, and determine whether the adjacent point being traversed is passable based on the initialization parameter information. If it is not passable, add the current adjacent point to the CloseList list, continue to traverse the next adjacent point, and add the passable adjacent point to the OpenList list.

5. The intelligent vehicle path planning method based on the improved A_star algorithm according to claim 1 is characterized in that: The specific method of deleting redundant collinear nodes and turning points based on the RDP (Ramer-Douglas-Peucker) algorithm in step 5 is as follows: Step 5.1: Set the tolerance value to 0, traverse all nodes, delete the intermediate nodes on the same line of the path nodes, and only keep the starting point, turning point and end point. Step 5.2: Set the tolerance value, perform equidistant sampling between two adjacent path nodes from the starting point to the target point, and then test several equidistant sampling points in turn, taking each node and the previous end path node to determine whether there is an obstacle on the path between the two points, and calculate the distance from the obstacle next to the path to the path, and compare the size of the tolerance value. If it is satisfied, it means that the current node is a non-essential turning point and should be deleted; otherwise, it means that the current node constitutes a dangerous path and is a key turning point, which should be retained. Step 5.3: Repeat step 5.2 again from the target point to the starting point.

Images