MPC 数值求解与实时实现

前置自测

📋 前置自测（答不出 ≥ 2 题 → 先回前置章节复习）

1. MPC 稳定性的四条件框架 (A1)-(A4) 中，(A1)-(A3) 分别保证了什么？（参见 §110）
2. 什么是 block-tridiagonal 矩阵？给出一个例子并说明它的 LU 分解复杂度。
3. Newton 法求解非线性方程组的收敛条件是什么？什么是 q-二次收敛？
4. Gauss-Newton 方法与 Newton 法的区别是什么？它为什么适合最小二乘问题？
5. 什么是 Runge-Kutta 方法的 Butcher tableau？RK4 是几阶方法？

---

本章目标

学完本章后，你应能够：

1. 区分三大转录方法（单射击/多射击/直接配点）并根据问题特性选择；
2. 理解 QP 结构利用：从 block-tridiagonal KKT 到 Riccati 递推的 \(O(N)\) 复杂度；
3. 掌握 RTI 的两阶段拆分（preparation/feedback）及其 contractivity 证明；
4. 使用 CasADi + acados 构建 NMPC：从建模到代码生成的完整流程；
5. 对比 QP 求解器：HPIPM（IPM）、qpOASES（active-set）、OSQP（ADMM）的适用场景。

---

知识树总览

MPC 数值求解与实时实现
├── §1 转录方法 ⭐⭐
│   ├── 单射击 (Sequential Shooting)
│   ├── 多射击 (Multiple Shooting)
│   └── 直接配点 (Direct Collocation)
├── §2 结构化 QP ⭐⭐（核心）
│   ├── Condensing (消状态 → dense QP)
│   ├── Sparse (保状态 → Riccati)
│   ├── Partial Condensing (混合)
│   └── BLASFEO panel-major 布局
├── §3 SQP 与 RTI ⭐⭐（核心）
│   ├── Full-step SQP
│   ├── Gauss-Newton Hessian
│   ├── RTI preparation/feedback
│   └── Contractivity 定理
├── §4 CasADi 符号建模 ⭐⭐
│   ├── SX vs MX
│   ├── 自动微分
│   └── Opti stack
├── §5 acados 框架 ⭐⭐
│   ├── 四层架构
│   ├── Python API
│   └── SQP_RTI 模式
├── §6 IPOPT 与内点法 ⭐⭐⭐
│   ├── Log-barrier
│   ├── Inertia correction
│   └── Filter line-search
├── §7 代码生成与嵌入式部署 ⭐⭐
│   └── 交叉编译 → Jetson/STM32
├── §8 配点法细节 ⭐⭐⭐
│   ├── Hermite-Simpson
│   └── Radau IIA
└── 选学
    ├── §A 伪谱法 ⭐⭐⭐⭐
    ├── §B Partial Condensing 调优 ⭐⭐⭐
    ├── §C 混合整数 MPC ⭐⭐⭐⭐
    ├── §D Explicit MPC ⭐⭐⭐
    ├── §E GPU/MPPI ⭐⭐⭐
    ├── §F 可微 MPC ⭐⭐⭐⭐
    └── §G 学习型 warm-start ⭐⭐⭐

---

§1 三大转录方法：单射击、多射击、直接配点 ⭐⭐

动机：从连续 OCP 到有限维 NLP

上一章（§110）讨论了 MPC 的稳定性——那里假设 OCP 能被"精确求解"。但真实计算中，连续时间 OCP 是无限维问题（控制 \(u(t)\) 是函数空间中的元素），必须**离散化为有限维 NLP** 才能交给计算机处理。

这个离散化过程叫做**转录 (transcription)**——它是从"黑板上的数学"到"CPU 上的代码"的第一步。

如果不关心转录方法会怎样

• 选了单射击 + 长时域 → IPOPT 报 restoration failed（条件数爆炸）；
• 选了高阶 Radau 配点但用显式积分 → 失去 L-稳定性，刚性系统发散；
• 对不稳定系统（如倒立摆）用单射击 → Hessian 条件数 \(\sim e^{2\lambda T}\)，优化器无法收敛。

连续 OCP 标准形式

考虑 Bolza 型连续 OCP：

\[ \min_{x(\cdot), u(\cdot)} \Phi(x(t_f)) + \int_{t_0}^{t_f} L(x, u)\, dt, \quad \dot{x} = f(x, u),\ x(t_0) = x_0,\ g(x,u) \le 0. \]

**转录**把这个无限维问题转化为有限维 NLP：\(\min_z F(z)\) s.t. \(c_{\text{eq}}(z) = 0\)，\(c_{\text{ineq}}(z) \le 0\)。三大方法仅在决策变量 \(z\) 的选取和动力学约束的处理方式上不同。

单射击 (Sequential / Single Shooting)

决策变量：仅保留控制序列 \(z_{\text{SS}} = (u_0^\top, \dots, u_{N-1}^\top)^\top \in \mathbb{R}^{Nm}\)。

状态消去：状态通过积分器 \(\phi_k\) 显式前向传播：

\[ x_1 = \phi_0(x_0, u_0), \quad x_2 = \phi_1(x_1, u_1), \quad \dots, \quad x_N = \phi_{N-1}(x_{N-1}, u_{N-1}). \]

每个 \(x_k\) 都是 \(u_0, \dots, u_{k-1}\) 的隐函数。目标函数和约束仅用 \(u\) 表达。

优点：变量数最少（\(Nm\)）；无等式约束。

致命缺点——条件数爆炸：

定理 1.1（单射击条件数爆炸）：对线性不稳定系统 \(\dot{x} = Ax + Bu\)（\(A\) 有特征值实部 \(\lambda > 0\)），单射击 reduced Hessian 条件数

\[ \mathrm{cond}(\tilde{H}_{\text{SS}}) \ge c_1 e^{2\lambda T}. \]

证明思路：解析解 \(x(T) = e^{AT} x_0 + \int_0^T e^{A(T-\tau)} Bu\, d\tau\)。单射击目标的 Hessian 中包含 \(\int e^{A^\top(T-\tau_1)} Q e^{A(T-\tau_2)} d\tau\) 型项，其最大特征值在不稳定方向上 \(\sim e^{2\lambda T}\)，最小 \(\sim 1\)，条件数指数增长。

工程后果：对倒立摆（\(\lambda \approx 3\) rad/s），\(T = 2\) s 时条件数 \(\sim e^{12} \approx 160000\)——IPOPT 需要 double precision 的极限才能收敛。

多射击 (Multiple Shooting, Bock-Plitt 1984)

Bock, Plitt (1984), IFAC Proceedings 17(2):1603-1608, DOI: 10.1016/S1474-6670(17)61205-9。

决策变量：同时把节点状态与控制作为变量

\[ z_{\text{MS}} = (x_0, u_0, x_1, u_1, \dots, x_N) \in \mathbb{R}^{(N+1)n + Nm}. \]

动力学以等式约束形式出现：

\[ c_k := x_{k+1} - \phi_k(x_k, u_k) = 0, \quad k = 0, \dots, N-1. \]

核心优势——条件数控制：

定理 1.2（多射击条件数）：多射击 full Hessian 条件数

\[ \mathrm{cond}(H_{\text{MS}}) \le c_2 e^{2\lambda T/N}. \]

直觉：多射击把长时域 \(T\) 切成 \(N\) 段，每段积分长度仅 \(T/N\)，不稳定模式的放大因子从 \(e^{\lambda T}\) 降为 \(e^{\lambda T/N}\)——这是 Bock 的原始动机：把不稳定 BVP 分成多个稳定 IVP 段。

KKT 系统结构：多射击 NLP 的 KKT 矩阵具有 block-tridiagonal 结构——这是 §2 中 Riccati 递推能高效求解的根本原因。

直接配点 (Direct Collocation)

状态用分段多项式 \(p(t)\) 表示，控制类似。在配点 \(\tau_i\) 上强制动力学：

\[ \dot{p}(\tau_i) = f(p(\tau_i), u(\tau_i)) \quad \text{(defect 约束)}. \]

Hermite-Simpson (HS)：3 级 Lobatto IIIA，4 阶精度。 Radau IIA：\(s\) 级，阶数 \(2s-1\)，L-稳定，适合刚性系统。 Legendre-Gauss-Radau (LGR)：伪谱法的基础，光滑解下谱收敛。

定理 1.3（HS 3 阶精度，Kelly 2017）：HS defect 约束下全局控制误差 \(\|u_k - u^*(t_k)\|_\infty \le C h^3\)，状态端点误差 \(\le C h^4\)。

定理 1.4（LGR 谱收敛）：真解解析时，\(N\) 次 LGR 伪谱解 \(\|\hat{x} - x^*\|_\infty \le C \sigma^{-N}\)（\(\sigma > 1\)）。

三方法对比表

维度	单射击	多射击	直接配点
NLP 变量数	\(O(Nm)\)	\(O(N(n+m))\)	\(O(Ns(n+m))\)
动力学约束	无（已消去）	\(N\) 条连续性	\(Ns\) 条 defect
KKT 结构	稠密	block 双对角	block 带状
对不稳定系统	差（\(e^{\lambda T}\)）	好（\(e^{\lambda T/N}\)）	好
Warm-start	差	很好（shift trick）	好
并行性	几无	节点并行	网格并行
代表实现	fmincon+ode45	MUSCOD-II, acados	Drake, GPOPS-II

工程决策

本质洞察：多射击几乎总是 NMPC 的首选。即便变量数多出 \(Nn\)，结构化 QP 求解 \(O(N(n+m)^3)\) 远优于单射击的 dense QP \(O((Nm)^3)\)；数值稳定性与 warm-start 优势不可替代。

场景	推荐方法	理由
实时 NMPC（四旋翼/四足）	多射击 + ERK/IRK	warm-start + Riccati
离线轨迹优化（航天）	LGR 伪谱	高精度 + 协态映射
教学/快速原型	Drake DirectCollocation	易用 + HS 默认
刚性化工过程	Radau IIA 配点	L-稳定

⚠️ 常见陷阱

#	陷阱	后果	对策
1	单射击 + 长时域不稳定系统	IPOPT restoration failed	切多射击
2	HS defect 公式符号错	阶数降为 2 阶	对拍 Kelly 2017 Eq.(4.9)
3	Radau 配点用显式 RK 实现	失去 L-稳定性	stage 变量全作 NLP 决策变量

练习

1. 定量估算：对倒立摆 \(\lambda = 3\) rad/s，\(T = 2\) s，\(N = 20\)。分别估算单射击和多射击的 Hessian 条件数。
2. 编程题：用 CasADi 实现双轮车（bicycle model）的多射击 NLP，\(N = 20\)，比较 RK4 与 Euler 积分的精度。
3. 选型题：给定化工 CSTR（\(\tau = 30\) min，刚性比 \(10^4\)），选择转录方法和积分器，说明理由。

---

§2 结构化 QP：Condensing vs Sparse vs Partial Condensing ⭐⭐

动机：多射击产生的 QP 有宝贵的结构

多射击转录后，SQP 每步产生的 QP 子问题为：

\[ \min_{x_{0:N}, u_{0:N-1}} \sum_{k=0}^{N-1} \frac{1}{2} \begin{bmatrix}x_k \\ u_k\end{bmatrix}^\top \begin{bmatrix}Q_k & S_k^\top \\ S_k & R_k\end{bmatrix} \begin{bmatrix}x_k \\ u_k\end{bmatrix} + q_k^\top x_k + r_k^\top u_k + \frac{1}{2} x_N^\top Q_N x_N + q_N^\top x_N \]

\[ \text{s.t.} \quad x_{k+1} = A_k x_k + B_k u_k + b_k, \quad k = 0, \dots, N-1. \]

KKT 系统具有 block-tridiagonal 结构。三条处理路线利用这个结构的方式不同。

Condensing：消去状态——完整推导

Condensing 是理解 MPC QP 结构的关键技术。这里给出从动力学递推到 dense QP 的完整推导链，不跳步。

Step 1：动力学递推展开

用动力学递推把所有状态用初始状态 \(x_0\) 和控制序列 \(u_{0:N-1}\) 表达。从第一步开始：

\[ x_1 = A_0 x_0 + B_0 u_0 + b_0 \]

\[ x_2 = A_1 x_1 + B_1 u_1 + b_1 = A_1 A_0 x_0 + A_1 B_0 u_0 + B_1 u_1 + A_1 b_0 + b_1 \]

一般地：

\[ x_k = \underbrace{\prod_{i=0}^{k-1} A_i}_{\Phi_k} x_0 + \sum_{j=0}^{k-1} \underbrace{\prod_{i=j+1}^{k-1} A_i \cdot B_j}_{\Gamma_{k,j}} u_j + \underbrace{\sum_{i=0}^{k-1} \prod_{l=i+1}^{k-1} A_l \cdot b_i}_{\hat{x}_k} \]

其中空积 \(\prod_{i=k}^{k-1} A_i = I\)（\(j = k-1\) 时 \(\Gamma_{k,k-1} = B_{k-1}\)）。

物理含义： - \(\Phi_k x_0\)：初始状态的自由响应（无控制时的自然演化） - \(\Gamma_{k,j} u_j\)：第 \(j\) 步控制 \(u_j\) 对第 \(k\) 步状态的影响——通过 \(k-j-1\) 步动力学传播 - \(\hat{x}_k\)：仿射偏移（来自动力学中的常数项 \(b_i\)，如重力、参考轨迹偏移）

Step 2：矩阵形式

把所有状态堆叠为向量 \(\mathbf{x} = (x_1, \ldots, x_N)^\top \in \mathbb{R}^{Nn_x}\)：

\[ \mathbf{x} = \boldsymbol{\Phi} x_0 + \boldsymbol{\Gamma} \mathbf{u} + \hat{\mathbf{x}} \]

其中 \(\boldsymbol{\Gamma} \in \mathbb{R}^{Nn_x \times Nn_u}\) 是**下三角块矩阵**（因果性：\(x_k\) 只依赖 \(u_0, \ldots, u_{k-1}\)）。

Step 3：代入二次代价

原始 QP 代价（含状态变量）：

\[ J = \frac{1}{2}\sum_{k=0}^{N} x_k^\top Q_k x_k + x_k^\top q_k + \frac{1}{2}\sum_{k=0}^{N-1} u_k^\top R_k u_k + u_k^\top r_k + u_k^\top S_k x_k \]

把 \(\mathbf{x} = \boldsymbol{\Phi} x_0 + \boldsymbol{\Gamma}\mathbf{u} + \hat{\mathbf{x}}\) 代入，按 \(\mathbf{u}\) 的二次项、一次项、常数项整理：

