前言
1. 深度优先（DFS）和广度优先（BFS）
2. 深度优先搜索(DFS)
- 2.1 算法基本思想
- 2.2 深度优先搜索算法（C）
3. 广度优先搜索(BFS)
- 3.1 算法基本思想
- 3.2 广度优先搜索(BFS)（C）
4. Dijkstra算法
- 4.1 Dijkstra算法原理
- 4.2 Dijkstra算法基本步骤
5. MATLAB编写Dijkstra算法
- 5.1 defColormap.m
- 5.2 getNeighborNodes.m
- 5.3 Dijkstra.m
- - S/U集的内容
- 5.4 实验效果
- - Dijkstra 8邻域
  - Dijkstra 4邻域
  - 改进后的效果
  - 平滑后的效果
- 运行时长
声明

前言

这个学期学校开设了相应的课程，同时也在学习古月居机器人学系列的《基于栅格地图的机器人路径规划指南》，为了巩固知识，方便自己的学习与整理，遂以学习笔记的形式记录。

1. 深度优先（DFS）和广度优先（BFS）

深度优先搜索（ Depth First Search , DFS )：首先从某个顶点出发，依次从它的各个未被访问的邻接点出发深度优先搜索遍历图，直至图中所有和该顶点有路径相通的顶点都被访问到。若此时尚有其他顶点未被访问到，则另选一个未被访问的顶点作起始点，重复上述过程，直至图中所有顶点都被访问到为止。

广度优先搜索（ Breadth First Search , BFS )：从图中某顶点出发，依次访问的各个未曾访问过的邻接点，然后分别从这些邻接点出发依次访问它们的邻接点，并使得“先被访问的顶点的邻接点先于后被访问的顶点的邻接点被访问，直至图中所有已被访问的顶点的邻接点都被访问到。
深度优先搜索和广度优先搜索分别类似于树的前序遍历和层次遍历。
Dijkstra 算法属于典型的广度优先搜索算法。

2. 深度优先搜索(DFS)

2.1 算法基本思想

首先访问图中某一个顶点V_i，以该顶点为出发点；
任选一与该顶点V_i邻接的未被访问的顶点V_j；访问V_j；
以V_j为新的出发点继续进行深度优先搜索，直至图中所有和V_i有路径的顶点被访问到。

2.2 深度优先搜索算法（C）

从图的某一点 v 出发，递归地进行深度优先遍历算法描述：

void DFSTraverse(Graph G)
{for (v=0; v<G.vexnum; ++v) visited[v] = FALSE; /*访问标志数组初始化*/for (v=0; v<G.vexnum; ++v) if (!visited[v]) DFS(G, v); /*对尚未访问的顶点调用 DFS*/
}void DFS(Graph G,int v ) /*从第 v 个顶点出发递归地深度优先遍历图 G*/
{ visited[v]=TRUE;Visit(v); /*访问第 v 个顶点*/
for(w=FisrtAdjVex(G,v);w>=0; w=NextAdjVex(G,v,w))if (!visited[w]) DFS(G,w); /*对 v 的尚未访问的邻接顶点 w 递归调用 DFS*/
}

以邻接表为存储结构的整个图 G 进行深度优先遍历实现的 C 语言描述:

void DFSTraverseAL(ALGraph G) /*深度优先遍历以邻接表存储的图 G*/
{   int i;for (i=0;i<G.vexnum;i++)visited[i]=FALSE; /*标志向量初始化*/for (i=0;i<G.vexnum;i++)if (!visited[i]) DFSAL(G,i); /*vi未访问过，从 vi开始 DFS 搜索*/
}void DFSAL(ALGraph G,int i) /*以 vi为出发点对邻接表存储的图 G 进行 DFS 搜索*/
{ ArcNode *p;Visit(G.adjlist[i]); /*访问顶点 vi*/visited[i]=TRUE; /*标记 vi已访问*/p=G.adjlist[i].firstarc; /*取 vi边表的头指针*/while(p) /*依次搜索 vi的邻接点 vj，j=p->adjvex*/
{   if (!visited[p->adjvex]) /*若 vj尚未访问，则以 vj为出发点向纵深搜索*/DFSAL(G,p->adjvex);p=p->nextarc; /*找 vi的下一个邻接点*/} }

