无人驾驶（移动机器人）路径规划之RRT与RRTStar算法及其matlab实现

在自动驾驶与移动机器人路径规划时，必定会用到经典的算法RRT与RRT Star。下面是RRT与RRTStar的matlab实现效果。可以发现RRTStar效果明显改善。

一、效果比较

1.1 RRT算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

1.2 RRTStar算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

二、RRT与RRTStar算法

2.1 RRT算法

2.2 RRTStar算法

三、核心代码（完整代码见上述链接）

3.1 地图创建

3.2 RRT主代码

3.3 RRTStar主代码

3.4 rewire代码。

一、效果比较

1.1 RRT算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

代码链接：

移动机器人自主路径规划之RRT算法MATLAB实现代码资源-CSDN文库

（1）原始地图信息。

（2）规划路径信息。

1.2 RRTStar算法效果（黑色为障碍物，红色线为最终路径，蓝色三角形为开始点，粉色三角形为目标点）

代码链接:

移动机器人自主路径规划之RRTStar算法MATLAB实现代码资源-CSDN文库

（1）原始地图信息。

（2）规划路径信息。

二、RRT与RRTStar算法

2.1 RRT算法

RRT算法，全称为Rapidly-Exploring Random Tree（快速探索随机树），是一种在路径规划和运动规划领域广泛应用的算法，特别是在自动驾驶、机器人导航等场景下。该算法由Steven M. LaValle于1998年提出，其核心在于利用随机采样技术构建一个能够快速覆盖配置空间（Configuration Space）的随机树结构，从而找到从初始状态到目标状态的可行路径。

初始化：算法开始时，从起始点创建一个单一的节点作为随机树的根节点。
随机采样：算法在每一步中都会随机选取一个状态空间中的点，这个点通常是在整个可行区域或者配置空间中随机产生的。
接近性判断与树扩展：计算新采样点与树中最近节点的距离，并检查从最近节点沿直线向采样点方向延伸是否遇到障碍物。如果没有障碍阻挡，就在该方向上添加一个新节点至采样点的位置，这个新节点成为树的一部分。
目标检测与终止条件：如果新添加的节点或采样点落在目标区域，或者与目标点足够接近（依据预设的阈值），则认为找到了一条可行路径，并停止扩展。此时可以通过回溯随机树从起始节点到目标节点来获取路径。
重复采样与扩展：如果上述步骤未达到终止条件，则重复步骤2至4，直至找到路径或满足其他停止条件（如达到最大迭代次数、计算资源限制等）。

RRT算法具有概率完备性，即只要存在可行路径，随着迭代次数的增加，算法总能以概率1找到这条路径。然而，RRT算法生成的路径通常不是最优的，且可能包含很多不必要的转折。因此，RRT经常与其他算法（如优化算法）结合使用，以改进路径的质量。

RRT算法在许多领域都有广泛的应用，包括机器人导航、无人机飞行路径规划、自动驾驶等。它特别适用于那些难以用传统方法（如网格法）处理的复杂环境，能够高效地处理障碍物多且分布不规则的情况。

2.2 RRTStar算法

RRTStar（Rapidly-exploring Random Tree Star）算法是RRT（Rapidly-exploring Random Tree）算法的一个改进版本，旨在提高路径规划的质量和效率。RRTStar算法继承了RRT算法的基本框架，即通过在空间中随机采样和构建树来探索可行路径。然而，与RRT相比，RRTStar引入了一些关键的改进，使得它在路径优化和避免局部最优解方面更具优势。

以下是RRTStar算法的主要特点和改进之处：

