企业官网建设流程全解析

四足机器人做跨楼层规划，听起来是个挺硬的课题，但这几年随着宇树、MIT这类开源平台普及，愿意折腾的人越来越多了。我自己在Go1上跑过一段时间纯2D导航，一遇到楼梯就“哑火”——雷达能扫到台阶但代价地图不知道怎么处理，局部规划器直接把台阶当成障碍物，或者干脆绕路绕到死胡同。后来换了SCAN-Planner-Pure-ROS2这套方案，才慢慢把跨楼层的链路跑通。这篇东西就是给想复现、想少走弯路的兄弟写的，内容覆盖原理、环境、实操和排坑，照着做基本能把系统转起来。

SCAN-Planner-Pure-ROS2本质上是一套面向3D环境的机器人运动规划开源实现，底层基于ETH的SPT算法（Search-based Planning Lab的SCAN Planner），社区里有人把它从ROS1迁移到了ROS2，并且针对四足机器人做了步态约束适配。最直观的价值是：它能把激光雷达点云灌进八叉树地图，在包含楼梯、斜坡、平台的三维空间里搜索出一条满足机器人运动学约束的轨迹，这样四足机器人就能真正“跨楼层”而不是只在平地上转圈。

适合谁来参考？一类是正在做四足导航、但被2D导航框架困住的研究生和工程师；另一类是想在Gazebo里做仿真验证、又不想自己从零写3D规划算法的朋友。哪怕你用的是轮式底盘，只要环境里有坡道、楼梯这类非平面地形，这套思路也值得看。

1. 四足机器人跨楼层规划到底难在哪

1.1 这不是一个“把A*升级成3D版”那么简单的事

很多人的第一反应是：平面导航用A*、Dijkstra，跨楼层就把搜索空间从2D网格变成3D体素不就行了？实际操作下来会发现这思路有两个大坑。

第一个坑是搜索维度和状态空间爆炸。2D导航时机器人的状态是(x, y, yaw)，三个自由度。到了跨楼层场景，姿态里加了pitch和roll，因为上下楼梯时车身一定是倾斜的，这就变成五六个自由度的状态搜索。再加上3D体素地图本身的分辨率要求，A这类算法在生存时间内根本搜不出来。我试过用普通A在0.05m分辨率的3D OctoMap里直跑，规划超时是家常便饭，更别提轨迹还要满足腿部可落地性。

第二个坑是可通行性判断。平面导航里判断一个格子能不能走，基本就看有没有障碍物。跨楼层时你得知道台阶的坡度、台阶边缘的高度差、机器人腿部摆动空间够不够。这就是为什么很多看起来能用的3D规划器，真放到楼梯前就“虚了”——它在几何层面找到了一条穿越路径，但这条路径腿根本够不着或者会打滑。

SCAN Planner的方式是用折线轨迹在膨胀地图里做约束搜索，节点的扩展方式跟普通图搜索完全不一样。它生成的轨迹是带曲率约束的“折线+样条”混合形式，天然适合机器人这种不能原地旋转、不能瞬移的运动体。加上四足步态约束之后，轨迹上的每个点的车身高度、俯仰角、横滚角都在可行域里，这就比裸A*靠谱得多。

1.2 为什么选SCAN-Planner-Pure-ROS2这套方案

选这套方案有几个很现实的原因。

首先是工程链路完整。整个项目从点云采集、八叉树建图、SPT规划到速度指令输出全部用ROS2原生节点串起来，不像很多学术代码那样只有算法没有驱动，也不像某些商业方案黑盒到调不了参。你想理解某一环，直接看源码和Topic就能理清。

其次是纯ROS2实现。现在Humble、Jazzy都成熟了，ROS1停更是大趋势，很多新买的激光雷达、IMU驱动都默认只发布ROS2消息。这套方案直接用rclcpp、sensor_msgs、nav_msgs这些ROS2标准接口，省去了ROS1到ROS2桥接的麻烦。

第三点是可扩展性。项目里把规划器、地图服务器、可视化层分得很清楚，你甚至可以把四足步态约束评估器摘出来换掉，接自己的腿部规划器。我自己就在上面加了一个自定义的楼梯检测节点，规划器不用改一行代码，只通过Topic订阅新增的语义信息。

1.3 项目能解决什么问题，适合谁参考

