path-planning 路径规划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 =[0 0 0 0 0 ];
end_node   = [100 100 0 0 0];

% 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
 
      
  % create world data structure

    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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% collision
%%   check to see if a node is in collsion with obstacles
function collision_flag = collision（node parent world）;

collision_fla