CN111207767A

CN111207767A - Vehicle planning algorithm improved based on RRT algorithm

Info

Publication number: CN111207767A
Application number: CN202010104879.XA
Authority: CN
Inventors: 吴迪; 郑亚楠; 王欣; 胡顺奇
Original assignee: Dalian University of Technology
Current assignee: Dalian University of Technology
Priority date: 2020-02-20
Filing date: 2020-02-20
Publication date: 2020-05-29
Anticipated expiration: 2040-02-20
Also published as: CN111207767B

Abstract

A vehicle planning algorithm based on RRT algorithm improvement belongs to the field of path planning and comprises two stages, wherein a certain number of guide root nodes are found in the first stage. Firstly, knowing the number and distribution of obstacles on a map, a starting node and a target node, a set of guide root nodes is obtained through calculation and collision detection, and the guide root nodes are distributed on a path of the starting node pointing to the target node, have guidance and do not collide with surrounding obstacles. And in the second stage, the starting node, the target node and the guide root node are put into a root node set, the nodes are expanded one by one, the growth of the tree is guided, and when the nearest distance between the leaf nodes of the two trees is smaller than or equal to the step length, the two trees are merged into one tree. When all the trees are merged into one tree, namely the tree contains the starting node and the target node at the same time, the path finding is successful. The invention improves the original RRT path planning algorithm, so that the path can be quickly searched and quickly converged under the complex road condition.

Description

Vehicle planning algorithm improved based on RRT algorithm

Technical Field

The invention belongs to the field of path planning, relates to improvement of a rapid-expansion random Tree algorithm (RRT), and particularly relates to a vehicle planning algorithm MT-RRT based on RRT algorithm improvement.

Background

In recent years, the development of unmanned technology is very rapid, and path planning is also widely concerned and developed as an important technology. The path planning of the vehicle is to plan a path which can pass through without colliding with an obstacle for the vehicle from a starting position to a target position in a complex environment. The types of the current path planning algorithms are various, and the common algorithms mainly comprise an ant colony algorithm, an a-star algorithm, an artificial potential field method, a genetic algorithm, an RRT algorithm and the like.

There are also a great variety of improvements on RRT algorithms, and Urmson et al propose an RRT algorithm with node bias probability, making the expansion more purposefully enhance the utilization of nodes. Frazzoil proposes an RRT algorithm with progressive optimality, and adds a random geometric figure and a pruning algorithm optimization theory on the basis of RRT node expansion based on uniform sampling to ensure that nodes of a random tree can converge to a current optimal value, but the calculation amount of the nodes is large. Besides, there are SRRT algorithm, RRT-Connect algorithm, etc. The improvement of the RRT algorithm is respectively made in different degrees from whether the sampling of random points is uniform, whether the step length is dynamically changed and the extension direction, so that the performance of the algorithm is improved. Yet another improvement to the RRT algorithm is mainly through fusion with other path planning algorithms, such as with the a-algorithm, and with the artificial potential field algorithm. The MT-RRT algorithm provided by the article calculates a certain number of root nodes to generate a local spanning tree, so that the guidance of the algorithm is improved, the utilization rate of the nodes is enhanced, and the convergence speed of the algorithm is accelerated.

Disclosure of Invention

Aiming at the problems of low node utilization rate, high region search repetition degree, lack of destination and low convergence rate of the RRT algorithm, the invention provides an improved MT-RRT algorithm based on the RRT algorithm, and the method is called as a Multi-direction-based fast-expanding Random Tree (MT-RRT). That is, by calculating a certain number of collision-free guide root nodes on the target path, a plurality of local spanning trees are generated and finally merged into one tree to find a passable path.

In order to achieve the purpose, the invention adopts the technical scheme that:

a vehicle planning algorithm MT-RRT improved based on RRT algorithm is used for solving the problem of vehicle path planning, namely, a collision-free path is planned from a starting position to a target position, and the method comprises the following steps:

