A research method of improved RRT path planning based on secondary rotation angle constraint

Abstract

The application relates to the technical field of global path planning of automatic driving vehicles, in particular to a research method of improved RRT path planning based on secondary corner constraints, which improves the biased RRT algorithm, samples the space constraint of the environment map based on the planning starting point and the ending point, prunes the sampling range of the map space, and then obtains the random sampling point in real time based on the biased sampling of the probability P; then the selection of the random sampling point is updated based on the vehicle collision size and the obstacle collision detection; then the first steering constraint is performed based on the random point to obtain the extended node update tree and the rough solution path; the tree node is updated based on the secondary corner constraint of the discrete method, and the node meeting the smoothness and the vehicle steering is obtained; finally, the discrete path is updated based on the B-spline curve smooth path. Through fitting adjustment, the application improves the path smoothness and the planning speed of the RRT global path planning algorithm.

Description

Technical Field

[0001] This invention relates to the field of global path planning technology for autonomous vehicles, and specifically to a research method for improved RRT path planning based on quadratic turning angle constraints. Background Technology

[0002] Path planning for autonomous vehicles involves making behavioral decisions based on the vehicle's real-time surrounding environment and the location of the driving target. It provides information on drivable path areas and assists in driving. Path planning for autonomous vehicles can be divided into two categories based on their ability to understand the surrounding environment: the first is global path planning, which is based on verified environmental information; the second is local path planning, which is based on sensor information. Global path planning requires a known map database of the autonomous vehicle and uses feedback and optimization methods to plan drivable areas and optimal paths, combined with the vehicle's target location. Based on different principles, global path planning can be categorized into graph search algorithms such as A* and its variants (D*, Dijkstra's algorithm); random sampling algorithms such as Rapid-exploration Random Tree (RRT) and its variants (RRT*, Monte Carlo random sampling); and intelligent algorithms such as deep learning algorithms and ant colony algorithms. While graph search-based algorithms can find relatively good paths, their high computational complexity, susceptibility to local minima due to the influence of heuristic function settings, and the presence of path inflections often prevent them from providing paths that meet the vehicle's turning requirements. This necessitates additional post-processing methods to smooth discrete points, further increasing the algorithm's speed. While intelligent algorithms such as deep learning can effectively obtain optimal paths, their computational complexity often leads to excessively long solution times, making them unsuitable for global path planning algorithms. The Randomized Randomized Search (RRT) algorithm, a stochastic search algorithm, can quickly find global paths, but it is prone to problems such as redundant node expansion, aimless search direction, and paths that do not match the vehicle's turning angles. Summary of the Invention

[0003] The purpose of this invention is to provide a research method for improved RRT path planning based on quadratic turning angle constraints, based on the improvement of the bias RRT algorithm and the strategy to ensure safe driving of vehicles, thereby improving the smoothness and safety of the driving path of autonomous vehicles.

[0004] To achieve the above objectives, this invention provides a research method for improved RRT path planning based on quadratic rotation constraints, comprising the following steps:

[0005] Step 1: Based on the planning start and end points, constrain the sampling and pruning of the environmental map spatial sampling range;

[0006] Step 2: Obtain real-time random sampling points based on the bias probability P;

[0007] Step 3: Update the selection of random sampling points based on vehicle collision size and obstacle collision detection;

[0008] Step 4: Based on the random points, perform the first turning constraint to obtain the nodes to be expanded and update the random tree to obtain the coarse solution path;

[0009] Step 5: Apply secondary rotation constraints to the tree nodes based on the discrete method and update the tree nodes to obtain nodes that meet the requirements of smoothness and vehicle steering.

[0010] Step 6: Update the road curvature smoothness based on the B-spline curve fitting of discrete point paths.

[0011] Optionally, during the execution of step 1, when setting the start and end points in the environment map, a fixed-size sampling point range is clipped based on the positions of the start and end points, and the location of the current unclipped point set in the original map is marked using the point association method between the clipped map and the original map. The updated environment map is then output.

[0012] Optionally, during the process of obtaining real-time random sampling points based on the bias probability P, a random function is selected with a threshold probability to determine the random sampling points or plan the target points, so that the random tree can find the target node more quickly. The expression is as follows:

[0013]

[0014] Where P is the bias probability, p threshold X represents the threshold probability, Rand represents randomly generated sampling points, and X represents the threshold probability. goal As the endpoint, X sample These are the final selected random sampling points;

[0015] Optionally, the execution process of step 3 includes the following steps:

[0016] Step 3.1: Define the vehicle dimension expansion circle:

[0017]

[0018] Where δ is the relaxation factor, L is the vehicle length, and B is the vehicle width;

[0019] Step 3.2: Detect whether the vehicle's size expansion sampling points will collide with obstacles. If a collision with an obstacle is detected, perform random point resampling.

[0020] Define obstacle detection and collision detection methods:

[0021]

[0022] Among them l left For the left-offline straight line, l right The line is offset to the right, and the flag is the collision flag.

[0023] Step 3.3: Use the random points after obstacle detection as output to update the nodes to be expanded; otherwise, resample the random points.

[0024] Optionally, the steering constraint equation in step 4 is expressed as follows:

[0025]

[0026] Among them, (x n ,y n ) is X nearest The coordinates, (x p ,y p ) is X sample The coordinates, (x r ,y r ) is X sample coordinates X parent θ is a vector and The included angle.

[0027] Optionally, the nodal expansion equation expression for the new expanded node is as follows:

[0028] X new =X nearest +l×[sin(α) cos(α)]

[0029] Among them, X new As a new expansion node, X nearest Let l be the tree node closest to the random sampling point, and let l be the random sampling point and X. nearest The Euclidean distance, α is the distance between X and X. nearest The angle between the sampled point and the random sampling point.

[0030] Optionally, during the execution of step 4, the updated new node is placed into the search tree. When the target point is searched, by backtracking each node of the search tree, a coarse solution path that satisfies the first turning constraint threshold is obtained, which is the discrete point path where the vehicle has no collision with obstacles and the turning angle meets the first constraint threshold.

[0031] The selection expression for updating the tree node in step 5 is as follows:

[0032]

[0033] In the formula, i = (1, 2, ..., n-1), N is the number of discrete points, m1 and m2 represent the unit vectors between the i-th point and the (i-1)-th point and the (i+1)-th point, respectively, and n j (j = 1, 2, ..., N) and n k (k = 2N, 2N-1, ..., N-1) represents the location information of discrete points.

[0034] Optionally, in step 6, continuity is required at adjacent nodes on the original path. The second-order continuity of the cubic B-spline curve at the node vector satisfies the requirement for continuous speed and acceleration when the vehicle is moving. The B-spline curve function acts on the path planning to smooth the process through interpolation, approximation and fitting, thereby improving the smoothness of the path.

[0035] This invention provides an improved RRT path planning method based on quadratic turning constraints. By improving the bias RRT algorithm, it prunes the map spatial sampling range based on the spatial constraints of the environment map, according to the planning start and end points. Then, it obtains random sampling points in real time based on bias sampling with probability P. Next, it updates the selection of random sampling points based on vehicle collision size and obstacle collision detection. Then, it performs a first turning constraint based on the random points to obtain an updated tree with expanded nodes, obtaining a coarse solution path. A second turning constraint is applied to the tree nodes using a discrete method, and the tree nodes are updated to obtain nodes that are smooth and conform to vehicle turning. Finally, the discrete path is updated based on B-spline curve smoothing. After fitting and adjustment, this invention improves the path smoothness and planning speed of the RRT global path planning algorithm. Attached Figure Description

[0036] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0037] Figure 1 This is a flowchart illustrating a research method for an improved RRT path planning based on quadratic rotation constraints according to the present invention.