此部分详细原理解释可以参考严蔚敏的数据结构（C语言版）
遍历图的过程实质上是对每个顶点查找其邻接点的过程，其耗费的时间则取决于所采用的存储结构。当以邻接矩阵为图的存储结构时，查找每个顶点的邻接点所需时间为O(n2) ，其中 n 为图中顶点数。而当以邻接表作图的存储结构时，找邻接点所需时间为 O(e)，其中e 为无向图中边的数或有向图中弧的数。由此，当以邻接表作存储结构时，深度优先搜索遍历图的时间复杂度为 O(n+e) 。

3. 广度优先搜索(BFS)

3.1 算法基本思想

广度优先搜索（BFS）遍历类似于树的按层次遍历。
（1）首先访问图中某一指定的出发点V_i；
（2）然后依次访问V_I的所有邻接点V_i1，V_i2……V_it；
（3）再依次以V_i1，V_i2……V_it为顶点，访问各顶点未被访问的邻接点，依此类推，直到图中所有顶点均被访问为止。

3.2 广度优先搜索(BFS)（C）

从图的某一点 v 出发，进行广度优先遍历算法描述：

void BFSTraverse (MGraph G) /*按广度优先非递归遍历图 G，使用辅助队列 Q*/
{for (v=0; v<G.vexnum; ++v) visited[i] = FALSE; /*访问标志数组初始化*/for (v=0; v<G.vexnum; ++v) if (!visited[v]) BFS(G, v); /*对尚未访问的顶点调用 BFS*/
}void BFS (Graph G,int v) {InitQueue(Q); /*置空的辅助队列 Q*/visited[v]=TRUE; Visit(v); /*访问 v*/EnQueue(Q,v); /*v 入队列*/while (!QueueEmpty(Q)) {   DeQueue(Q,u); /*队头元素出队并置为 u*/for(w=FistAdjVex(G,u); w>=0; w=NextAdjVex(G,u,w))if(!visited[w]){visited[w]=TRUE; Visit(w);EnQueue(Q,w); /*u 尚未访问的邻接顶点 w 入队列 Q*/} }
}

以邻接矩阵为存储结构的整个图 G 进行广度优先遍历实现的 C 语言描述。

void BFSTraverseAL(MGraph G) /*广度优先遍历以邻接矩阵存储的图 G*/
{int i;for (i=0;i<G.vexnum;i++) visited[i]=FALSE; /*标志向量初始化*/for (i=0;i<G.vexnum;i++)if (!visited[i]) BFSM(G,i); /* vi未访问过，从 vi开始 BFS 搜索*/
}
void BFSM(MGraph G,int k) /*以 vi为出发点，对邻接矩阵存储的图 G 进行 BFS 搜索*/
{int i,j;sqQueue Q;InitQueue(Q);Visit(G.vexs[k]); /*访问原点 Vk*/visited[k]=TRUE;EnQueue(Q,k); /*原点 Vk入队列*/while (!QueueEmpty(Q)){i=DeQueue(Q); /*Vi出队列*/for (j=0;j<G.vexnum;j++) /*依次搜索 Vi的邻接点 Vj*/if(G.edges[i][j]==1 && !visited[j]) /*若 Vj未访问*/{Visit (G.vexs[j]); /*访问 Vj */visited[j]=TRUE;EnQueue(Q,j); /*访问过的 Vj入队列*/}}  }

此部分详细原理解释可以参考严蔚敏的数据结构（C语言版）

4. Dijkstra算法

