Path planning method combining improved RRT and artificial potential field method based on vehicle corner constraint

Abstract

The invention relates to the field of global path planning of an automatic driving vehicle, in particular to a path planning method combining an improved RRT and an artificial potential field method based on vehicle corner constraint, and the method comprises the steps: firstly introducing a bias probability P in a random tree expansion process of an RRT algorithm; adding a gravitational component into the random tree based on an artificial potential field method, and then establishing a repulsive force field around the obstacle based on the artificial potential field method; selecting points based on the gravitation component and the repulsive force field, and expanding by dynamic step length to obtain a rough solution path; performing secondary corner constraint based on nodes on the rough solution path to obtain nodes conforming to vehicle steering; and finally, on the basis of B-spline curve smooth path updating and fitting of the constrained new nodes, a final path is formed. According to the method, the problem that an artificial potential field method is prone to falling into local optimum is solved, and the method has obvious optimization effects in the aspects of time, path length, iteration times and path smoothness of an RRT global path planning algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of global path planning for autonomous driving vehicles, and in particular to a path planning method combining an improved RRT and an artificial potential field method based on vehicle turning angle constraints. Background Art

[0002] In recent years, with the continuous development of autonomous driving technology, path planning, as one of the key technologies of intelligent driving, determines the best driving path of the vehicle on the road through algorithms, which directly affects driving efficiency and safety. At present, path planning methods are mainly divided into global path planning and local path planning according to different planning scopes. Global path planning is to plan an optimal path from the starting point to the end point for the vehicle when the starting point and the end point are known. Local path planning is to plan the driving path for the vehicle at the next moment, focusing on real-time and adaptability, and considering factors such as road conditions and traffic conditions. Among them, according to different planning principles, global path planning can be divided into algorithms based on random sampling, such as the Rapid-exploration Random Tree (RRT) algorithm and its variant algorithm RRT*; A* algorithm and Dijkstra algorithm based on graph search; as well as deep learning algorithms, genetic algorithms, ant colony algorithms, etc. Path planning methods based on graph search have obvious disadvantages in path continuity, vehicle dynamics constraints, and computational complexity. Path planning methods based on deep learning have the problem of too long solution time. The RRT algorithm based on random sampling has the problems of purposeless search, node redundancy, and the formed path does not meet the vehicle turning angle constraint. Summary of the invention

[0003] The purpose of the present invention is to provide a path planning method combining an improved RRT and an artificial potential field method based on vehicle turning angle constraints, thereby improving the efficiency of the traditional RRT algorithm and improving the smoothness and safety of the driving path of an autonomous driving vehicle.

[0004] To achieve the above object, the present invention provides a path planning method combining an improved RRT and an artificial potential field method based on vehicle turning angle constraints, comprising the following steps:

[0005] Step 1: Introduce bias probability P for sampling;

[0006] Step 2: Adding gravitational components to random trees based on the artificial potential field method;

[0007] Step 3: Establish a repulsive field around the obstacle based on the artificial potential field method;

[0008] Step 4: Based on the gravitational component, the repulsive field selects the sampling points as nodes and expands them with a dynamic step size to obtain a rough solution path;

[0009] Step 5: Perform secondary turning angle constraints based on the nodes on the rough solution path to obtain nodes that meet the vehicle turning requirements;

[0010] Step 6: The final path is formed based on B-spline curve fitting.

[0011] Optionally, during the execution of step 1, first, a bias probability P needs to be defined to control the degree of bias of the target point during the sampling process. P needs to be adjusted according to the actual situation and cannot be too large or too small. If it is too large, it will cause the potential optimal path to be missed, and if it is too small, it will reduce the search path and increase the path length. Based on the random number function, a random number between 0 and 1 is obtained, and it is compared with the defined bias probability P. If it is less than the bias probability, the sampling will use the target point as the sampling point. If it is greater than the bias probability, random sampling will be performed. The specific expression is as follows:

[0012]

[0013] Among them, P is the bias probability, rand is a random tree between 0 and 1, and X rand is the sampling point generated by random sampling, X goal is the sampling point obtained by sampling with the target point as the sampling point, X sample The final generated sampling points.

