开源项目 ¶

约 1609 个字 110 行代码预计阅读时间 8 分钟

与四足机器人感知行走项目的结合方案 ¶

这个仓库是什么？¶

一句话定性：这是一个基于 ros2_control 框架的四足机器人控制器集成仓库，把 OCS2、强化学习（RL）、PD 控制器统一接入 ros2_control 硬件抽象层，可以直接驱动 Unitree Go2/Go1/A1/B2、Xiaomi CyberDog、DeepRobotics 等真实机器人，也可以在 Gazebo Harmonic / Mujoco / Isaac Sim 仿真中运行。

与 ocs2_ros2 的关系：

ocs2_ros2          →  提供 OCS2 算法库（MPC求解器、步态规划、感知接口）
        ↓  依赖
quadruped_ros2_control  →  把上述算法包装成 ros2_control 控制器插件
                            提供硬件接口、仿真世界、键盘/手柄输入
                            可以直接跑 Go2 真机或 Gazebo 仿真

完整文件结构解读 ¶

quadruped_ros2_control/
│
├── commands/                     ★ 输入命令模块（如何控制机器人）
│   ├── control_input_msgs/       自定义消息：Inputs.msg（速度、模式等）
│   ├── keyboard_input/           ★ 键盘控制节点（WASD + 数字键）
│   ├── joystick_input/           手柄控制节点
│   └── unitree_joystick_input/   宇树专用手柄
│
├── controllers/                  ★ 核心控制器（选一个跑）
│   ├── leg_pd_controller/        纯PD腿部控制（最简单，调试用）
│   ├── unitree_guide_controller/ ★ 宇树Guide算法控制器（快速跑起来用）
│   ├── ocs2_quadruped_controller/★★★ OCS2 MPC控制器（本项目核心）
│   │   ├── config/
│   │   │   ├── elevation_mapping.yaml    感知地图配置
│   │   │   ├── convex_plane_decomposition.yaml  平面分割配置
│   │   │   └── visualize_ocs2.rviz       RViz可视化配置
│   │   ├── include/ocs2_quadruped_controller/
│   │   │   ├── control/          GaitManager、TargetManager、CtrlComponent
│   │   │   ├── estimator/        状态估计（Kalman滤波/真值/里程计）
│   │   │   ├── FSM/              有限状态机（Passive↔OCS2MPC）
│   │   │   ├── interface/        OCS2问题定义（约束、代价函数）
│   │   │   ├── perceptive/       ★★ 感知模块（楼梯核心！）
│   │   │   │   ├── constraint/   FootCollision、FootPlacement、SDF约束
│   │   │   │   ├── interface/    PerceptiveLeggedInterface（感知版MPC接口）
│   │   │   │   ├── synchronize/  PlanarTerrainReceiver（接收地形话题）
│   │   │   │   └── visualize/    足端落点可视化
│   │   │   └── wbc/              全身控制（WBC）：HierarchicalWbc、WeightedWbc
│   │   ├── launch/
│   │   │   ├── gazebo.launch.py        ★ Gazebo仿真启动
│   │   │   ├── mujoco.launch.py        Mujoco仿真启动
│   │   │   └── elevation_mapping.launch.py  ★ 感知地图启动
│   │   └── src/                  与include一一对应的实现文件
│   │
│   └── rl_quadruped_controller/  强化学习控制器（备选方向）
│
├── descriptions/                 ★ 机器人描述（URDF/xacro）
│   ├── unitree/
│   │   ├── go2_description/      ★★ Unitree Go2（推荐使用的机器人）
│   │   │   ├── config/ocs2/
│   │   │   │   ├── task.info     ★ Go2专用MPC参数（已调好！）
│   │   │   │   ├── gait.info     步态定义
│   │   │   │   └── reference.info
│   │   │   ├── xacro/robot.xacro  机器人模型（含传感器接口）
│   │   │   └── urdf/             生成的URDF
│   │   ├── go1_description/      Go1
│   │   ├── a1_description/       A1
│   │   └── b2_description/       B2（大狗）
│   ├── anybotics/anymal_c_description/  ANYmal C（与ocs2_ros2一致）
│   ├── deep_robotics/            Lite3、X30
│   └── magiclab&xiaomi/          CyberDog
│
├── hardwares/                    硬件接口层
│   ├── hardware_unitree_sdk2/    ★ 宇树真机接口（Go2真机部署用）
│   └── gz_quadruped_hardware/    Gazebo硬件接口
│
└── libraries/
    ├── gz_quadruped_playground/  ★★ Gazebo仿真环境（含世界地图）
    │   ├── worlds/
    │   │   ├── default.sdf       默认平地世界
    │   │   ├── warehouse.sdf     ★ 仓库场景（有货架、障碍物）
    │   │   └── baylands.sdf      室外场景
    │   └── models/
    │       ├── D435/             Intel RealSense D435 深度相机模型
    │       └── LiDAR3DV1/        激光雷达模型
    ├── controller_common/        FSM基础类（StatePassive等）
    └── qpoases_colcon/           QP求解器依赖

