Intelligent vehicle path searching method based on heuristic RRT*

Abstract

The invention relates to the technical field of vehicle path planning, in particular to an intelligent vehicle path searching method based on heuristic RRT *. A heuristic function is introduced into a traditional RRT * algorithm, so that the traditional RRT * algorithm has the tendency of tending to an end point when new nodes are developed, firstly, an empty tree is created on a known map, and a first new node is generated at a starting point to serve as an initial node; updating a grower, generating a new node, adding the new node into the tree after the new node is inspected to be qualified, randomly generating the new node again to be qualified if the new node is unqualified, and guiding the node construction process through a heuristic function; and then rewriting and rewiring the new nodes, and improving the node connection mode in the tree. Through improvement on a node growth mode, node generation does not only have a certain probability to randomly face an end point, but preferentially grows a dominant node after a heuristic function is considered. According to the method, the calculation efficiency is improved, the time complexity of calculation is reduced, and the path can be better searched on the known map.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle path planning, and in particular to an intelligent vehicle path search method based on heuristic RRT*. Background Art

[0002] Path planning is a classic problem for safe navigation in dynamic, dense and unknown environments. In order to ensure safe and efficient point-to-point navigation, an appropriate algorithm should be selected. Among the many path planning algorithms, the RRT* (Rapidly-exploring Random Tree Star) algorithm, as a sampling-based path planning algorithm, has good adaptability and scalability in unknown or dynamic environments. Therefore, it is widely used in robot path planning, game development, drone flight, and autonomous driving.

[0003] The RRT* algorithm is an improvement on the traditional RRT (Rapidly-exploring Random Tree) algorithm. The RRT algorithm constructs a random tree in free space by random sampling and incremental expansion to search for the path from the starting point to the target point. However, the RRT algorithm does not guarantee that the optimal path is found, and may contain more redundant nodes and corners, resulting in an unsmooth path and affecting the robot's motion performance.

[0004] To solve this problem, the RRT* algorithm introduces rewrite and reconnect to optimize the generated path. In the process of adding new nodes to the search tree, the RRT algorithm not only considers the nearest node as its parent node, but also considers all nodes in a neighborhood circle with the new node as the center and r1 as the optimization radius. In this neighborhood circle, the RRT* algorithm will find the point with the smallest path cost after connecting to the new node, and use it as the parent node of the new node; at the same time, the RRT* algorithm will let all nodes in another neighborhood circle with a radius of r2 consider whether changing their parent node to the new node makes the path better. In this way, not only is it guaranteed that the newly added node is locally optimal, but as the tree expands, the entire path will gradually approach the global optimal solution.

[0005] Although the RRT* algorithm has made significant progress in path planning, it still has some limitations. For example, the computational efficiency of the RRT* algorithm is relatively low because it needs to continuously perform collision detection and rewrite and relink operations, which may make it unsuitable for scenarios with high real-time requirements. In addition, the performance of the RRT* algorithm is sensitive to the selection of parameters such as step size and sampling strategy, and appropriate parameter adjustments are required. Summary of the invention

[0006] The purpose of the present invention is to provide an intelligent vehicle path search method based on heuristic RRT*, aiming to solve the technical problem that the traditional sampling-based path planning method cannot meet the real-time requirements when autonomous driving and drone navigation encounter complex and dynamic environments.

[0007] To achieve the above object, the present invention provides an intelligent vehicle path search method based on heuristic RRT*, comprising the following steps:

[0008] Step 1: Receive map information and initialize, and determine to create an empty tree and initial nodes according to the map information;

[0009] Step 2: Update the grower in the tree, generate new nodes by the grower, check the eligibility of the new nodes, and add them to the tree if they are qualified; if they are not qualified, randomly generate new nodes again until the new nodes are qualified;

[0010] Step 3: Rewrite and rewire the new nodes to change the way the nodes in the tree are connected;

[0011] Step 4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step 2 and continue to generate new nodes.

[0012] Optionally, the map in step 1 is an established map, which is fixed and unchanged. The map must have three members: a starting point, an end point, and an obstacle set. The obstacle set may be an empty set.

[0013] In step 1, nodes and trees are generated. The tree is a random tree. The node is the basic component of the random tree, representing the location of a feasible path. It has three private members: parent node pointer, node position, and heuristic function. The tree is both a collection of nodes and the commander of node growth. It determines which nodes are eligible for growth and has two private members: node set and grower.

[0014] The initial node is created at the starting point and added to the tree.

[0015] Optionally, the grower is a subset of the node set, and only the nodes in the set are qualified to grow new nodes, and has two private members: grower capacity and qualified node container;

[0016] The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are eligible for growth; the eligible node container is a container that stores pointers to nodes that are eligible for growth.

