【四足机器人跨楼层轨迹优化】SCAN-Planner-Pure-ROS2 精简版开源实现【附 GitHub 开源链接】

总阅读本公众号的老粉，会注意到，我目前再储备四足机器人跨楼层的规划解决方案，尤其是涉及到楼梯间的绕障的轨迹优化问题。

最近上近期，上海交通大学秦通老师课题组基于浙江大学高飞老师团队开源的高性能轨迹优化算法 EGO-Planner，针对地面机器人以及四足机器人爬楼梯等 2.5D 工况，做了许多创新性的优化工作。

本文的开源项目SCAN-Planner-Pure-ROS2 核心算法也是源自上海交通大学秦通老师团队的优秀开源工作，特此感谢。

具体可参见往期文章：

1.【四足跨楼层自主绕障技术】轨迹优化器SCAN-Planner算法

2.【四足跨楼层自主绕障技术】从 EGO-Planner 到 SCAN-Planner：面向地面四足机器人的轨迹优化算法的改进分析

3.【四足跨楼层自主绕障技术】SCAN-Planner 与 ego-planner 地图和规划算法代码级差异分析

如果对我之前基于高飞老师团队开源的 EGO-Planner 所做的二维地面精简适配版本 EGO-Planner-2D-ROS2 感兴趣，可以通过下述文章链接下载

https://github.com/JackJu-HIT/Ego-Planner-2D-ROS2

https://github.com/JackJu-HIT/SCAN-Planner-Pure-ROS2.git

SCAN-Planner-Pure-ROS2 是 SCAN-Planner 轨迹优化器的精简版本，便于快速集成并适配不同系统的软件框架。我做这个也是为了未来某一天，我所负责的项目如果需要使用此技术，我花个2个小时，就可以适配到我们的项目中，非常适合部署，我觉得很有意义。

这个工程保留了轨迹优化、碰撞检测、A* 搜索和 B 样条轨迹生成等核心能力，同时使用一个轻量的 ROS 2 节点完成数据输入和 RViz2 可视化。对于希望快速理解局部轨迹规划，或者准备把轨迹优化器接入自研导航系统的开发者来说，这种“纯 C++ 算法核心 + ROS 2 外壳”的组织方式比较直观。

本项目在原版 SCAN-Planner 的基础上进行了重新整理与精简，主要包括：

• 重写并简化地图 grid_map 模块；
• 移除原项目中较为复杂的决策与业务处理逻辑；
• 保留轨迹搜索与轨迹优化相关的核心算法；
• 将核心算法封装为可独立调用的 C++ 模块；
• 在核心算法外层提供轻量化 ROS 2 节点与可视化接口；
• 降低算法模块与 ROS 2 通信框架之间的耦合度。

这种设计更适合算法验证、二次开发以及跨系统移植，开发者可以根据自身项目的软件架构，对地图输入、规划触发、轨迹输出和控制接口进行适配。

2026年7月23日 21:44:01

柏贤于沈阳

工程中的关键目录如下：

SCAN-Planner-Pure-ROS2/
├── include/
│   └── trajectory_obstacles_publisher.h
├── launch/
│   └── scan_planner.launch.py
├── planner/
│   ├── bspline_opt/       # B 样条轨迹优化
│   ├── path_searching/    # A* 搜索
│   ├── plan_env/          # GridMap 局部占据地图
│   └── plan_manage/       # 规划器对外接口
├── src/
│   └── trajectory_publisher.cpp
├── rviz.rviz
├── CMakeLists.txt
└── package.xml

一次规划的数据链路可以概括为：

RViz2 输入
  ├── /initialpose   -> 当前机器人位姿
  ├── /goal_pose     -> 模拟障碍物
  └── /clicked_point -> 全局参考路径点
            |
            v
TrajectoryAndObstaclesPublisher
            |
            v
PlannerInterface
  ├── 更新 GridMap
  ├── 参数化初始 B 样条
  ├── 碰撞段 A* 搜索
  ├── rebound 优化



    
  └── 动力学可行性检查与时间重分配
            |
            v
Path / PointCloud2 / MarkerArray
            |
            v
RViz2 可视化

2.1 推荐环境

本文使用以下环境：

• Ubuntu 22.04；
• ROS 2 Humble；
• GCC 11；
• CMake 3.22 或更高版本；
• Eigen3、Boost、RViz2；
• colcon 和 rosdep。

项目当前挂载在名为 ros2_dev（需要替换你自己的） 的 Docker 容器中，容器内路径为：

/workspace/SCAN/SCAN-Planner-Pure-ROS2

进入容器：

    
docker exec -it ros2_dev bash

