机器人 / 系统课时
用 A* 在栅格地图中搜索路径
用 A* 在栅格地图中搜索路径
二维栅格中有障碍时,直线连接起终点往往会撞墙。本课用 Python 标准库实现四邻接 A*:维护开放与关闭集合、记录父节点、回溯可行路径,并用固定地图检查绕行步数及不可达结果。
学习目标
- 区分开放集合、关闭集合与优先队列的职责。
- 使用曼哈顿启发式实现 A* 并回溯最短路径。
- 通过固定地图验证障碍绕行和不可达处理。
定义可搜索的栅格
将地图表示为 6 行 8 列的字符数组:. 代表可通行,# 代表障碍;坐标写作 (行, 列),从 0 开始。起点为 (2, 1),终点为 (2, 6)。第 1~4 行的第 3 列是一堵竖墙,上下两端都能绕行。
本例只允许上、右、下、左四个方向移动,每次代价为 1,不允许斜走或穿越障碍。直线估计只需水平跨越 5 列,但必须绕墙:经第 0 行绕行需要 9 步,经第 5 行需要 11 步。正确结果应选择上方通道。
用评分维护开放与关闭集合
A* 给每个候选格计算 : 是从起点已走的实际步数, 是到终点的曼哈顿距离 。open_set 存放待扩展格,最小堆 open_heap 负责优先取出 最小的格;取出并处理后放入 closed_set。
例如起点 (2, 1) 的 、、;向右到 (2, 2) 后 、、,再向右却遇到障碍。搜索会转而尝试其它候选格。只有找到更小的 时才更新 parent;目标出堆后,沿 parent 逐格回溯到起点。

实现 A* 并重建路径
下面的完整 Python 3 示例只依赖标准库。堆元素依次保存 f、g 和坐标;相同 f 时按 g、行列坐标确定顺序,因此本地图的扩展数量也可复现。堆允许保留旧条目,出堆时用最新 g_score 跳过失效记录。
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。预期输出:
步数: 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)。