[0014] Optionally, during the execution of step 2, a gravitational component is added to the random tree based on the artificial potential field method. The core is to introduce a gravitational component into each newly generated node, thereby changing the resultant force in the growth direction, controlling the random tree to grow toward the target node, and reducing randomness. The specific expression is as follows:

[0015] F(n)=R(n)+G(n)

[0016]

[0017] Among them, F(n) is the function of determining new nodes in the random tree search process, G(n) is the gravitational function of the target node, R(n) is the random growth function of the new node, ρ is the step size, g is the gravitational gain coefficient, and x goal is the target position vector, |x goal -x near | for node x goal and node x near The absolute value of the geometric distance;

[0018] Optionally, during the execution of step 3, a repulsive field is established around the obstacle based on the artificial potential field method, and the concept of obstacle repulsion in the artificial potential field method is introduced into the target bias RRT algorithm to guide the local random tree to grow away from the obstacle. The core is to introduce a repulsive component to the new nodes generated during the growth and expansion of the local random tree. The specific expression is as follows:

[0019] F(n)=R(n)+T(n)

[0020]

[0021] Where F(n) is the function for determining new nodes in the random tree search process, R(n) is the random growth function of new nodes, T(n) is the repulsion function of obstacles, ρ is the step size, k rep is the gravitational gain coefficient, p(x) is the shortest distance from a random node to an obstacle, p0 is the distance of influence of the obstacle on the node, and x obstacle is the position vector of the obstacle;

[0022] Optionally, during the execution of step 4, sampling points are selected as nodes based on the gravitational component and the repulsive field and expanded with a dynamic step size. The resultant force is calculated based on the parallelogram law according to the gravitational component and the repulsive component in the above expression and the randomly generated nodes, thereby obtaining new nodes, and calculating the distance between the random node and the adjacent node. If the distance is less than the set step size, the current distance is used as the temporary step size. If the distance is greater than the step size, the step size remains unchanged as the initially defined step size. The specific expression is as follows:

[0023] U(n)=R(n)+G(n)+T(n)

[0024] Among them, U(n) is the direction vector of the resultant force, R(n) is the random growth function of the new node, G(n) is the gravitational function of the target node, and T(n) is the repulsive function of the obstacle.

[0025] Optionally, during the execution of step 4, the final resultant force vector U(n) is obtained, and the dynamic step length p tem , then the new node expansion equation expression is as follows:

[0026]

[0027] Among them, x new is the new extended node, x near is the tree node closest to the random sampling point, p tem is the dynamic step length, and U(n) is the final resultant force vector.

[0028] After step 4 is completed, the updated new node is placed in the search tree. When the target point is found, the rough solution path is obtained by backtracking the nodes of the search tree and connecting them. Step 5 is executed to determine whether the nodes on the path meet the characteristics of vehicle steering, namely the angle constraint, and the nodes and path are updated according to the angle constraint.

[0029] Optionally, the expression for the first rotation angle in step 5 is as follows:

[0030] x pn =x near -x parent

[0031] x ns =x sample -x near

[0032]

[0033] Among them, x parent For x near The parent node, x near is the distance x sample The closest point, θ is the angle between the two vectors. If θ satisfies the first rotation constraint, then x sample If it meets the requirements, keep the node; if not, reselect x sample Until all x that meet the requirements are found sample , update the rough solution path to obtain a path that satisfies the first corner constraint.

[0034] Optionally, during the execution of step 5, after the rough solution path is updated to obtain a path that satisfies the first transition constraint, select x parent and x near The midpoint x1, x near and x sample The midpoint x2 of x1 and x2 is then selected. point , calculate x according to the expression in claim 8 parent , x point , x sample The angle formed is used to determine whether the second rotation angle constraint is satisfied. If so, x is used. point Replace x near Update the path again.

[0035] Optionally, during the execution of step 6, strict requirements are imposed on the continuity of the path finally updated in step 5 at adjacent nodes, in order to ensure the stability of the vehicle during movement. The cubic B-spline curve, with its unique characteristic of achieving second-order continuity at the node vector, perfectly meets the requirements for the continuity of speed and acceleration when the vehicle moves. The B-spline curve function uses a variety of mathematical methods such as interpolation, approximation and fitting to finely process the uneven parts in the path planning, making it smoother and in line with actual driving conditions.

