第 2 章：MPPI 核心算法与 GPU 并行实现¶

第 1 章回答了**为什么**——为什么"撒一批随机轨迹、按指数加权、取平均"能解非线性随机最优控制。那一章的终点，是一段约 20 行的 NumPy 核心循环：它正确，但既不实时、也不可部署。本章回答**怎么把它变成能在真实机器人上以毫秒级运行的工程系统**。

这中间隔着三类东西。其一是**几个决定成败的算法细节**——\(\rho\) 减法（数值稳定）、warm-start（复用上周期计算）、Savitzky–Golay 平滑（驯服抖动的控制）——它们不在 Ch1 的理论里，却是"能跑的玩具"和"能用的系统"之间的鸿沟。其二是 GPU 并行：Ch1 反复强调的"\(K\) 条 rollout 彼此独立"这一性质，在这里兑现为一个 CUDA kernel，把数千条轨迹同时铺到 GPU 上，换来约 50 倍加速。

其三是**工程抽象**：以 Georgia Tech 的 MPPI-Generic 库为范本，看它如何用 C++ 模板把动力学、代价、采样器、控制器四个维度正交解耦，使得换一个机器人只需换一个模板参数。最后，我们把 MPPI 放回最优控制的版图，与 iLQR/DDP 做一次定量对比，讲清"什么时候该用谁"——以及 MPPI 存在的那个不可替代的核心理由。

学完本章，Ch1 那段 20 行核心就长成了一套你能读懂、能改、能部署、能调试的实时控制器。

📋 前置自测¶

答不出 \(\geq 2\) 题，先回第 1 章（权重公式推导、receding horizon）与 CUDA / Eigen 基础，再读本章。

写出第 1 章的 MPPI 权重公式 \(w_k\)。式子里为什么要减去 \(\rho=\min_k S_k\)？（提示：它在归一化里会抵消，那它到底解决什么问题？）
第 1 章反复强调"\(K\) 条 rollout 彼此独立"。这个独立性对**硬件**意味着什么？为什么说它是 GPU 的"完美负载"？
CUDA 基础：什么是 warp？shared memory 与 global memory 在访问速度和作用域上有什么区别？
Eigen 基础：固定大小矩阵（Matrix3d）与**动态大小矩阵**（MatrixXd）各在什么时候用？为什么固定大小在高频循环里更快？
第 1 章的"接收水平控制"：MPPI 每周期为什么**只执行第一个控制 \(u_0\)**、然后重规划？这与本章的 warm-start 有什么关系？

本章目标¶

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

手写**一份完整、可部署的 MPPI 伪代码——含 warm-start shift、\(\rho\) 减法稳定化、Savitzky–Golay 平滑、控制约束钳位与终端代价，并说清每个细节**不做会怎样；
解释 CUDA 并行化 MPPI 的**线程映射策略**（每 warp / 每 block 一条轨迹）与三级**内存层级**（register / shared / global）使用，说出各自的访存与占用权衡；
说明 **Eigen::Map 与 CUDA 的零拷贝互操作**模式——C++ MPPI 把主机端矩阵接到设备端 kernel 的核心工程手法；
复述 MPPI-Generic 的四维模板化设计（Dynamics / Cost / Sampler / Feedback Controller），理解模板参数如何在编译期组合出不同控制器；
在 MPPI 与 iLQR/DDP 之间做出定量对比与合理选型，并说清 MPPI 那个不可替代的杀手级场景；
**调对**温度 \(\lambda\) 与协方差 \(\Sigma\)（以 ESS 为探针），并理解贯穿全章的"样本效率"主线——MPPI 为何要采上千条、大半为何被浪费；
用结构化流程**诊断并监控** MPPI 的健康运行（输出抖动、权重坍缩、不收敛、NaN、延迟超预算）。

本章知识导航¶

本章四节是一条**从算法到部署**的递进链：先把 Ch1 的理论核补全成完整算法（§2.1），再让它在 GPU 上跑快（§2.2），然后用工程抽象把它组织成可复用的库（§2.3），最后把它放回版图、与梯度法对比择优（§2.4）。

节	知识点	难度	它解决什么	依赖
§2.1	完整伪代码（\(\rho\)/warm-start/SGF）+ 调参（\(\lambda\)/\(\Sigma\)/ESS）+ 样本效率 + 终端代价 + 控制约束	⭐⭐	把"能跑的玩具"补成"能部署的算法"	Ch1 权重公式
§2.2	CUDA 并行化策略（线程映射 / 内存层级）	⭐⭐⭐	把"能用"变成"实时"（~\(50\times\) 加速）	§2.1、CUDA 基础
§2.3	MPPI-Generic 的 C++ 模板化设计	⭐⭐⭐	把"一次性脚本"变成"可复用的库"	§2.2、Eigen/模板
§2.4	MPPI vs iLQR/DDP 系统对比	⭐⭐	把"会用一个"变成"知道何时用谁"	§2.1、梯度 MPC 章

关于本章的代码：本章涉及三种运行环境——纯 Python/NumPy（CPU，用于讲清算法逻辑与做数值验证）、C++（CPU，对应真实部署语言）、CUDA（GPU，真正的实时实现）。书中的 NumPy 与 C++ 片段都经过实际运行验证；CUDA kernel 给出的是与 MPPI-Generic 一致的正确实现，用于讲解线程映射与内存层级的设计，其数值正确性由对应的 CPU 版本佐证，GPU 性能数字引自 MPPI-Generic 论文的基准测试。

前置知识桥接¶

回顾第 1 章：MPPI 权重公式。 第 1 章从自由能–KL 对偶推出，最优控制更新是对 \(K\) 条 rollout 噪声的加权平均，权重为

\[ w_k = \frac{\exp\!\big(-(S_k-\rho)/\lambda\big)}{\sum_{j=1}^{K}\exp\!\big(-(S_j-\rho)/\lambda\big)},\qquad \rho=\min_k S_k, \]

其中 \(S_k\) 是第 \(k\) 条 rollout 的总代价，\(\lambda\) 是温度，\(\rho\) 是为数值稳定而减去的基准。控制更新为 \(u_t \leftarrow u_t + \sum_k w_k\,\varepsilon_k(t)\)，\(\varepsilon_k(t)\) 是第 \(k\) 条轨迹在时刻 \(t\) 注入的噪声。本章 §2.1 要把这个公式补上工程细节、§2.2 要把求和并行到 GPU。

回顾第 1 章：\(K\) 条 rollout 彼此独立。 第 1 章的本质洞察指出，路径积分把动态规划的 backward sweep 换成了 forward sampling——\(K\) 条轨迹各自独立前向积分、互不依赖。本章把这个"独立性"从一句理论断言，落实为一个 CUDA kernel 的并行结构：每条 rollout 分给一组 GPU 线程，数千条同时跑。

回顾第 1 章：接收水平控制。 MPPI 每个控制周期只执行算出序列的第一个控制 \(u_0\)，然后在新测得的状态上重新规划——单次求解是开环序列，"执行一步—重测—重规划"的循环把它变成闭环反馈。本章的 warm-start 正是利用这个循环：既然每周期只前进一步，上一周期辛苦算出的控制序列其余部分仍然几乎有效，左移一位即可作为新一轮的热启动。

CUDA 与 Eigen 基础（本章假定已具备）。 本章假定你了解 CUDA 的执行模型（grid / block / thread、warp 是 32 个线程的调度单位、__global__ kernel 启动、shared memory 是 block 内共享的高速片上内存）与 Eigen 的基本用法（固定大小矩阵 Matrix3d 栈分配、动态大小 MatrixXd 堆分配）。不熟悉也不必中断——每次用到具体特性时，正文都会用 2-3 句重新激活它的核心含义。

如果跳过本章会怎样¶

场景一：你照着 Ch1 的 20 行 NumPy 写了个 MPPI 控制器，仿真里看着还行，一接执行器就发现控制信号高频抖动、电机嗡嗡作响——因为你不知道原始 MPPI 输出天然带噪声、需要 §2.1 的平滑；接着你想上真车，发现每周期从零优化跟不上 50 Hz 的控制频率——因为你不知道 §2.1 的 warm-start 和 §2.2 的 GPU 并行。
场景二：你想给一个四旋翼写 MPPI 穿越障碍林，代价里用了"碰到障碍就 \(+\infty\)"的 indicator。你的同事建议用 iLQR，结果它在碰撞点梯度为零处直接失效。你不理解为什么——因为你没读 §2.4，不知道这正是 MPPI 不可替代的核心场景。

预计阅读时间¶

模式	时间	适合
精读（含手写伪代码、跑通 CPU 示例、推演 CUDA 线程映射）	8–11 小时	要动手实现 MPPI 的工程师
速读（读懂算法与设计、跳过 CUDA 细节）	3–4 小时	先建立全局图景
速查（查 \(\rho\) 减法 / warm-start / API）	0.5 小时	已实现过、回来查细节

科研发展脉络¶

MPPI 从概念验证到开源库，主线由 Georgia Tech 的 Theodorou/Rehg 组推动，大致经历了四个阶段：

年份	工作	出处	核心贡献
2016	Williams, Drews, Goldfain, Rehg, Theodorou, Aggressive Driving with MPPI	ICRA 2016, pp. 1433–1440	首次实车 + GPU：在 1/5 比例 AutoRally 越野车上 GPU 实现 MPPI，完成激进越野驾驶；已采用 free-energy / relative-entropy 的信息论框架
2017	Williams, Aldrich, Theodorou, MPPI: From Theory to Parallel Computation	JGCD 40(2):344	GPU 工程篇：register / shared / global 三级内存层级下的 rollout kernel 设计，本章 §2.2 的主要依据
2018	Williams 等, Information-Theoretic MPC: Theory and Applications to Autonomous Driving	IEEE T-RO 34(6):1603–1622	信息论统一：自由能–KL 对偶的完整推导（见 Ch1 §1.3），并扩展到非仿射动力学
2019	Goldfain, Drews, You 等, AutoRally: An Open Platform for Aggressive Autonomous Driving	IEEE CSM 39(1):26–55	完整硬件平台：1/5 比例越野车 + 板载 GPU + ROS，因子图状态估计 + CNN 可行驶区分割
2024	Vlahov, Gibson, Gandhi, Williams, Theodorou, MPPI-Generic: A CUDA Library for Stochastic Trajectory Optimization	arXiv:2409.07563	统一 C++/CUDA 库：模板元编程把 dynamics / cost / sampler / controller 四维解耦，本章 §2.3 的精读对象

这条线的内在逻辑很清晰：2016 先证明"GPU 上的 MPPI 能开真车"，2017 把 GPU 工程化讲透，2018 把理论根基补完整（即 Ch1），2024 把这一切沉淀成一个任何人都能用的开源库。本章基本沿着这条脉络展开——§2.1 的算法细节散见于 2016/2017，§2.2 的 GPU 来自 2017，§2.3 来自 2024。

§2.1 MPPI 完整伪代码 ⭐⭐¶

动机：从"能跑的玩具"到"能用的算法"¶

第 1 章 §1.5 的 20 行 NumPy 核心，在数学上是完整的：采样、按 \(w_k=\mathrm{softmax}(-S_k/\lambda)\) 加权、更新控制序列。把它放进一个 for 循环里反复跑，代价确实会下降。但如果你真的拿它去控制一台机器人，会接连撞上三堵墙——而这三堵墙，没有一堵能在 Ch1 的理论里找到答案。

第一堵墙是**数值**。真实任务的代价 \(S_k\) 动辄是几百上千（想想"距离目标平方 + 碰撞罚值"累加几十步），而权重要算 \(e^{-S_k/\lambda}\)。当 \(S_k/\lambda\) 是 \(1200\) 时，\(e^{-1200}\) 在 64 位浮点下就是一个硬邦邦的 \(0\)——所有权重一起下溢成零，归一化时 \(0/0\) 给你一个 NaN，控制器当场失效。

第二堵墙是**算力**。机器人控制要求每个周期（比如 50 Hz 即 20 ms）算完一次规划，可"采 \(K\) 条、每条积分 \(T\) 步"是一笔不小的计算；如果每周期都从零开始优化，单核 CPU 根本跟不上。第三堵墙是**执行器**。原始 MPPI 的输出是大量随机轨迹的加权平均，天然带着高频噪声——画出来是一条毛刺丛生的控制曲线，直接喂给电机会让它嗡嗡作响、迅速磨损。

本节要做的，就是把这三堵墙逐一拆掉，对应三个工程细节：\(\rho\) 减法（拆数值墙）、warm-start（拆算力墙的一半，另一半由 §2.2 的 GPU 承担）、Savitzky–Golay 平滑（拆执行器墙）。把它们加进 Ch1 的核心，你就得到了一份**可部署**的 MPPI——这正是从 2016 年 Williams 在 AutoRally 越野车上真正跑起来的那个版本。

如果不这样做会怎样¶

把三个细节一起去掉，逐条看后果：

缺失的细节	直接现象	后果
不做 \(\rho\) 减法	\(e^{-S_k/\lambda}\) 全部下溢为 \(0\)	权重归一化 \(0/0=\text{NaN}\)，控制器输出 NaN，机器人失控
不做 warm-start	每周期从零控制序列开始优化	单次优化要更多迭代才收敛；高频控制下根本算不完，丢周期
不做 SGF 平滑	控制信号高频抖动（chattering）	执行器嗡鸣、发热、磨损；某些系统甚至被抖动激发出共振而发散

这三条不是"锦上添花的优化"，而是"不做就不能用"的硬性前提。尤其第一条：很多人第一次自己写 MPPI，仿真里用的代价小（个位数），侥幸没遇到下溢，一换到真实尺度的代价就崩——这是新手最常见、也最隐蔽的坑（见本节末陷阱）。

历史：这些细节从哪来¶

这三个细节都不是 Ch1 那套优雅理论的一部分，它们是**把理论搬上真实硬件时被现实逼出来的**。\(\rho\) 减法本质上是数值计算里历史悠久的 log-sum-exp 技巧——任何需要在浮点下计算 \(\log\sum e^{x_i}\) 或 \(\mathrm{softmax}\) 的场合（机器学习里的交叉熵、隐马尔可夫模型的前向算法）都要用它，MPPI 只是又一个应用。warm-start 来自模型预测控制（MPC）一脉的常识：既然接收水平控制每周期只前进一步，重规划自然该从上一步的解出发，而非推倒重来；这个思想在梯度 MPC（如 acados 的实时迭代）里同样核心。

Savitzky–Golay 滤波则来自信号处理，由 Savitzky 与 Golay 在 1964 年提出，本是用来平滑光谱数据的；它被引入 MPPI，是因为 Williams 等人发现原始采样控制的抖动会损害执行器——这是一个典型的"理论最优让位于工程现实"的取舍。把这三者拼到一起的，正是 2016 年 ICRA 那篇在 AutoRally 上首次实车的工作，以及 2017 年 JGCD 那篇把 GPU 工程讲透的续作。

完整伪代码¶

先把补全后的完整算法摆出来，再逐个细节深入。与 Ch1 的核心相比，新增的部分已用注释标出：

算法：Information-Theoretic MPPI（可部署版）
输入：初始状态 x₀，当前控制序列 U={u₀,...,u_{T-1}}，温度 λ，采样数 K，协方差 Σ
输出：执行的控制 u₀，以及移位后的控制序列 U'（供下一周期热启动）

每个控制周期：
  // —— 1. GPU 并行采样与 rollout（§2.2 并行化）——
  for k = 1..K  并行 on GPU:
    ε_k ~ N(0, Σ)                 // 第 k 条轨迹的噪声序列，长度 T
    x ← x₀;  S_k ← 0
    for t = 0..T-1:
      v_t = u_t + ε_k(t)          // 微扰后的控制
      x ← F(x, v_t)               // 前向动力学（黑箱：仿真器/神经网络皆可）
      S_k += ℓ(x, v_t)            // 累加运行代价
    S_k += φ(x)                   // 加终端代价

  // —— 2. 计算权重（CPU 或 GPU 归约）——
  ρ = min_k S_k                   // ★ 数值稳定化：log-sum-exp 的基准
  for k = 1..K:
    w_k = exp(-(S_k - ρ) / λ)     // 减 ρ 后指数不会上溢/下溢
  η = Σ_k w_k                     // 归一化常数
  w_k ← w_k / η

  // —— 3. 加权更新控制序列 ——
  for t = 0..T-1:
    u_t ← u_t + Σ_k w_k · ε_k(t)

  // —— 4. ★ 可选：Savitzky–Golay 平滑（驯服抖动）——
  U ← SGFilter(U)

  // —— 5. 执行 u₀，★ warm-start shift（复用本周期计算）——
  execute(u₀)
  U' ← {u₁, u₂, ..., u_{T-1}, u_init}   // 左移一位，末尾补 u_init

三处带 ★ 的就是本节的主角。注意它们各自插在不同阶段：\(\rho\) 减法在权重计算里（第 2 步），SGF 在更新之后、执行之前（第 4 步），warm-start 在执行之后、为下一周期做准备（第 5 步）。下面逐一深入——每一个都先讲清"为什么这样写"，再给正确写法、错误写法和对比。

工程细节一：\(\rho\) 减法与 log-sum-exp ⭐⭐¶

为什么需要它。 问题出在浮点数的表示范围上。64 位双精度浮点能表示的最小正数约为 \(10^{-308}\)，对应到指数上，\(e^{-x}\) 在 \(x\gtrsim 709\) 时就下溢为 \(0\)，在 \(x\lesssim -709\) 时上溢为 inf。

现在看 MPPI 的权重 \(w_k=e^{-S_k/\lambda}\)：真实机器人任务里，一条 rollout 的总代价 \(S_k\) 是几十个时步的运行代价加终端代价之和，轻松到几百上千的量级。取 \(\lambda=1\)、\(S_k=1200\)，那么 \(e^{-1200}\) 直接是 \(0\)。

更糟的是，所有 rollout 的代价都在同一个大尺度上（比如都在 \(1200\) 附近），于是 \(K\) 个权重**集体**下溢为零，归一化那一步 \(w_k/\sum_j w_j\) 变成 \(0/0\)，结果是 NaN。NaN 一旦产生就会污染整条控制链——控制输出是 NaN，机器人失控。这不是小概率事件，而是"只要代价尺度大就必然发生"的确定性故障。

核心机制：减去一个基准。 救命的观察是：权重公式对 \(S_k\) 整体平移是**不变**的。给每个 \(S_k\) 都减去同一个常数 \(\rho\)，分子分母同乘 \(e^{\rho/\lambda}\)，归一化后权重分毫不变：

\[ \frac{e^{-(S_k-\rho)/\lambda}}{\sum_j e^{-(S_j-\rho)/\lambda}} = \frac{e^{\rho/\lambda}\,e^{-S_k/\lambda}}{e^{\rho/\lambda}\sum_j e^{-S_j/\lambda}} = \frac{e^{-S_k/\lambda}}{\sum_j e^{-S_j/\lambda}}. \]

既然减任何常数都不改变结果，我们就挑一个**让指数不再溢出**的常数。取 \(\rho=\min_k S_k\)（这一批 rollout 里代价最小、也就是最好的那条），则对每个 \(k\)，\(S_k-\rho\ge 0\)，于是 \(-(S_k-\rho)/\lambda\le 0\)，\(e^{-(S_k-\rho)/\lambda}\in(0,1]\)——绝不会上溢。

而且代价最小的那条恰好得到 \(e^0=1\)，是权重最大的一条，符合直觉。这就是 log-sum-exp 技巧：要稳定地算 \(\sum_k e^{-S_k/\lambda}\)，先减去最值再算，避免中间结果溢出。

设计动机：为什么减 \(\min\) 而不是别的。 你可能会问，既然减任何常数都行，为什么偏偏减 \(\min_k S_k\)？因为我们要防的是**下溢**（代价大、\(-S_k/\lambda\) 很负、\(e\) 趋零），减去最小代价 \(\rho=\min S_k\) 后，最好那条的指数被抬到 \(e^0=1\)，其余都在 \((0,1]\)，整批权重里至少有一个是 \(1\) 量级、不会全军覆没。

（在最大化奖励的等价表述里，符号相反，对应减去 \(\max\)；二者是同一技巧的两面，关键都是"减去最值、把指数压到 \(\le 0\) 那一侧"。）顺带一提，\(\rho\) 还有第二个身份：\(-\lambda\log(\frac1K\sum_k e^{-(S_k-\rho)/\lambda})+\rho\) 就是 Ch1 讲的自由能的样本估计——\(\rho\) 不只是数值技巧，它也是自由能的"主导项"。

边界与易错点。 这个技巧几乎没有适用边界（任何代价尺度都该用），但有两个易错点。其一是**符号**：MPPI 用代价（要最小化），减的是 \(\min\)；如果你的实现里用的是奖励（要最大化），权重是 \(e^{+R_k/\lambda}\)，要减的就是 \(\max_k R_k\)。搞反方向会让本该防下溢变成催上溢。

其二是**仍可能下溢的尾部**：减 \(\rho\) 后，那些代价远高于最优的 rollout（\(S_k-\rho\) 很大）的权重仍会下溢为 \(0\)——但这**没关系**，它们本来就该被赋予接近零的权重，下溢成 \(0\) 与赋予 \(10^{-300}\) 在加权平均里没有可观测差别。\(\rho\) 减法要保证的是"不会所有权重一起归零"，而不是"每个权重都非零"。

下面按四步给出代码。

第一步：为什么这样写。 核心就是上面那句——先减 \(\min\) 再取指数，把最大的指数钉在 \(1\)，杜绝整批下溢。

第二步：正确写法。

import numpy as np
def mppi_weights(S, lam):                 # S:(K,) 各 rollout 总代价
    rho = S.min()                         # 基准 = 最优(最小代价)
    w = np.exp(-(S - rho) / lam)          # 指数 ∈ (0,1]，不会上溢
    return w / w.sum()                    # 归一化；分母至少含一个 1，绝不为 0

第三步：错误写法（以及为什么错）。

# ❌ 错误 1：不减 ρ，直接取指数
def mppi_weights_bad(S, lam):
    w = np.exp(-S / lam)                  # S~1200 时 e^{-1200}=0，全部下溢
    return w / w.sum()                    # 0/0 = NaN！控制器失效
# 现象：仿真里代价小（个位数）时侥幸正常，一上真实尺度代价就输出 NaN

# ❌ 错误 2：减错了方向（对代价却减 max）
def mppi_weights_wrong_sign(S, lam):
    rho = S.max()                         # 减最大代价
    w = np.exp(-(S - rho) / lam)          # 指数 ∈ [?, 0]... 这里数值上没炸
    return w / w.sum()                    # 但最优那条 e^{-(min-max)/λ} 可能上溢为 inf
# 现象：当 (max-min)/λ 很大时，最优 rollout 的权重变 inf，inf/inf 又是 NaN

第四步：多种实现对比。

# === 三种数值稳定的等价写法 ===
# 方式 A：减 min（最常见，直观）
w_A = np.exp(-(S - S.min()) / lam);  w_A /= w_A.sum()

# 方式 B：用 scipy 的 softmax（工程省心，内部就是 log-sum-exp）
from scipy.special import softmax
w_B = softmax(-S / lam)               # 等价于 A，库已处理稳定性

# 方式 C：log 域计算（极端尺度下最稳，避免任何中间指数）
logw = -S / lam
logw -= logw.max()                    # 注意：-S/λ 的 max 对应 S 的 min
w_C = np.exp(logw);  w_C /= w_C.sum()

# 对比：
# | 方式 | 可读性 | 稳定性 | 适用场景 |
# | A    | ⭐⭐⭐  | 足够   | 教学、绝大多数实现 |
# | B    | ⭐⭐    | 足够   | 有 scipy 依赖的工程代码 |
# | C    | ⭐⭐    | 最强   | 代价尺度极端、需要 log 域归约 |

把它跑出来，亲眼看一次"减与不减"的差别（\(\lambda=1\)，一批典型的大代价）：

S = np.array([1200., 1205., 1210., 1220.])
raw = np.exp(-S / 1.0)                                 # 不减 ρ
print("不减 ρ:", raw, "→ 归一化:", raw / raw.sum())    # 全 0 → NaN
rho = S.min(); st = np.exp(-(S - rho) / 1.0)
print("减 ρ:  ", np.round(st, 4), "→ 权重:", np.round(st / st.sum(), 4))

输出：

不减 ρ: [0. 0. 0. 0.] → 归一化: [nan nan nan nan]
减 ρ:   [1.     0.0067 0.     0.    ] → 权重: [0.9933 0.0067 0.     0.    ]

不减 \(\rho\)，四个权重一起下溢、归一化成 NaN；减 \(\rho\) 后，最优那条得到权重 \(0.9933\)，次优 \(0.0067\)，其余两条因代价高而下溢为 \(0\)（无害），权重之和正好是 \(1\)。这就是一行 S.min() 的全部意义——它不改变任何权重的数学值，却把算法从"必然崩溃"变成"数值稳定"。

工程细节二：warm-start shift ⭐⭐¶

为什么需要它。 回顾 Ch1 的接收水平控制：MPPI 每个周期算出一整条长度为 \(T\) 的控制序列 \(U=(u_0,\dots,u_{T-1})\)，但只执行第一个 \(u_0\)，下一周期在新状态上重新规划。问题来了——下一周期重新规划时，控制序列 \(U\) 该从什么初值开始？最朴素的答案是"从零开始"（\(U\leftarrow 0\)），但这浪费得惊人。

想想看：这一周期你花了 \(K\) 条 rollout 的算力，好不容易优化出一条不错的序列 \((u_0,u_1,\dots,u_{T-1})\)；执行 \(u_0\)、系统前进一步后，剩下的 \((u_1,\dots,u_{T-1})\) 其实仍然几乎最优——因为机器人只动了一步，世界没有天翻地覆，"从下一个状态出发的最优计划"和"上一周期算出的、去掉第一步的计划"高度重合。把这份现成的好序列丢掉、从零再优化，等于每周期都在重新发明轮子。

核心机制：左移一位、末尾补一个。 warm-start 的做法极简单：执行 \(u_0\) 后，把整条序列**左移一位**，让原来的 \(u_1\) 顶到 \(u_0\) 的位置、\(u_2\) 顶到 \(u_1\)……原来的 \(u_{T-1}\) 顶到 \(u_{T-2}\)，而新空出来的末位 \(u_{T-1}\) 用一个默认值 \(u_\text{init}\) 填充（通常是零、或上一条序列的最后一个控制、或某个保持性控制）：

