我正在尝试为寻路问题实现A *算法。
它起作用，就像10次中的9次一样，但在某些时候我得到（可能）无限循环，并且程序找不到最佳路径。你能明白为什么会这样吗？
A *：
import java.util.*;
public abstract class AStar<T>
{
private class Path implements Comparable{
public T point;
public Double f;
public Double g;
public Path parent;
public Path(){
parent = null;
point = null;
g = f = 0.0;
}
public Path(Path p){
this();
parent = p;
g = p.g;
f = p.f;
}
public int compareTo(Object o){
Path p = (Path)o;
return (int)(f - p.f);
}
public T getPoint(){
return point;
}
public void setPoint(T p){
point = p;
}
}
protected abstract boolean isGoal(T node);
protected abstract Double g(T from, T to);
protected abstract Double h(T from, T to);
protected abstract List<T> generateSuccessors(T node);
private PriorityQueue<Path> paths;
private HashMap<T, Double> mindists;
private Double lastCost;
private int expandedCounter;
public int getExpandedCounter(){
return expandedCounter;
}
public AStar(){
paths = new PriorityQueue<Path>();
mindists = new HashMap<T, Double>();
expandedCounter = 0;
lastCost = 0.0;
}
protected Double f(Path p, T from, T to){
Double g = g(from, to) + ((p.parent != null) ? p.parent.g : 0.0);
Double h = h(from, to);
p.g = g;
p.f = g + h;
return p.f;
}
private void expand(Path path){
T p = path.getPoint();
Double min = mindists.get(path.getPoint());
if(min == null || min.doubleValue() > path.f.doubleValue())
mindists.put(path.getPoint(), path.f);
else
return;
List<T> successors = generateSuccessors(p);
for(T t : successors){
Path newPath = new Path(path);
newPath.setPoint(t);
f(newPath, path.getPoint(), t);
paths.offer(newPath);
}
expandedCounter++;
}
public Double getCost(){
return lastCost;
}
public List<T> compute(T start){
try{
Path root = new Path();
root.setPoint(start);
/* Needed if the initial point has a cost. */
f(root, start, start);
expand(root);
for(;;){
Path p = paths.poll();
if(p == null){
lastCost = Double.MAX_VALUE;
return null;
}
T last = p.getPoint();
lastCost = p.g;
if(isGoal(last)){
LinkedList<T> retPath = new LinkedList<T>();
for(Path i = p; i != null; i = i.parent){
retPath.addFirst(i.getPoint());
}
return retPath;
}
expand(p);
}
}
catch(Exception e){
e.printStackTrace();
}
return null;
}
}
使用main的路径查找类：
import java.util.*;
public class PathFinder extends AStar<PathFinder.Node>
{
private int[][] map;
private int endx;
private int endy;
public static class Node{
public int x;
public int y;
Node(int x, int y){
this.x = x;
this.y = y;
}
public String toString(){
return "(" + x + ", " + y + ") ";
}
public int getX(){
return x;
}
public int getY(){
return y;
}
} public PathFinder(int[][] map, int endx, int endy){
this.map = map;
this.endx=endx;
this.endy=endy;
}
protected boolean isGoal(Node node){
return (node.x == endx) && (node.y == endy);
}
protected Double g(Node from, Node to){
if(from.x == to.x && from.y == to.y){
// System.out.println("To x1 " + to.x);
// System.out.println("To y1 " + to.y);
return 0.0;}
if(map[to.y][to.x] == 1){
//System.out.println("To x2 " + to.x);
// System.out.println("To y2 " + to.y);
return 1.0;}
return Double.MAX_VALUE;
}
protected Double h(Node from, Node to){
return new Double(Math.abs(endx - to.x) + Math.abs(endy - to.y));
}
protected List<Node> generateSuccessors(Node node){
List<Node> ret = new LinkedList<Node>();
int x = node.x;
int y = node.y;
if(y < map[0].length-1 && map[y+1][x] == 1)
ret.add(new Node(x, y+1));
if(x <map.length-1 && map[y][x+1] == 1)
ret.add(new Node(x+1, y));
if(y !=0 && map[y-1][x] == 1)
ret.add(new Node(x, y-1));
if(x !=0 && map[y][x-1] == 1)
ret.add(new Node(x-1, y));
return ret;
}
public static void main(String [] args){
WorldGenerator gen = new WorldGenerator();
int ammountOfBlocks =200;
int width = 25;
int length = 25;
int startX = 1;
int startY = 1;
int endX = 24;
int endY = 24;
int[][] map = gen.createWorld(ammountOfBlocks,width,length,startX,startY,endX,endY);
int a=map.length;
int b=map[0].length;
int[][] map2=new int[b][a];
for(int i=0; i<map.length; i++){
for(int j=0; j<map[0].length;j++)
{map2[j][i]=map[i][j];
}
}
PathFinder pf = new PathFinder(map,endX,endY);
/* for(int i = 0; i < map.length; i++){
for(int j = 0; j < map[0].length; j++)
System.out.print(map[i][j] + " ");
System.out.println();
}*/
long begin = System.currentTimeMillis();
List<Node> nodes = pf.compute(new PathFinder.Node(startX,startY));
long end = System.currentTimeMillis();
System.out.println("Time = " + (end - begin) + " ms" );
//System.out.println("Expanded = " + pf.getExpandedCounter());
System.out.println("Cost = " + pf.getCost());
if(nodes == null)
System.out.println("No path");
else{
for(int i=0; i<nodes.size();i++){
Node n=nodes.get(i);
int x= n.getX();
int y= n.getY();
map[x][y]=4;
}
/* for(int i = 0; i < map.length; i++){
for(int j = 0; j < map[0].length; j++)
System.out.print(map[i][j] + " ");
System.out.println();
}*/
}
}
}
WorldGenerator类仅生成1和0的二维数组。 提前谢谢！
答案 0 :(得分：0)
从我的臀部拍摄，但如果您想使用“直线距离”作为启发式，则代码中会出现错误。
当前的启发式是： delta x和y之和最小的节点最接近。
假设我们有一个5 x 5网格，目标是2.2，然后使用这个启发式2,0将同样最优，因为1,1当然是错误的。
尝试使用毕达哥拉斯进行新的启发式算法：到最后距离最短的节点是最接近的。
protected Double h(Node from, Node to) {
int dx = Math.abs(endx - to.x);
int dy = Math.abs(endy - to.y);
return new Double(Math.sqrt(dx*dx) + (dy*dy));
}
这会使您的算法使用http://en.wikipedia.org/wiki/Admissible_heuristic，这是A *的标准：http://en.wikipedia.org/wiki/A_star#Admissibility_and_optimality
希望这有帮助。
适用于我的解决方案：
<强> AStar.java
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
public abstract class AStar<T> {
private class Path<T> implements Comparable {
public T point;
public Double f;
public Double g;
public Path<T> parent;
public Path() {
parent = null;
point = null;
g = f = 0.0;
}
public Path(Path<T> p) {
this();
parent = p;
g = p.g;
f = p.f;
}
@Override
public int compareTo(Object o) {
AStar.Path p = (AStar.Path) o;
return (int) (f - p.f);
}
public T getPoint() {
return point;
}
public void setPoint(T p) {
point = p;
}
}
protected abstract boolean isGoal(T node);
protected abstract Double g(T from, T to);
protected abstract Double h(T from, T to);
protected abstract List<T> generateSuccessors(T node, T parent);
private PriorityQueue<AStar.Path> paths;
private HashMap<T, Double> mindists;
private Double lastCost;
private int expandedCounter;
public int getExpandedCounter() {
return expandedCounter;
}
public AStar() {
paths = new PriorityQueue<>();
mindists = new HashMap<>();
expandedCounter = 0;
lastCost = 0.0;
}
protected Double f(AStar.Path p, T from, T to) {
Double g = g(from, to) + ((p.parent != null) ? p.parent.g : 0.0);
Double h = h(from, to);
p.g = g;
p.f = g + h;
return p.f;
}
private void expand(Path<T> path) {
if (expandedCounter > 1000000) {
return;
}
T p = path.getPoint();
Double min = mindists.get(path.getPoint());
if (min == null || min.doubleValue() > path.f.doubleValue()) {
mindists.put(path.getPoint(), path.f);
} else {
return;
}
List<T> successors = generateSuccessors(p, path.parent != null ? path.parent.getPoint() : null);
for (T t : successors) {
AStar.Path newPath = new AStar.Path(path);
newPath.setPoint(t);
f(newPath, path.getPoint(), t);
paths.offer(newPath);
}
expandedCounter++;
}
public Double getCost() {
return lastCost;
}
public List<T> compute(T start) {
try {
AStar.Path root = new AStar.Path();
root.setPoint(start);
/*
* Needed if the initial point has a cost.
*/
f(root, start, start);
expand(root);
for (;;) {
Path<T> p = paths.poll();
if (p == null) {
lastCost = Double.MAX_VALUE;
return null;
}
T last = p.getPoint();
lastCost = p.g;
if (isGoal(last)) {
LinkedList<T> retPath = new LinkedList<T>();
for (Path<T> i = p; i != null; i = i.parent) {
retPath.addFirst(i.getPoint());
}
return retPath;
}
expand(p);
}
} catch (Exception e) {
e.printStackTrace();
}
return null;
}
}
<强> PathFinder.java
package playground;
import java.util.*;
public class PathFinder extends AStar<PathFinder.Node> {
private int[][] map;
private int endx;
private int endy;
public static class Node {
public int x;
public int y;
Node(int x, int y) {
this.x = x;
this.y = y;
}
public String toString() {
return "(" + x + ", " + y + ") ";
}
public int getX() {
return x;
}
public int getY() {
return y;
}
}
public PathFinder(int[][] map, int endx, int endy) {
this.map = map;
this.endx = endx;
this.endy = endy;
}
protected boolean isGoal(Node node) {
return (node.x == endx) && (node.y == endy);
}
protected Double g(Node from, Node to) {
if (from.x == to.x && from.y == to.y) {
// System.out.println("To x1 " + to.x);
// System.out.println("To y1 " + to.y);
return 0.0;
}
if (map[to.y][to.x] == 1) {
//System.out.println("To x2 " + to.x);
// System.out.println("To y2 " + to.y);
return 1.0;
}
return Double.MAX_VALUE;
}
protected Double h(Node from, Node to) {
int dx = Math.abs(endx - to.x);
int dy = Math.abs(endy - to.y);
return new Double(Math.sqrt(dx * dx) + (dy * dy));
//return new Double(Math.abs(endx - to.x) + Math.abs(endy - to.y));
}
@Override
protected List<Node> generateSuccessors(Node node, Node parent) {
List<Node> ret = new LinkedList<Node>();
int x = node.x;
int y = node.y;
if (y < map[0].length - 1 && map[y + 1][x] == 1 && (parent == null || (parent != null && !(parent.x == x && parent.y == y + 1)))) {
ret.add(new Node(x, y + 1));
}
if (x < map.length - 1 && map[y][x + 1] == 1 && (parent == null || (parent != null && !(parent.x == x + 1 && parent.y == y)))) {
ret.add(new Node(x + 1, y));
}
if (y != 0 && map[y - 1][x] == 1 && (parent == null || (parent != null && !(parent.x == x && parent.y == y - 1)))) {
ret.add(new Node(x, y - 1));
}
if (x != 0 && map[y][x - 1] == 1 && (parent == null || (parent != null && !(parent.x == x - 1 && parent.y == y)))) {
ret.add(new Node(x - 1, y));
}
return ret;
}
public static void main(String[] args) {
int ammountOfBlocks = 200;
int width = 25;
int length = 25;
int startX = 1;
int startY = 1;
int endX = 24;
int endY = 24;
int[][] map = {
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 0, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 0, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1},
{1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1},
{1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1},
{0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0},
{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1}
};
int a = map.length;
int b = map[0].length;
int[][] map2 = new int[b][a];
for (int i = 0; i < map.length; i++) {
for (int j = 0; j < map[0].length; j++) {
map2[j][i] = map[i][j];
}
}
PathFinder pf = new PathFinder(map, endX, endY);
/*
* for(int i = 0; i < map.length; i++){ for(int j = 0; j <
* map[0].length; j++) System.out.print(map[i][j] + " ");
* System.out.println(); }
*/
long begin = System.currentTimeMillis();
List<Node> nodes = pf.compute(new PathFinder.Node(startX, startY));
long end = System.currentTimeMillis();
System.out.println("Time = " + (end - begin) + " ms");
//System.out.println("Expanded = " + pf.getExpandedCounter());
System.out.println("Cost = " + pf.getCost());
if (nodes == null) {
System.out.println("No path");
} else {
for (int i = 0; i < nodes.size(); i++) {
Node n = nodes.get(i);
int x = n.getX();
int y = n.getY();
map[x][y] = 4;
}
for(int i = 0; i < map.length; i++){
for(int j = 0; j < map[0].length; j++)
System.out.print(map[i][j] + " ");
System.out.println();
}
}
}
}