A*算法的實現(xiàn)
引言
? A*算法是路徑搜索中的一種較為常見的算法干像，也是兼顧了搜索效率和搜索結果的一種路徑最優(yōu)算法。下面狐史，我將在植保航線規(guī)劃環(huán)境中哲嘲，基于地圖坐標系來實現(xiàn)A*算法。
1. 網(wǎng)格化區(qū)域
? 由于地圖坐標系很精細查辩，如果利用真實的地圖來劃分出一塊網(wǎng)格腺律，數(shù)據(jù)量將會非常巨大。所以要對需要搜索路徑的區(qū)域進行網(wǎng)格粗烈巳猓化匀钧，就是將很精細的地圖網(wǎng)格粗化到相對每個格子更大的網(wǎng)格，這里給出網(wǎng)格粗撩担化方法：
1. 創(chuàng)建出網(wǎng)格
創(chuàng)建網(wǎng)格的做法很簡單之斯。先找出給定的區(qū)域的最大最小的緯度、經度值遣铝；利用給定粗劣铀ⅲ化步長確定網(wǎng)格的長、寬為多少個單位酿炸；這樣網(wǎng)格就已經完成了瘫絮。
/**
* 獲取粗粒化的網(wǎng)格
*
* @param bound 區(qū)域邊界點的集合
* @param obstacleBoundPoints 障礙物集合
* @param step 粗撂钏叮化步長
* @return 粗谅笥化網(wǎng)格數(shù)據(jù)bean
*/
public static PalisadeMap getPalisadeMap(List<? extends PointD> bound, List<List<? extends PointD>> 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<? extends PointD> obstacleBoundPoint : obstacleBoundPoints) {
signRange(map, obstacleBoundPoint, step, maxMinLatLng);
}
}
return new PalisadeMap(map, step);
}
2. 標記障礙物點以及連線
創(chuàng)建完網(wǎng)格后，就要標記區(qū)域內的不可達網(wǎng)格扁眯。因為給定的邊界是點的集合壮莹，首先要標記邊界點為不可達，而后要標記相鄰的點的連線處不可達姻檀。
/**
* 標記邊界點位置
*
* @param map 網(wǎng)格點
* @param bound 區(qū)域邊界點的集合
* @param step 粗撩化步長
* @param maxMinLatLng 整個區(qū)域最大最小值bean
*/
private static void signRange(Node[][] map, List<? extends PointD> bound, double step, MaxMinLatLng maxMinLatLng) {
List<Node> 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) 首先要找到邊界點所對應的網(wǎng)格中的位置，利用整個區(qū)域的最小緯度绣版、最小經度構成的整個區(qū)域中最左上角的點胶台，根據(jù)步長，計算給定的gps點在區(qū)域內的x杂抽、y位置：
/**
* 給定gps坐標诈唬，找到該坐標所對應的網(wǎng)格的位置
*
* @param map 網(wǎng)格點
* @param leftTopPointD 整個區(qū)域最左上角的gps坐標點
* @param step 粗粒化步長
* @param pointD 要尋找的gps點
* @return 找到gps點所對應的網(wǎng)格內的點
*/
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) 標記兩個點的連線處為不可達區(qū)域默怨，主要做法為將線段經過的網(wǎng)格都標記為不可達讯榕。
/**
* 給定兩個點，表姐兩個點的連線處不可達
*
* @param map 網(wǎng)格點
* @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. 實現(xiàn)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；
/**
* 創(chuàng)建者： hucanhua
* 創(chuàng)建時間：2017/08/28
* 說明：
*/
public class AStar {
public static final int STEP = 10;
public static final int OBLIQUE = 14;
/**
* 查找路徑
*
* @param map 網(wǎng)格
* @param startPosition 開始位置
* @param goalPosition 結束位置
* @return 找到的路徑
*/
public static Node findRouter(Node[][] map, Node startPosition, Node goalPosition) {
long lastTime = System.currentTimeMillis();
Timber.d("-----------開始查找路徑---------");
List<Node> openTable = new ArrayList<>();
List<Node> closeTable = new ArrayList<>();
openTable.add(startPosition);
int num = 0;
while (openTable.size() != 0) {
Node tempNode = getMinFNode(openTable);
openTable.remove(tempNode);
closeTable.add(tempNode);
List<Node> 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數(shù)目：%s,closeTable數(shù)目：%s", openTable.size(), closeTable.size());
return goalPosition;
} else if (System.currentTimeMillis() - lastTime > 1000) {
lastTime = System.currentTimeMillis();
Timber.d("---搜索路徑時間過長报辱，定時打佑胙辍：openTable數(shù)目：%s,closeTable數(shù)目：%s---", openTable.size(), closeTable.size());
}
}
return startPosition;
}
/**
* 優(yōu)化路徑<br/>
* 此方法的目的是優(yōu)化A*算法。由于A*算法基于網(wǎng)格碍现，它形成的路徑是由多個接近的網(wǎng)格的點構成的路徑幅疼，針對地圖上航線規(guī)劃，
* 過多的點會導致航線轉折過多昼接，此方法目的是將部分路徑由多個點構成優(yōu)化成直連方式
*
* @param map 網(wǎng)格
* @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 網(wǎng)格
* @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;
}
/**
* 當節(jié)點在open表中慢睡，則重新計算G逐工、H的值
*
* @param currentNode 當前節(jié)點
* @param nextNode 在open表中找到的節(jié)點
*/
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表中沒有此節(jié)點，則加入到open表
*
* @param openTable open表
* @param currentNode 當前節(jié)點
* @param nextNode 未在open表中找到的節(jié)點
* @param goalPosition 目標節(jié)點
*/
private static void noFoundPoint(List<Node> 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 當前節(jié)點
* @param node 下一個節(jié)點
* @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到目標點的啟發(fā)式評估代價
*
* @param currentNode 當前節(jié)點
* @param endNode 目標節(jié)點
* @return 估計代價
*/
private static double calcH(Node currentNode, Node endNode) {
return getManhattanDistance(STEP, currentNode, endNode);
}
/**
* 查找周圍可達的點
*
* @param map 網(wǎng)格
* @param node 節(jié)點
* @return 周圍可達的點的集合
*/
private static List<Node> surroundNodes(Node[][] map, Node node) {
List<Node> 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 網(wǎng)格
* @param startNode 出發(fā)點
* @param node 節(jié)點
* @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<Node> nodes) {
Collections.sort(nodes);
return nodes.get(0);
}
}
