A*算法的实现

A*算法的实现

引言

A*算法是路径搜索中的一种较为常见的算法,也是兼顾了搜索效率和搜索结果的一种路径最优算法。下面,我将在植保航线规划环境中,基于地图坐标系来实现A*算法

1. 网格化区域

​ 由于地图坐标系很精细,如果利用真实的地图来划分出一块网格,数据量将会非常巨大。所以要对需要搜索路径的区域进行网格粗粒化,就是将很精细的地图网格粗化到相对每个格子更大的网格,这里给出网格粗粒化方法:

1. 创建出网格

创建网格的做法很简单。先找出给定的区域的最大最小的纬度、经度值;利用给定粗粒化步长确定网格的长、宽为多少个单位;这样网格就已经完成了。

/**
     * 获取粗粒化的网格
     *
     * @param bound               区域边界点的集合
     * @param obstacleBoundPoints 障碍物集合
     * @param step                粗粒化步长
     * @return 粗粒化网格数据bean
     */
    public static PalisadeMap getPalisadeMap(List bound, List> obstacleBoundPoints, double step) {
        MaxMinLatLng maxMinLatLng = MapUtils.getMaxMinLatLng(bound);
        int v = (int) ceil((maxMinLatLng.getMaxLat() - maxMinLatLng.getMinLat()) / step) + 1;
        int h = (int) ceil((maxMinLatLng.getMaxLng() - maxMinLatLng.getMinLng()) / step) + 1;
        Node[][] map = new Node[v][h];
        for (int i = 0; i < v; i++) {
            for (int j = 0; j < h; j++) {
                map[i][j] = new Node(i, j);
            }
        }
        signRange(map, bound, step, maxMinLatLng);
        if (obstacleBoundPoints != null) {
            for (List obstacleBoundPoint : obstacleBoundPoints) {
                signRange(map, obstacleBoundPoint, step, maxMinLatLng);
            }
        }
        return new PalisadeMap(map, step);
    }

2. 标记障碍物点以及连线

创建完网格后,就要标记区域内的不可达网格。因为给定的边界是点的集合,首先要标记边界点为不可达,而后要标记相邻的点的连线处不可达。

    /**
     * 标记边界点位置
     *
     * @param map          网格点
     * @param bound        区域边界点的集合
     * @param step         粗粒化步长
     * @param maxMinLatLng 整个区域最大最小值bean
     */
    private static void signRange(Node[][] map, List bound, double step, MaxMinLatLng maxMinLatLng) {
        List boundNodes = new ArrayList<>();
        for (int i = 0; i < bound.size(); i++) {
            Node node = findPosition(map, new PointD(maxMinLatLng.getMaxLat(), maxMinLatLng.getMinLng()), step, bound.get(i));
            if (i != 0) {
                signCannotReachable(map, boundNodes.get(boundNodes.size() - 1), node);
            }
            boundNodes.add(node);
        }
        signCannotReachable(map, boundNodes.get(boundNodes.size() - 1), boundNodes.get(0));
        boundNodes.clear();
    }

​ 1) 首先要找到边界点所对应的网格中的位置,利用整个区域的最小纬度、最小经度构成的整个区域中最左上角的点,根据步长,计算给定的gps点在区域内的x、y位置:

    /**
     * 给定gps坐标,找到该坐标所对应的网格的位置
     *
     * @param map           网格点
     * @param leftTopPointD 整个区域最左上角的gps坐标点
     * @param step          粗粒化步长
     * @param pointD        要寻找的gps点
     * @return 找到gps点所对应的网格内的点
     */
    public static Node findPosition(Node[][] map, PointD leftTopPointD, double step, PointD pointD) {
        int x = (int) round((leftTopPointD.x - pointD.x) / step);
        int y = (int) round((pointD.y - leftTopPointD.y) / step);
        x = max(0, x);
        x = min(map.length - 1, x);
        y = max(0, y);
        y = min(map[0].length - 1, y);
        Node node = map[x][y];
        return node;
    }

