机器人实时控制栈中的 MPC

10 HzPolicy / task

→

100 HzMPC

→

500 HzWBC

→

10–40 kHzCurrent loop

1. 一种典型层级

task / behavior planner                 1–20 Hz
        ↓ goals, mode, route
trajectory planner / locomotion MPC    20–200 Hz
        ↓ base/contact/force references
whole-body controller (QP)            200–1000 Hz
        ↓ joint torque / impedance targets
joint servo / actuator interface       0.5–2 kHz
        ↓ current / torque command
motor drive FOC current loop           10–40+ kHz

sensors → time sync → state estimator  → feed all relevant layers

频率是常见量级而非标准；robot dynamics、bus、drive 和 compute 会改变。关键原则是下层显著快于上层，并对上层呈现可预测的 closed-loop behavior。

2. MPC 到底控制什么

层级	state/model	可能输出
Centroidal MPC	CoM、momentum、contact schedule	contact wrenches / foothold adjustment
Whole-body MPC	full rigid-body q/q̇ + contact	joint torque 与 contact force trajectory
Vehicle MPC	pose、velocity、steering/tire model	steering、acceleration/brake
Manipulation MPC	arm/object state、collision	joint trajectory/torque/contact force

模型越完整，constraint 表达越直接，但 state 维度和 solve time 增长。centroidal model 常用于 legged locomotion，因为保留 balance/contact force 主效应又比 full dynamics 小。

3. Whole-Body Control 的职责

WBC 常在单个 tick 解 inverse dynamics QP：满足 rigid-body dynamics、contact acceleration/friction 和 torque limits，同时按优先级跟踪 base、swing foot、hand、posture task。它通常 horizon 很短或没有长 horizon；MPC 负责“未来几步如何走”，WBC 负责“当前全身怎样一致执行”。

4. RL policy 可以放哪里

• 替代 locomotion planner/MPC，直接输出 joint targets；
• 为 MPC 生成 reference、terminal value 或 residual dynamics；
• 替代 WBC 输出 torque，再保留 motor safety inner loop；
• 作为 fallback 不合适：未验证的 learned policy 不等同 safety controller。

无论 policy 是 PPO/SAC 训练还是 imitation/VLA，真实部署仍需 state estimator、rate conversion、limit、watchdog 和 actuator protocol。

5. 接口 contract 要写清楚

1. 语义：position/velocity/torque/current 还是 normalized action；
2. 单位：rad vs degree、Nm vs motor-side current、world vs body frame；
3. 时序：timestamp、valid-until、sample/hold rate、latency；
4. limits：谁 clamp，clamp 前后是否回报；
5. mode：contact/motor mode 切换是否原子化；
6. failure：stale、NaN、solver timeout、bus loss 时下层怎么做。

6. Deadline 是 correctness 的一部分

一个 5 ms controller 在 4 ms 平均完成、偶尔 20 ms，不是“平均可用”。需要统计 p50/p95/p99/max、deadline miss bursts 与 CPU/GPU contention。memory allocation、logging、garbage collection、page fault 和 thermal throttling 都会破坏 jitter。

7. 安全链不能只在 optimizer 里

optimizer constraints
  → command sanity / rate limiter
    → actuator torque-current-voltage limits
      → firmware watchdog
        → hardware E-stop / power cutoff

software constraint 防正常计划越界；firmware/hardware 处理上位机 hang、network loss 和 memory corruption。不同层需要独立 failure detection，避免 single point of failure。

8. Timeout 与 infeasible 的 fallback ladder

1. 若上一计划仍新鲜且可行，执行 shift 后下一步；
2. 切到 local LQR/PD stabilization 或保持姿态；
3. 降低任务目标、扩大 soft tracking error，而不放松安全边界；
4. 进入 controlled stop/sit-down；
5. 状态不可置信、通信丢失或 limit 触发时 disable drive/E-stop。

每级进入/退出条件要有 hysteresis，避免两个 controller 来回抖动；切换时确保 command continuity 和 integrator reset。

9. 从仿真到真机的验证阶梯

阶段	重点
unit/model test	Jacobian、discretization、frame/unit、constraint sign
closed-loop simulation	disturbance、noise、delay、saturation、contact variation
software/HIL	真实周期、bus packet、timestamp、watchdog
suspended/low-power test	actuator sign、gain、limit 与 E-stop
progressive envelope	从低速度/轻 payload 扩大 operating region

10. 最值得持续记录的指标

• state estimate freshness/covariance 与 contact confidence；
• tracking error、constraint margin、slack；
• command、actual torque/current 与 saturation duty cycle；
• solver status、KKT residual、iterations、solve time tail；
• bus latency/drop、control jitter、watchdog events；
• battery voltage、motor/drive temperature 与 derating。

11. 四个实时栈预算例子

自测

1. 为什么 motor current loop 要比 MPC 快很多？

它要把电气 dynamics 闭环成上层可近似控制的 torque source，并快速抑制 back-EMF 与电流扰动。

2. WBC 与 MPC 的典型分工是什么？

MPC 规划有限时域 body/contact evolution；WBC 在当前 tick 满足全身 dynamics/接触约束并分配 joint torque。

3. 什么情况下不应继续用上一条 MPC command？

state/计划已过期、contact mode 改变、constraint 不再可行或 safety monitor 触发时。