Part IV · 第 13 章反馈控制 · 13.4

模型预测控制 MPC

模型预测控制在每个控制周期里用模型预测未来几秒，在力矩、速度、障碍等约束下解一个优化问题，只执行第一步，下个周期再重新求解。这一节讲清楚它为什么是反馈、时域之外该放什么（LQR 的终端代价）、它和"把 LQR 输出截断"有什么不同、它在什么时候会把自己逼进死胡同（递归可行性），以及怎样在几毫秒内把它解完：稠密与稀疏 QP、热启动，再到非线性 MPC 与 MPPI 在自行车模型上的路径跟踪。

约 45 分钟前置：6.8 线性规划与二次规划 · 6.11 采样优化 · 6.12 动态规划 · 6.13 轨迹优化 · 9.1 移动机器人 · 13.3 LQR 7 个交互

1为什么机器人需要它

13.3 节的 LQR 给出一个漂亮的答案：$\mathbf u=-K\mathbf x$，一次矩阵乘法，在它的假设下是最优的。可是它的假设里没有"不能"：没有力矩上限，没有关节限位，没有速度上限，没有障碍物，也没有"足底力必须在摩擦锥里"。真实机器人处处是"不能"：

执行器饱和：电机力矩、电流、舵机转速都有上限。LQR 算出 50 N·m，电机只能给 20 N·m，截断之后 LQR 的最优性和稳定性证明都不再成立（第 3 节）。
状态约束：关节限位、速度上限、车道边界、与人保持的距离。这些是"永远不许违反"的，而不是代价里的一项。
预瞄：前方 20 米有个急弯，或者参考轨迹在 2 秒后会突然变化。一个只看当前误差的反馈律，等误差出现才反应，已经晚了。
接触：四足机器人的足底只能推、不能拉，摩擦力不能超过 $\mu f_z$。这些不等式决定了身体能怎样加速。

模型预测控制（Model Predictive Control，MPC）的想法很直接：既然有模型，就在线解一个有限时域的最优控制问题，把所有约束都写进去；只执行第一步，下个周期用新测到的状态再解一次。6.8 节已经看到，线性模型加二次代价加线性约束，这个问题就是一个 QP，并用一辆一维小车演示了"碰到约束就饱和"。这一节回答 6.8 节没回答的问题：为什么"只执行第一步再重解"是反馈；时域之外的未来该怎么算（终端代价，与 LQR 的精确关系）；它与"把 LQR 截断"差在哪里；它什么时候会把自己逼进无解的状态（递归可行性）；以及怎样在几毫秒内解完（稠密与稀疏的 QP、热启动、实时迭代）。最后把它用到非线性的自行车模型上，并和 6.11 节的采样式 MPC（MPPI）对比。

2直觉：看得见几秒，就只为这几秒做打算

夜里开车，车灯照亮前方 50 米。你按这 50 米里看到的东西决定怎么踩油门、打方向，可你不会把这个计划"执行完"：一秒钟后车灯照到了新的路面，你就按新看到的东西重新打算。计划很长，承诺很短：每次只兑现计划的第一小步。

MPC 每个周期做三件事：

$$ \begin{aligned}&\text{1. measure } \mathbf x(t)\\ &\text{2. solve } \min_{\mathbf u_0,\dots,\mathbf u_{N-1}} \sum_{k=0}^{N-1}\ell(\mathbf x_k,\mathbf u_k)+V_f(\mathbf x_N)\ \ \text{s.t. model, constraints},\ \mathbf x_0=\mathbf x(t)\\ &\text{3. apply } \hl{u0}{\mathbf u_0^\star}\text{, discard the rest}\end{aligned} $$

下图是一辆小车（质点，控制量是加速度，$|u|\le1$ m/s²，$|v|\le1$ m/s），每 0.1 s 解一次时域为 $N$ 步的 QP。上面的轨道上拖动琥珀色的目标；中间的图里，绿色是已经走过的轨迹，蓝色虚线是此刻的计划，青色的淡线是过去每 0.5 s 留下的旧计划；下面是输入，琥珀色的那一根就是此刻真正执行的 $\mathbf u_0^\star$。试试"踢一脚"（瞬间加 0.8 m/s），再把模式切到"开环"（只在开始时规划一次，然后照着计划执行到底）。

FIG 1滚动时域：每步重新规划，只执行第一步

代价 $\sum_k (p_k-p_{\rm ref})^2+0.1\,v_k^2+0.1\,u_k^2$，终端代价是同一问题的 LQR 矩阵 $P$（第 3 节）。轨道上的蓝点是当前计划里每隔 0.2 s 的预测位置。"模型误差"让真实小车的质量是模型以为的倍数（加速度变成 $u/\text{倍数}$），控制器并不知道。开环模式下拖动目标会触发一次新的规划；计划用完后 $u=0$。

试一试

在滚动时域模式下按"踢一脚"，等小车回到目标 2 cm 以内且速度小于 0.02 m/s。注意被踢之后的第一个新计划：它是从被踢之后的状态出发的。
切到"开环"，把模型误差调到 ×0.8 或更小、×1.2 或更大，按"重置"。计划用完半秒后，小车离目标超过 20 cm。同样的模型误差下，切回滚动时域再重置，误差去哪了？

核心洞见

