U2 鲁棒规划与安全滤波：从"名义轨迹"到"管状安全包络"

本章定位：本章是不确定性规划专题里工程生态最成熟、复用价值最高的一章（🏭🔬 安全关键系统核心）。它对应"五安全谱"里最保守的一端——鲁棒规划：对扰动集内的所有可能扰动都给出 100% 的约束满足保证。四足在未知地形上的抗滑、无人机在阵风下的不越界、机械臂在力控容差下的安全，底层都靠本章的两套工具：Tube MPC（把名义轨迹包进一条"安全管道"）与 CBF 安全滤波（在控制输出端加一道实时"护栏"）。

读者画像：已完成 U0 总论，掌握名义 MPC（用过 acados / OCS2 / CasADi 之一）、凸优化（QP/NLP）、李雅普诺夫稳定性基础的算法工程师。

在累积项目中的位置：本章给统一导航测试台加装"执行端不确定性"的处理层——先用 Tube MPC 让真实轨迹被约束在名义轨迹外围的管道内（应对动力学扰动 \(w\)），再用 CBF-QP 安全滤波器在控制输出端兜一道瞬时安全底线，实现零碰撞。

本章与 U0 的衔接：U0 §3 把鲁棒规划定义为"对所有 \(w\in\mathcal{W}\) 保证约束满足、信息需求最低（只需扰动集边界）、但最保守"。本章就是把这句话**展开成可实现的算法**——并在 §U2.2 起逐步回答"如何在保留鲁棒保证的同时减少保守度"。

---

前置自测

进入本章前，确认以下 5 题。答不出 \(\ge 2\) 题，建议先补对应前置。

1. 写出一个标准名义 NMPC 的优化问题（含动力学约束、状态约束、控制约束、代价），并说明它在每个控制周期求解什么。（不会 → 回 U0 前置桥接 / MPC 基础）
2. Minkowski 和 \(\mathcal{A}\oplus\mathcal{B}\) 是什么？给两个区间 \([-1,1]\oplus[-2,2]\) 等于什么？（不会 → 本章 §U2.1 会补定义，但建议先了解集合运算）
3. 离散线性系统 \(e_{k+1}=Me_k\) 在什么条件下渐近稳定？谱半径 \(\rho(M)\) 在其中起什么作用？（不会 → 回线性系统基础）
4. 一个凸二次规划（QP）\(\min \tfrac12 u^\top H u + g^\top u\) s.t. \(Au\le b\) 的求解复杂度大致如何？为什么它能做到微秒级？（不会 → 回凸优化 / QP 求解器基础）
5. 李雅普诺夫函数 \(V(x)\) 用来证明什么？"\(V\) 沿轨迹递减"意味着什么？（不会 → 回稳定性理论基础）

这 5 题对应本章的支柱：名义 MPC（Tube 的"名义系统"分量）、集合运算（RPI 集与约束收紧）、线性系统稳定性（误差动力学）、QP（CBF 滤波器的在线求解）、李雅普诺夫（CBF 是"安全版"的李雅普诺夫论证）。

---

本章目标

学完本章后，你应该能够：

1. 推导 Tube MPC 的核心分解——把不确定闭环系统拆成"名义系统 + 反馈修正"，并说清真实状态为何被约束在围绕名义轨迹的 RPI 集内；
2. 独立实现 Rigid Tube MPC：离线算 RPI 集、收紧约束、在线解名义 MPC，并用蒙特卡洛验证轨迹不出管；
3. 说清 Rigid / Homothetic / Elastic 三代 Tube 在"保守度 vs 计算代价"上的权衡，并据此选型；
4. 理解 FaSTrack / RTD / Funnel Libraries 三种可达性方法如何给出"与规划器解耦"的安全包络；
5. 搭建 GP-MPC 的标准架构：名义模型 + GP 残差 → 状态分布前向传播 → 机会约束；
6. 实现 CBF-QP 安全滤波器，理解它如何"以最小修正"把任意名义控制器变安全，以及它与 MPC 的层级互补关系；
7. 把 acados SQP-RTI 作为鲁棒 MPC 的默认工业级求解器用起来，知道约束收紧如何落到 OCP 定义里。

在六个核心方法之外，本章后半还提供**方法选型决策指南**（手头问题该用哪个）、从 Python 原型到 C++ 部署（含 Eigen+qpOASES 的部署版 CBF-QP 与 ROS2 实时节点）、以及把累积项目推广到**动态障碍 / 机会约束**的进阶——帮你从"理解算法"走到"上真机部署"。

---

本章知识导航 ⭐

本章沿一条主线展开：给名义控制加上"应对扰动"的能力，而做这件事有两大类思路——事前在规划里预留余量（Tube / 可达性 / GP-MPC，§U2.1–U2.4）和**事中在控制输出端实时纠偏**（CBF 安全滤波，§U2.5），最后落到**工业级求解器**（acados，§U2.6）。

                 名义 MPC（无鲁棒性，U0 已掌握）
                          │ 加"应对扰动"的能力
          ┌───────────────┴────────────────┐
   事前预留余量（规划层）            事中实时纠偏（控制层）
          │                                │
  §U2.1 Tube MPC 本质                §U2.5 CBF-QP 安全滤波
  §U2.2 三代 Tube 选型                      │
  §U2.3 可达性(FaSTrack/RTD/Funnel)         │
  §U2.4 GP-MPC（数据驱动减不确定性）         │
          └───────────────┬────────────────┘
                          ▼
              §U2.6 acados：工业级求解器落地

本章方法谱系（这条线怎么来的）：鲁棒 MPC 从 1998 年 Scokaert–Mayne 的 min-max 形式化起步（理论优美但计算爆炸），2004–2005 年由 Langson 与 Mayne 等人的 Tube 思想化解（名义系统 + 不变管道，计算复杂度随时域线性而非指数增长），2012–2016 年经 Homothetic / Elastic Tube 进一步降低保守度，2017 年起分两支并行发展——可达性方法（HJ/zonotope/SOS 离线算跟踪误差包络）与 CBF 安全滤波（与 MPC 互补的实时安全层），并在数据驱动方向衍生出 GP-MPC。各方法的精确出处在对应小节给出并已逐一核实。

小节	主题	难度
§U2.1	Tube MPC 的数学本质	⭐⭐⭐
§U2.2	三代 Tube 的选型	⭐⭐
§U2.3	可达性方法：FaSTrack / RTD / Funnel	⭐⭐⭐
§U2.4	GP-MPC：数据驱动的鲁棒控制	⭐⭐⭐
§U2.5	CBF-QP 安全滤波器	⭐⭐⭐
§U2.6	acados：工业级求解器落地	⭐⭐

推荐阅读路径：§U2.1 是全章地基（Tube 的数学本质），务必读透；§U2.2 紧随其后讲选型。若你的目标是"尽快在真机上有安全保障"，可在读完 §U2.1 后直接跳到 §U2.5（CBF-QP，工程上最快见效）和 §U2.6（acados 落地），把 §U2.3/§U2.4 留作进阶。若追求理论完整，按顺序读。

---

前置知识桥接

回顾——名义 MPC 做了什么：你在前序方向里求解过 $\(\min_{u_{0:N-1}} \ \sum_{k=0}^{N-1}\ell(x_k,u_k)+\ell_f(x_N)\quad\text{s.t.}\quad x_{k+1}=Ax_k+Bu_k,\ x_k\in\mathcal{X},\ u_k\in\mathcal{U}.\)$ 它默认动力学**精确**、无扰动。本章做的事，是把动力学改成带扰动的 \(x_{k+1}=Ax_k+Bu_k+w_k\)（\(w_k\in\mathcal{W}\)），然后重新设计这个优化问题，使得"无论扰动怎么取，约束都满足"。

回顾——QP 求解：CBF 安全滤波器（§U2.5）的核心是一个凸 QP——在线以微秒级求解，把名义控制"投影"到安全集里。你用过的 OSQP / qpOASES / ProxQP 在这里直接复用。

本章在前置之上加什么：名义 MPC 给"性能/经济最优"，本章给"在不确定下的安全保证"。关键转变是——决策对象从"一条名义轨迹"升级为"一条名义轨迹 + 一条包住它的管道"（U0 跨越一）。

本质洞察：Tube MPC 最聪明的地方在于**把"在线对抗扰动"这件难事，拆成了"离线准备"和"在线求解名义问题"两块**。离线：算出一个反馈增益 \(K\) 和一个不变管道 \(\Omega\)（一次性、可慢慢算）；在线：只解一个和名义 MPC 同样大小的优化问题（快）。这样，鲁棒性的代价主要由离线买单，在线计算几乎不增加——这正是它能上嵌入式实时系统的根本原因。

---

如果跳过本章会怎样

场景一：名义 MPC 控四足上斜坡打滑摔倒。 仿真里 NMPC 控四足走得稳，真机一上斜坡就摔——因为真机摩擦、电机延迟、负载偏差都偏离名义模型，而名义 MPC 没有任何余量去吸收偏差，预测轨迹与实际轨迹越走越偏，最终触碰约束。Tube MPC（§U2.1）给名义轨迹外围预留一条管道，把这些偏差"框"在可控范围内。

场景二：RL 策略在真机上偶发危险动作，无人敢部署。 你训了个 RL 四足策略，仿真表现好，但偶尔会输出导致摔倒/碰撞的动作，没有任何安全保证，不敢上真机。CBF 安全滤波（§U2.5）能套在任意名义控制器（包括 RL 策略）外面，以"最小修正"实时拦截危险动作——让"不可证明安全的策略"获得形式化的安全底线。

这两个场景会在累积项目的 U2 模块里被复现：先看名义 baseline 在扰动下的碰撞率，再看加装 Tube + CBF 后碰撞率降到零。

---

预计阅读时间

阅读方式	时间	适合谁
精读（含推导 + 实现 Rigid Tube 与 CBF-QP）	12–16 小时	第一次系统学鲁棒规划的读者
速读（抓 §U2.1 本质 + §U2.5 CBF + §U2.6 落地，跳过可达性/GP 推导）	4 小时	有 MPC 背景、要快速上手安全层的工程师
速查（只看各节对比表 + 故障排查 + API 速查）	40 分钟	已在实现、回来查某个 API 或选型

---

§U2.1 Tube MPC 的数学本质 ⭐⭐⭐

动机：名义 MPC 在扰动下为什么不安全

名义 MPC 预测的是"理想轨迹"——它假设系统严格按 \(x_{k+1}=Ax_k+Bu_k\) 演化。但真实系统有扰动 \(w_k\)：每一步，真实状态都会被 \(w_k\) 推离预测一点点，这些偏离会随时间**累积放大**（误差的演化由闭环动力学决定，若不主动镇定，偏差可能发散）。结果是：名义 MPC 算出的轨迹哪怕"贴着约束边界飞"在理想世界里最优，在真实世界里只要一点扰动就越界。

鲁棒规划的提问是：给定扰动落在已知有界集合 \(w_k\in\mathcal{W}\) 内，如何让真实轨迹在所有可能的扰动序列下都满足约束？ Tube MPC 给出的答案优雅得出人意料——它不去在线枚举所有扰动（那会爆炸，正是 §U2.2 会提到的 min-max 方法的困境），而是把不确定闭环系统**分解**成一个确定的"名义系统"加一个被反馈镇定住的"偏差系统"。

反面：如果不收紧约束会怎样

设想你已经用 Tube 的反馈律 \(u_k=v_k+Ke_k\) 把偏差镇定住了，偏差始终落在某个集合 \(\Omega\) 里。如果你在求解名义轨迹 \(z_k\) 时仍然用**原始**约束 \(z_k\in\mathcal{X}\)——也就是让名义轨迹贴着原边界走——会发生什么？真实状态 \(x_k=z_k+e_k\) 比名义状态多了一个 \(e_k\in\Omega\) 的偏差，于是 \(x_k\) 会越过 \(\mathcal{X}\) 的边界。正确做法是把名义轨迹的可行域向内收紧 \(\Omega\) 那么多（即 \(z_k\in\mathcal{X}\ominus\Omega\)），给真实偏差预留空间。这个"约束收紧"是 Tube MPC 的核心机制，下面逐步推导。

理论：Tube MPC 的核心推导

先约定两个集合运算（贯穿本章）： - Minkowski 和：\(\mathcal{A}\oplus\mathcal{B}=\{a+b:a\in\mathcal{A},\,b\in\mathcal{B}\}\)。直观上是"把 \(\mathcal{B}\) 沿 \(\mathcal{A}\) 的每个点扫一遍"。例如 \([-1,1]\oplus[-2,2]=[-3,3]\)。 - Pontryagin（Minkowski）差：\(\mathcal{A}\ominus\mathcal{B}=\{x:x+b\in\mathcal{A}\ \forall b\in\mathcal{B}\}\)。直观上是"把 \(\mathcal{A}\) 向内腐蚀 \(\mathcal{B}\) 那么多"，使得腐蚀后的集合加回 \(\mathcal{B}\) 仍不超出 \(\mathcal{A}\)。注意它**不是** \(\oplus\) 的逆运算（一般 \((\mathcal{A}\ominus\mathcal{B})\oplus\mathcal{B}\subseteq\mathcal{A}\)，不取等）。

第 1 步——问题设定。考虑带有界加性扰动的线性系统 $\(x_{k+1}=Ax_k+Bu_k+w_k,\qquad w_k\in\mathcal{W},\)$ 状态约束 \(x_k\in\mathcal{X}\)、控制约束 \(u_k\in\mathcal{U}\)，其中 \(\mathcal{W}\) 是已知的有界凸集（如多面体或盒集），\(\mathcal{X},\mathcal{U}\) 为凸约束集。我们的目标是设计一个控制律，使真实轨迹对**任意**扰动序列 \(\{w_k\}\) 都满足约束。

第 2 步——核心分解。引入一个不受扰动的**名义系统** $\(z_{k+1}=Az_k+Bv_k,\)$ 其中 \(z_k\) 是名义状态、\(v_k\) 是名义控制（待优化的决策变量）。定义**偏差** \(e_k\triangleq x_k-z_k\)，并采用如下控制律： $\(\boxed{u_k=v_k+K(x_k-z_k)=v_k+Ke_k,}\)$ 其中 \(K\) 是**预先设计好的反馈增益**（离线选定，使 \(A+BK\) 稳定）。这条控制律的含义是：名义部分 \(v_k\) 负责"往目标走"，反馈部分 \(Ke_k\) 负责"把真实状态拉回名义轨迹"。

第 3 步——偏差动力学。把上面三式代入，算偏差怎么演化： $$ \begin{aligned} e_{k+1}&=x_{k+1}-z_{k+1}\ &=(Ax_k+Bu_k+w_k)-(Az_k+Bv_k)\ &=A(x_k-z_k)+B(u_k-v_k)+w_k\ &=Ae_k+B(Ke_k)+w_k\ &=(A+BK)e_k+w_k. \end{aligned} $$ 关键观察：偏差动力学 \(e_{k+1}=(A+BK)e_k+w_k\) 是一个**自治的、被 \(K\) 镇定的、只受扰动驱动的**线性系统——它和名义轨迹 \(v_k\)、和目标都无关。这意味着"对抗扰动"这件事被完全隔离到了偏差系统里，与"往哪走"（名义系统）解耦。这就是 Tube MPC 把难题拆开的那一刀。

第 4 步——RPI 集与不变性。既然 \(A+BK\) 稳定，偏差不会发散，那它会被困在多大的范围里？这就是 RPI（Robustly Positively Invariant，鲁棒正不变）集**的概念。称集合 \(\Omega\)（本章 \(\Omega\) 表示 RPI 集，不同于 U4 POMDP 章的观测空间 \(\Omega\)。）对偏差动力学是 RPI 的，若 $\((A+BK)\,\Omega\ \oplus\ \mathcal{W}\ \subseteq\ \Omega.\)$ **这个条件的含义与证明：假设某时刻 \(e_k\in\Omega\)，那么下一步 $\(e_{k+1}=(A+BK)e_k+w_k\in (A+BK)\Omega\oplus\mathcal{W}\subseteq\Omega,\)$ 即 \(e_{k+1}\) 仍在 \(\Omega\) 里。由归纳法，只要初始 \(e_0\in\Omega\)，则 \(e_k\in\Omega\) 对所有 \(k\) 成立、且对任意扰动序列成立。一句话：偏差一旦进入 \(\Omega\) 就永远留在 \(\Omega\) 里。这个 \(\Omega\) 就是"管道的横截面"——真实轨迹 \(x_k=z_k+e_k\) 始终落在以名义轨迹 \(z_k\) 为中心、形状为 \(\Omega\) 的管道内。

第 5 步——最小 RPI 集（mRPI）及其计算。满足 RPI 条件的 \(\Omega\) 有很多（越大越容易满足），我们想要**最小**的那个，以减少保守度。理论上的**最小 RPI 集**为 $\(\Omega_\infty=\bigoplus_{i=0}^{\infty}(A+BK)^i\,\mathcal{W}.\)$ 它的含义是把当前与所有历史扰动经闭环动力学传播后的影响全加起来。问题是这是一个**无穷 Minkowski 和**，一般不可有限精确计算。工程上用 Raković, Kerrigan, Kouramas, Mayne（IEEE Transactions on Automatic Control, 50(3):406–410, 2005）给出的方法：计算一个 RPI 的**外逼近** \(\Omega_\alpha\supseteq\Omega_\infty\)，它可被任意精确地逼近 \(\Omega_\infty\)，且本身是 RPI 集（这套算法是 Tube MPC 实现里离线阶段的标准步骤，累积项目会用到）。

第 6 步——约束收紧。现在把"真实轨迹满足约束"翻译成"名义轨迹满足什么约束"。由 \(x_k=z_k+e_k\) 且 \(e_k\) 可取遍 \(\Omega\)：

• 要保证 \(x_k\in\mathcal{X}\) 对所有 \(e_k\in\Omega\) 成立，需 \(z_k+e\in\mathcal{X}\ \forall e\in\Omega\)，即 $\(z_k\in\mathcal{X}\ominus\Omega.\)$
• 要保证 \(u_k=v_k+Ke_k\in\mathcal{U}\) 对所有 \(e_k\in\Omega\) 成立，需 \(v_k+Ke\in\mathcal{U}\ \forall e\in\Omega\)，即 $\(v_k\in\mathcal{U}\ominus K\Omega.\)$

第 7 步——在线只解名义 MPC。综合起来，Tube MPC 在线求解的是一个**和名义 MPC 同样结构、只是约束被收紧**的优化问题： $\(\min_{v_{0:N-1}}\ \sum_{k=0}^{N-1}\ell(z_k,v_k)+\ell_f(z_N)\quad\text{s.t.}\quad z_{k+1}=Az_k+Bv_k,\ z_k\in\mathcal{X}\ominus\Omega,\ v_k\in\mathcal{U}\ominus K\Omega,\)$ 初始条件通常取 \(z_0\) 使 \(x_0-z_0\in\Omega\)（如 \(z_0=x_0\)）。解出名义控制 \(v_0\) 后，实际施加的是 \(u_0=v_0+K(x_0-z_0)\)。离线一次性算 \(K\) 和 \(\Omega\)，在线只解这个收紧的名义 QP——这就是 Tube MPC 的完整工作流。

多视角理解：管道的三种看法 - 几何视角：一条以名义轨迹 \(z_k\) 为中心、横截面为 \(\Omega\) 的"管道"，真实轨迹被关在管道里。 - 控制视角：\(K\) 是一个"内环镇定器"，把真实状态拉回名义轨迹；名义 MPC 是"外环规划器"，决定名义轨迹往哪走。内外环解耦。 - 集合视角：把"单条轨迹满足约束"升级成"一族轨迹（管道）整体满足约束"，方法是把约束集向内腐蚀管道半径那么多（\(\ominus\Omega\)）。

本质洞察：约束收紧 \(\mathcal{X}\ominus\Omega\) 把"保守度"显式地量化成了一个集合运算——\(\Omega\) 越大（扰动越强或镇定越弱），可行域被腐蚀得越多，名义轨迹的活动空间越小、越保守。这正是 U0 §1 强调的"保守度应由不确定性模型推导"在 Tube MPC 里的精确体现：你不是凭感觉加裕度，而是让裕度等于 \(\Omega\)——一个由 \(\mathcal{W}\) 和 \(K\) 算出来的集合。\(\Omega\) 太大就说明要么扰动假设太悲观、要么反馈 \(K\) 太弱（这恰好引出 §U2.2 的"如何减少保守度"）。

⚠️ 本节常见误区

💡 编程陷阱：把名义 MPC 的约束直接当成真实约束，忘了收紧 - 现象/报错：仿真里加上扰动后，机器人偶尔越过状态约束边界（如越过墙、超速），但优化器从不报 infeasible——因为它解的是没收紧的名义问题，自以为可行。 - 错误代码：在 OCP 里写 z_k ∈ X、v_k ∈ U（直接用原约束）。 - 正确写法：约束应为 z_k ∈ X ⊖ Ω、v_k ∈ U ⊖ KΩ；多面体约束 \(Cx\le d\) 收紧为 \(Cz\le d-\max_{e\in\Omega}Ce\)（逐行减去 \(\Omega\) 在该方向上的支撑函数值）。 - 根本原因：约束收紧是 Tube MPC 提供鲁棒保证的唯一来源，漏掉它就退化成了普通名义 MPC，鲁棒性荡然无存。 - 检验方法：蒙特卡洛跑 1000 条扰动轨迹，统计实际状态是否始终在 \(\mathcal{X}\) 内；若偶有越界，十有八九是约束没收紧或 \(\Omega\) 算小了。

💡 概念误区：以为反馈增益 \(K\) 是在线和名义控制一起优化的 - 新手想法："\(K\) 也是控制律的一部分，应该和 \(v_k\) 一起在 MPC 里优化出来。" - 现象/后果：试图在线优化 \(K\) 导致问题非凸、求解困难，或与 \(\Omega\) 的计算自相矛盾（\(\Omega\) 依赖 \(K\)）。 - 根本原因：在 Rigid Tube MPC 里，\(K\) 是**离线固定**的——正因为 \(K\) 固定，偏差动力学 \((A+BK)\) 才确定，\(\Omega\) 才能离线算出。在线优化的只有名义控制 \(v_k\)。把 \(K\) 也放进在线优化，就破坏了"离线准备、在线轻量"的整个架构。 - 正确做法：离线选 \(K\)（常用对名义系统做 LQR 得到的增益），据此离线算 \(\Omega\)；在线只优化 \(v_k\)。（想让管道也能在线自适应？那是 §U2.2 的 Homothetic/Elastic Tube 在做的事，但它们优化的是管道的**缩放/形状参数**，仍不是 \(K\) 本身。）

🧠 思维陷阱：以为 \(\Omega\) 越小越好，一味追求最小 RPI - 新手想法："\(\Omega\) 越小保守度越低，所以一定要算到最小 RPI 集 \(\Omega_\infty\)。" - 现象/后果：花大量精力逼近 \(\Omega_\infty\)，但 \(\Omega_\infty\) 本身不可有限表示，逼近到很高精度后边际收益极小，反而拖慢离线准备、复杂化在线约束。 - 根本原因：\(\Omega\) 的大小由 \(\mathcal{W}\) 和 \(K\) 共同决定；在 \(\mathcal{W}\)、\(K\) 给定时，\(\Omega_\infty\) 是下界，但工程上一个**略大于** \(\Omega_\infty\) 的简单多面体外逼近（如 Raković 2005 的 \(\Omega_\alpha\)）通常就足够，且约束更少、求解更快。真正想大幅减小 \(\Omega\)，应该去改 \(K\)（更强的镇定 → 更小的 \(\Omega\)）而非死磕逼近精度。 - 正确做法：用 Raković 2005 的方法取一个精度适中的外逼近；若仍嫌保守，优先重新设计 \(K\) 或升级到 Homothetic Tube（§U2.2）。

练习

1. [A 型·推导] 自己从头推一遍偏差动力学 \(e_{k+1}=(A+BK)e_k+w_k\)，并说明为什么它与名义控制 \(v_k\) 无关。再解释：若不加反馈（\(K=0\)），偏差动力学变成什么？在 \(A\) 不稳定时会发生什么？
2. [A 型·实现，累积项目 U2 起点] 对 2D 双积分器系统 \(x_{k+1}=Ax_k+Bu_k+w_k\)（\(\mathcal{W}\) 取小盒集）实现 Rigid Tube MPC：(1) 对名义系统做 LQR 得 \(K\)；(2) 用 Raković 2005 方法算 RPI 外逼近 \(\Omega\)；(3) 收紧约束 \(\mathcal{X}\ominus\Omega,\ \mathcal{U}\ominus K\Omega\)；(4) 在线解收紧的名义 MPC（OSQP）。蒙特卡洛 1000 次，验证所有实际轨迹落在管道内、不越界。
3. [思考题] 多面体约束 \(Cx\le d\) 收紧为 \(Cz\le d-h_\Omega(C^\top)\)，其中 \(h_\Omega\) 是 \(\Omega\) 的支撑函数。试解释为什么收紧量恰好是 \(\Omega\) 在约束法向上的支撑函数值，而不是 \(\Omega\) 的"半径"。

---

§U2.2 三代 Tube 的选型 ⭐⭐

动机：Rigid Tube 为什么常常过度保守

§U2.1 的 Rigid Tube 用一个**固定**的管道横截面 \(\Omega\) 走完全程。但真实系统里，"需要多大余量"是时变的——离障碍远时其实不需要那么宽的管道，离得近、机动剧烈时才需要。用一个固定的最坏情形 \(\Omega\) 走全程，等于在不需要余量的地方也背着最大余量，自然保守。三代 Tube 的演化史，本质就是一部**逐步让管道"该宽则宽、该窄则窄"以减少保守度的历史**——代价是在线计算逐步增加。

理论：三代 Tube 的参数化思路

第一代 Rigid Tube（Mayne–Seron–Raković, Automatica 41(2):219–224, 2005）：管道横截面是一个**固定**的 RPI 集 \(\Omega\)，全程不变。在线只解名义 MPC，最快、最简单，但管道"一刀切"地用最坏情形大小，最保守。（其思想前身为 Langson, Chryssochoos, Raković, Mayne, "Robust model predictive control using tubes", Automatica 40(1):125–133, 2004。）

