路径规划算法学习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
end
1
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.函数解读
后续会在这放关于此算法中所用大的函数用法链接

作者：李传学

链接：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

CRP：http://research.microsoft.com/pubs/145688/crp-sea.pdf

RRT路径规划算法
地图
RRT算法原理
路径平滑处理
总结


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

基于采样的路径规划算法——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*
在这里插入图片描述
如图所示在优化路径的过程中，不在全局采样而是在一个椭圆中采样。这个椭圆被设计成以起点和终点为椭圆圆心，当前路径的长度假设成一根绳子，如图绕成的椭圆。

如图所示在优化路径的过程中，不在全局采样而是在一个椭圆中采样。这个椭圆被设计成以起点和终点为椭圆圆心，当前路径的长度假设成一根绳子，如图绕成的椭圆。