2.2 安装依赖

进入容器后加载 ROS 2 环境：

source /opt/ros/humble/setup.bash
cd /workspace/SCAN/SCAN-Planner-Pure-ROS2

使用 rosdep 安装包清单中声明的依赖：

rosdep install --from-paths . --ignore-src -r -y

如果使用的镜像已经包含 ROS 2 Desktop、Eigen3 和 Boost，这一步通常只会检查依赖，不会安装太多额外软件。

在项目目录执行：

source /opt/ros/humble/setup.bash
colcon build --packages-select scan_planner

编译完成后加载安装空间：

source install/setup.bash

检查 ROS 2 是否识别到节点：

ros2 pkg executables scan_planner

正常情况下会得到：

scan_planner motion_plan

如果修改过包名、CMake 目标或者 Launch 文件，建议清理 CMake 缓存后重新构建：

colcon build \
  --packages-select scan_planner \
  --cmake-clean-cache

工程已经提供 scan_planner.launch.py，它会启动：

• scan_planner/motion_plan；
• rviz2；
• 安装目录中的 rviz.rviz 配置。

一键启动：

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch scan_planner scan_planner.launch.py

服务器或无图形界面环境只启动规划节点：

ros2 launch scan_planner scan_planner.launch.py use_rviz:=false

也可以分别启动：

# 终端 1
ros2 run scan_planner motion_plan

# 终端 2，在项目根目录执行
rviz2 -d install/scan_planner/share/scan_planner/rviz/rviz.rviz

这套演示程序约定了三个 RViz2 工具：

5.1 使用 2D Pose Estimate 设置机器人起点

2D Pose Estimate 发布 /initialpose，回调函数读取：

• position.x；
• position.y；
• 四元数对应的 yaw。

代码把当前高度设为 0.2 m，并将 yaw 保存到 cur_pose_.theta。规划器随后利用 yaw 构造初始速度和初始加速度方向。

理想用法是：

• 在 RViz2 中点击机器人起点；
• 拖动鼠标给出车头朝向；
• 如果没有设置，则使用程序中显式配置的默认起点。

需要特别注意：当前 PathPoint cur_pose_ 没有显式默认初始化，因此现有代码不能安全地依赖“默认起点”。工程接入时建议至少增加：

scan_planner::PathPoint cur_pose_{0.0F, 0.0F, 0.2F, 0.0F, 0.0F};

或者将默认起点声明为 ROS 2 参数。完成这一步后，“不点击 2D Pose Estimate，直接使用默认起点”才是确定行为。

5.2 使用 2D Nav Goal 添加模拟障碍物

在这个工程中，2D Nav Goal 没有被当作路径目标，而是被复用为模拟障碍物输入。

/goal_pose 的回调只取目标位置的 x、y：

/goal_pose
    -> goal_pose_callback()
    -> add_obstacle_at_position(x, y)
    -> obstacles_

可以多次点击 2D Nav Goal，每次点击都会向 obstacles_ 追加一个障碍物点。随后这些点被转换为高度 0.2 m 的  Eigen::Vector3d，用于更新局部 GridMap。

5.3 使用 Publish Point 设置全局目标点

Publish Point 发布 /clicked_point。每次点击会向 global_plan_traj_ 追加一个 PathPoint：

/clicked_point
    -> rviz_point_callback()
    -> global_plan_traj_.push_back(point)

因此更准确地说，Publish Point 添加的是“全局参考路径点”。连续点击可以形成一条折线参考路径，最后一个点才是最终目标。

当前 discretize_trajectory() 要求输入至少有两个点。所以在不修改代码的情况下，建议至少点击两个 Publish Point：

1. 第一个点作为参考路径的中间点或靠近起点的点；
2. 第二个点作为全局目标点；
3. 更复杂的道路可以继续添加更多路径点。

5.4 当前版本可稳定触发规划的顺序

期望的产品交互顺序是：

GridMap 位于：

planner/plan_env/include/plan_env/grid_map.h
planner/plan_env/src/grid_map.cpp

6.1 地图数据结构

GridMapConfig 包含以下配置：

地图维护两份一维缓存：

raw_buffer_       原始障碍物占据层
inflated_buffer_  膨胀后的碰撞层

缓存元素是 unsigned char，只记录 0/1。三维索引 (x, y, z) 通过下面的行主序方式压成一维地址：

address = (x * Ny + y) * Nz + z

以当前 50 × 50 × 5 m、0.1 m 分辨率计算：

Nx = 500
Ny = 500
Nz = 50
体素数量 = 12,500,000

