A*算法（matlab）求解题目

代码（Astar.m文件）
% A*算法主函数，用于求从起点到目标节点的最短路径
function [path, cost] = AStar(w, startIndex, goalIndex)
% w为邻接矩阵，表示图中节点间的连接关系和距离
% startIndex为起点节点的索引
% goalIndex为目标节点的索引

% 定义节点结构体，用于存储节点的相关信息
nodeStruct = struct('index', [], 'g', [], 'h', [], 'f', [], 'parent', []);

% 初始化openList和closedList
openList = nodeStruct;
closedList = nodeStruct;

% 将起点节点加入openList
startNode = nodeStruct;
startNode.index = startIndex;
startNode.g = 0;
startNode.h = heuristic(startIndex, goalIndex); % 计算启发式值
startNode.f = startNode.g + startNode.h;
startNode.parent = [];
openList(end + 1) = startNode;

% 主循环，只要openList不为空就继续探索
while ~isempty(openList)
    % 找到openList中f值最小的节点
    [~, idx] = min([openList.f]);
    currentNode = openList(idx);

    % 将当前节点从openList移到closedList
    openList(idx) = [];
    closedList(end + 1) = currentNode;

    % 判断是否到达目标节点
    if currentNode.index == goalIndex
        path = reconstructPath(currentNode);
        cost = currentNode.g;
        return;
    end

    % 获取当前节点的邻居节点（根据邻接矩阵确定邻居）
    neighbors = getNeighbors(currentNode.index, w);

    % 遍历邻居节点
    for i = 1:size(neighbors, 2)
        neighborIndex = neighbors(i);
        neighborNode = nodeStruct;
        neighborNode.index = neighborIndex;

        % 计算到邻居节点的临时g值（实际代价）
        tentative_gScore = currentNode.g + w(currentNode.index, neighborIndex);

        % 检查邻居节点是否在closedList中，如果在且临时g值不小于已有的g值，跳过该邻居节点
        if isInList(neighborNode, closedList) && tentative_gScore >= getNodeGScore(neighborNode, closedList)
            continue;
        end

        % 计算邻居节点的h值（启发式估计代价）和f值
        neighborNode.h = heuristic(neighborNode.index, goalIndex);
        neighborNode.f = tentative_gScore + neighborNode.h;
        neighborNode.g = tentative_gScore;
        neighborNode.parent = currentNode;

        % 如果邻居节点不在openList中或者新的f值更小，则更新或添加邻居节点到openList
        if ~isInList(neighborNode, openList) || (isInList(neighborNode, openList) && neighborNode.f < getNodeFScore(neighborNode, openList))
            openList(end + 1) = neighborNode;
        end
    end
end

% 如果没有找到路径，返回空路径和无穷大的代价
path = [];
cost = Inf;

end

% 计算启发式函数值，这里简单以欧几里得距离示例（实际需按需优化）
function h = heuristic(nodeIndex, goalIndex)
h = sqrt((nodeIndex - goalIndex)^2); % 简单用索引差值的欧几里得距离，可按实际场景改
end

% 获取节点的邻居节点，根据邻接矩阵判断连接关系
function neighbors = getNeighbors(nodeIndex, w)
neighbors = [];
[n, ~] = size(w);
for i = 1:n
if w(nodeIndex, i) ~= Inf
neighbors(end + 1) = i;
end
end
end

% 判断节点是否在节点列表中
function flag = isInList(node, list)
flag = false;
for i = 1:size(list, 2)
if list(i).index == node.index
flag = true;
break;
end
end
end

% 获取节点在节点列表中的g值
function g = getNodeGScore(node, list)
for i = 1:size(list, 2)
if list(i).index == node.index
g = list(i).g;
break;
end
end
end

% 获取节点在节点列表中的f值
function f = getNodeFScore(node, list)
for i = 1:size(list, 2)
if list(i).index == node.index
f = list(i).f;
break;
end
end
end

% 回溯路径，从目标节点根据父节点一直回溯到起点
function path = reconstructPath(node)
path = [];
while ~isempty(node)
path(:, end + 1) = node.index;
node = node.parent;
end
path = path(:, end