最近在调试一个四足机器人的导航模块遇到一个很具体的问题当机器人需要跨越不同楼层比如从一楼大厅上到二楼走廊时现有的全局路径规划器给出的路线常常“不切实际”。它可能会规划出一条穿过墙壁、或者试图让机器人从楼梯扶手外侧“飞”上去的路径。这背后暴露的其实是传统导航算法在面对复杂、非平面的多层环境时对空间连通性理解的缺失。它们把世界看作一个二维或2.5维的平面却不知道一楼的天花板就是二楼的地板。这时一个名为SCAN-Planner的方案进入了视野。它不是一个全新的、从零构建的庞然大物而更像是一个精巧的“插件”或“增强模块”。其核心思想非常直观利用预先构建的、包含多层结构信息的三维地图如八叉树地图在规划路径时主动“扫描”并理解环境中的可通行表面如地板、斜坡、楼梯从而生成真正符合物理世界多层结构的可行轨迹。最近其Pure-ROS2 精简版在 GitHub 上开源剔除了对特定仿真或中间件的依赖使其能更干净地集成到基于 ROS 2 的机器人系统中。这个开源实现的价值不在于提供了一个“即插即用”的万能导航黑盒而在于清晰地揭示了一种思路将环境的三维结构信息转化为路径规划中的硬约束与软启发是解决跨楼层、非结构化场景导航的关键。对于正在自研机器人或深度定制导航栈的开发者来说研究它比直接调用它更有意义。1. 跨楼层导航的“坑”为什么传统方法会失效在开始研究 SCAN-Planner 之前我们必须先理解问题到底出在哪里。很多人以为给机器人装上激光雷达和 SLAM 建图就能实现全楼导航但实际一跑就发现机器人经常“卡住”或者规划出荒谬的路径。1.1 二维导航的“平面幻觉”最经典的导航栈如 ROS 1 的move_base或其 ROS 2 对应实现nav2其默认的全局规划器如NavFn或Global Planner通常工作在二维代价地图上。即使你用了三维传感器建图最终也会被压缩成一个二维的“占据栅格地图”Occupancy Grid Map。这张地图只有“可行走”和“障碍物”两种状态。问题来了当存在多层空间时二维地图无法区分不同高度上的障碍物。例如垂直重叠一楼大厅的吊灯和二楼走廊的地板在二维地图的 (x, y) 坐标上是重叠的。规划器无法知道机器人在一楼时吊灯是障碍而上到二楼后同一坐标点对应的地板却是可通行的表面。连通性误判楼梯在二维地图上通常被表示为一条狭窄的、被“障碍物”楼梯扶手、墙壁包围的通道。规划器可能因为这条通道太窄或地图噪声而认为其不可通行或者更糟它可能规划一条穿过一楼墙壁试图“水平”移动到二楼某个点的路径因为它不知道这两个点之间存在垂直方向的连接楼梯。1.2 2.5D 高程地图的局限为了解决高度问题一些方案引入了 2.5D 高程地图Elevation Map。它为每个 (x, y) 格子存储一个高度值。这确实进了一步能识别出斜坡、台阶的高度变化。但它的核心局限在于“每个位置只有一个高度”的假设。这在单层环境或简单地形中有效但在多层环境中就失效了。对于楼梯井这种位置同一个 (x, y) 点随着机器人爬升对应的高度值机器人脚下的地面高度是连续变化的。一个静态的、单一数值的高程图无法表达这种“垂直通道”的概念。规划器仍然缺乏对“空间在 Z 轴方向如何连通”的理解。1.3 路径“飞檐走壁”的根源综合以上两点传统规划器失效的根源就很清晰了信息缺失规划器使用的环境表示二维栅格或2.5D高程图丢失了关键的三维结构连通信息。约束缺失规划算法本身没有嵌入“机器人必须附着在可通行表面上”这一基本的物理运动约束。它只是在抽象的自由空间里搜索最短路径而这个“自由空间”可能包含了墙壁内部和楼层之间的虚无区域。SCAN-Planner 的出发点正是为了弥补这两个缺失。2. SCAN-Planner 的核心思路从“搜索空间”到“扫描表面”SCAN-Planner 不是一个替代A*或Dijkstra的全新搜索算法而是一个前端处理层。它的工作是在调用传统规划器之前先对世界进行一番理解构建一个对多层导航更友好的“规划上下文”。2.1 核心输入八叉树地图SCAN-Planner 强依赖一个关键输入三维占据栅格地图通常以八叉树OctoMap的形式存储。八叉树是一种高效表示三维空间占据状态的数据结构它能告诉我们空间中每个小体素voxel是空闲的、占据的还是未知的。与二维栅格地图相比八叉树完整保留了环境的垂直结构信息。楼梯井、中空的大厅、不同楼层的房间布局都在八叉树中有清晰的表达。2.2 “扫描”可通行表面这是 SCAN-Planner 得名的关键步骤。给定一个起点和目标点都是三维坐标算法不会直接去搜索。它会先以这两个点为中心在一定半径内对八叉树地图进行一种特殊的“扫描”表面提取算法会识别出地图中那些可以被视为“可通行表面”的占据体素。这通常意味着是接近水平的平面如地板、楼梯踏板并且其上方有一定的自由空间供机器人通过。连通性分析算法会分析这些提取出来的表面在三维空间中是如何连接的。比如一段楼梯的踏板表面会连接一楼地面和二楼地面。生成“可达图”最终算法会生成一个简化的图Graph。这个图的节点代表一个个可通行的“表面区域”或“落脚点”边代表这些区域之间在物理上是连通的例如通过斜坡或楼梯相连。这个图才是后续路径规划器真正工作的搜索空间。2.3 与传统流程的对比我们可以用一个表格来清晰对比传统流程和融入 SCAN-Planner 的流程步骤传统导航流程 (如 nav2)融入 SCAN-Planner 的流程1. 环境表示二维占据栅格地图 / 2.5D 高程地图三维八叉树地图(核心基础)2. 规划预处理膨胀障碍物生成代价地图扫描起点和目标点周围的可通行表面分析三维连通性3. 搜索空间二维栅格的所有空闲格子由可通行表面及其连接关系构成的图4. 路径搜索在二维栅格上运行 A* 等算法在“可达图”上运行搜索算法路径由一系列表面节点构成5. 输出路径一系列 (x, y) 坐标点一系列 (x, y, z) 坐标点且每个点都附着在具体的可通行表面上核心优势计算快适合平面环境路径在物理上真实可行天然支持多层、非平面环境通过这个流程SCAN-Planner 确保了规划器只在机器人实际能站上去的地方进行搜索从根本上杜绝了“穿墙”和“空中飞人”式的路径。3. Pure-ROS2 精简版开源实现如何上手与集成这次开源的SCAN-Planner-Pure-ROS2版本可以看作是一个“内核”或“算法库”。它剥离了原版中可能存在的、与特定机器人仿真平台或早期通信中间件的强耦合使其成为一个更纯粹的 ROS 2 节点/库。3.1 环境准备与依赖项目通常需要以下核心依赖ROS 2推荐 Humble 或 Rolling 版本。这是通信和节点管理的基础。OctoMap用于提供三维地图数据。通常通过octomap和octomap_msgsROS 2 包安装。Eigen3用于矩阵和几何计算。PCL (Point Cloud Library)可选但常用于处理点云数据这些数据可能用于生成或更新八叉树地图。安装依赖的典型命令如下以 Ubuntu 和 ROS 2 Humble 为例# 安装 ROS 2 和核心工具 sudo apt update sudo apt install ros-humble-desktop # 安装 OctoMap 相关 sudo apt install ros-humble-octomap* ros-humble-octomap-msgs # 安装 Eigen3 和 PCL sudo apt install libeigen3-dev libpcl-dev注意在克隆和编译 SCAN-Planner 仓库前务必确认你的 ROS 2 工作空间已正确配置 (source /opt/ros/humble/setup.bash)并且所有依赖包都能找到。3.2 核心节点与话题理解其 ROS 2 接口是集成的关键。精简版通常会提供至少一个可运行的节点。你需要关注以下几个核心话题输入话题/octomap_full或/octomap_binary(octomap_msgs/Octomap)最重要的输入提供当前环境的三维八叉树地图。/goal_pose(geometry_msgs/PoseStamped)三维目标点应包含有意义的 Z 坐标高度。/start_pose(geometry_msgs/PoseStamped)三维起始点。有时起始点可能通过服务调用或参数设置。输出话题/global_plan或/path(nav_msgs/Path)规划出的全局路径。这条路径的每个位姿点都将是三维的 (pose.position.x, y, z)并且其 Z 坐标是贴合在扫描出的可通行表面上的。/scanning_surfaces(可视化消息如visualization_msgs/MarkerArray)用于在 Rviz2 中可视化算法扫描出的可通行表面对于调试和理解算法行为至关重要。服务/参数可能提供重规划服务。通过 ROS 2 参数服务器配置扫描半径、表面法线阈值、最小表面面积等关键参数。3.3 最小集成示例假设你已经有一个能发布/octomap_full的 SLAM 模块如基于激光雷达或深度相机的octomap_server集成 SCAN-Planner 的基本步骤如下编译与启动将 SCAN-Planner 源码放入你的 ROS 2 工作空间编译 (colcon build)然后启动其节点。source install/setup.bash ros2 run scan_planner scan_planner_node配置参数根据你的机器人大小和楼梯尺寸调整扫描半径和表面检测参数。例如扫描半径需要大于单级楼梯的跨度。提供地图与目标确保octomap_server在运行并发布地图。通过 Rviz2 或程序发布一个三维目标位姿。观察与调试在 Rviz2 中订阅/global_plan和/scanning_surfaces。你应该能看到一条从起点到目标点的、贴合在楼梯或斜坡表面的三维路径同时能看到算法识别出的那些绿色或其它颜色的可通行表面区域。路径消费将规划出的/global_plan路径输入到你的局部规划器如TEB、MPC或控制器中。这里有一个关键点你的局部规划器/控制器也需要能处理三维路径点或者你需要将三维路径投影到机器人当前所处的局部二维平面上进行跟踪。4. 从“跑通Demo”到“工程实用”的关键考量让 SCAN-Planner 在仿真里规划出一条漂亮的跨楼层路径只是第一步。要将其用于真实的机器人或长期运行的系统中以下几个方面的考量至关重要。4.1 地图质量是生命线SCAN-Planner 的输出质量完全取决于输入地图的质量。“垃圾进垃圾出”在这里体现得淋漓尽致。建图完整性楼梯区域必须被完整、准确地重建。缺失的台阶或噪声都会导致表面扫描失败从而找不到连通路径。地图更新在动态环境中八叉树地图需要能在线更新。SCAN-Planner 能否处理动态变化的地图规划出的路径是否会因为地图局部更新而突然失效这需要测试。计算与存储高分辨率、大范围的八叉树地图对内存和计算都是挑战。需要权衡地图精度与系统性能。实操建议先用一个静态的、预先建好的高质量八叉图进行算法验证和参数调优。确保在这个“理想地图”上SCAN-Planner 能稳定工作后再接入在线 SLAM 系统。4.2 参数调优理解每个旋钮的含义开源实现通常会暴露一系列参数不理解它们就调参如同盲人摸象。scanning_radius围绕起点和目标点的扫描半径。太小可能找不到连接上下楼层的表面如楼梯的中间部分太大会增加不必要的计算量并可能引入无关的表面区域。min_surface_area/voxel_min_z,voxel_max_z定义什么是“可通行表面”。这些参数需要根据你的机器人底盘大小、离地间隙和跨越能力来设置。它们过滤掉太小、太陡或高度不合适的表面。normal_threshold用于判断表面是否“水平”的法线角度阈值。对于楼梯这个值需要适当放宽因为楼梯踏板并非完全水平。connection_height_threshold判断两个表面是否“连通”的最大允许高度差。这个值决定了算法是否认为你可以从一块地板跨到另一块地板或上一级台阶。调试方法论开启表面可视化 (/scanning_surfaces)然后逐个参数调整观察可视化结果的变化。你会直观地看到哪些区域被算法识别为“可通行”它们又是如何被连接起来的。这是理解算法行为最有效的方式。4.3 与下游模块的衔接规划出一条三维路径只是成功了一半。如何让机器人执行这条路径局部规划与控制器大多数轮式或足式机器人的底层控制器仍然期望一个二维的、基于机体坐标系的速度或位姿指令。你需要一个能理解三维路径的局部规划器或者一个将三维路径点实时投影到机器人当前姿态所在局部二维平面的模块。重规划与实时性SCAN-Planner 的“扫描-分析”过程比传统的二维 A* 更耗时。在机器人移动过程中如果环境变化或偏离路径需要触发重规划。重规划的频率和计算耗时必须在你的系统可接受范围内。异常处理如果算法找不到路径表面不连通系统应该如何反应是报错停止还是尝试回退到传统的二维规划必须有兜底策略。4.4 适用边界与不适用场景清醒地认识工具的边界比盲目乐观更重要。适用场景已知的、结构化的多层室内环境如办公楼、商场、图书馆、地铁站。环境地图相对静态楼层间通过楼梯、斜坡、电梯井连接。不适用或需谨慎的场景完全未知的探索算法依赖预先存在的、质量尚可的八叉树地图。在完全未知环境中进行前沿探索不是它的强项。高度动态环境如果环境中移动障碍物太多地图更新和重规划的压力会很大。非结构化野外地形对于岩石、草地、泥泞等没有清晰“表面”概念的地形基于表面扫描的方法可能失效。对实时性要求极高的场景如果机器人需要每秒规划多次如高速避障本方法的计算开销可能过大。SCAN-Planner-Pure-ROS2 的开源提供了一个绝佳的“透视镜”让我们能深入看到解决复杂导航问题的另一种可能路径。它或许不会立刻成为你机器人上的默认规划器但它所蕴含的**“将三维结构信息转化为规划约束”** 的思想是任何希望让机器人走出简单平面、进入真实多层世界的开发者都必须认真思考和借鉴的。真正的价值不在于代码本身而在于它为我们打开的那一扇理解空间、连接逻辑与物理世界的窗。