\[ J = \frac{1}{2}\mathbf{u}^\top \underbrace{\left(\boldsymbol{\Gamma}^\top \mathbf{Q} \boldsymbol{\Gamma} + \mathbf{R}\right)}_{\tilde{H}} \mathbf{u} + \underbrace{\left(\boldsymbol{\Gamma}^\top \mathbf{Q}(\boldsymbol{\Phi} x_0 + \hat{\mathbf{x}}) + \boldsymbol{\Gamma}^\top \mathbf{q} + \mathbf{r} + \mathbf{S}^\top(\boldsymbol{\Phi} x_0 + \hat{\mathbf{x}})\right)^\top}_{\tilde{g}^\top} \mathbf{u} + \text{const} \]

其中 \(\mathbf{Q} = \text{blkdiag}(Q_1, \ldots, Q_N)\)，\(\mathbf{R} = \text{blkdiag}(R_0, \ldots, R_{N-1})\)。

Step 4：约束 condensing

状态约束 \(C_k x_k \le d_k\) 代入 \(x_k\) 的表达式：

\[ C_k (\Phi_k x_0 + \sum_j \Gamma_{k,j} u_j + \hat{x}_k) \le d_k \quad \Rightarrow \quad \underbrace{C_k [\Gamma_{k,0}, \ldots, \Gamma_{k,N-1}]}_{\tilde C_k} \mathbf{u} \le d_k - C_k(\Phi_k x_0 + \hat{x}_k) \]

堆叠所有约束得到 dense QP 的不等式约束 \(\tilde C \mathbf{u} \le \tilde d\)。

Condensing 的最终结果：仅含 \(\mathbf{u} \in \mathbb{R}^{Nm_u}\) 的 dense QP：

\[ \min_{\mathbf{u}} \frac{1}{2} \mathbf{u}^\top \tilde{H} \mathbf{u} + \tilde{g}^\top \mathbf{u}, \quad \text{s.t.} \quad \tilde{C} \mathbf{u} \le \tilde{d}. \]

计算复杂度详细分析：

步骤	Flop 数	瓶颈
构造 \(\boldsymbol{\Gamma}\)（\(N(N-1)/2\) 次矩阵乘法）	\(O(N^2 n_x^2 n_u)\)	\(N\) 大时
构造 \(\tilde{H} = \boldsymbol{\Gamma}^\top\mathbf{Q}\boldsymbol{\Gamma} + \mathbf{R}\)	\(O(N^2 n_x n_u^2)\)	\(n_u\) 大时
构造 \(\tilde{g}\)	\(O(N n_x n_u)\)	通常不是瓶颈
求解 dense QP（Cholesky + back-sub）	\(O((Nn_u)^3)\)	\(Nn_u > 100\) 时

总计：\(O(N^2 n_x^2 n_u + (Nn_u)^3)\)。对比 sparse Riccati 的 \(O(N(n_x + n_u)^3)\)。

适用场景：\(N\) 小且 \(n_u \ll n_x\) 时，dense QP 尺寸 \(Nn_u\) 可控。例如四足 SRBD MPC：\(n_x = 13\)，\(n_u = 12\)，\(N = 10\) → dense QP 维度 120，qpOASES 在 \(< 1\) ms 求解。

Sparse (Banded)：保留状态，Riccati 递推

保留所有 \(x_k, u_k\) 作为变量，KKT 系统为 block-tridiagonal。用**Riccati 递推**直接求解：

定理 2.1（Riccati 递推的线性复杂度）

若 stage Hessian 严格正定，则 block-tridiagonal KKT 可经 Riccati 递推求解：

\[ P_N = Q_N, $$ $$ P_k = Q_k + A_k^\top P_{k+1} A_k - (S_k + B_k^\top P_{k+1} A_k)^\top (R_k + B_k^\top P_{k+1} B_k)^{-1} (S_k + B_k^\top P_{k+1} A_k). \]

计算量 \(O(N(n_x + n_u)^3)\)，内存 \(O(N(n_x + n_u)^2)\)。

证明思路：每个 stage 做一次 \((n_x + n_u) \times (n_x + n_u)\) 块的 Cholesky 分解与若干 GEMM/TRSM 运算。\(N\) 个 stage 累加得线性复杂度。这比 dense KKT 的 \(O((N(n_x + n_u))^3)\) 快 \(N^2\) 倍。

跨领域类比：Riccati 递推与 LQR 的关系就像快速傅里叶变换 (FFT) 与 DFT 的关系——利用了问题的递归结构把 \(O(N^3)\) 降为 \(O(N)\)。两者的"secret sauce"都是：大问题可以逐级分解为小问题的迭代。

Condensing vs Sparse 的 Break-even

定理 2.2（Break-even 点）

Condensing flop \(\sim N^3 n_u^3\)；sparse flop \(\sim N(n_x + n_u)^3\)。存在

\[ N^\star \approx \frac{n_x + n_u}{n_u} \cdot c \quad (c \in [1, 3] \text{ 硬件常数}), \]

使 \(N < N^\star\) 时 condensing 更快，否则 sparse 更快。

典型数值：

\((n_x, n_u, N)\)	Condensing time	Sparse/Riccati time	Winner
(13, 12, 10)	0.3 ms	0.5 ms	Condensing
(13, 4, 30)	2.1 ms	0.8 ms	Sparse
(24, 12, 50)	15 ms	1.5 ms	Sparse

Partial Condensing (Axehill 2015)

Axehill (2015), Controlling the level of sparsity in MPC, Syst. Control Lett. 76:1-7, DOI: 10.1016/j.sysconle.2014.12.005。

把 horizon 分为 \(N_2\) 段，每段长 \(M = N/N_2\)：段内 condensing（消去段内状态），段间保留稀疏。结果是一个 \(N_2\) 步的 "粗粒度" OCP-QP，每步变量维度 \(n_x + M n_u\)。

最优 \(N_2^\star\) 的确定：理论公式 \(N_2 \approx \lceil N n_u / (n_x + n_u) \rceil\) 仅作初值。实际做法：网格搜索 \(N_2 \in \{1, 3, 5, 8, 10, N\}\)，测量 time_qp 确定——ARM 与 Intel 的最优点差异显著。

BLASFEO：panel-major 布局的革命

Frison et al. (2018), BLASFEO: Basic Linear Algebra Subroutines for Embedded Optimization, ACM TOMS 44(4):42, DOI: 10.1145/3210754。

问题：OpenBLAS/MKL 的 GEMM 为大矩阵设计——每次调用前做 \(O(mn)\) 的 packing（把列主序数据重排为 panel 格式以利 SIMD）。对 MPC 的小矩阵（\(m, n \le 100\)），packing 开销无法被 \(O(mnk)\) 计算摊销。

解决方案：BLASFEO 直接以 panel-major 格式存储矩阵——面板宽度 \(p_s \in \{4, 8\}\) 对应 SIMD 宽度（AVX2 = 4 double，AVX-512 = 8 double）。消除了 packing 开销。

性能数据：BLASFEO-HP 在 \(m = n \le 100\) 区间比 MKL/OpenBLAS 快 20-30%，LAPACK 例程（Cholesky、LU）快 2-3 倍。这是 acados 能在 Jetson Orin 上做 200 Hz whole-body NMPC 的底层原因。

QP 求解器对比

求解器	算法	warm-start	适用场景
HPIPM	Mehrotra IPM + Riccati	弱（IPM 不便热启）	acados 默认，OCP-QP
qpOASES	参数 active-set	极佳（同伦 + Givens）	小 dense QP（化工、SRBD）
OSQP	ADMM (一阶)	好（\(x, z, y\) 可热启）	通用稀疏，代码生成
ProxQP	半光滑 Newton + ALM	好	WBC/IK 退化 QP

OSQP 的独特优势与局限：

• 优势：单次因子分解（setup 阶段做一次 \(LL^\top\) 或 \(LDL^\top\)），后续迭代仅需矩阵-向量乘和投影——非常适合嵌入式代码生成。Warm-start 只需设初始 \((x^0, z^0, y^0)\)。
• 局限：一阶方法收敛慢——ill-conditioned 问题可能需 5000+ 迭代。解的精度受 ADMM 固有限制（通常 \(10^{-3}\) 到 \(10^{-4}\)，不如 IPM 的 \(10^{-8}\)）。

HPIPM 的 Mehrotra predictor-corrector：

1. Affine direction：解线性系统得到"预测方向"\((\Delta x^a, \Delta s^a, \Delta \lambda^a)\)；
2. Centering + correcting：利用 affine 步的信息估计最优 \(\mu\) 并加入互补修正；
3. 合并方向：一次线性求解得到最终方向，步长由 fraction-to-boundary 决定。

每次迭代的主要计算量是 Riccati 递推求解 KKT 系统——复杂度 \(O(N(n_x + n_u)^3)\)。外层迭代数 \(k_{\text{IPM}} \in [10, 30]\)，独立于 \(N\)（这是 IPM 的核心理论优势）。

⚠️ 常见陷阱

#	陷阱	后果	对策
1	长 horizon 用 full condensing	Dense QP \(O((Nm)^3)\) 爆炸	切 sparse 或 partial condensing
2	BLASFEO target 不匹配	性能降 60-70%	精确指定 CPU 架构
3	OSQP 处理 ill-conditioned QP	>5000 ADMM 迭代	Ruiz 预调 + \(\rho\) 自适应 + 放松容差
4	qpOASES 活动集剧变	Cold-start 退化 \(O(n^3)\)	限参考变化率 + fallback HPIPM

练习

1. 计算题：四旋翼 \(n_x = 13\)，\(n_u = 4\)，\(N = 30\)。分别计算 condensing 和 sparse 的 flop 数，判断哪个更快。
2. 编程题：用 Python 实现 LTV-MPC 的 Riccati 递推求解器。验证其解与 numpy.linalg.solve（直接法）一致但快 \(N\) 倍。
3. 调优题：对给定 acados 问题，扫描 qp_solver_cond_N = {1, 5, 10, 20, N}，画出 time_qp vs cond_N 曲线。

---

§3 SQP 与 Real-Time Iteration ⭐⭐

动机：从"解到收敛"到"一步即用"

Full SQP 需要多次迭代才能收敛——对 200 Hz 的四足 MPC，每周期只有 5 ms，根本来不及跑 5-10 次 Newton 迭代。RTI 的天才之处是：每周期只做一次 SQP 迭代，但通过 preparation/feedback 拆分和 warm-start，让这"一步"足够好。

Full-step SQP

在 \(w^k = (x^k, u^k, \lambda^k, \mu^k)\) 处二阶展开 Lagrangian \(\mathcal{L} = \Phi + \lambda^\top g + \mu^\top h\)，解 QP：

\[ \min_{\Delta w} \frac{1}{2} \Delta w^\top B^k \Delta w + \nabla\Phi^\top \Delta w, \quad \text{s.t. } g(w^k) + \nabla g^\top \Delta w = 0,\ h(w^k) + \nabla h^\top \Delta w \le 0, \]

其中 \(B^k \approx \nabla^2_{ww} \mathcal{L}\) 是 Lagrangian Hessian 的近似。

Gauss-Newton Hessian：MPC 的"最佳近似"

对 least-squares cost \(\Phi = \frac{1}{2} \|R(w)\|^2\)（几乎所有跟踪 MPC 都是这种形式），取

\[ B^k_{\text{GN}} = J(w^k)^\top J(w^k), \]

丢弃二阶项 \(\sum R_i \nabla^2 R_i\)。

三大优势： 1. 自动 PSD：\(J^\top J \succeq 0\)，QP 子问题总是凸的——不需要 Hessian 正则化； 2. 不需要二阶导：只需一阶 Jacobian \(J\)，与多射击前向灵敏度兼容； 3. 对低残差 tracking NMPC 几乎与 exact Newton 同速。

定理 3.1（GN 零残差 q-二次收敛）：

对 \(\Phi = \frac{1}{2}\|R(w)\|^2\) 无约束问题，若 \(R(w^\ast) = 0\)（零残差），则 GN 迭代在 \(w^\ast\) 邻域内 q-二次收敛：

\[ \|w^{k+1} - w^\ast\| = O(\|w^k - w^\ast\|^2). \]

若小残差 \(\|R(w^\ast)\| > 0\) 但小，则 q-线性收敛，速率 \(\sigma^\ast = \|(J^{\ast\top} J^\ast)^{-1} \sum_i R_i(w^\ast) \nabla^2 R_i(w^\ast)\|\)。

反事实推理：如果不用 GN 而用 exact Newton 会怎样？(1) 需要二阶导——对复杂动力学（如 Pinocchio 的多体模型）计算量巨大；(2) exact Hessian 可能不定——需要正则化，引入额外复杂性；(3) 对 tracking MPC（残差小），GN 已经是二次收敛，exact Newton 没有加速。结论：GN 是 tracking NMPC 的"自然选择"。

RTI：每周期一次迭代

Diehl, Bock, Schlöder (2005), A real-time iteration scheme for nonlinear optimization in optimal feedback control, SIAM J. Control Optim. 43(5):1714-1736, DOI: 10.1137/S0363012902400713。

RTI 把每个采样周期的计算拆成两阶段：

Preparation 阶段（在前一周期末执行，此时新测量 \(\hat{x}\) 尚未到达）：

1. Shift warm-start：\(\tilde{w}^k = \text{shift}(w^{k-1})\)（把上一步解平移一格，末端外推）；
2. 前向积分 + 灵敏度：沿 \(\tilde{w}^k\) 积分得 \(F_x, F_u\)（动力学 Jacobian）；
3. Hessian/梯度评估：\(\nabla\Phi\)，GN Hessian \(J^\top J\)；
4. Condensing 或 Riccati 因子分解。

Feedback 阶段（新测量 \(\hat{x}_0\) 到达后）：

1. 注入初始状态：lbx_0 = ubx_0 = x̂_0；
2. 求解 QP → \((\Delta x, \Delta u, \Delta\lambda, \Delta\mu)\)；
3. 施加第一步：\(u_0^k := u_0^{\text{init}} + \Delta u_0\)。

关键指标：\(\Delta t_{\text{fb}} / \Delta t_{\text{prep}} \in [10^{-3}, 10^{-2}]\)——feedback 阶段比 preparation 快 100-1000 倍！这意味着"测量到控制输出"的延迟极小。

RTI Contractivity 定理

定理 3.2（Diehl-Bock-Schlöder 2005 Thm 3.5）

设参数族 \(\{P(\hat{x}_0)\}\) 在解路径邻域满足 LICQ + SSOSC + Lipschitz + 严格互补。存在 \(\kappa < 1\)、\(\omega \ge 0\) 与邻域 \(\mathcal{N}\)，对 \(w^k \in \mathcal{N}\)：