第二代 Homothetic Tube（Raković, Kouvaritakis, Findeisen, Cannon, Automatica 48(8):1631–1638, 2012）：管道横截面是固定形状 \(\Omega\) 的**几何相似缩放** \(\alpha_k\Omega\)，其中缩放因子 \(\alpha_k\ge 0\) 随时间在线优化。这样管道可以"该粗则粗、该细则细"，在控制能力有余量时收窄管道、减少保守度。在线问题比 Rigid 多了 \(\alpha_k\) 这组标量决策变量，计算略增。

⚠️ 记号提醒：这里的缩放因子 \(\alpha_k\)（一个标量序列）与 §U2.5 里 CBF 条件中的 class-K 函数 \(\alpha(\cdot)\) 是完全不同的两个东西——前者是 Homothetic Tube 的管道缩放标量，后者是 CBF 里的一个函数。两者各自语境清晰，但请勿混淆（这也是 U0 符号约定表里特别标注 \(\alpha\) 多义的原因之一）。

第三代 Elastic Tube（Raković, Levine, Açıkmeşe, ACC 2016, pp. 3594–3599）：管道横截面是**参数化可变形的多面体**——不仅能缩放，还能改变形状（各个面方向上独立伸缩），是 Rigid 与 Homothetic 的自然推广。保守度最小，但在线问题里管道形状参数最多、QP 规模最大。

三代选型对比

维度	Rigid Tube (2005)	Homothetic Tube (2012)	Elastic Tube (2016)
管道横截面	固定 RPI 集 \(\Omega\)	相似缩放 \(\alpha_k\Omega\)	参数化可变形多面体
在线决策变量	仅 \(v_k\)	\(v_k\) + 缩放 \(\alpha_k\)	\(v_k\) + 多面体形状参数
保守度	最高	中	最低
在线计算	最低（名义 QP）	中（名义 QP + 标量）	高（QP 规模增大）
实现复杂度	最低	中	高
工程推荐	嵌入式/实时首选	控制能力有余量时升级	学术最优性研究为主

本质洞察：这是一条"用在线计算换保守度"的连续谱。三代 Tube 不是"谁取代谁"，而是同一权衡轴上的三个点——你为减少保守度付出的，恰好是在线优化更多的管道参数（从"无"到"一个标量"到"一组形状参数"）。这与 U0 §3 的总权衡（保守度 ↔ 计算量）完全一致，只是这里发生在 Tube 内部。选型的本质就是问自己：我的平台在线算力够吗？我的场景对保守度敏感吗？

实际建议

四足、无人机等**嵌入式实时**场景（控制频率几十到几百赫兹、算力受限）首选 Rigid Tube + acados——计算最快、保证最强、实现最简，工程上绝大多数情况够用。只有当你确认 Rigid Tube 的保守度明显损害了性能（如频繁绕远路、机动受限）、且平台有富余算力时，才升级到 Homothetic。Elastic Tube 在线开销大，目前更多用于学术上研究"保守度能压到多低"，实时嵌入式部署较少。

⚠️ 本节常见误区

🧠 思维陷阱：以为"更先进的 Elastic Tube 总是更好的工程选择" - 新手想法："Elastic 保守度最低，那真机就该上 Elastic。" - 现象/后果：在算力受限的嵌入式平台上，Elastic Tube 的大 QP 跑不到控制频率，实时性崩溃，反而不如 Rigid。 - 根本原因：保守度低是有代价的（在线 QP 规模大）。工程选型要在"保守度收益"和"实时性代价"之间权衡，而嵌入式平台往往实时性优先。 - 正确做法：默认 Rigid，仅在"保守度确实是瓶颈 + 算力有余"时才升级。检验方法：先用 Rigid 测性能损失有多大，再决定是否值得为减少保守度付出计算代价。

练习

1. [A 型·对比实验] 在 §U2.1 练习 2 的双积分器测试台上，把 Rigid Tube 与 Homothetic Tube（在线优化缩放 \(\alpha_k\)）对比：测两者在同一狭窄通道任务上的(1)平均求解时间、(2)能否通过通道（保守度）。定量说明"用计算换保守度"的权衡。
2. [思考题] Homothetic Tube 通过在线优化缩放因子 \(\alpha_k\) 减少保守度。从"偏差动力学"的角度解释：为什么允许管道随时间缩放，能比固定 \(\Omega\) 更贴合真实偏差的演化？（提示：真实偏差从 \(e_0\) 出发逐步被 \(K\) 镇定，初期可能小于稳态 \(\Omega\)。）

---

为什么 Homothetic 能减保守度：一个可手算的数值对比

上面的对比表说清了"是什么"，这里用实现一的标量例（§完整实现精读）把"为什么 Homothetic 更省"算成数字。Rigid 下整条管道半宽恒为稳态值 \(\Omega\approx0.162\)。但真实偏差是从 \(e_0=0\) 被扰动**逐步撑大**的——第 \(k\) 步只累积到 \(\sum_{i=0}^{k-1}(0.382)^i\times0.1\)，即 \(k=1\) 时 0.1、\(k=2\) 时 0.138、\(k=3\) 时 \(\approx 0.153\)，渐增到稳态 0.162。

关键观察：预测早期的真实偏差远小于稳态！Rigid 却在每一步都按稳态 0.162 收紧——在偏差还没长起来的前几步白白浪费了可行域。Homothetic 正是抓住这点：让缩放因子 \(\alpha_k\) 随 \(k\) 由小增大，早期收紧更少、可行域更大。对一个起点离约束较近、恰好卡在前几步可行性上的问题，这就可能是"可行 vs 不可行"的差别——这便是表里"保守度：Rigid 高 / Homothetic 中"那一格背后的实际机制。（Homothetic 的代码实现见"完整实现精读"的"实现一进阶"。）

---

§U2.3 可达性方法：FaSTrack / RTD / Funnel ⭐⭐⭐

动机：把"安全证书"从规划器里解耦出来

§U2.1 的 Tube MPC 把鲁棒性**绑进了 MPC 求解器**——约束收紧是 MPC 优化问题的一部分。但有时我们想要另一种架构：用任意一个快的规划器（A*、RRT*、甚至一个简单 MPC）做规划，而**安全保证来自一个独立的、离线算好的"误差包络"**，与具体用哪个规划器无关。这样做的好处是模块化——换规划器不影响安全证书。可达性方法就是这条路线，它的共同套路是：

离线预计算"规划器/控制器跟踪误差的最坏情形包络"，在线用这个包络膨胀障碍（或约束）。

三种代表方法的区别，只在于**用什么数学工具算这个误差包络**：FaSTrack 用 Hamilton-Jacobi（HJ）可达性，RTD 用前向可达集（Forward Reachable Set, FRS），Funnel Libraries 用平方和（Sum-of-Squares, SOS）验证的李雅普诺夫漏斗。

理论：三种方法各自怎么算误差包络

FaSTrack（Herbert, Chen, Han, Bansal, Fisac, Tomlin, IEEE CDC 2017, pp. 1517–1522；期刊扩展版 2021, arXiv:2102.07039） - 设定：一个低维**规划模型**（planning model，简单、规划快）+ 一个高维**跟踪模型**（tracking model，贴近真实）。 - 离线：对规划模型与跟踪模型的**相对动力学**（看成一场"跟踪器追、规划器+扰动逃"的追逃博弈），用 HJ 可达性求解最大**跟踪误差集 TEB**（Tracking Error Bound）——它回答"无论规划器怎么动、扰动怎么来，跟踪器与规划器的偏差最坏能有多大"。TEB 是该相对动力学 HJ 值函数的一个水平集。 - 在线：把所有障碍按 TEB 膨胀 → 任意规划器在膨胀后的空间里用简单规划模型规划 → 跟踪器保证真实轨迹落在规划轨迹的 TEB 邻域内 → 因此不碰真实障碍。 - 优势：规划器完全解耦，A*/RRT*/MPC 都能直接接入，安全证书来自 HJ 而非规划器。局限：TEB 的 grid-based HJ 计算受**维度诅咒**，直接应用一般限于约 5 维以内的相对动力学。

RTD（Reachability-based Trajectory Design；Kousik, Vaskov, Bu, Johnson-Roberson, Vasudevan, IJRR 2020, arXiv:1809.06746） - 用**前向可达集 FRS** 而非 HJ 网格。离线：对一个**轨迹产生模型**（参数化的轨迹族），用 SOS 优化计算 FRS——它同时界定了跟踪误差。在线：把障碍映射到"会导致碰撞的轨迹参数"集合，于是在线问题变成在**轨迹参数空间**里挑一个安全参数（一组有限的非线性约束，可实时求解）。 - 它特别强调 persistent feasibility（持续可行性）——通过规定最短规划时长 + 最短传感器视界来保证"新轨迹总能在旧轨迹执行完前算出来"。硬件验证在差速 Segway 与类车 Rover 上。 - 澄清一处常见误述：RTD 核心（IJRR 2020）用的是 FRS（SOS 计算）；用 zonotope 表示可达集、扩展到 7-DOF 机械臂的是其后续工作 ARMTD（Autonomous Reachability-based Manipulator Trajectory Design）。把 RTD 笼统说成"用 zonotope"是把两者混淆了。

Funnel Libraries（Majumdar, Tedrake, IJRR 36(8):947–982, 2017, arXiv:1601.04037） - 离线：沿不同机动预计算一个**漏斗（funnel）库**——每个漏斗是一个随时间演化的不变集，当执行对应机动的反馈控制器时，状态保证留在漏斗内（即使存在有界扰动）。漏斗与一个严格稳定的李雅普诺夫函数关联，用 SOS programming 计算（漏斗 \(\approx\) 李雅普诺夫水平集的时变包络）。在线：把漏斗按顺序**拼接（sequential composition）**成完整运动计划，保证全程安全。硬件验证在约 12 mph（约 5.4 m/s）的定翼机高速避障上。

三种可达性方法对比

维度	FaSTrack	RTD	Funnel Libraries
误差包络的计算工具	HJ 可达性（PDE）	前向可达集 FRS（SOS）	李雅普诺夫漏斗（SOS）
离线产物	跟踪误差集 TEB	参数空间 FRS	漏斗库
在线做什么	膨胀障碍 + 任意规划器	在轨迹参数空间选安全参数	拼接漏斗序列
维度可扩展性	受 HJ 维度诅咒（约 \(\le 5\)D）	中（SOS）；zonotope 版 ARMTD 可达 7-DOF	中（SOS）
与规划器的耦合	完全解耦	与轨迹参数化耦合	与机动库耦合
代表硬件	四旋翼等	Segway + Rover	~12 mph 定翼机

本质洞察：TEB / FRS / 漏斗 与 Tube MPC 的 RPI 集是同一类对象。它们本质上都是"真实轨迹相对名义/规划轨迹的偏差被框住的集合"——都是某种**鲁棒不变（或可达）集**。区别只在用法：Tube MPC 把这个集合绑进 MPC 在线约束收紧（§U2.1），可达性方法把它解耦出来离线算、在线膨胀障碍。这正是 §U2.1 练习里那道思考题（RPI 与 TEB 的关系）的答案——两者可以统一在"鲁棒不变/可达集"框架下，差异是工程架构而非数学本质。

⚠️ 本节常见误区

💡 概念误区：以为 FaSTrack 的 TEB 能算任意高维系统 - 新手想法："HJ 可达性很通用，那 FaSTrack 应该能处理任意维度的机器人。" - 现象/后果：试图对一个十几维的系统直接算 grid-based HJ TEB，内存/时间爆炸，根本算不出来。 - 根本原因：grid-based HJ 可达性的计算量随状态维数**指数增长**（维度诅咒），实践中直接应用一般限于约 5 维以内的相对动力学。这正是 RTD 改用 SOS/FRS、以及近年用神经网络近似 HJ 值函数（如 DeepReach 类方法）的根本动机。 - 正确做法：高维系统改用 RTD（参数空间 + SOS）、系统分解（FaSTrack 也支持分解降维）、或学习化 HJ 近似。检验方法：先估相对动力学的维数，>5D 就别指望 grid-based HJ。

🧠 思维陷阱：以为"解耦了规划器"就等于"可以用任意烂规划器" - 新手想法："FaSTrack 保证了安全，那我规划器随便写，反正撞不上。" - 现象/后果：规划器规划质量差，机器人虽然不碰障碍，但路径绕、慢、甚至在膨胀后的空间里找不到可行路。 - 根本原因：可达性方法解耦的是**安全证书**，不是**规划质量**。TEB 只保证"不碰膨胀后的障碍"，不保证轨迹优或可行；膨胀后障碍变大，差规划器可能在拥挤场景找不到路。 - 正确做法：安全交给 TEB/FRS，但规划器质量仍要单独保证（好的代价设计、足够的规划预算）。检验方法：在拥挤场景测规划成功率，而不只测碰撞率。

练习

1. [A 型·可达性工具链] 用 hj_reachability（JAX）对一个简单系统（如 Dubins 车跟踪一个移动点）计算跟踪误差集 TEB，可视化误差包络的形状。观察 TEB 如何随扰动大小变化。
2. [B 型·工具链梯度] 比较 HJ 可达性工具链的三个层次：helperOC（MATLAB，教学）→ optimized_dp（Python/HeteroCL，加速）→ hj_reachability（JAX，最现代）。跑同一个 2D 例子，对比计算时间与易用性。
3. [思考题] FaSTrack 的 TEB 和 Tube MPC 的 RPI 集 \(\Omega\) 在概念上是什么关系？两者能否统一到同一个数学框架下？（提示：都是"偏差被框住的鲁棒不变/可达集"，差异在于绑进求解器还是解耦。）

---

深入：三种可达性方法的内部机制与维度对策

§U2.3 给了三者的对比，这里补各自的内部机制要点（读论文/源码时的抓手）：

FaSTrack 的内部： - 相对动力学的状态维 \(\approx\) 跟踪模型维与规划模型维的组合，HJ 在这个相对状态上算。 - 系统分解（system decomposition）：把高维系统拆成低维子系统分别算 TEB 再组合，是 FaSTrack 应对维度诅咒的主要手段。 - 在线只需查 TEB（一个预计算集合）+ 用值函数梯度取安全控制，极快。

RTD 的内部： - 轨迹产生模型是低维参数化轨迹族（用几个参数描述一族曲线）。 - FRS 在"参数 × 状态"空间用 SOS 算，离线得到"哪些参数会导致碰到哪些点"。 - 在线把感知到的障碍点映射成"被禁止的参数子集"，优化退化成在安全参数集里挑一个——有限非线性约束、可实时。 - 机械臂版 ARMTD 用 zonotope 表示可达集，扩到 7-DOF。

Funnel Libraries 的内部： - 每个漏斗 = 一条标称机动 + 一个反馈控制器 + 一个 SOS 验证的时变不变集（漏斗沿轨迹变粗变细）。 - 漏斗库 = 一组这样的"机动-漏斗"对，覆盖不同动作。 - 在线做 sequential composition：选一串首尾相接（前一个漏斗的出口落在后一个入口）的漏斗，拼成安全计划。

共同的维度对策：grid-based HJ（FaSTrack）受维度诅咒最重 → 系统分解、或换 SOS（RTD/Funnel，复杂度随多项式次数而非网格指数增长）、或近年的神经网络近似 HJ。三者的取舍是"保证的强度 vs 可扩展的维度"。

一句话：FaSTrack 在"误差状态"上算 HJ、RTD 在"轨迹参数"上算 FRS、Funnel 在"机动轨迹"上算 SOS 漏斗——算的都是误差包络，只是选了不同的空间与工具来绕开维度诅咒。

---

§U2.4 GP-MPC：数据驱动的鲁棒控制 ⭐⭐⭐

动机：与其假设最坏扰动，不如从数据里学残差

前面的鲁棒方法（Tube、可达性）都假设你**知道**扰动集 \(\mathcal{W}\) 或模型误差的界，然后对界内最坏情况买单。但很多模型误差是**系统性的、可学习的**——四旋翼的气动残差、赛车的轮胎-路面交互、机械臂的摩擦。对这类误差，与其用一个保守的最坏情形界，不如**从飞行/行驶数据里学出这个残差**，并量化"学得有多准"，据此**自适应地**收紧约束。这就是 GP-MPC 的思路：用高斯过程（GP）学残差，把残差的**均值**喂给动力学预测、把残差的**方差**喂给机会约束。

为什么是 GP 而不是神经网络：GP 天生给出预测的不确定性（方差）——在数据多的区域方差小（信任模型），在数据少/外推的区域方差大（自动保守）。这个"何时信任、何时保守"的信号是 GP-MPC 的灵魂。神经网络也能学残差，但要额外手段才能估不确定性（MC Dropout、Deep Ensemble），这是本节末尾的思考题。

理论：GP-MPC 的标准架构（Hewing–Kabzan–Zeilinger, IEEE TCST 28(6):2736–2743, 2020）

\[ \begin{aligned} \text{名义模型（物理先验）:}\quad & x_{k+1}=f_{\text{nom}}(x_k,u_k)\\ \text{GP 残差（数据学习）:}\quad & d_k\sim\mathcal{GP}\big(\mu_d(x_k,u_k),\ \Sigma_d(x_k,u_k)\big)\\ \text{组合模型:}\quad & x_{k+1}=f_{\text{nom}}(x_k,u_k)+d_k\\ \text{均值预测:}\quad & \hat{x}_{k+1}=f_{\text{nom}}(x_k,u_k)+\mu_d(x_k,u_k)\\ \text{协方差前向传播:}\quad & \Sigma_{k+1}=g(\Sigma_k,\ \Sigma_d)\\ \text{机会约束:}\quad & \Pr\!\big(x_k\in\mathcal{X}_{\text{safe}}\big)\ge 1-\delta \end{aligned} \]

逐项含义：名义模型给物理先验，GP 学它与真实系统的差（残差 \(d_k\)）；组合后用 GP 均值 \(\mu_d\) 修正预测、用 GP 方差 \(\Sigma_d\) 量化"这块区域信不信得过"。状态分布的协方差 \(\Sigma_k\) 沿预测时域往前传播（融合了 GP 方差与上一步协方差），再用机会约束保证"真实状态落在安全集内的概率 \(\ge 1-\delta\)"。

机会约束如何变成可解的收紧约束：对线性约束 + 高斯近似，\(\Pr(c^\top x_k\le b)\ge 1-\delta\) 等价于把名义约束向内收紧 $\(c^\top\hat{x}_k\ \le\ b-\Phi^{-1}(1-\delta)\sqrt{c^\top\Sigma_k\,c},\)$ 其中 \(\Phi^{-1}\) 是标准正态的分位数函数。收紧量 \(\Phi^{-1}(1-\delta)\sqrt{c^\top\Sigma_k c}\) 由协方差 \(\Sigma_k\) 决定——协方差大（不确定性高）则收紧多、保守；协方差小则收紧少、进取。

承上启下：这一步——"协方差传播 + 机会约束 \(\Phi^{-1}(1-\delta)\) 收紧"——正是 U3（机会约束规划）的核心机制在本章的预演。GP-MPC 可以看作"残差驱动的机会约束 MPC"，是 U2 通往 U3 的天然桥梁。读到 U3 时你会发现这套协方差收紧的逻辑被系统化地展开。

关键工程选择： - 诱导点数量：完整 GP 的推断复杂度是 \(O(N^3)\)（\(N\) 为数据点数），在线根本跑不动，必须用**稀疏 GP**（诱导点 / FITC 近似），工程上常取约 20 个诱导点以保实时。 - 协方差传播近似：UT（unscented transform）/ 线性化 / moment matching，精度与计算各有取舍。 - 残差学什么：通常学"名义模型解释不了的那部分动力学"，如气动、轮胎力，而非整个动力学（保留物理先验能大幅减少数据需求）。

实验证据（已核实）

• 赛车（Kabzan, Hewing, Liniger, Zeilinger, IEEE RA-L 4(4):3363–3370, 2019）：在 AMZ Formula Student 无人方程式赛车"gotthard"（全尺寸 FS 赛车，非微缩车）上，用在线更新的 GP 学轮胎-路面等残差，GP 字典收敛后单圈时间缩短约 10%。（方法论奠基是 Hewing–Kabzan–Zeilinger 的 TCST 2020，在 20 ms 采样的 RC 赛车上验证。）
• 四旋翼（Torrente, Kaufmann, Foehn, Scaramuzza, IEEE RA-L 2021, arXiv:2102.05773）：用 GP 学气动残差并嵌入 MPC，在**速度达 14 m/s、加速度超过 4g** 的高速飞行中，相较 state-of-the-art 线性阻力模型，轨迹跟踪误差最多降低 70%。

本质洞察：GP-MPC 的精髓不是"用 GP 替换 \(f\)"，而是"用 GP 的**均值**修正 \(f\)、用 GP 的**方差**驱动保守度"。这把 U0 跨越二（模型 + 数据混合）落到了实处——学到的不确定性（方差）直接、自动地转化为约束收紧的力度：方差大 → 收紧多 → 保守，方差小 → 收紧少 → 进取。相比 Tube MPC 用一个**固定**的扰动集 \(\mathcal{W}\)，GP-MPC 用的是**状态相关、数据驱动**的不确定性——这是"让保守度由数据决定"的最优雅范例。

多视角理解：GP-MPC 站在三条线的交汇处 - 鲁棒视角：它用"学到的、状态相关的不确定性"替代了 Tube MPC 的"固定扰动集"，保守度从常数变成自适应。 - 学习视角：它是 model-based RL 的一个特例（学动力学残差 + 在模型上规划），区别是规划器是 MPC、且显式利用了不确定性。 - 机会约束视角：它是 U3 机会约束的一个"残差驱动"实例（协方差传播 + \(\Phi^{-1}\) 收紧）。

⚠️ 本节常见误区

💡 编程陷阱：在线用完整 GP，\(O(N^3)\) 拖垮实时性 - 现象/报错：随着采集的数据点增多，MPC 每步求解越来越慢，最终跑不到控制频率。 - 错误做法：把所有历史数据点都放进 GP 做完整推断。 - 正确写法：用稀疏 GP——固定一组诱导点（约 20 个），或用 FITC / 在线字典（如 Kabzan 2019 按"数据点对 GP 的信息量"自动增删字典）控制规模。 - 根本原因：完整 GP 的核矩阵求逆是 \(O(N^3)\)、预测是 \(O(N^2)\)，\(N\) 随时间无界增长则必然爆炸；稀疏近似把复杂度降到与诱导点数（常数）相关。 - 检验方法：监控单步求解时间随数据量的变化；若随时间线性/超线性增长，说明没做稀疏化。

💡 概念误区：以为 GP 方差大就一定是"数据不够，多采集就行" - 新手想法："这块区域 GP 方差大，再去多飞几趟采数据就能压下去。" - 现象/后果：采了很多数据，方差仍降不下来。 - 根本原因：GP 的预测方差混合了 epistemic（外推/数据少，可靠采集消减）和 aleatoric（系统固有噪声地板，采集消不掉）两部分（呼应 U0 §2 认知 vs 偶然）。若方差主要来自 aleatoric，多采数据无用。 - 正确做法：先判断方差主导成分；aleatoric 主导时应接受它并用机会约束容忍，而非死磕采数据。把 epistemic/aleatoric 形式化分开，本身仍是开放问题。

🧠 思维陷阱：只用 GP 均值、忽略方差 - 新手想法："GP 学了个更准的均值模型，那我把均值当新动力学做普通 NMPC 就行了。" - 现象/后果：在 GP 外推区域（方差大但均值不可靠）盲目自信，反而比纯名义 MPC 更危险。 - 根本原因：只用均值 = 丢掉了 GP-MPC 的全部鲁棒价值。GP 的方差才是"何时该保守"的信号；扔掉方差，GP-MPC 退化成一个"可能在外推区翻车"的普通 NMPC。 - 正确做法：均值和方差都用——均值进动力学，方差进机会约束。检验方法：在训练数据稀疏的状态区测试，看控制器是否表现出应有的保守。

练习

1. [A 型·GP-MPC 入门] 用 GPy 或 GPflow 学一个简单的 1D 残差（如给单摆动力学加一个未建模的非线性项），把 GP 均值嵌入 CasADi NMPC。对比有无 GP 残差的跟踪误差，并观察 GP 方差在数据稀疏处如何增大、如何自然地提供"何时信任、何时保守"的信号。
2. [B 型·协方差传播] 在上面的 GP-MPC 里实现两种协方差传播（线性化 vs UT），对比它们在预测时域末端给出的 \(\Sigma_k\) 大小与机会约束收紧量的差异。
3. [思考题] 如果用神经网络替代 GP 学残差，你如何估计 NN 的不确定性以驱动机会约束？（提示：MC Dropout、Deep Ensemble；以及 NeuroBEM 那类工作如何分解 epistemic / aleatoric。）

---

深入：GP 的核选择、多输出与超参学习

GP-MPC 用得好不好，很大程度在 GP 本身的几个工程选择： - 核函数选择：默认 RBF（平滑、各向同性）；若残差在不同维度尺度不同，用 ARD（每维一个长度尺度）；若残差有已知周期/结构，可组合周期核、线性核。核的选择编码了你对残差平滑性的先验。 - 多输出 GP：状态残差通常多维（如四旋翼三轴气动力）。最简做法是每维独立一个 GP；更精确可用多输出 GP（coregionalization）建模维度相关，但更贵。工程上独立 GP 常已够用。 - 超参学习：长度尺度、信号方差、噪声方差通过最大化边际似然学得——gp.optimize() 干的就是这件事。噪声方差项（WhiteKernel）吸收 aleatoric 噪声，长度尺度控制外推快慢（呼应 §U2.4 的 epistemic/aleatoric 陷阱）。 - 诱导点选择：稀疏 GP 的诱导点可均匀撒、按数据密度放、或在线字典动态增删（Kabzan 2019 按信息量增删）。

worked：协方差传播的数值感受。接实现二，设某状态分量名义雅可比 \(J=0.98\)（弱收缩）、GP 残差标准差 \(\sigma_d=0.05\)、\(dt=0.1\)。单步协方差增量 \((dt\,\sigma_d)^2=(0.005)^2=2.5\times10^{-5}\)。沿时域传播 \(\Sigma_{k+1}=J^2\Sigma_k+2.5\times10^{-5}\)，稳态 \(\Sigma_\infty=\frac{2.5\times10^{-5}}{1-0.98^2}\approx6.3\times10^{-4}\)，标准差 \(\approx0.025\)。代入机会约束（\(\delta=0.05\)）收紧量 \(\approx1.645\times0.025\approx0.041\)。可见**弱收缩（\(J\) 接近 1）会让不确定性累积到不可忽略**——这也是"名义模型越准（\(J\) 越收缩）、GP-MPC 收紧越小"的数字解释。

