a star算法matlab代码

以下是A*算法的Matlab代码示例：

function [path, cost] = astar(start, goal, map)
% A* algorithm implementation in Matlab
% Input arguments:
% start - starting node coordinates [x, y]
% goal - goal node coordinates [x, y]
% map - binary matrix representing the environment, where 0 is free and 1 is occupied
% Output arguments:
% path - array of nodes representing the path from start to goal
% cost - total cost of the path

% Define the heuristic function (Euclidean distance)
heuristic = @(a,b) sqrt((a(1)-b(1))^2 + (a(2)-b(2))^2);

% Define the cost function (Euclidean distance)
costfunc = @(a,b) sqrt((a(1)-b(1))^2 + (a(2)-b(2))^2);

% Define the open and closed lists
openList = start;
closedList = [];

% Define the gScore and fScore of the starting node
gScore = inf(size(map));
gScore(start(2), start(1)) = 0;
fScore = inf(size(map));
fScore(start(2), start(1)) = heuristic(start, goal);

% Start the search loop
while ~isempty(openList)
    % Select the node with the lowest fScore in the openList
    [~, current] = min(fScore(openList(:,2), openList(:,1)));
    current = openList(current,:);
    
    % Check if the current node is the goal node
    if isequal(current, goal)
        % Reconstruct the path and compute the cost
        path = [current];
        cost = 0;
        while ~isequal(current, start)
            [~, prev] = min(gScore(current(2)-1:current(2)+1, current(1)-1:current(1)+1) + ...
                costfunc(current, [current(1)-1:current(1)+1; current(2)-1:current(2)+1]'));
            current = [current(1)-1:current(1)+1; current(2)-1:current(2)+1](:,prev);
            path = [current path];
            cost = cost + costfunc(path(:,1), path(:,2));
        end
        return
    end
    
    % Move the current node from the openList to the closedList
    openList(ismember(openList, current, 'rows'),:) = [];
    closedList(end+1,:) = current;
    
    % Generate the neighbors of the current node
    neighbors = [current(1)-1 current(2);
                 current(1)+1 current(2);
                 current(1) current(2)-1;
                 current(1) current(2)+1];
    neighbors(neighbors(:,1)<1 | neighbors(:,1)>size(map,2) | ...
              neighbors(:,2)<1 | neighbors(:,2)>size(map,1) | ...
              map(sub2ind(size(map),neighbors(:,2),neighbors(:,1))), :) = [];
    
    % Iterate over the neighbors of the current node
    for i = 1:size(neighbors,1)
        neighbor = neighbors(i,:);
        % Check if the neighbor is already in the closedList
        if any(ismember(closedList, neighbor, 'rows'))
            continue
        end
        % Compute the tentative gScore of the neighbor
        tentative_gScore = gScore(current(2), current(1)) + costfunc(current, neighbor);
        % Check if the neighbor is already in the openList
        if ~any(ismember(openList, neighbor, 'rows'))
            openList(end+1,:) = neighbor;
        % Check if the tentative gScore is greater than the current gScore of the neighbor
        elseif tentative_gScore >= gScore(neighbor(2), neighbor(1))
            continue
        end
        % Update the gScore and fScore of the neighbor
        gScore(neighbor(2), neighbor(1)) = tentative_gScore;
        fScore(neighbor(2), neighbor(1)) = tentative_gScore + heuristic(neighbor, goal);
    end
end

% If the goal node is not reachable, return an empty path and infinite cost
path = [];
cost = inf;

end

这个代码中的输入参数 `map` 是一个二元矩阵，其中 0 表示空闲，1 表示障碍物。输出参数 `path` 是一个节点数组，表示从起点到终点的路径；`cost` 是该路径的总成本。