RRT、RRTConnect、RRT*——Matlab算法 - 骏骏 - 博客园 (2024)

matlab-rrt-variants===================RRT*, RRT-connect, lazy RRT and RRT extend have been implemented for 2d and 3d c-spaces with visualization#General Information:This is a basic yet meaningful implementation of RRT and its variants in Matlab.#How to runAll you need to do is fire up the benchmarkRRT.m file, it is pretty self explanatory.# Specify the number of runs for each planner* num_of_runs =1;# Specify if we want to run the specific planner or not, 1 for yes and 0 for no.* run_RRTconnect =0 or 1; * run_RRTextend = 0 or 1;* run_LazyRRT = 0 or 1;* run_RRTstar = 0 or 1;# Specify whether to run the planner in 2D or 3D (only for now)* dim = 3;# Specify the step size, the world is 100 \* 100 for 2D and 100 \* 100 \*100 for 3D * stepsize = [10];# Specify whether to use random obstacles or to use pre programmed obstacles* random_world = 0 or 1;# For RRT* only*radius = 10;*samples = 4000;# Showing output or not*show_output = 0 or 1;*show_benchmark_results = 0 or 1;

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction benchmarkRRTclc;close all;clear all;num_of_runs =1;run_RRTconnect =1;run_RRTextend = 0;run_LazyRRT = 0;run_RRTstar = 0;dim = 3;stepsize = [10];random_world = 1;radius = 10;samples = 4000;show_output = 1;show_benchmark_results = 0;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%t_lazy = [];t_extend = [];t_connect = [];t_star = [];l_lazy = [];l_extend = [];l_connect = [];l_star = [];p_lazy = [];p_extend = [];p_connect = [];p_star = [];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%for sits = 1:size(stepsize,2) segmentLength = stepsize(sits); if run_LazyRRT == 1 time = 0; avg_its = 0; avg_path = 0; for i = 1:num_of_runs [n_its path_n run_time] = LazyRRT3D(dim,segmentLength,random_world,show_output); time = time + run_time; avg_its = avg_its + n_its; avg_path = avg_path + path_n; end str1 = ['The time taken by Lazy RRT for ', num2str(num_of_runs), ' runs is ', num2str(time)]; str2 = ['The averagae time taken by Lazy RRT for each run is ', num2str(time/num_of_runs)]; str3 = ['The averagae number of states explored by Lazy RRT for each run is ', num2str(avg_its/num_of_runs)]; str4 = ['The averagae number of state in Path by Lazy RRT for each run is ', num2str(avg_path/num_of_runs)]; disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); disp(str1); disp(str2); disp(str3); disp(str4); disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); t_lazy = [t_lazy time/num_of_runs]; l_lazy = [l_lazy avg_its/num_of_runs]; p_lazy = [p_lazy avg_path/num_of_runs]; end if run_RRTstar == 1 time = 0; avg_its = 0; avg_path = 0; for i = 1:num_of_runs [n_its path_n,run_time] = RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples); time = time + run_time; avg_its = avg_its + n_its; avg_path = avg_path + path_n; end str1 = ['The time taken by RRT-Star for ', num2str(num_of_runs), ' runs is ', num2str(time)]; str2 = ['The averagae time taken by RRT_Star for each run is ', num2str(time/num_of_runs)]; str3 = ['The averagae number of states explored by RRT_Star for each run is ', num2str(avg_its/num_of_runs)]; str4 = ['The averagae number of state in Path by RRT-Star for each run is ', num2str(avg_path/num_of_runs)]; disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); disp(str1); disp(str2); disp(str3); disp(str4); disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); t_star = [t_star time/num_of_runs]; l_star = [l_star avg_its/num_of_runs]; p_star = [p_star avg_path/num_of_runs]; end if run_RRTextend == 1 time = 0; avg_its = 0; avg_path = 0; for i = 1:num_of_runs [n_its path_n,run_time] = RRTextend3D(dim,segmentLength,random_world,show_output); time = time + run_time; avg_its = avg_its + n_its; avg_path = avg_path + path_n; end str1 = ['The time taken by RRT-Extend for ', num2str(num_of_runs), ' runs is ', num2str(time)]; str2 = ['The averagae time taken by RRT_Extend for each run is ', num2str(time/num_of_runs)]; str3 = ['The averagae number of states explored by RRT_Extend for each run is ', num2str(avg_its/num_of_runs)]; str4 = ['The averagae number of state in Path by RRT-Extend for each run is ', num2str(avg_path/num_of_runs)]; disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); disp(str1); disp(str2); disp(str3); disp(str4); disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); t_extend = [t_extend time/num_of_runs]; l_extend = [l_extend avg_its/num_of_runs]; p_extend = [p_extend avg_path/num_of_runs]; end if run_RRTconnect == 1 time = 0; avg_its = 0; avg_path = 0; for i = 1:num_of_runs [n_its path_n,run_time] = RRTconnect3D(dim,segmentLength,random_world,show_output); time = time + run_time; avg_its = avg_its + n_its; avg_path = avg_path + path_n; end str1 = ['The time taken by RRT-Connect for ', num2str(num_of_runs), ' runs is ', num2str(time)]; str2 = ['The averagae time taken by RRT-Connect for each run is ', num2str(time/num_of_runs)]; str3 = ['The averagae number of states explored by RRT-Connect for each run is ', num2str(avg_its/num_of_runs)]; str4 = ['The averagae number of state in Path by RRT-Connect for each run is ', num2str(avg_path/num_of_runs)]; disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); disp(str1); disp(str2); disp(str3); disp(str4); disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); t_connect = [t_connect time/num_of_runs]; l_connect = [l_connect avg_its/num_of_runs]; p_connect = [p_connect avg_path/num_of_runs]; endendif show_benchmark_results == 1 figure; hold on; plot(stepsize,t_lazy,'r','LineWidth',2); plot(stepsize,t_extend,'g','LineWidth',2); plot(stepsize,t_connect,'b','LineWidth',2); ylabel('Computational Time'); xlabel('Step Size'); dim_str = sprintf('Comparison of computational time for %d Dimensional C-Space',dim); title(dim_str) hold off; figure; hold on; plot(stepsize,l_lazy,'r','LineWidth',2); plot(stepsize,l_extend,'g','LineWidth',2); plot(stepsize,l_connect,'b','LineWidth',2); ylabel('Number of States Explored'); xlabel('Step Size'); dim_str = sprintf(' Comparison of number of states explored for %d Dimensional C-Space',dim); title(dim_str) hold off; figure; hold on; plot(stepsize,p_lazy,'r','LineWidth',2); plot(stepsize,p_extend,'g','LineWidth',2); plot(stepsize,p_connect,'b','LineWidth',2); ylabel('Number of States in Path'); xlabel('Step Size'); dim_str = sprintf('Comparison for number of states in path for %d Dimensional C-Space',dim); title(dim_str) hold off; end % t_lazy% l_lazy% p_lazy% % % % t_extend% l_extend% p_extend% % t_connect% l_connect% p_connect end

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] = RRTconnect_3D(dim,segmentLength,random_world,show_output)% dim = 2;% segmentLength = 5;% random_world = 0;random_world表示是否使用随机障碍物（1）还是事先设定好的障碍物（0）% standard length of path segments% start_cord - 开始节点坐标% goal_cord - 目标节点坐标if dim ==2start_cord = [5,5];goal_cord = [95,95];elsestart_cord = [5,5,5];goal_cord = [95,95,95];end%----------------------- create random world-----------------------------%Size = 100; %世界的坐标轴尺寸NumObstacles = 100; %障碍物的个数if random_world ==1world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);else[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);end% randomly select start and end nodes%start_node = generateRandomNode(world,dim)%end_node = generateRandomNode(world,dim)%---------------------- set starPoint and endPoint----------------------%%%node = [point,goal_flag,cost,min_parent_idx]start_node = [start_cord,0,0,0];end_node = [goal_cord,0,0,0];%----------------establish tree starting with the start node------------%tree = start_node;a = clock;%--------check to see if start_node connects directly to end_node-------%if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )... &&(collision(start_node,end_node,world,dim)==0) ) path = [start_node; end_node];%------------Make randPoint as newPoint,Constant iterative-------------%else nIterations = 0; numPaths = 0; flag = 0; while numPaths<1, [tree,flag] = extendTree(tree,end_node,segmentLength,world,dim); %%每次生成随机节点，从树中最近点拓展到该随机节点，并作为树中的新节点 numPaths = numPaths + flag; nIterations = nIterations+1; endend%-----------------find Minimum Path------------------------------------%path = findMinimumPath(tree,end_node,dim);sizePath = size(path,1);b = clock;%-----------------calculate Simulation time-----------------------------------%run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1% find path with minimum cost to end_nodefigure;plotExpandedTree(world,tree,dim);plotWorld(world,path,dim);endend%%%*******************************生成随机障碍物地图***************************************%%%%function world = createWorld(NumObstacles, endcorner, origincorner,dim)% endcorner - 地图右上角的坐标，即终点坐标% oringincorner - 地图左下角的坐标，即初始点的坐标% NumObstacles - 随机障碍物的个数 if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); %返回最小的坐标长度 maxRadius = 5*maxRadius/NumObstacles/2; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; end end elseif dim ==3; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)]; maxRadius = min(bounds); maxRadius = 5*maxRadius/NumObstacles; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; cz = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; world.cz(i) = cz; end end endend%%%*******************************生成已知障碍物地图***************************************%%%%function [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)NumObstacles = 5; if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; world.cx(1) = cx; world.cy(1) = cy; world.radius(2) = maxRadius; cx = 75; cy = 25; world.cx(2) = cx; world.cy(2) = cy; world.radius(3) = maxRadius; cx = 25; cy = 75; world.cx(3) = cx; world.cy(3) = cy; world.radius(4) = maxRadius; cx = 25; cy = 25; world.cx(4) = cx; world.cy(4) = cy; world.radius(5) = maxRadius; cx = 75; cy = 75; world.cx(5) = cx; world.cy(5) = cy; end elseif dim == 3 NumObstacles = 9; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; cz = 50; world.cx(1) = cx; world.cy(1) = cy; world.cz(1) = cz; world.radius(2) = maxRadius; cx = 25; cy = 25; cz = 25; world.cx(2) = cx; world.cy(2) = cy; world.cz(2) = cz; world.radius(3) = maxRadius; cx = 75; cy = 75; cz = 75; world.cx(3) = cx; world.cy(3) = cy; world.cz(3) = cz; world.radius(4) = maxRadius; cx = 25; cy = 25; cz = 75; world.cx(4) = cx; world.cy(4) = cy; world.cz(4) = cz; world.radius(5) = maxRadius; cx = 75; cy = 75; cz = 25; world.cx(5) = cx; world.cy(5) = cy; world.cz(5) = cz; world.radius(6) = maxRadius; cx = 25; cy = 75; cz = 25; world.cx(6) = cx; world.cy(6) = cy; world.cz(6) = cz; world.radius(7) = maxRadius; cx = 75; cy = 25; cz = 25; world.cx(7) = cx; world.cy(7) = cy; world.cz(7) = cz; world.radius(8) = maxRadius; cx = 75; cy = 25; cz = 75; world.cx(8) = cx; world.cy(8) = cy; world.cz(8) = cz; world.radius(9) = maxRadius; cx = 25; cy = 75; cz = 75; world.cx(9) = cx; world.cy(9) = cy; world.cz(9) = cz; end endend%%%*******************************生成随机节点***************************************%%%%function node=generateRandomNode(world,dim)if dim ==2;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];endelseif dim ==3;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];endendend%%%*******************************检测是否冲突***************************************%%%%function collision_flag = collision(node, parent, world,dim)% node - 节点端点% parent - 另一节点端点collision_flag = 0;%是否超出地图范围for i=1:dim if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i)) collision_flag = 1; endend%是否在障碍物的范围内if collision_flag == 0 && dim ==2 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)),%%求矩阵范数，即节点在障碍物范围内 collision_flag = 1; break; end end endelseif collision_flag == 0 && dim ==3 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endendend%%%*******************************检测节点是否可用***************************************%%%%function collision_flag = is_point_valid(point, world,dim)collision_flag = 0;%是否超出地图范围for i=1:dim if (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i)) collision_flag = 1; endend%是否在障碍物的范围内if collision_flag == 0 && dim ==2 p = point(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)), collision_flag = 1; break; end endelseif collision_flag == 0 && dim ==3 p = point(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)), collision_flag = 1; break; end endendend%%%*******************************检测最后一个节点是否需要**********************************%%%%function flag = canEndConnectToTree(tree,end_node,minDist,world,dim) flag = 0; % check only last node added to tree since others have been checked if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)... && (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ), flag = 1; endend%%%*******************************树节点拓展***************************************%%%%function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim) flag = 0; % select a random point randomPoint = zeros(1,dim); for i=1:dim randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; %%%在地图中产生随机节点 end % find leaf on node that is closest to randomPoint % -选择tree中节点和随机节点randomPoint的欧式距离最小的点为 newpoint（最近节点nearPoint） tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); min_parent_idx = idx; new_point = tree(idx,1:dim); new_node = tree(idx,:); pflag = 0; %%%从树节点中的nearPoint沿randomPoint方向，不断向randomPoint拓展，直到到达randomPoint为止 while norm(new_point-randomPoint)>0 && pflag==0 %随机节点和上一个新节点（nearPoint）的距离小于segmentLength时，将随机节点取为下一个新的节点 if norm(new_point-randomPoint)<segmentLength pflag = collision(randomPoint,tree(min_parent_idx,:),world,dim); if pflag == 0 new_point = randomPoint; min_cost = cost_np(tree(min_parent_idx,:),new_point,dim);%%计算上一个newPoint（nearPoint）到下一个newPoint（randPoint）的代价值， new_node = [new_point,0,min_cost,min_parent_idx];%%min_cost为从树的主节点到最终选择的newPoint的代价值累加和 tree = [tree;new_node]; %%增加新的树节点 pflag = 1; goal_flag = is_goal(new_node,end_node,segmentLength,world,dim); if goal_flag == 1; tree(end,dim+1)=1; flag = 1; end end %随机节点和上一个新节点（nearPoint）的距离大于segmentLength时，在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点 else new_point = (randomPoint-tree(min_parent_idx,1:dim)); new_point = tree(min_parent_idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(min_parent_idx,:),new_point,dim); new_node = [new_point, 0, min_cost, min_parent_idx]; pflag = collision(new_node,tree(min_parent_idx,:),world,dim); if pflag == 0 tree = [tree ; new_node]; min_parent_idx = size(tree,1); goal_flag = is_goal(new_node,end_node,segmentLength,world,dim); if goal_flag == 1; tree(end,dim+1)=1; % mark node as connecting to end. pflag = 1; flag = 1; end end end end new_tree = tree; end function goal_flag = is_goal(node,end_node,segmentLength,world,dim) goal_flag = 0; if (norm(node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(node,end_node,world,dim)==0) goal_flag = 1; endend %%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%% function e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);%array中元素平方值for i=1:dim sqr_e_dist(:,i) = array(:,i).*array(:,i); ende_dist = zeros(size(array,1),1);for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); endend%%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%% %calculate the cost from a node to a pointfunction [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a nodefunction [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;cost = norm(diff);end%%%*******************找到所有树节点到目标点的最短路径**********************************%%%% function path = findMinimumPath(tree,end_node,dim) % find nodes that connect to end_node connectingNodes = []; for i=1:size(tree,1), if tree(i,dim+1)==1, connectingNodes = [connectingNodes ; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,dim+2)); % construct lowest cost path path = [connectingNodes(idx,:); end_node]; parent_node = connectingNodes(idx,dim+3); while parent_node>1, parent_node = tree(parent_node,dim+3); path = [tree(parent_node,:); path]; end endfunction plotExpandedTree(world,tree,dim) ind = size(tree,1); while ind>0 size(tree); branch = []; node = tree(ind,:); branch = [ branch ; node ]; parent_node = node(dim+3); while parent_node > 1 cur_parent = parent_node; branch = [branch; tree(parent_node,:)]; parent_node = tree(parent_node,dim+3); end ind = ind - 1; if dim == 2 X = branch(:,1); Y = branch(:,2); p = plot(X,Y); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; elseif dim == 3 X = branch(:,1); Y = branch(:,2); Z = branch(:,3); p = plot3(X,Y,Z); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; end endendfunction plotWorld(world,path,dim) % the first element is the north coordinate % the second element is the south coordinate if dim ==2 N = 10; th = 0:2*pi/N:2*pi; axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2)]); hold on for i=1:world.NumObstacles, X = world.radius(i)*sin(th) + world.cx(i); Y = world.radius(i)*cos(th) + world.cy(i); fill(X,Y,'blue'); end X = path(:,1); Y = path(:,2); p = plot(X,Y); elseif dim ==3 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2),... world.origincorner(3), world.endcorner(3)]); hold on for i=1:world.NumObstacles, [X Y Z] = sphere(10); X = (X*world.radius(i)); Y = (Y*world.radius(i)); Z = (Z*world.radius(i)); surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i)); colormap([0.5 0.2 0.3]); end X = path(:,1); Y = path(:,2); Z = path(:,3); p = plot3(X,Y,Z); end set(p,'Color','black','LineWidth',3) xlabel('X axis'); ylabel('Y axis'); zlabel('Z axis'); title('RRT Connect Algorithm');end

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] = LazyRRT3D(dim,segmentLength,random_world,show_output)% dim = 2;% segmentLength = 5;% random_world = 0;if dim ==2start_cord = [5,5];goal_cord = [95,95];else start_cord = [5,5,5];goal_cord = [95,95,95];end% create random worldSize = 100;NumObstacles = 100;if random_world ==1world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);else[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);end% randomly select start and end nodes%start_node = generateRandomNode(world,dim)%end_node = generateRandomNode(world,dim)start_node = [start_cord,0,0,0];end_node = [goal_cord,0,0,0];% establish tree starting with the start nodetree = start_node;a = clock;% check to see if start_node connects directly to end_nodeif ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )... &&(collision(start_node,end_node,world,dim)==0) ) path = [start_node; end_node];else nIterations = 0; numPaths = 0; flag = 0; while numPaths<1, [tree,flag] = extendLazyTree(tree,end_node,segmentLength,world,dim); numPaths = numPaths + flag; nIterations = nIterations+1; endend% find path with minimum cost to end_nodeLazyPath = findMinimumPath(tree,end_node,dim);path = RepairLazyPath(LazyPath,segmentLength,world,dim);sizePath = size(path,1);b = clock;run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1figure;plotExpandedTree(world,tree,dim);plotWorld(world,path,dim);% figure(2);% plotWorld(world,path,dim);%plotExpandedTree(world,tree,dim);endendfunction path = RepairLazyPath(LazyPath,segmentLength,world,dim)path = [];start_flag = 0;end_flag = 0;cflag = 0;for i=1:(size(LazyPath,1)-1) cflag = collision(LazyPath(i+1,:),LazyPath(i,:),world,dim); if cflag == 1 && start_flag == 0 start_collision_node = LazyPath(i,:); start_flag = 1; end_flag = 1; breakage_from = i; elseif end_flag == 1 && cflag == 0 end_collision_node = LazyPath(i,:); start_flag = 0; end_flag = 0; tree = start_collision_node; end_node = end_collision_node; flag = 0; while flag == 0 [tree,flag] = extendTree(tree,end_node,segmentLength,world,dim); end repaired_segment = findMinimumPath(tree,end_node,dim); path = [path ; repaired_segment]; breakage_to = i; elseif start_flag == 0 && end_flag == 0 && cflag == 0 path = [path ; LazyPath(i,:) ; LazyPath(i+1,:)]; end endendfunction world = createWorld(NumObstacles, endcorner, origincorner,dim) if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); maxRadius = 5*maxRadius/NumObstacles/2; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; end end elseif dim ==3; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)]; maxRadius = min(bounds); maxRadius = 5*maxRadius/NumObstacles; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; cz = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; world.cz(i) = cz; end end endendfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)NumObstacles = 5; if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; world.cx(1) = cx; world.cy(1) = cy; world.radius(2) = maxRadius; cx = 75; cy = 25; world.cx(2) = cx; world.cy(2) = cy; world.radius(3) = maxRadius; cx = 25; cy = 75; world.cx(3) = cx; world.cy(3) = cy; world.radius(4) = maxRadius; cx = 25; cy = 25; world.cx(4) = cx; world.cy(4) = cy; world.radius(5) = maxRadius; cx = 75; cy = 75; world.cx(5) = cx; world.cy(5) = cy; end elseif dim == 3 NumObstacles = 9; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; cz = 50; world.cx(1) = cx; world.cy(1) = cy; world.cz(1) = cz; world.radius(2) = maxRadius; cx = 25; cy = 25; cz = 25; world.cx(2) = cx; world.cy(2) = cy; world.cz(2) = cz; world.radius(3) = maxRadius; cx = 75; cy = 75; cz = 75; world.cx(3) = cx; world.cy(3) = cy; world.cz(3) = cz; world.radius(4) = maxRadius; cx = 25; cy = 25; cz = 75; world.cx(4) = cx; world.cy(4) = cy; world.cz(4) = cz; world.radius(5) = maxRadius; cx = 75; cy = 75; cz = 25; world.cx(5) = cx; world.cy(5) = cy; world.cz(5) = cz; world.radius(6) = maxRadius; cx = 25; cy = 75; cz = 25; world.cx(6) = cx; world.cy(6) = cy; world.cz(6) = cz; world.radius(7) = maxRadius; cx = 75; cy = 25; cz = 25; world.cx(7) = cx; world.cy(7) = cy; world.cz(7) = cz; world.radius(8) = maxRadius; cx = 75; cy = 25; cz = 75; world.cx(8) = cx; world.cy(8) = cy; world.cz(8) = cz; world.radius(9) = maxRadius; cx = 25; cy = 75; cz = 75; world.cx(9) = cx; world.cy(9) = cy; world.cz(9) = cz; end endendfunction node=generateRandomNode(world,dim)if dim ==2;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];endelseif dim ==3;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dim if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i)) collision_flag = 1; endendif collision_flag == 0 && dim ==2 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endelseif collision_flag == 0 && dim ==3 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endendendfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim) flag = 0; % check only last node added to tree since others have been checked if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)... & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ), flag = 1; endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim) flag = 0; flag1 = 0; while flag1==0, % select a random point randomPoint = ones(1,dim); for i=1:dim randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; end % find leaf on node that is closest to randomPoint tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); new_point = (randomPoint-tree(idx,1:dim)); new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(idx,:),new_point,dim); new_node = [new_point, 0, min_cost, idx]; if collision(new_node, tree(idx,:), world,dim)==0 new_tree = [tree;new_node]; flag1 = 1; else flag1=0; end end % check to see if new node connects directly to end_node if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(new_node,end_node,world,dim)==0) ) flag = 1; new_tree(end,dim+1)=1; % mark node as connecting to end. endendfunction [new_tree,flag] = extendLazyTree(tree,end_node,segmentLength,world,dim) % select a random point randomPoint = ones(1,dim); for i=1:dim randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; end % find leaf on node that is closest to randomPoint tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); new_point = (randomPoint-tree(idx,1:dim)); new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(idx,:),new_point,dim); new_node = [new_point, 0, min_cost, idx]; new_tree = [tree;new_node]; % check to see if new node connects directly to end_node if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(new_node,end_node,world,dim)==0) ) flag = 1; new_tree(end,dim+1)=1; % mark node as connecting to end. else flag = 0; end endfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);for i=1:dim sqr_e_dist(:,i) = array(:,i).*array(:,i); ende_dist = zeros(size(array,1),1);for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); endend%calculate the cost from a node to a pointfunction [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a nodefunction [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;cost = norm(diff);endfunction path = findMinimumPath(tree,end_node,dim) % find nodes that connect to end_node connectingNodes = []; for i=1:size(tree,1), if tree(i,dim+1)==1, connectingNodes = [connectingNodes ; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,dim+2)); % construct lowest cost path path = [connectingNodes(idx,:); end_node]; parent_node = connectingNodes(idx,dim+3); while parent_node>1, parent_node = tree(parent_node,dim+3); path = [tree(parent_node,:); path]; end endfunction plotExpandedTree(world,tree,dim) ind = size(tree,1); while ind>0 branch = []; node = tree(ind,:); branch = [ branch ; node ]; parent_node = node(dim+3); while parent_node > 1 cur_parent = parent_node; branch = [branch; tree(parent_node,:)]; parent_node = tree(parent_node,dim+3); end ind = ind - 1; if dim == 2 X = branch(:,1); Y = branch(:,2); p = plot(X,Y); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; elseif dim == 3 X = branch(:,1); Y = branch(:,2); Z = branch(:,3); p = plot3(X,Y,Z); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; end endendfunction plotWorld(world,path,dim) % the first element is the north coordinate % the second element is the south coordinate if dim ==2 N = 10; th = 0:2*pi/N:2*pi; axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2)]); hold on for i=1:world.NumObstacles, X = world.radius(i)*sin(th) + world.cx(i); Y = world.radius(i)*cos(th) + world.cy(i); fill(X,Y,'blue'); end X = path(:,1); Y = path(:,2); p = plot(X,Y); elseif dim ==3 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2),... world.origincorner(3), world.endcorner(3)]); hold on for i=1:world.NumObstacles, [X Y Z] = sphere(10); X = (X*world.radius(i)); Y = (Y*world.radius(i)); Z = (Z*world.radius(i)); surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i)); colormap([0.5 0.2 0.3]); end X = path(:,1); Y = path(:,2); Z = path(:,3); p = plot3(X,Y,Z); end set(p,'Color','black','LineWidth',3) xlabel('X axis'); ylabel('Y axis'); zlabel('Z axis'); title('Lazy RRT Algorithm');end

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction [its,sizePath,run_time] = RRTextend3D(dim,segmentLength,random_world,show_output)% dim = 2;% radius =0;% segmentLength = 5;% random_world = 0;% n_its = 1000;% standard length of path segmentsif dim ==2start_cord = [5,5];goal_cord = [95,95];else start_cord = [5,5,5];goal_cord = [95,95,95];end% create random worldSize = 100;NumObstacles = 100;if random_world ==1world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);else[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);end% randomly select start and end nodes%start_node = generateRandomNode(world,dim)%end_node = generateRandomNode(world,dim)start_node = [start_cord,0,0,0];end_node = [goal_cord,0,0,0];% establish tree starting with the start nodetree = start_node;a = clock;% check to see if start_node connects directly to end_nodeif ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )... &&(collision(start_node,end_node,world,dim)==0) ) path = [start_node; end_node];else its = 0; numPaths = 0; flag = 0; while numPaths < 1, [tree,flag] = extendTree(tree,end_node,segmentLength,world,flag,dim); numPaths = numPaths + flag; its = its+1; end end% find path with minimum cost to end_nodepath = findMinimumPath(tree,end_node,dim);sizePath = size(path,1);b = clock;run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1figure;plotExpandedTree(world,tree,dim);plotWorld(world,path,dim);% figure(2);% plotWorld(world,path,dim);%plotExpandedTree(world,tree,dim);endendfunction world = createWorld(NumObstacles, endcorner, origincorner,dim) if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); maxRadius = 5*maxRadius/NumObstacles/2; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; end end elseif dim ==3; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)]; maxRadius = min(bounds); maxRadius = 5*maxRadius/NumObstacles; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; cz = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; world.cz(i) = cz; end end endendfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)NumObstacles = 5; if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; world.cx(1) = cx; world.cy(1) = cy; world.radius(2) = maxRadius; cx = 75; cy = 25; world.cx(2) = cx; world.cy(2) = cy; world.radius(3) = maxRadius; cx = 25; cy = 75; world.cx(3) = cx; world.cy(3) = cy; world.radius(4) = maxRadius; cx = 25; cy = 25; world.cx(4) = cx; world.cy(4) = cy; world.radius(5) = maxRadius; cx = 75; cy = 75; world.cx(5) = cx; world.cy(5) = cy; end elseif dim == 3 NumObstacles = 9; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; cz = 50; world.cx(1) = cx; world.cy(1) = cy; world.cz(1) = cz; world.radius(2) = maxRadius; cx = 25; cy = 25; cz = 25; world.cx(2) = cx; world.cy(2) = cy; world.cz(2) = cz; world.radius(3) = maxRadius; cx = 75; cy = 75; cz = 75; world.cx(3) = cx; world.cy(3) = cy; world.cz(3) = cz; world.radius(4) = maxRadius; cx = 25; cy = 25; cz = 75; world.cx(4) = cx; world.cy(4) = cy; world.cz(4) = cz; world.radius(5) = maxRadius; cx = 75; cy = 75; cz = 25; world.cx(5) = cx; world.cy(5) = cy; world.cz(5) = cz; world.radius(6) = maxRadius; cx = 25; cy = 75; cz = 25; world.cx(6) = cx; world.cy(6) = cy; world.cz(6) = cz; world.radius(7) = maxRadius; cx = 75; cy = 25; cz = 25; world.cx(7) = cx; world.cy(7) = cy; world.cz(7) = cz; world.radius(8) = maxRadius; cx = 75; cy = 25; cz = 75; world.cx(8) = cx; world.cy(8) = cy; world.cz(8) = cz; world.radius(9) = maxRadius; cx = 25; cy = 75; cz = 75; world.cx(9) = cx; world.cy(9) = cy; world.cz(9) = cz; end endendfunction node=generateRandomNode(world,dim)if dim ==2;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];endelseif dim ==3;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dim if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i)) collision_flag = 1; endendif collision_flag == 0 && dim ==2 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endelseif collision_flag == 0 && dim ==3 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endendendfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim) flag = 0; % check only last node added to tree since others have been checked if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)... & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ), flag = 1; endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,flag_chk,dim) r = 0; flag1 = 0; while flag1==0, % select a random point randomPoint = ones(1,dim); for i=1:dim randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; end % find leaf on node that is closest to randomPoint tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); min_parent_idx = idx; new_point = (randomPoint-tree(idx,1:dim)); new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(idx,:),new_point,dim); new_node = [new_point, 0, min_cost, idx]; if collision(new_node, tree(idx,:), world,dim)==0 tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point); dist = sqr_eucl_dist(tmp_dist,dim); near_idx = find(dist <= r^2); if size(near_idx,1)>1 size_near = size(near_idx,1); for i = 1:size_near if collision(new_node, tree(near_idx(i),:), world,dim)==0 cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim); if cost_near < min_cost min_cost = cost_near; min_parent_idx = near_idx(i); end end end end new_node = [new_point, 0 , min_cost, min_parent_idx]; new_tree = [tree; new_node]; new_node_idx = size(new_tree,1); if size(near_idx,1)>1 reduced_idx = near_idx; for j = 1:size(reduced_idx,1) near_cost = new_tree(reduced_idx(j),dim+2); lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim); if near_cost > min_cost + lcost ... && collision(new_tree(reduced_idx(j),:),new_node,world,dim) before = new_tree(reduced_idx(j),dim+3) new_tree(reduced_idx(j),dim+3) = new_node_idx; after = new_tree(reduced_idx(j),dim+3) end end end flag1=1; end end if flag_chk == 0 % check to see if new node connects directly to end_node if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(new_node,end_node,world,dim)==0) ) flag = 1; new_tree(end,dim+1)=1; % mark node as connecting to end. else flag = 0; end else flag = 1; endendfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);for i=1:dim sqr_e_dist(:,i) = array(:,i).*array(:,i); ende_dist = zeros(size(array,1),1);for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); endend%calculate the cost from a node to a pointfunction [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a nodefunction [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;cost = norm(diff);endfunction path = findMinimumPath(tree,end_node,dim) % find nodes that connect to end_node connectingNodes = []; for i=1:size(tree,1), if tree(i,dim+1)==1, connectingNodes = [connectingNodes ; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,dim+2)); % construct lowest cost path path = [connectingNodes(idx,:); end_node]; parent_node = connectingNodes(idx,dim+3); while parent_node>1, parent_node = tree(parent_node,dim+3); path = [tree(parent_node,:); path]; end endfunction plotExpandedTree(world,tree,dim) ind = size(tree,1); while ind>0 branch = []; node = tree(ind,:); branch = [ branch ; node ]; parent_node = node(dim+3); while parent_node > 1 cur_parent = parent_node; branch = [branch; tree(parent_node,:)]; parent_node = tree(parent_node,dim+3); end ind = ind - 1; if dim == 2 X = branch(:,1); Y = branch(:,2); p = plot(X,Y); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; elseif dim == 3 X = branch(:,1); Y = branch(:,2); Z = branch(:,3); p = plot3(X,Y,Z); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; end endendfunction plotWorld(world,path,dim) % the first element is the north coordinate % the second element is the south coordinate if dim ==2 N = 10; th = 0:2*pi/N:2*pi; axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2)]); hold on for i=1:world.NumObstacles, X = world.radius(i)*sin(th) + world.cx(i); Y = world.radius(i)*cos(th) + world.cy(i); fill(X,Y,'blue'); end X = path(:,1); Y = path(:,2); p = plot(X,Y); elseif dim ==3 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2),... world.origincorner(3), world.endcorner(3)]); hold on for i=1:world.NumObstacles, [X Y Z] = sphere(10); X = (X*world.radius(i)); Y = (Y*world.radius(i)); Z = (Z*world.radius(i)); surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i)); colormap([0.5 0.2 0.3]); end X = path(:,1); Y = path(:,2); Z = path(:,3); p = plot3(X,Y,Z); end set(p,'Color','black','LineWidth',3) xlabel('X axis'); ylabel('Y axis'); zlabel('Z axis'); title('RRT Extend Algorithm');end

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction [its,sizePath,run_time] = RRTstar3D(dim,segmentLength,radius,random_world,show_output,samples)% dim = 2;% radius =0;% segmentLength = 5;% random_world = 0;% n_its = 1000;% standard length of path segmentsif dim ==2start_cord = [5,5];goal_cord = [95,95];else start_cord = [5,5,5];goal_cord = [95,95,95];end% create random worldSize = 100;NumObstacles = 100;if random_world ==1world = createWorld(NumObstacles,ones(1,dim)*Size,zeros(1,dim),dim);else[world NumObstacles] = createKnownWorld(ones(1,dim)*Size,[0;0;0],dim);end% randomly select start and end nodes%start_node = generateRandomNode(world,dim)%end_node = generateRandomNode(world,dim)start_node = [start_cord,0,0,0];end_node = [goal_cord,0,0,0];% establish tree starting with the start nodetree = start_node;a = clock;% check to see if start_node connects directly to end_nodeif ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )... &&(collision(start_node,end_node,world,dim)==0) ) path = [start_node; end_node];else if samples >0 draw = samples/8; its = 0; numPaths = 0; flag = 0; for i = 1:samples [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim); numPaths = numPaths + flag; its = its+1; if its == draw tree_500 = tree; elseif its == draw*2 tree_1000 = tree; elseif its == draw*3 tree_1500 = tree; elseif its == draw*4 tree_2000 = tree; elseif its == draw*5 tree_2500 = tree; elseif its == draw*6 tree_3000 = tree; elseif its == draw*7 tree_3500 = tree; elseif its == draw*8 tree_4000 = tree; end end else its = 0; numPaths = 0; flag = 0; while numPaths < 1, [tree,flag] = extendTree(tree,end_node,segmentLength,radius,world,flag,dim); numPaths = numPaths + flag; its = its+1; end end end% find path with minimum cost to end_nodepath = findMinimumPath(tree,end_node,dim);b = clock;run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));path_500 = findMinimumPath(tree_500,end_node,dim);path_1000 = findMinimumPath(tree_1000,end_node,dim);path_1500 = findMinimumPath(tree_1500,end_node,dim);path_2000 = findMinimumPath(tree_2000,end_node,dim);path_2500 = findMinimumPath(tree_2500,end_node,dim);path_3000 = findMinimumPath(tree_3000,end_node,dim);path_3500 = findMinimumPath(tree_3500,end_node,dim);path_4000 = findMinimumPath(tree_4000,end_node,dim);sizePath = size(path,1);if show_output == 1figure;plotExpandedTree(world,tree_500,dim);plotWorld(world,path_500,dim);figure;plotExpandedTree(world,tree_1000,dim);plotWorld(world,path_1000,dim);figure;plotExpandedTree(world,tree_1500,dim);plotWorld(world,path_1500,dim);figure;plotExpandedTree(world,tree_2000,dim);plotWorld(world,path_2000,dim);figure;plotExpandedTree(world,tree_2500,dim);plotWorld(world,path_2500,dim);figure;plotExpandedTree(world,tree_3000,dim);plotWorld(world,path_3000,dim);figure;plotExpandedTree(world,tree_3500,dim);plotWorld(world,path_3500,dim);figure;plotExpandedTree(world,tree_4000,dim);plotWorld(world,path_4000,dim);figure;plotExpandedTree(world,tree,dim);plotWorld(world,path,dim);endendfunction world = createWorld(NumObstacles, endcorner, origincorner,dim) if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); maxRadius = 5*maxRadius/NumObstacles/2; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; end end elseif dim ==3; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)]; maxRadius = min(bounds); maxRadius = 5*maxRadius/NumObstacles; for i=1:NumObstacles, % randomly pick radius world.radius(i) = maxRadius*rand; % randomly pick center of obstacles cx = origincorner(1) + world.radius(i)... + (endcorner(1)-origincorner(1)-2*world.radius(i))*rand; cy = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; cz = origincorner(2) + world.radius(i)... + (endcorner(2)-origincorner(2)-2*world.radius(i))*rand; world.cx(i) = cx; world.cy(i) = cy; world.cz(i) = cz; end end endendfunction [world NumObstacles] = createKnownWorld(endcorner, origincorner,dim)NumObstacles = 5; if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; world.cx(1) = cx; world.cy(1) = cy; world.radius(2) = maxRadius; cx = 75; cy = 25; world.cx(2) = cx; world.cy(2) = cy; world.radius(3) = maxRadius; cx = 25; cy = 75; world.cx(3) = cx; world.cy(3) = cy; world.radius(4) = maxRadius; cx = 25; cy = 25; world.cx(4) = cx; world.cy(4) = cy; world.radius(5) = maxRadius; cx = 75; cy = 75; world.cx(5) = cx; world.cy(5) = cy; end elseif dim == 3 NumObstacles = 9; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) | (endcorner(2) <= origincorner(2)) | (endcorner(3) <= origincorner(3)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxRadius = 10; world.radius(1) = maxRadius; cx = 50; cy = 50; cz = 50; world.cx(1) = cx; world.cy(1) = cy; world.cz(1) = cz; world.radius(2) = maxRadius; cx = 25; cy = 25; cz = 25; world.cx(2) = cx; world.cy(2) = cy; world.cz(2) = cz; world.radius(3) = maxRadius; cx = 75; cy = 75; cz = 75; world.cx(3) = cx; world.cy(3) = cy; world.cz(3) = cz; world.radius(4) = maxRadius; cx = 25; cy = 25; cz = 75; world.cx(4) = cx; world.cy(4) = cy; world.cz(4) = cz; world.radius(5) = maxRadius; cx = 75; cy = 75; cz = 25; world.cx(5) = cx; world.cy(5) = cy; world.cz(5) = cz; world.radius(6) = maxRadius; cx = 25; cy = 75; cz = 25; world.cx(6) = cx; world.cy(6) = cy; world.cz(6) = cz; world.radius(7) = maxRadius; cx = 75; cy = 25; cz = 25; world.cx(7) = cx; world.cy(7) = cy; world.cz(7) = cz; world.radius(8) = maxRadius; cx = 75; cy = 25; cz = 75; world.cx(8) = cx; world.cy(8) = cy; world.cz(8) = cz; world.radius(9) = maxRadius; cx = 25; cy = 75; cz = 75; world.cx(9) = cx; world.cy(9) = cy; world.cz(9) = cz; end endendfunction node=generateRandomNode(world,dim)if dim ==2;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;chi = 0;cost = 0;node = [px, py, chi, cost, 0];endelseif dim ==3;% randomly pick configurationpx = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];% check collision with obstaclewhile collision(node, node, world,dim),px = (world.endcorner(1)-world.origincorner(1))*rand;py = (world.endcorner(2)-world.origincorner(2))*rand;pz = (world.endcorner(3)-world.origincorner(3))*rand;chi = 0;cost = 0;node = [px, py, pz, chi, cost, 0];endendendfunction collision_flag = collision(node, parent, world,dim)collision_flag = 0;for i=1:dim if (node(i)>world.endcorner(i))|(node(i)<world.origincorner(i)) collision_flag = 1; endendif collision_flag == 0 && dim ==2 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cx(i); world.cy(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endelseif collision_flag == 0 && dim ==3 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2);p(3)]-[world.cx(i); world.cy(i); world.cz(i)])<=1*world.radius(i)), collision_flag = 1; break; end end endendendfunction flag = canEndConnectToTree(tree,end_node,minDist,world,dim) flag = 0; % check only last node added to tree since others have been checked if ( (norm(tree(end,1:dim)-end_node(1:dim))<minDist)... & (collision(tree(end,1:dim), end_node(1:dim), world,dim)==0) ), flag = 1; endendfunction [new_tree,flag] = extendTree(tree,end_node,segmentLength,r,world,flag_chk,dim) flag1 = 0; while flag1==0, % select a random point randomPoint = ones(1,dim); for i=1:dim randomPoint(1,i) = (world.endcorner(i)-world.origincorner(i))*rand; end % find leaf on node that is closest to randomPoint tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); min_parent_idx = idx; new_point = (randomPoint-tree(idx,1:dim)); new_point = tree(idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(idx,:),new_point,dim); new_node = [new_point, 0, min_cost, idx]; if collision(new_node, tree(idx,:), world,dim)==0 tmp_dist = tree(:,1:dim)-(ones(size(tree,1),1)*new_point); dist = sqr_eucl_dist(tmp_dist,dim); near_idx = find(dist <= r^2); if size(near_idx,1)>1 size_near = size(near_idx,1); for i = 1:size_near if collision(new_node, tree(near_idx(i),:), world,dim)==0 cost_near = tree(near_idx(i),dim+2)+line_cost(tree(near_idx(i),:),new_point,dim); if cost_near < min_cost min_cost = cost_near; min_parent_idx = near_idx(i); end end end end new_node = [new_point, 0 , min_cost, min_parent_idx]; new_tree = [tree; new_node]; new_node_idx = size(new_tree,1); if size(near_idx,1)>1 reduced_idx = near_idx; for j = 1:size(reduced_idx,1) near_cost = new_tree(reduced_idx(j),dim+2); lcost = line_cost(new_tree(reduced_idx(j),:),new_point,dim); if near_cost > min_cost + lcost ... && collision(new_tree(reduced_idx(j),:),new_node,world,dim) before = new_tree(reduced_idx(j),dim+3) new_tree(reduced_idx(j),dim+3) = new_node_idx; after = new_tree(reduced_idx(j),dim+3) end end end flag1=1; end end if flag_chk == 0 % check to see if new node connects directly to end_node if ( (norm(new_node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(new_node,end_node,world,dim)==0) ) flag = 1; new_tree(end,dim+1)=1; % mark node as connecting to end. else flag = 0; end else flag = 1; endendfunction e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);for i=1:dim sqr_e_dist(:,i) = array(:,i).*array(:,i); ende_dist = zeros(size(array,1),1);for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); endend%calculate the cost from a node to a pointfunction [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;end%calculate the cost from a node to a nodefunction [cost] = cost_nn(from_node,to_node,dim)diff = from_node(:,1:dim) - to_node(:,1:dim);eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;endfunction [cost] = line_cost(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;cost = norm(diff);endfunction path = findMinimumPath(tree,end_node,dim) % find nodes that connect to end_node connectingNodes = []; for i=1:size(tree,1), if tree(i,dim+1)==1, connectingNodes = [connectingNodes ; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,dim+2)); % construct lowest cost path path = [connectingNodes(idx,:); end_node]; parent_node = connectingNodes(idx,dim+3); while parent_node>1, parent_node = tree(parent_node,dim+3); path = [tree(parent_node,:); path]; end endfunction plotExpandedTree(world,tree,dim) ind = size(tree,1); while ind>0 branch = []; node = tree(ind,:); branch = [ branch ; node ]; parent_node = node(dim+3); while parent_node > 1 cur_parent = parent_node; branch = [branch; tree(parent_node,:)]; parent_node = tree(parent_node,dim+3); end ind = ind - 1; if dim == 2 X = branch(:,1); Y = branch(:,2); p = plot(X,Y); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; elseif dim == 3 X = branch(:,1); Y = branch(:,2); Z = branch(:,3); p = plot3(X,Y,Z); set(p,'Color','r','LineWidth',0.5,'Marker','.','MarkerEdgeColor','g'); hold on; end endendfunction plotWorld(world,path,dim) % the first element is the north coordinate % the second element is the south coordinate if dim ==2 N = 10; th = 0:2*pi/N:2*pi; axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2)]); hold on for i=1:world.NumObstacles, X = world.radius(i)*sin(th) + world.cx(i); Y = world.radius(i)*cos(th) + world.cy(i); fill(X,Y,'blue'); end X = path(:,1); Y = path(:,2); p = plot(X,Y); elseif dim ==3 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2),... world.origincorner(3), world.endcorner(3)]); hold on for i=1:world.NumObstacles, [X Y Z] = sphere(10); X = (X*world.radius(i)); Y = (Y*world.radius(i)); Z = (Z*world.radius(i)); surf(X+world.cx(i),Y+world.cy(i),Z+world.cz(i)); colormap([0.5 0.2 0.3]); end X = path(:,1); Y = path(:,2); Z = path(:,3); p = plot3(X,Y,Z); end set(p,'Color','black','LineWidth',3) xlabel('X axis'); ylabel('Y axis'); zlabel('Z axis'); title('RRT Star Algorithm');end