Dijkstra 算法是由荷兰计算机科学家迪杰斯特拉于1959年提出的，是从一个节点遍历其余各节点的最短路径算法，解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始，采用贪心算法的策略，每次遍历到始点距离最近且未访问过的顶点的邻接节点，直到扩展到终点为止。

4.1 Dijkstra算法原理

初始点看作一个集合S，其它点看作另一个集合挨个的把离初始点最近的点找到并加入集合S，集合中所有的点的d[i]都是该点到初始点最短路径长度，由于后加入的点是根据集合S中的点为基础拓展的，所以能找到最短路径。

用途：用于求图中指定两点之间的最短路径，或者是指定一点到其它所有点之间的最短路径。

4.2 Dijkstra算法基本步骤

将图上的初始点看作一个集合S，其它点看作另一个集合
根据初始点，求出其它点到初始点的距离d[i] （若相邻，则d[i]为边权值；若不相邻，则d[i]为无限大）
选取最小的d[i]（记为d[x]），并将此d[i]边对应的点（记为x）加入集合S（实际上，加入集合的这个点的d[x]值就是它到初始点的最短距离）
再根据x，更新跟 x 相邻点 y 的d[y]值：d[y] = min{ d[y], d[x] + 边权值w[x][y] }，因为可能把距离调小，所以这个更新操作叫做松弛操作。
重复3，4两步，直到目标点也加入了集合，此时目标点所对应的d[i]即为最短路径长度。（注：重复第三步的时候，应该从所有的d[i]中寻找最小值，而不是只从与x点相邻的点中寻找）

图片中 B（23）应该是B（13）

Dijkstra 算法十分简洁，能够有效的找到最优解，不足之处在数据节点庞大时所需的节点繁多，效率随着数据节点的增加而下降，耗费大量内存空间与计算时间。

5. MATLAB编写Dijkstra算法

MATLAB 编写 Dijkstra 算法的几个核心要素：

可以考虑将第1讲的栅格地图场景定义单独存为一个函数，无需出现在算法的主程序里。
前面的案例基于拓扑地图展开叙述，那么对于栅格地图，通常采用8邻域。考虑到“遍历每一个节点的邻近节点”的功能在程序中会多次出现，故可以考虑将其单独存为一个函数，以便调用。

5.1 defColormap.m

可以将之前所创建的栅格地图作为一个函数来使用
详见——路径规划——基于MATLAB的栅格地图

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ...       % 1-白色-空地0 0 0; ...           % 2-黑色-静态障碍1 0 0; ...           % 3-红色-动态障碍1 1 0;...            % 4-黄色-起始点 1 0 1;...            % 5-品红-目标点0 1 0; ...           % 6-绿色-到目标点的规划路径   0 1 1];              % 7-青色-动态规划的路径% 构建颜色MAP图
colormap(cmap);% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);% 障碍物区域
obsRate = 0.2;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

5.2 getNeighborNodes.m

