0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看威廉希尔官方网站 视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

Dijkstra和A*算法及其Matlab实现

3D视觉工坊 来源:古月居 作者:古月居 2022-12-07 15:04 次阅读

写在前面的话:只是对两种路径优化算法进行简单的理解和尝试,为后续使用做准备。如果用到,请再次好好理解原理和Matlab源码。

首先给出Matlab下的三个脚本文件:

TestScript.m

%
% TestScript for Assignment 1
%


%% Define a small map
% map = false(10);
map = ans;
% Add an obstacle
% map (1:5, 6) = true;
map = logical(map);
start_coords = [1, 1];
dest_coords = [40, 20];


%%
close all;
%  [route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
% Uncomment following line to run Astar
 [route, numExpanded] = AStarGrid (map, start_coords, dest_coords);


%HINT: With default start and destination coordinates defined above, numExpanded for Dijkstras should be 76, numExpanded for Astar should be 23.

AStarGrid.m

function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords)
% Run A* algorithm on a grid.
% Inputs : 
%  input_map : a logical array where the freespace cells are false or 0 and
%  the obstacles are true or 1
%  start_coords and dest_coords : Coordinates of the start and end cell
%  respectively, the first entry is the row and the second the column.
% Output :
%  route : An array containing the linear indices of the cells along the
%  shortest route from start to dest or an empty array if there is no
%  route. This is a single dimensional vector
%  numExpanded: Remember to also return the total number of nodes
%  expanded during your search. Do not count the goal node as an expanded node. 


% set up color map for display用一个map矩阵来表示每个点的状态
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited 相当于CLOSED列表的作用
% 4 - blue - on list 相当于OPEN列表的作用
% 5 - green - start
% 6 - yellow - destination


cmap = [1 1 1; ...
  0 0 0; ...
  1 0 0; ...
  0 0 1; ...
  0 1 0; ...
  1 1 0; ...
  0.5 0.5 0.5];


colormap(cmap);


% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;


[nrows, ncols] = size(input_map);


% map - a table that keeps track of the state of each grid cell用来上色的
map = zeros(nrows,ncols);


map(~input_map) = 1;  % Mark free cells
map(input_map) = 2;  % Mark obstacle cells


% Generate linear indices of start and dest nodes将下标转换为线性的索引值
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));


map(start_node) = 5;
map(dest_node) = 6;


% meshgrid will `replicate grid vectors' nrows and ncols to produce
% a full grid
% type `help meshgrid' in the Matlab command prompt for more information
parent = zeros(nrows,ncols);%用来记录每个节点的父节点


% 
[X, Y] = meshgrid (1:ncols, 1:nrows);


xd = dest_coords(1);
yd = dest_coords(2);


% Evaluate Heuristic function, H, for each grid cell
% Manhattan distance用曼哈顿距离作为启发式函数
H = abs(X - xd) + abs(Y - yd);
H = H';
% Initialize cost arrays
f = Inf(nrows,ncols);
g = Inf(nrows,ncols);


g(start_node) = 0;
f(start_node) = H(start_node);


% keep track of the number of nodes that are expanded
numExpanded = 0;


% Main Loop


while true
  
  % Draw current map
  map(start_node) = 5;
  map(dest_node) = 6;
  
  % make drawMapEveryTime = true if you want to see how the 
  % nodes are expanded on the grid. 
  if (drawMapEveryTime)
    image(1.5, 1.5, map);
    grid on;
    axis image;
    drawnow;
  end
  
  % Find the node with the minimum f value,其中的current是index值,需要转换
  [min_f, current] = min(f(:));
  
  if ((current == dest_node) || isinf(min_f))
    break;
  end;
  
  % Update input_map
  map(current) = 3;
  f(current) = Inf; % remove this node from further consideration
  numExpanded=numExpanded+1;
  % Compute row, column coordinates of current node
  [i, j] = ind2sub(size(f), current);
  
  % *********************************************************************
  % ALL YOUR CODE BETWEEN THESE LINES OF STARS
  % Visit all of the neighbors around the current node and update the
  % entries in the map, f, g and parent arrays
  %
  action=[-1 0; 1 0; 0 -1; 0 1];%上,下,左,右
  for a=1:4
    expand=[i,j]+action(a,:);
    expand1=expand(1,1);
    expand2=expand(1,2);
    %不超出边界,不穿越障碍,不在CLOSED列表里,也不是起点,则进行扩展
    if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=nrows && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
            if ( g(expand1,expand2)> g(i,j)+1 )
        g(expand1,expand2)= g(i,j)+1;
        f(expand1,expand2)= g(expand1,expand2)+H(expand1,expand2);
        parent(expand1,expand2)=current;
        map(expand1,expand2)=4;
      end
    end
  end
  %*********************************************************************
  
  
end


%% Construct route from start to dest by following the parent links
if (isinf(f(dest_node)))
  route = [];
