PID 在当前误差上生成需求，再由下游裁剪；MPC 则在计算需求时就预测未来状态并显式考虑能力约束。本课输入是当前状态、参考、离散模型和约束，输出仍只是当前一个控制量。先推一般线性多步形式，再用两步标量问题手算最优解，最后说明为什么有限时域最优不自动等于闭环稳定。
符号 含义与维度 单位／坐标约定 x k , u k , A d , B d x_k,u_k,A_d,B_d x k , u k , A d , B d 状态n×1、输入m×1、离散矩阵n×n与n×m 例子为NED一轴位置/速度与加速度；矩阵元素按单位映射 N , X , U , X r N,X,U,X_r N , X , U , X r 预测步数、堆叠状态nN×1、输入mN×1、参考nN×1 N无量纲；各分量继承物理单位 Φ , Γ \Phi,\Gamma Φ , Γ 自由响应与输入响应矩阵 nN×n、nN×mN Q ˉ , R ˉ , H , f \bar Q,\bar R,H,f Q ˉ , R ˉ , H , f 状态权重、输入权重、Hessian、线性项 nN×nN、mN×mN、mN×mN、mN×1；以归一化变量计无量纲 P , K , X f P,K,\mathcal X_f P , K , X f 终端权重、局部反馈、终端集合 n×n、m×n、状态空间子集；同一归一化误差坐标
需要补的数学只读这些： 平方目标与最小二乘 和状态空间预测 。先能口头解释本页小例，再进入完整推导；不必预先背下矩阵公式。
先不用优化器：在几条未来动作之间做选择
MPC每拍做三件事：用模型预测一段未来；在允许的动作中比较好坏；只执行所选序列的第一个动作。下一拍重新测量、重新规划，因此它仍是反馈控制，而不是一次算好后闭眼执行整段计划。
代价、约束与梯度各回答不同的问题。 代价回答“可行方案里更喜欢哪个”；约束回答“哪些方案根本不允许”；梯度回答“当前方案稍微改一点，代价朝哪里变”。例如速度误差超过偏好范围可以得到更高代价，但推进器绝不能超过硬上限。把上限只写成一个很大的罚分，并不等于严格禁止越界。
以归一化的一个动作 u u u x = 1 x=1 x = 1 x + = x + u x^+=x+u x + = x + u
J ( u ) = ( 1 + u ) 2 + 0.1 u 2 . J(u)=(1+u)^2+0.1u^2. J ( u ) = ( 1 + u ) 2 + 0.1 u 2 .
第一项衡量动作后还剩多少误差；第二项惩罚动作幅度，系数 0.1 0.1 0.1
梯度在一维就是斜率：d J / d u = 2 ( 1 + u ) + 0.2 u dJ/du=2(1+u)+0.2u dJ / d u = 2 ( 1 + u ) + 0.2 u u = 0 u=0 u = 0 u u u
令斜率为零得到无约束最优 u = − 1 / 1.1 u=-1/1.1 u = − 1/1.1 − 0.6 ≤ u ≤ 0.6 -0.6\le u\le0.6 − 0.6 ≤ u ≤ 0.6 u = − 0.6 u=-0.6 u = − 0.6 u u u
后面两步例会揭示新的困难：第一个动作还影响第二个未来状态，两个动作不再独立。因此“一维最优值裁剪到区间”的直觉不能直接逐项用于多步MPC。Hessian是各方向的斜率怎样变化，即代价曲率；它不是传感器观测矩阵，也不是艇体惯性矩阵。
为什么用Lyapunov“能量”解释稳定性
稳定性需要知道误差是否持续趋近零，而不只看某一拍代价小。Lyapunov函数是一个非负的误差尺度：除目标外为正，沿闭环运动下降。它可以像能量，但不必是真实焦耳数；对归一化状态常选 V = x T P x V=x^TPx V = x T P x P P P
先看 x ˙ = − λ x \dot x=-\lambda x x ˙ = − λ x x x x λ > 0 \lambda>0 λ > 0 s − 1 \mathrm{s^{-1}} s − 1 V = x 2 / 2 V=x^2/2 V = x 2 /2 V ˙ = x x ˙ = − λ x 2 < 0 \dot V=x\dot x=-\lambda x^2<0 V ˙ = x x ˙ = − λ x 2 < 0 P = I P=I P = I
微练习： 约束最优点的梯度非零是否说明求解器错了？把控制代价权重加大是否保证更快到目标？
概念答案：先看允许方向，再谈偏好
不一定错，应检查所有可行微小方向是否还能降低代价。加大控制权重通常更不愿用力，未必更快，可能跟踪更慢。先做二维分配与限幅 认识可达域，再比较本课两步约束优化；分配练习不是MPC求解器，不应据它宣称已完成预测控制。
先把可预测模型写对
x = [ p v ] , p ˙ = v , v ˙ = u , x k + 1 = A d x k + B d u k , A d = [ 1 T 0 1 ] , B d = [ T 2 / 2 T ] . (3.19) \begin{aligned}x&=\begin{bmatrix}p\\v\end{bmatrix},\quad\dot p=v,\quad\dot v=u,\\x_{k+1}&=A_dx_k+B_du_k,\quad A_d=\begin{bmatrix}1&T\\0&1\end{bmatrix},\quad B_d=\begin{bmatrix}T^2/2\\T\end{bmatrix}.\end{aligned}\tag{3.19} x x k + 1 = [ p v ] , p ˙ = v , v ˙ = u , = A d x k + B d u k , A d = [ 1 0 T 1 ] , B d = [ T 2 /2 T ] . ( 3.19 ) 为什么位置那一项不是 T 2 T^2 T 2 周期内速度从 v k v_k v k v k + T u k v_k+Tu_k v k + T u k v k + T u k / 2 v_k+Tu_k/2 v k + T u k /2 T T T B d B_d B d T 2 / 2 T^2/2 T 2 /2 T = 0.2 s T=0.2\,\mathrm s T = 0.2 s p k = 1 m p_k=1\,\mathrm m p k = 1 m v k = 0.3 m / s v_k=0.3\,\mathrm{m/s} v k = 0.3 m/s u k = 0.5 m / s 2 u_k=0.5\,\mathrm{m/s^2} u k = 0.5 m/ s 2 0.4 m / s 0.4\,\mathrm{m/s} 0.4 m/s 1.07 m 1.07\,\mathrm m 1.07 m A d A_d A d B d B_d B d
这是周期内加速度恒定的精确积分：先把速度积分得到增加量 uT，再把线性变化的速度积分得到位移 vT+uT²/2。若已有阻尼模型，应对它离散，而不是把所有对象都当双积分器。物理位置 m 与速度 m/s 不能不加说明地直接平方相加；可先分别除以允许误差尺度，再选择无量纲权重。输入也除以代表性加速度尺度。以下二次规划（QP） 推导默认已经完成这种归一化。
x k + 1 ∣ k = A d x k + B d u k ∣ k , x k + 2 ∣ k = A d 2 x k + A d B d u k ∣ k + B d u k + 1 ∣ k , x k + j ∣ k = A d j x k + ∑ i = 0 j − 1 A d j − 1 − i B d u k + i ∣ k . (3.20) \begin{aligned}x_{k+1|k}&=A_dx_k+B_du_{k|k},\\x_{k+2|k}&=A_d^2x_k+A_dB_du_{k|k}+B_du_{k+1|k},\\x_{k+j|k}&=A_d^jx_k+\sum_{i=0}^{j-1}A_d^{j-1-i}B_du_{k+i|k}.\end{aligned}\tag{3.20} x k + 1∣ k x k + 2∣ k x k + j ∣ k = A d x k + B d u k ∣ k , = A d 2 x k + A d B d u k ∣ k + B d u k + 1∣ k , = A d j x k + i = 0 ∑ j − 1 A d j − 1 − i B d u k + i ∣ k . ( 3.20 ) 推广式的指数来自输入作用了多久。 第 i i i B d B_d B d j − 1 − i j-1-i j − 1 − i j j j A d j − 1 − i B d A_d^{j-1-i}B_d A d j − 1 − i B d i = j − 1 i=j-1 i = j − 1 B d B_d B d i = 0 i=0 i = 0 j j j 0 ≤ i < j 0\le i<j 0 ≤ i < j A d = 0.9 , B d = 0.1 A_d=0.9,B_d=0.1 A d = 0.9 , B d = 0.1 0.81 x k + 0.09 u k + 0.1 u k + 1 0.81x_k+0.09u_k+0.1u_{k+1} 0.81 x k + 0.09 u k + 0.1 u k + 1
第二行把第一行代入一次状态转移；继续代入就得到第三行。上标 j 表示矩阵幂，不是单位。竖线后的 k 表示这些量都是在当前时刻 k 的预测，它们不是未来已经测到的真值。把未来一至 N 步的状态竖着堆叠，过去输入对更晚状态持续起作用，因此输入矩阵是下三角块结构。
X = [ x k + 1 ∣ k ⋮ x k + N ∣ k ] = Φ x k + Γ U , U = [ u k ∣ k ⋮ u k + N − 1 ∣ k ] , Φ = [ A d A d 2 ⋮ A d N ] , Γ = [ B d 0 ⋯ 0 A d B d B d ⋯ 0 ⋮ ⋮ ⋱ ⋮ A d N − 1 B d A d N − 2 B d ⋯ B d ] . (3.21) \begin{aligned}X&=\begin{bmatrix}x_{k+1|k}\\\vdots\\x_{k+N|k}\end{bmatrix}=\Phi x_k+\Gamma U,\quad U=\begin{bmatrix}u_{k|k}\\\vdots\\u_{k+N-1|k}\end{bmatrix},\\\Phi&=\begin{bmatrix}A_d\\A_d^2\\\vdots\\A_d^N\end{bmatrix},\quad\Gamma=\begin{bmatrix}B_d&0&\cdots&0\\A_dB_d&B_d&\cdots&0\\\vdots&\vdots&\ddots&\vdots\\A_d^{N-1}B_d&A_d^{N-2}B_d&\cdots&B_d\end{bmatrix}.\end{aligned}\tag{3.21} X Φ = x k + 1∣ k ⋮ x k + N ∣ k = Φ x k + Γ U , U = u k ∣ k ⋮ u k + N − 1∣ k , = A d A d 2 ⋮ A d N , Γ = B d A d B d ⋮ A d N − 1 B d 0 B d ⋮ A d N − 2 B d ⋯ ⋯ ⋱ ⋯ 0 0 ⋮ B d . ( 3.21 ) 堆叠是在消去状态决策变量。 第 j j j Φ \Phi Φ Γ \Gamma Γ j j j i i i X X X n N nN n N U U U m N mN m N Γ ∈ R n N × m N \Gamma\in\mathbb R^{nN\times mN} Γ ∈ R n N × m N N = 2 N=2 N = 2 Γ \Gamma Γ 4 × 2 4\times2 4 × 2 ( T 2 / 2 , T ) T (T^2/2,T)^T ( T 2 /2 , T ) T ( 3 T 2 / 2 , T ) T (3T^2/2,T)^T ( 3 T 2 /2 , T ) T
二次目标为什么产生这个 Hessian
J = ( X − X r ) ⊤ Q ˉ ( X − X r ) + U ⊤ R ˉ U , e 0 = Φ x k − X r , J = ( e 0 + Γ U ) ⊤ Q ˉ ( e 0 + Γ U ) + U ⊤ R ˉ U = 1 2 U ⊤ H U + f ⊤ U + e 0 ⊤ Q ˉ e 0 , H = 2 ( Γ ⊤ Q ˉ Γ + R ˉ ) , f = 2 Γ ⊤ Q ˉ e 0 . (3.22) \begin{aligned}J&=(X-X_r)^\top\bar Q(X-X_r)+U^\top\bar R U,\\e_0&=\Phi x_k-X_r,\\J&=(e_0+\Gamma U)^\top\bar Q(e_0+\Gamma U)+U^\top\bar R U\\&=\tfrac12U^\top H U+f^\top U+e_0^\top\bar Qe_0,\\H&=2(\Gamma^\top\bar Q\Gamma+\bar R),\qquad f=2\Gamma^\top\bar Qe_0.\end{aligned}\tag{3.22} J e 0 J H = ( X − X r ) ⊤ Q ˉ ( X − X r ) + U ⊤ R ˉ U , = Φ x k − X r , = ( e 0 + Γ U ) ⊤ Q ˉ ( e 0 + Γ U ) + U ⊤ R ˉ U = 2 1 U ⊤ H U + f ⊤ U + e 0 ⊤ Q ˉ e 0 , = 2 ( Γ ⊤ Q ˉ Γ + R ˉ ) , f = 2 Γ ⊤ Q ˉ e 0 . ( 3.22 ) Hessian 是代价曲率，不是系统动力学。 对 1 2 U T H U \tfrac12U^THU 2 1 U T H U H U HU H U H H H H H H z z z z T H z = 2 ( Γ z ) T Q ˉ ( Γ z ) + 2 z T R ˉ z > 0 z^THz=2(\Gamma z)^T\bar Q(\Gamma z)+2z^T\bar Rz>0 z T H z = 2 ( Γ z ) T Q ˉ ( Γ z ) + 2 z T R ˉ z > 0
堆叠权重取 Q ˉ = blkdiag ( Q 1 , … , Q N ) \bar Q=\operatorname{blkdiag}(Q_1,\ldots,Q_N) Q ˉ = blkdiag ( Q 1 , … , Q N ) R ˉ = blkdiag ( R 0 , … , R N − 1 ) \bar R=\operatorname{blkdiag}(R_0,\ldots,R_{N-1}) R ˉ = blkdiag ( R 0 , … , R N − 1 ) Q N = P Q_N=P Q N = P U T H U U^THU U T H U Q ˉ \bar Q Q ˉ R ˉ \bar R R ˉ H H H H U + f = 0 HU+f=0 H U + f = 0
u min ≤ U ≤ u max ⟺ [ I − I ] U ≤ [ u max − u min ] , x min ≤ Φ x k + Γ U ≤ x max ⟺ [ Γ − Γ ] U ≤ [ x max − Φ x k − x min + Φ x k ] . (3.23) \begin{aligned}u_{\min}\le U\le u_{\max}&\Longleftrightarrow\begin{bmatrix}I\\-I\end{bmatrix}U\le\begin{bmatrix}u_{\max}\\-u_{\min}\end{bmatrix},\\x_{\min}\le\Phi x_k+\Gamma U\le x_{\max}&\Longleftrightarrow\begin{bmatrix}\Gamma\\-\Gamma\end{bmatrix}U\le\begin{bmatrix}x_{\max}-\Phi x_k\\-x_{\min}+\Phi x_k\end{bmatrix}.\end{aligned}\tag{3.23} u m i n ≤ U ≤ u m a x x m i n ≤ Φ x k + Γ U ≤ x m a x ⟺ [ I − I ] U ≤ [ u m a x − u m i n ] , ⟺ [ Γ − Γ ] U ≤ [ x m a x − Φ x k − x m i n + Φ x k ] . ( 3.23 ) 双边约束为什么要变成两组。 一个上界 Γ U ≤ x max − Φ x k \Gamma U\le x_{\max}-\Phi x_k Γ U ≤ x m a x − Φ x k Γ U ≥ x min − Φ x k \Gamma U\ge x_{\min}-\Phi x_k Γ U ≥ x m i n − Φ x k − Γ U ≤ − x min + Φ x k -\Gamma U\le -x_{\min}+\Phi x_k − Γ U ≤ − x m i n + Φ x k x + = x + u x^+=x+u x + = x + u x = 0.8 x=0.8 x = 0.8 − 1 ≤ x + ≤ 1 -1\le x^+\le1 − 1 ≤ x + ≤ 1 u ≤ 0.2 u\le0.2 u ≤ 0.2 − u ≤ 1.8 -u\le1.8 − u ≤ 1.8 − 1.8 ≤ u ≤ 0.2 -1.8\le u\le0.2 − 1.8 ≤ u ≤ 0.2
这些上下界都是按时域堆叠的向量。第一组只限制命令，第二组把位置或速度限制转换成决策变量的不等式。若控制变化率有限，再用差分矩阵形成相邻输入之差，第一行必须减去已经施加的上一拍输入。水下艇还可以把 G 的可达集合变成线性约束；若忽略混控的多轴争用，即使 QP 自己可行，实际推进器仍可能不可行。
两步手算：不能把无约束解逐项裁剪就当作 MPC
取归一化标量速度模型 x k + 1 = x k + u k x_{k+1}=x_k+u_k x k + 1 = x k + u k x = 1 x=1 x = 1 1 1 1 0.1 0.1 0.1
Φ = [ 1 1 ] , Γ = [ 1 0 1 1 ] , J = ( 1 + u 0 ) 2 + ( 1 + u 0 + u 1 ) 2 + 0.1 ( u 0 2 + u 1 2 ) , H = [ 4.2 2 2 2.2 ] , f = [ 4 2 ] , det H = 5.24 > 0 , U f r e e = − H − 1 f = [ − 0.91603053 − 0.07633588 ] . (3.24) \begin{aligned}\Phi&=\begin{bmatrix}1\\1\end{bmatrix},\quad\Gamma=\begin{bmatrix}1&0\\1&1\end{bmatrix},\\J&=(1+u_0)^2+(1+u_0+u_1)^2+0.1(u_0^2+u_1^2),\\H&=\begin{bmatrix}4.2&2\\2&2.2\end{bmatrix},\quad f=\begin{bmatrix}4\\2\end{bmatrix},\quad\det H=5.24>0,\\U_{\rm free}&=-H^{-1}f=\begin{bmatrix}-0.91603053\\-0.07633588\end{bmatrix}.\end{aligned}\tag{3.24} Φ J H U free = [ 1 1 ] , Γ = [ 1 1 0 1 ] , = ( 1 + u 0 ) 2 + ( 1 + u 0 + u 1 ) 2 + 0.1 ( u 0 2 + u 1 2 ) , = [ 4.2 2 2 2.2 ] , f = [ 4 2 ] , det H = 5.24 > 0 , = − H − 1 f = [ − 0.91603053 − 0.07633588 ] . ( 3.24 ) 手工复算每个矩阵元素。 Γ T Γ \Gamma^T\Gamma Γ T Γ 1 2 + 1 2 = 2 1^2+1^2=2 1 2 + 1 2 = 2 0 2 + 1 2 = 1 0^2+1^2=1 0 2 + 1 2 = 1 1 1 1 0.1 I 0.1I 0.1 I 4.2 , 2.2 4.2,2.2 4.2 , 2.2 2 2 2 ( 1 , 1 ) T (1,1)^T ( 1 , 1 ) T 2 Γ T e 0 = ( 4 , 2 ) T 2\Gamma^Te_0=(4,2)^T 2 Γ T e 0 = ( 4 , 2 ) T H U = − f HU=-f H U = − f 5.24 5.24 5.24
第一个输入违反下界，因此先假设它在下界；固定它之后重新优化第二个输入，而不是保留原来的第二项。将 u 0 = − 0.6 u_0=-0.6 u 0 = − 0.6 2 ( 0.4 + u 1 ) + 0.2 u 1 = 0 2(0.4+u_1)+0.2u_1=0 2 ( 0.4 + u 1 ) + 0.2 u 1 = 0
u 0 ∗ = − 0.6 , u 1 ∗ = − 0.4 1.1 = − 0.36363636 , X ∗ = [ 0.4 0.03636364 ] , H U ∗ + f = [ 0.75272727 0 ] , J ∗ = 0.21054545. (3.25) \begin{aligned}u_0^*&=-0.6,\quad u_1^*=-\frac{0.4}{1.1}=-0.36363636,\\X^*&=\begin{bmatrix}0.4\\0.03636364\end{bmatrix},\\HU^*+f&=\begin{bmatrix}0.75272727\\0\end{bmatrix},\qquad J^*=0.21054545.\end{aligned}\tag{3.25} u 0 ∗ X ∗ H U ∗ + f = − 0.6 , u 1 ∗ = − 1.1 0.4 = − 0.36363636 , = [ 0.4 0.03636364 ] , = [ 0.75272727 0 ] , J ∗ = 0.21054545. ( 3.25 ) 活动集是一组需要验证的假设。 假定第一项在下界只减少了一个未知数，并没有自动证明答案正确。还必须检验第二项在区间内，以及固定变量的负梯度指向可行域外（梯度沿可行方向使代价增加）。对于下界 u i ≥ l i u_i\ge l_i u i ≥ l i Δ u i ≥ 0 \Delta u_i\ge0 Δ u i ≥ 0 g i Δ u i g_i\Delta u_i g i Δ u i g i ≥ 0 g_i\ge0 g i ≥ 0 g i ≤ 0 g_i\le0 g i ≤ 0 g i = 0 g_i=0 g i = 0
下界处唯一可行的微小方向是增大 u 0 u_0 u 0
参考解答（展开查看）
// 教学实现：枚举两变量盒约束QP的九种活动集。 #include <array> #include <cassert> #include <cmath> #include <iomanip> #include <iostream> #include <limits> struct Result { double cost; std ::array <double , 2 > u,gradient; }; Result solve ( double bound ) { const double H[ 2 ][ 2 ] = {{ 4.2 , 2 },{ 2 , 2.2 }}, f[ 2 ] = { 4 , 2 }; Result best{ std :: numeric_limits < double >:: infinity (),{}, {}}; for ( int s0 =- 1 ;s0 <= 1 ; ++ s0) for ( int s1 =- 1 ;s1 <= 1 ; ++ s1) { const int status[ 2 ] = {s0,s1}; std :: array < double , 2 > u = {s0 * bound,s1 * bound}, g{}; if (s0 == 0 && s1 == 0 ){ const double det = H[ 0 ][ 0 ] * H[ 1 ][ 1 ] - H[ 0 ][ 1 ] * H[ 1 ][ 0 ]; u = {( - f[ 0 ] * H[ 1 ][ 1 ] + H[ 0 ][ 1 ] * f[ 1 ]) / det, ( - H[ 0 ][ 0 ] * f[ 1 ] + f[ 0 ] * H[ 1 ][ 0 ]) / det}; } else if (s0 == 0 || s1 == 0 ) { const int i = s0 == 0 ? 0 : 1 ,j = 1 - i; u[i] =- (f[i] + H[i][j] * u[j]) / H[i][i]; } bool valid = true ; for ( int i = 0 ;i < 2 ; ++ i){ g[i] = H[i][ 0 ] * u[ 0 ] + H[i][ 1 ] * u[ 1 ] + f[i]; if ( std :: abs (u[i]) > bound + 1 e- 10 ) valid = false ; if (status[i] ==- 1 && g[i] <- 1 e- 9 ) valid = false ; if (status[i] == 1 && g[i] > 1 e- 9 ) valid = false ; if (status[i] == 0 && std :: abs (g[i]) > 1 e- 9 ) valid = false ; } if ( ! valid) continue ; const double x1 = 1 + u[ 0 ],x2 = x1 + u[ 1 ]; const double cost = x1 * x1 + x2 * x2 + .1 * (u[ 0 ] * u[ 0 ] + u[ 1 ] * u[ 1 ]); if (cost < best.cost) best = {cost,u,g}; } return best; } int main (){ const auto r = solve ( .6 ); std ::cout << std :: setprecision ( 12 ) << r.cost << ' ' << r.u[ 0 ] << ' ' << r.u[ 1 ] << ' \n ' ; assert ( std :: abs (r.cost - .21054545454545454 ) < 1 e- 10 ); assert ( std :: abs (r.u[ 1 ] + 4.0 / 11 ) < 1 e- 10 ); assert ( std :: abs ( solve ( .4 ).cost - .432 ) < 1 e- 10 ); assert ( std :: abs ( solve ( 1 ).u[ 0 ] + .916030534351145 ) < 1 e- 10 ); }
滚动时域、终端代价与稳定性的真正连接
一次求解得到整段 U U U
V N ( x ) = min U ∑ i = 0 N − 1 ℓ ( x i , u i ) + V f ( x N ) , x 0 = x , x i + 1 = A d x i + B d u i , i = 0 , … , N − 1 , x i ∈ X , u i ∈ U , x N ∈ X f ⊆ X , ℓ ( x , u ) = x ⊤ Q x + u ⊤ R u , V f ( x ) = x ⊤ P x , u = K x , A K = A d + B d K , A K ⊤ P A K − P ⪯ − ( Q + K ⊤ R K ) , x ∈ X f ⇒ K x ∈ U , A K x ∈ X f . (3.26) \begin{aligned}V_N(x)&=\min_U\sum_{i=0}^{N-1}\ell(x_i,u_i)+V_f(x_N),\\x_0&=x,\quad x_{i+1}=A_dx_i+B_du_i,\quad i=0,\ldots,N-1,\\x_i&\in\mathcal X,\quad u_i\in\mathcal U,\quad x_N\in\mathcal X_f\subseteq\mathcal X,\\\ell(x,u)&=x^\top Qx+u^\top Ru,\qquad V_f(x)=x^\top Px,\\u&=Kx,\quad A_K=A_d+B_dK,\\A_K^\top PA_K-P&\preceq-(Q+K^\top RK),\\x\in\mathcal X_f&\Rightarrow Kx\in\mathcal U,\quad A_Kx\in\mathcal X_f.\end{aligned}\tag{3.26} V N ( x ) x 0 x i ℓ ( x , u ) u A K ⊤ P A K − P x ∈ X f = U min i = 0 ∑ N − 1 ℓ ( x i , u i ) + V f ( x N ) , = x , x i + 1 = A d x i + B d u i , i = 0 , … , N − 1 , ∈ X , u i ∈ U , x N ∈ X f ⊆ X , = x ⊤ Q x + u ⊤ R u , V f ( x ) = x ⊤ P x , = K x , A K = A d + B d K , ⪯ − ( Q + K ⊤ R K ) , ⇒ K x ∈ U , A K x ∈ X f . ( 3.26 ) 这些条件分别承担什么证明责任。 初始约束 x 0 = x x_0=x x 0 = x x i ∈ X , u i ∈ U x_i\in\mathcal X,u_i\in\mathcal U x i ∈ X , u i ∈ U x N ∈ X f ⊆ X x_N\in\mathcal X_f\subseteq\mathcal X x N ∈ X f ⊆ X Q , R , P Q,R,P Q , R , P Q ≻ 0 , R ≻ 0 , P ≻ 0 Q\succ0,R\succ0,P\succ0 Q ≻ 0 , R ≻ 0 , P ≻ 0 K K K u = K x u=Kx u = K x x x x V f ( A K x ) − V f ( x ) ≤ − ℓ ( x , K x ) V_f(A_Kx)-V_f(x)\le-\ell(x,Kx) V f ( A K x ) − V f ( x ) ≤ − ℓ ( x , K x )
第一组条件保证终端局部控制每走一步减少的终端代价，至少抵偿这一步的阶段成本；第二组保证终端集合内的局部控制满足约束并留在集合中。若当前最优序列可行，下一拍把余下的输入左移，再接上终端反馈，便构造了一条新的可行候选。比较新旧候选成本，中间项相消，终端不等式消去尾部，留下当前阶段成本的严格下降。TB03-S4
V N ( x k + 1 ) − V N ( x k ) ≤ − ℓ ( x k , u k ) . (3.27) V_N(x_{k+1})-V_N(x_k)\le-\ell(x_k,u_k).\tag{3.27} V N ( x k + 1 ) − V N ( x k ) ≤ − ℓ ( x k , u k ) . ( 3.27 ) 把移位证明完整写下来。 旧最优序列为 ( u 0 ∗ , … , u N − 1 ∗ ) (u_0^*,\ldots,u_{N-1}^*) ( u 0 ∗ , … , u N − 1 ∗ ) ( x 0 ∗ , … , x N ∗ ) (x_0^*,\ldots,x_N^*) ( x 0 ∗ , … , x N ∗ ) x 1 ∗ x_1^* x 1 ∗ ( u 1 ∗ , … , u N − 1 ∗ , K x N ∗ ) (u_1^*,\ldots,u_{N-1}^*,Kx_N^*) ( u 1 ∗ , … , u N − 1 ∗ , K x N ∗ ) N − 1 N-1 N − 1 A K x N ∗ A_Kx_N^* A K x N ∗ − ℓ ( x 0 ∗ , u 0 ∗ ) + ℓ ( x N ∗ , K x N ∗ ) + V f ( A K x N ∗ ) − V f ( x N ∗ ) -\ell(x_0^*,u_0^*)+\ell(x_N^*,Kx_N^*)+V_f(A_Kx_N^*)-V_f(x_N^*) − ℓ ( x 0 ∗ , u 0 ∗ ) + ℓ ( x N ∗ , K x N ∗ ) + V f ( A K x N ∗ ) − V f ( x N ∗ )
这条证明需要模型准确、初始可行、终端集合有效、求解满足相应条件以及状态成本正定等假设；“时域很长”不是替代证明的句子。以标量积分器、Q = 1 Q=1 Q = 1 R = 0.1 R=0.1 R = 0.1 P 2 − P − 0.1 = 0 P^2-P-0.1=0 P 2 − P − 0.1 = 0 P ≈ 1.091608 P\approx1.091608 P ≈ 1.091608 K = − P / ( 0.1 + P ) ≈ − 0.916080 K=-P/(0.1+P)\approx-0.916080 K = − P / ( 0.1 + P ) ≈ − 0.916080 ∣ x ∣ ≤ 0.6 / ∣ K ∣ ≈ 0.655 |x|\le0.6/|K|\approx0.655 ∣ x ∣ ≤ 0.6/∣ K ∣ ≈ 0.655 1 + K ≈ 0.08392 1+K\approx0.08392 1 + K ≈ 0.08392 P P P
模型有误差时，上述移位序列未必仍满足状态约束。鲁棒 MPC 可以用误差界收紧约束并配局部反馈；扰动增广 MPC 可以把上一课的扰动估计带入预测，减少稳态偏差。二者解决的不是同一个问题：估计值是预测的中心，误差界才决定鲁棒裕度。
接回 ArduSub：先选择控制层级，再选优化变量
若 MPC 输出导航系速度，它应接在制导与 Guided 速度目标之间，保留现有速度控制和深度通道；预测模型就必须包含内环动态，而非假设速度命令瞬间实现。若输出导航系加速度，则需要明确替代位置/速度串级的哪一层，并继续处理姿态旋转、G 的局部效能与 AP_Motors6DOF 最终可达域。预测输入与项目输入类型一致，比“把 PID 换成 MPC”这句口号更重要。
手写练习：把盒约束缩成 ±0.4，重新枚举。
参考解答（展开查看）
答案两项都在下界，预测状态为 (0.6,0.2)，梯度为 (1.52,0.32)，代价为 0.432；两项梯度均非负满足下界最优条件。再把上限放宽到 1，恢复无约束解。精读 TB03-S4 时先看线性二次最优控制与有限时域约束，再读终端条件、递归可行性和稳定性证明；每个抽象条件都用本课标量例代入一次。
TB03-S4 · J. B. Rawlings, D. Q. Mayne, M. Diehl — Model Predictive Control: Theory, Computation, and Design, 2nd ed.
免费教材；本课以自行构造的两步QP和标量终端例连接预测、约束与稳定性。
写出二维两步预测矩阵，再谈通用软件
对本课双积分器且 N = 2 N=2 N = 2 A d 2 = [ 1 2 T 0 1 ] A_d^2=\begin{bmatrix}1&2T\\0&1\end{bmatrix} A d 2 = [ 1 0 2 T 1 ] T v Tv T v A d B d = ( 3 T 2 / 2 , T ) T A_dB_d=(3T^2/2,T)^T A d B d = ( 3 T 2 /2 , T ) T
Φ = [ 1 T 0 1 1 2 T 0 1 ] , Γ = [ T 2 / 2 0 T 0 3 T 2 / 2 T 2 / 2 T T ] . \Phi=\begin{bmatrix}1&T\\0&1\\1&2T\\0&1\end{bmatrix},\qquad
\Gamma=\begin{bmatrix}T^2/2&0\\T&0\\3T^2/2&T^2/2\\T&T\end{bmatrix}. Φ = 1 0 1 0 T 1 2 T 1 , Γ = T 2 /2 T 3 T 2 /2 T 0 0 T 2 /2 T . 逐行解释：第一拍位置受第一个输入的半周期平方影响；第一拍速度受一个周期输入影响；第二拍位置既受第一拍输入持续改变的速度影响，也受第二拍输入本身影响；第二拍速度累积两个输入。取 T = 0.2 T=0.2 T = 0.2 ( p , v ) = ( 1 , 0.3 ) (p,v)=(1,0.3) ( p , v ) = ( 1 , 0.3 ) ( u 0 , u 1 ) = ( 0.5 , − 0.5 ) (u_0,u_1)=(0.5,-0.5) ( u 0 , u 1 ) = ( 0.5 , − 0.5 ) ( 1.07 , 0.4 ) (1.07,0.4) ( 1.07 , 0.4 ) ( 1.14 , 0.3 ) (1.14,0.3) ( 1.14 , 0.3 )
矩阵堆叠只是一种求解形式，不是 MPC 的定义。也可以把每步状态作为优化变量，保留稀疏动力学等式；凝聚形式把状态消掉，变量少但矩阵可能更稠密。小例选择凝聚形式是为了看清 Hessian 来源，不代表大系统一定应显式构造或求逆整个 Hessian。数值实现通常解线性方程而非计算逆矩阵；本章二维逆解只是手算检查。
目标权重的单位：为什么“都设成一”可能没有意义
假设允许的位置误差尺度为 s p = 0.2 m s_p=0.2\,\mathrm m s p = 0.2 m s v = 0.1 m / s s_v=0.1\,\mathrm{m/s} s v = 0.1 m/s s u = 0.5 m / s 2 s_u=0.5\,\mathrm{m/s^2} s u = 0.5 m/ s 2 x r x_r x r A d x r = x r A_dx_r=x_r A d x r = x r x ~ = S x − 1 ( x − x r ) \tilde x=S_x^{-1}(x-x_r) x ~ = S x − 1 ( x − x r ) S x = diag ( s p , s v ) S_x=\operatorname{diag}(s_p,s_v) S x = diag ( s p , s v ) u ~ = u / s u \tilde u=u/s_u u ~ = u / s u A ~ = S x − 1 A d S x \tilde A=S_x^{-1}A_dS_x A ~ = S x − 1 A d S x B ~ = S x − 1 B d s u \tilde B=S_x^{-1}B_ds_u B ~ = S x − 1 B d s u
对移动参考，应改为 u ~ k = ( u k − u r , k ) / s u \tilde u_k=(u_k-u_{r,k})/s_u u ~ k = ( u k − u r , k ) / s u x r , k + 1 = A d x r , k + B d u r , k x_{r,k+1}=A_dx_{r,k}+B_du_{r,k} x r , k + 1 = A d x r , k + B d u r , k S x − 1 ( A d x r , k + B d u r , k − x r , k + 1 ) S_x^{-1}(A_dx_{r,k}+B_du_{r,k}-x_{r,k+1}) S x − 1 ( A d x r , k + B d u r , k − x r , k + 1 )
如果不显式归一化，也可以把量纲吸收到物理权重中：位置平方权重带 m − 2 \mathrm{m^{-2}} m − 2 s 2 / m 2 \mathrm{s^2/m^2} s 2 / m 2
约束与失败处理：在线一拍真正需要做的事
一拍 MPC 的最小流程是：读取当前状态与上一拍执行输入 → 将参考变成与模型一致的误差坐标 → 组装自由响应及线性项 → 更新约束右端 → 求 QP → 检查可行性残差与解是否有限 → 只输出第一项 → 保存移位候选作为下一拍初值。对静态模型，Γ , H \Gamma,H Γ , H f f f
本页二维活动集代码用完整 KKT 符号测试，而不是只枚举九个点；自由变量还会通过方程重新求解。对于真实多维 MPC，活动集合数指数增长，不能把这段枚举器直接推广为在线大规模解法；它的教学价值是每个候选为何接受或拒绝都能手算。若希望加入输入变化率约束，第一拍差分为 u 0 − u p r e v u_0-u_{\rm prev} u 0 − u prev u i − u i − 1 u_i-u_{i-1} u i − u i − 1
再核对一次终端例的代数
标量模型为 x + = x + u x^+=x+u x + = x + u Q = 1 , R = 0.1 Q=1,R=0.1 Q = 1 , R = 0.1 V ( x ) = P x 2 V(x)=Px^2 V ( x ) = P x 2 P x 2 = min u [ x 2 + 0.1 u 2 + P ( x + u ) 2 ] Px^2=\min_u[x^2+0.1u^2+P(x+u)^2] P x 2 = min u [ x 2 + 0.1 u 2 + P ( x + u ) 2 ] u u u 0.2 u + 2 P ( x + u ) = 0 0.2u+2P(x+u)=0 0.2 u + 2 P ( x + u ) = 0 u = − P x / ( 0.1 + P ) u=-P x/(0.1+P) u = − P x / ( 0.1 + P ) x 2 x^2 x 2 P = 1 + P − P 2 / ( 0.1 + P ) P=1+P-P^2/(0.1+P) P = 1 + P − P 2 / ( 0.1 + P ) P 2 − P − 0.1 = 0 P^2-P-0.1=0 P 2 − P − 0.1 = 0 P P P K K K
取约束 ∣ u ∣ ≤ 0.6 |u|\le0.6 ∣ u ∣ ≤ 0.6 X f = { x : ∣ x ∣ ≤ 0.6 / ∣ K ∣ } \mathcal X_f=\{x:|x|\le0.6/|K|\} X f = { x : ∣ x ∣ ≤ 0.6/∣ K ∣ } X f ⊆ X \mathcal X_f\subseteq\mathcal X X f ⊆ X ∣ 1 + K ∣ < 1 |1+K|<1 ∣1 + K ∣ < 1