根据查阅网上关于RRT算法的原理及代码，再结合自己的理解，使用MATLAB实现RRT算法生成路径的整个过程。RRT的原理部分我不再赘述，网上介绍的文章还是很多的。
下面主要展示自己编的RRT算法，并使用MATLAB自带的RRT路径规划函数进行简单比对。

RRT算法自编代码实现

clear;
close all;
C=zeros(30,30);%一般默认权值大于0.65的网格为有障碍物的网格
%注意障碍物的权值按行列生成，而路径曲线是以左下角为原点生成，注意两者的区别
%C为生成整个网格地图的各个网格权值的矩阵
C(15:18,10:16)=0.9*ones(4,7);
C(21:26,15:27)=0.9*ones(6,13);
C(6:12,3:15)=0.9*ones(7,13);
%生成带权值的网格地图
costmap = vehicleCostmap(C,'CellSize',1);%每个网格边长1米
%设置障碍物的膨胀范围
ccConfig =inflationCollisionChecker('CenterPlacements',[0.2 0.5 0.8],'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
plot(costmap);hold on;
set(gca,"XTick",0:1:30);
set(gca,"YTick",0:1:30);
grid on;%hold on;
legend("off");

%检测有障碍物的区域
occMat = checkOccupied(costmap);

x_start=[1,1];%起点坐标
goal=[27,27];%终点坐标
grow_distance=1;%生长步长
goal_radius=1.5;%搜索停止阈值，如果新生成的节点距离终点距离小于1.5米，则停止搜索路径
%画出起点
plot(x_start(1),x_start(2),'k>','MarkerFaceColor','g','MarkerSize',9);hold on;
%画出终点
plot(goal(1),goal(2),'ko','MarkerFaceColor','r','MarkerSize',9);hold on;
%使用结构体存储子节点，父节点，以及父节点在子节点中的位置
tree.child=[];
tree.parent=[];
tree.parent_id