简介路径规划中经典的A算法Python实现面向机器人导航、游戏寻路与地图规划等场景的开发者适合希望掌握启发式搜索并提升路径求解效率的读者。压缩包共3个文件全部为py脚本总大小仅10KB包含基础a_star.py、双向搜索a_star_searching_from_two_side.py以及a_star_variants.py覆盖节点类、开放与关闭列表、曼哈顿距离启发式函数、邻居扩展和路径重建完整实现。已有1009人学习下载。通过阅读源码可理解A如何融合Dijkstra全局最优性与贪心最佳优先的局部高效性并对比双向搜索和不同变体的适用场景。文件体积小、模块清晰适合算法学习者直接迁移到实际路径规划项目也可作为机器人导航或游戏开发前的快速参考。1. 为什么自己写一遍 A* 比调库更值钱路径规划A算法 python实现这个标题几乎是进入路径规划最稳的一步。我之前做模拟项目X的机器人导航时一开始直接用第三方寻路库接口传入地图和起终点就能出路径。直到地图从栅格变成带高程的代价图库的权重参数怎么调都调不出预期我才意识到自己不懂内部黑匣子的逻辑。A的核心就是一句话用已经走过的代价加上对未来代价的估计决定下一步往哪走。把这句话用 Python 落成一个最小实现你才能真正理解启发函数、开放列表和回溯路径的关系。我按这个思路整理适合正在做机器人导航、游戏 NPC 寻路或者准备算法面试的同学先从能跑通的代码讲起参数怎么换、哪里容易翻车也会一并说清楚。2. A* 核心逻辑估价函数、开放列表与贪心倾向2.1 估价函数 f g h 里藏着路径质量的秘密A* 每一步都从候选节点里挑一个看起来最优先的节点去扩展。这个优先级就是 f g hg 是从起点实际走过来的代价h 是当前点到终点的估计代价。h 如果永远是 0A* 就退化成 Dijkstra算法会像水波一样向四周扩散扩展节点很多但路径一定最短h 如果设置合理搜索会带着方向感直奔目标扩展节点大幅减少。h 必须满足一个性质对任意节点h 不能大于从该节点到终点的真实最小代价这叫可采纳性。只有当 h 是可采纳的时候第一次从开放列表里弹出终点得到的路径才是全局最短的。这个性质理解不到位后面的调参和排错都会出问题。我在项目里最常被问到的就是为什么 A* 绕远路多半是 h 被放大了。常见地图里曼哈顿距离适合只能上下左右四方向移动的网格对角线距离适合八方向且斜走代价 sqrt(2) 的网格欧几里得距离适合任意角度运动。代码实现非常简单import math def manhattan(a, b): return abs(a[0] - b[0]) abs(a[1] - b[1]) def diagonal(a, b): dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) return dx dy (math.sqrt(2) - 2) * min(dx, dy)逻辑说明diagonal 在曼哈顿基础上做了一个修正每少走一步斜线就省下约 0.414 的代价。参数说明sqrt(2) - 2 是修正系数表示把两步直角路径变成一步斜线后省下的路程。欧几里得需要开平方计算更重如果把平方距离直接当 h会严重高估真实代价不可采纳。验证方法是跑同一个随机地图分别统计三种启发函数下扩展节点的数量你会发现 h 越接近真实代价扩展节点越少。我一般会写个循环记录 len(open_heap) 的峰值这组数据比任何理论都直观。2.2 为什么 open 和 closed 必须分开维护开放列表open保存已经生成但还没扩展的候选节点封闭列表closed保存已经扩展过、确认过当前代价的节点。如果不区分这两个集合算法会退化成一个不断重复扫描的无序过程。常见的错误写法是从 open 里取出节点后不把它加进 closed导致同一个节点被反复扩展open 越来越大另一种错误是新找到的邻居路径更短但该邻居已经在 closed 里就直接跳过这样会把更优路线给丢了。正确的做法是用一个字典 g_score 记录当前已知最短距离只有新代价更小才更新并重新放入 open。我用表格做个对比数据结构适合用途主要代价列表节点数少时勉强可用min() 和 in 都是 O(n)setclosed 快速判重无法保存 g 和父节点dict记录 g、父节点、h需要额外管理优先级heapqopen 的优先级队列元素无法直接修改需压入新版本配合 heapq 的典型代码片段是这样的g_score {start: 0} open_heap [(0, start)] closed set() while open_heap: f, node heapq.heappop(open_heap) if node in closed: continue closed.add(node)逻辑说明同一个节点可能因为路径更新被压入多次旧版本弹出时发现它已经在 closed 里就直接丢弃。参数说明g_score 的键是坐标元组值是起点到该坐标的实际代价。2.3 数据结构选型用 heapq 而不是每次 min()如果你用 min(open_list, keylambda x: x.f) 来找最小节点节点数到几千就会明显卡顿几万节点就是灾难。heapq 维护一个小根堆push 和 pop 都是 O(log n)这才是 A* 在大地图上能跑的底子。heapq 有一个容易被新手忽略的细节如果堆里存的是 (f, node) 这样的元组当两个 f 相等时Python 会继续比较 node。node 如果是自定义 Node 对象会直接抛 TypeError。解决办法有两种要么把 node 设计成可比较的元组要么在元组里加一个自增计数器让每个元素严格有序import heapq counter 0 open_heap [] heapq.heappush(open_heap, (node.f, counter, node)) counter 1逻辑说明引入了 counter 之后即使 f 相同也能保证元组比较继续到 counter不会去比较 node。参数说明counter 从 0 开始递增每次 push 都要更新。这样做的副作用是同一节点的多个旧版本会一直占着堆空间所以弹出后一定要用 g_score 或 closed 判断是否过期。把这三个决定想清楚A* 的主干就通了。接下来我会直接给一个能运行的最小实现你照着抄跑通之后再去动参数。3. 用 Python 写一个可跑的 A*网格地图最小实现3.1 地图建模与节点类设计我用二维数组表示网格地图0 是可行走区域1 是障碍物。数组下标是 grid[row][col]row 是行col 是列。为了和数组索引一致坐标一律用 (row, col)别再用 x、y很多坑都是坐标语义不一致引起的。节点类需要保存坐标、g、h、f 和父节点指针。父节点用于最后回溯路径。我习惯在类上加上slots这样一张大网格能省掉不少内存class Node: __slots__ (row, col, g, h, f, parent) def __init__(self, row, col): self.row row self.col col self.g float(inf) self.h 0 self.f float(inf) self.parent None逻辑说明slots会限制实例只能拥有声明的属性替代了默认的dict内存占用更低。参数说明g 初始化为无穷大这样新找到的路径只要比无穷大小时就能更新。地图和起点终点的定义如下。你可以先拿这个小地图测试grid [ [0, 0, 0, 0, 0], [0, 1, 1, 1, 0], [0, 0, 0, 1, 0], [0, 1, 0, 0, 0], [0, 0, 0, 0, 0], ] start (0, 0) goal (4, 4)地图建模的关键是要先明确数值含义。障碍值写 1可行走写 0后面扩展邻居时直接判断 grid[nr][nc] 1 就能跳过。3.2 主循环与邻居扩展完整实现如下我把关键的斜穿检查也写进去了避免路径从墙角钻过去import math import heapq def diagonal(a, b): dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) return dx dy (math.sqrt(2) - 2) * min(dx, dy) def a_star_search(grid, start, goal): rows, cols len(grid), len(grid[0]) open_heap [] nodes {} def get_node(pos): if pos not in nodes: nodes[pos] Node(pos[0], pos[1]) return nodes[pos] start_node get_node(start) start_node.g 0 start_node.h diagonal(start, goal) start_node.f start_node.g start_node.h heapq.heappush(open_heap, (start_node.f, start)) closed set() while open_heap: _, pos heapq.heappop(open_heap) if pos in closed: continue if pos goal: path [] cur pos while cur is not None: path.append(cur) cur nodes[cur].parent return path[::-1] closed.add(pos) for dr, dc in ((1,0),(-1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)): nr, nc pos[0] dr, pos[1] dc if not (0 nr rows and 0 nc cols): continue if grid[nr][nc] 1: continue if dr ! 0 and dc ! 0: if grid[pos[0] dr][pos[1]] 1 or grid[pos[0]][pos[1] dc] 1: continue step_cost math.sqrt(2) if dr ! 0 and dc ! 0 else 1.0 nxt (nr, nc) new_g nodes[pos].g step_cost if nxt not in nodes or new_g nodes[nxt].g: node get_node(nxt) node.g new_g node.h diagonal(nxt, goal) node.f node.g node.h node.parent pos heapq.heappush(open_heap, (node.f, nxt)) return []逻辑说明主循环先弹出一个节点如果它已经到过终点就回溯路径如果这个节点已经在 closed 里说明这是旧版本直接跳过。扩展时先做边界和障碍检查然后判断斜向穿墙最后比较新代价是否更小是的话就更新节点信息并重新入堆。参数说明step_cost 在直走时是 1斜走时是 sqrt(2)这对应八方向网格。closed 用 set 存坐标元组。nodes 字典保存所有节点对象避免重复创建。注意我弹出时用_忽略 f 值因为堆已经按 f 排序不需要再用它判断是否过期。这里有一个关键设计当 nxt 已经在 nodes 中且 new_g 更小时我不会新建节点而是复用旧节点并更新它的 g、f、parent。但旧节点可能在堆里还有另一个版本所以弹出后需要检查 closed。3.3 路径回溯与可视化验证回溯逻辑其实已经写在主循环里每个节点通过 parent 指向上一个节点找到终点后不断回溯到起点最后把列表反转就得到从起点到终点的顺序。下面这段用于验证path a_star_search(grid, start, goal) print(path)拿到路径后要确认它不是绕远路。我一般会把路径画出来用 imshow 看形状import matplotlib.pyplot as plt vis [row[:] for row in grid] for r, c in path: vis[r][c] 2 plt.imshow(vis, cmapgray, originlower) plt.show()逻辑说明把所有路径点标记成 2在灰度图上显示为浅色。originlower 让坐标原点在左下角避免行列和屏幕 y 轴反了。如果你没有带界面的环境也可以用文字打印把路径上的坐标用 * 标出来for r in range(len(grid)): line for c in range(len(grid[0])): if (r, c) in path: line * elif grid[r][c] 1: line # else: line . print(line)能跑通这一步A* 的最小闭环就完成了。接下来就是调参阶段如何选启发函数、如何用权重换速度、如何让路径更顺。4. A* 的启发函数选择与权重调节最优性还是实时性4.1 三种启发函数的适用范围与代价差异在同一个地图里换启发函数扩展节点数差别可能非常明显。我画过几组随机障碍图的对比结论是四方向移动时曼哈顿几乎是最优选择因为它准确反映了实际代价八方向移动时用 diagonal 比曼哈顿扩展的节点更少因为曼哈顿在八方向网格里小于真实代价符合可采纳性但比较保守导致扩展多。用一个表格对比常见启发启发函数典型移动方式一致性计算开销曼哈顿四方向满足整数加减对角线八方向带 sqrt(2) 斜代价满足一个 min 和乘法欧几里得任意角度满足开方实际操作中我会先用对角线距离跑一遍因为大部分网格寻路都允许斜走。如果地图的障碍非常不规则再用欧几里得。要注意欧几里得虽然符合可采纳性但计算开销大在大地图上会拖慢速度如果为了快可以比较平方距离但平方后的 h 会远大于真实代价不满足可采纳性路径可能变差。所以我不建议用平方距离。4.2 权重因子 epsilon用最优性换实时性当网格超过 500x500 或者需要在实时系统里规划时完全最优的 A* 往往不够快。加权 A* 把 f 改成 g epsilon * hepsilon 大于 1 时算法会更激进地冲向目标但可能错过更优的绕行路线。工程上常用 epsilon 取 2 到 4如果要求尽量接近最优可以用 epsilon 从 4 开始跑一个快速解再以这个解为界逐渐降低 epsilon 重新搜索。代码改动只有一行node.f node.g node.h * weight。我封装成一个参数def weighted_a_star(grid, start, goal, weight2.0): # 在节点更新处 node.f node.g node.h * weight参数说明weight1 时就是标准 A*weight 越大扩展节点越少路径越急。weight 不是越大越好太大会绕大圈甚至路线完全不像是人走的。我一般会保存 weight1 的最优路径长度作为基线再对比不同 weight 的路径长度确保偏差在可接受的 5% 以内。4.3 打破平局让路径更直的两种小技巧当 f 相同的情况频繁出现时A* 会扩展很多无用的节点路径也会出现无意义的抖动。两个常见技巧能改善这个问题。技巧一给 h 乘一个接近 1 的微小系数比如 1.001。这会让算法稍微偏好离目标更近的节点路径更直。缺点是这个系数破坏了严格的可采纳性虽然实际影响很小但在某些极端地图上可能出现非最优路径。技巧二入堆时让 h 大的节点优先。向堆里插入 (f, -h, counter, pos)因为 heapq 是小根堆-h 会让 h 更大的先弹出路径更倾向于往目标方向推进。代码如下heapq.heappush(open_heap, (node.f, -node.h, counter, pos)) counter 1逻辑说明当 f 相同-h 更小的会先弹出也就是 h 更大的会先弹出。参数说明counter 保证当 f 和 -h 都相同时节点之间仍然可比较。这两个技巧不用同时用优先试第二个因为它没有破坏可采纳性。调参的过程要靠数据说话我会用同一张地图跑几组参数对比路径长度和扩展节点数再决定用哪个。5. A* 避坑指南5 个真实翻车现场与排查顺序5.1 死循环open 永远清空不了现象程序运行很久不结束内存不断增长任务管理器里 CPU 占用打满。原因最常见的是没有把扩展过的节点加入 closed或者加入 closed 后又在生成邻居时放行 closed 中的节点导致同一个节点被反复扩展。另一个原因是边界判断写反比如把 0 nr rows 写成 0 nr rows偶尔越界时会直接抛异常但如果没有抛异常就可能反复访问同一区域。解决在 while 循环开头加if pos in closed: continue在生成新邻居时检查坐标边界。另外可以在每次循环末尾打印 open_heap 的长度如果长度只增不减基本就是死循环了。我遇到这种情况时会把障碍图缩小到 5x5逐步打印节点定位很快。5.2 斜穿墙角的非法路径现象路径看起来贴着墙角斜着穿过实际上两个对角障碍之间只有空隙机器人根本过不去。原因八方向扩展时只检查了目标格子是不是障碍没有检查斜向经过的两个直角格子。A* 会把斜穿当作一步 sqrt(2) 代价走过去这在物理上可能是穿墙。解决在斜向扩展时补上直角检查if dr ! 0 and dc ! 0: if grid[pos[0] dr][pos[1]] 1 or grid[pos[0]][pos[1] dc] 1: continue这段代码我在前面实现里写过但很多人第一次会漏掉。检查办法是看路径中相邻两个点是否有 (r1, c1) 这样的斜对同时中间两个直角格之一为障碍。5.3 行列坐标颠倒导致的地图越界现象地图定义正常但运行时报 list index out of range或者路径完全对不上位置。原因Python 二维数组索引是 grid[row][col]也就是第一个下标是行第二个是列。如果你把传入坐标当成 (x, y)访问 grid[y][x] 或者直接用 (x, y) 作为 (row, col)在宽高不相等的矩形地图上必然越界。解决统一使用 (row, col)在入口处把外部坐标转换好。我见过一个项目把 start 存成 (col, row)网格访问时又用 grid[row][col]最后路径画出来是转置的。测试时先跑一个 3x5 的不对称地图能暴露问题。5.4 启发函数过大导致路径并非最短现象算法几毫秒就跑完但路径绕过了一大圈明显不是最短路径。原因h 被设置得太大不满足可采纳性。常见操作是把欧几里得距离写成了 dxdx dydy或者八方向网格用曼哈顿实际上曼哈顿在八方向网格里小于真实代价仍可采纳真正过大的是把平方距离当 h或者把对角线距离的系数弄错例如把 sqrt(2)2 写成了 sqrt(2)-2导致 h 大于真实代价。解决把 h 计算公式打印出来对照。验证方法将 h 设为 0 跑一次标准 A*得到最优路径长度再把当前 h 跑一次比较两者。如果 h 更短但路径更长说明 h 不可采纳。调试时还可以检查对角线函数的系数是否书写正确。5.5 大网格内存爆炸与速度滑坡现象200x200 还能跑2000x2000 就卡死10000x10000 直接内存溢出。原因为每个坐标创建 Node 对象保存 row、col、g、h、f、parent 完整信息再加上 dict 和 set 的开销几个百万节点的地图很快会把内存吃光。另一个原因是使用了 min(list) 而不是 heapq时间复杂度从 O(log n) 变成 O(n)节点一多就明显。解决用slots压缩对象把 nodes 字典的键改成 grid[r][c] 的线性索引 index r * cols c而不是元组。closed 也可以用 set 存整数索引。如果还需要更快就考虑双向 A* 或 JPS而不是继续在单次搜索上硬扛。我自己的习惯是节点数超过一百万个时先处理地图压缩再考虑算法改进。6. 让 A* 更快双向搜索与跳点剪枝的落地思路当起点和终点都很明确双向 A* 是性价比最高的加速方案。它同时从起点向前、从终点向后搜索两个方向各扩展一半总扩展次数远小于单向搜索。终止条件不是严格的两个方向在中间相遇而是当某一方向的 closed 集合与另一方向 open 中的节点相交时拼接两条路径。下面是最小骨架注意后向搜索时它的目标是起点所以启发函数也要相应调整def bidirectional_a_star(grid, start, goal): open_f [(0, start)] open_b [(0, goal)] g_f {start: 0} g_b {goal: 0} came_from_f {start: None} came_from_b {goal: None} # 交替从两个堆各扩展一层 # 检查是否有节点同时出现在两个方向的 visited 集合 # 相遇后分别回溯两段路径拼接时去掉重复节点写完核心逻辑后验证方法是跑同一张 1000x1000 地图分别记录单向和双向的扩展节点数与耗时。一般双向能省 30% 到 50% 的节点但代码维护成本更高如果地图特别稀疏效果明显如果地图很稠密两个方向很快各走各的收益不大。跳点搜索JPS是另一种思路只在关键转折点生成节点中间直线段可以直接跳过去。它适合障碍分布稀疏、网格均匀的场景不适合障碍密集或代价不均匀的地图。我通常先评估地图上是否有大量连续可直线穿越的区域有才考虑 JPS。我现在的习惯是100x100 以内直接用普通 A* 加权重调参节点上了百万先测双向 A* 的扩展节点数仍然不满足再上 JPS。不要一上来就堆算法先把第 5 章那些坑避开再用数据决定下一步。希望帮到你。本文还有配套的精品资源点击获取