\[ \boxed{\|w^{k+1} - w^\ast(\hat{x}_0^{k+1})\| \le \kappa \|w^k - w^\ast(\hat{x}_0^k)\| + \omega \|\hat{x}_0^{k+1} - \hat{x}_0^k\|} \]

RTI 收敛的完整分析

四个关键假设的含义

假设	数学含义	物理含义	违反时后果
LICQ	活动约束 Jacobian 满秩	约束"方向"线性独立	乘子不唯一→方向不确定
SSOSC	缩减 Hessian 正定	最优解是严格局部最优	可能有多个局部最优→Newton 方向不唯一
Lipschitz	\(f, \ell, g\) 连续可微	物理系统光滑	非光滑（接触切换）→\(\omega\) 爆炸
严格互补	\(\lambda_i^\star > 0\) for all active	活动约束"显著"活动	活动集识别困难→chattering

证明的完整展开

Step 1（Robinson 强正则性）：KKT 系统 \(F(w, p) = 0\) 在 \((w^\ast, p)\) 满足 LICQ + SSOSC + 严格互补时，隐函数定理的强形式（Robinson 1980）保证解映射 \(w^\ast: p \mapsto w^\ast(p)\) 是局部 Lipschitz 的：

\[ \|w^\ast(p_1) - w^\ast(p_2)\| \le L_w\|p_1 - p_2\| \]

其中 \(L_w\) 由 KKT 矩阵的最小特征值决定。设 \(\omega = L_w\)。

Step 2（固定参数 Newton 收缩）：对固定 \(p\)，full-step SQP 是 KKT 系统的 Newton 法。在 \(w^\ast(p)\) 邻域内，Newton 法收缩：

\[ \|w^{k+1} - w^\ast(p)\| \le \kappa_N\|w^k - w^\ast(p)\| + O(\|w^k - w^\ast\|^2) \]

\(\kappa_N < 1\) 取决于 KKT Jacobian 的 Lipschitz 常数。对 GN Hessian，\(\kappa_N\) 由残差大小控制——零残差时 \(\kappa_N \to 0\)（二次收敛）。

Step 3（三角不等式合并）：时刻 \(k\) 参数为 \(p^k\)，时刻 \(k+1\) 参数为 \(p^{k+1}\)。一次 SQP 迭代后：

\[ \begin{aligned} \|w^{k+1} - w^\ast(p^{k+1})\| &\le \|w^{k+1} - w^\ast(p^k)\| + \|w^\ast(p^k) - w^\ast(p^{k+1})\| \\ &\le \kappa_N\|w^k - w^\ast(p^k)\| + \omega\|p^k - p^{k+1}\| \end{aligned} \]

取 \(\kappa = \kappa_N\)，\(\omega = L_w\)，得到 contractivity 不等式。

Step 4（联合闭环 ISS 推导）

设闭环状态满足 ISS-MPC：\(V_N^\ast(x^+) - V_N^\ast(x) \le -\alpha_\ell(\|x\|) + \sigma_w(\|w\|)\)。

RTI 引入的次优性误差 \(\delta^k = \|w^k - w^\ast\|\) 相当于"额外扰动"。联合闭环：

\[ V_N^\ast(x^+) - V_N^\ast(x) \le -\alpha_\ell(\|x\|) + \sigma_w(\|w_\text{ext}\|) + L_\text{cost}\delta^k \]

由 contractivity，\(\delta^k\) 有界且收敛→联合闭环 ISS。

工程含义：结合 §110 的 ISS-Lyapunov 不等式——若测量差分 \(\|\hat{x}_0^{k+1} - \hat{x}_0^k\|\) 有界（闭环 ISS），则 RTI 跟踪误差亦有界。这就是"数值近似的 ISS + 模型 MPC 的 ISS = 联合 ISS"定理。

RTI 何时失效

RTI 的 contractivity 定理有局限性——它只在解路径邻域 \(\mathcal{N}\) 内成立。以下情况可能导致 \(w^k \notin \mathcal{N}\)，contractivity 失效：

失效场景	原因	工程对策
大扰动（如被踢一脚）	\(\|\Delta x_0\|\) 超出 \(\omega\) 的线性近似	多迭代 SQP（max_iter > 1）
活动集突变（步态切换）	LICQ/严格互补在切换点失效	Shift 时重新识别 phase
初始猜测太差（cold-start）	\(w^0\) 远离 \(w^\ast\)	MPPI warm-start 或 fallback
非光滑动力学（接触）	\(f\) 不 Lipschitz	Contact-scheduled（预定义切换）

RTI 伪代码

每周期 k:
[Preparation 于 k-1 末]
  1. w̃^k = shift(w^{k-1})              // warm-start 平移一格
  2. 多射击积分 + 前向灵敏度 → F_x, F_u
  3. ∇Φ, GN-Hessian J^T J
  4. condensing 消状态 → dense QP (或 Riccati 因子分解)
  5. 因子分解 (Cholesky / Riccati)
[Feedback 于 k 末（测量 x̂_0 到达）]
  6. set lbx_0 = ubx_0 = x̂_0
  7. 解 QP → (Δx, Δu, Δλ, Δμ)
  8. u_0^k := u_0^init + Δu_0, 施加到 plant
  9. w^k := w̃^k + Δw                   // full-step, 无 merit

Suboptimal MPC 理论支撑

Scokaert, Mayne, Rawlings (1999), Suboptimal model predictive control (feasibility implies stability), IEEE TAC 44(3):648-654。

定理 3.3（Feasibility implies stability）：若每步返回的是可行解 \(\tilde{\mathbf{u}}\) 且满足

\[ V_N(x^+, \tilde{\mathbf{u}}^+) \le V_N(x, \tilde{\mathbf{u}}) - \ell(x, \tilde{u}_0), \]

则闭环渐近稳定——无需全局最优。

与 RTI 的关系：RTI 的 shift warm-start 构造的候选序列恰好满足此条件（§110 证明中的 Step 1-2），因此 RTI 单次迭代产生的解自动满足 suboptimal MPC 的稳定性条件。这是 RTI 理论的深层保障。

⚠️ 常见陷阱

#	陷阱	后果	对策
1	RTI 未拆两阶段	feedback 延迟 = preparation + feedback 总时间	手工 rti_phase=1/2
2	GN 用于非 LS cost	Hessian 不 PSD，上升方向	LM 阻尼 或切 BFGS
3	Shift 时只移 primal 不移 dual	active-set 扰乱	同时 shift \(\lambda, \mu\)
4	不稳定系统 warm-start 偏差太大	RTI 不收缩（\(w^k \notin \mathcal{N}\)）	多做几次 SQP 迭代（nlp_solver_max_iter > 1）

练习

1. 推导题：对线性 MPC（\(f\) 线性，\(\ell\) 二次），证明 RTI 一步就给出精确最优解（无需迭代）。
2. 编程题：用 acados 实现倒立摆 NMPC，分别设 nlp_solver_type = 'SQP' 和 'SQP_RTI'，比较求解时间和闭环性能。
3. 分析题：RTI contractivity 在何种 \(\hat{x}_0\) 跳变幅度下失效？结合 §110 ISS 理论推导联合闭环界。

---

§4 CasADi：符号建模与自动微分 ⭐⭐

核心概念

CasADi（Andersson et al. 2019, Math. Prog. Comp. 11(1):1-36, DOI: 10.1007/s12532-018-0139-4）是符号-数值混合框架：C++ 核心 + Python/MATLAB 绑定。

四层架构：

1. 符号核心（SX/MX/DM）——构建计算图；
2. Function 对象——封装计算图为可调用函数；
3. 数值接口（nlpsol/qpsol/integrator）——连接求解器；
4. 第三方后端（IPOPT/qpOASES/OSQP/HPIPM/SNOPT）。

SX vs MX

维度	SX (Scalar Expression)	MX (Matrix Expression)
图粒度	每个非零是标量节点	矩阵操作为单节点
图规模	大（\(mn\) 级）	小
代码生成	小文件，适合嵌入式	大文件
适用	动力学表达式、小维度	长 horizon OCP 目标、含函数调用

经验规则：acados 模型用 SX（因为要代码生成到 C），CasADi Opti 用 MX 或 SX 皆可。

SX vs MX 的选择深入分析

这不是一个随意的选择——选错可能让代码生成文件从 100 KB 暴增到 10 MB，或者让构建时间从 1 秒增加到 30 秒。

场景	推荐	理由
小维度动力学（\(n \le 20\)）	SX	标量展开后代码紧凑，适合 codegen
大维度动力学（\(n > 30\)）	MX	SX 的计算图爆炸（\(n^2\) 个标量节点）
含矩阵运算（\(M^{-1}b\)）	MX	SX 把矩阵逆展开为 \(n^3\) 标量运算
含 CasADi 外部函数调用	MX	SX 不支持外部函数节点
含 if_else 条件分支	MX	SX 的 if_else 展开两个分支计算
acados 代码生成	SX	acados 的 C 模板要求 SX

常见错误：在 CasADi 的 SX 图中调用 numpy 函数（如 np.sin(x)）。这会把 CasADi 符号变量强制转换为数值，断裂 AD 链——得到的"导数"是零。正确做法：全用 casadi.sin、casadi.cos 等 CasADi 原生函数。

另一个常见错误：用 Python for 循环在 CasADi 图中做矩阵运算。CasADi 的 SX 会为每次循环创建独立的标量节点——100 次循环 = 100 个标量乘法节点，而不是一次矩阵乘法。正确做法：用 casadi.mtimes（矩阵乘法）、casadi.vertcat（垂直拼接）等矩阵操作。

本质洞察：CasADi 的 SX/MX 选择本质上是"标量 AD 图 vs 矩阵 AD 图"的选择。标量图给出最小的 C 代码（每个运算展开为基本操作），矩阵图给出最紧凑的图结构（每个运算保持矩阵形式）。对小系统（\(n < 20\)），标量展开的代码量可接受且运行最快；对大系统，矩阵形式必须保留以防代码爆炸。

Opti stack 示例

from casadi import *
import numpy as np

# 系统参数
nx, nu, N, T = 4, 2, 40, 2.0
dt = T / N

# 动力学: 自行车模型
def f_bicycle(x, u):
    # x = [px, py, theta, v], u = [a, delta]
    L = 2.5  # 轴距
    return vertcat(
        x[3] * cos(x[2]),          # dx/dt = v*cos(theta)
        x[3] * sin(x[2]),          # dy/dt = v*sin(theta)
        x[3] / L * tan(u[1]),      # dtheta/dt = v/L*tan(delta)
        u[0]                        # dv/dt = a
    )

# 创建 Opti 实例
opti = Opti()
X = opti.variable(nx, N+1)  # 状态轨迹
U = opti.variable(nu, N)    # 控制序列
p0 = opti.parameter(nx)     # 初始状态（参数化）

# 动力学约束 (RK4 积分)
for k in range(N):
    k1 = f_bicycle(X[:, k], U[:, k])
    k2 = f_bicycle(X[:, k] + dt/2 * k1, U[:, k])
    k3 = f_bicycle(X[:, k] + dt/2 * k2, U[:, k])
    k4 = f_bicycle(X[:, k] + dt * k3, U[:, k])
    x_next = X[:, k] + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
    opti.subject_to(X[:, k+1] == x_next)

# 约束
opti.subject_to(X[:, 0] == p0)                    # 初始约束
opti.subject_to(opti.bounded(-1, U[0, :], 1))     # 加速度限制
opti.subject_to(opti.bounded(-0.5, U[1, :], 0.5)) # 转向角限制
opti.subject_to(opti.bounded(-3, X[1, :], 3))     # 横向位置限制

# 代价
Q = diag([1, 10, 1, 1])  # 状态权重
R = diag([0.1, 0.01])    # 控制权重
cost = 0
for k in range(N):
    cost += X[:, k].T @ Q @ X[:, k] + U[:, k].T @ R @ U[:, k]
cost += X[:, N].T @ (10 * Q) @ X[:, N]  # 终端代价
opti.minimize(cost)

# 求解器设置
opti.solver('ipopt', {}, {'max_iter': 200, 'tol': 1e-8, 'print_level': 0})

# 使用
opti.set_value(p0, [0, 1, 0.1, 5])  # 设置初始状态
sol = opti.solve()
u_opt = sol.value(U[:, 0])  # 取第一步控制

代码生成

# 冻结 Opti 为 Function（参数 → 第一步控制）
F = opti.to_function('mpc_step', [p0], [U[:, 0]])

# 生成 C 代码
F.generate('mpc_step.c', {'with_header': True, 'with_mem': True})
# 编译: gcc -O3 -fPIC -shared mpc_step.c -o mpc_step.so -lm

⚠️ 常见陷阱

#	陷阱	后果	对策
1	MX 下 codegen 比 SX 大 10-50 倍	嵌入式空间不足	小维度 OCP 用 SX
2	在 CasADi 图中调用 numpy 函数	AD 链断裂	全用 casadi.sin/cos/if_else
3	Opti 不能直接 codegen IPOPT 循环	只能导出 callbacks	用 sqpmethod + qrqp 后端

---

§5 acados：模块化嵌入式最优控制框架 ⭐⭐

四层架构

acados（Verschueren et al. 2022, Math. Prog. Comp. 14(1):147-183, DOI: 10.1007/s12532-021-00208-8）定位**实时心脏**：

前端 (Python / MATLAB / Simulink) — 用 CasADi 写模型
  ↓ Tera 模板渲染 → 专属 C 工程 + Makefile + 共享库
C API (acados_c) — ocp_nlp / ocp_qp 接口
  ↓
Core (C): SQP / SQP_RTI / DDP, 积分器, condensing
  ↓
QP: HPIPM (默认) / qpOASES / OSQP / DAQP
  ↓
线性代数: BLASFEO (panel-major, 架构定制内核)

BLASFEO Target 重要性

BLASFEO 微架构 target 包括：X64_INTEL_HASWELL、X64_INTEL_SKYLAKE_X、ARMV8A_APPLE_M1、ARMV8A_ARM_CORTEX_A76/A57/A53。

Target 不匹配将损失 60-70% 性能——这不是小问题！在 Jetson Orin 上选错 target 可能让 5 ms 的求解时间变成 15 ms，直接超时。

Python API 完整示例

from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
import casadi as ca
import numpy as np

# === 模型定义 ===
nx, nu = 13, 4  # 四旋翼: [pos(3), vel(3), quat(4), omega(3)], [thrust(4)]
x = ca.SX.sym('x', nx)
u = ca.SX.sym('u', nu)
xdot = ca.SX.sym('xdot', nx)

