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

yuan〇

发布时间 2023.06.26阅读数 1542 评论数 0

前言

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

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 > 0
    child_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;
        end
    end
end

% 上节点
if row-1 > 0
    child_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)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(2,2) = cost;
    end
end

% 右上节点
if row-1 > 0 && col+1 <= cols
    child_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;
          end
    end
end

% 左节点
if  col-1 > 0
    child_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)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(4,2) = cost;
    end
end

% 右节点
if  col+1 <= cols
    child_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)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(5,2) = cost;
    end
end

% 左下节点
if row+1 <= rows && col-1 > 0
    child_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 <= rows
    child_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)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(7,2) = cost;
    end
end

% 8.右下节点
if row+1 <= rows && col+1 <= cols
    child_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;
        end
    end
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*cols
    path{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];
            end
        end
    end
end


%% 画栅格界面
% 找出目标最优路径
path_opt = path{goalPos,2};
% 给所有访问过的节点上色
for i = 1:rows*cols
     if ~isempty(path{i,2}) 
        field((path{i,1})) = 7;
     end
 end
  
field(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算法对于栅格地图越大的情况，搜索时间越长，但其总耗时较长，不适用于实时的路径规划，不适用于局部路径规划，适用于全局路径规划。

声明

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

MATLAB 路径规划经验分享 Dijkstra 图论

打赏 0

上一篇：【ROS-Navigation】—— Astar路径规划算法解析

下一篇：【ROS】—— 机器人导航(仿真)—导航实现(十八)[重要][重要][重要]

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

yuan〇

目录

前言

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邻域

改进后的效果

平滑后的效果

运行时长

声明

为你推荐

精选ROS2学习笔记（一）-- ROS2安装

点云地面分割算法——Fast Segmentation of 3D Point Clouds for Ground Vehicles文章分析（一）

精选自动驾驶控制算法之概况介绍

世界坐标系转换/相机标定/畸变矫正-Python

VSLAM特征之线特征&面特征

自动驾驶中camera方案（二）MIPI-CSI2详解

评论（0）

关于作者

yuan〇

23

0

151

2

自动驾驶路径规划——A*（Astar）算法

精选自动驾驶路径规划——基于概率采样的路径规划算法（RRT、RRT*）

精选【ROS-Navigation】—— Astar路径规划算法解析

相关推荐

Apollo星火计划学习笔记——第八讲Apollo控制模块解析与实践2

车辆运动学建模(二)

精选卫星导航与gazebo仿真

精选thrust工程化学习(四)----降采样操作

精选自动驾驶TPM技术杂谈 ———— PAPS(Partially Automated Parking System)标准（ISO 20900）

精选hrust工程化学习(六)----最近邻搜索

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板