先记住一句话

MPC = model + finite-horizon objective + constraints + repeated online solve；“只执行第一步再重规划”是它获得反馈能力的关键。

读取 x̂→展开 horizon→加入约束→解 QP→执行 u₀→下一拍重解
Estimatex̂ₖ
→
Predictx₁…x_N
→
Optimizeu₀…u_N−1
→
Applyonly u₀
↺
Receding horizon 的核心不是保留整条计划，而是 measurement 到来后反复改写它。

1. 标准 linear-quadratic MPC

min Σₖ₌₀ᴺ⁻¹ ‖xₖ−xrefₖ‖²_Q + ‖uₖ−urefₖ‖²_R + ‖x_N−xref_N‖²_P
s.t. xₖ₊₁=Axₖ+Buₖ, x₀=x̂now
x_min≤xₖ≤x_max,　u_min≤uₖ≤u_max

线性 equality/inequality 加 convex quadratic cost 构成 Quadratic Program (QP)，可用成熟 solver 快速可靠求解。

2. Receding horizon loop

at every control tick:
  1. receive synchronized state estimate x̂now
  2. shift previous solution as warm start
  3. update references, dynamics and constraints
  4. solve N-step QP before deadline
  5. safety-check and apply only u₀*
  6. repeat from new measurement

后面的 u₁…uN−1 不是浪费：它们让第一步提前考虑未来 constraint 与 braking distance。

3. Horizon 怎么选

NΔt 要足以覆盖主要 settling/braking/contact planning 时间。太短会 myopic，靠近 constraint 才猛刹；太长增加变量和 model error，收益递减。sample time 决定反应速度与问题规模，不能只增 N 而忽略 compute deadline。

4. Constraint 是 MPC 的中心

  • input：torque、current、steering、thrust bounds；
  • input rate：motor/valve slew、jerk、舒适性；
  • state：joint angle、velocity、temperature、workspace；
  • mixed：friction pyramid、contact wrench、power；
  • terminal：horizon 末必须进入可继续安全控制的 set。

对 linear MPC，polyhedral constraints 能直接保持 QP；collision avoidance、friction cone 精确形式或 discrete contact choice 常引入 nonconvexity。

5. Hard 与 soft constraints

hard constraint 绝不能违反，但 sensor noise/model mismatch 可能让问题 infeasible。soft constraint 引入 slack ξ≥0 并在 objective 高额惩罚：

x ≤ xmax + ξ,　cost += λ₁‖ξ‖₁ + λ₂‖ξ‖²

电流/碰撞安全上限通常应 hard 或由下层独立 limiter 保证；舒适性、tracking corridor 可 soft。给所有 constraint 加 slack 会得到“总有解”，也可能允许危险答案。

6. Terminal cost/set 为什么重要

finite horizon 看不到 horizon 外后果。用 LQR cost-to-go 近似 terminal cost P，并要求 terminal state 进入 local invariant set，可建立 recursive feasibility 与 stability 条件。实际也常用较长 horizon + terminal penalty，但这不是自动证明。

7. Condensed 与 sparse QP

把 dynamics 展开可消去 states，只优化 inputs（condensing），变量少但 Hessian 可能 dense/condition 差；保留每步 state/input 得 sparse banded KKT structure，变量多但 solver 可利用稀疏性。选择取决于 state/input 维度、horizon 和硬件。

8. Warm start 与 deadline

把上一周期 solution 左移一格，通常接近新 optimum。runtime 不只看平均值，要看 worst-case/p99 solve time、iteration cap 和 constraint activation。若超过 deadline：

  • 不能无条件等待 optimizer；
  • 可采用上次计划的下一步、local LQR/PD backup 或安全制动；
  • 必须标记 solution age/feasibility 并进入 supervisor。

9. 线性 MPC 与 LQR 的关系

去掉 constraints，horizon 足够长并选择 Riccati terminal cost 时，MPC 第一动作趋向 LQR feedback。可把 MPC 看成“把 LQR 的 quadratic tradeoff 放入有限时域，并显式加入 constraints、reference 和在线重规划”。

10. 四个可手算的 MPC 例子

例 1：先展开预测

x_next=x+u，x₀=1，候选 inputs=[−0.4,−0.3,−0.2]：x₁=0.6，x₂=0.3，x₃=0.1。MPC objective 和 constraints 正是在这条预测轨迹上逐拍计算。

例 2：硬约束直接淘汰候选

若 |u|≤0.5，候选首拍 u₀=−0.8 即使能最快消除 error，也 infeasible；u₀=−0.5 才在边界内。hard constraint 不是把 cost 加大，而是从可行集删除。

例 3：soft constraint 的 slack

规定 position≤1.0，但预测 1.2，因此最小 slack ε=0.2。若 penalty ρ ε² 且 ρ=100，单这一项 cost 为 100×0.2²=4；ρ 越大越不愿违反，但并不把它变成 hard guarantee。

例 4：deadline 与 warm start

100 Hz controller 每拍只有 10 ms。estimation 用 2 ms、通信预留 1 ms，则 optimizer budget=7 ms；cold start p99=11 ms 会 miss deadline，而 warm start p99=5 ms 可留下 2 ms margin。

常见误解：

solver 返回 solved 不代表 controller 正确。单位/坐标系错、state timestamp 旧、model sign 错时，QP 仍可能完美求出一个危险的数学 optimum。

自测

1. 为什么每周期只执行第一步？

未来 disturbance 与 model error 会使旧计划失效；新 measurement 到来后重解，把 optimization 闭成 feedback loop。

2. 什么时候用 soft constraint？

当严格满足可能导致 infeasible，且有限违反比无控制输出更可接受时；安全硬边界仍需独立保证。

3. horizon 越长越好吗？

不一定；计算和 model error 增长，超过主要动态时间尺度后收益常递减。

一手资料

Rawlings、Mayne 与 Diehl 的 MPC 教材系统覆盖稳定性、估计、鲁棒/随机/非线性 MPC；OSQP 官方 MPC example给出 constrained linear-quadratic QP 与 receding solve。