# 动力学 f(x, u) — 四旋翼简化
def quad_dynamics(x, u):
    # ... (省略具体动力学)
    return f_expr  # nx × 1 CasADi SX

model = AcadosModel()
model.x, model.u, model.xdot = x, u, xdot
model.f_expl_expr = quad_dynamics(x, u)
model.f_impl_expr = xdot - quad_dynamics(x, u)
model.name = 'quadrotor'

# === OCP 设置 ===
ocp = AcadosOcp()
ocp.model = model
ocp.dims.N = 40
ocp.solver_options.tf = 2.0

# 代价 (NONLINEAR_LS 形式)
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.model.cost_y_expr = ca.vertcat(x, u)  # 残差向量
ocp.cost.W = np.diag([*[1]*nx, *[0.01]*nu])  # 权重矩阵
ocp.cost.yref = np.zeros(nx + nu)  # 参考

# 约束
ocp.constraints.lbu = np.array([0, 0, 0, 0])      # 推力下界
ocp.constraints.ubu = np.array([10, 10, 10, 10])   # 推力上界
ocp.constraints.idxbu = np.arange(nu)
ocp.constraints.x0 = np.zeros(nx)  # 初始状态

# === 求解器选项 ===
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.qp_solver_cond_N = 5       # partial condensing 段数
ocp.solver_options.integrator_type = 'ERK'     # 显式 RK
ocp.solver_options.sim_method_num_stages = 4   # RK4
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.regularize_method = 'PROJECT'
ocp.solver_options.nlp_solver_max_iter = 1     # RTI = 1 次迭代
ocp.solver_options.time_limit = 0.005          # 硬 5ms 超时

# === 创建求解器（生成 C 代码）===
solver = AcadosOcpSolver(ocp, json_file='quad_ocp.json')

# === 在线 RTI 循环 ===
def mpc_step(x_meas):
    # Preparation phase
    solver.options_set('rti_phase', 1)
    solver.solve()

    # 注入新测量
    solver.set(0, 'lbx', x_meas)
    solver.set(0, 'ubx', x_meas)

    # Feedback phase
    solver.options_set('rti_phase', 2)
    status = solver.solve()

    # 获取结果
    u0 = solver.get(0, 'u')

    # 诊断
    t_tot = solver.get_stats('time_tot')
    t_qp = solver.get_stats('time_qp')
    sqp_iter = solver.get_stats('sqp_iter')

    return u0, status, t_tot

积分器选择

类型	方法	适用
ERK	RK1/Heun/RK4	非刚性，快速
IRK	Gauss-Legendre / Radau IIA	刚性，DAE
GNSF-IRK	自动检测线性子系统	提速 2-5 倍
DISCRETE	用户自定义离散模型	已有离散动力学

WCET 保证

1. nlp_solver_max_iter — SQP 硬上限（RTI = 1）；
2. time_limit — 内部定时器，超时返回 ACADOS_MAXTIME（status = 7）；
3. qp_solver_iter_max — HPIPM 内点迭代上限。

练习

1. 编程题：用 acados 实现倒立摆 NMPC，比较 ERK vs IRK 积分器的求解时间。
2. 调优题：修改 qp_solver_cond_N，画出 QP 求解时间曲线，找最优点。
3. 部署题：生成 C 代码后，用 gcc -O3 编译，测量纯 C 调用的延迟。

---

§6 IPOPT 与 Primal-Dual 内点法 ⭐⭐⭐

算法核心

IPOPT（Wächter-Biegler 2006, Math. Prog. 106(1):25-57, DOI: 10.1007/s10107-004-0559-y）对

\[ \min f(x), \quad \text{s.t.} \quad c(x) = 0,\ x_L \le x \le x_U \]

引入 log-barrier：

\[ \varphi_\mu(x) := f(x) - \mu \sum_i \log(x^{(i)} - x_L^{(i)}) - \mu \sum_i \log(x_U^{(i)} - x^{(i)}), \quad \mu \downarrow 0. \]

Inertia Correction

Newton 系统是**对称不定**的：

\[ \begin{bmatrix} W_k + \Sigma_k + \delta_w I & A_k \\ A_k^\top & -\delta_c I \end{bmatrix} \begin{bmatrix} d_k^x \\ d_k^\lambda \end{bmatrix} = -\begin{bmatrix} \text{grad} \\ c(x_k) \end{bmatrix}. \]

下降方向要求 inertia = \((n, m, 0)\)（\(n\) 正、\(m\) 负、\(0\) 零特征值）。IPOPT 迭代调 \(\delta_w, \delta_c\) 直到 inertia 正确。

IPOPT 的 Filter Line-Search

IPOPT 不使用 Armijo line-search（DDP/SQP 的标准选择），而是使用 filter 方法——同时监控目标值 \(f(x)\) 和约束违反量 \(\theta(x) = \|c(x)\|_1\)。

Filter 的核心思想：一个试探点被接受当且仅当它在 \((f, \theta)\) 平面上**不被 filter 中的任何点支配**——即至少在一个维度上严格改进。这避免了 merit function 中权重参数 \(\nu\) 的调参问题（FDDP 的 \(\ell_1\) merit 需要选 \(\nu\)，而 filter 不需要）。

Filter 的两个接受条件（Wachter-Biegler 2006 Algorithm A-5.4）：

1. h-type step（约束改进步）：\(\theta(x^k + \alpha d) \le (1-\gamma_\theta)\theta(x^k)\)
2. f-type step（目标改进步）：\(f(x^k + \alpha d) \le f(x^k) - \gamma_f\theta(x^k)\) 且 Armijo 条件

两种步类型交替出现——先改善可行性，再改善最优性。

IPOPT 与 SQP 的深层对比

维度	SQP (acados)	IPOPT
Warm-start	极佳（shift 即可）	差（barrier \(\mu\) 需重置）
MPC 采样	RTI \(\mu\)s-ms	ms-s
Cold-start	弱（需 MPPI warm-start）	强（全局收敛性好）
嵌入式	静态 C（无动态分配）	依赖 HSL/MUMPS（动态分配）
Hessian 选择	GN 自动 PSD	Exact / L-BFGS / GN
约束处理	显式 KKT（稀疏）	log-barrier + filter
典型用途	在线 NMPC	离线轨迹优化
外部依赖	HPIPM + BLASFEO	HSL 线性求解器（MA27/MA57/MA97）

IPOPT 的 warm-start 为什么差？

IPM 的 barrier 参数 \(\mu\) 从大值（\(\mu_0 \approx 0.1\)）递减到小值（\(\mu \to 10^{-10}\)）。每次 MPC 周期开始时，如果直接用上一步的 \(\mu_\text{final} \approx 10^{-10}\)，barrier Hessian 的条件数极大（\(\sim 1/\mu\)），Newton 步方向不可靠。必须把 \(\mu\) 重置为较大值再递减——这浪费了 5-10 次 IPM 迭代。

相比之下，SQP 的 warm-start 只需移位变量——不涉及任何内部参数的重置。这是 SQP 在 MPC 中优于 IPM 的根本原因。

**但 IPOPT 在离线中不可替代**的三个理由：

1. 全局收敛保证：filter + inertia correction + restoration 使 IPOPT 在极差初始点也能收敛——对 cold-start 轨迹优化至关重要。
2. 稀疏线性代数：HSL 的 MA57 是工业级稀疏对称不定求解器，对大规模 NLP（变量 > 10000）远优于 dense 方法。
3. 通用约束：IPOPT 原生支持非线性等式/不等式约束，无需 AL/penalty 转化——对 contact-implicit、MPCC 等特殊约束更方便。

经验规则：快采样 + LS cost → acados RTI；大 \(N\) + 复杂约束 + cold-start → IPOPT via CasADi。

IPOPT 调参经验

参数	默认值	MPC 推荐	离线推荐
max_iter	3000	50	3000
tol	1e-8	1e-4	1e-8
acceptable_tol	1e-6	1e-3	1e-6
mu_strategy	adaptive	adaptive	adaptive
linear_solver	mumps	mumps	ma57（需 HSL）
print_level	5	0	3
warm_start_init_point	no	yes	no

最常见的 IPOPT 错误

错误码	含义	对策
Solve_Succeeded	正常	--
Maximum_Iterations_Exceeded	迭代超限	增大 max_iter 或放松 tol
Restoration_Failed	无法恢复可行性	检查初始猜测、约束一致性
Invalid_Number_Detected	NaN/Inf	检查动力学求值、边界值
Search_Direction_Becomes_Too_Small	步长过小	检查 Hessian 条件数、正则化

---

§7 代码生成与嵌入式部署 ⭐⭐

端到端流程

[1] Python 建模: CasADi SX → AcadosOcp
    ↓
[2] AcadosOcpSolver(ocp) → c_generated_code/
    ↓
[3] 本机回归测试: status, sqp_iter, time_tot
    ↓
[4] 交叉编译 (x86 → aarch64):
    BLASFEO -DTARGET=ARMV8A_ARM_CORTEX_A76
    ↓
[5] 打包: libacados.so + libhpipm.so + libblasfeo.so
    ↓
[6] Jetson / STM32MP1 运行
    ROS2 节点: prepare() → wait(imu) → feedback() → publish(u0)

交叉编译关键命令

cmake .. \
  -DCMAKE_TOOLCHAIN_FILE=../cmake/toolchain-aarch64-linux-gnu.cmake \
  -DTARGET=ARMV8A_ARM_CORTEX_A76 \
  -DLA=HIGH_PERFORMANCE -DMF=PANELMAJ \
  -DBLASFEO_CROSSCOMPILING=ON -DBLAS_API=ON

关键陷阱：不设 BLASFEO_CROSSCOMPILING=ON 会在 x86 上做 ISA 探测并注入 -mavx2，导致 aarch64 编译失败。

WCET 工程手段：从"平均快"到"保证快"

实时控制系统关心的不是平均延迟而是**最坏情况执行时间 (WCET)**。以下是让 MPC 从"仿真里很快"变成"实机上总是及时"的完整工程手段。

层次 1：算法层

手段	原理	效果
nlp_solver_max_iter = 1	RTI 单次迭代	消除迭代数不确定性
qp_solver_iter_max = 30	HPIPM IPM 上限	防止 ill-conditioned QP 无限迭代
time_limit = 5 ms	内部定时器	超时返回次优解（status=7）
GN Hessian	避免 exact Newton	\(Q_{uu}\) 自动 PSD，无正则化分支

层次 2：系统层

手段	命令	原理
CPU 频率锁定	cpufreq-set -g performance	防止动态降频导致延迟突增
CPU 隔离	isolcpus=2,3 in GRUB	MPC 线程独占核心
实时调度	chrt -f 99 ./mpc_node	SCHED_FIFO 最高优先级
中断隔离	irqaffinity	硬件中断不打扰 MPC 核
内存锁定	mlockall(MCL_CURRENT|MCL_FUTURE)	防止 page fault 导致 ms 级延迟

层次 3：数值层

• BLASFEO panel-major + 汇编 microkernel：矩阵以 SIMD 宽度的面板存储，消除 packing 开销。每次 GEMM 调用耗时方差比 OpenBLAS 小 5-10 倍——对 WCET 保证至关重要。
• 静态内存分配：acados 生成的 C 代码不调用 malloc——所有内存在初始化时一次分配。这消除了 glibc 堆分配器的不确定性延迟（\(\mu\)s-ms 级）。
• 避免分支预测失败：GN Hessian 使 backward pass 中不存在"正则化→重试"分支，减少 pipeline flush。

层次 4：验证层

部署前必须做 延迟剖析：

# 测量 10000 次 MPC 求解的延迟分布
for i in $(seq 10000); do
    echo $(./mpc_benchmark)
done | sort -n > latency.txt

# 统计
mean=$(awk '{s+=$1}END{print s/NR}' latency.txt)
p99=$(awk 'NR==int(0.99*10000)' latency.txt)
max=$(tail -1 latency.txt)
echo "mean=$mean  p99=$p99  max=$max"

安全标准：p99 延迟 < 采样周期的 80%。如果 p99 = 4.5 ms 且采样周期 = 5 ms，剩余 0.5 ms 用于通信和执行器命令——刚好安全。如果 p99 = 4.9 ms，太危险。

Safety fallback 策略

当 MPC 超时（status = ACADOS_MAXTIME）时：

1. 保持上一步控制：\(u_k = u_{k-1}\)（零阶保持）。安全但非最优。
2. 用 \(K_0\) 反馈：\(u_k = u_{k-1}^\star + K_0(x_k - x_{k-1}^\star)\)。利用上一步的 Riccati 反馈增益。
3. 切换 PD 阻抗：放弃 MPC 优化，用预设的 PD 控制器稳定系统。最保守但最安全。

推荐策略：策略 2 作为默认 fallback（延迟 < 0.1 ms），策略 3 作为连续 3 次超时后的紧急模式。

ROS2 MPC 节点的完整架构

┌─────────────────────────────────────────────────────┐
│  ROS2 MPC Node (C++, rt-thread)                      │
│                                                       │
│  [Subscriber: /state_estimate]                        │
│       ↓ x̂ (每 1ms)                                    │
│  [State Buffer] ← 最新状态                             │
│       ↓                                               │
│  [Preparation Thread]                                 │
│    shift warm-start → 导数计算 → Riccati 因子化        │
│       ↓ 完成信号                                       │
│  [Feedback Thread] (最高优先级)                        │
│    注入 x̂ → 解 QP → u₀ 输出                           │
│       ↓ u₀ (每 5ms)                                   │
│  [Publisher: /joint_commands]                          │
│       ↓                                               │
│  [Watchdog] — 超时检测 → fallback                      │
└─────────────────────────────────────────────────────┘

关键设计选择：

1. 双线程分离：preparation 在普通线程，feedback 在实时线程（SCHED_FIFO）。两者通过无锁缓冲区通信。
2. 状态缓冲区：保存最近 10 个状态估计，feedback 时取最新——不等待同步。
3. Watchdog：如果 feedback 超过 \(T_\text{budget}\) 未完成，自动切 fallback。

---

§8 Direct Collocation 细节 ⭐⭐⭐

Hermite-Simpson 的完整推导

HS 方法是直接配点法中最常用的——4 阶精度、实现简单、Drake 的默认选择。

推导思路：在 \([t_k, t_{k+1}]\) 上用三次 Hermite 多项式插值状态：

\[ p(t) = a_0 + a_1(t-t_k) + a_2(t-t_k)^2 + a_3(t-t_k)^3 \]

