Centroidal MPC — 人形机器人质心模型预测控制

基于 OCS2 框架的人形机器人全身质心 MPC 仿真系统，使用 Unitree G1 机器人模型（29 自由度），集成 MuJoCo 物理仿真引擎，支持多步态规划、抗扰实验和实验数据记录。

系统要求

• 操作系统：Ubuntu 22.04
• ROS 发行版：ROS 2 Humble
• 物理引擎：MuJoCo 3.4.0（安装在 ~/mujoco-3.4.0）
• Python：3.10+（需 rclpy、numpy、matplotlib）

项目结构

Centroidal MPC/
│
├── humanoid_nmpc/                          # MPC 源码
│   ├── humanoid_centroidal_mpc/            # 质心 MPC 核心（动力学、代价、约束）
│   │   ├── src/dynamics/                   #   质心动力学流图 (CentroidalDynamicsAD)
│   │   ├── src/cost/                       #   代价函数 (ICP, 足端轨迹)
│   │   ├── src/constraint/                 #   约束 (速度、运动学)
│   │   ├── src/mrt/                        #   MRT 实时控制循环
│   │   └── src/command/                    #   目标轨迹生成
│   ├── humanoid_centroidal_mpc_ros2/       # ROS2 仿真节点
│   ├── humanoid_common_mpc/                # 公共 MPC 组件
│   ├── humanoid_common_mpc_ros2/           # ROS2 启动配置
│   ├── humanoid_common_mpc_pyutils/        # Python 工具
│   ├── humanoid_mpc_msgs/                  # 自定义 ROS2 消息
│   ├── humanoid_wb_mpc/                    # 全身 MPC（扩展预留）
│   └── remote_control/                     # 键盘遥控节点 (Python)
│
├── robot_models/unitree_g1/                # Unitree G1 机器人模型
│   ├── g1_description/                     # URDF/MJCF 描述 + STL 网格
│   └── g1_centroidal_mpc/                  # MPC 配置 + launch 文件
│       ├── config/mpc/task.info            #   MPC 参数（权重、频率、摩擦等）
│       ├── config/command/reference.info   #   目标速度/高度命令
│       └── launch/                         #   各类 launch 文件
│
├── build/                875MB             构建缓存（symlink-install，无需重新编译）
├── install/              14MB              编译产物（已预编译，可直接运行）
├── lib/                  59MB              OCS2 第三方依赖库
├── cppad_code_gen/       22MB              CppAD 自动微分生成的 C 代码
├── robot_runtime/                          MuJoCo 运行时接口
├── scripts/                                实验脚本
│   ├── disturbance_recorder.py             关节状态数据记录器
│   ├── plot_all_gaits.py                   多步态足端轨迹离线绘图
│   └── plot_gait_foot_height.py            slow_walk 步态足端高度绘图
│
├── launch_mujoco.sh                        一键启动脚本
└── README.md

快速启动

cd ~/"Centroidal MPC"
bash launch_mujoco.sh

首次启动时，CppAD 会自动编译 31 个自动微分模型（约 5-10 分钟），只需等待一次，后续启动秒开。编译完成后 MuJoCo 窗口将显示 G1 机器人站立。

启动脚本说明

launch_mujoco.sh 执行以下步骤：

1. 加载 ROS 2 Humble 环境 (/opt/ros/humble/setup.bash)
2. 加载工作区编译产物 (install/setup.bash)
3. 设置 MuJoCo 路径 (MUJOCO_PATH=~/mujoco-3.4.0)
4. 执行 ros2 launch g1_centroidal_mpc mujoco_core.launch.py

该 launch 文件启动以下节点：

节点	功能
humanoid_centroidal_mpc_sim	主仿真节点（MPC 求解 + MRT 控制 + MuJoCo 物理）
robot_state_publisher	发布当前关节状态的 TF 变换
terminal_robot_state_publisher	发布终端状态（用于可视化）
target_robot_state_publisher	发布目标状态（用于可视化）

键盘控制

在仿真运行中，启动键盘遥控节点：

# 新终端
cd ~/"Centroidal MPC"
source install/setup.bash
ros2 run remote_control fixed_forward_velocity_publisher

按键	功能	说明
w / s	加速 / 减速	步长 0.05 m/s
a / d	左转 / 右转	偏航控制
q / e	左移 / 右移	横向移动
+	施加推力干扰	30 N 前向推力
Space	急停	目标速度置零

