【路径规划】A*算法 Java实现

news2024/9/23 21:20:59

A*（A-Star）算法是一种广泛使用的寻路算法，尤其在计算机科学和人工智能领域。

算法思想

通过评估函数来引导搜索过程，从而找到从起始点到目标点的最短路径。评估函数通常包括两部分：一部分是已经走过的实际距离，称为g值；另一部分是对当前位置到目标位置的估计距离，称为h值。A*算法每次选择g值加上h值最小的节点作为下一个要访问的节点，直到找到目标节点为止。

A*算法维护一个开放列表和一个关闭列表。开放列表包含待访问的节点，而关闭列表包含已经访问过的节点。算法从起始节点开始，将其加入开放列表。然后，算法从开放列表中选择一个评估函数值最小的节点进行扩展，将其邻居节点加入开放列表，并将当前节点移入关闭列表。算法不断重复这个过程，直到找到目标节点或者开放列表为空。

在这里插入图片描述

代码实现

单元格类实现

/**
 * 表示地图上的一个单元格
 */
class Cell {
    int i, j; // 单元格的行和列索引
    int f, g, h; // f值、g值和h值
    Cell parent; // 父节点
    boolean closed, visited; // 是否关闭和是否访问过

    // 构造函数
    public Cell(int i, int j) {
        this.i = i;
        this.j = j;
        this.f = 0;
        this.g = 0;
        this.h = 0;
        this.parent = null;
        this.closed = false;
        this.visited = false;
    }
}

A*算法类实现

/**
 * A*算法实现类
 */
public class AStar {
    private int[][] map; // 地图
    private int startI, startJ; // 起始位置的行和列索引
    private int endI, endJ; // 目标位置的行和列索引
    private List<Cell> openList = new ArrayList<>(); // 开放列表
    private static final int DIAGONAL_COST = 14; // 对角线移动的代价
    private static final int VERTICAL_HORIZONTAL_COST = 10; // 垂直或水平移动的代价

    // 构造函数
    public AStar(int[][] map, int startI, int startJ, int endI, int endJ) {
        this.map = map;
        this.startI = startI;
        this.startJ = startJ;
        this.endI = endI;
        this.endJ = endJ;
    }

    /**
     * 搜索路径
     */
    public void search() {
        // 创建起始和目标单元格对象
        Cell start = new Cell(startI, startJ);
        Cell end = new Cell(endI, endJ);
        // 将起始单元格添加到开放列表中
        openList.add(start);
        while (!openList.isEmpty()) {
            // 按照f值对开放列表进行排序，选择f值最小的单元格作为当前单元格
            Collections.sort(openList, Comparator.comparingInt(cell -> cell.f));
            Cell current = openList.get(0);
            // 如果当前单元格是目标单元格，则找到路径，打印路径并返回
            if (current.i == end.i && current.j == end.j) {
                printPath(current);
                return;
            }
            // 从开放列表中移除当前单元格，并将其标记为已关闭
            openList.remove(current);
            current.closed = true;
            // 遍历邻居单元格
            for (int[] direction : directions) {
                int newI = current.i + direction[0]; // 计算新的行索引
                int newJ = current.j + direction[1]; // 计算新的列索引
                // 如果新的索引越界，则跳过该邻居单元格的处理
                if (newI < 0 || newI >= map.length || newJ < 0 || newJ >= map[0].length) {
                    continue;
                }
                // 如果邻居单元格是障碍物，则跳过该邻居单元格的处理
                if (map[newI][newJ] == 1) {
                    continue;
                }
                Cell neighbor = new Cell(newI, newJ);
                if (neighbor.closed) { // 已关闭的单元格处理
                    continue;
                }
                // 计算代价和启发式函数值
                int g = current.g + getCost(current, neighbor);
                int h = heuristic(neighbor, end);
                if (!neighbor.visited) { // 未访问过的邻居单元格处理
                    neighbor.visited = true;
                    neighbor.parent = current; // 设置父节点
                    neighbor.g = g; // 设置g值
                    neighbor.h = h; // 设置h值
                    neighbor.f = g + h; // 设置f值
                    openList.add(neighbor); // 添加到开放列表中
                } else if (g < neighbor.g) { // 已访问过的邻居单元格，且新的路径代价更小处理
                    neighbor.parent = current; // 更新父节点
                    neighbor.g = g; // 更新g值
                    neighbor.f = g + neighbor.h; // 更新f值
                }
            }
        }
        System.out.println("No path found."); // 没有找到路径的情况处理
    }

    // 计算从当前单元格到邻居单元格的代价
    private int getCost(Cell current, Cell neighbor) {
        int dx = Math.abs(current.i - neighbor.i);
        int dy = Math.abs(current.j - neighbor.j);
        if (dx == 1 && dy == 1) { // 对角线移动
            return DIAGONAL_COST;
        } else { // 垂直或水平移动
            return VERTICAL_HORIZONTAL_COST;
        }
    }

    // 启发式函数，计算当前单元格到目标单元格的预计代价
    private int heuristic(Cell current, Cell goal) {
        int dx = Math.abs(current.i - goal.i);
        int dy = Math.abs(current.j - goal.j);
        return dx + dy; // 使用曼哈顿距离作为启发式函数
    }

    // 打印路径
    private void printPath(Cell end) {
        List<Cell> path = new ArrayList<>();
        Cell current = end;
        while (current != null) {
            path.add(current);
            current = current.parent;
        }
        Collections.reverse(path);
        System.out.print("Path: ");
        for (Cell cell : path) {
            System.out.print("(" + cell.i + "," + cell.j + ") ");
        }
        System.out.println();
    }

    // 八个方向的移动向量
    private static final int[][] directions = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}, {0, 1}, {1, -1}, {1, 0}, {1, 1}};

    public static void main(String[] args) {
        int[][] map = {{0, 0, 0, 0, 0}, {0, 1, 1, 0, 0}, {0, 0, 0, 0, 0}, {0, 1, 1, 1, 0}, {0, 0, 0, 0, 0}}; // 定义地图，0表示可通过，1表示障碍物
        //打印一次地图用于观察
        for (int[] arr : map) {
            for (int x : arr) {
                System.out.print(x + "   ");
            }
            System.out.println();
        }
        AStar astar = new AStar(map, 0, 0, 4, 4); // 创建A*对象，设置起始位置和目标位置
        astar.search(); // 搜索路径
    }
}