A*算法 matlab

以下是A*算法的MATLAB实现：

function [path, cost] = A_star(start, goal, map)
% A_star算法
% start: 起点坐标
% goal: 终点坐标
% map: 地图，1表示可行，0表示障碍
% path: 路径，包括起点和终点
% cost: 路径总代价

% 定义启发函数
heuristic = @(pos) sqrt(sum((pos - goal) .^ 2));

% 初始化open和closed列表
open_list = [start, 0, heuristic(start)];
closed_list = [];

while ~isempty(open_list)
% 从open列表中选取F值最小的节点
[~, idx] = min(open_list(:, 3));
current = open_list(idx, 1:2);
g = open_list(idx, 2);

% 判断是否到达终点
if isequal(current, goal)
path = reconstruct_path(current);
cost = g;
return;
end

% 将当前节点加入closed列表
open_list(idx, :) = [];
closed_list = [closed_list; current];

% 扩展当前节点的邻居节点
neighbors = get_neighbors(current, map);
for i = 1:size(neighbors, 1)
neighbor = neighbors(i, :);
% 判断邻居节点是否在closed列表中
if any(ismember(closed_list, neighbor, 'rows'))
continue;
end
% 计算邻居节点的代价
temp_g = g + sqrt(sum((neighbor - current) .^ 2));
% 判断邻居节点是否在open列表中
idx = find(ismember(open_list(:, 1:2), neighbor, 'rows'));
if isempty(idx)
% 加入open列表
open_list = [open_list; neighbor, temp_g, heuristic(neighbor)];
elseif temp_g < open_list(idx, 2)
% 更新open列表中的节点代价
open_list(idx, 2) = temp_g;
end
end
end

% 没有找到路径
path = [];
cost = inf;

% 重构路径
function path = reconstruct_path(current)
path = current;
while ~isequal(path(1,:), start)
parent = get_parent(path(1,:), closed_list);
path = [parent; path];
end
end

% 获取邻居节点
function neighbors = get_neighbors(current, map)
[m, n] = size(map);
i = current(1);
j = current(2);
neighbors = [];
if i > 1 && map(i-1, j) == 1 % 上
neighbors = [neighbors; i-1, j];
end
if i < m && map(i+1, j) == 1 % 下
neighbors = [neighbors; i+1, j];
end
if j > 1 && map(i, j-1) == 1 % 左
neighbors = [neighbors; i, j-1];
end
if j < n && map(i, j+1) == 1 % 右
neighbors = [neighbors; i, j+1];
end
end

% 获取父节点
function parent = get_parent(node, closed_list)
idx = find(ismember(closed_list(:, 1:2), node, 'rows'));
parent = closed_list(idx, 4:5);
end

end

使用示例：

map = [
1 1 1 0 0 0;
1 0 1 0 1 0;
1 0 1 1 1 0;
0 0 0 0 1 0;
0 1 1 1 1 1;
0 0 0 0 0 1;
];

start = [1, 1];
goal = [6, 6];
[path, cost] = A_star(start, goal, map);

disp(path);
disp(cost);