最近在调试一个四足机器人的导航模块，遇到一个很具体的问题：当机器人需要跨越不同楼层，比如从一楼大厅上到二楼走廊时，现有的全局路径规划器给出的路线常常“不切实际”。它可能会规划出一条穿过墙壁、或者试图让机器人从楼梯扶手外侧“飞”上去的路径。这背后暴露的，其实是传统导航算法在面对复杂、非平面的多层环境时，对空间连通性理解的缺失。它们把世界看作一个二维或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 路径“飞檐走壁”的根源

综合以上两点，传统规划器失效的根源就很清晰了：

1. 信息缺失：规划器使用的环境表示（二维栅格或2.5D高程图）丢失了关键的三维结构连通信息。
2. 约束缺失：规划算法本身没有嵌入“机器人必须附着在可通行表面上”这一基本的物理运动约束。它只是在抽象的自由空间里搜索最短路径，而这个“自由空间”可能包含了墙壁内部和楼层之间的虚无区域。

SCAN-Planner 的出发点，正是为了弥补这两个缺失。

2. SCAN-Planner 的核心思路：从“搜索空间”到“扫描表面”

SCAN-Planner 不是一个替代A*或Dijkstra的全新搜索算法，而是一个前端处理层。它的工作是在调用传统规划器之前，先对世界进行一番理解，构建一个对多层导航更友好的“规划上下文”。

2.1 核心输入：八叉树地图

SCAN-Planner 强依赖一个关键输入：三维占据栅格地图，通常以八叉树（OctoMap）的形式存储。八叉树是一种高效表示三维空间占据状态的数据结构，它能告诉我们空间中每个小体素（voxel）是空闲的、占据的还是未知的。

与二维栅格地图相比，八叉树完整保留了环境的垂直结构信息。楼梯井、中空的大厅、不同楼层的房间布局，都在八叉树中有清晰的表达。

2.2 “扫描”可通行表面

这是 SCAN-Planner 得名的关键步骤。给定一个起点和目标点（都是三维坐标），算法不会直接去搜索。它会先以这两个点为中心，在一定半径内，对八叉树地图进行一种特殊的“扫描”：

1. 表面提取：算法会识别出地图中那些可以被视为“可通行表面”的占据体素。这通常意味着是接近水平的平面（如地板、楼梯踏板），并且其上方有一定的自由空间供机器人通过。
2. 连通性分析：算法会分析这些提取出来的表面在三维空间中是如何连接的。比如，一段楼梯的踏板表面会连接一楼地面和二楼地面。
3. 生成“可达图”：最终，算法会生成一个简化的图（Graph）。这个图的节点代表一个个可通行的“表面区域”或“落脚点”，边代表这些区域之间在物理上是连通的（例如通过斜坡或楼梯相连）。这个图，才是后续路径规划器真正工作的搜索空间。

2.3 与传统流程的对比

我们可以用一个表格来清晰对比传统流程和融入 SCAN-Planner 的流程：

通过这个流程，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 的基本步骤如下：

1. 编译与启动：将 SCAN-Planner 源码放入你的 ROS 2 工作空间，编译 (colcon build)，然后启动其节点。

   source install/setup.bash ros2 run scan_planner scan_planner_node

2. 配置参数：根据你的机器人大小和楼梯尺寸，调整扫描半径和表面检测参数。例如，扫描半径需要大于单级楼梯的跨度。

3. 提供地图与目标：确保octomap_server在运行并发布地图。通过 Rviz2 或程序发布一个三维目标位姿。

4. 观察与调试：在 Rviz2 中订阅/global_plan和/scanning_surfaces。你应该能看到一条从起点到目标点的、贴合在楼梯或斜坡表面的三维路径，同时能看到算法识别出的那些绿色（或其它颜色）的可通行表面区域。

5. 路径消费：将规划出的/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 与下游模块的衔接

规划出一条三维路径只是成功了一半。如何让机器人执行这条路径？

1. 局部规划与控制器：大多数轮式或足式机器人的底层控制器仍然期望一个二维的、基于机体坐标系的速度或位姿指令。你需要一个能理解三维路径的局部规划器，或者一个将三维路径点实时投影到机器人当前姿态所在局部二维平面的模块。
2. 重规划与实时性：SCAN-Planner 的“扫描-分析”过程比传统的二维 A* 更耗时。在机器人移动过程中，如果环境变化或偏离路径，需要触发重规划。重规划的频率和计算耗时必须在你的系统可接受范围内。
3. 异常处理：如果算法找不到路径（表面不连通），系统应该如何反应？是报错停止，还是尝试回退到传统的二维规划？必须有兜底策略。

4.4 适用边界与不适用场景

清醒地认识工具的边界，比盲目乐观更重要。

• 适用场景：已知的、结构化的多层室内环境，如办公楼、商场、图书馆、地铁站。环境地图相对静态，楼层间通过楼梯、斜坡、电梯井连接。
• 不适用或需谨慎的场景：
  • 完全未知的探索：算法依赖预先存在的、质量尚可的八叉树地图。在完全未知环境中进行前沿探索不是它的强项。
  • 高度动态环境：如果环境中移动障碍物太多，地图更新和重规划的压力会很大。
  • 非结构化野外地形：对于岩石、草地、泥泞等没有清晰“表面”概念的地形，基于表面扫描的方法可能失效。
  • 对实时性要求极高的场景：如果机器人需要每秒规划多次（如高速避障），本方法的计算开销可能过大。

SCAN-Planner-Pure-ROS2 的开源，提供了一个绝佳的“透视镜”，让我们能深入看到解决复杂导航问题的另一种可能路径。它或许不会立刻成为你机器人上的默认规划器，但它所蕴含的**“将三维结构信息转化为规划约束”** 的思想，是任何希望让机器人走出简单平面、进入真实多层世界的开发者，都必须认真思考和借鉴的。真正的价值不在于代码本身，而在于它为我们打开的那一扇理解空间、连接逻辑与物理世界的窗。