1. 从二维栅格到三维点云路径规划到底变了什么很多人做路径规划是从二维栅格地图入门的A*、Dijkstra、RRT 这些算法在平面栅格上跑得挺顺代码也不复杂。但一旦把场景换成三维点云事情就完全不一样了。我最初接触三维点云路径规划是在一个室内巡检项目里激光雷达扫出来的点云有几十万个点直接拿二维那套思路往上套结果要么是内存爆掉要么是规划出来的路径穿墙。这个经历让我意识到三维点云路径规划不是简单地把二维算法加一个Z轴而是从数据组织、空间搜索到路径优化都需要重新设计。三维点云路径规划的核心任务是在一堆离散的、带有噪声的、分布不均匀的三维点中找到一条从起点到终点的无碰撞路径。听起来和二维差不多但难点在于点云没有明确的“可通行区域”和“障碍物”的区分你需要自己从点云中提取出哪些地方能走、哪些地方不能走。而且三维空间中的搜索空间比二维大了一个数量级算法效率直接决定了能不能落地。这篇文章主要面向已经了解基础路径规划算法、想往三维场景迁移的开发者或者正在做SLAM建图后接导航规划的朋友。我会从点云的数据结构讲起一直讲到实际可跑的路径规划方案中间会穿插我自己踩过的坑和调参经验。关键词里的八叉树、跳点搜索、SLAM这些都会涉及到但不会只讲概念而是讲它们在三维路径规划里具体怎么用、为什么这么用。2. 点云不是地图三维空间表达方式的选型逻辑2.1 原始点云为什么不能直接拿来规划激光雷达输出的原始点云是一堆三维坐标点的集合每个点包含XYZ坐标有时还有强度信息。这种数据结构对于路径规划来说有几个致命问题。第一点的数量太大一台16线激光雷达转一圈就是几万个点64线的话直接上几十万在这种规模的数据上做最近邻搜索或者碰撞检测计算量大得离谱。第二点云是离散的两个点之间有没有障碍物你无法直接判断需要额外的空间推理。第三点云中有大量噪声点和动态物体产生的离群点如果不做处理规划出来的路径可能会被这些噪声“吓跑”。所以从原始点云到可用于路径规划的地图表示中间必须有一个转换过程。这个转换的核心目标是把“点”变成“空间占用信息”也就是回答一个基本问题空间中任意一个位置是空闲的还是被占用的。2.2 八叉树地图为什么它成了三维规划的主流选择在三维空间表达方式中八叉树是目前路径规划领域用得最多的结构。它的核心思想是递归地把三维空间分成八个卦限每个卦限再继续细分直到达到设定的分辨率或者某个卦限内的点云密度低于阈值。我为什么推荐八叉树而不是体素栅格体素栅格是把空间均匀切成小方块每个方块标记占用或空闲。这种方式实现简单但内存消耗是固定的——不管你场景里有没有障碍物整个空间都要分配内存。假设你要覆盖一个100米×100米×10米的空间分辨率设为0.1米那就是1000×1000×100个体素一共一亿个格子。每个格子哪怕只用一个字节也是100MB起步。而八叉树只在实际有点云存在的区域才展开细分空旷区域用一个大节点就表示了内存效率高得多。八叉树还有一个好处是它天然支持多分辨率查询。做全局规划的时候可以用粗分辨率快速找到大致路径做局部避障的时候切换到细分辨率保证安全距离。这种灵活性在二维栅格里很难做到。2.3 从点云构建八叉树的关键参数构建八叉树时有几个参数直接决定了后续规划的效果我逐个说一下我的经验值。分辨率Resolution这是八叉树最底层叶节点的最小尺寸。设得太小内存和计算量暴涨设得太大细小的障碍物会被忽略。我的经验是分辨率设为机器人半径的1/2到1/3比较合适。比如机器人半径0.3米分辨率设0.1到0.15米。这样既能保证障碍物不会被漏掉又不会让树太深。占用概率阈值Occupancy Threshold八叉树通常用概率的方式表示节点是否被占用。每个点云插入时会给对应节点增加占用概率多次观测后概率会收敛。一般来说占用概率大于0.7认为是被占用小于0.3认为是空闲中间的是未知区域。未知区域在规划时通常当作障碍物处理除非你有额外的传感器信息来补充。最大深度Max Depth这决定了八叉树最多细分到多少层。最大深度和分辨率是关联的给定空间范围和分辨率最大深度就确定了。但有些实现允许你单独设置这时候要注意不要让最大深度和分辨率产生矛盾。# 以Python的octomap库为例构建八叉树的基本流程 import octomap # 创建八叉树分辨率0.1米 tree octomap.OcTree(0.1) # 插入点云数据 for point in point_cloud: tree.updateNode(point, True) # True表示该点被占用 # 更新内部节点状态 tree.updateInnerOccupancy()这段代码看起来简单但实际使用时有个坑点云插入的顺序会影响最终结果。如果先插入了一个点标记为占用后来又插入了同一个位置标记为空闲八叉树会根据概率更新来调整。但如果你的点云没有做运动畸变校正同一个物体在不同帧的位置会有偏移导致八叉树中出现“拖影”规划时会把本来可以通过的区域标记为障碍。所以插入八叉树之前一定要确保点云已经做了畸变校正和配准。3. 在八叉树里找路搜索算法的选择与改造3.1 A*在三维八叉树上的适配问题A*是路径规划里最经典的算法二维栅格上的实现大家都很熟悉。但直接搬到三维八叉树上会遇到几个问题。第一个问题是邻居节点的定义。二维栅格上每个格子有8个邻居四连通是4个八连通是8个三维空间里如果按26连通来算每个节点有26个邻居。搜索空间直接翻了3倍多。而且26连通会产生很多“斜穿”的路径实际机器人走起来需要频繁转向不平滑。第二个问题是启发函数的设计。二维上用欧几里得距离或者曼哈顿距离都行三维上欧几里得距离仍然适用但如果你用的是八叉树节点大小不一致启发函数的计算需要考虑节点实际覆盖的空间范围。第三个问题是搜索效率。三维空间的节点数量远大于二维A的开放列表和关闭列表会迅速膨胀。我实测过一个50米×50米×5米的室内场景分辨率0.1米八叉树展开后有大约80万个叶节点。在这个规模上跑标准A单次规划耗时在秒级对于需要实时规划的机器人来说太慢了。3.2 跳点搜索JPS在三维中的变体思路跳点搜索Jump Point Search是A*的一种加速变体核心思想是在搜索过程中跳过那些“对称”的路径只保留必要的转折点。在二维栅格上JPS可以把搜索节点数减少一个数量级。但三维空间中的JPS实现要复杂得多因为三维的对称性判断比二维复杂。我的做法是在八叉树上做一个简化版的跳点策略在每一层八叉树节点上先判断该节点是否完全空闲。如果是直接跳到该节点的边界不需要展开内部子节点。这其实利用了八叉树的多分辨率特性相当于在粗粒度上做跳点。实测下来这种策略可以把搜索节点数减少60%到70%规划时间从秒级降到百毫秒级。具体实现时你需要在A*的扩展过程中加一个判断当前节点如果是空闲的且不是目标所在节点就尝试沿着当前搜索方向“跳跃”到下一个可能产生转折的节点。跳跃的距离由八叉树的层级决定——层级越高跳跃距离越远。3.3 实际项目中的算法选型对比我在几个不同项目里试过不同的搜索算法这里做一个对比方便你根据场景选择。算法适用场景规划时间80万节点路径质量实现难度标准A*小规模场景1-3秒最优低JPS变体中大场景100-500毫秒接近最优中RRT*高维空间500毫秒-2秒渐近最优中高贪心最佳优先快速预览50-200毫秒次优低如果场景不是特别复杂我一般推荐JPS变体它在效率和路径质量之间取得了比较好的平衡。如果场景中有很多狭窄通道RRT*的采样特性反而更有优势因为它不需要在规则网格上搜索。4. 路径不是搜出来就能用后处理与平滑4.1 为什么原始搜索路径需要平滑A*或者JPS搜出来的路径是一系列八叉树叶节点的中心点连成的折线。这条折线有几个问题第一它紧贴着障碍物边缘因为搜索算法倾向于走最短路径而最短路径往往就是贴着障碍物走。实际机器人有体积贴着走很容易发生碰撞。第二折线的转折角很尖锐机器人无法直接跟踪需要减速甚至停下来转向。第三八叉树的分辨率决定了路径的“锯齿”程度分辨率越粗锯齿越明显。所以搜出路径之后必须做后处理。后处理一般分两步先做安全距离膨胀再做路径平滑。4.2 安全距离膨胀的实操细节安全距离膨胀的思路很简单把路径上的每个点往外推直到距离最近的障碍物满足最小安全距离。但实现时有几个细节需要注意。首先是膨胀方向的选择。你不能简单地把点沿着法线方向推因为法线方向可能指向另一个障碍物。我的做法是在每个路径点周围做一个球形查询找到最近的障碍物点然后沿着“远离障碍物”的方向推。如果周围有多个障碍物就取合力方向。其次是膨胀距离的设定。这个距离应该等于机器人半径加上一个安全余量。安全余量一般取机器人半径的20%到30%。比如机器人半径0.3米安全距离设为0.36到0.39米。如果场景中有动态障碍物安全余量还要加大。膨胀之后要重新做碰撞检测确保膨胀后的路径仍然是无碰撞的。有时候膨胀会导致路径进入新的障碍物区域这时候需要局部调整或者重新规划。4.3 用B样条做路径平滑的参数调法路径平滑我常用B样条曲线因为它有局部支撑性——修改一个控制点只影响局部曲线不会导致整条路径变形。具体做法是把膨胀后的路径点作为控制点生成一条三阶或四阶B样条曲线。参数调法上阶数选择很关键。三阶B样条C2连续已经能保证曲率连续适合大多数机器人。四阶更平滑但计算量稍大如果机器人运动学约束比较宽松三阶就够了。控制点的采样间隔也需要注意。如果直接用所有路径点做控制点曲线会过度拟合产生不必要的摆动。我一般会做一次降采样每隔3到5个路径点取一个控制点。这样曲线更平滑同时不会偏离原始路径太远。# 用scipy做B样条平滑的示例 import numpy as np from scipy import interpolate # path是膨胀后的路径点shape为(N, 3) # 降采样每隔3个点取一个控制点 control_points path[::3] # 生成三阶B样条 tck, u interpolate.splprep([control_points[:,0], control_points[:,1], control_points[:,2]], k3, s0.1) # 在参数域上均匀采样得到平滑路径 u_new np.linspace(0, 1, 200) smooth_path interpolate.splev(u_new, tck)这里的s参数是平滑因子s0表示强制通过所有控制点s越大曲线越平滑但偏离控制点越多。我一般从0.1开始试根据实际效果调整。5. 当SLAM遇上路径规划建图与规划的衔接问题5.1 SLAM输出的地图直接用来规划会怎样SLAM建图输出的通常是点云地图或者经过转换的八叉树地图。很多教程讲到这里就结束了好像SLAM建完图路径规划就是水到渠成的事。但实际项目中SLAM地图直接拿来规划会有一堆问题。最常见的问题是地图中的“重影”。SLAM在闭环或者回环校正的时候地图会发生整体变形之前建好的部分和新建的部分可能对不齐。如果你在SLAM还在运行的时候就拿地图去规划规划出来的路径可能穿过这些重影区域实际执行时发现那里根本没有障碍物或者反过来有障碍物但地图上没显示。另一个问题是地图的更新频率。SLAM建图是渐进的地图在不断完善。但路径规划需要一张相对稳定的地图。如果地图每秒钟都在变规划器就需要不断重新规划计算资源消耗很大而且路径会跳来跳去。5.2 建图与规划的异步处理策略我的做法是把建图和规划做成异步的。SLAM模块持续建图但规划模块只在一个“地图快照”上进行。具体来说每隔固定时间比如2秒或者地图变化量超过阈值时从SLAM模块导出一份当前地图的副本规划模块在这份副本上做规划。这样规划器看到的地图是稳定的同时又能跟上环境的变化。实现上可以用一个双缓冲机制SLAM模块往缓冲区A写地图规划模块从缓冲区B读地图。当SLAM完成一次完整的地图更新后交换A和B。这样规划模块永远不会读到写了一半的地图。5.3 动态障碍物的处理规划器需要知道什么如果场景中有动态障碍物比如走动的人或者其他机器人SLAM建出的地图会把它们也建进去。但这些动态障碍物不应该被当作永久障碍物否则规划器会认为某些区域永远不可通行。处理动态障碍物的常见做法是在八叉树中维护一个“衰减”机制。每个被标记为占用的节点有一个时间戳如果一段时间内没有新的点云观测到该节点被占用就逐渐降低其占用概率最终标记为空闲。这个衰减时间需要根据场景调整人走动频繁的场景设短一些比如2到3秒静态场景可以设长一些。但衰减机制有个副作用如果机器人停在某个位置不动它自己的点云可能会被建到地图里然后因为衰减被清除导致地图出现“空洞”。所以一般会把机器人自身周围的区域从点云中过滤掉或者给机器人所在位置一个永久的空闲标记。6. 从仿真到实机那些只有跑起来才知道的事6.1 仿真环境里跑通不代表实机能用在Gazebo或者类似的仿真环境里点云是完美的没有噪声没有运动畸变时间戳也是对齐的。路径规划在仿真里跑得再好搬到实机上大概率会出问题。我遇到过的典型问题包括实机点云的噪声导致八叉树中出现大量“毛刺”节点规划器为了避开这些噪声点路径变得非常绕实机的时间戳不同步导致点云配准错误地图出现错位实机的计算资源有限仿真里跑得动的算法在实机上因为CPU占用过高而卡顿。所以我的建议是仿真里验证算法逻辑但一定要尽早搬到实机上测试。实机测试时先把分辨率调粗减少计算量等基本跑通了再逐步细化。6.2 计算资源的分配与优化三维点云路径规划对计算资源的需求不低。以我的经验一个中等复杂度的室内场景八叉树构建加上路径搜索单次规划需要50到200毫秒的CPU时间。如果机器人上还要跑SLAM、感知、控制等其他模块CPU很容易成为瓶颈。优化方向有几个。第一把八叉树的构建和更新放到单独的线程不要和规划线程抢资源。第二规划时使用多分辨率策略先用粗分辨率快速搜一条大致路径再在路径附近用细分辨率做局部优化。第三如果硬件支持把八叉树的构建和碰撞检测放到GPU上做能获得数倍的加速。6.3 路径跟踪阶段的常见问题规划出一条路径只是第一步机器人能不能沿着这条路径走又是另一回事。我见过很多项目规划模块输出了一条漂亮的平滑路径但控制模块跟踪的时候偏差很大最后撞上了障碍物。问题通常出在路径的曲率上。如果路径的曲率超过了机器人的最小转弯半径机器人就无法精确跟踪。所以在路径平滑阶段需要加入曲率约束。具体做法是在B样条优化时加一个惩罚项当曲率超过阈值时增加代价迫使优化器产生满足曲率约束的路径。另一个问题是路径的“可跟踪性”。有些路径虽然平滑但需要机器人频繁地前进后退切换这种路径对差速机器人来说很难跟踪。规划时应该考虑机器人的运动学模型把运动学约束加入到搜索过程中而不是规划完了再检查。7. 几个值得关注的进阶方向7.1 基于深度学习的端到端路径规划传统的路径规划是“感知-建图-规划-控制”的流水线每个模块独立优化。最近几年有一些工作尝试用深度学习做端到端的路径规划输入原始点云直接输出控制指令。这种方式的优势是避免了模块间的误差累积但缺点是可解释性差出了问题很难排查。目前在实际项目中我还没有看到端到端方案完全替代传统流水线的案例但在局部避障等子任务上学习-based的方法已经开始展现优势。7.2 多机器人协同规划当场景中有多个机器人时路径规划就从单智能体问题变成了多智能体问题。每个机器人不仅要考虑静态障碍物还要考虑其他机器人的运动。常见的做法是给每个机器人分配一个优先级高优先级的机器人先规划低优先级的机器人把高优先级机器人的路径当作动态障碍物来避让。这种方式实现简单但可能陷入死锁。更复杂的做法是用协同A*或者基于博弈论的方法但计算量会大很多。7.3 语义信息与路径规划的融合纯几何的路径规划只关心“能不能走”不关心“该不该走”。比如一片草地几何上是可通行的但如果机器人是室内服务机器人就不应该走草地。把语义信息融合到路径规划中可以让规划结果更符合实际需求。实现方式通常是在八叉树节点上附加语义标签规划时根据任务需求给不同语义区域设置不同的通行代价。我在实际项目中的体会是三维点云路径规划没有“一招鲜”的方案每个场景都需要根据传感器特性、计算资源、机器人运动学约束来调整。八叉树加JPS变体是我用得最顺手的组合但也不是万能的。最重要的是把整个链路跑通从点云预处理到路径跟踪每个环节都实际测过才知道哪里是真正的瓶颈。