3D Path Planning Algorithm for Robot Using Matlab

Path Planning Algorithm For Robots (RRT*)

Aim:- To find path for robot in obstacle using Rapidly Exploring Random Tree * (RRT*)

Theory:-

RRT* is a modified version of RRT. When the number of nodes approaches infinity, the RRT* algorithm will deliver the shortest possible path to the goal. While realistically unfeasible, this statement suggests that the algorithm does work to develop a shortest path. The basic principle of RRT* is the same as RRT, but two key additions to the algorithm result in significantly different results.

First, RRT* records the distance each vertex has travelled relative to its parent vertex. This is referred to as the cost () of the vertex. After the closest node is found in the graph, neighbourhoods of vertices in a fixed radius from the new node are examined. If a node with a cheaper cost() than the proximal node is found, the cheaper node replaces the proximal node. The second difference RRT* adds is the renovating of the tree. After a vertex has been connected to the cheapest neighbour, the neighbours are again examined. Neighbours are checked if being renovated to the newly added vertex will make their cost decrease. If the cost does indeed decrease, the neighbour is rewired to the newly added vertex. This feature makes the path smoother.

Objective:-

To find path for robots through many obstacles and also to find shortest path to reach destination. It can also be used in self driving cars to drive without drivers control for that it has to search for different nodes and reach to its destination.

Code:-

path function to specify dfferent variables:-

function A = path(qr, qn, val, eps)
   qnew = [0 0];
   if val >= eps
       qnew(1) = qn(1) + ((qr(1)-qn(1))*eps)/dist_3d(qr,qn);
       qnew(2) = qn(2) + ((qr(2)-qn(2))*eps)/dist_3d(qr,qn);
       qnew(3) = qn(3) + ((qr(3)-qn(3))*eps)/dist_3d(qr,qn);
   else
       qnew(1) = qr(1);
       qnew(2) = qr(2);
       qnew(3) = qr(3);
   end
   
   A = [qnew(1), qnew(2), qnew(3)];
end

dist3d function to specify distance aspects:-

function d = dist3d(q1,q2)
    d = sqrt((q1(1)-q2(1))^2 + (q1(2)-q2(2))^2 + (q1(3)-q2(3))^2);  % d=((q1(1)-                                         
                                                          q2(1))^2+(q1(2)-q2(2))^2+ 
                                                          (q1(3)-                                                                       
                                                                  q2(3)))^1/2
end

Main Program:-

clear all
close all
clc
x_max = 640;                                 % Maximum Value of X-Co-ordinate
y_max = 480;                                 % Maximum Value of Y-Co-ordinate
z_max = 400;                                 % Maximum Value of Z-Co-ordinate

EPS = 50;
numNodes = 2000;        

q_start.coord = [0 0 0];                      % Starting point of path
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [500 340 160];                % Goal Point co-ordinates
q_goal.cost = 0;

nodes(1) = q_start;                          % First Node
% Plotting
figure(1)

for i = 1:1:numNodes
    q_rand = [rand(1)*x_max rand(1)*y_max rand(1)*z_max];
    plot3(q_rand(1), q_rand(2), q_rand(3), 'x', 'Color',  [0 0.4470 0.7410])
    
    % Stop if goal node is already reached
    for j = 1:1:length(nodes)
        if nodes(j).coord == q_goal.coord
            break
        end
    end
    
    % Find the closest node from existing points to form branches from them
    ndist = [];
    for j = 1:1:length(nodes)
        n = nodes(j);
        tmp = dist3d(n.coord, q_rand);
        ndist = [ndist tmp];
    end
    [val, idx] = min(ndist);
    q_near = nodes(idx);
    
    q_new.coord = path(q_rand, q_near.coord, val, EPS);
    line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], [q_near.coord(3), q_new.coord(3)], 'Color', 'k', 'LineWidth', 2);
    drawnow
    hold on
    q_new.cost = dist_3d(q_new.coord, q_near.coord) + q_near.cost;
    
    % Within a radius of r, find all existing nodes
    q_nearest = [];                                    % To find nearest point
    r = 40;                                            % Given Radius
    nearest_count = 1;
    for j = 1:1:length(nodes)
        if (dist_3d(nodes(j).coord, q_new.coord)) <= r
            q_nearest(nearest_count).coord = nodes(j).coord;
            q_nearest(nearest_count).cost = nodes(j).cost;
            nearest_count = nearest_count+1;
        end
    end
    
    % Initialize cost to currently known value
    q_min = q_near;
    C_min = q_new.cost;
    
    % Iterate through all nearest neighbors to find alternate lower
    % close paths
    
    for k = 1:1:length(q_nearest)
        if q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord) < C_min
            q_min = q_nearest(k);
            C_min = q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord);
            line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], [q_min.coord(3), q_new.coord(3)], 'Color', 'g');            
            hold on
        end
    end
    
    % Update parent to least cost-from node
    for j = 1:1:length(nodes)
        if nodes(j).coord == q_min.coord
            q_new.parent = j;
        end
    end
    
    % Append to nodes
    nodes = [nodes q_new];
end

D = [];
for j = 1:1:length(nodes)
    tmpdist = dist_3d(nodes(j).coord, q_goal.coord);
    D = [D tmpdist];
end

% Search backwards from goal to start to find the optimal least cost path
[val, idx] = min(D);
q_final = nodes(idx);
q_goal.parent = idx;
q_end = q_goal;
nodes = [nodes q_goal];
while q_end.parent ~= 0
    start = q_end.parent;
    line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], [q_end.coord(3), nodes(start).coord(3)], 'Color', 'r', 'LineWidth', 4);
    hold on
    q_end = nodes(start);
end

Explaination of Code:-

First we have made two function i.e. path and dist3d.

Function path to give values of different parameter used for giving direction.

Function dist3d to give different distance related things using RRT*.

Then in the main program specify the maximum values of x,y,z co-ordinates

Then specify co-ordinate of start point of plot

Then specify Co-ordinate of Goal point

Make a node in which all the values are stored

Make nearest point tracker which select only nearest point to form path as it will form a short path.

Give stop value if goal point is reached already.

Search in backward direction i.e. from goal to start point to get shortest path.

Output:-

Starting of path planning

Final Shortest Path Found shown as Orange Thick Line

Error Occured:-

Error can be resolve by using higher graphic card in laptop or PC.

Conclusion:-

It can be concluded that RRT* is one of the method used for path planning for robots it has little less accuracy as less amount of random points are generated in this method to get shortest path. Moreover it is a quick process than other path planning algorithm.