A RRT path planning method based on space constraint and dynamic heuristic probability

Abstract

The application relates to a RRT path planning method based on space constraints and dynamic heuristic probability, which comprises the following steps: acquiring map binary information of space according to perception information to perform modeling; performing focal length expansion on an ellipse based on planning start and end point coordinates to limit and optimize a global sampling domain; dynamically and adaptively generating a heuristic probability according to an expansion degree of an RRT tree towards a target point, determining an expansion search mode based on the heuristic probability, performing global search, and realizing path planning; traversing all nodes of the RRT tree based on the planning start and end point coordinates to obtain characteristic points of a feasible path, and performing pruning optimization processing according to a position relationship of the characteristic points in a planning space to obtain a pruned path; and smoothing the pruned path based on a quadratic programming method to obtain a path satisfying motion constraints of a research object. Compared with the prior art, the application has the advantages of being capable of reducing the generation of redundant branches and invalid nodes, guaranteeing that the RRT algorithm has strong exploration capability, and improving planning efficiency, and the like.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving vehicle path planning, in particular to a RRT path planning method based on space constraints and dynamic heuristic probability. BACKGROUND

[0002] Path planning is one of the key technologies for autonomous planning and obstacle avoidance in the field of automatic driving, and path planning technology has become a hot direction for current automatic driving companies and academia. Rapid-exploration Random Tree (RRT) algorithm is an incremental random sampling algorithm, which has high efficiency in planning. Its basic strategy is to obtain feature points by random sampling in the planning space, and to obtain a feasible path connecting the starting point and the ending point based on the feature point information. It is widely used in the motion planning research of automatic driving vehicles.

[0003] However, the RRT algorithm has strong randomness, poor stability and low search efficiency due to its random sampling strategy. Patent No. CN113485375A discloses an indoor environment robot exploration method based on heuristic bias sampling. This method generates sampling points by setting a target probability bias formula, guides the robot to navigate to the target point and updates the map. Patent No. CN113359775A discloses a dynamic variable sampling region RRT unmanned vehicle path planning method. This method uses a probability target bias formula to improve the blindness of sampling to generate new nodes, and considers the maximum turning angle constraint of vehicle motion, and uses a cubic B-spline for optimization. Patent No. CN115390559A discloses a path planning method based on Informed-RRT* fusion constraint sampling space and artificial potential field. The target bias strategy is designed to obtain the parameters of the initial elliptical sampling domain from the first planning result. The final path is obtained by repeated iteration. Patent No. CN113219998A discloses a vehicle path planning method based on improved bidirectional informed-RRT. This method obtains an initial path by bidirectional RRT*, and obtains an elliptical sampling set from the initial path. The above methods can improve the uncertainty of sampling by designing a target bias strategy, but they all use a fixed bias probability. The setting of the bias probability will directly affect the generalization and timeliness of RRT. A high bias probability will reduce the exploration ability of the algorithm, and a low bias probability will make the algorithm lack of target. Moreover, the method of obtaining the initial path to determine the parameters of the elliptical optimization sampling domain will increase the complexity of the algorithm. SUMMARY

[0004] The present application aims to provide a RRT path planning method based on space constraint and dynamic heuristic probability, which overcomes the defects of single probability bias and redundant planning space of existing planning technology, and improves the planning efficiency and path quality.

[0005] The present application aims to provide a RRT path planning method based on space constraint and dynamic heuristic probability, which overcomes the defects of single probability bias and redundant planning space of existing planning technology, and improves the planning efficiency and path quality.

[0006] A RRT path planning method based on space constraint and dynamic heuristic probability, comprising the following steps:

[0007] Step 1) obtaining the space of the environment based on the perception information, modeling the space by the value information; Figure Two

[0008] Step 2) based on the planning start and end point coordinates, performing ellipse focal length expansion, and optimizing the global sampling domain limit;

[0009] Step 3) dynamically and adaptively generating heuristic probability according to the expansion degree of the RRT tree towards the target point, determining the expansion search mode based on the heuristic probability, performing global search, and realizing path planning;

[0010] Step 4) based on the planning start and end point coordinates, traversing all nodes of the RRT tree to obtain the characteristic points of the feasible path, and performing pruning optimization processing according to the position relationship of the characteristic points in the planning space, to obtain the pruned path;