[0038] Figure 2 This is a schematic diagram of spatial pruning of the environmental map according to the present invention.

[0039] Figure 3 This is a schematic diagram of the vehicle collision handling method of the present invention.

[0040] Figure 4 This is a schematic diagram of obstacle collision detection according to the present invention.

[0041] Figure 5 This is a schematic diagram of the first angle constraint limitation of the present invention.

[0042] Figure 6 This is a schematic diagram of the secondary rotation angle limitation of the present invention. Detailed Implementation

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

[0044] The following are some of the terms and definitions used in this invention:

[0045] RRT: Rapid-exploration Random Tree.

[0046] Please see Figure 1 This invention provides a research method for improved RRT path planning based on quadratic turning angle constraints, comprising the following steps:

[0047] S1: Spatial sampling range of environmental map constrained by the planning start and end points;

[0048] S2: Obtain real-time random sampling points based on bias probability P;

[0049] S3: Update the selection of random sampling points based on vehicle collision size and obstacle collision detection;

[0050] S4: Based on the random point, perform the first turning constraint to obtain the node to be expanded and update the random tree to obtain the coarse solution path;

[0051] S5: Based on the discrete method, perform secondary rotation constraints on the tree nodes and update the tree nodes to obtain nodes that meet the requirements of smoothness and vehicle steering.