注意：+ 键（Shift+=）发送 Float64MultiArray 消息 [30.0, 0.2]（推力幅值 + 目标速度）。

实验数据记录

关节状态记录器

录制所有关节的位置和速度数据，用于分析受扰后的关节响应：

# 在仿真运行时另开终端
cd ~/"Centroidal MPC"
source install/setup.bash
python3 scripts/disturbance_recorder.py

• 订阅话题：/joint_states（sensor_msgs/JointState）
• 输出文件：disturbance_data_YYYYMMDD_HHMMSS.csv
• 停止方式：Ctrl+C 自动保存

完整实验流程（三终端）

# 终端 1 ── 启动仿真
cd ~/"Centroidal MPC"
bash launch_mujoco.sh

# 终端 2 ── 启动数据记录
cd ~/"Centroidal MPC"
source install/setup.bash
python3 scripts/disturbance_recorder.py

# 终端 3 ── 键盘遥控
cd ~/"Centroidal MPC"
source install/setup.bash
ros2 run remote_control fixed_forward_velocity_publisher

流程示例：在终端 3 按 w 加速至 0.2 m/s，待机器人稳定行走后按 + 施加推力干扰，观察响应，结束后 Ctrl+C 停止记录器，CSV 文件自动保存。

离线绘图工具

# 四种步态足端轨迹对比图
python3 scripts/plot_all_gaits.py

# slow_walk 步态足端高度图（论文插图用）
python3 scripts/plot_gait_foot_height.py

步态配置

系统支持四种预设步态，定义在 install/humanoid_common_mpc/share/humanoid_common_mpc/config/command/gait.info：

步态	周期	速度	模式序列
slow_walk	1.7 s	0.20 m/s	LF → STANCE → RF → STANCE
walk	1.4 s	0.40 m/s	LF → STANCE → RF → STANCE
slower_trot	1.2 s	0.55 m/s	LF → RF
trot	1.0 s	1.00 m/s	LF → RF

足端轨迹使用 Hermite 三次样条插值，抬脚高度 0.08 m，离地速度 0.05 m/s。

MPC 参数配置

关键配置文件：robot_models/unitree_g1/g1_centroidal_mpc/config/mpc/task.info

主要可调参数：

参数类别	参数	说明
质心动力学	centroidalModelType	0=FullCentroidalDynamics, 1=SRBD
MPC 频率	mpc.frequency	MPC 求解频率 (Hz)
MRT 频率	mrt.frequency	实时控制频率 (Hz)
关节 PD	mrt.kp_joint / mrt.kd_joint	关节位置/速度增益
代价权重	Q.joint_angle / R.waist	状态/输入二次型权重
摆线轨迹	swingHeight / liftOffVelocity	足端摆动参数
摩擦力	frictionCoefficient	地面摩擦系数

MPC 架构

┌──────────────────────────────────────────────────────┐
│              MPC 求解器线程 (20 Hz)                    │
│  ┌──────────┐  ┌────────────┐  ┌──────────────────┐  │
│  │ 动力学   │  │ 代价函数   │  │ SQP 求解器       │  │
│  │(CppAD AD)│  │(二次型+ICP)│  │(HPIPM+BLASFEO)   │  │
│  └──────────┘  └────────────┘  └──────────────────┘  │
└───────────────────────┬──────────────────────────────┘
                        │ 最优轨迹 (状态 + 输入序列)
┌───────────────────────▼──────────────────────────────┐
│             MRT 实时控制线程 (200 Hz)                   │
│  ┌──────────┐  ┌──────────────┐  ┌────────────────┐  │
│  │PD 控制器 │  │逆动力学(禁用)│  │ MuJoCo 仿真    │  │
│  │kp=200    │  │kd=0(关闭)   │  │ 物理步进       │  │
│  │kd=30     │  │             │  │                │  │
│  └──────────┘  └──────────────┘  └────────────────┘  │
└──────────────────────────────────────────────────────┘

• 质心动力学：FullCentroidalDynamics，通过 Pinocchio 计算质心动量矩阵 $A_g(q)$
• 自动微分：CppAD 为流图、约束、代价生成精确解析雅可比
• 状态 $x = [h_{\text{com}},\ q_{\text{base}},\ q_j]^\top$（动量 + 基座位姿 + 关节角）
• 输入 $u = [\mathcal{W}_l,\ \mathcal{W}_r,\ \dot q_j]^\top$（接触旋量 + 关节速度）