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)

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

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

发表于

我来说两句

0 条评论
登录 后参与评论

扫码关注云+社区

领取腾讯云代金券