手把手教学RRT三维算法MATLAB仿真（代码可直接运行，视频手把手教学）

最近有很多人问我RRT算法的一些问题，就是怎么添加自己的障碍物还有碰撞检测部分，于是给大家做了这期教学视频，讲解的很详细，每个细节都涉及到，以及最后的程序演示和代码都给大家安利了下去。视频制作不易，希望大家喜欢。

视频地址在这里  手把手教rrt算法(1)-变量定义_哔哩哔哩_bilibili。

这里是算法演示后的展示效果，当然也可以通过自己修改障碍物信息以及初始位置目标位置信息得到自己想要的演示图。整个代码每个模块相对来说比较独立，可移植性非常好。如果有用到这套算法的朋友可以去看看我的视频讲解，会对你理解代码有很大的帮助。

废话不多说直接上代码：

1.main函数（主函数）

在进行RRT算法时首先要定义变量，关于空间位置以及大小的描述还有障碍物信息的描述，在这里给大家演示了三种最常用的障碍物，长方体，圆柱和球形障碍物。最后也是根据RRT寻找的路径进行可图形的绘制操作。

%% 清空变量
clear;
clc

%% 定义变量
axisStart = [0 0 0];
axisLWH = [1000 1000 1000];

%定义障碍物
cubeInfo.exist = 0;
cylinderInfo.exist = 0;
sphereInfo.exist = 0;

pathPoint = [0 0 0;
            100 100 100;
             1000 1000 1000];  %一系列的路径点

cubeInfo = createCubeObject(cubeInfo);       %创建长方体障碍物信息
cylinderInfo = createCylinderObject(cylinderInfo);  %创建圆柱障碍物信息
sphereInfo = createSphereObject(sphereInfo);    %创建球形障碍物信息

%% 画图
figure(1)
colorMatCube = [1 0 0];
colorMatCylinder = [0 1 0];
colorMatSphere = [0 0 1];
pellucidity = 0.6;    %透明度
hold on;
scatter3(pathPoint(1,1),pathPoint(1,2),pathPoint(1,3),'MarkerEdgeColor','k','MarkerFaceColor',[1 0 0]);
scatter3(pathPoint(end,1),pathPoint(end,2),pathPoint(end,3),'MarkerEdgeColor','k','MarkerFaceColor','b');
drawCubeObject(cubeInfo,colorMatCube,pellucidity);     %画长方体障碍物
drawCylinderObject(cylinderInfo,colorMatCylinder,pellucidity);   %画圆柱体障碍物
drawSphereObject(sphereInfo,colorMatSphere,pellucidity);  %画球形障碍物
text(pathPoint(1,1),pathPoint(1,2),pathPoint(1,3),'起点');
text(pathPoint(end,1),pathPoint(end,2),pathPoint(end,3),'终点');
view(3)
grid on;
axis equal;
axis([0 1000 0 1000 0 1000])
xlabel('x')
ylabel('y')
zlabel('z')


%% 寻找路径
totalPath = [];
for k1 = 1:size(pathPoint,1)-1
    startPoint = pathPoint(k1,:);
    goalPoint = pathPoint(k1+1,:);
    Path = RRT(startPoint,axisStart,axisLWH,goalPoint,cubeInfo,cylinderInfo,sphereInfo);
    
    if ~isempty(Path)
        for k2 = 1:size(Path,1)-1
            line([Path(k2,1) Path(k2+1,1)],[Path(k2,2) Path(k2+1,2)],[Path(k2,3) Path(k2+1,3)],'LineWidth',1,'Color','red');
        end
        totalPath = [totalPath;Path];
    end
end

2.createCubeObject(创建长方体障碍物)

function  cubeInfo = createCubeObject(cubeInfo)

cubeInfo.axisX = [500 300];
cubeInfo.axisY = [200 400];
cubeInfo.axisZ = [100 100];
cubeInfo.length = [100 100];
cubeInfo.width = [100 50];
cubeInfo.height = [100 100];

cubeInfo.exist = 1;

end

3.createCylinderObject(创建圆柱体障碍物)

function  cylinderInfo = createCylinderObject(cylinderInfo)