else
  route = [dest_node];
  
  while (parent(route(1)) ~= 0)
    route = [parent(route(1)), route];
  end


  % Snippet of code used to visualize the map and the path
  for k = 2:length(route) - 1    
    map(route(k)) = 7;
    pause(0.1);
    image(1.5, 1.5, map);
    grid on;
    axis image;
  end
end
end

DijkstraGrid.m

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
% Run Dijkstra's algorithm on a grid.
% Inputs : 
%  input_map : a logical array where the freespace cells are false or 0 and
%  the obstacles are true or 1
%  start_coords and dest_coords : Coordinates of the start and end cell
%  respectively, the first entry is the row and the second the column.
% Output :
%  route : An array containing the linear indices of the cells along the
%  shortest route from start to dest or an empty array if there is no
%  route. This is a single dimensional vector
%  numExpanded: Remember to also return the total number of nodes
%  expanded during your search. Do not count the goal node as an expanded node.




% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue - on list
% 5 - green - start
% 6 - yellow - destination


cmap = [1 1 1; ...
    0 0 0; ...
    1 0 0; ...
    0 0 1; ...
    0 1 0; ...
    1 1 0; ...
 0.5 0.5 0.5];


colormap(cmap);


% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;


[nrows, ncols] = size(input_map);


% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);


map(~input_map) = 1;  % Mark free cells
map(input_map) = 2;  % Mark obstacle cells


% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));


map(start_node) = 5;
map(dest_node) = 6;


% Initialize distance array
distanceFromStart = Inf(nrows,ncols);


% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);


distanceFromStart(start_node) = 0;


% keep track of number of nodes expanded 
numExpanded = 0;


% Main Loop
while true
  
  % Draw current map
  map(start_node) = 5;
  map(dest_node) = 6;
  
  % make drawMapEveryTime = true if you want to see how the 
  % nodes are expanded on the grid. 
  if (drawMapEveryTime)
    image(1.5, 1.5, map);
    grid on;
    axis image;
    drawnow;
  end
  
  % Find the node with the minimum distance
  [min_dist, current] = min(distanceFromStart(:));
  
  if ((current == dest_node) || isinf(min_dist))
    break;
  end;
  
  % Update map
  map(current) = 3;     % mark current node as visited
  numExpanded=numExpanded+1;
  % Compute row, column coordinates of current node
  [i, j] = ind2sub(size(distanceFromStart), current);
  
  % ********************************************************************* 
  % YOUR CODE BETWEEN THESE LINES OF STARS
  
  % Visit each neighbor of the current node and update the map, distances
  % and parent tables appropriately.
  action=[-1 0; 1 0; 0 -1; 0 1];%上,下,左,右
  for a=1:4
    expand=[i,j]+action(a,:);
    expand1=expand(1,1);
    expand2=expand(1,2);
    %不超出边界,不穿越障碍,不在CLOSED列表里,则进行扩展
    if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=ncols && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5 )
%           if ( expand1>=1 && expand1<=nrows && expand2>=1 && expand2<=ncols && map(expand1,expand2)~=2 && map(expand1,expand2)~=3 && map(expand1,expand2)~=5)
      if ( distanceFromStart(expand1,expand2)> distanceFromStart(i,j)+1 )
        distanceFromStart(expand1,expand2)= distanceFromStart(i,j)+1;
        parent(expand1,expand2)=current;
        map(expand1,expand2)=4;
      end
    end
  end
  distanceFromStart(current) = Inf; % remove this node from further consideration
  %*********************************************************************


end


%% Construct route from start to dest by following the parent links
if (isinf(distanceFromStart(dest_node)))
  route = [];
else
  route = [dest_node];
  
  while (parent(route(1)) ~= 0)
    route = [parent(route(1)), route];
  end
  
    % Snippet of code used to visualize the map and the path
  for k = 2:length(route) - 1    
    map(route(k)) = 7;
    pause(0.1);
    image(1.5, 1.5, map);
    grid on;
    axis image;
  end
end
end

注:运行环境,Matlab 2019a 版本,安装 RTB(Robotic Tool Box)工具包,链接地址为,RTB安装链接。

该工具包中可以运行作者大佬写到的matlab/simulink四轴历程,只需要使用指令 sl_quadrotor 即可。

b0b40faa-725c-11ed-8abf-dac502259ad0.png

b0cd2cec-725c-11ed-8abf-dac502259ad0.png

使用方法

在Matlab 中,使用 makemap(30) 来生成地图,通过鼠标来设置障碍形状。该例子生成了一个30*30的方阵,然后直接运行TestScript.m即可。

其中要在TestScript.m中选择是采用A算法,还是Dijkstra算法。同时设置起始点和终点在哪。下图显示得到的A算法路径优化结果。

其中绿色点为起点,黄色点为终点,黑色表示障碍,白色表示空闲,红色表示搜寻过,灰色表示最后规划的路径。

b0e4161e-725c-11ed-8abf-dac502259ad0.png

下图显示Dijkstra算法的路径优化结果:

b0fb2e62-725c-11ed-8abf-dac502259ad0.png

对应的动态效果已经录屏,下面给出传送门(录屏水印广告请忽略):