[0017] The heuristic function consists of four terms: starting cost term, end cost term, obstacle cost term, and curve smoothness term;

[0018] Optionally, the heuristic function _inferioritylevel is calculated as

[0019]

[0020] Where _inferioritylevel is the heuristic function value; _distoorigincost is the starting cost term, which is the path distance from the node to the starting point; is the cost term of curve smoothness, which is the node N i and the positions of all its previous parent nodes; _distoendcost is the end cost item, which is N i Position to end point P goal Manhattan distance; _obstaclecost is the obstacle cost term, which is N i A magnification of the minimum distance from the represented robot or vehicle outline to any obstacle outline.

[0021] Optionally, the grower further has two member functions, namely, an update grower function and a growth function;

[0022] The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large, except for the initial node N init After any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large. If there are multiple nodes with the same heuristic function value, the new node is always ranked before the old node.

[0023] The growth function is responsible for the generation of new nodes and also for detecting whether the nodes are qualified.

[0024] Optionally, the rewriting and rewiring functions in step 3 are member functions of the random tree, and the running steps are as follows:

[0025] Search all nodes in the tree, if the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle;

[0026] Rewrite: If array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope The node in the loop traverses the entire array;

[0027] Rewiring: If array NWithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

[0028] The present invention provides an intelligent vehicle path search method based on heuristic RRT*. A heuristic function is introduced into the traditional RRT* algorithm so that it has a tendency to tend toward the end point when developing new nodes. First, an empty tree is created on a known map, and the first new node is generated at the starting point as the initial node. Then, a grower is updated, and a new node is generated and added to the tree after checking the eligibility. If it is unqualified, it is randomly generated again until it is qualified. The process of constructing the node is guided by the heuristic function; then the new node is rewritten and rewired to improve the node connection method in the tree. Through the improvement in the node growth method, the node generation is not just randomly toward the end point with a certain probability, but the advantageous nodes are preferentially grown after considering the heuristic function. The present invention improves the calculation efficiency, reduces the time complexity of the calculation, and can better search for paths on a known map. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0030] Figure 1 It is a schematic diagram of the steps of an intelligent vehicle path search method based on heuristic RRT* of the present invention.

[0031] Figure 2 This invention is class N i Schematic diagram of the composition structure.

[0032] Figure 3 It is a schematic diagram of the composition structure of class T of the present invention.

[0033] Figure 4 It is a schematic diagram of the composition structure of the GrowthApparatus class of the present invention. DETAILED DESCRIPTION

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

[0035] See also Figure 1 The present invention provides an intelligent vehicle path search method based on heuristic RRT*, comprising the following steps:

[0036] S1: receiving map information and initializing, and determining to create an empty tree and an initial node according to the map information;

[0037] S2: Update the grower in the tree, generate new nodes by the grower, check the eligibility of the new nodes, and add them to the tree if they are qualified; if they are not qualified, randomly generate new nodes again until the new nodes are qualified;

[0038] S3: rewrite and rewire the new nodes, changing the way nodes in the tree are connected;

[0039] S4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step S2 and continue to generate new nodes.

[0040] The following is a detailed explanation of some variables or key functions in the specific embodiments and operation steps:

[0041] 1. Node construction and assignment

[0042] Class N i is a node, the subscript i distinguishes different nodes, N i There are three private members: parent node pointer, node position, and heuristic function. Each new node needs to be initialized. Figure 2 Schematic diagram of the composition of the node.

[0043] Constructing a node involves the following steps:

[0044] Step 1, assign the parent node pointer fathernode.

[0045] Step 2: Assign the node position carRWMidpoint.

[0046] Step 3, calculate the heuristic function inferiority level.

[0047] The parent node pointer stores the pointer of the parent node of the node, with an initial value of 0, except for the initial pointer N init The parent node pointer is 0, and the items of other nodes cannot be empty, so that they form a linked list for easy retrieval and printing of results.

[0048] The node position can be the position of the robot or the target vehicle. It needs to have two elements: Cartesian coordinate system coordinates and direction. In the verification program of the method of the present invention, its Cartesian coordinate system coordinates are the midpoint of the rear axle of the vehicle, and the calculation formula of direction_pose is

[0049]

[0050] Where k is the node N i The fifth-order polynomial curve fitted by the positions of its first five parent nodes is in N i The slope of the tangent line at the position, α is N i The angle between the vector formed by connecting the position of the node and its parent node and the unit vector of the tangent line pointing in the positive direction of the x-axis.

[0051] The calculation formula of the heuristic function _inferioritylevel is

[0052]

[0053] _inferioritylevel is the heuristic function value.

[0054] _distoorigincost is the origin cost item, which is the path distance from the node to the origin. Its calculation formula is:

[0055]