这个方案解决的核心问题是：让机器人在包含楼梯、斜坡、错层平台的复杂3D地形里，规划出一条不仅几何可达、而且运动学可行的轨迹。

• 对于做多楼层巡检、仓储跨层搬运的朋友，这套方案可以直接用于样机验证，减少从0到1的时间。
• 对于做算法研究的人，SCAN Planner和OctoMap的代码很工整，适合作为SPT、八叉树、运动约束搜索的“活教材”。
• 对于只做仿真验证的朋友，Gazebo里搭一个带楼梯的楼层环境，配合这套规划器，能提前暴露很多实机才会出现的问题，比如轨迹抖动、地图分层错位。

一句话总结：它是从“平面导航能用”迈向“跨楼层导航能跑”之间，一块非常顺手的跳板。

2. 核心原理拆解：从SPT到八叉树地图

2.1 SPT搜索规划器的底层逻辑

SPT全称是Search-based Planning Tool，是ETH自动驾驶实验室开源的运动规划库的一部分。SCAN Planner本质上是用SPT库里的折线轨迹在占据栅格地图上做搜索，所以理解SPT的底层逻辑很重要。

稍微有点抽象，我用白话拆一遍。SPT的思路是：不用离散的格点来描述机器人轨迹，而用“折线段”来描述可能的运动趋势。每次节点扩展时，不是向上、下、左、右各走一格，而是尝试一系列带曲率的方向，比如“向前直走0.3米”“左转15度走0.5米”“右转20度走0.4米”。每条折线再根据前轮转角或者可转向能力做平滑，变成机器人真正能走的曲线路径。

这套做法有两个好处：

• 轨迹天然满足运动学约束。因为扩展方向就是机器人能走的方向，而不是网格上“能走就走”的简化，规划出来的轨迹几乎不需要再做后处理平滑，直接可以下发给底盘。
• 搜索效率高。它结合了A的启发式搜索框架和运动学采样的扩展方式，在3D复杂地形里能在数百毫秒到一两秒内找到可行轨迹，而不像RRT那样需要反复采样、做碰撞检测。

在SCAN-Planner-Pure-ROS2里，坐标轴被映射到机器人的可通行空间，z轴方向不是完全自由的——机器人不能飞，也不能穿地，只能沿着地面、楼梯斜面走。所以搜索时会对每条折线轨迹做投影和地形贴合检查，这也是跨楼层规划能成立的先决条件。

2.2 OctoMap在跨楼层场景下的关键作用

地图部分用的是OctoMap八叉树占据地图，常见实现是octomap_server，它订阅点云话题，把点云更新进一棵八叉树里。

为什么强调八叉树而不是普通3D体素栅格？很简单：内存和更新效率。普通3D网格是把空间切成均匀小方块，整栋楼按0.05m分辨率切下来，内存直接爆炸。八叉树有“懒惰展开”机制，大块空白区域用一个大节点表示，有障碍物的区域才递归细化。这样一栋几百平米的办公楼，点云地图体积能控制在几十MB以内，规划时查询占据状态也是O(1)或O(log n)级别。

跨楼层场景里，OctoMap还有一个特殊价值：它对动态物体的更新是增量式的。比如某一楼层有人在走动，octomap_server可以通过多帧点云把动态点清除。楼梯口这种人员流动大的区域，动态点越多，对规划影响越大，增量更新能显著减少误判。

不过要提醒一点：OctoMap的“占据”和“空闲”判断取决于体素内点云密度和阈值。设置不当会出现“透明楼梯”或“厚墙”两种极端。后面实操部分我会给参数经验值。

2.3 四足步态约束怎么融入轨迹生成

这是SCAN-Planner-Pure-ROS2最值得看的地方。它不单单输出一条空间曲线，而是在每个轨迹点上附加一个“状态可行性检查”。

四足机器人不是Acrobot那样的全向移动体，它有明显的步态特性：

• 身体可以有一个倾斜角（上下坡、上下楼时），但倾斜角有极限。
• 每条腿的摆动空间有限，台阶太高跨不上去。
• 行走时至少需要三条腿支撑，也就是说身体重心必须落在支撑多边形里。

所以这套项目里加了一个约束评估层：当SPT搜索产生一条候选轨迹后，评估器沿轨迹采样多个点，对每个点检查机器人状态是否落在stair-gait的可行域里。一旦某个点超出限制，这条候选轨迹直接剪枝，不进入代价评估。

