程序员经验分享-hwhale

2、无人驾驶--路径规划算法：Dijkstra

2023-05-16

目录

    • 2、Dijkstra
      • 2.1、算法简介
      • 2.2、算法思路
          • 具体流程：
      • 2.3、算法具体实现
        • 2.3.1、程序详解

2、Dijkstra

声明：本文是学习古月居~基于栅格地图的机器人路径规划算法指南 • 黎万洪 后写的笔记，好记性不如烂笔头！方便自己日后的巩固与复习，这个教程讲的很高，值得推荐！

2.1、算法简介

迪杰斯特拉算法是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又称为狄克斯特拉算法；

它是从一个节点遍历其余各个节点的最短路径算法，解决的是有权图中最短路径问题。

主要特点：从起点开始，采用贪心算法的策略，每次遍历到初始点距离最近且未访问过的顶点的邻接点，直到扩展到终点为止。

2.2、算法思路

如上图G=(V,E)是一个带权有向图，把图中节点集合V分成两组：
第一组为已经求出最短路径的节点集合（用S表示，初始时S只有一个源点，即：D(0),之后每求得一条最短路径，就将该节点加入到集合S中，指导全部节点都加入到S中，算法就结束了）；第二组为其余未确定最短路径的节点集合（U表示），按最短路径长度的递增次序把第二组的节点加入S中。在加入的过程中，总保持从源点V到S中节点的最短路径长度不大于从源点V到U中任何节点的最短路径长度。
此外，每个节点对应一个距离,S中的节点的距离就是从V到此节点的最短路径长度,U中的节点的距离，是从V到此节点只包含S中的节点为中间节点的当前最短路径长度。

具体流程：

设D为起点，A为终点，找到D~A的最短路径
1、S中只包含起节点D（0）；U包含除S外的其它节点，如C节点，C与D相邻，所以C（3）表示C到D的距离为3，而F与D不相邻，所以设F到D的距离为∞。

2、在U中选出最短节点，这里为C（3），将C移到S中，并在U中删除。

3、按上述方法依次选出，在U中选出最短节点，此时为E（4），将E移到S中，并在U中删除。

4、此时就要对比DCB=13;DCF=9;DEF=6;DEG=12的距离，所以将F（6）移到S中，并在U中删除；

5、依次按照上述方法推算

6、直到U为空集，此时，可以得到最短距离：A(22) = DEF~A =4+2+16；

2.3、算法具体实现

defColorMap.m文件：

作用：生成栅格图

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.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

getNeighborNodes.m文件：

作用：搭建个节点之间的关系；实现查找当前父节点临近的周围8个子节点

function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
[row, col] = ind2sub([rows,cols], lineIndex);
neighborNodes = inf(8,2);

%% 查找当前父节点临近的周围8个子节点
% 左上节点
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));
    neighborNodes(1,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(1,2) = cost;
    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));
    neighborNodes(3,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(3,2) = cost;
    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));
    neighborNodes(6,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(6,2) = cost;
    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));
    neighborNodes(8,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(8,2) = cost;
    end
end

Dijkstra.m文件：

具体的算法实现

% 基于栅格地图的机器人路径规划算法
% 第2节：Dijkstra算法
clc
clear
close all

%% 栅格界面、场景定义
% 行数和列数
rows = 10;
cols = 20;
[field,cmap] = defColorMap(rows, cols);

% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows*cols-2;
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: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: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: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};
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;

2.3.1、程序详解

①、 算法初始化

%% 算法初始化
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:,1) = (1: rows*cols)';
U(:,2) = inf;
S = [startPos, 0];
U(startPos,:) = [];

对U集合进程初始化，生成如下表，第一列未线性索引值，第二列为距离都设为∞
U(:,1) = (1: rows*cols)';
U(:,2) = inf;


%将初始节点放入S集合中，并设置它的距离为0；再U集合中删除初始节点！
S = [startPos, 0];
U(startPos,:) = [];

②、更新起点的邻节点及代价

% 更新起点的邻节点及代价
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
for i = 1:8
    childNode = neighborNodes(i,1);
    
    % 判断该子节点是否存在
    if ~isinf(childNode)
        idx = find(U(:,1) == childNode);
        U(idx,2) = neighborNodes(i,2);
    end
