四足机器人做跨楼层规划听起来是个挺硬的课题但这几年随着宇树、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有几点要提醒第一次编译时间会比较长SCAN Planner依赖很多建议先编译单独包colcon build --packages-select scan_planner有问题好定位。Jazzy版本有些包改了API比如TF2的消息类型和时间戳处理如果你在Jazzy上编译报错多半是tf2_ros::Buffer接口变了改一下头文件引用就能过。不建议用二进制安装的octomap_server直接跑跨楼层场景因为很多场景参数需要改launch文件里面的参数源码编译后调试起来方便得多。3.2 节点架构与Topic设计启动后整套系统大致有这些节点在跑节点作用订阅发布livox_ros_driver2激光雷达驱动输出点云-livox/lidarpointcloud_preprocessor点云降采样、去离群点/livox/lidar/cloud/filteredoctomap_server接收点云构建八叉树地图/cloud/filtered/octomap_full、/projected_mapscan_planner核心规划器订阅地图和目标点/octomap_full、/odom、/goal/plan/global、/cmd_velrviz2可视化订阅全部-这套架构的精华在于规划器并不直接控制腿部它只输出全局轨迹和期望速度。真正的腿部控制交给底层步态控制器比如宇树的运动控制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米处原地旋转360°把楼梯坡面和台阶边缘的完整点云带回地图。再让机器人以半自主方式缓慢上楼梯边走边建图把楼梯中途的点云补全。最后在上层楼梯口再扫一遍把上下两个楼层的点云“接起来”。这个顺序的用意是楼道口的空间通常很窄雷达盲区多如果只靠楼层内的点云插值推算楼梯容易出现台阶缺失、边界模糊。专门停一次做原地旋转扫描楼梯区域的点云密度会高很多对后续OctoMap建图的完整度帮助极大。如果直接把机器人放到楼梯中间开始建图也不是不行但起点附近点云缺失严重规划时楼梯中段容易被当成自由空间或者障碍物轨迹容易飘。4.2 点云预处理参数经验点云预处理是关键中的关键直接决定下游地图质量。SCAN-Planner-Pure-ROS2自带的点云预处理节点通常做三件事降采样、去离群点、裁剪视角。降采样体素大小我建议设在0.05m~0.1m之间。太细0.02m以下会导致点云数量巨大OctoMap更新时间明显变长规划器实时性受影响太粗0.2m以上楼梯台阶的小边缘直接糊掉地图里变成一面斜坡轨迹不精确。离群点剔除用统计滤波器邻域点数为10~20标准差阈值设2.0左右。这能有效去掉楼上楼下反射回来的一些杂散点。视角裁剪把机器人本体上方、后方的无效点云裁掉。四足机器人背上经常有支架、天线这些点如果不裁掉会当成障碍物导致规划器以为“机器人永远被自己挡住”。有个经验大家可能第一次接触时会忽略裁剪时千万别把楼梯扶手裁掉。我踩过这个坑为了美观把高处的点全滤掉结果楼梯扶手变成透明墙规划器直接规划穿扶手而过的轨迹实机腿会被卡住。4.3 地图构建与坐标对齐地图坐标系对齐是跨楼层规划最容易翻车的环节之一。用octomap_server建图时地图坐标系的z0平面在哪个楼层直接决定机器人站到二楼后规划器把它当成了“地下生物”还是“空中飞人”。我的做法是以一楼起点为原点建立map坐标系。用2D SLAM比如Cartographer或者基于Fast-LIO的里程计作为前端持续输出odom→base_link的变换。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。很多人第一次发目标点会忘掉这个规划器找半天找不到点。跨楼层切换的具体做法有两种先建好跨楼层的统一OctoMap然后在一个地图里直接搜索轨迹。这种方式实现最简单但地图很大时规划耗时长而且机器人在地图上“当前位置”需要精确校准。多地图切换每层楼单独建一份OctoMap在楼梯口设置“切换点”当机器人到达切换点时规划器从“当前楼层地图”切换到“目标楼层地图”继续搜索。这种方式更贴近实际部署也更容易维护。如果只是验证原理建议先走方案1。等系统稳定了再做方案2的多楼层拓扑切换。把每层楼的OctoMap存成.bt文件用octomap_server的octomap_save服务保存切换时重新加载即可。5. 参数调优与常见问题排查5.1 关键参数速查表以下参数主要来自SCAN-Planner-Pure-ROS2的config文件我按影响程度排序给一个可用的起点值参数建议值说明voxel_resolution0.05OctoMap体素分辨率楼梯细节和内存占用之间的平衡点occupancy_thresh0.35空间被占据的体素概率阈值调高会“薄墙”调低会“胖墙”step_length0.3SPT每次节点扩展的最大直线长度楼梯上建议0.2~0.3max_curvature0.8折线扩展允许的最大曲率越小轨迹越“直”四足越容易走max_planning_time2.0单次规划的最长耗时超过后返回当前最优轨迹min_ground_clearance0.15机器人底盘最低离地高度检查值max_pitch0.5最大允许俯仰角弧度实际楼梯坡度约30°~35°goal_tolerance0.2判定到达目标的距离阈值这几个参数是“活”的跟你的机器人腿长、底盘尺寸、雷达高度强相关。建议每次只改一个参数观察Rviz2里的轨迹变化别一次性全改否则出了匪夷所思的规划结果你根本不知道是哪一项惹的祸。5.2 开发中踩过的坑坑1编译找不到octomap_msgs大概率是octomap_msgs没有安装成ROS2版本。ROS1的octomap_msgs头文件放在/opt/ros/noetic/includeROS2的放在/opt/ros/humble/include。确认你source的ROS发行版和环境变量正确。另外别用c的#include octomap_msgs/octomap.h要用ROS2风格的#include octomap_msgs/msg/octomap.hpp。坑2Rviz2里看不到规划轨迹先检查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看我分享几个实际效率很高的组合拳用ros2 topic hz检查关键Topic频率。规划器的输入是雷达点云、里程计、OctoMap。任何一个Topic掉到5Hz以下规划器计算出的轨迹都会明显抖动。先排查Topic频率再排查算法参数。用ros2 bag record录制完整数据包离线重放调参。这是最推荐的调参方法。实机上跑一次很费事多录几次bag回来反复改参数离线跑找最优参数组再回实机验证。千万别在实机上边跑边调浪费时间且容易翻车。给机器人加一个“测试模式”。模拟状态下规划器可以输出轨迹但不下发实际速度指令只在Rviz里看轨迹合理性。这个模式改动很小就是加一个publish_cmd的布尔参数但能避免很多意外运动。为每个地图写一份launch配置。不同楼层的OctoMap文件路径、原点偏移都固化到launch参数里切换测试环境时直接一行命令启动完整系统不用开会时手忙脚乱改坐标。最后再说一句个人体会。跨楼层规划这种活听着高深拆开看核心就三件事靠谱的3D地图、带机器人运动学约束的搜索规划器、稳定的里程计融合。SCAN-Planner-Pure-ROS2帮我们把前两件事的大部分工作做好了真正需要花心思的是里程计融合和参数适配。不管你在哪层楼只要这一条链路不断机器人就能稳稳当当地爬上去。