MPC 是一个反馈律 $\mathbf u=\kappa_N(\mathbf x)$，只不过这个函数不是写成公式，而是"以 $\mathbf x$ 为初值解一个优化问题，取第一个输入"。反馈来自重新测量、重新求解：模型不准、被踢一脚，下一个周期的计划就从真实的状态出发。开环执行同一个最优计划，模型误差会原封不动地留在结果里。

3动手玩：时域之外，和约束之内

时域之外的代价：终端代价 $P_f$

有限时域有一个天生的盲点：第 $N$ 步以后发生什么，优化器完全不管。对一个不稳定的系统，这可能是致命的。考虑线性化的倒立摆 $\ddot\theta=10\,\theta+u$（$u$ 是角加速度输入，零阶保持离散化，$\Delta t=0.1$ s），代价 $\theta^2+0.1\dot\theta^2+0.1u^2$。如果时域很短又没有终端代价，优化器会想：反正 $N$ 步之内摆还倒不下去，何必花力气？于是它几乎不出力，摆就真倒了。

解决办法来自 6.12 节的贝尔曼原理：如果时域结束后剩下的代价能精确算出来，有限时域就等价于无限时域。约束不起作用时，从 $\mathbf x_N$ 出发的无限时域最优代价正是 LQR 的 $\mathbf x_N^\top P\mathbf x_N$（$P$ 是离散代数 Riccati 方程的解，13.3 节）。把它当终端代价 $V_f(\mathbf x_N)=\mathbf x_N^\top\hl{Pf}{P_f}\mathbf x_N$，$P_f=P$：

FIG 2终端代价：时域多短才会出事？

相平面 (θ, θ̇)：拖动琥珀色初值

θ(t)

蓝色实线是 MPC 的闭环轨迹，紫色虚线是无限时域 LQR，青色点线是 $t=0$ 时 MPC 的预测（空心大点是 $\mathbf x_N$）。琥珀色椭圆是 LQR 价值函数过初值的等值线 $\mathbf x^\top P\mathbf x=\mathbf x_0^\top P\mathbf x_0$。这里不加输入约束，所以 MPC 律是线性的，$\mathbf u_0^\star=-K_N\mathbf x$，读数给出 $K_N$ 和闭环矩阵的谱半径 $\rho$（$\rho<1$ 稳定）。"闭环代价 / LQR" 是 8 s 内两者实际付出的代价之比。

试一试

选"无终端代价"，从 $N=10$ 往下减，找出摆仍然会倒（$\rho>1$）的最长时域。这个时域对应几秒？再看 $P_f=Q$ 时界限移动了多少。
选 $P_f=P$，把时域减到 $N=1$：$K_N$ 与 $K_\infty$ 的差小于 $10^{-6}$。只看一步的 MPC，为什么和无限时域的 LQR 一模一样？

核心洞见

终端代价是"时域之外的未来"的替身。$P_f=P$ 时，只要约束不起作用，MPC 就等于 LQR，与 $N$ 无关：时域只在约束起作用时才需要长。没有终端代价时，时域必须长到足以"看见"不作为的后果；本例需要 $N\ge8$（0.8 s），而且 $N=8$ 时闭环代价仍是 LQR 的约 6 倍。工程上的结论：终端代价几乎总该放 LQR 的 $P$，它不花任何在线计算。

约束之内：MPC 与"截断的 LQR"

最省事的处理约束的办法是：照算 LQR，再把输出截到 $[-u_{\max},u_{\max}]$。它常常够用，但它有两个盲点。第一，LQR 不知道将来会饱和：它以为刹车时能出任意大的减速度，于是放心地加速，等真到了刹车的时候才发现力气不够，只好冲过头（和 13.1 节的积分饱和是同一类病）。第二，它根本看不见状态约束：前面有堵墙，它也照样冲。MPC 把 $\hl{umax}{|u_k|\le u_{\max}}$ 和 $p_k\le p_{\rm wall}$ 都写进预测里，所以会提前刹车。

下图两辆小车从同一点出发去 $p=0$，前方有一堵可以拖动的墙。两者用同一个代价（$Q=\mathrm{diag}(q,0.2)$，$R=1$），MPC 的时域 $N=30$（3 s），终端代价 $P$。

FIG 3截断的 LQR vs. 知道约束的 MPC

紫色 = 截断的 LQR（$u=\mathrm{clip}(-K\mathbf x)$），蓝色虚线 = MPC；红线是墙，绿色虚线是目标 $p=0$，琥珀色虚线是 $\pm u_{\max}$。每次改动都重新仿真 10 s（100 个控制周期，每个周期 MPC 解一个 30 变量的 QP），小车按两倍速重播。起点很远、电机又很弱时，MPC 也可能开到一个 QP 无解的状态（第 4 节的递归可行性问题），这时读数会显示不可行的步数，控制器退而满力刹车。

试一试

让控制更激进（加大 $q$）或让电机更弱（减小 $u_{\max}$），直到截断的 LQR 越过墙 10 cm 以上，而 MPC 仍然停在墙前（越墙不到 0.5 cm）。
反过来：找一组参数，让两条输入曲线在整个 10 s 内完全重合（$\max|u_{\rm LQR}-u_{\rm MPC}|<10^{-4}$），而输入确实碰到过 $u_{\max}$。截断并不总是错的：什么时候它恰好就是最优？

核心洞见

截断的 LQR 只在"此刻"尊重约束，MPC 在"整个预测"里尊重约束。差别出现在现在的选择会让将来的约束起作用的时候：加速太猛，刹车时就会饱和。如果饱和只发生在一开始，之后线性律能接得住，截断恰好就是最优的（上面第二个任务）；可一旦"现在"决定了"将来够不够力"，只有能预测的控制器才算得对。

