A*算法和Dijkstra算法的matlab实现

最新推荐文章于 2023-03-14 07:00:03 发布

weixin_54754244

最新推荐文章于 2023-03-14 07:00:03 发布

阅读量887

点赞数 3

分类专栏：路径规划算法

本文链接：https://blog.csdn.net/weixin_54754244/article/details/119611069

版权

本文介绍了如何在MATLAB中实现A*算法和Dijkstra算法，用于基于栅格地图的机器人路径规划。作者首先展示了地图生成函数，接着详细解释了查找邻节点的函数，最后给出了主函数的实现，包括算法初始化、循环遍历和路径规划。通过比较，A*算法能更有效地避免局部最优解，减少搜索节点数量。

摘要由CSDN通过智能技术生成

A*算法和Dijkstra算法的matlab实现

写在前面：最近看了古月学院的视频课，记录一下我看的东西
这个课是付费的链接在这：古月学院基于栅格地图的机器人路径规划算法指南 • 黎万洪
我自己也是现学现卖，大家可以多去古月学院看看，课挺好的，下面的内容算是我自己记的笔记，个人不太喜欢Markdown的编辑方式，代码部分不好理解的我自己加了一点备注。
这个代码最初是挺好的，我给改出了一点小问题，希望也有小伙伴帮我看看
一、简单走一下dj流程：
在这里插入图片描述
1.初始状态D为起点，S集合中只有一个起点D,而U集合是未被扩展的节点的集合，此时C和E作为D的邻节点，计算他们的cost.
2.进行第一轮循环，将cost最小的点C添加到U集合中去，并在U集合中删去C。将C作为扩展节点（在程序里面叫做父节点）进行搜索C的邻节点BFE，并计算C到所有临节点的代价。最后将C节点的cost加上C节点到邻节点的cost与邻节点原有的cost进行对比，如果原有的小那么保留原有的cost，不更新记录的路径，反之更新邻节点的cost，并在记录路径的元胞数组中更新记录的路径。此时BEF都应该进行更新。
在这里插入图片描述
3.在上一步骤中，U集合中cost最小的节点为E,其cost为4，因此将节点E从U集合中移动到S集合中并计算E到他的邻节点的代价值。然后按照上一步的方式进行比较。结果发现，F记录的代价为9，然而从E到F在加上E点的代价为6，更新F点的代价值，并更新记录路径的元胞数组中F点的路径记录。G点的代价值为12.

4.按照以上方法进行循环，最后当U集合中没有点了，或者是已经发现了终点，循环结束。

##matlab实现

1.函数1，定义地图

传入的参数是地图的行数与列数，输出结果为栅格地图field和cmap颜色MAP图

function [field,cmap] = defColorMap(rows, cols)
%定义颜色的rgb值
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.25;                            %障碍物出现的概率
obsNum = floor(rows*cols*obsRate);         %计算障碍物数量并取整数
obsIndex = randi([1,rows*cols],obsNum,1);  %
field(obsIndex) = 2;                       %将障碍物区域颜色定义为黑色

第20 行代码的解释
r=randi([iMin,iMax],m,n)在开区间（iMin,iMax）生成m*n型随机矩阵
`有了这个函数以后在主函数中执行下列指令就可以看到生成的地图了：``

clc
clear
rows = 40;
cols = 40;
[field,cmap] = defColorMap(rows, cols);             %调用函数生成地图
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.函数2用来查找邻节点的函数

function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
%输入值为行数，列数，要查的节点的线性索引值，以及地栅格地图
%将节点的线性索引值转化为行列索引
[row, col] = ind2sub([rows,cols], lineIndex);
%定义数据类型为8行2列的矩阵，第一列用来记录邻节点的索引值，第二列用来记录从该节点到
%邻节点的代价
neighborNodes = inf(8,2);

%% 查找当前父节点临近的周围8个子节点
% 左上节点
if row-1 > 0 && col-1 > 0
    %将左上个节点的行列索引计算出来并转化为线性索引，最后将索引值放到函数返回矩阵中
    child_node_sub = [row-1, col-1];
    child_node_line = sub2ind(