路径优化：RRTStar算法在构建树的过程中，不仅关注如何快速找到一条从起点到终点的路径，还致力于优化这条路径。它采用了重连策略，即在添加新节点后，会检查是否存在更优的连接方式，如果有，则重新连接树中的节点，以形成更平滑、更短的路径。这种优化过程随着采样点的增加而持续进行，使得最终得到的路径质量更高。
避免局部最优解：RRT算法有时可能陷入局部最优解，即找到一条看似不错的路径但实际上并不是全局最优的。RRTStar通过引入启发式函数和重新布线策略来避免这个问题。启发式函数用于估计当前节点与目标点之间的距离，引导树的扩展方向，使其更有可能找到全局最优解。重新布线策略则通过调整树的结构，使得路径更加平滑，并减少不必要的转折。
适应性和鲁棒性：RRTStar算法对不同类型的环境和问题具有较好的适应性和鲁棒性。无论是障碍物分布密集还是稀疏的环境，无论是二维还是三维空间，RRTStar都能有效地进行路径规划。此外，由于它是基于随机采样的方法，因此不依赖于特定的地图表示或环境模型，使得它在实际应用中更加灵活和方便。

需要注意的是，虽然RRTStar算法在路径优化和避免局部最优解方面表现出色，但它仍然存在一定的局限性。例如，在复杂环境中，RRTStar可能需要较长的计算时间来找到一条满意的路径。此外，由于算法基于随机采样，因此生成的路径可能不是严格的最优解，而是在可接受范围内的次优解。在实际应用中，需要根据具体需求和环境特点来选择合适的算法参数和策略。

RRT*的实现通常涉及复杂的启发式搜索策略、成本函数的设计以及高效的邻近节点查询机制，比如使用KD树或平衡二叉树来加速最近节点的查找过程，确保算法的高效执行。此外，算法还可能包括对采样策略的优化，以更好地探索环境并避免陷入局部最优。

三、核心代码（完整代码见上述链接）

3.1 地图创建

RRT与RRTStar所用到的地图是相同的。

function [start,target,max_x,max_y,wall,resolution]=creat_map_ob()%创建障碍物地图
%创建地图
clear all;
resolution = 1;%每个格子的长度%map的边界
max_x = 50;
max_y = 50;%obstacle
obstacle(1,:) = [25,5,10,5];
obstacle(2,:) = [15,25,5,25];
obstacle(3,:) = [40,20,5,10];
obstacle(4,:) = [30,25,5,10];
obstacle(5,:) = [5,0,5,20];
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;wall = [obstacle];
for i = 1:1:size(wall,1)fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...[wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
pause(1);%延时一秒
%初始点
h=msgbox('初始位置标记！');%弹出初始框提示标记初始位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框delete(h);
end
xlabel('请设置初始点X轴！ ','Color','black');%设置x轴
but = 0;
while(but ~= 1)%收到左键点击[xval,yval,but] = ginput(1);xval=floor(xval);yval=floor(yval);
end
start.x = xval;%初始位置
start.y = yval;
plot(xval,yval,'b^','MarkerFaceColor','b','MarkerSize',10*resolution);%绘制初始点
text(xval + 1,yval + 0.5,'Start');
pause(1);%延时一秒
%目标点
h=msgbox('目标位置标记！');%弹出目标提示标记目标位置
uiwait(h,3);%3s后关闭对话框
if ishandle(h) == 1%删除对话框delete(h);
end
xlabel('请设置目标点X轴！ ','Color','black');%设置x轴
but1 = 0;
while(but1 ~= 1)%收到左键点击[xval,yval,but1] = ginput(1);xval=floor(xval);yval=floor(yval);
end
target.x = xval;%目标位置
target.y = yval;
plot(xval,yval,'m^','MarkerFaceColor','m','MarkerSize',10*resolution);%绘制目标点
text(xval + 1,yval + 0.5,'Target');
hold on;

3.2 RRT主代码

function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...[wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离tree.child = start;
tree.parent = start;
tree.distance = 0;%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;
while target_distance > target_radiusrandom_point.x = max_x*rand();random_point.y = max_y*rand();handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);[angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);pause(0.001);handle2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); %根据步长选择新节点pause(0.001);new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);obflag = 1;step = 0.01;%判断路径是否位于障碍物之中if new_node.x < tree.child(min_idx).xstep = -step;endfor k = 1:1:size(wall,1)for i = tree.child(min_idx).x:step:new_node.xif angle>pi/2-5e-03 && angle<pi/2+5e-03j = tree.child(min_idx).y+i-tree.child(min_idx).x;elseif angle>-pi/2-5e-03 && angle<-pi/2+5e-03j = tree.child(min_idx).y-i+tree.child(min_idx).x;elsej = tree.child(min_idx).y+(i-tree.child(min_idx).x)*tan(angle);endif i >= wall(k,1) - 0.005 && i <= (wall(k,1)+wall(k,3)) + 0.005if j >= wall(k,2) - 0.005 && j <= (wall(k,2) + wall(k,4)) + 0.005obflag = 0;breakendendendif obflag == 0breakendend%更新pause(0.001);if obflag == 1tree.child(end + 1) = new_node;tree.parent(end + 1) = tree.child(min_idx);tree.distance(end + 1) = 1 + tree.distance(min_idx);target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);%删除红色点delete(handle3);plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node%画新连接线plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);endpause(0.001);%删除随机点delete(handle1);%删除最近点与随机点连线。delete(handle2);pause(0.001);
end
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
final_distance = tree.distance(end);
title('RRT, distance:',num2str(final_distance));
current_index = length(tree.child);
while current_index ~= 1plot([tree.child(current_index).x,tree.parent(current_index).x],[tree.child(current_index).y,tree.parent(current_index).y],'-','LineWidth',3*resolution,'Color',[0.8500 0.3250 0.0980]);for i = 1:length(tree.child)if tree.child(i).x == tree.parent(current_index).xif tree.child(i).y == tree.parent(current_index).ycurrent_index = i;breakendendend
end