一句话：GP-MPC 的性能上限由 GP 学残差的质量决定——核选对、超参学好、诱导点够，方差才既不过度自信（外推区该大就大）也不过度保守。

---

§U2.5 CBF-QP 安全滤波器 ⭐⭐⭐

动机：在控制输出端加一道实时"护栏"

§U2.1–U2.4 都是"**事前**在规划里预留余量"（规划层）。CBF 安全滤波走的是互补的另一条路——"**事中**在控制输出端实时纠偏"（控制层）：让任意一个名义控制器（PD、MPC、甚至一个黑箱 RL 策略）先给出控制 \(u_{\text{nom}}\)，再用一个轻量 QP 在输出端**以最小修正**把它拉回安全集。这道护栏特别适合给"**不可证明安全**的控制器"兜底——比如一个仿真表现好但偶尔输出危险动作的 RL 策略（呼应"如果跳过本章会怎样"的场景二）。

反面：没有安全滤波时会怎样

一个只追求性能的名义控制器（如直奔目标的 PD）在障碍附近会毫不犹豫地朝目标方向输出控制，哪怕那会撞上障碍——因为它的目标里没有"安全"这一项。CBF 滤波器的作用，就是在 \(u_{\text{nom}}\) 即将导致越界时，在所有满足安全约束的控制里挑一个最接近 \(u_{\text{nom}}\) 的，从而既尊重名义控制器的意图、又不越界。

理论：从安全集到 CBF-QP

安全集与前向不变性。把"安全"编码为一个集合 \(\mathcal{C}=\{x:h(x)\ge 0\}\)，其中 \(h\) 连续可微（例如"离障碍至少 \(r\)"写成 \(h(x)=\|x-x_{\text{obs}}\|^2-r^2\)）。我们要保证状态**永远留在** \(\mathcal{C}\) 内（即 \(\mathcal{C}\) 前向不变）。控制屏障函数（CBF）条件**给出：存在控制 \(u\) 使 $\(\dot{h}(x,u)\ \ge\ -\alpha\big(h(x)\big),\)$ 其中 \(\alpha(\cdot)\) 是扩展类 \(\mathcal{K}_\infty\) 函数（常取线性 \(\alpha(h)=\gamma h\)，\(\gamma>0\)；必须定义在整个 \(\mathbb{R}\) 上以覆盖 \(h<0\) 的恢复行为，详见 01_数学/40_控制理论/80_CLF_CBF与QP安全控制.md）。**直觉：当 \(h\) 接近 0（贴近边界）时，约束趋于 \(\dot{h}\ge 0\)，强制 \(h\) 不再减小，把状态挡在边界内；当 \(h\) 很大（远离边界）时约束很松，几乎不干预。

⚠️ 记号提醒：此处 \(\alpha(\cdot)\) 是 CBF 的 class-K 函数，与 §U2.2 的 Homothetic Tube 缩放因子 \(\alpha_k\)、以及 U5 的 CVaR 置信水平 \(\alpha\) 都不同（见 U0 符号约定表）。

对控制仿射系统的可解形式。对 \(\dot{x}=f(x)+g(x)u\)，由链式法则 \(\dot{h}=\nabla h^\top(f+gu)=L_f h(x)+L_g h(x)\,u\)（\(L_f h,\ L_g h\) 为李导数）。CBF 条件就变成对 \(u\) 的**线性**不等式： $\(L_f h(x)+L_g h(x)\,u\ \ge\ -\alpha\big(h(x)\big).\)$ 于是**CBF-QP 安全滤波器**是一个凸二次规划： $\(u^\*=\arg\min_{u}\ \|u-u_{\text{nom}}\|^2\quad\text{s.t.}\quad L_f h(x)+L_g h(x)\,u\ge-\alpha(h(x)),\ \ u\in\mathcal{U}.\)$ 它在"最接近名义控制"的前提下强制满足安全约束，微秒级可解，适合 1 kHz+ 的高频安全滤波。

下面是单积分器（\(\dot{x}=u\)，即 \(f=0,\ g=I\)）+ 圆形障碍的最简实现，可直接作为累积项目 U2 的 CBF 模块：

import numpy as np
import cvxpy as cp

def cbf_qp_filter(x, u_nom, x_obs, r, gamma, u_min, u_max):
    """单积分器 x_dot = u 的 CBF-QP 安全滤波器。
    安全集 C = {x : h(x) = ||x - x_obs||^2 - r^2 >= 0}。
    在最接近 u_nom 的前提下，强制满足 CBF 约束，返回 u*。
    """
    n = x.shape[0]
    h = float(x @ x - 2 * x @ x_obs + x_obs @ x_obs - r**2)  # ||x-x_obs||^2 - r^2
    grad_h = 2.0 * (x - x_obs)         # ∇h
    Lf_h = 0.0                         # f = 0  -> L_f h = 0
    Lg_h = grad_h                      # g = I  -> L_g h = ∇h (1 x n)

    u = cp.Variable(n)
    constraints = [Lf_h + Lg_h @ u >= -gamma * h,   # CBF: L_f h + L_g h u >= -gamma h
                   u >= u_min, u <= u_max]
    cp.Problem(cp.Minimize(cp.sum_squares(u - u_nom)), constraints).solve()
    return u.value

高阶 CBF（相对度 > 1）。上面假设控制 \(u\) 直接出现在 \(\dot{h}\) 里（\(L_g h\not\equiv 0\)）。但若 \(h\) 只与位置有关、而控制是加速度（\(L_g h\equiv 0\)，控制要到 \(\ddot{h}\) 才出现），就需要**高阶 CBF（HOCBF）**——对 \(h\) 反复求导直到控制出现，构造一串嵌套的 CBF 条件（Xiao & Belta, IEEE TAC 67(7):3655–3662, 2022）。这就是 U2 教学目标里的"CBF 高阶相对度处理"。

多视角理解：CBF 条件 \(\dot h\ge-\alpha(h)\) 的三种看法 - 几何视角：在安全集边界 \(\{h=0\}\) 处，条件收紧为 \(\dot h\ge0\)（不许再往外），形成一道"软墙"；离边界越远（\(h\) 越大）墙越松，几乎不干预。 - 控制视角：CBF-QP 是一个"调速器（governor）"——平时放行名义控制 \(u_{\text{nom}}\)，只在状态逼近边界、\(u_{\text{nom}}\) 会越界时，以最小修正把它拉回可行。 - 类比（标边界）：像汽车的电子限速 / ABS——相似**在"只在接近极限时介入、平时不打扰"；**不同**在 CBF 给的是基于前向不变性的**形式化保证（从安全集内出发数学上保证不出界），而不是工程上的启发式干预。

三个视角分别服务"理解它挡在哪 / 它怎么改控制 / 它像什么"——配合后面"CBF 是安全版李雅普诺夫"的本质洞察，CBF 的全貌就清楚了。

CBF 与 MPC 的互补：层级协作而非替代

CBF 不是用来取代 MPC 的，两者是**层级协作**——MPC 处理 10–50 步长时域的经济/性能优化（给方向），CBF 保证每一步的瞬时安全（给护栏）。Grandia, Taylor, Ames, Hutter（IEEE ICRA 2021, pp. 8352–8358）在 ANYmal 上给出一个代表性范例：他们把 CBF 地形安全约束**同时嵌入一个低频 kinodynamic MPC 和一个高频逆动力学（WBC）跟踪控制器**，在 stepping-stones（落足点离散稀疏）场景下同时实现安全落足与动态稳定。

说明：典型腿足控制栈里，外层 MPC 频率较低（几十 Hz 量级）、内层跟踪/全身控制频率高（约 1 kHz 量级），CBF 安全约束可出现在不同频率的层上。具体频率因系统而异，并非固定数值；关键在于"长时域优化"与"高频瞬时安全"的分工，而非某一组特定赫兹数。

本质洞察：CBF 是"安全版的李雅普诺夫"。李雅普诺夫函数用 \(\dot{V}\le 0\) 证稳定（把状态吸引到平衡点），CBF 用 \(\dot{h}\ge-\alpha(h)\) 证安全（把状态约束在安全集内）。一个"拉向某点"，一个"挡在某集合内"。这个对偶是理解 CBF 的钥匙（呼应前置自测第 5 题）——你已经熟悉的稳定性论证，换个不等号方向就成了安全性论证。

本质洞察：CBF-QP 是"最小侵入式安全"。它不替代你的控制器，只在必要时以最小修正介入——平时几乎不动 \(u_{\text{nom}}\)，只在接近边界时才出手。正因如此，它能给**任意**名义控制器（包括黑箱 RL 策略）套一层形式化安全保证，是连接"经典安全"与"学习控制"的关键接口（呼应 U0 §6）。

⚠️ 本节常见误区

💡 编程陷阱：CBF-QP 在执行器受限时 infeasible - 现象/报错：QP 求解返回 infeasible，控制器卡死或输出 NaN。 - 根本原因：当 \(u\in\mathcal{U}\)（执行器能力）与 CBF 约束冲突时——即执行器的力/力矩不足以满足安全约束（如刹不住车），QP 无解。 - 正确写法：给 CBF 约束加松弛变量 \(\delta\ge 0\)（\(L_f h+L_g h\,u\ge-\alpha(h)-\delta\)，并在目标里重罚 \(\delta\)），或在设计阶段保证 CBF 的可行性（控制不变集设计）。 - 检验方法：扫描状态空间，检查在执行器极限附近是否存在 CBF 约束与 \(\mathcal{U}\) 冲突的区域。

💡 概念误区：以为 CBF 保证"从任意状态恢复安全" - 新手想法："加了 CBF，机器人就绝对安全了，哪怕已经在障碍里也能救回来。" - 现象/后果：从不安全状态（\(h(x)<0\)）启动时，CBF 不保证把状态拉回安全集，机器人可能继续违规。 - 根本原因：CBF 保证的是**前向不变性**——从安全集内出发**留在**安全集内，不保证从集外恢复到集内（那是 reach-avoid / 控制可达性问题，需要别的机制）。 - 正确做法：确保初始状态在安全集内；若需从不安全状态恢复，组合可达性方法或专门的恢复控制器。

🧠 思维陷阱：以为 class-K 增益 \(\gamma\) 越大越安全 - 新手想法："\(\gamma\) 大约束更强，应该更安全。" - 现象/后果：\(\gamma\) 调大后机器人贴着障碍飞、险象环生；\(\gamma\) 调小又过度保守、寸步难行，怎么调都不对。 - 根本原因：\(\gamma\) 不是"安全程度"旋钮，而是"多晚才开始减速"旋钮。\(\dot{h}\ge-\gamma h\) 中 \(\gamma\) 大 → 允许 \(h\) 下降更快 → 状态可更靠近边界才被拦（更激进）；\(\gamma\) 小 → 更早开始干预（更保守）。两种极端都不好。 - 正确做法：把 \(\gamma\) 理解为"刹车时机"来调——根据系统响应速度与安全裕度需求折中。检验方法：固定场景扫 \(\gamma\)，观察轨迹离边界的最近距离与任务完成度的权衡曲线。

练习

1. [A 型·实现，累积项目 U2 第二部分] 用上面的 cbf_qp_filter（或 Eigen + OSQP 版本）实现单积分器 + 圆形障碍的 CBF 安全滤波器。让名义 PD 控制器直冲障碍，验证 CBF 修正后的轨迹不碰撞；扫描 \(\gamma\)，画出"离障碍最近距离 vs \(\gamma\)"曲线。
2. [A 型·高阶 CBF] 对相对度为 2 的系统（位置安全约束、加速度作为控制）实现 HOCBF，对比它与"直接对位置用一阶 CBF"（会失败，因 \(L_g h\equiv 0\)）的区别。
3. [思考题] 为什么 CBF-QP 能给 RL 策略兜底？它和"在 RL 训练的 reward 里加安全惩罚"有什么本质区别？（提示：部署时的硬保证 vs 训练时的软引导；前者无论策略如何都不越界，后者只是倾向于安全。）

---

深入：CBF 的变体与离散时间形式

§U2.5 讲的是连续时间一阶 CBF。实战里常遇到几个变体： - 离散时间 CBF（DT-CBF）：MPC 本身是离散的，连续条件 \(\dot h\ge-\alpha(h)\) 的离散版是 \(h(x_{k+1})-h(x_k)\ge-\gamma\,h(x_k)\)（\(0<\gamma\le1\)），即 \(h(x_{k+1})\ge(1-\gamma)h(x_k)\)。把它作为约束加进 MPC 每一步，就得到"CBF 约束的 MPC"（Grandia 2021 在 MPC 层加 CBF 即此类）。注意离散 CBF 的可行性与采样周期相关，周期太大可能失去保证。 - 自适应 CBF（adaptive CBF）：当安全集本身含未知参数（如不确定的障碍大小），在线估计参数并调整 CBF。 - 鲁棒 CBF / ISSf：当动力学有扰动（\(\dot x=f+gu+w\)），\(\dot h\) 多出扰动项，需在 CBF 条件里对最坏扰动留余量（input-to-state safety）——这正是把 §U2.1 的"对扰动留余量"用到 CBF 上，与 Tube 同源。 - CBF + CLF：把控制李雅普诺夫函数（CLF，管稳定/到达）与 CBF（管安全）放进同一个 QP，安全硬约束优先、稳定尽力而为（常给 CLF 加松弛）。这是"安全 + 性能"在控制层的统一。

与 MPC 的关系：CBF 既可作 MPC **之后**的滤波层（§U2.5 主线、低耦合、给任意控制器兜底），也可作 MPC **之内**的约束（离散 CBF，Grandia 2021、规划时就考虑安全、更协调但耦合更紧）。两种用法都常见，按"要不要解耦"选。

离散 CBF 的常见坑：直接把连续 CBF 增益搬到离散用，采样周期稍大就可能违反保证——离散 CBF 的 \(\gamma\) 要结合采样周期重新整定（呼应调参速查里 CBF 增益那一行）。

---

§U2.6 acados：工业级求解器落地 ⭐⭐

动机：把鲁棒 MPC 跑到真机的控制频率上

前面的 Tube MPC（约束收紧的名义 MPC）、GP-MPC（机会约束 MPC）、CBF-QP（安全滤波）最终都要在嵌入式平台上以几十到上千赫兹**实时求解**。怎么把它们落地成能跑在真机上的求解器？工业界的默认答案之一是 acados——你在前序方向里大概率已经用过它，这里把它和鲁棒 MPC 的关系讲清楚。

工程要点：acados 怎么做到又快又灵活

acados（Verschueren, Frison, Kouzoupis, Frey, van Duijkeren, Zanelli, Novoselnik, Albin, Quirynen, Diehl, Mathematical Programming Computation 14:147–183, 2022, arXiv:1910.13753） 是一个模块化开源（BSD-2 许可）嵌入式最优控制框架：C 核心、线性代数基于高性能库 BLASFEO、QP 求解基于 HPIPM、建模兼容 CasADi、提供 Matlab/Python 接口。

• HPIPM（高性能内点法）：利用 OCP 的**块带状（block-banded）KKT 结构**高效求解。这个块结构与你在 SLAM 里见过的因子图/信息滤波稀疏结构**数学同源**——都是利用时序/链式的稀疏性把大问题拆成块。换句话说，SLAM 里的稀疏块求解直觉，在 MPC 里以 Riccati/HPIPM 的形式再现。
• SQP-RTI（实时迭代）：把 NMPC 求解拆成"准备阶段（preparation，在拿到当前状态前就算好线性化与条件化）+ 反馈阶段（feedback，拿到当前状态后只解一个 QP）"，把反馈延迟压到亚毫秒级。这是 acados 能上高频实时控制的关键。
• 约束收紧落地：Tube MPC 的 \(\mathcal{X}\ominus\Omega\)、GP-MPC 的机会约束收紧，都可直接在 acados 的 OCP 约束定义里实现——把收紧后的状态/控制边界作为约束传入即可。
• Multi-Phase OCP：acados 支持多相 OCP，可用于 scenario tree / contingency planning（与 U1 衔接）。

下面是用 acados Python 前端定义一个**带 Tube 约束收紧的鲁棒 NMPC** 的骨架（示意，省略代价与完整动力学）：

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

ocp = AcadosOcp()
model = AcadosModel(); model.name = "robust_mpc"
x = ca.SX.sym("x", nx); u = ca.SX.sym("u", nu)
model.x, model.u = x, u
model.f_expl_expr = f_nom(x, u)          # 名义动力学 z_{k+1}=f_nom
ocp.model = model
ocp.solver_options.N_horizon = N
# ... 代价 ocp.cost 设定（略）...

# Tube 约束收紧：用向内收紧 Ω 之后的状态边界 (X ⊖ Ω)
ocp.constraints.idxbx = np.arange(nx)
ocp.constraints.lbx = x_lb + omega_margin   # 下界抬高
ocp.constraints.ubx = x_ub - omega_margin   # 上界压低

# 求解器选项：SQP-RTI 准备-反馈两阶段 + HPIPM
ocp.solver_options.nlp_solver_type = "SQP_RTI"
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
solver = AcadosOcpSolver(ocp)

# 在线（每个控制周期）：把当前状态作为初值约束，解一步，取 v0，再叠加 tube 反馈 K(x-z)
# solver.set(0, "lbx", x0); solver.set(0, "ubx", x0)
# solver.solve(); v0 = solver.get(0, "u"); u0 = v0 + K @ (x_real - x0)

可微 MPC 方向（埋伏笔，详见本章前沿）：近年一个活跃方向是把 MPC 作为**可微层**嵌入神经网络、用 RL 调 MPC 的参数（起点是 Amos, Jimenez, Sacks, Boots, Kolter, "Differentiable MPC for End-to-end Planning and Control", NeurIPS 2018）。这把"MPC 执行 + RL 调参"融合，是 §U2 前沿会展开的一条线。

本质洞察：acados 的设计哲学是"模块化 + 不牺牲性能"——通过模块化（NLP solver / integrator / QP solver 可互换）获得灵活性，通过 C 核心 + BLASFEO 保持性能，且**刻意不依赖自动代码生成**（便于维护与扩展）。对工程师的意义：你不必为每个新问题手写求解器，而是把不同算法组件像积木一样拼起来——这也是为什么它适合作为鲁棒 MPC 各种变体的统一落地平台。

⚠️ 本节常见误区

💡 编程陷阱：把 acados 当黑箱，误用 RTI 的准备-反馈拆分 - 现象/后果：本想用 SQP-RTI 压低延迟，结果反馈延迟仍很大、控制频率上不去。 - 根本原因：没理解 RTI 的"准备阶段做重活、反馈阶段只解一个 QP"的拆分——若把本该在准备阶段做的线性化/条件化放到反馈阶段，延迟优势就没了。 - 正确做法：理解并正确使用 phase（preparation / feedback）接口，让重计算在等待新状态时就完成。

💡 概念误区：以为 acados 靠自动代码生成 - 新手想法："嵌入式 MPC 求解器都靠 codegen 吧。" - 现象/后果：误判 acados 的工作方式，找不到 codegen 配置而困惑。 - 根本原因：acados 恰恰强调**不**依赖自动代码生成，而是靠模块化 + 预编译高性能 C 库（BLASFEO/HPIPM）获得性能与可维护性。

练习

1. [A 型·acados 鲁棒 NMPC] 用 acados Python 前端为四旋翼（或 Dubins 车）定义 NMPC，加入推力/角速度约束。对比"有无 Tube 风格约束收紧（margin）"两种设定在受扰动下的跟踪性能与越界率。
2. [A 型·RTI 计时] 在 acados 里实现一个 SQP-RTI 的 NMPC，分别测量准备阶段与反馈阶段的耗时，验证反馈延迟确实被压到亚毫秒级。

---

理论补遗：Min-max、递归可行性、分布鲁棒

六个方法讲完，这里补三块"知道了会更通透"的理论，它们解释了 Tube/鲁棒 MPC 为什么长成现在这样。

Min-max MPC：Tube 的前身

在 Tube MPC 之前，鲁棒 MPC 最直接的做法是 min-max（Scokaert & Mayne, IEEE TAC 43(8):1136–1142, 1998）： $\(\min_{\text{控制策略}}\ \max_{w\in\mathcal{W}}\ J(x,u,w),\)$ 即对扰动的**最坏情况**优化。问题是：要对扰动在整个时域上的所有可能实现（一棵指数增长的扰动树）做反馈优化，计算量随时域**指数爆炸**，不实用。

Tube MPC 的关键洞察正是把它**可计算化**：与其"对最坏扰动在线优化"，不如"对名义系统优化 + 用约束收紧预留最坏偏差的余量"（§U2.1）。RPI 集 \(\Omega\) 一次性、离线地刻画了"最坏偏差能有多大"，于是在线只需解一个名义 MPC（多项式复杂度）。所以 Tube 不是凭空冒出来的，而是 min-max 的可计算化身——理解这一点，你就明白 §U2.1 那套"分解 + 收紧"为什么是进步。

递归可行性与稳定性

鲁棒 MPC 不只要"这一步可行"，更要"每一步都可行"——这叫**递归可行性（recursive feasibility）**。否则中途某步解不出来，控制器就崩了。

• 怎么保证：Tube MPC 用约束收紧 + **终端集（terminal set）**保证递归可行——让名义轨迹的终端落在一个**鲁棒控制不变集**里，这样下一时刻总能把上一时刻的解"续上一步"（标准的 shifting 论证）。
• 稳定性：在合适的终端代价 / 终端集下，名义系统是 ISS（input-to-state stable）的，而真实状态被困在名义轨迹的 tube（\(\Omega\) 邻域）内，于是整体闭环稳定、且最终被界定在 \(\Omega\) 附近。
• 工程含义：别只测单步能不能解出来，要测**长时域闭环不崩**；终端集 / 终端代价的设计（而非只调代价权重）才是递归可行性的关键。许多"仿真好好的、跑久了发散"的 bug 根源就在这里。

分布鲁棒优化（DRO）一瞥

U0 提过鲁棒 / 随机 / DRO 的三分。分布鲁棒优化**是中间路线： - **随机 MPC 假设知道精确的扰动分布（常是高斯）——但你往往不知道。 - 鲁棒 MPC 只用支撑集 \(\mathcal{W}\) 的最坏情况——但这可能太保守（最坏情况罕见）。 - DRO 取折中：对"一个**分布族（ambiguity set）**内的最坏分布"优化。

常用的 ambiguity set 有两类：矩约束（只假设已知均值/协方差，不假设分布形状）、Wasserstein 球（离经验分布一定 Wasserstein 距离内的所有分布）。

与本章的关系：DRO 正是 GP-MPC（用学到的分布）与 Tube（用支撑集）之间的桥——当你对分布"半知半解"（知道一些矩、但不确定形状）时，DRO 比两者都合适，给出"分布最坏情况"的概率保证而非"支撑集最坏情况"的硬保证。这是 U5（风险敏感 / CVaR）与前沿的活跃方向，CVaR 本身就可写成一类 DRO。

---

完整实现精读：从公式到可运行代码

累积项目给的是"五范式集成测试台"的简化骨架；这一节反过来，对本章最核心的两个方法——Rigid Tube MPC 与 GP-MPC——给出更严谨、更完整的独立实现，把 §U2.1 与 §U2.4 的公式逐行落到代码。读完你应当能从零写出这两个控制器，而不只是调库。

实现一：完整 Rigid Tube MPC（显式 ε-mRPI + 激活反馈 \(K\)）

相比累积项目里"令 \(z_0=x_0\)"的简化，这里做两件更完整的事：(1) 用 Raković et al.（2005）的 ε-mRPI 外逼近**显式计算 RPI 集 \(\Omega_\alpha\)（以支撑函数表示），(2) 把 \(z_0\) 设为**决策变量（约束 \(x_0-z_0\in\Omega\)），从而真正激活反馈 \(K\)——这才是 §U2.1 第 7 步描述的完整 Tube MPC。

import numpy as np, cvxpy as cp
from scipy.linalg import solve_discrete_are

# ── 离线 1: LQR 镇定增益 K（使 A+BK 稳定）──
def lqr_gain(A, B, Q, R):
    P = solve_discrete_are(A, B, Q, R)
    return -np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)

# ── 离线 2: ε-mRPI 外逼近 Ω_α（Raković et al. 2005），以支撑函数返回 ──
def epsilon_mrpi_support(Ak, w_half, alpha=0.05, s_max=200):
    """对称盒扰动 W = {w : |w_j| <= w_half_j}。
    选最小 s 使 Ak^s W ⊆ α W，则 Ω_α = (1-α)^{-1} ⊕_{i=0}^{s-1} Ak^i W ⊇ Ω_∞。
    返回 (supp, s)，其中 supp(c) = h_{Ω_α}(c) 是 Ω_α 在方向 c 上的支撑函数。"""
    n = Ak.shape[0]
    hW = lambda c: w_half @ np.abs(c)                  # 盒 W 的支撑函数 h_W(c)=Σ_j w_half_j|c_j|
    # 1) 找最小 s：在 W 的面法向 ±e_j 上验证 h_{Ak^s W}(e) <= α·h_W(e)
    s, M, dirs = 0, np.eye(n), np.vstack([np.eye(n), -np.eye(n)])
    while s < s_max:
        s, M = s + 1, Ak @ M
        if max(hW(M.T @ e) / hW(e) for e in dirs) <= alpha:
            break
    # 2) Ω_α 的支撑函数：截断 Minkowski 和的支撑，再乘 (1-α)^{-1} 得外逼近
    def supp(c):
        tot, Mi = 0.0, np.eye(n)
        for _ in range(s):
            tot, Mi = tot + hW(Mi.T @ c), Ak @ Mi
        return tot / (1.0 - alpha)
    return supp, s

