简介路径规划A算法的Python实现面向机器人导航、游戏角色寻路、地图路径计算等常见需求也适合正在系统学习启发式搜索的算法爱好者和中高级Python开发者。压缩包共3个文件全部为.py脚本大小仅10KB分别对应标准A算法、双向搜索A以及多种改进变体代码结构清晰、无冗余依赖。已有1009人学习/下载是快速上手A的轻量示例。内容从节点类定义、启发式函数如曼哈顿距离设计到开放列表/关闭列表的维护、邻居扩展与路径回溯均给出可运行的Python代码并展示了f(n)g(n)h(n)估价函数如何平衡最优性与搜索效率。通过对比不同变体还能直观看出双向搜索对路径收敛速度的改善以及各版本在不同网格环境下的适应性。将其迁移到自己的项目中可省去从零实现基础算法的时间更适合作为教学演示或原型验证的基础工具。1. 路径规划A*算法 python实现为什么是它而不是BFS或Dijkstra如果你正在做机器人导航、游戏寻路或者物流调度这类事情大概率会撞上同一个需求给定一张带障碍的地图从一个点到另一个点找一条走得通、最好还最短的路线。用广度优先搜索BFS能保证找到路径但地图一大它会把无关区域全部扫一遍用Dijkstra倒是能算最短可它完全不看终点在哪浪费的节点数同样吓人。路径规划A算法 python实现本质上就是在这两者之间找一个平衡它把“已经走掉的路程”和“估计还要走的路程”一起放进优先级搜得少还保底最优。本文面向有Python基础、想在网格地图上落地寻路模块的开发者从原理、代码、可视化到踩坑一步步把A做成能直接用的东西而不是停在跑通一个demo。2. A*核心机制拆解fgh、启发式与优先队列为什么缺一不可2.1 从“贪心已付代价”理解fgh公式里每一项是干什么的A*的核心只有一行f(n) g(n) h(n)。g(n)是起点到当前节点的实际代价也就是已经走过的路h(n)是当前节点到终点的估计代价通常叫启发式函数heuristicf(n)就是这条路径的优先级。算法每次从待探索列表里挑f最小的点往下走而不是像BFS那样按层扩展。这个公式之所以聪明在于它同时抓住了两个信息源。g项保证了算法不会只往终点方向猛冲而忽略已经付出的代价h项保证了算法大致知道终点在哪个方向不会像Dijkstra那样从起点四面八方同步扩散。你可以把A*理解为“带着方向感的Dijkstra”它把搜索预算花在最有可能通向终点的方向上。实现时要特别注意一个边界h必须满足“可采纳性”也就是h(n)永远不大于从n到终点的真实最短代价。只有满足这一点A*第一次弹出终点时返回的路径才是全局最优。如果h高估了距离算法就会为了赶路而放弃一些实际更短的绕行路径结果就是路径非最优。这一点在后面的避坑章节会专门讲。2.2 三种启发式函数曼哈顿、欧几里得、对角线距离怎么选在二维网格里启发式函数的选择直接决定了搜索效率和路径质量。最常见的三种是曼哈顿距离、欧几里得距离和对角线距离。曼哈顿距离h |dx| |dy|它假设只能横平竖直地走所以最适合四邻域移动欧几里得距离h √(dx² dy²)是直线距离哪个邻域都能用但它在网格地图里往往偏小导致探索范围偏大对角线距离h dx dy (√2 - 2) * min(dx, dy)专门为八邻域设计它把斜向移动的√2代价也算进了估计里是八邻域场景下的标准选择。选错了会怎样最典型的错误就是八邻域地图用曼哈顿距离。八邻域允许斜着走真实路径代价比曼哈顿估计值更小于是h高估A*会变成“近视眼”返回的路径可能比最优长一截。反过来四邻域地图用欧几里得距离h低估太多搜索圈会大一圈速度变慢但路径还是最优。工程实践里我一般这样定先确认移动方式四邻域用曼哈顿八邻域用对角线距离强迫症可以加个表格对比验证。启发式公式适用邻域是否可采纳搜索倾向曼哈顿dx dy四邻域是偏向折线路径欧几里得√(dx²dy²)任意是探索范围偏大对角线dxdy(√2-2)·min(dx,dy)八邻域对角代价√2是贴合八方向移动切比雪夫max(dx, dy)八邻域对角代价1仅当对角代价等于直线简化地图可用2.3 数据结构选型为什么open表用heapq、closed表用setA*性能的第二个关键在数据结构。open表要反复取出f最小的节点如果用一个普通list然后min()扫描每次取出都是O(n)地图稍微大一点就会卡到怀疑人生。正确做法是用Python自带的heapq实现最小堆插入和弹出都是O(log n)在大地图上这是数量级的差距。closed表则用set来存已经处理过的节点查找和插入都是O(1)。g值用dict保存key是坐标元组value是当前已知最小代价。这里有个Python特有的坑heapq里存的元组如果前几个元素比如f和g相等它会继续比较后面的元素如果你把自定义的Node对象放进元组而Node又没有定义__lt__程序会直接抛TypeError。业界通行做法是给元组加一个自增counter字段保证元组之间绝对不相等从而绕开对象比较问题。这个坑在避坑章节会详细展开。选型对了A*在开阔地图上的表现会非常直观从起点出发的探索范围是一个指向终点的狭长区域而Dijkstra是一个以起点为圆心的标准圆形。两者节点数差距在复杂地图上可以到几十倍这就是数据结构加启发式一起带来的收益。3. 网格地图上的A*实现从邻居扩展到主循环的完整Python代码3.1 网格建模用二维数组表达可通行区域与障碍物落地A*的第一步是定义地图。最常见的做法是用二维数组0表示可通行1表示障碍。来源可以是代码直接生成、读取CSV或者把二值图片转成数组。这里我用numpy生成一张带墙壁的测试地图读者可以照抄跑通。import numpy as np # 0可通行, 1障碍 grid np.zeros((30, 40), dtypenp.int8) grid[10:20, 15] 1 # 一堵竖墙 grid[15, 5:20] 1 # 一堵横墙和竖墙组成T型障碍 start (2, 3) # (row, col)注意行在前 goal (25, 35)numpy数组的好处是切片方便、内存紧凑dtypenp.int8足以表达0/1不浪费内存。坐标用(row, col)元组在代码里保持一致否则画图时会弄反x和y。这里还要强调一句A*本身不关心你用的是栅格还是路网栅格只是最常见的一种表达但栅格地图在高分辨率下会产生巨大的节点数量这是后面讲性能优化时要回头面对的问题。3.2 邻居扩展与移动代价四邻域和八邻域分别怎么算邻居扩展是A*里最容易写歪的部分主要涉及两个决策要不要允许斜着走以及斜走的代价是多少。四邻域只有上下左右四个方向移动代价统一为1八邻域额外加四个斜方向直线代价1对角代价√2。代价不写√2而写成1会让算法认为斜着走和横着走没区别路径质量会变差。这里有一个初学者必踩的细节八邻域斜穿墙角。从(1,1)斜走到(2,2)时如果(1,2)或(2,1)是障碍物那么这条斜线实际上穿过了墙脚真实的移动体比如AGV是过不去的。所以扩展对角线之前必须检查相邻的两个正交格子是否可通行。def neighbors(grid, x, y, allow_diagonalTrue): rows, cols grid.shape dirs [(1, 0), (-1, 0), (0, 1), (0, -1)] if allow_diagonal: dirs [(1, 1), (1, -1), (-1, 1), (-1, -1)] for dx, dy in dirs: nx, ny x dx, y dy if not (0 nx rows and 0 ny cols): continue if grid[nx][ny] 1: continue # 斜向移动时检查墙角防止穿墙 if dx ! 0 and dy ! 0: if grid[x][ny] 1 or grid[nx][y] 1: continue cost math.sqrt(2) if dx ! 0 and dy ! 0 else 1.0 yield nx, ny, cost这个生成器函数一次产出一个邻居坐标和对应的移动代价。墙体检查在障碍物判断之后顺序别反否则会有多余计算。math.sqrt(2)每次调用都重算性能敏感时可以全局定义DIAG_COST 1.41421356。allow_diagonal参数是留给做对比实验用的关掉它就能观察四邻域和八邻域的搜索范围差异。3.3 A*主循环一个能直接跑的astar()实现主循环是所有逻辑的中枢从heapq里弹出f最小的节点判断是否到达终点否则扩展邻居更新g值和父节点并把新状态推回堆里。我习惯用元组(f, g, counter, x, y)入堆这样既不用给Node类写排序方法又能在弹出时拿到全部信息。import heapq import math def astar(grid, start, goal, heuristicdiagonal, weight1.0): rows, cols grid.shape counter 0 open_heap [] # (f, g, counter, x, y) g_score {start: 0.0} came_from {} f0 weight * h_func(start, goal, heuristic) heapq.heappush(open_heap, (f0, 0.0, counter, start[0], start[1])) counter 1 expanded [] while open_heap: f, g, _, cx, cy heapq.heappop(open_heap) current (cx, cy) if current goal: return reconstruct_path(came_from, current), expanded # 跳过被重复压入堆里的旧记录 if g g_score.get(current, math.inf) 1e-9: continue expanded.append(current) for nx, ny, step_cost in neighbors(grid, cx, cy): nxt (nx, ny) tentative_g g step_cost if tentative_g g_score.get(nxt, math.inf): came_from[nxt] current g_score[nxt] tentative_g f tentative_g weight * h_func(nxt, goal, heuristic) heapq.heappush(open_heap, (f, tentative_g, counter, nx, ny)) counter 1 return None, expanded # open表空了说明根本没有路径展开说一下这个循环里的几个参数。weight是启发式权重等于1.0时A*保证最优大于1.0会更快但路径可能变差这是实际项目中提速最直接的手段。1e-9这个浮点容差必须留因为对角线代价√2和整数1相加会产生浮点误差直接比较可能让算法把同代价路径误判成更优路径。g_score.get(nxt, math.inf)第一次访问邻居时返回无穷大所以第一次发现邻居一定满足tentative_g inf会正常入堆。整个函数如果open_heap耗尽还没到goal说明起点和终点不在同一个连通区域返回None而不是空路径这样调用方才能区分“没路径”和“路径为空”两种语义。3.4 路径回溯从终点沿parent字典倒推整条路线A*只负责搜索真正把路线拼出来的是回溯这一步。在循环里每次找到更优的邻居时都用came_from[nxt] current记录它的父节点。搜索结束后从终点一路向前找父节点直到起点再把列表反转就得到了从起点到终点的完整路径。def reconstruct_path(came_from, current): path [current] while current in came_from: current came_from[current] path.append(current) path.reverse() return path注意这个函数的输入是终点坐标输出是包含起点和终点的列表。while current in came_from这个条件比while current ! start更稳妥因为理论上如果代码有bug导致起点和终点在同一连通域但came_from链断裂前者不会死循环。路径上每个元素都是(row, col)元组后续无论是画图、转成机器人运动指令还是做路径平滑都是基于这个列表。不用递归写回溯也是刻意的。栅格地图上路径可能很长递归深度超过Python默认的1000就会抛RecursionError用迭代虽然代码多一行但上限只受内存约束。这个习惯在后面对接真实机器人导航时能省不少事。4. 把搜索过程可视化用matplotlib看清节点扩展顺序和参数效果4.1 静态可视化一张图看清障碍、路径与探索范围A*跑到什么程度才算好光看返回的路径长度不够还得看它扩展了多少节点。扩展节点多说明启发式偏保守扩展节点少说明h选得准。把expanded列表画出来视觉冲击力比任何指标都直接。import matplotlib.pyplot as plt def draw_result(grid, path, expanded, start, goal): fig, ax plt.subplots(figsize(8, 8)) ax.imshow(grid.T, cmapgray_r, originlower) if expanded: xs [p[0] for p in expanded] ys [p[1] for p in expanded] ax.scatter(ys, xs, s2, clightskyblue, labelexpanded) if path: xs2 [p[0] for p in path] ys2 [p[1] for p in path] ax.plot(ys2, xs2, ccrimson, lw2, labelpath) ax.scatter([start[1]], [start[0]], cgreen, s60, markero, labelstart) ax.scatter([goal[1]], [goal[0]], corange, s60, marker*, labelgoal) ax.legend() return fig这里有个容易弄反的细节numpy数组索引是(row, col)而matplotlib的scatter和plot默认输入是(x, y)。所以画点时要把坐标反过来传入[p[1] for p in expanded]作为x[p[0] for p in expanded]作为y同时grid.T做转置保证图上的方向感和坐标轴一致。颜色上用浅蓝色表示探索区域、红色表示路径对比足够强烈。看到一张图结果时先看蓝色区域是否呈“指向终点的锥形”而不是“以起点为圆心的圆”后者说明启发式函数没起作用。4.2 动画化搜索过程用动画找出A*跑偏的原因静态图只能看到最终结果动画能让你看到搜索的先后顺序。这个“过程感”在调试启发式函数时特别有用能直观看到算法是不是在一开始就朝着错误方向猛冲或者在一个死胡同区域反复打转。from matplotlib.animation import FuncAnimation def animate_search(grid, expanded, start, goal, step10, interval30): fig, ax plt.subplots(figsize(8, 8)) ax.imshow(grid.T, cmapgray_r, originlower) scatter ax.scatter([], [], s2, clightskyblue) ax.scatter([start[1]], [start[0]], cgreen, s60, markero) ax.scatter([goal[1]], [goal[0]], corange, s60, marker*) def update(frame): data expanded[: (frame 1) * step] if data: scatter.set_offsets([[p[1], p[0]] for p in data]) return scatter, ani FuncAnimation(fig, update, frameslen(expanded) // step, intervalinterval, blitFalse) return aniFuncAnimation的update函数每次只画前N个已扩展节点看起来就是搜索范围从起点逐步向外“生长”。step控制每帧增加多少个节点地图大就调大到50地图小调5帧数等于len(expanded)/step。在Jupyter里跑动画需要先设置plt.rcParams[animation.html] jshtml或者用ani.save(a_star.gif, writerpillow)存成gif查看到效果。这里不用blitTrue省去处理动态artist的麻烦性能差一点但胜在不容易出错。4.3 参数对照实验邻域、启发式与权重w对路径的影响可视化做出来后最值得做的一件事就是跑一组参数对照让数据说话。我自己常用的对照组是固定地图分别跑四邻域曼哈顿、八邻域曼哈顿、八邻域对角线、八邻域对角线权重1.5记录四个维度的结果——路径长度、扩展节点数、运行耗时、路径是否有锯齿。试过的读者通常会得出一个结论八邻域曼哈顿在部分地图上能搜完但路径偶尔比最优长权重提到1.5之后扩展节点可能砍半代价是路径多3%到5%。这个实验最大的价值是帮你建立“量化直觉”。之后你在别的项目里再调weight就会知道0.1的变化大概会带来多少搜索量的变化而不是凭感觉调一个神秘参数。切记每个配置跑三次取平均因为Python的heapq在f相同的情况下弹出顺序不完全确定单次运行会有波动。我习惯把结果汇总成一张表横轴是配置名称纵轴是四个指标一眼就能看出取舍关系。参数实验的另一个隐藏福利是能验证算法实现有没有问题。比如某张地图上八邻域对角线距离跑出来的路径比八邻域曼哈顿更短说明后者确实高估了h如果前者的路径反而更长那就要回头检查你的对角线距离公式是不是写错了大概率是min(dx,dy)前漏了系数。5. A*实现避坑指南Python版本最容易翻车的5个细节5.1 heapq元组比较报错Node对象在堆里不能直接排序现象heapq.heappush(open_heap, (f, node))代码本身不报错但进行第二次push时报TypeError: not supported between instances of Node and Node。原因两个元组的f值和g值恰好相等时Python会继续比较元组里的第三个元素也就是Node对象而Node类没有定义__lt__。解决不用Node对象全部用(f, g, counter, x, y)元组入堆counter每次自增保证任何两个元组都不可能走到比较对象那一步。这是A* Python实现里最经典的翻车点一踩一个准。5.2 对角线穿墙八邻域扩展漏了贴墙检查现象路径图上出现一条斜线斜线两侧紧贴障碍物机器人实际根本走不过去。原因扩展对角线邻居时只查了目标格子是否可通行没查目标格子旁边的两个正交格子算法把斜穿墙角当成了合法动作。解决在neighbors函数里加墙角检查斜向移动时同时验证grid[x][ny]和grid[nx][y]是否为空任一为障碍就跳过这个方向。不做这个检查A*能搜出路径但这条路径在真实机器人项目里是废的属于隐藏最深的逻辑错误。5.3 不可达地图卡死open表弹完还是找不到终点现象程序跑很久不结束或者最后返回一个空路径调用方不知道是“没有路径”还是“路径就是空的”。原因起点和终点被障碍物完全隔开open_heap从满到空是个漫长的过程更隐蔽的原因是代码里没有跳过旧记录同一个节点被反复更新压入堆open表越来越大。解决主循环里加if g g_score.get(current, inf) 1e-9: continue跳过过期记录循环结束后返回None调用方用if path is None判断无路径。这个改动同时解决性能和语义两个问题。5.4 八邻域配曼哈顿距离能找到路径但不是最短现象路径距离终点很近时突然绕了一个不必要的弯。原因曼哈顿距离在八邻域地图里高估了真实代价破坏了A*的可采纳性算法会为了“看起来近”而牺牲真实更短的绕行方案。比如(0,0)到(2,2)曼哈顿h4实际最短路径是2√2≈2.83h比真实还大必然非最优。解决八邻域地图换用对角线距离h dx dy (√2 - 2) * min(dx, dy)。如果读代码的人看不懂这个公式就直接把heuristicdiagonal设成默认值别给曼哈顿留入口。5.5 路径频繁锯齿状搜索平局处理与运动约束现象路径长度OK但路线像锯齿一样左右摇摆直线段被拆成好几段。原因多个邻居的f值相同时heapq的弹出顺序不稳定算法会随机选择扩展方向。解决在f计算时给h乘一个非常接近1的系数比如f tentative_g h * 1.000001让远离直线的候选路径优先级略微降低路径会平滑很多。注意这个系数不能太大超过1.01就可能破坏最优性。更进阶的做法是在平局时优先选择“方向变化更小”的邻居这需要记录当前移动方向属于路径平滑的前置工作。6. 从基础A*向上走双向搜索、JPS与真实路径规划缺口6.1 双向A*起点终点一起搜的适用条件双向A的思路很直接从起点和终点同时各跑一个A当两个方向的搜索圈碰上时把两段路径拼起来。实现上需要两个open_heap、两个g_score、两个came_from每个方向使用各自到终点的启发式。它的收益主要来自搜索空间的缩减——单向搜索范围是一个大锥形双向是两个更小锥形加起来往往更小。但要注意两个坑一是终止条件不是“任一方向到达对方起点”而是两个方向的closed集合出现交集二是启发式函数在反向搜索时要从当前节点估计到原始起点。我的经验是地图越大、起点终点相距越远双向A*的收益越明显小地图上多出来的管理开销反而可能拖慢速度。6.2 JPS跳点搜索什么地图值得换算法JPSJump Point Search是网格A的一种无损加速它利用网格的对称性跳过大量等价的中间节点只在“跳点”上做决策。开阔空地越多的地图JPS相对A的加速倍数越高可以到几十倍但地图越密集、障碍越杂乱跳点越少收益就越小甚至因为跳点搜索的开销变慢。所以选型建议是大地图、障碍稀疏用JPS小地图、室内高密度障碍用普通A*加合理weight就够了。JPS本身不改变启发式函数的选型对角线距离依然适用。6.3 从栅格到真实机器人运动学、动态障碍与重规划栅格A在仿真里很好看落到真实机器人上还有三个常见缺口一是路径可能有直角急转而物理机器人有最小转弯半径常见做法是接一段轨迹平滑比如基于B样条或Dubins曲线的后处理二是地图里还有动态障碍物A只规划了一次实际执行时要用“局部重规划”兜底也就是走一段、感知一段、再规划一段三是多机器人场景要引入时间维度把“某时刻某位置是否被占用”也作为约束这就是时空A的方向。这些属于同一技术方向的自然延伸先把栅格A吃透再往这些方向追加比一上来就折腾复杂规划器靠谱得多。我自己的习惯是拿到新地图后永远先用A跑一版不带weight、不带平滑的原始结果确认路径在几何上可行、探索范围符合预期再逐步叠加优化项。多数“路径规划不好用”的抱怨最后查下来都是基础A里的小坑——没查墙角、启发式选错、过期节点没跳过。先把这层做好再谈高级算法。希望这篇笔记能帮你少踩几个坑把A*真正用起来也祝你调参顺利。本文还有配套的精品资源点击获取 
长河君 · 2026/10/15 16:43:24 · 阅读 1.2k 路径规划A*算法Python实现：原理、代码与可视化避坑指南
简介路径规划A算法的Python实现面向机器人导航、游戏角色寻路、地图路径计算等常见需求也适合正在系统学习启发式搜索的算法爱好者和中高级Python开发者。压缩包共3个文件全部为.py脚本大小仅10KB分别对应标准A算法、双向搜索A以及多种改进变体代码结构清晰、无冗余依赖。已有1009人学习/下载是快速上手A的轻量示例。内容从节点类定义、启发式函数如曼哈顿距离设计到开放列表/关闭列表的维护、邻居扩展与路径回溯均给出可运行的Python代码并展示了f(n)g(n)h(n)估价函数如何平衡最优性与搜索效率。通过对比不同变体还能直观看出双向搜索对路径收敛速度的改善以及各版本在不同网格环境下的适应性。将其迁移到自己的项目中可省去从零实现基础算法的时间更适合作为教学演示或原型验证的基础工具。1. 路径规划A*算法 python实现为什么是它而不是BFS或Dijkstra如果你正在做机器人导航、游戏寻路或者物流调度这类事情大概率会撞上同一个需求给定一张带障碍的地图从一个点到另一个点找一条走得通、最好还最短的路线。用广度优先搜索BFS能保证找到路径但地图一大它会把无关区域全部扫一遍用Dijkstra倒是能算最短可它完全不看终点在哪浪费的节点数同样吓人。路径规划A算法 python实现本质上就是在这两者之间找一个平衡它把“已经走掉的路程”和“估计还要走的路程”一起放进优先级搜得少还保底最优。本文面向有Python基础、想在网格地图上落地寻路模块的开发者从原理、代码、可视化到踩坑一步步把A做成能直接用的东西而不是停在跑通一个demo。2. A*核心机制拆解fgh、启发式与优先队列为什么缺一不可2.1 从“贪心已付代价”理解fgh公式里每一项是干什么的A*的核心只有一行f(n) g(n) h(n)。g(n)是起点到当前节点的实际代价也就是已经走过的路h(n)是当前节点到终点的估计代价通常叫启发式函数heuristicf(n)就是这条路径的优先级。算法每次从待探索列表里挑f最小的点往下走而不是像BFS那样按层扩展。这个公式之所以聪明在于它同时抓住了两个信息源。g项保证了算法不会只往终点方向猛冲而忽略已经付出的代价h项保证了算法大致知道终点在哪个方向不会像Dijkstra那样从起点四面八方同步扩散。你可以把A*理解为“带着方向感的Dijkstra”它把搜索预算花在最有可能通向终点的方向上。实现时要特别注意一个边界h必须满足“可采纳性”也就是h(n)永远不大于从n到终点的真实最短代价。只有满足这一点A*第一次弹出终点时返回的路径才是全局最优。如果h高估了距离算法就会为了赶路而放弃一些实际更短的绕行路径结果就是路径非最优。这一点在后面的避坑章节会专门讲。2.2 三种启发式函数曼哈顿、欧几里得、对角线距离怎么选在二维网格里启发式函数的选择直接决定了搜索效率和路径质量。最常见的三种是曼哈顿距离、欧几里得距离和对角线距离。曼哈顿距离h |dx| |dy|它假设只能横平竖直地走所以最适合四邻域移动欧几里得距离h √(dx² dy²)是直线距离哪个邻域都能用但它在网格地图里往往偏小导致探索范围偏大对角线距离h dx dy (√2 - 2) * min(dx, dy)专门为八邻域设计它把斜向移动的√2代价也算进了估计里是八邻域场景下的标准选择。选错了会怎样最典型的错误就是八邻域地图用曼哈顿距离。八邻域允许斜着走真实路径代价比曼哈顿估计值更小于是h高估A*会变成“近视眼”返回的路径可能比最优长一截。反过来四邻域地图用欧几里得距离h低估太多搜索圈会大一圈速度变慢但路径还是最优。工程实践里我一般这样定先确认移动方式四邻域用曼哈顿八邻域用对角线距离强迫症可以加个表格对比验证。启发式公式适用邻域是否可采纳搜索倾向曼哈顿dx dy四邻域是偏向折线路径欧几里得√(dx²dy²)任意是探索范围偏大对角线dxdy(√2-2)·min(dx,dy)八邻域对角代价√2是贴合八方向移动切比雪夫max(dx, dy)八邻域对角代价1仅当对角代价等于直线简化地图可用2.3 数据结构选型为什么open表用heapq、closed表用setA*性能的第二个关键在数据结构。open表要反复取出f最小的节点如果用一个普通list然后min()扫描每次取出都是O(n)地图稍微大一点就会卡到怀疑人生。正确做法是用Python自带的heapq实现最小堆插入和弹出都是O(log n)在大地图上这是数量级的差距。closed表则用set来存已经处理过的节点查找和插入都是O(1)。g值用dict保存key是坐标元组value是当前已知最小代价。这里有个Python特有的坑heapq里存的元组如果前几个元素比如f和g相等它会继续比较后面的元素如果你把自定义的Node对象放进元组而Node又没有定义__lt__程序会直接抛TypeError。业界通行做法是给元组加一个自增counter字段保证元组之间绝对不相等从而绕开对象比较问题。这个坑在避坑章节会详细展开。选型对了A*在开阔地图上的表现会非常直观从起点出发的探索范围是一个指向终点的狭长区域而Dijkstra是一个以起点为圆心的标准圆形。两者节点数差距在复杂地图上可以到几十倍这就是数据结构加启发式一起带来的收益。3. 网格地图上的A*实现从邻居扩展到主循环的完整Python代码3.1 网格建模用二维数组表达可通行区域与障碍物落地A*的第一步是定义地图。最常见的做法是用二维数组0表示可通行1表示障碍。来源可以是代码直接生成、读取CSV或者把二值图片转成数组。这里我用numpy生成一张带墙壁的测试地图读者可以照抄跑通。import numpy as np # 0可通行, 1障碍 grid np.zeros((30, 40), dtypenp.int8) grid[10:20, 15] 1 # 一堵竖墙 grid[15, 5:20] 1 # 一堵横墙和竖墙组成T型障碍 start (2, 3) # (row, col)注意行在前 goal (25, 35)numpy数组的好处是切片方便、内存紧凑dtypenp.int8足以表达0/1不浪费内存。坐标用(row, col)元组在代码里保持一致否则画图时会弄反x和y。这里还要强调一句A*本身不关心你用的是栅格还是路网栅格只是最常见的一种表达但栅格地图在高分辨率下会产生巨大的节点数量这是后面讲性能优化时要回头面对的问题。3.2 邻居扩展与移动代价四邻域和八邻域分别怎么算邻居扩展是A*里最容易写歪的部分主要涉及两个决策要不要允许斜着走以及斜走的代价是多少。四邻域只有上下左右四个方向移动代价统一为1八邻域额外加四个斜方向直线代价1对角代价√2。代价不写√2而写成1会让算法认为斜着走和横着走没区别路径质量会变差。这里有一个初学者必踩的细节八邻域斜穿墙角。从(1,1)斜走到(2,2)时如果(1,2)或(2,1)是障碍物那么这条斜线实际上穿过了墙脚真实的移动体比如AGV是过不去的。所以扩展对角线之前必须检查相邻的两个正交格子是否可通行。def neighbors(grid, x, y, allow_diagonalTrue): rows, cols grid.shape dirs [(1, 0), (-1, 0), (0, 1), (0, -1)] if allow_diagonal: dirs [(1, 1), (1, -1), (-1, 1), (-1, -1)] for dx, dy in dirs: nx, ny x dx, y dy if not (0 nx rows and 0 ny cols): continue if grid[nx][ny] 1: continue # 斜向移动时检查墙角防止穿墙 if dx ! 0 and dy ! 0: if grid[x][ny] 1 or grid[nx][y] 1: continue cost math.sqrt(2) if dx ! 0 and dy ! 0 else 1.0 yield nx, ny, cost这个生成器函数一次产出一个邻居坐标和对应的移动代价。墙体检查在障碍物判断之后顺序别反否则会有多余计算。math.sqrt(2)每次调用都重算性能敏感时可以全局定义DIAG_COST 1.41421356。allow_diagonal参数是留给做对比实验用的关掉它就能观察四邻域和八邻域的搜索范围差异。3.3 A*主循环一个能直接跑的astar()实现主循环是所有逻辑的中枢从heapq里弹出f最小的节点判断是否到达终点否则扩展邻居更新g值和父节点并把新状态推回堆里。我习惯用元组(f, g, counter, x, y)入堆这样既不用给Node类写排序方法又能在弹出时拿到全部信息。import heapq import math def astar(grid, start, goal, heuristicdiagonal, weight1.0): rows, cols grid.shape counter 0 open_heap [] # (f, g, counter, x, y) g_score {start: 0.0} came_from {} f0 weight * h_func(start, goal, heuristic) heapq.heappush(open_heap, (f0, 0.0, counter, start[0], start[1])) counter 1 expanded [] while open_heap: f, g, _, cx, cy heapq.heappop(open_heap) current (cx, cy) if current goal: return reconstruct_path(came_from, current), expanded # 跳过被重复压入堆里的旧记录 if g g_score.get(current, math.inf) 1e-9: continue expanded.append(current) for nx, ny, step_cost in neighbors(grid, cx, cy): nxt (nx, ny) tentative_g g step_cost if tentative_g g_score.get(nxt, math.inf): came_from[nxt] current g_score[nxt] tentative_g f tentative_g weight * h_func(nxt, goal, heuristic) heapq.heappush(open_heap, (f, tentative_g, counter, nx, ny)) counter 1 return None, expanded # open表空了说明根本没有路径展开说一下这个循环里的几个参数。weight是启发式权重等于1.0时A*保证最优大于1.0会更快但路径可能变差这是实际项目中提速最直接的手段。1e-9这个浮点容差必须留因为对角线代价√2和整数1相加会产生浮点误差直接比较可能让算法把同代价路径误判成更优路径。g_score.get(nxt, math.inf)第一次访问邻居时返回无穷大所以第一次发现邻居一定满足tentative_g inf会正常入堆。整个函数如果open_heap耗尽还没到goal说明起点和终点不在同一个连通区域返回None而不是空路径这样调用方才能区分“没路径”和“路径为空”两种语义。3.4 路径回溯从终点沿parent字典倒推整条路线A*只负责搜索真正把路线拼出来的是回溯这一步。在循环里每次找到更优的邻居时都用came_from[nxt] current记录它的父节点。搜索结束后从终点一路向前找父节点直到起点再把列表反转就得到了从起点到终点的完整路径。def reconstruct_path(came_from, current): path [current] while current in came_from: current came_from[current] path.append(current) path.reverse() return path注意这个函数的输入是终点坐标输出是包含起点和终点的列表。while current in came_from这个条件比while current ! start更稳妥因为理论上如果代码有bug导致起点和终点在同一连通域但came_from链断裂前者不会死循环。路径上每个元素都是(row, col)元组后续无论是画图、转成机器人运动指令还是做路径平滑都是基于这个列表。不用递归写回溯也是刻意的。栅格地图上路径可能很长递归深度超过Python默认的1000就会抛RecursionError用迭代虽然代码多一行但上限只受内存约束。这个习惯在后面对接真实机器人导航时能省不少事。4. 把搜索过程可视化用matplotlib看清节点扩展顺序和参数效果4.1 静态可视化一张图看清障碍、路径与探索范围A*跑到什么程度才算好光看返回的路径长度不够还得看它扩展了多少节点。扩展节点多说明启发式偏保守扩展节点少说明h选得准。把expanded列表画出来视觉冲击力比任何指标都直接。import matplotlib.pyplot as plt def draw_result(grid, path, expanded, start, goal): fig, ax plt.subplots(figsize(8, 8)) ax.imshow(grid.T, cmapgray_r, originlower) if expanded: xs [p[0] for p in expanded] ys [p[1] for p in expanded] ax.scatter(ys, xs, s2, clightskyblue, labelexpanded) if path: xs2 [p[0] for p in path] ys2 [p[1] for p in path] ax.plot(ys2, xs2, ccrimson, lw2, labelpath) ax.scatter([start[1]], [start[0]], cgreen, s60, markero, labelstart) ax.scatter([goal[1]], [goal[0]], corange, s60, marker*, labelgoal) ax.legend() return fig这里有个容易弄反的细节numpy数组索引是(row, col)而matplotlib的scatter和plot默认输入是(x, y)。所以画点时要把坐标反过来传入[p[1] for p in expanded]作为x[p[0] for p in expanded]作为y同时grid.T做转置保证图上的方向感和坐标轴一致。颜色上用浅蓝色表示探索区域、红色表示路径对比足够强烈。看到一张图结果时先看蓝色区域是否呈“指向终点的锥形”而不是“以起点为圆心的圆”后者说明启发式函数没起作用。4.2 动画化搜索过程用动画找出A*跑偏的原因静态图只能看到最终结果动画能让你看到搜索的先后顺序。这个“过程感”在调试启发式函数时特别有用能直观看到算法是不是在一开始就朝着错误方向猛冲或者在一个死胡同区域反复打转。from matplotlib.animation import FuncAnimation def animate_search(grid, expanded, start, goal, step10, interval30): fig, ax plt.subplots(figsize(8, 8)) ax.imshow(grid.T, cmapgray_r, originlower) scatter ax.scatter([], [], s2, clightskyblue) ax.scatter([start[1]], [start[0]], cgreen, s60, markero) ax.scatter([goal[1]], [goal[0]], corange, s60, marker*) def update(frame): data expanded[: (frame 1) * step] if data: scatter.set_offsets([[p[1], p[0]] for p in data]) return scatter, ani FuncAnimation(fig, update, frameslen(expanded) // step, intervalinterval, blitFalse) return aniFuncAnimation的update函数每次只画前N个已扩展节点看起来就是搜索范围从起点逐步向外“生长”。step控制每帧增加多少个节点地图大就调大到50地图小调5帧数等于len(expanded)/step。在Jupyter里跑动画需要先设置plt.rcParams[animation.html] jshtml或者用ani.save(a_star.gif, writerpillow)存成gif查看到效果。这里不用blitTrue省去处理动态artist的麻烦性能差一点但胜在不容易出错。4.3 参数对照实验邻域、启发式与权重w对路径的影响可视化做出来后最值得做的一件事就是跑一组参数对照让数据说话。我自己常用的对照组是固定地图分别跑四邻域曼哈顿、八邻域曼哈顿、八邻域对角线、八邻域对角线权重1.5记录四个维度的结果——路径长度、扩展节点数、运行耗时、路径是否有锯齿。试过的读者通常会得出一个结论八邻域曼哈顿在部分地图上能搜完但路径偶尔比最优长权重提到1.5之后扩展节点可能砍半代价是路径多3%到5%。这个实验最大的价值是帮你建立“量化直觉”。之后你在别的项目里再调weight就会知道0.1的变化大概会带来多少搜索量的变化而不是凭感觉调一个神秘参数。切记每个配置跑三次取平均因为Python的heapq在f相同的情况下弹出顺序不完全确定单次运行会有波动。我习惯把结果汇总成一张表横轴是配置名称纵轴是四个指标一眼就能看出取舍关系。参数实验的另一个隐藏福利是能验证算法实现有没有问题。比如某张地图上八邻域对角线距离跑出来的路径比八邻域曼哈顿更短说明后者确实高估了h如果前者的路径反而更长那就要回头检查你的对角线距离公式是不是写错了大概率是min(dx,dy)前漏了系数。5. A*实现避坑指南Python版本最容易翻车的5个细节5.1 heapq元组比较报错Node对象在堆里不能直接排序现象heapq.heappush(open_heap, (f, node))代码本身不报错但进行第二次push时报TypeError: not supported between instances of Node and Node。原因两个元组的f值和g值恰好相等时Python会继续比较元组里的第三个元素也就是Node对象而Node类没有定义__lt__。解决不用Node对象全部用(f, g, counter, x, y)元组入堆counter每次自增保证任何两个元组都不可能走到比较对象那一步。这是A* Python实现里最经典的翻车点一踩一个准。5.2 对角线穿墙八邻域扩展漏了贴墙检查现象路径图上出现一条斜线斜线两侧紧贴障碍物机器人实际根本走不过去。原因扩展对角线邻居时只查了目标格子是否可通行没查目标格子旁边的两个正交格子算法把斜穿墙角当成了合法动作。解决在neighbors函数里加墙角检查斜向移动时同时验证grid[x][ny]和grid[nx][y]是否为空任一为障碍就跳过这个方向。不做这个检查A*能搜出路径但这条路径在真实机器人项目里是废的属于隐藏最深的逻辑错误。5.3 不可达地图卡死open表弹完还是找不到终点现象程序跑很久不结束或者最后返回一个空路径调用方不知道是“没有路径”还是“路径就是空的”。原因起点和终点被障碍物完全隔开open_heap从满到空是个漫长的过程更隐蔽的原因是代码里没有跳过旧记录同一个节点被反复更新压入堆open表越来越大。解决主循环里加if g g_score.get(current, inf) 1e-9: continue跳过过期记录循环结束后返回None调用方用if path is None判断无路径。这个改动同时解决性能和语义两个问题。5.4 八邻域配曼哈顿距离能找到路径但不是最短现象路径距离终点很近时突然绕了一个不必要的弯。原因曼哈顿距离在八邻域地图里高估了真实代价破坏了A*的可采纳性算法会为了“看起来近”而牺牲真实更短的绕行方案。比如(0,0)到(2,2)曼哈顿h4实际最短路径是2√2≈2.83h比真实还大必然非最优。解决八邻域地图换用对角线距离h dx dy (√2 - 2) * min(dx, dy)。如果读代码的人看不懂这个公式就直接把heuristicdiagonal设成默认值别给曼哈顿留入口。5.5 路径频繁锯齿状搜索平局处理与运动约束现象路径长度OK但路线像锯齿一样左右摇摆直线段被拆成好几段。原因多个邻居的f值相同时heapq的弹出顺序不稳定算法会随机选择扩展方向。解决在f计算时给h乘一个非常接近1的系数比如f tentative_g h * 1.000001让远离直线的候选路径优先级略微降低路径会平滑很多。注意这个系数不能太大超过1.01就可能破坏最优性。更进阶的做法是在平局时优先选择“方向变化更小”的邻居这需要记录当前移动方向属于路径平滑的前置工作。6. 从基础A*向上走双向搜索、JPS与真实路径规划缺口6.1 双向A*起点终点一起搜的适用条件双向A的思路很直接从起点和终点同时各跑一个A当两个方向的搜索圈碰上时把两段路径拼起来。实现上需要两个open_heap、两个g_score、两个came_from每个方向使用各自到终点的启发式。它的收益主要来自搜索空间的缩减——单向搜索范围是一个大锥形双向是两个更小锥形加起来往往更小。但要注意两个坑一是终止条件不是“任一方向到达对方起点”而是两个方向的closed集合出现交集二是启发式函数在反向搜索时要从当前节点估计到原始起点。我的经验是地图越大、起点终点相距越远双向A*的收益越明显小地图上多出来的管理开销反而可能拖慢速度。6.2 JPS跳点搜索什么地图值得换算法JPSJump Point Search是网格A的一种无损加速它利用网格的对称性跳过大量等价的中间节点只在“跳点”上做决策。开阔空地越多的地图JPS相对A的加速倍数越高可以到几十倍但地图越密集、障碍越杂乱跳点越少收益就越小甚至因为跳点搜索的开销变慢。所以选型建议是大地图、障碍稀疏用JPS小地图、室内高密度障碍用普通A*加合理weight就够了。JPS本身不改变启发式函数的选型对角线距离依然适用。6.3 从栅格到真实机器人运动学、动态障碍与重规划栅格A在仿真里很好看落到真实机器人上还有三个常见缺口一是路径可能有直角急转而物理机器人有最小转弯半径常见做法是接一段轨迹平滑比如基于B样条或Dubins曲线的后处理二是地图里还有动态障碍物A只规划了一次实际执行时要用“局部重规划”兜底也就是走一段、感知一段、再规划一段三是多机器人场景要引入时间维度把“某时刻某位置是否被占用”也作为约束这就是时空A的方向。这些属于同一技术方向的自然延伸先把栅格A吃透再往这些方向追加比一上来就折腾复杂规划器靠谱得多。我自己的习惯是拿到新地图后永远先用A跑一版不带weight、不带平滑的原始结果确认路径在几何上可行、探索范围符合预期再逐步叠加优化项。多数“路径规划不好用”的抱怨最后查下来都是基础A里的小坑——没查墙角、启发式选错、过期节点没跳过。先把这层做好再谈高级算法。希望这篇笔记能帮你少踩几个坑把A*真正用起来也祝你调参顺利。本文还有配套的精品资源点击获取