A*地图最短路径算法

一、简介

       A*最短路径算法可以说是最为高效的地图最短路径搜索算法之一，也是目前用的比较广的地图最短路径搜索算法。说起地图最短路径搜索，第一映像就是广度优先搜索，遍历地图中所有可通的，找出最短的路径，但是这个算法随着地图的变大，计算量将剧增。而A*算法的原理是找出周边路径中优先级最高的节点（理目标节点权重最高），定向移动，直到找到目标节点。从原理上就不难看出A*算法效率极高。

二、算法实现原理

A*算法逻辑如下：

* 初始化openQueue（优先级队列）和closeSet；
* 将起点加入openQueue中；
* 如果openQueue不为空，则从openQueue中选取优先级最高的节点n：
    * 如果节点n为终点，则：
        * 从终点开始逐步追踪parent节点，一直达到起点；
        * 返回找到的结果路径，算法结束；
    * 如果节点n不是终点，则：
        * 将节点n从openQueue中删除，并加入closeSet中；
        * 遍历节点n所有的邻近节点：
            * 如果邻近节点m在closeSet中，则：
                * 跳过，选取下一个邻近节点
            * 如果邻近节点m不在closeSet中，则：
                * 设置节点m的parent为节点n
                * 计算节点m的优先级
                * 将节点m加入openQueue中

其中优先级的选取依赖启发函数，用于计算节点距离起点和终点的权重优先级，可以通过调节启发函数我们可以控制算法的速度和精确度。对于网格形式的图，一般使用的启发函数如下：

（1）如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。

（2）如果图形中允许朝八个方向移动，则可以使用对角距离。

（3）如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

三、算法Java实现

/**
 * 位置
 */
public class Pos {
    private int x;

    private int y;

    public Pos(int x, int y) {
        this.x = x;
        this.y = y;
    }

    public int getX() {
        return x;
    }

    public int getY() {
        return y;
    }

    @Override
    public boolean equals(Object obj) {
        if (obj instanceof Pos) {
            Pos pos = (Pos) obj;
            return x == pos.x && y == pos.y;
        }
        return false;
    }
}

/**
 * 路径节点
 */
public class Node implements Comparable<Node> {
    /**
     * 坐标
     */
    private Pos pos;

    /**
     * 父结点
     */
    private Node parent;

    /**
     * 起点到当前结点的代价
     */
    private int G;

    /**
     * 当前结点到目的结点的预计代价
     */
    private int H;

    public Node(int x, int y) {
        this.pos = new Pos(x, y);
    }

    public Node(Pos pos, Node parent, int g, int h) {
        this.pos = pos;
        this.parent = parent;
        G = g;
        H = h;
    }

    @Override
    public int compareTo(Node o) {
        if (o == null) return -1;
        if (G + H > o.G + o.H)
            return 1;
        else if (G + H < o.G + o.H) return -1;
        return 0;
    }

    public Pos getPos() {
        return pos;
    }

    public void setPos(Pos pos) {
        this.pos = pos;
    }

    public Node getParent() {
        return parent;
    }

    public void setParent(Node parent) {
        this.parent = parent;
    }

    public int getG() {
        return G;
    }

    public void setG(int g) {
        this.G = g;
    }

    public int getH() {
        return H;
    }

    public void setH(int h) {
        this.H = h;
    }
}


/**
 * 路径搜索信息
 */
public class SearchInfo {
    /**
     * 二维数组的地图
     */
    private int[][] grid;

    /**
     * 地图的宽
     */
    private int width;

    /**
     * 地图的高
     */
    private int hight;

    /**
     * 起始结点
     */
    private Node start;

    /**
     * 目标结点
     */
    private Node target;

    public SearchInfo(int[][] grid, Node start, Node end) {
        this.grid = grid;
        this.width = grid[0].length;
        this.hight = grid.length;
        this.start = start;
        this.target = end;
    }

    public int[][] getGrid() {
        return grid;
    }

    public int getWidth() {
        return width;
    }

    public int getHight() {
        return hight;
    }

    public Node getStart() {
        return start;
    }

    public Node getTarget() {
        return target;
    }
}

/**
 * A*图最短路径算法
 */
public class AstarAlgo {
    /**
     * 障碍值
     */
    public final static int BAR = -1;

    /**
     * 路径
     */
    public final static int PATH = 2;

    /**
     * 横竖移动代价
     */
    public final static int DIRECT_VALUE = 10;

    /**
     * 斜移动代价
     */
    public final static int OBLIQUE_VALUE = 14;

    /**
     * 优先队列(升序)
     */
    Queue<Node> openList = new PriorityQueue<Node>();

    List<Node> closeList = new ArrayList<Node>();

