A star算法matlab

以下是一个简单的 A* 算法的 MATLAB 实现：

function [path, cost] = Astar(start, goal, map)
% start：起点坐标
% goal：终点坐标
% map：地图

[row, col] = size(map); %获取地图大小
start_node.pos = start; %设置起始节点
start_node.g = 0;
start_node.h = heuristic(start, goal); %启发函数
start_node.f = start_node.g + start_node.h;
start_node.parent = 0;
open_list = [start_node];
closed_list = [];

while ~isempty(open_list)
    [min_f, index] = min([open_list.f]); %选择开放列表中f值最小的节点
    current_node = open_list(index);

    if current_node.pos == goal %到达终点
        path = [];
        cost = current_node.f;
        while current_node.parent ~= 0 %回溯路径
            path = [current_node.pos; path];
            current_node = current_node.parent;
        end
        path = [current_node.pos; path];
        return;
    end

    open_list(index) = []; %从开放列表中移除当前节点
    closed_list = [closed_list; current_node]; %添加到关闭列表中

    %添加相邻节点
    for i = -1:1
        for j = -1:1
            if i == 0 && j == 0
                continue;
            end
            neighbor = [current_node.pos(1)+i, current_node.pos(2)+j];
            if neighbor(1) < 1 || neighbor(1) > row || neighbor(2) < 1 || neighbor(2) > col %超出地图范围
                continue;
            end
            if map(neighbor(1), neighbor(2)) == 1 %障碍物
                continue;
            end
            if ismember(neighbor, [closed_list.pos], 'rows') %该节点已在关闭列表中
                continue;
            end
            g = current_node.g + 1; %计算g值
            h = heuristic(neighbor, goal); %计算h值
            f = g + h; %计算f值
            if ~ismember(neighbor, [open_list.pos], 'rows') %该节点不在开放列表中
                neighbor_node.pos = neighbor;
                neighbor_node.g = g;
                neighbor_node.h = h;
                neighbor_node.f = f;
                neighbor_node.parent = current_node;
                open_list = [open_list; neighbor_node]; %添加到开放列表中
            else %该节点已在开放列表中
                index = find([open_list.pos] == neighbor);
                if g < open_list(index).g %更新g值
                    open_list(index).g = g;
                    open_list(index).f = g + open_list(index).h;
                    open_list(index).parent = current_node;
                end
            end
        end
    end
end

%找不到路径
path = [];
cost = -1;
end

function h = heuristic(node, goal) %曼哈顿距离
h = abs(node(1)-goal(1)) + abs(node(2)-goal(2));
end

使用方法：

1. 定义地图，0 表示可通过，1 表示障碍物。

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

2. 调用 A* 算法。

[start, goal] = [1, 1; 8, 8];
[path, cost] = Astar(start, goal, map);

其中，path 为路径，cost 为路径长度。