​ 2) 标记两个点的连线处为不可达区域,主要做法为将线段经过的网格都标记为不可达。

    /**
     * 给定两个点,表姐两个点的连线处不可达
     *
     * @param map   网格点
     * @param nodeA 点A
     * @param nodeB 点B
     */
    private static void signCannotReachable(Node[][] map, Node nodeA, Node nodeB) {
        int diffV = nodeB.getX() - nodeA.getX();
        int diffH = nodeB.getY() - nodeA.getY();

        double slope = diffH * 1.0D / diffV;
        int num = max(0, diffV);
        int last = Integer.MAX_VALUE;
        for (int j = min(0, diffV); j <= num; j++) {
            int low;
            int high;
            if (slope == Double.NEGATIVE_INFINITY || slope == Double.POSITIVE_INFINITY) {
                low = min(0, diffH);
                high = max(0, diffH);
            } else {
                low = (int) floor(slope * (j - 0.1));
                high = (int) ceil(slope * (j + 0.1));
                int tempMax = max(low, high);
                low = min(low, high);
                high = tempMax;
                if (j != min(0, diffV)) {
                    if (slope > 0) {
                        low = low > last ? last : low;
                    } else {
                        high = high < last ? last : high;
                    }
                }
            }
            for (int k = low; k <= high; k++) {
                int tempV = nodeA.getX() + j;
                int tempH = nodeA.getY() + k;
                if (tempV >= 0 && tempV < map.length && tempH >= 0 && tempH < map[0].length) {
                    if ((tempV <= nodeA.getX() || tempV <= nodeB.getX())
                            && (tempV >= nodeA.getX() || tempV >= nodeB.getX())
                            && (tempH <= nodeA.getY() || tempH <= nodeB.getY())
                            && (tempH >= nodeA.getY() || tempH >= nodeB.getY())) {
                        map[tempV][tempH].setReachable(false);
                    }
                }
            }
            last = slope > 0 ? high + 1 : low - 1;
        }
    }
}

2. 实现A*算法

A*算法主要由已经移动的实际成本(G)和当前位置到目标位置的估计成本(H)构成,计算路径的公式为:

F = G + H

其寻路方式是:

  1. 将开始点A加入到open table中;
  2. 取出open table的中F值最小的点Q,将Q加入到close table中;
  3. 找出Q点相邻可达的点,当相邻的点在close table中则忽略,当相邻的点在open table中则重新计算G、F的值,当相邻的点既不在close table中也不在open table中,则将周围可达的点加入到open table中;
  4. 判断open table中是否包含目标点B,如果包含B点则结束,反之重复步骤2;
/**
 * 创建者: hucanhua
 * 创建时间:2017/08/28
 * 说明:
 */
public class AStar {
    public static final int STEP = 10;
    public static final int OBLIQUE = 14;

    /**
     * 查找路径
     *
     * @param map           网格
     * @param startPosition 开始位置
     * @param goalPosition  结束位置
     * @return 找到的路径
     */
    public static Node findRouter(Node[][] map, Node startPosition, Node goalPosition) {
        long lastTime = System.currentTimeMillis();
        Timber.d("-----------开始查找路径---------");
        List openTable = new ArrayList<>();
        List closeTable = new ArrayList<>();
        openTable.add(startPosition);
        int num = 0;
        while (openTable.size() != 0) {
            Node tempNode = getMinFNode(openTable);
            openTable.remove(tempNode);
            closeTable.add(tempNode);
            List surroundNodes = surroundNodes(map, tempNode);
            for (Node surroundNode : surroundNodes) {
                if (closeTable.contains(surroundNode)) {
                    continue;
                }
                if (openTable.contains(surroundNode)) {
                    foundPoint(tempNode, surroundNode);
                } else {
                    noFoundPoint(openTable, tempNode, surroundNode, goalPosition);
                }
            }
            if (openTable.contains(goalPosition)) {
                optimizationRouter(map, goalPosition);
                Timber.d("openTable数目:%s,closeTable数目:%s", openTable.size(), closeTable.size());
                return goalPosition;
            } else if (System.currentTimeMillis() - lastTime > 1000) {
                lastTime = System.currentTimeMillis();
                Timber.d("---搜索路径时间过长,定时打印:openTable数目:%s,closeTable数目:%s---", openTable.size(), closeTable.size());
            }
        }
        return startPosition;
    }