仅两份占据缓存理论上约占 25 MB，还不包含原始点云、膨胀点云、偏移表和容器开销。对于真正只做平面导航的系统，map_z_size_ 是很值得优先缩减的参数。

6.2 初始化

GridMap::init() 完成四件事：

1. 检查分辨率和地图尺寸是否合法；
2. 使用 ceil(size / resolution) 计算各轴体素数量；
3. 为原始层和膨胀层分配缓存；
4. 预计算障碍物膨胀偏移 inflation_offsets_。

膨胀偏移只在初始化时计算一次，因此每次更新地图不需要重复生成圆柱模板。

6.3 局部地图更新

updateLocalMap() 每次规划前执行：

reset()
  -> 清空原始层、膨胀层和可视化点云

origin_ = current_position - 0.5 * local_map_size
origin_.z() = ground_height

遍历 global_cloud
  -> 过滤地图外点
  -> 世界坐标转体素索引
  -> 写入原始占据层
  -> 生成膨胀占据层

XY 原点跟随机器人当前位置，因此这是一个机器人中心局部地图；Z 原点固定为 ground_height。

这套实现适合已经完成感知处理的障碍物点集，因为它没有：

• 概率占据更新；
• 射线清障；
• 点云滤波；
• 时间衰减；
• 多传感器融合。

换句话说，输入点被直接视为准确障碍物。

6.4 世界坐标与体素索引转换

世界坐标转索引：

index = floor((world_position - origin) / resolution)

索引转体素中心：

position = origin + (index + 0.5) * resolution

查询时使用左闭右开的地图边界：

origin <= position < max_boundary

这样可以避免上边界点被映射到越界索引。

6.5 圆柱障碍物膨胀

rebuildInflationOffsets() 在 XY 平面内枚举候选偏移，只保留满足：

hypot(dx, dy) <= inflation_radius

的体素，再叠加 [-inflation_z_down, inflation_z_up] 的 Z 范围，最终得到竖直圆柱膨胀模板。

每插入一个原始障碍物体素，addInflationAround() 都会套用这组偏移。由于写缓存前会检查是否已经占据，所以重叠膨胀体素不会重复写入可视化点云。

6.6 双圆柱碰撞查询

queryDoubleCylinder(position, yaw) 沿机器人朝向计算：

front = position + offset * heading
rear  = position - offset * heading

然后分别查询前后两个中心在膨胀层中的占据状态。任意一个中心不为空闲，就返回对应状态。

这种方法可以用两个圆近似长方形车体，比单圆模型更适合具有明显车长的 AGV/AMR。参数 double_cylinder_offset 应结合轴距和车身长度调整。

6.7 一个容易忽视的边界语义

queryDoubleCylinder() 遇到前圆柱位于地图外时会返回 kOutOfMap；但 getInflateOccupancy() 只在返回值等于 kOccupied 时才返回 true。因此当前上层 A* 会把“地图外”视为“没有占据”。

在安全要求较高的机器人上，通常应采用：

kOutOfMap -> 按占据处理

否则规划器可能在局部地图边缘接受一段缺少环境信息的轨迹。

7.1 节点初始化

节点名称为：

scan_planner_interactive_node

构造函数创建发布器、订阅器和一个 200 ms 周期定时器，即 5 Hz 调用 publish_and_plan()。

主要订阅接口：

主要发布接口：

7.2 路径预处理

进入规划前，程序会对参考路径做两次离散化：

1. 以 0.2 m 间隔离散原始路径；
2. 找到离当前机器人最近的参考点；
3. 将当前位姿插入路径头部；
4. 丢弃机器人身后的参考路径；
5. 再以 0.2 m 间隔离散一次。

这个过程使局部轨迹从当前机器人位置接入全局参考路径，而不是机械地从全局路径第一个点开始。

需要注意，discretize_trajectory() 当前声明的默认间隔是 0.1 m，但实际规划调用传入的是 0.2 m。调参时应以调用处的 0.2 m 为准。

7.3 GridMap 更新

发布节点把 RViz 输入的障碍物交给  PlannerInterface::setObstacles()。每个二维障碍物会被转换为：

(x, y, 0.2)

规划开始时调用：

grid_map_->updateLocalMap(current_position_, global_cloud_)

由当前机器人位置确定局部地图窗口，再生成膨胀层。

7.4 B 样条规划主流程

PlannerInterface::makePlan() 依次构造：

• 起点；
• 起点速度；
• 起点加速度；
• 局部目标；
• 目标速度；
• 全局参考点集。

当前起始速度方向为：

(cos(yaw), sin(yaw), 0)