我建议你先用默认参数跑一遍，再动手改这些约束。默认值会保守一些，轨迹成功率更高，但路径会比较绕。熟悉之后再逐步放开俯仰角限制，你就能看到轨迹怎么变得越来越“胆大”。注意每次改约束都要在仿真里先验，别直接拿实机试，四足翻车的维修成本你是懂的。

3. 工程落地：环境配置与ROS2架构

3.1 软件栈和依赖安装

我在Ubuntu 22.04 + ROS2 Humble和Ubuntu 24.04 + ROS2 Jazzy上都编译通过，两个版本差别不大。这里以Humble为主线讲。

安装依赖时要留意，有些包名字容易搞混：

sudo apt install ros-humble-desktop sudo apt install ros-humble-octomap ros-humble-octomap-msgs sudo apt install ros-humble-octomap-server sudo apt install ros-humble-nav2-msgs sudo apt install ros-humble-tf2-eigen sudo apt install ros-humble-rviz2

另外还需要Eigen和OMPL（可选，部分版本用于碰撞检测后端）。

sudo apt install libeigen3-dev sudo apt install libompl-dev

编译工作区的方式很常规：

mkdir -p ~/scanner_ws/src cd ~/scanner_ws/src git clone https://github.com/your-fork/SCAN-Planner-Pure-ROS2.git cd ~/scanner_ws colcon build --symlink-install source install/setup.bash

有几点要提醒：

1. 第一次编译时间会比较长，SCAN Planner依赖很多，建议先编译单独包，colcon build --packages-select scan_planner，有问题好定位。
2. Jazzy版本有些包改了API，比如TF2的消息类型和时间戳处理，如果你在Jazzy上编译报错，多半是tf2_ros::Buffer接口变了，改一下头文件引用就能过。
3. 不建议用二进制安装的octomap_server直接跑跨楼层场景，因为很多场景参数需要改launch文件里面的参数，源码编译后调试起来方便得多。

3.2 节点架构与Topic设计

启动后整套系统大致有这些节点在跑：

这套架构的精华在于规划器并不直接控制腿部，它只输出全局轨迹和期望速度。真正的腿部控制交给底层步态控制器（比如宇树的运动控制SDK或者别的MPC/WBC控制器）。规划器给的/cmd_vel是线速度和角速度指令，底层控制器负责把它翻译成腿的摆动序列。

这种分层架构的好处很明显：

• 规划器和步态控制器解耦，换机器人平台不用改规划器。
• 底层步态控制器可以有自己的安全逻辑，比如检测到腿打滑就减速或者暂停。

3.3 硬件选型建议

如果你手里已经有四足机器人，那我建议优先看这几类传感器：

• 激光雷达：我实测Livox MID-360的效果很好，视场角大（360°×59°），跨楼层时楼梯上方和下方的点云都能扫到，而且它对低纹理的白色墙面适应性比深度相机好太多。AVIA也可以，但视场角略小，室内小空间楼梯容易“扫不全”。
• 深度相机：RealSense D435i可以作为补充，主要用来检测近距离楼梯边缘和台阶高度。不过阳光强的环境下深度相机容易失效，室内用没问题，户外跨楼层还是以激光雷达为主。
• IMU：用雷达自带的IMU也行，但建议买一个外部IMU（如VN-100），输出频率更高，对地图分层问题的缓解非常明显。

没有实机的话，仿真里用Gazebo Classic加载带楼梯的模型完全够用。仿真和实机最大的区别在于里程计噪声：仿真里程计几乎零漂移，规划器怎么跑都稳，实机跑几分钟就可能出现地图错位、轨迹偏离。所以仿真里验证通过后，一定要在实机上先做一个楼梯附近的“悬停-规划-小步试探”流程，确认状态估计靠谱再放手跑。

4. 实操流程：从点云到跨楼层轨迹

4.1 数据采集要点

跨楼层场景的数据采集跟平面建图有明显区别。平面建图就是扫一圈平地，而跨楼层需要分层扫描、楼梯重点照顾。

我的采集顺序是：

