• 大小: 7KB
    文件类型: .m
    金币: 1
    下载: 0 次
    发布日期: 2021-06-07
  • 语言: Matlab
  • 标签: RRT  

资源简介

在2D环境下,随机产生起始点和终点,以及障碍,然后用快速搜索随机树(RRT)算法来实现,本编码是在原来一个经典样例的基础上,在matlab2009a版本的环境下修改而成,并加注注释,保证好用,希望对朋友们有所帮助!

资源截图

代码片段和文件信息

%% pathRRT
%%  - create a path from a start node to an end node
%%    using the RRT algorithm.
%%  - RRT = Rapidly-exploring Random Tree
%%  
%% 赵燕江修改版

function pathRRT;

% create random world
Size = 100;
NumObstacles = 100;
world = createWorld(NumObstacles[Size; Size][0;0]);

% standard length of path segments
segmentLength = 5;

% randomly select start and end nodes
start_node = generateRandomNode(world);
end_node   = generateRandomNode(world);

% establish tree starting with the start node
tree = start_node;

% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:2)-end_node(1:2))    &(collision(start_nodeend_nodeworld)==0) )
  path = [start_node; end_node];
else
  numPaths = 0;
  while numPaths<1
      [treeflag] = extendTree(treeend_nodesegmentLengthworld);
      numPaths = numPaths + flag;
  end
end

% find path with minimum cost to end_node
path = findMinimumPath(treeend_node);
plotWorld(worldpathtree);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% createWorld
%%  - create random world with obstacles
%%  the first element is the north coordinate
%%  the second element is the south coordinate
function world = createWorld(NumObstacles NEcorner SWcorner);

  % check to make sure that the region is nonempty
  if (NEcorner(1) <= SWcorner(1)) | (NEcorner(2) <= SWcorner(2))
      disp(‘Not valid corner specifications!‘)
      world=[];
      
  % create world data structure
  else
    world.NumObstacles = NumObstacles;
    world.NEcorner = NEcorner;
    world.SWcorner = SWcorner;
                          
    % create NumObstacles 
    maxRadius = min(NEcorner(1)- SWcorner(1) NEcorner(2)-SWcorner(2));
    maxRadius = 5*maxRadius/NumObstacles/2;
    for i=1:NumObstacles
        % randomly pick radius
        world.radius(i) = maxRadius*rand;
        % randomly pick center of obstacles
        cn = SWcorner(1) + world.radius(i)...
            + (NEcorner(1)-SWcorner(1)-2*world.radius(i))*rand;
        ce = SWcorner(2) + world.radius(i)...
            + (NEcorner(2)-SWcorner(2)-2*world.radius(i))*rand;
        world.cn(i) = cn;
        world.ce(i) = ce;
    end
  end
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% generateRandomNode
%%   create a random node (initialize)
function node=generateRandomNode(world);

% randomly pick configuration
pn       = (world.NEcorner(1)-world.SWcorner(1))*rand;
pe       = (world.NEcorner(2)-world.SWcorner(2))*rand;
chi      = 0;
cost     = 0;
node     = [pn pe chi cost 0];

% check collision with obstacle
while collision(node node world)
  pn       = (world.NEcorner(1)-world.SWcorner(1))*rand;
  pe       = (world.NEcorner(2)-world.SWcorner(2))*rand;
  chi      = 0;
  cost     = 0;
  node     = [pn pe chi cost 0];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

评论

共有 条评论