    public int search(SearchInfo searchInfo) {
        if (searchInfo == null) {
            return -1;
        }
        // clean
        openList.clear();
        closeList.clear();
        // 開始搜索
        openList.add(searchInfo.getStart());
        return calcRoute(searchInfo);
    }

    /**
     * 路径搜索
     */
    private int calcRoute(SearchInfo searchInfo) {
        while (!openList.isEmpty()) {
            Node current = openList.poll();
            // 判断是否是目标节点
            if (current.getPos().equals(searchInfo.getTarget().getPos())) {
                return getDist(searchInfo.getGrid(), searchInfo.getTarget());
            }
            closeList.add(current);
            addNeighborNodeToOpen(searchInfo, current);
        }

        return -1;
    }

    /**
     * 推断结点是否是终于结点
     */
    private boolean isEndNode(Pos end, Pos pos) {
        return pos != null && end.equals(pos);
    }

    /**
     * 推断结点是否能放入Open列表
     */
    private boolean canAddNodeToOpen(SearchInfo searchInfo, int x, int y) {
        // 是否在地图中
        if (x < 0 || x >= searchInfo.getWidth() || y < 0 || y >= searchInfo.getHight()) {
            return false;
        }
        // 推断是否是不可通过的结点
        if (searchInfo.getGrid()[y][x] == BAR) {
            return false;
        }
        // 推断结点是否存在close表
        if (isPosInClose(x, y)) {
            return false;
        }

        return true;
    }

    /**
     * 推断坐标是否在close表中
     */
    private boolean isPosInClose(int x, int y) {
        if (closeList.isEmpty()) return false;
        for (Node node : closeList) {
            if (node.getPos().getX() == x && node.getPos().getY() == y) {
                return true;
            }
        }
        return false;
    }

    private int calcH(Pos end, Pos pos) {
        return Math.abs(end.getX() - pos.getX()) + Math.abs(end.getY() - pos.getY());
    }

    private Node findNodeInOpen(Pos pos) {
        if (pos == null || openList.isEmpty()) return null;
        for (Node node : openList) {
            if (node.getPos().equals(pos)) {
                return node;
            }
        }
        return null;
    }

    /**
     * 增加全部邻结点到open表
     */
    private void addNeighborNodeToOpen(SearchInfo searchInfo, Node current) {
        int x = current.getPos().getX();
        int y = current.getPos().getY();
        // 左
        addNeighborNodeToOpen(searchInfo, current, x - 1, y, DIRECT_VALUE);
        // 上
        addNeighborNodeToOpen(searchInfo, current, x, y - 1, DIRECT_VALUE);
        // 右
        addNeighborNodeToOpen(searchInfo, current, x + 1, y, DIRECT_VALUE);
        // 下
        addNeighborNodeToOpen(searchInfo, current, x, y + 1, DIRECT_VALUE);
        // 左上
        addNeighborNodeToOpen(searchInfo, current, x - 1, y - 1, OBLIQUE_VALUE);
        // 右上
        addNeighborNodeToOpen(searchInfo, current, x + 1, y - 1, OBLIQUE_VALUE);
        // 右下
        addNeighborNodeToOpen(searchInfo, current, x + 1, y + 1, OBLIQUE_VALUE);
        // 左下
        addNeighborNodeToOpen(searchInfo, current, x - 1, y + 1, OBLIQUE_VALUE);
    }

    /**
     * 增加一个邻结点到open表
     */
    private void addNeighborNodeToOpen(SearchInfo searchInfo, Node current, int x, int y, int value) {
        if (canAddNodeToOpen(searchInfo, x, y)) {
            Node end = searchInfo.getTarget();
            Pos pos = new Pos(x, y);
            // 计算邻结点的G值
            int G = current.getG() + value;
            Node child = findNodeInOpen(pos);
            if (child == null) {
                // 计算H值
                int H = calcH(end.getPos(), pos);
                if (isEndNode(end.getPos(), pos)) {
                    child = end;
                    child.setParent(current);
                    child.setG(G);
                    child.setH(H);
                } else {
                    child = new Node(pos, current, G, H);
                }
                openList.add(child);
            } else if (child.getG() > G) {
                child.setG(G);
                child.setParent(current);
                // 又一次调整堆
                openList.add(child);
            }
        }
    }

    /**
     * 获取距离
     * @param grid 地图
     * @param end 目标节点
     */
    private int getDist(int[][] grid, Node end) {
        if (end == null || grid == null) {
            return -1;
        }

        int dist = 0;
        while (end.getParent() != null) {
            end = end.getParent();
            dist ++;
        }

        return dist;
    }
}

四、参考资料

1、路径规划之 A* 算法 - 知乎

2、A星算法（Java实现） - yutingliuyl - 博客园