RRT、RRTConnect、RRT*——Matlab算法
1.RRT
RRT算法倾向于拓展到开放的未探索区域，只要时间足够，迭代次数足够多，没有不会被探索到的区域。
2.RRT-Connect
RRT-Connect算法：基于RRT搜索空间的盲目性，节点拓展环节缺乏记忆性的缺点，为了提高空间内的搜索速。在RRT算法的基础上加上了两棵树双向抖索的引导策略，并且在生长方式的基础上加上了贪婪策略加快了搜索速度，并且减少了空白区域的无用搜索，节省了搜索时间。
3.RRT*算法
RRT-Connect算法增加了启发式策略，以及贪婪思想，但RRT算法和RRT-Connect算法的共同缺点是，他们的路径都不是最优的，没有添加评价路径长短花费的函数，搜索路径策略都是基于随机采样的搜索。渐进最优的RRT*算法，该算法在原有的RRT算法上，改进了父节点选择的方式，采用代价函数来选取拓展节点领域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算的复杂度和渐进最优解。（如：基于高斯采样策略的RRT*算法）
4.代码
代码的原地址为：https://github.com/adnanmunawar/matlab-rrt-variants
代码中包含了：RRT-Connect、LazyRRT、RRTextend、RRT*的2D和3D算法
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 run All 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 Institute function benchmarkRRT clc; 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]; end end if 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 Institute function [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 ==2 start_cord = [5,5]; goal_cord = [95,95]; else start_cord = [5,5,5]; goal_cord = [95,95,95]; end %----------------------- create random world-----------------------------% Size = 100; %世界的坐标轴尺寸 NumObstacles = 100; %障碍物的个数 if random_world ==1 world = 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; end end %-----------------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_node figure; plotExpandedTree(world,tree,dim); plotWorld(world,path,dim); end end %%%*******************************生成随机障碍物地图***************************************%%%% 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 end end %%%*******************************生成已知障碍物地图***************************************%%%% 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 end end %%%*******************************生成随机节点***************************************%%%% function node=generateRandomNode(world,dim) if dim ==2; % randomly pick configuration 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]; % check collision with obstacle while 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]; end elseif dim ==3; % randomly pick configuration 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]; % check collision with obstacle while 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]; end end end %%%*******************************检测是否冲突***************************************%%%% 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; end end %是否在障碍物的范围内 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 end elseif 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 end end end %%%*******************************检测节点是否可用***************************************%%%% 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; end end %是否在障碍物的范围内 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 end elseif 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 end end end %%%*******************************检测最后一个节点是否需要**********************************%%%% 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; end end %%%*******************************树节点拓展***************************************%%%% 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; end end %%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%% 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); end e_dist = zeros(size(array,1),1); for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); end end %%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%% %calculate the cost from a node to a point function [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 node function [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; end function [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 end function 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 end end function 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 Institute function [nIterations,sizePath,run_time] = LazyRRT3D(dim,segmentLength,random_world,show_output) % dim = 2; % segmentLength = 5; % random_world = 0; if dim ==2 start_cord = [5,5]; goal_cord = [95,95]; else start_cord = [5,5,5]; goal_cord = [95,95,95]; end % create random world Size = 100; NumObstacles = 100; if random_world ==1 world = 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 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]; 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; end end % find path with minimum cost to end_node LazyPath = 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 == 1 figure; plotExpandedTree(world,tree,dim); plotWorld(world,path,dim); % figure(2); % plotWorld(world,path,dim); %plotExpandedTree(world,tree,dim); end end function 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 end end function 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 end end 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 end end function node=generateRandomNode(world,dim) if dim ==2; % randomly pick configuration 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]; % check collision with obstacle while 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]; end elseif dim ==3; % randomly pick configuration 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]; % check collision with obstacle while 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]; end end end function 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; end end 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 end elseif 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 end end end 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; end end function [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. end end function [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 end function 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); end e_dist = zeros(size(array,1),1); for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); end end %calculate the cost from a node to a point function [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 node function [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; end function [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 end function 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 end end function 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 Institute function [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 segments if dim ==2 start_cord = [5,5]; goal_cord = [95,95]; else start_cord = [5,5,5]; goal_cord = [95,95,95]; end % create random world Size = 100; NumObstacles = 100; if random_world ==1 world = 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 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]; 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_node path = 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 == 1 figure; plotExpandedTree(world,tree,dim); plotWorld(world,path,dim); % figure(2); % plotWorld(world,path,dim); %plotExpandedTree(world,tree,dim); end end function 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 end end 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 end end function node=generateRandomNode(world,dim) if dim ==2; % randomly pick configuration 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]; % check collision with obstacle while 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]; end elseif dim ==3; % randomly pick configuration 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]; % check collision with obstacle while 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]; end end end function 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; end end 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 end elseif 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 end end end 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; end end function [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; end end function 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); end e_dist = zeros(size(array,1),1); for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); end end %calculate the cost from a node to a point function [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 node function [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; end function [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 end function 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 end end function 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 Institute function [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 segments if dim ==2 start_cord = [5,5]; goal_cord = [95,95]; else start_cord = [5,5,5]; goal_cord = [95,95,95]; end % create random world Size = 100; NumObstacles = 100; if random_world ==1 world = 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 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]; 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_node path = 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 == 1 figure; 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); end end function 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 end end 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 end end function node=generateRandomNode(world,dim) if dim ==2; % randomly pick configuration 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]; % check collision with obstacle while 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]; end elseif dim ==3; % randomly pick configuration 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]; % check collision with obstacle while 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]; end end end function 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; end end 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 end elseif 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 end end end 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; end end function [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; end end function 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); end e_dist = zeros(size(array,1),1); for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); end end %calculate the cost from a node to a point function [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 node function [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; end function [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 end function 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 end end function 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
修改后：
将RRT-Connect代码中的障碍物由圆形修改为长方体，并且将随机节点改为从工作空间中随机选择节点，保证求解的可用性。
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); end end end
%Author : Adnan Munawar %Email : amunawar@wpi.edu ; adnan.munawar@live.com %MS Robotics, Worcester Polytechnic Institute function [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 ==2 start_cord = [5,5]; goal_cord = [95,95]; else start_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 ==1 world = createWorld(NumObstacles,EndWorld(1,1:dim)*Size,OriginWorld(1,1:dim)*Size,dim); else OriginWorld=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; end end %-----------------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_node figure; plotExpandedTree(world,tree,dim); plotWorld(world,path,dim); end end %%%*******************************生成随机障碍物地图***************************************%%%% 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 end end %%%*******************************生成已知障碍物地图***************************************%%%% 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 end end %%%*******************************检测是否冲突***************************************%%%% 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; end end %是否在障碍物的范围内 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 end elseif 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 end end end %%%*******************************检测节点是否可用***************************************%%%% 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; end end %是否在障碍物的范围内 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 end elseif 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 end end end %%%****************************从工作空间中随机选择节点**************************************%%%% 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; end end %%%*******************计算各个节点之间的欧式距离（矩阵中向量的二范数）**********************************%%%% 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); end e_dist = zeros(size(array,1),1); for i=1:dim e_dist = e_dist+sqr_e_dist(:,i); end end %%%*******************用树所有节点到另一节点坐标的范数值作为节点间的代价值**********************************%%%% %calculate the cost from a node to a point function [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 end function 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 end end function 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
