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值的函数,一个是计算某点与邻居距离的函数,还有一个是构造路径的函数。
如下图所示栅格地图,指定起始点和目标点,智能体(或机器人)只能在“上、下、左、右”四个方向移动,找出最短路径:
结果如下:
=========