function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
[row, col] = ind2sub([rows,cols], lineIndex);
% neighborNodes = inf(4,2);  neighborNodes = inf(8,2);%% 查找当前父节点临近的周围8个子节点 注释掉后为4邻域
% 左上节点
if row-1 > 0 && col-1 > 0child_node_sub = [row-1, col-1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));child_brother_node_sub1 = [row-1, col];child_brother_node_sub2 = [row, col-1];neighborNodes(1,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2 if  ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)cost = norm(child_node_sub - [row, col]); % 欧式距离，计算出代价neighborNodes(1,2) = cost;endend
end% 上节点
if row-1 > 0child_node_sub = [row-1, col];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));neighborNodes(2,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(2,2) = cost;end
end% 右上节点
if row-1 > 0 && col+1 <= colschild_node_sub = [row-1, col+1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));child_brother_node_sub1 = [row-1, col];child_brother_node_sub2 = [row, col+1];neighborNodes(3,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2 if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)        cost = norm(child_node_sub - [row, col]);neighborNodes(3,2) = cost;endend
end% 左节点
if  col-1 > 0child_node_sub = [row, col-1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));neighborNodes(4,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(4,2) = cost;end
end% 右节点
if  col+1 <= colschild_node_sub = [row, col+1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));neighborNodes(5,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(5,2) = cost;end
end% 左下节点
if row+1 <= rows && col-1 > 0child_node_sub = [row+1, col-1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));child_brother_node_sub1 = [row, col-1];child_brother_node_sub2 = [row+1, col];neighborNodes(6,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2 if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)cost = norm(child_node_sub - [row, col]);neighborNodes(6,2) = cost;end    end
end% 7.下节点
if row+1 <= rowschild_node_sub = [row+1, col];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));neighborNodes(7,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2cost = norm(child_node_sub - [row, col]);neighborNodes(7,2) = cost;end
end% 8.右下节点
if row+1 <= rows && col+1 <= colschild_node_sub = [row+1, col+1];child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));child_brother_node_sub1 = [row, col+1];child_brother_node_sub2 = [row+1, col];neighborNodes(8,1) = child_node_line;if field(child_node_sub(1), child_node_sub(2)) ~= 2  if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)cost = norm(child_node_sub - [row, col]);neighborNodes(8,2) = cost;endend
end

5.3 Dijkstra.m

% 基于栅格地图的机器人路径规划算法
% 第2节：Dijkstra算法
clc
clear
close all%% 栅格界面、场景定义
% 行数和列数
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows*cols -1 ;
field(startPos) = 4;
field(goalPos) = 5;%% 算法初始化
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:,1) = (1: rows*cols)';
U(:,2) = inf;
S = [startPos, 0];
U(startPos,:) = [];% 更新起点的邻节点及代价
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
% for i = 1:4
for i = 1: 8 childNode = neighborNodes(i,1);% 判断该子节点是否存在if ~isinf(childNode)idx = find(U(:,1) == childNode);U(idx,2) = neighborNodes(i,2);end
end% S集合的最优路径集合
for i = 1:rows*colspath{i,1} = i;
end
% for i = 1:4   for i = 1: 8 childNode =  neighborNodes(i,1);if ~isinf(neighborNodes(i,2))path{childNode,2} = [startPos,neighborNodes(i,1)];end
end%% 循环遍历
while ~isempty(U)% 在U集合找出当前最小距离值的节点,视为父节点，并移除该节点至S集合中[dist_min, idx] = min(U(:,2));parentNode = U(idx, 1);S(end+1,:) = [parentNode, dist_min];U(idx,:) = [];% 获得当前节点的临近子节点neighborNodes = getNeighborNodes(rows, cols, parentNode, field);% 依次遍历邻近子节点，判断是否在U集合中更新邻节点的距离值%  for i = 1:4   for i = 1: 8 % 需要判断的子节点childNode = neighborNodes(i,1);cost = neighborNodes(i,2);if ~isinf(childNode)  && ~ismember(childNode, S)% 找出U集合中节点childNode的索引值idx_U = find(childNode == U(:,1));            % 判断是否更新if dist_min + cost < U(idx_U, 2)U(idx_U, 2) = dist_min + cost;% 更新最优路径path{childNode, 2} = [path{parentNode, 2}, childNode];endendend
end%% 画栅格界面
% 找出目标最优路径
path_opt = path{goalPos,2};
% 给所有访问过的节点上色
for i = 1:rows*colsif ~isempty(path{i,2}) field((path{i,1})) = 7;endendfield(startPos) = 4;
field(goalPos) = 5;
field(path_opt(2:end-1)) = 6;% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
% 画出轨迹
[y0,x0] = ind2sub([rows,cols], path_opt);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
% 对轨迹进行平滑——贝塞尔曲线
points = [x',y'];
M = 1000;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
% 对轨迹进行平滑——spcrv
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:), 'b','LineWidth',2.5);

S/U集的内容