[0011] Step 5) based on the quadratic programming method, smoothing the pruned path to obtain the path satisfying the motion constraint of the research object.

[0012] The step 1) comprises the following steps:

[0013] Step 1-1) obtaining the spatial position information of the environment and obstacle information based on the perception system;

[0014] Step 1-2) classifying and processing the feasible domain and the obstacle domain of the space, and performing binary modeling, so that the pixel point 0 represents the feasible domain, and the pixel point 1 represents the obstacle or boundary domain.

[0015] The step 2) comprises the following steps:

[0016] Step 2-1) determining the planning start and end point coordinates as q start and q goal , respectively;

[0017] Step 2-2) optimizing the focal length parameter 2c of the sampling region to be the Euclidean distance between the planning start and end points;

[0018] Step 2-3) to avoid unnecessary waste of space and computing resources caused by initial pre-planning, the long axis parameter of the sampling domain is obtained by focal length expansion, i.e. a=kc, and the specific parameters are as follows: ​

[0019]

[0020] a = k c, k > 1

[0021]

[0022] where the start and end points are denoted as q start (x start ,y start ), q goal (x goal ,y goal ), a, b, c are structure parameters, and k is the expansion ratio.

[0023] The step 3) comprises the following steps:

[0024] Step 3-1) Set the RRT expansion tree to store the following four state information: the position coordinates (q x , q y ) of the tree node, the order n of the tree node in the RRT tree, and the order n parent of the parent node of the tree node;

[0025] Step 3-2) Design and construct a dynamic adaptive heuristic probability function for determining an adaptive heuristic probability, so as to meet the requirements of ensuring the expansion ability and generalization of the RRT in the early expansion stage with a small heuristic probability, accelerating the convergence speed and improving the planning timeliness near the end point with a large heuristic probability;

[0026] Step 3-3) Calculate the expansion degree of the expansion tree, which is described by the relative distance between the new node generated by the last expansion and the end point, and obtain the heuristic probability p dynamic at the moment based on the expansion degree and the dynamic heuristic probability function p dynamic = f(x p ), wherein f(x p ) is the dynamic adaptive heuristic probability function, and x p represents the expansion degree;

[0027] Step 3-4) Generate a random probability p rand between 0 and 1 by using a random variable function rand;

[0028] Step 3-5) Determine the expansion search mode based on the relationship between p dynamic and p rand , and perform the search;

[0029] Step 3-6) Repeat steps 3-3) to 3-5) until the path planning is successful.

[0030] The step 3-5) is specifically:

[0031] If p dynamic < p rand , generate a random point coordinate (x rand ,y rand ) in the planning space, find a nearest point coordinate (x near ,y near ) in the RRT tree, search a new node coordinate (x new ,y new ) by the vector direction from the nearest point to the random point, if the new node is collision-free in the space domain, add the new node to the RRT tree, otherwise return to step 3-4); if p dynamic ≥ p rand , replace the random sampling point with q goal to generate and search.

[0032] The dynamic adaptive heuristic probability function is specifically:

[0033]

[0034] Wherein, A and B are function adjustment parameters, and satisfy: A∈[0, 1], B∈[1, ∞).

[0035] The extension degree is represented as:

[0036]

[0037] Wherein, q newlast represents a new node generated by the last search of the search tree, q start and q goal respectively represent the start and end point coordinates of the planning.

[0038] The step 4) comprises the following steps:

[0039] Step 4-1) traverses all nodes of the RRT tree to obtain characteristic points of the feasible path:

[0040] [(x start ,y start ),(x2,y2),…,(x n-1 ,y n-1 ),(x goal ,y goal )]

[0041] Wherein, the coordinate (x n-1 ,y n-1 ) is the parent node of the coordinate (x n ,y n ), q start (x start ,y start ), q goal (xgoal , y goal ) represents the start and end points;

[0042] Step 4-2) pruning and short-circuiting the path planning result based on the principle of the triangle inequality, deleting the redundant branches between any two nodes.

[0043] The step 5) comprises the following steps:

[0044] Step 5-1) comprehensively considering the lateral coordinate y, the first-order derivative of the lateral coordinate and the second-order derivative of the lateral coordinate determine the optimization target vector, and solve the optimization target vector based on the quadratic programming method to smooth the path;