# ── 在线: 完整 Tube MPC（z0 为决策变量 ⇒ 激活反馈 K）──
def tube_mpc(A, B, x0, goal, K, supp, N, x_half, u_half, Q, R):
    nx, nu = B.shape
    # 用盒外逼近 Ω：状态方向 e_j 的支撑、KΩ 在控制方向 e_i 的支撑
    s_state = np.array([supp(e) for e in np.eye(nx)])        # h_Ω(e_j)
    s_ctrl  = np.array([supp(K.T @ e) for e in np.eye(nu)])  # h_{KΩ}(e_i)=h_Ω(K^T e_i)
    z, v, z0 = cp.Variable((nx, N+1)), cp.Variable((nu, N)), cp.Variable(nx)
    xg, cost = np.concatenate([goal, np.zeros(nx-2)]), 0
    cons = [z[:, 0] == z0, cp.abs(x0 - z0) <= s_state]       # x0 - z0 ∈ Ω（盒外逼近）
    for k in range(N):
        cost += cp.quad_form(z[:, k]-xg, Q) + cp.quad_form(v[:, k], R)
        cons += [z[:, k+1] == A @ z[:, k] + B @ v[:, k]]
        cons += [cp.abs(z[:, k]) <= x_half - s_state]        # 状态约束收紧 X ⊖ Ω
        cons += [cp.abs(v[:, k]) <= u_half - s_ctrl]         # 控制约束收紧 U ⊖ KΩ
    cost += cp.quad_form(z[:, N]-xg, 20*Q)
    cp.Problem(cp.Minimize(cost), cons).solve(solver=cp.OSQP, warm_start=True)
    u0 = v[:, 0].value + K @ (x0 - z0.value)                 # 真实控制 = 名义 v0 + 反馈 K(x0-z0)
    return u0

逐块对应 §U2.1 的公式：lqr_gain 给镇定增益 \(K\)（第 2 步）；epsilon_mrpi_support 算 RPI 集 \(\Omega_\alpha\)（第 4–5 步，Raković 2005）；tube_mpc 里 cp.abs(z[:,k]) <= x_half - s_state 是约束收紧 \(\mathcal{X}\ominus\Omega\)（第 6 步），u0 = v0 + K@(x0-z0) 是 tube 控制律（第 2 步），而把 z0 设为决策变量并约束 x0-z0 ∈ Ω，正是"初始状态作决策变量"激活反馈 \(K\) 的关键——此时 \(x_0-z_0\) 不再恒为 0，反馈项真正起作用。

实现要点：为什么用盒外逼近 \(\Omega\)。严格的 \(\Omega_\alpha\) 是多面体，约束应在其所有面法向上写。这里用坐标方向 \(\pm e_j\) 得到 \(\Omega\) 的**盒外逼近**（更保守但实现最简、约束最少）。若要更紧，可在更多方向（如对角方向）上取支撑，得到更贴合的多面体外逼近——这是"保守度 vs 约束数"的又一次权衡，与 §U2.2 三代 Tube 的精神一致。

实现一的数值走查：一个可手算验证的标量例子

为了让上面的代码不只是黑箱，用一个**能纯手算**的标量例子走一遍完整流程。系统 \(x_{k+1}=x_k+u_k+w_k\)（\(A=B=1\)），扰动 \(|w|\le0.1\)，约束 \(|x|\le1\)、\(|u|\le0.5\)，取 \(Q=R=1\)。

1. LQR 增益：离散 Riccati \(P=Q+A^2P-\frac{(APB)^2}{R+B^2P}=1+P-\frac{P^2}{1+P}\)，化简得 \(P^2-P-1=0\)，故 \(P=\frac{1+\sqrt5}{2}\approx1.618\)（恰是黄金比）。增益 \(K=-\frac{BP}{R+B^2P}=-\frac{P}{1+P}\approx-0.618\)，闭环 \(A+BK\approx0.382\)。
2. RPI 集：标量下 mRPI 可精确求和——\(\Omega=\bigoplus_{i\ge0}(0.382)^i[-0.1,0.1]=\Big[-\tfrac{0.1}{1-0.382},\ \tfrac{0.1}{1-0.382}\Big]\approx[-0.162,\ 0.162]\)（等比级数）。
3. 约束收紧：状态 \(z\in\mathcal{X}\ominus\Omega=[-0.838,0.838]\)；控制 \(v\in\mathcal{U}\ominus K\Omega=[-0.4,0.4]\)（因 \(|K|\cdot0.162\approx0.1\)）。
4. 解读：约 **16% 的状态范围**与 **20% 的控制权限**被划给了鲁棒裕度——这就是为应对 \(|w|\le0.1\) 的扰动所付的"保守度代价"。扰动减半则 \(\Omega\) 减半、裕度减半；用更强的 \(K\)（更小的 \(A+BK\)）则等比级数收敛更快、\(\Omega\) 更小。这把 §U2.1 的"保守度由 \(\mathcal{W}\) 与 \(K\) 共同决定"变成了可触摸的数字。

把 tube_mpc 的 \(A,B,\mathcal{W}\) 换成这个标量系统，epsilon_mrpi_support 在 \(e_1\) 方向算出的支撑应当 ≈0.162，与上面手算一致——这是验证你实现是否正确的最简单 sanity check。

实现一的进阶：Homothetic Tube MPC（在线缩放 \(\alpha_k\)）

§U2.2 的 worked 对比说 Homothetic 让管道横截面 \(\Omega_k=\alpha_k\bar\Omega\) 在线缩放。把实现一的 Rigid 版升级成 Homothetic，只需改三处：(1) 引入决策变量 \(\alpha_k\ge0\)；(2) 收紧量从常数 \(s\) 变成 \(\alpha_k s\)；(3) 加 \(\alpha\) 的包含递推约束。

import numpy as np, cvxpy as cp

def homothetic_tube_mpc(A, B, x0, goal, K, supp, N, x_half, u_half, Q, R,
                        lam, mu, a0):
    """Homothetic Tube MPC：Ω_k = α_k·Ω̄ 在线缩放（α_k 为决策变量）。
    与 Rigid 版相比只改三处（见下方注释）。lam, mu, a0 需按 Ω̄、W 标定。"""
    nx, nu = B.shape
    s_state = np.array([supp(e) for e in np.eye(nx)])         # Ω̄ 在状态方向的支撑
    s_ctrl  = np.array([supp(K.T @ e) for e in np.eye(nu)])   # KΩ̄ 在控制方向的支撑
    z, v = cp.Variable((nx, N+1)), cp.Variable((nu, N))
    a = cp.Variable(N+1, nonneg=True)                         # 改动①: 缩放因子 α_k>=0
    xg, cost = np.concatenate([goal, np.zeros(nx-2)]), 0
    cons = [z[:, 0] == x0, a[0] == a0]                        # 初始管道大小 α_0
    for k in range(N):
        cost += cp.quad_form(z[:, k]-xg, Q) + cp.quad_form(v[:, k], R) + 1e-2*a[k]
        cons += [z[:, k+1] == A @ z[:, k] + B @ v[:, k]]
        cons += [a[k+1] >= lam*a[k] + mu]                     # 改动③: α 包含递推(示意)
        cons += [cp.abs(z[:, k]) <= x_half - a[k]*s_state]    # 改动②: 收紧量随 α_k 缩放
        cons += [cp.abs(v[:, k]) <= u_half - a[k]*s_ctrl]
    cost += cp.quad_form(z[:, N]-xg, 20*Q)
    cp.Problem(cp.Minimize(cost), cons).solve(solver=cp.OSQP, warm_start=True)
    return v[:, 0].value + K @ (x0 - z[:, 0].value)

三处改动的含义：①把缩放因子 \(\alpha_k\) 设为决策变量；②收紧量 a[k]*s_state 随 \(\alpha_k\) 变化（Rigid 是常数 s_state），于是早期 \(\alpha_k\) 小处收紧更少；③a[k+1] >= lam*a[k] + mu 是包含条件 \((A+BK)(\alpha_k\bar\Omega)\oplus\mathcal{W}\subseteq\alpha_{k+1}\bar\Omega\) 的标量形式（\(\lambda\) 与 \(\rho(A+BK)\) 相关、\(\mu\) 与 \(\mathcal{W}\) 相关）。目标里 1e-2*a[k] 轻罚管道大小、鼓励早期用小 \(\alpha\)。代价：多了 \(N+1\) 个标量变量 + \(N\) 个递推约束，在线 QP 比 Rigid 略大——这正是 §U2.2 "保守度↓、计算量↑"的代码级体现。

注：lam、mu、a0 需按具体 \(\bar\Omega\) 与 \(\mathcal{W}\) 标定，此处为结构示意；严格的 Homothetic 包含条件与可行性见 Raković et al. (2012)。Elastic Tube 进一步把单个 \(\alpha_k\) 换成多参数/各向异性缩放，QP 更大、保守度更低。

实现二：GP-MPC worked example（GP 学残差 + 方差驱动机会约束）

下面用一个 1D 例子把 §U2.4 的"均值修正动力学、方差驱动约束收紧"跑通。真系统含一个名义模型不知道的非线性残差 \(d(x)\)；名义 MPC 会因为系统性误判而违反状态上界，GP-MPC 则用 GP 均值修正预测、用 GP 方差经 \(\Phi^{-1}(1-\delta)\) 收紧约束。

import numpy as np, cvxpy as cp
from sklearn.gaussian_process import GaussianProcessRegressor
from sklearn.gaussian_process.kernels import RBF, WhiteKernel
from scipy.stats import norm

dt = 0.1
def true_step(x, u, rng):                       # 真系统: 含未建模残差 d(x)=0.6 sin(1.5x)
    return x + dt*(u + 0.6*np.sin(1.5*x)) + 0.01*rng.standard_normal()
def nom_step(x, u):                             # 名义模型: 忽略残差
    return x + dt*u

# 1) 采数据并用 GP 学残差 d(x) = (x_next - nom_step) / dt
rng = np.random.default_rng(0)
X = rng.uniform(-3, 3, size=(60, 1)); U = rng.uniform(-2, 2, size=60)
D = np.array([(true_step(X[i, 0], U[i], rng) - nom_step(X[i, 0], U[i])) / dt
              for i in range(60)])
gp = GaussianProcessRegressor(kernel=RBF(1.0) + WhiteKernel(1e-3),
                              normalize_y=True).fit(X, D)

# 2) GP-MPC 单步：均值修正动力学，方差→机会约束收紧 Pr(x_next <= x_max) >= 1-δ
def gp_mpc_step(x, x_max, delta=0.05, u_max=2.0):
    mu, std = gp.predict(np.array([[x]]), return_std=True)   # 残差均值 μ_d、标准差 σ_d
    u = cp.Variable()
    x_next_mean = x + dt*(u + mu[0])                         # 用 GP 均值修正预测
    tighten = norm.ppf(1 - delta) * dt * std[0]             # Φ^{-1}(1-δ)·dt·σ_d
    cons = [x_next_mean <= x_max - tighten, cp.abs(u) <= u_max]
    cp.Problem(cp.Minimize(cp.square(u + mu[0])), cons).solve()  # 例: 抵消残差的最小控制
    return float(u.value), float(std[0])

# 对比: 名义 MPC（忽略 μ_d、σ_d）会系统性误判 x_next，更易越过 x_max；
# GP-MPC 用 μ_d 修正、用 σ_d 在数据稀疏处自动多留裕度（tighten 变大）。

逐块对应 §U2.4：gp.predict(..., return_std=True) 给出残差的均值 \(\mu_d\) 与标准差 \(\sigma_d\)；x_next_mean = x + dt*(u + mu) 是"用 GP 均值修正动力学"；tighten = Φ^{-1}(1-δ)·dt·σ_d 是机会约束的收紧量（\(\sigma_d\) 大 → 收紧多 → 保守，正是"方差驱动保守度"）。在 \(x\in[-3,3]\) 之外（训练数据稀疏区）GP 的 \(\sigma_d\) 会增大，tighten 随之增大——这就是 §U2.4 强调的"数据稀疏处自动保守"。

从单步到多步：上面是单步机会约束（最清楚）。完整 GP-MPC 要在预测时域上**前向传播协方差** \(\Sigma_{k+1}=g(\Sigma_k,\Sigma_d)\)（线性化 / UT / moment matching），每步用 \(\Sigma_k\) 收紧——这正是 §U2.4 架构式里那一行，也是 §U2.4 练习 2 的内容。

下面把单步扩成**多步**：沿预测时域线性化传播协方差 \(\Sigma_{k+1}=J_k^2\,\Sigma_k+dt^2\sigma_d^2(x_k)\)（\(J_k\) 为组合动力学对 \(x\) 的雅可比），每步用 \(\Sigma_k\) 收紧。

def gp_mpc_multistep(x0, x_max, N=15, delta=0.05, u_max=2.0, du=0.05):
    """多步 GP-MPC：沿时域传播方差 + 逐步机会约束（教学版用前向模拟 + 一维搜索）。
    工程版应在 cvxpy/acados 里把整条控制序列联合优化，而非网格搜索。"""
    from scipy.stats import norm
    z = norm.ppf(1 - delta)
    def rollout(u_seq):
        x, Sig = x0, 0.0
        for u in u_seq:
            mu, std = gp.predict(np.array([[x]]), return_std=True)
            dmu = (gp.predict([[x+1e-3]])[0] - gp.predict([[x-1e-3]])[0]) / 2e-3
            J   = 1.0 + dt * float(dmu)                   # 雅可比 ∂/∂x[x+dt(u+d(x))]=1+dt d'(x)
            Sig = J*J*Sig + (dt*float(std[0]))**2         # 协方差前向传播 Σ_{k+1}
            x   = x + dt*(u + float(mu[0]))               # 均值预测
            if x + z*np.sqrt(Sig) > x_max:                # 机会约束 Pr(x<=x_max)>=1-δ
                return None                               # 该序列在某步违反，剔除
        return x
    best, best_u = -np.inf, 0.0
    for u0 in np.arange(-u_max, u_max + du, du):          # 一维搜索(教学简化)
        xN = rollout([u0]*N)
        if xN is not None and xN > best:
            best, best_u = xN, u0
    return best_u

关键是 Sig = J*J*Sig + (dt*std)**2 这一行：不确定性沿时域**累积**，越往后 \(\Sigma_k\) 越大、机会约束收紧越多——这正是 §U2.4 架构式里"协方差前向传播 \(\Sigma_{k+1}=g(\Sigma_k,\Sigma_d)\)"那一行的落地。这里用线性化（雅可比平方）传播，是 UT / moment matching 的最简版本。

实现三：HOCBF 的完整推导与 worked example（双积分器避障）

§U2.5 指出相对度 >1 时要用 HOCBF，但没展开。这里把累积项目里 hocbf_filter 用的那条约束**从头推一遍**，并手算一个具体时刻。

系统：2D 双积分器 \(\dot p=v,\ \dot v=u\)。安全约束：离圆障碍 \(p_o\)（半径 \(r\)）至少 \(r\)，即 \(h(p)=\|p-p_o\|^2-r^2\ge0\)。控制 \(u\) 不出现在 \(\dot h\) 里（相对度 2），一阶 CBF 失效。

HOCBF 构造（取线性 class-K，\(\alpha_1,\alpha_2>0\) 为常数）： $\(\psi_0=h,\qquad \psi_1=\dot\psi_0+\alpha_1\psi_0=\dot h+\alpha_1 h,\qquad \text{要求}\quad \dot\psi_1+\alpha_2\psi_1\ge0.\)$

逐阶求导（用 \(\dot p=v,\ \dot v=u\)）： $\(\dot h=2(p-p_o)^\top v,\qquad \ddot h=2\,v^\top v+2(p-p_o)^\top u.\)$

代入 \(\dot\psi_1+\alpha_2\psi_1=\ddot h+\alpha_1\dot h+\alpha_2(\dot h+\alpha_1 h)\ge0\)，整理成**对 \(u\) 线性**的约束： $\(\underbrace{2\,v^\top v}_{\ddot h\text{ 无 }u\text{ 项}}+\underbrace{2(p-p_o)^\top u}_{\ddot h\text{ 的 }u\text{ 项}}+(\alpha_1+\alpha_2)\underbrace{2(p-p_o)^\top v}_{\dot h}+\alpha_1\alpha_2\underbrace{(\|p-p_o\|^2-r^2)}_{h}\ \ge\ 0.\)$

这正是累积项目 hocbf_filter 里的那一行。HOCBF-QP 即 \(\min_u\|u-u_{\text{nom}}\|^2\) s.t. 上式 \(+\ u\in\mathcal{U}\)。

worked 走查（纯手算）：设 \(p=(1,0),\ p_o=(2,0),\ r=0.5,\ v=(1,0)\)（正朝障碍冲），\(\alpha_1=\alpha_2=4\)。 - \(h=\|(-1,0)\|^2-0.25=0.75\)；\(\dot h=2(-1,0)\cdot(1,0)=-2\)（负值 ⇒ 正在接近）。 - 约束：\(\big(2\cdot1+2(-1,0)\cdot u\big)+8\cdot(-2)+16\cdot0.75\ge0\) → \(2-2u_x-16+12\ge0\) → \(-2u_x-2\ge0\) → \(u_x\le-1\)。 - 解读：HOCBF 强制 \(u_x\le-1\)，即必须在 \(x\) 方向以至少 1 的速率减速。若名义控制 \(u_{\text{nom}}=(2,0)\)（朝障碍加速），HOCBF-QP 会否决它、改成最接近的可行解 \(u_x=-1\)（刹车）。这就是相对度 2 系统上"安全层在最后一刻接管"的具体数字。

为什么一阶 CBF 在这里失效：一阶 CBF 要求 \(\dot h\ge-\alpha(h)\)，但 \(\dot h=2(p-p_o)^\top v\) 里**没有 \(u\)**——你无法通过选 \(u\) 直接影响 \(\dot h\)。HOCBF 多求一阶导，让 \(u\) 在 \(\ddot h\) 中现身，约束才对 \(u\) 可解。\(\alpha_1,\alpha_2\) 越大，允许越晚才开始减速（呼应 §U2.5 的 class-K 增益陷阱）。

实现四：FaSTrack 风格 TEB 的最小走查（可达性）

§U2.3 说 FaSTrack 离线算"跟踪误差集 TEB"，这里用最简设置把它怎么来讲清。 - 规划模型（快、简单，规划器用）：单积分器 \(\dot p_{\text{plan}}=u_{\text{plan}}\)，\(\|u_{\text{plan}}\|\le\bar v\)。 - 跟踪模型（贴近真实）：双积分器 \(\dot p=v,\ \dot v=a\)，\(\|a\|\le\bar a\)。 - 相对动力学：令相对状态 \(e=(p-p_{\text{plan}},\,v)\)。把规划器指令 \(u_{\text{plan}}\) 看成**对抗性扰动**、跟踪器 \(a\) 看成**镇定输入**，构成一场追逃博弈 \(\min_a\max_{u_{\text{plan}}}\)。 - TEB：该相对动力学 HJ 值函数 \(V(e)\) 的一个水平集 \(\{e:V(e)\le0\}\) 的外包络——即"跟踪器在最坏规划指令下，相对误差最终被困住的范围"。 - 在线：障碍按 TEB 膨胀 → 规划器在膨胀空间用单积分器快速规划 → 跟踪器保证真实 \(p\) 落在 \(p_{\text{plan}}\) 的 TEB 邻域内 → 不碰真实障碍。

# 概念骨架（API 以 hj_reachability 官方文档为准，此处仅示意流程）
# import hj_reachability as hj
# 1) 定义相对动力学 e_dot = f_rel(e, a_tracker, u_planner)   # 追逃：a 镇定, u_planner 对抗
# 2) 在状态网格上求解 HJ PDE, 得值函数 V(e)（收敛到稳态 BRT）
# 3) TEB = {e : V(e) <= 0} 的外包络 → 在线用于膨胀障碍

走查要点：TEB 的大小由"跟踪器能力 \(\bar a\)"与"规划器激进度 \(\bar v\)"的博弈决定——跟踪器越强、规划器越温和，TEB 越小。这与 Tube MPC 的 \(\Omega\) 由 \(K\) 与 \(\mathcal{W}\) 决定**完全对应**（§U2.1 本质洞察）；差别只是 TEB 用 HJ 在线膨胀障碍、\(\Omega\) 用收紧绑进 MPC。**维度诅咒**就出在第 2 步：网格 HJ 在相对状态维数 >~5 时不可行（§U2.3 误区），这也是近年用神经网络近似 HJ 的动机。

实现五：CBF-QP 的标量 worked example（单积分器，可手算）

把 §U2.5 的 cbf_qp_filter 退到一维，手算一遍最清楚。系统 \(\dot x=u\)（单积分器），安全约束"别越过墙 \(x\le x_{\max}\)"编码为 \(h(x)=x_{\max}-x\ge0\)。

• CBF 条件：\(\dot h\ge-\gamma h\)。因 \(\dot h=-\dot x=-u\)，得 \(-u\ge-\gamma(x_{\max}-x)\)，即 \(u\le\gamma(x_{\max}-x)\)。
• CBF-QP：\(\min_u(u-u_{\text{nom}})^2\) s.t. \(u\le\gamma(x_{\max}-x)\)。
• worked（手算）：取 \(x_{\max}=1,\ x=0.9,\ \gamma=2,\ u_{\text{nom}}=1\)（名义想以速度 1 冲向墙）。约束 \(u\le2(1-0.9)=0.2\)。\(u_{\text{nom}}=1>0.2\) 违反，QP 取最近可行解 \(u^\*=0.2\)（减速）。当 \(x\to1\)（贴墙），约束 \(u\le2(1-x)\to0\)，强制停下。
• 解读：CBF 把"撞墙速度 1"在最后 0.1 距离内平滑刹到 0。\(\gamma\) 大 → 允许更晚刹车（约束在远处更松）；\(\gamma\) 小 → 更早减速、更保守——这是 §U2.5 "class-K 增益是刹车时机旋钮"陷阱的数字版。

三个尺度同一回事：这个一维手算 → 推到 2D 圆障碍就是累积项目的 cbf_qp_filter → 推到相对度 2（位置约束、加速度控制）就是实现三的 HOCBF。CBF 的骨架不变，变的只是 \(h\) 的形式与相对度。

推导补遗：机会约束的 \(\Phi^{-1}(1-\delta)\) 收紧从何而来

§U2.4 与实现二都用了 \(\Phi^{-1}(1-\delta)\) 收紧却没推。这里补上（高斯情形），它是 GP-MPC、本章实现二/多步、以及 **U3 整章**的共同基石。

设状态 \(x\sim\mathcal{N}(\mu,\Sigma)\)，要满足线性机会约束 \(\Pr(c^\top x\le b)\ge1-\delta\)： 1. \(c^\top x\) 仍是高斯：\(c^\top x\sim\mathcal{N}(c^\top\mu,\ c^\top\Sigma c)\)。记标准差 \(s=\sqrt{c^\top\Sigma c}\)。 2. 标准化：\(\Pr\!\Big(\frac{c^\top x-c^\top\mu}{s}\le\frac{b-c^\top\mu}{s}\Big)\ge1-\delta\)，即 \(\Phi\!\Big(\frac{b-c^\top\mu}{s}\Big)\ge1-\delta\)。 3. 两边取单调的 \(\Phi^{-1}\)：\(\frac{b-c^\top\mu}{s}\ge\Phi^{-1}(1-\delta)\)，整理为**确定性收紧约束** $\(c^\top\mu\ \le\ b-\Phi^{-1}(1-\delta)\,\sqrt{c^\top\Sigma c}.\)$

解读：原约束的均值版 \(c^\top\mu\le b\) 被向内收紧了 \(\Phi^{-1}(1-\delta)\sqrt{c^\top\Sigma c}\)。\(\delta\) 越小（要求越严）→ \(\Phi^{-1}(1-\delta)\) 越大 → 收紧越多；\(\Sigma\) 越大（越不确定）→ 收紧越多。常用值：\(\delta=0.05\) 时 \(\Phi^{-1}(0.95)\approx1.645\)，\(\delta=0.01\) 时 \(\approx2.326\)。

依赖高斯假设：上式只在 \(x\)（近似）高斯时成立。非高斯时要么换更保守的集中不等式（Chebyshev / Cantelli，无需分布形状但更松），要么用采样/场景法——这是 U3 会系统展开的。GP-MPC 里之所以能用，是因为对状态分布做了高斯近似（协方差传播那一步）。

源码精读：OCS2 / legged_control 的 MPC→WBC 数据流

前面的实现都是单文件玩具；真实的腿足控制栈长什么样？以 legged_control（Qiayuan Liao，基于 OCS2 + ros-control 的 NMPC-WBC 框架，被广泛认为是性能最好的开源 model-based 腿足 MPC 栈之一）为例做一次**架构级**精读——不逐行抠代码，而是看数据如何在 MPC 与 WBC 之间流动，把 §U2.5（CBF-MPC 互补）与 §U2.6（OCP 落地）落到一个真实仓库上。

NMPC 求解的 OCP（经 OCS2 接口）。每个控制周期，NMPC 通过 OCS2 求解： $\(\min_{u(\cdot)}\ \phi(x(t_I))+\int_{t_0}^{t_I}\!l(x,u,t)\,dt\ \ \text{s.t.}\ \ x(t_0)=x_0,\ \dot x=f(x,u,t),\ g_1(x,u,t)=0,\ g_2(x,t)=0,\ h(x,u,t)\ge0,\)$ 其中 \(\phi\) 终端代价、\(l\) 阶段代价、\(f\) 系统流图、\(g_1\) 状态-输入等式约束、\(g_2\) 纯状态等式约束、\(h\) 不等式约束。OCS2 提供 SLQ/DDP/SQP 等求解器，动力学用质心/kinodynamic 模型，依赖 pinocchio（刚体动力学）与 hpp-fcl（碰撞）。

数据流（自底向上三块）： 1. 状态估计（legged_estimation）：融合 IMU、关节编码器、接触信息，输出当前状态 \(x_0\)。 2. NMPC（legged_interface 定义 OCP + OCS2 求解，数十 Hz）：输入 \(x_0\)，输出一条**最优状态 + 输入轨迹** \(\{x^\*(t),u^\*(t)\}\)（质心运动 + 落足/接触力参考）。 3. WBC（legged_wbc，见 src/WbcBase.cpp，约 1 kHz）：把 MPC 的期望质心运动与接触力，通过一个（分层）QP 映射成**关节力矩**——在满足全身动力学、接触、摩擦锥等约束下让真实机器人跟踪 MPC 轨迹；力矩经 legged_controllers（ros-control 接口）下发执行器。

