MATLAB上的栅格地图A*算法代码

以下是MATLAB上的栅格地图A*算法代码：

% 定义栅格地图
map = [0 0 0 0 0 0 0 0 0;
0 1 1 1 1 1 1 1 0;
0 1 0 0 0 0 0 1 0;
0 1 0 1 1 1 0 1 0;
0 1 0 0 0 0 0 1 0;
0 1 1 1 0 1 1 1 0;
0 0 0 0 0 0 0 0 0];

% 定义起点和终点
start = [2, 2];
goal = [6, 7];

% 初始化 open 和 closed 列表
open = [];
closed = [];

% 将起点加入 open 列表
open = [open; start, 0, 0];

% A* 算法
while ~isempty(open)
% 找到 f 值最小的节点
[~, index] = min(open(:, 4));
current = open(index, :);
% 如果当前节点为终点，则搜索成功
if current(1:2) == goal
% 回溯路径
path = [current(1:2)];
while ~isequal(path(1,:), start)
for i = 1:size(closed, 1)
if isequal(closed(i, 1:2), path(1,:))
path = [closed(i, 5:6); path];
break;
end
end
end
% 显示路径
disp(path)
break;
end
% 将当前节点从 open 列表中移除
open(index, :) = [];
% 将当前节点加入 closed 列表
closed = [closed; current];
% 扩展当前节点的邻居
for i = -1:1
for j = -1:1
% 忽略当前节点和斜向邻居
if i == 0 && j == 0 || abs(i) == abs(j)
continue;
end
% 计算邻居的坐标
neighbor = current(1:2) + [i, j];
% 忽略超出地图范围的邻居
if neighbor(1) < 1 || neighbor(1) > size(map, 1) || neighbor(2) < 1 || neighbor(2) > size(map, 2)
continue;
end
% 忽略障碍物
if map(neighbor(1), neighbor(2)) == 1
continue;
end
% 计算邻居的 g 值
g = current(3) + norm([i, j]);
% 如果邻居已经在 closed 列表中，则忽略
if any(ismember(closed(:, 1:2), neighbor, 'rows'))
continue;
end
% 如果邻居不在 open 列表中，则加入 open 列表
if ~any(ismember(open(:, 1:2), neighbor, 'rows'))
h = norm(neighbor - goal);
open = [open; neighbor, g, h, g + h];
% 否则更新邻居的 g 值和 f 值
else
index = find(ismember(open(:, 1:2), neighbor, 'rows'));
if g < open(index, 3)
open(index, 3) = g;
open(index, 4) = g + open(index, 5);
end
end
end
end
end

如果以上代码无法正常运行，请检查栅格地图、起点和终点的定义是否正确。