RRT算法三维避障的MATLAB实现

RRT算法又称为快速随机扩展数算法，是一种普适路径规划算法， 为什么说是普适算法，因为它什么样的苛刻的条件都会极大的可能性找到一条路径。

但是这样的算法也往往会伴随缺点：

1.每次迭代都是在随机找点，这就导致了迭代时间很大程度依赖运气

2.由于这种算法的随机性强，导致在很简单的避障环境下也很难快速找到一条路径

本文针对最初始的RRT算法(没经过任何对算法的改进)进行代码上的仿真,意在多个障碍物的条件下，找到一条路径即可，算法本身不涉及优化，所以对寻找到的路径也没有做任何优化。

RRT算法模型在网上有详细的解释，而且非常通俗易懂，这里不做赘述，只给出了代码部分实现。这是本人对RRT算法的一些自己的见解，或许有些浅显，只是希望能给你们或多或少的帮助，不喜勿喷。

（ps：因为有很多人问我相关的问题，再就是这个代码本身写的并不是很好，所以我写了一篇代码可读性较好的RRT算法文章，在我的另一篇博客里《手把手教RRT算法》，里面还附带了整个视频教学的资料地址，内容可以说是非常详细，很适合初学者）

代码简介：

在一个充满长方体和圆柱体障碍物的空间，给出任意一些点作为目标点进行路径规划

 迭代完毕后如下部分红色线所示，为找到的路径

 代码部分：

主函数，文件命名为rrtmain.m

%% 清空变量

clear
clc

%% 导入数据
[dat_chicun,dat_jiaodian,dat_xia] = xlsData();

%% 画图
color_mat=rand(size(dat_jiaodian,1),3);   %生成随机矩阵作为颜色用
hold on   %画在同一个图像上
grid on   % 画网格
for k1=1:size(dat_jiaodian,1)
    plotcube(dat_chicun(k1,[2,1,3]),dat_jiaodian(k1,1:3),0.6,color_mat(k1,:))%这个是画长方体障碍物的函数
end

for k2=1:size(dat_xia,1)
    plot_cylinder(dat_xia(k2,1:3),dat_xia(k2,4),dat_xia(k2,5),1,rand(1,3));%这个是画圆柱体障碍物的函数
end
axis([0 2000 0 2000 -200 600 ])   %设置图像的可视化范围
axis equal     % 图像坐标轴可视化间隔相等  
xlabel('x');   
ylabel('y')


%% 数据重处理

num_cube = size(dat_jiaodian(2:end,:),1);                  %长方体障碍物个数15
[dat_cube,dat_pingban] = cube_coor_deal(dat_jiaodian);     %将长方体角点元素存储在元胞数组，方便索引
num_cylinder = size(dat_xia,1);                            %圆柱体障碍物个数
dat_cylinder = cylinder_coor_deal(dat_xia);                %将圆柱体数据存放在元胞数组里，方便访问

% num_cube:     长方体障碍物个数
% dat_cube:     长方体角点的元胞数组
% dat_pingban:  平板角点元胞数组
% num_cylinder: 圆柱体障碍物个数
% dat_cylinder: 圆柱体障碍物数据元胞

%% rrt算法部分
GG = [704 700 -30;612 700 -30;537 680 -30;447 980 -30];%目标点坐标，有多个目标点就将每个目标点坐标放在这里
PATH = [];
for k3 = 1:size(GG,1)-1
        start = GG(k3,:);
        goal = GG(k3+1,:);
        path_best = RRT(num_cube,dat_cube,num_cylinder,dat_cylinder,start,goal);%每次找到最好路径存放在这个变量里
                                                                                %注意：这个路径点是经过冗余点处理以后的，所以点很少
%         line(path_best(:,1),path_best(:,2),path_best(:,3),'LineWidth',2,'Color','b');%将找到路径的点集合用直线画出来
        PATH = [PATH;path_best];  
end

---

function dist = calculate_distance3(mat_start,mat_goal)
%% 此函数是计算三维点的距离的
dist = sqrt((mat_start(1)-mat_goal(1))^2+(mat_start(2)-mat_goal(2))^2+(mat_start(3)-mat_goal(3))^2);

