用matlab写一个A*算法

好的，以下是用 MATLAB 写的 A* 算法示例代码：

function [path, cost] = a_star(start, goal, map)
% A* algorithm implementation
% Inputs:
%   start: starting node coordinates [x,y]
%   goal: goal node coordinates [x,y]
%   map: map of the environment, 0 for free space and 1 for obstacles
% Outputs:
%   path: path from start to goal nodes as a list of [x,y] coordinates
%   cost: cost of the path

% Define Heuristic Function: Euclidean Distance
heuristic = @(a,b) sqrt((a(1)-b(1))^2 + (a(2)-b(2))^2);

% Define cost function
cost_function = @(a,b) ((a(1)-b(1))^2 + (a(2)-b(2))^2);

% Initialize start node
start_node.g_cost = 0;
start_node.h_cost = heuristic(start, goal);
start_node.f_cost = start_node.g_cost + start_node.h_cost;
start_node.coordinates = start;
start_node.parent = [];

% Initialize open and closed sets
open_set = [start_node];
closed_set = [];

% Loop until goal reached or no path exists
while ~isempty(open_set)
    % Find the node with the lowest f_cost value
    current_node_idx = 1;
    current_node = open_set(current_node_idx);
    for i = 1:length(open_set)
        if open_set(i).f_cost < current_node.f_cost
            current_node_idx = i;
            current_node = open_set(current_node_idx);
        end
    end

    % Check if goal reached
    if current_node.coordinates == goal
        path = [];
        cost = current_node.g_cost;
        while ~isempty(current_node.parent)
            path = [current_node.coordinates; path];
            current_node = current_node.parent;
        end
        path = [start; path];
        return
    end

    % Remove current node from open set and add to closed set
    open_set(current_node_idx) = [];
    closed_set = [closed_set; current_node];

    % Find neighbors of current node
    [neighbors_x, neighbors_y] = find(neighbors(map, current_node.coordinates));
    neighbors = [neighbors_x, neighbors_y];

    % Loop through each neighbor
    for i = 1:length(neighbors)
        neighbor_node.coordinates = neighbors(i,:);
        neighbor_node.g_cost = current_node.g_cost + cost_function(current_node.coordinates, neighbor_node.coordinates);
        neighbor_node.h_cost = heuristic(neighbor_node.coordinates, goal);
        neighbor_node.f_cost = neighbor_node.g_cost + neighbor_node.h_cost;
        neighbor_node.parent = current_node;

        % Check if neighbor is already in closed set
        if any(ismember(closed_set, neighbor_node.coordinates, 'rows'))
            continue
        end

        % Check if neighbor is already in open set
        neighbor_idx = find(ismember(open_set, neighbor_node.coordinates, 'rows'));
        if ~isempty(neighbor_idx)
            % Check if new path to neighbor is better
            if neighbor_node.g_cost < open_set(neighbor_idx).g_cost
                open_set(neighbor_idx) = neighbor_node;
            end
        else
            % Add neighbor to open set
            open_set = [open_set; neighbor_node];
        end
    end
end

% If no path exists
path = [];
cost = inf;

end


function [neighborhood] = neighbors(map, coordinates)
% Returns a 3x3 neighborhood around the given coordinates
neighborhood = map(coordinates(1)-1:coordinates(1)+1, coordinates(2)-1:coordinates(2)+1);
end

这个函数的输入参数包括起点 `start`，终点 `goal`，以及一张地图 `map`，其中 0 表示空闲区域，1 表示障碍。输出参数包括路径 `path` 和成本 `cost`。该算法使用 Euclidean 距离作为启发式函数，并将代价函数定义为两点之间欧几里得距离的平方。该算法使用一个开放集合和一个已关闭集合来保持搜索状态。