4第二个视角：MPC 会把自己逼进死胡同

约束带来一个 LQR 没有的新问题：优化问题可能无解。6.8 节讲过，初始状态本身违反硬约束时 QP 不可行。更隐蔽的是：此刻问题可行，MPC 自己选的动作却把系统带到了一个下一步不可行的状态。这叫失去递归可行性（recursive feasibility）。

还是那辆小车（$|u|\le1$），墙在 $p=0$，目标在 $p=-0.3$。在相平面 $(p,v)$ 里有两个重要的集合：

可挽救集 $\hl{Cinf}{\mathcal C_\infty}$：从这些状态出发，满力刹车能停在墙前。速度越大，需要的刹车距离越长（$v^2/2u_{\max}$），边界是一条抛物线。在这个集合外面，任何控制器都救不了。
$N$ 步可行集 $\hl{XN}{\mathcal X_N}$：MPC 的 $N$ 步问题此刻有解的状态。如果 $N$ 步之内的刹车距离不够把车停下，但 $N$ 步之内还没撞到墙，问题依然"可行"：撞墙发生在时域之外，优化器看不到。

$\mathcal X_N$ 比 $\mathcal C_\infty$ 大出来的那一条（下图红色）就是陷阱：此刻可行，却已注定撞墙。更糟的是，MPC 可以从安全的绿色区域自己开进红色区域：它为了尽快到达目标而加速，而墙在时域之外。

FIG 4递归可行性：此刻可行，不代表下一步可行

相平面 (p, v)：拖动琥珀色起点

绿色：$N$ 步问题可行，且在可挽救集内。红色：$N$ 步问题可行，但已在可挽救集外（注定撞墙）。很淡的绿色（只在加终端约束时出现）：还救得了，但这个更严格的 $N$ 步问题不可行。绿色曲线是连续时间的刹车抛物线 $v=\sqrt{2u_{\max}(p_{\rm wall}-p)}$，与离散的可挽救集边界几乎重合。可行性用"满力刹车"精确判断：它让以后每一步的位置都最小。蓝色是闭环轨迹，青色点线是当前计划；问题不可行时画一个红色 ✕，控制器退而满力刹车。代价 $(p-p_{\rm ref})^2+0.1v^2+0.1u^2$，没有终端代价，$\Delta t=0.1$ s。

试一试

不加终端约束，从静止于 $p_0\approx-6$ m 出发（偏差 0.25 m 以内，可用"起点"按钮），找出能一路可行到达目标的最短时域 $N$（再短一步就会撞上不可行）。它和小车跑出的最高速度的刹车时间比，差多少？
打开终端约束 $v_N\le0$（计划结束时必须已经停下或在后退），把时域缩到 $N\le8$（例如 $N=8$），从一个此刻可行的起点（例如静止于 −6 m）运行 MPC：它全程可行并到达目标。代价是什么？再试试 $N=3$：20 s 后它还在半路上。

核心洞见

递归可行性靠的是终端集：要求计划结束时落在一个"控制不变集" $\mathcal X_f$ 里，即一个进去以后有办法永远待在里面（且满足约束）的集合。这里 $\mathcal X_f=\{v\le0\}$：停下或后退的小车，令 $u=0$ 就永远不会撞墙。于是上一步的计划去掉第一步、末尾补一步 $u=0$，就是下一步的一个可行计划，可行性一步步传下去（第 6 节的证明）。代价是保守：$N$ 很短时，小车只敢以"$N$ 步内能停下"的速度前进，慢得像爬。

5第三个视角：几毫秒内解完

同一个 QP，两种写法：稠密（缩并）与稀疏

MPC 的 QP 有两种标准写法，对应 6.13 节的打靶与配点：

稠密（缩并，condensed）：用动力学把所有状态消掉，$\mathbf X=S_x\mathbf x_0+S_u\mathbf U$，只剩 $Nm$ 个输入变量。Hessian $\hl{H}{H}$ 是 $Nm\times Nm$ 的稠密矩阵，因为每个状态都依赖之前所有的输入。
稀疏（sparse）：保留所有状态为变量，动力学作为等式约束。变量多了（$N(n+m)$ 个），但 KKT 矩阵 $\hl{KKT}{K}$ 是带状的：第 $k$ 步只和第 $k\pm1$ 步耦合。用 6.12 节的 Riccati 递推解它，代价随 $N$ 线性增长。

哪种更快，取决于状态维数 $n$、输入维数 $m$ 和时域 $N$。下图上方两幅是两种矩阵的非零结构，下方是一次分解所需的浮点运算数随 $N$ 的变化。

FIG 5稠密 vs. 稀疏：结构与运算量

稠密 Hessian H

稀疏 KKT 矩阵

第一幅：紫色是稠密 $H$ 的非零元（全满）。第二幅：稀疏 KKT 矩阵，变量顺序 $(\mathbf u_0,\mathbf x_1,\mathbf u_1,\dots,\mathbf x_N)$ 之后是动力学的乘子；蓝色是代价的块（$R$、$Q$），绿色是动力学的块（$A$、$B$、$-I$）。下方：每次分解的运算量，稠密取 Cholesky 的 $(Nm)^3/3$，"每步缩并"再加上构造 $H$ 的约 $N^2n^2m$；稀疏取本页 Riccati 递推的朴素实现 $N(4n^3+6n^2m+4nm^2+m^3/3)$。这些是主导项的估计，真实求解器的常数不同（利用对称性约可减半），但增长的阶不变。

