首页 > 编程语言 >使用栅格地图复现迪杰斯特拉算法

使用栅格地图复现迪杰斯特拉算法

时间:2023-01-24 19:47:59浏览次数:47  
标签:rows end field neighborNodes 迪杰 cols 栅格 复现 节点

Dijkstra算法可以计算出在有权图中从某个起点出发到其他任何一点的最短路径长度

算法思想:

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

定义起点s,终点t,集合U表示还没有找到起点到该点的最短路径的点的集合,集合S表示已经找到最短路径的点的集合,初始情况下S集合中只有起点一个点,最短路径是0



假设要求D到A点的最短路径:

算法实现过程中用到的一些技巧:

  • 删除矩阵某一行:

    >> A = [1,2,3;4,5,6;7,8,9]
    
    A =
    
         1     2     3
         4     5     6
         7     8     9
    
    >> A(2,:) = []    % 删除A矩阵的第2行
    
    A =
    
         1     2     3
         7     8     9
    
    >>
    
  • 在矩阵末尾添加一行

    A(end+1,:) = [1,2,3]
    
  • 枚举某个点相邻的8个节点:

    for i = -1 : 1
        for j = -1 : 1
            nx = x + i;
            ny = y + j;
            if (i == 0 && j == 0) || nx <= 0 || nx > rows || ny <= 0 || ny > cols
                continue
            end
            ......
        end
    end
    
  • 使用元胞数组cell存储起点到达每个点的路径

    >> A = {1,2,3;'text',rand(5,10,2),{11; 22; 33}}
    
    A =
    
      2×3 cell 数组
    
        {[   1]}    {[          2]}    {[     3]}
        {'text'}    {5×10×2 double}    {3×1 cell}
    
    >> 
    

源代码:

init.m: 初始化栅格地图

function [field,cmap] = init(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个子节点
neighborIndex = 1;
for i = -1 : 1
    for j = -1 : 1
        nx = row + i;
        ny = col + j;
        if (i == 0 && j == 0) || nx <= 0 || nx > rows || ny <= 0 || ny > cols
            continue
        end
        child_node_line = sub2ind([rows,cols], nx, ny);
        neighborNodes(neighborIndex,1) = child_node_line;
        if field(nx, ny) ~= 2
            cost = norm([nx, ny] - [row, col]);
            neighborNodes(neighborIndex,2) = cost;
        end
        neighborIndex = neighborIndex + 1;
    end
end

Dijkstra.m: 得到起点到终点的最短路径并绘制

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

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

% 起点、终点、障碍物区域
startPos = 3;
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,:) = [];  % 在U集合中删除起点,表示起点已经得到最短路径0

% 更新起点的邻节点及代价
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];  % 加入S集合中
    U(idx,:) = [];  % 在U集合中删除
    
    % 获得当前节点的临近子节点
    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;

运行结果:

绿色表示最短路径:

标签:rows,end,field,neighborNodes,迪杰,cols,栅格,复现,节点
From: https://www.cnblogs.com/junlin623/p/17065702.html

相关文章

  • 使用Matlab构建二维栅格地图
    常使用的地图一般有以下几种:尺度地图:具有真实的物理尺寸,如栅格地图、特征地图、点云地图、常用于地图构建、定位、SLAM、小规模路径规划拓扑地图:不具备真实的物理尺寸,......
  • 地理信息技术GIS学习(6):栅格数据空间分析、实例:学校选址/修路最佳路径
    设置分析环境:arcGIS菜单中【地理处理】-【环境】  两个工作空间可以修改为chp8/ex1,方便之后保存文件都在这个路径。不用每次都去点了  处理范围也可以选一个。......
  • CVE-2022-26134 Confluence OGNL RCE 复现
    一、漏洞概述     AtlassianConfluence是一款各企业广泛使用的wiki系统。在AtlassianConfluenceServerandDataCenter上存在OGNL注入漏洞,远程攻击者在......
  • CVE-2022-46463复现文章
    本文来自博客YX'BLOGhttp://535yx.cnHarbor是为企业用户设计的容器镜像仓库开源项目,包括了权限管理(RBAC)、LDAP、审计、安全漏洞扫描、镜像验真、管理界面、自我注册......
  • C语言最短路径[迪杰斯特拉算法][2023-01-16]
    C语言最短路径[迪杰斯特拉算法][2023-01-16]算法与数据结构课程设计要求一、 题目:最短路径二、课程设计报告要求1、设计目的(1)要求熟练掌握C语言的基本知识和编程技......
  • 永恒之蓝(MS17-010)的复现
     漏洞介绍:MS17-010又称永恒之蓝漏洞,利用Windows系统的SMB漏洞可以获取系统最高权限。最有名的安全事件就是不法分子通过改造“永恒之蓝”制作了wannacry勒索病毒。攻击......
  • 永恒之黑漏洞复现
    准备一台安装了python的电脑(可以是虚拟机)随便一个受到影响的windows电脑(可以是虚拟机,因为会蓝屏)受影响的windows电脑,包括:Windows10Version1903for32-bitSy......
  • ms17-010漏洞复现
    一、ms17-010漏洞复现(1)启用msf (2)搜索msf中和ms17-010漏洞相关的脚本   (3)使用ms17-010的辅助模块脚本(即搜到的3号模块)   (4)配置辅助模块脚本相关内容......
  • 2022MoeCTF整理复现(NSS平台)
    一、[MoeCTF2022]小纸条1.jpg上的像是某种形式的猪圈密码,找一个对照表边猜边对照2.根据点和角的位置方向以及可能拼写成的单词,得到flagmoectf{ILOVEMYBIGBED}二、[M......
  • Nepnep x CATCTF赛后复现
    因为开赛时咩了,当时只签了几个到,只能赛后复现一下一、Cat_Jump1.刚开始使用VM挂载试了一下,打开后没什么反应,索性直接010打开搜索一下flag的头2.得到flagCatCTF{EFI_1s......