根据查阅网上关于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_idx=[];
%初始化结构体tree
tree.child=x_start;
tree.parent=x_start;
tree.parent_idx=1;flag=1;
while flag%在地图范围内随机生成一个点(rd_x，rd_y)rd_x=30*rand(1);rd_y=30*rand(1);fix_rd_x=fix(rd_x)+1;fix_rd_y=fix(rd_y)+1;%调用自定义函数cal_distance，计算新随机点离子节点tree.child中哪个子节点最近，%返回最近的那个子节点行号、两者的连线和x坐标轴的夹角、最短距离[angle,min_idx,distance]=cal_distance(rd_x,rd_y,tree);%随机点的坐标转换为权值矩阵上网格所在的行列，并判断该点是否在障碍物上，%如果该点不在障碍物上，并且最短距离大于grow_distance（防止在计算新节点时超边界），则生成新节点if (occMat(31-fix_rd_y,fix_rd_x)==false) && (distance>grow_distance)%此处注意路径曲线上的点坐标和障碍物权值矩阵之间对应关系的转换%沿着上述夹角方向按生长步长生成新节点new_node_x=tree.child(min_idx,1)+grow_distance*cos(angle);new_node_y=tree.child(min_idx,2)+grow_distance*sin(angle);new_node=[new_node_x,new_node_y];%沿着上述夹角方向，在生长步长内生成100个中间点，点越多，则检测碰撞的精度越高，当然计算量也会越大L=linspace(0,grow_distance,100);direc_x=double(rd_x>tree.child(min_idx,1))*2-1;%判断新节点相对于父节点在x轴上的方向（正方向或负方向）direc_y=double(rd_y>tree.child(min_idx,2))*2-1;%判断新节点相对于父节点在y轴上的方向（正方向或负方向）x_cha=tree.child(min_idx,1)+direc_x*(L*abs(cos(angle)));%100个中间点的横坐标y_cha=tree.child(min_idx,2)+direc_y*(L*abs(sin(angle)));%100个中间点的纵坐标cha_occMat=[];%计算线段上所有这100个点是否位于障碍物上for j=1:length(x_cha)cha_occMat=[cha_occMat,double(occMat(31-(fix(y_cha(j))+1),fix(x_cha(j))+1)==false)];  end%所有的点都不处在障碍物上，才将上述生产的新节点新增到结构体中if sum(cha_occMat)==length(x_cha)tree.child(end+1,:)=new_node;tree.parent(end+1,:)=[tree.child(min_idx,1),tree.child(min_idx,2)];tree.parent_idx(end+1)=min_idx;plot(rd_x,rd_y,'.r');hold on;%画出随机生成的点plot(new_node_x,new_node_y,'.g');hold on;%画出新节点plot([tree.child(min_idx,1),new_node_x],[tree.child(min_idx,2),new_node_y],'b-');hold on;%画出新节点及其父节点的连线end%如果新节点到终点的距离小于搜索停止阈值，则停止搜索，%并将终点加入到tree中if sqrt((new_node_x-goal(1,1))^2+(new_node_y-goal(1,2))^2)<=goal_radiustree.child(end+1,:)=goal;tree.parent(end+1,:)=new_node;tree.parent_idx(end+1)=size(tree.parent,1)-1;break;endend
end%画出生成的路径
tem1=length(tree.parent_idx);%从终点所在的行开始搜索
%zuiyou_idx存储生成路径的点所在tree中的行号
zuiyou_idx=tem1;%首先将终点所在的行加入进去
while tem1>1 %如果没有到起点的行号（也就是第一行），则一直找下去zuiyou_idx(end+1)=tree.parent_idx(tem1);%该行的parent_idx存着该行父节点parent在子节点child中的行号tem1=tree.parent_idx(tem1);%使用当前行的parent_idx更新tem1
end
zuiyou_lujing=tree.child(zuiyou_idx,:);
%画出所生成的路径
plot(zuiyou_lujing(:,1),zuiyou_lujing(:,2),'-r',"LineWidth",2);
hold off;

其中自定义函数如下：

