无人机要穿过一片楼宇密集的城区机械臂要从堆满零件的料筐里取出一只螺丝无人车要在立体车库中规划一条不会碰壁的上楼路线——这些任务背后有一个共同的计算核心在三维空间里找出一条从起点到终点、避开所有障碍物的通路。这就是3D路径规划而其中最经典、最适合入门的图搜索算法就是A*。这篇文章是《Python运动规划库》系列教程中关于3D图搜索的部分。我不会去调OMPL、MoveIt这些现成的规划库而是纯手写一个基于numpy的3D A搜索器把地图建模、邻居生成、启发式函数、主搜索循环、3D可视化整个走通最后再聊一聊生产环境里真正会踩到的坑。不管你是刚接触运动规划的学生还是准备把自主导航塞进自己项目的开发者照着这篇文章的思路写一遍后续再看DLite、JPS这些高级算法都会顺很多。1. 3D路径规划的问题建模与算法选型1.1 为什么二维规划解决不了这些问题先说个很直接的场景。一架无人机在城市里执行巡检任务面前横着一座50层高的写字楼。二维栅格地图只能告诉你这栋楼的“占地轮廓”是哪里但无人机明明可以从300米高度直接飞过去这栋楼根本挡不住它。如果硬要用2D地图去规划路径会绕出一大圈甚至在某些狭窄街区间直接判定“无路可走”。这不是算法的问题是地图维度压根不够。类似的场景还有很多。仓储AGV在货架间穿行地面上横着一根消防管道AGV底盘有20厘米离地间隙它其实可以直接从管道上方碾过去但2D地图会把它当成一个不可穿越的障碍物。机械臂的操作空间天然就是三维的虽然真正的机械臂规划是在六维关节空间里做的但当末端执行器需要绕过工作台、夹具这些固定障碍时很多工程师第一步仍然会把问题投影到三维笛卡尔空间先用3D路径搜索找一条参考路径再交给逆运动学去细化。所以结论很清楚只要工作空间里存在“高度维度带来的自由度”2D规划就会产生误判。3D图搜索解决的不只是“地图多了一个维度”而是把“能不能走”的判断从平面扩展到了立体空间。1.2 3D空间在计算机里怎么表示做3D规划之前先得解决“空间怎么存”的问题。目前主流有三种表示方式各有各的适用场景。**体素栅格Voxel Grid**是最直观的一种。直接把三维空间切成一格一格的小立方体用三维数组存0代表可通行1代表被障碍物占据。我在这篇文章里用的就是这种。它的优点非常明显随机访问是O(1)A*的邻居扩展天然就是数组下标加减写起来几乎不费脑筋。缺点是分辨率跟内存呈立方级增长100米×100米×50米的空间用1米分辨率也就50万个格子但换成0.1米分辨率就是5亿个格子内存直接飙到500MB起步。**八叉树OctoMap**是对体素栅格的改良空白区域用大节点表示只在障碍物附近细分。在SLAM建图结果上做规划时八叉树的内存效率比均匀栅格高得多但代价是邻居节点查询要走树结构代码量明显更大。**点云Point Cloud**是激光雷达的直接输出信息量最大但不适合直接作为A*的输入一般会先做体素化或者转成八叉树才能用。我在这篇文章里选择均匀体素栅格原因很简单这是理解一切三维搜索的地基。等你把栅格A*吃透了以后换到八叉树只是换一个邻居查询接口搜索框架是完全一样的。一上来就搞复杂的数据结构容易把核心算法淹没在无关细节里。1.3 为什么选A*而不是Dijkstra或RRT每次讲路径规划都会有读者问同一个问题为什么要用A*Dijkstra不是也能找最短路径吗RRT不是更适合高维空间吗Dijkstra确实保证最优解但它的搜索方式像水波一样向着四周均匀扩散在3D网格里这个“水波”的体积大得吓人。从起点出发它会把所有比终点路径代价小的节点都扩展一遍在100×100×100的地图里这意味着可能要探测上百万个节点纯Python循环跑下来基本没法用。贪心最佳优先搜索只盯着启发式h来判断方向确实快但容易被局部障碍物误导可能绕一大圈远路极端情况下甚至找不到路径。A把两者结合起来g是已经付出的代价h是到终点的预估代价两者相加每一次都优先扩展“最有希望通向目标”的节点。在静态栅格地图上A是“既能保证最优、又兼顾效率”的最优解。至于RRT它用在连续高维空间里确实很香比如六自由度机械臂的关节空间规划那里面没有现成的栅格可用。但如果我们手上已经有一张体素化的3D地图图搜索的路径质量、确定性和复现性都优于RRT。RRT给出的路径是随机采样出来的折线往往还需要额外的平滑处理。一句话总结A*是静态栅格地图上的默认解也是后续所有高级图搜索算法的起点。2. A*算法核心原理f、g、h是怎么协同工作的2.1 从Dijkstra到A*多出来的“启发式”A*的核心公式短得可以写在一张便签上f(n) g(n) h(n)其中g(n)从起点到当前节点n已经花掉的实际代价。h(n)从当前节点n到终点还需要花掉的代价的估计值。f(n)经过节点n这条路从起点到终点的总代价估计。打个比方。你在一个陌生城市里从城南去城北g是你已经走过的公里数h是你根据地图上两点直线距离估算的“还差多少公里”f就是“走你当前这条路全程大概多少公里”。正常人都会优先尝试那些“走得不算远、离终点也不远”的路线而不是一条已经绕了30公里、据说只剩2公里的岔路。A*每一步做的事情就是从待扩展列表open list里取出f值最小的节点扩展它更新邻居的g值再把邻居放进待扩展列表。重复这个过程直到弹出终点节点或者待扩展列表耗尽。2.2 为什么h必须是“不乐观”的A*能保证找到最优路径有一个非常关键的前提h(n)必须小于等于从n到终点的真实最短距离。这个性质在算法导论里叫可采纳性Admissible。如果h高估了剩余代价会发生什么假设最优路径上某个节点的真实代价是100但你把h估成了200它的f值被抬高到300排到了另一个真实代价是150的路径后面。搜索就会先走那条看起来“近”但实际上不是最短的路线最终得到的路径就不是全局最优的了。如果h恰好等于真实代价那A*的效率会达到理论最高——它几乎直奔目标每个节点只扩展一次。但现实里h等于真实代价意味着你已经提前知道了最短路径这通常是不可能的。如果h等于0A*就退化成了Dijkstra保证最优但速度最慢。所以设计启发式函数的策略很清晰**在保证不高于真实代价的前提下让h尽量贴近真实值。**这也是为什么启发式选型在3D搜索里如此重要的原因。2.3 open list和closed list到底在干嘛很多教材把A*描述成维护两个表open list放待扩展节点closed list放已经扩展完的节点。理论上没错但工程实现上我习惯的做法是用一个字典记录当前已知的最小g值外加一个二叉堆heap来维护待扩展节点不显式维护closed list。流程是这样的把起点塞进堆g(start) 0。从堆里弹出f值最小的节点current。如果堆里记录的这条g值大于当前已知的g_score[current]说明这是一条过期记录直接跳过。扩展current的所有邻居计算新的g值比已知的更小就更新parent指针并推入堆。重复2-4直到堆空或者弹出goal节点。这里允许同一个节点多次入堆初看有点浪费但实际是聪明的做法。因为二叉堆从中间删除一个节点的复杂度是O(N)而重复入堆只是多存一条记录堆弹出的复杂度始终是O(log m)。空间上多花了一点时间上却避免了最麻烦的堆内删除操作。这个细节笔试面试和工程实践里都特别常见。3. 3D图搜索的邻居定义、移动代价与启发式函数3.1 邻居定义直接决定路径形态2D网格里大家很熟悉4邻居和8邻居的说法。到了3D邻居数量变成了三个档位6邻居上下左右前后只能沿坐标轴移动。18邻居在6邻居基础上加上12个“边斜向”即两个坐标方向同时变化的移动。26邻居在18邻居基础上再补上8个“角斜向”即三个坐标方向同时变化的移动。这个选择直接决定路径长什么样。6邻居走出来的路径全是直角折线转弯极其僵硬26邻居可以斜着穿空间路径明显更短也更平滑但代价是每个节点要检查26个方向计算量是6邻居的四倍还多。实际项目中怎么选地面机器人跑2D平面8邻居最常见无人机和机械臂的运动自由度高26邻居是标配。我在文章后面的实现里默认开了26邻居但代码里用allow_diagonal这个开关可以随时切回6邻居方便大家对比效果。3.2 移动代价必须精确匹配邻居集合这是入门阶段最容易踩的一个坑。3D栅格里不同方向的移动距离是不同的沿轴走一步代价1。二维对角线比如dx1dy1dz0代价sqrt(2)约等于1.414。三维对角线dx1dy1dz1代价sqrt(3)约等于1.732。如果图省事把所有移动代价都设成1A*会认为斜着走和直着走一样“便宜”。这时候它就会疯狂偏爱对角线移动因为“反正代价一样我斜着一口气窜过去还省了中间节点”。等你拿真实欧几里得距离一量会发现它规划出来的路径明显偏长而且路径形态很诡异。正确的做法是每次生成邻居时实时算一遍移动距离三行代码的事move_cost np.sqrt(dx * dx dy * dy dz * dz)不要图省事写死。这个代价直接参与g值计算一旦错了后面所有最优性证明都白搭。3.3 3D启发式函数选型对比3D空间里最常用的启发式有三种适用场景完全不同启发式类型计算公式适合的邻居可采纳性搜索效率曼哈顿距离dx dy dz仅6邻居6邻居下可采纳较高欧几里得距离sqrt(dx² dy² dz²)18/26邻居永远可采纳中等3D对角距离精确匹配1/sqrt(2)/sqrt(3)移动代价18/26邻居可采纳最高曼哈顿距离在26邻居下会严重高估剩余代价因为它默认所有移动都必须沿轴走可一旦允许斜穿真实距离比它算出来的小启发式就不再可采纳搜索结果就失去了最优性保证。欧几里得距离永远小于或等于真实最短距离所以一定可采纳是新手最稳妥的选择。代价是它比真实代价矮一截搜索会多探索一些节点。3D对角距离在26邻居下精确匹配了移动代价它先把三轴差值的最大值、中间值、最小值拆出来分别乘以对应的单位代价加起来。这种情况下启发式非常贴近真实代价搜索节点数最少效率最高。代码也不复杂就是把三个坐标差排个序的事。4. Python实现一个完整的3D A*搜索器4.1 地图与数据结构下面这段是我在实际项目里整理出来的一个精简版本完全可以直接运行。整篇文章的代码都用同一个核心类方便大家对照。import heapq import numpy as np from typing import List, Tuple, Optional class AStar3D: 3D体素栅格地图上的A*搜索器 def __init__(self, grid: np.ndarray, allow_diagonal: bool True): Parameters ---------- grid : 三维numpy数组0可通行1障碍物 allow_diagonal : 是否允许斜向移动默认True26邻居 if grid.ndim ! 3: raise ValueError(grid must be a 3D numpy array) self.grid grid.astype(np.int8) self.allow_diagonal allow_diagonal self.shape grid.shape def _get_neighbors(self, node): x, y, z node neighbors [] if self.allow_diagonal: for dx in (-1, 0, 1): for dy in (-1, 0, 1): for dz in (-1, 0, 1): if dx 0 and dy 0 and dz 0: continue nx, ny, nz x dx, y dy, z dz if not (0 nx self.shape[0] and 0 ny self.shape[1] and 0 nz self.shape[2]): continue if self.grid[nx, ny, nz] 1: continue move_cost np.sqrt(dx * dx dy * dy dz * dz) neighbors.append(((nx, ny, nz), float(move_cost))) else: for dx, dy, dz in ((1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0), (0, 0, 1), (0, 0, -1)): nx, ny, nz x dx, y dy, z dz if not (0 nx self.shape[0] and 0 ny self.shape[1] and 0 nz self.shape[2]): continue if self.grid[nx, ny, nz] 1: continue neighbors.append(((nx, ny, nz), 1.0)) return neighbors地图就是用numpy三维数组表示的体素栅格。这里我用了int8而不是bool一方面numpy的bool底层也是uint8但int8在很多索引场景下更直观另一方面网格数据后续如果要做膨胀inflate obstacles、多分辨率金字塔int8的空间足够扩展。_get_neighbors这个方法承担了三个责任遍历方向集合、做边界检查、做障碍物检查。26邻居的实现就是三层循环遍历-1、0、1的所有组合把零向量跳过。每次找到合法邻居顺手算一个真实的欧几里得移动代价。这个方法会被主循环调用无数次所以我把边界检查和障碍物检查放在同一个if里提前continue尽量减少无效迭代。4.2 启发式函数与主搜索循环下面继续看核心搜索逻辑。staticmethod def _heuristic(a, b, h_typeeuclidean): 启发式函数支持 euclidean / manhattan / diagonal 三种 dx abs(a[0] - b[0]) dy abs(a[1] - b[1]) dz abs(a[2] - b[2]) if h_type euclidean: return float(np.sqrt(dx * dx dy * dy dz * dz)) elif h_type manhattan: return float(dx dy dz) elif h_type diagonal: dmax max(dx, dy, dz) dmin min(dx, dy, dz) dmid dx dy dz - dmax - dmin return float((dmin * np.sqrt(3)) (dmid - dmin) * np.sqrt(2) (dmax - dmid) * 1.0) else: raise ValueError(funknown h_type: {h_type}) def search(self, start, goal, h_typeeuclidean): A*主搜索返回路径点列表找不到返回None for pt, name in ((start, start), (goal, goal)): if not (0 pt[0] self.shape[0] and 0 pt[1] self.shape[1] and 0 pt[2] self.shape[2]): raise ValueError(f{name} point out of grid: {pt}) if self.grid[pt] 1: raise ValueError(f{name} point is inside obstacle: {pt}) open_heap [] # 堆元素: (f_score, g_score, node) g_score {start: 0.0} came_from {} heapq.heappush(open_heap, (self._heuristic(start, goal, h_type), 0.0, start)) while open_heap: f, g, current heapq.heappop(open_heap) if g g_score.get(current, float(inf)): continue if current goal: path [] node current while node is not None: path.append(node) node came_from.get(node) path.reverse() return path for neighbor, move_cost in self._get_neighbors(current): tentative_g g_score[current] move_cost if tentative_g g_score.get(neighbor, float(inf)): came_from[neighbor] current g_score[neighbor] tentative_g f_next tentative_g self._heuristic(neighbor, goal, h_type) heapq.heappush(open_heap, (f_next, tentative_g, neighbor)) return None这里有几个实现细节值得单独说一下。第一个是堆里存的是(f_score, g_score, node)三元组。这样做的好处是即使同一个节点因为发现了更短路径而重复入堆堆也能根据f值正确排序。弹出时用if g g_score.get(current, float(inf))判断当前这条记录是不是“过期版本”如果是就跳过。这个判断极其关键没有它过期记录会导致路径回溯出错甚至死循环。第二个细节是came_from字典。它记录每个节点是从哪个节点走过来的。找到目标后沿着came_from一路回溯到起点再把列表反转就是一条从起点到终点的完整路径。这里要注意回溯循环的终止条件起点的parent不存在所以node came_from.get(node)拿到None时循环自然结束。第三个细节是搜索结束返回None的情况。如果堆弹空了还没碰到goal说明起点和终点之间根本不存在通路可能是地图被障碍物隔断了。这是非常常见的失败模式后面会专门讲怎么排查。4.3 生成测试地图与3D可视化算法写完了光看代码跑不出结果总觉得不踏实。我写了一个生成测试地图的函数让场景可视化出来才有说服力。def make_test_grid(size(24, 24, 24), seed7): 生成一个带空中墙体的测试地图 np.random.seed(seed) grid np.zeros(size, dtypenp.int8) # 随机障碍物密度约8% grid[np.random.random(size) 0.08] 1 # 人工加一堵空中墙体放在x8..14, y6..18, z4..10 # 这堵墙只挡到第10层从第11层以上可以翻越 grid[8:15, 6:19, 4:11] 1 start (2, 2, 2) goal (21, 21, 21) grid[start] 0 grid[goal] 0 return grid, start, goal这个场景特意模拟了一个“二维地图无法表达”的困境一堵立在空间中的高墙但它的顶部没封死上方的通道是畅通的。2D规划遇到这堵墙只能绕到两侧而3D规划可以大摇大摆从墙顶跨过去。这就是三维搜索真正的价值所在。可视化用matplotlib的三维散点就够了。障碍物全部画成小灰点路径画成蓝色折线起点和终点用特殊标记标出来。import matplotlib.pyplot as plt def visualize(grid, pathNone, startNone, goalNone): fig plt.figure(figsize(10, 8)) ax fig.add_subplot(111, projection3d) obs np.argwhere(grid 1) if len(obs): ax.scatter(obs[:, 0], obs[:, 1], obs[:, 2], c#aaaaaa, markers, s2, alpha0.4, labelobstacle) if path is not None: path np.array(path) ax.plot(path[:, 0], path[:, 1], path[:, 2], cblue, linewidth3, labelA* path) ax.scatter(path[:, 0], path[:, 1], path[:, 2], cblue, s10) if start is not None: ax.scatter([start[0]], [start[1]], [start[2]], cgreen, s80, markero, labelstart) if goal is not None: ax.scatter([goal[0]], [goal[1]], [goal[2]], cred, s80, marker*, labelgoal) ax.set_xlabel(X) ax.set_ylabel(Y) ax.set_zlabel(Z) ax.legend() plt.tight_layout() plt.show()4.4 跑起来看效果主程序调用非常简单grid, start, goal make_test_grid() planner AStar3D(grid, allow_diagonalTrue) path planner.search(start, goal, h_typeeuclidean) print(fPath found: {path is not None}, nodes: {len(path) if path else 0}) visualize(grid, path, start, goal)我在自己的电脑上跑这个24×24×24的地图A*几乎是在几十毫秒内返回结果。路径节点数通常在六七十个左右路线会从起点出发绕到那堵“空中墙体”的侧面然后从墙顶上方翻过去再落到终点那一侧。把可视化窗口转个角度一眼就能看出那条路径比2D规划“聪明”在哪。这里也提一句如果地图尺寸涨到100×100×100纯Python版本的A*可能要跑几秒甚至十几秒这是正常的。性能问题我们在第5章细聊。5. 常见问题与性能调优实战5.1 搜索失败的第一时间排查A*返回None通常逃不出这几个原因起点或终点本身就在障碍物里。这个我在search函数里已经做了显式检查但现实项目中从传感器读到的起点坐标经常有噪点很可能“卡”在障碍物边界上。建议在调用前对外层坐标做一次合法性校验或者做一层腐蚀处理把靠近障碍物边缘的点自动挪开。起点和终点不连通。在稀疏障碍物地图里很少发生但如果障碍物密度超过一定阈值空间会被隔成几个孤立的子区域。快速判断方法先跑一次BFS或者使用并查集做连通性分析如果起点和终点不连通直接返回“无解”不用浪费A*的时间。地图太大open list被撑爆导致程序卡死。这个不是逻辑错误而是资源耗尽的工程问题解法见下面性能调优部分。5.2 路径不是最优的常见原因有人跑完A*后发现路径长度跟真实最短路径差了那么一截往往不是代码写错而是下面几种情况第一种启发式高估了代价。最常见的是26邻居下用了曼哈顿距离。我见过有人拿着2D代码直接改成3D只把坐标系换成三个轴启发式仍然用dxdydz结果路径走出来的确“看起来合理”但一测长度明显偏大。这种情况把启发式换成欧几里得距离就可以解决。第二种移动代价没对准。比如26邻居下所有移动代价都写了1搜索会偏爱斜穿导致路径整体歪向对角线方向。第三种地图边界值问题。比如地图原点不是(0,0,0)而是(100,200,50)代码里的边界判断用错了参考系虽然不会报错但搜索会错过一些本该能走的节点。建议所有坐标都以地图索引为准统一转换。5.3 性能优化的三板斧3D搜索最大的敌人就是维度灾难。100×100×100就是一百万个网格200×200×200就是八百万个纯Python的A*在这个规模下会慢得让人怀疑人生。实际项目中我推荐按顺序尝试下面三个方案。**第一板斧把g_score和came_from换成更高效的数据结构。**Python的字典很灵活但开销不小。对于尺寸固定的3D网格完全可以用三个numpy数组来存g_score_map np.full(shape, np.inf)parent_map np.full(shape (3,), -1)。这样索引和更新都是O(1)的numpy操作比字典快一个数量级。堆还是用heapq但节点可以用整数索引表示比如把三维坐标(x,y,z)编码成x * ny * nz y * nz z入堆时只压一个整数内存和比较开销都小很多。**第二板斧加权A让搜索更快。**把启发式函数乘上一个大于1的系数比如h(n) * 1.2A会更快地向目标方向收敛代价是不再保证全局最优。这个方法在实际工程里非常常用尤其是无人机路径规划很多时候“不错的路径”远好于“理论最短但晚三秒才算出来的路径”。具体做法给A*加一个weight参数默认1.0设置为1.2或者1.5时搜索节点数往往能降三分之一以上。**第三板斧换算法。**如果地图是均匀代价网格JPSJump Point Search是一个极其高效的剪枝算法它能在保持A最优性的前提下把大量“中间节点”整段跳过搜索速度提升一个数量级。JPS在2D网格上相当成熟3D场景也有对应的扩展版本但实现复杂度明显上升适合在掌握了基础A之后再研究。另外一个非常实用的方向是双向A*从起点和终点同时开始搜索两边交替扩展当两边边界相遇时合并路径。在3D大规模地图上双向搜索能把搜索空间砍掉一大半实现的改动也不大只需要两个堆和两套came_from。5.4 生产环境中的避坑建议写代码这关过了真正落地时还会遇到几件破事挑重点说几个。**地图分辨率选多大真是够用。**0.5米分辨率和0.1米分辨率搜索耗时的差距不是5倍而是几十倍因为网格数量是立方级增长的。做大型户外无人机巡检我一般先用1米或2米的低分辨率地图搜出一条粗糙路径然后只在这条路径的通道内用高分辨率做局部细化。这个“由粗到细”的策略比从头到尾用高分辨率地图高效太多。*动态环境不要用纯A反复重规划。**如果地图里的障碍物会移动每秒钟重新跑一遍A非常浪费。这类场景更适合DLite它能在上一次搜索结果的基础上做增量修复只更新受影响的那部分路径耗时通常只有全量重规划的十分之一。**斜穿障碍物角落的问题。**3D栅格中允许斜着走之后可能会让路径“擦着”障碍物角落偷过去。对于无人机这种有实际体积的机器人物理尺寸不能理想化为一个点路径与障碍物之间必须保留安全距离。工程上通常先把障碍物地图做膨胀处理把障碍物周围的格子标记为占用再用A*搜索。膨胀半径根据机器人最大尺寸设定这一步是必备的不做的话算法层面“找到了路”真机实测一头撞上去。**纯Python的性能天花板。**如果你的地图长期稳定在300×300×300纯Python A*基本没法实时跑。我试过用numba给邻居生成函数加jit装饰器性能提升非常明显但要注意numpy数组的类型签名写对。也可以把核心搜索循环改成C写一个Python扩展但工作量直线上升。对绝大多数学习和中小型项目而言numba加速加结构优化已经够用了。最后分享一点个人体会3D A看起来只需要把2D代码加一个维度但真正写完之后你会发现问题都藏在细节里邻居怎么定义、代价怎么算、启发式选哪一种、堆里面的过期记录怎么处理每一个都直接决定结果对不对、效率高不高。我的建议是拿到代码之后一定亲手改几个参数跑一遍。把allow_diagonal关掉对比路径形态把h_type换成manhattan观察路径长度变化把地图换成全空看看A能不能走出一条接近直线的路径。这种“破坏性实验”比照着代码抄十遍更管用。后续如果想继续深入建议沿着两个方向走一是把算法换成JPS和D* Lite解决大地图和动态环境的性能问题二是把栅格搜索的路径输出到真实运动规划链路里跟样条平滑、速度规划对接让路径真正可执行。那才是运动规划最出成果的地方。