A*アルゴリズムの実現
14639 ワード
A*アルゴリズムの実現
ことばを引く
A＊アルゴリズムは経路検索の中の比較的一般的なアルゴリズムであり、検索効率と検索結果を兼ね備えたパス最適アルゴリズムでもある.これから、私は植保航路計画環境において、地図座標系に基づいてA＊アルゴリズムを実現します.
1.グリッド領域
地図座標系が細かいので、実際の地図を使ってグリッドを分割すれば、データ量は非常に大きいです.したがって、検索パスが必要な領域をメッシュ粗粒化するには、非常に微細な地図メッシュを各格子より大きいメッシュに粗大化することである.ここではメッシュ粗粒化方法を与える.
1.グリッドを作成する
グリッドの作成は簡単です.先に指定された領域の最大最小緯度、経度値を探します.与えられた粗粒化ステップを利用して、グリッドの長さ、幅をいくつの単位に決定します.これでグリッドが完成しました.
グリッドを作成した後、範囲内のグリッドに到達しないようにマークします.与えられた境界は点の集合であるため、まず境界点を到達不能としてマークし、その後、隣接点の接続ラインをマークすることは不可能です.
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を繰り返す.
ことばを引く
A＊アルゴリズムは経路検索の中の比較的一般的なアルゴリズムであり、検索効率と検索結果を兼ね備えたパス最適アルゴリズムでもある.これから、私は植保航路計画環境において、地図座標系に基づいてA＊アルゴリズムを実現します.
1.グリッド領域
地図座標系が細かいので、実際の地図を使ってグリッドを分割すれば、データ量は非常に大きいです.したがって、検索パスが必要な領域をメッシュ粗粒化するには、非常に微細な地図メッシュを各格子より大きいメッシュに粗大化することである.ここではメッシュ粗粒化方法を与える.
1.グリッドを作成する
グリッドの作成は簡単です.先に指定された領域の最大最小緯度、経度値を探します.与えられた粗粒化ステップを利用して、グリッドの長さ、幅をいくつの単位に決定します.これでグリッドが完成しました.
/**
*
*
* @param bound
* @param obstacleBoundPoints
* @param step
* @return bean
*/
public static PalisadeMap getPalisadeMap(List extends PointD> 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 extends PointD> obstacleBoundPoint : obstacleBoundPoints) {
signRange(map, obstacleBoundPoint, step, maxMinLatLng);
}
}
return new PalisadeMap(map, step);
}
グリッドを作成した後、範囲内のグリッドに到達しないようにマークします.与えられた境界は点の集合であるため、まず境界点を到達不能としてマークし、その後、隣接点の接続ラインをマークすることは不可能です.
/**
*
*
* @param map
* @param bound
* @param step
* @param maxMinLatLng bean
*/
private static void signRange(Node[][] map, List extends PointD> 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();
}
/**
* 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;
}
/**
* ，
*
* @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;
}
}
}
A＊アルゴリズムは主に移動した実際のコスト(G)と現在の位置から目標位置までの推定コスト(H)から構成され、計算パスの公式は以下の通りである.
F=G+H
その探し方は:
/**
* ： 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);
}
}