Introduction to RRT algorithm
Definition of RRTRapidly Exploring Random Tree (RRT) algorithm is a sampling-based path planning algorithm, which is often used in mobile robot path planning, and is suitable for solving path planning problems in high-dimensional space and under complex constraints. The basic idea is to generate random points through a step to the target point search forward, effectively avoid obstacles, avoid the path into the local minimum value, fast convergence speed. In this paper, RRT algorithm is realized by MATLAB to solve the path planning problem of two-dimensional plane.2 mapIn order to facilitate the implementation of the algorithm, discrete quantities are used to express the environment map. Where, the value 0 indicates the empty area without obstacles, and the value 1 indicates that there are obstacles in this area.
colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)
Copy the codeNote: In the data column x axis, behavior y axis
3 RRT algorithm principle Through the MATLAB program to build a tree from the starting position to the target position, and generate the path connecting the two points. Instead of having two trees (one center at the start and one center at the target), use one tree with a center at the start. Write a MATLAB function whose input and output have the same form.
function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)
Copy the codeWhere: map:. Mat map matrix q_start: x and y coordinates of the starting point Q_goal: X and y coordinates of the target point k: cannot be found at the target point yes, the maximum number of iterations to control the generation of the search tree is K delta_q: the distance between q_new and q_near p: Q_goal is taken as the probability of Q_RAND. When the random number generated randomly is less than P, the target point is taken as the random point Q_RAND. When the random number generated randomly is greater than P, a random point is generated as the Q_RAND vertices: The x and y coordinates of the vertex, and the coordinates of all the points generated in the process of generating the random tree are stored in this matrix, with the first point as the starting point and the last point as the target point. Is a matrix deges with 2 rows and N columns: generates all branches of the random tree, with a total of N-1 branches, so the matrix has N-1 rows, and the two columns of each row represent the index numbers of the two points. Once the target point is found, the last line will represent the target point. Tracing the target point, we can find the path: the index from the starting point to the target point, which is a row vector. The following uses a graph to represent some variables in the algorithm mentioned above:
4 Obstacle DetectionTo detect whether the branch (i.e., the edge between Q_NEAR and Q_new) is in free space, the incremental method or equalization method can be used, as shown in the diagram below (assuming that there are 10 points between two points, the left figure is the incremental detection method, and the right figure is the equalization method. It can be seen from the diagram that the detection times of the equalization method are less) :
5 Path smoothing Processing After the completion of the basic RRT algorithm, we have obtained a path from the starting point to the end point, now smooth and reduce the noise of this path, after the completion of the processing, we will get a shorter path. Adopt greedy algorithm: connect Q_start and Q_goal. If there is an obstacle between the two points detected, replace Q_goal with the previous point until the two points can be connected (there is no obstacle in the middle). Once Q_goal is connected, define the smoothing function in MATLAB:
function [path_smooth] = smooth(map, path, vertices, delta)
Copy the codeAmong them: the path: Delta: The delta distance is used to detect if the direct connection between the vertices in the path is in free space. Each edge is divided by delta into several path_smooth segments: After smoothing, the path points will be reduced, and the path after smoothing will be recorded with path_smooth, which is still a row vector, recording the index number of the path
The path after smoothing is:
Part of the source code
function result = PlanPathRRTstar(param, p_start, p_goal)
% RRT*
% credit : Anytime Motion Planning using the RRT*, S. Karaman, et. al.
% calculates the path using RRT* algorithm
% param : parameters for the problem
% 1) threshold : stopping criteria (distance between goal and current
% node)
% 2) maxNodes : maximum nodes for rrt tree
% 3) neighborhood : distance limit used in finding neighbors
% 4) obstacle : must be rectangle-shaped #limitation
% 5) step_size : the maximum distance that a robot can move at a time
% (must be equal to neighborhood size) #limitation
% 6) random_seed : to control the random number generation
% p_start : [x;y] coordinates
% p_goal : [x;y] coordinates
% variable naming : when it comes to describe node, if the name is with
% 'node', it means the coordinates of that node or it is just an index of
% rrt tree
% rrt struct : 1) p : coordinate, 2) iPrev : parent index, 3) cost :
% distance
% obstacle can only be detected at the end points but not along the line
% between the points
% for cost, Euclidean distance is considered.
% output : cost, rrt, time_taken
% whether goal is reached or not, depends on the minimum distance between
% any node and goal
field1 = 'p';
field2 = 'iPrev';
field3 = 'cost';
field4 = 'goalReached';
rng(param.random_seed);
tic;
start();
function start(a)
rrt(1) = struct(field1.p_start.field2, 0, field3, 0, field4, 0);
N = param.maxNodes; % iterations
j = 1;
% while endcondition>param.threshold %&& j<=N
while j<=N
sample_node = getSample();
% plot(sample_node(1), sample_node(2), '.g');
% text(sample_node(1), sample_node(2), strcat('random',num2str(j)))
nearest_node_ind = findNearest(rrt, sample_node);
% plot(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), '.g');
% text(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), strcat('nearest', num2str(j)));
new_node = steering(rrt(nearest_node_ind).p, sample_node);
if (isObstacleFree(new_node)==1)
% plot(new_node(1), new_node(2), '.g');
% text(new_node(1), new_node(2) +3.strcat('steered: new node', num2str(j)))
neighbors_ind = getNeighbors(rrt, new_node);
if(~isempty(neighbors_ind))
parent_node_ind = chooseParent(rrt, neighbors_ind, nearest_node_ind,new_node);
% plot(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2), '.g');
% text(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2) +3.strcat('parent', num2str(j)));
else
parent_node_ind = nearest_node_ind;
end
rrt = insertNode(rrt, parent_node_ind, new_node);
if (~isempty(neighbors_ind))
rrt = reWire(rrt, neighbors_ind, parent_node_ind, length(rrt));
end
if norm(new_node-p_goal) == param.threshold
rrt = setReachGoal(rrt);
end
end
j = j + 1;
end
setPath(rrt);
% text1 = strcat('Total number of generated nodes:', num2str(j- 1))
% text1 = strcat('Total number of nodes in tree:', length(rrt))
end
function rrt=setReachGoal(rrt)
rrt(end).goalReached = 1;
end
function setPath(rrt)
for i = 1: length(rrt)- 1
p1 = rrt(i).p;
rob.x = p1(1); rob.y=p1(2);
plot(rob.x,rob.y,'.b')
child_ind = find([rrt.iPrev]==i);
for j = 1: length(child_ind)
p2 = rrt(child_ind(j)).p;
plot([p1(1),p2(1)], [p1(2),p2(2)].'b'.'LineWidth'.1);
end
end
[cost,i] = getFinalResult(rrt);
result.cost = cost;
result.rrt = [rrt.p];
while i ~= 0
p11 = rrt(i).p;
plot(p11(1),p11(2),'b'.'Marker'.'. '.'MarkerSize'.30);
i = rrt(i).iPrev;
if i ~= 0
p22 = rrt(i).p;
plot(p22(1),p22(2),'b'.'Marker'.'. '.'MarkerSize'.30);
% plot([p11(1),p22(1)],[p11(2),p22(2)].'b'.'LineWidth'.3);
end
end
result.time_taken = toc;
end
function [value,min_node_ind] = getFinalResult(rrt)
goal_ind = find([rrt.goalReached]==1);
if ~(isempty(goal_ind))
disp('Goal has been reached! ');
rrt_goal = rrt(goal_ind);
value = min([rrt_goal.cost]);
min_node_ind = find([rrt.cost]==value);
if length(min_node_ind)>1
min_node_ind = min_node_ind(1);
end
else
disp('Goal has not been reached! ');
for i =1:length(rrt)
norm_rrt(i) = norm(p_goal-rrt(i).p);
end
[value,min_node_ind]= min(norm_rrt);
value = rrt(min_node_ind).cost;
end
end
% if it is obstacle-free.return 1.
% otherwise, return 0
function free=isObstacleFree(node_free)
free = 1;
for i = 1: length(param.obstacles(:,1))
obstacle = param.obstacles(i,:);
op1 = [obstacle(1), obstacle(2)];
op2 = [op1(1)+obstacle(3), op1(2)];
op3 = [op2(1), op1(2) + obstacle(4)];
op4 = [op1(1), op3(2)];
nx = node_free(1);
ny = node_free(2);
if ((nx>=op1(1) && nx<=op2(1)) && (ny>=op1(2) && ny<=op4(2)))
free = 0;
end
end
end
function new_node=steering(nearest_node, random_node)
dist = norm(random_node-nearest_node);
ratio_distance = param.step_size/dist;
x = (1-ratio_distance).* nearest_node(1)+ratio_distance .* random_node(1);
y = (1-ratio_distance).* nearest_node(2)+ratio_distance .* random_node(2);
new_node = [x;y];
end
function rrt = reWire(rrt, neighbors, parent, new)
for i=1:length(neighbors)
cost = rrt(new).cost + norm(rrt(neighbors(i)).p - rrt(new).p);
if (cost<rrt(neighbors(i)).cost)
% if norm(rrt(new).p-rrt(neighbors(i)).p)<param.step_size
% % plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.b');
% rrt(neighbors(i)).p = steering(rrt(new).p, rrt(neighbors(i)).p);
% end
% plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.m');
rrt(neighbors(i)).iPrev = new;
rrt(neighbors(i)).cost = cost;
end
end
end
function rrt = insertNode(rrt, parent, new_node)
rrt(end+1) = struct(field1, new_node, field2, parent, field3, rrt(parent).cost + norm(rrt(parent).p-new_node), field4, 0);
end
function parent = chooseParent(rrt, neighbors, nearest, new_node)
min_cost = getCostFromRoot(rrt, nearest, new_node);
parent = nearest;
for i=1:length(neighbors)
cost = getCostFromRoot(rrt, neighbors(i), new_node);
if (cost<min_cost)
min_cost = cost;
parent = neighbors(i);
end
end
end
function cost = getCostFromRoot(rrt, parent, child_node)
cost = rrt(parent).cost + norm(child_node - rrt(parent).p);
end
function neighbors = getNeighbors(rrt, node)
neighbors = [];
for i = 1:length(rrt)
dist = norm(rrt(i).p-node);
if (dist<=param.neighbourhood)
neighbors = [neighbors i];
end
end
end
Copy the codeThird, the operation result
Iv. Matlab version and references
1 Matlab version 2014A
Steamed stuffed bun [1] Yang, YU jizhou, Yang Shan. [2] ZHANG Yan, WU Shuigen. Intelligent Optimization Algorithm and MATLAB Example (2nd edition) [M]. Publishing House of Electronics Industry, 2016. MATLAB Optimization Algorithm source code [M]. Tsinghua University Press, 2017. [3]RRT path planning algorithm