cylinderInfo.X = [500 300];
cylinderInfo.Y = [500 300];
cylinderInfo.Z = [100 100];
cylinderInfo.radius = [50 20];
cylinderInfo.height = [200 100];

cylinderInfo.exist = 1;

end

4.createSphereObject(创建球形障碍物信息)

function sphereInfo = createSphereObject(sphereInfo)

sphereInfo.centerX = [700 800];
sphereInfo.centerY = [700 800];
sphereInfo.centerZ = [700 800];
sphereInfo.radius = [50 80];

sphereInfo.exist = 1;


end

5.drawCubeObject(绘制长方体障碍物)

function drawCubeObject(cubeInfo,colorMatCube,pellucidity)
% 画长方体障碍物的函数

if cubeInfo.exist
    for k1 = 1:size(cubeInfo.axisX,2)
        
        plotcube([cubeInfo.length(k1) cubeInfo.width(k1) cubeInfo.height(k1)],[cubeInfo.axisX(k1) cubeInfo.axisY(k1) cubeInfo.axisZ(k1)],pellucidity,colorMatCube);
    end
    
end

end

6.drawCylinderObject(绘制圆柱体障碍物)

function drawCylinderObject(cylinderInfo,colorMatCylinder,pellucidity)

if cylinderInfo.exist
    %  第一个参数是圆柱体的底部圆心坐标值，第二个参数是圆柱体直径，第三个参数是圆柱高度
    %  第四个参数是透明度，第五个参数是颜色矩阵
    for k1 = 1:size(cylinderInfo.X,2)
        coor = [cylinderInfo.X(k1) cylinderInfo.Y(k1) cylinderInfo.Z(k1)];
        diameter = cylinderInfo.radius(k1)*2;
        height = cylinderInfo.height(k1);
        facealpha = pellucidity;
        color = colorMatCylinder;
        plotcylinder(coor,diameter,height,facealpha,color)
        
    end
    
end

end

7.drawSphereObject(绘制球形障碍物)

function drawSphereObject(sphereInfo,colorMatSphere,pellucidity)

if sphereInfo.exist
    for k1 = 1:size(sphereInfo.centerX,2)
        xCoor = sphereInfo.centerX(k1);
        yCoor = sphereInfo.centerY(k1);
        zCoor = sphereInfo.centerZ(k1);
        radius = sphereInfo.radius(k1);
        
        [x,y,z] = sphere(50);
        mesh(x*radius+xCoor,y*radius+yCoor,z*radius+zCoor,'FaceColor',colorMatSphere,'EdgeColor','none','FaceAlpha',pellucidity);
        
        
    end 
    
end

end

8.expandPoint(RRT算法扩展新点)

function  newCoor = expandPoint(nearCoor,randCoor,step)

deltaX = randCoor(1) - nearCoor(1);
deltaY = randCoor(2) - nearCoor(2);
deltaZ = randCoor(3) - nearCoor(3);

r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
fai = atan2(deltaY,deltaX);
theta = acos(deltaZ/r);

x = step*sin(theta)*cos(fai);
y = step*sin(theta)*sin(fai);
z = step*cos(theta);

newCoor = [x+nearCoor(1) ,y+nearCoor(2),z+nearCoor(3)];


end

9.findNearPoint(RRT算法寻找树上最近点)

function  [nearCoor,preIndex] = findNearPoint(randCoor,T)

tempDis = inf;
calcuDis = @(x,y) sqrt((x(1)-y(1))^2+(x(2)-y(2))^2+(x(3)-y(3))^2);

for k1 = 1:size(T.x,2)
    dis = calcuDis([T.x(k1) T.y(k1) T.z(k1)],randCoor);
    if tempDis>dis
        tempDis = dis;
        index = k1;
    end    

end

nearCoor = [T.x(index) T.y(index) T.z(index)];
preIndex = index;

end

10.isCubeCollision(长方体障碍物碰撞检测)

function cubeFlag = isCubeCollision(cubeInfo,nearCoor,newCoor,step)
%% 长方体碰撞检测函数，如果发生碰撞则返回1

cubeFlag = 0;