包结构速览：legged_controllers（控制器 + ros-control 接口）、legged_interface（OCS2 问题定义：\(f,g,h\)、代价）、legged_estimation（状态估计）、legged_wbc（全身控制 QP）、legged_examples/legged_unitree（机器人专属配置，如 legged_controllers/config/<robot>/task.info 放代价/约束权重）。

与本章的连接： - 这正是 §U2.5 "MPC 给方向、底层给瞬时执行"的工业级实现——MPC 出长时域质心轨迹（数十 Hz），WBC 出 ~1 kHz 关节力矩。 - Grandia et al.（2021）在这类栈里把 CBF 地形安全约束加进 MPC 的 \(h(x,u,t)\ge0\) 与 WBC 的 QP 约束，正对应这里的 \(h\) 与 WBC 约束——你能在代码里定位 §U2.5 的 CBF 落在何处。 - §U2.6 的 acados/HPIPM 在这里换成 OCS2 的 SLQ/DDP，但"块结构 OCP + 实时求解"的内核一致。

精读路线（先粗后细）：README 的 OCP 形式 → legged_interface 看 \(f/g/h\) 如何定义 → legged_wbc/src/WbcBase.cpp 看 QP 如何把 MPC 输出映射到关节力矩 → config/<robot>/task.info 看代价/约束权重配置。

避坑：OCS2 是巨型 monorepo，不要编译整个仓库——只编 ocs2_legged_robot_ros 及其依赖（OCS2、pinocchio、hpp-fcl）。

补充 worked 计算与完整 acados OCP

为把集合运算、机会约束、求解器落地都坐实，再给三个小而具体的片段。

(a) Minkowski 和与 Pontryagin 差的具体计算。取二维盒 \(\mathcal{X}=[-1,1]^2\)、\(\Omega=[-0.2,0.2]^2\)： - Minkowski 和：\([-1,1]^2\oplus[-0.2,0.2]^2=[-1.2,1.2]^2\)（逐维相加）。 - Pontryagin 差（约束收紧）：\(\mathcal{X}\ominus\Omega=[-0.8,0.8]^2\)（逐维向内腐蚀 0.2）。 - 非逆运算的体感：本例 \((\mathcal{X}\ominus\Omega)\oplus\Omega=[-0.8,0.8]^2\oplus[-0.2,0.2]^2=[-1,1]^2=\mathcal{X}\) 恰相等（盒对盒的巧合）；一般情形 \((\mathcal{X}\ominus\Omega)\oplus\Omega\subsetneq\mathcal{X}\)（如 \(\Omega\) 非盒时）。这就是 §U2.1 约束收紧的最简数值版——名义轨迹活动盒每边缩 0.2，给偏差留空间。

(b) 机会约束收紧的数值例。某步状态分量 \(x\sim\mathcal{N}(\mu,\sigma^2)\)，\(\sigma=0.2\)，约束 \(x\le1\)，风险 \(\delta=0.05\)：收紧量 \(=\Phi^{-1}(0.95)\cdot0.2\approx1.645\times0.2=0.329\)，故名义需满足 \(\mu\le1-0.329=0.671\)。若 \(\delta=0.01\)（\(\Phi^{-1}(0.99)\approx2.326\)），收紧量增至 \(0.465\)、\(\mu\le0.535\)——更严的风险要求换来更大保守度，数字一目了然。

(c) acados 完整 OCP 走查（双积分器鲁棒 NMPC）。把 §U2.6 的骨架补成更完整的例子（结构示意，具体 API 以 acados 官方文档为准）：

from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
import casadi as ca, numpy as np, scipy.linalg as sla

# 1) 模型：2D 双积分器 x=[px,py,vx,vy], u=[ax,ay]
model = AcadosModel(); model.name = "double_integrator_robust"
px,py,vx,vy = ca.SX.sym('px'),ca.SX.sym('py'),ca.SX.sym('vx'),ca.SX.sym('vy')
ax,ay = ca.SX.sym('ax'), ca.SX.sym('ay')
x, u = ca.vertcat(px,py,vx,vy), ca.vertcat(ax,ay)
model.x, model.u = x, u
model.f_expl_expr = ca.vertcat(vx, vy, ax, ay)             # 连续动力学 ẋ = f(x,u)
model.xdot = ca.SX.sym('xdot', 4)
model.f_impl_expr = model.xdot - model.f_expl_expr

# 2) OCP 与时域
ocp = AcadosOcp(); ocp.model = model
N, Tf = 20, 2.0
ocp.solver_options.N_horizon, ocp.solver_options.tf = N, Tf

# 3) 二次代价（线性最小二乘）
nx, nu = 4, 2
Q, R = np.diag([5.,5.,1.,1.]), 0.1*np.eye(2)
ocp.cost.cost_type = ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.cost.W, ocp.cost.W_e = sla.block_diag(Q, R), 20*Q
ocp.cost.Vx  = np.vstack([np.eye(nx), np.zeros((nu, nx))])
ocp.cost.Vu  = np.vstack([np.zeros((nx, nu)), np.eye(nu)])
ocp.cost.Vx_e = np.eye(nx)
ocp.cost.yref   = np.array([5.,5.,0.,0.,0.,0.])
ocp.cost.yref_e = np.array([5.,5.,0.,0.])

# 4) Tube 约束收紧：状态/控制盒向内收 Ω（omega_margin 由 epsilon_mrpi_support 给出）
omega_margin = np.array([0.05,0.05,0.08,0.08])             # 示意: Ω 在各状态方向半宽
ocp.constraints.idxbx = np.arange(nx)
ocp.constraints.lbx = -np.array([6.,6.,2.,2.]) + omega_margin
ocp.constraints.ubx =  np.array([6.,6.,2.,2.]) - omega_margin
ocp.constraints.idxbu = np.arange(nu)
ocp.constraints.lbu, ocp.constraints.ubu = -np.array([3.,3.]), np.array([3.,3.])
ocp.constraints.x0 = np.zeros(nx)

# 5) 求解器：SQP-RTI + HPIPM
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.integrator_type = 'ERK'
solver = AcadosOcpSolver(ocp)

# 6) 闭环一步：当前状态作初值约束 → 解 → 取 u0（再叠加 tube 反馈 K(x-z) 即真实控制）
def step(x_now):
    solver.set(0, 'lbx', x_now); solver.set(0, 'ubx', x_now)
    solver.solve()
    return solver.get(0, 'u')

逐块对应：f_expl_expr 是 \(\dot x=f\)；LINEAR_LS 代价对应 §U2.6 的二次代价；第 4 块 lbx/ubx ± omega_margin 就是 Tube 约束收紧 \(\mathcal{X}\ominus\Omega\)（margin 来自实现一的 epsilon_mrpi_support）；SQP_RTI+HPIPM 是 §U2.6 的实时迭代 + 块结构 QP。把它和实现一的 RPI 计算、累积项目的测试台拼起来，就是一个可上真机的鲁棒 NMPC 雏形。

场景树 MPC 的最小骨架（连接 U1 的分支思想）

Tube / GP / CBF 处理的都是**连续有界**扰动。但有些不确定性是**离散**的——前车要么左变道要么右变道、地面要么是冰要么是干。这时与其对所有可能性按最坏情况买单（太保守），不如显式枚举几个**场景（scenario），在一棵场景树上联合优化，并加**非预期约束（non-anticipativity）：在不确定性尚未揭示前，各场景必须共用同一个控制（你此刻还分不清是哪种情况）。这正是 U1 分支 / contingency / MPDM 规划的内核，这里给一个 U2 视角的最小骨架。

理论：场景树的根是当前状态，分支对应离散不确定性实现（\(M\) 个模式），每个场景是一条从根到叶的路径、带概率 \(p_j\)。多阶段 MPC 求解 $\(\min\ \sum_{j=1}^{M}p_j\,J_j(\mathbf x_j,\mathbf u_j)\quad\text{s.t.}\quad\text{每场景各自的动力学与约束},\ \ \underbrace{u_j^{(0)}=u_{j'}^{(0)}\ \forall j,j'}_{\text{非预期约束}}.\)$ 它给出的是"对几个代表性场景的**期望最优** + 共享的安全初始动作"，而非"对整个连续集的最坏情况鲁棒"——保守度更低，且能表达"先走一个对所有场景都安全的动作，等看清了再分叉"。

import cvxpy as cp, numpy as np

def scenario_mpc(x0, goal, b=(0.5, -0.5), p=(0.5, 0.5), N=15, dt=0.1, u_max=2.0):
    """两场景 1D 例：模式 j 的动力学 x+ = x + dt(u + b_j)，b=(+0.5 逆风, -0.5 顺风)。
    在场景树上联合优化 + 非预期约束（第一步控制对所有场景相同）。"""
    M = len(b)
    xs = [cp.Variable(N+1) for _ in range(M)]        # 每场景一条状态轨迹
    us = [cp.Variable(N)   for _ in range(M)]        # 每场景一条控制轨迹
    cost, cons = 0, []
    for j in range(M):
        cons += [xs[j][0] == x0]
        for k in range(N):
            cons += [xs[j][k+1] == xs[j][k] + dt*(us[j][k] + b[j])]
            cons += [cp.abs(us[j][k]) <= u_max]
        cost += p[j]*(cp.sum_squares(xs[j] - goal) + 0.1*cp.sum_squares(us[j]))
    for j in range(1, M):                            # 非预期约束：此刻还分不清是哪种风
        cons += [us[j][0] == us[0][0]]
    cp.Problem(cp.Minimize(cost), cons).solve(solver=cp.OSQP)
    return us[0][0].value                            # 执行共享的第一步控制

解读：us[j][0] == us[0][0] 是灵魂——它强制"在不确定性揭示前，必须采取对所有场景都合理的同一动作"；下一步信息到来后树才分叉。这把"鲁棒"从"对连续最坏情况"换成"对离散场景的期望最优 + 共享初始动作"，正是 U1（分支 / MPDM / contingency）的算法内核，也对应 §U2.6 提到的 acados Multi-Phase OCP。把 b_j 换成更多模式、把 1D 换成车辆/无人机动力学，就是 U1 要展开的完整框架。

Funnel / SOS 漏斗的概念走查（1D 可手算）

§U2.3 说 Funnel Libraries 用 SOS 算"漏斗"（不变集沿机动的时变演化）。SOS 求解需要专门工具，但"漏斗"概念本身可以用一个 1D 李雅普诺夫例子纯手算坐实。

系统（已被反馈镇定的误差动力学）：\(\dot e=-k e+w\)，\(k>0\)，扰动 \(|w|\le\bar w\)。取李雅普诺夫函数 \(V(e)=e^2\)，沿轨迹 \(\dot V=2e\dot e=-2ke^2+2ew\)。

漏斗 = \(V\) 的水平集 \(\{e:V(e)\le\rho\}=\{|e|\le\sqrt\rho\}\)。要它**不变**（状态进入后不出去），在边界 \(V=\rho\)（即 \(e=\pm\sqrt\rho\)）上要求 \(\dot V\le0\)： $\(\dot V\big|_{V=\rho}=-2k\rho+2e w\le-2k\rho+2\sqrt\rho\,\bar w\le0\ \Longrightarrow\ \sqrt\rho\ge\frac{\bar w}{k}\ \Longrightarrow\ \rho^\*=\Big(\frac{\bar w}{k}\Big)^2.\)$ 故最小不变漏斗是 \(|e|\le\bar w/k\)——状态保证被困在半径 \(\bar w/k\) 的漏斗内。

本质洞察：漏斗 = Tube 的 \(\Omega\) = FaSTrack 的 TEB。漏斗半径 \(\bar w/k\) 由扰动 \(\bar w\) 与镇定强度 \(k\) 决定（扰动越大、镇定越弱，漏斗越宽）——这与 Tube 标量例的 \(\Omega\approx0.1/(1-0.382)\)、FaSTrack 的 TEB 是**同一个量**的不同算法表达：都是"被有界扰动撑开、被反馈拉回的不变误差集"。Funnel Libraries 的贡献，是用 SOS 把这个 1D 直觉推广到**沿一条机动轨迹的时变非线性**情形（漏斗沿轨迹变粗变细），再把多个漏斗拼接成完整运动计划。至此，§U2.1（Tube/\(\Omega\)）、§U2.3（TEB/FRS/漏斗）在"鲁棒不变误差集"这一个概念下彻底打通——这是全章最重要的统一视角。

---

工具与源码精读拓展

API 速查里列了工具链清单，这里对几个关键工具做架构级精读——重点是它们的**工作流与定位**，帮你按需选型、读源码时不迷路。

HJ 可达性工具链：从 MATLAB 到 JAX

三个工具解同一类问题（哈密顿-雅可比可达性，算 BRT / 值函数），但成熟度与生态不同： - helperOC（MATLAB）：最经典的教学工具，建立在 Mitchell 的 Level Set Methods Toolbox 之上。工作流：定义系统动力学（继承一个 DynSys 类）→ 设状态网格 → 调 HJ PDE 求解器算值函数 → 取零水平集得 BRT、取梯度得最优安全控制。直观、文档全，但慢、依赖 MATLAB。 - optimized_dp（Python + HeteroCL）：把 helperOC 的算法用 HeteroCL 加速，Python 生态、显著快于 helperOC，适合中等规模问题。 - hj_reachability（JAX）：最现代，借 JAX 拿到 GPU 加速与自动微分，适合与学习方法（神经 HJ、DeepReach 类）结合。FaSTrack 的 TEB（§U2.3、实现四）可用它算。

共同工作流：定义动力学 \(\dot x=f(x,u,d)\)（含对抗扰动 \(d\)）→ 在状态网格上求解 HJI PDE 得值函数 \(V(x)\) → BRT \(=\{x:V(x)\le0\}\) → 安全控制由 \(\arg\max_u\min_d\nabla V^\top f\) 给出。**维度诅咒**就卡在"状态网格"——这也是三个工具都难超约 5–6 维、近年转向神经网络近似的根因（§U2.3 误区）。

do-mpc：多阶段鲁棒 NMPC 的源码精读

§U2.3 的场景树 MPC、实现里的 scenario_mpc 骨架，在工业级 Python 工具里对应 do-mpc（建立在 CasADi 之上的 NMPC/MHE 框架）。它的鲁棒控制用的正是**多阶段（multi-stage）方法： - **核心思想：把不确定参数在每个控制步的可能实现枚举成一棵**场景树**；对非线性系统，最坏情况通常落在不确定区间 \([p_{\min},p_{\max}]\) 的**边界**上，故实践中只取边界值就够。 - 关键设计——robust horizon：场景树随时域指数膨胀，do-mpc 用"鲁棒时域"限制分支只在前几步发生、之后不再分叉，把规模压下来（与 §U2.6 的 Multi-Phase、以及 tube-enhanced multi-stage 思路一致）。 - 数据结构：解的结构 opt_x 按 [时间步, 场景, 配点] 索引——读 do-mpc 源码时，理解这个"场景维"是关键（普通 MPC 没有这一维）。

# do-mpc 多阶段鲁棒 NMPC 骨架（示意，API 以 do-mpc 官方文档为准）
# import do_mpc, numpy as np
# model = do_mpc.model.Model('continuous')
# x = model.set_variable('_x', 'x', shape=(nx,1))
# u = model.set_variable('_u', 'u', shape=(nu,1))
# p = model.set_variable('_p', 'p')                       # 不确定参数(将在边界取值)
# model.set_rhs('x', f(x, u, p)); model.setup()
# mpc = do_mpc.controller.MPC(model)
# mpc.set_param(n_horizon=20, n_robust=1, t_step=0.1)     # n_robust = 鲁棒时域(分支步数)
# mpc.set_objective(mterm=term_cost, lterm=stage_cost); mpc.set_rterm(u=1e-2)
# mpc.set_uncertainty_values(p=np.array([p_min, p_max]))  # 场景 = 参数边界值
# mpc.setup()

与本章的对应：do-mpc 的多阶段 = 实现里 scenario_mpc 的工业版；n_robust（鲁棒时域）正是"分支几步后用 tube 兜住"的折中旋钮（§U2.2 保守度↔计算量权衡的又一处体现）。注意 do-mpc 目前主推多阶段、tube-based 尚未内置——要 tube 收紧得自己实现（实现一）或上 acados。

可微 MPC 工具：把 MPC 接进学习回路

§U2.6 与前沿提到的"可微 MPC"，工具上有几条路： - cvxpylayers（Agrawal, Amos 等）：把一个 cvxpy 凸问题包成可微层、对解 backprop——最通用，适合凸 MPC/QP 层。 - mpc.pytorch（Amos et al., NeurIPS 2018）：可微 LQR/iLQR，最早把 MPC 作端到端可微模块的工作之一。 - acados / theseus：acados 提供 SQP 灵敏度（对参数求导），theseus（Meta）做可微非线性最小二乘。 - 用途：把 MPC 当 actor、用 RL 调它的代价/模型参数（Actor-Critic MPC 即此思路），或把"可证明安全的 MPC/CBF 层"接到神经策略后端（前沿三篇的共同套路）。

选型一句话：凸问题 / QP 层 → cvxpylayers；经典 LQR/iLQR 可微 → mpc.pytorch；要工业级 NMPC + 灵敏度 → acados。

---

从 Python 原型到 C++ 部署

本章代码用 Python（numpy + cvxpy）讲算法，是为了让代码贴着公式、信噪比最高。但真实机器人部署是 C++ 的天下——这一节把"原型→部署"这座桥讲清楚，并给一个 C++ 对照示例。这不是二选一，而是工业界的标准两段式。

两段式工作流

阶段	语言/工具	干什么
研究 / 设计 / 验证	Python（numpy, cvxpy, GP 库, JAX）	推导、快速试错、调参、蒙特卡洛、可视化——本章所有代码
部署 / 实时回路	C++（Eigen + qpOASES/OSQP/HPIPM, ROS2）	硬实时控制、嵌入式、上真机

重活其实**早已在 C/C++ 里**：acados 是 C 核（BLASFEO/HPIPM），OSQP 是 C，你写的"Python 代码"本质是 Python 在**编排** C/C++ 求解器。所以 Python 这层不是玩具，而是研究层的标准形态——而且 **acados 能从 Python 的问题定义直接 codegen 出 C 代码**去部署，两段式天然衔接。

C++ 在哪里登场、为什么

• 硬实时回路：CBF-QP 安全滤波、WBC 都要 ~1 kHz，MPC 数十到数百 Hz。Python 的 GIL、垃圾回收、解释器开销会带来**不可预测的时延抖动**，而硬实时回路要的是**确定性时序**——这是 C++ + 实时 OS 的领域。
• 嵌入式 / MCU：算力/内存受限、常无 Python 运行时，只能 C/C++。
• ROS2 实时节点：控制节点用 C++（rclcpp）才能配合实时执行器与 DDS QoS。
• 已有 C++ 栈集成：OCS2、legged_control、ros-control 全是 C++，融进去自然用 C++。

关键区分：C++ 的不可替代性在**部署**（确定性、嵌入式），不在**理解**。学算法阶段被工具链劝退是本末倒置；理解透了再落到 C++ 才顺。

真实 C++ 栈（本章已点过的）

• OCS2 / legged_control（纯 C++）：§U2.5 源码精读过的 NMPC-WBC 栈，MPC（SLQ/DDP）+ WBC（QP）+ 状态估计全 C++。
• acados 生成的 C：§U2.6 的 acados 从 Python 定义 codegen 出独立 C 代码，可直接嵌入。
• QP 求解器核：qpOASES（C++，active-set，WBC/CBF 常用）、OSQP（C，ADMM，cvxpy 后端）、HPIPM（C，acados 后端）。

C++ 版 CBF-QP（Eigen + qpOASES）对照示例

§U2.5 实现五的 Python/cvxpy 版 CBF-QP，部署版长这样（qpOASES API 已核对官方 example）：

// CBF-QP 安全滤波器的 C++ 部署版（Eigen 装配 + qpOASES 求解）
// 对应 §U2.5：min ||u - u_nom||^2  s.t.  Lf h + (Lg h) u >= -gamma * h
#include <qpOASES.hpp>
#include <Eigen/Dense>
USING_NAMESPACE_QPOASES

// 单积分器 2D 避圆障碍：h(p)=||p-p_o||^2 - r^2,  Lf h = 0,  Lg h = 2 (p-p_o)^T
Eigen::Vector2d cbf_qp_filter(const Eigen::Vector2d& p,        // 当前位置
                              const Eigen::Vector2d& u_nom,    // 名义控制
                              const Eigen::Vector2d& p_o,      // 障碍中心
                              double r, double gamma, double u_max) {
    const int nV = 2, nC = 1;
    // 目标 0.5 u^T H u + g^T u, 取 H=2I, g=-2 u_nom  => 等价 min ||u - u_nom||^2
    real_t H[nV*nV] = { 2.0, 0.0, 0.0, 2.0 };
    real_t g[nV]    = { (real_t)(-2.0*u_nom.x()), (real_t)(-2.0*u_nom.y()) };
    // CBF 约束：(Lg h) u >= -gamma h，写成 qpOASES 的 lbA <= A u <= ubA
    double h = (p - p_o).squaredNorm() - r*r;
    Eigen::RowVector2d Lgh = 2.0 * (p - p_o).transpose();
    real_t A[nC*nV] = { (real_t)Lgh(0), (real_t)Lgh(1) };
    real_t lbA[nC]  = { (real_t)(-gamma*h) };                 // 下界 = -gamma h
    real_t ubA[nC]  = { 1.0e9 };                              // 无上界
    real_t lb[nV]   = { (real_t)(-u_max), (real_t)(-u_max) }; // 控制盒约束
    real_t ub[nV]   = { (real_t)( u_max), (real_t)( u_max) };

    QProblem qp(nV, nC);
    Options opt; opt.printLevel = PL_NONE; qp.setOptions(opt);
    int_t nWSR = 50;
    qp.init(H, g, A, lb, ub, lbA, ubA, nWSR);   // 实时回路第二次起改用 hotstart 复用工作集
    real_t uOpt[nV];
    qp.getPrimalSolution(uOpt);
    return Eigen::Vector2d(uOpt[0], uOpt[1]);
}

与 Python 版的对应：数学完全一样（同一个 \(h\)、同一个 \(L_gh\)、同一个 QP），只是 numpy→Eigen、cvxpy 的 Problem.solve()→qpOASES 的 init/getPrimalSolution。在 1 kHz 回路里，第二次起用 hotstart（复用上一次的工作集）能把单次求解压到几十微秒——这正是 active-set QP 在实时控制里的优势。

Python 原型	C++ 部署
numpy 数组	Eigen 矩阵/向量
cvxpy Problem(...).solve()	qpOASES QProblem::init/hotstart
pip install 即跑	CMake / colcon 构建 + 链接 Eigen/qpOASES
脱机验证、可视化	ROS2 节点、实时回路

落地建议：先在 Python 把算法、调参、蒙特卡洛跑通（本章累积项目），确认指标达标；再把**热路径**（CBF-QP、MPC 求解）用 C++ 重写或用 acados codegen，外围（参数加载、日志、可视化）可留 Python。别一上来就写 C++——先用 Python 确认"算法对不对"，再用 C++ 解决"快不快、稳不稳"。

把 CBF-QP 包成 ROS2 实时节点（C++ 骨架）

上面的 cbf_qp_filter 是纯函数；真实部署要把它包成订阅状态、发布安全控制的 ROS2 节点（rclcpp，示意，API 以 ROS2 文档为准）：

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <Eigen/Dense>

class CBFSafetyFilter : public rclcpp::Node {
public:
  CBFSafetyFilter() : Node("cbf_safety_filter") {
    sub_cmd_  = create_subscription<geometry_msgs::msg::Twist>(            // 名义控制(上层/RL)
        "cmd_nominal", 10, [this](auto m){ u_nom_ = {m->linear.x, m->linear.y}; });
    sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(             // 当前状态(估计)
        "odom", 10, [this](auto m){ p_ = {m->pose.pose.position.x, m->pose.pose.position.y}; });
    pub_      = create_publisher<geometry_msgs::msg::Twist>("cmd_safe", 10);
    timer_ = create_wall_timer(std::chrono::milliseconds(1),             // 1 kHz 硬实时回路
                               [this]{ this->control_loop(); });
  }
private:
  void control_loop() {
    Eigen::Vector2d u = cbf_qp_filter(p_, u_nom_, p_o_, r_, gamma_, u_max_);
    geometry_msgs::msg::Twist out; out.linear.x = u.x(); out.linear.y = u.y();
    pub_->publish(out);
  }
  Eigen::Vector2d p_{0,0}, u_nom_{0,0}, p_o_{2,2};
  double r_{0.5}, gamma_{2.0}, u_max_{2.0};
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<CBFSafetyFilter>());
  rclcpp::shutdown();
}

节点订阅"名义控制"（来自上层规划器/RL）与"状态"（来自状态估计），在 1 kHz 回调里跑 CBF-QP、发布"安全控制"给执行器——这正是 §U2.5 "安全滤波器串在任意控制器与执行器之间"的真实部署形态：上层可以是 Python 的 RL 策略，安全层是 C++ 的硬实时节点。

acados 的 codegen 部署流程

§U2.6 的 acados 不止能在 Python 里解，还能**生成独立 C 代码**部署到无 Python 的目标机： 1. 在 Python 里定义 OCP（如补充 worked 的完整 OCP）； 2. AcadosOcpSolver(ocp, json_file=...) 生成 c_generated_code/ 目录（求解器 C 源 + Makefile）； 3. 在目标机上编译这些 C 代码、链接进 C++ 控制程序（acados 提供 C/C++ 接口）； 4. 运行时无需 Python——纯 C 求解器、确定性时序。

这是"Python 设计 → C 部署"最丝滑的一条路：只在 Python 里描述问题，acados 替你把 SQP-RTI + HPIPM 的高效 C 代码生成出来。OCS2 则是另一条路（直接写 C++）。两者对应"代码生成"与"手写库"两种工程风格。

C++ 部署的额外坑（实时 / 数值）