试一试

选"四旋翼"（$n=12,m=4$，线性定常）：从 $N=10$ 开始加长时域，停在稀疏刚刚变得更省的那个 $N$（交叉点或再多一步）。再切到"四足凸 MPC"（$m=12$），交叉点移到了哪里？为什么输入维数大时稠密吃亏？

实时预算与热启动

控制器每 10 ms 要给出一个输入，可迭代求解器（ADMM、内点法、有效集法）需要的迭代数是不确定的。两条工程经验：

热启动（warm start）：这一周期的最优计划，和上一周期的计划"向前平移一步、末尾补一步"非常接近。从这里出发，而不是从零出发，求解器只需修一修。
迭代预算：给每个周期设一个迭代上限，到点就交出当前的近似解。热启动之后，几步迭代往往已经足够好；冷启动的几步迭代可能离最优解很远。

下图用 OSQP 式的 ADMM（6.8 节）解小车撞墙问题（$N=30$，墙在 0.05 m，$q=10$），两辆车同时跑：紫色每个周期从零开始，蓝色从平移后的上一个解开始。柱子是每个周期用掉的迭代数（对数坐标），半透明的柱子表示用完预算仍未收敛。

FIG 6热启动：每周期只有几步迭代时谁还靠得住

ADMM 参数：$\rho=0.3$，$\sigma=10^{-6}$，松弛 $\alpha=1.6$，收敛判据为原始与对偶残差都低于 $10^{-3}$（绝对加相对）；约束行做了归一化，代价乘以 0.1（6.8 节说过的预处理）。矩阵只分解一次，每次迭代是一次回代。热启动把上一周期的原变量和对偶变量都平移一步。紫色 = 冷启动，蓝色 = 热启动，红色虚线是迭代预算。

试一试

先在默认预算下看完一次运行，比较两者的平均迭代数。然后把预算降到 2 到 5 次之间，跑完 8 s：冷启动的小车越墙超过 5 cm，热启动的仍在 1 cm 以内。

次优也稳定

热启动在理论上也站得住：第 6 节的稳定性证明里，下一周期的候选计划正是"平移后的上一计划"，它已经让代价下降。所以只要求解器从这个候选出发、并且不让代价变大、不违反约束，即使远没有收敛，闭环依然稳定（Scokaert、Mayne 与 Rawlings，1999："可行即稳定"）。FIG 6 的 ADMM 并不严格满足这些条件（它的中间迭代可能略微违反约束，代价也不保证单调下降），但道理相同：热启动的起点就是平移后的上一计划，离最优解已经很近，所以几步迭代就够用；冷启动的几步迭代离最优解很远，也没有这种保证。

6再看数学

提示：把鼠标悬停在公式中带底色的符号上，对应的图元素会被高亮（FIG 1 的第一个输入柱、FIG 2 的琥珀色价值椭圆、FIG 3 的 $\pm u_{\max}$ 线、FIG 4 的红色区域或绿色边界、FIG 5 的两个矩阵、FIG 6 的热启动柱）。

问题

线性 MPC 在每个采样时刻，以测得的 $\mathbf x(t)$ 为初值解

$$ \begin{aligned} V_N(\mathbf x)=\min_{\mathbf u_0,\dots,\mathbf u_{N-1}}\ &\sum_{k=0}^{N-1}\big(\mathbf x_k^\top Q\mathbf x_k+\mathbf u_k^\top R\mathbf u_k\big)+\mathbf x_N^\top\hl{Pf}{P_f}\mathbf x_N\\ \text{s.t.}\quad &\mathbf x_{k+1}=A\mathbf x_k+B\mathbf u_k,\ \ \mathbf x_0=\mathbf x,\ \ \hl{umax}{\mathbf u_k\in\mathcal U},\ \ \mathbf x_k\in\mathcal X,\ \ \mathbf x_N\in\mathcal X_f, \end{aligned} $$

并执行 $\kappa_N(\mathbf x)=\hl{u0}{\mathbf u_0^\star(\mathbf x)}$。$\mathcal U,\mathcal X$ 是多面体（线性不等式），$\mathcal X_f$ 是终端集。跟踪一个参考 $\mathbf x_{\rm ref}$ 时，把 $\mathbf x$ 换成 $\mathbf x-\mathbf x_{\rm ref}$（参考须是平衡点，或者另加前馈）。

缩并成稠密 QP

把动力学展开：$\mathbf x_k=A^k\mathbf x_0+\sum_{j\lt k}A^{k-1-j}B\mathbf u_j$。叠成向量 $\mathbf X=(\mathbf x_1,\dots,\mathbf x_N)$、$\mathbf U=(\mathbf u_0,\dots,\mathbf u_{N-1})$：

$$ \mathbf X=S_x\mathbf x_0+S_u\mathbf U,\quad S_x=\begin{bmatrix}A\\A^2\\ \vdots\\ A^N\end{bmatrix},\quad S_u=\begin{bmatrix}B&0&\cdots&0\\ AB&B&\cdots&0\\ \vdots&&\ddots&\\ A^{N-1}B&A^{N-2}B&\cdots&B\end{bmatrix}, $$ $$ \begin{aligned}&\min_{\mathbf U}\ \tfrac12\mathbf U^\top\hl{H}{H}\mathbf U+(F\mathbf x_0)^\top\mathbf U\quad\text{s.t.}\quad G\mathbf U\le\mathbf w+E\mathbf x_0,\\ &H=2\big(S_u^\top\bar Q S_u+\bar R\big),\qquad F=2S_u^\top\bar QS_x, \end{aligned} $$

