Autonomous Navigation of a Mobile Robot: RRT Path Planning Algorithm
Read this article in Turkish / Bu makaleyi Türkçe olarak okuyun: https://medium.com/@kucar17/bir-mobil-robotun-otonom-%C5%9Fekilde-yol-bulmas%C4%B1-rrt-algoritmas%C4%B1-57a8c51dfab4
Motion Planning in Robotics is a term which means to find a feasible set of configurations which takes a robot from an initial location to the desired one. Basic RRT Path Planning Algorithm accomplishes that by creating a tree of nodes which connects the starting point to the end (target) point.
On every step, the algorithm selects a random point and then connects that point to the closest point which was already calculated and inserted into the tree of nodes/points. Hence an edge/path is created between those two points. Then the path which connects the two points is checked to see if it collides with any of the obstacles. If there is no collision with the obstacles, this node is added to the tree of nodes. If the path collides with any of the obstacles however, it is decided that the movement is not feasible and same calculations are performed again with a newly picked point. These operations are performed iteratively until the target point or tolerance range is reached.
This algorithm is widely used in Robotics since it can be applied to environments which consist of obstacles. Even though it makes the robot reach its target point, the path calculated by the basic RRT Algorithm is often not optimal as it uses randomly picked points. Despite the fact that there are modified algorithms such as RRT* which are used to optimize the path, we are going to be inspecting the basic RRT Algorithm which has few small touches.
Here below, we are going to be covering the act of moving our robot to its objective without colliding into any of the obstacles. RRT Algorithm is going to be run multiple times, in order to create different solutions to particular Path Planning Problem.
Let us inspect the environment (C-space) where our robot is going to find its way:
Our starting point is located at (-0.5, -0.5) and desired point is located at (0.5, 0.5) points.
Since our environment is relatively simple, we can expect our algorithm to give a result which is very close to an optimal one.
We can see the coded — in MATLAB — version of our algorithm below. Firstly our code takes an “obstacles.csv” file as its input and then creates an obstacles matrix with that. Then we assign our start (startNode) and target/end (endNode) nodes to two different variables and limit our algorithm with 50 points. Therefore, our algorithm is going to try to transport our robot to its final configuration by calculating 50 different points at maximum. If the robot is not at its target location after 50 different points are calculated, algorithm is going to tell us that it failed.
In the code, there are few things which might catch your attention. One of them is the fact that sample spaces in X and Y axes get narrowed down separately in each step. For example if our robot reaches the point
(-0.15, 0. 20) without any collision, we tell our algorithm that its new sample spaces are (-0.15, 0.50] and (0.20, 0.50] in X and Y axes, respectively. That way, our algorithm is going to be using those intervals while picking new random points. Even though this approach has the potential of causing problems on certain environments, it helps to create a path which is very close to the optimal one in our instance. Another thing is the fact that straight lines are used in order to connect two points to each other. In systems which have motion constraints, different motion planners are used instead of straight lines. In order to ease the understanding, we are going to be assuming that our robot has no motion constraints.
%% Clearing the command window and workspace
clc
cleartic%% Defining obstacles
obstacles = readmatrix('obstacles.csv');
centerX = obstacles(:,1);
centerY = obstacles(:,2);
radius = obstacles(:,3)/2;obstacleCenterX = zeros(1, length(obstacles));
obstacleCenterY = zeros(1, length(obstacles));
obstacleRadius = zeros(1, length(obstacles));for i = 1 : length(obstacles)
obstacleCenterX(i) = centerX(i);
obstacleCenterY(i) = centerY(i);
obstacleRadius(i) = radius(i);
end%% Setting up the RRT Aims and Parameters:
startNode = [-0.5 -0.5];
endNode = [0.5 0.5];
maxTreeSize = 50;T{1} = startNode;
rangeX = startNode(1) : 0.01 : endNode(1);
rangeY = startNode(2) : 0.01 : endNode(2);
nodeNumber = 1;
edge = [];%% Starting the RRT Algorithm:
while length(T) < maxTreeSize
if(length(rangeX) ~= 0)
xAxis = randsample(rangeX*0.1, 1);
end
if(length(rangeY) ~= 0)
yAxis = randsample(rangeY*0.1, 1);
end
xSamp = [xAxis yAxis];
for i = 1 : length(T)
distances = sqrt((T{1}(1) - xAxis)^2 + (T{1}(2) - yAxis)^2);
index = find(distances == min(distances));
xNearest = T{i};
end
dRange = 0 : 0.005: 0.1;
distanceX = randsample(dRange, 1);
distanceY = randsample(dRange, 1);
xNewAxisX = xNearest(1) + distanceX;
xNewAxisY = xNearest(2) + distanceY;
xNew = [xNewAxisX xNewAxisY];
if (xNew(1) - xNearest(1) == 0)
continue
end
lineCoeffs = polyfit([xNearest(1) xNew(1)], [xNearest(2) xNew(2)], 1);
slope = lineCoeffs(1);
yIntercept = lineCoeffs(2);
% Checking if the line intersects any of the obstacles:
for i = 1 : length(obstacles)
a = linecirc(slope,yIntercept,obstacleCenterX(i),obstacleCenterY(i),obstacleRadius(i));
% If a is not a NaN array (1 by 2), this means collision and loop
% is terminated:
if (~isnan(a))
addCondition = 0;
break
else
addCondition = 1;
end
end
if xNew(1)> 0.5
xNew(1) = 0.5;
end
if xNew(2)> 0.5
xNew(2) = 0.5;
end
% If the line does not intersect the obstacles, xNew is added to the
% tree and vertex/edge is created between xNearest and xNew:
if (addCondition ~= 0)
T{length(T) + 1} = xNew;
nodeDistance = sqrt((xNearest(1) - xNew(1))^2 + (xNearest(2) - xNew(2))^2);
edge{length(edge) + 1} = [nodeNumber nodeNumber+1 nodeDistance];
nodeNumber = nodeNumber + 1;
% Narrowing down the range of sample in order to get closer to the
% goal node:
narrowRangeX = rangeX < xNew(1);
narrowRangeY = rangeY < xNew(2);
rangeX(narrowRangeX) = [];
rangeY(narrowRangeY) = [];
if xNew == endNode
disp('RRT is completed successfully!')
for j = 1 : length(T)
nodes(j, :) = [j T{j}];
end
for j = 1 : length(edge)
edges(j, :) = edge{j};
end
writematrix(edges, 'edges.csv');
writematrix(nodes, 'nodes.csv');
writematrix(nodes(:,1)', 'path.csv')
toc
return
end
end
end
disp('RRT is not completed successfully!')
toc
We expect to get the result “RRT is completed successfully!”, every time we run our code. The reason for that is, we had set the limit of 50 maximum points at the beginning of our code. This limit of 50 points is not a very hard condition for our algorithm. I ran the code approximately 15 times and the maximum number of points the algorithm calculated was 29. However, if we set this limit as 25 or use a different environment, we can expect our algorithm to fail occasionally.
We can see two different paths our algorithm has calculated below (The simulation of the paths were performed in CoppeliaSim simulation software):
As the two different results show, algorithm may yield different results each time, since it uses randomly picked points.
You can find the code and .csv file which consists of obstacles information as well as the other simulation files on my GitHub page.
The simulation environment I used in this article was part of a simulation environment package which was provided by a MOOC series named “Modern Robotics: Mechanics, Planning, and Control Specialization”, which can be found on Coursera. Also this project was inspired by the second week assignment of the fourth course of the same MOOC series.