[0036] The present invention provides a path planning method combining an improved RRT based on vehicle turning angle constraint and an artificial potential field method. First, a bias probability P is introduced in the random tree expansion process of the RRT algorithm; then, a gravitational component is added to the random tree based on the artificial potential field method, and then a repulsive field is established around the obstacle based on the artificial potential field method; points are selected based on the gravitational component and the repulsive field, and expanded with a dynamic step size to obtain a rough solution path; secondary turning angle constraints are performed based on the nodes on the rough solution path to obtain nodes that meet the vehicle steering requirements; finally, the new nodes after the constraints are updated and fitted based on the B-spline curve smooth path to form a final path. The present invention overcomes the problem that the artificial potential field method is prone to fall into local optimality, and has obvious optimization effects in terms of time, path length, number of iterations and path smoothness of the RRT global path planning algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] 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.

[0038] Figure 1 It is a flow chart of a path planning method combining improved RRT based on vehicle turning angle constraint and artificial potential field method according to the present invention.

[0039] Figure 2 It is a schematic diagram of the gravitational component and the repulsive component of the present invention.

[0040] Figure 3 It is a schematic diagram of two corner constraints of the present invention. DETAILED DESCRIPTION

[0041] 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.

[0042] The following are some nouns and terminology descriptions of the present invention:

[0043] RRT: Rapid-exploration Random Tree, fast random search tree.

[0044] See also Figure 1 The present invention provides a path planning method combining an improved RRT and an artificial potential field method based on vehicle turning angle constraints, comprising the following steps:

[0045] S1: introduce bias probability P for sampling;

[0046] S2: Adding gravitational components to random trees based on the artificial potential field method;

[0047] S3: Establish a repulsive field around the obstacle based on the artificial potential field method;

[0048] S4: Based on the gravitational component, the repulsive field selects the sampling points as nodes and expands them with a dynamic step size to obtain a rough solution path;

[0049] S5: Perform secondary turning angle constraints based on the nodes on the rough solution path to obtain nodes that meet the vehicle turning requirements;

[0050] S6: forming a final path based on B-spline curve fitting.

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

[0052] Step S1: introducing bias probability P for sampling;

[0053] Specifically, first, a bias probability P needs to be defined to control the degree of bias of the target point during the sampling process. P needs to be adjusted according to the actual situation and cannot be too large or too small. Too large will cause the potential optimal path to be missed, while too small will reduce the search path and increase the path length. Based on the random number function, a random number between 0 and 1 is obtained, and it is compared with the defined bias probability P. If it is less than the bias probability, the target point will be used as the sampling point for this sampling. If it is greater than the bias probability, random sampling will be performed. The specific expression is as follows:

[0054]

[0055] Among them, P is the bias probability, rand is a random tree between 0 and 1, and X rand is the sampling point generated by random sampling, X goal is the sampling point obtained by sampling with the target point as the sampling point, X sample The final generated sampling points.

[0056] Step S2: adding a gravitational component to the random tree based on the artificial potential field method;

[0057] Specifically, the gravitational component is added to the random tree based on the artificial potential field method. The core is to introduce the gravitational component into each newly generated node, thereby changing the resultant force in the growth direction, controlling the random tree to grow toward the target node, and reducing randomness. The specific expression is as follows:

[0058] F(n)=R(n)+G(n)

[0059]

[0060] Among them, F(n) is the function of determining new nodes in the random tree search process, G(n) is the gravitational function of the target node, R(n) is the random growth function of the new node, ρ is the step size, g is the gravitational gain coefficient, and x goal is the target position vector, |x goal -x near | for node x goal and node x near The absolute value of the geometric distance;

[0061] Step S3: establishing a repulsive field around the obstacle based on the artificial potential field method;

[0062] Specifically, the concept of obstacle repulsion in the artificial potential field method is introduced into the target bias RRT algorithm to guide the local random tree to grow away from obstacles. The core is to introduce repulsion components into the new nodes generated during the growth and expansion of the local random tree. The specific expression is as follows:

[0063] F(n)=R(n)+T(n)

[0064]

[0065] Where F(n) is the function for determining new nodes in the random tree search process, R(n) is the random growth function of new nodes, T(n) is the repulsion function of obstacles, ρ is the step size, k rep is the gravitational gain coefficient, p(x) is the shortest distance from a random node to an obstacle, p0 is the distance of influence of the obstacle on the node, and x obstacle is the position vector of the obstacle;