[0056] in For node N i The fitting curve distance from its parent node, which is N i and its first five parent nodes are fitted with a fifth-order polynomial, N i To its parent node N i′s fathernode The curve integral of the segment is calculated as

[0057]

[0058] Among them C 0 , C 1 , ..., C 5 N i and the coefficients of the fifth-order polynomial curve fitted to the positions of its first five parent nodes, x i N i′s fathernode The x-axis coordinates of the node position start at N i The 10th derivative of the x-coordinate of the position.

[0059] is the curve smoothness cost term, which is N i and all its previous parent nodes (i.e. N iThe position of the node in the linked list) and the standard deviation of the derivative of the curve fitted by a 5th-order polynomial. Where _rank is the position of the node in its linked list, representing how many parent nodes (N) there are before the node. init _rank is 0), _smoothnessSum is the sum of curve smoothness, and the calculation formula is

[0060]

[0061] in YesN i The sample standard deviation of the derivative of the fifth-order polynomial fitting curve at the positions of its first five parent nodes is calculated as follows:

[0062]

[0063] Where D 0 , D 1 , ..., D 5 N i and the derivative coefficients of the fifth-order polynomial curve fitted to the positions of its first five parent nodes, x i N i The x-axis coordinate of the parent node position starts at N i 1 / 50 of the x-axis coordinate of the position, N i and the mean of the derivatives of the fifth-order polynomial curve fitted to the positions of its first five parent nodes.

[0064] _distoendcost is the end cost term, which is N i Position to P goal The Manhattan distance (ManhattanDistance) is calculated as follows:

[0065]

[0066] in For node N i The x-axis coordinate of is the x-axis coordinate of the end point, For node N i The y-axis coordinate of is the y-axis coordinate of the end point.

[0067] _obstaclecost is the obstacle cost term, which is N i The amplification term of the minimum distance from the robot or vehicle outline represented to the outline of any obstacle is calculated as

[0068]

[0069] Where DOBmin N i The minimum distance from the represented robot or vehicle outline to any obstacle outline.

[0070] 2. Tree composition and member functions

[0071] See also Figure 3 , class T is a random tree, consisting of node N i In addition to being a collection of all nodes, it also has the functions of screening nodes eligible for growth, rewriting (Rewrite), and rewiring (Reconnect). Its constructor is empty (does nothing). It has 2 private members: node set (nodeset) and growth apparatus (GrowthApparatus). The node set is a container for storing all nodes. It is the only entity of all nodes when the algorithm is running. The growth apparatus contains pointers to nodes eligible for growth. Class T has 2 member functions (Member Function), namely Join () and Rewrite and Reconnect ().

[0072] The logic of the join function is relatively simple, which is to add the new node to the node set.

[0073]

[0074] The rewrite and rewire function is more complicated. Here are the steps for running the function:

[0075] Step 1: Search all nodes in the tree. If the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle.

[0076] Step 2, rewrite. If the array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope This node in the loop iterates through the entire array.

[0077] Step 3, rewiring. If array N WithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

[0078] The road cost of a node is recorded as The calculation formula is:

[0079]

[0080] Among them, _distoorigincost is the starting cost item, is the cost of curve smoothness.

[0081] 3. Composition and member functions of the grower

[0082] See also Figure 4 , the grower (GrowthApparatus) has two private members: the grower capacity (_GQCsize) and the qualified node container (_GrowthQualificationContainer). Its constructor is empty (does nothing). The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are qualified for growth. The method of the present invention is set to 500. The qualified node container is a container for storing pointers to nodes that are qualified for growth. The grower has two member functions, namely, update the grower (Refresh()) and grow (Grow()).

[0083] The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large (the smaller the heuristic function value of the node, the more advantageous it is), except N init After any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large, and if there are multiple nodes with the same heuristic function value, the new node is always ranked before the old node. The following are the steps for running the function:

[0084] Step 1: Create three subscripts of the container _GrowthQualificationContainer: high subscript high, low subscript low, and mid subscript mid. If the number of nodes stored in _GrowthQualificationContainer is less than _GQCsize, set high to be equal to the number of nodes stored in the container, otherwise set high to be equal to _GQCsize. Low is equal to 0.

[0085] Step 2, run the binary search loop. If the input node N of the function new satisfy

[0086] N new._inferioritylevel≤_GrowthQualificationContainer[mid]._inferioritylevel, let high equal mid, otherwise let low equal mid.

[0087] Step 3: When high>low+1 is satisfied, run the loop and go to step 2; otherwise, end the loop.

[0088] Step 4: N new Placed after the _GrowthQualificationContainer[high] item in the container.

[0089] Step 5: If the container capacity is greater than 500, it is considered overflow and the last item is deleted.