• 别在实时回调里动态分配内存：预分配所有缓冲（Eigen 用固定尺寸 Matrix<double,N,M>），new/malloc 会带来不可预测延迟。
• 别在热路径打磁盘日志：用无锁/异步日志，否则 1 kHz 丢拍。
• 数值一致性：Python（双精度 + 成熟库）与 C++ 手写可能有细微数值差异——部署后要在真机/HIL 上重新验证指标，别假设"仿真过了就行"（呼应故障排查最后一条）。
• 求解器失败兜底：QP/MPC 偶尔 nWSR 用尽或 infeasible，C++ 里必须有 fallback（上一拍解、安全停车），不能崩。
• 实时 QoS：ROS2 话题用合适的 QoS（如 BestEffort + 小队列）减少延迟抖动。

---

方法选型决策指南

学完六种方法，真实项目里到底用哪个？这一节给一个可操作的决策流程——先问自己四个问题。

问题 1：不确定性是什么类型？ - 连续有界扰动（风、地面摩擦波动、未建模小扰动）→ Tube MPC（§U2.1–U2.2）或可达性（§U2.3）。 - 系统性、可学习的残差（气动、轮胎力）→ GP-MPC（§U2.4），用数据把保守度压下来。 - 离散模式 / 多假设（前车意图、路面类别）→ 场景树 / 分支 MPC（连 U1）。 - 感知端、部分可观（不知障碍在哪、状态估不准）→ 留给 U4（POMDP / belief）。

问题 2：实时预算多少？ - 极紧（>1 kHz，嵌入式）→ CBF-QP 安全层（§U2.5，微秒级）+ Rigid Tube（§U2.1，名义 QP 轻）；避免 Elastic Tube、full GP、grid HJ。 - 中等（几十~几百 Hz）→ GP-MPC（稀疏 GP）、acados 上的鲁棒 NMPC（§U2.6）都可行。 - 离线算力充足→ 可达性（FaSTrack/RTD/Funnel）的重计算可接受。

问题 3：要不要给一个"不可证明安全"的控制器兜底？（如 RL 策略、复杂规划器） - 要 → CBF-QP 安全滤波（§U2.5）是首选：最小侵入、给任意名义控制器套上形式化安全。前沿的 One Filter（HJ 值网络）、CBF-RL（训练期内化）是其进阶。

问题 4：保守度 vs 计算量怎么权衡？ - 余量足够、要最简最快 → Rigid Tube / 一阶 CBF。 - 余量紧、愿付在线计算 → Homothetic/Elastic Tube（§U2.2）、HOCBF。 - 想让保守度随数据自适应 → GP-MPC（方差驱动收紧）。

常见组合（方法不互斥，实际常分层叠加）： - 规划层鲁棒 + 控制层安全：Tube/GP-MPC 出名义轨迹（留扰动余量）+ CBF-QP 高频兜避障底线——这正是累积项目的架构，也是腿足栈（§U2.5 源码精读）的范式。 - 离线证书 + 在线任意规划器：FaSTrack/RTD 离线算误差包络 + 在线快规划器，规划与安全解耦。 - 学习 + 经典：RL / 可微 MPC 出策略 + CBF / HJ 滤波兜底（前沿三篇）。

一句话决策：嵌入式实时、要兜底 → CBF-QP + Rigid Tube；有系统残差、能采数据 → GP-MPC；要对离散意图鲁棒 → 场景树（U1）；离线算力足、要解耦规划器 → 可达性。几乎所有真实系统都是"某种鲁棒规划 + CBF 安全层"的组合，而非单一方法。

回到五安全谱（U0）：本章覆盖的是谱的最保守端（鲁棒 100%）到机会约束的过渡——Tube / 可达性 / CBF 给硬保证，GP-MPC 已探到"概率保证"（机会约束），这正是 U3 的入口。选型的本质，就是在"安全谱"上根据你的不确定性类型与代价容忍度选一个点：要绝对安全且扰动有界，选最保守端；能接受小概率违反以换性能，往机会约束 / 风险敏感端走。

---

方向落地案例

把六种方法落到三个最常见的移动机器人方向，看它们在真实系统里如何**组合**（涉及的论文事实均已核实）。

案例一：四足机器人——MPC + CBF + WBC 的安全运动

问题：粗糙地形上既要精确落足（safe foothold）又要动态稳定，而可行落足点可能离散稀疏（stepping-stones）。

控制栈（自上而下）： - MPC（质心 / kinodynamic 模型，数十 Hz，OCS2 的 SLQ/DDP）出质心轨迹 + 落足/接触力参考； - WBC（~1 kHz QP）把期望质心运动与接触力映射成关节力矩（见 §U2.5 源码精读、§U2.6）； - **状态估计**融合 IMU + 编码器 + 接触，闭环。

安全怎么进来： - Grandia, Taylor, Ames, Hutter（ICRA 2021）把 CBF 地形安全约束同时嵌入低频 MPC 与高频 WBC，ANYmal 在 stepping-stones 上实现安全落足 + 动态稳定——CBF 在这里编码"落足点必须落在可行地形内"。 - legged_control 自身的工作（Liao, Li, Thirugnanam, Zeng, Sreenath, IROS 2023, pp. 2723–2730）用 duality-based optimization 做窄空间安全运动，是这套开源栈的安全升级。

学习增强（前沿）： - One Filter（Lin, Peng, Bansal, 2024）：HJ 值网络给**任意**四足策略（含 RL）兜底，Unitree Go1，无需针对策略调参。 - CBF-RL（Yang, Werner, de Sa, Ames, 2025）：训练期内化 CBF，Unitree G1 安全避障 + 上楼梯，部署免在线滤波。

对应本章：Tube 思想体现在"MPC 留裕度 + WBC 跟踪"；CBF（§U2.5）是地形/碰撞安全层；GP（§U2.4）可学接触/地形残差。工程要点：MPC 与 WBC 的频率分工、CBF 约束放哪一层、状态估计质量几乎决定一切。

案例二：四旋翼 / 无人机——GP-MPC 与学习增强的高速飞行

问题：高速下气动残差成为主导扰动，跟踪误差大且极难建模。

方法落地： - GP-MPC（Torrente, Kaufmann, Foehn, Scaramuzza, 2021）：GP 学气动残差嵌入 MPC，速度达 14 m/s、>4g，跟踪误差最多降 70%——§U2.4 的标杆案例。 - Actor-Critic MPC（Romero, Song, Scaramuzza 等, ICRA 2024 / T-RO 2025）：可微 MPC 作 actor、RL 端到端训练，无人机竞速 21 m/s，兼得 MPC 结构与 RL 灵活——§U2.6 "可微 MPC"伏笔的兑现。 - FaSTrack（Herbert et al., 2017）最初就在四旋翼上演示用 TEB 保证安全。

对应本章：GP-MPC（§U2.4）是核心；CBF（§U2.5）可做避障安全层；可达性（§U2.3）给 TEB。工程要点：实时性（稀疏 GP + acados RTI）、残差学什么（学气动而非整个动力学，保留物理先验）、高速下的数值稳定。

案例三：自动驾驶——场景 / contingency 与残差学习

问题：连续扰动（路面、风）+ 离散不确定性（他车意图、路面类别）+ 安全硬约束，三者并存。

方法落地： - 连续侧：GP-MPC 学轮胎-路面残差（Kabzan, Hewing, Liniger, Zeilinger, 2019，AMZ Formula Student 车，GP 字典收敛后单圈快约 10%）；Tube MPC 处理有界扰动。 - 离散侧：场景树 / contingency MPC（本章场景骨架，连 U1 的 MPDM / branch）处理他车多假设——非预期约束保证"看清意图前先走对所有意图都安全的动作"。 - 可达性：RTD（Kousik et al., 2020）在 Segway / 类车 Rover 上保证安全且实时的规划，并有面向乘用车的扩展。

对应本章：Tube + GP（连续）+ 场景（离散）+ CBF（安全层）的组合，正是"几乎所有真实系统都是组合"的典型。工程要点：意图预测的不确定性如何进入规划（接 U4 的 belief）、计算预算、安全与舒适/效率的权衡。

三个案例的共性：没有单一方法包打天下，都是"鲁棒规划 + 安全层（+ 学习增强）"的分层组合；方法的选择由该领域的不确定性结构决定——四足重地形/接触，无人机重气动，自驾重他车意图。这把"方法选型决策指南"的抽象框架落到了具体方向上。

---

前沿与开放问题

鲁棒规划与安全滤波近年的活跃前沿，大多围绕**"经典安全保证 × 学习"**的融合展开（出处已核实）：

• 可微 MPC 作 RL 的 actor：Romero, Song, Scaramuzza（及期刊版 Romero, Aljalbout, Song, Scaramuzza），"Actor-Critic Model Predictive Control"，IEEE ICRA 2024, pp. 14777–14784 / IEEE T-RO 2025（arXiv:2306.09852）。把可微 MPC 嵌入 actor-critic：MPC 负责短时域结构化优化，RL 负责长时域端到端学习与探索；真机无人机竞速达 21 m/s，与无模型 RL 的超人表现持平，同时保留 MPC 的可解释性与分布外鲁棒性。
• HJ 值网络作通用安全滤波器：Lin, Peng, Bansal，"One Filter to Deploy Them All: Robust Safety for Quadrupedal Navigation in Unknown Environments"（arXiv:2412.09989, 2024）。用观测条件可达性（OCR）值网络在部署时预测 HJ 安全值函数（LiDAR 输入构造安全区 + 扰动估计应对动力学不确定性），**无需重训或针对策略调参**即可给各种名义四足控制器（model-based 与 learning-based 皆可）兜底，Unitree Go1 真机验证。
• 训练期内化 CBF：Yang, Werner, de Sa, Ames，"CBF-RL: Safety Filtering Reinforcement Learning in Training with Control Barrier Functions"（arXiv:2510.14959, 2025）。把 CBF 安全滤波放进 RL 训练（而非仅在线部署），让策略"知道"CBF 的存在，从而内化安全约束、部署时无需在线滤波器，减少保守性；Unitree G1 人形上实现安全避障与上楼梯。
• 学习化 HJ 破解维度诅咒：用神经网络近似 HJ 值函数（DeepReach 类方法），把 FaSTrack/可达性方法的"TEB 只能算约 5 维"的瓶颈往高维推。

开放问题：(1) 如何在高维非线性系统上高效计算 RPI 集？(2) 如何把 GP/神经网络残差的不确定性**形式化**地（带保证地）传给 Tube/CBF/机会约束？(3) 如何把残差不确定性分解成 epistemic 与 aleatoric 并分别处理（呼应 U0 §2）？(4) 学习化安全证书（值网络、神经 CBF）的保证如何严格化，而非仅经验有效？

---

累积项目：U2 模块（Tube MPC + CBF 安全层）

模块目标

在 U0 搭好的"统一不确定性导航测试台"上，给它加装**执行端不确定性**的处理：用 Tube 风格的约束收紧让名义 MPC 对有界扰动 \(w\in\mathcal{W}\) 留出安全余量，再用 HOCBF 安全层在控制输出端兜一道避障底线。目标是把 U0 baseline（风险中性 NMPC）在扰动 + 障碍下的碰撞率降到零——对应 U 线里程碑"CBF 滤波器零碰撞"。

测试台为 2D 双积分器（状态 \(x=[p_x,p_y,v_x,v_y]\)、控制 \(u=[a_x,a_y]\)），每步注入有界扰动，场地含圆形障碍。下面给出可运行的实现骨架，逐块对应 §U2.1（Tube）与 §U2.5（CBF）。

import numpy as np, cvxpy as cp
from scipy.linalg import solve_discrete_are

# ── 1) 测试台：2D 双积分器 + 有界扰动 + 圆障碍 ──
class NavTestbed:
    def __init__(self, dt=0.1, w_max=0.02,
                 obstacles=((np.array([2.5, 2.4]), 0.7),),
                 goal=np.array([5., 5.]), x_half=np.array([6., 6.])):
        I2, Z2 = np.eye(2), np.zeros((2, 2))
        self.A = np.block([[I2, dt*I2], [Z2, I2]])          # 4x4
        self.B = np.block([[0.5*dt**2*I2], [dt*I2]])        # 4x2
        self.dt, self.w_max = dt, w_max                     # W = [-w_max, w_max]^4
        self.obstacles, self.goal, self.x_half = obstacles, goal, x_half
    def step(self, x, u, rng):
        w = rng.uniform(-self.w_max, self.w_max, size=4)    # 偶然扰动 w∈W
        return self.A @ x + self.B @ u + w
    def collided(self, x):
        return any(np.linalg.norm(x[:2]-c) < r for c, r in self.obstacles)

# ── 2) 离线：LQR 反馈增益 K + RPI 集 Ω 的坐标支撑(盒近似) ──
def design_tube(A, B, Q, R, w_max, n_iter=15):
    P = solve_discrete_are(A, B, Q, R)
    K = -np.linalg.inv(R + B.T@P@B) @ (B.T@P@A)             # 镇定增益 (u = K x)
    Ak, nx = A + B@K, A.shape[0]
    # Ω ≈ ⊕_{i=0}^{n} Ak^i W；对称盒 W 在方向 e_j 的支撑 = w_max·||(Ak^i)^T e_j||_1
    support, M = np.zeros(nx), np.eye(nx)
    for _ in range(n_iter):
        support += w_max * np.sum(np.abs(M), axis=1)
        M = M @ Ak
    return K, support           # support[j] = Ω 在第 j 坐标方向上的半宽

# ── 3) 在线：约束收紧的名义 MPC (X ⊖ Ω) ──
def nominal_mpc(env, x0, K, support, N=20, u_max=3.0, Q=None, R=None):
    A, B = env.A, env.B
    Q = np.diag([5., 5., 1., 1.]) if Q is None else Q
    R = 0.1*np.eye(2) if R is None else R
    xg = np.concatenate([env.goal, np.zeros(2)])
    z, v = cp.Variable((4, N+1)), cp.Variable((2, N))
    cost, cons = 0, [z[:, 0] == x0]                         # 简化: z0 = x0(见下文说明)
    for k in range(N):
        cost += cp.quad_form(z[:, k]-xg, Q) + cp.quad_form(v[:, k], R)
        cons += [z[:, k+1] == A@z[:, k] + B@v[:, k]]
        cons += [cp.abs(z[:2, k]) <= env.x_half - support[:2]]   # 状态约束收紧
        cons += [cp.abs(v[:, k]) <= u_max]                       # 控制约束
    cost += cp.quad_form(z[:, N]-xg, 20*Q)
    cp.Problem(cp.Minimize(cost), cons).solve(solver=cp.OSQP, warm_start=True)
    return v[:, 0].value

# ── 4) HOCBF 安全层：双积分器(相对度2)避圆障碍，最小修正名义控制 ──
def hocbf_filter(x, u_nom, obstacles, a1=4.0, a2=4.0, u_max=3.0):
    p, vel, u = x[:2], x[2:], cp.Variable(2)
    cons = [cp.abs(u) <= u_max]
    for c, r in obstacles:
        d = p - c
        h  = float(d@d - r**2)                  # h = ||p-c||^2 - r^2 (相对度2)
        hd = float(2*d@vel)                      # ḣ = 2 d·v
        # HOCBF: ḧ + (a1+a2)ḣ + a1·a2·h ≥ 0, 其中 ḧ = 2||v||^2 + 2 d·u
        cons += [2*float(vel@vel) + 2*d@u + (a1+a2)*hd + a1*a2*h >= 0]
    prob = cp.Problem(cp.Minimize(cp.sum_squares(u - u_nom)), cons)
    prob.solve(solver=cp.OSQP)
    return u.value if u.value is not None else u_nom   # infeasible 时退回名义(见故障排查)

# ── 5) 控制器与蒙特卡洛评测 ──
def make_controller(env, K, support, robust):
    def ctrl(x, rng):
        if robust:
            v0 = nominal_mpc(env, x, K, support)           # 收紧名义 MPC
            v0 = v0 if v0 is not None else np.zeros(2)
            return hocbf_filter(x, v0, env.obstacles)       # + CBF 安全层
        else:                                               # baseline: 不收紧, 无 CBF
            v0 = nominal_mpc(env, x, K, np.zeros(4))        # support=0 ⇒ 不收紧
            return v0 if v0 is not None else np.zeros(2)
    return ctrl

def run_episode(env, ctrl, rng, T=150):
    x = np.array([0., 0., 0., 0.])
    for _ in range(T):
        x = env.step(x, ctrl(x, rng), rng)
        if env.collided(x): return "collision"
        if np.linalg.norm(x[:2]-env.goal) < 0.25: return "success"
    return "timeout"

def monte_carlo(env, ctrl, n=300, seed=0):
    rng = np.random.default_rng(seed)
    from collections import Counter
    return Counter(run_episode(env, ctrl, rng) for _ in range(n))

# 用法:
# env = NavTestbed()
# K, support = design_tube(env.A, env.B, np.diag([5,5,1,1]), 0.1*np.eye(2), env.w_max)
# print("baseline   :", monte_carlo(env, make_controller(env, K, support, robust=False)))
# print("Tube+CBF   :", monte_carlo(env, make_controller(env, K, support, robust=True)))

关于 \(z_0=x_0\) 的简化说明：上面为清晰起见每步令名义初值 \(z_0=x_0\)，此时 tube 反馈项 \(K(x-z_0)\) 在求解时刻为零，鲁棒性主要来自约束收紧。完整 Tube MPC 把 \(z_0\) 当作**决策变量**（约束在 \(x_0\ominus\Omega\) 邻域内），从而激活反馈 \(K\)——这正是 §U2.1 练习 2 要你补全的部分。本模块先把"收紧 + CBF"的骨架跑通。

预期结果与验收

跑蒙特卡洛对比，典型现象是：baseline（不收紧、无 CBF）在障碍处频繁碰撞、在扰动下偶尔越界；Tube+CBF 把碰撞率压到零（CBF 保证）、且名义轨迹因收紧而留有余量。把结果整理成下表（具体数字随参数变化，重在对比趋势）：

配置	碰撞率	到达率	说明
baseline（风险中性，U0）	高	中	名义轨迹直穿障碍区、无扰动余量
+ Tube 收紧	中	中	状态约束留余量，但仍可能撞障碍（收紧不针对障碍）
+ Tube + HOCBF	≈0	高	CBF 保证避障；收紧保证不越状态边界

验收标准（Bronze / Silver / Gold）： - 🥉 Bronze：跑通 baseline，用蒙特卡洛**定量复现** U0 提出的"风险中性会失效"（记录碰撞率/越界率）。 - 🥈 Silver：加 HOCBF 安全层，使碰撞率降到零；扫 class-K 增益 \(a_1,a_2\)，画"离障碍最近距离 vs 增益"曲线。 - 🥇 Gold：补全完整 Tube MPC（把 \(z_0\) 设为决策变量、显式算 RPI 集 \(\Omega\) 并激活反馈 \(K\)），并加入第二个障碍与更强扰动，验证鲁棒性；为 U3（机会约束）预留接口——把 HOCBF 的硬避障替换为"碰撞概率 \(\le\delta\)"。

承上启下：本模块的"约束收紧 + 安全层"是后续各章的基线对照。U3 会把这里的硬约束换成机会约束（用协方差 \(\Sigma_k\) 与 \(\Phi^{-1}(1-\delta)\) 收紧）；U4 会在此测试台上加感知端不确定性与 belief 规划；附录会在同一测试台上 benchmark 五范式。保持测试台接口不变、只替换/叠加规划器，是做"控制变量对比"的关键。

---

完整可运行 demo：把 Tube + CBF 跑起来并可视化

光有模块还不够，下面给一段把它们串起来、能**看到效果**的 demo——跑多条带扰动的轨迹并画出来，baseline 与 Tube+CBF 并排对比。

import numpy as np, matplotlib.pyplot as plt

def simulate_trajectory(env, ctrl, rng, T=150):
    x, traj = np.array([0., 0., 0., 0.]), [np.zeros(4)]
    for _ in range(T):
        x = env.step(x, ctrl(x, rng), rng)
        traj.append(x.copy())
        if env.collided(x) or np.linalg.norm(x[:2] - env.goal) < 0.25:
            break
    return np.array(traj)

def plot_demo():
    env = NavTestbed()
    Q, R = np.diag([5., 5., 1., 1.]), 0.1*np.eye(2)
    K, support = design_tube(env.A, env.B, Q, R, env.w_max)
    ctrl_base   = make_controller(env, K, support, robust=False)
    ctrl_robust = make_controller(env, K, support, robust=True)

    fig, axes = plt.subplots(1, 2, figsize=(11, 5))
    for ax, ctrl, title in [(axes[0], ctrl_base,   "baseline（无收紧无 CBF）"),
                            (axes[1], ctrl_robust, "Tube + HOCBF")]:
        for s in range(8):                                   # 8 条不同扰动样本
            tr = simulate_trajectory(env, ctrl, np.random.default_rng(s))
            ax.plot(tr[:, 0], tr[:, 1], lw=1, alpha=0.7)
        for c, r in env.obstacles:                           # 画障碍（红）
            ax.add_patch(plt.Circle(c, r, color='r', alpha=0.3))
        ax.plot(*env.goal, 'g*', ms=15); ax.plot(0, 0, 'ko')  # 目标(绿星)、起点(黑点)
        ax.set_title(title); ax.set_aspect('equal')
        ax.set_xlim(-1, 6); ax.set_ylim(-1, 6); ax.grid(alpha=0.3)
    plt.tight_layout(); plt.savefig("robust_nav_demo.png", dpi=120)

def compare():                                               # 蒙特卡洛数值对比
    env = NavTestbed()
    Q, R = np.diag([5., 5., 1., 1.]), 0.1*np.eye(2)
    K, support = design_tube(env.A, env.B, Q, R, env.w_max)
    print("baseline :", dict(monte_carlo(env, make_controller(env, K, support, False))))
    print("Tube+CBF :", dict(monte_carlo(env, make_controller(env, K, support, True))))

# plot_demo(); compare()

你会看到：左图 baseline 的多条轨迹有的直穿红色障碍区（碰撞）、有的被扰动推得歪歪扭扭；右图 Tube+HOCBF 的轨迹都**绕开**障碍、且因约束收紧而不贴边。compare() 打印的碰撞率会从 baseline 的明显非零降到 Tube+CBF 的 ≈0。把这张图和打印结果放进报告，就是消融分析最直观的呈现。

动手提示：改 env.w_max（扰动大小）看轨迹发散程度与碰撞率怎么变；改 HOCBF 的 a1,a2 看轨迹离障碍多近；加第二个障碍 obstacles=((c1,r1),(c2,r2)) 看 CBF 同时处理多约束。这就是把"调参与诊断速查"那张表变成可视实验的过程。

消融解读：每一层各贡献了什么

把三种配置的蒙特卡洛结果摆在一起，能清楚看到每层的作用——这也是写鲁棒系统时最该做的"控制变量"分析：

• baseline（风险中性 NMPC，无收紧无 CBF）：名义轨迹是"无扰动假设下的最优路径"，会贴着甚至穿过障碍区抄近路；一旦注入扰动，既可能撞障碍、也可能越状态边界。这定量复现了 U0 的核心论点——风险中性在有不确定性时会系统性失效。
• + Tube 收紧（仍无 CBF）：状态约束向内收 \(\Omega\)，名义轨迹离边界留出余量、扰动下不越界；但收紧是对**状态盒**做的、并不针对障碍，所以**仍可能撞障碍**（碰撞率只小幅下降）。这说明 Tube 解决的是"扰动下守约束"，不是"避障"。
• + Tube + HOCBF（完整）：HOCBF 在控制输出端兜避障底线，碰撞率压到 ≈0；Tube 继续保证不越状态边界。两层各司其职——Tube 管"扰动余量"、CBF 管"瞬时避障"，缺一不可。

怎么读这些数字：碰撞率反映安全层（CBF）是否到位，越界率反映鲁棒余量（Tube 收紧）是否够，到达率反映保守度是否过头（裕度留太多会到不了目标）。一个调好的鲁棒系统应当：碰撞率 ≈0、越界率 ≈0、到达率高。若到达率明显下降，说明 \(\Omega\) 或 CBF 增益过保守，该回头调 \(K\)（减小 \(\Omega\)）或放松 class-K 增益——这把前面所有"保守度旋钮"的讨论变成了可观测的指标。

为什么这是"控制变量"的范本：三种配置只差"加不加某一层"，其余（测试台、扰动、障碍、随机种子）完全相同，于是碰撞率/越界率的变化可**干净地归因**到那一层。这正是附录里 benchmark 五范式时要遵循的方法——保持测试台不变、逐个替换或叠加组件，才能说清"是哪一项带来了改变"。

进阶：动态障碍与机会约束版（U3/U4 桥）

累积项目目前是静态障碍 + 硬 CBF。加两个进阶，既练手又通往后续章节。

进阶一：动态（移动）障碍——时变 CBF

障碍在动：\(p_o(t)\) 以速度 \(\dot p_o\) 移动。安全函数 \(h(x,t)=\|p-p_o(t)\|^2-r^2\) 现在**显含时间**，\(\dot h\) 多出时间项。对双积分器：\(\dot h=2(p-p_o)^\top(v-\dot p_o)\)，即用**相对速度** \(v-\dot p_o\) 代替 \(v\)。HOCBF 约束相应地把 \(v\) 换成相对速度：

def hocbf_filter_moving(p, v, u_nom, p_o, v_o, r, a1, a2, u_max):
    """移动障碍的 HOCBF 滤波：用相对速度 v_rel = v - v_o 代替 v（障碍匀速 a_o=0）。"""
    import cvxpy as cp, numpy as np
    rel, v_rel = p - p_o, v - v_o                     # 关键：相对量
    h  = rel @ rel - r**2
    hd = 2 * rel @ v_rel                              # ḣ = 2 rel·(v - v_o)
    u = cp.Variable(2)
    hdd = 2 * (v_rel @ v_rel) + 2 * rel @ u           # ḧ = 2||v_rel||² + 2 rel·u
    cons = [hdd + (a1 + a2) * hd + a1 * a2 * h >= 0,  # HOCBF 约束(同实现三, 用相对量)
            cp.abs(u) <= u_max]
    cp.Problem(cp.Minimize(cp.sum_squares(u - u_nom)), cons).solve(solver=cp.OSQP)
    return u.value if u.value is not None else np.clip(u_nom, -u_max, u_max)

唯一变化是 \(v\to v-v_o\)——这把实现三的静态 HOCBF 推广到动态障碍，是行人/车辆避让的基础。坑：若障碍也加速（\(a_o\ne0\)），\(\ddot h\) 里要补 \(-2\,rel\cdot a_o\) 项，否则高速接近时会失效。