起始加速度方向也使用相同单位方向。这里表达的是方向，不是由里程计测得的真实速度和加速度。接入真实底盘时，应改为实际状态估计值。

之后执行：

1. 将参考点参数化为三阶均匀 B 样条；
2. 检测初始控制点碰撞段；
3. 对碰撞段调用 A* 搜索；
4. 执行 rebound 轨迹优化；
5. 检查速度和加速度可行性；
6. 必要时延长轨迹时间并二次优化；
7. 每  0.1 s 采样一次轨迹用于发布。

7.5 可视化输出

定时器每次都会发布全局路径、局部轨迹、原始障碍物、A* 路径和膨胀体素。

膨胀障碍物有三种显示方式：

• PointCloud2：适合观察体素中心；
• CUBE_LIST：适合观察实际占据体积；
• LINE_LIST：适合观察体素边界和重叠关系。

这种多层可视化对检查分辨率、膨胀半径和地图边界非常有帮助。

当前参数主要硬编码在三个位置：

include/trajectory_obstacles_publisher.h
planner/plan_manage/src/planner_interface.cpp
planner/bspline_opt/src/bspline_optimizer.cpp

8.1 运动学参数

这里存在一组不一致：

• PlannerInterface 使用 max_vel = 2.0、 max_acc = 3.0；
• BsplineOptimizer::setParam() 内部又设置 max_vel = 1.0、max_acc = 0.5。

接入实际系统前应统一这两组物理限制，否则轨迹时间分配和优化代价可能采用不同标准。

8.2 地图参数

对于室内 AGV，可以从下面的组合开始：

resolution:             0.10 ~ 0.20 m
local_map_size:         20 × 20 × 1 m
inflation_radius:       机器人半宽 + 0.10 ~ 0.20 m
double_cylinder_offset: 约为车体有效长度的 1/4

注意：map_origin_  和 map_inflate_value_ 虽然从发布节点传入 initEsdfMap()，但当前实现只打印它们，没有真正用于配置 GridMap。真正生效的膨胀半径是函数内部硬编码的 0.5 m。

8.3 优化器权重

BsplineOptimizer::setParam() 当前设置：

调参思路：

• 轨迹贴障太近：增大 dist0_ 或 lambda2_；
• 轨迹绕行过大：适当减小 lambda2_，同时检查膨胀半径；
• 轨迹不够平滑：增大 lambda1_；
• 轨迹偏离参考路径太多：增大 lambda4_；
• 速度、加速度超限：提高 lambda3_，并统一优化器与规划接口的动力学上限。

lambda2_ = 1000 已经非常强调避障。如果轨迹出现过度远离障碍物的现象，应先检查它与 dist0_ = 1.0 m、inflation_radius = 0.5 m 的叠加效果。

8.4 A* 参数

A* 节点池初始化为：

100 × 100 × 100

搜索步长直接使用 GridMap 分辨率。0.1 m 分辨率下，节点池覆盖范围和内存开销都需要关注。

虽然 GridMap 使用三维缓存，当前 A* 搜索会根据起终点的 XY 投影，在搜索平面上插值 Z 索引。这使搜索主体更接近平面搜索，但底层数据结构仍然是三维的。

这种多层可视化对检查分辨率、膨胀半径和地图边界非常有帮助。

如果要把这个精简版接入真实机器人，建议按以下顺序改造。

第一步：把硬编码参数改成 ROS 2 参数

优先参数化：

max_vel
max_acc
max_jerk
map_resolution
map_size_x/y/z
inflation_radius
double_cylinder_offset
lambda1/2/3/4
dist0

这样可以通过 YAML 为不同车型维护独立配置，而不需要重新编译。

第二步：接入真实状态

使用里程计或状态估计替换演示数据：

• 起点位置来自定位；
• yaw 来自姿态估计；
• 起始速度来自里程计；
• 起始加速度来自滤波后的状态估计。

第三步：接入真实障碍物

将激光雷达、深度相机或代价地图输出转换为 global_cloud_。如果输入是原始点云，还应补充：

• 体素滤波；
• 地面分割；
• 高度过滤；
• 传感器时间戳；
• 障碍物超时清理。

第四步：完善规划状态机

当前演示节点适合交互测试，但实际部署还需要：

• 明确的规划触发条件；
• 规划成功/失败返回值；
• 目标更新和障碍物更新触发重规划；
• 空路径与单点路径保护；
• 并发数据快照，避免长时间持有互斥锁；
• 地图外按占据处理的安全策略。

至此结束。。。。

另外我也建立了个轨迹优化与运动控制方向交流群，欢迎各位同行专家加入交流群，不收费哈，欢迎添加作者的微信，加入交流群。