[0045] Step 5-2) longitudinally equidistantly discretizing the planning result after the quadratic programming method smoothing, to obtain the final path planning result.

[0046] The objective function of the quadratic programming method is:

[0047]

[0048] s.t.l≤CY≤u

[0049] In the formula, Y is a 3n×1 optimization target column vector, Q matrix is a semi-positive definite matrix, P is a 3n×1 column vector, u and l are upper and lower boundaries of constraints, respectively, and C is a boundary constraint matrix.

[0050] Compared with the prior art, the present application has the following beneficial effects:

[0051] (1) The present application adopts dynamic adaptive heuristic probability to guide the optimization sampling distribution, ensures that the algorithm has strong generalization ability, dynamically decides the heuristic bias probability according to the RRT tree planning degree, improves the efficiency of the algorithm, uses small probability in the early stage of algorithm sampling to ensure the exploration ability and generalization of the algorithm, increases the probability near the planning end point to quickly converge, and improves the efficiency of the algorithm.

[0052] (2) The present application obtains an optimized sampling domain according to the pre-inflation of the planning start point and the end point, reduces the number of invalid planning and the generation of invalid nodes, improves the timeliness and algorithm efficiency, and finally smooths the planning path according to the motion constraint of the vehicle, so that the path has higher feasibility. BRIEF DESCRIPTION OF DRAWINGS

[0053] Figure 1 is a flow chart of the method of the present application;

[0054] Figure 2 is a schematic diagram of the feasible region of the planning space;

[0055] Figure 3 For optimizing the parameter structure diagram of the elliptical domain;

[0056] Figure 4 For the dynamic heuristic probability function schematic diagram;

[0057] Figure 5 For the schematic diagram of the scene planning result in an embodiment. DETAILED DESCRIPTION

[0058] The present application will be described in detail below in conjunction with the drawings and specific embodiments. The embodiments are implemented on the premise of the technical solutions of the present application, and detailed implementation manners and specific operation processes are given, but the protection scope of the present application is not limited to the following embodiments.

[0059] The embodiment provides a RRT path planning method based on space constraints and dynamic heuristic probability, as shown in the following steps: Figure 1

[0060] Step 1) According to the perception information, the space is modeled by obtaining the digital information of the space. Figure Two

[0061] Step 1-1) Based on the perception system, the spatial position information of the environment and obstacle information is obtained;

[0062] Step 1-2) The feasible domain and obstacle domain of the space are classified and processed and binarized modeling, with pixel point 0 representing the feasible domain and pixel point 1 representing the obstacle or boundary domain.

[0063] Step 2) Based on the planning start and end point coordinates, the focal length of the ellipse is expanded, and the optimization processing of the global sampling domain limit is performed.

[0064] Step 2-1) The planning start and end point coordinates are determined as q start and q goal ;

[0065] Step 2-2) The focal length parameter 2c of the sampling region is optimized to be the Euclidean distance between the planning start and end points;

[0066] Step 2-3) In order to avoid unnecessary space and calculation resource waste caused by initial pre-planning, the long axis parameter of the sampling domain is obtained by focal length expansion, that is, a=kc, and the specific parameters are as follows:

[0067]

[0068] a=kc, k>1

[0069]

[0070] In the formula, the start and end points are represented as q start ​​(x start ,y start ), q goal (x goal ,y goal ), a, b, c are structure parameters, and k is the expansion ratio.

[0071] The feasible region of the embodiment is shown in Fig. 1, and the structure diagram of the optimized elliptical region is shown in Fig. 2. Figure 2 Figure 3

[0072] Step 3) A heuristic probability is dynamically and adaptively generated according to the expansion degree of the RRT tree towards the target point, and an expansion search mode is determined based on the heuristic probability, so that the global search is performed and the path planning is realized.

[0073] Step 3-1) The RRT expansion tree stores the following four state information: the position coordinates (q x , q y ) of the tree node, the order n of the tree node in the RRT tree, and the order n parent of the parent node.

[0074] Step 3-2) A dynamic adaptive heuristic probability function is designed and constructed to determine the adaptive heuristic probability, so that a smaller heuristic probability is used in the initial expansion (near the starting point) to ensure the expansion ability and generalization of the RRT, and a larger heuristic probability is used near the terminal point (target point) to enhance the sampling target and accelerate the convergence speed, thereby improving the planning timeliness.