3.3 RRTStar主代码

function RRT()
clc
clear all
close all;
[start,target,max_x,max_y,wall,resolution] = creat_map_ob();
figure;%创建新窗口
axis([0 max_x 0 max_y]);
set(gca,'XTick',0:resolution:max_x);%x轴的间隔为1
set(gca,'YTick',0:resolution:max_y);%y轴的间隔为1
grid on;
title('RRT');
xlabel('x');
ylabel('y');
hold on;
for i = 1:1:size(wall,1)fill([wall(i,1),wall(i,1) + wall(i,3),wall(i,1) + wall(i,3),wall(i,1)],...[wall(i,2),wall(i,2),wall(i,2) + wall(i,4),wall(i,2) + wall(i,4)],'k');
end
plot(start.x,start.y,'b^','MarkerFaceColor','b','MarkerSize',20*resolution);%绘制初始点
text(start.x + 1,start.y + 0.5,'Start');
plot(target.x,target.y,'m^','MarkerFaceColor','m','MarkerSize',20*resolution);%绘制目标点
text(target.x + 1,target.y + 0.5,'Target');
%初始化随机树
tree.child = [];%存储所有节点
tree.parent = [];%存储节点的父节点
tree.distance = [];%存储child到起点的距离tree.child = start;
tree.parent = start;
tree.distance = 0;
radius = 5;
%存储新衍生的节点，起点初始化
%target_distance用于存储new_node到终点的距离
new_node.x = start.x;
new_node.y = start.y;target_distance = sqrt((target.x-new_node.x)^2+(target.y-new_node.y)^2);
grow_distance = 1;
target_radius = 1.5;line1.l = [];
line1.id = [];
while target_distance > target_radiusrandom_point.x = max_x*rand();random_point.y = max_y*rand();handle1 = plot(random_point.x,random_point.y,'p','MarkerEdgeColor',[0.9290 0.6940 0.1250],'MarkerFaceColor',[0.9290 0.6940 0.1250],'MarkerSize',10*resolution);[angle,min_idx] = find_closet_node(random_point.x,random_point.y,tree);%pause(0.01);handle2 = plot([tree.child(min_idx).x,random_point.x],[tree.child(min_idx).y,random_point.y],'-','Color',[0.7 0.7 0.7],'LineWidth',0.8*resolution); %根据步长选择新节点%pause(0.01);new_node.x = tree.child(min_idx).x + grow_distance*cos(angle);new_node.y = tree.child(min_idx).y + grow_distance*sin(angle);handle3 = plot(new_node.x,new_node.y,'.r','MarkerFaceColor','r','MarkerSize',10*resolution);step = 0.005;%判断路径是否位于障碍物之中%更新pause(0.001);if isobstacle(wall,step,tree,new_node,min_idx,angle) == 1target_distance = sqrt((target.x-new_node.x)^2 + (target.y-new_node.y)^2);%删除红色点delete(handle3);plot(new_node.x,new_node.y,'.g','MarkerFaceColor','g','MarkerSize',10*resolution); % draw the new node%画新连接线handle4 = plot([tree.child(min_idx).x,new_node.x],[tree.child(min_idx).y,new_node.y],'-k','LineWidth',0.8*resolution);% pause(0.001);%删除随机点delete(handle1);%删除最近点与随机点连线。delete(handle2);%pause(0.001);theta = linspace(0, 2*pi, 100);handle6 = plot(new_node.x+radius*cos(theta),new_node.y+radius*sin(theta),'b--');[minid,min_dis_fs,newid] = chooseparent(wall,tree,radius,min_idx,new_node,step,resolution);tree.child(end + 1) = new_node;tree.parent(end + 1) = tree.child(minid);tree.distance(end + 1) = min_dis_fs;delete(handle4);line1.l(end+1) = plot([tree.child(minid).x,new_node.x],[tree.child(minid).y,new_node.y],'-k','LineWidth',0.8*resolution);line1.id(end+1) = length(tree.child);[deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs);if(deid ~= 0)for j = 1:1:length(line1.id)if(line1.id(j) == deid)%pause(2);delete (line1.l(j));breakendend% pause(2);line1.l(j) = plot([tree.child(deid).x,new_node.x],[tree.child(deid).y,new_node.y],'-k','LineWidth',0.8*resolution);%pause(2);enddelete(handle6);else%删除随机点%pause(0.001);delete(handle3);delete(handle1);%删除最近点与随机点连线。delete(handle2);%pause(0.001);endend
tree.child(end + 1) = target;
tree.parent(end + 1) = new_node;
tree.distance(end + 1) = 2 + tree.distance(min_idx);
final_distance = tree.distance(end);
title('RRT, distance:',num2str(final_distance));
current_index = length(tree.child);
while current_index ~= 1plot([tree.child(current_index).x,tree.parent(current_index).x],[tree.child(current_index).y,tree.parent(current_index).y],'-','LineWidth',3*resolution,'Color',[0.8500 0.3250 0.0980]);for i = 1:length(tree.child)if tree.child(i).x == tree.parent(current_index).xif tree.child(i).y == tree.parent(current_index).ycurrent_index = i;breakendendend
end

3.4 rewire代码。

function [deid] = rewire(wall,tree,minid,new_node,step,newid,min_dis_fs)
deid = 0;
for i = 1:1:length(newid)if(newid(i) ~= minid)newcost = min_dis_fs + sqrt((tree.child(newid(i)).x-new_node.x)^2+(tree.child(newid(i)).y-new_node.y)^2);if(newcost < tree.distance(newid(i)))angle = atan2(new_node.y-tree.child(newid(i)).y,new_node.x-tree.child(newid(i)).x);if isobstacle(wall,step,tree,new_node,newid(i),angle) == 1tree.parent(newid(i)) = new_node;tree.distance(newid(i)) = newcost;deid = newid(i);endendend
end