5. MPC
MPC（Model Predictive Control，模型预测控制）可以看成“每一帧都重新规划一小段未来”的控制器。它用模型预测未来，在约束下优化控制输入，然后只执行第一步，下一帧再重新观测、重新优化。
5.1 滚动时域
每个控制周期，MPC 做三件事：
- 从当前状态
出发，预测未来 步。 - 求一串控制输入
，让未来轨迹尽量接近期望。 - 只执行第一个控制输入
，下一帧重复。
这就是“滚动时域”：
当前状态
-> 预测未来 H 步
-> 优化一串控制
-> 执行第一步
-> 收到新状态后重新优化
它不会相信自己对很远未来的预测，而是用高频反馈不断修正。
5.2 优化问题
简化形式：
满足动力学约束：
以及输入和状态约束：
这里的
5.3 约束优势
PID 的优势是简单、高频、可靠。LQR 的优势是把线性模型和二次代价放进反馈增益。MPC 的优势是显式考虑未来和约束。
它能自然表达：
- 关节角不能超过限位。
- 电机力矩不能超过上限。
- 移动机器人速度和加速度有限。
- 机械臂末端不能进入障碍物区域。
- 四足机器人接触力要满足摩擦约束。
这类约束很难优雅地塞进普通 PID。你可以对 PID 输出限幅，但限幅只是事后裁剪；MPC 是在优化时就知道这些限制。
5.4 简化模型
MPC 依赖模型，但不意味着必须用完整高保真动力学。
常见简化模型：
| 机器人 | 常用模型 | 适合任务 |
|---|---|---|
| 差速移动机器人 | unicycle model | 路径跟踪、避障 |
| 汽车式底盘 | bicycle model | 自动驾驶、转向约束 |
| 机械臂 | 线性化关节动力学 | 局部轨迹跟踪 |
| 四足机器人 | 单刚体 / 质心动力学 | 质心速度、姿态、接触力控制 |
| 无人机 | 简化刚体模型 | 姿态和轨迹跟踪 |
MPC 的模型原则是：足够快，足够准，能帮助优化器做出比纯反馈更好的决定。
5.5 代价设计
一个轨迹跟踪 MPC 通常会惩罚：
- 位置误差
- 速度误差
- 姿态误差
- 控制输入大小
- 控制输入变化量
- 偏离参考轨迹的横向误差
如果控制输入变化太剧烈，可以加入：
这会让控制更平滑，减少执行器抖动。
设计代价函数时，不要只盯最后的成功率。要同时看：
- 轨迹误差曲线
- 控制输入是否频繁打满
- 动作是否平滑
- 求解时间是否稳定
- 失败时是约束不可行还是权重不合理
5.6 实时性
MPC 每个周期都要求解优化问题。如果控制周期是 20ms，那么求解器必须稳定地在 20ms 内给出结果，最好还要留出通信和执行余量。
工程里常见做法：
- 缩短预测时域。
- 使用简化模型。
- 使用 warm start，把上一帧解作为下一帧初值。
- 约束和代价保持稀疏结构。
- 求解失败时回退到安全控制器，例如 PD 或刹车。
一个能实时稳定求解的“简化 MPC”，通常比一个模型很漂亮但经常超时的“复杂 MPC”更有价值。
5.7 规划关系
运动规划通常回答“从 A 到 B 走哪条路径”，MPC 更关心“接下来这一小段怎么稳定执行”。
二者可以组合：
全局规划器 -> 参考路径 / 轨迹 -> MPC 局部跟踪 -> 执行器
移动机器人里，全局规划器给路径，MPC 负责在速度、转角和避障约束下跟踪。机械臂里，MoveIt 2 给轨迹，底层控制器负责执行；如果任务对约束和动态响应要求很高，可以在执行层加入 MPC。
5.8 适用边界
MPC 强大，但入门项目不要一开始就上 MPC。以下情况优先用 PID / PD：
- 单关节定点控制。
- 低速、低动态的简单轨迹跟踪。
- 模型还没整理清楚。
- 控制频率很高但算力有限。
- 你还没建立基本闭环调参直觉。
更好的学习顺序是：先用 PID / PD 跑通系统，再用 LQR 理解模型化反馈，最后在约束明显、轨迹复杂时引入 MPC。