The aim of this post is to understand the rapidly-exploring random tree and implement it in Matlab.
1 - Introduction
A rapidly exploring random tree (RRT) is an algorithm designed to efficiently search nonconvex, high-dimensional spaces by randomly building a space-filling tree. The tree is constructed incrementally from samples drawn randomly from the search space and is inherently biased to grow towards large unsearched areas of the problem. It widely used in autonomous robotic path planning.
2 - The Algorithm
RRTs were proposed as both a sampling algorithm and a data structure designed to allow fast searches in high-dimensional spaces in motion planning. RRTs are progressively built towards unexplored regions of the space from an initial configuration as shown in Figure 1.
Progressive construction of an RRT.
At every step a random $q\_rand$ configuration is chosen and for that configuration the nearest configuration already belonging to the tree $q\_near$ is found. For this a definition of distance is required (in motion planning, the euclidean distance is usually chosen as the distance measure). When the nearest configuration is found, a local planner tries to join $q\_near$ with qrand with a limit distance . If $q\_rand$ was reached, it is added to the tree and connected with an edge to $q\_near$. If $q\_rand$ was not reached, then the configuration $q\_new$ obtained at the end of the local search is added to the tree in the same way as long as there was no collision with an obstacle during the search. This operation is called the Extend step, illustrated in Figure 2. This process is repeated until some criteria is met, like a limit on the size of the tree.
Extend phase of an RRT.
Once the RRT has been built, multiples queries can be issued. For each query, the nearest configurations belonging to the tree to both the initial and the goal configuration are found. Then, the initial and final configurations are joined to the tree to those nearest configurations using the local planner and a path is retrieved by tracing back in the tree structure.
The key advantage of RRTs is that in their building process they are intrinsically biased towards regions with a low density of configurations. This can be explained by looking at the Voronoi diagrams at every step of the building process. The Voronoi regions of every node are larger when the area around that node has not been explored. This way, the probability of a configuration being sampled in an unexplored region are higher as larger Voronoi regions will be more likely to contain the sampled configuration. This has the advantage of naturally guiding the tree while just performing uniform sampling. Besides, the characteristics of the Voronoi diagram are an indicative of the adequateness of the tree: for example, a tree whose Voronoi diagram is formed by regions of similar size covers uniformly the search space, whereas large disparities in the size of the regions mean that the tree may have left big areas of the search space unexplored. Apart from this, another notable ch aracteristic is that RRTs are probabilistically complete, as they will cover the whole search space as the number of sampled configurations tends to infinity.[1]
2.1 - Adding new node
In the beginning, the graph is empty. We have only the starting and goal positions. The aim is to create random tree from the starting position to the goal position. First, we should create a random node and add it to the graph.
A simple iteration in performed in which each step attempts to extend the RRT by adding a new vertex that is biased by a randomly-selected state. The algorithm selects the nearest vertex already in the RRT to the given state by calculating the Euclidean distance.
Adding new vertex.
2.2 - Checking the delta
Another important process is checking the distance between new vertex and the vertex that is the closest one in RRT. Because the distance of the new vertex might be larger than the delta that is given. The lines between nodes cannot be larger than the delta value. If the distance is not larger than the delta, then we can pass the next step. Otherwise, we should create a new vertex which is delta away from the closest vertex, then pass the next step.
Finding the closest vertex in RRT.
Checking the distance between $q\_near$ and $q\_rand$
2.3 - Checking the intersection
During the creation of the tree, it is assumed that a fixed obstacle region, $X\_obs$ must be avoided, and that an explicit representation of $X\_obs$ is not avaliable. One can only check whether a given state lies in $X\_obs$ or not. Furthermore, each edge of the RRT will correspond to a path that lies entirely in $X\_free$.
Checking the intersection between $q\_near$ and $q\_rand$. (Outside the obstacle, there is not an intersection)
Checking the intersection between $q\_near$ and $q\_rand$. (Inside the obstacle, there is an intersection)
Checking the intersection between $q\_near$ and $q\_rand$. (Outside the obstacle, but there is an intersection)
The basic algorithm looks something like this:
- Start with an empty search tree
- Add your initial location to the search tree
- While your search tree has not reached the goal (and you haven't run out of time)
- Pick a location, $q\_r$, (with some sampling strategy)
- Find the vertex in the search tree closest to that random point, $q\_n$
- Try to add an edge (path) in the tree between $q\_n$ and $q\_r$, if you can link them without a collision occurring.
The resulting path should be smoothed before actual use. The first algorithm that comes to mind is: starting from the first point in the path, remove all the next intermediate points that have a line of sight except the last one, make it a check-point and repeat until the goal position is reached. The rule I used to determine if there is a line of sight between two points is: there should be no obstacles between nodes.
In order to make the path smoother, direct connection between the vertices of the path are considered. If the connection does not cross any obstacle then the path length can be reduced by rejecting other vertices in between this vertices. The pseudo-code to smooth the path is given below:
- Initialize the $start\_point$ by the goal node, $new\_point$ by next to goal node and $path\_smooth$ by the goal node.
- Until the starting node is encountered
- If the edge between the $start\_point$ & $new\_point$ is in free space then
- The next node in the path list is assigned to the $new\_point$.
- Otherwise
- Previous node in the path is assigned to the $path\_smooth$.
- Current node is assigned as the $start\_point$.
From the following figures, you can see that the thing is working in both graphs. This laboratory work was very helpful to understand the theoretical background of the rapidly-exploring random tree. It helped me to explore more information about it and how to apply to the robot path planning.
After smoothing the path, the visualization of the 'map.mat' file.
After smoothing the path, the visualization of the 'maze.mat' file.
[1] - Vidal Alcazar, Manuela Veloso, Daniel Borrajo, Adapting a Rapidly-Exploring Random Tree for Automated Planning, Proceedings, International Symposium on Combinatorial Search, 2011.
No comments:
Post a Comment