[0075] As shown in Fig. 3, the dynamic adaptive heuristic probability function of the embodiment is: Figure 4

[0076]

[0077] Wherein, A and B are function adjustment parameters, and satisfy: A∈[0,1], B∈[1,∞).

[0078] Step 3-3) The expansion degree of the expansion tree is calculated, the expansion degree is described by the relative distance between the new node generated by the last expansion and the terminal point, and the heuristic probability p dynamic at this moment is obtained based on the expansion degree and the dynamic heuristic probability function, wherein p dynamic =f(x p ), f(x p ) is the dynamic adaptive heuristic probability function, and x p represents the expansion degree:

[0079]

[0080] Wherein, q newlast represents the new node generated by the last search of the search tree, and q​​​start and q goal represent the planning start and end point coordinates respectively.

[0081] Step 3-4) Generate a random probability p between (0,1) using the random variable function rand rand .

[0082] Step 3-5) Based on the relationship between p dynamic and p rand , decide to expand the search mode and search, specifically:

[0083] If p dynamic < p rand , generate a random point coordinate (x rand , y rand ) in the planning space, find the nearest point coordinate (x near , y near ) in the RRT tree, and search in the direction of the vector from the nearest point to the random point to get the new node coordinate (x new , y new ). If the new node is collision-free in the spatial domain, add the new node to the RRT tree, otherwise return to step 3-4); if p dynamic ≥ p rand , replace the random sampling point with q goal and search.

[0084] Step 3-6) Repeat steps 3-3) to 3-5) until the path planning is successful.

[0085] Step 4) Based on the planning start and end point coordinates, traverse all nodes of the RRT tree to obtain the characteristic points of the feasible path, and perform pruning optimization processing according to the positional relationship of the characteristic points in the planning space to obtain the pruned path.

[0086] Step 4-1) Traverse all nodes of the RRT tree to obtain the characteristic points of the feasible path:

[0087] [(x start , y start ), (x2, y2), …, (x n-1 , y n-1 ), (x goal , y goal )]

[0088] Where (x n-1 , y n-1 ) is the parent node of (x n , y n ), q start (x start , y start ), q goal (xgoal , y goal ) represents the start and end points;

[0089] Step 4-2) pruning and short-circuiting the path planning result based on the principle of the triangle inequality, deleting the redundant branches between any two nodes.

[0090] Step 5) smoothing the pruned path based on the quadratic programming method to obtain a path that satisfies the motion constraints of the research object.

[0091] Step 5-1) considering the lateral coordinate y, the first derivative of the lateral coordinate and the second derivative of the lateral coordinate to determine the optimization target vector:

[0092]

[0093] and based on the quadratic programming method to solve the optimization target vector and smooth the path, wherein the objective function of the quadratic programming method is:

[0094]

[0095] s.t.l≤CY≤u

[0096] In the formula, Y is a 3n x 1 optimization target column vector, Q matrix is a semi-positive definite matrix, P is a 3n x 1 column vector, u and l are the upper and lower boundaries of the constraint respectively, and C is the boundary constraint matrix.

[0097] Step 5-2) longitudinally and equidistantly discretizing the planning result after smoothing by the quadratic programming method, and the discrete step length is δ s to obtain the final path planning result.

[0098] Figure 5 The figure is a planning result of the present application in a certain scene, and it can be seen that the present application can well realize obstacle avoidance and path planning.

[0099] The above describes the preferred embodiments of the present application in detail. It should be understood that those skilled in the art can make many modifications and changes without creative labor according to the concept of the present application. Therefore, any technical solution obtained by logical analysis, reasoning or limited experiments by those skilled in the art on the basis of the prior art according to the concept of the present application should be within the protection scope determined by the claims.

Claims

