机器人 / 系统课时

用 A* 在栅格地图中搜索路径

用 A* 在栅格地图中搜索路径

二维栅格中有障碍时,直线连接起终点往往会撞墙。本课用 Python 标准库实现四邻接 A*:维护开放与关闭集合、记录父节点、回溯可行路径,并用固定地图检查绕行步数及不可达结果。

学习目标

  • 区分开放集合、关闭集合与优先队列的职责。
  • 使用曼哈顿启发式实现 A* 并回溯最短路径。
  • 通过固定地图验证障碍绕行和不可达处理。

定义可搜索的栅格

将地图表示为 6 行 8 列的字符数组:. 代表可通行,# 代表障碍;坐标写作 (行, 列),从 0 开始。起点为 (2, 1),终点为 (2, 6)。第 1~4 行的第 3 列是一堵竖墙,上下两端都能绕行。

本例只允许上、右、下、左四个方向移动,每次代价为 1,不允许斜走或穿越障碍。直线估计只需水平跨越 5 列,但必须绕墙:经第 0 行绕行需要 9 步,经第 5 行需要 11 步。正确结果应选择上方通道。

用评分维护开放与关闭集合

A* 给每个候选格计算 f(n)=g(n)+h(n)f(n)=g(n)+h(n):g(n)g(n) 是从起点已走的实际步数,h(n)h(n) 是到终点的曼哈顿距离 ∣r−rg∣+∣c−cg∣|r-r_g|+|c-c_g|。open_set 存放待扩展格,最小堆 open_heap 负责优先取出 ff 最小的格;取出并处理后放入 closed_set。

例如起点 (2, 1) 的 g=0g=0、h=5h=5、f=5f=5;向右到 (2, 2) 后 g=1g=1、h=4h=4、f=5f=5,再向右却遇到障碍。搜索会转而尝试其它候选格。只有找到更小的 gg 时才更新 parent;目标出堆后,沿 parent 逐格回溯到起点。

A* 栅格障碍绕行、开放集合与关闭集合关系

实现 A* 并重建路径

下面的完整 Python 3 示例只依赖标准库。堆元素依次保存 f、g 和坐标;相同 f 时按 g、行列坐标确定顺序,因此本地图的扩展数量也可复现。堆允许保留旧条目,出堆时用最新 g_score 跳过失效记录。

python
from heapq import heappop, heappush

DIRECTIONS = [(-1, 0), (0, 1), (1, 0), (0, -1)]

def heuristic(a, b):
    # 四邻接、单位步长下,曼哈顿距离不会高估剩余代价。
    return abs(a[0] - b[0]) + abs(a[1] - b[1])

def a_star(grid, start, goal):
    if not grid or not grid[0]:
        raise ValueError('地图不能为空')
    rows, cols = len(grid), len(grid[0])
    if any(len(line) != cols for line in grid):
        raise ValueError('地图行宽不一致')
    for r, c in (start, goal):
        if not (0 <= r < rows and 0 <= c < cols) or grid[r][c] == '#':
            raise ValueError('起终点越界或位于障碍格')

    open_heap = [(heuristic(start, goal), 0, start)]
    open_set = {start}
    closed_set = set()
    g_score = {start: 0}
    parent = {}

    while open_heap:
        _, g, current = heappop(open_heap)
        # 堆中可能存在同一格的旧代价;已经扩展过的也不再处理。
        if current in closed_set or g != g_score[current]:
            continue
        open_set.remove(current)
        closed_set.add(current)
        if current == goal:
            path = [current]
            # 父节点记录反向链路,回溯后再翻转。
            while current in parent:
                current = parent[current]
                path.append(current)
            return path[::-1], len(closed_set)

        for dr, dc in DIRECTIONS:
            nxt = (current[0] + dr, current[1] + dc)
            r, c = nxt
            if not (0 <= r < rows and 0 <= c < cols):
                continue
            if grid[r][c] == '#' or nxt in closed_set:
                continue
            new_g = g + 1
            if new_g < g_score.get(nxt, float('inf')):
                # 仅当找到更短的到达路线时更新父节点。
                g_score[nxt] = new_g
                parent[nxt] = current
                heappush(open_heap, (new_g + heuristic(nxt, goal), new_g, nxt))
                open_set.add(nxt)

    return [], len(closed_set)

if __name__ == '__main__':
    grid = [
        '........',
        '...#....',
        '...#....',
        '...#....',
        '...#....',
        '........',
    ]
    path, expanded = a_star(grid, (2, 1), (2, 6))
    print('步数:', len(path) - 1 if path else -1)
    print('路径:', path)
    print('已扩展节点:', expanded)

    no_path, _ = a_star(['...', '###', '...'], (0, 0), (2, 2))
    print('封闭地图可达:', bool(no_path))

保存为 astar_grid.py,运行 python astar_grid.py。预期输出:

text
步数: 9
路径: [(2, 1), (2, 2), (1, 2), (0, 2), (0, 3), (0, 4), (0, 5), (0, 6), (1, 6), (2, 6)]
已扩展节点: 23
封闭地图可达: False

验证结果与适用边界

路径共有 10 个坐标、9 条相邻边;每对连续坐标恰好相差一个行号或列号,且所有格均为 .。第 0 行跨越障碍列,而不是穿过第 1~4 行的墙。用这三项检查路径,比只看是否到达终点更可靠。

第二组地图的中间行全为障碍,搜索耗尽候选格后返回空路径;起终点越界或落在 # 上则抛出 ValueError。在当前四邻接、单位代价和一致曼哈顿启发式下,首次弹出目标即得到最短路径;设地图有 R 行、C 列,最坏时间复杂度为 O(RC log(RC)),空间复杂度为 O(RC)。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 A* 在栅格地图中搜索路径

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通