程序员经验分享-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 的相关文章

  • 无人驾驶系统Autoware与仿真环境LGSVL Simulator联合配置

    在此 把在Ubuntu 16 04中 搭建无人驾驶系统Autoware编译环境 配置无人驾驶仿真环境LGSVL Simulator 并进行联合测试的步骤 记录下来 以备查阅 系统配置 我所用的配置 需要两个系统 一个Ubuntu系统 一个W

  • ROS navigation调试基础（实现真实机器人导航）

    最近使用了一下ROS中非常经典的导航包navigation 并通过自己的激光雷达以及相机里程计驱动了自己的小车在室内进行简单的定位以及导航 在此记录一下以免后期忘记 1 导航包安装 ROS中navigation导航包可以通过GitHub上下

  • Dijkstra算法-(迪杰斯特拉)算法的迭代实现与优先队列实现 图解算法过程

    Dijkstra算法 迪杰斯特拉 算法之迭代实现 Dijkstra算法 迪杰斯特拉 算法之优先队列实现 该算法的核心原理 很简单 如下图所示 先说说Dijkstra算法 迪杰斯特拉 算法之迭代实现 如下图为详细步骤 代码如下 两种实现方法都

  • 轻松学懂图（下）——Dijkstra和Bellman-Ford算法

    概述 在上一篇文章中讲述了Kruskal和Prim算法 用于得到最小生成树 今天将会介绍两种得到最短路径的算法 Dijlkstra和Bellman Ford算法 Dijkstra算法 算法的特点 属于单源最短路径算法 什么是单源呢 通俗的说

  • PaddleDetection的学习笔记

    1 PaddleDetection介绍 PaddleDetection是由百度推出的目标检测开源模型库 1 1 常见格式 pdparams 保存参数权重的文件格式 2 安装PaddleDetection Python版本 python lt

  • 最短路径——Dijkstra算法（C语言实现）

    最短路径 Dijkstra算法 基本概念 1 最短路径 非带权图 边数最少的路径 带权图 边上的权值之和最少的路径 基本思想 1 v 源点 S 已经生成最短路径的终点 w

  • 【自动驾驶技术】优达学城无人驾驶工程师学习笔记（六）——Github与Markdown相关教程

    Github与Markdown相关教程 本博文为笔者关于优达学城无人驾驶工程师课程中计算机视觉基础部分的学习笔记 该部分为实现车道线图像识别功能的基础课程 关于本课程的详细说明请参考优达学城官网 优达学城为学员提供了一个简短的Github教

  • 如何从多路径 Dijkstra 重建路径？

    我目前正在编写一个用于图形的 PHP 库 我已经成功实现了单路径 Dijkstra 算法 但现在在路径重建阶段很难实现多路径版本 取下图 为了简单起见 该图只有从顶点 A 到 J 的路径 经过多个其他顶点 这些顶点的成本都是相等的 即每条路

  • 一种最小遍历节点数的最短路径算法

    我正在寻找 Dijkstra 算法的实现 它也考虑了遍历的节点数量 我的意思是 典型的 Dijkstra 算法会考虑连接节点的边的权重 同时计算从节点 A 到节点 B 的最短路径 我想在其中插入另一个参数 我希望算法也对遍历的节点数量给予一

  • 如何使用 A-Star 或 Dijkstra 算法解决 15 个难题？

    我在一本人工智能书籍中读到 用于模拟或游戏中寻路的流行算法 A Star Dijkstra 也被用来解决著名的 15 谜题 谁能给我一些关于如何将 15 个拼图简化为节点和边图的指示 以便我可以应用其中一种算法 如果我将图中的每个节点视为游

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

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

  • 在Python中找到英文维基百科中两篇文章之间的最短路径

    问题 在英文维基百科中查找两篇文章之间的最短路径 如果存在文章 C i 并且文章 A 中存在指向文章 C 1 的链接 文章 C 1 中存在指向文章 C 2 的链接 则文章 A 和 B 之间存在路径 在文章 C n 中是指向文章 B 的链接

  • Dijkstra最短路径算法

    以下是我们教授给我们的算法摘要 步骤 3 中提到的图中节点的父节点是什么 我有点困惑 因为我认为节点只有邻居而没有父节点 我的第二个问题是关于第 3 步 拾取堆栈中的第索引条记录 由于堆栈只允许您查看顶部 所以我不确定拾取第索引记录意味着什

  • 具有负权重的 Dijkstra 算法

    我们可以使用具有负权重的 Dijkstra 算法吗 STOP 在你认为 哈哈 你可以在两点之间无休止地跳跃并获得一条无限便宜的路径 之前 我更倾向于考虑单向路径 其应用是具有点的山区地形 显然 从高到低并不需要能量 事实上 它会产生能量 因

  • 查找两个顶点之间的所有最短路径

    给定一个有向图G V E 两个顶点s t和两个权重函数w1 w2 我需要找到最短路径s to t by w2在所有最短路径之间s to t by w1 首先 我怎样才能找到两个顶点之间的所有最短路径s and t Dijkstra 算法帮助

  • 寻找多条短路径的算法

    寻求一种能够产生 N 条短路径的算法 有没有人有算法的经验来寻找多条短路径在有向图中 我的应用程序用于语言 查找同义词链 但从逻辑上讲 这可能用于地理或社交网络 我想要明显不同的路径 而不仅仅是沿途交换几个节点 我真的很想知道是否有办法避免

  • 实施 Dijkstra 算法

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

  • 数组中超过 640 000 个元素 - 内存问题 [Dijkstra]

    我有一个脚本将 803 803 644809 每个图表内有 1 000 000 个值 使用 500 500 一切正常 但现在它崩溃了 它尝试分配超过 64MB 的内存 我没有 解决办法是什么 以某种方式 分裂 它还是 result mysq

  • java有索引的最小优先级队列吗？

    我需要它来实现 Dijkstra 算法 并且我确实有自己的实现 但是使用 java 自己的类记录我的代码会更容易 不 Java标准库没有这样的数据结构 我想大多数人都用这个 http algs4 cs princeton edu 24pq

  • MongoDB + Neo4J vs OrientDB vs ArangoDB [关闭]

    Closed 这个问题是基于意见的 help closed questions 目前不接受答案 我目前正处于 MMO 浏览器游戏的设计阶段 游戏将包括一些实时位置的图块地图 因此每个单元格的图块数据 和通用世界地图 我更喜欢使用 Mongo

随机推荐

热门标签