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

本文主要是介绍无人驾驶(移动机器人)路径规划之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)的随机树结构,从而找到从初始状态到目标状态的可行路径。

  1. 初始化:算法开始时,从起始点创建一个单一的节点作为随机树的根节点。

  2. 随机采样:算法在每一步中都会随机选取一个状态空间中的点,这个点通常是在整个可行区域或者配置空间中随机产生的。

  3. 接近性判断与树扩展:计算新采样点与树中最近节点的距离,并检查从最近节点沿直线向采样点方向延伸是否遇到障碍物。如果没有障碍阻挡,就在该方向上添加一个新节点至采样点的位置,这个新节点成为树的一部分。

  4. 目标检测与终止条件:如果新添加的节点或采样点落在目标区域,或者与目标点足够接近(依据预设的阈值),则认为找到了一条可行路径,并停止扩展。此时可以通过回溯随机树从起始节点到目标节点来获取路径。

  5. 重复采样与扩展:如果上述步骤未达到终止条件,则重复步骤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算法的主要特点和改进之处:

  1. 路径优化:RRTStar算法在构建树的过程中,不仅关注如何快速找到一条从起点到终点的路径,还致力于优化这条路径。它采用了重连策略,即在添加新节点后,会检查是否存在更优的连接方式,如果有,则重新连接树中的节点,以形成更平滑、更短的路径。这种优化过程随着采样点的增加而持续进行,使得最终得到的路径质量更高。

  2. 避免局部最优解:RRT算法有时可能陷入局部最优解,即找到一条看似不错的路径但实际上并不是全局最优的。RRTStar通过引入启发式函数和重新布线策略来避免这个问题。启发式函数用于估计当前节点与目标点之间的距离,引导树的扩展方向,使其更有可能找到全局最优解。重新布线策略则通过调整树的结构,使得路径更加平滑,并减少不必要的转折。

  3. 适应性和鲁棒性: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

这篇关于无人驾驶(移动机器人)路径规划之RRT与RRTStar算法及其matlab实现的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/936054

相关文章

不懂推荐算法也能设计推荐系统

本文以商业化应用推荐为例,告诉我们不懂推荐算法的产品,也能从产品侧出发, 设计出一款不错的推荐系统。 相信很多新手产品,看到算法二字,多是懵圈的。 什么排序算法、最短路径等都是相对传统的算法(注:传统是指科班出身的产品都会接触过)。但对于推荐算法,多数产品对着网上搜到的资源,都会无从下手。特别当某些推荐算法 和 “AI”扯上关系后,更是加大了理解的难度。 但,不了解推荐算法,就无法做推荐系

hdu1043(八数码问题,广搜 + hash(实现状态压缩) )

利用康拓展开将一个排列映射成一个自然数,然后就变成了普通的广搜题。 #include<iostream>#include<algorithm>#include<string>#include<stack>#include<queue>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#inclu

康拓展开(hash算法中会用到)

康拓展开是一个全排列到一个自然数的双射(也就是某个全排列与某个自然数一一对应) 公式: X=a[n]*(n-1)!+a[n-1]*(n-2)!+...+a[i]*(i-1)!+...+a[1]*0! 其中,a[i]为整数,并且0<=a[i]<i,1<=i<=n。(a[i]在不同应用中的含义不同); 典型应用: 计算当前排列在所有由小到大全排列中的顺序,也就是说求当前排列是第

csu 1446 Problem J Modified LCS (扩展欧几里得算法的简单应用)

这是一道扩展欧几里得算法的简单应用题,这题是在湖南多校训练赛中队友ac的一道题,在比赛之后请教了队友,然后自己把它a掉 这也是自己独自做扩展欧几里得算法的题目 题意:把题意转变下就变成了:求d1*x - d2*y = f2 - f1的解,很明显用exgcd来解 下面介绍一下exgcd的一些知识点:求ax + by = c的解 一、首先求ax + by = gcd(a,b)的解 这个

综合安防管理平台LntonAIServer视频监控汇聚抖动检测算法优势

LntonAIServer视频质量诊断功能中的抖动检测是一个专门针对视频稳定性进行分析的功能。抖动通常是指视频帧之间的不必要运动,这种运动可能是由于摄像机的移动、传输中的错误或编解码问题导致的。抖动检测对于确保视频内容的平滑性和观看体验至关重要。 优势 1. 提高图像质量 - 清晰度提升:减少抖动,提高图像的清晰度和细节表现力,使得监控画面更加真实可信。 - 细节增强:在低光条件下,抖

hdu2544(单源最短路径)

模板题: //题意:求1到n的最短路径,模板题#include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#i

【数据结构】——原来排序算法搞懂这些就行,轻松拿捏

前言:快速排序的实现最重要的是找基准值,下面让我们来了解如何实现找基准值 基准值的注释:在快排的过程中,每一次我们要取一个元素作为枢纽值,以这个数字来将序列划分为两部分。 在此我们采用三数取中法,也就是取左端、中间、右端三个数,然后进行排序,将中间数作为枢纽值。 快速排序实现主框架: //快速排序 void QuickSort(int* arr, int left, int rig

动态规划---打家劫舍

题目: 你是一个专业的小偷,计划偷窃沿街的房屋。每间房内都藏有一定的现金,影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统,如果两间相邻的房屋在同一晚上被小偷闯入,系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组,计算你 不触动警报装置的情况下 ,一夜之内能够偷窃到的最高金额。 思路: 动态规划五部曲: 1.确定dp数组及含义 dp数组是一维数组,dp[i]代表

【C++】_list常用方法解析及模拟实现

相信自己的力量,只要对自己始终保持信心,尽自己最大努力去完成任何事,就算事情最终结果是失败了,努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一:什么是list? •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二:list常用接口 •🌰1.默认成员函数 🔥构造函数(⭐) 🔥析构函数 •🌰2.list对象

【Prometheus】PromQL向量匹配实现不同标签的向量数据进行运算

✨✨ 欢迎大家来到景天科技苑✨✨ 🎈🎈 养成好习惯,先赞后看哦~🎈🎈 🏆 作者简介:景天科技苑 🏆《头衔》:大厂架构师,华为云开发者社区专家博主,阿里云开发者社区专家博主,CSDN全栈领域优质创作者,掘金优秀博主,51CTO博客专家等。 🏆《博客》:Python全栈,前后端开发,小程序开发,人工智能,js逆向,App逆向,网络系统安全,数据分析,Django,fastapi