the method comprises the following steps: knowing the number and the distribution condition of the obstacles, the starting node and the target node, judging whether the distance from the starting node to the target node is less than the step length and whether the collision with the obstacles occurs: and if the distance is smaller than the step length and no collision occurs with surrounding obstacles, directly connecting the starting node with the target node, and then directly connecting the starting node with the target node to finish the path finding. Otherwise, calling the method for calculating the root node (collisionZ (x)_init,x_goalMap) method) to get a certain number of root nodes to put into the set. The method comprises the following specific steps:

(1.1) Call collisionZ (x)_init,x_goalMap) method, inputting the target node of the initial node, the number of obstacles on the Map, and the distribution condition. Connecting the originating nodes x_initAnd a target node x_goalAnd obtaining a straight line l, and calculating the slope and intercept of the straight line.

(1.2) setting a discontinuity value Deltax₁From x_initStart to x_goalOn the abscissa at intervals of Δ x₁Starts to detect a collision with a point on the straight line l, and calculates collision center points (x) of n colliding obstacles_m1,x_m2,x_m3...x_mn) Each collision center is respectively made into a perpendicular line (l) of a straight line l₁,l₂,l₃...l_n) Setting a discontinuity value Deltax on each vertical line₂At Δ x₂Performing collision detection for the interval to obtain the firstThe collision-free points are placed in a set newRoot of bootstrap root nodes.

Step two: will start node x_initTarget node x_goalAnd placing the guide Root node in the new Root into a tree list TreeList, and calling a method for generating a plurality of growing trees (a more Tree (step, TreeList, Map) method) in a program to expand each tree in the TreeList in a while loop, wherein the method needs to input a step, the Root node list TreeList and Map-related information and guides the growth of the plurality of trees until the trees are connected into one tree. The method comprises the following specific steps:

(2.1) calculating the number m of trees in the TreeList, and traversing each tree in the TreeList from 1 to m by using a for loop.

(2.2) traversing the current tree T_numThen, go through m trees except the current tree T_numEach node in other trees obtains the sequence number T of the tree nearest to the current tree_{_near}And a tree T_{_near}T _ n of the nearest node in.

(2.3) randomly generating a random sample point, setting a threshold value Bias (0< Bias <1), expanding the newly generated node towards the nearest tree with target Bias, carrying out collision detection on the new node, and adding the new node into the tree if no collision exists.

And (2.4) judging the new node and the root node of the nearest tree, performing collision detection, and merging the two trees if the distance is smaller than the step size. All nodes in the latest tree are added to the current tree, the latest tree is deleted from the list of trees, and the current for loop is exited.

(2.5) in a while loop, calculating whether the number of trees in a TreeList list of the trees is 1, if so, ending the loop, and searching a path from a starting node to a target node according to nodes in the trees; if not 1, the next iteration is continued, and the step 2.1 is returned.

The invention has the beneficial effects that: the improved MT-RRT algorithm based on the RRT algorithm provided by the invention has the advantages that the number of nodes for random sampling is lower, the average speed of convergence is higher, and the path is shorter. The method has good practical significance, not only further improves the efficiency of vehicle path planning, but also can obtain good experimental effect under the conditions of facing complex environments, such as dense obstacles, passing through narrow passages and the like.

Drawings

FIG. 1 is a flow chart of the present invention.

Fig. 2 is a specific process of finding a root node according to the present invention.

FIG. 3 shows the comparison experiment results of the RRT algorithm and the invention in the running time.

FIG. 4 is a comparison experiment result of the total number of nodes between the RRT algorithm and the present invention.

Detailed Description

The following provides a detailed description of the implementation process of the present invention with reference to the technical solutions and the accompanying drawings.

