A*算法Matlab代码
时间: 2023-05-19 09:03:28 浏览: 51
以下是一个简单的 A* 算法的 Matlab 代码:
function [path, cost] = A_star(start, goal, map)
% start: 起点坐标
% goal: 终点坐标
% map: 地图,1 表示障碍物,0 表示可行区域
% 初始化 open 和 close 列表
open_list = [start, 0, heuristic(start, goal)];
close_list = [];
while ~isempty(open_list)
% 从 open 列表中找到 f 值最小的节点
[~, idx] = min(open_list(:, 3));
current = open_list(idx, :);
% 如果当前节点是终点,返回路径和代价
if isequal(current(1:2), goal)
path = backtrack(current, close_list);
cost = current(2);
return;
end
% 将当前节点从 open 列表中移除,加入 close 列表
open_list(idx, :) = [];
close_list = [close_list; current];
% 扩展当前节点的邻居
neighbors = get_neighbors(current(1:2), map);
for i = 1:size(neighbors, 1)
neighbor = [neighbors(i, :), current(2)+1, heuristic(neighbors(i, :), goal)];
% 如果邻居节点已经在 close 列表中,跳过
if ismember(neighbor(1:2), close_list(:, 1:2), 'rows')
continue;
end
% 如果邻居节点不在 open 列表中,加入 open 列表
idx = find(ismember(open_list(:, 1:2), neighbor(1:2), 'rows'));
if isempty(idx)
open_list = [open_list; neighbor];
% 如果邻居节点已经在 open 列表中,更新其代价和父节点
else
if neighbor(2) < open_list(idx, 2)
open_list(idx, 2:4) = neighbor(2:4);
end
end
end
end
% 如果 open 列表为空,说明无法到达终点
path = [];
cost = Inf;
end
function h = heuristic(node, goal)
% 计算启发函数值,这里使用曼哈顿距离
h = abs(node(1)-goal(1)) + abs(node(2)-goal(2));
end
function neighbors = get_neighbors(node, map)
% 获取当前节点的邻居,这里只考虑上下左右四个方向
neighbors = [];
if node(1) > 1 && map(node(1)-1, node(2)) == 0
neighbors = [neighbors; node(1)-1, node(2)];
end
if node(1) < size(map, 1) && map(node(1)+1, node(2)) == 0
neighbors = [neighbors; node(1)+1, node(2)];
end
if node(2) > 1 && map(node(1), node(2)-1) == 0
neighbors = [neighbors; node(1), node(2)-1];
end
if node(2) < size(map, 2) && map(node(1), node(2)+1) == 0
neighbors = [neighbors; node(1), node(2)+1];
end
end
function path = backtrack(node, close_list)
% 回溯路径
path = [node(1:2)];
while ~isequal(node(1:2), close_list(1, 1:2))
idx = find(ismember(close_list(:, 1:2), node(4:5), 'rows'));
node = close_list(idx, :);
path = [path; node(1:2)];
end
path = flipud(path);
end