A*算法无法生成完整单一路径 求代码正确性验证

我实现了A*路径规划算法，但生成的目标路径不符合预期。节点评估看似正常，但最终路径列表中存在G值相同的节点，无法确定算法是否正常工作。

原实现代码 #

A*算法实现 #

startNode = getTile(new Point(3,6));
targetNode = getTile(new Point(1,4));
ArrayList<Node> closedList;
void AStar() {
    closedList = new ArrayList<>();
    ArrayList<Node> openList = new ArrayList<>();
    startNode.g = 0;
    startNode.h = heuristic(startNode,targetNode);
    startNode.f = startNode.g+startNode.h;
    Log.d("COST",startNode.h+"");
    cost = startNode.h;
    openList.add(startNode);
    while (!openList.isEmpty()) {

        //Find the node with the lowest f value
        int lowestFcost = openList.get(0).f;
        Node processNode = openList.get(0);
        for(int i = 1;i<openList.size();i++){
            if(openList.get(i).f < lowestFcost){
                lowestFcost = openList.get(i).f;
                processNode = openList.get(i);
            }
        }


        if (processNode.equals(targetNode)) {
            path(processNode);
            return;
        }
        openList.remove(processNode);
        closedList.add(processNode);

        for (Node n:nextNodes(processNode)) {
            //check if n is in the closed list
            if(closedList.contains(n)){
                // skip forloop once
                Log.d("SKIPLOOP","true");
            }else {


                if (!openList.contains(n)) {
                    openList.add(n);

                } else {
                    for (Node n2 : openList) {
                        if (n2.equals(n)) {


                            if (processNode.g<n.g) {


                                //better g score
                                n2.g = heuristic(processNode,startNode);
                                n2.h = heuristic(processNode,targetNode);
                                n2.f = n2.g+n2.h;
                                n2.parent = processNode;
                                Log.d("NODE","XY:"+n2.getPosition().toString()+" Gcost:"+n2.g+" Fcost:"+n2.f+" Hcost:"+n2.h);
                                //n2.view.setBackgroundColor(getResources().getColor(R.color.primary_light,null));


                            }
                    }


                    }


                }

            }
        }

    }
new ToastEasy(getApplicationContext(),"Not Possible to reach Target");
}


private Node getTile(Point tilePosition){
    Node tile = null;
    for (Node t:MapTileList) {
        if(tilePosition.equals(t.getPosition())){
            tile = t;
            break;
        }
    }
    return tile;
}


private ArrayList<Node> nextNodes(Node node){

    ArrayList<Node>NextNodeList = new ArrayList<>();
    if(node.getPosition().x-1 >= 0 ){
        //Add a left Node
        Node n = getTile(new Point(node.getPosition().x-1,node.getPosition().y));
        //n.parent = node;
        n.g = heuristic(n,startNode);
        n.h = heuristic(n,targetNode);
        n.f = n.g+n.h;
        n.view.setBackgroundColor(getResources().getColor(R.color.green,null));
        NextNodeList.add(n); //Left Node
    }
    if(node.getPosition().y-1 >= 0){
        //Add a top Node
        Node n = getTile(new Point(node.getPosition().x,node.getPosition().y-1));

        //n.parent = node;
        n.g = heuristic(n,startNode);
        n.h = heuristic(n,targetNode);
        n.f = n.g+n.h;
        n.view.setBackgroundColor(getResources().getColor(R.color.green,null));
        NextNodeList.add(n); //Top Node
    }
    if(node.getPosition().x+1 <= cols ){
        //Add a Right Node
        Node n = getTile(new Point(node.getPosition().x+1,node.getPosition().y));

        //n.parent = node;
        n.g = heuristic(n,startNode);
        n.h = heuristic(n,targetNode);
        n.f = n.g+n.h;
        n.view.setBackgroundColor(getResources().getColor(R.color.green,null));
        NextNodeList.add(n);
    }
    if(node.getPosition().y+1 <= rows) {
        //Add a Bottom Node
        Node n = getTile(new Point(node.getPosition().x, node.getPosition().y + 1));

        //n.parent = node;
        n.g = heuristic(n,startNode);
        n.h = heuristic(n,targetNode);
        n.f = n.g+n.h;
        n.view.setBackgroundColor(getResources().getColor(R.color.green,null));
        NextNodeList.add(n);

    }
    return NextNodeList;
}
void path(Node node){
    int lastCost = 0;
    ArrayList<Node>Path = new ArrayList<>();
    for (Node node1 : closedList) {
       if(node1.parent != null){

           node1.view.setBackgroundColor(getResources().getColor(R.color.black,null));
           Path.add(node1);

           //Log.d("PATHLIST",Path.get(i).getPosition()+" Gcost"+Path.get(i).g+" Hcost"+Path.get(i).h+" Fcost"+Path.get(i).f);
       }

    }

    Log.d("PATHLIST",Path.size()+"");

}