[0090] The growth function is responsible for the generation of new nodes. The generation rules are similar to the general RRT algorithm: sample a random point on the map, find the node closest to the random point in the qualified node container, let the node generate a new node in the direction of the random point and become the parent node of the new node. The distance between the new node and the old node is fixed as the growth step length (StepLength). The growth function also has the function of detecting whether the node is qualified. The following are the steps for running the function:

[0091] Step 1: Collect random points in M, whose coordinates are two random numbers (x rand ,y rand ).

[0092] Step 2: Find the node closest to the random point in the grower and assign it to the node N selected for growth when the function is run. select .

[0093] Step 3: Generate a new node. The position of the new node is N new .carRWMidpoint() is N select The position plus N select The unit vector of the vector connected with the position of the random point and the number of growth steps

[0094]

[0095] The parent node of the new node is N select , calculate the heuristic function value _inferioritylevel of the new node.

[0096] Step 4: Check the node. If N newIf the robot or vehicle outline it represents does not collide with obstacles, and the outline trajectory formed by it and its parent node does not collide with obstacles, then the node is qualified and the new node N is new As the function output; otherwise, clear N new Memory of the node. Returning 0 indicates that the generated node is unqualified or fails to generate.

[0097] In summary, compared with the traditional RRT* algorithm, the present invention improves the calculation efficiency, reduces the time complexity of the calculation, and can better search for paths on a known map.

[0098] 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. An intelligent vehicle path search method based on heuristic RRT*, characterized in that: The following steps are involved: Step 1: Receive map information and initialize, and determine to create an empty tree and initial nodes according to the map information; Step 2: Update the grower in the tree, generate new nodes by the grower, check the eligibility of the new nodes, and add them to the tree if they are qualified; if they are not qualified, randomly generate new nodes again until the new nodes are qualified; Step 3: Rewrite and rewire the new nodes to change the way the nodes in the tree are connected; Step 4: Determine whether the new node meets the end requirements. If so, record the node and output the linked list it is in as the result; if not, go to step 2 and continue to generate new nodes.

2. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 1, characterized in that: The map in step 1 is an established map, which is fixed and unchanged. The map must have three members: a starting point, an end point, and an obstacle set. The obstacle set can be an empty set. In step 1, nodes and trees are generated. The tree is a random tree. The node is the basic component of the random tree, representing the location of a feasible path. It has three private members: parent node pointer, node position, and heuristic function. The tree is both a collection of nodes and the commander of node growth. It determines which nodes are eligible for growth and has two private members: node set and grower. The initial node is created at the starting position and added to the tree.

3. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 2, characterized in that: The grower is a subset of the node set. Only nodes in the set are qualified to grow new nodes. It has two private members: grower capacity and qualified node container. The grower capacity is the number of nodes that the grower can accommodate, that is, the number of nodes in the tree that are eligible for growth; the eligible node container is a container that stores pointers to nodes that are eligible for growth.

4. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 3, characterized in that: The heuristic function consists of four terms: starting cost term, end cost term, obstacle cost term, and curve smoothness term; The calculation formula of the heuristic function _inferioritylevel is Where _inferioritylevel is the heuristic function value; _distoorigincost is the starting cost term, which is the path distance from the node to the starting point; is the cost term of curve smoothness, which is the node N i and the positions of all its previous parent nodes; _distoendcost is the end cost item, which is N i Position to end point P goal Manhattan distance; _obstaclecost is the obstacle cost term, which is N i A magnification of the minimum distance from the represented robot or vehicle outline to any obstacle outline.

5. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 4, characterized in that: The grower also has two member functions, namely, an update grower function and a growth function; The update grower function has the function of sorting all nodes in the tree according to the heuristic function value from small to large, except for the initial node N init After any new node is added to the tree, the update grower function will be run. The new node is inserted into the appropriate position of the grower using the binary sorting principle, so that its heuristic function value is sorted from small to large. If there are multiple nodes with the same heuristic function value, the new node is always ranked before the old node. The growth function is responsible for the generation of new nodes and also for detecting whether the nodes are qualified.

6. The intelligent vehicle path search method based on heuristic RRT* as claimed in claim 5, characterized in that: The rewriting and rewiring functions in step 3 are member functions of the random tree, and the running steps are as follows: Search all nodes in the tree, if the new node N new To any node N i The distance is less than the rewriting and rewiring range D scope , then N i Add to array N WithinScope middle; Rewrite: If array N WithinScope A node in becomes N new After the parent node, N new The road cost is smaller, then change N new The parent node is array N WithinScope The node in the loop traverses the entire array; Rewiring: If array N WithinScope The parent node of a node in is changed to N new After that, the road cost of the node is smaller, then change the array N WithinScope The parent node of this node is N new , looping through the entire array.

Images