用Matlab实现A*算法

下面是一个简单的 Matlab 实现 A* 算法的示例代码：

function [path, cost] = Astar(start, goal, obstacles)
% A* 算法求解路径
% 输入参数：
% start：起点坐标
% goal：终点坐标
% obstacles：障碍物坐标数组
% 输出参数：
% path：求解的路径
% cost：路径总代价

% 初始化 open 和 close 列表
open_list = start;
close_list = [];

% 初始化起点的代价和估价
g_score(start(1), start(2)) = 0;
h_score(start(1), start(2)) = heuristic(start, goal);

while ~isempty(open_list)
    % 选择 open 列表中代价最小的节点扩展
    current = find_min_fscore(open_list, g_score, h_score);
    
    % 如果当前节点为目标节点，则直接返回路径
    if isequal(current, goal)
        path = reconstruct_path(current, start, came_from);
        cost = g_score(current(1), current(2));
        return;
    end
    
    % 从 open 列表中移除当前节点，加入 close 列表
    open_list = remove_from_list(open_list, current);
    close_list = [close_list; current];
    
    % 扩展当前节点的相邻节点
    neighbors = get_neighbors(current, obstacles);
    for i = 1:size(neighbors, 1)
        neighbor = neighbors(i, :);
        
        % 如果相邻节点在 close 列表中，则跳过
        if ismember(neighbor, close_list, 'rows')
            continue;
        end
        
        % 计算从起点到相邻节点的代价
        tentative_g_score = g_score(current(1), current(2)) + ...
            euclidean_distance(current, neighbor);
        
        % 如果相邻节点不在 open 列表中，则加入 open 列表
        if ~ismember(neighbor, open_list, 'rows')
            open_list = [open_list; neighbor];
            tentative_is_better = true;
        else
            % 如果相邻节点已经在 open 列表中，则比较新的代价和原来的代价
            tentative_is_better = tentative_g_score < ...
                g_score(neighbor(1), neighbor(2));
        end
        
        % 如果新的代价更优，则更新代价和父节点
        if tentative_is_better
            came_from(neighbor(1), neighbor(2), :) = current;
            g_score(neighbor(1), neighbor(2)) = tentative_g_score;
            h_score(neighbor(1), neighbor(2)) = heuristic(neighbor, goal);
        end
    end
end

% 如果 open 列表为空，但仍未找到路径，则说明无法到达目标节点
path = [];
cost = Inf;
end

function min_node = find_min_fscore(node_list, g_score, h_score)
% 在 node_list 中查找 f_score 最小的节点
f_score = g_score(node_list(:,1), node_list(:,2)) + ...
    h_score(node_list(:,1), node_list(:,2));
[~, idx] = min(f_score);
min_node = node_list(idx, :);
end

function list = remove_from_list(list, node)
% 从 list 中移除节点 node
idx = find(all(bsxfun(@eq, list, node), 2));
list(idx, :) = [];
end

function neighbors = get_neighbors(node, obstacles)
% 获取 node 的相邻节点，不包括障碍物
x = node(1);
y = node(2);
neighbors = [x-1, y; x+1, y; x, y-1; x, y+1];
idx = any(neighbors < 1, 2) | neighbors(:,1) > size(obstacles, 1) | ...
    neighbors(:,2) > size(obstacles, 2) | ...
    obstacles(sub2ind(size(obstacles), neighbors(:,1), neighbors(:,2)));
neighbors(idx, :) = [];
end

function distance = euclidean_distance(node1, node2)
% 计算 node1 和 node2 之间的欧几里得距离
distance = norm(node1 - node2);
end

function distance = heuristic(node1, node2)
% 计算 node1 和 node2 之间的估价，这里使用欧几里得距离
distance = euclidean_distance(node1, node2);
end

function path = reconstruct_path(current, start, came_from)
% 从当前节点开始，回溯到起点，构造路径
path = current;
while ~isequal(current, start)
    current = squeeze(came_from(current(1), current(2), :))';
    path = [current; path];
end
end

在这个示例代码中，`start` 和 `goal` 分别表示起点和终点的坐标，`obstacles` 是一个二维数组，表示障碍物的位置。`Astar` 函数返回求解的路径和总代价。在算法的实现中，我们使用了一个三维数组 `came_from`，用于记录每个节点的父节点，以便构造路径。

在这个示例代码中，我们使用了 Euclidean 距离作为启发式函数，但实际上你可以根据具体情况选择不同的启发式函数。同时，我们也没有对算法进行优化，例如使用二叉堆等数据结构来维护 open 列表，因此在处理大规模地图时可能会比较慢。