private int heuristic(Node node1, Node node2){


    return Math.abs((node1.getPosition().x-node2.getPosition().x))
            +
            Math.abs((node1.getPosition().y-node2.getPosition().y));

}

Node类实现 #

import android.graphics.Point;
import android.widget.ImageButton;
import android.widget.TextView;

public class Node {
    Point position;
    public int f,g,h;
    public Node parent;
    public ImageButton view;
    public Node(Point position, int f, int g, int h, Node parent, ImageButton view) {
        this.position = position;
        this.f = f;
        this.g = g;
        this.h = h;
        this.parent = parent;
        this.view = view;
    }
    public Point getPosition(){

        return this.position;


    }
}

关键问题与修复方案 #

1. G值计算逻辑错误 #

G值代表从起点到当前节点的实际移动成本，不是当前节点到起点的曼哈顿距离。四方向移动每步成本为1，所以正确的G值计算应为父节点G值 + 1。

• 修复nextNodes方法中的G值计算：

// 替换原n.g = heuristic(n,startNode);
n.g = node.g + 1;

• 修复节点更新时的G值计算：

// 替换原n2.g = heuristic(processNode,startNode);
n2.g = processNode.g + 1;

2. H值更新错误 #

H值是当前节点到终点的曼哈顿距离，不是父节点到终点的距离。节点更新时应保持H值为当前节点的启发值：

// 替换原n2.h = heuristic(processNode,targetNode);
n2.h = heuristic(n2, targetNode);

3. 路径回溯逻辑错误 #

原path方法遍历closedList收集节点的方式完全错误，正确的路径应从目标节点开始，沿着parent回溯到起点：

void path(Node targetNode) {
    ArrayList<Node> path = new ArrayList<>();
    Node current = targetNode;
    while (current != null) {
        path.add(current);
        current.view.setBackgroundColor(getResources().getColor(R.color.black,null));
        current = current.parent;
    }
    // 反转路径得到从起点到终点的顺序
    Collections.reverse(path);
    Log.d("PATHLIST", "路径长度：" + path.size());
    for (Node node : path) {
        Log.d("PATHNODE", "XY:" + node.getPosition().toString() + " Gcost:" + node.g);
    }
}

4. 缺少equals和hashCode方法 #

Node类未重写equals和hashCode，导致contains方法无法正确判断节点是否为同一位置的节点。添加以下代码到Node类：

@Override
public boolean equals(Object o) {
    if (this == o) return true;
    if (o == null || getClass() != o.getClass()) return false;
    Node node = (Node) o;
    return position.equals(node.position);
}

@Override
public int hashCode() {
    return position.hashCode();
}

5. 节点更新时的判断逻辑优化 #

原代码中遍历openList查找节点效率低，且判断条件processNode.g < n.g应改为(processNode.g + 1) < n2.g，因为新的G值是父节点G+1：

// 替换原判断部分
if (!openList.contains(n)) {
    n.parent = processNode;
    n.g = processNode.g + 1;
    n.h = heuristic(n, targetNode);
    n.f = n.g + n.h;
    openList.add(n);
} else {
    int newG = processNode.g + 1;
    if (newG < n.g) {
        n.parent = processNode;
        n.g = newG;
        n.f = n.g + n.h;
        Log.d("NODE","XY:"+n.getPosition().toString()+" Gcost:"+n.g+" Fcost:"+n.f+" Hcost:"+n.h);
    }
}

修复后说明 #

修复以上问题后，G值会正确反映从起点到节点的实际步数，路径回溯会得到从起点到终点的正确路径，不会再出现G值异常的情况。

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