3维RRT避障路径规划算法

在3维空间内创建一个峰面障碍物，给定起始点和终止点，通过RRT搜索可以有效避开障碍物找到一条可行路线。

function c;
clc
clear all
close all
%map1 随机地表。
% a=10;
% b=0.2;
% c=0.1;
% d=0.6;
% e=1;
% f=0.1;
% g=0.1;
% for x=1:80
%     for y=1:80
% Z1=sin（y+a）+b*sin（x）+cos（d*（x^2+y^2）^（1/2））+e*cos（y）+f*sin（f*（x^2+y^2）^（1/2））+g*cos（y）;
% % Z1=SquareDiamond（628）;
%  figure（1）;
%  surf（Z1）; %画出三维曲面 
%  shading flat; %各小曲面之间不要网格 
% %map2 山峰图
tic;
h=[203525382025];
x0=[104045602020];
y0=[102550304510];
xi=[5.5854.55.53.5];
yi=[5765.564.5];
Z2=CeatHill（6hx0y0xiyi80）; 
figure（2）;
surf（Z2）; %画出三维曲面 
shading flat; %各小曲面之间不要网格 
%map3 合成图
%  Z3=max（Z1Z2）;
%  figure（3）;
%  surf（Z3）; %画出三维曲面 
%  shading flat; %各小曲面之间不要网格 
segmentLength =5;
start_node =  [5705000];
end_node   =[803010100];
hold on
plot3（start_node（:1）start_node（:2）start_node（:3）‘r*‘）;
plot3（end_node（:1）end_node（:2）end_node（:3）‘r*‘）;
tree = start_node;
if （ （norm（start_node（1:3）-end_node（1:3））    &（collision（start_nodeend_node）==0） ）
  path = [start_node; end_node];
  else
  numPaths = 0;
  while numPaths<1
      [treeflag] = extendTree（treeend_nodesegmentLengthZ2）;
      numPaths = numPaths + flag;
  end
end
 path = findMinimumPath（tree）;
 plot3（path（:1）path（:2）path（:3）‘r‘）;  
 toc;
 function [data]=CeatHill（Nhx0y0xiyinum） 
x=1:1:num;y=1:1:num;
for m=1:num
    for n=1:num
        Sum=0;
        for k=1:N
           s=h（k）*exp（-（（x（m）-x0（k））/xi（k））^2-（（y（n）-y0（k））/yi（k））^2）;
           Sum=Sum+s;
        end
        data（mn）=Sum;
    end
end

 function collision_flag = collision（node parentZ2high）;
collision_flag = 0;
 h=[203525382025];
x0=[104045602020];
y0=[102550304510];
xi=[5.5854.55.53.5];
yi=[5765.564.5];
Z1=Z2;
% for i=1:80
%     for j=1:80
%         if（Z1（ji）>high）
%             Z1（ji）=10000;
%         end
%     end
% end
if （（node（1）>80）...
    | （node（1）<0）...
    | （node（2）>80）...
    | （node（2）<0））
  collision_flag = 1;
else
     for sigma = 0:0.1:1
         p = sigma*node（1:3） + （1-sigma）*parent（1:3）;
          Sum1=0;
        for k=1:6
           s=h（k）*exp（-（（p（2）-x0（k））/xi（k））^2-（（p（1）-y0（k））/yi（k））^2）;
           Sum1=Sum1+s;
        end
        if（p（3）            collision_flag = 1;
        end
% [linerow]=find（Z1==10000）;
% ct=length（line）;
% for tt=1:ct
%     B=floor（p（1））;
%     C=floor（p（2））;
% if （B==line（tt1）&C==row（tt1））      
%         collision_flag = 1;
% end
% end

     end
end
 function [new_treeflaghigh] = extendTree（treeend_nodesegmentLengthZ2）;
  flag1 = 0;
  qet=1;
  while flag1==0
    % select a random point
    if（qet==0）
        randomPoint = [...
        80*rand...
        80*rand...
        80*rand];
    else
        randomPoint = [...
        end_node（:1）...
        end_node（:2）...
        end_node（:3）];
    end
    % find leaf on node that is closest to randomPoint
    %0.45*sqrt（tree（:1:

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        4925  2017-12-26 15:53  3维避障RRT算法\rrtsanwei.m
     目录           0  2018-05-19 18:25  3维避障RRT算法\