由四个条件确定 \(a_0, a_1, a_2, a_3\)： 1. \(p(t_k) = x_k\) 2. \(p(t_{k+1}) = x_{k+1}\) 3. \(\dot p(t_k) = f(x_k, u_k)\) 4. \(\dot p(t_{k+1}) = f(x_{k+1}, u_{k+1})\)

中点值（从 Hermite 插值公式）：

\[ p(t_{k+1/2}) = \frac{1}{2}(x_k + x_{k+1}) + \frac{h}{8}(f_k - f_{k+1}) \]

Simpson 积分（\(\dot x = f\) 从 \(t_k\) 到 \(t_{k+1}\) 积分）：

\[ x_{k+1} - x_k = \int_{t_k}^{t_{k+1}} f\,dt \approx \frac{h}{6}(f_k + 4f_{k+1/2} + f_{k+1}) \]

其中 \(f_{k+1/2} = f(x_{k+1/2}, u_{k+1/2})\)。

分离型 HS（\(x_{k+1/2}\) 和 \(u_{k+1/2}\) 也作为 NLP 决策变量）：

defect 约束 1（中点一致性）：

\[ x_{k+1/2} = \frac{1}{2}(x_k + x_{k+1}) + \frac{h}{8}(f_k - f_{k+1}) \]

defect 约束 2（Simpson 积分）：

\[ x_{k+1} - x_k = \frac{h}{6}(f_k + 4f_{k+1/2} + f_{k+1}) \]

NLP 变量：\(z = (x_0, u_0, x_{1/2}, u_{1/2}, x_1, u_1, \ldots)\)，维度 \((2N+1)n_x + 2N n_u\)。

最常见错误：把 \(+\frac{h}{8}(f_k - f_{k+1})\) 写成 \(+\frac{h}{8}(f_{k+1} - f_k)\)（符号反了）——NLP 仍可求解（因为约束仍满足一致性），但阶数悄悄降到 2 阶，mesh 加细误差变为线性下降而非 \(O(h^4)\)。这是一个极隐蔽的 bug。

检验方法：对已知解析解的 ODE（如 \(\dot x = x\)），用不同步长 \(h\) 计算全局误差 \(e(h) = \|x_N - x(T)\|\)。如果 \(e(h) / h^4 \to C\)（常数），则 4 阶正确。如果 \(e(h) / h^2 \to C\)，说明符号错了。

Radau IIA：刚性系统的最佳选择

\(s\) 级 Radau IIA 取 \([0,1]\) 上含右端点 \(\tau_s = 1\) 的配点（stiffly accurate）。阶数 \(2s-1\)，A-稳定 + L-稳定。

为什么需要 L-稳定？

考虑刚性系统 \(\dot x = -1000 x\)（时间常数 \(\tau = 1\) ms）。如果 \(h = 10\) ms（\(h/\tau = 10\)），标准 RK4 的稳定函数 \(|R(-1000h)| = |R(-10000)| > 1\)——发散！L-稳定方法保证 \(|R(-\infty)| = 0\)——无论 \(h/\tau\) 多大，刚性模式都被瞬间衰减。

定理 8.1（Radau IIA L-稳定）：稳定函数 \(R(z)\) 是 Padé \((s-1, s)\) 近似，\(|R(\infty)| = 0\)。对刚性模式 \(\dot{x} = \lambda x\)（\(\text{Re}\lambda \ll 0\)），一步把误差模式 \(\to 0\)。

2 级 Radau IIA（3 阶）的 Butcher tableau：

\[ \begin{array}{c|cc} 1/3 & 5/12 & -1/12 \\ 1 & 3/4 & 1/4 \\ \hline & 3/4 & 1/4 \end{array} \]

隐式方程：\(X_1 = x_k + h(\frac{5}{12}f(X_1) - \frac{1}{12}f(X_2))\)，\(X_2 = x_k + h(\frac{3}{4}f(X_1) + \frac{1}{4}f(X_2))\)。

关键：Radau IIA 的 stage 变量 \(X_{k,i}\) 必须全作 NLP 决策变量——如果用显式 RK "近似"实现（如 Newton 迭代求解隐式方程），完全失去 L-稳定性。正确做法是把 \(X_{k,1}, X_{k,2}\) 直接作为 NLP 变量，隐式方程作为等式约束交给 NLP solver。

在 acados 中使用 Radau IIA：

ocp.solver_options.integrator_type = 'IRK'
ocp.solver_options.sim_method_num_stages = 2    # 2 级 Radau IIA
ocp.solver_options.collocation_type = 'GAUSS_RADAU_IIA'

acados 内部自动处理隐式方程——用户只需指定级数。

配点法精度对比

方法	类型	阶数	稳定性	每段 NLP 变量	适用
Euler	单步	1	有条件	0	不推荐
RK4	多步	4	有条件	0（不作 NLP 变量）	快速原型
Hermite-Simpson	配点	4	有条件	\(n_x + n_u\)	非刚性默认
2-stage Radau IIA	配点	3	L-稳定	\(2n_x\)	弱刚性
3-stage Radau IIA	配点	5	L-稳定	\(3n_x\)	强刚性
LGR 伪谱	全局	\(2N-1\)	有条件	\(Nn_x\)	离线高精度

---

选学部分

§A 伪谱法：光滑问题的终极精度 ⭐⭐⭐⭐

核心思想：用高次多项式全局逼近状态轨迹，在配点（LGR/LGL/CGL 节点）上强制动力学一致性。

三种配点方案：

配点	全称	端点包含	阶数	适用
LGR	Legendre-Gauss-Radau	只含右端	\(2N-2\)	最常用
LGL	Legendre-Gauss-Lobatto	含两端	\(2N-2\)	教学
CGL	Chebyshev-Gauss-Lobatto	含两端	\(N\)	非多项式解

谱收敛：当真解解析时，LGR 伪谱解的误差以**指数速率**衰减：

\[ \|\hat x - x^\star\|_\infty \le C\sigma^{-N} \]

其中 \(\sigma > 1\) 取决于真解的解析延拓半径。对比有限差分的 \(O(h^p)\)（多项式衰减），谱收敛快得多。

Ross-Fahroo Costate Mapping Theorem：NLP 的 KKT 乘子 \(\lambda_k\) 可以通过权重矩阵 \(W\) 映射到连续时间协态 \(p(t)\)：\(p(\tau_k) = \lambda_k / w_k\)。这意味着伪谱法同时给出最优控制和协态——对间接法验证极有用。

代表实现：GPOPS-II（MATLAB，商业），pycollo（Python，开源），JuMP + BVProblem（Julia）。

为什么伪谱法不用于实时 MPC？ 因为 NLP 变量数 \(Nn_x\) 中的 \(N\) 虽然小（谱收敛只需 \(N \sim 20\)），但 KKT 矩阵不再是 block-tridiagonal（微分矩阵 \(D\) 是稠密的）——无法用 Riccati 递推，必须用通用稀疏 LU，复杂度 \(O(N^3 n_x^3)\) 而非 \(O(Nn_x^3)\)。这使得伪谱法比多射击慢一个数量级。

适用场景：离线轨迹优化（航天器再入、卫星轨道转移），需要极高精度（\(10^{-10}\)）且计算时间不限。

跨领域类比：伪谱法在控制中的角色，类似于谱方法（Fourier/Chebyshev 展开）在 PDE 数值求解中的角色——用全局基函数逼近，光滑解下精度远超有限差分/有限元，但对非光滑解（如激波/接触切换）效果差且计算成本高。在 MPC 中，多射击 + RK4 的"有限差分风格"更适合实时；伪谱法的"谱方法风格"更适合精度敏感的离线问题。

§B Partial Condensing 调优 ⭐⭐⭐

Partial condensing 是 condensing（\(N_2 = 1\)）和 sparse（\(N_2 = N\)）的连续插值——通过选择 \(N_2\)（粗粒度节点数），在 dense QP 的易用性和 sparse Riccati 的高效性之间取最优平衡。

\(N_2\) 的选择原则

Axehill 2015 的理论公式 \(N_2^\star \approx \lceil Nn_u / (n_x + n_u) \rceil\) 仅作初始估计。实际最优 \(N_2\) 高度依赖硬件——不同 CPU 的 cache 大小、SIMD 宽度、内存带宽对"临界矩阵尺寸"有不同影响。

实操方法：网格搜索 \(N_2 \in \{1, 3, 5, 8, 10, N\}\)，测量 time_qp。

典型搜索结果（四旋翼 \(n_x = 13, n_u = 4, N = 40\)，i7-12700H）：

\(N_2\)	dense QP 维度	time_qp	备注
1	\(160\)	3.2 ms	全 condensing，QP 太大
3	\(13 + 53 = 66\)	1.1 ms	中等
5	\(13 + 32 = 45\)	0.8 ms	最优
8	\(13 + 20 = 33\)	0.9 ms	接近最优
10	\(13 + 16 = 29\)	1.0 ms	Riccati 开销增加
40	\(13 + 4 = 17\)	1.3 ms	全 sparse

\(N_2 = 5\) 最优的原因：此时每段的 "condensed control" 维度为 \(10 \times 4 / 5 = 8\)，加上状态维度 13，总 Riccati 块大小为 \(21 \times 21\)——恰好适合 L1 cache（\(21^2 \times 8 = 3.5\) KB < 64 KB L1）。\(N_2\) 太小则 dense QP 太大放不进 L2；\(N_2\) 太大则 Riccati 步数多、每步块太小无法利用 SIMD。

ARM 平台的差异：Jetson Orin 的 L1 cache 为 32 KB/核，最优 \(N_2\) 可能更大（\(N_2 = 8\)-\(10\)）。必须在目标平台上重新搜索。

工程建议：在 acados_ocp_solver_create 之后、部署之前，跑一次 \(N_2\) 网格搜索脚本。这 5 分钟的工作可能带来 30-50% 的 QP 求解时间改善。

§C 混合整数 MPC ⭐⭐⭐⭐

问题：当 MPC 需要做离散决策（如"走左边还是右边绕障碍物"、"选哪只脚先迈"）时，优化问题包含整数变量——变成混合整数 QP (MIQP) 或混合整数 NLP (MINLP)。

Big-M 建模：把"if-then"逻辑转化为线性约束 + 二值变量：

\[ x \le b + M(1-\delta), \quad \delta \in \{0, 1\} \]

当 \(\delta = 1\) 时 \(x \le b\)（约束激活）；当 \(\delta = 0\) 时 \(x \le b + M\)（约束松弛，\(M\) 足够大）。

Marcucci-Tedrake 2019 的 perspective 松弛：对 Big-M 松弛给出比标准 LP 松弛更紧的凸近似——把二值约束的凸包用 perspective function 精确描述。

工程做法：MI-MPC 在离线规划（如接触序列选择、路径拓扑选择）中使用 Gurobi/CPLEX 求解，然后用结果 warm-start 在线凸 MPC。在线不做整数优化——计算量不可预测，不适合实时。

CoCo 方法（Cauligi et al. 2022）：用 NN 预测最优整数策略 → 固定整数 → 凸 QP 在线求解。比直接 Gurobi 快 10-100 倍。

§D Explicit MPC：把优化变成查表 ⭐⭐⭐

Bemporad, Morari, Dua, Pistikopoulos (2002), Automatica 38(1):3-20。

核心思想：线性 MPC 的最优解 \(u_0^\star(x_0)\) 是初始状态 \(x_0\) 的**分段仿射（PWA）函数**——状态空间被划分为若干多面体区域 \(\mathcal{R}_i\)，每个区域内 \(u_0^\star(x) = K_i x + k_i\)（仿射控制律）。

为什么是 PWA？ 考虑线性 MPC 的 QP：\(\min \frac{1}{2}z^\top H z + (Fx_0 + f)^\top z\) s.t. \(Gz \le w + Ex_0\)。KKT 条件给出最优解 \(z^\star = -H^{-1}(Fx_0 + f + G_\mathcal{A}^\top\lambda_\mathcal{A})\)，其中活动集 \(\mathcal{A}\) 决定了 \(\lambda_\mathcal{A}\) 的表达式。每个不同的活动集组合对应一个多面体区域——在该区域内 \(z^\star\) 是 \(x_0\) 的仿射函数。

离线计算（multi-parametric QP, mpQP）：

1. 枚举所有可能的活动集组合 \(\mathcal{A}_1, \mathcal{A}_2, \ldots, \mathcal{A}_R\)
2. 对每个 \(\mathcal{A}_i\)，解 KKT 系统得到 \(u_0^\star = K_i x_0 + k_i\)
3. 计算有效区域 \(\mathcal{R}_i = \{x_0 : \mathcal{A}_i \text{ is optimal active set}\}\)

在线查表：给定新的 \(x_0\)，找到包含它的区域 \(\mathcal{R}_i\)，读出 \(K_i, k_i\)，计算 \(u_0 = K_i x_0 + k_i\)。查表复杂度 \(O(\log R)\)（二叉搜索树）。

区域数量增长：\(R\) 在最坏情况下随 \(N, n_x\) 指数增长。实用边界约 \(n_x \le 5, N \le 10\)——超过此范围区域数可能 > 10000，存储和查表开销过大。

适用场景：嵌入式系统（无浮点运算单元、无 QP solver、严格 WCET 要求），如 STM32 上的电机控制、FPGA 上的电力电子控制。

MPT3 实现：

sys = LTISystem('A', A, 'B', B);
sys.x.min = [-5; -5]; sys.x.max = [5; 5];
sys.u.min = -1; sys.u.max = 1;
mpc = MPCController(sys, N);
explicit_mpc = mpc.toExplicit();
explicit_mpc.partition.plot()  % 可视化区域划分
u = explicit_mpc.evaluate(x0);  % 在线查表

§E GPU/MPPI：采样规划的并行革命 ⭐⭐⭐

MPPI 算法详解

Model Predictive Path Integral（Williams et al. ICRA 2016, arXiv:1509.01149）是一种基于采样的 MPC 方法——不需要梯度，完全靠并行 rollout 和加权平均。

算法流程：

1. 采样：从当前控制序列 \(\mathbf{u}^k\) 出发，采样 \(K\) 条扰动轨迹 \(\delta\mathbf{u}^i \sim \mathcal{N}(0, \Sigma)\)，\(i = 1, \ldots, K\)
2. Rollout：对每条扰动控制 \(\mathbf{u}^k + \delta\mathbf{u}^i\) 做前向仿真，得到轨迹 \(\mathbf{x}^i\)
3. 代价评估：\(S^i = \sum_{t=0}^{N-1} \ell(x_t^i, u_t^i) + \ell_f(x_N^i)\)
4. 加权更新：

\[ \mathbf{u}^{k+1} = \mathbf{u}^k + \frac{\sum_{i=1}^K w^i \delta\mathbf{u}^i}{\sum_{i=1}^K w^i}, \qquad w^i = \exp\left(-\frac{1}{\lambda}(S^i - S_{\min})\right) \]