function RRT% clc;% %close all;% clear all;num_of_runs =1;run_RRTconnect =1;dim = 3;stepsize = 1;random_world = 0;show_output = 1;for sits = 1:stepsize segmentLength = 0.1; if run_RRTconnect == 1 time = 0; avg_its = 0; avg_path = 0; for i = 1:num_of_runs [n_its,path_n,run_time] = RRTconnect_3D(dim,segmentLength,random_world,show_output); time = time + run_time; avg_its = avg_its + n_its; avg_path = avg_path + path_n; end str1 = ['The time taken by RRT-Connect for ', num2str(num_of_runs), ' runs is ', num2str(time)]; str2 = ['The averagae time taken by RRT-Connect for each run is ', num2str(time/num_of_runs)]; str3 = ['The averagae number of states explored by RRT-Connect for each run is ', num2str(avg_its/num_of_runs)]; str4 = ['The averagae number of state in Path by RRT-Connect for each run is ', num2str(avg_path/num_of_runs)]; disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); disp(str1); disp(str2); disp(str3); disp(str4); disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%'); plot3(0.15,0,0.36,'.k','Markersize',30); plot3([0.15 0.15],[0 0],[0.27 0.45],'r','LineWidth',5); endend end

%Author : Adnan Munawar%Email : amunawar@wpi.edu ; adnan.munawar@live.com%MS Robotics, Worcester Polytechnic Institutefunction [nIterations,sizePath,run_time] = RRTconnect_3D(dim,segmentLength,random_world,show_output)% dim = 2;% segmentLength = 5;% random_world = 0;random_world表示是否使用随机障碍物（1）还是事先设定好的障碍物（0）% standard length of path segments% start_cord - 开始节点坐标% goal_cord - 目标节点坐标if dim ==2start_cord = [5,5];goal_cord = [95,95];elsestart_cord = [-0.0048,-0.12,0.22];goal_cord = [0.146,0,0.389];end%----------------------- create random world-----------------------------%Size = 1; %世界的坐标轴尺寸NumObstacles = 10; %障碍物的个数OriginWorld=[-0.4,-0.4,0];EndWorld=[0.5,0.5,0.8];if random_world ==1world = createWorld(NumObstacles,EndWorld(1,1:dim)*Size,OriginWorld(1,1:dim)*Size,dim);elseOriginWorld=OriginWorld';[world NumObstacles] = createKnownWorld(EndWorld(1,1:dim)*Size,OriginWorld(1:dim,1)*Size,dim);end%---------------------- set starPoint and endPoint----------------------%%%node = [point,goal_flag,cost,min_parent_idx]start_node = [start_cord,0,0,0];end_node = [goal_cord,0,0,0];%----------------establish tree starting with the start node------------%tree = start_node;a = clock;%--------check to see if start_node connects directly to end_node-------%if ( (norm(start_node(1:dim)-end_node(1:dim))<segmentLength )... &&(collision(start_node,end_node,world,dim)==0) ) path = [start_node; end_node];%------------Make randPoint as newPoint,Constant iterative-------------%else nIterations = 0; numPaths = 0; flag = 0; while numPaths<1, [tree,flag] = extendTree(tree,end_node,segmentLength,world,dim); %%每次生成随机节点，从树中最近点拓展到该随机节点，并作为树中的新节点 numPaths = numPaths + flag; nIterations = nIterations+1; endend%-----------------find Minimum Path------------------------------------%path = findMinimumPath(tree,end_node,dim);sizePath = size(path,1);b = clock;%-----------------calculate Simulation time-----------------------------------%run_time = 3600*(b(4)-a(4)) + 60 * (b(5)-a(5)) + (b(6) - a(6));if show_output == 1% find path with minimum cost to end_nodefigure;plotExpandedTree(world,tree,dim);plotWorld(world,path,dim);endend%%%*******************************生成随机障碍物地图***************************************%%%%function world = createWorld(NumObstacles, endcorner, origincorner,dim)% endcorner - 地图右上角的坐标，即终点坐标% oringincorner - 地图左下角的坐标，即初始点的坐标% NumObstacles - 随机障碍物的个数 if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles maxlength = min(endcorner(1)- origincorner(1), endcorner(2)-origincorner(2)); %返回最小的坐标长度 maxlength = maxlength/NumObstacles; for i=1:NumObstacles, % randomly pick radius world.length(i) = maxlength*rand; world.wide(i) = maxlength*rand; % randomly pick center of obstacles cx = origincorner(1) + world.length(i)... + (endcorner(1)-origincorner(1)-2*world.length(i))*rand; cy = origincorner(2) + world.wide(i)... + (endcorner(2)-origincorner(2)-2*world.wide(i))*rand; world.cx(i) = cx; world.cy(i) = cy; % randomly pick length\wide\high end end elseif dim ==3; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)) disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles bounds = [endcorner(1)- origincorner(1), endcorner(2)-origincorner(2), endcorner(3)-origincorner(3)]; maxlength = min(bounds); maxlength = 2*maxlength/NumObstacles; for i=1:NumObstacles, % randomly pick length\wide\high world.length(i) = maxlength*rand; world.wide(i) = maxlength*rand; world.high(i) = maxlength*rand; % randomly pick center of obstacles cx = origincorner(1) + world.length(i)... + (endcorner(1)-origincorner(1)-2*world.length(i))*rand; cy = origincorner(2) + world.wide(i)... + (endcorner(2)-origincorner(2)-2*world.wide(i))*rand; cz = origincorner(2) + world.high(i)... + (endcorner(2)-origincorner(2)-2*world.high(i))*rand; world.cx(i) = cx; world.cy(i) = cy; world.cz(i) = cz; end end endend%%%*******************************生成已知障碍物地图***************************************%%%%function [world,NumObstacles] = createKnownWorld(endcorner, origincorner,dim)NumObstacles = 5; if dim == 2 % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles length = 0.4; wide = 0.6; world.length(1) = length; world.wide(1) = wide; cx = 50; cy = 50; world.cx(1) = cx; world.cy(1) = cy; end elseif dim == 3 NumObstacles = 2; % check to make sure that the region is nonempty if (endcorner(1) <= origincorner(1)) || (endcorner(2) <= origincorner(2)) || (endcorner(3) <= origincorner(3)), disp('Not valid corner specifications!') world=[]; % create world data structure else world.NumObstacles = NumObstacles; world.endcorner = endcorner; world.origincorner = origincorner; % create NumObstacles length(1) = 0.4; wide(1) = 0.6; high(1) = 0.27; world.length(1) = length(1); world.wide(1) = wide(1); world.high(1) = high(1); cx = 0.3; cy = 0; cz = 0.135; world.cx(1) = cx; world.cy(1) = cy; world.cz(1) = cz; length(2) = 0.1; wide(2) = 0.2; high(2) = 0.52; world.length(2) = length(2); world.wide(2) = wide(2); world.high(2) = high(2); cx = 0; cy = 0; cz = 0.26; world.cx(2) = cx; world.cy(2) = cy; world.cz(2) = cz; end endend%%%*******************************检测是否冲突***************************************%%%%function collision_flag = collision(node, parent, world,dim)% node - 节点端点% parent - 另一节点端点collision_flag = 0;%是否超出地图范围for i=1:dim if (node(i)>world.endcorner(i))||(node(i)<world.origincorner(i)) collision_flag = 1; endend%是否在障碍物的范围内if collision_flag == 0 && dim ==2 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2)),%%求矩阵范数，即节点在障碍物范围内 collision_flag = 1; break; end end endelseif collision_flag == 0 && dim ==3 for sigma = 0:.2:1, p = sigma*node(1:dim) + (1-sigma)*parent(1:dim); % check each obstacle for i=1:world.NumObstacles, if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2) && (abs(p(3)-world.cz(i))<=world.high(i)/2)), collision_flag = 1; break; end end endendend%%%*******************************检测节点是否可用***************************************%%%%function collision_flag = is_point_valid(point, world,dim)collision_flag = 0;%是否超出地图范围for i=1:dim if (point(i)>world.endcorner(i))||(point(i)<world.origincorner(i)) collision_flag = 1; endend%是否在障碍物的范围内if collision_flag == 0 && dim ==2 p = point(1:dim); % check each obstacle for i=1:world.NumObstacles, if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2)), collision_flag = 1; break; end endelseif collision_flag == 0 && dim ==3 p = point(1:dim); % check each obstacle for i=1:world.NumObstacles, if ((abs(p(1)-world.cx(i))<=world.length(i)/2) && (abs(p(2)-world.cy(i))<=world.wide(i)/2) && (abs(p(3)-world.cz(i))<=world.high(i)/2)), collision_flag = 1; break; end endendend%%%****************************从工作空间中随机选择节点**************************************%%%%function point=ChooseRandPoint load('RHandPos_3Freedom.mat'); [m,n]=size(HandPos); randIndex = randperm(m,1); point=[HandPos(randIndex,1),HandPos(randIndex,2),HandPos(randIndex,3)];end%%%*******************************树节点拓展***************************************%%%%function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,dim) flag = 0; point_valid_flag = 1; % select a random point randomPoint = zeros(1,dim); % whether randomPoint is valid while point_valid_flag ==1% for i=1:dim% randomPoint(i) = (world.endcorner(i)-world.origincorner(i))*rand+world.origincorner(i); %%%在地图中产生随机节点 % end randomPoint = ChooseRandPoint; point_valid_flag = is_point_valid(randomPoint, world,dim); end % find leaf on node that is closest to randomPoint % -选择tree中节点和随机节点randomPoint的欧式距离最小的点为 newpoint（最近节点nearPoint） tmp = tree(:,1:dim)-ones(size(tree,1),1)*randomPoint; sqrd_dist = sqr_eucl_dist(tmp,dim); [min_dist,idx] = min(sqrd_dist); min_parent_idx = idx; new_point = tree(idx,1:dim); new_node = tree(idx,:); pflag = 0; %%%从树节点中的nearPoint沿randomPoint方向，不断向randomPoint拓展，直到到达randomPoint为止 while norm(new_point-randomPoint)>0 && pflag==0 %随机节点和上一个新节点（nearPoint）的距离小于segmentLength时，将随机节点取为下一个新的节点 if norm(new_point-randomPoint)<segmentLength pflag = collision(randomPoint,tree(min_parent_idx,:),world,dim); if pflag == 0 new_point = randomPoint; min_cost = cost_np(tree(min_parent_idx,:),new_point,dim);%%计算上一个newPoint（nearPoint）到下一个newPoint（randPoint）的代价值， new_node = [new_point,0,min_cost,min_parent_idx];%%min_cost为从树的主节点到最终选择的newPoint的代价值累加和 tree = [tree;new_node]; %%增加新的树节点 pflag = 1; goal_flag = is_goal(new_node,end_node,segmentLength,world,dim); if goal_flag == 1; tree(end,dim+1)=1; flag = 1; end end %随机节点和上一个新节点（nearPoint）的距离大于segmentLength时，在上一个节点沿随机节点方向取步长为segmentLength的节点为下一个新节点 else new_point = (randomPoint-tree(min_parent_idx,1:dim)); new_point = tree(min_parent_idx,1:dim)+(new_point/norm(new_point))*segmentLength; min_cost = cost_np(tree(min_parent_idx,:),new_point,dim); new_node = [new_point, 0, min_cost, min_parent_idx]; pflag = collision(new_node,tree(min_parent_idx,:),world,dim); if pflag == 0 tree = [tree ; new_node]; min_parent_idx = size(tree,1); goal_flag = is_goal(new_node,end_node,segmentLength,world,dim); if goal_flag == 1; tree(end,dim+1)=1; % mark node as connecting to end. pflag = 1; flag = 1; end end end end new_tree = tree; end %%%*******************是否到达目标**********************************%%%% function goal_flag = is_goal(node,end_node,segmentLength,world,dim) goal_flag = 0; if (norm(node(1:dim)-end_node(1:dim))<segmentLength )... && (collision(node,end_node,world,dim)==0) goal_flag = 1; endend %%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%% function e_dist = sqr_eucl_dist(array,dim)sqr_e_dist = zeros(size(array,1),dim);%array中元素平方值for i=1:dim sqr_e_dist(:,i) = array(:,i).*array(:,i); ende_dist = zeros(size(array,1),1);for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); endend%%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%% %calculate the cost from a node to a pointfunction [cost] = cost_np(from_node,to_point,dim)diff = from_node(:,1:dim) - to_point;eucl_dist = norm(diff);cost = from_node(:,dim+2) + eucl_dist;end%%%*******************找到所有树节点到目标点的最短路径**********************************%%%% function path = findMinimumPath(tree,end_node,dim) % find nodes that connect to end_node connectingNodes = []; for i=1:size(tree,1), if tree(i,dim+1)==1, connectingNodes = [connectingNodes ; tree(i,:)]; end end % find minimum cost last node [tmp,idx] = min(connectingNodes(:,dim+2)); % construct lowest cost path path = [connectingNodes(idx,:); end_node]; parent_node = connectingNodes(idx,dim+3); while parent_node>1, parent_node = tree(parent_node,dim+3); path = [tree(parent_node,:); path]; end endfunction plotExpandedTree(world,tree,dim)ind = size(tree,1); while ind>0 size(tree); branch = []; node = tree(ind,:); branch = [ branch ; node ]; parent_node = node(dim+3); while parent_node > 1 cur_parent = parent_node; branch = [branch; tree(parent_node,:)]; parent_node = tree(parent_node,dim+3); end ind = ind - 1; if dim == 2 X = branch(:,1); Y = branch(:,2); p = plot(X,Y); set(p,'Color','r','LineWidth',0.5,'Marker','*','MarkerEdgeColor','m'); hold on; elseif dim == 3 X = branch(:,1); Y = branch(:,2); Z = branch(:,3); p = plot3(X,Y,Z); set(p,'Color','r','LineWidth',0.5,'Marker','*','MarkerEdgeColor','m'); hold on; end endendfunction plotWorld(world,path,dim) % the first element is the north coordinate % the second element is the south coordinate if dim ==2 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2)]); hold on for i=1:world.NumObstacles x0 = world.cx(i); y0 = world.cy(i); Lx = world.length(i); Ly = world.wide(i); x=[x0-Lx/2 x0-Lx/2 x0+Lx/2 x0+Lx/2]; y=[y0-Ly/2 y0+Ly/2 y0+Ly/2 y0-Ly/2]; fill(x,y,'blue'); end X = path(:,1); Y = path(:,2); p = plot(X,Y); elseif dim ==3 axis([world.origincorner(1),world.endcorner(1),... world.origincorner(2), world.endcorner(2),... world.origincorner(3), world.endcorner(3)]); hold on for i=1:world.NumObstacles %(x0,y0,z0)是中心点的位置; (Lx,Ly,Lz)是长方体的长宽高. x0 = world.cx(i); y0 = world.cy(i); z0 = world.cz(i); Lx = world.length(i); Ly = world.wide(i); Lz = world.high(i); x=[x0-Lx/2 x0-Lx/2 x0-Lx/2 x0-Lx/2 x0+Lx/2 x0+Lx/2 x0+Lx/2 x0+Lx/2]; y=[y0-Ly/2 y0-Ly/2 y0+Ly/2 y0+Ly/2 y0-Ly/2 y0-Ly/2 y0+Ly/2 y0+Ly/2]; z=[z0-Lz/2 z0+Lz/2 z0+Lz/2 z0-Lz/2 z0+Lz/2 z0-Lz/2 z0-Lz/2 z0+Lz/2]; index=zeros(6,5); index(1,:)=[1 2 3 4 1]; %按一定顺序得到长方体角点的位置 index(2,:)=[5 6 7 8 5]; index(3,:)=[2 1 6 5 2]; index(4,:)=[4 3 8 7 4]; index(5,:)=[1 6 7 4 1]; index(6,:)=[8 5 2 3 8]; for k=1:6 plot3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'r'); fill3(x(index(k,:)),y(index(k,:)),z(index(k,:)),'c'); %填充多边形函数 hold on end end X = path(:,1) Y = path(:,2) Z = path(:,3) p = plot3(X,Y,Z); end set(p,'Color','black','LineWidth',3) xlabel('X axis'); ylabel('Y axis'); zlabel('Z axis'); title('RRT Connect Algorithm');end