The invention discloses a vehicle path planning algorithm improved based on an RRT algorithm, which mainly comprises two stages, wherein a certain number of guide root nodes are found in the first stage. Firstly, knowing the number and distribution of obstacles on a map, a starting node and a target node, a set of guide root nodes is obtained through calculation and collision detection, and the guide root nodes are distributed on a path of the starting node pointing to the target node, have guidance and do not collide with surrounding obstacles. And in the second stage, the starting node, the target node and the guide root node are put into a root node set, the nodes are expanded one by one, the growth of the tree is guided, and when the nearest distance between the leaf nodes of the two trees is smaller than or equal to the step length, the two trees are merged into one tree. When all the trees are merged into one tree, that is, the tree contains both the starting node and the target node, the path finding is successful. The general flow chart of the MT-RRT algorithm is shown in the attached figure 1.

First, the map-related information including the size and shape of the obstacle is known (the maximum radius r of the obstacle is mainly known)_i) Number, distribution, and start node startNode and end node. Generally, the types of obstacles on a map are classified into a small number of large obstacles, dense obstacles, narrow passages, and the like.

Firstly, whether the distance from a starting node to a target node is smaller than a step length or not is judged, and a collision detection method is called to judge whether collision with surrounding obstacles occurs or not. If no collision occurs and the step size is smaller than the step size, the starting node and the target node are directly connected, and the routing is completed (if the generation of the root node is not guided, the program is ended). Otherwise, calling a colletion Z (startNode (1,1:2), endNode (1,1:2) and Map) method for calculating the guide root node to obtain a set of the guide root node, wherein the method comprises the following specific steps:

(1.1) calling collaisionZ (startNode (1,1:2), endNode (1,1:2) and Map method, inputting starting node startNode (1,1:2), target node endNode (1,1:2) and Map related information Map. And connecting the starting node and the target node to obtain a straight line l, and calculating the slope and intercept of the straight line.

(1.2) setting a discontinuity value Deltax₁0.5, starting from startNode (1,1:2) to endNode (1,1:2) at every Δ x on the abscissa₁Starts collision detection for a point on the straight line l with the distance of (c) as a unit, and calculates collision center points (x) of n colliding obstacles_m1,x_m2,x_m3...x_mn) Each collision center is respectively made into a perpendicular line (l) of a straight line l₁,l₂,l₃...l_n) Setting a discontinuity value Deltax on each vertical line₂＝r_i/4(r_iMaximum radius of the obstacle) by Δ x₂Collision detection is performed for intervals, a first collision-free point is obtained and is placed into the set of the guide root nodes, and the specific implementation process is shown in fig. 2.

And secondly, putting the starting node startNode (1,1:2), the target node endNode (1,1:2) and the obtained guide root node into a root node list TreeList to obtain n +2 root nodes. Next, setting success flag success equal to 0, when the while loop satisfies success equal to 0, calling a moreTree (step, TreeList, Map) method, inputting step, root node list TreeList, and Map related information Map, and guiding the growth of multiple local spanning trees until the local spanning trees are connected into one tree. The moreTree method comprises the following specific steps:

(2.1) when a more Tree (step, TreeList, Map) method is called each time, the number m of trees in the current TreeList is recalculated, and each tree in the TreeList is traversed from 1 to m by using a for loop.

(2.2) traversing the current tree T_numThen, a near _ TREE (TreeList, num) method is called, which traverses each node in the other TREEs except the current TREE to obtain the TREE sequence number T NEAREST to the current TREE_{_near}And T _ n of the node closest to the new node.

(2.3) setting a threshold value Bias to be 0.5, when the value of the random number is less than or equal to the threshold value, randomly generating a random sample point randPoint, otherwise, taking the random sample point as the nearest point T _ n of the nearest tree. Therefore, the newly generated nodes can be expanded towards the nearest tree with target bias, collision detection is carried out on the new nodes, and the nodes are added into the tree without collision.

And (2.4) carrying out collision detection on the new node and the root node of the nearest Tree, and calling a Tree _ merge (TreeList, num, T _ near) method to merge two trees if the distance is smaller than the step length. And adding the node of the latest tree into the current tree, deleting the latest tree in the list of the trees, and exiting the current for loop.

(2.5) in a while loop, calculating whether the number of the trees in a list TreeList of the trees is 1, if so, judging success to be 1, ending the loop, and calling a findPath method to carry out routing; if not 1, the next iteration is continued, and the more Tree (step, TreeList, Map) method is called continuously.

