MIT Cheetah 3的MPC控制器实战：如何用凸优化搞定四足机器人的复杂步态？
MIT Cheetah 3的MPC控制器实战：如何用凸优化搞定四足机器人的复杂步态？
当四足机器人在崎岖地形上以3米/秒的速度疾驰时，其背后是一套将复杂动力学转化为数学优化问题的精妙算法。MIT Cheetah 3的成功不仅在于其机械设计，更在于它开创性地运用模型预测控制（MPC）与凸优化技术，将原本需要超级计算机处理的非线性控制问题，转化为能在1毫秒内求解的二次规划问题。这种技术路径为动态腿式机器人提供了一种兼顾实时性与鲁棒性的解决方案。
1. 从复杂动力学到凸优化：问题重构的艺术
四足机器人的运动控制本质上是一个高维非线性优化问题。传统方法往往陷入两个困境：要么采用过度简化的模型导致控制精度不足，要么构建完整动力学模型使得实时求解变得不可能。MIT团队的核心突破在于找到了一个平衡点——通过精心设计的近似方法，将原始问题转化为凸优化问题，同时保留足够多的物理特性。
1.1 动力学简化的三个关键步骤
刚体假设是第一个重要简化。将重达40kg的Cheetah 3视为单一刚体，忽略占体重10%的腿部动力学。这种简化使得系统状态空间从几十维骤降至13维：
状态向量x = [p, v, Θ, ω, g]^T 其中： p ∈ R^3：质心位置 v ∈ R^3：线速度 Θ ∈ R^3：欧拉角（横滚、俯仰、偏航） ω ∈ R^3：角速度 g ∈ R^3：重力项小角度近似处理了最棘手的旋转动力学。当机器人横滚和俯仰角较小时（|φ|,|θ| < 15°），非线性旋转矩阵可线性化为：
R ≈ [ 1 -ψ θ ψ 1 -φ -θ φ 1 ]惯性张量冻结是第三个巧妙处理。假设惯性张量在预测时域内保持不变，即I(t) ≈ I(t₀)。这三个简化共同将原始非线性动力学转化为线性时变系统：
ẋ = A(t)x + B(t)u1.2 凸化处理的工程智慧
即使经过线性化，地面反作用力规划仍面临非凸约束。MIT团队采用两种工程实用策略：
摩擦锥的棱锥近似：将圆锥形摩擦约束用八边形棱锥替代，转化为线性不等式：
|f_x| ≤ μf_z, |f_y| ≤ μf_z → -μf_z ≤ f_x ≤ μf_z, -μf_z ≤ f_y ≤ μf_z接触力的整数松弛：通过预测时域内的接触时序，将离散的接触状态转化为连续的力约束。正在摆动的腿对应力直接设为零，接触腿则施加上述摩擦约束。
表1展示了完整模型与简化模型的对比：
| 特性 | 完整模型 | Cheetah 3简化模型 |
|---|---|---|
| 状态维度 | >30 | 13 |
| 动力学类型 | 非线性微分代数方程 | 线性时变微分方程 |
| 求解复杂度 | NP难 | 凸QP |
| 单次求解时间 | >100ms | <1ms |
| 处理频率 | <10Hz | 20-30Hz |
2. 实时MPC架构设计：从理论到实现
Cheetah 3的MPC控制器能在500μs内完成求解，这得益于精心设计的算法架构。其核心是将传统MPC的"预测-校正"循环拆解为可并行处理的模块化组件。
2.1 分层预测框架
高层轨迹生成器以50-100Hz运行，负责：
- 根据操作指令生成参考轨迹（速度、高度等）
- 确定步态时序和接触序列
- 计算未来0.3-0.5s的脚掌落点
中层QP求解器是核心计算单元，每20-30Hz执行一次，处理：
- 构建并求解凸优化问题
- 计算最优地面反作用力
- 验证解的可行性
低层转矩映射以1kHz运行，将优化得到的力转换为关节转矩：
τ = J^T·(R·f) + C(q,˙q) + G(q)其中雅可比矩阵J实时根据腿部构型更新。
2.2 高效QP求解技巧
采用qpOASES求解器时，团队开发了三种加速技术：
热启动：利用上一周期解作为初始猜测，减少迭代次数
Condensing技术：通过消元法将原问题规模压缩60%：
min (1/2)u^T H u + g^T u s.t. C u ≤ d其中H ∈ R^{3nk×3nk}，n为腿数(4)，k为预测步长(10-16)
稀疏性利用：虽然qpOASES不直接支持稀疏矩阵，但通过重组问题结构，使非零元素集中在特定区域。
表2展示了不同步态下的计算负载：
| 步态 | 预测时域 | QP变量数 | 约束数 | 平均求解时间 |
|---|---|---|---|---|
| 站立 | 0.5s | 120 | 160 | 0.4ms |
| 小跑 | 0.33s | 96 | 128 | 0.3ms |
| 飞奔 | 0.3s | 72 | 96 | 0.2ms |
| 跳跃 | 0.4s | 108 | 144 | 0.35ms |
3. 步态实现的关键参数调优
相同的MPC框架通过调整少量参数即可实现多种步态，这体现了凸优化方法的通用性。不同步态的本质区别体现在三个维度：
3.1 接触时序规划
每种步态对应特定的接触模式序列，例如：
- 小跑：对角线腿对交替接触（左前+右后 → 右前+左后）
- 飞奔：前腿对与后腿对交替接触
- 跳跃：四腿同步接触与腾空
通过调整以下参数定义步态：
class GaitParams: def __init__(self): self.contact_phases = [] # 各腿接触时序 self.swing_height = 0.15 # 摆动腿抬升高度 self.duty_factor = 0.6 # 接触时长占比3.2 权重矩阵设计
QP问题中的权重矩阵Q和R决定系统行为特性：
- Q矩阵：状态跟踪权重，对角线元素对应位置、姿态误差
- R矩阵：力大小权重，影响能量消耗与抗干扰能力
经验性调节规则：
- 增加姿态误差权重可增强抗踢扰能力
- 提高力权重能降低能耗但会减弱敏捷性
- z方向权重通常设为xy方向的5-10倍
3.3 参考轨迹生成
简单的启发式规则足以生成有效的参考轨迹。以飞奔为例，脚掌落点预测采用：
p_{foot}^{des} = p_{hip} + 0.5T·v_{CoM} + k·v_{CoM}其中T为预计接触时间，k为前视增益（通常0.1-0.3）
4. 实战中的挑战与解决方案
即使在凸优化框架下，实际部署仍面临诸多工程挑战。以下是三个典型问题及其解决方案：
4.1 地形适应性问题
当机器人在未知地形（如楼梯）运动时，预测模型会因地形高度变化而失效。Cheetah 3采用两级应对策略：
接触检测算法：通过监测脚部加速度和电流变化，在5ms内识别意外接触
if (||a_actual - a_des|| > threshold && I_motor > threshold) contact = true;在线参考调整：一旦检测到地形变化，立即重新规划参考轨迹：
- 提高下一步的脚掌抬升高度
- 延长下一步的预计接触时间
- 降低参考速度
4.2 执行器饱和处理
电机转矩和速度限制是制约性能的主要瓶颈。通过以下方法避免饱和：
- 在QP约束中添加转矩限幅：
τ_min ≤ J^T·f ≤ τ_max - 速度饱和时自动缩放参考轨迹：
if any(ω_hip > ω_max): v_ref = v_ref * 0.9
4.3 状态估计误差补偿
IMU和编码器噪声会导致估计误差。除了常规的卡尔曼滤波外，控制器通过两种机制增强鲁棒性：
- 误差积分项：在转矩映射层添加姿态误差积分
τ_fb = K_p·e + K_i·∫e dt + K_d·ė - MPC的滚动优化：即使存在估计误差，下一控制周期会重新校正
表3对比了不同扰动下的恢复性能：
| 扰动类型 | 扰动强度 | 恢复时间 | 最大姿态偏差 |
|---|---|---|---|
| 侧向踢击 | 1m/s²加速度 | 0.3s | 8° |
| 后向拉拽 | 0.8m/s²加速度 | 0.4s | 5° |
| 台阶冲击 | 10cm高度差 | 0.5s | 12° |
| 滑动摩擦突变 | μ:0.6→0.2 | 0.6s | 15° |
在开发Cheetah 3控制系统的过程中，最令人惊讶的发现是：即使使用高度简化的模型，只要更新频率足够高（>20Hz），MPC控制器仍能表现出惊人的鲁棒性。这颠覆了传统观念——模型精度并非越高越好，关键在于模型误差特性与更新频率的平衡。当我们在实验室第一次看到机器人在被猛烈踢击后自动调整步态恢复平衡时，就意识到凸优化MPC为动态腿式机器人提供了一条实用化路径。