其中 \(\lambda > 0\) 是温度参数（\(\lambda\) 小→集中于最优采样，\(\lambda\) 大→均匀采样）。

MPPI 的数学基础

MPPI 来自 path integral control theory（Kappen 2005），其中最优控制律由信息论的 free energy 给出。当代价可分解为控制代价 \(\frac{1}{2}u^\top R u\) 加状态代价 \(q(x)\)，且动力学含加性高斯噪声时，最优反馈律的分布有闭式：

\[ \pi^\star(u|x) \propto \exp\left(-\frac{1}{\lambda}J(x, u)\right) \]

MPPI 用 Monte Carlo 近似这个分布——采样即积分。

GPU 实现的性能数据

平台	采样数 \(K\)	rollout 时间	适用
CPU (i7, 1 core)	100	50 ms	教学
CPU (i7, 8 core)	500	30 ms	原型
GPU (RTX 3090)	10000	2 ms	实时
GPU (Jetson Orin)	4096	5 ms	嵌入式

**MJPC（MuJoCo Predictive Control）**在 GPU 上用 predictive sampling（MPPI 变体）做 200 Hz 全身 MPC——每周期 10000 条 rollout，用 MuJoCo 的 SIMD 仿真加速。

iLQR vs MPPI：系统化对比

维度	iLQR/DDP	MPPI
需要梯度	是（\(f_x, f_u\)）	否（黑箱仿真）
并行性	差（串行 Riccati）	极好（\(K\) 条 rollout 完全并行）
光滑问题收敛	超线性/二次	无保证（\(\sqrt{K}\) 采样误差）
非光滑/接触	差（\(f_{xx}\) 不连续）	好（不需要梯度）
GPU 利用率	低（矩阵操作小）	极高（embarrassingly parallel）
控制反馈	自然给出 \(K\) 矩阵	不给反馈增益
确定性	是	否（每次结果略不同）

工程建议：光滑凸问题用 iLQR（收敛快、确定性）；接触丰富/NN 动力学用 MPPI（鲁棒、不需梯度）；混合场景用 MPPI warm-start + iLQR 精炼。

§F 可微 MPC：从控制到学习的桥梁 ⭐⭐⭐⭐

核心思想：把 MPC 求解器视为可微函数层 \(\pi_\theta(x) = u_0^\star(x; \theta)\)，通过 KKT 不动点的隐式微分反向传播梯度 \(\partial u_0^\star / \partial\theta\)。

隐式微分 vs 展开（Unrolling）

方法	原理	优点	缺点
展开	把 \(K\) 次 iLQR 迭代展开为计算图	简单（autograd 直接用）	梯度爆炸/消失（\(K > 5\) 时）
隐式微分	在不动点处用 KKT 隐函数定理	梯度稳定、不依赖迭代数	需要 KKT 系统可逆（LICQ）

隐式微分的核心方程

设 KKT 条件为 \(F(z, \theta) = 0\)（\(z\) 是 primal-dual 变量，\(\theta\) 是参数）。在不动点 \(z^\star(\theta)\) 处：

\[ \frac{\partial z^\star}{\partial\theta} = -\left(\frac{\partial F}{\partial z}\right)^{-1}\frac{\partial F}{\partial\theta} \]

对 MPC 的 block-tridiagonal KKT，\(\partial F/\partial z\) 的逆可通过 Riccati 递推 \(O(N(n+m)^3)\) 求解——远优于通用 LU 的 \(O(N^3(n+m)^3)\)。

应用场景：

1. 学习代价参数：RL 的 reward shaping 变为"通过 MPC 反向传播，优化代价函数使闭环行为最优"
2. 逆强化学习：从专家演示恢复代价函数——\(\theta^\star = \arg\min \|u^\star_\text{demo} - u^\star_\text{MPC}(\theta)\|^2\)
3. Sim2real 微调：在仿真中训练 \(\theta\)，通过可微 MPC 自动调整动力学参数使仿真行为匹配实机

§G 学习型 warm-start ⭐⭐⭐

问题：MPC warm-start 通常用 shift 策略——但对全新任务（如从未执行过的跳跃高度），shift 无法提供好的初始猜测。

CoCo（Cauligi et al. 2022）：用 NN 预测混合整数 QP 的 logical strategy（哪些约束活动）→ 固定整数变量 → 快速求解剩余凸 QP。比直接 Gurobi 快 10-100 倍，成功率 >95%。

Memory of Motion（Mastalli 2020）：

1. 离线生成大量轨迹（不同目标位置、步态模式）
2. 训练 k-NN / GMM / 扩散模型，把任务描述映射到初始轨迹猜测
3. 用学习到的初始猜测 warm-start FDDP

效果：无 Memory 时 FDDP 需 50+ 迭代；有 Memory 时 3-5 迭代。这使得前空翻、侧翻等复杂运动也能在线规划。

2024-2025 年新趋势：扩散模型作为 warm-start 生成器——Diffusion Policy 可以生成多模态的初始轨迹猜测（而 k-NN 只能给单一最近邻），更好地覆盖多个局部最优。

§H AS-RTI：活动集感知的实时迭代 ⭐⭐⭐

Frey et al. (2024), arXiv:2403.07101。

标准 RTI 在约束活动集变化时（如步态切换、新障碍物出现），单次 SQP 迭代可能不足以正确识别新的活动集——导致约束违反或收敛变慢。

AS-RTI 的核心创新：在 preparation 阶段预测下一步可能的活动集变化，提前构建多个 QP 因子化（对应不同活动集假设）。Feedback 阶段根据新测量选择最匹配的因子化，一步得到正确的活动集。

性能数据：在四足机器人的障碍物出现场景中，AS-RTI 比标准 RTI 的约束违反量减少 10 倍，且求解时间增加 < 30%。

---

本章小结

主题	核心结论	关键工具	计算复杂度
转录 (§1)	多射击 = NMPC 默认选择	acados/CasADi	\(O(N(n+m))\) 变量
QP 结构 (§2)	Riccati 递推 \(O(N)\)	HPIPM/BLASFEO	\(O(N(n+m)^3)\)
RTI (§3)	单次迭代 + prep/fb 拆分	acados SQP_RTI	\(\Delta t_{\text{fb}} \ll \Delta t_{\text{prep}}\)
CasADi (§4)	符号建模 + AD	SX/MX/Opti	构建时间可忽略
acados (§5)	模板生成 → 嵌入式 C	Python API	ms 级在线求解
IPOPT (§6)	离线/cold-start 之王	filter line-search	\(O(k_{\text{IPM}} \cdot \text{sparse})\)
部署 (§7)	交叉编译 + WCET	BLASFEO target	p99 延迟保证
配点 (§8)	HS 4 阶 / Radau L-稳	Drake/acados IRK	刚性首选

---

累积项目：本章新增模块

项目：从零构建实时 MPC 控制器

• 前置已完成：DARE 终端设计（§110）
• 本章新增：
• CasADi 多射击 NLP 框架（自行车模型）
• acados SQP_RTI 控制器（四旋翼）
• Riccati 递推 QP 求解器（纯 Python 教学实现）
• 交叉编译脚本（x86 → Jetson Orin）
• ROS2 MPC 节点模板

---

延伸阅读

资源	难度	内容
Rawlings-Mayne-Diehl Ch.8 (Diehl 执笔)	⭐⭐	数值最优控制权威
Kelly 2017 SIAM Review	⭐⭐	配点法最佳教程
Frison PhD Thesis (DTU 2015)	⭐⭐⭐	BLASFEO/Riccati 完整推导
acados documentation (docs.acados.org)	⭐⭐	实操参考
Diehl Freiburg 讲义 (syscop.de)	⭐⭐⭐	理论+实现最系统
Betts Practical Methods 3e (SIAM 2020)	⭐⭐⭐	配点法/mesh refinement

---

🔧 故障排查手册

症状	可能原因	排查步骤	相关节
acados 返回 status=1 (NaN)	动力学发散或除零	1. 打印初始猜测 2. 检查 \(u\) 范围 3. 降低 \(dt\)	§1, §5
QP 求解时间 > budget	partial condensing \(N_2\) 不优	1. 扫描 cond_N 2. 检查 BLASFEO target	§2
RTI feedback > 1ms	未拆两阶段	1. 检查 rti_phase 设置 2. 分析 time_lin vs time_qp	§3
IPOPT restoration failed	单射击 + 不稳定系统	1. 切多射击 2. 提供更好初始猜测	§1, §6
交叉编译失败	BLASFEO target/toolchain 错	1. 设 CROSSCOMPILING=ON 2. 确认 target 架构	§7

---

跨章综合练习

综合题（需要 §110 + §1-§5 的知识）：

设计一个四旋翼 NMPC 控制器并验证稳定性：

1. 建模（§4）：用 CasADi SX 写四旋翼 13 维动力学；
2. 终端设计（§110 §3）：在悬停点线性化，解 DARE，设计 \((V_f, X_f)\)；
3. 转录（§1）：选择多射击 + RK4，\(N = 20\)，\(T = 1\) s；
4. acados 部署（§5）：配置 SQP_RTI + HPIPM，测量 time_tot；
5. 稳定性验证（§110 §2）：画 \(V_N^\ast(x_k)\) 曲线，验证单调下降；
6. 鲁棒性测试：加入风扰 \(\|w\| \le 0.5\) m/s\(^2\)，观察 ISS 行为。

---

机器人应用速查表

平台	模型 \((n_x/n_u/N)\)	求解方案	频率	来源
四旋翼	13/4/20-50	acados SQP_RTI + HPIPM	100 Hz	UZH RPG
四足凸 MPC	13/12/10	qpOASES dense QP	30 Hz	Di Carlo 2018
四足 whole-body	24-48/12-24/20	OCS2 SLQ + HPIPM	100-200 Hz	ANYmal
自动驾驶横向	4-6/2/30	OSQP / acados	30-100 Hz	Autoware
openpilot 纵向	3/1/33	acados + HPIPM	20 Hz	comma.ai
Crazyflie 27g	12/4/15	TinyMPC (ADMM)	500 Hz	CMU

---

从"稳"到"快"的三道桥

第一道桥：结构对齐。多射击的 block-tridiagonal KKT → Riccati 线性复杂度 → HPIPM panel-major + BLASFEO 微架构内核。没有这条桥，1 ms 内解 13 维四旋翼 NMPC 是幻觉。

第二道桥：算法截断。Full SQP 的多次 Newton 迭代 → RTI 单次 full-step + preparation/feedback 拆分 → GN Hessian 的 PSD + 零残差 q-二次。没有这道桥，机器人 NMPC 停在学术 benchmark 而非 Cheetah 的 3 m/s 奔跑。

第三道桥：数值与控制理论统一。RTI contractivity（定理 3.2）+ §110 ISS-Lyapunov 不等式 → 联合闭环 ISS。没有它，仿真里跑得再快也不敢装到 Jetson 上飞。

本质洞察：MPC 数值方法的历史等价于——把连续 OCP 压扁成 block-banded QP，再把 block-banded QP 压进 SIMD 寄存器。

---

附录 A：Condensing 推导的完整展开

从 block-tridiagonal 到 dense 的代数过程

考虑 LTV-MPC 的动力学 \(x_{k+1} = A_k x_k + B_k u_k + b_k\)。从 \(x_0\) 出发递推：

\[ x_1 = A_0 x_0 + B_0 u_0 + b_0 $$ $$ x_2 = A_1 x_1 + B_1 u_1 + b_1 = A_1 A_0 x_0 + A_1 B_0 u_0 + B_1 u_1 + A_1 b_0 + b_1 \]

一般地：

\[ x_k = \Phi_k x_0 + \sum_{j=0}^{k-1} \Gamma_{k,j} u_j + \hat{x}_k \]

其中： - \(\Phi_k = A_{k-1} A_{k-2} \cdots A_0\)（状态转移矩阵） - \(\Gamma_{k,j} = A_{k-1} \cdots A_{j+1} B_j\)（控制影响矩阵） - \(\hat{x}_k = \sum_{i=0}^{k-1} A_{k-1} \cdots A_{i+1} b_i\)（仿射偏移）

写成矩阵形式：

\[ \underbrace{\begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_N \end{bmatrix}}_{\mathbf{x}} = \underbrace{\begin{bmatrix} \Phi_1 \\ \Phi_2 \\ \vdots \\ \Phi_N \end{bmatrix}}_{\boldsymbol{\Phi}} x_0 + \underbrace{\begin{bmatrix} \Gamma_{1,0} & 0 & \cdots & 0 \\ \Gamma_{2,0} & \Gamma_{2,1} & \cdots & 0 \\ \vdots & & \ddots & \vdots \\ \Gamma_{N,0} & \Gamma_{N,1} & \cdots & \Gamma_{N,N-1} \end{bmatrix}}_{\boldsymbol{\Gamma}} \underbrace{\begin{bmatrix} u_0 \\ u_1 \\ \vdots \\ u_{N-1} \end{bmatrix}}_{\mathbf{u}} + \hat{\mathbf{x}}. \]

\(\boldsymbol{\Gamma}\) 是下三角块矩阵——这反映了因果性：\(x_k\) 只依赖 \(u_0, \dots, u_{k-1}\)。

代入二次代价 \(J = \frac{1}{2}\mathbf{x}^\top \mathbf{Q} \mathbf{x} + \frac{1}{2}\mathbf{u}^\top \mathbf{R} \mathbf{u} + \mathbf{x}^\top \mathbf{S} \mathbf{u}\)：

\[ J = \frac{1}{2}\mathbf{u}^\top \underbrace{(\boldsymbol{\Gamma}^\top \mathbf{Q} \boldsymbol{\Gamma} + \mathbf{R} + \boldsymbol{\Gamma}^\top \mathbf{S} + \mathbf{S}^\top \boldsymbol{\Gamma})}_{\tilde{H}} \mathbf{u} + \underbrace{(\boldsymbol{\Phi} x_0 + \hat{\mathbf{x}})^\top \mathbf{Q} \boldsymbol{\Gamma} + \cdots}_{\tilde{g}^\top} \mathbf{u} + \text{const}. \]

Condensing 的 Flop 计数

构造 \(\boldsymbol{\Gamma}\) 矩阵需要 \(N(N-1)/2\) 次 \(n_x \times n_x\) 矩阵乘法和 \(N(N-1)/2\) 次 \(n_x \times n_u\) 矩阵乘法。

