芯耀
与非AI
103
转载自公众号：敢敢AUTOHUB
https://github.com/lovelyyoshino/3D-Nav-Dynamic-EGO
0. 先看效果
三种规划模式跑起来之后，RViz 里大概是这样的。绿色体素层就是规划实际使用的碰撞图，蓝线是 A* 搜索产出的 warm start 路径，紫红色曲线是 B-spline 优化后的最终轨迹。不同模式下轨迹的行为完全不同——UAV 可以在空中任意穿行，Ground 必须贴着地面支撑走，2D 则是把三维信息投影到平面上做规划。
0.1 3D UAV 自由空间规划
无人机模式不需要地面支撑，只要采样点在地图边界内、不落入 occupied 体素、满足膨胀半径，就可以视为可通行。这是最自由的规划模式，轨迹可以上下穿越楼层间的空隙，只要那片空间确实是 free 的。
3D UAV 自由空间规划
0.2 3D Ground 有地面支撑
地面机器人虽然运行在 3D 体素地图里，但不能把空中的 free space 当作道路。每个采样点都必须找到可支撑的地面结构，否则轨迹可能漂在空中，或者贴着楼梯边沿走出不可执行的路径。这个约束让搜索空间大幅缩小，但也意味着连通性更容易断裂。
3D Ground 有地面支撑
0.3 2D Ground OccupancyGrid
2D ground 模式把 OctoMap 投影成 OccupancyGrid。只有 ground-supported 且有 clearance 的格子被标为 free，其他区域是 unknown 或 occupied。这里的关键是，2D 并不等于把地图外接矩形全部当作可走平面——它只是把「地面可通行」这个三维判断压到 XY 上。
2D Ground OccupancyGrid
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 兜底。整条链路的设计原则是：每个阶段的职责清晰，可以单独测试和调试。
PCD 点云
→ 点云过滤 + Voxel/OctoMap 构建
→ 2D/3D A*（或 JPS）离散搜索 ← warm start 来源
→ B-spline 控制点初始化
→ L-BFGS 优化（平滑 + 避障代价）
→ 局部 A* rebound cue ← 替代 ESDF 梯度
→ 二次轨迹碰撞采样检查
→ fallback FSM（缩短目标 / 急停）
→ ROS2 bridge → RViz / Web2.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()：
// 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 代价函数作为软约束// 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² 连续，解决的是「轨迹能不能被下游跟踪器执行」的问题。三步串起来，每一步出了问题都能单独定位。
4.2 和 MPC 的本质差别
MPC 是每个控制周期都重新求解一个有限时域优化问题，对动态障碍反应最快，但计算开销高、调参复杂、可预测性差。这里的方案是一次规划产生完整轨迹，动态障碍通过事件触发的 safety replan 处理。这意味着对障碍的响应不是每帧的，而是检测到碰撞风险时才介入。对于地图相对稳定、动态障碍是偶发事件的场景（仓库里的行人、临时放置的货物），这个取舍是合理的。
5. 动态障碍怎么处理
5.1 两个运行时输入
静态地图规划完成之后，动态障碍通过两个 ROS topic 进入系统。/nav3d/current_pose 提供机器人当前位姿，/nav3d/local_pointcloud 提供局部感知到的新点云，用于更新 LocalGrid。这两个输入是运行时局部能力的基础——不提供的话，系统只能执行静态地图上的规划结果，对环境变化完全无感知。
/nav3d/current_pose 当前机器人位姿（frame_id=map）
/nav3d/local_pointcloud 局部感知点云（更新 LocalGrid）5.2 SafetyMonitor 的判断逻辑
SafetyMonitor（safety_monitor.cpp）沿当前轨迹向前检查，根据 LocalGrid 的更新状态判断前方是否存在碰撞风险。近距离碰撞直接零速停机发布 safety_emergency_stop；稍远的碰撞触发 safety_replan_needed，从当前位姿到 active goal 重新规划一条轨迹。重规划成功则发布新轨迹继续执行，失败则降级为急停。
local_pointcloud_updated → safety_replan_needed
├─ safety_replan_success → 从当前位姿重规划，发布新轨迹
└─ safety_replan_emergency_stop → 被阻断，零 cmd_vel5.3 验证这条链路
跑内置 smoke 脚本就能验证整条 safety replan 链路。脚本会模拟局部点云更新，触发 SafetyMonitor 判断，观察 /nav3d/status 的状态转换是否符合预期。加参数可以模拟机器人沿轨迹推进、每 N 秒在前方和左右生成一组世界坐标系的 sticky 障碍物：
# 基础验证
ros2 run nav3d_ros2_bridge nav3d_local_replan_smoke.py
# 完整模拟：机器人推进 + 周期障碍
ros2 run nav3d_ros2_bridge nav3d_local_replan_smoke.py
--follow-trajectory --follow-speed 1.0
--follow-local-obstacle-count 4 --follow-local-side-offset 0.3
# launch 方式（默认每 5s 前方/左/右 spawn 3 个 cluster，5m 后丢弃）
ros2 launch nav3d_ros2_bridge nav3d_local_replan_smoke.launch.py
pcd_path:="$pcd_path"6. Web 控制台
6.1 定位：RViz 之外的交互入口
除了 RViz，还有一个浏览器端控制台（React 19 + Three.js），通过 rosbridge websocket 连接 ROS2。它的定位是精确坐标输入和多地图切换的辅助工具——适合需要输入精确 XYZ 坐标、在浏览器中做 3D 选点、或者不想在 RViz 里操作的场景。Web 控制台不做任何本地规划计算，它只显示 bridge 返回的真实轨迹。
# bridge 开启 rosbridge
ros2 launch nav3d_ros2_bridge nav3d_bridge.launch.py
pcd_path:="$pcd_path" rosbridge:=true rosbridge_port:=9090 rviz:=false
# Web
cd web_ui && npm install && npm run dev -- --port 51736.2 路径分色和状态指示
打开 http://localhost:5173/，看到状态栏「已连接」就可以开始使用。路径分三色：全局路径（钴蓝）、局部路径（紫红，前方约 2.5 m）、已走轨迹（暗灰半透明，自动淡出）。顶部 HUD 显示 plan 状态 chip（完整/部分/失败/待规划），partial=true 表示系统找到了一个缩短后的次优落点，不代表真正到达了目标。
6.3 地图切换和选点
右侧 dock 提供地图下拉切换（预置 4 张参考地图），选中后向 /nav3d/load_pcd_path 发布 PCD 路径并清空当前规划。选点方式是先点「起点」按钮再在 3D 体素上点一次，切回「目标」再点一次或多次。也可以直接用 XYZ 数字输入精确坐标，适合需要对照特定位置反复测试的场景。
7. 总结
目前的代码是可测试原型，不是完整生产级 Nav2 controller server。partial=true 是明确的缩短目标 fallback，不应解释为到达 requested goal。运行时局部规划必须下游持续提供 /nav3d/current_pose 和 /nav3d/local_pointcloud，不提供则只能执行静态图上的规划结果。后续可以往真机迁移。
