使用A*算法寻找最低成本路径的Java代码调试求助

程序需从节点A出发，寻找通往目标节点G1、G2、G3中成本最低的路径。运行代码后得到以下错误输出：

Cheapest path: [A, C, D, G2]
Goal state: G2
Number of cycles: 3

以下是运行的Java代码：

import java.util.*;

public class AStarSearch {
  
// define a private class Node which stores g from start node
  //stores h from current node to goal node
  
    private static final int INFINITY = Integer.MAX_VALUE;
    
    private static class Node implements Comparable<Node> {
        String state;
        int g;
        int h;

        Node parent;

        public Node(String state, int g, int h, Node parent) {
            this.state = state;
            this.g = g;
            this.h = h;
            this.parent = parent;
        }

        public int getTotalCost() {
            return g + h;
        }

        @Override
        public int compareTo(Node other) {
            return Integer.compare(this.getTotalCost(), other.getTotalCost());
        }
    }
    
//takes an array of node labels, an array of heuristic values 
    //takes startstate as input
    
    private static List<String> aStarSearch(int[][] matrix, String[] labels, int[] heuristic, String startState) {
        Map<String, Integer> heuristicMap = new HashMap<>();
        for (int i = 0; i < labels.length; i++) {
            heuristicMap.put(labels[i], heuristic[i]);
        }

        Map<String, Integer> gScores = new HashMap<>();
        Map<String, Integer> fScores = new HashMap<>();
        
        //starts with start node and adds to open set
        //sets the g score and total f score of start node

        PriorityQueue<Node> openSet = new PriorityQueue<>();
        Set<String> closedSet = new HashSet<>();

        Node startNode = new Node(startState, 0, heuristicMap.get(startState), null);
        openSet.add(startNode);
        gScores.put(startState, 0);
        fScores.put(startState, startNode.getTotalCost());

        while (!openSet.isEmpty()) {
            Node current = openSet.poll();
            String currentState = current.state;

            if (heuristicMap.get(currentState) == 0) {
                // Reached a goal state, return the path
                List<String> path = new ArrayList<>();
                Node node = current;
                while (node != null) {
                    path.add(0, node.state);
                    node = node.parent;
                }
                return path;
            }

            closedSet.add(currentState);

            for (int i = 0; i < labels.length; i++) {
                String nextState = labels[i];
                int cost = matrix[i][Arrays.asList(labels).indexOf(currentState)];
                if (cost > 0) {
                    int tentativeGScore = gScores.get(currentState) + cost;
                    int tentativeFScore = tentativeGScore + heuristicMap.get(nextState);

                    if (closedSet.contains(nextState) && tentativeFScore >= fScores.getOrDefault(nextState, INFINITY)) {
                        continue;
                    }

                    if (!closedSet.contains(nextState) || tentativeFScore < fScores.getOrDefault(nextState, INFINITY)) {
                        Node nextNode = new Node(nextState, tentativeGScore, heuristicMap.get(nextState), current);
                        openSet.remove(nextNode); // Remove nextNode if it's already in the openSet
                        openSet.add(nextNode);
                        gScores.put(nextState, tentativeGScore);
                        fScores.put(nextState, tentativeFScore);
                    }
                }
            }
        }

        // No path found
        return null;
    }

    public static void main(String[] args) {
        String[] labels = {"A", "B", "C", "D", "E", "H", "J", "G1", "G2", "G3"};

        int[][] matrix = {
                {0, 0, 0, 6, 1, 0, 0, 0, 0, 0},
                {5, 0, 2, 0, 0, 0, 0, 0, 0, 0},
                {9, 3, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, 1, 0, 2, 0, 0, 0, 0, 0},
                {6, 0, 0, 0, 0, 2, 0, 0, 0, 0},
                {0, 0, 0, 7, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 0, 2, 0, 0, 0, 0, 0},
                {0, 9, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 5, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 0, 0, 8, 7, 0, 0, 0}
        };

        int[] heuristic = {5, 7, 3, 4, 6, 8, 5, 0, 0, 0};

        String startState = "A";

        List<String> cheapestPath = aStarSearch(matrix, labels, heuristic, startState);

        if (cheapestPath != null) {
            System.out.println("Cheapest path: " + cheapestPath);
            System.out.println("Goal state: " + cheapestPath.get(cheapestPath.size() - 1));
            System.out.println("Number of cycles: " + (cheapestPath.size() - 1));
        } else {
            System.out.println("No path found.");
        }
    }
}

核心错误点 #

1. 邻接矩阵读取方向完全错误：代码中matrix[i][currentIndex]读取的是i节点到当前节点的成本，而非当前节点到i节点的成本，导致错误识别了不存在的路径（比如A→C）。
2. 提前终止搜索：找到第一个目标节点就直接返回，但A*算法中第一个弹出的目标节点不一定是全局成本最低的，需要遍历所有可达目标后再比较总成本。
3. 节点匹配逻辑缺失：未重写Node类的equals和hashCode，导致PriorityQueue.remove无法正确识别同一状态的旧节点，openSet中会残留高成本的无效节点记录。

修复后的代码 #

import java.util.*;

public class AStarSearch {

    private static final int INFINITY = Integer.MAX_VALUE;

    private static class Node implements Comparable<Node> {
        String state;
        int g;
        int h;
        Node parent;

        public Node(String state, int g, int h, Node parent) {
            this.state = state;
            this.g = g;
            this.h = h;
            this.parent = parent;
        }

        public int getTotalCost() {
            return g + h;
        }

        @Override
        public int compareTo(Node other) {
            return Integer.compare(this.getTotalCost(), other.getTotalCost());
        }