if cubeInfo.exist
    for k1 = size(cubeInfo.axisX,2)
        xMin = cubeInfo.axisX(k1);
        xMax = cubeInfo.axisX(k1)+cubeInfo.length(k1);
        yMin = cubeInfo.axisY(k1);
        yMax = cubeInfo.axisY(k1)+cubeInfo.length(k1);
        zMin = cubeInfo.axisZ(k1);
        zMax = cubeInfo.axisZ(k1)+cubeInfo.length(k1);
        
        for k2 = 0:step/100:step
            deltaX = newCoor(1) - nearCoor(1);
            deltaY = newCoor(2) - nearCoor(2);
            deltaZ = newCoor(3) - nearCoor(3);

            r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
            fai = atan2(deltaY,deltaX);
            theta = acos(deltaZ/r);

            x = k2*sin(theta)*cos(fai);
            y = k2*sin(theta)*sin(fai);
            z = k2*cos(theta);
            
            checkPoint = [x+nearCoor(1),y+nearCoor(2),z+nearCoor(3)];
            
            if (xMin<checkPoint(1) && checkPoint(1) < xMax) && (yMin<checkPoint(2) && checkPoint(2) < yMax) && (zMin<checkPoint(3) && checkPoint(3) < zMax)
                cubeFlag = 1;
                return;
            end
        end
        
    end
    
    
end

end

11.isCylinderCollision(圆柱体障碍物碰撞检测)

function cylinderFlag = isCylinderCollision(cylinderInfo,nearCoor,newCoor,step)
%% 圆柱体碰撞检测函数，当发生碰撞的时候返回1

cylinderFlag = 0;
calcuDis = @(x,y)  sqrt((x(1)-y(1))^2+(x(2)-y(2))^2);

if cylinderInfo.exist
    for k1 = 1:size(cylinderInfo.X,2)
        zMin = cylinderInfo.Z(k1);
        zMax = zMin+cylinderInfo.height(k1);
        
        for k2 = 0:step/100:step
            deltaX = newCoor(1) - nearCoor(1);
            deltaY = newCoor(2) - nearCoor(2);
            deltaZ = newCoor(3) - nearCoor(3);

            r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
            fai = atan2(deltaY,deltaX);
            theta = acos(deltaZ/r);

            x = k2*sin(theta)*cos(fai);
            y = k2*sin(theta)*sin(fai);
            z = k2*cos(theta);
            
            checkPoint = [x+nearCoor(1),y+nearCoor(2),z+nearCoor(3)];
            if calcuDis(checkPoint(1:2),[cylinderInfo.X(k1) cylinderInfo.Y(k1)])<cylinderInfo.radius(k1) && zMin<checkPoint(3) & checkPoint(3) < zMax
                cylinderFlag = 1;
                return;
            end
            
        end
        
    end
    
end


end

12.isSphereCollision(球形障碍物碰撞检测)

function sphereFlag = isSphereCollision(sphereInfo,nearCoor,newCoor,step)

sphereFlag = 0;
calcuDis = @(x,y)  sqrt((x(1)-y(1))^2+(x(2)-y(2))^2+(x(3)-y(3))^2);


if sphereInfo.exist
   for k1 = 1:size(sphereInfo.centerX,2)
       center = [sphereInfo.centerX(k1) sphereInfo.centerY(k1) sphereInfo.centerZ(k1)];
       
       for k2 = 0:step/100:step
            deltaX = newCoor(1) - nearCoor(1);
            deltaY = newCoor(2) - nearCoor(2);
            deltaZ = newCoor(3) - nearCoor(3);

            r = sqrt(deltaX^2+deltaY^2+deltaZ^2);
            fai = atan2(deltaY,deltaX);
            theta = acos(deltaZ/r);

            x = k2*sin(theta)*cos(fai);
            y = k2*sin(theta)*sin(fai);
            z = k2*cos(theta);
            
            checkPoint = [x+nearCoor(1),y+nearCoor(2),z+nearCoor(3)];
            if calcuDis(checkPoint,center)<sphereInfo.radius(k1)
                sphereFlag = 1;
                return;
            end
            
        end
       
   end
end

end

13.plotcube(绘制长方体障碍物关键函数)

