a星算法matlab代码

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

function path = AStar(startNode, goalNode, graph)
    % 初始化openSet和closedSet
    openSet = startNode;
    closedSet = [];

    % 初始化起始节点的代价和启发式估计值
    startNode.gCost = 0;
    startNode.hCost = heuristic(startNode, goalNode);
    startNode.fCost = startNode.gCost + startNode.hCost;

    while ~isempty(openSet)
        % 从openSet中选择fCost最小的节点
        currentNode = openSet(1);
        currentIndex = 1;
        for i = 1:length(openSet)
            if openSet(i).fCost < currentNode.fCost
                currentNode = openSet(i);
                currentIndex = i;
            end
        end

        % 如果当前节点是目标节点，返回路径
        if currentNode == goalNode
            path = reconstructPath(currentNode);
            return;
        end

        % 将当前节点从openSet中移除，并加入closedSet
        openSet(currentIndex) = [];
        closedSet = [closedSet, currentNode];

        % 遍历当前节点的邻居节点
        neighbors = getNeighbors(currentNode, graph);
        for i = 1:length(neighbors)
            neighbor = neighbors(i);

            % 如果邻居节点已经在closedSet中，跳过
            if ismember(neighbor, closedSet)
                continue;
            end

            % 计算邻居节点的代价和启发式估计值
            tentativeGCost = currentNode.gCost + distance(currentNode, neighbor);
            if ~ismember(neighbor, openSet) || tentativeGCost < neighbor.gCost
                neighbor.gCost = tentativeGCost;
                neighbor.hCost = heuristic(neighbor, goalNode);
                neighbor.fCost = neighbor.gCost + neighbor.hCost;
                neighbor.parent = currentNode;

                % 如果邻居节点不在openSet中，加入openSet
                if ~ismember(neighbor, openSet)
                    openSet = [openSet, neighbor];
                end
            end
        end
    end

    % 如果openSet为空，表示无法找到路径
    error("No path found.");
end

function h = heuristic(node1, node2)
    % 启发式函数，计算两个节点之间的估计距离
    h = sqrt((node1.x - node2.x)^2 + (node1.y - node2.y)^2);
end

function d = distance(node1, node2)
    % 计算两个节点之间的实际距离
    d = sqrt((node1.x - node2.x)^2 + (node1.y - node2.y)^2);
end

function path = reconstructPath(node)
    % 从目标节点开始，逆向构建路径
    path = [];
    while ~isempty(node.parent)
        path = [node, path];
        node = node.parent;
    end
end

function neighbors = getNeighbors(node, graph)
    % 获取当前节点的邻居节点
    neighbors = [];
    for i = 1:length(graph)
        if graph(node, i) == 1
            neighbors = [neighbors, i];
        end
    end
end

这段代码实现了A*算法的基本逻辑，包括初始化openSet和closedSet、计算节点的代价启发式估计值、选择fCost最小的节点、更新节点的代价和启发式估计值等。你可以根据自己的需求进行修改和扩展。