进阶二：机会约束版——障碍位置不确定（U3 桥）

真实感知给的障碍位置有噪声：\(p_o\sim\mathcal{N}(\hat p_o,\Sigma_o)\)。与其用点估计做硬 CBF，不如要求"碰撞概率 \(\le\delta\)"。最简做法：把障碍**膨胀**一个与不确定性、风险相关的量（用推导补遗的 \(\Phi^{-1}\)）：

def chance_inflated_radius(r, Sigma_o, delta, direction):
    """把障碍半径按位置不确定性 + 风险 δ 膨胀，再喂给(任意)CBF 滤波。
    direction: 机器人指向障碍的单位向量(沿此方向的不确定性最关键)。"""
    from scipy.stats import norm
    import numpy as np
    z = norm.ppf(1 - delta)                            # δ=0.05 -> 1.645
    sigma_dir = np.sqrt(direction @ Sigma_o @ direction)
    return r + z * sigma_dir                           # 膨胀后的"安全半径"
# 用法: r_safe = chance_inflated_radius(r, Sigma_o, 0.05, dir); 再调 hocbf_filter(..., r_safe)

这把"硬避障"软化成"碰撞概率 \(\le\delta\)"：\(\delta\) 越小 → 膨胀越多 → 越保守。它正是 §U2.4 的 GP 方差→机会约束、以及**整个 U3** 的思路在累积项目上的落地。把 \(\delta\) 从 0.1 扫到 0.01，你会看到碰撞率随之下降、到达率随之牺牲——亲手画出"风险旋钮"的权衡曲线。

两个进阶合起来，累积测试台就从"静态障碍 + 硬保证"升级成"动态障碍 + 概率保证"，已经触到 U3（机会约束）与 U4（感知不确定 / belief）的门口。这也再次示范了好测试台的价值：加一个范式只改一个模块，主干不动。

---

综合实战清单：从零搭一个鲁棒导航控制器

把全章串成一份"动手清单"——假设你要给一个移动机器人做"扰动下安全到达目标"的控制器，按下面步骤走，每步标了用本章哪一节 / 哪段代码。

第 0 步：明确不确定性与约束 - 列出不确定性类型：连续有界扰动（→ Tube / 可达性）、可学残差（→ GP）、离散模式（→ 场景树）、感知端（→ U4）。 - 列出硬约束（状态/控制边界、障碍）与安全要求（碰撞概率、最小间距）。 - 决定安全谱上的目标点：要 100% 硬保证，还是接受小概率违反（→ 方法选型决策指南）。

第 1 步：建名义模型与镇定 - 写名义动力学 \(f_{\text{nom}}\)（线性化或简化模型）。 - LQR 设计 \(K\)（实现一 lqr_gain），确认 \(\rho(A+BK)<1\)。

第 2 步：算鲁棒余量（Tube） - 标定扰动集 \(\mathcal{W}\)（用数据，别拍脑袋）。 - 算 RPI 集 \(\Omega\)（实现一 epsilon_mrpi_support，Raković 2005）。 - 收紧约束 \(\mathcal{X}\ominus\Omega\)、\(\mathcal{U}\ominus K\Omega\)，确认名义问题仍可行（否则回第 1 步加强 \(K\) 或缩 \(\mathcal{W}\)）。

第 3 步：搭名义 MPC 并落到求解器 - 写收紧后的名义 MPC（实现一 tube_mpc）。 - 上 acados（§U2.6、补充 worked 的完整 OCP）做 SQP-RTI 实时求解；控制律 \(u=v_0+K(x-z_0)\)。

第 4 步：加安全层（CBF） - 把障碍/安全约束写成 \(h(x)\ge0\)；相对度 1 用一阶 CBF（实现五），相对度 2 用 HOCBF（实现三）。 - 在名义控制后接 CBF-QP 滤波（§U2.5 代码、累积项目 hocbf_filter）；加松弛保证可行性。

第 5 步（可选）：数据驱动减保守（GP-MPC） - 若有系统性残差且能采数据，用 GP 学残差（实现二），均值进动力学、方差进机会约束（推导补遗的 \(\Phi^{-1}\) 收紧）。 - 用稀疏 GP 保实时。

第 6 步（可选）：处理离散不确定性（场景树） - 若有他车意图 / 路面类别等离散模式，上多阶段 MPC（场景树骨架、do-mpc），加非预期约束。

第 7 步：验证与调参 - 蒙特卡洛评测（累积项目 monte_carlo）：碰撞率、越界率、到达率。 - 做消融（baseline → +Tube → +CBF）确认每层有效（消融解读）。 - 按"调参与诊断速查"的顺序调：先镇定、再鲁棒余量、再安全层、最后性能。

第 8 步：上真机前的检查 - 递归可行性：长时域闭环不发散（终端集 / 终端代价，理论补遗）。 - \(\mathcal{W}\) 是否覆盖真机实际扰动（仿真零碰撞 ≠ 真机零碰撞，故障排查最后一条）。 - CBF 在执行器极限附近是否仍可行。

走完这八步，你就有了一个"Tube 留余量 + CBF 兜安全（+ GP 减保守 + 场景树应对离散）"的鲁棒导航控制器——这正是本章所有方法的集成形态，也是真实系统的标准架构。把它和累积项目的测试台拼起来，就是一个可以反复实验、逐步加装范式的完整平台。

---

本章代码索引与项目结构

本章代码不少，这里给一张索引 + 一个可落地的项目结构，方便把零散片段组织成真实工程。

代码片段索引

代码	在哪	作用
lqr_gain / epsilon_mrpi_support	实现一	算 \(K\) 与 RPI 集 \(\Omega\)
tube_mpc	实现一	Rigid Tube 名义 MPC
homothetic_tube_mpc	实现一进阶	Homothetic 缩放管道
gp_mpc_step / gp_mpc_multistep	实现二	GP-MPC 单步 / 多步
cbf_qp_filter（Python）	§U2.5 / 实现五	CBF 安全滤波
cbf_qp_filter（C++）	C++ 部署节	部署版（Eigen+qpOASES）
hocbf_filter / _moving	累积项目 / 进阶	静态 / 动态障碍 HOCBF
chance_inflated_radius	累积项目进阶	机会约束膨胀
scenario_mpc	场景树	多阶段鲁棒 MPC
NavTestbed / monte_carlo / plot_demo	累积项目	测试台 + 评测 + 可视化

建议的项目结构（Python 原型 + C++ 部署双包）

robust_nav/
├── prototype_py/                # Python 原型(研究/验证)
│   ├── tube.py                  # lqr_gain, epsilon_mrpi_support, tube_mpc, homothetic
│   ├── gp_mpc.py                # GP-MPC 单步/多步
│   ├── cbf.py                   # cbf_qp_filter, hocbf_filter(_moving)
│   ├── scenario.py              # scenario_mpc
│   ├── testbed.py               # NavTestbed, monte_carlo
│   └── experiments.py           # plot_demo, compare, 消融, δ 扫描
├── deploy_cpp/                  # C++ 部署(实时上真机)
│   ├── include/cbf_filter.hpp   # cbf_qp_filter (Eigen+qpOASES)
│   ├── src/cbf_node.cpp         # ROS2 实时节点
│   ├── CMakeLists.txt           # 链接 Eigen, qpOASES, rclcpp
│   └── config/params.yaml       # r, gamma, u_max, 障碍参数...
└── acados_gen/                  # acados 生成的 C(可选)
    └── c_generated_code/        # AcadosOcpSolver 生成

衔接工作流：在 prototype_py/ 把算法 / 参数 / 指标跑通 → 把热路径（CBF / MPC）移到 deploy_cpp/ 或用 acados_gen/ → 用 config/params.yaml 让 Python 调好的参数直接喂给 C++，避免两边手抄。这条结构对应"两段式工作流"，也是真实机器人项目的常见骨架。

---

🔧 故障排查手册

鲁棒规划落地时最常见的故障与定位：

症状	根本原因	排查 / 纠正步骤	相关节
名义 MPC 求解 infeasible	约束收紧过度（\(\Omega\) 过大）或收紧与可行域冲突	1. 检查 \(\Omega\) 是否过大（扰动假设是否过悲观、\(K\) 是否太弱）；2. 重设计 \(K\) 减小 \(\Omega\)；3. 必要时放松终端约束	§U2.1/§U2.2
实际轨迹跑出 tube / 越界	约束没收紧、\(\Omega\) 算小、或 \(K\) 未镇定	1. 验证 \(\rho(A+BK)<1\)（谱半径）；2. 检查约束是否真的收紧了 \(\Omega\)；3. 增大 RPI 迭代次数	§U2.1
CBF-QP 返回 infeasible	执行器约束 \(u\in\mathcal{U}\) 与 CBF 约束冲突（刹不住）	1. 给 CBF 约束加松弛变量并重罚；2. 检查执行器极限是否够；3. 提前减速（调 class-K 增益）	§U2.5
机器人贴着障碍飞、险象环生	CBF 的 class-K 增益 \(\gamma\)（或 \(a_1,a_2\)）太大	1. 调小增益让其更早干预；2. 画"最近距离 vs 增益"曲线选合适值	§U2.5
GP-MPC 单步求解越来越慢	完整 GP 推断 \(O(N^3)\) 随数据增长爆炸	1. 改稀疏 GP（诱导点/FITC）；2. 用在线字典控制规模	§U2.4
acados 反馈延迟大、上不去频率	RTI 准备/反馈阶段误用	1. 确认重计算在准备阶段完成；2. 正确使用 phase 接口	§U2.6
仿真零碰撞、真机仍失效	扰动集 \(\mathcal{W}\) 或分布假设与真机不符	1. 用真机数据校准 \(\mathcal{W}\)；2. 加 GP 残差学系统性误差；3. 必要时上 DRO	§U2.4 / U5
蒙特卡洛碰撞率远高于设计	用硬约束处理本质上是概率的事件	1. 区分确定性 vs 概率约束；2. 改用机会约束 + 风险分配	U3
动态障碍下 HOCBF 高速接近时失效	障碍加速度 \(a_o\) 没算进 \(\ddot h\)	补 \(-2\,rel\cdot a_o\) 项；或保守地按最大 \(a_o\) 留余量	累积项目进阶
场景树 MPC 规模爆炸、解不动	分支随时域指数增长	用 robust horizon 限制只前几步分支、之后用 tube 兜	场景节 / do-mpc
Homothetic 管道 \(\alpha_k\) 发散	包含递推 lam/mu 标定错或 \(\rho(A+BK)\ge1\)	重新标定 lam/mu；先保证闭环镇定	§U2.2 / 实现一进阶
C++ 部署后 1 kHz 丢拍、时延抖动	实时回调里动态分配内存或打磁盘日志	预分配缓冲（固定尺寸 Eigen）；异步日志；检查 QoS	C++ 部署节

部署与进阶阶段的专属故障

正文表里偏算法；下面这些是**进阶功能与 C++ 部署**阶段才会撞上的坑：

• C++ 与 Python 结果对不上：数值库差异 + 行/列主序 + 单双精度共同作用。逐模块对拍（同输入比输出），别整体比。
• qpOASES nWSR 用尽：增大 nWSR、或检查问题是否病态/infeasible；实时回路**必须有 fallback**（用上一拍解或安全停车），不能崩。
• acados 生成的 C 编译失败：检查 BLASFEO/HPIPM 依赖与目标平台编译器；先在主机跑通再交叉编译到目标机。
• ROS2 节点延迟大：话题 QoS 不当（队列太长 / Reliable 重传）；改 BestEffort + 小队列，控制节点设实时优先级。
• 移动障碍避让抖动：障碍速度 \(v_o\) 估不准导致相对速度算错。给 \(v_o\) 的不确定性留余量，或退回机会约束膨胀（进阶二）。
• 机会约束膨胀后到不了目标：\(\delta\) 太小、膨胀过度。放松 \(\delta\)，或只在高风险方向膨胀（别各向同性全膨胀）。
• 状态估计延迟拖垮 CBF：CBF 用的是"当前"状态，估计有延迟会让安全判断滞后。补偿延迟（前推一步）或在 \(\mathcal{W}\)/裕度里计入。
• 多障碍 CBF 互相打架：多个 CBF 约束同时紧时 QP 可能 infeasible。按距离只激活最近的几个、或用单一融合的 \(h\)。

部署阶段的总原则：算法在 Python 验证过≠在 C++ 真机上没问题。延迟、数值、实时性、求解器失败这四类问题只在部署时暴露——所以"两段式工作流"的第二段同样要认真测（HIL、真机小步验证）。

---

API 速查表

本章实现常用的库与接口（签名为示意，以官方文档为准）：

LQR 增益（SciPy）

from scipy.linalg import solve_discrete_are
P = solve_discrete_are(A, B, Q, R)
K = -np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)   # u = K x

QP / 安全滤波（cvxpy + OSQP）

u = cp.Variable(m)
prob = cp.Problem(cp.Minimize(cp.sum_squares(u - u_nom)), [G @ u <= h])
prob.solve(solver=cp.OSQP, warm_start=True)   # u.value 为 None 时表示 infeasible

鲁棒 NMPC（acados，要点）

from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel
ocp.solver_options.nlp_solver_type = "SQP_RTI"               # 实时迭代(准备/反馈)
ocp.solver_options.qp_solver       = "PARTIAL_CONDENSING_HPIPM"
ocp.constraints.lbx = x_lb + omega_margin                   # Tube 约束收紧 X⊖Ω
ocp.constraints.ubx = x_ub - omega_margin
solver.set(0, "lbx", x0); solver.set(0, "ubx", x0)          # 当前状态作初值约束
solver.solve(); u0 = solver.get(0, "u")

GP 残差（GPy，要点）

import GPy
k = GPy.kern.RBF(input_dim=d, ARD=True)
m = GPy.models.SparseGPRegression(X, Y, kernel=k, num_inducing=20)  # 稀疏 GP 保实时
m.optimize()
mu, var = m.predict(X_star)        # 均值进动力学, 方差进机会约束收紧

HJ 可达性工具链（按成熟度递进）：helperOC（MATLAB，教学入门）→ optimized_dp（Python/HeteroCL，加速）→ hj_reachability（JAX，最现代，算 TEB/BRT）。

源码精读清单（按"先易后难"）：do-mpc（multi-stage / scenario MPC 的 Python 实现，最易上手）→ acados 的 examples/acados_python/getting_started/（OCP 定义标准流程）→ CBF 类工具（如各家 CBF-QP / HOCBF 的开源实现，对照 §U2.5 的安全滤波）→ hj_reachability 的示例（可达性 / TEB 计算）→ leggedrobotics/ocs2 的 ocs2_mpc/（SLQ/DDP MPC 框架）→ qiayuanl/legged_control（OCS2 + WBC 完整四足栈，看 MPC→WBC 信号流）。

---

调参与诊断速查

鲁棒规划落地，一半工作是调参。这里把"哪个旋钮管什么、调错了什么症状"集中成一张速查表——配合故障排查手册用。

旋钮	控制什么	调大的效果	调小的效果	调错的典型症状
扰动集 \(\mathcal{W}\) 大小	鲁棒余量的来源	更保守、\(\Omega\) 更大	更进取、可能不够鲁棒	设小→真机越界；设大→过保守到不了目标
LQR 权重 \(Q,R\)（定 \(K\)）	反馈强度 → \(\Omega\) 大小	\(Q\) 大→\(K\) 强→\(\Omega\) 小	\(K\) 弱→\(\Omega\) 大	\(K\) 太弱→\(A+BK\) 不够稳→\(\Omega\) 发散
RPI 迭代次数 / \(\alpha\)	\(\Omega\) 逼近精度	更精确、略保守	更快、可能欠逼近	太少→\(\Omega\) 偏小→实际跑出 tube
预测时域 \(N\)	前瞻长度	更优、更慢	更快、可能短视	太短→不可行 / 震荡
终端集 / 终端代价	递归可行性	—	—	缺失或过小→长时域闭环发散
CBF 增益 \(\gamma\)（\(\alpha_1,\alpha_2\)）	刹车时机	允许更晚刹、更贴边	更早干预、更保守	太大→贴障碍危险；太小→寸步难行
CBF 松弛权重	可行性兜底	更易可行、可能轻微越界	更硬、易 infeasible	太硬→QP infeasible 卡死
机会约束 \(\delta\)	风险容忍	更激进、违反概率高	更保守、收紧多	与实际风险不符→碰撞率偏离设计
GP 诱导点数	精度 vs 实时	更准、更慢	更快、可能欠拟合	太多→跑不到控制频率

调参顺序建议（先粗后细）： 1. 先保证**镇定**：调 \(Q,R\) 让 \(A+BK\) 谱半径明显 <1（否则 \(\Omega\) 都算不出来）。 2. 再保证**鲁棒余量**：用真机/仿真数据标定 \(\mathcal{W}\)，算 \(\Omega\)，确认收紧后名义问题仍可行。 3. 再加**安全层**：CBF 增益从保守（小 \(\gamma\)）起调，逐步放开到任务能完成又不贴边。 4. 最后调**性能**：在安全/鲁棒满足的前提下，调代价权重、时域 \(N\)、\(\delta\) 提升效率。

黄金法则：安全/鲁棒的旋钮（\(\mathcal{W}\)、\(\Omega\)、CBF 增益、终端集）先调到"绝不出事"，再用性能旋钮（代价、\(N\)、\(\delta\)）在剩余空间里要效率。反过来——先调性能再补安全——几乎总会翻车。

---

本章 FAQ：高频疑问速答

读这一章时最常冒出的问题，集中速答。

Q：Tube MPC 和 CBF 到底用哪个？ 不是二选一——Tube 在规划层留扰动余量、CBF 在控制层兜瞬时安全，真实系统常两个一起用（累积项目就是）。

Q：\(\Omega\) 算出来很大怎么办？ 说明 \(K\) 太弱或 \(\mathcal{W}\) 太大。先加强 \(K\)（更大 \(Q\)，让 \(A+BK\) 谱半径更小），或重新标定 \(\mathcal{W}\)（别拍脑袋设大）。

Q：CBF-QP 经常 infeasible？ 多半是控制约束 \(\mathcal{U}\) 太紧、CBF 增益太大、或多个约束冲突。加松弛变量（软约束）兜底，或检查 \(\gamma\) 是否太大导致太晚刹车。

Q：GP-MPC 跑不到控制频率？ 用稀疏 GP（诱导点）、只学残差（不学整个动力学）、把 GP 推理与 MPC 求解分阶段优化。

Q：相对度判断错了会怎样？ 实际相对度 >1 却用一阶 CBF，约束里根本没有 \(u\)（\(L_gh=0\)），QP 无法影响安全——必须用 HOCBF（实现三）。

Q：可达性方法为什么不常用？ 网格 HJ 受维度诅咒（>5–6 维不可行）；SOS（RTD/Funnel）能扩维但门槛高。工程上 Tube+CBF 更轻量，可达性多用于离线证书或低维强保证场景。

Q：机会约束的高斯假设不成立怎么办？ 用更保守的集中不等式（Chebyshev/Cantelli）或采样/场景法——这是 U3 的内容。

Q：仿真零碰撞、真机却撞了？ 最常见是 \(\mathcal{W}\) 没覆盖真机实际扰动、或状态估计有偏差/延迟。回去扩 \(\mathcal{W}\)、查感知-控制延迟。

Q：一定要用 acados / C++ 吗？ 不。原型用 Python + cvxpy/scipy 足够；只有要实时/嵌入式上真机才需要 acados（或 OSQP/qpOASES）与 C++。先验证算法、再上高性能求解器与部署语言。

Q：这章的方法能直接用到机械臂 / 无人机吗？ 思想通用（误差集 + 收紧/滤波），但 \(h\)、动力学、相对度要按具体系统改——见方向落地案例。

Q：Homothetic / Elastic 值得上吗？ 多数情况 Rigid 够用且最简；只有当 Rigid 的恒定收紧让问题不可行或明显太保守、且你有在线算力时才升级。

Q：CBF 的 \(\gamma\)（class-K 增益）怎么定？ 从保守（小 \(\gamma\)，早刹车）起调，逐步放大到任务能完成又不贴障碍。离散实现时 \(\gamma\) 要结合采样周期重新整定。

还有疑问？回到对应小节的"本质洞察"与"常见误解"，多数疑问那里有更展开的答案。

---

本章常见误解汇总

误解	正确理解
Tube MPC 在线和名义控制一起优化反馈增益 \(K\)	\(K\) 离线固定，在线只优化名义控制 \(v\)（§U2.1）
加大安全裕度就等于做了鲁棒	裕度应由 \(\Omega\) 推导、随不确定性自适应（§U2.1 / U0）
\(\Omega\) 越小越好，必须算到最小 RPI	适中的外逼近就够；要减小 \(\Omega\) 应改 \(K\) 而非死磕逼近精度（§U2.1）
Elastic Tube 更先进所以工程上更好	保守度低但在线 QP 大；嵌入式实时首选 Rigid（§U2.2）
FaSTrack 的 TEB 能算任意高维系统	受 HJ 维度诅咒，直接应用约 ≤5D（§U2.3）
可达性解耦了规划器 = 可用任意烂规划器	解耦的是安全证书，不是规划质量（§U2.3）
GP-MPC 就是用 GP 替换动力学 \(f\)	用均值修正 \(f\)、用方差驱动保守度（§U2.4）
GP 方差大就是数据不够	可能是 aleatoric 噪声地板，采数据无用（§U2.4 / U0 §2）
CBF 保证从任意状态恢复安全	只保证前向不变（从安全集内出发留在集内）（§U2.5）
CBF 的 class-K 增益越大越安全	它是"刹车时机"旋钮，不是"安全程度"旋钮（§U2.5）
acados 靠自动代码生成获得性能	刻意不依赖 codegen，靠模块化 + C 库（BLASFEO/HPIPM）（§U2.6）
CBF 是用来取代 MPC 的	层级协作：MPC 给方向、CBF 给护栏（§U2.5）
真机部署必须整体用 C++	Python 验证算法、C++ 只重写热路径（实时回路）即可（C++ 部署节）
Python 写的所以算法本身慢	重活在 C 核（acados/OSQP/HPIPM），Python 只是编排（C++ 部署节）
静态障碍的 CBF 能直接用于移动障碍	要用相对速度、必要时算障碍加速度，否则高速失效（累积项目进阶）
机会约束 = 把障碍随便放大一圈	膨胀量由 \(\Phi^{-1}(1-\delta)\) 与方差决定、且应沿高风险方向（进阶二）
场景树枚举越多场景越好	取不确定区间边界 + robust horizon 限分支即可，否则规模爆炸（do-mpc）
仿真验证通过就能上真机	延迟 / 数值 / 实时 / 求解失败只在部署暴露，第二段同样要测（部署故障）
可微 MPC 是把 MPC 变慢的玩具	是把 MPC 接进学习回路（RL 调参 / 作 actor）的桥（工具拓展 / 前沿）
HOCBF 比一阶 CBF "更安全"	不是更安全，是处理相对度 >1（一阶 CBF 此时根本无效）（实现三）
调参先调性能再补安全	反了：先调到"绝不出事"，再用性能旋钮要效率（调参速查）

---

本章小结

本章把"给名义控制加上应对扰动的能力"拆成两条互补路线讲透：事前在规划里预留余量（Tube MPC / 可达性 / GP-MPC）与**事中在控制输出端实时纠偏**（CBF 安全滤波），并落到工业级求解器 acados。贯穿全章的主线是 U0 的"鲁棒 = 对扰动集内最坏情况买单、保守度应由不确定性模型推导"。

一条主线回顾：一个概念串起六种方法

如果只带走一个东西，应该是这个统一视角：本章六种方法，本质上都在和同一个对象打交道——"被有界不确定性撑开、被反馈/收紧拉住的误差集"。

• Tube MPC 的 RPI 集 \(\Omega\)、可达性的 TEB / FRS / 漏斗、Funnel 的李雅普诺夫水平集——§U2.1、§U2.3 里这些名字不同的对象，在实现一的标量例（\(\Omega\approx0.162\)）、实现四的 TEB 走查、Funnel 的 1D 走查（\(|e|\le\bar w/k\)）里被反复算出来，它们其实是**同一个量**：偏差被困住的不变集。
• 把这个误差集**绑进 MPC 在线收紧**，就是 Tube MPC（§U2.1）；离线算好、在线膨胀障碍，就是可达性（§U2.3）；让它的大小**随数据自适应**（用 GP 方差），就是 GP-MPC（§U2.4）；不在规划里预留、而在控制输出端用它**实时纠偏**，就是 CBF 安全滤波（§U2.5）。
• acados（§U2.6）是把上述任何一种落到真机控制频率的工具；场景树（连 U1）则把"连续误差集"换成"离散场景 + 共享初始动作"。

所以这一章不是六个孤立技巧，而是**同一个"鲁棒不变误差集"思想的六种工程化身**。理解了这条主线，下次遇到一个新的鲁棒控制方法，你第一个该问的就是：它的"误差集"是什么、怎么算、绑进求解器还是解耦、固定还是自适应？

术语速查表

术语	含义
RPI 集 \(\Omega\)	鲁棒正不变集——偏差进入后永不离开的"管道横截面"
Minkowski 和 \(\oplus\) / 差 \(\ominus\)	集合相加 / 向内腐蚀；约束收紧用 \(\ominus\)
mRPI	最小 RPI 集 \(\bigoplus_{i\ge0}(A+BK)^i\mathcal{W}\)，一般取外逼近
名义系统 \(z,v\)	无扰动的理想状态/控制（Tube 的"名义"分量）
tube 反馈 \(K\)	离线设计的镇定增益，把真实状态拉回名义轨迹
约束收紧 \(\mathcal{X}\ominus\Omega\)	名义轨迹的可行域向内收 \(\Omega\)，给偏差留空间
TEB	跟踪误差集（FaSTrack，由 HJ 可达性算）
FRS	前向可达集（RTD，由 SOS 算）
漏斗 funnel	不变集随机动演化的包络（SOS 李雅普诺夫）
协方差传播 \(\Sigma_k\)	状态分布不确定性沿预测时域的前传
机会约束 \(\delta\)	违反概率上界；\(\Phi^{-1}(1-\delta)\) 决定收紧量
CBF \(h(x)\)	控制屏障函数，安全集 \(\mathcal{C}=\{h\ge0\}\)
class-K 函数 \(\alpha(\cdot)\)	CBF 条件中的函数（与 CVaR 的 \(\alpha\)、Homothetic 的 \(\alpha_k\) 区分）
HOCBF	高阶 CBF，处理相对度 >1（控制不在 \(\dot h\) 中出现）
SQP-RTI	实时迭代：准备 + 反馈两阶段，压低 NMPC 延迟
HPIPM / BLASFEO	acados 的内点法 QP 求解器 / 高性能线性代数库