function plotcube(varargin)
% https://ww2.mathworks.cn/matlabcentral/fileexchange/15161-plotcube
% PLOTCUBE - Display a 3D-cube in the current axes
%
%   PLOTCUBE(EDGES,ORIGIN,ALPHA,COLOR) displays a 3D-cube in the current axes
%   with the following properties:
%   * EDGES : 3-elements vector that defines the length of cube edges
%   * ORIGIN: 3-elements vector that defines the start point of the cube
%   * ALPHA : scalar that defines the transparency of the cube faces (from 0
%             to 1)
%   * COLOR : 3-elements vector that defines the faces color of the cube
%
% Example:
%   >> plotcube([5 5 5],[ 2  2  2],.8,[1 0 0]);
%   >> plotcube([5 5 5],[10 10 10],.8,[0 1 0]);
%   >> plotcube([5 5 5],[20 20 20],.8,[0 0 1]);
% Default input arguments
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
  };
% Replace default input arguments by input values
inArgs(1:nargin) = varargin;
% Create all variables
[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);

14.plotcylinder(绘制圆柱体障碍物关键函数)

function plotcylinder(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)
 

15.RRT(RRT算法逻辑)

function Path = RRT(startPoint,axisStart,axisLWH,goalPoint,cubeInfo,cylinderInfo,sphereInfo)
%% RRT算法寻找路径点

%% 变量定义
calcuDis = @(x,y)  sqrt((x(1)-y(1))^2+(x(2)-y(2))^2+(x(3)-y(3))^2); 
iterMax = 5000;   %最大迭代次数
iter = 0;   %当前迭代次数
step = 5;  %步长
count = 1;  %计数器
Thr = 10;   %阈值

%构建树
T.x(1) = startPoint(1);
T.y(1) = startPoint(2);
T.z(1) = startPoint(3);
T.pre(1) = 0;

while iter < iterMax
    
    iter = iter+1;
    
    %% 在空间中随机采样
    randCoor = samplePoint(axisStart,axisLWH,goalPoint);
    
    %% 寻找树上最近点
    [nearCoor,preIndex] = findNearPoint(randCoor,T);
    
    %% 按照指定步长生成新的扩展点
    newCoor = expandPoint(nearCoor,randCoor,step);
    
    %% 碰撞检测
    cubeFlag = isCubeCollision(cubeInfo,nearCoor,newCoor,step);   %长方体碰撞检测函数
    cylinderFlag = isCylinderCollision(cylinderInfo,nearCoor,newCoor,step);  %圆柱体碰撞检测函数
    sphereFlag = isSphereCollision(sphereInfo,nearCoor,newCoor,step);   %球形障碍物碰撞检测函数
    
    if cubeFlag || cylinderFlag || sphereFlag
        continue;
    end
    
    %% 将新点插入树中
    count = count+1;
    T.x(count) = newCoor(1);
    T.y(count) = newCoor(2);
    T.z(count) = newCoor(3);
    T.pre(count) = preIndex;
    line([nearCoor(1) newCoor(1)],[nearCoor(2) newCoor(2)],[nearCoor(3) newCoor(3)],'LineWidth',1);  %绘制每一个新点
%     pause(0.01);
    
    if calcuDis(newCoor,goalPoint)<Thr
        break;
    end 
    
end

if iter==iterMax
    Path = [];
    disp('路径规划失败');
    return;
end

%% 寻找路径
index = T.pre(end);
count = 1;

while T.pre(index)~=0
    Path(count,1) = T.x(index);
    Path(count,2) = T.y(index);
    Path(count,3) = T.z(index);
    index = T.pre(index);
    count = count+1;
end

%将初始点添加到Path中
Path(count,1) = startPoint(1);
Path(count,2) = startPoint(2);
Path(count,3) = startPoint(3);

%将目标点添加到Path中
Path = flipud(Path);
count = count+1;
Path(count,1) = goalPoint(1);
Path(count,2) = goalPoint(2);
Path(count,3) = goalPoint(3);



end

16.samplePoint(RRT算法空间随机采样)

function randCoor = samplePoint(axisStart,axisLWH,goalPoint)

if rand<0.5
    randX = rand*axisLWH(1)+axisStart(1);
    randY = rand*axisLWH(2)+axisStart(2);
    randZ = rand*axisLWH(3)+axisStart(3);
    randCoor = [randX randY randZ];
    
else
    randCoor = goalPoint;  
end

end