-
路径规划算法_路径规划算法学习Day3
2020-12-16 13:18:28路径规划算法学习Day3-Dijkstra算法实现前言1、Dijkstra算法1.1、地图创建1.2、matlab实现1.3、20*20地图1.4、50*50地图2.函数解读前言算法原理:参考路径规划算法学习Day1路径规划算法学习Day1此方法会结合网络占...路径规划算法学习Day3-Dijkstra算法实现
前言
1、Dijkstra算法
1.1、地图创建
1.2、matlab实现
1.3、20*20地图
1.4、50*50地图
2.函数解读
前言
算法原理:参考路径规划算法学习Day1
路径规划算法学习Day1
此方法会结合网络占用法-栅格法来进行实现提示:本文会用matlab实现Dijkstra算法,并且会分享一些函数用法的链接,也是本人学习得来,供大家参考,批评指正
1、Dijkstra算法
1.1、地图创建
总所周知:栅格法生成地图常规是的自己一个一个打,这样既麻烦还浪费时间
这里介绍几种方法:
way1:在命令框中码:map=rand(k)>0.7 %k代表多少维地图
way2:在matlab中安装Robotics Toolbox工具箱 里有专门的函数makemap可以帮助我们生成一张地图1.2、matlab实现
function path=DJS(Map,origin,destination)
cmap = [1 1 1; ...white
0 0 0; ...black
0 1 0; ...green
1 1 0; ...yellow
1 0 0; ...red
0 0 1; ...blue
0 1 1];
colormap(cmap);%map visualization
[rows, cols]=size(Map);
logical_map = logical(Map);
map=zeros(rows, cols);
map(~logical_map)=1;%free
map(logical_map)=2;%obstacle
%定义一个变量node_cost_list来保存邻居以及它们到起始格的路程
%node_cost_list来保存这些信息,初始化为 Inf,表示从没有访问过。一旦有值,就说明是邻居,赋值的大小就表示该点跟起始点的路程。一旦变成红色,就把它的值再改回 Inf。
node_cost_list = Inf(rows, cols);
node_cost_list(origin(1),origin(2))=0;% set the node_cost of the origin node zero
%定义变量parent_list来保存路径
parent_list=zeros(rows, cols);% create parent_list
destination_index=sub2ind(size(Map),destination(1),destination(2));
origin_index=sub2ind(size(Map),origin(1),origin(2));
plan_success=false;
while true
map(origin(1),origin(2))=3;
map(destination(1),destination(2))=4;
image(0.5,0.5,map);
grid on;
set(gca,'xtick',1:1:rows);
set(gca,'ytick',1:1:cols);
axis image;
drawnow;
%找出距离最小的节点
%搜索中心与起始点的路程min_node,搜索中心的索引坐标:current_node,
[min_node,current_node]=min(node_cost_list(:));
if(min_node == inf || current_node == destination_index)
plan_success=true;
break;
end
node_cost_list(current_node) = inf;%当前搜索中心这个位置赋值为 Inf,表示它已经当过搜索中心了。min函数就不会再找这个位置
map(current_node) = 5;
[i,j]=ind2sub(size(Map),current_node);
for k = 0:3 % four direction
if(k == 0)
adjacent_node = [i-1,j];
elseif (k == 1)
adjacent_node = [i+1,j];
elseif (k == 2)
adjacent_node = [i,j-1];
elseif(k == 3)
adjacent_node = [i,j+1];
end
if((adjacent_node(1)>0 && adjacent_node(1)<=rows) && (adjacent_node(2)>0 &&adjacent_node(2)<=cols))
if(map(adjacent_node(1),adjacent_node(2)) ~= 2 && map(adjacent_node(1),adjacent_node(2)) ~= 5)
if(node_cost_list(adjacent_node(1),adjacent_node(2)) > min_node + 1)
node_cost_list(adjacent_node(1),adjacent_node(2)) = min_node + 1;
if(map(adjacent_node(1),adjacent_node(2)) == 3)
parent_list(adjacent_node(1),adjacent_node(2)) = 0;%如果相邻节点是原点,则将父节点设置为0。
else
parent_list(adjacent_node(1),adjacent_node(2))=current_node;%否则设置当前节点为父节点
end
map(adjacent_node(1),adjacent_node(2)) = 6;
end
end
end
end
end
if(plan_success)
path=[];
node=destination_index;
while parent_list(node)~=0
path=[parent_list(node),path];
node=parent_list(node);
end
for k = 2:size(path,2)
map(path(k)) = 7;
image(0.5,0.5,map);
grid on;
set(gca,'xtick',1:1:rows);
set(gca,'ytick',1:1:cols);
axis image;
drawnow;
end
else
path=[];
end
end1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
1.3、20*20地图
1.4、50*50地图
gif太大无法上传,后面我会完善
主要就是想对比一下,可以让大家看到迪杰斯特拉算法的缺点2.函数解读
后续会在这放关于此算法中所用大的函数用法链接
-
一种面向未知地图的六足机器人路径规划算法.pdf
2019-05-30 12:04:10一种对于六足机器人的改进的路径规划算法的介绍,基于人工势场法与模糊规则相结合,对于经典人工势场法引起的路径震荡以及未知的动态地图的路径规划有较好的作用。文档为期刊,仅供学术交流,没有代码,请谨慎下载。 -
常用的地图导航和路径规划算法
2018-12-06 19:27:31作者:李传学 ...明确一点,基本的图搜索算法dijkstra是无法满足互联网地图检索实时响应这种性能要求,所以各家公司都有各自的预处理方法:分层或者预计算。具体采用何种方式,这取决于采取的加速算法相...作者:李传学
链接:https://www.zhihu.com/question/24870090/answer/73834896
来源:知乎
著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。
明确一点,基本的图搜索算法dijkstra是无法满足互联网地图检索实时响应这种性能要求,所以各家公司都有各自的预处理方法:分层或者预计算。具体采用何种方式,这取决于采取的加速算法相关。在2008年前后,以KIT(http://algo2.iti.kit.edu/routeplanning.php)为主的研究院产出了多个路径规划加速算法,其中以contraction hierarchies 和 highway hierarchies较出名,加之微软研究院提出的Customizable Route Planning,与传统的A-star,基本上支撑了目前工业界地图产品的路径规划服务。
A-star:https://en.wikipedia.org/wiki/A*_search_algorthm
CH:http://algo2.iti.kit.edu/schultes/hwy/contract.pdf
HH:http://algo2.iti.kit.edu/documents/routeplanning/esa06HwyHierarchies.pdf
-
RRT路径规划算法
2019-08-29 21:43:42RRT(Rapidly-Exploring Random Tree)算法是一种基于采样的路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进,...
RRT(Rapidly-Exploring Random Tree)算法是一种基于采样的路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进,有效躲避障碍物,避免路径陷入局部极小值,收敛速度快。本文通过matlab实现RRT算法,解决二维平面的路径规划问题。地图
为了方便算法的实现,使用离散量来表达环境地图。其中,数值0表示无障碍物的空区域,数值1表示该区域有障碍物。
RRT算法中搜索到的顶点坐标为连续点,在地图中产生随机点,算法将通过连续的点构建树。此过程中,对树枝和顶点进行检测,检测顶点所处位置是否是空区域。下载附录中.dat文件,绘制地图。colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1]; imshow(uint8(map),colormap)
note:数据中的列为x轴,行为y轴
RRT算法原理
通过matlab程序构建从起始位置到目标位置的树,并生成连接两个点的路径。使用一颗中心点在起始点的树,而不是两颗树(一个中心点在起始位置,一个中心点在目标位置)。
编写一个matlab函数,输入和输出有相同的形式。function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)
其中:
map:.mat文件中的地图矩阵
q_start:起点的x和y坐标
q_goal:目标点的x和y坐标
k: 在目标点无法找到是,控制产生搜索树的最大迭代次数为k次
delta_q : q_new 和 q_near之间的距离
p: 将q_goal 作为q_rand 的概率,当随机产生的随机数小于p,将目标点作为随机点q_rand,当随机产生的数大于p时,产生一个随机点作为q_rand
vertices:顶点的x和y坐标,生成随机树的过程中产生的所有的点的坐标都存储在这个矩阵中,第一个点为起点,最后一个点为目标点。是一个2行n列的矩阵
deges:生成随机树的所有树枝,一共有n-1个树枝,因此该矩阵有n-1行,每一行的两列分别表示两个点的索引号。一旦搜索到目标点,最后一行将表示目标点,沿着目标点回溯,即可找到路径
path: 从起始点到目标点的索引,是一个行向量下面用一个图来表示上面提到的算法里的一些变量:
算法伪代码如下:
障碍物检测
检测树枝(即q_near和q_new之间的edge)是否处于自由空间,可以使用增量法或者等分法,示意图如下(假设两点之间有10个点,左图为为增量检测法,右图为等分法,从示意图中可以看出使用等分法检测次数更少):
在本文中,使用k=10000,delta_q=50,p=0.3, 我们将获得如下结果:
路径平滑处理
完成基本的RRT算法之后,我们获得了一条从起点到终点的路径,现在对这条路径进行平滑和降噪处理,处理完成之后,我们将得到一条更短的路径。
采用贪心算法:
连接q_start和q_goal,如果检测到两个点之间有障碍物,则将q_goal替换为前一个点,直到两个点能连接上(中间无障碍物)为止。一旦q_goal被连接上,
在matlab中定义平滑函数:function [path_smooth] = smooth(map, path, vertices, delta)
其中:
path: 从起始点到目标位置的路径索引号
vertices:树中所有的顶点坐标
delta:增量距离,用来检测路径顶点之间的直接连接是否在自由空间之内,每个edge都被delta分割成几段
path_smooth:经过平滑处理之后,路径点将会减少,用path_smooth记录平滑之后的路径,仍然是一个行向量,记录路径的索引号平滑处理之后的路径为:
总结
RRT算法是一种增量式的搜索算法,基于概率的思想,它是一种概率完备的路径优化算法,具有求解速度上的优势。RRT基本算法有其自身缺陷,求解得到的路径通常质量不好,带有棱角,不够光滑。因此需要对路径进行平滑处理,才能得到适合机器人路径跟踪的路径曲线。
reference
https://github.com/emreozanalkan/RRT
-
地图导航中的路径规划算法(综述)
2019-04-04 01:35:02NULL 博文链接:https://lvdccyb.iteye.com/blog/1328125 -
基于栅格地图的蚁群算法路径规划
2018-01-19 10:01:04用MATLAB 实现,基于栅格地图的蚁群算法路径规划,含蚁群相关文档。 -
基于栅格地图的A星算法路径规划
2018-01-19 09:46:23用matlab实现基于栅格地图的A星算法路径规划,代码中障碍物为任意障碍物。 -
基于A_算法的三维地图最优路径规划
2018-11-14 19:58:00研究了基于A*算法的适合人步行行走的山地环境下三维地图最优路径规划算法及实现. 本文考虑了三维山地无路网信息覆盖的条件较差环境, 对A*算法进行改进, 并利用三维地形DEM数据计算出一条相对平缓且长度 较短的三维... -
基于栅格地图的Dijkstra算法路径规划
2018-01-19 09:59:08用MATLAB 实现,基于栅格地图的Dijkstra算法路径规划。 -
基于采样的路径规划算法——RRT
2020-02-24 16:11:15基于采样的路径规划算法——RRT* ... 以RRT为代表的路径规划算法基于以下的思路:一个一个的栅格去找非常的费劲,若能找到一些点构成图代表这个地图,在这个图上再去搜索路径则会十分的快速。 ...基于采样的路径规划算法——RRT*
插播:代码已经放到github上面了,实现了A和RRT算法并在ROS下完成了可视化显示。https://github.com/linyicheng1/motionPlan
在上一篇博客中简介了基于搜索的路径规划算法A*的原理,这篇博客则会从另外一个角度去解决路径规划的问题。首先我们要从RRT算法说起,然后再探讨其优势和缺点然后给出一些改进的方法并介绍RRT*的改进之处。 以RRT为代表的路径规划算法基于以下的思路:一个一个的栅格去找非常的费劲,若能找到一些点构成图代表这个地图,在这个图上再去搜索路径则会十分的快速。
如图所示,可以看到仅仅用很少的几个点构成的图就能代表这个地图信息,从而找到一条路径。可以极大的降低搜索复杂度从而降低运行时间。这种方法最早被提出来称为概率路图 (Probabilistic Road Map ),可以很容易的想到这种搜索路径的方法分为两个部分:
学习阶段
-
随机在地图中生成一些点
-
去掉在障碍物中的点
-
将临近的点连接起来,并把经过障碍物的连线去掉
-
将所有的点和边生成一个图结构
过程可以从这个动图中看出
搜索阶段
- 通过A*或者其他算法在生成图上搜索得到一条可行路径
这样一种方法带来的优势是很明显的,它可以减少对很多点的查询次数,不需要再非常小的栅格中工作。但是它带来的缺点也有很多,有的缺点比较容易解决许多学者便提出一些方法进行一些小修小改,但是有一些缺点却是由其原理决定的,另外一些学者于是提出了许多的改进算法。
-
易于解决的缺点
-
两个点的连线是否通过障碍物的判断耗时较长。
解决方法:
1、无需对所有的点和连线进行判断是否通过障碍物
2、采用A*找到一条可行路径,判断路径中的连线和节点是否通过障碍物,若通过则把通过的连线和节点从图结构中去掉。
3、重新寻找一条可行路径并判断碰撞,直至找到一条无碰撞的路径。
-
-
难以解决的缺点
- 效率低下,需要分成两步操作。
- 无法保证路径是最优的。
- 对于一些狭小的区域,采样点只有很小概率落在里面。导致搜索效率低。
- …
基于以上的分析,有人就提出了很多的改进方法,接下来就根据这些缺点的改进介绍一些经典的算法。、
快速扩展随机树(Rapidly-exploring Random Trees )
为了提高效率,将之前分成两步的操作合并。有人提出快速扩展随机树的概念,它的步骤如下:
- 随机生成一个点
- 在随机生成的点和离它最近的节点的方向上取一定步长,得到新的扩展点
- 判断新的扩展点是否经过障碍物,没有通过则添加到图结构中
- 一直扩展直到找到目标点
- 从目标点反推回来便能得到路径,无需搜索
可以看到这种方法不再需要搜索的步骤,使得效率有一定的提升。但是解决了所有的问题呢?显然不是的,首先它找到的路径依旧是随机的可行路径而非最优的路径。其次在全地图内搜索会导致效率不够高,没有目的性的去搜索肯定的效率低下的,同样对于狭小的、采样概率较小区域的搜索也没有得到解决。因此许多学者又在这个基础上提出了接下来的一些改进方法。
生成两条搜索树
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-AuoCWzVN-1582531209679)(C:\Users\lyc\AppData\Roaming\Typora\typora-user-images\image-20200224143901716.png)]
可以看到目标点在狭小路径中也生长出一条路径则会大大提高搜索的效率。
RRT*(Rapidly-exploring Random Tree* )
为了解决路径最优的问题,学者提出了RRT*算法,和RRT不同的是在拓展图结构的同时,RRT*算法会同时调整自己的图结构,使得当前的结构是最优的,不断迭代下去寻找到的路径自然也会是最优的。
在RRT的基础上,算法流程做出如下改进:
1、得到一个随机点
2、找到和随机点较近的几个点
3、选择从起点到随机点最短路径,并将随机点添加到图中,父节点为其中一个节点
4、对于未被选中的节点,也和新节点连接起来,形成新的路径,比较新旧路径长度。
5、若新路径较短,则重组图结构,删除旧路径
可以看到RRT*算法可以得到一条最优的路径,但是其代价往往是昂贵的。因为它在全局进行采样和搜索导致的效率低下,如图
大部分的搜索都是无意义且浪费时间的,虽然获得了一条概率最优路径但是耗时上往往得不偿失。于是又有学者限制了采样点的范围,提出了Informed RRT*
Informed RRT*
在这里插入图片描述
如图所示在优化路径的过程中,不在全局采样而是在一个椭圆中采样。这个椭圆被设计成以起点和终点为椭圆圆心,当前路径的长度假设成一根绳子,如图绕成的椭圆。
如图所示在优化路径的过程中,不在全局采样而是在一个椭圆中采样。这个椭圆被设计成以起点和终点为椭圆圆心,当前路径的长度假设成一根绳子,如图绕成的椭圆。
-
-
路径规划算法进阶
2020-03-28 18:14:49最早是在大学期间学习路径规划算法,严蔚敏_吴伟民的《数据结构》讲的最短路径。当时感到有些晦涩难懂,并没有理解算法思想。回头看,主要是因为应付考试,没有和实际应用场景建立连接,所以体会不深。毕业后,因为... -
python路径规划算法可视化_浅谈路径规划算法
2020-12-20 05:14:23考虑以下情况:image物体(unit)最初位于地图的底端并且尝试向顶部移动。物体扫描的区域中(粉红色部分)没有任何东西显示它不能向上移动,因此它持续向上移动。在靠近顶部时,它探测到一个障碍物然后改变移动方向。... -
粒子群算法用于栅格地图路径规划_【候选论文】No.15 基于多线激光雷达的无人车路径规划算法...
2021-01-28 01:33:35DOI: 10. 3969 / j. issn. 1009-9492. 2019. 05. 003朱泽凡,曾碧. 基于多线激光雷达的无人车...广东省重大科技专项项目(编号:2016B010108004)基于多线激光雷达的无人车路径规划算法*朱泽凡,曾碧(广东工业大学计算... -
地图距离算法_一种单计算参数的自学习路径规划算法
2021-01-13 07:16:32机器人路径规划(Robot Path Planning,RPP)的主要研究目的是寻找工作空间内的一条从出发点到目标点的运动路径,使机器人可以避开所有障碍物,且路径长度最短。RPP问题的相关研究成果在物流、危险物资传送、大规模... -
无人车路径规划算法---(2)地图
2020-01-20 17:32:05后端平滑优化"的pipeline,本文将为大家介绍路径规划算法常用地图格式,文章主要分为以下三个模块: 常用地图格式 栅格地图 拓扑地图与高精度地图 生成一个随机二维栅格地图方法的代码将开源在博主github主页 ... -
rrt运动规划算法流程图_环境感知与规划专题(九)——基于采样的路径规划算法(一)...
2020-12-19 08:51:57而主流的路径规划算法分为基于图搜索的路径规划算法与基于采样的路径规划算法。 本文将阐述一种基于采样的路径规划算法——快速搜索随机树(RRT)。快速搜索随机树(RRT) 快速搜索随机树(RRT)算法从起始点开始,在... -
【路径规划】遗传算法之基于栅格地图机器人路径规划【Matlab 018期】
2021-01-22 14:50:54利用遗传算法处理栅格地图的机器人路径规划的难点主要包括:1保证路径不间断,2保证路径不穿过障碍。用遗传算法解决优化问题时的步骤是固定的,就是种群初始化,选择,交叉,变异,适应度计算这样,那么下面我就说... -
基于搜索的路径规划算法
2020-04-10 22:54:31A*是一个比较经典的启发式寻路算法,是基于dijkstra算法,但是加入了启发函数,使路径搜索效率更高。实现起来很简单。不过要做到通用性高,比如支持各种不同类型的地图,甚至不仅仅是地图,而是个图结构如解决拼图... -
路径规划算法学习Day4-Astar算法
2020-12-26 20:31:13路径规划算法学习Day4-Astar算法前言1.2、matlab实现1.3、20*20地图1.4、50*50地图2.函数解读 前言 算法原理:参考路径规划算法学习Day1 路径规划算法学习Day1 # 1、Astar算法 ## 1.1、地图创建 总所周知:栅格法... -
环境感知与规划专题(九)——基于采样的路径规划算法(一)
2020-06-01 08:12:38而主流的路径规划算法分为基于图搜索的路径规划算法与基于采样的路径规划算法。 本文将阐述一种基于采样的路径规划算法——快速搜索随机树(RRT)。 快速搜索随机树(RRT) 快速搜索随机树(RRT)算法从起始点... -
无人驾驶路径规划算法分析
2020-03-14 01:13:35关于路径规划算法 无人驾驶路径规划层分为全局路径规划和局部路径规划。而规划首先是建立于准确的决策层面和可靠的地图层面的,这两个及其重要的环节很大程度上影响着无人驾驶成功与否!在全局规划中,常用的算法有A... -
基于栅格地图的A-星算法路径规划
2018-01-19 09:51:05用 MATLAB 实现基于栅格地图的A-星算法路径规划,代码中障碍物是随机的。 -
【路径规划】遗传算法之基于栅格地图的机器人最优路径规划【Matlab 020期】
2021-01-22 14:58:26利用遗传算法处理栅格地图的机器人路径规划的难点主要包括:1保证路径不间断,2保证路径不穿过障碍。 用遗传算法解决优化问题时的步骤是固定的,就是种群初始化,选择,交叉,变异,适应度计算这样,那么下面我就说... -
路径规划: 浅谈路径规划算法
2018-03-22 10:11:03原文地址:http://theory.stanford.edu/~amitp/GameProgramming/1 导言1.1 算法...2.3 衡量单位2.4 精确的启发式函数2.4.1 预计算的精确启发式函数2.4.2 线性精确启发式算法2.5 网格地图中的启发式算法2.5.1... -
ros导航路径规划算法
2018-06-07 19:20:52可能这是最简单的路径规划算法,毕竟叫做simple trajectory generator。路径规划分成两部分:1.样本路径生成。2.最优路径选择第一部分 样本路径生成1.获取当前的x轴速度vx,y轴速度vy,和z轴转向角速度th。这部分... -
灭火机器人路径规划matlab_基于栅格地图——遗传算法的机器人最优路径规划
2021-01-06 10:49:18利用遗传算法处理栅格地图的机器人路径规划的难点主要包括:1保证路径不间断,2保证路径不穿过障碍。用遗传算法解决优化问题时的步骤是固定的,就是种群初始化,选择,交叉,变异,适应度计算这样,那么下面我就说... -
路径规划算法学习Day2
2020-12-05 15:00:52路径规划算法学习Day2-栅格法创建环境地图前言1、栅格法1.1、原理二、使用步骤1.引入库2.读入数据总结 前言 静态环境中机器人全局路径规划一直是路径规划中的一个重要问题,具有广阔的应用范围。用于全局路径规划的... -
父路径_环境感知与规划专题(十)——基于采样的路径规划算法(二)
2021-01-09 00:05:41前言 上一篇介绍了快速搜索随机树(RRT)算法的原理,这是一种基于采样的路径规划算法,在地图尺寸较大时,其效率将显著的优于基于图搜索的路径规划算法(如A*)。 然而,RRT也有其局限性,如:狭窄通道情况下的搜索...
-
NFS 实现高可用(DRBD + heartbeat)
-
基于虚拟积分激励的内容部署方法
-
C++代码规范和Doxygen根据注释自动生成手册
-
【干货教程】四川省考公务员报名照片要求及快速制作上传
-
2021年 系统分析师 系列课
-
浅析JavaScript的事件代理和委托
-
PAT (Advanced Level) Practice - 1063 Set Similarity (25 分)
-
Django+vue上传文件并返回文件在服务器中的路径
-
f_httpClient_七大主流的HttpClient比较_20210225
-
基于python的dango框架购物商城毕业设计毕设源代码使用教程
-
Crimeblog:使用gatsby和netlify的博客-源码
-
Android专项测试之GPU测试探索
-
整数最低位一清零
-
MySQL你该了解的那些事【服务端篇】
-
使用Jenkins进行持续集成
-
position: sticky无效
-
2021 年该学的 CSS 框架 Tailwind CSS 实战视频
-
方法加@Transactional(propagation = Propagation.REQUIRES_NEW)依旧回滚
-
Glasterfs 分布式网络文件系统
-
bdb:Eine简单的Beschlussdatenbank-源码