通过对比可以看出:A* 算法搜索速度较快,毕竟里面有贪心算法。

这在地图较大的场景应用较好。但是A*算法只能得到局部最优解,并不能保证全局最优解。

相比之下,Dijkstra算法尽管搜索速度慢,但是是全局最优解。不知道两种方法结合gmapping,hector或者cartographer生成的栅格地图会是什么样的效果。后面期待尝试一下。

审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • matlab
    +关注

    关注

    185

    文章

    2974

    浏览量

    230405
  • 算法
    +关注

    关注

    23

    文章

    4608

    浏览量

    92844
  • 源码
    +关注

    关注

    8

    文章

    639

    浏览量

    29185
  • Dijkstra
    +关注

    关注

    0

    文章

    13

    浏览量

    8441

原文标题:Dijkstra和A*算法及其Matlab实现

文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    使用dijkstra算法的准备工作

    使用dijkstra算法dijkstra算法是特别经典的路径分析算法,文章中的算法也确实很容易
    发表于 05-23 08:13

    数字信号处理及其MATLAB实现

    数字信号处理及其MATLAB实现
    发表于 03-25 15:05 23次下载

    数字信号处理及其MATLAB实现

    数字信号处理及其MATLAB实现
    发表于 03-26 14:13 373次下载

    SOFM网络及其MATLAB中的实现

    本文详细叙述了自组织映射网络的原理、算法及其Matlab实现的工具箱,并结合实例给出了SOFM 在Matlab 上的
    发表于 09-18 11:04 14次下载

    模糊推理的Mamdani算法及其Matlab实现

    模糊滤波的mamdani算法及其Matlab实现
    发表于 11-17 18:23 40次下载

    基于有向非负极图数据DIJKSTRA算法

    法相结合的方法。对Dijkstra算法改进,并求解关键节点(起点,终点和必经节点)间的最短路径,进而从关键节点所构成的矩阵中采用回溯法得到目标路径。通过实际的算法实现,测试大量的有向非
    发表于 11-03 15:22 8次下载
    基于有向非负极图数据<b class='flag-5'>DIJKSTRA</b><b class='flag-5'>算法</b>

    BP算法及其matlab实现

    高级自动控制算法:BP算法及其matlab实现
    发表于 12-02 11:45 2次下载

    基于Dijkstra最短路径的抽样算法

    针对社交网络中随机抽样算法抽样结果不能很好地代表原始网络的问题,设计了一种基于Dijkstra最短路径的抽样算法。首先,利用Dijkstra算法
    发表于 12-17 11:40 1次下载
    基于<b class='flag-5'>Dijkstra</b>最短路径的抽样<b class='flag-5'>算法</b>

    《图论算法及其MATLAB实现》电子教材和函数及函数功能列表概述

    全书分为相对独立的9章每章都是解决-类问题的算法思想及其MATLAB实现,首先介绍有关基础知识,然后给出相关著名实际问题及解决此向题的算法
    发表于 10-22 08:00 0次下载

    遗传算法原理及其MATLAB实现的详细资料说明

    本文档的主要内容详细介绍的是遗传算法原理及其MATLAB实现的详细资料说明。
    发表于 12-18 08:00 5次下载
    遗传<b class='flag-5'>算法</b>原理<b class='flag-5'>及其</b><b class='flag-5'>MATLAB</b><b class='flag-5'>实现</b>的详细资料说明

    系统仿真及其Matlab实现

    系统仿真及其Matlab实现方法介绍。
    发表于 06-17 17:13 27次下载

    基于STM32的A*(A星)寻路算法实现

    STM32 + LED点阵屏 实现A算法寻路A算法最初发表于1968年。它可以被认为是Dijkstr
    发表于 12-27 19:15 14次下载
    基于STM32的<b class='flag-5'>A</b>*(<b class='flag-5'>A</b>星)寻路<b class='flag-5'>算法</b><b class='flag-5'>实现</b>

    Dijkstra算法A*算法

    在本文中,我们将主要介绍Dijkstra算法A*算法,从成本计算的角度出发,并逐步展开讨论。 我们将从广度优先搜索开始,然后引入Dijkstra
    的头像 发表于 07-07 10:56 1527次阅读
    <b class='flag-5'>Dijkstra</b><b class='flag-5'>算法</b>和<b class='flag-5'>A</b>*<b class='flag-5'>算法</b>

    路径规划算法实现原理

    本文会用matlab实现Dijkstra算法,并且会分享一些函数用法的链接,也是本人学习得来,供大家参考,批评指正。
    的头像 发表于 09-06 15:36 775次阅读

    中国铁路网的Dijkstra算法实现案例

    该项目分别在DE1-SOC开发板的FPGA和HPS上实现Dijkstra算法,能在中国铁路网中找到两站之间的最短距离和路线。
    的头像 发表于 04-09 11:10 587次阅读
    中国铁路网的<b class='flag-5'>Dijkstra</b><b class='flag-5'>算法</b><b class='flag-5'>实现</b>案例