RRT (Rapidly-Exploring Random Tree)
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. RRTs were developed by Steven M. LaValle and James J. Kuffner Jr. They easily handle problems with obstacles and differential constraints (nonholonomic and kinodynamic) and have been widely used in autonomous robotic path planning.
An RRT grows a tree rooted at the starting configuration by using random samples from the search space. As each sample is drawn, a connection is attempted between it and the nearest state in the tree. If the connection is feasible (passes entirely through free space and obeys any constraints), this results in the addition of the new state to the tree. With uniform sampling of the search space, the probability of expanding an existing state is proportional to the size of its Voronoi region. As the largest Voronoi regions belong to the states on the frontier of the search, this means that the tree preferentially expands towards large unsearched areas.
The length of the connection between the tree and a new state is frequently limited by a growth factor. If the random sample is further from its nearest state in the tree than this limit allows, a new state at the maximum distance from the tree along the line to the random sample is used instead of the random sample itself. The random samples can then be viewed as controlling the direction of the tree growth while the growth factor determines its rate. This maintains the space-filling bias of the RRT while limiting the size of the incremental growth.
RRT growth can be biased by increasing the probability of sampling states from a specific area. Most practical implementations of RRTs make use of this to guide the search towards the planning problem goals. This is accomplished by introducing a small probability of sampling the goal to the state sampling procedure. The higher this probability, the more greedily the tree grows towards the goal.
Input: 
---->
Output: 
% % An example of rapidly-exploring random trees and path planning in 2-D
% % Ref: "Rapidly-Exploring Random Trees: A New Tool for Path Planning",
% % Steven M. LaValle, 1998
%~~~~
% Code can also be converted to function with input format
% [tree, path] = RRT(K, xMin, xMax, yMin, yMax, xInit, yInit, xGoal, yGoal, thresh)
% K is the number of iterations desired.
% xMin and xMax are the minimum and maximum values of x
% yMin and yMax are the minimum and maximum values of y
% xInit and yInit is the starting point of the algorithm
% xGoal and yGoal are the desired endpoints
% thresh is the allowed threshold distance between a random point the the
% goal point.
% Output is the tree structure containing X and Y vertices and the path
% found obtained from Init to Goal
%~~~~
% Written by: Omkar Halbe, Technical University of Munich, 31.10.2015
%~~~~
clear all; close all;
K=2000;
xMin=0; xMax=100;
yMin=0; yMax=100;
xInit=0; yInit=0; %initial point for planner
xGoal=100; yGoal=100; %goal for planner
thresh=5; %acceptable threshold for solution
tree.vertex(1).x = xInit;
tree.vertex(1).y = yInit;
tree.vertex(1).xPrev = xInit;
tree.vertex(1).yPrev = yInit;
tree.vertex(1).dist=0;
tree.vertex(1).ind = 1; tree.vertex(1).indPrev = 0;
xArray=xInit; yArray = yInit;
figure(1); hold on; grid on;
plot(xInit, yInit, 'ko', 'MarkerSize',10, 'MarkerFaceColor','k');
plot(xGoal, yGoal, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
for iter = 2:K
xRand = (xMax-xMin)*rand;
yRand = (yMax-yMin)*rand;
dist = Inf*ones(1,length(tree.vertex));
for j = 1:length(tree.vertex)
dist(j) = sqrt( (xRand-tree.vertex(j).x)^2 + (yRand-tree.vertex(j).y)^2 );
end
[val, ind] = min(dist);
tree.vertex(iter).x = xRand; tree.vertex(iter).y = yRand;
tree.vertex(iter).dist = val;
tree.vertex(iter).xPrev = tree.vertex(ind).x;
tree.vertex(iter).yPrev = tree.vertex(ind).y;
tree.vertex(iter).ind = iter; tree.vertex(iter).indPrev = ind;
if sqrt( (xRand-xGoal)^2 + (yRand-yGoal)^2 ) <= thresh
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
break
end
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
pause(0);
end
if iter < K
path.pos(1).x = xGoal; path.pos(1).y = yGoal;
path.pos(2).x = tree.vertex(end).x; path.pos(2).y = tree.vertex(end).y;
pathIndex = tree.vertex(end).indPrev;
j=0;
while 1
path.pos(j+3).x = tree.vertex(pathIndex).x;
path.pos(j+3).y = tree.vertex(pathIndex).y;
pathIndex = tree.vertex(pathIndex).indPrev;
if pathIndex == 1
break
end
j=j+1;
end
path.pos(end+1).x = xInit; path.pos(end).y = yInit;
for j = 2:length(path.pos)
plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
% plot([tree.vertex(i).x; tree.vertex(ind).x],[tree.vertex(i).y; tree.vertex(ind).y], 'r');
% pause(0);
end
else
disp('No path found. Increase number of iterations and retry.');
end
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. RRTs were developed by Steven M. LaValle and James J. Kuffner Jr. They easily handle problems with obstacles and differential constraints (nonholonomic and kinodynamic) and have been widely used in autonomous robotic path planning.
An RRT grows a tree rooted at the starting configuration by using random samples from the search space. As each sample is drawn, a connection is attempted between it and the nearest state in the tree. If the connection is feasible (passes entirely through free space and obeys any constraints), this results in the addition of the new state to the tree. With uniform sampling of the search space, the probability of expanding an existing state is proportional to the size of its Voronoi region. As the largest Voronoi regions belong to the states on the frontier of the search, this means that the tree preferentially expands towards large unsearched areas.
The length of the connection between the tree and a new state is frequently limited by a growth factor. If the random sample is further from its nearest state in the tree than this limit allows, a new state at the maximum distance from the tree along the line to the random sample is used instead of the random sample itself. The random samples can then be viewed as controlling the direction of the tree growth while the growth factor determines its rate. This maintains the space-filling bias of the RRT while limiting the size of the incremental growth.
RRT growth can be biased by increasing the probability of sampling states from a specific area. Most practical implementations of RRTs make use of this to guide the search towards the planning problem goals. This is accomplished by introducing a small probability of sampling the goal to the state sampling procedure. The higher this probability, the more greedily the tree grows towards the goal.
Input: ---->Output:
% % An example of rapidly-exploring random trees and path planning in 2-D
% % Ref: "Rapidly-Exploring Random Trees: A New Tool for Path Planning",
% % Steven M. LaValle, 1998
%~~~~
% Code can also be converted to function with input format
% [tree, path] = RRT(K, xMin, xMax, yMin, yMax, xInit, yInit, xGoal, yGoal, thresh)
% K is the number of iterations desired.
% xMin and xMax are the minimum and maximum values of x
% yMin and yMax are the minimum and maximum values of y
% xInit and yInit is the starting point of the algorithm
% xGoal and yGoal are the desired endpoints
% thresh is the allowed threshold distance between a random point the the
% goal point.
% Output is the tree structure containing X and Y vertices and the path
% found obtained from Init to Goal
%~~~~
% Written by: Omkar Halbe, Technical University of Munich, 31.10.2015
%~~~~
clear all; close all;
K=2000;
xMin=0; xMax=100;
yMin=0; yMax=100;
xInit=0; yInit=0; %initial point for planner
xGoal=100; yGoal=100; %goal for planner
thresh=5; %acceptable threshold for solution
tree.vertex(1).x = xInit;
tree.vertex(1).y = yInit;
tree.vertex(1).xPrev = xInit;
tree.vertex(1).yPrev = yInit;
tree.vertex(1).dist=0;
tree.vertex(1).ind = 1; tree.vertex(1).indPrev = 0;
xArray=xInit; yArray = yInit;
figure(1); hold on; grid on;
plot(xInit, yInit, 'ko', 'MarkerSize',10, 'MarkerFaceColor','k');
plot(xGoal, yGoal, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');
for iter = 2:K
xRand = (xMax-xMin)*rand;
yRand = (yMax-yMin)*rand;
dist = Inf*ones(1,length(tree.vertex));
for j = 1:length(tree.vertex)
dist(j) = sqrt( (xRand-tree.vertex(j).x)^2 + (yRand-tree.vertex(j).y)^2 );
end
[val, ind] = min(dist);
tree.vertex(iter).x = xRand; tree.vertex(iter).y = yRand;
tree.vertex(iter).dist = val;
tree.vertex(iter).xPrev = tree.vertex(ind).x;
tree.vertex(iter).yPrev = tree.vertex(ind).y;
tree.vertex(iter).ind = iter; tree.vertex(iter).indPrev = ind;
if sqrt( (xRand-xGoal)^2 + (yRand-yGoal)^2 ) <= thresh
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
break
end
plot([tree.vertex(iter).x; tree.vertex(ind).x],[tree.vertex(iter).y; tree.vertex(ind).y], 'r');
pause(0);
end
if iter < K
path.pos(1).x = xGoal; path.pos(1).y = yGoal;
path.pos(2).x = tree.vertex(end).x; path.pos(2).y = tree.vertex(end).y;
pathIndex = tree.vertex(end).indPrev;
j=0;
while 1
path.pos(j+3).x = tree.vertex(pathIndex).x;
path.pos(j+3).y = tree.vertex(pathIndex).y;
pathIndex = tree.vertex(pathIndex).indPrev;
if pathIndex == 1
break
end
j=j+1;
end
path.pos(end+1).x = xInit; path.pos(end).y = yInit;
for j = 2:length(path.pos)
plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
% plot([tree.vertex(i).x; tree.vertex(ind).x],[tree.vertex(i).y; tree.vertex(ind).y], 'r');
% pause(0);
end
else
disp('No path found. Increase number of iterations and retry.');
end