1. A path planning method for Restricted Response Time (RRT) based on spatial constraints and dynamic heuristic probability, characterized in that, Includes the following steps: Step 1) Obtain spatial map binarization information based on the perceived information and perform modeling; Step 2) Perform elliptical focal length dilation based on the coordinates of the planning start and end points, and optimize the global sampling domain constraints; Step 3) Dynamically and adaptively generate heuristic probabilities based on the extent of the RRT tree's expansion toward the target point, determine the expansion search mode based on the heuristic probabilities, perform a global search, and realize path planning; Step 4) Based on the coordinates of the start and end points of the planning, traverse all nodes of the RRT tree to obtain the feature points of the feasible path, and perform pruning optimization based on the positional relationship of the feature points in the planning space to obtain the pruned path; Step 5) Smooth the pruning path based on the quadratic programming method to obtain a path that satisfies the motion constraints of the research object; Step 3) includes the following steps: Step 3-1) Configure the RRT extended tree to store the following 4 pieces of state information: the position coordinates of the tree nodes ( The order of tree nodes in the RRT tree n and the order of its parent nodes. ; Step 3-2) Design and construct a dynamic adaptive heuristic probability function to determine the adaptive heuristic probability. This function ensures the scalability and generalization of the RRT by using a smaller heuristic probability in the early stages of expansion, and accelerates the convergence speed and improves the planning efficiency by using a larger heuristic probability near the endpoint. Step 3-3) Calculate the extent of expansion of the tree, which is described by the relative distance between the new node generated by the last expansion and the endpoint. Obtain the heuristic probability at this moment based on the extent of expansion and the dynamic heuristic probability function. ,in, , For dynamic adaptive heuristic probability function, Indicates the degree of expansion; the degree of expansion is expressed as: in, This represents the new node generated by the final search in the search tree. and These represent the coordinates of the start and end points of the plan, respectively. The dynamic adaptive heuristic probability function is specifically as follows: in, A , B Adjust the parameters of the function to satisfy: ; Steps 3-4) Use the random variable function rand to generate random probabilities between (0,1). ; Steps 3-5) Based on and Expand the search pattern for relational decision-making and perform the search; Steps 3-6) Repeat steps 3-3) to 3-5) until the path planning is successful.

2. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 1, characterized in that, Step 1) includes the following steps: Step 1-1) Obtain spatial location information of the environment and obstacles based on the sensing system; Steps 1-2) Classify and binarize the feasible region and obstacle region of the space, and use pixel 0 to represent the feasible region and pixel 1 to represent the obstacle or boundary region.

3. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 1, characterized in that, Step 2) includes the following steps: Step 2-1) Determine the coordinates of the starting and ending points of the plan, respectively represented as follows: and ; Step 2-2) Optimize the focal length parameters of the sampling area. c To plan the Euclidean distance between the start and end points; Steps 2-3) To avoid unnecessary waste of space and computational resources caused by initial pre-planning, the major axis parameter of the sampling domain is obtained from focal length dilation, i.e. a = kc The specific parameters are as follows; In the formula, the start and end points are represented as: , , a, b, c For structured parameters, k The expansion ratio is denoted as .

4. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 1, characterized in that, Steps 3-5) specifically refer to: like The coordinates of random points are generated within the planned space. Find the coordinates of the nearest neighbor in the RRT tree. The coordinates of the new node are obtained by moving from the nearest point towards the vector direction of the random point. If the new node has no conflict in the spatial domain, add the new node to the RRT tree; otherwise, return to steps 3-4. ,by Instead of generating random sampling points, a search is performed.

5. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 1, characterized in that, Step 4) includes the following steps: Step 4-1) Traverse all nodes of the RRT tree to obtain the feature points of feasible paths: Where, coordinates coordinates The parent node, , Represents the starting and ending points; Step 4-2) Based on the principle of triangle inequality, perform pruning and short-circuiting on the path planning results, and remove redundant branches between any two nodes.

6. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 1, characterized in that, Step 5) includes the following steps: Step 5-1) Take into account the horizontal coordinates of the path y First derivative of the horizontal coordinate and the second derivative of the horizontal coordinate The optimization objective vector is determined, and the optimization objective vector is solved based on the quadratic programming method to smooth the path. Step 5-2) Discretize the planning results after smoothing by the quadratic programming method at equal distances in the longitudinal direction to obtain the final path planning result.

7. The RRT path planning method based on spatial constraints and dynamic heuristic probability according to claim 6, characterized in that, The objective function of the quadratic programming method is: In the formula, Y It is 3 n The optimization objective column vector is ×1. , Q The matrix is ​​a positive semi-definite matrix. P 3 n A column vector of size 1, u , l These are the upper and lower boundaries, respectively. C This is the boundary constraint matrix.

Images