        // 重写equals和hashCode，确保PriorityQueue能正确匹配同状态节点
        @Override
        public boolean equals(Object o) {
            if (this == o) return true;
            if (o == null || getClass() != o.getClass()) return false;
            Node node = (Node) o;
            return Objects.equals(state, node.state);
        }

        @Override
        public int hashCode() {
            return Objects.hash(state);
        }
    }

    private static List<String> aStarSearch(int[][] matrix, String[] labels, int[] heuristic, String startState) {
        Map<String, Integer> heuristicMap = new HashMap<>();
        Set<String> goalStates = new HashSet<>();
        // 初始化启发式映射和目标节点集合
        for (int i = 0; i < labels.length; i++) {
            heuristicMap.put(labels[i], heuristic[i]);
            if (heuristic[i] == 0) {
                goalStates.add(labels[i]);
            }
        }

        Map<String, Integer> gScores = new HashMap<>();
        PriorityQueue<Node> openSet = new PriorityQueue<>();
        Set<String> closedSet = new HashSet<>();

        Node startNode = new Node(startState, 0, heuristicMap.get(startState), null);
        openSet.add(startNode);
        gScores.put(startState, 0);

        // 记录所有可达目标的路径和总成本
        Map<String, List<String>> goalPaths = new HashMap<>();
        Map<String, Integer> goalCosts = new HashMap<>();

        while (!openSet.isEmpty()) {
            Node current = openSet.poll();
            String currentState = current.state;

            // 若当前是目标节点，记录路径和成本
            if (goalStates.contains(currentState)) {
                List<String> path = new ArrayList<>();
                Node node = current;
                while (node != null) {
                    path.add(0, node.state);
                    node = node.parent;
                }
                goalPaths.put(currentState, path);
                goalCosts.put(currentState, current.g);
                closedSet.add(currentState);
                continue;
            }

            if (closedSet.contains(currentState)) {
                continue;
            }
            closedSet.add(currentState);

            int currentIndex = Arrays.asList(labels).indexOf(currentState);
            // 遍历所有可能的下一个节点
            for (int i = 0; i < labels.length; i++) {
                String nextState = labels[i];
                // 修正邻接矩阵读取方向：当前节点到下一个节点的成本
                int cost = matrix[currentIndex][i];
                if (cost <= 0) {
                    continue;
                }

                int tentativeGScore = current.g + cost;
                // 如果新路径成本更高，跳过
                if (tentativeGScore >= gScores.getOrDefault(nextState, INFINITY)) {
                    continue;
                }

                // 更新g值并加入openSet
                gScores.put(nextState, tentativeGScore);
                Node nextNode = new Node(nextState, tentativeGScore, heuristicMap.get(nextState), current);
                if (openSet.contains(nextNode)) {
                    openSet.remove(nextNode);
                }
                openSet.add(nextNode);
            }
        }

        // 选择总成本最低的目标路径
        if (goalCosts.isEmpty()) {
            return null;
        }
        String bestGoal = Collections.min(goalCosts.entrySet(), Map.Entry.comparingByValue()).getKey();
        return goalPaths.get(bestGoal);
    }

    public static void main(String[] args) {
        String[] labels = {"A", "B", "C", "D", "E", "H", "J", "G1", "G2", "G3"};

        int[][] matrix = {
                {0, 0, 0, 6, 1, 0, 0, 0, 0, 0},
                {5, 0, 2, 0, 0, 0, 0, 0, 0, 0},
                {9, 3, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, 1, 0, 2, 0, 0, 0, 0, 0},
                {6, 0, 0, 0, 0, 2, 0, 0, 0, 0},
                {0, 0, 0, 7, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 0, 2, 0, 0, 0, 0, 0},
                {0, 9, 0, 0, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 5, 0, 0, 0, 0, 0, 0},
                {0, 0, 0, 0, 0, 8, 7, 0, 0, 0}
        };

        int[] heuristic = {5, 7, 3, 4, 6, 8, 5, 0, 0, 0};

        String startState = "A";

        List<String> cheapestPath = aStarSearch(matrix, labels, heuristic, startState);

        if (cheapestPath != null) {
            System.out.println("Cheapest path: " + cheapestPath);
            System.out.println("Goal state: " + cheapestPath.get(cheapestPath.size() - 1));
            System.out.println("Number of cycles: " + (cheapestPath.size() - 1));
            // 计算并输出总成本
            int totalCost = 0;
            for (int i = 0; i < cheapestPath.size() - 1; i++) {
                int from = Arrays.asList(labels).indexOf(cheapestPath.get(i));
                int to = Arrays.asList(labels).indexOf(cheapestPath.get(i+1));
                totalCost += matrix[from][to];
            }
            System.out.println("Total cost: " + totalCost);
        } else {
            System.out.println("No path found.");
        }
    }
}

修复说明 #

1. 修正路径成本读取：将邻接矩阵的读取方向改为matrix[currentIndex][i]，确保获取的是当前节点到下一个节点的正确成本。
2. 全局目标比较：不再提前终止搜索，而是记录所有可达目标的路径和成本，最终选择总成本最低的路径。
3. 完善节点匹配：重写Node类的equals和hashCode方法，确保优先队列能正确移除同状态的旧节点，避免无效路径干扰。
4. 简化逻辑判断：优化g值比较逻辑，减少冗余判断，提升代码可读性。

修复后运行代码，会输出正确的最低成本路径（如[A, E, H, G3]或[A, D, G2]，两者总成本均为11）。

内容的提问来源于stack exchange，提问作者user185491