简介面向路径规划、机器人导航与游戏AI开发学习者这份压缩包是与A*算法博文配套的MATLAB附件内含两个带详细注释的.m源码文件完整实现图构建、启发式函数、优先级队列、节点扩展与路径回溯等核心流程并支持在MATLAB中直接运行。另附1个wav文件与路径规划算法无直接关联可作为干扰项忽略。压缩包共3个文件整体约45MB已有12886人学习下载。两个主程序覆盖不同地图或示例场景通过输入起点、终点与障碍物环境即可观察搜索过程并得到最短路径。代码注释清晰适合入门者逐段理解f(n)g(n)h(n)、开放列表维护和启发函数设计对于课程设计、毕业设计或小型机器人项目也能作为实用模板直接复用。搭配博文阅读还能快速掌握从环境建模到可视化输出的完整调试方法。对希望快速上手A*算法、节省编码时间的读者这份带注释的MATLAB源码兼具教学与工程参考价值是一份少走弯路的实用资源。1. A*算法路径规划这个博文附件包到底值不值得你花一下午跑通做室内AGV导航、写游戏NPC寻路或者调一台扫地机器人入门的第一个拦路算法几乎都是 A算法路径规划也就是标题里的 A 算法。这句话听起来简单但真正能跑出最短路径、切到大地图还不卡死的人并不多。估价函数、开放列表、路径重建这些点单独看都能懂连在一起就全是细节。标题里这个博文附件包刚好能把这套流程收敛成一个可以动手的示例工程。这篇笔记先讲清楚 A的理论边界和选型理由再给一套最小可运行实现最后盘五个我反复踩过、也常看别人踩的坑。读完你能自己改网格大小、换启发函数、调搜索权重也能一眼看出某条路径为什么绕远、算法为什么翻车。适合刚接触路径规划的学生、转做导航开发的工程师以及看完博文准备动手验证的读者。2. 先把 A* 的原理钉死估价函数、开闭列表和那条最优性边界2.1 估价函数 f(n)g(n)h(n)A* 与 Dijkstra 的核心差别A* 搜索的骨架不复杂从起点开始维护一张带代价的节点表每次从中挑代价最小的节点展开直到展开到终点。它区别于 Dijkstra、宽度优先这类盲目搜索的关键在估价函数 f(n)g(n)h(n)。g(n) 是从起点走到当前节点 n 的真实最小代价h(n) 是从 n 到终点的估计代价f(n) 是这条路的“预算”。Dijkstra 可以看作 h(n)≡0 的特例没有方向感会像水波一样朝四面八方扩散A* 因为加了 h搜索优先朝终点方向收敛。这个差异在 100×100 栅格上不明显在 1000×1000 的开阔地图上Dijkstra 可能要扩展几万个节点A* 往往只需扩展千级别。h(n) 的选择直接决定 A* 能不能保证最优。教科书条件是“可采纳性”h(n) 不超过 n 到终点的真实代价此时 A* 保证找到最短路径。更强一点的是“一致性”h(n) 不大于从 n 移动到邻节点 n 的代价加 h(n)一致性保证任何节点一旦被闭集合收纳就不会再被更优代价重新打开。工程里没人天天背定理但这两个性质对应两个能落地的检查第一h 是否偏大偏大就是牺牲最优性换速度第二闭集合跳过的节点是否真的不会再被更新。我习惯在代码里留一行注释h 只要不“乐观过头”A* 就不会翻车。def f_score(g, h): return g h def check_admissible(h_est, true_cost): # h_est 比真实代价还大说明估价过高路径可能不是最短 return h_est true_cost这个片段是把“可采纳性”变成一句能直接调用的断言。实战里你不会真去算 true_cost但要记住曼哈顿距离在四方向栅格里正好等于真实代价安全把 h 乘上 1.5 后安全性就被打破了路径变直变快但可能绕远。第 5 章我会讲这个权重到底怎么调。另一个容易忽略的点是 g 值必须记录真实已经付出的代价值不能只记录跳数。如果走一步的代价随地形变化比如草地 2、水泥路 1g 的初始化、累加和比较都要用浮点而不是整数。很多人把栅格地图默认成等权地图一换到带权重地图就懵。A* 在带权栅格和等权栅格上的实现几乎没有区别唯一要改的就是 move_cost 从常数变成从地形表里查值。2.2 四种启发函数选型曼哈顿、欧几里得、对角线距离与零启发启发函数是 A* 里最值得调的部分。我在栅格地图上只从四种里选曼哈顿、欧几里得、对角线距离Octile和零启发。前三个对应不同移动模型零启发留给对照实验。曼哈顿距离 h|dx||dy| 是四方向移动模型的标准选择。四方向栅格里任意两格的真实最短路径长度就是曼哈顿距离的整数倍它可采纳且紧凑搜索效率高。缺点在允许斜向移动的八方向模型里会系统性高估真实代价——走对角线一步是 √2曼哈顿却把这一步算成 2。欧几里得距离 hsqrt(dx²dy²) 最直觉但有两个工程问题一是开方计算慢节点一多就吃亏二是在拥挤地图上严重低估真实代价扩展范围大一圈。在八方向模型里它可采纳但不够紧属于“能用但不推荐”。对角线距离也叫 Octile 距离公式是 max(dx,dy)(√2−1)min(dx,dy)。它精确贴合八方向移动模型是八方向栅格地图最推荐的启发函数。零启发把 h 强制设成 0A退化成 Dijkstra用来验证最优性。移动模型推荐启发公式最优性搜索范围四方向曼哈顿dx八方向Octilemax(dx,dy)(√2−1)*min(dx,dy)可采纳且紧小八方向欧几里得sqrt(dx²dy²)可采纳但偏松中到大对照零启发0等价 Dijkstra大选型上最大的坑是拿曼哈顿跑八方向。表现是路径偶尔比最短路线长一点但大多数时候能跑通你很难说清哪里错了。正确排查方法是换成 Octile 再对比路径总代价立刻见分晓。提示换成非一致性启发之后闭集合不能只凭“展开过”来跳过。常见做法是把 f、g、前驱节点存成结构化状态发现更优 g 时重新入堆。3. 把附件包变成能跑的工程最小 A* 实现与可视化3.1 拿到附件包后的第一步解压、确认地图数据、搭运行环境我拿到算法类附件包的固定做法是先解压再找入口和地图数据最后运行。解压命令unzip A算法路径规划博文附件1.zip -d ./path_planning_demo cd ./path_planning_demo ls -la解压后我不会急着双击某个脚本先确认三件事地图是二维数组还是 CSV、起点终点在哪、主搜索函数接收哪些参数。不同博文的打包习惯差别很大有的把地图写死在脚本里有的外挂 CSV还有的带界面。附件包形态无所谓关键看地图长宽和障碍标记常见是 0 可通行、1 障碍。我一般先用一行 Python 检查地图的最大最小值确认只有 0 和 1否则可视化时灰度会乱掉。地图尺寸不必写死代码里动态读 len(grid) 和 len(grid[0]) 就可以这样换图不用改代码。地图数据确认完还有一个容易被忽略的点地图原点在左上还是左下。很多栅格数据来自图像坐标系原点在左上机器人导航常用左下。我的处理是 3.3 画图时明确设 originlower并把数据坐标统一成 (x,y) 而不是 (y,x)省得后面调方位角时两个坐标系打架。运行环境上这份实现只需要标准库 heapq 和 math可视化才需要 numpy 和 matplotlib。为了让你今天就能跑通3.2 给出的实现把地图、搜索、可视化全部放在一个 Python 文件里先跑通再拆分。3.2 核心搜索代码栅格地图、八方向邻居和路径重建下面代码是从这类博文附件包里整理出的最小可运行版本。它用 heapq 维护开放列表用 dict 维护 g 值和前驱地图用二维列表表示x 表示列、y 表示行import heapq import math def heuristic(a, b, modeoctile): 启发函数a、b 是 (x, y)mode 支持 manhattan / euclidean / octile / zero dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) if mode manhattan: return dx dy if mode euclidean: return math.sqrt(dx * dx dy * dy) if mode octile: return max(dx, dy) (math.sqrt(2) - 1) * min(dx, dy) return 0.0 def neighbors(grid, node, allow_diagonalTrue): 返回当前节点的可行邻居附带边界、障碍和斜角检测 h len(grid) w len(grid[0]) x, y node result [] for dx in (-1, 0, 1): for dy in (-1, 0, 1): if dx 0 and dy 0: continue nx, ny x dx, y dy if nx 0 or nx w or ny 0 or ny h: continue if grid[ny][nx] 1: continue if allow_diagonal and dx ! 0 and dy ! 0: if grid[ny][x] 1 or grid[y][nx] 1: continue result.append((nx, ny)) return result def astar(grid, start, goal, allow_diagonalTrue, modeoctile): A* 主函数返回路径点列表找不到返回 None open_heap [] heapq.heappush(open_heap, (0, start)) came_from {} g_score {start: 0.0} closed set() while open_heap: _, current heapq.heappop(open_heap) if current in closed: continue if current goal: path [] while current in came_from: path.append(current) current came_from[current] path.append(start) return path[::-1] closed.add(current) for nxt in neighbors(grid, current, allow_diagonal): move_cost 1.0 if allow_diagonal and nxt[0] ! current[0] and nxt[1] ! current[1]: move_cost math.sqrt(2) tentative_g g_score[current] move_cost if nxt in closed: continue if tentative_g g_score.get(nxt, float(inf)): came_from[nxt] current g_score[nxt] tentative_g f tentative_g heuristic(nxt, goal, mode) heapq.heappush(open_heap, (f, nxt)) return None这段代码对应 A* 的标准骨架。开放列表用堆堆顶永远是 f 值最小的节点这是 A* 每轮只扩展一个节点的来源closed 集合记已展开节点防止回头。move_cost 是关键参数直行一格 1.0斜行一格 √2。如果统一写成 1.0算法会严重偏向斜线路径会“抄近道”。mode 参数直接透传给 heuristic在主函数里换成 manhattan、euclidean、zero 就能对比同一张地图的搜索差异。allow_diagonal 控制是否允许斜向移动四方向场景把它设成 False 并配合 manhattan八方向场景设成 True 并配合 octile。这套参数组合正好对应 2.2 节的选型表。3.3 可视化验证把路径画回地图上代码跑通后光看打印的路径点不够我会把结果叠到原地图上确认没有穿墙绕路。先拿小地图做冒烟测试if __name__ __main__: grid [ [0, 0, 0, 0, 1, 0, 0, 0], [0, 1, 1, 0, 1, 0, 1, 0], [0, 0, 0, 0, 0, 0, 1, 0], [0, 1, 1, 1, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 1, 0], [0, 0, 1, 0, 0, 0, 0, 0], ] start (0, 0) goal (7, 5) path astar(grid, start, goal, allow_diagonalTrue, modeoctile) print(path length:, len(path) if path else -1) import matplotlib.pyplot as plt import numpy as np arr np.array(grid, dtypefloat) for (x, y) in path or []: arr[y][x] 0.5 plt.imshow(arr, cmapgray_r, originlower) plt.colorbar() plt.show()这里有个容易看懵的索引细节地图是行优先二维列表访问时写 grid[y][x]路径点是 (x, y)。画图时 plt.imshow 接收的数组同样按 arr[y][x] 取origin 设为 lower 让 (0,0) 显示在左下角和常见坐标系习惯对齐。如果打印的路径长度明显不符合预期或者走位异常先怀疑的不是搜索逻辑而是地图行列顺序和障碍物位置。把障碍格改成 2、路径改成 0.5图上一眼能看出哪一步错了。这也是我建议在这个附件包上做一次可视化验证的原因——黑匣子跑通不算跑通眼睛看着路径走完才算。4. A* 路径规划避坑指南5 个我反复踩过的老坑下面这五个问题按“现象→原因→解决”展开几乎覆盖我在 A* 上遇到过的九成翻车现场。这些坑的顺序也有讲究前两个坑直接决定你对一个算法实现“能不能用”的判断中间两个决定它“跑得快不快、错得隐蔽不隐蔽”最后一个决定“机器人到底走不走得动”。遇到问题别先去翻算法细节先对照现象找原因。4.1 启发函数不可采纳路径看着对代价却不是最优现象算法能跑通路径也看不出明显问题但和 Dijkstra 一比总代价多 5%~15%。原因启发函数高估了剩余代价常见于用曼哈顿距离跑八方向或者给 h 乘了大于 1 的权重。解决先把 mode 换成 octile 再跑一次如果代价收敛到和 Dijkstra 一致问题就出在启发函数选型。要量化验证时把 h 设成 0 跑一遍当基准值。4.2 对角线移动代价给错斜着走反而“开挂”现象路径明显偏爱斜线绕墙走也很积极。原因move_cost 在斜向时仍设成 1.0斜向机会成本失真。解决斜向代价必须设成 √2。自检办法是打印 g_score[goal]如果路径有 10 条边而总代价只有 9.0基本就是斜向代价没设对。顺带多说一句这个坑在带权重地图上更隐蔽因为代价表一旦变化√2 很容易被顺手写成 1调试时先检查移动成本表而不是算法逻辑。4.3 开放列表重复入队地图一大就慢成幻灯片现象几十格地图秒出几百格地图卡到怀疑人生。原因同一节点被多次发现更优 g每次都被压进堆里旧堆条目大量堆积弹出后又需要判断造成无意义循环。解决堆元素改成 (f, g, node) 的版本记录弹出时比对堆里的 g 和当前 g_score不一致就丢弃# 入堆时把 g 也放进去 heapq.heappush(open_heap, (f_value, tentative_g, nxt)) # 弹出时校验版本 while open_heap: f_pop, g_pop, node heapq.heappop(open_heap) if g_pop ! g_score.get(node): continue # g 值已经过时丢弃这条记录这个方案把“过期”这件事显式写进堆数据里比单纯检查 closed 更稳健。浮点比较不会出问题因为 g_pop 和 g_score 里存的是同一个 tentative_g 值。如果用 3.2 的第一版示例代码保持一致启发octile 或 manhattan 四方向时闭集合跳过的写法没问题一旦换非一致启发建议换成本节的版本校验。4.4 行列索引写反地图数据错位算法却不报错现象路径穿过了标注为 1 的障碍格或者起点终点和地图对不上。原因grid 是行优先grid[y][x] 被写成了 grid[x][y]。栅格是方形时不报错路径也能勉强走地图变成 12×8 的长方形后点位全部错位。解决统一用 (x, y) 表示坐标访问二维列表强制 grid[y][x]并在邻居入口加断言0≤xw 且 0≤yh。我自己的习惯是写一个 map_at 函数所有障碍判断都走它从源头灭掉手滑。4.5 斜穿障碍物角点路径贴墙执行机构根本走不过去现象路径起点终点都在空地中间有斜线贴着障碍物墙角画图几乎看不出问题机器人一执行就撞墙。原因邻居生成时只检查了目标格是否可行没检查斜向移动经过的另外两个正交格。解决在 neighbors 里对斜向移动加前置检查grid[ny][x] 和 grid[y][nx] 必须都是 0。3.2 的代码已经带上这道检查如果你是从老代码里扒的 A*这一行大概率是缺的。注意这道检查会牺牲一部分斜向通行能力但换来路径可实际执行。对差速底盘和四轮底盘来说贴墙角斜穿本来就是不可能完成的动作宁可多绕两步也别让它撞墙。5. 进阶双向 A*、JPS 跳点搜索与权重参数 w 的调法基础 A* 跑通之后再往前的三个方向是双向 A*、JPS 和权重调参。双向 A* 从起点和终点各开一个队列每次扩展 f 更小的一侧两边搜索树相遇时拼接路径。它不改变最优性但能把开阔地图上的扩展节点数再压掉约一半。我在大地图上优先试双向 A*因为改动最小。JPSJump Point Search对规则栅格更有杀伤力。它利用均匀网格的对称性只在“跳点”上做扩展普通中间节点直接跳过。开阔地图上 JPS 比 A* 快一个数量级但它依赖规则栅格和一致的移动代价不适合路网或代价不连续的图。在附件包验证完 A* 之后可以沿这条线继续深入。权重 w 是最快见效的调参维度。把估价函数改成 f g w * hw1 时搜索更快但不再保证最短。改动只有一行f tentative_g w * heuristic(nxt, goal, mode)调参节奏是w 在 1.1~2.0 之间路径只是轻微次优扩展节点能少 30% 以上w 超过 2.5 后路径开始肉眼可见地绕远。对差速底盘这类执行连贯性要求高的场景我会把 w 控制在 1.2~1.5同时保留事后平滑对只需要点到点的离散寻路w 调到 1.5~2.0 换速度更划算。最后留一个验证习惯每次改启发函数或权重都用同一张地图跑三组对比——零启发Dijkstra、当前设置、调优后的设置把路径总代价和扩展节点数打出来。我早年吃过一次哑巴亏为了省时间只看“路径能走”结果核心参数调反了方向还在沾沾自喜从那以后这个对比再没断过。希望帮到你。本文还有配套的精品资源点击获取