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

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

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

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

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

  • 图论之dijkstra

    di jkstra是由迪杰斯特拉提出的一种图论算法 用于求一个点到其他所有点的最短距离 dijkstra的核心思想就是将所有的点分成两个集合A B 从B集合中找到一个离A集合内的点最近的点 把他加入A集合 然后再用这个点去迭代其他所有点 注

  • NVIDIA 不同显卡对应的GPU计算能力

    转自 https blog csdn net dlhlsc article details 85088280 Fermi CUDA 3 2 until CUDA 8 deprecated from CUDA 9 SM20 or SM 20

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

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

  • 【仿真】Carla介绍与使用 [1] (附代码手把手讲解)

    0 参考与前言 主要介绍无人驾驶的仿真环境CARLA 开源社区维护 以下为相关参考链接 Carla官方文档 建议后续找的时候 先按好版本号 有些功能 api 是新版本里有的 Carla官方github Youtube Python Wind

  • 【路径规划】(1) Dijkstra 算法求解最短路，附python完整代码

    好久不见 我又回来了 这段时间把路径规划的一系列算法整理一下 感兴趣的点个关注 今天介绍一下机器人路径规划算法中最基础的 Dijkstra 算法 文末有 python 完整代码 那我们开始吧 1 算法介绍 1959 年 荷兰计算机科学家 E

  • 厉害｜百度28位离职技术大牛和他们创建的AI公司！

    转自 https www sohu com a 168170780 642762 全球人工智能 拥有十多万AI技术用户 核心用户来自 北大 清华 中科院 麻省理工 卡内基梅隆 斯坦福 哈佛 牛津 剑桥 以及谷歌 腾讯 百度 脸谱 微软 阿里

  • V2X应用场景之协同式自动驾驶

    转自 http zhidx com p 96637 html V2X应用场景之协同式自动驾驶 这个应用场景我觉得是比较典型的 也想多花点时间给大家介绍一下 就是关于V2X在自动驾驶里面很典型的应用 我们管它叫协同式自动驾驶车队 什么意思呢

  • CARLA平台+Q-learning的尝试（gym-carla）

    接触强化学习大概有半年了 也了解了一些算法 一些简单的算法在gym框架也实现了 那么结合仿真平台Carla该怎么用呢 由于比较熟悉gym框架 就偷个懒先从这个开始写代码 项目地址 https github com cjy1992 gym c

  • Dijkstra 负权重算法

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

  • 查找距离 get.shortest.paths() 的路线距离

    我正在使用igraph在 R 中封装来做一些相当简单的事情 计算网络中两个节点之间的最短距离 有没有一种直接的方法来提取通过计算得出的路径的距离get shortest paths 这是一些可重现的代码 说明了我的问题 reproducib

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

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

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

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

  • 带优先级队列的 Dijkstra 算法

    在我的 Dijkstra 算法的实现中 我有 1 个包含所有节点的数组和 1 个包含所有节点的优先级队列 每当一个节点出队时 我都会用新的距离和它的来源更新所有相邻节点 这样我就可以回溯路径 优先级队列中的节点将更新为新距离 数组中的节点将

  • 图 - 具有顶点权重的最短路径

    这是一个消费税 在某些图问题中 顶点可以有权重而不是 或者除了边的权重之外 设 Cv 为顶点的成本 v 和 C x y 边 x y 的成本 这个问题大家关心 寻找图 G 中顶点 a 和 b 之间最便宜的路径 路径的成本是边和顶点的成本之和

  • 如果广度优先搜索 (BFS) 可以更快地完成同样的事情，为什么还要使用 Dijkstra 算法呢？

    两者都可用于从单一源查找最短路径 BFS运行在O E V 而 Dijkstra 运行O V E log V 另外 我见过 Dijkstra 在路由协议中的使用很像 因此 如果 BFS 可以更快地完成同样的事情 为什么还要使用 Dijkstr

  • 具有负权重的 Dijkstra 算法

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

  • 使用字典中的特定键构建列表（python）？

    我正在用 Python 实现 Dijkstra 搜索算法 在搜索结束时 我使用前驱图重建最短路径 从目标节点的前驱开始 例如 path path append destination previous predecessor map des

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

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

随机推荐

热门标签