The method is improved on the basis of the original route planning algorithm RRT algorithm, and the results of simulation experiments on maltab are shown in the attached figures 3 and 4. Fig. 3 is a comparison result of the running time of the RRT algorithm and the running time of the MT _ RRT algorithm on the same map and the same start node and target node, and fig. 4 is a comparison result of the total number of generated nodes of the RRT algorithm and the total number of generated nodes of the MT _ RRT algorithm. The MT-RRT algorithm effectively reduces the selection of random nodes by a method of pre-selecting a certain number of guide root nodes with guidance to generate a plurality of random trees, so that the tree expansion is more purposeful, repeated exploration on areas is reduced, the convergence rate of the trees is accelerated, and the method is also suitable for complex environments.

The above-mentioned embodiments only express the embodiments of the present invention, but not should be understood as the limitation of the scope of the invention patent, it should be noted that, for those skilled in the art, many variations and modifications can be made without departing from the concept of the present invention, and these all fall into the protection scope of the present invention.

Claims

1. A vehicle planning algorithm MT-RRT based on RRT algorithm improvement is characterized by comprising the following steps:

the method comprises the following steps: knowing the number and the distribution condition of the obstacles, the starting node and the target node, judging whether the distance from the starting node to the target node is less than the step length and whether the collision with the obstacles occurs: if the distance is smaller than the step length and no collision occurs with surrounding obstacles, the starting node and the target node are directly connected, and the starting node and the target node are directly connected at the moment, so that the path finding is completed; otherwise, calling a method for calculating the guide root nodes to obtain a certain number of guide root nodes, and putting the guide root nodes into the set; the method comprises the following specific steps:

(1.1) inputting target nodes of the starting nodes, the number of obstacles on a map and the distribution condition; connecting the originating nodes x_initAnd a target node x_goalObtaining a straight line l, and calculating the slope and intercept of the straight line;

(1.2) setting a discontinuity value Deltax₁From x_initStart to x_goalOn the abscissa at intervals of Δ x₁Starts to detect a collision with a point on the straight line l, and calculates collision center points (x) of n colliding obstacles_m1,x_m2,x_m3...x_mn) Each collision center is respectively made into a perpendicular line (l) of a straight line l₁,l₂,l₃...l_n) Setting a discontinuity value Deltax on each vertical line₂At Δ x₂Performing collision detection for intervals to obtain a first collision-free point, and putting the first collision-free point into a newRoot set of guide root nodes;

step two: will start node x_initTarget node x_goalThe method calls a method for generating a plurality of growing trees in a program to expand each tree in the TreeList in while circulation, wherein the method needs to input step length, the Root node list TreeList and map related information to guide the growing of the plurality of trees until the trees are connected into a tree; the method comprises the following specific steps:

(2.1) calculating the number m of trees in the TreeList, and traversing each tree in the TreeList from 1 to m by using a for loop;

(2.2) traversing the current tree T_numThen, go through m trees except the current tree T_numEach node in other trees obtains the sequence number T of the tree nearest to the current tree_{_near}And a tree T_{_near}T _ n of the nearest node in;

(2.3) randomly generating a random sample point, setting a threshold value Bias, expanding a newly generated node towards a nearest tree with target Bias, carrying out collision detection on the new node, and adding the node into the tree if no collision exists;

(2.4) judging the new node and the root node of the nearest tree and carrying out collision detection, and merging the two trees if the distance is smaller than the step length; adding all nodes in the latest tree into the current tree, deleting the latest tree from the list of the tree, and exiting the current for loop;

(2.5) in a while loop, calculating whether the number of trees in a TreeList list of the trees is 1, if so, ending the loop, and searching a path from a starting node to a target node according to nodes in the trees; if not, the next iteration is continued, and the step (2.1) is returned.

2. The improved vehicle planning algorithm MT-RRT based on RRT algorithm of claim 1, wherein in step (2.3), 0< threshold Bias < 1.