1. 先把每一层楼的平面区域扫一遍，生成干净的楼层地图。
2. 然后让机器人停在楼梯口前1米处，原地旋转360°，把楼梯坡面和台阶边缘的完整点云带回地图。
3. 再让机器人以半自主方式缓慢上楼梯，边走边建图，把楼梯中途的点云补全。
4. 最后在上层楼梯口再扫一遍，把上下两个楼层的点云“接起来”。

这个顺序的用意是：楼道口的空间通常很窄，雷达盲区多，如果只靠楼层内的点云插值推算楼梯，容易出现台阶缺失、边界模糊。专门停一次做原地旋转扫描，楼梯区域的点云密度会高很多，对后续OctoMap建图的完整度帮助极大。

如果直接把机器人放到楼梯中间开始建图，也不是不行，但起点附近点云缺失严重，规划时楼梯中段容易被当成自由空间或者障碍物，轨迹容易飘。

4.2 点云预处理参数经验

点云预处理是关键中的关键，直接决定下游地图质量。SCAN-Planner-Pure-ROS2自带的点云预处理节点通常做三件事：降采样、去离群点、裁剪视角。

降采样：体素大小我建议设在0.05m~0.1m之间。太细（0.02m以下）会导致点云数量巨大，OctoMap更新时间明显变长，规划器实时性受影响；太粗（0.2m以上）楼梯台阶的小边缘直接糊掉，地图里变成一面斜坡，轨迹不精确。

离群点剔除：用统计滤波器，邻域点数为10~20，标准差阈值设2.0左右。这能有效去掉楼上楼下反射回来的一些杂散点。

视角裁剪：把机器人本体上方、后方的无效点云裁掉。四足机器人背上经常有支架、天线，这些点如果不裁掉，会当成障碍物导致规划器以为“机器人永远被自己挡住”。

有个经验大家可能第一次接触时会忽略：裁剪时千万别把楼梯扶手裁掉。我踩过这个坑，为了美观把高处的点全滤掉，结果楼梯扶手变成透明墙，规划器直接规划穿扶手而过的轨迹，实机腿会被卡住。

4.3 地图构建与坐标对齐

地图坐标系对齐是跨楼层规划最容易翻车的环节之一。

用octomap_server建图时，地图坐标系的z=0平面在哪个楼层，直接决定机器人站到二楼后，规划器把它当成了“地下生物”还是“空中飞人”。

我的做法是：

1. 以一楼起点为原点，建立map坐标系。
2. 用2D SLAM（比如Cartographer或者基于Fast-LIO的里程计）作为前端，持续输出odom→base_link的变换。
3. octomap_server订阅/tf，把所有点云变换到map坐标系下累积。

这里要特别强调：跨楼层时里程计的漂移会急剧增大。楼梯上下过程中，腿部滑动、接触冲击都会让轮式里程计或者腿部运动学推算的位置越偏越远。地图就会分层，比如楼梯建好的台阶在二楼被扫描时会被“切一刀”。

缓解方案有三个，按效果排序：

• 融合IMU的里程计，比如Fast-LIO2、LIO-SAM这类激光惯性里程计，比纯腿部运动学推算稳定得多。
• 在楼梯口增加回环检测。让机器人上下楼时多次经过一楼楼梯口，SLAM后端检测到回环后能把漂移拉回去。
• 如果都没有，那就在跨楼层后手动重定位。机器人上到二楼之后，在已知标志物（比如柱子或墙边）前停一下，用AMCL或者手动发送一个坐标修正，强制把地图错位拉回来。

4.4 轨迹规划与跨楼层切换实现

地图准备好之后，就可以跑规划器了。规划器启动方式在launch文件里：

ros2 launch scan_planner scan_planner_launch.py

发送目标点用Rviz2的“2D Goal Pose”按钮就行，也可以命令行发：

ros2 topic pub /goal geometry_msgs/msg/PoseStamped " {header: {frame_id: 'map'}, pose: {position: {x: 12.0, y: 3.5, z: 1.8}, orientation: {w: 1.0}}}"

注意z坐标要设成目标楼层的地面高度，不能是0。很多人第一次发目标点会忘掉这个，规划器找半天找不到点。

跨楼层切换的具体做法有两种：