与楼梯感知项目的结合方案 ¶

整体技术栈对应关系 ¶

你的项目目标：感知不同地形 → 稳定行走 → 上下楼梯
                    ↓
传感器数据流：
  Gazebo 仿真中的 D435/LiDAR
      ↓ 点云话题 /rgbd_d435/points
  elevation_mapping（高程地图）
      ↓ grid_map
  convex_plane_decomposition（平面分割）
      ↓ /planar_terrain 话题
  PlanarTerrainReceiver（本仓库实现）
      ↓ 更新 PlanarTerrain 数据结构
  PerceptiveLeggedInterface → OCS2 MPC求解
      ↓ 关节力矩指令
  ros2_control → 硬件执行

三步快速上手方案 ¶

用 unitree_guide_controller 先跑起来（1-2 天）¶

这是最简单的控制器，不依赖 OCS2，优先用它熟悉整个 ros2_control 框架：

# 编译
colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input --symlink-install

# 启动 Gazebo 仿真
ros2 launch gz_quadruped_playground gazebo.launch.py

# 键盘控制
ros2 run keyboard_input keyboard_input

键盘操作： - 1 → 趴下（Passive） - 2 → 站起来 - 3 → trot 行走 - W/S/A/D → 前后左右移动

切换到 ocs2_quadruped_controller（3-5 天）¶

切换到 OCS2 MPC 控制器，使用 Unitree Go2 机器人模型：

# 编译 OCS2 控制器
colcon build --packages-up-to ocs2_quadruped_controller --symlink-install

# 启动（默认 Go2 平地世界）
ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=go2_description

# 切换楼梯/复杂地形世界
ros2 launch ocs2_quadruped_controller gazebo.launch.py \
    pkg_description:=go2_description \
    world:=warehouse

# 键盘控制（同时打开）
ros2 run keyboard_input keyboard_input

Go2 与 ANYmal 的关键参数差异（已为你对比）：

参数	ANYmal C (ocs2_ros2)	Go2 ( 本仓库 )	差异说明
centroidalModelType	1 (SRBD)	0 ( 全动力学 )	Go2 用更精确的模型
swingHeight	0.10 m	0.08 m	Go2 腿短，默认摆腿低
comHeight	0.575 m	0.35 m	Go2 更矮
p_base_z 初始	0.575	0.35	-
MPC 频率	50 Hz	100 Hz	Go2 算力强
frictionCoefficient	0.5	0.3	-
WBC	无	有 (HierarchicalWbc)	本仓库多了全身控制层

启用感知模式（上楼梯，1-2 周）¶

这是项目的核心阶段。本仓库已经把感知流水线完整实现了：

# 终端1：启动带传感器的 Gazebo 仿真
ros2 launch ocs2_quadruped_controller gazebo.launch.py \
    pkg_description:=go2_description \
    world:=warehouse

# 终端2：启动 elevation_mapping + convex_plane_decomposition
ros2 launch ocs2_quadruped_controller elevation_mapping.launch.py

# 终端3：键盘控制
ros2 run keyboard_input keyboard_input

# 键盘切换到感知模式：
# 1 → Passive（趴下）
# 2 → OCS2 MPC（站起/行走，此时感知地形自动接入）

关键代码文件深度解读（必须理解的 5 个文件）¶

文件①：`PlanarTerrainReceiver.cpp`（感知数据入口）¶

路径： controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp

作用： 订阅 /planar_terrain 话题 → 更新 OCS2 内部地形模型 → 触发 SDF 重算

// 核心逻辑：收到地形消息后
subscription_ = node_->create_subscription<..PlanarTerrain>(
    mapTopic, 10, [this](const msg) {
        planarTerrain_ = fromMessage(msg);           // 消息→数据结构
        sdfPtr_->calculateSignedDistanceField(...);  // 重算有向距离场
    });

楼梯项目中要改的地方： 如果你使用不同传感器（激光雷达、D435），修改 elevation_mapping.yaml 中的话题名即可，这个文件本身不需要改。

文件②：`PerceptiveLeggedInterface.cpp`（感知 MPC 接口）¶

路径： controllers/ocs2_quadruped_controller/src/perceptive/interface/PerceptiveLeggedInterface.cpp

作用： 初始化带感知的 OCS2 问题，包括：足端落点约束、碰撞约束、SDF 约束

楼梯项目中要改的地方：

// 当前：初始化为 5m×5m 的平地（默认地形）
double width{5.0}, height{5.0};
plannerRegion.transformPlaneToWorld.setIdentity();

// 楼梯场景：不需要改这里！
// 启动后 PlanarTerrainReceiver 会自动用真实地形覆盖这个默认值

文件③：`go2_description/config/ocs2/task.info`（Go2 MPC 参数）¶

路径： descriptions/unitree/go2_description/config/ocs2/task.info

楼梯调参速查：

# 上楼梯前修改这些值（无需重新编译）

swing_trajectory_config
{
  swingHeight    0.08   →  改为  0.15   # 抬腿更高，跨越台阶
}