function [angle, min_idx,distance] = cal_distance(rd_x, rd_y, tree)distance = [];i = 1;while i<=size(tree.child,1)dx = rd_x - tree.child(i,1);dy = rd_y - tree.child(i,2);d = sqrt(dx^2 + dy^2);distance(i) = d;i = i+1;end[distance, min_idx] = min(distance);angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));
end

运行代码几次的展示结果如下：

MATLAB自带RRT算法实现

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米
%设置障碍物的膨胀范围
vdims = vehicleDimensions(2.2,0.6,1.5, ...'FrontOverhang',0.37,'RearOverhang',0.32);
numCircles = 3;
ccConfig =inflationCollisionChecker(vdims,numCircles,'CenterPlacements',[0.2 0.5 0.8], ...'InflationRadius',0);
costmap.CollisionChecker = ccConfig;
%画出整个地图
figure,plot(costmap);hold on;
set(gca,"XTick",0:1:30);
set(gca,"YTick",0:1:30);
grid on;%hold on;
legend("off");startPose = [1, 1, 0]; % 起始点位置及姿态
goalPose  = [27, 27, 90]; %终点位置及姿态planner = pathPlannerRRT(costmap);
refPath = plan(planner,startPose,goalPose);%规划路径plot(planner);legend("off");
hold off

运行代码几次的展示结果如下：

注：主要思路参考自这位大神https://blog.csdn.net/m0_55205668/article/details/123922046

无人驾驶全局路径规划之RRT算法相关推荐

全局路径规划：图搜索算法介绍6(A star)Matlab算法实现
本文接:全局路径规划:图搜索算法介绍2(A star) https://blog.csdn.net/gophae/article/details/103061702 % This is Yunchen ...
全局路径规划：图搜索算法介绍1(BFS/DFS)
对于全局路径规划的设计,我们先要了解什么是图搜索,在此之前,要先知道什么是图: 可以看到,图有很多种,有无向图,有向图,节点之间还可以有不同的weight, 用于表述从节点与节点直接迁移的代价. 而图 ...
全局路径规划：图搜索算法介绍4(RRT/RRT*)
本文课件来自香港科技大学,我的母校,感谢ELEC 本节介绍RRT/RRT*的算法: RRT的基本原理是: 我们首先初始化我们的起点,接下来随机撒点,选出一个x_rand, 在x_near 和 x_ra ...
全局路径规划：图搜索算法介绍2(A star)
接下来要介绍的是基于贪心算法的图搜索算法Dja, 和 A* 什么是贪心算法?意思就是我们总是向离终点最近的搜索方向去搜索: Dja 算法的原理就是通过对搜索点构造cost function, 搜索方向 ...
全局路径规划：图搜索算法介绍3(A stars tie breaker)
本文课件来自香港科技大学,我的母校,感谢ELEC 本节介绍A算法中的打破路径对称性的几种方法,在工程实践中有一定的参考意义. 根据上图,我们举一个简单的例子,可以看到,同样的A算法,我们在采用了不用的 ...
【全局路径规划】A*算法 A* Search Algorithm
A Formal Basis for the Heuristic Determination of Minimum Cost Paths PETER E.HART NILS J. NILSSON BE ...
苏宁 11.11：仓库内多 AGV 协作的全局路径规划算法研究
本文为『InfoQ x 苏宁 2018双十一』技术特别策划系列文章之一. 1. 背景随着物联网和人工智能的发展,越来越多的任务渐渐的被机器人取代,机器人逐渐在发展中慢慢进入物流领域,"智能 ...
ROS1云课→20迷宫不惑之A*大法（一种虽古老但实用全局路径规划算法）
ROS1云课→19仿真turtlebot(stage) 19提及的机器人如何实现全局路径规划?A*算法是一种可行的选择. www.gamedev.net/reference/articles/arti ...
【规划】RRT*算法图解
尽管RRT算法是一个相对高效率,同时可以较好的处理带有非完整约束的路径规划问题的算法,并且在很多方面有很大的优势,但是RRT算法并不能保证所得出的可行路径是相对优化的.因此许多关于RRT算法的改进也致 ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...

无人驾驶全局路径规划之RRT算法

RRT算法自编代码实现

MATLAB自带RRT算法实现

无人驾驶全局路径规划之RRT算法相关推荐

最新文章

热门文章