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];
endfunction 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_minfunction 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_coorfunction [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,:);
endfunction 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];
总结：
代码或许有点长，但是划分为多个子函数就会显得整洁的多。具体操作步骤，将每个代码复制到一个新的脚本文件中，除了主函数命名随意以外，其他脚本文件命名均为函数名称。具体形式如下即可。
每个子函数都有注释，主函数里面也加上了相应的注释，希望能对你们有帮助，感谢大家的支持~