其中 $\bar Q=\mathrm{diag}(Q,\dots,Q,P_f)$，$\bar R=\mathrm{diag}(R,\dots,R)$。$H$ 正定（$R\succ0$ 即可），所以这是一个严格凸 QP，有唯一解。注意 $\mathbf x_0$ 只出现在线性项和约束右端：$H$ 与当前状态无关，线性定常系统可以离线算好、甚至离线分解。状态约束 $\mathbf x_k\in\mathcal X$ 经 $S_u$ 变成关于 $\mathbf U$ 的稠密不等式，这就是 6.8 节 FIG 4 里"墙"的那些约束行。

无约束时就是 LQR。没有约束时 $\mathbf U^\star=-H^{-1}F\mathbf x_0$，第一块就是一个线性增益 $K_N$。若 $P_f=P$ 满足 Riccati 方程 $P=Q+A^\top PA-A^\top PB(R+B^\top PB)^{-1}B^\top PA$，则 6.12 节的反向递推从 $P_N=P$ 开始，每一步都得到同一个 $P$，于是 $K_N=K_\infty$ 对所有 $N$ 成立：这正是 FIG 2 里 $N=1$ 也与 LQR 相同的原因。

稀疏形式与 Riccati

不消元，取 $\mathbf z=(\mathbf u_0,\mathbf x_1,\mathbf u_1,\dots,\mathbf x_N)$，动力学写成等式约束 $\mathbf x_{k+1}-A\mathbf x_k-B\mathbf u_k=0$，乘子为 $\boldsymbol\lambda_{k+1}$。拉格朗日函数的 KKT 条件（6.7 节）是一个线性方程组：

$$ \hl{KKT}{\begin{bmatrix}\mathcal H & \mathcal G^\top\\ \mathcal G & 0\end{bmatrix}}\begin{bmatrix}\mathbf z\\ \boldsymbol\lambda\end{bmatrix}=\begin{bmatrix}0\\ \mathbf b(\mathbf x_0)\end{bmatrix}, $$ $$ \mathcal H=2\,\mathrm{diag}(R,Q,R,Q,\dots,P_f),\quad \mathcal G=\begin{bmatrix}-B & I & & & \\ & -A & -B & I & \\ && & \ddots & \end{bmatrix}. $$

$\mathcal H$ 块对角，$\mathcal G$ 块双对角：重排变量后整个矩阵是带宽约 $2n+m$ 的带状矩阵。消元的顺序如果从 $k=N$ 往回做，得到的正是 Riccati 递推：每一步只处理一个 $(n+m)$ 维的小块，总代价 $O(N(n+m)^3)$。内点法每次迭代只是给 $\mathcal H$ 的对角加上约束的权重（第 $k$ 步的约束只影响第 $k$ 块），所以结构不变，每次迭代仍是一次 Riccati 递推；HPIPM、FORCES Pro 等 MPC 专用求解器就是这么做的。

对比：稠密形式一次 Cholesky 是 $(Nm)^3/3$，对 $N$ 三次方；稀疏是 $N$ 的一次方，但每步付出 $n^3$。所以 $Nm$ 小、$n$ 大（短时域、多状态少输入）时稠密划算，长时域时稀疏划算；介于其间的"部分缩并"（把每几步缩成一块）是很多实时求解器的默认选择。

稳定性：最优代价是李雅普诺夫函数

设终端集 $\mathcal X_f$ 与终端代价满足两个条件（Mayne 等，2000）：在 $\mathcal X_f$ 里有一个局部控制律 $\mathbf u=-K_f\mathbf x$，它满足约束、使 $\mathcal X_f$ 不变；并且

$$ V_f\big((A-BK_f)\mathbf x\big)-V_f(\mathbf x)\le-\big(\mathbf x^\top Q\mathbf x+\mathbf x^\top K_f^\top RK_f\mathbf x\big)\qquad\forall\,\mathbf x\in\mathcal X_f . $$

取 $K_f=K_\infty$、$V_f=\mathbf x^\top P\mathbf x$，这个不等式取等号（这就是 Riccati 方程），$\mathcal X_f$ 取 LQR 不碰约束的一个不变椭圆或多面体。证明思路（"平移论证"）：设 $\mathbf x$ 处的最优计划是 $(\mathbf u_0^\star,\dots,\mathbf u_{N-1}^\star)$，终点 $\mathbf x_N^\star\in\mathcal X_f$。到了下一个状态 $\mathbf x^+=A\mathbf x+B\mathbf u_0^\star$，构造候选计划

$$ \hl{warm}{\tilde{\mathbf U}=\big(\mathbf u_1^\star,\ \mathbf u_2^\star,\ \dots,\ \mathbf u_{N-1}^\star,\ -K_f\mathbf x_N^\star\big)} . $$

它满足所有约束（前 $N-1$ 步本来就满足，最后一步由 $\mathcal X_f$ 的不变性保证），所以 $\mathbf x^+$ 处的问题可行：这就是递归可行性。它的代价是原最优代价减去第一步的代价，再把终端代价换成"多走一步加新的终端代价"，由上面的不等式，后者不会变大：

