ARAstar java实现

ARA*（Anytime Repairing A*）是对经典的A*算法的改进，它能够在保证最优解的前提下，更快地找到一条可行的路径，并且还能在运行过程中不断地改进路径。下面是ARA*算法的Java实现，假设我们要在一个网格图中寻找从起点到终点的最短路径：

import java.util.ArrayList;
import java.util.Comparator;
import java.util.List;
import java.util.PriorityQueue;

public class ARAStar {
    private final int[][] grid; // 网格地图
    private final int rows; // 行数
    private final int cols; // 列数
    private final int startX; // 起点x坐标
    private final int startY; // 起点y坐标
    private final int endX; // 终点x坐标
    private final int endY; // 终点y坐标
    private final double epsilon; // 松弛因子
    private final int[][] directions = {{1, 0}, {-1, 0}, {0, 1}, {0, -1}}; // 上下左右四个方向
    private final PriorityQueue<Node> openSet; // 开放列表，按照f值从小到大排序
    private final List<Node> closeSet; // 关闭列表，存放已经被探索过的节点
    private boolean[][] obstacleGrid; // 障碍网格地图

    public ARAStar(int[][] grid, int startX, int startY, int endX, int endY, double epsilon) {
        this.grid = grid;
        this.rows = grid.length;
        this.cols = grid[0].length;
        this.startX = startX;
        this.startY = startY;
        this.endX = endX;
        this.endY = endY;
        this.epsilon = epsilon;
        this.openSet = new PriorityQueue<>(Comparator.comparingDouble(node -> node.f));
        this.closeSet = new ArrayList<>();
        this.obstacleGrid = new boolean[rows][cols];
    }

    // ARA*算法主体
    public List<Node> search() {
        Node start = new Node(startX, startY);
        start.g = 0;
        start.h = heuristic(startX, startY, endX, endY);
        start.f = start.g + epsilon * start.h;
        openSet.offer(start);

        while (!openSet.isEmpty()) {
            Node curr = openSet.poll();
            if (curr.x == endX && curr.y == endY) {
                return getPath(curr);
            }
            closeSet.add(curr);
            for (int[] direction : directions) {
                int nextX = curr.x + direction[0];
                int nextY = curr.y + direction[1];
                if (nextX < 0 || nextX >= rows || nextY < 0 || nextY >= cols) {
                    continue;
                }
                if (obstacleGrid[nextX][nextY]) {
                    continue;
                }
                double nextG = curr.g + getDistance(curr.x, curr.y, nextX, nextY);
                double nextH = heuristic(nextX, nextY, endX, endY);
                double nextF = nextG + epsilon * nextH;
                Node nextNode = new Node(nextX, nextY);
                nextNode.g = nextG;
                nextNode.h = nextH;
                nextNode.f = nextF;
                if (closeSet.contains(nextNode)) {
                    continue;
                }
                if (openSet.contains(nextNode)) {
                    Node oldNode = openSet.stream().filter(node -> node.equals(nextNode)).findFirst().get();
                    if (oldNode.g > nextG) {
                        openSet.remove(oldNode);
                        openSet.offer(nextNode);
                    }
                } else {
                    openSet.offer(nextNode);
                }
            }
        }
        return null;
    }

    // 计算两点之间的欧几里得距离
    private double getDistance(int x1, int y1, int x2, int y2) {
        return Math.sqrt(Math.pow(x1 - x2, 2) + Math.pow(y1 - y2, 2));
    }

    // 计算启发函数估价值
    private double heuristic(int x1, int y1, int x2, int y2) {
        return getDistance(x1, y1, x2, y2);
    }

    // 获取从起点到终点的路径
    private List<Node> getPath(Node node) {
        List<Node> path = new ArrayList<>();
        path.add(node);
        while (node.parent != null) {
            node = node.parent;
            path.add(0, node);
        }
        return path;
    }

    // 设置障碍网格
    public void setObstacleGrid(int[][] obstacleGrid) {
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                this.obstacleGrid[i][j] = obstacleGrid[i][j] == 1;
            }
        }
    }

    // 节点类
    private static class Node {
        int x;
        int y;
        double g; // 起点到该点的实际代价
        double h; // 该点到终点的估价代价
        double f; // f = g + epsilon * h
        Node parent;

        public Node(int x, int y) {
            this.x = x;
            this.y = y;
        }

        @Override
        public boolean equals(Object o) {
            if (this == o) {
                return true;
            }
            if (o == null || getClass() != o.getClass()) {
                return false;
            }
            Node node = (Node) o;
            return x == node.x && y == node.y;
        }

        @Override
        public int hashCode() {
            return x * 31 + y;
        }
    }
}

使用示例：

public class Main {
    public static void main(String[] args) {
        int[][] grid = {{0, 0, 0, 0, 0},
                        {0, 1, 0, 1, 0},
                        {0, 0, 0, 0, 0},
                        {0, 0, 1, 0, 0},
                        {0, 0, 0, 0, 0}};
        ARAStar araStar = new ARAStar(grid, 0, 0, 4, 4, 1.5);
        int[][] obstacleGrid = {{0, 0, 0, 0, 0},
                                {0, 1, 0, 1, 0},
                                {0, 0, 0, 1, 0},
                                {0, 0, 1, 0, 0},
                                {0, 0, 0, 0, 0}};
        araStar.setObstacleGrid(obstacleGrid);
        List<ARAStar.Node> path = araStar.search();
        if (path != null) {
            for (ARAStar.Node node : path) {
                System.out.print("(" + node.x + ", " + node.y + ") -> ");
            }
            System.out.println("end");
        } else {
            System.out.println("No path found!");
        }
    }
}