构造 \(\tilde{H} = \boldsymbol{\Gamma}^\top \mathbf{Q} \boldsymbol{\Gamma} + \mathbf{R}\)： - \(\boldsymbol{\Gamma}^\top \mathbf{Q} \boldsymbol{\Gamma}\) 的每个 \((i,j)\) 块需要 \(\sum_{k=\max(i,j)}^{N} 2n_x n_u^2\) flops - 总计 \(\approx N^2 n_x n_u^2 + N n_x^2 n_u\)

求解 dense QP（Cholesky + back-substitution）：\(\frac{1}{3}(Nn_u)^3 + O((Nn_u)^2)\)。

总 condensing flop：\(O(N^2 n_x n_u^2 + N^3 n_u^3)\)。

与 Riccati 的 flop 对比

Riccati 递推每步：\((n_x + n_u) \times (n_x + n_u)\) 块 Cholesky（\(\frac{1}{3}(n_x+n_u)^3\)）+ GEMM（\(2n_x(n_x+n_u)^2\)）。

\(N\) 步总计：\(O(N(n_x + n_u)^3)\)。

Break-even 条件：\(N^3 n_u^3 \approx N(n_x + n_u)^3\) → \(N^2 \approx ((n_x + n_u)/n_u)^3\)。

Condensing 的数值陷阱

Condensing 有一个隐藏的数值问题：\(\boldsymbol{\Gamma}\) 矩阵的元素包含 \(\prod A_i\) 的乘积。对不稳定系统（\(\|A\| > 1\)），\(\Phi_k = A^k\) 指数增长，\(\Gamma_{k,j}\) 也随之增长——导致 \(\tilde H\) 条件数与单射击 Hessian 一样指数增长！

\[ \text{cond}(\tilde H) \ge c \cdot e^{2\lambda T} \]

这意味着 condensing 继承了单射击的所有条件数问题。对不稳定系统用 condensing + dense QP 可能比 sparse + Riccati 差很多——因为 Riccati 递推每步只处理一个 \(\Delta t\) 的放大，条件数仅 \(\sim e^{2\lambda\Delta t}\)。

本质洞察：Condensing 和单射击在数学上是等价的——condensing 只是把单射击 NLP 的 Hessian 显式构造出来。它"看起来是多射击的"（因为从 block-tridiagonal KKT 出发），但"算出来是单射击的"（因为消去了所有状态变量）。对稳定系统无所谓；对不稳定系统是致命缺陷。

何时用 condensing 何时用 sparse？

条件	推荐	理由
\(N \le 10\), \(n_u \ll n_x\)	Condensing	Dense QP 小，qpOASES 快
\(N > 15\) 或 \(n_u \approx n_x\)	Sparse/Riccati	\(O(N)\) vs \(O(N^3)\)
不稳定系统（\(\rho(A) > 1\)）	Sparse/Riccati	Condensing 条件数爆炸
嵌入式 + 代码生成	Partial condensing	折中——qp 维度适中

Riccati 递推作为 KKT 求解器的完整推导

Riccati 递推不仅是"LQR 的递推公式"——它同时是 block-tridiagonal KKT 系统的**最优消元算法**。让我们从 KKT 系统出发推导 Riccati。

Block-tridiagonal KKT 系统

多射击 QP 的 KKT 条件为：

\[ \begin{bmatrix} Q_0 & S_0^\top & A_0^\top & & \\ S_0 & R_0 & B_0^\top & & \\ A_0 & B_0 & & -I & \\ & & -I & Q_1 & S_1^\top & A_1^\top \\ & & & S_1 & R_1 & B_1^\top \\ & & & A_1 & B_1 & & -I \\ & & & & & \ddots & \end{bmatrix} \begin{bmatrix} \delta x_0 \\ \delta u_0 \\ \lambda_1 \\ \delta x_1 \\ \delta u_1 \\ \lambda_2 \\ \vdots \end{bmatrix} = -\begin{bmatrix} q_0 \\ r_0 \\ b_0 \\ q_1 \\ r_1 \\ b_1 \\ \vdots \end{bmatrix} \]

从后向前消元

Step 1. 在最后一步（\(k = N\)），只有 \(\delta x_N\)，\(P_N = Q_N\)，\(p_N = q_N\)。

Step 2. 在 \(k = N-1\) 步，把 \(\lambda_N\) 和 \(\delta x_N\) 用 \(\delta x_{N-1}, \delta u_{N-1}\) 表示（通过动力学约束的第三行消去 \(\lambda_N\)，再把 \(\delta x_N = A_{N-1}\delta x_{N-1} + B_{N-1}\delta u_{N-1} + b_{N-1}\) 代入）。

这给出一个关于 \((\delta x_{N-1}, \delta u_{N-1})\) 的二次系统：

\[ \begin{bmatrix} Q_{N-1} + A^\top P_N A & S_{N-1}^\top + A^\top P_N B \\ S_{N-1} + B^\top P_N A & R_{N-1} + B^\top P_N B \end{bmatrix} \begin{bmatrix} \delta x \\ \delta u \end{bmatrix} = -\begin{bmatrix} q + A^\top(P_N b + p_N) \\ r + B^\top(P_N b + p_N) \end{bmatrix} \]

Step 3. 对 \(\delta u\) 消元（Schur 补），得到 \(P_{N-1}\) 的递推公式（即 Riccati 方程）。

Step 4. 重复至 \(k = 0\)。

复杂度分析：每步消元需要一次 \((n_u \times n_u)\) 的 Cholesky 分解（\(\frac{1}{3}n_u^3\) flops）和若干 GEMM 操作。\(N\) 步总计 \(O(N(n_x + n_u)^3)\)——与直接求解 \((N(n_x+n_u)) \times (N(n_x+n_u))\) 稠密系统的 \(O(N^3(n_x+n_u)^3)\) 相比快 \(N^2\) 倍。

跨领域类比：Riccati 递推对 block-tridiagonal 矩阵的作用，就像 Thomas 算法对三对角矩阵的作用——都是利用带状结构把 \(O(N^3)\) 降为 \(O(N)\) 的消元技术。Riccati 的"块"版本处理的是 \((n_x+n_u)\) 维的块而非标量，但本质相同。

---

附录 B：RTI 收敛证明的完整推导

Robinson 强正则性

设参数化 NLP 为 \(\min_w F(w; p)\) s.t. \(G(w; p) = 0\)，\(H(w; p) \le 0\)。在 KKT 点 \((w^\ast(p), \lambda^\ast(p), \mu^\ast(p))\) 处，若 LICQ + SSOSC + 严格互补成立，则 Robinson 强正则性给出：

\[ \|w^\ast(p_1) - w^\ast(p_2)\| \le L_w \|p_1 - p_2\| \]

即最优解是参数的 Lipschitz 函数，常数 \(L_w\) 由 KKT 矩阵的最小特征值决定。

从 Newton 收缩到 RTI 收缩

固定参数下的 Newton 收缩：对固定 \(p\)，full-step SQP（= Newton on KKT system）在 \(w^\ast(p)\) 附近有

\[ \|w^{k+1} - w^\ast(p)\| \le \kappa_{\text{N}} \|w^k - w^\ast(p)\| + O(\|w^k - w^\ast\|^2) \]

其中 \(\kappa_{\text{N}} < 1\) 取决于 KKT 系统 Jacobian 的 Lipschitz 常数。

参数变化下的 RTI：在时刻 \(k\)，参数从 \(p^k = \hat{x}_0^k\) 变到 \(p^{k+1} = \hat{x}_0^{k+1}\)。一次 SQP 迭代后：

\[ \begin{aligned} \|w^{k+1} - w^\ast(p^{k+1})\| &\le \|w^{k+1} - w^\ast(p^k)\| + \|w^\ast(p^k) - w^\ast(p^{k+1})\| \\ &\le \kappa_{\text{N}} \|w^k - w^\ast(p^k)\| + L_w \|p^k - p^{k+1}\|. \end{aligned} \]

取 \(\kappa = \kappa_{\text{N}}\)，\(\omega = L_w\) 即得 Diehl 2005 的 contractivity 定理。

联合闭环 ISS 的推导

设闭环状态满足 ISS-MPC（§110 定理 7.1）：

\[ V_N^\ast(x^+) - V_N^\ast(x) \le -\alpha_\ell(\|x\|) + \sigma_w(\|w\|). \]

RTI 引入的次优性误差 \(\delta = \|w^k - w^\ast\|\) 满足 \(\delta^{k+1} \le \kappa \delta^k + \omega \|\Delta x_0\|\)。

把 RTI 误差视为"额外扰动"：effective 扰动 \(w_{\text{eff}} = w + \delta\)，则联合闭环：

\[ V_N^\ast(x^+) - V_N^\ast(x) \le -\alpha_\ell(\|x\|) + \sigma_w(\|w\|) + \sigma_\delta(\delta). \]

若 \(\kappa < 1\) 且闭环 ISS 保证 \(\|\Delta x_0\|\) 有界，则 \(\delta\) 有界，联合闭环 ISS 成立。

---

附录 C：HPIPM Mehrotra IPM 的详细步骤

标准形 OCP-QP

HPIPM 处理的标准形式为：

\[ \min \sum_{k=0}^{N-1} \frac{1}{2} \begin{bmatrix} x_k \\ u_k \end{bmatrix}^\top H_k \begin{bmatrix} x_k \\ u_k \end{bmatrix} + g_k^\top \begin{bmatrix} x_k \\ u_k \end{bmatrix} + \frac{1}{2} x_N^\top H_N x_N + g_N^\top x_N \]

s.t. \(x_{k+1} = A_k x_k + B_k u_k + b_k\)，\(\underline{d}_k \le C_k \begin{bmatrix} x_k \\ u_k \end{bmatrix} \le \bar{d}_k\)。

Mehrotra Predictor-Corrector

Step 1 (Affine scaling direction)：

设当前点 \((x, s_l, s_u, \lambda, \mu_l, \mu_u)\)，解：

\[ \begin{bmatrix} H + \Sigma & C^\top \\ C & 0 \end{bmatrix} \begin{bmatrix} \Delta w^a \\ \Delta\nu^a \end{bmatrix} = -\begin{bmatrix} r_d \\ r_p \end{bmatrix} \]

其中 \(\Sigma = S_l^{-1} M_l + S_u^{-1} M_u\) 是来自互补条件的对角矩阵。

KKT 系统利用 OCP 结构：通过 Riccati 递推求解——每步 \(O((n_x + n_u)^3)\)。

Step 2 (Step size + centering parameter)：

\[ \alpha^a = \max\{\alpha : (s + \alpha \Delta s^a, \mu + \alpha \Delta\mu^a) \ge 0\} $$ $$ \sigma = \left(\frac{(s + \alpha^a \Delta s^a)^\top (\mu + \alpha^a \Delta\mu^a)}{s^\top \mu}\right)^3 \]

Step 3 (Corrector direction)：

修改右侧加入 \(\sigma \mu \mathbf{1}\) 和 \(\Delta s^a \circ \Delta\mu^a\) 修正项，再解一次相同结构的线性系统（复用 Riccati 因子化！）。

Step 4 (Update)：

\[ (x, s, \mu) \leftarrow (x, s, \mu) + \alpha (\Delta x, \Delta s, \Delta\mu) \]

\(\alpha\) 由 fraction-to-boundary rule 确定：\(\alpha = \tau \cdot \min(1, \min_i \{-s_i / \Delta s_i : \Delta s_i < 0\})\)，\(\tau = 0.995\)。

收敛判据

HPIPM 检查四个残差： - primal residual: \(\|r_p\|_\infty \le \epsilon_{\text{abs}}\) - dual residual: \(\|r_d\|_\infty \le \epsilon_{\text{abs}}\) - complementarity: \(\mu \le \epsilon_{\text{abs}}\) - relative gap: \(|p^\ast - d^\ast| / (1 + |p^\ast|) \le \epsilon_{\text{rel}}\)

默认 \(\epsilon_{\text{abs}} = 10^{-8}\)，\(\epsilon_{\text{rel}} = 10^{-8}\)。对 MPC 可放松至 \(10^{-4}\) 以减少迭代。

---

附录 D：OSQP ADMM 详细算法

标准形式

\[ \min \frac{1}{2} x^\top P x + q^\top x \quad \text{s.t.} \quad l \le Ax \le u. \]

ADMM 迭代

引入辅助变量 \(z = Ax\)，对偶变量 \(y\)：

\[ x^{k+1} = (P + \sigma I + \rho A^\top A)^{-1} (\sigma x^k - q + A^\top (\rho z^k - y^k)) $$ $$ z^{k+1} = \Pi_{[l, u]}(A x^{k+1} + y^k / \rho) $$ $$ y^{k+1} = y^k + \rho(A x^{k+1} - z^{k+1}) \]

关键特性：

1. 单次因子分解：\((P + \sigma I + \rho A^\top A)\) 在 setup 阶段做一次 \(LDL^\top\) 分解，后续每次迭代只需一次前代/回代——\(O(\text{nnz})\)；
2. 投影极简：\(\Pi_{[l,u]}\) 是逐元素 clip 操作——\(O(m)\)；
3. warm-start：直接设 \((x^0, z^0, y^0)\) 为上一步解。

\(\rho\) 自适应

OSQP 用自适应 \(\rho\) 策略（Stellato et al. 2020 Sec. 5）：

\[ \rho^{k+1} = \rho^k \cdot \sqrt{\frac{\|r_p^k\|_2 / \|r_d^k\|_2}{\epsilon_p / \epsilon_d}} \]

当 primal residual 远大于 dual 时增大 \(\rho\)，反之减小。需要重新因子分解——但只在 \(\rho\) 变化超过阈值时触发。

---

附录 E：交叉编译完整指南

Jetson Orin NX 部署

硬件规格：ARM Cortex-A78AE，12 核，NEON SIMD（128-bit），L1 64KB/核。

BLASFEO target：ARMV8A_ARM_CORTEX_A76（A78 向下兼容 A76 内核）。

完整 CMake 命令序列：

# 1. 编译 BLASFEO
cd blasfeo && mkdir build_orin && cd build_orin
cmake .. \
  -DCMAKE_TOOLCHAIN_FILE=~/toolchains/aarch64-linux-gnu.cmake \
  -DTARGET=ARMV8A_ARM_CORTEX_A76 \
  -DLA=HIGH_PERFORMANCE \
  -DMF=PANELMAJ \
  -DBLASFEO_CROSSCOMPILING=ON \
  -DBLAS_API=ON \
  -DCMAKE_INSTALL_PREFIX=~/orin_install
make -j$(nproc) && make install

