A*算法matlab代码

A*算法是一种常用的启发式搜索算法，用于在图形或网络中找到最短路径。下面是一个简单的A*算法的Matlab代码示例：

function [path, cost] = Astar(start, goal, graph, heuristic)
    % 初始化起始节点和目标节点
    startNode = Node(start, 0, heuristic(start));
    goalNode = Node(goal, Inf, 0);
    
    % 初始化开放列表和关闭列表
    openList = PriorityQueue();
    openList.insert(startNode);
    closedList = containers.Map();
    
    while ~openList.isEmpty()
        % 从开放列表中选择具有最小总代价的节点
        currentNode = openList.pop();
        
        % 如果当前节点是目标节点，则找到了最短路径
        if currentNode == goalNode
            path = reconstructPath(currentNode);
            cost = currentNode.g;
            return;
        end
        
        % 将当前节点添加到关闭列表中
        closedList(currentNode.id) = currentNode;
        
        % 获取当前节点的邻居节点
        neighbors = getNeighbors(currentNode, graph);
        
        for i = 1:length(neighbors)
            neighbor = neighbors(i);
            
            % 如果邻居节点已经在关闭列表中，则跳过
            if isKey(closedList, neighbor.id)
                continue;
            end
            
            % 计算邻居节点的代价
            gScore = currentNode.g + distance(currentNode, neighbor);
            
            % 如果邻居节点不在开放列表中，则将其添加到开放列表中
            if ~openList.contains(neighbor)
                neighbor.g = gScore;
                neighbor.h = heuristic(neighbor.id);
                neighbor.parent = currentNode;
                openList.insert(neighbor);
            else
                % 如果邻居节点已经在开放列表中，并且新的代价更小，则更新代价和父节点
                if gScore < neighbor.g
                    neighbor.g = gScore;
                    neighbor.parent = currentNode;
                    openList.updatePriority(neighbor);
                end
            end
        end
    end
    
    % 如果开放列表为空，则无法找到最短路径
    path = [];
    cost = Inf;
end

function path = reconstructPath(node)
    % 从目标节点开始，沿着父节点回溯，构建最短路径
    path = [];
    while ~isempty(node)
        path = [node.id, path];
        node = node.parent;
    end
end

function distance = distance(node1, node2)
    % 计算两个节点之间的距离（可以根据具体情况进行修改）
    distance = norm(node1.id - node2.id);
end

function neighbors = getNeighbors(node, graph)
    % 获取当前节点的邻居节点（可以根据具体情况进行修改）
    neighbors = graph(node.id);
end

classdef Node
    properties
        id      % 节点ID
        g       % 从起始节点到当前节点的代价
        h       % 从当前节点到目标节点的启发式估计代价
        parent  % 父节点
    end
    
    methods
        function obj = Node(id, g, h)
            obj.id = id;
            obj.g = g;
            obj.h = h;
            obj.parent = [];
        end
    end
end

classdef PriorityQueue
    properties
        elements  % 元素列表
        priorities  % 优先级列表
    end
    
    methods
        function obj = PriorityQueue()
            obj.elements = {};
            obj.priorities = [];
        end
        
        function insert(obj, element)
            % 将元素插入到优先级队列中
            obj.elements{end+1} = element;
            obj.priorities(end+1) = element.g + element.h;
        end
        
        function element = pop(obj)
            % 弹出具有最小优先级的元素
            [~, index] = min(obj.priorities);
            element = obj.elements{index};
            obj.elements(index) = [];
            obj.priorities(index) = [];
        end
        
        function updatePriority(obj, element)
            % 更新元素的优先级
            index = find(obj.elements == element);
            obj.priorities(index) = element.g + element.h;
        end
        
        function containsElement = contains(obj, element)
            % 判断元素是否在优先级队列中
            containsElement = any(obj.elements == element);
        end
        
        function isEmptyQueue = isEmpty(obj)
            % 判断优先级队列是否为空
            isEmptyQueue = isempty(obj.elements);
        end
    end
end

这段代码实现了A*算法的基本逻辑，包括节点类、优先级队列类和主函数。你可以根据自己的需求进行修改和扩展。希望对你有帮助！