$$ V_N(\mathbf x^+)\ \le\ J(\mathbf x^+,\tilde{\mathbf U})\ \le\ V_N(\mathbf x)-\big(\mathbf x^\top Q\mathbf x+\mathbf u_0^{\star\top}R\,\mathbf u_0^\star\big). $$

$V_N$ 每步至少下降一个正的阶段代价，又有下界 0，所以它是闭环的李雅普诺夫函数（13.2 节），原点渐近稳定。FIG 4 的终端约束 $v_N\le0$ 是这个论证的一个简化版：$\mathcal X_f=\{v\le0\}$ 在 $u=0$ 下不变，只证了可行性，没要求代价下降。没有终端成分时，这两个结论都可能失败：FIG 2 的发散与 FIG 4 的不可行就是反例。时域足够长时它们通常又会成立（终端成分可以去掉），但"多长才够"依赖于问题，事先很难算。

软约束

状态约束硬性要求时，模型误差或扰动随时可能让问题不可行（6.8 节的误区）。实践中把它们写成软约束：$\mathbf c(\mathbf x_k)\le\boldsymbol\sigma_k$，$\boldsymbol\sigma_k\ge0$，代价加 $\rho_1\mathbf 1^\top\boldsymbol\sigma+\rho_2\|\boldsymbol\sigma\|^2$。线性项是"精确罚"：只要 $\rho_1$ 大于对应硬约束的拉格朗日乘子，原问题可行时的解与硬约束完全相同，只在不可行时才允许最小的违反。FIG 7 里道路边界和障碍物就是这样处理的。

非线性 MPC 与实时迭代；MPPI

模型非线性 $\mathbf x_{k+1}=\mathbf f(\mathbf x_k,\mathbf u_k)$ 时，问题是一个非线性规划，通常用 SQP（6.7 节）：在当前猜测处线性化动力学，$\delta\mathbf x_{k+1}=A_k\delta\mathbf x_k+B_k\delta\mathbf u_k$，$A_k=\partial\mathbf f/\partial\mathbf x$，$B_k=\partial\mathbf f/\partial\mathbf u$，用 Gauss-Newton 近似代价的 Hessian（6.6 节），解一个 QP 得到修正量。实时迭代（RTI，Diehl 等，2005）只做一次 SQP 迭代：上一周期的解平移之后已经很接近最优，一次迭代就够；并且线性化可以在测到新状态之前准备好，测量到达后只剩解一个 QP。时变的 $A_k,B_k$ 意味着每个周期都要重新缩并或重新做 Riccati，FIG 5 的"每步缩并"曲线就是这种情况。

MPPI（6.11 节）解的是同一个"滚动时域"问题，只是不求导：在平移后的上一计划附近采 $K$ 条噪声序列，各自用模型往前仿真得到代价 $S_i$，按 $w_i\propto\exp(-(S_i-S_{\min})/\lambda)$ 加权平均。它能直接用不连续的代价（撞了罚一个大数）和任意不可导的模型，代价是需要很多次仿真（通常在 GPU 上并行），而且只能把约束写成罚函数。

7机器人中的它：非线性 MPC 跟踪路径

自动驾驶的横向控制是 MPC 的经典应用。车用 9.1 节的运动学自行车模型：状态 $(x,y,\theta,\delta)$，$\dot x=v\cos\theta$，$\dot y=v\sin\theta$，$\dot\theta=\tfrac vL\tan\delta$，控制量是转向角速度 $\dot\delta=\omega$，$|\omega|\le0.4$ rad/s，$|\delta|\le0.55$ rad，轴距 $L=2.7$ m，车速 $v$ 恒定。路面宽 7 m，前方有一个可以拖动的红色障碍物。三个控制器同时跑：

非线性 MPC（蓝）：代价是横向误差、航向误差、转角和转向角速度的平方和；道路边界和障碍物写成横向误差的软约束 $e_{\rm lo}(s_k)\le e_k\le e_{\rm hi}(s_k)$。每 0.1 s 做一次实时迭代（一次 Gauss-Newton 线性化 + 一个 QP，用本页的内点法）。
MPPI（琥珀）：同一个模型、同一个代价（约束以同样的罚函数出现），$K$ 条采样，噪声 $\sigma=0.2$ rad/s，温度 $\lambda=1$。
纯追踪（紫，9.1 节）：预瞄距离 $\max(4,\,0.5v)$ m，同样的转向速率上限。它只看路径，看不见障碍。

FIG 7非线性 MPC、MPPI 与纯追踪：同一条路，同一个障碍

全景：拖动红色障碍

跟随视角

右上角选择要看哪个控制器的预测：非线性 MPC 的预测（蓝色虚线带点）、MPPI 的加权平均计划（琥珀色虚线）和其中 32 条采样（琥珀色淡线），或纯追踪的预瞄点（紫点）。三辆车从障碍物前 45 m 同时出发，绕赛道一直开下去；每经过一次障碍记录一次"绕过 / 碰到"（以车后轴中点加 1 m 车身半宽和车头车尾长度判断）。ms/步是你的浏览器里每个控制周期的实测计算时间。改动任何参数都会重新开始。

试一试

把障碍物拖到道路中心线的右侧（横向偏移 $-1.5\le e<0$ m，读数里有 $e$），车速调到 12 m/s 或更高：非线性 MPC 改从左边绕过去，且一次都不碰到。
把预测步数减到 $N\le10$，找到非线性 MPC 绕不过去（碰到障碍或冲出路面）的时域。这时的预瞄距离是多少米？转向角速度只有 0.4 rad/s，要横移 2 m 需要提前多久开始打方向？

