我用 ROS2 做了一个 3D 导航原型：从点云到动态避障全链路跑通

https://github.com/lovelyyoshino/3D-Nav-Dynamic-EGO

0. 先看效果

三种规划模式跑起来之后，RViz 里大概是这样的。绿色体素层就是规划实际使用的碰撞图，蓝线是 A* 搜索产出的 warm start 路径，紫红色曲线是 B-spline 优化后的最终轨迹。不同模式下轨迹的行为完全不同------UAV 可以在空中任意穿行，Ground 必须贴着地面支撑走，2D 则是把三维信息投影到平面上做规划。

0.1 3D UAV 自由空间规划

无人机模式不需要地面支撑，只要采样点在地图边界内、不落入 occupied 体素、满足膨胀半径，就可以视为可通行。这是最自由的规划模式，轨迹可以上下穿越楼层间的空隙，只要那片空间确实是 free 的。

0.2 3D Ground 有地面支撑

地面机器人虽然运行在 3D 体素地图里，但不能把空中的 free space 当作道路。每个采样点都必须找到可支撑的地面结构，否则轨迹可能漂在空中，或者贴着楼梯边沿走出不可执行的路径。这个约束让搜索空间大幅缩小，但也意味着连通性更容易断裂。

0.3 2D Ground OccupancyGrid

2D ground 模式把 OctoMap 投影成 OccupancyGrid。只有 ground-supported 且有 clearance 的格子被标为 free，其他区域是 unknown 或 occupied。这里的关键是，2D 并不等于把地图外接矩形全部当作可走平面------它只是把「地面可通行」这个三维判断压到 XY 上。

0.4 单测全绿

Core 层所有单测通过，覆盖体素化、A* 搜索、B-spline 优化、碰撞检测、safety monitor 等核心模块。这是确认规划链路每一步都正确工作的基础验证手段。

1. 为什么要做这个

1.1 论文到工程的距离

EGO-Planner 是一篇很漂亮的论文，但论文实现和能跑在自己地图上的工程之间，有一段很长的路。核心问题在于，论文关注的是算法正确性和对比实验，而工程需要的是可测试、可调试、可接入下游的完整链路。我想搞清楚这段路上每一步到底在做什么------不是读懂代码，而是亲手把它组起来、跑起来、测起来。

1.2 具体目标

所以这个项目的目标很具体：输入是一份 PCD 点云文件，输出是 RViz 或 Web 里可以看到的平滑轨迹，中间的每一步（体素化、A* 搜索、B-spline 优化、局部避障、safety replan）都要有对应的测试和可观察的中间状态。规划核心做成 ROS-free C++，再用 ROS2 bridge 和 Web UI 包住它，这样验证链路足够短，不需要整套 ROS 环境也能先跑通核心。

2. 整体链路

2.1 从点云到轨迹的六步

把整条规划链路拆开来看，每一步的输入输出都很明确。PCD 点云先经过过滤和体素化变成可查询的地图结构，然后 A* 或 JPS 在离散网格上搜出一条拓扑可行路径作为 warm start，B-spline 优化器用 L-BFGS 把控制点推到可通行空间里去，最后由碰撞检测和 fallback FSM 兜底。整条链路的设计原则是：每个阶段的职责清晰，可以单独测试和调试。

text 复制代码

PCD 点云
  → 点云过滤 + Voxel/OctoMap 构建
  → 2D/3D A*（或 JPS）离散搜索         ← warm start 来源
  → B-spline 控制点初始化
  → L-BFGS 优化（平滑 + 避障代价）
  → 局部 A* rebound cue                ← 替代 ESDF 梯度
  → 二次轨迹碰撞采样检查
  → fallback FSM（缩短目标 / 急停）
  → ROS2 bridge → RViz / Web

2.2 关键实现文件

每一步都有对应的代码模块。bspline_optimizer.cpp 承接 warm start 和 L-BFGS 优化，包括 rebound cue 的生成逻辑；ego_planner_core.cpp 串联整条链路并执行 fallback FSM；safety_monitor.cpp 负责沿轨迹前向碰撞预警和急停决策。换句话说，Web、RViz、ROS topic 都不是规划本体，真正决定轨迹质量的是 core 里的地图语义、搜索结果、优化代价和碰撞复查。

3. ESDF 去哪了？

3.1 原版方案的代价

EGO-Planner 原版用 ESDF（欧式符号距离场）给 B-spline 优化提供避障梯度。ESDF 的好处是梯度信息连续、方向准确，但有实际工程代价：需要维护一张全局距离场，每次地图更新都要重算一遍距离传播，内存占用和计算量都不小。尤其是在点云更新频繁、局部障碍不断进入视野的动态场景下，ESDF 既要更新距离场，又要提供稳定梯度，这两个需求本身就有张力。

3.2 局部 A* rebound cue

我换了一种更直接的方式。在 B-spline 优化的内层循环中，L-BFGS 检测到某个控制点位于 occupied 区域或 clearance 不足时，从该控制点向最近 free 体素跑一次小范围 3D A* 搜索，把这条局部逃离路径的方向作为 rebound cue 注入代价函数。L-BFGS 继续优化，直到控制点回到可通行空间。实现在 bspline_optimizer.cpp:335 的 buildReboundCues()：

cpp 复制代码

// core/planner_core/src/bspline/bspline_optimizer.cpp:335
// buildReboundCues()：局部 A* 替代 ESDF 梯度
//
// 1. 遍历所有控制点，检测 occupied 或 clearance 不足
// 2. 对碰撞控制点启动小范围 3D A*（搜索半径受限）
// 3. A* 路径第一段方向 → rebound direction
// 4. 注入 L-BFGS 代价函数作为软约束

cpp 复制代码

// core/planner_core/src/bspline/bspline_optimizer.cpp:404
// lbfgsDirection()：L-BFGS two-loop recursion
// 用 rebound cue + 平滑代价 + 碰撞代价的复合梯度驱动优化方向

// core/planner_core/src/bspline/bspline_optimizer.cpp:439
// makeWarmStart()：A* 搜索结果 → B-spline 控制点初始化

3.3 工程取舍的直觉

直观理解是，ESDF 梯度告诉你「几何上该往哪里推」，局部 A* cue 告诉你「从当前地图看，往哪里逃是通的」。在建筑、楼梯、狭窄走廊这类离散结构明显的地图里，后者更容易和搜索结果保持一致。代价是梯度信息不再连续------每次 rebound cue 是一次离散的局部重搜索，对高频动态障碍场景的梯度平滑性有所牺牲。

3.4 二次碰撞检查兜底

优化结束后，collision::TrajectoryChecker（ego_planner_core.cpp:300）会再做一次连续采样碰撞检查。如果曲线在控制点之间翻过薄障碍，这一步会拦下来，而不是只相信优化器的最终状态。检查仍失败的话，fallback FSM 依次尝试：保守 path-following → 提高 collision_weight → 缩短目标 → EmergencyStop。

4. A* + B样条 + L-BFGS，为什么不直接上 MPC

4.1 各阶段职责解耦

这也是我纠结过的问题。最终选择这套组合的原因是各阶段职责清晰、可以分开调试。A* 给出拓扑上可行的路径，作为 B-spline 的 warm start，解决的是「从哪里走」的问题；L-BFGS 在光滑代价函数上收敛快，解决的是「怎么走得平滑」的问题；B-spline 的均匀节点向量天然保证 C² 连续，解决的是「轨迹能不能被下游跟踪器执行」的问题。三步串起来，每一步出了问题都能单独定位。