1. 先建好跨楼层的统一OctoMap，然后在一个地图里直接搜索轨迹。这种方式实现最简单，但地图很大时规划耗时长，而且机器人在地图上“当前位置”需要精确校准。
2. 多地图切换：每层楼单独建一份OctoMap，在楼梯口设置“切换点”，当机器人到达切换点时，规划器从“当前楼层地图”切换到“目标楼层地图”，继续搜索。这种方式更贴近实际部署，也更容易维护。

如果只是验证原理，建议先走方案1。等系统稳定了，再做方案2的多楼层拓扑切换。把每层楼的OctoMap存成.bt文件，用octomap_server的octomap_save服务保存，切换时重新加载即可。

5. 参数调优与常见问题排查

5.1 关键参数速查表

以下参数主要来自SCAN-Planner-Pure-ROS2的config文件，我按影响程度排序，给一个可用的起点值：

这几个参数是“活”的，跟你的机器人腿长、底盘尺寸、雷达高度强相关。建议每次只改一个参数，观察Rviz2里的轨迹变化，别一次性全改，否则出了匪夷所思的规划结果你根本不知道是哪一项惹的祸。

5.2 开发中踩过的坑

• 坑1：编译找不到octomap_msgs

  大概率是octomap_msgs没有安装成ROS2版本。ROS1的octomap_msgs头文件放在/opt/ros/noetic/include，ROS2的放在/opt/ros/humble/include。确认你source的ROS发行版和环境变量正确。另外别用c++的#include <octomap_msgs/octomap.h>，要用ROS2风格的#include <octomap_msgs/msg/octomap.hpp>。

• 坑2：Rviz2里看不到规划轨迹

  先检查Fixed Frame是不是map，再查/plan/global有没有数据在发：

  ros2 topic echo /plan/global

  如果Topic有数据但Rviz不显示，检查轨迹消息里的frame_id，有些版本它硬编码成odom，你要么把世界坐标系对齐到odom，要么改源码里的frame_id为map。

• 坑3：轨迹到了楼梯边缘就停下来

  这个通常是安全距离参数太保守了。规划器在楼梯边缘检查时会认为“台阶以下没有支撑”，从而把轨迹截断。把ground_clearance、foot_clearance这类参数往小调一些，让检车器更信任OctoMap里已经建好的台阶面。但要注意不要调到太小，否则实机上腿会撞台阶侧沿。

• 坑4：上下楼梯时地图分层叠影

  原因就是前面说的里程计漂移。先确认IMU和里程计融合质量，再用Fast-LIO或Cartographer的重定位模式处理。如果整栋楼建完图，分层严重，直接把里程计后端换成回环图优化的方案，一劳永逸。

• 坑5：规划速度突然变慢

  大概率是OctoMap地图里动态点太多。人一走动、门一变位，规划器每次扩展都要新查占据状态，耗时暴涨。解决办法是在楼门口这些区域把octomap_server的“占据概率衰减”参数调大，让它更快遗忘旧动态点。

5.3 调试工具与效率技巧

调试这套系统，别只盯着Rviz2看，我分享几个实际效率很高的组合拳：

1. 用ros2 topic hz检查关键Topic频率。规划器的输入是雷达点云、里程计、OctoMap。任何一个Topic掉到5Hz以下，规划器计算出的轨迹都会明显抖动。先排查Topic频率，再排查算法参数。

2. 用ros2 bag record录制完整数据包，离线重放调参。这是最推荐的调参方法。实机上跑一次很费事，多录几次bag，回来反复改参数离线跑，找最优参数组，再回实机验证。千万别在实机上边跑边调，浪费时间且容易翻车。

3. 给机器人加一个“测试模式”。模拟状态下规划器可以输出轨迹但不下发实际速度指令，只在Rviz里看轨迹合理性。这个模式改动很小，就是加一个publish_cmd的布尔参数，但能避免很多意外运动。

4. 为每个地图写一份launch配置。不同楼层的OctoMap文件路径、原点偏移都固化到launch参数里，切换测试环境时直接一行命令启动完整系统，不用开会时手忙脚乱改坐标。

最后再说一句个人体会。跨楼层规划这种活，听着高深，拆开看核心就三件事：靠谱的3D地图、带机器人运动学约束的搜索规划器、稳定的里程计融合。SCAN-Planner-Pure-ROS2帮我们把前两件事的大部分工作做好了，真正需要花心思的是里程计融合和参数适配。不管你在哪层楼，只要这一条链路不断，机器人就能稳稳当当地爬上去。