1. Introduction to RRT algorithm

Definition of RRTRapid-exploring Random Tree (RRT) algorithm is a sampling-based path planning algorithm, which is often used for mobile robot path planning. It is suitable for solving path planning problems in high dimensional space and complex constraints. The basic idea is to generate random points through a step to search the target point forward, effectively avoid obstacles, avoid the path into local minimum, fast convergence. This paper realizes RRT algorithm through MATLAB to solve the path planning problem of two-dimensional plane.2 mapTo facilitate the implementation of the algorithm, discrete quantities are used to represent the environment map. Where, the value 0 represents an empty area without obstacles, and the value 1 represents an area with obstacles.The vertex coordinates searched in THE RRT algorithm are continuous points, and random points are generated in the map. The algorithm will build a tree by continuous points. In this process, the branches and vertices are detected to detect whether the vertex is in an empty area. Download the appendix. Dat file and draw the map.

colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)

Copy the code

Note: The column in the data is X-axis, and the behavior is Y-axis

3 RRT algorithm principle Through the MATLAB program to build a tree from the starting position to the target position, and generate a path connecting the two points. Use one tree with a center point at the starting point instead of two trees (one center point at the starting point and one center point at the target). 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 code

Where: 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: If the target point cannot be found, the maximum number of iterations to generate the search tree is controlled to be K. Delta_q: distance between q_new and q_near P: Q_goal is used as the probability of Q_RAND. When the random number generated is smaller than P, the target point is used as the random point Q_RAND. When the random number generated is greater than P, a random point is generated as the q_RAND vertices: The x and y coordinates of the vertices, the coordinates of all the points generated during the random tree generation are stored in this matrix, where the first point is the starting point, and the last point is the target point. Deges: Generate all branches of the random tree, there are n-1 branches in total, so the matrix has n-1 rows, and two columns of each row represent the index numbers of two points. Path: the index from the starting point to the target point, is a row vector. Here is a graph representing some of the variables in the algorithm mentioned above:

4 Obstacle DetectionTo detect whether the branch (the edge between q_near and q_new) is in free space, incremental method or equal division method can be used, as shown in the diagram below (assuming that there are 10 points between two points, incremental detection method is shown in the figure on the left and equal division method on the right, and it can be seen from the diagram that the number of detection times using equal division method is less) :In this paper, using k=10000, delta_q=50,p=0.3, we get the following results:

5 Path Smoothing After completing the basic RRT algorithm, we have a path from the start to the end. Now we smooth and de-noise this path. After completing the processing, we will have a shorter path. Greedy algorithm is used: connect Q_start and Q_GOAL, and if there is an obstacle detected between the two points, replace Q_GOAL with the previous point until the two points can be connected (there is no obstacle in the middle). Once q_goal has been linked, define the smoothing function in MATLAB:

function [path_smooth] = smooth(map, path, vertices, delta)

Copy the code

Among them: the path: Vertices: any of the vertices in the tree from the starting point to the target. Delta: the incremental distance used to detect whether the direct connection between the vertices of the path is in free space. Each edge is divided by delta into several segments path_smooth: After smoothing, the path points will be reduced. Use path_smooth to record the smoothed path, which is still a row vector, and record the index number of the path

The path after smoothing is: 6 summarizesRRT algorithm is an incremental search algorithm, based on the idea of probability, it is a probability complete path optimization algorithm, has the advantage of solving speed. The basic RRT algorithm has its own defects, the path obtained by solving is usually of poor quality, with edges and corners, not smooth enough. Therefore, it is necessary to smooth the path to obtain the path curve suitable for robot path tracking.

Two, some 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 code

3. Operation results

Matlab version and references

1 matlab version 2014A

[1] Yang Baoyang, YU Jizhou, Yang Shan. Intelligent Optimization Algorithm and Its MATLAB Example (2nd Edition) [M]. Publishing House of Electronics Industry, 2016. [2] ZHANG Yan, WU Shuigen. MATLAB Optimization Algorithm source code [M]. Tsinghua University Press, 2017. [3]RRT Path Planning Algorithm