end

这里调用了getNeighborNodes函数实现查找当前父节点临近的周围8个子节点

有三种情况：
①、两列都为inf，即这个节点不存在；
②、两个都是数字，即此节点存在，且为自由空间（可以走的节点）；
③、第一列是数字，第二列是inf，即此节点为障碍物

for i = 1:8；之后进行8次循环，判断该子节点是否存在，如果子节点存在，则将从父节点到子节点的距离存放到U集合中；

如上图中D是父节点，它有两个子节点C和E，那么就要再U集合中更新D到这两个节点的距离为3和4。

③、S集合的最优路径集合

% S集合的最优路径集合
for i = 1:rows*cols
    path{i,1} = i;
end
for i = 1:8
    childNode =  neighborNodes(i,1);
    if ~isinf(neighborNodes(i,2))
        path{childNode,2} = [startPos,neighborNodes(i,1)];
    end
end

这里是生成初始节点的最优路径

④、进行循环，循环的条件是U集合不为空

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: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

得出个节点的距离值，进行排序后得到最优路径！

Dijkstra算法动态效果实现图：
在这里插入图片描述
想要获取工程源码，可以关注公众号：Kevin的学习站，后台回复：“Dijkstra”即可获取；希望此文对您有一定的帮助，整理不易，但您的点赞、关注、收藏就是对我最大的鼓励！

在这里插入图片描述

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

Dijkstra

无人驾驶

路径规划算法

