简介这是一套基于机器人操作系统的三维导航功能包集合面向机器人开发者与导航研究者适用于Hydro版本框架下的三维空间自主定位、路径规划与避障任务。压缩包共103个文件包含21个launch启动配置、14个yaml参数文件、14个C算法源码如三维格点路径搜索与八叉树碰撞检测及头文件、编译脚本和可视化相关配置包体约5.04MB目录结构清晰便于按需二次开发。目前已有1654人学习使用。通过这套源码可以理解三维占据栅格地图构建、全局与局部规划器协同、姿态跟踪、传感器数据融合等关键环节还能借助rviz完成算法调试与结果验证并延伸到机器人导航性能优化思路。无论是课程项目还是产品原型这份资源都能提供从理论到实践的完整参照帮助缩短机器人导航系统的搭建与排错周期。1. 3d_navigation-hydro-devel 不是 move_baseSBPL 在三维网格里做搜索规划很多做机器人导航的工程师第一次打开3d_navigation-hydro-devel的源码列表都会愣一下environment_navxythetamlevlat.cpp、octomap_layer_projector.cpp、sbpl_lattice_planner_3d.cpp完全找不到 move_base 和 costmap_2d 的影子。它确实是 ROS 3D 导航相关实现但走的是 SBPLSearch-Based Planning Library这条学术路线用八叉树建图把三维代价环境投影成多分辨率高度图再在 (x, y, θ, m, lev) 五维离散状态空间里做 ARA* / AD* 启发式搜索。和 move_base“代价地图 实时采样”的黑盒组合不同这个包把环境抽象、碰撞判定、搜索器、执行层全部拆成独立 cpp可以逐个替换、批量实验、量化路径代价与搜索耗时。适合想深挖三维路径规划、复现论文实验或自研规划器的开发者hydro 版本虽老算法结构对 ROS 2 humble 时代的 3D 规划器设计仍然有参考价值。2. environment_navxythetamlevlat.cpp 与五维状态空间多分辨率格子环境怎么建2.1 多分辨率高度图为什么不能只建一张代价地图三维导航最朴素的做法是把点云栅格化到一张 2D 占用地图但这会丢失高度信息——桌子底下、坡道上方、悬空障碍全被拍平。environment_navxythetamlevlat.cpp的核心思路是用多个分辨率层级去描述同一块 XY 区域低分辨率层级负责大尺度连通性高分辨率层级负责机器人几何轮廓的精细校验。m和lev是这个环境里最容易混淆的两个维度lev是分辨率层号从粗到细编号m是当前层上的 XY 分块索引用于把不同分辨率下的格子坐标换算到全局坐标。提示SBPL 里这类环境名的后半段 mlev/lat 通常指 multi-level lattice。读源码时先看SetEnvParameter和Initialize两个入口能少走弯路。常见做法是把地图切成 4 层我一般会这样设分辨率层号网格边长典型用途lev00.4~0.5 m全局连通性检查lev10.2 m粗略地形通过性lev20.1 m机器人 footprint 预筛选lev30.025 m与 .bt 八叉树原分辨率对齐压缩包里的geb079_0.025.bt其中 0.025 就是这个包的最高分辨率体素边长。如果直接按 0.025 m 建一层 200×200×200 的状态空间状态数会到百万级搜索器在 hydro 时代根本跑不动分层后大部分搜索在粗层完成只有接近障碍时才下探到细层内存和搜索时间都能控制在可用范围。2.2 五维状态编码x、y、theta、m、lev 各自做了什么SBPL 的 lattice planner 和普通 A* 最大的区别是状态里带航向同样的 (x, y) 位置机器人朝东和朝西能执行的下一步运动完全不同所以 theta 必须进状态。再加上多分辨率分层状态就变成了五元组// environment_navxythetamlevlat 内部状态分量 struct State3D { int x; // 当前 lev 下的 x 网格索引 int y; // 当前 lev 下的 y 网格索引 int theta; // 航向离散编号0..num_theta-1 int m; // XY 分块编号跨层换算时的关键桥梁 int lev; // 分辨率层级0 为最粗 };这个五元组不是简单编码它决定了状态之间的邻接关系。theta 离散化数量直接决定搜索分支数num_theta16时每个状态最多向 16 个航向扩展num_theta32时转角颗粒度更细但后继节点数量接近翻倍内存和耗时都明显上涨。m是跨层换算的桥梁因为同一个格子在 lev0 和 lev3 下索引不同直接用 x、y 会让状态 ID 冲突所以用分块号把层与层衔接起来。初始化时通过 cfg 文件读入cellsize_m、num_theta、levels等参数这些参数决定整个搜索图规模。2.3 把环境成本写进格子SBPL 搜索器只认格子代价不关心你的碰撞检测是球模型还是网格模型。常见做法是在投影完成后把占据格的代价设为一个大数值可通行区域保持 0。我一般会这样写成本写入// 成本写入示意occupied 用大值unknown 用中间值 if (cell.log_odds 0.8f) { env.SetCost(x, y, theta, 1000); // 绝不可通行 } else if (cell.unknown) { env.SetCost(x, y, theta, 50); // 允许搜索器按风险自行决定 } else { env.SetCost(x, y, theta, 0); // 自由空间 }这里1000不是随便定的。SBPL 大多数环境把负代价视为未知高于obstacle_threshold的代价视为障碍。代价太高会让部分启发函数直接失效代价太低则路径会贴着障碍走。hydro 时代这套代码里常见的阈值在 5001000我一般保留环境默认值只在碰撞层做抽样测试确认阈值是否合理。成本写完后搜索器看到的是一张张带有“可通行 / 不可通行 / 未知”三态的分层网格。3. octomap_layer_projector 与 environment_nav_3d_collisions从八叉树叶子到碰撞代价3.1 为什么要投影而不是直接查八叉树OctoMap 适合增量建图和占用状态查询但它按体素存储没有“某个机器人三维姿态是否碰撞”的直接答案。3d_navigation 包把这两件事拆开octomap_layer_projector.cpp负责把八叉树叶子映射到多分辨率高度图environment_nav_3d_collisions.cpp负责按机器人 footprint 在被映射后的格子上判定碰撞。压缩包里的geb079_0.025.bt是二进制八叉树地图文件用 octomap 库加载后先遍历叶子再写高度图最后才交给碰撞层。为什么要多这一步投影因为八叉树是三维体素而 lattice planner 的碰撞检查需要快速回答“机器人中心放在这个格子、朝向某个角度会不会撞”。如果每次 replan 都重新查一遍八叉树搜索器展开几千个节点时性能会迅速恶化投影成高度图之后碰撞检查变成几次查表和比较速度完全不是一个量级。3.2 投影循环一个典型的叶子遍历实现// 示意将 octomap 叶子按体素边长投影到对应分辨率层 void projectLeafToHeightMap(const octomap::OcTree tree, MultiLevelHeightMap map) { for (octomap::OcTree::leaf_iterator it tree.begin_leafs(), end tree.end_leafs(); it ! end; it) { if (tree.isNodeOccupied(*it)) { octomap::point3d p it.getCoordinate(); double size it.getSize(); int lev map.levelBySize(size); // 体素越大进越粗的层 map.set(lev, p.x(), p.y(), p.z()); // 记录该层地表最高点 } } }叶子迭代器遍历整棵八叉树isNodeOccupied用占用阈值判断节点是否被占据getCoordinate取节点中心坐标getSize取体素边长。体素边长会随 octomap 更新时的合并而变化离传感器远的区域体素大投影到粗层近处点云密保留在高分辨率层。这也是为什么geb079_0.025.bt里的 0.025 只代表最细粒度而不是所有区域都用这个分辨率。投影完成后高度图还只是地形表面不是最终代价真正的 cost 要交给碰撞环境来生成。3.3 碰撞环境参数与调试方法environment_nav_3d_collisions.cpp的工作方式通常是对每个候选位姿 (x, y, theta)遍历机器人轮廓覆盖的格点只要任一格点高度差超过阈值就记一次碰撞。和 move_base 的 footprint 不同这里检查的是三维轮廓而不是地面投影。常用参数如下参数作用常见初始值footprint_radiusfootprint 外接圆半径0.25~0.35 mrobot_height机器人高度决定悬空障碍判定0.7 mmax_slope允许的最大地形斜率0.4obstacle_cost碰撞格点最终代价1000这几个参数互相制约footprint_radius设得太大窄通道全部被堵死max_slope设得太小稍陡的坡会被判定为障碍。调试时我一般会单独写一个测试节点给定开始和目标状态调用环境接口的IsValidState检查某个状态是否有效并打印状态 ID 和对应世界坐标确认碰撞判定是否符合预期。这个入口在demo_3dnav.cpp里已经搭好了框架稍加改造就能变成自己的状态合法性测试工具。4. sbpl_lattice_planner_3d.cppmprim 运动原语与 ARA*/AD* 搜索参数4.1 planner 初始化和一次 replan 调用sbpl_lattice_planner_3d.cpp把环境和搜索器接在一起。核心调用链不复杂先初始化环境再创建 planner设置起止状态最后 replan。下面是一次典型调用#include sbpl/planners/sbpl_lattice_planner.h EnvironmentNAVXYTHETAMLEVLAT env; env.Initialize(nav.cfg); // 读入分层网格参数与 mprim 路径 SBPLPlanner* planner new SBPLPlanner( env, /* forward_search */ false, env_cfg); planner-set_start(start_state_id); planner-set_goal(goal_state_id); planner-set_search_mode(true); // 允许 planner 自主决定何时停止 planner-set_initial_epsilon(3.0); // 起始次优度 std::vectorint solution; int cost 0; int ret planner-replan(solution, cost);SBPLPlanner是 sbpl 库的通用接口第一个参数指向环境对象forward_searchfalse表示允许反向搜索但如果机器人运动模型不支持倒车要改回 true。set_initial_epsilon(3.0)意思是先跑出一个最大 3 倍次优的解再在时间余量里持续优化。replan返回状态码常见约定是 0 表示成功非 0 表示搜索失败或超时。4.2 planner_t 的选择ARA* 还是 AD*sbpl_lattice_planner_3d.cpp里的planner_t参数决定底层算法不同 sbpl 版本的映射基本一致planner_t算法适用场景0ARA*Anytime Repairing A*静态地图、要求快速出解1AD*Anytime Dynamic A*地图增量更新、障碍物移动频繁2R* 或 Lazy ARA*版本不同有差异大场景粗略规划ARA* 在给定 epsilon 下先快速生成一个次优解再在时间窗口内不断收紧 epsilon 优化路径。如果机器人停在原地做全局规划ARA* 是最稳的选择。AD* 更适合环境动态变化的情况比如地图里突然多了一个障碍物它不会整张图重搜而是增量更新。代价是 AD* 维护 open、closed、incons 三套状态集合内存占用明显更高。hydro 时代的论文实验脚本里对比最多就是这两个算法。4.3 mprim 运动原语搜索图是怎么长出来的搜索器不是随便走格子而是按 mprim 文件里定义的运动原语生成后继节点。每个原语记录从当前 (x, y, theta) 出发的一组中间网格占用和终点位姿// mprim 描述一个运动原语起始航向 - 目标航向 struct MotionPrim { int start_theta; // 起始离散航向 int end_theta; // 终点离散航向 int end_x, end_y; // 以起始格为原点的偏移 double cost_multiplier; // 时间或能耗代价系数 std::vectorCellXY cells; // 原语覆盖的格子供碰撞检测 };mprim 直接影响搜索图的质量和路径的可执行性。0.1 m 分辨率环境和 0.025 m 分辨率环境应该配不同的 mprim如果直接把小车 mprim 用在大底盘机器人上会在转角处产生大量无效碰撞测试路径看起来“能走”实际执行时却频频卡住。调参优先级我一般这样排先选对 mprim再调 epsilon最后才看启发函数类型。这和 move_base 里先调膨胀半径再调路径权重的思路完全不同。5. pose_follower_3d 与 experiment_script批量实验反哺 mprim 参数5.1 从离散路径到连续位姿SBPL planner 输出的是一串状态 ID不能直接发给底盘控制器。pose_follower_3d.cpp的作用是把状态 ID 逐段翻译成带时间戳的 3D 位姿按 lookahead 距离从前向后取目标位姿再发布速度指令。常见做法是纯跟踪控制横向偏差转成角速度纵向偏差转成线速度。几个关键参数参数作用我常用的初始值lookahead前瞻距离越大路径越平滑1.2 mmax_lin_vel线速度上限0.5 m/smax_rot_vel角速度上限0.5 rad/sgoal_tolerance到达目标判定阈值0.1 m / 0.15 radlookahead是这里最敏感的参数太小会导致机器人绕着路径点反复修正航向太大则会在窄通道里切弯。调 pose_follower 之前先把上一章的 planner 路径用 rviz 里的 Path 显示出来确认路径本身没有异常抖动否则执行层怎么调都救不回来。5.2 用 experiment_script 做 epsilon 参数扫描experiments.cpp、experiment_script1.cpp、experiment_script2.cpp是批量测试框架。它们存在的意义不是单次跑通而是在不同 epsilon、不同 level 数、不同 mprim 下反复 replan记录 cost、planning time 和 path length。如果是从零搭 hydro 环境建议直接用 Docker 里的ros:hydro-ros-base镜像Ubuntu 20.04 上硬装 hydro 的 Debian 包早已不可用鱼香ROS一键安装脚本主要覆盖 noetic 和 humblehydro 依赖环境更适合容器化处理。批量扫描可以这样写// experiment_script1 风格扫描 epsilon 并记录到 CSV std::ofstream out(eps_scan.csv); for (double eps 1.0; eps 10.0; eps 0.5) { planner-set_initial_epsilon(eps); auto t0 steady_clock::now(); int ret planner-replan(path, cost); auto t1 steady_clock::now(); out eps , cost , duration_ms(t1 - t0).count() \n; }eps1.0 时 ARA* 等价于 A*最优但慢eps10 时能秒出次优解。这个扫描的价值在于同一张地图、同一个起始目标不同 epsilon 下的路径代价差异可能高达 30% 以上。建议同一组参数至少跑 10 次取中位数避免刚启动时的 cache 冷启动影响耗时数据。5.3 把批量结果反哺到 mprim 和 level 配置拿到eps_scan.csv后如果发现某组 level 配置下路径频繁绕路说明该层分辨率的格子边长偏大膨胀效果太重应该把对应层的网格边长下调一档比如从 0.2 m 改成 0.1 m如果路径贴近障碍但执行时频繁急停则说明obstacle_cost阈值偏低路径规划阶段没有把危险区域真正排斥掉。此时重新生成 mprim 并跑一次 demo 验证# 示意命令重新加载 mprim 和八叉树地图做单次演示对比 rosrun 3d_navigation demo_3dnav --config nav.cfg \ --bt geb079_0.025.bt --mprim nav.mprim --planner 0把实验脚本里记录的耗时和代价做成表格对比 planner_t0 与 planner_t1 在同一走廊地图上的表现你会发现 AD* 在静态地图上并不会有性能优势反而多出增量集合的维护开销只有当地图里频繁出现新增障碍时AD* 的增量重规划特性才能真正兑现。这个结论不是看文档得出来的是跑完 experiment_script 之后从数据里看到的。本文还有配套的精品资源点击获取