[0052] S6: Update road curvature smoothness based on B-spline curve fitting of discrete point paths.

[0053] The following provides further explanation in conjunction with the specific implementation steps:

[0054] In step S1, for details please refer to [link / reference]. Figure 2The provided environmental map spatial pruning model, when sending a request for a start point and an end point to the environmental map, prunes a sampling point range of a certain size based on the positions of the start point and the end point, and uses a point association method between the pruned map and the original map to mark the position of the current uncropped point set in the original map. The updated environmental map is then output as the sampling area.

[0055] Step S2: Obtain real-time random sampling points based on the bias probability P;

[0056] Specifically, during the propagation of sampling points in the random sampling algorithm, a random sampling function is set to obtain the bias probability. Based on the bias probability and the probability threshold, the location information of the real-time random sampling points is obtained, enabling the random tree to find the target node more quickly, thereby improving the path search speed and providing path search guidance.

[0057]

[0058] Where P is the bias probability, p threshold X represents the threshold probability, Rand represents randomly generated sampling points, and X represents the threshold probability. goal As the endpoint, X sample These are the final selected random sampling points;

[0059] Step S3: Update the selection of random sampling points based on vehicle collision size and obstacle collision detection.

[0060] For details, please refer to Figure 3 as well as Figure 4 The provided schematic diagram of vehicle expansion processing and obstacle detection method model define the vehicle size expansion circle:

[0061]

[0062] Where δ is the relaxation factor, L is the vehicle length, and B is the vehicle width;

[0063] Then, by detecting whether the vehicle's size expansion sampling points will collide with obstacles, if a collision with an obstacle is detected, random point resampling is performed.

[0064] Define obstacle detection and collision detection methods:

[0065]

[0066] Among them l left For the left-offline straight line, l right The line is offset to the right, and the flag is the collision flag.

[0067] Random point X sample Its nearest point X nearestA safe offset distance is used to construct straight lines with left and right offsets. If neither line passes through an obstacle, the random point X is considered to be... sample With the nearest point X nearest If the connection between the two nodes does not collide, the random point that meets the conditions after obstacle detection is used as the output and updated as the node to be expanded; otherwise, random point resampling is performed.

[0068] Step S4: Based on the random points, perform the first turning constraint to obtain the nodes to be expanded and update the random tree to obtain the coarse solution path;

[0069] For details, please refer to Figure 5 Steering constraint diagram, defining steering constraint equations:

[0070]

[0071] Among them, (x n ,y n ) is X nearest The coordinates, (x p ,y p ) is X sample The coordinates, (x r ,y r ) is X sample coordinates X parent θ is a vector and The included angle.

[0072] Set the rotation threshold θ in the free space of the pruning environment map. threshold1 The distance from the random sampling point X is obtained using Euclidean distance. sample Recent X nearest X nearest The parent node is X parent Calculate the turning angle between the three points based on the equation. If θ is satisfied... threshold1 Obtain a random point X that meets the first turning constraint. sample If the turning constraint is not met, the random points are resampled.

[0073] The nodal expansion equation for the newly expanded node is expressed as follows:

[0074] X new =X nearest +l×[sin(α) cos(α)]

[0075] Among them, X new As a new expansion node, X nearest Let l be the tree node closest to the random sampling point, and let l be the random sampling point and X. nearest The Euclidean distance, α is the distance between X and X. nearest The angle between the sampled point and the random sampling point.

[0076] The updated extended nodes are added to the search tree to obtain a coarse solution path that satisfies the steering constraint, which is a discrete point path where the vehicle has no collision with obstacles and the turning angle meets the threshold.

[0077] Step S5: Apply secondary rotation constraints to the tree nodes based on the discrete method and update the tree nodes to obtain nodes that meet the requirements of smoothness and vehicle steering.

[0078] For details, please refer to Figure 6 The schematic model of the secondary rotation constraint defines the selection of new nodes:

[0079]

[0080] Where i = (1, 2, ..., n-1), N is the number of discrete points, m1 and m2 represent the unit vectors between the i-th point and the (i-1)-th point, and between the (i+1)-th point and the i-th point, respectively, and n j (j = 1, 2, ..., N) and n k (k = 2N, 2N-1, ..., N-1) represents the location information of discrete points.

