专栏首页mwangblogA*算法例子

A*算法例子

A*算法程序代码

function[distance, path] = a_star(map_size, neighbors, start, goal)

%A* 算法, 找不到路径时返回 error

%map_size 输入 地图尺寸 == size(mymap)

%neighbors 输入 邻居 (cell)

%start 输入 起点

%goal 输入 目标点

%distance 输出 距离

%path 输出 路径

nodes_num =map_size(1) * map_size(2);

open_set =start;

close_set =[];

came_from =inf(nodes_num, 1);

gscore =inf(nodes_num, 1);

fscore =inf(nodes_num, 1);

gscore(start)= 0;

fscore(start)= gscore(start) + h_manhattan(map_size, start, goal);

while~isempty(open_set)

% 取出 open_set 中 f 最小的点 current

min_f = inf;

current = 0;

for i = 1:length(open_set)

node = open_set(i);

if fscore(node) < min_f

min_f = fscore(node);

current = node;

end

end

if current == goal

distance = gscore(current);

path = reconstruct_path(came_from,current);

return;

end

open_set = setdiff(open_set, current);

close_set = [close_set current];

for i = 1:length(neighbors{current})

neighbor = neighbors{current}(i);

if any(ismember(close_set, neighbor))

continue;

end

tentative_gscore = gscore(current) +get_distance(map_size, current, neighbor);

if ~any(ismember(open_set, neighbor))

open_set = [open_set neighbor];

elseif tentative_gscore >=gscore(neighbor)

continue;

end

came_from(neighbor) = current;

gscore(neighbor) = tentative_gscore;

fscore(neighbor) = gscore(neighbor) +h_manhattan(map_size, neighbor, goal);

end

end

error("Failure!");

end

%返回两点曼哈顿距离

functionhscore = h_manhattan(map_size, current, goal)

[current_row,current_col] = ind2sub(map_size, current);

[goal_row,goal_col] = ind2sub(map_size, goal);

hscore =abs(current_row - goal_row) + abs(current_col - goal_col);

end

%构造路径

function path= reconstruct_path(came_from, current)

path =[current];

whilecame_from(current) <= length(came_from)

path = [came_from(current) path];

current = came_from(current);

end

end

=========

从上面可以看出,neighbors,start, goal三个参数必不可少,而map_size参数可以用nodes_num替换,还需要用到三个函数,一个是计算h值的函数,一个是计算某点与邻居距离的函数,还有一个是构造路径的函数。

找栅格地图中两点间最短距离

如下图所示栅格地图,指定起始点和目标点,智能体(或机器人)只能在“上、下、左、右”四个方向移动,找出最短路径:

结果如下:

=========

本文分享自微信公众号 - mwangblog(mwangblog),作者:WM

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2019-05-21

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • A*算法

    从图的特定起始节点开始,A*旨在找到从起始节点到目标节点见具有最小代价的路径(最少行驶距离、最短时间等)。A*算法维护源自起始节点的路径树,并且一次一个地延伸这...

    mwangblog
  • python字典(Dictionary)

    mwangblog
  • gauss消元法

    mwangblog
  • Tree

    spilledyear
  • Leetcode 725. Split Linked List in Parts

    版权声明:博客文章都是作者辛苦整理的,转载请注明出处,谢谢! https://blog.csdn....

    Tyan
  • 摆图片确定行数

    版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/u010105969/article/details/...

    用户1451823
  • Ubuntu18.04(linux)安装MySQL

    Ubuntu18.04 安装mysql或者mariadb之后,发现普通用户和远程都没有权限连接。

    Ryan-Miao
  • ceph数据存储的几个概念

    最下面的蓝色长条可以看成一个个主机,里面的灰色圆柱形可以看成一个个OSD,紫色的cabinet可以也就是一个个机柜, 绿色的row可以看成一排机柜,顶端的roo...

    后端云
  • 致测试同仁们:让我们做安全测试吧!|洞见

    本文首发于InfoQ: http://www.infoq.com/cn/articles/to-test-colleagues-let-us-do-a-safe...

    ThoughtWorks
  • 【Rust项目推荐】Rust开发的一款超好用的接口设计工具

    Panda api是一款接口设计工具,它能够生成文档、提供接口模拟服务(在你没写任何代码之前)、自动测试后端接口,有效提升项目的开发效率和质量。

    MikeLoveRust

扫码关注云+社区

领取腾讯云代金券