    /**
     * 优化路径
* 此方法的目的是优化A*算法。由于A*算法基于网格,它形成的路径是由多个接近的网格的点构成的路径,针对地图上航线规划, * 过多的点会导致航线转折过多,此方法目的是将部分路径由多个点构成优化成直连方式 * * @param map 网格 * @param router 路径 */ private static void optimizationRouter(Node[][] map, Node router) { Node startNode = router; Node nextNode = startNode.getParent(); while (nextNode != null) { if (isWorkableRoute(map, startNode, nextNode)) { startNode.setParent(nextNode); } else { startNode = nextNode; } nextNode = nextNode.getParent(); } } /** * 判断当前点到目标点之间的路径是否可达,当当前点到目标点的连线距离周围不可达的点过近,则不为有效路径 * * @param map 网格 * @param startNode 当前点 * @param endNode 目标点 * @return 是否是有效路径 */ protected static boolean isWorkableRoute(Node[][] map, Node startNode, Node endNode) { int diffV = endNode.getX() - startNode.getX(); int diffH = endNode.getY() - startNode.getY(); double slope = diffH * 1.0D / diffV; int num = max(0, diffV); for (int j = min(0, diffV); j <= num; j++) { int low; int high; if (slope == Double.NEGATIVE_INFINITY || slope == Double.POSITIVE_INFINITY) { low = min(0, diffH); high = max(0, diffH); } else { low = (int) floor(slope * (j - 0.1)); high = (int) ceil(slope * (j + 0.1)); int tempMax = max(low, high); low = min(low, high); high = tempMax; } for (int k = low; k < high || k - low < 1; k++) { int tempV = startNode.getX() + j; int tempH = startNode.getY() + k; if (tempV >= 0 && tempV < map.length && tempH >= 0 && tempH < map[0].length) { if ((tempV <= startNode.getX() || tempV <= endNode.getX()) && (tempV >= startNode.getX() || tempV >= endNode.getX()) && (tempH <= startNode.getY() || tempH <= endNode.getY()) && (tempH >= startNode.getY() || tempH >= endNode.getY())) { if (!map[tempV][tempH].isReachable()) { return false; } } } } } return true; } /** * 当节点在open表中,则重新计算G、H的值 * * @param currentNode 当前节点 * @param nextNode 在open表中找到的节点 */ private static void foundPoint(Node currentNode, Node nextNode) { double G = calcG(currentNode, nextNode); if (G < nextNode.getG()) { nextNode.setParent(currentNode); nextNode.setG(G); nextNode.calcF(); } } /** * 当open表中没有此节点,则加入到open表 * * @param openTable open表 * @param currentNode 当前节点 * @param nextNode 未在open表中找到的节点 * @param goalPosition 目标节点 */ private static void noFoundPoint(List openTable, Node currentNode, Node nextNode, Node goalPosition) { nextNode.setParent(currentNode); nextNode.setG(calcG(currentNode, nextNode)); nextNode.setH(calcH(nextNode, goalPosition)); nextNode.calcF(); openTable.add(nextNode); } /** * 从初始结点到任意结点n的代价 * * @param currentNode 当前节点 * @param node 下一个节点 * @return 代价 */ private static double calcG(Node currentNode, Node node) { int G = (abs(currentNode.getX() - node.getX()) + abs(currentNode.getY() - node.getY())) == 1 ? STEP : OBLIQUE; return G + currentNode.getG(); } /** * 从结点n到目标点的启发式评估代价 * * @param currentNode 当前节点 * @param endNode 目标节点 * @return 估计代价 */ private static double calcH(Node currentNode, Node endNode) { return getManhattanDistance(STEP, currentNode, endNode); } /** * 查找周围可达的点 * * @param map 网格 * @param node 节点 * @return 周围可达的点的集合 */ private static List surroundNodes(Node[][] map, Node node) { List surroundPoints = new ArrayList<>(); Node tempNode = null; for (int x = node.getX() - 1; x <= node.getX() + 1; x++) { for (int y = node.getY() - 1; y <= node.getY() + 1; y++) { if (x >= 0 && x < map.length && y >= 0 && y < map[0].length) { tempNode = map[x][y]; if (canAdd(map, node, tempNode)) { surroundPoints.add(tempNode); } } } } return surroundPoints; } /** * 判断要查找的点是否可达,正上、正下、正左、正右直接判断是否可达,当为左上时,要判断正左或正上是否可达,以此类推 * * @param map 网格 * @param startNode 出发点 * @param node 节点 * @return 是否可达 */ private static boolean canAdd(Node[][] map, Node startNode, Node node) { if (abs(startNode.getX() - node.getX()) + abs(startNode.getY() - node.getY()) == 1) { return node.isReachable(); } else { return (map[startNode.getX()][node.getY()].isReachable() || map[node.getX()][startNode.getY()].isReachable()) && node.isReachable(); } } /** * 曼哈顿距离 * * @param cost 步长代价 * @param a 开始点 * @param b 目标点 * @return 估计代价 */ private static double getManhattanDistance(double cost, Node a, Node b) { return cost * (abs(a.getX() - b.getX()) + abs(a.getY() - b.getY())); } /** * 对角线距离 * * @param cost 步长代价 * @param a 开始点 * @param b 目标点 * @return 估计代价 */ private static double getDiagonalDistance(double cost, Node a, Node b) { return cost * max(abs(a.getX() - b.getX()), abs(a.getY() - b.getY())); } /** * 欧几里得距离 * * @param cost 步长代价 * @param a 开始点 * @param b 目标点 * @return 估计代价 */ private static double getEuclidDistance(double cost, Node a, Node b) { return cost * sqrt(pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2)); } /** * 平方后的欧几里得距离 * * @param cost 步长代价 * @param a 开始点 * @param b 目标点 * @return 估计代价 */ private static double getSquareEuclidDistance(double cost, Node a, Node b) { return cost * (pow(a.getX() - b.getX(), 2) + pow(a.getY() - b.getY(), 2)); } /** * 找到F值最少的点 * * @param nodes openTable的点集合 * @return F值最小的点 */ private static Node getMinFNode(List nodes) { Collections.sort(nodes); return nodes.get(0); } }

你可能感兴趣的:(A*算法的实现)