知识点总表

方法	核心思想	安全保证	在线计算	工程定位
Tube MPC（§U2.1）	名义系统 + 不变管道，约束收紧	扰动集内 100%	低（名义 QP）	嵌入式实时首选
三代 Tube（§U2.2）	rigid/homothetic/elastic	同上，保守度递减	递增	Rigid 默认，余量足再升级
可达性（§U2.3）	离线算误差包络 + 在线膨胀障碍	包络内 100%	离线重 / 在线轻	与规划器解耦
GP-MPC（§U2.4）	学残差（均值+方差）+ 机会约束	概率 \(\ge1-\delta\)	中（稀疏 GP）	数据驱动、自适应减保守
CBF-QP（§U2.5）	最小修正使安全集前向不变	前向不变	极低（微秒 QP）	高频安全层 / 给 RL 兜底
acados（§U2.6）	模块化嵌入式 OCP 求解器	（落地工具）	—	上述各法的工业级实现平台

---

本章核心公式速查

把全章最该记住的公式集中放一处，便于回查。

• 状态分解（Tube）：\(x_k=z_k+e_k\)，名义 \(z\) + 偏差 \(e\)。
• 偏差动力学：\(e_{k+1}=(A+BK)e_k+w_k\)。
• RPI 集（mRPI）：\(\Omega=\bigoplus_{i=0}^{\infty}(A+BK)^i\mathcal{W}\)；闭环需谱半径 \(\rho(A+BK)<1\)。
• 约束收紧：状态 \(z\in\mathcal{X}\ominus\Omega\)，控制 \(v\in\mathcal{U}\ominus K\Omega\)。
• Tube 控制律：\(u_k=v_k+K(x_k-z_k)\)。
• ε-mRPI 外逼近（Raković 2005）：选 \(s\) 使 \((A+BK)^s\mathcal{W}\subseteq\alpha\mathcal{W}\)，则 \(\Omega_\alpha=(1-\alpha)^{-1}\bigoplus_{i=0}^{s-1}(A+BK)^i\mathcal{W}\)。
• 支撑函数（Minkowski 和）：\(h_{A\oplus B}(c)=h_A(c)+h_B(c)\)；对称盒 \(h_{\mathcal{W}}(c)=\sum_j\bar w_j|c_j|\)。
• GP 组合模型：\(x_{k+1}=f_{\text{nom}}(x_k,u_k)+d_k\)，\(d_k\sim\mathcal{GP}(\mu_d,\Sigma_d)\)。
• 协方差传播（线性化）：\(\Sigma_{k+1}=J_k\Sigma_k J_k^\top+\Sigma_d\)。
• 机会约束收紧（高斯）：\(\Pr(c^\top x\le b)\ge1-\delta\iff c^\top\mu\le b-\Phi^{-1}(1-\delta)\sqrt{c^\top\Sigma c}\)。
• CBF 条件：\(\dot h(x,u)\ge-\alpha(h(x))\)；控制仿射下 \(L_f h+L_g h\,u\ge-\alpha(h)\)。
• CBF-QP：\(u^\*=\arg\min_u\|u-u_{\text{nom}}\|^2\) s.t. \(L_f h+L_g h\,u\ge-\alpha(h),\ u\in\mathcal{U}\)。
• HOCBF（相对度 2）：\(\ddot h+(\alpha_1+\alpha_2)\dot h+\alpha_1\alpha_2 h\ge0\)。
• Funnel 半径（1D 李雅普诺夫）：\(|e|\le\bar w/k\)（\(V=e^2,\ \dot e=-ke+w\)）。

这些公式的共同结构是：误差集 / 不确定性 → 收紧或约束 → 可解的 QP/MPC。记住这个结构，比记住每个符号更重要。

---

本章符号速查

本章符号较多，集中列出（尤其注意 \(\alpha\) 在不同语境的不同含义）。

符号	含义
\(x_k,\ z_k,\ e_k\)	真实状态、名义状态、偏差（\(x_k=z_k+e_k\)）
\(u_k,\ v_k\)	真实控制、名义控制（\(u_k=v_k+Ke_k\)）
\(A,\ B\)	名义线性动力学矩阵 \(z_{k+1}=Az_k+Bv_k\)
\(K\)	tube 反馈增益（LQR 设计，定 \(\Omega\) 大小）
\(\mathcal{W}\)	扰动集（有界）
\(\Omega\)	RPI / mRPI 集（偏差被困住的不变集，即管道横截面）
\(\bar\Omega,\ \alpha_k\)	Homothetic 的基准管道与**缩放标量**（此 \(\alpha_k\) 是标量）
\(\mathcal{X},\ \mathcal{U}\)	状态、控制约束集；收紧后为 \(\mathcal{X}\ominus\Omega\)、\(\mathcal{U}\ominus K\Omega\)
\(\oplus,\ \ominus\)	Minkowski 和、Pontryagin 差
\(\rho(\cdot)\)	谱半径（要求 \(\rho(A+BK)<1\)）
\(h(x)\)	CBF / 安全函数（安全集 \(\{x:h(x)\ge0\}\)）
\(\alpha(\cdot)\)	CBF 中的 class-K 函数（是函数，与 Homothetic 的标量 \(\alpha_k\) 不同）
\(\gamma,\ \alpha_1,\alpha_2\)	CBF / HOCBF 的 class-K 增益（刹车时机旋钮）
\(\delta\)	机会约束的风险水平（\(\Pr(\text{违反})\le\delta\)）
\(\mu,\ \Sigma\)	（GP / 状态）分布的均值、协方差
\(\Phi^{-1}(\cdot)\)	标准正态分位数（机会约束收紧系数）
\(J_k\)	协方差传播中的雅可比

最易混的 \(\alpha\)：CBF 里 \(\alpha(\cdot)\) 是 class-K 函数；Homothetic Tube 里 \(\alpha_k\) 是每步的缩放**标量**；到了 U5，\(\alpha\) 又会指 CVaR 的置信水平。三者无关，务必看语境区分——这是整个不确定性规划簇最常踩的符号坑。

---

实践索引与一页纸总览

把全章压成两张可快速查阅的表，做项目时直接定位。

实践索引：遇到什么用什么、去哪查

你的情况	用哪个方法	本章位置
有界扰动、要嵌入式实时	Rigid Tube MPC	§U2.1、实现一
余量紧、要降保守度	Homothetic / Elastic Tube	§U2.2、homothetic 实现
给任意控制器 / RL 兜避障底线	CBF-QP 安全滤波	§U2.5、实现五、累积项目
位置约束、加速度控制（相对度 2）	HOCBF	实现三
有系统残差、能采数据	GP-MPC	§U2.4、实现二
要解耦规划器、离线算力足	FaSTrack / RTD / Funnel	§U2.3、实现四
离散意图 / 模式	场景树 / 多阶段 MPC	场景骨架、do-mpc
把鲁棒 MPC 落到真机频率	acados（SQP-RTI+HPIPM）	§U2.6、补充 worked
分布半知半解	DRO	理论补遗
把 MPC 接进学习回路	可微 MPC	工具拓展、前沿

方法在五安全谱上的定位（呼应 U0）

方法	安全谱位置	保证类型
Tube / 可达性 / CBF	最保守端（鲁棒 100%）	对扰动集的硬保证
GP-MPC	鲁棒 ↔ 机会约束之间	概率保证（高斯近似）
机会约束（→ U3）	机会约束端	\(\Pr(\text{违反})\le\delta\)
DRO	鲁棒 ↔ 随机之间	分布族最坏情况
CVaR / 风险敏感（→ U5）	尾部风险端	风险度量

一句话总览：本章覆盖五安全谱从"鲁棒 100%"到"机会约束"的这一段；六种方法都在算同一个"鲁棒不变误差集"，区别在于怎么算、绑进求解器还是解耦、固定还是自适应、硬保证还是概率保证。带着这张地图，你既能为手头问题选对方法，也能顺利过渡到 U3（机会约束）、U4（信念）、U5（风险敏感）。

---

延伸阅读

出处均已逐一核实；难度：⭐⭐ 入门精读，⭐⭐⭐ 需相应基础，⭐⭐⭐⭐ 研究级。

Tube MPC - P. O. M. Scokaert, D. Q. Mayne. "Min-max feedback model predictive control for constrained linear systems." IEEE TAC 43(8):1136–1142, 1998.（min-max 起点）⭐⭐⭐ - W. Langson, I. Chryssochoos, S. V. Raković, D. Q. Mayne. "Robust model predictive control using tubes." Automatica 40(1):125–133, 2004.（Tube 思想前身）⭐⭐⭐ - D. Q. Mayne, M. M. Seron, S. V. Raković. "Robust model predictive control of constrained linear systems with bounded disturbances." Automatica 41(2):219–224, 2005.（Rigid Tube 奠基）⭐⭐⭐ - S. V. Raković, E. C. Kerrigan, K. I. Kouramas, D. Q. Mayne. "Invariant approximations of the minimal robustly positively invariant set." IEEE TAC 50(3):406–410, 2005.（mRPI 计算）⭐⭐⭐⭐ - S. V. Raković, B. Kouvaritakis, R. Findeisen, M. Cannon. "Homothetic tube model predictive control." Automatica 48(8):1631–1638, 2012.⭐⭐⭐⭐ - S. V. Raković, W. S. Levine, B. Açıkmeşe. "Elastic Tube Model Predictive Control." ACC 2016, pp. 3594–3599.⭐⭐⭐⭐

可达性方法 - S. L. Herbert, M. Chen, S. Han, S. Bansal, J. F. Fisac, C. J. Tomlin. "FaSTrack: A modular framework for fast and guaranteed safe motion planning." IEEE CDC 2017, pp. 1517–1522（期刊扩展版 arXiv:2102.07039, 2021）.⭐⭐⭐ - S. Kousik, S. Vaskov, F. Bu, M. Johnson-Roberson, R. Vasudevan. "Bridging the Gap Between Safety and Real-Time Performance in Receding-Horizon Trajectory Design for Mobile Robots." IJRR, 2020（arXiv:1809.06746）.⭐⭐⭐ - A. Majumdar, R. Tedrake. "Funnel libraries for real-time robust feedback motion planning." IJRR 36(8):947–982, 2017（arXiv:1601.04037）.⭐⭐⭐⭐

GP-MPC - L. Hewing, J. Kabzan, M. N. Zeilinger. "Cautious Model Predictive Control Using Gaussian Process Regression." IEEE TCST 28(6):2736–2743, 2020（arXiv:1705.10702）.⭐⭐⭐ - J. Kabzan, L. Hewing, A. Liniger, M. N. Zeilinger. "Learning-Based Model Predictive Control for Autonomous Racing." IEEE RA-L 4(4):3363–3370, 2019.⭐⭐⭐ - G. Torrente, E. Kaufmann, P. Foehn, D. Scaramuzza. "Data-Driven MPC for Quadrotors." IEEE RA-L, 2021（arXiv:2102.05773）.⭐⭐⭐

CBF 与安全滤波 - A. D. Ames, X. Xu, J. W. Grizzle, P. Tabuada. "Control Barrier Function Based Quadratic Programs for Safety Critical Systems." IEEE TAC 62(8):3861–3876, 2017（arXiv:1609.06408）.（CBF-QP 奠基）⭐⭐⭐ - W. Xiao, C. Belta. "High-Order Control Barrier Functions." IEEE TAC 67(7):3655–3662, 2022.（HOCBF）⭐⭐⭐⭐ - R. Grandia, A. J. Taylor, A. D. Ames, M. Hutter. "Multi-Layered Safety for Legged Robots via Control Barrier Functions and Model Predictive Control." IEEE ICRA 2021, pp. 8352–8358（arXiv:2011.00032）.⭐⭐⭐ - Q. Liao, Z. Li, A. Thirugnanam, J. Zeng, K. Sreenath. "Walking in Narrow Spaces: Safety-Critical Locomotion Control for Quadrupedal Robots with Duality-Based Optimization." IEEE/RSJ IROS 2023, pp. 2723–2730.（legged_control 的安全运动工作）⭐⭐⭐⭐

求解器 - R. Verschueren, G. Frison, D. Kouzoupis, J. Frey, N. van Duijkeren, A. Zanelli, B. Novoselnik, T. Albin, R. Quirynen, M. Diehl. "acados—a modular open-source framework for fast embedded optimal control." Mathematical Programming Computation 14:147–183, 2022（arXiv:1910.13753）.⭐⭐⭐

前沿（经典 × 学习） - A. Romero, Y. Song, D. Scaramuzza（及 Aljalbout）. "Actor-Critic Model Predictive Control." IEEE ICRA 2024 / IEEE T-RO 2025（arXiv:2306.09852）.⭐⭐⭐⭐ - A. Lin, S. Peng, S. Bansal. "One Filter to Deploy Them All: Robust Safety for Quadrupedal Navigation in Unknown Environments." arXiv:2412.09989, 2024.⭐⭐⭐⭐ - L. Yang, B. Werner, M. de Sa, A. D. Ames. "CBF-RL: Safety Filtering Reinforcement Learning in Training with Control Barrier Functions." arXiv:2510.14959, 2025.⭐⭐⭐⭐ - B. Amos, I. D. Jimenez, J. Sacks, B. Boots, J. Z. Kolter. "Differentiable MPC for End-to-end Planning and Control." NeurIPS 2018.（可微 MPC 起点）⭐⭐⭐⭐

教材 - J. B. Rawlings, D. Q. Mayne, M. M. Diehl. Model Predictive Control: Theory, Computation, and Design.（鲁棒/随机 MPC 标准教材）⭐⭐⭐

---

综合练习与项目挑战

各节的练习偏单点；这里给几个**贯穿全章**的综合任务，适合作为阶段性项目或面试准备。

1. 三法对决（控制变量）：在累积项目测试台上，分别用 (i) 纯 Rigid Tube、(ii) 纯 HOCBF 安全层、(iii) Tube+HOCBF 跑同一组蒙特卡洛（同种子、同扰动、同障碍），画三条"碰撞率 / 越界率 / 到达率"对比柱，并用消融解读那一节的逻辑解释每根柱的高低。目标：亲手验证"Tube 管守约束、CBF 管避障"。
2. 从硬约束到机会约束（接 U3）：把累积项目里 HOCBF 的硬避障换成"碰撞概率 ≤ δ"——给障碍距离加一个高斯不确定性，用推导补遗的 \(\Phi^{-1}(1-\delta)\) 收紧。对比 δ=0.1 / 0.05 / 0.01 下的碰撞率与到达率，体会"风险旋钮"。
3. 给 RL 兜底（接前沿）：训练（或手写）一个会偶尔朝障碍冲的"名义策略"，套上 §U2.5 的 CBF-QP 滤波，统计滤波器介入的频率与介入时的修正幅度。目标：理解"最小侵入式安全"——平时不动、危急才出手。
4. 复现一个数据点（接 GP-MPC）：用实现二的 GP-MPC 框架，在一个带未建模阻力的 1D/2D 系统上定量复现"GP 修正后跟踪误差下降"的趋势（不必到 70%，看到明显下降即可），并可视化 GP 方差在数据稀疏处如何增大。
5. 选型论证题：给定三个场景——(a) 嵌入式四足、1 kHz、要给 RL 策略兜底；(b) 全尺寸赛车、有轮胎残差数据、几十 Hz；(c) 仓库 AGV、离线地图、要换不同规划器——分别论证你会选哪套方法组合并说明理由（用选型决策指南的四个问题作框架）。

这些题的共同点是**把多个方法放进同一个测试台对比**，而非孤立实现单个算法——这是把本章知识转化为工程能力的关键一步。

---

本章各节关系

小节	与本章其他节/其他章的关系
§U2.1 Tube 本质	全章地基；其 RPI 集 \(\Omega\) 与 §U2.3 的 TEB/FRS/漏斗同源
§U2.2 三代 Tube	§U2.1 的延伸（减保守度）；体现 U0 的"保守度↔计算量"权衡
§U2.3 可达性	与 §U2.1 互补（解耦 vs 绑进求解器）；前沿"One Filter"是其学习化版本
§U2.4 GP-MPC	数据驱动减不确定性；其协方差+机会约束是 U3 的预演
§U2.5 CBF-QP	控制层安全；给 RL 兜底连接 U0 §6；前沿"CBF-RL"把它放进训练
§U2.6 acados	上述各法的统一落地平台；Multi-Phase OCP 衔接 U1
全章	上承 U0（五安全谱最保守端），下接 U3（硬约束→机会约束）、U4（加感知端不确定性）、附录（同台 benchmark）；向方向层输出四足（腿足 MPC+CBF+WBC）、无人机（GP-MPC、鲁棒 NMPC）的安全基础

---

本章与后续章节的关系

上一节的"本章各节关系"梳理章内脉络；这里专门列出本章如何为后续章节铺垫。

后续章节	关系	本章铺垫的知识点
U3 机会约束	把 Tube 的"对有界扰动 100% 硬保证"放松成"违反概率 ≤δ"；§U2.4 的 \(\Phi^{-1}(1-\delta)\) 收紧、GP-MPC 的协方差传播是 U3 整章的基石	约束收紧思想、\(\Phi^{-1}\) 收紧推导、协方差传播、GP-MPC（= 数据驱动 CC 的前身）
U4 POMDP / Belief	本章假设状态可测、扰动在执行端；U4 转向感知端不确定性与主动感知。CBF/Tube 可作为 belief 空间策略的安全兜底层	CBF 安全滤波（给不可证明安全的策略兜底）、安全集与前向不变性
U5 风险敏感 / CVaR	从"对最坏有界扰动鲁棒"过渡到"对尾部风险敏感"；理论补遗的 DRO 与 U5 的 CVaR-DRO 对偶直接相连	min-max 鲁棒、DRO（分布鲁棒）、理论补遗的分布鲁棒 MPC
附录（基准与对照）	把 Tube / CBF 与 U3–U5 各范式放同一测试台横向 benchmark	累积项目测试台、Tube-vs-名义消融、安全谱定位

一句话：本章是不确定性规划"执行端 + 硬保证"这一极的支柱（工程生态最强）。它向 U3 输出"约束收紧"的全套机制（Tube 是 CC 在 δ→0、扰动有界下的极限），向 U4 输出 CBF 安全层，向 U5 输出 min-max/DRO 的鲁棒根基。

---

研究与实践建议

给工程师：先在累积项目里把"baseline → 加 CBF 安全层 → 补全 Tube"这条路走一遍——你会亲眼看到碰撞率怎么从高降到零。工程落地的优先级是 §U2.5（CBF-QP，最快见效、可给任意控制器兜底）+ §U2.6（acados，把 Tube/机会约束落到实时）；Rigid Tube（§U2.1–U2.2）是嵌入式鲁棒 MPC 的默认选择，无需一上来就追求 Homothetic/Elastic 的低保守度。GP-MPC（§U2.4）在"模型有系统性残差且可采数据"时收益最大。

给研究者：本章的开放问题集中在"学习化安全"——可微 MPC 作 RL actor（Actor-Critic MPC）、HJ 值网络作通用滤波器（One Filter）、训练期内化 CBF（CBF-RL）三条线都很活跃，但它们的**形式化保证**仍弱于经典 Tube/CBF。一个值得深挖的方向是：如何把 GP/神经网络残差的不确定性（含 epistemic/aleatoric 分解）**带保证地**传给 Tube/CBF/机会约束，让"学到的不确定性"真正转化为"可证明的安全裕度"，而非仅经验有效。§U2.4 的 GP 方差→机会约束收紧是这个方向最成熟的一步，往神经网络推广是前沿。

给初学者的学习路径：建议按"先拿到直觉、再补严格、最后上手"三步走。第一步只读三个⭐⭐⭐节的"动机 + 本质洞察"——§U2.1（Tube=名义+管道）、§U2.5（CBF=安全版李雅普诺夫）、§U2.4（GP-MPC=均值修正+方差驱动），先建立"鲁棒不变误差集"这条主线。第二步回到 §U2.1 把 7 步推导和实现一的标量例（黄金比 LQR、\(\Omega\approx0.162\)）逐行算一遍——这是全章最该吃透的一节，其余方法都是它的变体。第三步动手：先写 §U2.5 的 CBF-QP（最短、最快见效），再做累积项目的"baseline→CBF→Tube"消融，最后挑一道综合练习。§U2.2（三代 Tube 选型）、§U2.6（acados）可在需要时再回看，§U2.3（可达性）作为拓宽视野的并行线。整章按"§U2.1 → §U2.5 → 累积项目 → §U2.4 → 其余"的顺序学，比从头顺读更高效。

学完本章你应当能：说清五种范式各自的"误差集"是什么、怎么算、绑进求解器还是解耦、固定还是自适应；手写一个 CBF-QP 与一个 Rigid Tube MPC；在测试台上做出"加某一层→碰撞率下降"的控制变量实验；并对一个新的鲁棒控制问题给出有理有据的方法选型。带着这些能力，你就可以进入 U3（机会约束）把"硬保证"放松成"概率保证"了。

---

版本信息速查

本章代码以 Python 原型 + C++ 部署双线给出。下表汇总所基于的主流库/框架版本（版本号为**建议最低值**，向后兼容多数更高版本；实际以你的环境为准）与各核心方法的出处年份。方法年份与出处已在"延伸阅读"逐一核实。

库 / 框架版本（指示性最低值）

库 / 框架	建议版本	本章用途
Python	3.10+	原型语言
NumPy	1.24+	矩阵 / 集合运算 / 协方差
SciPy	1.10+	solve_discrete_are（LQR）、norm.ppf（\(\Phi^{-1}\)）
CVXPY	1.4+	Tube / CBF / 场景 MPC 的 QP 建模
OSQP	0.6+	QP 后端（收紧名义 MPC、CBF-QP）
qpOASES	3.2+	C++ 部署的 QP 后端（CBF 滤波热路径）
scikit-learn	1.3+	高斯过程（GP-MPC，§U2.4）
acados	0.3+	工业级实时 NMPC（SQP-RTI + HPIPM，§U2.6）
CasADi	3.6+	acados 的符号建模后端
Eigen	3.4+	C++ 部署的线性代数
OCS2	—	腿足 NMPC-WBC 框架（源码精读，随仓库版本）

核心方法出处年份（已核实）

方法	年份	出处
Rigid Tube MPC	2005	Mayne–Seron–Raković, Automatica 41(2)
mRPI 外逼近	2005	Raković–Kerrigan–Kouramas–Mayne, IEEE TAC 50(3)
CBF-QP 安全滤波	2017	Ames–Xu–Grizzle–Tabuada, IEEE TAC 62(8)
高阶 CBF（HOCBF）	2022	Xiao–Belta, IEEE TAC 67(7)
多层 CBF（MPC+WBC）	2021	Grandia–Taylor–Ames–Hutter, IEEE ICRA
GP-MPC（数据驱动鲁棒）	2020	Hewing–Kabzan–Zeilinger, IEEE TCST 28(6)

注：本章算法对库版本不敏感（核心是集合运算、约束收紧、凸求解与协方差传播）；真正影响结果的是**求解器选择与实时性**（acados/OSQP/qpOASES）以及**扰动集 / 分布假设是否贴合真机**，而非具体版本号。

---

附录：前置自测参考答案

回到开头那 5 道自测题，这里给参考答案——读完全章再核对一遍，能检验你是否真正吃透了支柱概念。

1. 标准名义 NMPC：\(\min_{u_{0:N-1}}\ \sum_{k=0}^{N-1}\ell(x_k,u_k)+\phi(x_N)\) s.t. \(x_{k+1}=f(x_k,u_k)\)（动力学）、\(x_k\in\mathcal{X}\)（状态约束）、\(u_k\in\mathcal{U}\)（控制约束）、\(x_0=\) 当前测得状态。每个控制周期：以当前状态为初值解这个有限时域优化，只执行第一个控制 \(u_0\)，下一周期拿新状态重解（receding horizon）。Tube MPC 的"名义系统"分量就是这个问题的无扰动版。
2. Minkowski 和：\(\mathcal{A}\oplus\mathcal{B}=\{a+b:a\in\mathcal{A},b\in\mathcal{B}\}\)。\([-1,1]\oplus[-2,2]=[-3,3]\)（区间端点相加）。它是 RPI 集 \(\Omega=\bigoplus(A+BK)^i\mathcal{W}\) 与约束收紧 \(\mathcal{X}\ominus\Omega\) 的基础运算。
3. 稳定性条件：\(e_{k+1}=Me_k\) 渐近稳定 \(\iff\) \(M\) 所有特征值的模 \(<1\)，即谱半径 \(\rho(M)<1\)。\(\rho(M)\) 决定收敛速度（越小越快），\(\rho\ge1\) 则不收敛。这正是 Tube 要求闭环 \(\rho(A+BK)<1\)（否则 \(\Omega\) 的等比级数发散）的根据。
4. QP 复杂度：凸 QP（\(H\succeq0\)）可用内点法 / active-set / OSQP 等求解，复杂度多项式；小规模（几到几十个变量）能做到**微秒级**，因为问题小、可 warm-start、求解器高度优化。这是 CBF-QP 能跑 1 kHz+ 高频安全滤波的原因。
5. 李雅普诺夫函数：\(V(x)\ge0\)（平衡点处为 0）用来证稳定性——若沿轨迹 \(\dot V\le0\)（"能量"不增），系统不发散；\(\dot V<0\)（严格递减）则渐近稳定，状态被吸引到平衡。CBF 把不等号方向反过来用（\(\dot h\ge-\alpha(h)\)）证**安全**（安全集前向不变）而非稳定——这就是"CBF 是安全版李雅普诺夫"的确切含义（§U2.5）。

如果这 5 题现在都能流畅作答，你已经握住了本章的全部支柱：名义 MPC、集合运算、线性系统稳定性、QP、李雅普诺夫——剩下的六种方法都是把它们按不同方式组装起来应对不确定性。