解决含有运动力学约束的路径规划问题的RRT算法 （ 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 = 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

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

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        7123  2013-12-05 14:44  pathRRT.m