首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >A*算法例子

A*算法例子

作者头像
mwangblog
发布2019-05-22 00:01:53
1.3K0
发布2019-05-22 00:01:53
举报
文章被收录于专栏:mwangblogmwangblog

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值的函数,一个是计算某点与邻居距离的函数,还有一个是构造路径的函数。

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

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

结果如下:

=========

本文参与 腾讯云自媒体分享计划,分享自微信公众号。
原始发表:2019-05-21,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 mwangblog 微信公众号,前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体分享计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • A*算法程序代码
  • 找栅格地图中两点间最短距离
领券
问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档