[0081] By analyzing the current node X in the coarse solution path i respectively with X i-1 and X i+1 Discretize the line connecting the two points at equal intervals to obtain discrete points n1, n2, n3, and n4. Connect the two discrete points n1 and n4, and take the average of the two points to obtain X. point Calculate the included angle θ1 between the three points, and determine whether the threshold θ of the secondary steering angle constraint is satisfied. threshold2 If this condition is met, i.e., X point Replace X i Simply repeat the above steps. point The above equation is selected for determination.

[0082] Step S6: Update the road curvature smoothness based on B-spline curve fitting of discrete point paths;

[0083] Specifically, the second-order continuity of cubic B-spline curves at the node vectors satisfies the requirement for continuous velocity and acceleration when a vehicle is moving. B-spline functions act on smoothing path planning through interpolation, approximation, and fitting.

[0084] When k = 3 (order k + 1) and n = 3, the B-spline function is as follows:

[0085]

[0086]

[0087] Where 0 ≤ u ≤ 1, i = 0, 1, ..., k-1, P i For the i-th control point,

[0088] Finally, the mathematical expression for the basis function of the cubic uniform B-spline curve can be obtained as follows:

[0089]

[0090]

[0091] The above description discloses only one preferred embodiment of the present invention, and should not be construed as limiting the scope of the present invention. Those skilled in the art will understand that all or part of the processes of the above embodiments can be implemented, and equivalent changes made in accordance with the claims of the present invention are still within the scope of the invention.

Claims

1. A research method for improved RRT path planning based on quadratic rotation constraints, characterized in that, Includes the following steps: Step 1: Based on the planning start and end points, constrain the sampling and pruning of the environmental map spatial sampling range; Step 2: Obtain real-time random sampling points based on the bias probability P; Step 3: Update the selection of random sampling points based on vehicle collision size and obstacle collision detection; Step 4: Based on the random points, perform the first turning constraint to obtain the nodes to be expanded and update the random tree to obtain the coarse solution path; The steering constraint equation in step 4 is expressed as follows: in, for coordinates for coordinates for coordinates , For vectors The included angle; The extended equation for the new node is expressed as follows: in, For new expansion nodes, The tree node closest to the random sampling point. For random sampling points and European distance, for The angle between the sampled point and the random sampling point; During the execution of step 4, the updated new node is placed into the search tree. When the target point is searched, the coarse solution path that satisfies the first turning constraint threshold is obtained by backtracking each node of the search tree. This is the discrete point path where the vehicle has no collision with obstacles and the turning angle meets the first constraint threshold. Step 5: Apply secondary rotation constraints to the tree nodes based on the discrete method and update the tree nodes to obtain nodes that meet the requirements of smoothness and vehicle steering. The selection expression for updating the tree node in step 5 is as follows: Where, in the formula, , The number of discrete points. They represent the first Point and the The unit vector between points and the first Point and the Unit vector between points and Represents the location information of discrete points; Step 6: Update the road curvature smoothness based on the B-spline curve fitting of discrete point paths.

2. The research method for improved RRT path planning based on quadratic turning angle constraints as described in claim 1, characterized in that, During the execution of step 1, when setting the start and end points in the environment map, a fixed-size sampling point range is clipped based on the positions of the start and end points. The location of the current uncropped point set in the original map is marked using the point association method between the clipped map and the original map. The updated environment map is then output.

3. The research method for improved RRT path planning based on quadratic turning angle constraints as described in claim 2, characterized in that, In the process of obtaining real-time random sampling points based on the bias probability P, a random function is selected with a threshold probability to determine the random sampling points or plan the target points, so that the random tree can find the target node. The expression is as follows: in, For the bias probability, For the threshold probability, For randomly generated sampling points, As the endpoint, These are the final randomly selected sampling points.

4. The research method for improved RRT path planning based on quadratic turning angle constraints as described in claim 3, characterized in that, The execution process of step 3 includes the following steps: Step 3.1: Define the vehicle dimension expansion circle: in As a relaxation factor, For vehicle length, For vehicle width; Step 3.2: Detect whether the vehicle's size expansion sampling points will collide with obstacles. If a collision with an obstacle is detected, perform random point resampling. Define obstacle detection and collision detection methods: in A straight line offset to the left. This is a straight line offset to the right. This is a collision flag. Step 3.3: Use the random points after obstacle detection as output to update the nodes to be expanded; otherwise, resample the random points.

Images