# 2. 编译 HPIPM
cd ../../hpipm && mkdir build_orin && cd build_orin
cmake .. \
  -DCMAKE_TOOLCHAIN_FILE=~/toolchains/aarch64-linux-gnu.cmake \
  -DBLASFEO_PATH=~/orin_install \
  -DCMAKE_INSTALL_PREFIX=~/orin_install
make -j$(nproc) && make install

# 3. 编译 acados
cd ../../acados && mkdir build_orin && cd build_orin
cmake .. \
  -DCMAKE_TOOLCHAIN_FILE=~/toolchains/aarch64-linux-gnu.cmake \
  -DACADOS_WITH_QPOASES=OFF \
  -DACADOS_WITH_OSQP=OFF \
  -DBLASFEO_PATH=~/orin_install \
  -DHPIPM_PATH=~/orin_install \
  -DCMAKE_INSTALL_PREFIX=~/orin_install
make -j$(nproc) && make install

# 4. 编译 OCP solver
cd ~/mpc_project/c_generated_code
cmake . \
  -DCMAKE_TOOLCHAIN_FILE=~/toolchains/aarch64-linux-gnu.cmake \
  -Dacados_DIR=~/orin_install/lib/cmake/acados
make

STM32MP1 部署（资源受限）

• BLASFEO target: ARMV7A_ARM_CORTEX_A7
• 限制：无 NEON double precision（只有 single float）→ 必须用 float 精度
• 内存：256KB SRAM → 静态分配，禁止 malloc
• acados 选项：ACADOS_WITH_QPOASES=ON（比 HPIPM 更适合小 QP）

---

附录 F：QP 求解器性能基准

典型场景对比

测试条件：四旋翼 \(n_x = 13\)，\(n_u = 4\)，\(N = 20\)，Intel i7-12700H。

求解器	配置	time_qp (mean)	time_qp (p99)	迭代数
HPIPM (sparse)	Mehrotra, tol=1e-8	0.12 ms	0.18 ms	8-12
HPIPM (partial, N2=5)	Mehrotra, tol=1e-8	0.09 ms	0.14 ms	8-12
qpOASES (condensed)	active-set	0.08 ms	0.35 ms	3-15
OSQP (sparse)	ADMM, tol=1e-4	0.15 ms	0.45 ms	50-200
OSQP (sparse)	ADMM, tol=1e-3	0.08 ms	0.25 ms	20-80

观察： 1. qpOASES mean 最快（得益于 warm-start），但 p99 最差（active-set 剧变时）； 2. HPIPM 最稳定——p99/mean ratio 最小（IPM 迭代数几乎恒定）； 3. OSQP 精度换速度——放松 tol 到 \(10^{-3}\) 后接近 HPIPM 但精度低 4 个量级。

ARM Cortex-A76 (Jetson Orin) 同一问题

求解器	time_qp (mean)	相对 x86
HPIPM (partial, N2=5)	0.35 ms	3.9x slower
HPIPM (GENERIC target)	0.95 ms	10.6x slower

启示：正确 BLASFEO target 带来 2.7 倍加速——差了一个 target 就差了半个数量级。

扩展 Benchmark：不同机器人系统的 NLP 求解时间

系统	\(n_x/n_u/N\)	求解器	方法	mean (ms)	p99 (ms)	频率上限
四旋翼	13/4/20	acados HPIPM	SQP_RTI	0.4	0.7	1.4 kHz
四旋翼	13/4/40	acados HPIPM	SQP_RTI	0.9	1.3	770 Hz
四足 SRBD	13/12/10	qpOASES dense	condensed QP	0.5	1.2	830 Hz
四足 whole-body	24/12/20	OCS2 HPIPM	SLQ	8	12	83 Hz
四足 whole-body	36/12/20	Crocoddyl	BoxFDDP 3iter	6	9	111 Hz
人形上身	37/17/20	Crocoddyl	BoxFDDP 3iter	15	22	45 Hz
人形全身	74/30/20	Aligator	ProxDDP	25	40	25 Hz
自动驾驶	6/2/30	acados HPIPM	SQP_RTI	0.2	0.4	2.5 kHz
Crazyflie 27g	12/4/15	TinyMPC ADMM	嵌入式	0.4	0.6	1.6 kHz

观察：

1. 维度决定量级：从 13 维到 74 维，求解时间增长约 50 倍——大致符合 \(O(n^3)\) 的 Riccati 递推复杂度。
2. 求解器选择影响 3-10 倍：同一问题用 qpOASES vs OSQP 可能差 3 倍；用 GENERIC vs 正确 BLASFEO target 差 3 倍。
3. RTI 是实时的关键：acados SQP_RTI 的频率上限远超 full SQP——因为每周期只做 1 次迭代。
4. 人形全身 MPC 仍是挑战：25 Hz 对步态控制勉强够用（步态频率 ~2 Hz），但对精细操作不够。

QP 求解器选型的量化决策

从 benchmark 数据提炼的选型规则：

dense QP 维度 Nm_u < 100?
├── 是 → qpOASES（active-set，warm-start 极佳）
│   └── 但 p99 延迟要求严格? → HPIPM（IPM 更稳定）
└── 否 → OCP-QP 结构可利用?
    ├── 是 → HPIPM + Riccati（$O(N)$ 复杂度）
    │   └── ARM 平台? → 确保 BLASFEO target 正确
    └── 否 → 通用稀疏 QP
        ├── 精度要求 < 1e-4 → OSQP（一阶，代码生成友好）
        └── 精度要求 >= 1e-6 → ProxQP 或 HPIPM

---

附录 G：关键论文 DOI 速查表

主题	文献	DOI / arXiv
多射击	Bock-Plitt 1984	10.1016/S1474-6670(17)61205-9
RTI	Diehl-Bock-Schloder 2005	10.1137/S0363012902400713
Collocation 教程	Kelly 2017 SIAM Rev.	10.1137/16M1062569
HPIPM	Frison-Diehl 2020	arXiv:2003.02547
BLASFEO	Frison et al. 2018	10.1145/3210754
CasADi	Andersson et al. 2019	10.1007/s12532-018-0139-4
acados	Verschueren et al. 2022	10.1007/s12532-021-00208-8
IPOPT	Wachter-Biegler 2006	10.1007/s10107-004-0559-y
qpOASES	Ferreau et al. 2014	10.1007/s12532-014-0071-1
OSQP	Stellato et al. 2020	10.1007/s12532-020-00179-2
Partial condensing	Axehill 2015	10.1016/j.sysconle.2014.12.005
Explicit MPC	Bemporad et al. 2002	10.1016/S0005-1098(01)00174-1
MPPI	Williams et al. 2016	10.1109/ICRA.2016.7487277
DiffMPC	Amos et al. 2018	arXiv:1810.13400
CoCo	Cauligi et al. 2022	10.1109/LRA.2021.3135931
AS-RTI	Frey et al. 2024	arXiv:2403.07101

---

附录 H：教材对照表

教材	转录方法	QP 结构	RTI	内点法	配点
Rawlings-Mayne-Diehl Ch.8	全覆盖	Riccati	核心	简述	简述
Betts 3e (SIAM 2020)	Ch.4	--	--	Ch.3	Ch.4-5 核心
Nocedal-Wright 2e	--	--	Ch.18 SQP	Ch.19	--
Kelly 2017 SIAM Rev.	全覆盖教程	--	--	--	核心
Hairer-Wanner ODE II	--	--	--	--	Ch.IV 核心
Tedrake Underactuated	Ch.10	--	--	--	Ch.10

---

附录 I：六步学习路径

1. 数学补位（2 周）：QP 对偶、KKT、Nocedal-Wright Ch.14-19
2. CasADi 入门（1-2 周）：Mehrez 视频 + 手写 MPC
3. acados 实战（2-3 周）：四旋翼 13 维 NMPC，RTI 100 Hz
4. 机器人任务（4 周）：四足/AD/四旋翼选一
5. 学习耦合（2-4 周）：L4CasADi/l4acados + PyTorch NN

每阶段的详细目标

第 1 阶段（数学补位）——不急着写代码，先确保理论基础扎实：

日	内容	产出
1-3	QP 对偶理论（Nocedal Ch.16）	手推 KKT 条件 + 对偶问题
4-6	Newton / GN / LM 方法（Nocedal Ch.6,10）	理解为什么 GN 对 LS cost 特别快
7-9	SQP 基础（Nocedal Ch.18）	理解 SQP = 序列 QP + merit
10-12	内点法概念（Nocedal Ch.19）	理解 barrier + centering
13-14	复习 + 做习题	确认能独立推导 KKT 系统

第 2 阶段（CasADi 入门）——从 Hello World 到完整 MPC：

日	内容	代码
1-2	SX/MX 基础 + 自动微分	对 Rosenbrock 函数求 Hessian
3-4	Opti stack + IPOPT	双积分器 MPC（\(N = 20\)）
5-7	自行车模型 MPC	含 RK4 积分 + 约束
8-10	代码生成 + C 调用	Function.generate + gcc 编译

第 3 阶段（acados 实战）——从 Python API 到 200 Hz：

日	内容	关键指标
1-3	倒立摆 NMPC（SQP）	收敛到 \(\|Q_u\| < 10^{-6}\)
4-6	四旋翼 NMPC（SQP_RTI）	time_tot < 5 ms
7-10	Partial condensing 调优	找到最优 cond_N
11-14	闭环仿真 + \(V_N^\ast\) 曲线	验证 Lyapunov 下降
15-20	加扰动 + ISS 分析	验证 ISS 增益

第 4 阶段（机器人任务）——根据研究方向选一：

方向	推荐任务	工具链
四足	ANYmal trotting MPC	OCS2 或 Crocoddyl
操作臂	7-DoF reaching + 避障	acados + CasADi
四旋翼	激进飞行 + 地标跟踪	acados + PX4
自动驾驶	车道保持 + 避障 MPC	acados + Autoware

从"能跑"到"能上实机"的五个里程碑

里程碑	标准	验证方法
M1	MPC 在仿真中闭环稳定	\(V_N^\ast(x_k)\) 单调下降
M2	求解时间 < 采样周期的 80%	p99 延迟测量
M3	加入传感器噪声仍稳定	ISS 分析 + 扰动仿真
M4	约束在所有测试中满足	\(\|g^+\|_\infty = 0\)（硬约束）
M5	Fallback 策略在超时时正确切换	注入人为超时测试

到达 M5 后才能安全部署到实机。M1-M2 可以在一周内完成，M3-M5 通常需要 2-4 周的工程调试。

本章常见误解汇总

误解	正确理解
MPC 只需要选一个好的 NLP solver 就能实时	实时性的关键不是 solver 本身，而是问题结构的利用——condensing 把稀疏问题压成 dense QP，Riccati 递推利用时间结构实现 \(O(N)\)，BLASFEO 利用硬件结构做 panel-major 矩阵运算
Direct collocation 比 shooting 精度更高	精度取决于离散化阶数（Euler vs RK4 vs 隐式），两种转录方式可以使用相同的积分器；shooting 在动力学可行性上有天然优势，collocation 在约束处理上更灵活
RTI（一步 SQP）的解质量很差	RTI 的前提是良好的 warm-start（上一时刻的解平移一步），在 MPC 闭环中相邻时刻的 OCP 变化很小，一步 SQP 的修正量通常足够；Diehl 2005 的理论证明了闭环收缩性
代码生成（codegen）只是编译优化	代码生成在编译期展开循环、消除动态分配、内联 Riccati 步骤、硬编码问题维度——这些是运行时 solver 无法做到的；acados codegen 比解释执行快 10-100 倍
ADMM（如 OSQP）不适合 MPC	ADMM 的优势是无需矩阵分解、对 ill-conditioned 问题鲁棒、支持 warm-start；TinyMPC 在微控制器上用 ADMM 实现了 kHz 级 MPC，证明了其实用性
Gauss-Newton Hessian 近似总比精确 Hessian 差	GN 近似 \(J^\top J\) 保证正半定（无需正则化），且计算量小于精确 Hessian（无需二阶动力学导数）；对最小二乘型代价函数，GN 在最优解附近的收敛率与 Newton 相同
嵌入式部署只需要把 PC 上的代码交叉编译	嵌入式 MPC 还需关注：WCET 分析（最坏执行时间）、PREEMPT_RT 补丁（实时调度）、堆分配消除（栈上预分配）、缓存预热、fallback 策略（超时时的安全动作）

---

与前后章节的完整桥梁

本章（§120 MPC 数值求解）是整个 MPC 三部曲的最后一环。回顾完整链条：

章节	关注点	核心问题
§90 DDP/iLQR	算法原理	"DDP 如何工作？"
§100 约束 DDP	约束处理	"有约束时怎么办？"
§110 MPC 稳定性	理论保证	"为什么 MPC 稳定？"
§120 MPC 数值	实现部署	"如何让 MPC 在实机上跑？"

每一章都依赖前一章的结论：

• §120 的 Riccati 递推 ← §90 的 backward pass 就是 Riccati
• §120 的 RTI ← §100 的 warm-start + §110 的 suboptimal MPC
• §120 的 WCET ← §110 的 ISS（数值误差可视为扰动）
• §120 的 BLASFEO ← §90 的 \(O(Nm^3)\) 复杂度分析

本质洞察：MPC 数值方法的进步等价于"把连续 OCP 压扁成 block-banded QP，再把 block-banded QP 压进 SIMD 寄存器"。每一次压缩都利用了问题结构——时间结构（Riccati）、代价结构（GN Hessian）、硬件结构（BLASFEO panel-major）。理解这三层结构利用，就理解了为什么机器人 NMPC 能从 2012 年的 Hz 级提升到 2025 年的 kHz 级。

2025-2026 年的前沿方向

方向	核心突破	代表工作
GPU 加速 NMPC	CUDA 并行 NLP solver	NVIDIA Isaac MPC
学习增强 MPC	NN warm-start / cost learning	L4CasADi, DiffMPC
接触感知 MPC	硬摩擦锥 + 实时	Aligator ProxDDP
多体并行 Riccati	树结构 \(O(\log N)\)	并行 LQR (Plancher)
量化 MPC	定点数 ADMM	TinyMPC (CMU)
扩散策略 warm-start	Diffusion → 初始轨迹	Diffusion Policy + MPC

这些方向共同指向一个目标：让任何机器人都能在任意环境中实时做最优决策——从 27g 的 Crazyflie 到 100kg 的人形，从平地行走到月球探索。MPC 数值方法是实现这个目标的计算引擎。

理解了本章的转录、QP 结构利用、RTI、代码生成这四根支柱，你就掌握了从"黑板上的 OCP"到"Jetson 上的 kHz 控制器"的完整工程链——这正是现代机器人 MPC 工程师的核心技能栈。 6. 鲁棒/安全（选修）：Tube-MPC、CBF-MPC