mpc
{
  timeHorizon    1.0    →  改为  1.5    # 看得更远，提前规划
}

Q
{
  (8,8)   1500.0        →  改为  2000.0  # 机身高度跟踪更严格
  (10,10) 300.0         →  改为  500.0   # 俯仰角控制更严格（防前倾）
}

文件④：`elevation_mapping.yaml`（高程地图配置）¶

路径： controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml

# 当前配置支持两种传感器（重要！）
input_sources:
  front:                         # D435 深度相机
    topic: /rgbd_d435/points
  lidar:                         # LiDAR 激光雷达
    topic: /scan/points
    ignore_points_above: 0.5    # 过滤机身以上的点（防自遮挡）
    ignore_points_below: -0.5

# 地图参数（楼梯场景建议调整）
length_in_x: 5.0      # 地图范围前后5m
length_in_y: 5.0
resolution: 0.03      # 3cm分辨率（楼梯台阶~20cm，足够）

楼梯场景优化建议： - resolution: 0.03 → 0.02（提高精度，识别台阶边缘更准） - length_in_x: 5.0 → 3.0（减小范围，提高更新频率）

文件⑤：`convex_plane_decomposition.yaml`（平面分割配置）¶

路径： controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml

楼梯场景关键参数：

sliding_window_plane_extractor:
  plane_inclination_threshold_degrees: 30.0  # 允许最大倾斜角
                                              # 楼梯台面接近水平，30°够用
  plane_patch_error_threshold: 0.02          # 平面拟合误差阈值(m)
                                              # 楼梯台面较平整，0.02合适

postprocessing:
  extracted_planes_height_offset: 0.02       # 台阶高度补偿
  nonplanar_height_offset: 0.03              # 非平面区域高度补偿

与 ocs2_ros2 的关系：二选一 vs 组合使用 ¶

方案	使用方式	适用阶段
纯 ocs2_ros2	直接用 `legged_robot_ddp.launch.py`	学习 OCS2 算法原理，快速验证参数
纯 quadruped_ros2_control	用 `ocs2_quadruped_controller/gazebo.launch.py`	工程部署、感知接入、真机测试
组合使用	ocs2_ros2 调参→移植到本仓库	推荐！理解算法 + 工程落地

推荐工作流：

ocs2_ros2（ANYmal, Dummy仿真）  →  理解 MPC 参数含义，快速迭代
         ↓  参数调好后
quadruped_ros2_control（Go2, Gazebo）  →  真实物理仿真验证
         ↓  仿真验证后
quadruped_ros2_control（Go2, 真机）    →  实机部署

楼梯感知项目完整启动流程 ¶

# === 编译 ===
colcon build --packages-up-to ocs2_quadruped_controller gz_quadruped_playground --symlink-install
source install/setup.bash

# === 启动仿真（含楼梯世界）===
ros2 launch ocs2_quadruped_controller gazebo.launch.py \
    pkg_description:=go2_description \
    world:=warehouse      # warehouse 场景有障碍物

# === 启动感知流水线 ===
ros2 launch ocs2_quadruped_controller elevation_mapping.launch.py

# === 键盘控制 ===
ros2 run keyboard_input keyboard_input
# → 按 1：趴下（等传感器初始化）
# → 按 2：站起来，MPC接管
# → 按 3：开始 trot 行走
# → W键：前进，遇到地形自动用感知规划

# === 观察感知结果 ===
# RViz 中 visualize_ocs2.rviz 会显示：
# - 高程地图（elevation map）
# - 平面分割结果（colored regions）
# - 足端落点规划（foot placement markers）

核心文件快速索引 ¶

需要理解（读懂）：
  controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h  控制器主类
  controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp          OCS2状态机
  controllers/ocs2_quadruped_controller/src/perceptive/synchronize/PlanarTerrainReceiver.cpp  感知入口

需要修改（调参）：
  descriptions/unitree/go2_description/config/ocs2/task.info           ★ MPC参数
  descriptions/unitree/go2_description/config/ocs2/gait.info           ★ 步态
  controllers/ocs2_quadruped_controller/config/elevation_mapping.yaml   传感器配置
  controllers/ocs2_quadruped_controller/config/convex_plane_decomposition.yaml  地形分割

需要了解（看看就行）：
  libraries/gz_quadruped_playground/worlds/*.sdf                       仿真世界
  controllers/ocs2_quadruped_controller/src/wbc/WeightedWbc.cpp        全身控制
  controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp  状态估计

总结对比 ¶

维度	ocs2_ros2	quadruped_ros2_control
定位	OCS2 算法库 + ANYmal 示例	ros2_control 控制器集成
仿真	Dummy（纯轨迹）	Gazebo/Mujoco（物理）
机器人	ANYmal C	Go2/Go1/A1/B2 等多款
感知	算法接口完整，需自己接入	已完整接入，开箱即用
WBC	无	有（HierarchicalWbc）
真机	不支持	支持 Go2 真机
适合阶段	学习算法 / 快速调参	工程部署 / 感知验证
推荐	先学这个理解原理	再用这个做工程项目