function flag = collision_checking_cube(num_cube,dat_cube,coor_new,coor_near,Delta,deta)
%发生碰撞返回1
flag = 0;
% flag1 = 0;
% flag2 = 0;
% flag2_mid1 = 0;
% flag2_mid2 = 0;
% flag2_mid3 = 0;
for k1=1:num_cube
    x_min = min(dat_cube{k1}(:,1));
    x_max = max(dat_cube{k1}(:,1));
    y_min = min(dat_cube{k1}(:,2));
    y_max = max(dat_cube{k1}(:,2));
    z_min = min(dat_cube{k1}(:,3));
    z_max = max(dat_cube{k1}(:,3));
    
    for r=0:Delta/deta:Delta
        coor_mid = rrt_steer(coor_new,coor_near,r);
        if ((x_min

function flag = collision_checking_cylinder(num_cylinder,dat_cylinder,coor_new,coor_near,Delta,deta)
%发生碰撞返回1
flag = 0;
 
for k1=1:num_cylinder
    x_coor = dat_cylinder{k1}(1);
    y_coor = dat_cylinder{k1}(2);
    z_coor = dat_cylinder{k1}(3);
    R = dat_cylinder{k1}(4)/2;
    height = dat_cylinder{k1}(5);
     
    for r=Delta/deta:Delta
        coor_mid = rrt_steer(coor_new,coor_near,r);
        if (((x_coor-coor_mid(1))^2+(y_coor-coor_mid(2))^2-R^2) < 0) && (z_coor

function [dat_cube,dat_pingban] = cube_coor_deal(dat_jiaodian)
%% 将15个障碍物角点存放在15个元胞数组里面
mid1 = dat_jiaodian(1:end,:);
lin = size(mid1,1);
col = size(mid1,2);
mid3 = zeros(col/3,3);
dat_cube_mid = cell(lin,1);

for k1=1:lin
    for k2=1:col/3
        mid2 = mid1(k1,3*(k2-1)+1:3*(k2-1)+3);
        mid3(k2,:) = mid2;
    end
    dat_cube_mid{k1}(:,:) = mid3;
end
dat_cube = dat_cube_mid;
dat_cube(1,:)=[];
dat_pingban{1} = dat_cube_mid{1};

function dat_cylinder = cylinder_coor_deal(dat_xia)
%% 将16个圆柱障碍物直径和高度等参数放在元胞数组里，至于为什么存在元胞数组里面，没有为什么，个人爱好
lin = size(dat_xia,1);
dat_cylinder=cell(lin,1);
for k1=1:lin
    dat_cylinder{k1}=dat_xia(k1,:);
end

function path_best = delete_redundant_points(path,num_cylinder,dat_cylinder,num_cube,dat_cube)

num_points = size(path,1);
count = 1;
start_point = path(1,:);
index = zeros(1,num_points);
for k1 = 1:num_points-2
    count = count+1;
    final_point = path(count+1,:);
    if  (collision_checking_cylinder(num_cylinder,dat_cylinder,final_point,start_point,calculate_distance3(final_point,start_point),3)||...
           collision_checking_cube(num_cube,dat_cube,final_point,start_point,calculate_distance3(final_point,start_point),3))
       start_point = path(count,:);
    else
        index(count) = count;
    end
end
index(index==0) = [];
path([index(end:-1:1)],:) = [];
path_best = path;
    

function plot_cylinder(coor,diameter,height,facealpha,color)

%% plot_cylinder(dat_xia(k2,1:3),dat_xia(k2,4),dat_xia(k2,5),1,rand(1,3));
%  第一个参数是圆柱体的底部圆心坐标值，第二个参数是圆柱体直径，第三个参数是圆柱高度
%  第四个参数是透明度，第五个参数是颜色矩阵

%% 函数解释：把这个函数当做黑箱处理，只需要记住函数的输入就可以，知道是干什么的，内部
%% 实现过于复杂，很难解释清楚

% coor:         中心坐标
% diameter:     直径
% height:       高度
% facealpha:    透明度
% color:        颜色

r = diameter/2;
theta = 0:0.3:pi*2;
hold on

for k1 = 1:length(theta)-1
    X=[coor(1)+r*cos(theta(k1)) coor(1)+r*cos(theta(k1+1)) coor(1)+r*cos(theta(k1+1)) coor(1)+r*cos(theta(k1))];
    Y=[coor(2)+r*sin(theta(k1)) coor(2)+r*sin(theta(k1+1)) coor(2)+r*sin(theta(k1+1)) coor(2)+r*sin(theta(k1))];
    Z=[coor(3),coor(3),coor(3)+height,coor(3)+height];
    h=fill3(X,Y,Z,color);
    set(h,'edgealpha',0,'facealpha',facealpha)  
end

X=[coor(1)+r*cos(theta(end)) coor(1)+r*cos(theta(1)) coor(1)+r*cos(theta(1)) coor(1)+r*cos(theta(end))];
Y=[coor(2)+r*sin(theta(end)) coor(2)+r*sin(theta(1)) coor(2)+r*sin(theta(1)) coor(2)+r*sin(theta(end))];
Z=[coor(3),coor(3),coor(3)+height,coor(3)+height];
h=fill3(X,Y,Z,color);
set(h,'edgealpha',0,'facealpha',facealpha)


fill3(coor(1)+r*cos(theta),coor(2)+r*sin(theta),coor(3)*ones(1,size(theta,2)),color)
fill3(coor(1)+r*cos(theta),coor(2)+r*sin(theta),height+coor(3)*ones(1,size(theta,2)),color)
view(3)

function plotcube(varargin)
%% plotcube(dat_chicun(k1,[2,1,3]),dat_jiaodian(k1,1:3),0.6,color_mat(k1,:))
%% 第一个参数是每个长方体的长宽高数值，第二个参数是角点的第一个坐标值，第三个参数是透明度，范围是0-1。
%% 第四个参数是颜色矩阵[a,b,c]  abc每个值范围是0-1
%% 
inArgs = { ...
  [10 56 100] , ... % Default edge sizes (x,y and z)
  [10 10  10] , ... % Default coordinates of the origin point of the cube
  .7          , ... % Default alpha value for the cube's faces
  [1 0 0]       ... % Default Color for the cube
  };

inArgs(1:nargin) = varargin;

[edges,origin,alpha,clr] = deal(inArgs{:});

XYZ = { ...
  [0 0 0 0]  [0 0 1 1]  [0 1 1 0] ; ...
  [1 1 1 1]  [0 0 1 1]  [0 1 1 0] ; ...
  [0 1 1 0]  [0 0 0 0]  [0 0 1 1] ; ...
  [0 1 1 0]  [1 1 1 1]  [0 0 1 1] ; ...
  [0 1 1 0]  [0 0 1 1]  [0 0 0 0] ; ...
  [0 1 1 0]  [0 0 1 1]  [1 1 1 1]   ...
  };

XYZ = mat2cell(...
  cellfun( @(x,y,z) x*y+z , ...
    XYZ , ...
    repmat(mat2cell(edges,1,[1 1 1]),6,1) , ...
    repmat(mat2cell(origin,1,[1 1 1]),6,1) , ...
    'UniformOutput',false), ...
  6,[1 1 1]);


cellfun(@patch,XYZ{1},XYZ{2},XYZ{3},...
  repmat({clr},6,1),...
  repmat({'FaceAlpha'},6,1),...
  repmat({alpha},6,1)...
  );

view(3);

function path_best = RRT(num_cube,dat_cube,num_cylinder,dat_cylinder,start,goal)
%% 流程初始化

Delta=2;                % 设置扩展步长，扩展结点允许的最大距离，这个数据越大迭代越快，但是比最优解效果越差
max_iter = 10000;        % 最大迭代次数，如果超过这个次数还没找到路径则认为找不到路径
Map = [goal(1)-start(1),goal(2)-start(2),goal(3)-start(3)];
count = 1;

%% 构建初始化树
T.x(1) = start(1);
T.y(1) = start(2);
T.z(1) = start(3);
T.xpar(1) = goal(1);
T.ypar(1) = goal(2);
T.zpar(1) = goal(3);
T.dist(1) = 0;
T.indpre(1) = 0;

tic
for iter = 1:max_iter

    % step1: 在地图上随机采样
    coor_rand = rrt_sample(Map,goal,start);     %在空间进行随机采样，coor_rand是一个  1×3  的数组
    % plot3(coor_rand(1),coor_rand(2),coor_rand(3),'r*')
    
    % step2 : 遍历树，找到最近的父节点
    [coor_near,coor_index] = rrt_near(coor_rand,T);
    
    % step3: 扩展得到新的节点
    coor_new = rrt_steer(coor_rand,coor_near,Delta);
    
    % step4: 碰撞检测,发生碰撞就会返回1
    flag1 = collision_checking_cube(num_cube,dat_cube,coor_new,coor_near,Delta,3);%这部分是检测是否和长方体障碍物碰撞的函数
    flag2 = collision_checking_cylinder(num_cylinder,dat_cylinder,coor_new,coor_near,Delta,3);%这部分是检测是否和圆柱体障碍物碰撞的参数

    if flag1 || flag2
        continue;
    end

    count = count+1;
    % step5:将新点插入进去
    T.x(count) = coor_new(1);
    T.y(count) = coor_new(2);
    T.z(count) = coor_new(3);
    T.xpar(count) = coor_near(1);
    T.ypar(count) = coor_near(2);
    T.zpar(count) = coor_near(3);
    T.dist(count) = calculate_distance3(coor_new,coor_near);
    T.indpre(count) = coor_index;
    line([coor_near(1),coor_new(1)],[coor_near(2),coor_new(2)],[coor_near(3),coor_new(3)],'LineWidth',1);
    pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察；
    % 注意：  pause函数时暂停函数，如果为了显示动画则这部分十分重要，不过不加上则会静止动画，不能展示动图

    % step6:每次迭代出新点后都检查一遍是否可以直接和终点相连
    if    ~(collision_checking_cylinder(num_cylinder,dat_cylinder,goal,coor_new,calculate_distance3(goal,coor_new),20)||...
           collision_checking_cube(num_cube,dat_cube,goal,coor_new,calculate_distance3(goal,coor_new),20))
        count = count+1;
        T.x(count) = goal(1);
        T.y(count) = goal(2);
        T.z(count) = goal(3);
        T.xpar(count) = coor_new(1);
        T.ypar(count) = coor_new(2);
        T.zpar(count) = coor_new(3);
        T.dist(count) = calculate_distance3(coor_new,goal);
        T.indpre(count) = 0;
        line([goal(1),coor_new(1)],[goal(2),coor_new(2)],[goal(3),coor_new(3)],'LineWidth',3,'MarkerSize',2);
        pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察
        break;
    end

    
end

toc

% 算法找到路径点后找到到达终点的父代点集合存储在path变量中
if iter>max_iter
    error('超过最大迭代次数，路径规划失败');
end
path(1,1) = T.x(end);path(1,2) = T.y(end);path(1,3) = T.z(end);
path(2,1) = T.x(end-1);path(2,2) = T.y(end-1);path(2,3) = T.z(end-1);
count2 = 2;
ind_pre = T.indpre(end-1);
if iter<=max_iter
    while ~(ind_pre==0)
        count2 = count2+1;
        path(count2,1) = T.x(ind_pre);
        path(count2,2) = T.y(ind_pre);
        path(count2,3) = T.z(ind_pre);
        ind_pre = T.indpre(ind_pre);
    end

end
% line(path(:,1),path(:,2),path(:,3),'LineWidth',1,'Color','r');

        
%% RRT算法找到新点全部集合，接下来要去除冗余点
path_best = delete_redundant_points(path,num_cylinder,dat_cylinder,num_cube,dat_cube);
line(path_best(:,1),path_best(:,2),path_best(:,3),'LineWidth',3,'Color','r');

function [coor_near,coor_index] = rrt_near(coor_rand,T)

min_distance = calculate_distance3(coor_rand,[T.x(1),T.y(1),T.z(1)]);

for T_iter=1:size(T.x,2)
    temp_distance=calculate_distance3(coor_rand,[T.x(T_iter),T.y(T_iter),T.z(T_iter)]);
    
    if temp_distance<=min_distance
        min_distance=temp_distance;
        coor_near(1)=T.x(T_iter);
        coor_near(2)=T.y(T_iter);
        coor_near(3)=T.z(T_iter);
        coor_index=T_iter;
    end
end

function coor_rand = rrt_sample(Map,goal,start)

rat = 1.5;
if unifrnd(0,1)<0.5
   coor_rand(1)= unifrnd(-0.2,rat)* Map(1);   
   coor_rand(2)= unifrnd(-0.2,rat)* Map(2);   
   coor_rand(3)= unifrnd(-0.2,rat)* Map(3);   
   coor_rand = coor_rand+start;
else
   coor_rand=goal;
end

function coor_new = rrt_steer(coor_rand,coor_near,Delta)

 deltaX = coor_rand(1)-coor_near(1);
 deltaY = coor_rand(2)-coor_near(2);
 deltaZ = coor_rand(3)-coor_near(3);
 r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
 fai = atan2(deltaY,deltaX);  
 theta = acos(deltaZ/r);
 
 
 R = Delta;
 x1 = R*sin(theta)*cos(fai);
 x2 = R*sin(theta)*sin(fai);
 x3 = R*cos(theta);
 
 coor_new(1) = coor_near(1)+x1;
 coor_new(2) = coor_near(2)+x2;
 coor_new(3) = coor_near(3)+x3;
 
end
 

function PATH = smooth_deal(PATH)

hold on;
x = PATH(:,1)';
y = PATH(:,2)';
z = PATH(:,3)';
%三次样条插值
t1=1:1:size(PATH,1);
t=1:0.5:size(PATH,1);
XX=spline(t1,x,t);
YY=spline(t1,y,t);
ZZ=spline(t1,z,t);
plot3(XX,YY,ZZ,'r-')
 
view(3)

function [dat_chicun,dat_jiaodian,dat_xia] = xlsData()
%% 十五个长方体障碍物的宽，长，高，第一行不是，第一行是地板，不作为障碍物

dat_chicun =  [0.1,  0.1,    0.1;
                624,   358,  700;
                140,   173,  267;
                121,   210,  105;
                150,    90,  130;
                150,    90,  130;
                115,    88,  122;
                140,   103,  142;
                140,   103,  142;
                135,    75,   91;
                75,    160,  216;
                75,    160,  216;
                111,    60,   98;
                118,    44,   31;
                75,    160,  216;
                75,     88,  125];    
 %% 十五个长方体障碍物的角点坐标，第一行不是
            
  dat_jiaodian = [0,0,0,0,0,1,1767,0,1,1767,0,0,1767,1679,0,1767,1679,1,0,1679,0,0,1679,1;
                  704,573,-80,704,573,620,1062,573,620,1062,573,-80,1062,1197,-80,1062,1197,620,704,1197,-80,704,1197,620;
                  1550,1539,1,1550,1539,268,1723,1539,268,1723,1539,1,1723,1679,1,1723,1679,268,1550,1679,1,1550,1679,268;
                  150,1280,-74,150,1280,31,360,1280,31,360,1280,-74,360,1401,-74,360,1401,31,150,1401,-74,150,1401,31;
                  264,1080,1,264,1080,131,354,1080,131,354,1080,1,354,1230,1,354,1230,131,264,1230,1,264,1230,131;
                  264,910,1,264,910,131,354,910,131,354,910,1,354,1060,1,354,1060,131,264,1060,1,264,1060,131;
                  398,1020,1,398,1020,123,486,1020,123,486,1020,1,486,1135,1,486,1135,123,398,1135,1,398,1135,123;
                  447,858,-40,447,858,102,550,858,102,550,858,-40,550,998,-40,550,998,102,447,998,-40,447,998,102;
                  447,710,-40,447,710,102,550,710,102,550,710,-40,550,850,-40,550,850,102,447,850,-40,447,850,102;
                  537,565,-41,537,565,50,612,565,50,612,565,-41,612,700,-41,612,700,50,537,700,-41,537,700,50;
                  1450,1320,1,1450,1320,217,1610,1320,217,1610,1320,1,1610,1395,1,1610,1395,217,1450,1395,1,1450,1395,217;
                  1450,1201,1,1450,1201,217,1610,1201,217,1610,1201,1,1610,1276,1,1610,1276,217,1450,1276,1,1450,1276,217;
                  1170,579,-48,1170,579,50,1230,579,50,1230,579,-48,1230,690,-48,1230,690,50,1170,690,-48,1170,690,50;
                  451,1160,1,451,1160,32,495,1160,32,495,1160,1,495,1278,1,495,1278,32,451,1278,1,451,1278,32;
                  1390,640,1,1390,640,217,1550,640,217,1550,640,1,1550,715,1,1550,715,217,1390,715,1,1390,715,217;
                  1262,525,1,1262,525,126,1350,525,126,1350,525,1,1350,600,1,1350,600,126,1262,600,1,1262,600,126];
              
              
 %% 前三列是十六个圆柱形障碍物底部圆心坐标，第四列是直径，第五列是高度
              
 dat_xia = [  952,1330,-51,50,181;
             1032,1330,-51,50,181;
             1112,1330,-51,50,181;
             1430,1079,-51,40,145;
             1420,1032,-51,40,145;
             1410,985,-51,40,145;
             882,1330,-45,30,123;
             707,1330,-51,50,181;
             607,1330,-45,30,123;
             1450,1600,-43,30,123;
             1310,500,-53,50,167;
             1310,430,-53,50,167;
             1112,1570,1,130,310;
             960,1570,1,60,215;
             360,1570,1,180,370;
             657,1570,1,180,370];

总结：

代码或许有点长，但是划分为多个子函数就会显得整洁的多。具体操作步骤，将每个代码复制到一个新的脚本文件中，除了主函数命名随意以外，其他脚本文件命名均为函数名称。具体形式如下即可。

 每个子函数都有注释，主函数里面也加上了相应的注释，希望能对你们有帮助，感谢大家的支持~