第一列索引值	第二列索引值(代价值——距离)	代表含义
lnf	lnf	未遍历
number	lnf	障碍物
number	number	空地

5.4 实验效果

Dijkstra 8邻域

注：黄色——起点
紫色——终点
白色——空地
黑色——障碍物
绿色——最终路径
可以看到，Dijkstra算法几乎将所有能访问的点都访问了，其运算量较大，但同时能够得到最优解。

Dijkstra 4邻域

4邻域走的路径大多是直角，相比8邻域，不太平滑。
如下图所示：8邻域会出现这样一种状况。显然在现实中这种走法会与障碍相碰撞。对此，需要进行相关约束。例如，当要走左下方向时，还需要保证其左节点和下节点不为障碍物。

改进后的效果

可以看到不会再出现上述问题。但仍有一个缺陷——在部分转角时，会发生转向过大的问题，这对实际的无人车控制显然是不太合理的，对此采用贝塞尔曲线进行平滑。

平滑后的效果

红色为未平滑路径，绿色方块为最终路径，黄色为贝塞尔曲线拟合得到，蓝色为spcrv函数平滑得到。

运行时长

栅格地图大小（20x20）
栅格地图大小（30x30）

栅格地图大小（40x40）
可以看到Dijkstra算法对于栅格地图越大的情况，搜索时间越长，但其总耗时较长，不适用于实时的路径规划，不适用于局部路径规划，适用于全局路径规划。

声明

本人所有文章仅作为自己的学习记录，若有侵权，联系立删。

自动驾驶路径规划——Dijkstra算法相关推荐

自动驾驶路径规划论文解析(5)
解析论文:A Sampling-Based Local Trajectory Planner for Autonomous Driving along a Reference Path 文章依然采用了 ...
自动驾驶路径规划概况
文章目录前言介绍 1. 路径规划在自动驾驶系统架构中的位置 2. 全局路径规划的分类 2.1 基础图搜索算法 2.1.1 Dijkstra算法 2.1.2 双向搜索算法 2.1.3 Floyd算法 ...
自动驾驶路径规划五大常用算法（Dijkstra/人工势场/图搜索等）
编辑 | 希骥智能网联汽车点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[规划控制]技术交流群后台回复[规划控制综述]获取自动驾驶 ...
自动驾驶路径规划：A*（Astar）算法
来源丨古月居点击进入->3D视觉工坊学习交流群 1. 最佳优先搜索(Best-First Search) 最佳优先搜索(BFS),又称A算法,是一种启发式搜索算法(Heuristic Algo ...
自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）
目录 1. RRT算法背景 1.1 RRT算法核心思想 1.2 RRT算法优缺点 2. 经典RRT算法 2.1 RRT算法流程 2.2 RRT伪代码 3. 基于目标概率采样 4. RRT*算法 4.1 ...
自动驾驶路径规划——基于概率采样的路径规划算法（PRM）
目录 1. PRM算法流程 1.1 预处理 1.2 搜索 2. PRM算法案例 2.1 构型采样 2.2 邻域计算 2.3 图搜索(A*搜索) 3. 采样数量的影响 4. 采样策略 4.1 基于障碍物 ...
自动驾驶路径规划技术-高速公路路径规划
Path Planning - Highway Driving project Github: https://github.com/williamhyin/CarND-Path-Planning E ...
汇总！自动驾驶路径规划中的障碍物处理方法总结
作者 | 西涯先生编辑 | 汽车人原文链接:https://zhuanlan.zhihu.com/p/635971229 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨 ...
自动驾驶路径规划——路径规划入门须知
目录前言 1.无人驾驶关键技术 2.路径规划基本概念与分类 2.1 路径规划基本概念 2.1.1 路径规划需要解决的问题 2.1.2 路径规划--现在的研究 2.2路径规划的分类 2.3路径规划的流 ...

自动驾驶路径规划——Dijkstra算法

目录

前言