astar算法matlab代码

Sure! Here's an example of A* algorithm implemented in MATLAB:

function [path, cost] = astar(start, goal, map)
    % Initialize open and closed lists
    openList = PriorityQueue();
    openList.insert(start, 0);
    closedList = containers.Map('KeyType','char','ValueType','double');
    
    % Initialize costs and parent nodes
    gCost = inf(size(map));
    gCost(start) = 0;
    fCost = inf(size(map));
    fCost(start) = heuristic(start, goal);
    parent = containers.Map('KeyType','char','ValueType','char');
    
    % Start A* search
    while ~openList.isEmpty()
        % Get the current node with the lowest fCost
        current = openList.remove();
        
        % Check if goal reached
        if current == goal
            path = reconstructPath(parent, current);
            cost = gCost(current);
            return;
        end
        
        % Add current node to closed list
        closedList(num2str(current)) = fCost(current);
        
        % Generate neighbors of current node
        neighbors = generateNeighbors(current);
        
        for i = 1:length(neighbors)
            neighbor = neighbors(i);
            
            % Check if neighbor is in closed list
            if isKey(closedList, num2str(neighbor))
                continue;
            end
            
            % Calculate tentative gCost for neighbor
            tentativeGCost = gCost(current) + 1;
            
            % Check if neighbor is already in open list
            if openList.contains(neighbor)
                % Skip if the new gCost is higher than the current gCost
                if tentativeGCost >= gCost(neighbor)
                    continue;
                end
            else
                openList.insert(neighbor, inf);
            end
            
            % Update gCost, fCost, and parent
            gCost(neighbor) = tentativeGCost;
            fCost(neighbor) = gCost(neighbor) + heuristic(neighbor, goal);
            parent(num2str(neighbor)) = num2str(current);
            
            % Update priority of neighbor in open list
            openList.updatePriority(neighbor, fCost(neighbor));
        end
    end
    
    % No path found
    path = [];
    cost = inf;
end

function h = heuristic(node, goal)
    % Manhattan distance heuristic
    h = abs(node(1) - goal(1)) + abs(node(2) - goal(2));
end

function neighbors = generateNeighbors(node)
    % Generate 4-connected neighbors
    neighbors = [node(1)-1, node(2);
                 node(1)+1, node(2);
                 node(1), node(2)-1;
                 node(1), node(2)+1];
end

This code assumes that the map is represented as a grid where obstacles are marked as non-traversable cells. The `start` and `goal` variables represent the start and goal positions on the map, respectively. The `PriorityQueue` class is used to efficiently store and retrieve nodes with the lowest fCost.

You can use this code by calling the `astar` function with the appropriate inputs. It will return the path from the start to the goal, as well as the cost of the path. If no path is found, an empty path and infinite cost will be returned.

Please note that this is a basic implementation of A* algorithm and may not be optimized for large-scale problems.