\[ U' = (u_1,\ u_2,\ \dots,\ u_{T-1},\ u_\text{init}). \]

下一周期就以这个 \(U'\) 为初值，让 MPPI 在它附近采样、微调。由于 \(U'\) 的前 \(T-1\) 项已经是上一周期优化过的好控制，MPPI 往往只需很少的"调整"就能收敛——甚至单次加权更新就够用（配合高频重规划）。只有末位那一项 \(u_\text{init}\) 是"新猜的"，需要靠采样去探索，但它对应的是最远的未来（时域末端），影响最小。

设计动机：warm-start 就是在构造一个好的提议分布。 这里有一个把 Ch1 理论和本章工程缝起来的关键视角。Ch1 §1.4 证明过一个结论：重要性采样的理想（零方差）提议分布，就是目标分布本身；MPPI 的全部改进，本质上都是"想办法让采样的提议分布更接近那个隐式最优分布"。warm-start 正是这一原则最廉价、最有效的实现——上一周期优化出的控制序列，已经是对"最优控制长什么样"的一个相当好的估计，以它为采样中心，等于把提议分布直接挪到了最优解附近。

本质洞察：warm-start 不只是"省点计算"的工程小技巧，它是 Ch1"提议分布质量优先于样本数"原则的直接兑现。从零冷启动，等于每周期都把提议分布扔回原点、让采样从一无所知开始；warm-start 则把上一周期的解当作提议分布的中心，让每一次采样都站在前一次的肩膀上。这解释了一个乍看反直觉的现象：warm-start 的威力常常**大于**单纯把 \(K\) 翻倍——因为前者改善的是提议分布的"位置"（采样撒在哪），后者只增加提议分布的"样本数"（在原地多撒几个），而 Ch1 告诉我们，位置比数量重要得多。

边界与易错点。 warm-start 的前提是"相邻周期的最优解高度连续"——这在大多数场景成立，但有两个边界。其一，当环境**突变**（障碍突然出现、目标瞬间跳变）时，上一周期的解可能已经过时，warm-start 反而把采样锚在了错误的地方，这时需要更大的探索（临时增大 \(\Sigma\)）或干脆重置。

其二，**末位填充的选择**有讲究：填零意味着"假设末端不施加控制"，对很多系统是合理的中性选择；但对需要持续控制才能维持的系统（如悬停的四旋翼），填零会让末端"松手"，更好的选择是复制上一条序列的最后一个控制 \(u_{T-1}\)，或填入某个已知的保持性控制。易错的还有一个实现细节：左移操作不要原地覆盖出错（见错误写法）。

下面按四步给出代码。

第一步：为什么这样写。 核心是"复用上一周期 \(T-1\) 步的优化成果"——左移让它们顺位前移，只有时域末端那一项需要重新探索。

第二步：正确写法。

import numpy as np
def warm_start_shift(U, u_init=None):     # U:(T, m) 上一周期优化后的控制序列
    T, m = U.shape
    if u_init is None:
        u_init = np.zeros(m)              # 默认中性填充；悬停类系统可用 U[-1]
    return np.vstack([U[1:], u_init[None, :]])   # 左移一位，末尾补 u_init

第三步：错误写法（以及为什么错）。

# ❌ 错误 1：每周期从零开始，丢弃上一周期的优化成果
U = np.zeros((T, m))                      # 冷启动：提议分布扔回原点
# 现象：单次更新远不足以收敛；高频控制下要么丢周期，要么输出劣质控制

# ❌ 错误 2：原地左移时下标写错，造成数据错位/越界
for t in range(T):
    U[t] = U[t+1]                         # t=T-1 时 U[T] 越界！且原地覆盖污染后续
# 正确应整体切片 U[1:] 再拼接，或从后往前写，避免原地覆盖的别名问题

# ❌ 错误 3：末位用"上上次的残留值"而非有意义的填充
# （忘记填充，让末位保留循环开始前的脏数据）
U_shifted = U.copy(); U_shifted[:-1] = U[1:]   # 末位 U_shifted[-1] 未更新！
# 现象：末端控制是上一周期的 u_{T-1}（可能恰好可用），也可能是脏值，行为不可预测

第四步：多种填充策略对比。

# === 末位 u_init 的三种填充策略 ===
# 策略 A：填零（中性，适合"不控制就自然停下"的系统，如带阻尼的点质量）
U_A = np.vstack([U[1:], np.zeros((1, m))])

# 策略 B：复制末控制（适合需持续控制维持的系统，如悬停四旋翼）
U_B = np.vstack([U[1:], U[-1:]])

# 策略 C：填入保持性控制 u_hold（如重力补偿，适合机械臂）
U_C = np.vstack([U[1:], u_hold[None, :]])

# 对比：
# | 策略 | 末端假设 | 适用系统 |
# | A    | 末端松手、自然停 | 阻尼主导、欠驱动导航 |
# | B    | 末端维持上一动作 | 悬停、定速巡航 |
# | C    | 末端施加已知保持力 | 重力补偿的机械臂/足式 |

warm-start 的效果有多大？用 §2.1 末尾那个 2-D 导航例子做对照实验，唯一的差别是"是否做左移"：

首次到达目标的步数：  warm-start = 47 步      每周期冷启动 = 从未到达

差距是定性的：warm-start 让控制器 47 步抵达目标，而每周期从零冷启动的版本在整个 120 步预算内**根本没能到达**——因为在固定的单次采样预算下，冷启动每一步都在"重新摸索"，始终没能积累出一条通向目标的好序列。这印证了上面的本质洞察：warm-start 改善的是提议分布的位置，其收益远超"在原地多撒几个样本"。

工程细节三：Savitzky–Golay 平滑 ⭐⭐¶

为什么需要它。 MPPI 的控制更新是 \(u_t\leftarrow u_t+\sum_k w_k\,\varepsilon_k(t)\)——本质上是大量随机噪声 \(\varepsilon_k(t)\) 的加权平均。

加权平均确实抑制了一部分噪声，但抑制得并不干净：有限的 \(K\) 条样本、每条都带独立随机扰动，加权后的结果在**时间维上**仍然毛糙——相邻时刻的控制值 \(u_t\) 与 \(u_{t+1}\) 之间跳来跳去，画成曲线是锯齿状的高频抖动（chattering）。

对仿真无所谓，但真实执行器吃不消：电机要跟随一条毛刺曲线，意味着频繁的正反向加速，表现为嗡鸣、发热、机械磨损；更糟的情形下，高频抖动正好落在系统的某个共振频率附近，会把机器人激励到振荡发散。所以在把控制送出去之前，需要对控制序列做一次**时间维的平滑**，磨掉高频毛刺、保留低频的真实意图。

为什么不用最简单的滑动平均。 最朴素的平滑是滑动平均（moving average）：每个点取其邻域的算术平均。它确实能压噪声，但有个致命副作用——它会把真实的峰、谷和拐弯也一起抹平。控制序列里那些"该急转""该全力"的尖锐特征，本是任务需要的，滑动平均会把它们削成圆钝的丘陵，让控制变得迟钝、跟不上动态。这就引出了 Savitzky–Golay（SG）滤波的好处：它在平滑噪声的同时，保留信号的形状特征（峰高、拐点、斜率）。

核心机制：滑动窗口里的多项式最小二乘。 SG 滤波的想法很巧。对每个点，取它周围一个固定宽度的窗口（比如左右各 2 个点，共 5 个），然后在这个窗口内用一个低阶多项式（比如 2 次）去**最小二乘拟合**这些点，再用拟合多项式在**窗口中心**的取值，作为该点的平滑输出。

逐点滑动这个窗口，就得到整条平滑曲线。关键在于：低阶多项式拟合既能"抹平"那些拟合不上的高频噪声（它们偏离多项式趋势），又能"保住"局部的多项式形状——因为如果窗口内的真实信号本就近似一条抛物线（有峰、有拐），多项式拟合会忠实地跟上它，而不像算术平均那样无脑取中。

由于拟合是线性最小二乘，且窗口宽度、多项式阶数固定，"用拟合多项式在中心取值"这一整套运算可以**预先化简成一组固定的卷积系数**——对每个点，平滑输出就是窗口内各点的一个固定加权和。设窗口宽 \(2h+1\)、用 \(p\) 阶多项式，记设计矩阵 \(A\)（各行是 \([1,\delta,\delta^2,\dots,\delta^p]\)，\(\delta\) 取 \(-h,\dots,h\)），则中心点的平滑系数是 \((A^\top A)^{-1}A^\top\) 投影矩阵的中心那一行。换句话说，SG 滤波在实现上就是一次固定系数的卷积，开销极小。

设计动机：用信息论最优性换执行器友好。 这里要诚实地点明一个取舍。Ch1 推导的 MPPI 更新，在信息论意义上是最优的——它是 KL 正则问题的精确解。而 SG 平滑是在这个最优解**之后**额外加的一道手脚，它**破坏了信息论最优性**：平滑后的控制序列不再是那个加权平均的精确结果，而是被人为磨过的近似。

我们为什么愿意付这个代价？因为"理论最优的控制"如果会抖坏电机，那它在工程上就是不可用的；牺牲一点最优性、换来执行器能平稳跟随的控制，是一笔划算的买卖。这是工程里反复出现的母题——理论的最优解常常要让位于硬件的现实约束。值得强调的是，SGF 是**可选**的一步：如果你的系统对抖动不敏感（仿真、或带有低通特性的执行器），完全可以不做，保留 MPPI 的理论最优性。

本质洞察：MPPI 里有两种截然不同的"平滑"，别混为一谈。一种是**采样层面**的平滑（如有色噪声、把控制提升到更高阶再积分，见第 3 章），它从源头让采样出的轨迹就平滑，**不破坏**信息论最优性；另一种是 SGF 这样的**事后**平滑，它在最优解算完之后再磨一遍，**破坏**最优性但简单直接。前者是"让最优解本身就平滑"，后者是"把不平滑的最优解强行磨平"。工程上常先试事后平滑（便宜），不够再上采样层面的方案（讲究）。近年也有工作指出，像 SG 这样的事后滤波器只是在结果上抹平，无法在**源头**保证诸如"导数有界"之类的性质（控制的变化率仍可能超限）——这恰恰是采样层面方案（直接在优化里约束平滑度）被认为更彻底的原因。

边界与易错点。 SG 的两个超参数——窗口宽度和多项式阶数——需要权衡：窗口越宽、阶数越低，平滑越狠但越容易抹掉真实特征；窗口越窄、阶数越高，越保形但去噪越弱。极端情形 \(p=0\)（零阶多项式）退化成普通滑动平均，丢掉了 SG 保形的全部好处，所以 SG 一般用 \(p=2\) 或 \(3\)。

易错点有三：其一，窗口宽度必须是奇数（要有明确的中心点）；其二，序列的**头尾各 \(h\) 个点**没有完整窗口，需要特殊处理（截断、补边或用非对称系数），实现时容易忘记导致端点出错；其三，别把 SGF 当成 MPPI 的"必要组成部分"——它是可选的工程外挂，理解它破坏最优性这一点很重要（见本节末概念误区）。

下面按四步给出代码。

第一步：为什么这样写。 核心是"窗口内多项式最小二乘、取中心拟合值"，它既压高频噪声又保形；化简后就是一次固定系数卷积。

第二步：正确写法。

import numpy as np
def savgol(u, win=5, poly=2):             # win 必须奇数；poly 一般取 2 或 3
    half = win // 2
    # 设计矩阵：各行 [1, δ, δ², ...]，δ 从 -half 到 +half
    A = np.vander(np.arange(-half, half + 1), poly + 1, increasing=True)
    coef = (A @ np.linalg.pinv(A.T @ A) @ A.T)[half]   # 投影矩阵中心行 = 卷积系数
    out = u.copy()
    for i in range(half, len(u) - half):  # 端点 half 个不动（最简处理）
        out[i] = coef @ u[i - half : i + half + 1]
    return out

第三步：错误写法（以及为什么错）。

# ❌ 错误 1：用滑动平均代替 SG，抹平真实峰谷
def moving_average(u, win=5):
    return np.convolve(u, np.ones(win)/win, mode='same')
# 现象：噪声是压下去了，但控制里"该急转"的尖峰被削圆，系统响应变迟钝

# ❌ 错误 2：窗口取偶数，没有明确中心
coef = savgol_coef(win=4)                 # 偶数窗口，中心落在两点之间，相位偏移
# 现象：平滑后的序列整体偏移半个采样周期，引入额外延迟

# ❌ 错误 3：把 SGF 当成"免费午餐"，在对最优性敏感的场景滥用
U = savgol(U)                             # 不加判断就平滑
# 问题：SGF 破坏了 MPPI 的信息论最优性；若系统本不需平滑（仿真/低通执行器），
#       这一步是在白白损失最优性。它是可选项，不是必选项

第四步：与其它平滑方式对比。

# === 三种事后平滑方式对比 ===
# 方式 A：Savitzky–Golay（保形，推荐）—— 见上
# 方式 B：滑动平均（最简，但抹平特征）
u_B = np.convolve(u, np.ones(5)/5, mode='same')
# 方式 C：一阶低通/指数滑动平均（有相位延迟，但在线友好）
alpha = 0.3; u_C = u.copy()
for i in range(1, len(u)): u_C[i] = alpha*u[i] + (1-alpha)*u_C[i-1]

# 对比：
# | 方式 | 保形性 | 相位延迟 | 是否破坏最优性 | 适用 |
# | A SG       | ⭐⭐⭐ | 无(对称) | 是 | 离线/整条序列可得 |
# | B 滑动平均  | ⭐    | 无(对称) | 是 | 只需粗略去噪 |
# | C 指数低通  | ⭐⭐   | 有       | 是 | 严格在线、逐点到达 |

跑一次看效果——给一条正弦信号叠上噪声，比较平滑前后的"相邻差分均方"（这个量越小表示越平滑）：

rng = np.random.default_rng(0)
noisy = np.sin(np.linspace(0, 3, 30)) + 0.3 * rng.standard_normal(30)
smooth = savgol(noisy)
print("相邻差分均方：原始 %.4f → SG 平滑后 %.4f" %
      (np.mean(np.diff(noisy)**2), np.mean(np.diff(smooth)**2)))

输出：

相邻差分均方：原始 0.1362 → SG 平滑后 0.0372

抖动指标从 \(0.1362\) 降到 \(0.0372\)，降了约四分之三，而正弦的整体形状（峰、谷、过零点）保留完好。这正是 SG 优于滑动平均的地方：压噪声，但不削峰。

采样分布的两个旋钮：温度 \(\lambda\) 与协方差 \(\Sigma\) ⭐⭐¶

前三个细节解决的是"算得对、算得省、控得稳"。但要让 MPPI 真正好用，还有两个**旋钮**必须调对——温度 \(\lambda\) 和采样协方差 \(\Sigma\)。它们不是数值技巧，而是直接决定"采样撒多开、权重多挑剔"的核心超参。第 1 章给了它们的理论含义，这里讲怎么在工程上把它们调对。

温度 \(\lambda\)：权重有多挑剔。 回顾 Ch1：\(\lambda\) 是权重 \(w_k\propto e^{-S_k/\lambda}\) 里的温度。\(\lambda\) 小，指数对代价差异极敏感，权重高度集中在少数最优 rollout 上（贪婪、近似 argmin）；\(\lambda\) 大，指数被压平，权重趋于均匀，更新接近所有 rollout 的简单平均（保守、几乎不挑）。工程上判断 \(\lambda\) 调得好不好，有一个利器——有效样本数（Effective Sample Size, ESS）：

\[ \text{ESS} = \frac{\big(\sum_k w_k\big)^2}{\sum_k w_k^2} = \frac{1}{\sum_k \hat w_k^2},\qquad \hat w_k = \frac{w_k}{\sum_j w_j}, \]

它衡量"\(K\) 条 rollout 里实际有多少条在有意义地贡献"，取值在 \([1,K]\) 之间：ESS 接近 \(1\) 说明几乎只有一条在起作用（\(\lambda\) 太小，退化成贪婪选择，浪费了采样、方差大）；ESS 接近 \(K\) 说明权重几乎均匀（\(\lambda\) 太大，没在利用代价信息，更新方向模糊）。把它跑出来看：

import numpy as np
rng = np.random.default_rng(0); K = 2048
S = rng.uniform(800, 900, K)              # 一批代价，跨度约 100
for lam in [1.0, 5.0, 20.0, 100.0, 1000.0]:
    rho = S.min(); w = np.exp(-(S - rho)/lam); w /= w.sum()
    ess = 1.0 / np.sum(w**2)
    print(f"λ={lam:>6.0f}  ESS={ess:>7.1f}  ESS/K={ess/K:>5.1%}")

输出：

λ=     1  ESS=   50.5  ESS/K= 2.5%
λ=     5  ESS=  209.1  ESS/K=10.2%
λ=    20  ESS=  808.1  ESS/K=39.5%
λ=   100  ESS= 1890.9  ESS/K=92.3%
λ=  1000  ESS= 2046.3  ESS/K=99.9%

代价跨度约 \(100\) 时，\(\lambda=1\) 只有 \(2.5\%\) 的 rollout 在贡献（太挑剔）、\(\lambda=1000\) 则有 \(99.9\%\)（太平均），中间的 \(\lambda=20\) 让约 \(40\%\) 的样本有效贡献，落在健康区间。关键洞察是：\(\lambda\) 的"对错"取决于代价的尺度——同一个 \(\lambda\) 在代价跨度 \(100\) 和跨度 \(10000\) 的问题上效果天差地别。

所以实战中不要凭空设 \(\lambda\)，而是**监控 ESS**：让 ESS 维持在 \(K\) 的某个比例（经验上常取 \(5\%\sim50\%\)），ESS 太低就增大 \(\lambda\)、太高就减小 \(\lambda\)。这把"玄学调 \(\lambda\)"变成了"盯一个可观测指标调"。

协方差 \(\Sigma\)：采样撒多开。 \(\Sigma\) 控制每个时步注入噪声 \(\varepsilon_k(t)\sim\mathcal N(0,\Sigma)\) 的幅度，也就是 rollout 在控制空间里探索的范围。\(\Sigma\) 太小，所有 rollout 挤在当前控制序列附近，探索不足，遇到需要大动作（急转、全力）的情况找不到好轨迹；\(\Sigma\) 太大，采样撒得太散，大量 rollout 落在明显糟糕的区域被浪费，更新也更抖。

\(\Sigma\) 的设计有几个实战要点：其一，按控制维度分别设——不同执行器量纲不同（如四旋翼的总推力和力矩范围差很多），用对角 \(\Sigma\) 给每一维独立的标准差，而非所有维共用一个值；其二，与执行器物理范围匹配——标准差大致取该维控制范围的一个合理比例，让采样既覆盖有用动作又不大量越界；其三，可**自适应**——环境突变（warm-start 失效）时临时增大 \(\Sigma\) 加强探索，稳定跟踪时调小以降抖动。

本质洞察：\(\lambda\) 和 \(\Sigma\) 调的是同一件事的两端——探索与利用的平衡。\(\Sigma\) 管"往哪撒、撒多开"（探索的广度），\(\lambda\) 管"撒完之后多听最优那几条"（利用的强度）。两者还会**相互作用**：\(\Sigma\) 大（采样散）时，代价跨度也大，需要相应增大 \(\lambda\) 才不会让权重过尖；\(\Sigma\) 小（采样密）时代价跨度小，\(\lambda\) 也要相应减小才挑得动。这解释了为什么"只调一个"常常无效——它俩得一起调。把 ESS 当探针、把执行器范围当 \(\Sigma\) 的标尺，是把这对旋钮从"碰运气"变成"有据可调"的工程方法。这也呼应 §2.1 那个思维陷阱：与其盲目加大 \(K\)，不如先把 \(\lambda\)、\(\Sigma\) 调到 ESS 健康。

进阶：用 ESS 自动调 \(\lambda\)¶

既然 ESS 是个可观测的探针，自然的下一步是**让 \(\lambda\) 自己跟着 ESS 走**，省去人工试。做法是加一个简单的反馈控制器：每周期看当前 ESS，低于目标带就升温（\(\lambda\) 乘个系数）、高于目标带就降温（\(\lambda\) 除一个系数），把 ESS 拉回设定的区间。这样不必为每个任务、每个代价尺度手调 \(\lambda\)。把它和固定 \(\lambda\) 对比跑一遍（2-D 导航，目标带取 \(K\) 的 \(10\%\sim40\%\)）：

lo, hi = 0.10*K, 0.40*K                       # ESS 目标带
# ... 每周期算出 ess 后：
if   ess < lo: lam *= 1.5                      # 权重太尖 → 升温
elif ess > hi: lam /= 1.5                      # 权重太平 → 降温

输出：

固定 λ=1:  ESS 范围 [12, 508]（沿轨迹波动约 42×），到达 79 步
自适应 λ:  ESS 范围 [12, 242]，λ 自动游走于 [0.03, 1.50]，到达 63 步

固定 \(\lambda=1\) 时，ESS 沿轨迹从 \(12\) 一路变到 \(508\)——波动约 \(42\) 倍，因为代价的尺度在运动过程中不断变化（离障碍近、离目标远时代价大，反之小），同一个 \(\lambda\) 时而过尖、时而过平。自适应 \(\lambda\) 则把高端从 \(508\) 压到 \(242\)、整体波动收窄，并因为权重质量更稳定而更快到达（\(63\) vs \(79\) 步），全程不用人工碰 \(\lambda\)。

要诚实说明：这个控制器是**反应式**的（看到 ESS 偏了才调，滞后一步），所以突变瞬间 ESS 仍可能短暂掉到低端（输出里的 \(12\) 就来自这种瞬态）；更讲究的做法会预测性地调，但"盯 ESS、偏了就反向调 \(\lambda\)"这个朴素版已经能消除大半手调负担。这也是把 §2.1 那条"用 ESS 当探针"的原则从手动升级为自动的自然一步。

样本效率：MPPI 的核心工程矛盾 ⭐⭐⭐¶

调好了 \(\lambda\) 和 \(\Sigma\)，还有一个更深的问题值得想清楚：MPPI 为什么非得采上千条 rollout？ §2.2 的基准里 \(K\) 动辄 \(2048\)、\(4096\)，而上面 ESS 的实验告诉我们，这上千条里往往只有几十到几百条在有意义地贡献——大半被浪费了。这不是某个实现没写好，而是 MPPI 与生俱来的核心矛盾：样本效率。理解它，才能看懂这门技术的全貌，以及后续所有改进的动机。

矛盾的来源：盲目的提议分布。 回顾 Ch1：MPPI 是用蒙特卡洛重要性采样去估计那个隐式的最优控制分布。蒙特卡洛估计天然带**方差**，而 MPPI 用的是最朴素的提议分布——在当前控制序列周围撒各向同性高斯噪声。这个提议分布**完全不看代价地形的形状**：它不知道哪个方向有障碍、哪个方向通向目标，只是均匀地往四面八方撒。

在低维、简单的问题上还好；一旦维度升高、地形复杂（多障碍、窄通道、强非线性），盲撒出的 rollout 绝大多数落在平庸或糟糕的区域，拿到近零权重（ESS 低），真正有用的只有少数几条。要让加权平均稳定可靠，就得撒得足够多、指望"广撒网捞到几条好的"——这就是 \(K\) 必须上千的根本原因。

研究界对此有清晰共识：标准 MPPI 的各向同性采样不适配地形几何，样本效率低，稳定运行往往需要数千条并行 rollout（Schramm 等的方差缩减 MPPI、DM-MPPI 等近年工作都以此为出发点）。

更糟的失效模式：均值跑偏。 样本效率问题有一个尖锐的极端情形。提议分布的**均值**就是当前（warm-start 的）控制序列。如果这个均值因为某种原因指向了一个不可行区域——初值很差、遇到突发扰动、或环境突变让上一周期的解过时（正是 §2.1 warm-start 边界提到的情形）——那么在它周围撒出的**几乎所有** rollout 都会撞墙、拿到高代价，加权更新基于一堆"全是坏的"样本，给出的方向毫无意义，控制器可能就此崩掉。

这时增大 \(\Sigma\)（撒得更开）能帮助采样"逃离"坏区域、找到可行轨迹，但代价是要更多样本才能维持足够的 ESS——探索与样本数的矛盾进一步收紧（o-MPPI 等工作明确分析过这个"均值跑偏 → 大批 rollout 失败 → 需更大协方差与更多样本"的链条）。

把"提议质量决定样本效率"这件事跑出来看。在 2-D 导航的同一个状态、同样 \(K=512\) 下，对比两种提议的均值——一条充分优化过的、接近最优的序列（好提议，warm-start 想逼近的就是它），和一条全零序列（坏提议）：

# 同一状态、同样 K=512，比较好/坏提议的有效样本数 ESS
def ess_of(U, rng):                          # 给定提议均值 U，返回 ESS
    eps = sigma*rng.standard_normal((K,T,2)); V = U[None] + eps
    S = np.tile(x0,(K,1)); J = np.zeros(K)
    for t in range(T): S = step_batch(S, V[:,t]); J += run_cost(S)*dt
    rho = J.min(); w = np.exp(-(J-rho)/lam); w /= w.sum()
    return 1.0/np.sum(w**2)
ess_bad  = ess_of(U_bad,  np.random.default_rng(1))   # U_bad = 全零序列
ess_good = ess_of(U_good, np.random.default_rng(1))   # U_good = 已充分优化的序列
print(f"坏提议(零序列)    ESS={ess_bad:.1f}  ({ess_bad/K:.1%} 有效)")
print(f"好提议(已优化序列) ESS={ess_good:.1f}  ({ess_good/K:.1%} 有效)")

输出：

坏提议(零序列)    ESS=22.5   (4.4% 有效)
好提议(已优化序列) ESS=323.0  (63.1% 有效)

同样撒 \(512\) 条，坏提议下只有约 \(4\%\)（\(22\) 条）在有意义地贡献、其余九成多全浪费了；好提议下有 \(63\%\)（\(323\) 条）在贡献——有效样本多了约 \(14\) 倍。这就是"提议质量决定样本效率"的最直接证据：把提议均值挪到接近最优处（正是 warm-start 做的事），同样的 \(K\) 一下子从"大半浪费"变成"大半有用"。反过来看也成立：如果你的提议很好，用更小的 \(K\) 也能维持足够的 ESS——好提议直接换算成省下的算力。

本质洞察：样本效率是贯穿 MPPI 全部技术的那条主线。回头看本章学过的一切，几乎都是在和"大半样本被浪费"这个矛盾搏斗——ESS 是**度量**浪费的探针；warm-start 是通过改善提议分布的**均值（位置）来减少浪费（上面 \(14\) 倍的差距就是它的价值）；\(\lambda\)/\(\Sigma\) 是在调**探索与利用**以平衡浪费；而 GPU（§2.2）则是"既然非得撒上千条、那就用数千核心同时撒"的**硬件答案——样本效率低正是 GPU 对 MPPI 不可或缺的深层原因。Ch1 的偏差分析说"加大 \(K\) 收益按 \(O(1/K)\) 递减"，本节补上另一半：之所以仍需要很大的 \(K\)，是因为提议分布是盲目的。这也预示了后续章节的方向——几乎每一种 MPPI 改进（有色噪声让采样更平滑连贯、把先验知识或学到的分布注入采样、用模型信息做方差缩减）本质上都是同一件事：让提议分布更聪明，从而用更少的 rollout 捞到更多有用的轨迹。换句话说，MPPI 研究的核心问题不是"怎么采样"，而是"怎么采得更聪明、让更少的样本被浪费"。把这条主线记在心里，后面的变体就不再是零散技巧，而是对同一个矛盾的不同解法。

完整工作示例：2-D 点质量导航（含障碍）¶

把三个细节装回 Ch1 的核心，跑一个完整的闭环例子。任务是一个 2-D 点质量从原点导航到目标 \((4,4)\)，途中要绕开一个圆心在 \((2,2)\)、半径 \(1.0\) 的障碍。状态是 \([x,y,v_x,v_y]\)，控制是二维加速度 \([a_x,a_y]\)。代价用"到目标距离平方 + 障碍罚值 + 速度正则"，其中**障碍罚值是一个不连续的跳变**——进入障碍圆就 \(+1000\)，否则 \(0\)：

import numpy as np
dt = 0.05
GOAL = np.array([4.0, 4.0]); OBS = np.array([2.0, 2.0]); R = 1.0
def step_batch(S, A):                     # S:(K,4) A:(K,2)，向量化 K 条(模拟 GPU SIMT)
    x, y, vx, vy = S[:,0], S[:,1], S[:,2], S[:,3]
    vx = 0.95*vx + A[:,0]*dt; vy = 0.95*vy + A[:,1]*dt   # 轻阻尼
    return np.stack([x+vx*dt, y+vy*dt, vx, vy], 1)
def run_cost(S):
    d2goal = (S[:,0]-GOAL[0])**2 + (S[:,1]-GOAL[1])**2
    dobs = np.sqrt((S[:,0]-OBS[0])**2 + (S[:,1]-OBS[1])**2)
    hit = (dobs < R).astype(float) * 1000.0             # ★ 不连续的障碍罚值
    return d2goal + hit + 0.05*(S[:,2]**2 + S[:,3]**2)

def mppi_step(x0, U, K, T, lam, sigma, rng):
    eps = sigma * rng.standard_normal((K, T, 2))
    V = U[None,:,:] + eps
    S = np.tile(x0, (K,1)); J = np.zeros(K)
    for t in range(T):
        S = step_batch(S, V[:,t,:]); J += run_cost(S)*dt
    rho = J.min()                         # ★ ρ 减法
    w = np.exp(-(J - rho)/lam); w /= w.sum()
    return U + (w[:,None,None]*eps).sum(0)

rng = np.random.default_rng(0)
K, T, lam, sigma = 512, 25, 1.0, 3.0
x = np.zeros(4); U = np.zeros((T, 2))
for step in range(120):
    U = mppi_step(x, U, K, T, lam, sigma, rng)
    x[:] = step_batch(x[None,:], U[0:1])[0]             # 执行 u₀
    U = np.vstack([U[1:], np.zeros((1,2))])             # ★ warm-start shift
    if np.hypot(x[0]-GOAL[0], x[1]-GOAL[1]) < 0.2: break
print(f"{step+1} 步到达目标，终点距离 {np.hypot(x[0]-GOAL[0], x[1]-GOAL[1]):.3f}")

输出：

49 步到达目标，终点距离 0.188

控制器 49 步抵达目标，全程没有闯入障碍（轨迹与障碍中心的最近距离约 \(1.042\)，刚好擦着半径 \(1.0\) 的边界绕过）。这个例子麻雀虽小，却同时用上了本节的三个细节：J.min() 做 \(\rho\) 减法保证权重不溢出，np.vstack([U[1:], ...]) 做 warm-start 复用上一周期的解（去掉它，正如前面实验所示，120 步内根本到不了目标）；SGF 在这个阻尼系统上非必需，故从略，但加上它会让加速度曲线更平滑。

更要紧的是那个**不连续的障碍罚值**。hit = (dobs < R) * 1000 是一个阶跃函数——在障碍边界上代价从 \(0\) 瞬间跳到 \(1000\)，处处不可微、梯度为零。MPPI 对此毫不在意：它只把这个标量代价丢进 \(e^{-S/\lambda}\)，撞上障碍的 rollout 因代价暴增而被赋予近零权重，加权平均自然绕开障碍。这正是 Ch1 强调的"\(S_k\) 只作标量进指数、可不连续/不可微"在工程上的体现——也为 §2.4 的杀手级场景（碰撞 indicator 代价让梯度法失效、MPPI 仍能工作）埋下伏笔。

时域长度 \(T\) 与终端代价：看多远、怎么收尾 ⭐⭐¶

上面的例子用了一个简单代价，没有单列终端代价。但完整伪代码里那一项 \(S_k \mathrel{+}= \phi(x)\)（终端代价 \(\phi\)，在 rollout 末尾加一次）绝不是摆设——在真实任务里，它和时域长度 \(T\) 一起决定了 MPPI "看多远"。这是一对必须一起理解的设计量。

\(T\) 决定看多远，但不能太长。 MPPI 每条 rollout 只前向积分 \(T\) 步，也就是只"预见" \(T\cdot\Delta t\) 这么长的未来。\(T\) 太短，控制器**短视**：它看不到 \(T\) 步之外的后果，可能做出局部最优、全局糟糕的决策——比如全速冲向目标，却因为"看不到"快到时需要减速而冲过头。

\(T\) 太长又有两个代价：一是 §2.2 讲的串行链变长、每次迭代变慢（GPU 也压不动时步串行）；二是远期预测越来越不准（动力学误差累积、环境变化），花算力去优化看不准的远期得不偿失。所以 \(T\) 要取在"够看到关键后果"和"算得起、看得准"之间。

终端代价 \(\phi\) 让短时域也不短视。 解决"\(T\) 短则短视"的关键，就是终端代价 \(\phi(x)\)。它的作用是**近似时域末端那个状态的"剩余代价"（cost-to-go）**——即"从这个终端状态出发，按最优策略继续走下去，未来还要付多少代价"。

如果 \(\phi(x)\) 能准确反映剩余代价，那么 MPPI 即便只看 \(T\) 步，也等于"看到了无穷远"：前 \(T\) 步精确优化，\(T\) 步之后的一切浓缩进 \(\phi\)。这就是为什么终端代价如此重要——它是用一个**短时域**实现**远视**的杠杆。

本质洞察：终端代价 \(\phi\) 本质上就是**值函数（value function）——它正是动态规划里"从某状态出发的最优剩余代价"。这把 MPPI 和梯度法、强化学习接到了同一个概念上：iLQR 用 Riccati 递推算局部值函数，RL 用 critic 网络学全局值函数，而 MPPI 把值函数当作终端代价 \(\phi\) 直接加在 rollout 末尾。理想情况下 \(\phi\) 等于真实值函数，则 \(T=1\) 就够（退化成贪婪的单步值函数优化）；现实中值函数难求，于是用一个较长的 \(T\) 去"展开"出大部分剩余代价、再用一个粗略的 \(\phi\) 收尾。这解释了 §2.4 会提到的方向：**用 RL 学到的值函数作为 MPPI 的终端代价，就能把所需的 \(T\) 大幅缩短（剩余代价由学到的 \(\phi\) 提供），从而省下 §2.2 的串行时间——MPPI 看得更远却算得更快。"\(T\) 和 \(\phi\) 此消彼长"是这背后的统一规律：\(\phi\) 越准，\(T\) 可越短。

实战中 \(\phi\) 怎么设。 真实值函数通常不知道，所以 \(\phi\) 是个近似，常见几种：其一，到目标的距离/二次型——最简单，把"离目标还有多远"当剩余代价的粗略代理（上面的例子其实就是把它融进了运行代价）；其二，学到的值函数——用 RL 训练一个 critic，输出状态的剩余代价估计，作为 \(\phi\)（这是 §2.4 提到的 TD-MPC 一类方法的核心）；其三，任务相关的手工代价——如"末端速度应为零"（避免冲过头）、"末端姿态应稳定"。

易错点是**完全不设终端代价（\(\phi=0\)）却用很短的 \(T\)**——这会让 MPPI 极度短视，典型现象是冲向目标却不减速、或在障碍前不提前规避。要么把 \(T\) 加长到能看见后果，要么补一个像样的 \(\phi\)，二者至少占一个。

控制约束：把采样钳到执行器能做到的范围 ⭐⭐¶

还有一个真实机器人上绕不开、却最容易被忽略的细节：控制约束。MPPI 的微扰控制是 \(v_t = u_t + \varepsilon_k(t)\)，而 \(\varepsilon_k(t)\) 是高斯噪声、理论上能取任意大的值——于是 \(v_t\) 很可能超出执行器的物理极限（电机扭矩上限、舵机角度范围、油门 \([0,1]\) 区间等）。真实机器人根本执行不了一个超限的控制，仿真器要么也饱和、要么给出非物理的结果。如果不处理，你就是在优化一批**机器人做不到**的控制序列，sim-to-real 时必然出问题。

为什么需要钳位、钳在哪。 解决办法是**钳位（clamp / saturation）：把微扰后的控制限制到执行器的可行区间 \([u_\min, u_\max]\)。关键是钳在**正确的位置——要在 rollout 内部、把 \(v_t\) 喂给动力学**之前**钳，因为 rollout 必须基于"机器人真能执行的控制"来预测，这样算出的代价才反映真实后果；同时，最终执行的 \(u_0\) 也要钳。把它跑出来看（执行器极限设 \(\pm 5\)）：

A_MAX = 5.0
rng = np.random.default_rng(2)
eps = sigma*rng.standard_normal((K,T,2)); V = U[None] + eps   # 微扰控制
print(f"不钳位: |a| 最大 = {np.abs(V).max():.2f}  (超出 ±{A_MAX})")
Vc = np.clip(V, -A_MAX, A_MAX)                                # ★ 钳到执行器范围
print(f"钳位后: |a| 最大 = {np.abs(Vc).max():.2f}")

输出：

不钳位: |a| 最大 = 12.76  (超出 ±5.0)
钳位后: |a| 最大 = 5.00

不钳位时，采样控制的幅度冲到 \(12.76\)、远超执行器能力的 \(\pm 5\)；钳位后规规矩矩落在 \(\pm 5\) 内。而且钳位**不损害**到达能力——把钳位加进闭环（rollout 内钳、执行 \(u_0\) 也钳），2-D 导航仍在 \(56\) 步内到达目标。下面按四步给出。

第一步：为什么这样写。 钳位让 rollout 基于"真能执行的控制"做预测、代价才真实，且最终输出不会发出机器人做不到的指令。

第二步：正确写法。

def step_batch_clamped(S, A, a_max):       # rollout 内：喂动力学前先钳
    A = np.clip(A, -a_max, a_max)          # ★ 钳到执行器范围
    vx = 0.95*S[:,2] + A[:,0]*dt; vy = 0.95*S[:,3] + A[:,1]*dt
    return np.stack([S[:,0]+vx*dt, S[:,1]+vy*dt, vx, vy], 1)
# ... 加权更新后，执行前再钳一次：
u_exec = np.clip(U[0], -a_max, a_max)      # ★ 实际下发的控制也要钳

第三步：错误写法（以及为什么错）。

# ❌ 错误 1：根本不钳，直接把超限控制喂给动力学/机器人
S = step_batch(S, U[None] + eps)           # |a| 可能 >> a_max
# 现象：仿真里 rollout 基于做不到的大控制，算出的最优序列在真机上无法执行，sim-to-real 崩

# ❌ 错误 2：只在最后钳执行的 u₀，rollout 内不钳
u_exec = np.clip(U[0], -a_max, a_max)      # 执行钳了，但 rollout 全程没钳
# 现象：rollout 用的是超限控制预测的轨迹，代价失真，优化出的 U 本身就基于幻觉

# ❌ 错误 3：去钳噪声 ε 而非控制 v
eps = np.clip(eps, -a_max, a_max)          # 钳错对象！钳的应是 v=u+ε，不是 ε
# 现象：u 本身不为零时，u+ε 仍可能超限；约束没真正生效

第四步：不同处理方式对比。

# === 三种处理控制约束的方式 ===
# 方式 A：硬钳位 clip（最简单，推荐起步）——超限就削到边界
v_A = np.clip(u + eps, -a_max, a_max)
# 方式 B：tanh 软饱和（平滑、可微，适合需光滑控制的场景）
v_B = a_max * np.tanh((u + eps) / a_max)
# 方式 C：在代价里加越限惩罚（软约束，让 MPPI 自己学会不越限）
#   cost += w_lim * relu(|v| - a_max)**2     # 不强制，但倾向不越限
# 对比：
# | 方式 | 是否硬保证不越限 | 平滑性 | 适用 |
# | A clip       | 是 | 不可微(边界有拐) | 绝大多数，简单可靠 |
# | B tanh       | 是 | 平滑 | 需光滑控制/与梯度法混用 |
# | C 罚值       | 否(软) | 取决于罚函数 | 想让采样"自然"避开边界 |

最后一个值得注意的关联：钳位与样本效率相互作用。如果 \(\Sigma\) 取得过大、执行器范围又窄，大量 rollout 的控制会被一起削到同一条边界上——这些被"压扁"到边界的样本彼此雷同，等于白撒，又是一种样本浪费（呼应 §2.1 的样本效率主线）。所以 \(\Sigma\) 的标尺要与执行器范围匹配（§2.1 的 \(\Sigma\) 设计要点），别让钳位把探索"吃掉"。

代价函数设计：MPPI 真正的自由度 ⭐⭐¶

MPPI 最大的卖点是"代价可以是任意标量函数"（§2.4 的杀手级场景就建立在这之上）。但"任意"是一把双刃剑——它意味着**代价函数完全由你设计，MPPI 只会忠实地优化你写下的东西，包括你写错的东西**。代价设计不好，再怎么调 \(\lambda\)/\(\Sigma\)/\(K\) 都救不回来。所以代价设计本身就是一门要专门讲的手艺。

运行代价 vs 终端代价的分工。 代价分两部分：运行代价 \(\ell(x,v)\) 在每个时步累加，塑造**整条路径**的好坏（离障碍多近、控制多费力、是否贴着参考轨迹）；终端代价 \(\phi(x)\) 在末尾加一次，编码**时域之外的剩余代价**（§2.1 终端代价，本质是值函数）。

两者要平衡：终端权重过大，MPPI 只盯着"末端到没到"而不在乎沿途（可能横冲直撞过去）；运行权重过大、终端缺位，则短视（§2.1 已分析）。一个常见的健康结构是"运行代价管过程、终端代价管收尾"，二者量级相当。

代价尺度与 \(\lambda\) 耦合。 这是最容易被忽略的一点：把代价整体放大若干倍，等价于改变了 \(\lambda\)。因为权重是 \(e^{-S/\lambda}\)，代价乘 \(10\) 和 \(\lambda\) 除以 \(10\) 对权重的效果一样。

所以代价各项的权重和温度 \(\lambda\) 不是独立的——它们一起决定权重的尖锐程度（ESS）。实践上：先把代价设计在一个合理的量级（各项可比、不爆炸），再用 ESS 调 \(\lambda\)（§2.1 \(\lambda/\Sigma\)）；别一边乱放大代价、一边乱调 \(\lambda\)，那是在和自己较劲。

硬惩罚（indicator）vs 软惩罚（barrier）。 对障碍、约束这类"不希望进入的区域"，有两种写法。indicator（进入就 \(+\) 一大笔）非光滑——MPPI 能处理（§2.4 的核心优势），但它只给"撞没撞"的二元信号，在边界附近不提供"离得越近越该躲"的梯度，MPPI 全靠采样里恰好有跨越边界的样本来感知它。

软 barrier（代价随接近而平滑增长，如 \(\propto e^{-d^2/\sigma^2}\) 或 \(1/d\)）则给出一个**渐变的引导信号**，在还没接触时就温和地把轨迹推开，往往更利于引导、采样效率也更高（§2.4 练习 1 让你亲手对比）。取舍是：indicator 是"二元安全线"（要么安全要么不），barrier 是"渐变引导"（早早避让但边界不绝对）；安全攸关常二者叠加——barrier 引导 + 硬约束兜底。

常见的代价设计坑。 其一，各项量级悬殊：一项是 \(10^3\) 级的碰撞罚、一项是 \(10^{-2}\) 级的控制正则，求和时小项被淹没（fp32 下"大数吃小数"更甚，§2.2），等于那项白写——各项要无量纲化或调到可比量级。

其二，忘记控制正则项：不惩罚控制大小，MPPI 会用尽可能大的控制去压代价，输出剧烈、还容易越限。其三，**代价尺度与 \(\lambda\) 失配**导致 ESS 长期不健康（接上一条）。把代价当成"你对任务的精确描述"来写——MPPI 会一字不差地照办，所以描述错了，行为就错。

至此 §2.1 把 Ch1 那个朴素的权重公式补成了一个能部署的算法。把这一节的工程细节收成一张表，看清每一个"是什么、为什么、不做会怎样"——它们各自补上了朴素版的一处缺口：

细节	解决什么	不做会怎样	必需性
\(\rho\) 减法	大代价下指数数值稳定	\(e^{-S/\lambda}\) 整批下溢 → \(0/0\) → NaN（GPU fp32 尤甚）	不做就崩
warm-start	复用上周期解、改善提议位置	每周期冷启动、提议扔回原点 → 收敛慢甚至到不了	实时必需
SGF 平滑	磨平控制高频抖动	控制抖动、电机嗡鸣（执行器受损）	可选（看执行器）
\(\lambda\) / \(\Sigma\)	调利用强度 / 探索广度	权重过尖（贪婪、方差大）或过平（不利用代价）	必调
ESS 监控	度量样本浪费、指导调 \(\lambda\)	不知道采样在不在干活、\(\lambda\) 全靠猜	强烈建议
终端代价 \(\phi\)	近似 cost-to-go、让短时域不短视	短 \(T\) 下冲过目标、不提前规避	短 \(T\) 必需
控制约束钳位	把采样钳到执行器范围	优化出机器人做不到的控制 → sim-to-real 崩	真机必需
代价设计	精确描述任务目标	描述错 → 行为错（量级失衡、缺正则）	必需

这张表也是一条"从玩具到部署"的清单：Ch1 的 20 行核加上这 8 项，才是一个能上真车的 MPPI。它们不是零散技巧，而是分别堵上了朴素版在**数值**（\(\rho\)）、实时（warm-start）、执行器友好（SGF、钳位）、采样质量（\(\lambda\)/\(\Sigma\)/ESS）、远见（终端代价）、任务正确性（代价设计）这几个维度上的漏洞。

⚠️ 常见陷阱¶

编程陷阱：权重计算不减 \(\rho\)，大代价下直接 NaN。 - 错误描述：直接写 w = np.exp(-S / lam); w /= w.sum()，没有先减去 S.min()。 - 现象/后果：仿真里代价是个位数时侥幸正常；一换到真实尺度的代价（几百上千），exp(-S/λ) 整批下溢为 \(0\)，归一化得到 NaN，控制输出 NaN，机器人失控。这是新手最常见、最隐蔽的故障——因为它在小代价测试里不暴露。 - 根本原因：64 位浮点的 \(e^{-x}\) 在 \(x\gtrsim 709\) 时下溢为 \(0\)；MPPI 的代价是几十步累加，轻松超过这个阈值，导致权重集体归零。 - 正确做法：rho = S.min(); w = np.exp(-(S - rho)/lam); w /= w.sum()——减 \(\rho\) 不改变权重的数学值（归一化里抵消），但把最大指数钉在 \(e^0=1\)，杜绝整批下溢。 - 自检方法：在权重计算后加断言 assert np.isfinite(w).all() and abs(w.sum()-1) < 1e-6；或故意把代价乘以一个大系数跑一遍，看不减 \(\rho\) 的版本是否崩、减 \(\rho\) 的版本是否稳。

概念误区：以为 SGF 平滑是 MPPI 算法的必要组成部分。 - 错误描述：认为"MPPI = 采样 + 加权 + SGF 平滑"，把 SGF 当成和权重公式同等地位的核心步骤。 - 现象/后果：在对抖动不敏感的场景（仿真、带低通特性的执行器）也无脑套 SGF，白白损失 MPPI 的信息论最优性；或误以为不加 SGF 的 MPPI "不完整""有 bug"。 - 根本原因：没分清"理论核心"与"工程外挂"。MPPI 的理论核心是 Ch1 推出的加权更新；SGF 是事后额外加的、破坏**信息论最优性的平滑步骤，用途仅是改善执行器体验。 - **正确做法：把 SGF 视为可选项——抖动伤执行器时才加，否则保留理论最优的原始输出。需要平滑又想保最优性时，转向采样层面的方案（有色噪声等，第 3 章）。

思维陷阱：以为"加大 \(K\)"能解决一切，忽视 warm-start/提议分布。 - 错误描述：控制器表现不好时，第一反应是把采样数 \(K\) 翻倍、再翻倍，指望"多撒点样本"自动变好。 - 现象/后果：\(K\) 翻倍换来算力翻倍，但若提议分布本身偏（如每周期冷启动），收益递减得很快；该收敛的还是不收敛，钱花了效果差。 - 根本原因：Ch1 已证明"提议分布质量优先于样本数"——\(K\) 只增加提议分布的样本数（在原地多撒），而真正决定成败的是提议分布的位置（撒在哪）。warm-start 改善的恰是位置。 - 正确做法：先确保 warm-start 把采样锚在上一周期的好解附近（改善位置），再在此基础上按 ESS 调 \(\lambda\)、按需调 \(\Sigma\)；\(K\) 取够用即可，不是越大越好。

练习¶

（实现题） 在上面的 2-D 导航代码里加入 SGF 平滑：每周期更新 \(U\) 后、执行 \(u_0\) 前，对两维控制序列各做一次 savgol。分别记录加 SGF 前后的"加速度相邻差分均方"和"到达目标步数"，量化 SGF 在这个阻尼系统上对平滑度和性能各自的影响。
（调试挑战） 给你一段 MPPI 代码，它在仿真小代价下正常，一旦把代价整体乘以 \(100\) 就输出 NaN。已知它的权重计算是 w = np.exp(-S/lam); w/=w.sum()。定位 bug、解释为什么乘 \(100\) 才触发、给出修复，并说明修复后权重的数学值是否改变、为什么。
（设计扩展题） 把 warm-start 的末位填充策略从"填零"改成"复制末控制 \(U[-1]\)"，在 2-D 导航（阻尼系统）和一个你设想的"悬停"系统（无阻尼、需持续控制维持）上分别推演：哪种系统下两种填充策略差别明显？为什么？把你的推理和 §2.1 中三种填充策略的适用表对照。
（实现题·调参） 在 2-D 导航的 MPPI 里，每周期算出权重后顺带计算并打印 ESS \(=1/\sum_k\hat w_k^2\)。先固定 \(\Sigma\)，把 \(\lambda\) 从很小扫到很大，记录 ESS 与到达目标步数的变化，找出让 ESS 落在 \(K\) 的 \(5\%\sim50\%\) 的 \(\lambda\)；再固定这个 \(\lambda\) 改变 \(\Sigma\)，观察 ESS 如何随之变化。用你的数据说明"\(\lambda\) 与 \(\Sigma\) 必须一起调"这一点。
（设计扩展题·终端代价） 给 2-D 导航把时域 \(T\) 砍到很短（如 \(T=5\)），不加终端代价直接跑，观察是否出现"冲过目标/不提前避障"的短视现象。然后加一个终端代价 \(\phi(x)=w\cdot\big(\|p-p_\text{goal}\|^2+\|v\|^2\big)\)（末端位置接近目标且速度趋零），重跑并对比。解释终端代价为什么能让短时域不再短视，并把它和"\(\phi\) 近似值函数（cost-to-go）"这一本质联系起来。

§2.2 CUDA 并行化策略 ⭐⭐⭐¶

动机：把"独立的 \(K\) 条 rollout"真正同时跑起来¶

§2.1 给出了完整且数值稳定的 MPPI 算法，但它还停在"能算对"的阶段，没到"算得快"。瓶颈一眼可见——伪代码第 1 步那个 for k = 1..K 的 rollout 循环：要采 \(K\) 条轨迹（实战里 \(K\) 常取 \(1000\) 到 \(4000\)），每条都要前向积分 \(T\) 步（\(T\) 常取 \(30\) 到 \(128\)），每步还要算一次动力学和代价。在单核 CPU 上一条接一条地串行跑，这是一笔沉重的计算；机器人控制要求每个周期（\(50\,\text{Hz}\) 即 \(20\,\text{ms}\)）算完，串行 CPU 根本跟不上。

但回顾 Ch1 那个反复强调的本质洞察：\(K\) 条 rollout 彼此完全独立——第 \(k\) 条轨迹的前向积分，不依赖第 \(j\) 条的任何中间结果。这种"互不依赖、可同时进行"的计算，在并行计算里有个专门的名字叫**尴尬并行（embarrassingly parallel）**，而它正是 GPU 最擅长的负载。

GPU 有数千个计算核心，如果把 \(K\) 条 rollout 分摊到这数千个核心上同时跑，那个串行的 for k 循环就被"摊平"成一次并行计算——这就是本节要做的事，也是 MPPI 能在真实机器人上实时运行的根本原因。

如果不这样做会怎样¶

坚持用 CPU 串行跑 rollout，代价是定量的：

实现	配置	单次迭代耗时	能否 50 Hz 实时
CPU 串行（Eigen, i7-12700）	\(K=2048,\ T=64\), 12 维状态	~80 ms	✗（差 4 倍）
GPU 并行（RTX 3080）	\(K=2048,\ T=64\), 12 维状态	~1.5 ms	✓（余量充足）
GPU 并行（RTX 3080）	\(K=4096,\ T=128\), 12 维状态	~4 ms	✓

（数据引自 MPPI-Generic 论文 Vlahov 2024 的基准测试。）CPU 串行版单次迭代 \(80\,\text{ms}\)，连 \(20\,\text{ms}\) 的周期预算都进不去，更别说留余量给感知、估计、通信；而 GPU 版 \(1.5\,\text{ms}\) 绰绰有余——约 50 倍的加速，正是这个加速把 MPPI 从"仿真里的算法"变成了"真车上的控制器"。不做 GPU 并行，要么被迫把 \(K\) 砍到几十（采样太少、控制质量崩坏），要么被迫降低控制频率（反应迟钝、稳定性变差），两条路都走不通。

历史：GPU 工程篇¶

把 MPPI 的 GPU 实现讲透的，是 Williams、Aldrich、Theodorou 2017 年发在 JGCD 40(2) 的 MPPI: From Theory to Parallel Computation。

如果说 2018 年的 T-RO（即 Ch1）回答了"为什么对"，这篇 2017 的工作回答的就是"怎么算快"——它详细给出了 register / shared / global 三级内存层级下的 rollout kernel 该如何设计，是本节内容的主要来源。

再往前，2016 年那篇 AutoRally 首次实车的工作已经在 GPU 上跑通了 MPPI，证明这条路可行；2017 把工程细节系统化，2024 的 MPPI-Generic 则把这套设计沉淀成可复用的库（§2.3）。

线程映射：谁来算哪一条轨迹 ⭐⭐⭐¶

为什么需要想清楚映射。 GPU 的计算资源是按层级组织的：一次 kernel 启动会拉起一个 grid，grid 由若干 block 组成，每个 block 又含若干 thread，而硬件实际调度的最小单位是 warp——32 个线程锁步执行同一条指令。

要把 \(K\) 条 rollout 跑在这套层级上，第一件事就是决定**映射**：哪些线程负责哪一条 rollout、一条 rollout 内部的 \(T\) 个时步和状态各维又怎么分给线程。映射没设计好，要么大量线程闲置（占用率低、算力浪费），要么同一 warp 内的线程走上不同分支（warp divergence，串行化执行、性能暴跌）。所以"线程映射"不是实现细节，而是决定 GPU 版 MPPI 快慢的核心设计。

两种主流映射方案。 围绕"用多少线程算一条 rollout"，有两种典型策略：

方案	一条 rollout 由谁算	优点	缺点
每 thread 一条轨迹	1 个线程串行算完整条 rollout 的 \(T\) 步	实现最简单，无 warp 内协作	单线程要存整条轨迹的状态，寄存器压力大；状态维高时占用率低
每 warp/block 一条轨迹	一个 warp（或 block）的多个线程协同算一条 rollout	状态各维、代价归约可并行；寄存器分摊	需要 warp 内同步与归约，实现复杂；时步间仍有串行依赖

第一种把"第 \(k\) 条 rollout"整个交给第 \(k\) 个线程，线程内部用一个 for t 循环串行走完 \(T\) 步——简单直接，\(K\) 条 rollout 就是 \(K\) 个线程，天然并行。它的短板在于：每个线程要独占地保存当前状态 \(x_t\)、控制 \(v_t\) 等，状态维度一高（比如四旋翼的 12 维），单线程的寄存器需求就很大，挤占了 GPU 能同时驻留的线程数（占用率下降），并行度反而受限。

第二种（MPPI-Generic 采用的风格）把一条 rollout 交给一**组**线程协同处理：一个 warp 的 32 个线程里，一部分负责算控制的各维、一部分负责算状态的各维、一部分参与代价的并行归约。

这样状态的多个维度可以同时算、累积代价可以用 warp 内归约快速求和，单个线程的寄存器压力也被分摊。代价是实现复杂——需要 warp 内的同步与归约原语，而且一条 rollout 的 \(T\) 个时步之间存在**串行依赖**（\(x_{t+1}\) 要等 \(x_t\) 算完），这部分无法在时步维上并行，只能靠 warp 内对"每个时步内部"的并行来提速。

设计动机：占用率与 warp divergence 的权衡。 为什么 MPPI-Generic 倾向"每 warp/block 一条轨迹"？两个原因。其一是**占用率（occupancy）**：高维状态下，"每线程一条"的寄存器压力会限制同时驻留的线程数，把状态各维分摊到 warp 内多个线程能缓解这一点。

其二是**减少 warp divergence**：如果一个 warp 内的 32 个线程各算**不同**的 rollout，而不同 rollout 可能走进代价函数里不同的分支（比如有的撞障碍、有的没撞），同一 warp 内就会出现分支分歧，硬件被迫串行执行两条分支路径，性能腰斩。

让一个 warp 协同算**同一条** rollout，则 warp 内线程走的是同一条控制流，从根本上避免了这类 divergence。这是一个典型的"用实现复杂度换硬件效率"的工程取舍。

下面给出一个最简的 rollout kernel 骨架（"每 thread 一条轨迹"方案，最易懂；MPPI-Generic 的 warp 协同版更复杂，结构见 §2.3）。注意：这段 CUDA 代码用于讲解线程映射的结构，不在本章环境中编译运行（本机无 GPU），其数值逻辑与 §2.1 的 NumPy/CPU 版完全一致、由后者佐证正确性。

// 最简 rollout kernel：每个线程负责一条 rollout（教学示意，非编译产物）
__global__ void rollout_kernel(
    const float* x0,        // 初始状态 [state_dim]
    const float* U,         // 当前控制序列 [T * control_dim]
    const float* eps,       // 预生成的噪声 [K * T * control_dim]
    float* J,               // 输出：每条 rollout 的总代价 [K]
    int K, int T, int state_dim, int control_dim)
{
    int k = blockIdx.x * blockDim.x + threadIdx.x;   // 这个线程算第 k 条 rollout
    if (k >= K) return;                              // 越界保护

    float x[STATE_DIM];                              // 当前状态（寄存器/局部）
    for (int i = 0; i < state_dim; ++i) x[i] = x0[i];
    float cost = 0.0f;

    for (int t = 0; t < T; ++t) {                    // 时步：串行依赖，逃不掉
        float v[CONTROL_DIM];
        for (int j = 0; j < control_dim; ++j)
            v[j] = U[t*control_dim + j] + eps[(k*T + t)*control_dim + j];  // 微扰控制
        forward_dynamics(x, v);                      // 黑箱动力学：x ← F(x, v)
        cost += running_cost(x, v);                  // 累加运行代价
    }
    cost += terminal_cost(x);                        // 终端代价
    J[k] = cost;                                     // 写回 global memory（每条只写一次）
}

这个 kernel 的并行结构一目了然：第 k 个线程通过 blockIdx.x * blockDim.x + threadIdx.x 拿到自己的全局编号，独立地算第 \(k\) 条 rollout，互不干扰；\(K\) 条 rollout 就是 \(K\) 个线程，由 GPU 自动调度到数千个核心上同时跑。时步循环 for t 仍是串行的（状态有前后依赖），但**条与条之间**完全并行——这正是 Ch1"\(K\) 条独立"那句话在 CUDA 里的样子。

进阶：warp 协同 rollout 的真实结构。 上面的"每线程一条"是教学起点，MPPI-Generic 实际用的是"每 block 多条、warp 内协同算一条"的映射——还记得 §2.3 最小示例里那行 dynamics_rollout_dim_ = dim3(64, STATE_DIM, 1) 吗？现在能完全读懂它了：这个 dim3 说的是"一个 block 配 \(64\times\text{STATE\_DIM}\) 个线程"，其中**第一维 \(64\)** 是"这个 block 同时处理 64 条 rollout"，第二维 \(\text{STATE\_DIM}\) 是"用 STATE_DIM 个线程协同处理一条 rollout 的状态各维"。

也就是说，block 里的线程排成一个二维网格：沿 \(x\) 方向是 64 条不同的 rollout，沿 \(y\) 方向是同一条 rollout 的 STATE_DIM 个状态分量，每个线程负责"某条 rollout 的某个状态维"。

这种映射好在哪？回到前面那张权衡表的两条理由。其一，状态各维并行：前向动力学 \(x\leftarrow F(x,v)\) 里，更新 12 个状态分量的计算可以分给 12 个线程同时做，而不是一个线程串行算 12 遍——单个时步内部被并行化了（注意：时步**之间**仍串行，这一点没变）。

其二，寄存器压力分摊：每个线程只负责一个状态维，不必独占整条轨迹的全部状态，寄存器需求小、能驻留更多线程、占用率高。代价是需要 warp 内的协同——比如算运行代价时，各维线程算出的部分代价要在 warp 内归约求和（用 __shfl_down_sync 这类 warp 级原语，比走 shared memory 还快）。

把"每线程一条"和"每 block 多条、warp 协同"对照着记：前者是"一个人从头到尾干完一件事"，简单但一个人要记住所有中间状态（寄存器压力大）；后者是"一组人分工干同一件事的不同部分"，要协调（同步、归约）但每个人负担轻、还能并行推进。

dim3(64, STATE_DIM, 1) 这一行，就是 MPPI-Generic 把后一种分工写死在 kernel 启动配置里——调性能时调的第一维（多少条一组）和它与 warp 大小 32 的关系，直接影响占用率和 divergence。

内存层级：什么数据放哪一级¶

GPU 的内存是分层的，访问速度差着数量级：**寄存器（register）**最快但每线程容量有限，**共享内存（shared memory）**是 block 内线程共享的高速片上内存（比寄存器稍慢、但远快于全局内存），**全局内存（global memory）**容量最大但最慢。MPPI 的 rollout kernel 要跑得快，关键就是把对的数据放对的层级——频繁访问的放近的（寄存器/共享），只用一次的放远的（全局）：

数据	放哪一级	为什么
当前时步的状态 \(x_t\)、控制 \(v_t\)	寄存器（per-thread）	每个时步都要读写、访问最频繁，必须放最快的寄存器
当前 warp/block 的累积代价、噪声片段	共享内存（block 内共享）	warp 内多线程协同归约时要互相访问，放共享内存兼顾速度与可见性
所有 rollout 的最终代价 \(J[K]\)、权重 \(w[K]\)	全局内存	每条 rollout 只写一次最终代价、之后由归约/CPU 读取，访问不频繁，放全局内存即可

这个分配背后的原则很统一：访问频率决定存放层级。状态 \(x_t\) 在 \(T\) 个时步里被反复读写，放进每线程的寄存器；最终代价 \(J[k]\) 整条 rollout 只写一次，放全局内存完全够用，把它放进稀缺的寄存器反而浪费。理解这条原则，你就能自己判断任何中间量该放哪——这也是 §2.3 里 MPPI-Generic 的 kernel 反复在做的事。

权重归约：把 \(K\) 个代价并行地汇成权重¶

rollout kernel 算出 \(K\) 个代价 \(J[K]\) 后，第 2 步要做三件归约：求 \(\rho=\min_k J_k\)、算 \(w_k=e^{-(J_k-\rho)/\lambda}\)、求 \(\eta=\sum_k w_k\)。

其中两个求极值/求和是典型的**归约（reduction）——把一个长数组汇成一个标量。在 GPU 上，归约不是用一个线程串行扫一遍（那就退回串行了），而是用**树形归约：每一轮让线程把数组对半折叠（前半与后半逐元素取 min 或相加），\(\log_2 K\) 轮后汇成一个值，并行度随之逐层减半但总轮数只有对数级。

这套树形归约的逻辑可以在 CPU 上用 NumPy 模拟出来，验证它与"直接算"完全等价（这也间接确认了 GPU 版归约的正确性，尽管 kernel 本身不在本机运行）：

import numpy as np
def tree_min(a):                          # 模拟 GPU 树形 min 归约
    a = a.copy()
    while len(a) > 1:
        if len(a) % 2: a = np.append(a, a[-1])      # 补齐偶数
        h = len(a)//2; a = np.minimum(a[:h], a[h:]) # 前后半折叠取 min
    return a[0]
def tree_sum(a):                          # 模拟 GPU 树形 sum 归约
    a = a.copy()
    while len(a) > 1:
        if len(a) % 2: a = np.append(a, 0.0)
        h = len(a)//2; a = a[:h] + a[h:]
    return a[0]

rng = np.random.default_rng(3); K = 2048; lam = 1.0
J = rng.uniform(800, 1300, K)             # 模拟 K 条 rollout 的代价
rho = tree_min(J)                         # 树形求 ρ
wexp = np.exp(-(J - rho)/lam)             # 减 ρ 后取指数（数值稳定）
eta = tree_sum(wexp); w = wexp/eta        # 树形求归一化常数
rho_ref = J.min(); w_ref = np.exp(-(J - rho_ref)/lam); w_ref /= w_ref.sum()
print(f"ρ 一致={np.isclose(rho, rho_ref)}, 权重最大误差={np.abs(w - w_ref).max():.1e}")

输出：

ρ 一致=True, 权重最大误差=1.4e-17

树形归约的结果与直接 J.min() / sum() 在浮点精度内完全一致（权重误差 \(1.4\times10^{-17}\)，即机器精度）。GPU 上真正的实现会用 shared memory 在 block 内做这棵树、再跨 block 汇总，但**逻辑就是这个折叠**——理解了 CPU 模拟，就理解了 GPU 归约在算什么。这一步也是常见竞态 bug 的高发区（折叠的写后读需要同步，见本节陷阱）。

GPU 上的真实 kernel 长什么样？下面是 block 内 shared memory 树形求和的标准写法（求 \(\eta=\sum_k w_k\) 的核心，示意、未编译）。注意 __syncthreads() 的位置——它正是本节陷阱里那个竞态的解药：

// block 内 shared memory 树形归约求和（每 block 归约一段 w，示意未编译）
__global__ void reduce_sum_kernel(const float* w, float* block_sums, int K) {
    extern __shared__ float sdata[];
    int tid = threadIdx.x;
    int i   = blockIdx.x * blockDim.x + threadIdx.x;
    sdata[tid] = (i < K) ? w[i] : 0.0f;     // 每个线程搬一个元素进 shared memory
    __syncthreads();                         // ★ 屏障：确保全部搬完再开始折叠

    // 对半折叠：每轮活跃线程减半，log2(blockDim) 轮
    for (int s = blockDim.x / 2; s > 0; s >>= 1) {
        if (tid < s) sdata[tid] += sdata[tid + s];   // 前半 += 后半
        __syncthreads();                     // ★ 屏障：这一层写完，下一层才能读
    }
    if (tid == 0) block_sums[blockIdx.x] = sdata[0]; // 每 block 的部分和
}

把它和前面的 CPU tree_sum 对照：for (int s = blockDim.x/2; s>0; s>>=1) 就是 CPU 里的 while(len>1){...len//=2}——同一个"对半折叠"，只是 GPU 上每一层的加法由多个线程并行完成。两个 __syncthreads() 缺一不可：第一个保证所有元素都搬进 shared memory 后才开始折叠；循环内的那个保证"这一层的写"对"下一层的读"可见。

漏掉循环内的 __syncthreads()，就是本节编程陷阱里那个"时对时错、每次结果不同"的竞态——某些线程还没写完 sdata[tid+s]，另一些线程已经读它去加了。每个 block 算出一个部分和后，再用同样的方式（或一次小的二级归约）把 block_sums 汇总成最终的 \(\eta\)。求 \(\rho=\min_k J_k\) 完全同理，只需把 += 换成 min。

主机与设备之间的数据流：与 warm-start 的配合¶

最后一个常被忽略、却直接影响实时性的工程点：主机（CPU）和设备（GPU）之间每周期传什么。性能基准表里提过，数据传输有固定开销；如果每周期把所有东西（整条噪声数组 \(K\times T\times m\)、控制序列、动力学参数）在 CPU 和 GPU 之间来回搬，这个开销会迅速吃掉 GPU 的并行收益，尤其当 \(K\) 大时噪声数组本身就有几百万个浮点数。

正确的做法是**让大块数据常驻设备**，每周期只传极小的量。具体说：噪声缓冲区、控制序列 \(U\)、动力学/代价的参数，都在初始化时分配在 GPU 上、之后一直留在那里；每个控制周期只需把**当前状态 \(x_0\)（几个浮点数）从 CPU 传到 GPU 作为 rollout 的起点，再把**要执行的 \(u_0\)（或整条更新后的 \(U\)，也很小）传回 CPU。\(K\times T\times m\) 的噪声生成、rollout、归约、控制更新，全程在设备上完成，不产生大块传输。

这一点和 §2.1 的 warm-start 配合得尤其自然：既然控制序列 \(U\) 常驻 GPU，那么 warm-start 的"左移一位、末尾补填"也就在设备上原地完成——上一周期优化好的 \(U\) 本来就在 GPU 显存里，左移它不需要任何主机↔设备传输。换句话说，warm-start 既复用了**计算**（上一周期的优化成果），又天然避免了**传输**（\(U\) 一直在设备端）。这是"算法设计"与"硬件数据流"互相成就的一个漂亮例子。

易错点有二：其一，别每周期重新分配设备缓冲区——cudaMalloc/cudaFree 开销很大，应在初始化时一次分配、循环里复用；其二，别每周期把整条噪声数组从主机传过去——噪声应在设备上用 cuRAND 之类直接生成，而非在 CPU 生成再拷贝。这两个坑都会让"看似已经 GPU 化"的实现实际被传输和分配拖垮，表现为加速比远不及预期。

设备端的两个细节：噪声生成与访存合并¶

上面提到"噪声应在设备上用 cuRAND 直接生成"和"访存要合并"，这两点是 GPU MPPI 跑得快的隐形功臣，值得展开。它们都涉及 CUDA，按本章约定**不在此环境编译**，但其原理是通用的、决定性的。

设备端噪声生成（cuRAND）。 §2.1 的伪代码每周期要采 \(K\times T\times m\) 个高斯噪声 \(\varepsilon_k(t)\)——\(K=2048,T=64,m=4\) 就是 \(52\) 万个浮点数。

如果在 CPU 上生成再拷到 GPU，每周期就要传半兆数据，正是 §2.2 数据流要避免的大块传输。正确做法是用 cuRAND（CUDA 的随机数库）直接在设备上生成：每个线程持有自己的 RNG 状态（curandState），在 rollout 时即用即采，噪声从不经过主机。模式大致是：

// 设备端噪声生成（cuRAND）：每个线程用自己的 RNG 状态采高斯噪声（示意，未编译）
__global__ void init_rng(curandState* states, unsigned long seed, int K) {
    int k = blockIdx.x * blockDim.x + threadIdx.x;
    if (k < K) curand_init(seed, k, 0, &states[k]);   // 每条 rollout 一个独立 RNG 流
}
__global__ void rollout_kernel(/*...*/, curandState* states) {
    int k = blockIdx.x * blockDim.x + threadIdx.x;
    curandState local = states[k];                    // 取到寄存器，循环里快
    for (int t = 0; t < T; ++t) {
        float eps = curand_normal(&local);            // 即用即采，不经主机
        float v = U[t] + eps;                         // 微扰控制
        /* forward_dynamics(x, v); cost += ... */
    }
    states[k] = local;                                // 写回 RNG 状态，供下周期续用
}

关键点有三：其一，每条 rollout 一个独立的 RNG 流（用线程编号 k 作为序列号 curand_init(seed, k, ...)），保证 \(K\) 条轨迹的噪声互相独立——这正是 Ch1"\(K\) 条独立"的前提，若所有线程共用一个流就会产生相关噪声、破坏采样。

其二，把 RNG 状态取到寄存器（local）再在循环里采，比每次读写全局内存的 states[k] 快得多（呼应 §2.2 的内存层级）。其三，RNG 状态要写回并跨周期续用，否则每周期重新 curand_init 既慢又可能产生周期间相关的噪声。

访存合并（memory coalescing）。 GPU 访问全局内存有一个硬性特性：当一个 warp 的 32 个线程访问**连续**的内存地址时，硬件能把它们合并成一次（或少数几次）内存事务；而当它们访问**跨步、分散**的地址时，则退化成多次事务，有效带宽暴跌。

这对 MPPI 的数据布局提出了要求——存放噪声 \(\varepsilon[K][T][m]\)、代价 \(J[K]\)、状态等数组时，要让"同一 warp 内相邻线程在同一时刻访问的元素"在内存里**相邻**。

本质洞察：在 GPU 上，"算什么"往往不如"内存怎么排"重要。同一个 rollout kernel，仅仅因为噪声数组的维度顺序不同（是 \(\varepsilon[K][T][m]\) 还是 \(\varepsilon[T][m][K]\)），就可能让相邻线程的访问从"连续可合并"变成"跨步分散"，性能差出数倍。这解释了为什么 §2.2 反复强调内存——register/shared/global 的**层级**决定单次访问快慢，而**布局**（合并与否）决定批量访问的有效带宽。一个"算法完全正确却慢得离谱"的 GPU MPPI，问题十有八九不在算法，而在访存模式：要么没合并、要么 warp divergence、要么 RNG 状态在全局内存里反复读写。写 GPU 代码时，脑子里要时刻有一张"这 32 个线程此刻在读哪 32 个地址、它们连续吗"的图。

这件事在工程上有个标准的抓手——结构体数组（AoS）还是数组结构体（SoA）。假设每条 rollout 的状态有多个字段（位置、速度……），AoS 是"把一条 rollout 的所有字段打包成一个结构体、再排成数组"（state[k].x, state[k].y, ...），SoA 是"每个字段单独排成一个数组"（x[k], y[k], ...）。

在"每线程一条 rollout、同一时刻所有线程都在读各自状态的 x 分量"这个 MPPI 典型访问模式下，SoA 让相邻线程读的 x[k]、x[k+1]……在内存里连续，硬件一次合并取走；AoS 则让相邻线程读的地址相隔一整个结构体的宽度（跨步访问），合并失败。

所以 GPU 上的数据通常按 SoA 组织——这不是风格偏好，而是为了让 warp 的访问落在连续地址上、吃到合并。这也呼应上面的洞察：同样的算法，AoS→SoA 的一个布局改动，可能就是几倍的带宽差。

这两个细节单独看都不起眼，合起来却常常是"GPU 版没达到预期加速"的真凶——噪声没在设备生成（大块传输）、访存没合并（带宽浪费）、RNG 状态在全局内存抖动（访问开销）。MPPI-Generic 这类成熟库在这些地方都做了精心处理，这也是它比"自己随手写的 kernel"快的原因之一。

GPU 上的数值精度：fp32 让 \(\rho\) 减法更关键¶

还有一个 GPU 特有、且与 §2.1 的 \(\rho\) 减法紧密相关的细节：浮点精度。GPU 上单精度（fp32）的算力通常是双精度（fp64）的若干倍（消费级显卡上差距尤其悬殊），所以 GPU MPPI 几乎都用 fp32 跑 rollout 和归约——这是性能的现实选择。但 fp32 的表示范围远窄于 fp64，这让 §2.1 那个"\(e^{-S/\lambda}\) 整批下溢"的数值问题来得**更早、更凶**。

把两种精度下指数函数的溢出阈值摆出来对比（实测）：

精度	\(e^{x}\) 上溢为 inf 的阈值	\(e^{-x}\) 下溢为 0 的阈值
fp64（双精度）	\(x \gtrsim 709\)	\(x \gtrsim 745\)
fp32（单精度）	\(x \gtrsim 88\)	\(x \gtrsim 104\)

差距是数量级的：fp64 要到代价/温度 \(S/\lambda \approx 709\) 才出问题，而 fp32 在 \(S/\lambda \approx 88\) 就开始上溢、\(104\) 左右就下溢为零。回想 §2.1——真实任务的代价 \(S\) 是几十步累加，几百上千是常态。

在 fp64 下，不减 \(\rho\) 也许还能撑到代价很大时才崩；但在 GPU 的 fp32 下，代价/温度只要过百，\(e^{-S/\lambda}\) 就整批下溢、归一化 \(0/0\) 给出 NaN——\(\rho\) 减法从"建议"变成了"不做就几乎必崩"。换句话说，§2.1 那行 S.min() 在 CPU fp64 上是保险，在 GPU fp32 上是命门。

本质洞察：精度选择把 §2.1 的数值技巧和 §2.2 的硬件现实拧在了一起。GPU 为了速度用 fp32，fp32 的窄范围又让 \(\rho\) 减法、log-sum-exp 这些"减最值再取指数"的技巧从可选变成必需——你越是想跑得快（fp32），就越离不开数值稳定化。这也是为什么成熟的 GPU MPPI 实现把 \(\rho\) 减法刻进归约的第一步：它不是锦上添花，而是 fp32 下不崩的前提。附带一提，fp32 的有限精度（约 7 位有效数字）还会让"几十步代价累加"损失尾部精度，所以代价的量纲设计要避免一项极大、一项极小同时累加（大数吃小数），必要时对代价做无量纲化或分段累加——这些都是 fp64 下不太显眼、fp32 下要当心的细节。

性能基准：50 倍加速从哪来、又在哪失效¶

把前面的数字摊开看：同样 \(K=2048,\ T=64\)、12 维状态，CPU 串行（Eigen, i7-12700）约 \(80\,\text{ms}\)，GPU（RTX 3080）约 \(1.5\,\text{ms}\)，约 50 倍。

这个加速比从哪来？根本是**吞吐**：CPU 几个核心一条一条算 \(2048\) 条 rollout，GPU 数千核心几乎同时开工，把"乘以 \(K\)"从时间成本变成了空间（核心数）成本。\(T\) 个时步仍是串行的（前后依赖），这部分 CPU、GPU 都逃不掉，但条间并行让总时间几乎只取决于"一条 rollout 的串行时长"，而非"\(K\) 条的总和"。

但 GPU 不是万能加速器，有几个会让加速比缩水甚至变负的场景，必须知道：

场景	为什么 GPU 不划算
\(K\) 很小（几十条）	拉不满 GPU 的数千核心，大量核心闲置；kernel 启动开销占比变大
kernel 启动 / 数据传输频繁	每周期主机↔设备传状态、传回代价有固定开销；\(K\) 小时这开销可能盖过收益
动力学/代价极轻量	每条 rollout 的计算太少，访存和调度开销占主导，并行收益被淹没
warp divergence 严重	同 warp 内线程走不同分支被串行化，等效核心数骤减（见陷阱）

换句话说，GPU 加速的前提是"\(K\) 足够大、每条 rollout 计算足够重、控制流足够齐整"。MPPI 恰好满足前两条（\(K\) 上千、每条要积分几十步带非平凡动力学），所以受益巨大；但如果你把 \(K\) 砍到 \(32\)、动力学简化成一行线性递推，GPU 版很可能还不如 CPU 快——这不是 GPU 的错，是负载不匹配。第三条（控制流齐整）则要靠 §2.2 的线程映射来保证。

本质洞察：GPU 给 MPPI 的，不是"把每条 rollout 算得更快"，而是"把 \(K\) 条 rollout 同时算"。单条 rollout 在 GPU 上甚至可能比 CPU 慢（GPU 单核频率低于 CPU），但 GPU 用数千核心的并行吞吐，把"\(\times K\)"这个乘法变成了几乎免费的并行展开。这也解释了 Ch1 的偏差分析（加大 \(K\) 收益递减 \(O(1/K)\)）与本章的工程现实如何衔接：理论说"\(K\) 不必无限大"，硬件说"\(K\) 也不能太小否则喂不饱 GPU"，二者一起把实战的 \(K\) 框在了一两千这个甜区。

没有 GPU 也能预演：CPU 上的批处理加速¶

GPU 的 ~50 倍加速数字是引自论文、且需要硬件才能复现。但"批量并行处理 \(K\) 条独立 rollout 远快于一条接一条"这个**原理**，不需要 GPU 就能在 CPU 上亲眼验证——只要把朴素的 for k 循环改写成 NumPy 向量化（一次对 \(K\) 条做同一运算），NumPy 底层会用 SIMD 和批处理替你并行，这正是 GPU SIMT（同指令多数据）思想在 CPU 上的影子。下面把两种写法计时对比（\(K=2048,\ T=64\)）：

import numpy as np, time
dt = 0.05; K = 2048; T = 64
rng = np.random.default_rng(0)
x0 = np.zeros(4); U = np.zeros((T,2)); eps = 3.0*rng.standard_normal((K,T,2))

# 朴素 for-k：一条接一条
def step1(s, a):
    vx = 0.95*s[2]+a[0]*dt; vy = 0.95*s[3]+a[1]*dt
    return np.array([s[0]+vx*dt, s[1]+vy*dt, vx, vy])
t0 = time.perf_counter(); J_loop = np.zeros(K)
for k in range(K):
    x = x0.copy()
    for t in range(T):
        x = step1(x, U[t]+eps[k,t]); J_loop[k] += ((x[0]-4)**2+(x[1]-4)**2)*dt
t_loop = time.perf_counter()-t0

# 向量化：一次算 K 条（SIMT 式）
def step_batch(S, A):
    vx = 0.95*S[:,2]+A[:,0]*dt; vy = 0.95*S[:,3]+A[:,1]*dt
    return np.stack([S[:,0]+vx*dt, S[:,1]+vy*dt, vx, vy], 1)
t0 = time.perf_counter(); S = np.tile(x0,(K,1)); J_vec = np.zeros(K)
for t in range(T):
    S = step_batch(S, U[t][None,:]+eps[:,t,:])
    J_vec += ((S[:,0]-4)**2+(S[:,1]-4)**2)*dt
t_vec = time.perf_counter()-t0
print(f"朴素 for-k: {t_loop*1000:.1f} ms | 向量化: {t_vec*1000:.1f} ms | "
      f"加速 {t_loop/t_vec:.1f}× | 数值最大差 {np.abs(J_loop-J_vec).max():.1e}")

输出：

朴素 for-k: 466.6 ms | 向量化: 7.8 ms | 加速 59.5× | 数值最大差 2.8e-14

仅仅把"一条接一条"改成"一次算 \(K\) 条"，在 CPU 上就快了约 60 倍，而且两种写法的结果在浮点精度内完全相同（差 \(2.8\times10^{-14}\)）。要说清楚：这个 \(60\times\) 是"向量化 NumPy vs 朴素 Python 循环"在 CPU 上的对比，与前面那个"GPU vs 优化过的 CPU(Eigen)"的 \(50\times\) 是两回事，不能混为一谈——但它们指向同一个原理：把 \(K\) 条独立 rollout 批量并行处理，远胜于串行逐条。这也是把算法移植到 GPU 前的标准做法：先在 CPU 上向量化跑通（验证逻辑与数值），确认无误再写 CUDA kernel，避免在 GPU 上调试逻辑错误。本章累积项目的"向量化 GPU 雏形"模块正是这一步。

控制频率与延迟预算：什么把 \(K\) 和 \(T\) 框死了¶

实时控制有一条铁律：每个控制周期必须在**截止时间**前算完。\(50\,\text{Hz}\) 的控制就是 \(20\,\text{ms}\) 一拍，MPPI 的"采样 + rollout + 归约 + 更新"必须挤进这 \(20\,\text{ms}\)（还要留出感知、估计、通信的时间）。这条预算直接框死了你能用多大的 \(K\) 和多长的 \(T\)——理解这个约束，才能在"采样够多、看得够远"和"算得够快"之间找到落点。

先看耗时怎么随 \(K\) 涨。把 §2.2 的向量化 rollout 在 CPU 上对不同 \(K\) 计时（\(T=64\)）：

import numpy as np, time
dt = 0.05; T = 64
def one_iter(K, rng):
    eps = 3.0*rng.standard_normal((K,T,2)); U = np.zeros((T,2))
    S = np.zeros((K,4)); J = np.zeros(K)
    for t in range(T):
        A = U[t][None,:] + eps[:,t,:]
        vx = 0.95*S[:,2]+A[:,0]*dt; vy = 0.95*S[:,3]+A[:,1]*dt
        S = np.stack([S[:,0]+vx*dt, S[:,1]+vy*dt, vx, vy], 1)
        J += ((S[:,0]-4)**2+(S[:,1]-4)**2)*dt
    rho = J.min(); w = np.exp(-(J-rho)); w /= w.sum()
    return (w[:,None,None]*eps).sum(0)
for K in [256, 512, 1024, 2048, 4096]:
    rng = np.random.default_rng(0); one_iter(K, rng)            # warmup
    t0 = time.perf_counter()
    for _ in range(5): one_iter(K, rng)
    print(f"K={K:5d}: {(time.perf_counter()-t0)/5*1000:5.1f} ms/迭代")

某次运行的输出（耗时随机器与负载波动，看趋势）：

K=  256:   2.8 ms/迭代
K=  512:   4.3 ms/迭代
K= 1024:   6.8 ms/迭代
K= 2048:  11.8 ms/迭代
K= 4096:  21.8 ms/迭代

耗时随 \(K\) 近似线性**增长（每条 rollout 的工作量相同，\(K\) 条就是 \(K\) 倍）。在这台 CPU 上，\(K=2048\) 约 \(11.8\,\text{ms}\)、刚好挤进 \(20\,\text{ms}\) 预算，但 \(K=4096\) 就 \(21.8\,\text{ms}\)、**超了——纯 CPU 下你被迫把 \(K\) 压在 \(2000\) 上下。

这正是 §2.2 GPU 的意义所在：同样 \(K=2048\)，GPU 约 \(1.5\,\text{ms}\)（§2.2 基准），不仅轻松进预算，还留出大把余量让你把 \(K\) 加到 \(4096\)、甚至把省下的时间分给更长的 \(T\)。

延迟预算对 \(K\) 和 \(T\) 的约束机理不同，值得分开看。\(K\) 影响吞吐：\(K\) 条 rollout 并行，CPU 上线性增加耗时、GPU 上只要喂得饱核心就几乎不增加（直到耗尽核心）——所以 GPU 平台可以用大 \(K\)，CPU 平台只能用小 \(K\)。

\(T\) 影响串行链长：一条 rollout 的 \(T\) 个时步是串行的（§2.2 反复强调的依赖），\(T\) 越大，单条 rollout 越慢，这部分 GPU 也省不掉（并行只在条间）——所以即便有 GPU，\(T\) 也不能无限长，它直接拉长每次迭代的最短时间。一句话：GPU 让你能用大 \(K\)，但救不了大 \(T\)；想看得更远（大 \(T\)）又要实时，得靠更高效的单步动力学、或用学到的值函数缩短所需 \(T\)（§2.4 提过的终端值函数）。

本质洞察：MPPI 的实时性不是"算得快不快"的单一问题，而是"\(K\)、\(T\)、控制频率、硬件"四者的联合约束。控制频率定下每拍的截止时间；\(T\) 决定单条 rollout 的最短串行时长（GPU 也压不动）；\(K\) 决定要并行多少条（GPU 能压、CPU 不能）；硬件决定并行吞吐。调 MPPI 上实时，本质是在这个四维约束里找可行点：频率太高就得减 \(T\) 或换 GPU，\(T\) 必须大就得降频或简化动力学，没 GPU 就得砍 \(K\)（牺牲采样质量）。Ch1 的偏差分析（\(K\) 收益递减 \(O(1/K)\)）在这里和硬件约束合流——理论说 \(K\) 不必太大，预算说 \(K\) 不能太大，二者共同把实战的 \(K\) 钉在硬件允许的甜区。

从 CPU 到 GPU：一条务实的移植路线¶

把前面散落的工程建议收成一条可操作的路线。直接上手写 CUDA kernel 是新手最容易踩坑的做法——逻辑错误和 CUDA 工程错误混在一起，极难排查。务实的顺序是"先在 CPU 上把逻辑跑对，再逐步搬上 GPU"：

CPU 向量化跑通：先用 NumPy 把 rollout 写成向量化形式（step_batch 一次处理 \(K\) 条，§2.2 的批处理预演），\(\rho\) 减法、warm-start、权重更新全部跑对，在目标任务上验证能到达、数值合理。这一步确立"算法正确"的基准。
固定随机种子，留一份参考输出：记下这份 CPU 版在固定种子下的关键数值（代价、ESS、到达步数）。后面每搬一步上 GPU，都拿 GPU 结果和这份参考对比，数值对不上就说明这一步搬错了——这是把"逻辑 bug"和"CUDA bug"分开的关键纪律。
搬 rollout kernel：把向量化的 rollout 改写成 CUDA kernel（每线程一条、或 warp 协同，§2.2 线程映射），动力学与代价写成 device 函数。先只搬 rollout、把代价 J[K] 拷回 CPU 算权重，验证 J[K] 与 CPU 参考一致。
搬权重归约上 GPU：把求 \(\rho\)、\(\eta\) 的归约也写成 kernel（§2.2 的 shared memory 树形归约），别忘了 __syncthreads()。验证权重与参考一致。
让数据常驻设备：把噪声、控制序列 \(U\)、参数都留在 GPU 上，每周期只传 \(x_0\) 进、\(u_0\) 出（§2.2 数据流），warm-start 的左移也在设备上做。这一步通常带来最大的实测提速——因为消除了大块传输。
设备端生成噪声：用 cuRAND 在 kernel 里即用即采，彻底去掉噪声的主机↔设备拷贝（§2.2 cuRAND）。
profile 与调优：用 nsys/ncu（Nsight）profile，看访存是否合并、occupancy 高不高、有没有 warp divergence，再调 dynamics_rollout_dim_ 的 block 尺寸等。

这条路线的精髓是**增量验证**：每一步都有一个"和 CPU 参考对数值"的检查点，任何一步搬错都能立刻定位，而不是写完整个 GPU 版再面对一个不知错在哪的黑箱。本章累积项目的"向量化 GPU 雏形"模块正对应第 1-2 步，把它打牢，后面的 3-7 步就是按部就班的工程搬运。

occupancy 与 block 尺寸：移植路线第 7 步的调优细节¶

移植路线最后一步"profile 与调优"里，最常调的就是 dynamics_rollout_dim_ 的第一维——也就是"一个 block 放多少条 rollout"。它直接影响 occupancy（占用率），这是 GPU 调优的核心指标，值得单独讲清。

为什么 occupancy 重要。 GPU 的每个 SM（流多处理器）能同时驻留若干个 warp，occupancy 就是"实际驻留的 warp 数 / 该 SM 能驻留的最大 warp 数"。它为什么影响性能？因为 GPU 靠**切换 warp 来掩盖延迟**：当一个 warp 卡在等待全局内存（几百个时钟周期）时，调度器立刻切到另一个就绪的 warp 继续算，让计算单元不空转。

驻留的 warp 越多（occupancy 越高），可切换的"备胎"越多，越能把访存延迟藏在计算背后。MPPI 的 rollout 既有计算（动力学、代价）又有访存（读控制、写代价），occupancy 不足时访存延迟暴露出来，kernel 就慢。

什么限制 occupancy。 三个资源共同卡住能驻留多少 block/warp：每线程用的**寄存器数**、每 block 用的**共享内存**、以及 **block 尺寸**本身。dim3(64, STATE_DIM, 1) 意味着每 block 有 \(64\times\text{STATE\_DIM}\) 个线程；这个数太小，SM 驻留不满（occupancy 低、备胎少）；太大，则每个线程分到的寄存器变少（或一个 block 占用太多寄存器/共享内存，导致 SM 只能放下很少几个 block），occupancy 反而下降。所以存在一个甜区。

实践要点。 其一，block 尺寸应是 warp 大小（32）的整数倍——否则最后会凑出一个不满 32 线程的"残 warp"，其中的空线程白占调度资源。其二，别迷信"线程越多越快"：增大 block 尺寸若把寄存器压力顶上去、反而压低 occupancy，得不偿失（这也是 §2.2 编程/概念误区的延伸——GPU 性能不是线性堆线程堆出来的）。

其三，用工具定位而非猜：Nsight Compute（ncu）会直接报出 kernel 的 occupancy、以及"是寄存器、共享内存还是 block 尺寸在卡 occupancy"，照着它的提示调 dynamics_rollout_dim_ 的第一维，比盲试高效得多。MPPI-Generic 把这个维度暴露成可配置参数，正是为了让你按自己的 GPU 和状态维度找到这个甜区。

小结：GPU 优化要点一览¶

§2.2 讲了不少 GPU 工程点，把它们收成一张"让 MPPI 在 GPU 上跑快"的检查表，方便回顾与对照实现：

要点	做什么	不做会怎样	关联小节
线程映射	\(K\) 条 rollout 映射到线程/warp/block，状态各维可协同	映射不当 → 并行度低或 warp divergence	线程映射
内存层级	按访问频率分配 register/shared/global	高频量放 global → 访问慢	内存层级
访存合并 + SoA	让同 warp 线程读连续地址（数据按 SoA 排）	跨步访问 → 带宽暴跌数倍	设备端细节
设备端噪声	cuRAND 在 kernel 内即用即采	CPU 生成再拷 → 每周期大块传输	设备端细节
数据常驻设备	大块数据留 GPU，每周期只传 \(x_0\)/\(u_0\)	频繁主机↔设备拷贝 → 吃掉并行收益	数据流
\(\rho\) 减法（fp32）	权重前减 \(\min S\) 防下溢	fp32 下代价过百即整批下溢 → NaN	fp32 精度
occupancy	block 尺寸取 32 倍数、平衡寄存器压力	occupancy 低 → 访存延迟暴露	occupancy
归约同步	shared memory 树形归约配 `__syncthreads()`	漏同步 → 竞态、结果随机错	权重归约

这张表也是"GPU 版没达到预期加速"时的排查顺序：先确认数据是否常驻、噪声是否设备端生成（传输问题最常见），再看访存是否合并、occupancy 是否够（带宽与并行度），最后查归约同步与数值稳定。把这些逐项做对，才能拿到 §2.2 基准里那约 50 倍的加速。

⚠️ 常见陷阱¶

编程陷阱：shared memory 归约缺少 __syncthreads()，产生竞态。 - 错误描述：在 warp/block 协同做代价归约（把各线程的部分和累加到共享内存）时，读写共享内存之间漏掉了 __syncthreads() 同步屏障。 - 现象/后果：归约结果时对时错、随运行随机变化——有的线程还没写完，别的线程就来读了，读到旧值。这类竞态 bug 极难复现和定位，因为它依赖线程调度的随机时序。 - 根本原因：block 内的线程并非严格同步执行，不同 warp 的进度可能不同；对共享内存的"写后读"必须用 __syncthreads() 显式同步，否则行为未定义。 - 正确做法：每一轮归约的写阶段和读阶段之间插入 __syncthreads()，确保所有线程都写完再开始下一步读取。 - 自检方法：用 cuda-memcheck --tool racecheck（或新版 compute-sanitizer --tool racecheck）扫描，能直接报出共享内存竞态的位置；另可固定随机种子多跑几次，结果若不一致即强烈提示竞态。

概念误区：以为"用了 GPU 就一定比 CPU 快"。 - 错误描述：认为把任何 MPPI 实现挪到 GPU 上都会自动获得几十倍加速。 - 现象/后果：在 \(K\) 很小、或动力学极轻量、或每周期频繁传数据的场景套 GPU，结果比 CPU 还慢，然后困惑"GPU 不是更快吗"。 - 根本原因：GPU 的优势是大规模并行吞吐，前提是负载足够大（\(K\) 大、每条计算重）且控制流齐整；负载不匹配时，kernel 启动和数据传输的固定开销会盖过并行收益。 - 正确做法：GPU 化前先估负载——\(K\) 是否上千、每条 rollout 计算是否非平凡、控制流是否齐整。满足才上 GPU；\(K\) 很小的轻量任务，优化好的 CPU/SIMD 版本可能更优。

思维陷阱：试图把时步循环 for t 也并行掉。 - 错误描述：看到"GPU 擅长并行"，就想把一条 rollout 的 \(T\) 个时步也分给多个线程并行算，以为能再快 \(T\) 倍。 - 现象/后果：要么实现不出来（后一步依赖前一步的状态），要么强行并行得到错误结果（用了未算好的中间状态）。 - 根本原因：rollout 的时步之间有**本质的串行依赖**——\(x_{t+1}=F(x_t,v_t)\) 必须等 \(x_t\) 算完。这是数据依赖决定的，不是实现不够聪明。MPPI 的并行性只存在于"条与条之间"，不存在于"一条内部的时步之间"。 - 正确做法：并行只在 \(K\) 这一维（条间）和"单个时步内部的状态各维/归约"上做；时步维老老实实串行。想缩短串行链，只能减小 \(T\)（缩短时域，§2.4 会讨论时域权衡）或用更高效的单步动力学。

练习¶

（实现题·GPU 归约） 续写 §2.2 的 rollout kernel：在它算出 J[K] 之后，写一个并行归约 kernel 求 \(\rho=\min_k J_k\)，再写一个 kernel 计算权重 \(w_k=e^{-(J_k-\rho)/\lambda}\) 并归约出 \(\eta=\sum_k w_k\)。

给出每一步用 shared memory 做树形归约的结构，并标注哪里需要 __syncthreads()。（无 GPU 也可在 CPU 上用 NumPy 模拟归约逻辑，验证数值与 §2.1 一致。） 2. （调试挑战） 给你一个 block 内代价归约 kernel，它偶尔输出错误的总和、且每次运行结果不同。已知归约循环里只在末尾有一个 __syncthreads()。定位竞态、解释为什么"偶尔"出错而非每次出错、给出修复（提示：写后读屏障的位置）。 3. （思考题·选型） 你的任务是 \(K=64\)、动力学是一行线性递推、\(T=10\) 的简单系统，每周期还要把状态从 CPU 传到 GPU、再把代价传回。推演：GPU 版相对优化良好的 CPU 版，可能更快还是更慢？把你的推理对照 §2.2 "GPU 在哪失效"的表格，指出主导开销是哪一项。

至此，§2.1 把 Ch1 的理论核补成了完整可部署的算法，§2.2 把它的 rollout 并行结构、内存层级、归约与数据流讲清，让它在 GPU 上以毫秒级实时运行。但到目前为止，我们的实现还是"为某一个具体系统手写的一段代码"——换一个机器人、换一套动力学或代价，几乎要重写。如何用工程抽象把动力学、代价、采样器、控制器解耦，使得换系统只需换一个模板参数？这正是下一节 MPPI-Generic 的 C++ 模板化设计要解决的问题。

§2.3 MPPI-Generic 的 C++ 模板化设计 ⭐⭐⭐¶

动机：为什么需要一套抽象，而不是手写一遍又一遍¶

到 §2.2 为止，我们已经能写出一个跑得快的 GPU MPPI——但它是"为某一个具体系统手写的一段代码"。rollout kernel 里硬编码了某个动力学 forward_dynamics、某个代价 running_cost、用的是高斯噪声采样。现在假设你要把这套 MPPI 用到三个不同的机器人上：一台差速小车、一架四旋翼、一只四足。

它们的动力学维度不同、代价函数不同；你可能还想对每个都试两种采样分布（高斯 vs 有色噪声）、两种控制器变体（普通 MPPI vs 鲁棒版）。如果每换一个组合就复制粘贴整个 kernel、改几行、再调一遍，很快你就会有一堆几乎雷同、却各自微妙不同的代码副本——改一个 bug 要在十几处同步修改，加一个新动力学要重写整条主循环。这正是工程上最怕的**组合爆炸**：\(3\) 种动力学 \(\times\) \(2\) 种代价 \(\times\) \(2\) 种采样器 \(\times\) \(2\) 种控制器 \(=24\) 份近乎重复的代码。

MPPI-Generic（Vlahov 等 2024，arXiv:2409.07563，Georgia Tech ACDS Lab 的官方 C++/CUDA 库）要解决的就是这个问题。它的核心设计理念是**四维正交解耦**：把 MPPI 系统拆成四个互相独立、可自由组合的维度——动力学（Dynamics）、代价（Cost）、采样分布（Sampling Distribution / Sampler）、反馈控制器（Feedback Controller）。MPPI 的主循环代码只写一遍，四个维度各自实现成可插拔的组件；换系统、换采样、换控制器，只需替换对应的组件，主循环一字不改。\(24\) 份重复代码塌缩成"\(1\) 份主循环 + \(3+2+2+2\) 个组件"。

如果不这样做会怎样¶

不做这层抽象，用复制粘贴的方式管理多个 MPPI，会陷入几个具体的困境：

做法	后果
每个系统复制一份完整 kernel	组合爆炸：\(N\) 个系统 \(\times M\) 种配置 = \(N\times M\) 份近重复代码
改主循环 bug	要在所有副本里同步修改，漏一处就埋一个不一致的 bug
加新动力学	重写整条主循环 + 归约 + 数据流，而非只写一个动力学组件
对比两种采样器	难以公平对比——因为它们嵌在各自的副本里，其他部分可能已经漂移

抽象的价值不只是"少写代码"，更是**可复用、可对比、可维护**：组件化之后，"换一个采样分布看效果"变成换一个模板参数的事，对照实验天然公平；主循环只有一份，正确性只需保证一次。

历史：从论文代码到通用库¶

MPPI 的实现经历了从"论文附带代码"到"通用库"的演进。2016 年 AutoRally 的 path_integral.cu 是为那辆越野车手写的、与具体车型耦合的实现；之后每个用 MPPI 的项目大多各写各的。

直到 2024 年的 MPPI-Generic，Georgia Tech 把多年积累的工程经验沉淀成一个**动力学与代价无关（agnostic）**的统一库——同一套主循环支持 MPPI、Tube-MPPI、Robust MPPI 三种控制器，内置差速车、四旋翼、CartPole、双积分器、AutoRally 等多种现成动力学与多种采样分布，并开放 API 让使用者写自己的组件而无需改动 MPPI 主循环代码。这是"把研究代码工程化、让所有人都能即插即用"的典型努力。

四维正交解耦：把系统拆成可插拔的组件 ⭐⭐⭐¶

为什么是这四个维度。 MPPI 主循环（§2.1 的伪代码）里，哪些部分是"会随问题变"的？逐句看：rollout 里调用的前向动力学 \(x\leftarrow F(x,v)\) 是一个维度（换机器人就换它）；累加的运行代价与终端代价 \(\ell,\phi\) 是第二个维度（换任务就换它）；生成噪声序列 \(\varepsilon_k\) 的采样分布是第三个维度（高斯、有色噪声、平滑采样等）；而 Tube-MPPI / Robust-MPPI 这类变体需要一个把真实系统拉回名义轨迹的**反馈控制器**（如 iLQR），这是第四个维度。除此之外的部分——采样循环的组织、\(\rho\) 减法、权重归约、warm-start、GPU 调度——对所有系统都一样，属于"主循环"，只写一遍。这就是四维解耦的依据：把会变的拆成组件，把不变的留在主循环。

每个维度的"接口"约定。 正交解耦能成立，靠的是每个维度都遵守一个固定的接口，主循环只通过接口与组件交互、不关心其内部实现。大致是：Dynamics 组件要提供"给定状态和控制、算出下一状态（或状态导数）"的方法（如 computeStateDeriv / computeNextState）；Cost 组件要提供"算运行代价"和"算终端代价"的方法（computeRunningCost / computeTerminalCost）；Sampler 组件要提供"生成噪声样本"的方法（generateSamples）；Feedback Controller 组件（可选）要提供反馈律。只要一个组件满足对应接口，就能插进主循环——这正是 §2.1 伪代码里把动力学写成"黑箱 \(F\)"、代价写成"标量 \(\ell,\phi\)"在工程上的兑现：主循环本就不关心它们怎么算，只要能调用。

核心机制：编译期组合，而非运行时多态。 这是 MPPI-Generic 的关键技术选择。把组件插进主循环，有两条路：一条是**运行时多态**（C++ 虚函数/继承）——主循环持有基类指针，运行时通过虚函数表分派到具体实现；另一条是**编译期组合**（C++ 模板）——把组件作为模板参数传给主循环，编译器在编译期就把具体类型"填"进去。MPPI-Generic 走的是后者，主循环大致长成这样（签名体现四个维度）：

// 控制器在四个维度上模板化（编译期组合的设计示意）
template <class DYNAMICS, class COST, class SAMPLER,
          class FEEDBACK_CONTROLLER = NoFeedback>
class VanillaMPPI {
    // DYNAMICS:  computeStateDeriv() / computeNextState()
    // COST:      computeRunningCost() / computeTerminalCost()
    // SAMPLER:   generateSamples()    —— 高斯 / 有色噪声 / 平滑采样
    // FEEDBACK_CONTROLLER: Tube/Robust-MPPI 的 iLQR 跟踪控制器（默认关闭）
};

为什么选编译期而非运行时？因为 MPPI 的主循环要在 GPU 上对数千条 rollout 反复调用动力学和代价——如果每次调用都走虚函数表的动态分派，不仅有间接跳转的开销，更关键的是**虚函数在 CUDA device 代码里支持受限、且会阻碍编译器内联与优化**。

用模板把类型在编译期定死，编译器就能把动力学和代价的代码**直接内联**进 kernel、做跨函数优化，生成的机器码和"手写专用 kernel"几乎一样快——既拿到了抽象的灵活，又没付运行时分派的代价。这是一个典型的"零成本抽象（zero-cost abstraction）"：抽象只存在于源码层面，编译后开销归零。

下面用一个**可编译运行的最简 C++ 例子**（纯 C++17、无 CUDA，只为讲清编译期组合这个核心思想）来演示。我们在两个维度（Dynamics、Cost）上模板化一个迷你控制器，然后通过换模板参数组合出不同行为：

#include <array>
#include <cstdio>
constexpr int STATE_DIM = 2, CONTROL_DIM = 1;
using State = std::array<double, STATE_DIM>;
using Control = std::array<double, CONTROL_DIM>;

struct SingleIntegrator {                       // Dynamics 组件 A
    static State step(const State& x, const Control& u, double dt) { return {x[0]+u[0]*dt, x[1]}; }
};
struct DampedIntegrator {                       // Dynamics 组件 B（带阻尼）
    static State step(const State& x, const Control& u, double dt) {
        double v = 0.9*x[1] + u[0]*dt; return {x[0]+v*dt, v}; }
};
struct QuadraticCost {                          // Cost 组件 A
    static double eval(const State& x, const Control& u) { return x[0]*x[0] + 0.01*u[0]*u[0]; }
};
struct BarrierCost {                            // Cost 组件 B（含不连续障碍罚值）
    static double eval(const State& x, const Control& u) {
        double hit = (x[0]>1.0 && x[0]<2.0) ? 100.0 : 0.0; return x[0]*x[0] + hit + 0.01*u[0]*u[0]; }
};

template <class DYNAMICS, class COST>           // 主循环：在两个维度上模板化
struct MPPIController {
    static double rollout_cost(State x, const Control& u, int T, double dt) {
        double J = 0.0;
        for (int t = 0; t < T; ++t) { x = DYNAMICS::step(x,u,dt); J += COST::eval(x,u); }  // 编译期绑定
        return J;
    }
};

int main() {
    State x0 = {3.0, 0.0}; Control u = {-1.0}; int T = 10; double dt = 0.1;
    // 换组件 = 换模板参数，主循环代码一字不改：
    printf("SingleIntegrator + Quadratic : J = %.4f\n",
           MPPIController<SingleIntegrator, QuadraticCost>::rollout_cost(x0,u,T,dt));
    printf("DampedIntegrator  + Quadratic : J = %.4f\n",
           MPPIController<DampedIntegrator,  QuadraticCost>::rollout_cost(x0,u,T,dt));
    printf("SingleIntegrator + Barrier   : J = %.4f\n",
           MPPIController<SingleIntegrator, BarrierCost   >::rollout_cost(x0,u,T,dt));
}

用 g++ -O2 -std=c++17 编译运行，输出：

SingleIntegrator + Quadratic : J = 60.9500
DampedIntegrator  + Quadratic : J = 79.9357
SingleIntegrator + Barrier   : J = 160.9500

三次调用，控制器主循环 MPPIController::rollout_cost 的代码完全相同，只是模板参数不同——编译器为每个组合生成了一份专用的、内联好的代码。这就是 MPPI-Generic 四维解耦的微缩版：真实库在这个思想上再加两个维度（Sampler、Feedback Controller）、把 step/eval 放进 CUDA device 函数、并处理 GPU 内存与归约，但"用模板在编译期把组件填进统一主循环"的骨架，与这二十几行一模一样。

本质洞察：MPPI-Generic 的模板化不是"炫技式"的 C++ 模板，而是**为 GPU 性能服务的设计选择**。运行时多态（虚函数）在 CPU 上只是小开销，但在要被数千条 rollout 反复调用、且运行在 CUDA device 上的 MPPI 主循环里，它既难以内联、又在 device 代码里支持受限。模板把类型在编译期定死，让编译器把动力学和代价直接内联进 kernel——抽象的灵活与手写 kernel 的速度二者兼得。这解释了为什么一个"控制算法库"会把 C++ 模板用到这种程度：它是被"既要灵活、又要在 GPU 上极致快"这对约束逼出来的。

把这四个维度收成一张表，方便对照"要换什么、就动哪一维"：

维度	抽象掉什么	换它意味着	典型可换项
Dynamics	系统怎么演化（\(x_{t+1}=f(x_t,u_t)\)）	换机器人/换物理模型	单积分、双积分、单摆、四旋翼、神经网络动力学
Cost	任务目标怎么打分（\(\ell(x,u)\)、\(\phi(x)\)）	换任务/换目标	二次型、避障罚值、CNN cost-map、摆起代价
Sampling Distribution	噪声怎么撒（提议分布）	改采样策略（提样本效率）	高斯、有色噪声、平滑采样、学到的先验
Feedback Controller	执行时如何抗扰跟踪	加/换反馈层	无反馈、PID、iLQR/DDP 跟踪

读这张表的关键是"正交"二字：四列彼此独立——换 Dynamics 不影响 Cost 怎么写，换 Sampler 不影响主循环怎么跑。正因正交，组合数是乘法（4 个动力学 \(\times\) 5 个代价 \(\times\) 3 个采样器……），而你只需各写一份、主循环写一遍。

这正是 §2.3 开头"\(24\) 份重复代码"困境的解法：把会独立变化的维度拆开，用编译期组合代替复制粘贴。后面三列还分别呼应本章/后续的主线——Sampling Distribution 是 §2.1 样本效率主线和第 3 章变体的主战场，Feedback Controller 对应 §2.4 的混合方案（Tube/RMPPI），Cost/Dynamics 则随各应用而变。

Eigen::Map 与 CUDA 的零拷贝互操作¶

四维解耦解决了"组件如何组合"，还剩一个具体的工程接缝：MPPI 的主机端（C++/Eigen）和设备端（CUDA kernel）如何**共享同一块内存而不来回拷贝**。这正是 §2.2 "让大块数据常驻设备、每周期只传 \(x_0\)" 在代码层面的落地手法，关键工具是 Eigen::Map。

为什么需要它。 Eigen 的矩阵类型（如 Matrix3d、VectorXd）自己管理内存。但在 MPPI 里，状态、控制、噪声这些数据常常已经躺在一块由 CUDA 分配的缓冲区里（裸指针 float*），或反过来需要被 kernel 直接读写。

如果每次都把 Eigen 矩阵的内容拷进 CUDA 缓冲、算完再拷回，就回到了 §2.2 批判的"频繁主机↔设备传输"。我们想要的是：让 Eigen 和 CUDA 指向同一块内存，主机端用 Eigen 的友好语法读写它、设备端的 kernel 也读写它，中间不发生拷贝。

核心机制：Map 把裸指针"包装"成 Eigen 视图。 Eigen::Map 不分配内存，它接管一个已存在的裸指针，把它**当作** Eigen 矩阵来用——所有对这个 Map 的读写，直接作用在原始内存上。这就是"零拷贝"：

// Eigen::Map：把一块已存在的裸内存当作 Eigen 矩阵，不拷贝（示意）
float* buf = /* 一块 12 维状态的缓冲区（可来自 CUDA 统一内存）*/;
Eigen::Map<Eigen::Matrix<float, 12, 1>> x(buf);   // x 是 buf 的“视图”，不另分配
x(0) += 1.0f;        // 直接改写 buf[0]，无拷贝
// 同一个 buf 指针可传给 CUDA kernel，设备端与主机端共享这块内存

配合 CUDA 的统一内存（cudaMallocManaged，由驱动自动在主机↔设备间迁移）或固定内存（pinned memory），主机端的 Eigen 代码和设备端的 kernel 就能围着同一块 buf 工作：CPU 端用 Eigen::Map 以矩阵语法准备初始状态，kernel 直接在这块内存上 rollout，算完 CPU 端再用 Map 读结果——全程零拷贝。（这段互操作涉及 CUDA，未在本章环境编译；Eigen::Map 本身是纯 CPU 特性、可独立编译验证其零拷贝别名行为。）

易错点。 Eigen::Map 最大的坑是**生命周期**：Map 只是个视图，它不拥有内存；如果底层 buf 被释放（或 CUDA 缓冲被 cudaFree），Map 就变成悬空引用，读写它是未定义行为。其二是**对齐与行主序/列主序**：Eigen 默认列主序，若裸数据是行主序排列，要用 Eigen::Map<..., Eigen::RowMajor> 显式指定，否则读出来是转置的乱序。

目录结构与三种控制器¶

把上面的设计落到代码组织上，MPPI-Generic 的目录大致按"四个维度 + 主循环 + 工具"划分（以官方仓库 ACDSLab/MPPI-Generic 为准）：

MPPI-Generic/
└── include/mppi/
    ├── controllers/          # 主循环：MPPI / Tube-MPPI / RMPPI
    ├── core/                 # 核心 CUDA kernel（rollout、归约的公共实现）
    ├── cost_functions/       # 代价组件（含 Dynamics 无关的二次代价）
    ├── dynamics/             # 动力学组件（四旋翼/双积分器/CartPole/差速车/AutoRally…）
    ├── sampling_distributions/  # 采样器（高斯/有色噪声/NLN/平滑采样）
    ├── feedback_controllers/ # 反馈控制器（如 DDP/iLQR，供 Tube/Robust 用）
    └── utils/                # Eigen–CUDA 互操作等工具

库里实现了三种控制器，它们共享主循环、只在"是否引入反馈、反馈如何作用"上不同：

控制器	一句话	关键差别
MPPI（Vanilla）	基础版，§2.1 的算法	无反馈控制器，纯采样 + 加权
Tube-MPPI	加一个 iLQR 跟踪控制器，把真实系统拉回名义轨迹	MPPI 不感知跟踪器，可能与之"对抗"
Robust-MPPI（RMPPI）	把跟踪反馈纳入 MPPI 的采样之中	修正了 Tube-MPPI 中 MPPI 与跟踪器互斗的问题

这三者的演进逻辑很清楚：Vanilla MPPI 对状态扰动的鲁棒性有限；Tube-MPPI 引入一个 iLQR 反馈把系统拉回名义轨迹来增强鲁棒性，但 MPPI 主循环并不知道这个反馈的存在，二者可能互相较劲；RMPPI 进一步把反馈的影响放进 MPPI 采样里，让 MPPI"知道"跟踪器在做什么，从而协同而非对抗。

在四维模板的视角下，这三者的差别**完全落在 FEEDBACK_CONTROLLER 这一维**——换一个模板参数（NoFeedback / iLQR-tube / iLQR-robust）即可切换，主循环不变。这正是四维解耦的威力：三种听起来很不同的控制器，在代码里只是同一主循环的三种模板实例。

Plant：与 ROS 对接的 MPC 封装。 库里还有一个 Plant 概念，它是套在控制器外面的 MPC 封装层——真实机器人上的 ROS 订阅（接收状态）、发布（发出控制）等接口都在 Plant 里实现。控制器只管"给定状态算控制"，Plant 管"从哪拿状态、把控制发到哪、以什么频率跑"。这把"算法"与"系统集成"也解耦了：换一个机器人平台改 Plant，控制算法本身不动。

为什么是 header-only：模板与编译模型的取舍¶

注意 MPPI-Generic 一个容易被忽略却很关键的工程选择：它是 header-only（纯头文件）库——所有实现都在 .cuh/.hpp 头文件里，没有需要单独编译链接的 .cu/.cpp 源文件。这不是偶然，而是被它的模板化设计**逼出来**的。

为什么模板库几乎必须 header-only。 C++ 的模板有个硬性约束：编译器在实例化一个模板（比如 VanillaMPPIController<CartpoleDynamics, ...>）时，必须能**看到模板的完整定义**，而不只是声明。

普通（非模板）函数可以"声明放头文件、定义放源文件、分开编译再链接"，因为链接器能在别处找到定义；但模板不行——VanillaMPPIController<DynA,...> 和 VanillaMPPIController<DynB,...> 是编译器现场生成的两份不同代码，定义必须在使用处可见，编译器才能为你这次用到的具体类型组合生成代码。

如果把模板定义藏在源文件里，使用者的编译单元看不到，链接时就会报"未定义的引用"。所以模板库的标准做法就是**把定义全放进头文件**——header-only 是模板"编译期组合"（§2.3）的自然结果。

这个选择牺牲了什么、换来了什么。 header-only 的好处对使用者极友好：不需要预编译这个库、不需要配置链接，#include 进来就能用，跨平台也省心（没有预编译二进制的 ABI 兼容问题）。代价主要落在**编译时间**上：每个用到这些模板的编译单元都要重新实例化、重新编译一遍模板代码，模板嵌套越深、用到的类型组合越多，编译越慢——这也是 §2.3 思维陷阱"模板维度别过多"的另一面，维度爆炸不仅让错误信息变长，也让编译时间膨胀。对 MPPI-Generic 这种"模板是核心机制、且要让研究者方便地即插即用"的库，header-only 的便利显然压过编译时间的代价，所以是对的选择。

本质洞察：header-only 不是"图省事不拆源文件"，而是模板化设计在**编译模型**层面的必然延伸。§2.3 反复讲的"编译期组合、零成本抽象"，其前提就是编译器在实例化处能看到完整定义——这直接决定了实现必须在头文件里。所以当你看到一个库是 header-only，往往能反推出"它重度依赖模板"；反过来，一个重度模板化的库若强行拆出源文件，使用者多半会被链接错误劝退。编译期组合（性能）、header-only（分发）、编译时间（代价）这三者，是同一个设计选择的三个面。

最小可用示例：实例化与调用¶

前面的 VanillaMPPI<DYNAMICS,COST,SAMPLER,FEEDBACK> 是为讲清四维而画的简化示意。真实库的接口更具体，看一遍官方的最小示例，能把抽象落到能跑的代码。先厘清：MPPI-Generic 实际由**六类**组成——Dynamics、Cost、Sampling Distribution、Feedback Controller 是四个可插拔维度，Controller（主循环）和 Plant（MPC/ROS 封装）是两层编排。下面是它"单次求解"的最小用法（基于官方仓库，CartPole swing-up）：

#include <mppi/controllers/MPPI/mppi_controller.cuh>
#include <mppi/cost_functions/cartpole/cartpole_quadratic_cost.cuh>
#include <mppi/dynamics/cartpole/cartpole_dynamics.cuh>
#include <mppi/feedback_controllers/DDP/ddp.cuh>

const int NUM_TIMESTEPS = 100;            // T：编译期常量
const int NUM_ROLLOUTS  = 2048;           // K：编译期常量
using DYN_T  = CartpoleDynamics;
using COST_T = CartpoleQuadraticCost;
using FB_T   = DDPFeedback<DYN_T, NUM_TIMESTEPS>;                       // 反馈：默认关闭但必须提供
using SAMPLING_T   = mppi::sampling_distributions::GaussianDistribution<DYN_T::DYN_PARAMS_T>;
using CONTROLLER_T = VanillaMPPIController<DYN_T, COST_T, FB_T, NUM_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T>;

int main() {
    float dt = 0.02;
    auto dynamics = std::make_shared<DYN_T>();
    auto cost     = std::make_shared<COST_T>();
    auto fb       = std::make_shared<FB_T>(dynamics.get(), dt);
    SAMPLING_T::SAMPLING_PARAMS_T sp;
    std::fill(sp.std_dev, sp.std_dev + DYN_T::CONTROL_DIM, 1.0);        // 各控制维标准差=1
    auto sampler = std::make_shared<SAMPLING_T>(sp);

    CONTROLLER_T::TEMPLATED_PARAMS cp;
    cp.dt_ = dt;  cp.lambda_ = 1.0;                                     // Δt 与温度 λ
    cp.dynamics_rollout_dim_ = dim3(64, DYN_T::STATE_DIM, 1);          // ★ 线程映射！
    cp.cost_rollout_dim_     = dim3(NUM_TIMESTEPS, 1, 1);
    auto controller = std::make_shared<CONTROLLER_T>(
        dynamics.get(), cost.get(), fb.get(), sampler.get(), cp);

    DYN_T::state_array x = dynamics->getZeroState();                    // 初始状态
    controller->computeControl(x, 1);                                  // 跑一次 MPPI 优化
    auto U = controller->getControlSeq();                              // 取最优控制序列(Eigen::Matrix)
    std::cout << U << std::endl;
}

这段代码把前面几节的概念逐一对上了号，值得逐点读：

真实模板签名里，\(T\) 和 \(K\) 也是编译期参数。 注意 VanillaMPPIController<DYN_T, COST_T, FB_T, NUM_TIMESTEPS, NUM_ROLLOUTS, SAMPLING_T>——时域 \(T\)（NUM_TIMESTEPS）和采样数 \(K\)（NUM_ROLLOUTS）不是运行时变量，而是**编译期常量模板参数**。

为什么？因为编译期就知道 \(T,K\)，编译器才能为 kernel 静态分配寄存器/共享内存、展开循环、确定 grid/block 尺寸——这是 §2.3 "编译期组合 = 零成本抽象"在维度数字上的延伸。代价是改 \(T\) 或 \(K\) 要重新编译，但 MPPI 部署时这两个值通常固定，划算。（前面的简化示意只列了四个类型维度、省略了 \(T,K\)；真实签名把它们也纳入模板，类名是 VanillaMPPIController。）

dynamics_rollout_dim_ = dim3(64, STATE_DIM, 1) 正是 §2.2 的线程映射。 这个 dim3 配置 block 的线程组织：第一维 \(64\) 是"一个 block 同时处理 64 条 rollout"，第二维 STATE_DIM 是"用 STATE_DIM 个线程协同处理一条 rollout 的状态各维"。这恰好印证了 §2.2 讲的"每 block 多条 rollout、状态各维分摊给协同线程"的映射——抽象的设计在这里变成一个具体的 dim3。换 GPU 或调性能时，调的就是这个数字。

computeControl(x, 1) 与 getControlSeq() 是核心两步。 computeControl(x, num_iter) 从状态 x 出发跑 num_iter 次 MPPI 优化（这里 1 次，配合外层重规划即 §2.1 的接收水平）；getControlSeq() 返回优化好的整条控制序列，类型是 Eigen::Matrix——又一次 Eigen–CUDA 的接缝（§2.3 的 Eigen::Map）：kernel 在设备上算，结果以 Eigen 矩阵交回主机。

MPC 用法靠 Plant 封装。 上面是"单次求解"。真实机器人上用的是 MPC 模式：写一个继承 BasePlant<CONTROLLER_T> 的 Plant（构造时传 (controller, hz, optimization_stride)），在其中实现 pubControl（把控制发给系统/ROS）、checkStatus 等接口；Plant 以指定频率 hz 驱动控制器、管理状态时间戳。

Plant 还能取出 MPPIFreeEnergyStatistics——把 Ch1 讲的自由能作为运行时诊断量暴露出来（自由能落在 \([\min S,\ \text{平均代价}]\) 之外就提示有 bug，正是 Ch1 §1.3 那条健全性检查的工程化）。这就是"算法"与"系统集成"解耦的落地：控制器只管"给状态算控制"，Plant 管"何时、从哪拿状态、把控制发到哪"。

动手：写一个自己的 Dynamics 组件¶

四维解耦的真正价值，在你要控制一个**库里没有**的系统时才完全显现——这时你只需写一个满足 Dynamics 接口的组件，插进同一个主循环，其余一切（采样、归约、warm-start、GPU 调度）原封不动复用。真实库的 Dynamics 接口比较完整（要实现 computeStateDeriv()/computeNextState()、定义参数结构 DYN_PARAMS_T、声明 STATE_DIM/CONTROL_DIM 等），但其**本质**和 §2.3 那个可编译的 C++ 模板 demo 完全一样：写一个提供"给定状态和控制、算出下一状态"的类型，作为模板参数传进控制器。我们把那个 demo 扩展一下，给它加一个自定义的单摆动力学组件：

// 在 §2.3 的模板 demo 基础上，新增一个自定义 Dynamics 组件（纯 C++17，可编译）
struct PendulumDynamics {                     // state=[θ, ω]，control=力矩
    static constexpr double g = 9.81, l = 1.0, m = 1.0, b = 0.1;
    static State step(const State& x, const Control& u, double dt) {   // 满足主循环要求的接口
        double th = x[0], om = x[1];
        double dom = (u[0] - b*om - m*g*l*std::sin(th)) / (m*l*l);     // 单摆动力学
        om += dom*dt;  th += om*dt;
        return {th, om};
    }
};

int main() {
    // 现成组件：单积分器
    printf("SingleIntegrator + Quadratic : J = %.4f\n",
           MPPIController<SingleIntegrator, QuadraticCost>::rollout_cost({3.0,0.0}, {-1.0}, 10, 0.1));
    // ★ 把自定义的 PendulumDynamics 插进同一个主循环——主循环代码一字不改
    printf("PendulumDynamics + Quadratic : J = %.4f (自定义组件即插即用)\n",
           MPPIController<PendulumDynamics, QuadraticCost>::rollout_cost({3.14,0.0}, {0.5}, 20, 0.05));
}

g++ -O2 -std=c++17 编译运行，输出：

SingleIntegrator + Quadratic : J = 60.9500
PendulumDynamics + Quadratic : J = 217.4893 (自定义组件即插即用)

MPPIController 的主循环代码一行没改，仅仅把模板参数从 SingleIntegrator 换成新写的 PendulumDynamics，就让控制器在一个全新的系统上算出了 rollout 代价。

这就是"为新机器人写 MPPI"在真实库里的样子——你写的只是那个 step（在真实库里是 computeStateDeriv/computeNextState + 参数结构），主循环、采样、归约、GPU kernel 全部复用。回头看 §2.3 开头那个"\(24\) 份重复代码"的组合爆炸困境：有了这套接口，加一个新动力学就是加一个 struct，而不是复制一份 kernel。这正是工程抽象兑现价值的时刻。

动手：写一个自己的 Cost 组件¶

代价维度同理。换一个任务（同一个机器人、不同目标），要改的只是 Cost 组件——主循环、动力学、采样器全都不动。延续上面的单摆，给它写一个"摆起"代价：惩罚偏离竖直向上（\(\theta=0\)），让 MPPI 把摆从下方甩到上方：

// ★ 自定义 Cost 组件：单摆“摆起”代价（纯 C++17，可编译）
struct SwingUpCost {                          // 满足接口：静态 eval(x, u) → 标量
    static double eval(const State& x, const Control& u) {
        double upright = 1.0 - std::cos(x[0]); // θ=0(向上) 时为 0，θ=π(向下) 时为 2
        return 5.0*upright + 0.1*x[1]*x[1] + 0.01*u[0]*u[0];
    }
};

int main() {
    State x0 = {3.14, 0.0}; Control u = {0.5}; int T = 20; double dt = 0.05;
    // 自定义 Dynamics × 现成 Cost
    printf("Pendulum + Quadratic : J = %.4f\n",
           MPPIController<PendulumDynamics, QuadraticCost>::rollout_cost(x0,u,T,dt));
    // ★ 自定义 Dynamics × 自定义 Cost——四维里换了两维，主循环依旧不变
    printf("Pendulum + SwingUp   : J = %.4f (自定义代价即插即用)\n",
           MPPIController<PendulumDynamics, SwingUpCost>::rollout_cost(x0,u,T,dt));
}

g++ -O2 -std=c++17 编译运行，输出：

Pendulum + Quadratic : J = 217.4893
Pendulum + SwingUp   : J = 198.7062

至此你完整地体会了四维解耦的意义：从最初的 SingleIntegrator + QuadraticCost，到换动力学（PendulumDynamics），再到换代价（SwingUpCost），MPPIController 这个主循环**从头到尾一个字符都没改**——每一次都只是把一个模板参数换成一个新写的 struct。

在真实的 MPPI-Generic 里，这两个 struct 会更完整（Dynamics 实现 computeStateDeriv + 参数结构、Cost 实现 computeRunningCost/computeTerminalCost），且都是 CUDA device 代码，但"换组件 = 换模板参数、主循环复用"的骨架与这里一模一样。这正是为什么用 MPPI-Generic 适配一个新机器人/新任务，工作量是"写一两个 struct"而非"重写一套控制器"。

编译期接口检查：把"模板报错一长串"变成一句人话¶

模板组合有个公认的痛点（也是 §2.3 的编程陷阱）：如果你写的组件**漏了**某个必需方法，编译器不会说"你的 Dynamics 缺 computeStateDeriv"，而是吐出一长串从主循环深处展开的、几百行的模板实例化错误，让人摸不着头脑。成熟的模板库会主动**在编译期检查接口**，把错误信息从"天书"变成"人话"。

最朴素的手段是 static_assert 配合**类型萃取（type traits）**，在主循环里断言"组件必须有某方法"。下面用 C++17 的 detection idiom 实现一个 has_step 萃取，并在控制器里断言（这段经实编译验证）：

#include <type_traits>
using State = std::array<double,2>; using Control = std::array<double,1>;
// detection idiom：检测 T 是否有可调用的 static step(State, Control, double)
template <class, class = void> struct has_step : std::false_type {};
template <class T>
struct has_step<T, std::void_t<decltype(T::step(std::declval<State>(),
                                               std::declval<Control>(), 0.0))>>
    : std::true_type {};

struct GoodDyn { static State step(const State& x,const Control& u,double dt){ return {x[0]+u[0]*dt,x[1]}; } };
struct BadDyn  { static int foo(){ return 0; } };          // 忘了写 step

template <class DYN>
struct Controller {
    static_assert(has_step<DYN>::value,                    // ★ 编译期断言接口
                  "Dynamics 组件必须提供 static step(State, Control, dt) 方法");
    static State run(State x, Control u, double dt){ return DYN::step(x,u,dt); }
};

编译运行，检测结果如预期（Controller<GoodDyn> 正常）：

has_step<GoodDyn> = 1
has_step<BadDyn>  = 0
Controller<GoodDyn>::run -> x0=1.05

而一旦把控制器实例化成 Controller<BadDyn>（BadDyn 漏写了 step），编译器直接在 static_assert 那行报出一句中文：

error: static assertion failed: Dynamics 组件必须提供 static step(State, Control, dt) 方法

一句话指到病灶，而不是让你在几百行模板展开里考古。更现代的是 C++20 的 concept，把同样的接口要求写成一个可命名、可复用的约束：

// 用 concept 声明 Dynamics 组件必须满足的接口（C++20，示意）
template <class D>
concept DynamicsConcept = requires(typename D::State x, typename D::Control u, double dt) {
    { D::step(x, u, dt) } -> std::same_as<typename D::State>;   // 必须有 step(x,u,dt)→State
};
template <DynamicsConcept DYNAMICS, class COST>                  // 约束模板参数
struct MPPIController { /* ... */ };

这样一来，如果某个组件不满足 DynamicsConcept（比如 step 签名不对、或干脆没有），编译器会直接报"该类型不满足 DynamicsConcept，因为缺少/不匹配 step"——同样一句话指到病灶。C++17 的 detection idiom 写起来啰嗦些但可用，C++20 的 concept 更简洁可读；二者达到的效果一致：把接口检查提前到模板边界。

这是 §2.3 编程陷阱的正面解药。 那个陷阱说"换了组件后编译报错一长串模板错误"，根因是组件没满足接口、而错误在深处才暴露。接口检查（static_assert/concept）把检查**提前到模板边界**，错误信息就落在"你的组件不符合接口"这一层，可读性天差地别。

代价是库作者要多写这些约束，但对使用者是极大的体验提升——这也是"零成本抽象"在工程上要补的一课：抽象本身零运行时成本，但要让它**好用**（错误可读），得在编译期接口检查上下功夫。对你自己写组件而言，遇到长串模板错误时，先回到接口约定逐项核对方法名与签名（§2.3 陷阱的自检思路），就是在手动做编译器本该替你做的那件事。

⚠️ 常见陷阱¶

编程陷阱：Eigen::Map 的底层内存被释放，造成悬空视图。 - 错误描述：用 Eigen::Map 包装一块 CUDA 缓冲或临时数组，之后这块内存被 cudaFree/析构释放，却仍通过 Map 读写它。 - 现象/后果：读到垃圾值或随机崩溃，且因为 Map 看起来像个正常 Eigen 矩阵，bug 极难定位——错误现场往往离释放点很远。 - 根本原因：Eigen::Map 是**视图**，不拥有内存；它的有效性完全依赖底层指针存活。释放了底层内存，Map 就成了悬空引用。 - 正确做法：确保底层缓冲的生命周期**长于**所有引用它的 Map；用 RAII（如把缓冲封进一个管理类）统一管理 CUDA 内存的分配与释放，Map 只在缓冲存活期间使用。

概念误区：以为模板化只是"代码风格"，与性能无关。 - 错误描述：认为 MPPI-Generic 用模板而非虚函数继承，只是作者的编码偏好，换成继承也一样。 - 现象/后果：自己实现时图省事用虚函数做组件分派，在 GPU 上跑出来比手写 kernel 慢一截，且 device 代码里虚函数还编译报错。 - 根本原因：在被数千条 rollout 反复调用、且运行在 CUDA device 上的主循环里，虚函数的动态分派难以内联、device 支持受限；模板的编译期组合则让编译器把组件内联进 kernel，达到零成本抽象。 - 正确做法：性能敏感、运行在 device 上的组件分派用编译期模板；只有在主机端、低频、确实需要运行时切换类型的地方，才考虑虚函数。

思维陷阱：以为"组件越多、模板越泛化越好"。 - 错误描述：照搬 MPPI-Generic 的四维模板，给自己的项目也设计五六个维度、层层模板嵌套，追求"极致通用"。 - 现象/后果：编译时间爆炸、模板报错信息长到无法阅读、新人完全看不懂，维护成本反而高于当初想避免的复制粘贴。 - 根本原因：抽象有成本（编译时间、可读性、调试难度）。维度数应匹配**真实的变化需求**——MPPI-Generic 选四维，是因为这四个恰好是 MPPI 里真正会独立变化的部分，不多不少。 - 正确做法：先识别"哪些部分真的会独立变化"，只对它们解耦；不会变的别硬抽象。通用性与简单性要权衡，不是维度越多越好。

练习¶

（实现题） 扩展 §2.3 那个可编译的 C++ 模板 demo：加入第三个维度 SAMPLER（采样器），给出两个采样器组件（如"零均值高斯"和"带漂移的高斯"），让 MPPIController 在三个维度上模板化。编译运行，验证换采样器只需换模板参数、主循环不变。
（设计扩展题） 在 demo 里再加一个 FEEDBACK_CONTROLLER 维度（可用一个最简的比例反馈），并提供一个 NoFeedback 默认组件。说明你如何用同一个 MPPIController 模板、仅通过模板参数，组合出"无反馈版"和"带反馈版"两种控制器——并把这对应到 MPPI-Generic 的 Vanilla 与 Tube-MPPI。
（思考题） MPPI-Generic 用 C++ 模板在**编译期**解耦四个维度；另一套足式控制框架 OCS2 用 C++ 继承 + 虚函数在**运行时**做类似解耦。对比两者的权衡：编译期组合在性能、编译时间、运行时灵活性上各是什么取舍？什么场景下你会反而偏向运行时多态？

§2.4 MPPI vs iLQR/DDP：何时用谁 ⭐⭐¶

动机：手里有两把锤子，得知道各砸什么钉子¶

学到这里，你手里有两类截然不同的最优控制工具。一类是**梯度法**——iLQR/DDP 及其实时变体（如 acados 的 SQP-RTI），你在前面的梯度 MPC 章节已经熟悉：它们沿名义轨迹做线性化和二次近似，用 Riccati 反向递推求出反馈律，确定性、收敛快、精度高。另一类就是本章的 MPPI——零阶、采样、GPU 并行、对代价和动力学几乎没有要求。

这两类不是"谁取代谁"的关系，而是各有适用域的互补工具。选错了，代价很实在：在一个本该用 iLQR 的光滑、强约束问题上硬上 MPPI，你会浪费算力、精度还差；在一个本该用 MPPI 的非光滑、黑箱问题上硬上 iLQR，你会发现它**根本算不出来**。本节的目标，就是把这两把锤子的适用边界讲清楚，让你拿到一个新问题时知道该抄哪一把。

如果不这样做会怎样¶

不理解二者边界，最常见的两种误用：

在非光滑/黑箱问题上用 iLQR/DDP：代价里有碰撞 indicator（撞上就罚一大笔）、或动力学是个不可微的仿真器/神经网络。iLQR/DDP 需要 \(\partial f/\partial x\)、\(\partial f/\partial u\) 和代价的 Hessian——这些在不可微处不存在或处处为零。结果是要么求不出梯度、要么二次近似完全失真，控制器失效或给出危险的解。
在光滑、强约束、CPU-only 问题上用 MPPI：比如一个动力学光滑可微、有硬性状态/输入约束、且平台没有 GPU 的精密控制任务。MPPI 的软约束（靠罚值）不如 iLQR 的硬约束（QP 内点法）精确，采样的方差也带来抖动；没有 GPU 时 MPPI 还跑不快。这里 iLQR 又快又准，MPPI 是下策。

系统对比¶

把两类方法逐维摆开（iLQR/DDP 作为梯度法的代表）：

维度	MPPI	iLQR / DDP
是否需梯度	否（零阶，只评估标量代价）	需 \(\partial f/\partial x\)、\(\partial f/\partial u\) + 代价 Hessian
代价函数限制	无限制（含不连续 indicator、CNN cost-map）	需光滑、可二次近似
动力学限制	黑箱——仿真器、神经网络皆可	需可微
GPU 并行性	极高（\(K\) 条 rollout 独立）	低（Riccati 反向递推串行）
收敛性	概率性，全局搜索但方差大	局部二次收敛，确定性
约束处理	软约束（罚值/投影/拒绝采样）	硬约束（QP 内点法/ADMM）
典型计算量	\(K=2048,T=64\)：~1.5 ms（GPU）	\(N=20\)：~0.5 ms（CPU, SQP-RTI）
杀手级场景	代价含碰撞 indicator、动力学是神经网络	动力学可微、约束重要、CPU-only 平台

逐维深入：每一行背后的"为什么"¶

梯度需求。 这是最根本的分野。iLQR/DDP 的整套机器建立在"能算导数"之上——它沿当前名义轨迹把动力学线性化（要 \(\partial f/\partial x,\partial f/\partial u\)）、把代价二次化（要梯度和 Hessian），再用这些导数做反向递推。MPPI 则只需要把每条 rollout 的总代价算成一个**标量**，丢进 \(e^{-S/\lambda}\)——它从不对代价或动力学求导。这一条差别派生出后面几乎所有差别。

代价与动力学的限制。 因为 iLQR 要二次近似代价，代价就必须**光滑**到能被抛物面合理逼近；因为它要线性化动力学，动力学就必须**可微**。MPPI 没有这些要求：代价可以是阶跃的碰撞罚值、可以是一张 CNN 输出的 cost-map、可以处处不可微；动力学可以是一个你只能"输入状态控制、输出下一状态"的黑箱仿真器，甚至一个神经网络。这正是 Ch1 那句"\(S_k\) 只作标量进指数、动力学是黑箱 \(F\)"的实战意义——它直接决定了 MPPI 能吃下 iLQR 噎死的那类问题。

并行性与计算量。 iLQR 的 Riccati 反向递推有**时序依赖**（第 \(t\) 步的反馈增益依赖第 \(t+1\) 步的值函数），是串行的，难以 GPU 并行；MPPI 的 \(K\) 条 rollout 彼此独立，是 §2.2 讲透的尴尬并行。

但注意计算量那一行的微妙：单看一次求解，优化良好的 iLQR/SQP-RTI 在 CPU 上 \(N=20\) 步可能只要 ~0.5 ms，比 MPPI 的 ~1.5 ms（GPU）还快——因为 iLQR 用导数信息"直奔"最优，而 MPPI 靠大量采样"撞"出最优，本质上 MPPI 是用算力换"不需要导数"。所以"MPPI 更快"只在"有 GPU 且问题非光滑/高维"时成立，不是无条件的。

收敛性与约束。 iLQR 局部二次收敛、确定性——给定初值，每次跑出同样的解，且在最优解附近收敛极快；代价是它只找到**局部**最优，且依赖好的初值。MPPI 是概率性的全局搜索——能跳出局部、探索多模，但带采样方差、解不唯一、收敛慢（Ch1 已分析）。约束上，iLQR 能用 QP 内点法处理**硬约束**（严格不可违反），MPPI 只能靠罚值/投影/拒绝采样做**软约束**（倾向不违反，但不保证）——这在安全攸关、约束绝不能破的场景是 iLQR 的优势。

杀手级场景：MPPI 不可替代的核心理由¶

前面的对比看起来像"各有千秋"，但有一类场景是 MPPI 不可替代**的，它定义了 MPPI 存在的核心理由。Williams 等人 2017 年那篇 JGCD（MPPI: From Theory to Parallel Computation）给出了最有说服力的实验：让一群四旋翼（论文里是九架）飞越一片**杂乱障碍环境（cluttered forest），代价函数里包含碰撞惩罚——撞上障碍，代价瞬间跳到极高。

在这个任务上，MPPI 明显**超越了 state-of-the-art 的 DDP**。原因正是前面那条最根本的分野：DDP 要沿名义轨迹把代价做**二次近似**，但碰撞惩罚是**非光滑、不可微**的——在障碍边界上代价骤跳，那里梯度为零或无定义，Hessian 更无从谈起。

DDP 的二次近似在这种代价上彻底失真：它"看不见"障碍（局部梯度为零，仿佛代价是平的），自然规避不了。而 MPPI 对此毫不费力——它只是评估每条 rollout 的标量代价，撞上障碍的 rollout 代价暴增、被赋予近零权重，加权平均自然绕开障碍，根本不需要代价可微。

同一个道理在 AutoRally 越野车上有另一个面孔。那里代价来自车载摄像头经 CNN 分割出的**可行驶区代价图（cost-map）**——一张像素化的代价场，可行驶区代价低、非可行驶区代价高，边界处是陡变甚至阶跃的，且整张图由神经网络生成、没有解析表达式可言。对梯度法，这是双重打击：cost-map 非光滑（没法二次近似），还来自一个不可微的 CNN（没法对它求 \(\partial \ell/\partial x\)）。

而对 MPPI，cost-map 不过是一个"输入位置、查表得标量代价"的黑箱——每条 rollout 在图上采样累加代价即可，CNN 是否可微、cost-map 是否光滑都无关紧要。这就是为什么 MPPI 能直接吃下"感知给的代价图"做端到端的激进驾驶，而梯度法必须先把代价图强行光滑化或参数化（损失信息、改变问题）才能用。碰撞 indicator 与 CNN cost-map 是同一类东西的两副面孔：非光滑、或黑箱、或两者皆是的代价——这正是梯度法的盲区、MPPI 的主场。

本质洞察：MPPI 存在的核心理由，不是"它比 iLQR 快"（很多时候并不快），而是"它能解 iLQR 解不了的问题"。当代价非光滑（碰撞 indicator、稀疏奖励、CNN cost-map）、或动力学是黑箱（仿真器、神经网络）时，所有依赖导数与二次近似的方法（iLQR/DDP/SQP）从根上失效——不是调参能救的，是它们的数学前提被违反了。MPPI 把这类问题从"无法处理"变成"照常处理"，靠的正是 Ch1 那个看似朴素的设计：代价只作标量进指数、动力学只当黑箱前向。这就是为什么即便 MPPI 收敛慢、有方差、约束是软的，它依然在机器人领域占据一席之地——它填的是梯度法留下的那块空白。这也是 §2.1 那个 2-D 导航例子用"不连续障碍罚值"的深意：那正是这个杀手级场景的最小缩影。

反过来：什么时候 iLQR/DDP 才是对的选择¶

杀手级场景容易让人误以为"MPPI 全面优于 iLQR"——这是错的。在它们各自的主场，梯度法依然不可替代。当问题满足"动力学光滑可微、代价能二次近似、约束必须严格满足、平台只有 CPU、且要求高精度与可复现"时，iLQR/DDP/SQP 明显更优：它们用导数信息直奔最优、收敛快（局部二次），在 CPU 上单次求解可能比 MPPI 还快；硬约束用 QP 严格处理，不会像 MPPI 的软约束那样"偶尔违反一点"；确定性意味着同样输入给同样输出，便于验证和认证（安全攸关系统看重这点）。典型例子：一台动力学建模良好的机械臂做精密轨迹跟踪、有明确的关节限位（硬约束）、跑在没有 GPU 的工业控制器上——这里 iLQR 是正解，MPPI 反而又慢又不精确。

选型决策¶

把上面的分析收成一个可操作的判断流程：

代价或动力学是否非光滑/不可微/黑箱？（碰撞 indicator、CNN cost-map、仿真器/神经网络动力学）
是 → MPPI（梯度法在此失效，这是 MPPI 的杀手级场景）。
否 → 进入下一问。
是否有 GPU、且问题高维或需要全局探索（多模、强非凸）？
是 → MPPI 划算（GPU 并行 + 全局搜索是其强项）。
否 → 进入下一问。
是否有硬约束必须严格满足、或要求高精度/确定性/可复现？
是 → iLQR/DDP/SQP（硬约束 + 二次收敛 + 确定性）。
否 → 两者皆可，按团队熟悉度与现有代码栈选。

一个常被忽略的选项是**两者结合**：用梯度法（如 iLQR）作为 MPPI 的反馈控制器或采样引导——这正是 §2.3 里 Tube-MPPI / RMPPI 在做的（iLQR 做跟踪、MPPI 做采样规划），以及前沿的 Biased-MPPI（把 iLQR/PID 的输出注入 MPPI 的采样分布）。所以"MPPI vs iLQR"并非总是二选一，高级方案常常是"让它们各司其职、协同工作"。

选型走查：三个真实场景¶

把上面的决策流套到三个具体场景上，体会它怎么用。

场景一：越野车在已知 cost-map 上高速避障。 代价来自一张感知模块输出的 cost-map（障碍、可行驶区），它**非光滑**（障碍边界处代价骤变），车上有 GPU。走决策流：第 1 问"代价是否非光滑/黑箱？"——是（cost-map 非光滑）→ 直接选 MPPI。这正是 MPPI 的主场，也是 AutoRally 的原始场景（§2.2 历史）：cost-map 非光滑让梯度法的二次近似失真，而 MPPI 只评估标量代价、毫不在意。

场景二：工业机械臂精密轨迹跟踪，有关节硬限位，控制器无 GPU。 动力学是建模良好的刚体（光滑可微），代价是光滑的跟踪误差，但有**硬性关节限位**（绝不能超），平台是 CPU-only 的工业控制器，且要求高精度、可复现。

走决策流：第 1 问非光滑/黑箱？——否（都光滑）→ 第 2 问有 GPU 且高维/需全局探索？——否（CPU-only，问题不高维也不强非凸）→ 第 3 问有硬约束/要高精度/确定性？——是（硬限位 + 高精度 + 可复现）→ 选 iLQR/SQP。这里 MPPI 是下策：它的软约束可能偶尔越限（安全风险）、采样方差伤精度、没 GPU 还跑不快，而 iLQR 用 QP 严格处理限位、二次收敛又快又准。

场景三：四足机器人在接触丰富地形上行走，动力学含接触、用学到的模型。 动力学是一个**神经网络**学到的接触模型（黑箱、且接触切换处不光滑），代价含落足点的非光滑约束。走决策流：第 1 问非光滑/黑箱？——是（神经网络动力学 + 接触非光滑）→ 选 MPPI（梯度法对黑箱神经网络动力学和接触不连续都失效）。

但要注意 §2.4 前沿提到的开放问题：接触丰富场景下，接触/非接触的代价不连续虽然 MPPI 理论上能处理，实际需要**很大的 \(K\)** 才能覆盖到有效的接触序列——这时纯 MPPI 可能采样效率不足，两者结合（用一个跟踪反馈或学到的采样先验引导 MPPI，如 §2.3 的 RMPPI 或前沿的 Biased-MPPI）往往更实际。

这三个场景覆盖了决策流的三条主要分支：非光滑/黑箱直接走 MPPI（场景一、三），光滑+强约束+CPU 走 iLQR（场景二），而接触这类"理论可处理但采样难"的硬骨头则指向"MPPI + 引导"的混合方案（场景三）。选型的核心始终是先判断**问题性质**（代价/动力学是否光滑可微、是否有硬约束、有无 GPU），而非凭"哪个听起来更先进"或"哪个更快"。

MPPI 与梯度法的混合：第三条路¶

选型走查里反复出现"两者结合"——这不是和稀泥，而是 MPPI 工程中一条成熟且活跃的路线。前面把 MPPI 与 iLQR 摆成对立面是为了讲清各自边界，但真实系统里，最强的方案往往是**让采样法的全局探索与梯度法的局部精度协同**。把这些混合范式系统梳理一下，能看清它们都在拿一方的长处补另一方的短处：

混合范式	谁补谁	代表
梯度法做反馈跟踪	iLQR 的确定性反馈补 MPPI 的鲁棒性	Tube-MPPI / RMPPI（§2.3）
梯度法的解注入采样	iLQR/PID 的解改善 MPPI 的提议均值（提样本效率）	Biased-MPPI
MPPI 给梯度法做热启动	MPPI 的全局解帮梯度法跳出局部最优	采样初始化 + 梯度精修
梯度法/RL 提供终端值函数	学到的值函数缩短 MPPI 所需时域	TD-MPC 一类（§2.1 终端代价）

逐条看它们补的是什么短板。反馈跟踪型（Tube/RMPPI）：vanilla MPPI 对状态扰动鲁棒性有限，用一个 iLQR 反馈把真实系统拉回名义轨迹增强抗扰——梯度法的确定性反馈补了采样法的鲁棒短板（§2.3 已展开，RMPPI 还把反馈纳入采样以免与跟踪器对抗）。

采样注入型（Biased-MPPI）：直接对应 §2.1 的样本效率主线——把一个由 iLQR 或 PID 算出的"还不错"的控制序列作为采样的偏置均值，让大多数 rollout 撒在更可能成功的区域，ESS 立刻上去，所需 \(K\) 下降。

热启动型：反过来，先用 MPPI 在非凸地形上找到一个全局可行的粗解，再交给 iLQR 做局部二次收敛精修——采样法补了梯度法"只找局部最优、依赖好初值"的短板。终端值函数型：用 RL 学一个值函数当 MPPI 的终端代价 \(\phi\)，让短时域也远视，省下 §2.2 的串行时间。

本质洞察：MPPI 与梯度法的对立是教学上的简化，工程上它们更像**互补的两个模块**——一个负责"在难地形上广撒网找到可行区域"（全局、无导数、抗非光滑），一个负责"在可行区域里快速精确收敛"（局部、二次收敛、确定性）。几乎每一种混合都能用 §2.1 的两条主线解释清楚：要么在**改善提议**（注入先验均值、用值函数缩短时域——提样本效率），要么在**补足反馈与精度**（跟踪反馈、梯度精修）。所以"MPPI vs iLQR"在成熟系统里常常不是单选题，而是"让它们各管一段"的架构题。理解这一点，你在面对真实任务时就不会被"必须二选一"困住——而会问"这个问题的哪一部分适合采样、哪一部分适合梯度"。

实时流水线：测量到控制输出的延迟¶

选完了方法，部署时还要关心一个量——从测得状态到发出控制的延迟。这个延迟（而非单次迭代的总耗时）才是直接影响闭环稳定性的关键：测量越"陈旧"，控制越滞后，相位裕度越差。看 MPPI 的一拍由哪些阶段组成：

rollout 阶段：拿到当前状态 \(x_0\)，在 GPU 上并行跑 \(K\) 条 rollout，算出各自代价 \(J[K]\)。这是计算量的大头。
权重更新阶段：归约求 \(\rho\)、\(\eta\)，算权重，更新控制序列，取出 \(u_0\)。相对很轻。

从"测得 \(x_0\)"到"发出 \(u_0\)"的延迟，主要由 rollout 阶段决定——而 rollout 必须等 \(x_0\) 到了才能开始（它是 rollout 的起点）。这就引出一个实时优化的思路：能不能把不依赖 \(x_0\) 的工作挪到测量到来之前做？ MPPI 里确实有这样的部分——噪声 \(\varepsilon_k\) 的生成、设备缓冲的分配、warm-start 序列的左移，这些都不需要 \(x_0\)，完全可以在"等待下一帧测量"的空档里预先完成（设备端用 cuRAND 预生成噪声尤其自然，§2.2）。把这些前置后，测量一到，rollout 立刻能用上现成的噪声开跑，"测量→控制"的延迟里就只剩真正依赖 \(x_0\) 的计算。这是把固定的控制周期"对齐"到最小反馈延迟的常见手法。

值得一提的是，梯度 MPC（如 acados 的实时迭代 SQP-RTI）也有类似的"分阶段、把重计算前置"的思想——它把求解拆成"准备阶段"（积分、线性化）和"反馈阶段"（解 QP），用意同样是压缩测量到控制的延迟。两者的流水线拆分哲学有何异同、各自把"重计算"挪到了测量的哪一侧、为什么这样能降低反馈延迟，留作本节的思考题（见下）——这正是理解实时控制流水线设计的一道好题。

MPPI 的局限与边界¶

§2.4 前面讲了 MPPI 的杀手级场景，但一个成熟的工程判断必须同时知道它**不擅长**什么。把 MPPI 的主要局限摊开，避免把它当万金油：

高维动作空间下样本效率急剧恶化。MPPI 在控制空间撒高斯噪声，维度越高，"撒中好方向"的概率越低（维度灾难）。几维到十几维（多数移动机器人、四旋翼）尚可；一旦到几十上百维（高自由度类人、密集多智能体的联合动作），盲目采样几乎撒不到有用区域，所需 \(K\) 爆炸——这正是 §2.1 样本效率主线的极端表现。
长时域下串行链拖慢、远期预测失真。\(T\) 受 §2.2 的串行依赖限制，太长则每次迭代慢、且远期动力学误差累积；想看很远又要实时，往往得靠学到的终端值函数（§2.1 终端代价）来缩短 \(T\)，而非一味加长。
软约束不适合硬安全要求。MPPI 的约束靠罚值/钳位/拒绝采样，是"倾向满足"而非"保证满足"。安全攸关、约束绝不能破的场景（如严格避障、关节硬限位），需要在 MPPI 外再套一层硬保证（控制屏障函数滤波等），或改用能处理硬约束的方法（§2.4 的 iLQR/QP）。
接触丰富场景理论可处理但采样难。代价的接触/非接触不连续 MPPI 理论上能吃，但实际要采到"恰好以正确顺序接触"的轨迹，需要极大的 \(K\)（§2.4 选型走查场景三、以及本章前沿提到的开放问题）。
概率性、有方差、不可复现。同样输入、不同随机种子给出略不同的解；这对需要确定性、可认证行为的系统（安全认证、可复现实验）是劣势，也是 §2.4 对比里 iLQR"确定性"的相对优点。

认清这些边界，才能把 MPPI 用在它真正发光的地方（非光滑/黑箱/中等维度/有 GPU），而不是硬塞进它吃力的场景再抱怨它"不好用"。这也呼应本章反复出现的样本效率主线：MPPI 的几乎所有局限，追到根上都是"盲目采样在难问题上不够高效"——而这正是后续变体与学习式方法要攻克的。

⚠️ 常见陷阱¶

编程陷阱：把不可微代价/黑箱动力学硬塞给 iLQR/DDP 求导。 - 错误描述：在代价里写了碰撞 indicator（阶跃罚值），或动力学是个黑箱仿真器，却用 iLQR/DDP，并试图对它们做数值微分或自动微分。 - 现象/后果：在不可微处梯度为零或无定义——数值微分给出零梯度（"看不见"障碍）或巨大噪声梯度；控制器要么规避不了障碍、要么发散。debug 时还容易误以为是"调参没调好"。 - 根本原因：iLQR/DDP 的数学前提是代价光滑、动力学可微；阶跃罚值和黑箱违反了这个前提，不是调参能补救的。 - 正确做法：识别出"非光滑/黑箱"这个信号，直接换 MPPI（它不需要导数）；若必须用梯度法，则要把代价改写成光滑近似（如把 indicator 换成光滑 barrier），但这会改变问题本身、需谨慎。

概念误区：以为"MPPI 全面优于 iLQR，因为它限制更少"。 - 错误描述：看到 MPPI"不需梯度、代价无限制、能并行"，就认为它在所有场景都该取代 iLQR。 - 现象/后果：在光滑、强约束、CPU-only 的精密控制问题上硬上 MPPI，结果精度差、约束偶尔被违反、还跑不快，性能全面不如 iLQR。 - 根本原因：MPPI 的"无限制"是用代价换来的——采样方差、软约束、收敛慢、需要 GPU。这些代价在梯度法的主场（光滑+约束+CPU+高精度）会暴露成明显劣势。 - 正确做法：按"选型决策"判断——非光滑/黑箱/高维+GPU 用 MPPI，光滑/强约束/CPU/高精度用 iLQR；二者是互补工具，不是替代关系。

思维陷阱：以为"快"是选择控制器的首要标准。 - 错误描述：选控制器时只比"单次求解谁更快"，看到 iLQR 0.5 ms、MPPI 1.5 ms 就断定 iLQR 更好（或反之）。 - 现象/后果：在一个非光滑问题上，因为"iLQR 更快"而选了它，结果它根本解不了——快但错，毫无意义。 - 根本原因：速度只在"两者都能正确求解"的前提下才可比。MPPI 存在的核心理由是"能解 iLQR 解不了的问题"，而非"更快"；先看"能不能解对"，再看"快不快"。 - 正确做法：选型先问"这个问题的代价/动力学性质，哪类方法在数学上适用"，确定能正确求解的候选后，再在候选里比速度、精度、约束处理等。

练习¶

（实现题/对比） 在 §2.1 的 2-D 导航问题上，把障碍罚值从"不连续阶跃 +1000"改成一个**光滑** barrier（如 \(1000\cdot e^{-d^2/\sigma^2}\)，\(d\) 为到障碍中心距离）。用 MPPI 跑通后，思考：光滑化之后，这个问题是否变得"iLQR 也能处理"？光滑化付出了什么代价（提示：barrier 的形状如何影响轨迹）？
（调试挑战） 给你一段用 iLQR 控制四旋翼穿越障碍的代码，代价含碰撞 indicator，运行时四旋翼直接撞上障碍、仿佛"没看见"它。定位问题的**根本原因**（不是某行代码的 bug，而是方法选择的错），解释为什么 iLQR 在这里"看不见"障碍，并说明换成 MPPI 为何能解决。
（思考题） acados 的 SQP-RTI 把一次求解分成"准备阶段"（积分 + 线性化，可在上一拍测量到来前预先算）和"反馈阶段"（拿到新测量后只解一个 QP）；MPPI 分成"rollout 阶段"（GPU 并行采样）和"权重更新阶段"（归约）。两者都在想方设法压缩"测量到达 → 控制输出"的延迟。对比这两种流水线拆分的设计哲学：它们各自把"重计算"挪到了哪里？为什么这样挪能降低反馈延迟？

本章常见误解汇总¶

误解	正确理解	相关节
权重公式里减不减 \(\rho\) 无所谓，反正会归一化	减 \(\rho\) 不改变权重的数学值，但不减会让大代价下 \(e^{-S/\lambda}\) 整批下溢、归一化得 NaN——是"不做就崩"的必需	§2.1
SGF 平滑是 MPPI 算法的核心组成部分	SGF 是可选的事后平滑，破坏信息论最优性，仅为改善执行器体验；不需平滑时应省略	§2.1
控制不好就加大 \(K\)，样本越多越好	\(K\) 只增加提议分布的样本数；warm-start 改善提议分布的位置，收益常大于翻倍 \(K\)（Ch1：位置比数量重要）	§2.1
MPPI 靠采样探索，对坏的初始化/提议很鲁棒	不然：提议分布的均值若跑偏进不可行区（坏初值/扰动/突变），几乎所有 rollout 失败、更新失效；MPPI 对提议质量相当敏感	§2.1
单次 MPPI 求解就是闭环反馈	单次解是开环序列；反馈来自"执行 \(u_0\) + 重测 + 重规划"的接收水平循环，warm-start 复用其计算	§2.1
用了 GPU 就一定比 CPU 快	前提是 \(K\) 大、每条计算重、控制流齐整；\(K\) 小或频繁传数据时，固定开销会盖过并行收益	§2.2
rollout 的时步循环也能并行加速	时步间有本质串行依赖（\(x_{t+1}\) 依赖 \(x_t\)）；并行只在条间（\(K\) 维）和单步内部	§2.2
模板化只是编码风格，与性能无关	在 device 上被反复调用的主循环里，模板的编译期内联避开了虚函数的分派开销与 device 限制——是为 GPU 性能服务	§2.3
`Eigen::Map` 拥有它包装的内存	Map 只是视图、不拥有内存；底层缓冲被释放后再用 Map 是悬空引用	§2.3
MPPI 全面优于 iLQR，因为限制更少	MPPI 的"无限制"用采样方差、软约束、收敛慢、需 GPU 换来；光滑+强约束+CPU+高精度场景 iLQR 更优	§2.4
选控制器先比谁更快	速度只在"都能正确求解"时可比；MPPI 的核心理由是"能解 iLQR 解不了的"，先看能否解对再看快慢	§2.4

本章小结¶

本章把第 1 章那段 20 行的理论核，沿"算法 → 实时 → 抽象 → 选型"四步，长成了一套可部署、可复用、可择优的工程系统。§2.1 补上了 \(\rho\) 减法、warm-start、SGF 三个决定成败的工程细节，让玩具变算法；§2.2 用 CUDA 把 \(K\) 条独立 rollout 并行到 GPU、讲清线程映射与三级内存层级，让算法变实时（~\(50\times\) 加速）；§2.3 以 MPPI-Generic 为范本，用 C++ 模板把动力学/代价/采样器/反馈控制器四维正交解耦，让一次性脚本变可复用的库；§2.4 把 MPPI 放回最优控制版图，与 iLQR/DDP 逐维对比，讲清了 MPPI 那个不可替代的核心理由——能解梯度法解不了的非光滑/黑箱问题。

如果要在四节之上再抽一条主线，那就是**样本效率**。回看全章：MPPI 的提议分布是盲目的各向同性高斯，大半 rollout 落在平庸区域、被浪费，所以才需要上千条（§2.1 样本效率）；ESS 是度量这份浪费的探针（§2.1）；warm-start 通过改善提议均值把浪费降下来（实测有效样本约 \(14\) 倍差距）；\(\lambda\)/\(\Sigma\) 在调探索与利用的平衡；而 GPU（§2.2）正是"既然非得撒上千条，那就用数千核心同时撒"的硬件答案——样本效率低，正是 GPU 对 MPPI 不可或缺的深层原因。连 §2.4 的混合方案（注入先验、学到的值函数）也是在攻同一个问题。把这条主线记住，第 3 章的各种变体就不再是零散技巧，而是对"如何让提议更聪明、更少浪费样本"这一个矛盾的不同解法。

术语速查表¶

术语（中/英）	一句话定义
\(\rho\) 减法 / baseline subtraction	权重计算前减去 \(\rho=\min_k S_k\)，使 \(e^{-(S_k-\rho)/\lambda}\in(0,1]\)，避免整批下溢（log-sum-exp 技巧）
log-sum-exp 技巧	稳定计算 \(\log\sum e^{x_i}\) 或 softmax 的标准手法：先减最值再取指数
warm-start shift	每周期把控制序列左移一位、末尾补填，复用上一周期的优化成果作为热启动
Savitzky–Golay 滤波 (SGF)	滑动窗口内多项式最小二乘平滑，去高频噪声同时保留峰/拐等形状特征
chattering	控制信号的高频抖动，源于采样噪声的加权平均，会损害执行器
接收水平控制 / receding horizon	每周期只执行 \(u_0\)、重测、重规划；把开环序列变成闭环反馈
尴尬并行 / embarrassingly parallel	子任务彼此完全独立、可同时进行的计算（MPPI 的 \(K\) 条 rollout 即是）
warp	GPU 的调度最小单位，32 个线程锁步执行同一指令
线程映射 / thread mapping	把 rollout 分配给 GPU 线程的方案（每 thread 一条 vs 每 warp/block 一条）
warp divergence	同一 warp 内线程走入不同分支，被迫串行执行各分支，性能下降
内存层级 / memory hierarchy	register（最快/per-thread）/ shared（block 内共享）/ global（最慢/最大）三级
树形归约 / tree reduction	用 \(\log_2 K\) 轮对半折叠把长数组并行汇成标量（求 min/sum）
四维正交解耦	把 MPPI 拆成 Dynamics/Cost/Sampler/Feedback Controller 四个可独立替换的维度
零成本抽象 / zero-cost abstraction	抽象只存在于源码层面，编译后无运行时开销（模板的编译期组合即是）
Eigen::Map	把已存在的裸指针"包装"成 Eigen 矩阵视图、不拷贝内存（零拷贝互操作）
Tube-MPPI / RMPPI	引入 iLQR 跟踪反馈增强鲁棒性的 MPPI 变体；RMPPI 把反馈纳入采样以避免与跟踪器对抗
Plant	套在控制器外的 MPC 封装层，承载 ROS 订阅/发布等系统集成接口
温度 \(\lambda\) / temperature	权重 \(w_k\propto e^{-S_k/\lambda}\) 里的尺度：小则贪婪（权重集中）、大则保守（趋均匀）；调对与否取决于代价尺度，用 ESS 监控
协方差 \(\Sigma\) / covariance	采样噪声 \(\varepsilon\sim\mathcal N(0,\Sigma)\) 的幅度，控制探索范围；按控制维度分别设、与执行器范围匹配
有效样本数 ESS	\(1/\sum_k\hat w_k^2\)，衡量 \(K\) 条 rollout 里实际有多少在贡献；调 \(\lambda\) 的可观测探针，健康区间约 \(K\) 的 \(5\%\sim50\%\)
终端代价 \(\phi\) / terminal cost	加在 rollout 末尾、近似时域之外剩余代价（cost-to-go）的项；本质是值函数，让短时域也不短视
样本效率 / sample efficiency	MPPI 的核心矛盾：盲目的各向同性提议使大半 rollout 贡献近零、被浪费，故需上千条；多数改进都在攻这一点

知识点总表¶

编号	知识点	核心要点	对应节	难度
1	\(\rho\) 减法与 log-sum-exp	减 \(\min_k S_k\) 防整批下溢；不改权重数学值；是"不做就崩"的必需	§2.1	⭐⭐
2	warm-start shift	左移复用上周期解；本质是改善提议分布的位置（Ch1 零方差理想）	§2.1	⭐⭐
3	Savitzky–Golay 平滑	滑窗多项式拟合，保形去噪；可选、破坏最优性、换执行器友好	§2.1	⭐⭐
4	CUDA 线程映射	每 thread vs 每 warp/block 一条 rollout；权衡占用率与 warp divergence	§2.2	⭐⭐⭐
5	三级内存层级	访问频率决定存放层级：register / shared / global	§2.2	⭐⭐⭐
6	权重树形归约	\(\log_2 K\) 轮对半折叠求 min/sum；写后读需 `__syncthreads()`	§2.2	⭐⭐⭐
7	主机–设备数据流	大块数据常驻设备、每周期只传 \(x_0\)/\(u_0\)；与 warm-start 天然配合	§2.2	⭐⭐⭐
8	四维正交解耦	Dynamics/Cost/Sampler/Feedback 可插拔；主循环只写一遍	§2.3	⭐⭐⭐
9	编译期组合 vs 运行时多态	模板内联进 kernel = 零成本抽象；虚函数在 device 上受限且难内联	§2.3	⭐⭐⭐
10	Eigen::Map 零拷贝互操作	包装裸指针为 Eigen 视图，主机端与 device 共享内存不拷贝	§2.3	⭐⭐⭐
11	MPPI vs iLQR/DDP 选型	非光滑/黑箱→MPPI；光滑/强约束/CPU/高精度→iLQR；二者互补	§2.4	⭐⭐
12	MPPI 不可替代的核心理由	能解梯度法因代价非光滑/不可微而失效的问题（碰撞 indicator、CNN cost-map、神经网络动力学）	§2.4	⭐⭐
13	温度 \(\lambda\) 与协方差 \(\Sigma\) 调参	\(\lambda\) 管利用、\(\Sigma\) 管探索，须一起调；用 ESS 作探针	§2.1	⭐⭐
14	样本效率（核心矛盾）	盲目提议使大半 rollout 被浪费 → 需上千条 → 需 GPU；多数改进都在攻此	§2.1	⭐⭐⭐
15	时域 \(T\) 与终端代价 \(\phi\)	\(\phi\) 近似 cost-to-go（值函数），\(\phi\) 越准则 \(T\) 可越短	§2.1	⭐⭐

累积项目：本章新增模块¶

延续第 1 章的 Mini-MPPI 累积项目（上一章交付了 20 行的理论核），本章把它升级为**可部署版**，新增三个模块：

可部署核心（deployable core）：在 Ch1 的核心循环上，把本章 §2.1 讨论过的工程细节**全部集成进一个完整实现**——\(\rho\) 减法、warm-start shift、可选 SGF 平滑、\(\lambda\)/\(\Sigma\) 旋钮、控制约束钳位、终端代价、以及 ESS 监控。下面是这个可部署核心的参考实现（已实跑验证）：

import numpy as np
class MPPI:
    def __init__(self, step, run_cost, term_cost, m, T=25, K=512,
                 lam=1.0, sigma=3.0, a_max=None, use_sgf=False):
        self.step, self.run_cost, self.term_cost = step, run_cost, term_cost
        self.T, self.K, self.lam, self.sigma = T, K, lam, sigma
        self.a_max, self.use_sgf, self.m = a_max, use_sgf, m
        self.U = np.zeros((T, m)); self.last_ess = None   # U 常驻，供 warm-start
    def _sgf(self, u, win=5, poly=2):                     # Savitzky–Golay 平滑(可选)
        h=win//2; A=np.vander(np.arange(-h,h+1),poly+1,increasing=True)
        c=(A@np.linalg.pinv(A.T@A)@A.T)[h]; out=u.copy()
        for i in range(h,len(u)-h): out[i]=c@u[i-h:i+h+1]
        return out
    def control(self, x0, rng):
        eps = self.sigma*rng.standard_normal((self.K,self.T,self.m))
        V = self.U[None] + eps                            # 微扰控制
        if self.a_max is not None: V = np.clip(V,-self.a_max,self.a_max)  # 控制约束:钳位
        S = np.tile(x0,(self.K,1)); J = np.zeros(self.K)
        for t in range(self.T):
            S = self.step(S, V[:,t]); J += self.run_cost(S)*0.05
        J += self.term_cost(S)                            # 终端代价(近似 cost-to-go)
        rho = J.min()                                     # ρ 减法:数值稳定
        w = np.exp(-(J-rho)/self.lam); w /= w.sum()
        self.last_ess = 1.0/np.sum(w**2)                  # ESS 监控(诊断 λ)
        self.U = self.U + (w[:,None,None]*(V-self.U[None])).sum(0)  # 加权更新
        if self.use_sgf:                                  # 事后平滑(可选)
            for j in range(self.m): self.U[:,j] = self._sgf(self.U[:,j])
        u0 = self.U[0].copy()
        if self.a_max is not None: u0 = np.clip(u0,-self.a_max,self.a_max)  # 执行也钳
        self.U = np.vstack([self.U[1:], np.zeros((1,self.m))])  # warm-start 左移
        return u0

把它接到 2-D 导航（含障碍、带终端代价、执行器极限 \(\pm 5\)）上闭环运行，输出：

完整参考实现: 第 53 步到达, 距离 0.180
全程平均 ESS = 56.8/512 (11%)，含: ρ减法+warm-start+λΣ+钳位+终端代价+ESS监控

这个不到 35 行的类，就是本章 §2.1 全部工程细节的归宿——交付物是一个能在 2-D 导航上稳定跑通、\(53\) 步到达、全程 ESS 维持在健康区（约 \(11\%\)）的闭环控制器。它把散落在各小节的细节装进了一个可直接复用的接口（传入 step/run_cost/term_cost 与超参即可），是后续章节继续扩展的代码基座。 2. 向量化 GPU 雏形（vectorized prototype）：把 rollout 从 Python 的 for k 循环改写成 NumPy 向量化（step_batch 一次处理 \(K\) 条，模拟 GPU 的 SIMT），并用树形归约（§2.2 的 tree_min/tree_sum，已验证与直接计算等价）替代直接求 min/sum。这一步在 CPU 上模拟了 GPU kernel 的并行结构，为将来移植到真正的 CUDA（§2.2 给出的 kernel 骨架）打好接口。 3. 模板化骨架（templated skeleton）：用 §2.3 的可编译 C++ 模板 demo 作为起点，把控制器在 Dynamics/Cost 两维上模板化，验证"换组件只需换模板参数、主循环不变"。这是把 Mini-MPPI 从"一段脚本"过渡到"可复用结构"的第一步，对应 MPPI-Generic 四维解耦的微缩版。

这三个模块合起来，让 Mini-MPPI 从第 1 章的"能验证理论的玩具"，前进到第 2 章的"数值稳定、并行结构清晰、组件可插拔的雏形"。后续章节（变体、约束、不确定性）会继续在这个雏形上添加新的采样器、代价与控制器组件——而得益于本章的模板化骨架，每一次添加都只是"插入一个新组件"，不必重写主循环。

延伸阅读¶

核心论文（⭐⭐ 必读）

Williams, G., Drews, P., Goldfain, B., Rehg, J. M., Theodorou, E. A. (2016). Aggressive Driving with Model Predictive Path Integral Control. ICRA 2016, pp. 1433–1440（DOI: 10.1109/ICRA.2016.7487277）。MPPI 首次实车：在 1/5 比例 AutoRally 越野车上 GPU 实现，完成激进越野驾驶；已采用 free-energy / relative-entropy 框架。理解 MPPI"为何而生"的起点。
Williams, G., Aldrich, A., Theodorou, E. A. (2017). Model Predictive Path Integral Control: From Theory to Parallel Computation. JGCD 40(2):344–357。本章 §2.2 的主要依据：register/shared/global 三级内存层级下的 rollout kernel 设计，以及九架四旋翼穿越 cluttered 环境、MPPI 超越 DDP 的关键实验（§2.4 杀手级场景的出处）。

工程库与平台（⭐⭐⭐ 进阶）

Vlahov, B., Gibson, J., Gandhi, M., Theodorou, E. A. (2024). MPPI-Generic: A CUDA Library for Stochastic Trajectory Optimization.（arXiv: 2409.07563；仓库 ACDSLab/MPPI-Generic）。本章 §2.3 的精读对象：header-only C++/CUDA 库，四维模板解耦 Dynamics/Cost/Sampler/Feedback Controller，含 MPPI/Tube-MPPI/RMPPI 三种控制器。精读 include/mppi/core/（rollout kernel）与 include/mppi/controllers/MPPI/（主循环）。
Goldfain, B., Drews, P., You, C. 等 (2019). AutoRally: An Open Platform for Aggressive Autonomous Driving. IEEE CSM 39(1):26–55（DOI: 10.1109/MCS.2018.2876958；arXiv: 1806.00678；仓库 AutoRally/autorally）。完整开源硬件平台：1/5 比例越野车 + 板载 GPU + ROS，因子图状态估计 + CNN 可行驶区分割。精读 autorally_control/src/path_integral/path_integral.cu，看 Williams 2016 的原始 MPPI CUDA 实现。

实现参考（⭐⭐ 核心）

UM-ARM-Lab/pytorch_mppi：纯 PyTorch 的 MPPI 实现，核心 pytorch_mppi/mppi.py；支持 SMPPI/KMPPI 扩展与基于 CMA-ES 的自动超参调优。在 GPU 上用 PyTorch 复现 MPPI 的最佳入门，便于和本章的 NumPy/CUDA 思路对照。

背景与方法（⭐⭐⭐ 进阶）

Savitzky, A., Golay, M. J. E. (1964). Smoothing and Differentiation of Data by Simplified Least Squares Procedures. Analytical Chemistry 36(8):1627–1639。SGF 的原始论文（§2.1 平滑的来源），本是平滑光谱数据，被 MPPI 借来驯服控制抖动。
前沿方向（都在攻 §2.1 的"样本效率"主线——让提议分布更聪明、更少浪费样本，可在第 3 章变体部分深入）：Biased-MPPI（RA-L 2024）把 ancillary controller（PID/iLQR）的输出注入采样分布；Transformer-MPPI（2024）用 Transformer 预测 cost landscape 指导采样以减少所需 \(K\)；2025 年的一条活跃路线是用**生成模型当采样先验**——以扩散模型或 conditional flow matching 生成多模、贴合任务的轨迹先验给 MPPI 精修，反过来 MPPI 的精修结果又作为生成模型的 warm-start，形成"全局生成 ↔ 局部精修"的双向闭环；另一条路线是**方差缩减**——把一个近似模型/曲率信息融入采样，只对模型与真实代价的残差采样，从而用更少 rollout 达到稳定（如把先验模型分解出来、或重构局部 Jacobian 做 Gauss–Newton 式更新）。这些都印证了那条主线：MPPI 的进步史，基本就是"如何采得更聪明"的历史。

仓库的 Star 数与具体版本随时间变动，使用前请以当前 GitHub / 官方文档为准。

本章与后续章节的关系¶

后续主题	关系	本章铺垫的知识点
MPPI 变体（有色噪声、协方差自适应、平滑采样、多模）	本章是 vanilla 基线，变体都在"改善提议分布"上做文章	§2.1 warm-start = 改善提议位置；§2.3 Sampler 维度可插拔
CEM 与采样优化统一视角	MPPI（软指数权）与 CEM（精英硬选）共享本章的"采样–评代价–加权–迭代"骨架	§2.1 完整伪代码；§2.2 并行 rollout
约束 / 鲁棒 / 风险敏感 MPPI	Tube/RMPPI（§2.3 已预览）与 CBF 安全滤波、CVaR 风险等在本章主循环上加层	§2.3 Feedback Controller 维度；§2.4 软 vs 硬约束
学习式世界模型 + 采样规划（如 TD-MPC）	动力学换成神经网络——正是本章 §2.4 强调的 MPPI 杀手级场景（黑箱可微/不可微动力学）	§2.4 黑箱动力学；§2.1 代价/动力学无限制
各应用方向（自驾/无人机/腿足/操作）	换系统 = 换 §2.3 的 Dynamics/Cost 组件，主循环不变	§2.3 四维解耦；§2.2 GPU 并行

一句话：本章交付的是**所有后续采样式规控的公共底座**——往后的章节，要么在"提议分布"维度做改进（变体、学习采样），要么在"反馈/约束"维度加层（鲁棒、安全、风险），要么换"动力学/代价"组件（各应用），但都站在本章的完整算法 + GPU 并行 + 模板化骨架之上。

运行健康自检¶

故障排查手册是"出了问题再查"，而部署时更主动的做法是**持续监控几个可观测量，在问题变严重前就察觉**。下面这张表给出 MPPI 跑得健康时各指标应处的状态——把它们打在日志里，一眼就能看出控制器是否在正常工作：

观测量	健康状态	异常信号 → 含义	关联节
ESS / K	落在约 \(5\%\sim50\%\)	接近 \(1/K\) → 权重坍缩（\(\lambda\) 太小或提议太差）；接近 \(100\%\) → 近均匀（\(\lambda\) 太大）	§2.1 \(\lambda/\Sigma\)、样本效率
rollout 代价沿时域	整体随迭代下降、趋于平稳	不降或震荡 → 提议跑偏、\(\lambda\)/\(\Sigma\) 失配或代价设计有误	§2.1 样本效率
控制信号	平滑、在执行器范围内	高频抖动 → 缺平滑/\(\lambda\) 太小；顶到边界 → \(\Sigma\) 太大或限位太紧	§2.1 SGF、控制约束
权重数值	全部有限、和为 1	出现 NaN → \(\rho\) 减法缺失或代价溢出（GPU fp32 下尤甚）	§2.1 \(\rho\) 减法、§2.2 fp32
单周期耗时	稳定低于控制周期（如 \(<20\,\text{ms}@50\text{Hz}\)）	逼近或超出截止时间 → \(K\)/\(T\) 过大或没用上 GPU 并行	§2.2 延迟预算
到达/跟踪误差	持续收敛到目标	卡住不动 → 可能陷入局部、提议坍缩或终端代价缺失	§2.1 终端代价、样本效率

实践上最值得常驻日志的是 ESS 和**单周期耗时**这两项：前者一眼反映"采样有没有在干活"（样本效率主线的直接读数），后者反映"实时性还有没有余量"。这两个数稳定，MPPI 基本就在健康区间运行；任何一个跑偏，对照上表就能快速定位到本章的相应小节。把"出问题再排查"前移成"持续看仪表盘"，是工程上让 MPPI 稳定上线的关键习惯。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关节
控制输出 NaN	权重计算缺 \(\rho\) 减法，大代价下 \(e^{-S/\lambda}\) 整批下溢 → \(0/0\)	1. 打印一批 \(S_k\) 的量级 2. 检查权重计算是否减了 `S.min()` 3. 加断言 `assert np.isfinite(w).all()`	§2.1
控制信号高频抖动、电机嗡鸣	缺平滑 / \(\lambda\) 太小（权重过尖）/ \(\Sigma\) 太大（采样过散）	1. 加 SGF 平滑试试 2. 按 ESS 调 \(\lambda\) 3. 适当减小 \(\Sigma\)	§2.1
到不了目标 / 收敛极慢	缺 warm-start（每周期冷启动，提议分布扔回原点）	1. 确认每周期做了左移 warm-start 2. 检查末位填充是否合理 3. 复核 \(\lambda\)/\(\Sigma\)	§2.1
GPU 版没加速甚至比 CPU 慢	\(K\) 太小喂不饱 / 每周期频繁主机↔设备传输 / warp divergence	1. 估 \(K\) 是否上千 2. 确认大块数据常驻设备、只传 \(x_0\) 3. 查代价函数是否有大量分支	§2.2
归约结果时对时错、每次运行不同	shared memory 归约缺 `__syncthreads()`，写后读竞态	1. 检查归约每轮写读之间是否同步 2. `compute-sanitizer --tool racecheck` 扫描 3. 固定种子多跑看是否一致	§2.2
换了组件后编译报错一长串模板错误	组件未满足主循环要求的接口（缺某个方法/签名不匹配）	1. 对照接口约定检查组件方法名与签名 2. 从报错最内层定位缺哪个接口 3. 参考库里现成组件的写法	§2.3
突发扰动/环境突变后控制器突然失灵	采样均值（warm-start 的序列）跑偏进不可行区，几乎所有 rollout 撞墙、拿高代价，加权更新基于全坏样本	1. 打印每周期 ESS 看是否骤降 2. 检测到失效时临时增大 \(\Sigma\) 加强探索 3. 必要时重置 warm-start 序列	§2.1
控制信号偶尔跳到不合理的大值	采样 \(v=u+\varepsilon\) 后未对执行器极限做钳位，微扰把控制推出物理范围	1. 在生成微扰控制后 clamp 到执行器上下限 2. 检查 \(\Sigma\) 是否过大导致频繁越限 3. 确认代价里对越限有惩罚	§2.1

API 速查表¶

本章涉及的核心函数与模式（Python 为本章验证用实现，CUDA/C++ 为部署形态）：

名称	签名 / 形态	说明
权重计算（\(\rho\) 减法）	`rho=S.min(); w=np.exp(-(S-rho)/lam); w/=w.sum()`	数值稳定的 softmax；减 \(\rho\) 防下溢
warm-start 移位	`np.vstack([U[1:], u_init[None,:]])`	左移一位、末尾补填，复用上周期解
Savitzky–Golay 平滑	`savgol(u, win=5, poly=2)`	滑窗多项式拟合，保形去噪；`win` 须奇数
树形归约（min/sum）	`tree_min(a)` / `tree_sum(a)`	\(\log_2 K\) 轮对半折叠，模拟 GPU 归约
rollout kernel	`__global__ void rollout_kernel(x0,U,eps,J,K,T,...)`	每线程算一条 rollout，写回 `J[k]`
MPPI-Generic 控制器	`#include <mppi/controllers/MPPI/mppi_controller.cuh>`	主循环；模板参数为四个维度
MPPI-Generic 动力学	`#include <mppi/dynamics/.../*.cuh>`，含 `computeStateDeriv()`	可插拔动力学组件
MPPI-Generic 代价	`#include <mppi/cost_functions/.../*.cuh>`，含 `computeRunningCost()`/`computeTerminalCost()`	可插拔代价组件
MPPI-Generic 反馈控制器	`#include <mppi/feedback_controllers/DDP/ddp.cuh>`	Tube/Robust-MPPI 的 iLQR 跟踪器
Eigen::Map（零拷贝）	`Eigen::Map<Eigen::Matrix<float,N,1>> x(ptr);`	把裸指针包装成 Eigen 视图，不拷贝

调参速查表¶

本章的可调量散见各节，这里汇成一张"出问题先看哪个旋钮"的速查表（对应节见括号）：

旋钮	它控制什么	调大会怎样	调小会怎样	怎么定（探针/标尺）
采样数 \(K\)（§2.2 延迟预算）	提议分布的样本数	估计更稳但更慢，受延迟预算上限约束	更快但方差大、易不稳	取够用即可（常 \(1000\sim4000\)）；先改善提议再加 \(K\)
时域 \(T\)（§2.1 终端代价）	看多远	看得远但串行链长、远期失真	算得快但短视	够看到关键后果即可；想看远配好终端代价而非一味加长
温度 \(\lambda\)（§2.1 \(\lambda/\Sigma\)）	权重多挑剔（利用强度）	权重趋均匀、方向模糊	权重过尖、退化成贪婪、方差大	用 ESS 当探针，维持在 \(K\) 的 \(5\%\sim50\%\)
协方差 \(\Sigma\)（§2.1 \(\lambda/\Sigma\)）	采样撒多开（探索广度）	探索强但浪费样本、更抖；过大还会被钳位吃掉	探索不足、遇大动作找不到	按控制维度分设，标尺\(\approx\)执行器范围的一个比例
是否用 SGF（§2.1 SGF）	事后平滑控制	—（开）抖动小但破坏最优性	—（关）保最优但可能抖	抖动伤执行器才开；否则关，或转采样层平滑
执行器极限 \(a_\max\)（§2.1 控制约束）	钳位边界	越宽越接近无约束	越窄越保守、过窄+大 \(\Sigma\) 浪费样本	取执行器真实物理极限，rollout 内与执行时都钳
终端代价权重（§2.1 终端代价）	末端剩余代价的强度	末端行为被强约束	短时域下易短视	近似 cost-to-go；短 \(T\) 时尤其重要

用法：控制器表现不佳时，先看 ESS（太低调大 \(\lambda\)、太高调小 \(\lambda\)）、再确认 warm-start 与提议质量（§2.1 样本效率）、再核对 \(\Sigma\) 与执行器范围是否匹配，最后才考虑加大 \(K\)。这套顺序背后的依据是本章的样本效率主线——位置（提议）比数量（\(K\)）重要。

研究实践建议¶

先在 CPU 上跑对，再上 GPU。 本章的 NumPy 向量化版（step_batch + 树形归约）能在 CPU 上完整验证算法逻辑与数值正确性；确认无误后再移植到 CUDA，可把"算法 bug"和"CUDA 工程 bug"分开排查，省去在 GPU 上调试逻辑错误的痛苦。
从 MPPI-Generic 的现成组件起步。 想用 MPPI 控制一个新系统，不必从零写 kernel——先用库里现成的双积分器/CartPole/四旋翼等动力学跑通，再按 §2.3 的接口写自己的 Dynamics/Cost 组件，主循环复用。
把"改善提议分布"作为优化的第一优先级。 性能不佳时，先确认 warm-start 到位（改善位置）、再按 ESS 调 \(\lambda\)、按需调 \(\Sigma\)，最后才考虑加大 \(K\) 或换更高级的采样器（有色噪声等，第 3 章）。这与 Ch1"提议分布质量优先于样本数"的结论一致。
选型先问"能不能解对"，再问"快不快"。 拿到新问题，先按 §2.4 的决策流判断梯度法是否适用（代价是否光滑、动力学是否可微）；非光滑/黑箱直接选 MPPI，不要被"iLQR 更快"误导到一个它根本解不了的问题上。
复现经典实验建立直觉。 用 pytorch_mppi 在 GPU 上复现 CartPole swing-up，对照 Ch1 的 NumPy 版测加速比；或参考 MPPI-Generic 的 mppi_common 为 Dubins 车写一个最简 CUDA kernel——动手一遍胜过读十遍。

版本信息速查¶

项目	版本 / 说明
本章 Python 验证环境	NumPy 2.4.4（CPU，向量化模拟 SIMT；本章 NumPy 片段均经实跑验证）
本章 C++ 验证环境	g++ 13.3.0，`-std=c++17`（模板组合 demo 经实编译运行验证）
CUDA / GPU 代码	按 MPPI-Generic 模式编写，未在本章环境编译运行（无 GPU）；数值正确性由 CPU 版佐证，性能数字引自 Vlahov 2024 论文
MPPI-Generic	`ACDSLab/MPPI-Generic`，依赖 CUDA + Eigen3；header-only
Eigen	Eigen3（`libeigen3-dev`），本章用到 `Eigen::Map` 等
性能基准来源	Vlahov 2024（arXiv:2409.07563）：RTX 3080，\(K{=}2048,T{=}64\),12 维 ~1.5 ms；CPU(i7-12700) ~80 ms

第 2 章未完。 本章四节已把第 1 章的理论核补成可部署算法（§2.1）、并行到 GPU（§2.2）、用模板抽象成可复用库（§2.3）、并与梯度法对比择优（§2.4）。下一阶段将继续充实各节的深度与示例、补全练习与故障场景，并对全章执行 G1–G5 质量门禁。