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
其寻路方式是：
- 将开始点A加入到open table中；
- 取出open table的中F值最小的点Q，将Q加入到close table中；
- 找出Q点相邻可达的点，当相邻的点在close table中则忽略，当相邻的点在open table中则重新计算G、F的值，当相邻的点既不在close table中也不在open table中，则将周围可达的点加入到open table中；
- 判断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);
}
}