View
5
Download
0
Category
Preview:
Citation preview
NAME: MANDEEP SINGH ROLL NO.: 14363
ME766A PROJECT-4 __________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________________
CODE FOR PATH PLANNING IN 3D USING RAPIDLY EXPLORING RANDOM TREE:
INDOOR ENVIRONMENT
%% Path planning in 3D indoor environment using RRT
clear all;
clc;
%% DECLARATION OF THE WORKSPACE: Indoor Environment
%% Declare the obstacles:
%obstacle 1: Wall with a small slit defined using meshes
vertices1 = [750, 0, 0; 750, 0, 1000; 750, 1000, 1000; 750, 1000, 0;...
800, 1000, 0; 800, 1000, 1000; 800, 0, 1000; 800, 0, 0;...
750, 500, 400; 750, 500, 700; 750, 800, 700; 750, 800, 300;...
800, 800, 300; 800, 800, 700; 800, 500, 700; 800, 500, 400];
faces1 = [1 2 7 8; 2 3 6 7; 3 4 5 6; 4 1 8 5; 1 9 10 2; 10 11 3 2; 11 12 4 3;...
12 9 1 4; 8 16 15 7; 15 14 6 7; 14 13 5 6; 13 16 8 5; 9 10 15 16;...
10 11 14 15; 11 12 13 14; 9 16 13 12;];
% obstacle 2 : Wall
vertices2 = [200, 300, 0; 250, 300, 0; 250, 1000, 0; 200, 1000, 0;...
200, 300, 1000; 250, 300, 1000; 250, 1000, 1000; 200, 1000, 1000];
faces2 = [1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8; 1 2 3 4; 5 6 7 8];
% obstacle 3: pillar in the room
cylinder = [500 500 0 500 500 800 50];
%% Declare the Start and Goal points using the structure:
Startpt.coord = [50,50,50];
Startpt.parent = 0;
Startpt.cost = 0;
Goalpt.coord = [900,900,900];
Goalpt.parent = 0;
Goalpt.cost = 0;
%% Display the workspace:
figure(1)
axis([0 1000 0 1000 0 1000])
hold on;
title('Path planning in 3D using RRT')
hold on;
scatter3([Startpt.coord(1) Goalpt.coord(1)],[Startpt.coord(2) Goalpt.coord(2)],...
[Startpt.coord(3) Goalpt.coord(3)],'filled', 'b'); hold on;
drawMesh(vertices1, faces1,'b'); hold on; light;
drawMesh(vertices2, faces2,'g'); hold on; light;
drawCylinder(cylinder, 'FaceColor','y'); light;
hold on
%% Rapidly exploring Random Tree (RRT) with single seed at start point:
% Parameters used in RRT:
% Maximum no. of nodes and step size between parent & child node of tree
Max_nodes = 1000;
step = 50;
% Nodes: array which stores all the nodes in the tree
Nodes(1) = Startpt;
count = 1;
while count < Max_nodes
% Generate random point in the workspace
RandNode = [floor(rand(1)*1000) floor(rand(1)*1000) floor(rand(1)*1000)];
D = 2*1000; %D is initialised to maximum distance
CloseNode = Startpt; %Closest node is initialised as Start point
%% Find the closest node to the random node
for j = 1:length(Nodes)
dist2j = norm(Nodes(j).coord-RandNode);
if dist2j < D
D = dist2j;
CloseNode = Nodes(j);
k = j;
end
end
% New node is generated in the same direction as that of random node
% with the distance between closest and new node = step size
NewNode = CloseNode.coord + step*(RandNode-CloseNode.coord)/D;
%% Check the intersection of the obstacles with the line between closest
%% node and the new node
line=[CloseNode.coord NewNode];
points1 = intersectLineMesh3d(line, vertices1, faces1);
points2 =intersectLineMesh3d(line, vertices2, faces2);
points3 = intersectLineCylinder(line, cylinder);
if ~isempty(points1) || ~isempty(points2) || ~isempty(points3)
continue
end
% draw the line between the new node and the closest node
plot3([NewNode(1) CloseNode.coord(1)], [NewNode(2) CloseNode.coord(2)],...
[NewNode(3) CloseNode.coord(3)],'Color','k');
drawnow
hold on
%% New point is added to the Nodes array
Newpt.coord = NewNode;
Newpt.parent = k;
Newpt.cost = Nodes(k).cost + D;
Nodes = [Nodes Newpt];
count = count + 1;
end
%% Find the distance of the closest node to the goal point
D = 3000;
for j = 1:length(Nodes)
dist2j = dist(Nodes(j).coord, Goalpt.coord');
if dist2j < D
D = dist2j;
k = j;
end
end
Goalpt.parent = k;
Nodes = [Nodes Goalpt];
p = length(Nodes);
%% Trace back the path generated from the goal point to start point
while Nodes(p).parent>0
plot3([Nodes(p).coord(1), Nodes(Nodes(p).parent).coord(1)],...
[Nodes(p).coord(2), Nodes(Nodes(p).parent).coord(2)],...
[Nodes(p).coord(3), Nodes(Nodes(p).parent).coord(3)],'Color' ,
'r','LineWidth', 2)
drawnow;
hold on
p = Nodes(p).parent;
end
OUTPUTS CASE 1: SINGLE WALL WITH SLIT (1 OBSTACLES)
CASE 2(A): 2 WALLS AND PILLAR (3 OBSTACLES)
CASE 2(B): 2 WALLS AND PILLAR (3 OBSTACLES)
FAILED OUTPUT: MAXIMUM NUMBER OF NODES FORMED & CODE IS TERMINATED
Recommended