真实系统里

起源：MPC 最早来自炼油和化工的过程控制（动态矩阵控制 DMC 等，Qin 与 Badgwell 的综述统计了数千套工业应用），那里采样周期是分钟级的。机器人把它推到了毫秒级。四足：MIT Cheetah 3 的凸 MPC（Di Carlo 等，2018）把机身看作单刚体，绕当前偏航角线性化姿态动力学，以未来若干步的足底力为决策变量、摩擦锥线性化成金字塔，每个周期解一个 QP；计算出的足底力再交给更高频的腿部控制（13.7 节的全身控制会接着做）。ETH 的 OCS2 库在 ANYmal 等机器人上做非线性 MPC。自动驾驶：Autoware 的横向控制器提供一个线性 MPC 实现，默认用带一阶转向延迟的运动学自行车模型；赛车 MPC 用 MPCC（轮廓控制，Liniger 等，2015）同时优化路径进度。四旋翼：非线性 MPC 用全刚体模型直接输出推力与角速度指令。求解器：OSQP（ADMM，Stellato 等，2020）、qpOASES（有效集，热启动极好，Ferreau 等，2014）、HPIPM（稀疏内点法，Riccati 结构）、acados（Verschueren 等，2022：在 HPIPM 之上实现 RTI，自动生成 C 代码）、CasADi（建模与自动微分）；MPPI 则常在 GPU 上运行。显式 MPC：对很小的线性问题，可以离线算出分段仿射的 $\kappa_N(\mathbf x)$（Bemporad 等，2002），在线只查表，适合没有余力解 QP 的嵌入式芯片。

8常见误区

误区 1：时域越长越好，终端代价可有可无

长时域的代价是计算（稠密形式对 $N$ 三次方增长）和对模型误差更敏感的远期预测。没有终端代价时，短时域可以直接让闭环不稳定（FIG 2：$N\le7$ 时摆会倒）。正确的顺序是：先放 $P_f=P$，让时域只需覆盖"约束会起作用的那段未来"，例如 FIG 4 里刹车所需的距离；再按计算预算选 $N$。

误区 2：所有约束都写成硬约束

输入约束（电机力矩、转向速率）是物理事实，硬约束天然可行。状态约束（位置、速度、车道）一旦遇上模型误差、测量噪声或扰动，硬约束就可能让 QP 不可行，控制器在最需要输出的时刻没有输出。状态约束应当写成带精确罚的软约束，并给"求解失败"准备一个后备动作（例如刹车或沿用平移后的上一计划）。

误区 3：MPC 自带鲁棒性

重新测量、重新求解确实给了 MPC 反馈（FIG 1），但第 6 节的稳定性和可行性证明都假设模型精确。模型误差下，"平移的上一计划"不再可行，名义 MPC 可能违反约束甚至失稳。需要保证时，要用鲁棒 MPC（例如管道 MPC：把约束收紧一个由扰动界算出的余量）或随机 MPC（机会约束）。另外，跟踪常值参考而模型有偏差时，名义 MPC 会留下稳态误差，要像 13.1 节的积分项那样加一个扰动估计（偏移消除）。

误区 4：忽略计算延迟

求解要花时间：在 $t$ 时刻测到的状态，算出的输入要到 $t+\Delta$ 才生效。如果 $\Delta$ 与采样周期相当，优化里应该从"预测的 $t+\Delta$ 时刻的状态"出发，或者把延迟写进模型。RTI 把线性化挪到测量之前（准备阶段），正是为了把测量到输出的延迟压到只剩一个 QP。另一方面，求解时间不稳定也很危险：要设迭代上限（FIG 6），并确保上限内交出的解至少可行。

误区 5：模型越精细越好

MPC 只能和它的模型一样好，但模型也决定了能不能及时解完。FIG 7 的运动学自行车模型假设轮胎不侧滑，侧向加速度接近 $\mu g$ 时就不对了（9.1 节的误区）。FIG 7 本身就越过了这条线：赛道最急的弯半径约 12.5 m，光是沿路径开过去就需要 $v^2/R$：10 m/s 时约 8 m/s²，12 m/s 时约 11.5 m/s²，14 m/s 时约 16 m/s²，而干燥路面上轮胎最多给出约 $\mu g\approx9$ m/s²。所以 12 到 14 m/s 的结果只说明控制器在这个模型里的行为，真车会侧滑，这时应换成动力学自行车模型；可每多一个状态，稠密 QP 就更大、非线性更强。四足的凸 MPC 故意用单刚体模型，正是为了每个周期都能解一个凸 QP。选模型时要问：控制器需要预测的那几秒里，哪些效应真的重要？

9小练习

1. MPC 每个周期解出一整串最优输入 $\mathbf u_0^\star,\dots,\mathbf u_{N-1}^\star$，却只执行 $\mathbf u_0^\star$。主要原因是什么？

选项 C。如果模型完美、没有扰动，执行整串计划和滚动执行的结果相同；现实里两者都不成立。FIG 1 的开环模式里，模型误差 ×1.2 就让小车停在离目标几十厘米处，而滚动时域每步都从真实状态重新规划，误差被消除。

2. 线性系统、无约束（或约束都不起作用），终端代价取 Riccati 方程的解 $P$。时域 $N$ 从 1 增加到 50，MPC 的控制律会怎样？

