RRT路径规划

%% 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

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