2、无人驾驶--路径规划算法：Dijkstra 的相关文章

  • 游戏中的寻路算法--Dijkstra算法和AStar(A*)算法

    前言 如今游戏中最最常用的两种寻路算法为Dijkstra算法和A 算法 xff0c 虽然现代引擎中的Al寻路算法看似很复杂 其实大部分是Dijkstra算法或者A 算法的变种 导航网格 图数据 无论是2D游戏的导航网格或者3D游戏导航网格

  • 迪杰斯特拉(Dijkstra)算法

    一 算法介绍 迪杰斯特拉算法是由荷兰计算机科学家在1956年发现的算法 此算法使用类似广度优先搜索的方法解决了带权图的单源最短路径问题 它是一个贪心算法 二 核心思想 1 选定一个点 这个点满足两个条件 1 未被选过 2 距离最短 2 对于

  • 【MATLAB】最短路径Dijkstra算法

    目录 1 Dijkstra算法 1 1使用范围 1 2算法思路 1 3实例 2 代码 2 1dijstra函数 2 2调用函数 1 Dijkstra算法 1 1使用范围 bullet 寻求从一固定顶点到其余各点的最短路径

  • Apollo如何通知/订阅主题topic

    转自 https blog csdn net u012423865 article details 80024870 How to advertise and subscribe a topic 导读 众所周知 Apollo是基于ROS开发

  • 一种基于光流的无人机仿生智能避障方法

    转自 https patents google com patent CN106155082A zh 一种基于光流的无人机仿生智能避障方法 技术领域 0001 本发明涉及一种单目视觉的基于光流的无人机智能仿生避障方法 特别是属于人 工智能以

  • 迪杰斯特拉(Dijkstra)算法解决最短路径问题

    Dijkstra 算法介绍 迪杰斯特拉算法 Dijkstra 是由荷兰计算机科学家狄克斯特拉于1959年提出的 因此又叫狄克斯特拉算法 迪杰斯特拉 Dijkstra 算法是最经典的最短路径算法之一 用于计算一个结点到其他结点的最短路径 它的

  • 传感器超声波雷达

    转自 http www itsiwei com 21962 html 在上一次分享中 我介绍了毫米波雷达的原理 数据特性及优缺点 毫米波雷达的低环境敏感和低成本的特性使得其在ADAS和自动驾驶领域得到了广泛的应用 今天要介绍的是一款极其常见

  • KITTI数据集--参数

    一 kitti数据集 label解析 16个数代表的含义 第1个字符串 代表物体类别 Car Van Truck Pedestrian Person sitting Cyclist Tram Misc or DontCare 注意 Dont

  • 天梯题集——紧急救援（Dijkstra+倒序打印分析）

    Dijkstra算法 用于求单源到其他点的最短路径 紧急救援 该题与 Dijkstra模板题 的不同之处在于该题需要记录更多信息 主要思路从局部最优到整体最优 类似dp的思想 include

  • 《学习篇》学会这18个常用ROS命令集合就能入门ROS了

    常用ROS命令概述 ROS常用命令可以按照其使用场景分为ROSshell命令 ROS执行命令 ROS信息命令 ROS catkin命令与ROS功能包命令 虽然很难从一开始就很熟练地使用所有的命令 但是随着使用的次数增多 你会发现常用的几个R

  • Dijkstra 负权重算法

    好吧 首先我知道 Dijkstra 不适用于负权重 我们可以使用 Bellman ford 代替它 但在我遇到的一个问题中 它指出所有边的权重都从 0 到 1 不包括 0 和 1 而路径的成本实际上就是产品 所以我的想法就是只取日志 现在所

  • Dijkstra算法：如果有两个或多个权重最小的节点怎么办？

    在 Dijkstra 算法中 如果算法中的某个点有两个或多个权重最小的节点 我该怎么办 在维基百科中 http en wikipedia org wiki Dijkstra 27s algorithm在步骤号 6 它说 将暂定距离最小的未访

  • CUDA dijkstra 算法 [关闭]

    很难说出这里问的是什么 这个问题模棱两可 含糊不清 不完整 过于宽泛或言辞激烈 无法以目前的形式合理回答 如需帮助澄清此问题以便重新打开 访问帮助中心 是否有人针对给定的稀疏矩阵 cuSPARSE 图实现了 Dijkstra 算法的 CUD

  • 如果顶点属性是指针，如何使用 boost::graph dijkstra 算法？

    我使用 boost graph 来管理图表 我需要制作一个 maxmin 树 现在我尝试使用 boost dijkstra 算法 但我使用指向我的类的指针作为顶点属性 而不是使用typedef property

  • 网格中两点之间的最短路径。有一个捕获

    我遇到这个问题 我必须通过向右或向下移动来找到 NxM 网格中从 A 点 总是左上角 到 B 点 总是右下角 的最短路径 听起来很容易 是吗 问题是 我只能移动我现在坐在的图块上显示的数字 让我举例说明 2 5 1 2 9 2 5 3 3

  • 当这个常规队列版本也是正确的时候，为什么 Dijkstra 算法需要优先级队列？

    我已阅读以下内容 但请查看下面我的代码 为什么Dijkstra算法使用堆 优先级队列 https stackoverflow com questions 12481256 why dijkstras algorithm uses heap

  • 让Boost Dijkstra算法在到达目的节点时停止

    我正在使用 boost graph 及其 Dijkstra 实现 当有人使用Dijkstra算法时 可能是为了知道图中2个节点之间的最短路径 但是 由于您需要检查图中的所有节点以找到最短路径 通常 如 boost 算法 Dijkstra 会

  • 实施 Dijkstra 算法

    我的任务是 大学课程 实施某种形式的寻路 现在 在规范中 我可以实现强力 因为要搜索的节点数量有限制 开始 中间两个 结束 但我想重新使用此代码并来实现迪杰斯特拉算法 http en wikipedia org wiki Dijkstra

  • 原始地理坐标和图形节点之间的最短路径

    我已经实现了一个简单的 Dijkstra 算法 用于使用 Java 查找 osm 地图上的最短路径 从 osm 文件创建的图形中的寻路效果非常好 但是 如果用户的当前位置和 或目的地不是该图的节点 只是原始坐标 我们如何将这些坐标 链接 到

  • Dijkstra 谈“软件工程”[关闭]

    很难说出这里问的是什么 这个问题是含糊的 模糊的 不完整的 过于宽泛的或修辞性的 无法以目前的形式得到合理的回答 如需帮助澄清此问题以便重新打开 访问帮助中心 help reopen questions 埃兹格 迪杰斯特拉 Edsger D

随机推荐

热门标签