选项 B。$\mathbf x_N^\top P\mathbf x_N$ 恰好是从 $\mathbf x_N$ 出发的无限时域最优代价，所以有限时域问题与无限时域问题的最优解相同（贝尔曼原理）；Riccati 递推从 $P_N=P$ 出发每一步都得到 $P$，增益恒为 $K_\infty$。FIG 2 里 $N=1$ 时差别小于 $10^{-6}$。选项 A 描述的是 $P_f=0$ 或 $P_f=Q$ 的情形。

3. FIG 4 里，没有终端约束、$N=10$ 的 MPC 从安全的起点出发，几秒后变得不可行。最根本的原因是什么？

选项 A。起点在可挽救集里（选项 D 错：满力刹车能停住，更不用说一开始就慢慢走），QP 是凸的且每步都精确求解（B、C 错）。问题出在 MPC 自己的选择：它进入了"此刻可行、却已注定"的红色区域。加终端约束 $v_N\le0$，或者把时域加长到覆盖刹车距离，都能恢复递归可行性。

4. 一个状态维数 $n=12$、输入维数 $m=4$ 的线性 MPC，时域从 $N=20$ 加到 $N=80$。稠密（缩并）形式每次 Cholesky 分解的运算量大约变成原来的几倍？稀疏 Riccati 形式呢？

选项 D。稠密 Hessian 是 $Nm\times Nm$，Cholesky 是 $(Nm)^3/3$，$N$ 变成 4 倍，运算量变成 $4^3=64$ 倍。Riccati 每步处理固定大小的块，总量与 $N$ 成正比，4 倍。所以长时域几乎总是用稀疏形式（或部分缩并）。

5. 关于 MPPI 与基于梯度的非线性 MPC（SQP/RTI），哪个说法是对的？

选项 B。MPPI 同样每个周期预测 $N$ 步、执行第一步、平移计划（6.11 节）。它以采样代替求导，所以代价和模型可以不可导；约束只能通过罚项进入，满足程度取决于采样是否找到可行区域。选项 D 错：RTI 每周期只做一次 SQP 迭代，靠热启动在实践中工作良好，而"可行即稳定"的结论说明次优解也可以稳定。

10延伸阅读

J. B. Rawlings, D. Q. Mayne & M. M. Diehl, Model Predictive Control: Theory, Computation, and Design, 2nd ed., Nob Hill Publishing, 2017 — MPC 的标准教材：稳定性、递归可行性、鲁棒与数值方法
F. Borrelli, A. Bemporad & M. Morari, Predictive Control for Linear and Hybrid Systems, Cambridge University Press, 2017 — 线性 MPC、不变集与显式 MPC
D. Q. Mayne, J. B. Rawlings, C. V. Rao & P. O. M. Scokaert, Constrained Model Predictive Control: Stability and Optimality, Automatica 36(6), 2000 — 终端代价与终端集条件的经典综述
P. O. M. Scokaert, D. Q. Mayne & J. B. Rawlings, Suboptimal Model Predictive Control (Feasibility Implies Stability), IEEE Trans. Automatic Control 44(3), 1999 — 次优解为什么也能稳定
S. J. Qin & T. A. Badgwell, A Survey of Industrial Model Predictive Control Technology, Control Engineering Practice 11(7), 2003 — MPC 在工业中的历史与现状
M. Diehl, H. G. Bock & J. P. Schlöder, A Real-Time Iteration Scheme for Nonlinear Optimization in Optimal Feedback Control, SIAM J. Control and Optimization 43(5), 2005 — 实时迭代（RTI）
A. Bemporad, M. Morari, V. Dua & E. N. Pistikopoulos, The Explicit Linear Quadratic Regulator for Constrained Systems, Automatica 38(1), 2002 — 显式 MPC
B. Stellato, G. Banjac, P. Goulart, A. Bemporad & S. Boyd, OSQP: An Operator Splitting Solver for Quadratic Programs, Mathematical Programming Computation 12, 2020 — FIG 6 的 ADMM 来源
R. Verschueren et al., acados: a Modular Open-Source Framework for Fast Embedded Optimal Control, Mathematical Programming Computation 14, 2022 — RTI、HPIPM 与代码生成
J. Di Carlo, P. M. Wensing, B. Katz, G. Bledt & S. Kim, Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control, IROS 2018 — 四足凸 MPC
G. Williams, A. Aldrich & E. A. Theodorou, Model Predictive Path Integral Control: From Theory to Parallel Computation, J. Guidance, Control, and Dynamics 40(2), 2017 — MPPI
A. Liniger, A. Domahidi & M. Morari, Optimization-Based Autonomous Racing of 1:43 Scale RC Cars, Optimal Control Applications and Methods 36(5), 2015 — 赛车的轮廓控制 MPC（MPCC）

1为什么机器人需要它

2直觉：看得见几秒，就只为这几秒做打算

3动手玩：时域之外，和约束之内

时域之外的代价：终端代价 \(P_f\)

约束之内：MPC 与"截断的 LQR"

4第二个视角：MPC 会把自己逼进死胡同

5第三个视角：几毫秒内解完

同一个 QP，两种写法：稠密（缩并）与稀疏

实时预算与热启动

6再看数学

问题

缩并成稠密 QP

稀疏形式与 Riccati

稳定性：最优代价是李雅普诺夫函数

软约束

非线性 MPC 与实时迭代；MPPI

7机器人中的它：非线性 MPC 跟踪路径

8常见误区

9小练习

10延伸阅读