[0066] Step S4: Based on the gravitational component, the repulsive field selects the sampling points as nodes and expands them with a dynamic step size to obtain a rough solution path;

[0067] For details, please refer to Figure 2 The schematic diagram of introducing gravitational component and repulsive component is shown in Figure 1. Based on the gravitational component and repulsive field, sampling points are selected as nodes and expanded with dynamic step size. According to the gravitational component and repulsive component in the above expression and the randomly generated nodes, the resultant force is calculated based on the parallelogram law to obtain new nodes. The distance between the random node and the adjacent node is calculated. If the distance is less than the set step size, the current distance is used as the temporary step size. If the distance is greater than the step size, the step size remains unchanged as the initially defined step size. The specific expression is as follows:

[0068] U(n)=R(n)+G(n)+T(n)

[0069] Among them, U(n) is the direction vector of the resultant force, R(n) is the random growth function of the new node, G(n) is the gravitational function of the target node, and T(n) is the repulsive function of the obstacle.

[0070] Get the final resultant force vector U(n), dynamic step length p tem , then the new node expansion equation expression is as follows:

[0071]

[0072] Among them, x new is the new extended node, x near is the tree node closest to the random sampling point,

[0073] p tem is the dynamic step length, and U(n) is the final resultant force vector.

[0074] After step 4 is completed, the updated new node is placed in the search tree. When the target point is found, the rough solution path is obtained by backtracking the nodes of the search tree and connecting them.

[0075] Step S5: performing secondary turning angle constraints based on the nodes on the rough solution path to obtain nodes that meet the vehicle turning requirements;

[0076] For details, please refer to Figure 3 Schematic model of secondary rotation constraint.

[0077] The expression for the first rotation angle in step 5 is as follows:

[0078] x pn =x near -x parent

[0079] x ns =x sample -x near

[0080]

[0081] Among them, x parent For x near The parent node, x near is the distance x sample The closest point, θ is the angle between the two vectors. If θ satisfies the first rotation constraint, then x sample If it meets the requirements, keep the node; if not, reselect x sample Until all x that meet the requirements are found sample , update the rough solution path to obtain a path that satisfies the first corner constraint.

[0082] During the execution of step 5, after updating the rough solution path to obtain a path that satisfies the first transition constraint, select x parent and x near The midpoint x1, x near and x sample The midpoint x2 of x1 and x2 is then selected. point , calculate x according to the expression in claim 8 parent , x point , x sample The angle formed is used to determine whether the second rotation angle constraint is satisfied. If so, x is used. point Replace x near Update the path again.

[0083] Step S6: forming a final path based on B-spline curve fitting.

[0084] Specifically, the second-order continuity of the cubic B-spline curve at the node vector meets the requirement of velocity and acceleration continuity when the vehicle moves, and the cubic uniform B-spline function is used to smooth the path through interpolation, approximation and fitting.

[0085] 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. Ordinary technicians in this field 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 path planning method based on vehicle turning angle constraint and combining improved RRT and artificial potential field method, characterized in that: The following steps are involved: Step 1: Introduce bias probability P for sampling; Step 2: Adding gravitational components to random trees based on the artificial potential field method; Step 3: Establish a repulsive field around the obstacle based on the artificial potential field method; Step 4: Based on the gravitational component, the repulsive field selects the sampling points as nodes and expands them with a dynamic step size to obtain a rough solution path; Step 5: Perform secondary turning angle constraints based on the nodes on the rough solution path to obtain nodes that meet the vehicle turning requirements; Step 6: The final path is formed based on B-spline curve fitting.

2. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: During the execution of step 1, first, a bias probability P needs to be defined to control the degree of bias of the target point during the sampling process. P needs to be adjusted according to the actual situation and cannot be too large or too small. Too large will cause the potential optimal path to be missed, and too small will reduce the search path and increase the path length. Based on the random number function, a random number between 0 and 1 is obtained, and it is compared with the defined bias probability P. If it is less than the bias probability, the sampling will use the target point as the sampling point. If it is greater than the bias probability, random sampling will be performed. The specific expression is as follows: Among them, P is the bias probability, rand is a random tree between 0 and 1, and X rand is the sampling point generated by random sampling, X goal is the sampling point obtained by sampling with the target point as the sampling point, X sample The final generated sampling points.

3. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: During the execution of step 2, a gravitational component is added to the random tree based on the artificial potential field method. The core is to introduce a gravitational component into each newly generated node, thereby changing the resultant force in the growth direction, controlling the random tree to grow toward the target node, and reducing randomness. The specific expression is as follows: F(n)=R(n)+G(n) Among them, F(n) is the function of determining new nodes in the random tree search process, G(n) is the gravitational function of the target node, R(n) is the random growth function of the new node, ρ is the step size, g is the gravitational gain coefficient, and x goal is the target position vector, |x goal -x near | for node x goal and node x near The absolute value of the geometric distance.

4. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: During the execution of step 3, a repulsive field is established around the obstacle based on the artificial potential field method. The concept of obstacle repulsion in the artificial potential field method is introduced into the target bias RRT algorithm to guide the local random tree to grow away from obstacles. The core is to introduce a repulsive component to the new nodes generated during the growth and expansion of the local random tree. The specific expression is as follows: F(n)=R(n)+T(n) Where F(n) is the function for determining new nodes in the random tree search process, R(n) is the random growth function of new nodes, T(n) is the repulsion function of obstacles, ρ is the step size, and k rep is the gravitational gain coefficient, p(x) is the shortest distance from a random node to an obstacle, p0 is the distance of influence of the obstacle on the node, and x obstacle is the position vector of the obstacle.

5. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: During the execution of step 4, sampling points are selected as nodes based on the gravitational component and repulsive field and expanded with dynamic step length. According to the gravitational component and repulsive component in the above expression and the randomly generated nodes, the resultant force is calculated based on the parallelogram law to obtain new nodes. The distance between the random node and the adjacent node is calculated. If the distance is less than the set step length, the current distance is used as the temporary step length. If the distance is greater than the step length, the step length remains unchanged as the initially defined step length. The specific expression is as follows: U(n)=R(n)+G(n)+T(n) Among them, U(n) is the direction vector of the resultant force, R(n) is the random growth function of the new node, G(n) is the gravitational function of the target node, and T(n) is the repulsive function of the obstacle.

6. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: During the execution of step 4, the final resultant force vector U(n) is obtained, and the dynamic step length p tem , then the new node expansion equation expression is as follows: Among them, x new is the new extended node, x near is the tree node closest to the random sampling point, p tem is the dynamic step length, and U(n) is the final resultant force vector.

7. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 1, characterized in that: After step 4 is completed, the updated new node is placed in the search tree. When the target point is found, the rough solution path is obtained by backtracking the nodes of the search tree and connecting them. Step 5 is executed to determine whether the nodes on the path meet the characteristics of vehicle steering, namely the angle constraint, and the nodes and path are updated according to the angle constraint.

8. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 7, characterized in that: The expression for the first rotation angle in step 5 is as follows: x pn =x near -x parent x ns =x sample -x near Among them, x parent For x near The parent node, x near is the distance x sample The closest point, θ is the angle between the two vectors. If θ satisfies the first rotation constraint, then x sample If it meets the requirements, keep the node; if not, reselect x sample Until all x that meet the requirements are found sample , update the rough solution path to obtain a path that satisfies the first corner constraint.

9. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 8, characterized in that: During the execution of step 5, after updating the rough solution path to obtain a path that satisfies the first transition constraint, select x parent and x naar The midpoint x1, x near and x sample The midpoint x2 of x1 and x2 is then selected. point , calculate x according to the expression in claim 8 parent , x point , x sample The angle formed is used to determine whether the second rotation angle constraint is satisfied. If so, x is used. point Replace x near Update the path again.

10. The path planning method combining improved RRT and artificial potential field method based on vehicle turning angle constraint as claimed in claim 9, characterized in that: During the execution of step 6, strict requirements are imposed on the continuity of the path finally updated in step 5 at adjacent nodes, in order to ensure the stability of the vehicle during movement. The cubic B-spline curve, with its unique characteristic of achieving second-order continuity at the node vector, perfectly meets the requirements for the continuity of speed and acceleration when the vehicle moves. The B-spline curve function uses a variety of mathematical methods such as interpolation, approximation and fitting to finely process the uneven parts in path planning, making it smoother and in line with actual driving conditions.

Images