本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

M08 轨迹优化规划器——从代价函数设计到工业方案选型¶

性质: ❌ 纯机械臂 | 时长: 1.5 周 (15-20 学时) 前置: M07(OMPL采样规划), M04(碰撞检测/SDF); v8 Ch17(Ceres/GTSAM因子图) 下游: M09(GPU加速规划) → M10(时间参数化) → M14(MoveIt2集成)

M08.0 前置自测¶

开始本章之前，请独立回答以下 5 题。若答不出 3 题以上，建议先回顾 M07 和 M04。

#	问题	期望答案要点
1	M07 中 RRT-Connect 找到的路径有什么典型缺陷？	路径锯齿状（不光滑）、可能绕远路、不考虑动力学约束（速度/加速度/力矩）、路径长度远非最优
2	签名距离场(SDF)是什么？给定空间中一点 $x$，$\text{SDF}(x)$ 的正负值分别表示什么？	SDF 是到最近障碍物表面的带符号距离。SDF > 0 表示在障碍外（安全），SDF < 0 表示在障碍内（碰撞），SDF = 0 表示在表面上
3	非线性优化中，什么是"局部最优"？梯度下降为什么不能保证找到全局最优？	局部最优是在邻域内代价最小的点，但不一定是全局最小。梯度下降沿负梯度方向下降，但非凸函数可能有多个极小值，梯度为零不代表全局最优
4	什么是 SQP(Sequential Quadratic Programming)？它与普通梯度下降有何区别？	SQP 在每步将非线性问题近似为二次规划(QP)，然后精确求解 QP 得到搜索方向。比梯度下降收敛快（二阶收敛 vs 一阶收敛），且能处理约束
5	GTSAM 中的因子图由什么构成？变量和因子分别对应什么？	变量(Variable)是待优化的量（如位姿、关节角），因子(Factor)是变量间的约束或观测（如里程计、回环）。MAP推断等价于非线性最小二乘

本章目标¶

学完本章后，你将能够： 1. 理解采样规划与轨迹优化的**互补关系**——不是 A 或 B，而是 A 然后 B 2. 掌握 CHOMP/TrajOpt/STOMP/GPMP2 四大经典轨迹优化器的数学框架和工程特性 3. 设计轨迹优化的代价函数（平滑性、碰撞避障、约束满足） 4. 理解 SDF 梯度在轨迹优化中的核心作用及其与碰撞检测(M04)的关联 5. 在 MoveIt2 中配置链式规划管线（OMPL → STOMP/CHOMP） 6. 针对焊接/装配/码垛等工业场景进行规划方案选型

M08.1 采样 vs 优化的互补关系 ⭐¶

动机：为什么 RRT-Connect 的路径"不能直接用"？¶

回顾 M07：RRT-Connect 在 20ms 内找到可行路径——快速、概率完备。但这条路径有严重问题：

RRT-Connect 路径示意 (2D 投影):

  q_start ──┐
             │      ┌── 障碍物 ──┐
             └──────┤            ├──── 绕远路
                    └────────────┘
                         ↘ 锯齿转折
                           ↘
                             └──── q_goal

问题	原因	后果
锯齿状	随机采样导致路径不光滑	关节加速度剧烈变化 → 电机震荡
绕远路	不是最优算法	执行时间长 → 产线效率低
无动力学约束	只考虑几何碰撞	可能要求不可能的加速度或力矩
不连续	折线连接离散路径点	速度/加速度在折点处不连续

如果直接执行 RRT-Connect 的路径会怎样¶

把锯齿路径直接发给 JointTrajectoryController（回顾 M12 的 ros2_control 框架），电机控制器会尝试跟踪每个折点——导致：

在折点处关节加速度瞬间跳变（"jerk 无穷大"）
电机发出异响（高频振动）
机械臂末端轨迹抖动（传递到工作空间的位置误差）
减速器磨损加剧（谐波减速器对冲击力矩敏感）

两种范式的互补性¶

回顾 M07.4 的本质洞察："先找到一条能用的路径，质量交给后续优化。"现在我们正式进入"后续优化"阶段。

采样规划(M07)的优势: - 概率完备：如果路径存在，给足时间一定能找到 - 对窄通道有效（只要采样到通道就能通过） - 找到**拓扑上正确**的路径（绕障碍物的正确一侧）

轨迹优化的优势: - 路径光滑、可执行 - 可以嵌入动力学约束（速度、加速度、力矩限制） - 可以优化特定指标（最短路径、最小 jerk、最少能量）

跨领域类比: 采样规划 + 轨迹优化的关系，类似于 SLAM 中的**前端(tracking) + 后端(optimization)**。前端（视觉里程计）快速给出粗糙估计，后端（因子图优化）精确优化所有变量。前端保证实时性和鲁棒性，后端保证精度。运动规划中，采样规划是"前端"（快速找到可行解），轨迹优化是"后端"（精确优化路径质量）。

MoveIt2 的工程实践¶

MoveIt2 Kilted (2025+) 将这种互补关系放进规划管线、request/response adapter 和多 pipeline 机制中。下面的 YAML 是概念示意，表达"采样规划给初值，轨迹优化做后处理"；精确字段名和插件名称应以当前 MoveIt2 官方教程与示例包为准。

# MoveIt2 pipeline_configs.yaml 只声明可用 planning pipeline。
# “OMPL -> STOMP/CHOMP 后处理”需要由 request/response adapter、
# MoveItCpp 编排代码或自定义 orchestrator 显式调用，不是下面字段自动完成。
planning_pipelines:
  pipeline_names: [ompl, stomp, chomp]

custom_planning_orchestrator:
  # 合法伪配置：表达调度意图，不是 MoveIt2 原生稳定字段。
  chains:
    ompl_then_stomp:
      stages:
        - pipeline: ompl
          planner_id: RRTConnectkConfigDefault
        - pipeline: stomp

    ompl_then_chomp:
      stages:
        - pipeline: ompl
          planner_id: RRTConnectkConfigDefault
        - pipeline: chomp

反事实推理: 如果只用轨迹优化（不用采样做初始化）会怎样？从直线初始轨迹出发，如果直线穿过障碍，优化器可能：(1) 被"拉向"障碍物错误的一侧（局部最优）；(2) 在复杂环境中完全无法收敛。采样规划提供的拓扑正确的初始路径，让优化器在正确的"山谷"中搜索。

⚠️ 常见陷阱¶

思维陷阱: 认为轨迹优化可以完全替代采样规划

🧠 这是一个常见误解。轨迹优化器是局部优化器——
   它只能在初始解附近搜索。如果初始解的拓扑错误
   （绕障碍物错误的一侧），优化器无法"跳过"障碍。
   采样规划提供的拓扑信息不可替代。
   唯一的例外: STOMP 因为使用随机扰动，有一定探索能力，
   但在复杂环境中仍不如 采样+优化 的组合可靠。

概念误区: 混淆"路径"和"轨迹"

💡 路径(Path): 构型空间中的几何曲线 σ(s), s ∈ [0,1]，无时间信息。
   轨迹(Trajectory): 含时间参数化的路径 q(t), t ∈ [0,T]，有速度/加速度。
   M07 和 M08 输出的都是路径（无时间信息）。
   将路径转化为轨迹需要时间参数化（M10, TOPP-RA/Ruckig）。
   很多文献把两者混用——本教材严格区分。

练习¶

[实验] 在 MoveIt2 中分别测试三种管线：(a) 纯 RRT-Connect；(b) 纯 CHOMP（从直线初始路径）；(c) 链式 RRT-Connect → CHOMP。在有复杂障碍（多个 box + 桌面）的场景中，记录成功率和路径长度。
[思考] 为什么 TrajOpt (Tesseract 栈) 可以不依赖 OMPL 初始化而直接从直线出发？它用了什么策略弥补没有拓扑信息的缺陷？（提示：多起点 SQP + 连续碰撞检测 CCD。）

M08.2 代价函数设计 ⭐⭐¶

动机：轨迹优化"优化什么"？¶

所有轨迹优化器的核心都是一个代价函数。将路径表示为 $N+1$ 个路径点的序列：

\[ \xi = [q_0, q_1, \ldots, q_N] \in \mathbb{R}^{(N+1) \times n} \]

总代价是三项之和：

\[ \mathcal{J}(\xi) = \underbrace{\mathcal{J}_{\text{smooth}}(\xi)}_{\text{平滑性}} + \lambda_{\text{obs}} \cdot \underbrace{\mathcal{J}_{\text{obs}}(\xi)}_{\text{碰撞避障}} + \lambda_{\text{cstr}} \cdot \underbrace{\mathcal{J}_{\text{cstr}}(\xi)}_{\text{额外约束}} \]

平滑性代价: 为什么要惩罚加速度？¶

关节电机受力矩限制。由 Newton-Euler 方程（回顾 M01 的 RNEA），关节力矩 $\tau \approx M(q)\ddot{q} + h(q, \dot{q})$。过大的加速度 $\ddot{q}$ 意味着过大的力矩。最自然的平滑性度量是**加速度的积分**：

\[ \mathcal{J}_{\text{smooth}} = \sum_{t=1}^{N-1} \left\| \frac{q_{t+1} - 2q_t + q_{t-1}}{\Delta t^2} \right\|^2 \]

这可以写成矩阵形式：

\[ \mathcal{J}_{\text{smooth}} = \xi^T A \xi \]

其中 $A$ 是有限差分加速度矩阵——一个带状对称正定矩阵：

\[ A = \frac{1}{\Delta t^4} \begin{bmatrix} 1 & -2 & 1 & & \\ -2 & 5 & -4 & 1 & \\ 1 & -4 & 6 & -4 & 1 \\ & \ddots & \ddots & \ddots & \ddots \end{bmatrix} \]

为什么不惩罚速度？ 惩罚速度 $\sum \|q_{t+1} - q_t\|^2$ 会使路径尽量短（接近直线），但直线可能穿过障碍。惩罚加速度允许路径弯曲绕过障碍，同时保证运动平滑。

更高阶: Jerk 最小化

如果惩罚 jerk（加速度的导数 $\dddot{q}$），路径会更加平滑——加速度连续，电机不受冲击。这对应六次差分矩阵。cuRobo (M09) 默认使用 jerk 惩罚 + L-BFGS 优化。

碰撞代价: SDF 的梯度是关键¶

碰撞避障代价基于**签名距离场(SDF)**。回顾 M04（碰撞检测）：SDF 给出空间中每个点到最近障碍物表面的带符号距离。

碰撞代价函数设计（Zucker et al., IJRR 2013）:

\[ c(x) = \begin{cases} -d(x) + \frac{\epsilon}{2} & \text{if } d(x) < 0 \quad \text{(在障碍内)} \\ \frac{1}{2\epsilon}(d(x) - \epsilon)^2 & \text{if } 0 \leq d(x) \leq \epsilon \quad \text{(安全裕度内)} \\ 0 & \text{if } d(x) > \epsilon \quad \text{(足够远)} \end{cases} \]

其中 $d(x) = \text{SDF}(x)$，$\epsilon$ 是安全裕度（activation distance）。

物理直觉: 距离障碍 $\epsilon$ 以外的点代价为零（安全）；进入 $\epsilon$ 范围后代价平方增长（预警）；穿入障碍后代价线性增长（惩罚）。分段设计保证在 $d = \epsilon$ 和 $d = 0$ 处连续可微。

从工作空间到构型空间的代价传递:

路径上的每个路径点 $q_t$ 对应机器人身体上所有控制点。总碰撞代价是所有路径点、所有控制点代价之和：

\[ \mathcal{J}_{\text{obs}}(\xi) = \sum_{t=0}^{N} \sum_{j=1}^{B} c\!\left(x_j(q_t)\right) \cdot \left\|\frac{d x_j}{d u}(u_t)\right\| \]

其中 $x_j(q_t) = FK_j(q_t)$ 是第 $j$ 个控制点的工作空间位置（用 M01 的 Pinocchio FK 计算），$u \in [0,1]$ 是路径参数而非物理时间，$\left\|\frac{d x_j}{d u}\right\|$ 是弧长加权——工作空间路径走得更长的部分权重更高。

SDF 梯度的计算——完整推导¶

碰撞代价的梯度是优化器的驱动力——"告诉优化器往哪个方向移动才能远离障碍"。关键链式法则：

\[ \frac{\partial c}{\partial q} = \underbrace{\frac{\partial c}{\partial d}}_{\text{代价对距离}} \cdot \underbrace{\frac{\partial d}{\partial x}}_{\text{SDF梯度}} \cdot \underbrace{\frac{\partial x}{\partial q}}_{\text{Jacobian}} \]

Step 1: 代价对距离的导数 $\frac{\partial c}{\partial d}$

从 $c(x)$ 的分段定义直接求导：

\[ \frac{\partial c}{\partial d} = \begin{cases} -1 & \text{if } d < 0 \quad \text{(在障碍内: 线性惩罚)} \\ \frac{1}{\epsilon}(d - \epsilon) & \text{if } 0 \leq d \leq \epsilon \quad \text{(安全裕度内: 递减推力)} \\ 0 & \text{if } d > \epsilon \quad \text{(足够远: 无梯度)} \end{cases} \]

物理直觉: 在障碍内（$d<0$），梯度恒为 $-1$（最大推力）。进入安全裕度后，推力线性递减——距离越远推力越小。超出 $\epsilon$ 后推力为零。这个分段设计保证了 $c(x)$ 在 $d=0$ 和 $d=\epsilon$ 处连续可微——这是梯度下降数值稳定的必要条件。

Step 2: SDF 梯度 $\nabla \text{SDF}(x)$

SDF 梯度有两种计算方法：

方法 A: 有限差分（体素网格上）

当 SDF 预计算在体素网格上时（分辨率 $h$），用中心差分：

\[ \frac{\partial \text{SDF}}{\partial x_i}(x) \approx \frac{\text{SDF}(x + h \cdot e_i) - \text{SDF}(x - h \cdot e_i)}{2h} \]

其中 $e_i$ 是第 $i$ 轴的单位向量。三个分量组成梯度向量。

// SDF 梯度有限差分计算
Eigen::Vector3d sdf_gradient(const VoxelGrid& sdf,
                              const Eigen::Vector3d& x) {
    double h = sdf.resolution();  // 体素分辨率
    return Eigen::Vector3d(
        (sdf.query(x + h*Eigen::Vector3d::UnitX())
       - sdf.query(x - h*Eigen::Vector3d::UnitX())) / (2*h),
        (sdf.query(x + h*Eigen::Vector3d::UnitY())
       - sdf.query(x - h*Eigen::Vector3d::UnitY())) / (2*h),
        (sdf.query(x + h*Eigen::Vector3d::UnitZ())
       - sdf.query(x - h*Eigen::Vector3d::UnitZ())) / (2*h)
    );
}

方法 B: 解析梯度（几何原语）

对简单几何体（球、OBB、平面），SDF 梯度有解析表达式：

\[ \nabla \text{SDF}(x) = \frac{x - x_{\text{nearest}}}{\|x - x_{\text{nearest}}\|} \]

其中 $x_{\text{nearest}}$ 是障碍物表面上距 $x$ 最近的点。梯度方向就是**从最近表面点指向查询点的单位向量**——即远离障碍物的方向。解析梯度在任意精度下都精确，但只对简单几何体可用；复杂 mesh 障碍仍需 SDF 体素网格。

Step 3: 控制点 Jacobian $J_j(q) = \frac{\partial x_j}{\partial q}$

// Pinocchio 计算控制点 Jacobian
pinocchio::computeFrameJacobian(
    model, data, q,
    frame_id_j,                    // 控制点所在 frame
    pinocchio::LOCAL_WORLD_ALIGNED, // 世界坐标系下的 Jacobian
    J_j);                           // 输出 6×n Jacobian
// 取前 3 行 (位置部分): J_j_pos = J_j.topRows(3)

完整碰撞梯度组装:

\[ \frac{\partial \mathcal{J}_{\text{obs}}}{\partial q_t} = \sum_{j=1}^{B} \frac{\partial c}{\partial d}\bigg|_{d_j(q_t)} \cdot J_{j,\text{pos}}(q_t)^T \cdot \nabla \text{SDF}\!\left(x_j(q_t)\right) \cdot \left\|\frac{d x_j}{d u}(u_t)\right\| \]

每一项的物理含义：$\frac{\partial c}{\partial d}$ 决定推力大小，$\nabla \text{SDF}$ 决定工作空间中的推力方向，$J^T$ 将工作空间推力映射到关节空间。

项	含义	计算方法
$\frac{\partial c}{\partial d}$	代价对距离的导数	由 $c(x)$ 的分段定义直接求导（见 Step 1）
$\frac{\partial d}{\partial x} = \nabla \text{SDF}(x)$	SDF 梯度	有限差分 or 解析（见 Step 2）
$\frac{\partial x}{\partial q} = J_j(q)$	控制点的 Jacobian	Pinocchio `computeFrameJacobian`（见 Step 3）

本质洞察: SDF 梯度 $\nabla \text{SDF}(x)$ 指向**远离障碍物的方向**。将它通过 Jacobian 映射回关节空间，就得到了"在关节空间中怎么移动才能远离障碍"的信息。这就是轨迹优化的碰撞避障机制——**梯度推力**把路径从障碍物上"推开"。

反事实推理: 如果不用 SDF 梯度而用二值碰撞标记（碰撞=1/不碰撞=0）会怎样？二值标记在边界处梯度为零（或未定义），优化器无法知道"往哪走才能远离障碍"——只知道"碰了"但不知道"怎么逃"。SDF 提供了**连续可微的距离信息**，让梯度下降有明确方向。这正是 STOMP（M08.5）存在的原因——当代价函数不可微时，需要无梯度方法。

⚠️ 常见陷阱¶

编程陷阱: SDF 分辨率太低

⚠️ SDF 通常预计算在体素网格上。如果网格分辨率太低
   （如 5cm），小障碍物可能完全被忽略。
   现象: 轨迹优化后路径仍然碰撞。
   根本原因: SDF 中小障碍的距离值不准确。
   正确做法: SDF 分辨率 ≤ 机器人最小 link 半径的一半，
            通常 1-2cm 对桌面操作场景足够。
   工程方案: cuRobo (M09) 用 GPU 实时生成高分辨率 ESDF。

概念误区: 混淆"软约束"和"硬约束"碰撞处理

💡 碰撞代价是软约束——它惩罚碰撞但不保证消除碰撞。
   如果碰撞代价权重 λ_obs 太小，优化器可能为了路径
   更短而"容忍"轻微碰撞。
   硬约束 (TrajOpt): 把碰撞放在 QP 的不等式约束中——
     不满足就不是可行解，求解器必须满足。
   CHOMP/STOMP 用软约束，TrajOpt 用硬约束——
   这是它们在安全性上的核心区别。

练习¶

[手推] 对 $N=4$（5 个路径点）的 1-DOF 路径，写出平滑性矩阵 $A$（3x3 内部点矩阵）。从直线初始路径 $\xi = [0, 0.25, 0.5, 0.75, 1.0]$ 出发，计算 $\mathcal{J}_{\text{smooth}}$。
[编程] 在 2D 空间中预计算一个 SDF（100x100 网格，一个圆形障碍），可视化 SDF 的值（热力图）和梯度方向（箭头图）。验证梯度指向远离障碍的方向。
[思考] 为什么碰撞代价要乘以弧长加权 $\|d x / d u\|$？如果不加权会怎样？（提示：考虑相邻路径点重合或工作空间位移很小的路径段——它们应该占同样大的碰撞积分权重吗？）

M08.3 CHOMP——协变梯度下降 ⭐⭐¶

动机：最直观的轨迹优化¶

Ratliff et al. (ICRA 2009, ~1200 引用) 和 Zucker et al. (IJRR 2013, ~1000 引用) 提出 CHOMP (Covariant Hamiltonian Optimization for Motion Planning)——用**梯度下降**直接优化路径。

历史背景¶

CHOMP 的灵感来自 Hamilton 力学中的变分原理和黎曼几何中的协变导数。传统梯度下降在函数空间中有"度量选择"问题——用不同的内积定义"最速下降方向"得到不同结果。CHOMP 选择**协变度量**（用平滑性矩阵 $A$ 作为内积），使得更新后的路径自然保持光滑。

算法推导¶

总代价：$\mathcal{J}(\xi) = \underbrace{\xi^T A \xi}_{\text{平滑性}} + \underbrace{\mathcal{J}_{\text{obs}}(\xi)}_{\text{碰撞}}$

普通梯度下降:

\[ \xi \leftarrow \xi - \eta \cdot \nabla \mathcal{J}(\xi) \]

问题：$\nabla \mathcal{J}_{\text{obs}}$ 可能很"尖锐"——在障碍物边界处梯度方向突变，产生不光滑的更新。

CHOMP 的协变梯度下降:

\[ \xi \leftarrow \xi - \eta \cdot A^{-1} \cdot \nabla \mathcal{J}(\xi) \]

关键区别在于乘了 $A^{-1}$——平滑性矩阵的逆。

为什么 $A^{-1}$ 有效？

$A$ 是平滑性矩阵（惩罚高频振荡——它的特征值对应不同频率，高频对应大特征值）。$A^{-1}$ 偏好低频变化（高频成分被抑制）。乘以 $A^{-1}$ 后，碰撞梯度的"高频尖刺"被平滑化——更新后的路径天然光滑。

数学上，这等价于在由 $A$ 定义的黎曼度量下做梯度下降：

\[ \langle f, g \rangle_A = f^T A g \]

在此度量下，$A^{-1} \nabla \mathcal{J}$ 才是"最速下降方向"（而非 $\nabla \mathcal{J}$）。

跨领域类比: CHOMP 的协变更新类似于自然梯度下降（Natural Gradient Descent）。在概率模型优化中，普通梯度下降在参数空间中沿欧氏梯度方向走，但自然梯度下降沿 Fisher 信息矩阵定义的黎曼梯度方向走——后者在概率分布空间中才是"最速下降"。类比地，CHOMP 在路径函数空间中用 $A$ 定义的度量走，才是路径空间中的"最速下降"。两者的共同思想：好的度量比好的步长更重要。

协变梯度的严格推导¶

为什么 $A^{-1} \nabla \mathcal{J}$ 才是路径空间中的"正确梯度方向"？以下是从黎曼几何角度的完整推导。

Step 1: 定义路径空间的内积

在普通欧氏空间中，梯度 $\nabla f$ 满足：

\[ df(\delta\xi) = \langle \nabla f, \delta\xi \rangle_{\text{Euclidean}} = (\nabla f)^T \delta\xi \]

但路径空间中，我们用平滑性矩阵 $A$ 定义内积：

\[ \langle f, g \rangle_A = f^T A g \]

这个内积惩罚"不光滑"的路径变化——高频成分的范数更大。

Step 2: 在 $A$-度量下重新定义梯度

在 $A$-度量下，梯度 $\bar{\nabla} f$ 必须满足：

\[ df(\delta\xi) = \langle \bar{\nabla} f, \delta\xi \rangle_A = (\bar{\nabla} f)^T A \cdot \delta\xi \]

对比欧氏定义 $df(\delta\xi) = (\nabla f)^T \delta\xi$，得到：

\[ (\bar{\nabla} f)^T A = (\nabla f)^T \implies \bar{\nabla} f = A^{-1} \nabla f \]

因此 $A^{-1} \nabla \mathcal{J}$ 才是在平滑路径空间中的"最速下降方向"。

Step 3: 理解 $A^{-1}$ 的频率滤波效果

$A$ 的特征分解 $A = U \Lambda U^T$（$U$ 是特征向量矩阵，$\Lambda = \text{diag}(\lambda_1, \ldots, \lambda_N)$ 是特征值）中： - 低频特征向量对应小特征值 $\lambda_k \propto k^4$（二阶差分矩阵的性质） - 高频特征向量对应大特征值

$A^{-1}$ 将特征值取倒数：$\Lambda^{-1} = \text{diag}(1/\lambda_1, \ldots, 1/\lambda_N)$。乘以 $A^{-1}$ 后，高频分量（大 $\lambda_k$）被 $1/\lambda_k$ 缩小，低频分量被放大。效果：碰撞梯度中的高频"尖刺"被抑制，更新后的路径天然光滑。

本质洞察: CHOMP 的协变更新不是简单的"乘一个矩阵"——它在数学上等价于在路径函数空间中选择了正确的黎曼度量。这个度量让优化器在搜索"平滑路径"构成的子空间中移动，而非在"所有可能路径变化"的全空间中移动。结果：每一步更新都保证产生光滑路径，不需要后处理。

完整算法流程¶

CHOMP(q_start, q_goal, obstacles, max_iter):
    // 初始化: 直线路径 (或 M07 的 OMPL 路径)
    ξ = linspace(q_start, q_goal, N+1)

    // 预计算
    A = 有限差分加速度矩阵 (带状, 正定)
    A_inv = Cholesky(A).solve(I)  // 高效求逆
    SDF = 从障碍物计算签名距离场 (回顾 M04)

    for iter = 1 to max_iter:
        // 计算碰撞代价梯度
        grad_obs = zeros(N+1, n)
        for t = 1 to N-1:           // 不更新起点和终点
            for j = 1 to B:         // B 个控制点
                x_j = FK_j(ξ[t])    // 工作空间位置 (M01)
                d_j = SDF(x_j)      // 签名距离
                if d_j < ε:         // 在激活距离内
                    grad_x = ∇SDF(x_j)       // SDF 梯度
                    J_j = Jacobian_j(ξ[t])    // 控制点 Jacobian
                    grad_obs[t] += J_j.T * dc_dd(d_j) * grad_x

        // 总梯度
        grad = 2 * A * ξ + grad_obs

        // 协变更新 (核心!)
        delta = A_inv * grad
        ξ[1:N] -= η * delta[1:N]    // 只更新内部点

        // 收敛判断
        if ||delta|| < tol: break

    return ξ

CHOMP 在 MoveIt2 中的使用¶

# MoveIt2 中 CHOMP 配置
chomp:
  planning_time_limit: 10.0
  max_iterations: 200
  max_iterations_after_collision_free: 5   # 无碰撞后继续优化几步
  smoothness_cost_weight: 0.1              # 平滑性权重
  obstacle_cost_weight: 1.0                # 碰撞代价权重
  learning_rate: 0.01                       # 步长 η
  smoothness_cost_velocity: 0.0            # 速度惩罚 (通常为 0)
  smoothness_cost_acceleration: 1.0         # 加速度惩罚
  smoothness_cost_jerk: 0.0                # jerk 惩罚
  ridge_factor: 0.01                       # 正则化 (防止 A 奇异)
  collision_clearance: 0.2                  # 安全裕度 ε (m)
  collision_threshold: 0.07                # 碰撞距离阈值
  use_stochastic_descent: true             # HMC 逃逸局部极小
  enable_failure_recovery: true             # 失败时加噪声重启

IJRR 版本改进 (Zucker et al., 2013): - HMC (Hamiltonian Monte Carlo): 在梯度下降中注入随机动量，帮助逃逸局部极小 - 硬约束支持: 通过投影梯度法处理关节限位

CHOMP 的可视化教学价值¶

CHOMP 的最大教学优势是**可视化直观**：可以观察路径在每次迭代中如何被碰撞代价场"推出"障碍物。MoveIt2 提供了 CHOMP 的迭代可视化——路径像被一只看不见的手慢慢拉出障碍区域。

⚠️ 常见陷阱¶

概念误区: 认为 CHOMP 能保证无碰撞路径

💡 CHOMP 的碰撞处理是软约束——碰撞代价可以被"容忍"。
   如果 obstacle_cost_weight 不够大，或初始路径深入障碍，
   优化后的路径可能仍然轻微碰撞。
   正确做法: 优化后必须做一次硬碰撞检测 (M04) 验证。
   MoveIt2 自动做这一步——CHOMP 输出后经过碰撞验证。

编程陷阱: A_inv 的数值稳定性

⚠️ 平滑性矩阵 A 的条件数随 N 增大而增大。
   直接求 A.inverse() 可能数值不稳定。
   正确做法: 用 Cholesky 分解 A = LLᵀ，通过回代求 A⁻¹ξ。
   MoveIt2 的 CHOMP 实现已做此优化。

练习¶

[手推] 对 $n=1$（1-DOF）、$N=4$（5 个路径点）的路径，写出 $A$ 和 $A^{-1}$。从直线初始路径出发，假设 $q=0.5$ 处有障碍（SDF 梯度方向为"朝 $q>0.5$"），手动执行一步协变更新。对比协变更新和普通梯度更新的结果——协变更新应该更光滑。
[编程] 在 MoveIt2 中配置 CHOMP，设置桌面抓取场景。观察 CHOMP 迭代过程中路径如何被"推出"障碍。

M08.4 TrajOpt——序贯凸优化的工业标杆 ⭐⭐⭐¶

动机：碰撞约束应该是"软的"还是"硬的"？¶

CHOMP 将碰撞处理为代价（软约束）——简单但不保证安全。工业环境要求**碰撞约束必须满足**（硬约束）。Schulman et al. (RSS 2013, ~1200 引用) 提出 TrajOpt：用 SQP 将碰撞作为**硬约束**处理。

历史背景¶

TrajOpt 来自 UC Berkeley Pieter Abbeel 组，其核心创新有三： 1. 将非凸碰撞约束**线性化**为一阶近似 2. 在**信赖域**(Trust Region)内求解凸化后的 QP 3. 用**连续碰撞检测(CCD)**替代离散碰撞检查——更安全

SQP 数学框架¶

原始非凸问题：

\[ \min_{\xi} \quad \mathcal{J}_{\text{smooth}}(\xi) $$ $$ \text{s.t.} \quad d_j(q_t) \geq \delta_{\text{safe}}, \quad \forall t \in \{0,\ldots,N\}, \; j \in \{1,\ldots,B\} \]

其中 $d_j(q_t)$ 是路径点 $q_t$ 的第 $j$ 个 link 到最近障碍的距离，$\delta_{\text{safe}}$ 是安全裕度。

SQP 迭代: 在当前路径 $\xi_k$ 处，将非线性约束线性化：

\[ d_j(q_t + \Delta q_t) \approx d_j(q_t) + \nabla_{q} d_j^T \cdot \Delta q_t \geq \delta_{\text{safe}} \]

得到 QP 子问题：

\[ \min_{\Delta\xi} \quad \frac{1}{2} \Delta\xi^T H_k \Delta\xi + g_k^T \Delta\xi $$ $$ \text{s.t.} \quad J_{\text{col},k} \cdot \Delta\xi + d_k \geq \delta_{\text{safe}}, \quad \|\Delta\xi\|_\infty \leq \rho_k \]

其中 $H_k$ 近似 Hessian（通常取平滑性矩阵），$g_k$ 是梯度，$\rho_k$ 是信赖域半径。

信赖域策略:

ratio = actual_cost_reduction / predicted_cost_reduction

if ratio > 0.75:
    trust_region *= 2.0     // 模型准确, 扩大信赖域
    accept step
elif ratio > 0.25:
    accept step             // 还行, 保持
else:
    trust_region *= 0.5     // 模型不准, 缩小, 拒绝此步

SQP 碰撞约束线性化的完整推导¶

TrajOpt 的核心数学贡献是将非凸碰撞约束线性化为 QP 可处理的形式。以下是完整推导。

原始约束: 对路径点 $q_t$，第 $j$ 个控制点到最近障碍的距离 $d_j(q_t) \geq \delta_{\text{safe}}$。

$d_j(q_t)$ 是关于 $q_t$ 的非线性函数（因为它包含 FK + SDF 查询）。在当前路径 $q_t^{(k)}$ 处做一阶 Taylor 展开：

\[ d_j(q_t^{(k)} + \Delta q_t) \approx d_j(q_t^{(k)}) + \nabla_q d_j^T \cdot \Delta q_t \]

$\nabla_q d_j$ 的计算: 这正是 M08.2 中推导的 SDF 梯度链式法则：

\[ \nabla_q d_j = J_{j,\text{pos}}(q_t)^T \cdot \nabla \text{SDF}(x_j(q_t)) \]

将线性化约束代入 QP：

\[ d_j(q_t^{(k)}) + \nabla_q d_j^T \cdot \Delta q_t \geq \delta_{\text{safe}} \]

整理为标准线性不等式形式：

\[ \nabla_q d_j^T \cdot \Delta q_t \geq \delta_{\text{safe}} - d_j(q_t^{(k)}) \]

左侧是决策变量 $\Delta q_t$ 的线性函数，右侧是常数——这正是 QP 可以处理的线性不等式约束。

为什么需要信赖域？ 一阶线性化只在 $\Delta q_t$ 较小时准确。如果 QP 给出很大的 $\Delta q_t$，线性化近似不再有效——可能导致更新后的路径实际碰撞加重。信赖域 $\|\Delta q_t\|_\infty \leq \rho$ 限制每步移动幅度，保证线性化质量。

连续碰撞检测 (CCD) 的优势¶

回顾 M04（碰撞检测）：离散碰撞检查只检查路径点，可能遗漏路径点之间的碰撞。

TrajOpt 使用**扫掠体积(Swept Volume)**碰撞检测：

离散检查:  ●────────────●  (只检查两端点)
                        ↑ 薄壁障碍可能被遗漏

CCD 检查:  ●════════════●  (检查整个扫掠体积)
                        ↑ 保证全程无碰撞

TrajOpt 的 CCD 使用 Bullet 物理引擎的 convexSweepTest——比 FCL 的 CCD 更成熟。

反事实推理: 如果 TrajOpt 只用离散碰撞检查会怎样？优化后的路径可能在路径点处无碰撞，但路径点之间穿过薄壁障碍。这在工业焊接中是致命的——焊枪撞到车身薄板。CCD 是 TrajOpt 区别于 CHOMP 的安全关键特性。

Tesseract 工业栈中的 TrajOpt¶

TrajOpt 在 Tesseract Robotics 的工业栈（tesseract-robotics/trajopt，Apache-2.0 许可）中得到了生产级维护。

核心代码路径: - trajopt_sco/src/solver_interface.cpp — QP 求解器接口 - trajopt/src/problem_description.cpp — OptimizeProblem() SQP 主循环 - trajopt_sco/include/trajopt_sco/bpmpd_interface.hpp — QP 后端

SQP 主循环精读（教学简化）:

// TrajOpt SQP 主循环 (教学简化版, 精简自 BasicTrustRegionSQP)
TrajOptResult optimize(TrajOptProblem& prob) {
    DblVec x = prob.getInitTrajectory();  // 初始路径
    double trust_region = 1.0;            // 信赖域半径

    for (int iter = 0; iter < max_iter; ++iter) {
        // Step 1: 在当前 x 处线性化所有约束
        auto model = prob.convexify(x);  // 碰撞约束线性化
        model.setTrustRegion(trust_region);

        // Step 2: 求解 QP 子问题
        DblVec delta_x = model.optimize();  // OSQP / qpOASES

        // Step 3: 评估实际改善 vs 预测改善
        double actual = prob.cost(x) - prob.cost(x + delta_x);
        double predicted = model.predictedImprove(delta_x);
        double ratio = actual / (predicted + 1e-10);

        // Step 4: 信赖域更新
        if (ratio > 0.75) {
            trust_region = std::min(trust_region * 2.0, max_trust);
            x = x + delta_x;       // 接受步
        } else if (ratio > 0.25) {
            x = x + delta_x;       // 接受步, 不调整
        } else {
            trust_region *= 0.5;    // 缩小, 拒绝此步
            if (trust_region < min_trust) break;
        }

        // Step 5: 收敛判断
        if (norm(delta_x) < tol &&
            prob.allConstraintsSatisfied(x)) break;
    }
    return {x, prob.cost(x)};
}

TrajOpt 的惩罚方法: 从硬约束到软约束的平滑过渡¶

在 SQP 的实际实现中，如果所有碰撞约束都作为硬约束放入 QP，可能导致 QP 无可行解（当初始路径深入障碍时）。TrajOpt 使用**L1 惩罚方法**将约束违反转化为代价：

\[ \min_{\Delta\xi} \quad \frac{1}{2}\Delta\xi^T H \Delta\xi + g^T \Delta\xi + \mu \sum_{i} \max(0, \delta_{\text{safe}} - d_i - \nabla d_i^T \Delta q) \]

当惩罚系数 $\mu$ 足够大时，L1 惩罚的最优解等价于原始硬约束问题的最优解。TrajOpt 自动调整 $\mu$：

初始 μ = 10
每次迭代:
  如果所有约束满足 → μ 不变
  如果仍有约束违反 → μ *= 10 (指数增长)
  当 μ > 10^6 时报告失败 (约束不可满足)

为什么用 L1 而非 L2 惩罚？ L1 惩罚在约束刚好满足时梯度非零（kink），这推动解精确满足约束（而非"接近"满足）。L2 惩罚在约束满足时梯度为零，只能渐近满足约束。

跨领域类比: TrajOpt 的 L1 惩罚方法类似于压缩感知中的 L1 正则化（LASSO）——两者都利用 L1 的 kink 特性推动解到精确的边界上。在压缩感知中，L1 推动稀疏性（系数精确为零）；在 TrajOpt 中，L1 推动约束满足（距离精确等于安全裕度）。

CHOMP vs TrajOpt 核心对比¶

维度	CHOMP	TrajOpt
碰撞处理	软约束（代价）	硬约束（QP 不等式）
优化方法	一阶（协变梯度下降）	二阶（SQP → QP）
碰撞检测	离散 (EDF/ESDF)	连续 (CCD via Bullet)
收敛速度	慢（100-200 步）	快（10-30 步）
安全保证	不保证无碰撞	保证约束满足
实现复杂度	低（梯度 + 矩阵乘）	高（需 QP 求解器）
MoveIt2 集成	内置插件	Tesseract 独立栈
教学价值	★★★★★	★★★★☆

Tesseract 的工程亮点¶

除了 TrajOpt 求解器本身，Tesseract 栈还有两个值得精读的工程设计：

1. Command Language (tesseract_command_language)

// 工业机器人示教器程序的 C++ 对象化
MoveInstruction move1(CartesianWaypoint({0.4, 0.0, 0.5}),
                      MoveInstructionType::LINEAR);
MoveInstruction move2(StateWaypoint({0, 0.5, 0, -1.5, 0, 2, 0}),
                      MoveInstructionType::FREESPACE);

2. Task Composer (tesseract_task_composer)

基于 Taskflow 的 DAG 任务流水线——把"简化 → 约束检查 → 时间参数化 → 运动规划 → 输出验证"串成有向无环图。

⚠️ 常见陷阱¶

编程陷阱: QP 求解器的选择影响精度和速度

⚠️ TrajOpt 支持多种 QP 后端:
   OSQP:    开源, 适用于大多数场景, 但精度较低 (~1e-3)
   qpOASES: 更高精度, 对中小规模 QP 速度快
   Gurobi:  最快最精确, 但需要商业许可证
   工业高精度场景: 优先 qpOASES 或 Gurobi。
   研究/原型: OSQP 足够。

练习¶

[精读] 阅读 trajopt_sco/src/solver_interface.cpp 中 BasicTrustRegionSQP::optimize() 方法（约 100 行）。标注 QP 构造、QP 求解、步长检查、信赖域更新——对应论文 Algorithm 1 的哪一步？
[工程] 用 Tesseract 的 TrajOpt 为 UR5 规划一条避障路径。切换 QP 后端（OSQP vs qpOASES），对比求解速度和路径质量。
[思考] TrajOpt 把碰撞约束放在 QP 的**硬约束**里，CHOMP 把碰撞放在**代价**里。两种方式在什么场景下各有优势？（提示：考虑初始解质量差时的行为差异。）

M08.5 STOMP——无梯度轨迹优化 ⭐⭐¶

动机：如果代价函数不可微怎么办？¶

CHOMP 和 TrajOpt 都需要代价函数的梯度。但有些代价函数不可微：

二值碰撞标记（碰撞=1, 不碰撞=0，在边界处不连续）
力矩限制阈值（$|\tau_i| \leq \tau_{\max}$ 是不可微的约束）
离散代价函数（如"穿过传感器盲区"的惩罚）

Kalakrishnan et al. (ICRA 2011, ~1000 引用) 提出 STOMP (Stochastic Trajectory Optimization for Motion Planning)：**不需要梯度**的轨迹优化方法。

算法：路径积分(PI²)的运动规划变体¶

STOMP 源自强化学习中的**路径积分方法 (PI², Policy Improvement with Path Integrals)。核心思想：**不算梯度，用随机扰动探索方向。

STOMP(ξ_init, cost_fn, max_iter):
    ξ = ξ_init                    // 初始路径
    R = smoothness_covariance()   // 噪声协方差 (由 A^{-1} 决定)

    for iter = 1 to max_iter:
        // Step 1: 生成 K 条扰动路径
        for k = 1 to K:
            ε_k ~ N(0, R)         // 高斯噪声 (自然光滑)
            ξ_k = ξ + ε_k         // 扰动路径

        // Step 2: 评估每条路径的代价 (不需要梯度!)
        for k = 1 to K:
            S_k = cost_fn(ξ_k)    // 任意代价函数

        // Step 3: 指数加权平均
        for t = 1 to N-1:
            w_k(t) = exp(-S_k(t) / λ) / Σ_j exp(-S_j(t) / λ)
            δξ(t) = Σ_k w_k(t) * ε_k(t)

        // Step 4: 更新路径
        ξ += δξ

    return ξ

为什么有效？ 低代价的扰动方向获得高权重，高代价的方向获得低权重。加权平均自然地让路径"朝代价低的方向移动"——无需显式计算梯度。

噪声协方差 $R$ 的设计: 用 $A^{-1}$（平滑性矩阵的逆）作为协方差——生成的噪声天然光滑。不会产生"锯齿状"扰动。

PI^2 的信息论推导¶

STOMP 的数学基础来自 PI^2（Policy Improvement with Path Integrals）。以下是关键推导。

Step 1: 最优控制 → 指数变换

将代价最小化问题做 Feynman-Kac 变换：定义值函数 $V(q) = -\lambda \log \mathbb{E}[\exp(-S/\lambda)]$。

Step 2: 概率解释

$\exp(-S_k/\lambda)$ 是路径 $k$ 的"概率权重"——代价越低概率越高。温度参数 $\lambda$ 控制"贪心度"： - $\lambda \to 0$: 只保留最低代价路径（确定性最优） - $\lambda \to \infty$: 所有路径等权（纯随机）

Step 3: 加权平均 = 最优更新方向

\[ \delta\xi^*(t) = \frac{\sum_{k=1}^{K} \exp(-S_k(t)/\lambda) \cdot \epsilon_k(t)}{\sum_{k=1}^{K} \exp(-S_k(t)/\lambda)} \]

这是 softmax 加权——在所有扰动方向中做"软选择"。当 $K \to \infty$ 时，这个更新方向收敛到代价函数的负梯度方向——即 STOMP 在大样本极限下等价于梯度下降，但不需要显式计算梯度。

本质洞察: STOMP 的 PI^2 基础揭示了一个深刻联系——随机采样-加权-平均**在数学上等价于**梯度下降。这就是为什么 STOMP 虽然不计算梯度，却能收敛到（局部）最优解。代价是用 $K$ 个采样"估计"了梯度方向——$K$ 越大估计越准，但计算越贵。这和蒙特卡洛梯度估计（REINFORCE 算法）的思想完全一致。

跨领域类比: STOMP 的采样-加权-平均机制类似于**粒子滤波**（Particle Filter）。粒子滤波用大量粒子探索状态空间，按似然重加权，高似然粒子被保留。STOMP 用大量扰动路径探索路径空间，按代价重加权，低代价路径获得高权重。两者都不需要显式的梯度或 Jacobian——只需要能评估"好坏"。

STOMP 在 MoveIt2 中的配置¶

# MoveIt2 STOMP 配置
stomp:
  num_timesteps: 60           # 路径离散化点数
  num_iterations: 50          # 最大迭代次数
  num_rollouts: 10            # 每步扰动路径数 K
  max_rollout_cost: 10000     # 代价上限 (裁剪异常)
  exponentiated_cost_sensitivity: 10.0  # 温度 λ (越大越贪心)
  control_cost_weight: 0.1    # 平滑性代价权重

四大优化器对比总表¶

维度	CHOMP	TrajOpt	STOMP	GPMP2
底层优化	协变梯度下降	SQP → QP	随机采样加权	因子图 (GTSAM)
碰撞处理	软约束 (EDF)	硬约束 (CCD)	任意代价	软约束 (因子)
需要梯度?	需要	需要	不需要	需要
收敛速度	慢 (100-200步)	快 (10-30步)	中 (50-100步)	中
安全保证	不保证	保证约束满足	不保证	不保证
全局搜索	差	差	有一定能力	差
MoveIt2 集成	内置	Tesseract 栈	内置	无
最佳搭配	OMPL 初始化	独立或 OMPL	OMPL 初始化	独立
适用场景	教学/简单场景	工业安全关键	不可微代价	SLAM 背景

⚠️ 常见陷阱¶

思维陷阱: 认为 STOMP 的零阶优势很大

🧠 STOMP 不需要梯度——但这也意味着收敛更慢。
   在可微代价（如 SDF）场景中，CHOMP/TrajOpt 用梯度信息
   10-30 步收敛，STOMP 可能需要 50-100 步。
   STOMP 的真正价值: 处理不可微代价函数时，
   CHOMP/TrajOpt 无法使用，STOMP 是唯一选择。
   决策原则: 代价函数全部可微 → 优先 TrajOpt（更快更精确）。
               有不可微代价 → 用 STOMP。

练习¶

[编程] 在 MoveIt2 中配置链式管线 RRT-Connect → STOMP。对比纯 RRT-Connect 和链式管线的路径质量。
[实验] 实现一个不可微代价函数（如"关节 3 的角度不能在 $[\pi/4, \pi/2]$ 范围内"），分别用 STOMP 和 CHOMP 尝试优化。CHOMP 的梯度在不连续点如何处理？

M08.6 GPMP2——因子图轨迹优化 ⭐⭐⭐¶

动机：SLAM 工程师的最自然过渡¶

Mukadam et al. (IJRR 2018, ~350 引用) 提出 GPMP2 (Gaussian Process Motion Planning 2)：把运动规划建模为**因子图上的 MAP 推断**——如果你学过 v8 Ch17-18 的 GTSAM，这几乎就是换了变量名的 SLAM 后端优化。

核心思想: 轨迹即因子图¶

SLAM 因子图 (回顾 v8 Ch17):
  x₀ ──[里程计]── x₁ ──[里程计]── x₂ ──[里程计]── x₃
       |                |                |
    [GPS观测]        [地标观测]       [回环检测]

GPMP2 因子图:
  q₀ ──[GP先验]── q₁ ──[GP先验]── q₂ ──[GP先验]── q₃
       |                |                |
   [起点因子]       [碰撞因子]       [终点因子]

变量与因子的对应关系:

SLAM 概念	GPMP2 概念	物理含义
位姿 $x_i$	关节构型 $q_t$	待优化的量
里程计因子	GP 先验因子	路径光滑性（偏好匀速运动）
地标观测因子	碰撞因子	避障（远离障碍物的"观测"）
GPS 因子	起点/终点固定因子	边界条件
MAP 推断	路径优化	最小化负对数后验
Gauss-Newton/LM	同样!	求解算法完全复用

本质洞察: GPMP2 的深刻之处在于它揭示了 SLAM 和运动规划的**数学同构性**——两者都是因子图上的 MAP 估计问题。区别只在于变量的物理含义（位姿 vs 关节角）和因子的物理含义（观测 vs 碰撞避障）。底层的稀疏 Cholesky 分解、增量更新（iSAM2）、因子图结构——完全相同。

GP 先验因子: 路径光滑性¶

高斯过程(GP)定义了路径的**先验分布**——偏好什么样的路径。

匀速运动 GP 先验:

\[ p(q_{t+1} \mid q_t) = \mathcal{N}\!\left(q_t + \dot{q}_t \Delta t, \; \Sigma_{GP}\right) \]

因子的误差函数:

\[ e_{\text{GP}}(q_t, q_{t+1}) = q_{t+1} - q_t - \dot{q}_t \Delta t \]

这和 SLAM 中里程计因子 $e_{\text{odo}} = x_{t+1} - x_t \ominus u_t$ 结构完全一样。

GP 协方差 $\Sigma_{GP}$ 控制先验的"紧度"： - $\Sigma_{GP}$ 小 → 强烈偏好匀速运动（路径很直） - $\Sigma_{GP}$ 大 → 允许路径弯曲（碰撞因子更容易把路径拉开）

碰撞因子: 似然函数¶

碰撞因子将 SDF 距离转化为"观测似然"：

\[ p(z_t \mid q_t) \propto \exp\!\left(-\frac{1}{2} \frac{c(q_t)^2}{\sigma_{\text{obs}}^2}\right) \]

其中 $c(q_t)$ 是碰撞代价（同 M08.2 中的定义）。

因子图求解: 将所有因子相乘，取 MAP 估计等价于最小化负对数似然——即非线性最小二乘，用 GTSAM 的 Gauss-Newton 或 Levenberg-Marquardt 求解。

GTSAM 代码示意¶

// GPMP2 路径优化 (教学简化, 展示因子图结构)
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gpmp2/gp/GaussianProcessPriorLinear.h>
#include <gpmp2/obstacle/ObstaclePlanarSDFFactor.h>

using namespace gtsam;
using namespace gpmp2;

int main() {
    NonlinearFactorGraph graph;
    Values initial_estimate;

    // === 1. 添加 GP 先验因子 (路径光滑性) ===
    for (int t = 0; t < N; ++t) {
        graph.add(GaussianProcessPriorLinear(
            Symbol('x', t), Symbol('v', t),      // 位置和速度变量
            Symbol('x', t+1), Symbol('v', t+1),
            dt, Qc));                              // 时间步长, GP 协方差
    }

    // === 2. 添加碰撞因子 (避障) ===
    for (int t = 0; t <= N; ++t) {
        graph.add(ObstaclePlanarSDFFactor(
            Symbol('x', t),
            robot_model,                   // 机器人形状模型
            sdf,                           // 签名距离场
            cost_sigma,                    // 碰撞因子方差
            epsilon));                     // 安全裕度
    }

    // === 3. 添加起点/终点固定因子 ===
    graph.add(PriorFactor<Vector>(Symbol('x', 0), q_start, start_noise));
    graph.add(PriorFactor<Vector>(Symbol('x', N), q_goal, goal_noise));

    // === 4. 设置初始估计 (直线插值) ===
    for (int t = 0; t <= N; ++t) {
        double s = static_cast<double>(t) / N;
        initial_estimate.insert(Symbol('x', t),
                                (1-s) * q_start + s * q_goal);
        initial_estimate.insert(Symbol('v', t),
                                (q_goal - q_start) / (N * dt));
    }

    // === 5. 求解 (和 SLAM 完全一样!) ===
    GaussNewtonParams params;
    params.setMaxIterations(100);
    GaussNewtonOptimizer optimizer(graph, initial_estimate, params);
    Values result = optimizer.optimize();

    // 提取优化后的路径
    for (int t = 0; t <= N; ++t) {
        Vector q_t = result.at<Vector>(Symbol('x', t));
        std::cout << "q[" << t << "] = " << q_t.transpose() << "\n";
    }
    return 0;
}

GP 先验因子的完整推导¶

为什么用 GP 作为路径先验？这不是随意选择——GP 提供了对连续时间运动的最自然的概率建模。

从物理运动模型出发: 假设机器人在无外力时做匀速运动（最自然的运动先验），即状态转移为：

\[ \begin{bmatrix} q_{t+1} \\ \dot{q}_{t+1} \end{bmatrix} = \underbrace{\begin{bmatrix} I & \Delta t \cdot I \\ 0 & I \end{bmatrix}}_{\Phi(\Delta t)} \begin{bmatrix} q_t \\ \dot{q}_t \end{bmatrix} + w_t \]

其中 $w_t \sim \mathcal{N}(0, Q_c \cdot \Delta t)$ 是过程噪声（表示对"匀速"假设的偏离程度）。

GP 先验的物理含义: - $Q_c$ 小 → 强烈偏好匀速运动 → 路径很直 - $Q_c$ 大 → 允许大幅度偏离匀速 → 路径可以弯曲（碰撞因子容易把路径拉开）

因子的误差函数推导:

\[ e_{\text{GP}} = \begin{bmatrix} q_{t+1} \\ \dot{q}_{t+1} \end{bmatrix} - \Phi(\Delta t) \begin{bmatrix} q_t \\ \dot{q}_t \end{bmatrix} \]

这个误差的协方差是 $Q = Q_c \cdot \Delta t$。因子的负对数似然：

\[ -\log p \propto \frac{1}{2} e_{\text{GP}}^T Q^{-1} e_{\text{GP}} \]

这正是 GTSAM 中因子的标准形式——和 SLAM 里程计因子完全一样，只是符号含义不同。

GP 插值: GPMP2 的另一个优势是可以在**任意时刻**（不仅是离散路径点）查询路径状态。这通过 GP 条件分布实现：给定 $q_t$ 和 $q_{t+1}$，中间时刻 $\tau \in (t, t+1)$ 的分布是解析的高斯分布。这意味着碰撞检测可以在路径点之间**连续插值检查**——类似于 TrajOpt 的 CCD，但用概率方法实现。

GPMP2 的增量更新: iSAM2 实时重规划¶

GPMP2 的杀手锏：当障碍物移动时，可以用 iSAM2（回顾 v8 Ch18）增量更新因子图——只重新优化受影响的变量，而非从头求解。

// 障碍物移动时增量更新
ISAM2 isam;
isam.update(initial_graph, initial_values);

// 障碍物移动 → 更新碰撞因子
NonlinearFactorGraph new_factors;
Values new_values;
for (int t : affected_timesteps) {
    // 删除旧碰撞因子, 添加新碰撞因子 (新 SDF)
    new_factors.add(ObstaclePlanarSDFFactor(
        Symbol('x', t), robot_model, new_sdf, sigma, eps));
}
isam.update(new_factors, new_values);
Values updated_result = isam.calculateEstimate();
// 只有受影响的变量被重新优化!

⚠️ 常见陷阱¶

概念误区: 认为 GPMP2 是"将 GP 用于学习"

💡 GPMP2 中的 "GP" 不是用于学习——而是用于定义
   路径的先验分布（偏好光滑运动）。
   这和 GP 回归（给数据拟合曲线）的用法完全不同。
   GPMP2 的 "GP" 更像是 "先验正则化"，
   而非 "数据驱动学习"。不需要训练数据。

练习¶

[编程] 用 GPMP2 为 2D 3-link 平面臂规划避障路径。画出因子图结构（节点 = 变量，方块 = 因子）。
[跨章综合] 对比 GPMP2 和 v8 Ch17 中学过的 SLAM 因子图优化：列出变量、因子、求解器的对应关系。讨论：能否用同一个 GTSAM 引擎同时做 SLAM 和运动规划？（提示：Frank Dellaert 的 SAM 系列工作。）
[思考] GPMP2 的 GP 插值允许在路径点之间连续查询碰撞。这和 TrajOpt 的 CCD 有什么异同？哪种方法提供更强的安全保证？

M08.7 时间最优轨迹规划接口 ⭐⭐¶

动机：几何路径 ≠ 可执行轨迹¶

M07-M08 到此为止输出的都是**几何路径**——一系列关节角序列 $[q_0, q_1, \ldots, q_N]$，没有时间信息。要让机械臂执行这条路径，还需要**时间参数化**：为每个路径点分配时间戳，满足速度、加速度、力矩限制。

数据流:
M07 (OMPL)            M08 (轨迹优化)          M10 (时间参数化)
─────────────────    ─────────────────       ─────────────────
关节角序列             平滑关节角序列           时间戳 + 关节角序列
[q₀, q₁,..., q_N]    [q₀',q₁',...,q_N']     [(t₀,q₀'), (t₁,q₁'),...]
  锯齿状                光滑                    可执行
  无时间信息             无时间信息               有时间信息

标准工具: - TOPP-RA (Pham & Pham, T-RO 2018): 基于可达性分析的时间最优参数化 - Ruckig (Berscheid & Kroeger, RSS 2021): 实时三阶(Jerk-limited)时间最优轨迹生成

两者在 M10 中专门讲。这里只明确接口：M08 的输出（平滑路径）是 M10 的输入。

Crocoddyl/Drake 中的联合优化¶

对于更复杂的场景（同时优化路径形状和时间分配），Crocoddyl 和 Drake 提供了联合优化框架：

Crocoddyl (loco-3d, INRIA 学派): - 基于 Pinocchio (回顾 M01) 的 DDP/iLQR 轨迹优化 - 同时优化关节角路径和力矩序列 - 后继项目 Aligator (Simple-Robotics) 正在取代 Crocoddyl

Drake (TRI 学派): - MultibodyPlant<T> + MathematicalProgram 统一框架 - 支持 SNOPT/IPOPT 大规模 NLP 求解器 - 适合接触丰富的操作规划

Drake 联合轨迹优化代码¶

Drake 的 MathematicalProgram 允许同时优化路径形状和时间分配——这是 CHOMP/TrajOpt 分离式方法无法做到的。

# Drake 联合轨迹优化 (Python, 教学简化)
from pydrake.all import (
    MultibodyPlant, Parser, DiagramBuilder,
    AddMultibodyPlantSceneGraph,
    Solve,
    KinematicTrajectoryOptimization,
    MinimumDistanceLowerBoundConstraint
)
import numpy as np

# === 1. 加载机器人模型 ===
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.001)
Parser(plant).AddModels("panda.urdf")
plant.Finalize()
diagram = builder.Build()
root_context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(root_context)

# === 2. 构建轨迹优化问题 ===
num_control_points = 10  # B-spline 控制点数
traj_opt = KinematicTrajectoryOptimization(
    plant.num_positions(),      # 7 DOF
    num_control_points,
    spline_order=4,             # 4 阶 B-spline = degree 3；简单 knot 下通常 C² 连续
    duration=2.0                # 初始总时间 (秒)
)
prog = traj_opt.get_mutable_prog()

# === 3. 添加约束 ===
# 起点和终点约束
q_start = np.array([0, -0.5, 0, -2.0, 0, 1.5, 0.7])
q_goal  = np.array([1.0, 0.5, 0.3, -1.5, 0.5, 2.0, 0.3])
traj_opt.AddPathPositionConstraint(
    lb=q_start, ub=q_start, s=0)  # s=0: 路径起点
traj_opt.AddPathPositionConstraint(
    lb=q_goal,  ub=q_goal,  s=1)  # s=1: 路径终点

# 速度和加速度限制
traj_opt.AddVelocityBounds(
    lb=-np.full(7, 2.0),  # 最大速度 2.0 rad/s
    ub= np.full(7, 2.0))
traj_opt.AddAccelerationBounds(
    lb=-np.full(7, 5.0),  # 最大加速度 5.0 rad/s²
    ub= np.full(7, 5.0))

# 碰撞避障 (最小距离约束)
# Drake 在内部自动处理 FK + 碰撞几何 + 梯度
min_distance = MinimumDistanceLowerBoundConstraint(
    plant, 0.05, plant_context)  # 最小安全距离 5cm

# AddPathPositionConstraint 的 s 参数是单个标量路径位置；
# 要在多个路径点检查，需要逐点添加约束。
for s in np.linspace(0, 1, 20):
    traj_opt.AddPathPositionConstraint(min_distance, float(s))

# 时间约束 (允许优化器调整总时间)
traj_opt.AddDurationConstraint(0.5, 5.0)  # 总时间在 0.5-5.0 秒之间

# === 4. 添加代价函数 ===
traj_opt.AddDurationCost(1.0)        # 惩罚长时间 → 鼓励快速
traj_opt.AddPathLengthCost(1.0)      # 惩罚路径长度 → 走直线

# === 5. 求解 ===
result = Solve(prog)
if result.is_success():
    trajectory = traj_opt.ReconstructTrajectory(result)
    print(f"优化后总时间: {trajectory.end_time():.2f} s")
    print(f"B-spline 控制点数: {trajectory.num_control_points()}")
    # trajectory 是 BsplineTrajectory, 可直接查询任意时刻的 q, dq, ddq
    for t in np.linspace(0, trajectory.end_time(), 50):
        q_t = trajectory.value(t)        # 位置
        dq_t = trajectory.EvalDerivative(t, 1)  # 速度
        ddq_t = trajectory.EvalDerivative(t, 2) # 加速度

Drake 的独特优势:

优势	说明
联合时间-路径优化	同时优化路径形状和时间分配（CHOMP/TrajOpt 做不到）
B-spline 参数化	4 阶 B-spline 是 3 次曲线；简单 knot 下通常 $C^2$ 连续，不需要额外几何平滑
自动微分	`MultibodyPlant<AutoDiffXd>` 自动计算碰撞约束梯度
SNOPT/IPOPT	工业级 NLP 求解器，全局收敛保证（在信赖域内）
接触感知	天然支持接触约束（peg-in-hole、多指抓取）

Drake vs CHOMP/TrajOpt/STOMP 选型:

场景	推荐	理由
MoveIt2 生态内快速集成	CHOMP/STOMP	内置插件，零配置
工业安全关键	TrajOpt (Tesseract)	硬约束 + CCD
接触丰富的操作（装配）	Drake	接触动力学原生支持
研究/最优控制原型	Drake	联合优化 + 自动微分
无梯度代价	STOMP	唯一的无梯度选项

时间分配策略¶

M07-M08 输出的几何路径没有时间信息。在进入 M10（TOPP-RA/Ruckig）做最终时间参数化之前，轨迹优化器内部也需要初步的时间分配——这影响加速度惩罚的量级和碰撞检测的时序。

策略 1: 均匀时间分配（最简单）

\[ \Delta t = \frac{T_{\text{total}}}{N} \]

所有路径段用相同时间步长。$T_{\text{total}}$ 通常启发式设定为路径长度除以最大速度的 1.5 倍。

策略 2: 弧长等分（更合理）

\[ \Delta t_k = \frac{\|q_{k+1} - q_k\|}{\sum_{i=0}^{N-1} \|q_{i+1} - q_i\|} \cdot T_{\text{total}} \]

路径段越长分配越多时间——速度近似均匀。避免短段加速度过大、长段浪费时间。

策略 3: 速度限制感知（安全优先）

# 速度限制感知的时间分配
def velocity_aware_timing(path, v_max, a_max):
    """为每段分配刚好不超速的时间"""
    dt = []
    for k in range(len(path) - 1):
        dq = path[k+1] - path[k]
        # 每个关节需要的最短时间 (匀速)
        dt_vel = np.max(np.abs(dq) / v_max)
        # 每个关节需要的最短时间 (匀加速: s = 0.5*a*t^2)
        dt_acc = np.max(np.sqrt(2 * np.abs(dq) / a_max))
        dt.append(max(dt_vel, dt_acc))
    return dt

本质洞察: 时间分配策略的选择不是 "哪个最好"——而是 "什么时候做"。在轨迹优化阶段，粗略的时间分配（均匀或弧长等分）足够，因为最终的时间最优参数化由 M10 的 TOPP-RA 或 Ruckig 完成。轨迹优化中时间分配的作用是让加速度惩罚有合理的量级——$\Delta t$ 太小会导致加速度项过大而主导优化，$\Delta t$ 太大则加速度惩罚失效。

MPC 在机械臂操作中的新趋势¶

值得注意的是，MPC 正在从轨迹规划工具进化为实时操作控制核心：

工作	会议	核心贡献
VLMPC (Huang et al.)	RSS 2024	视觉-语言模型 + MPC 集成
ToMPC (ContactNet)	2025	ADMM 实现杂乱环境安全操作
Contact-Implicit MPC	T-RO 2024	实时多接触发现

这些工作模糊了"规划"和"控制"的边界——MPC 在每个控制周期重新规划短期轨迹。

⚠️ 常见陷阱¶

思维陷阱: 混淆"轨迹优化"和"时间参数化"

🧠 轨迹优化 (M08) 解决的是路径形状——在空间中怎么走。
   时间参数化 (M10) 解决的是沿路径的速度分配——什么时候到哪。
   两者是独立的步骤（虽然可以联合优化）。
   常见错误: 认为 CHOMP/TrajOpt 输出就是"轨迹"——
   实际上它们输出的是几何路径，还需要 TOPP-RA/Ruckig 赋予时间。

练习¶

[思考] Crocoddyl 的 DDP 可以同时优化路径和力矩。这相比 CHOMP + TOPP-RA 的两阶段方法有什么优势？有什么劣势？（提示：考虑问题规模和求解速度。）

M08.8 工业案例: 规划方案选型 ⭐⭐¶

焊接场景¶

需求: 焊枪沿焊缝做匀速直线运动, 朝向固定
约束: 定向约束 + 路径约束 + 窄通道(车身内部)
安全要求: 100% 无碰撞 (CE 认证)

选型方案:
  拓扑搜索: PRM* (环境静态, 多焊点路标复用, 回顾 M07.3)
  路径优化: TrajOpt (硬约束保证安全 + CCD 检测薄壁)
  时间参数化: TOPP-RA (速度+加速度约束)
  理由: 焊接安全要求极高, 必须用硬约束而非软约束;
        CCD 保证焊枪不会穿过薄板。

装配场景 (peg-in-hole)¶

需求: 销钉插入孔洞, 接近阶段位控 → 接触阶段力控
约束: 高精度对位 (±0.1mm), 接触力限制

选型方案:
  接近阶段: RRT-Connect → CHOMP (快速找路 + 平滑)
  精对位:   pick-ik (回顾 M03.7, 优化式 IK)
  接触阶段: 切换到阻抗控制 (F 系列力控)
  理由: 接触前后策略完全不同, 不能用单一规划器覆盖。
  补充: Drake 的 ContactImplicit MPC 可以统一处理接触/非接触,
        但计算代价高 (~200ms), 适合研究原型而非产线部署。

码垛场景¶

需求: 重复搬运箱子, 固定工位, 追求最高节拍
约束: 速度最大化, 避免碰撞, 力矩限制

选型方案:
  路标图: PRM* (固定环境, 预计算一次)
  路径优化: STOMP (允许力矩代价, 不可微约束)
  时间参数化: Ruckig (实时三阶轨迹, 节拍最短)
  GPU 加速: cuRobo (M09, 如有 GPU → 30ms 全栈运动生成)
  理由: 码垛追求速度, PRM*+Ruckig 组合在固定环境中最高效。

KOMO: 增广拉格朗日学术框架¶

值得一提的是 Marc Toussaint (TU Berlin) 的 KOMO (k-Order Markov Optimization)，它在学术上影响深远但工业采用较少。

KOMO 的独特视角: 将轨迹优化视为 k 阶马尔可夫优化问题——每个路径点的代价只依赖相邻 k 个点。当 k=2 时退化为加速度惩罚（CHOMP），k=3 时对应 jerk 惩罚。

求解方法: 增广拉格朗日方法(Augmented Lagrangian)——比 SQP 更简单，但收敛更慢：

\[ \mathcal{L}_\rho(\xi, \lambda) = \mathcal{J}(\xi) + \lambda^T h(\xi) + \frac{\rho}{2} \|h(\xi)\|^2 \]

其中 $h(\xi)$ 是约束，$\lambda$ 是拉格朗日乘子，$\rho$ 是罚参数。交替更新 $\xi$（固定 $\lambda$，优化路径）和 $\lambda$（固定 $\xi$，更新乘子）。

KOMO 实现在 rai 框架中（github.com/MarcToussaint/rai），适合学术研究但不推荐用于工业部署。

选型决策流程¶

            ┌────────────────────────────────────┐
            │       代价函数全部可微?              │
            └───────────┬────────────────────────┘
               ┌────────┴────────┐
               ▼                  ▼
             是                 否
               │                  │
        安全关键?            用 STOMP
               │             (唯一选择)
        ┌──────┴──────┐
        ▼              ▼
      是              否
        │              │
   用 TrajOpt       用 CHOMP
   (硬约束+CCD)     (简单高效)

特殊情况:
  接触丰富操作 → Drake (联合优化)
  SLAM 背景/增量更新 → GPMP2 (因子图)
  学术研究/原型验证 → KOMO/rai 或 Drake

全栈规划管线延迟分解¶

从用户下发目标到可执行轨迹的完整延迟分解：

完整规划管线 (CPU, MoveIt2 Kilted):

  用户指定目标位姿 T_goal
       │
       ├── IK 求解 (M03 pick-ik): 5-50 ms
       │   → q_goal
       │
       ├── OMPL 采样规划 (M07 RRT-Connect + FCL): 20-200 ms
       │   → 锯齿几何路径 [q₀, ..., q_N]
       │
       ├── 路径简化 (OMPL simplify): 5-20 ms
       │   → 简化路径 (减少节点数)
       │
       ├── 轨迹优化 (M08 STOMP): 50-200 ms
       │   → 平滑路径
       │
       ├── 时间参数化 (M10 TOPP-RA/Ruckig): 1-5 ms
       │   → 时间戳+关节角序列
       │
       └── 碰撞验证 (最终安全检查): 5-10 ms
           → 确认无碰撞

  总延迟: 86-485 ms (中位 ~200 ms)

练习¶

[设计] 你的团队要为一家汽车工厂设计机械臂焊接系统。需求：20 个焊点，车身内部有窄通道，安全认证要求 100% 无碰撞，节拍时间 < 2 秒/焊点。设计完整的规划方案（含预处理、在线规划、路径优化、时间参数化），说明每个环节的选型理由。
[跨章综合] 结合 M04(碰撞检测) + M07(OMPL) + M08(本章)：画出从"用户指定目标位姿"到"平滑可执行路径"的完整数据流图。标注每个环节的输入、输出、典型延迟。
[手推] 对 TrajOpt 的 L1 惩罚方法：给定 1-DOF 问题，当前 $d = 0.02$，安全裕度 $\delta_{\text{safe}} = 0.05$，$\nabla_q d = 0.3$，惩罚系数 $\mu = 100$。写出 QP 的约束和代价函数。手动求解 $\Delta q$ 使约束满足。

M08.9 本章小结¶

知识点	核心要点	工程价值
采样 vs 优化互补 (M08.1)	采样找拓扑, 优化提质量	MoveIt2 链式管线
代价函数设计 (M08.2)	平滑性 + SDF 碰撞 + 约束	理解所有优化器的共同基础
CHOMP (M08.3)	协变梯度下降, 软约束	可视化直观, 教学首选
TrajOpt (M08.4)	SQP + CCD, 硬约束	工业安全关键场景
STOMP (M08.5)	无梯度, 任意代价函数	不可微约束场景
GPMP2 (M08.6)	因子图, GTSAM 底层	SLAM 背景无缝过渡
时间分配策略 (M08.7)	均匀/弧长等分/速度感知	影响加速度惩罚量级
Drake 联合优化 (M08.7)	同时优化路径+时间, B-spline	接触丰富操作首选
时间参数化接口 (M08.7)	几何路径→可执行轨迹	M10 衔接
工业选型 (M08.8)	场景驱动的方案选择	焊接/装配/码垛
KOMO (M08.8)	增广拉格朗日学术框架	学术研究参考

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

机械臂全栈项目进度: - M01: 加载 URDF → Pinocchio Model - M03: IK 求解器 (opw/TRAC-IK/pick-ik) - M04: 碰撞检测管线 (FCL/Coal) + SDF 生成 - M07: OMPL 运动规划 (RRT-Connect/BIT*) → 几何路径 - M08 (本章新增): 轨迹优化模块 - 将 M07 的锯齿路径输入 CHOMP/STOMP 平滑优化 - 配置 MoveIt2 链式管线 (OMPL → STOMP) - 输出：平滑关节角序列

下一步 (M09): 当 CPU 上的采样+优化仍不够快（> 100ms）时，用 GPU (cuRobo) 或 SIMD (VAMP) 将全栈运动生成加速到 30ms。

M07-M08 知识网络:

M03 (IK) ←──── q_goal 计算 ────→ M07 (采样规划)
     ↑                                  │
     │ Jacobian                    可行路径 (锯齿)
     │                                  │
M04 (碰撞检测) ←── SDF/FCL ──→ M08 (轨迹优化) ← 本章
     │                                  │
     │ ESDF                       平滑路径
     ↓                                  │
M09 (GPU加速) ←── 加速碰撞 ──→ M10 (时间参数化)
                                        │
                                   可执行轨迹
                                        ↓
                                M12 (ros2_control)

延伸阅读¶

资源	难度	说明
Zucker et al., "CHOMP", IJRR 2013	⭐⭐	CHOMP 完整论文, 含 HMC 扩展
Schulman et al., "TrajOpt", RSS 2013	⭐⭐⭐	SQP + 连续碰撞检测
Kalakrishnan et al., "STOMP", ICRA 2011	⭐⭐	无梯度轨迹优化
Mukadam et al., "GPMP2", IJRR 2018	⭐⭐⭐	因子图运动规划
Ratliff et al., "CHOMP", ICRA 2009	⭐⭐	CHOMP 原始短文
Sundaralingam et al., "cuRobo", ICRA 2023	⭐⭐⭐	GPU 并行轨迹优化 (M09 预读)
Toussaint, KOMO, rai 框架	⭐⭐⭐⭐	增广拉格朗日/SQP 学术框架
Drake KinematicTrajectoryOptimization 文档	⭐⭐⭐	Drake 联合路径-时间优化 API
Pham & Pham, "TOPP-RA", T-RO 2018	⭐⭐⭐	时间最优路径参数化 (M10 预读)
Berscheid & Kroeger, "Ruckig", RSS 2021	⭐⭐	实时三阶轨迹生成 (M10 预读)
MoveIt2 STOMP/CHOMP 文档 moveit.picknik.ai	⭐⭐	集成配置指南

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
CHOMP 优化后仍碰撞	obstacle_cost_weight 太低	1. 增大权重 (10+) 2. 减小 collision_clearance 3. 优化后做硬碰撞检测验证	M04
TrajOpt 不收敛	初始路径太差 / 信赖域过小	1. 用 OMPL 初始化 (M07) 2. 增大初始信赖域 3. 放宽约束容差	M07
STOMP 收敛太慢	噪声方差不匹配 / 温度参数不当	1. 调小 exponentiated_cost_sensitivity 2. 增加 num_rollouts 3. 增加迭代次数	—
GPMP2 因子图求解失败	碰撞因子方差太小导致 Hessian 病态	1. 增大碰撞因子方差 2. 增加 GP 先验方差 3. 检查 GTSAM 的 LM damping	v8 Ch17
链式管线输出不稳定	OMPL 路径拓扑随机不一致	1. 固定 OMPL 随机种子 2. 增加 OMPL 超时 3. 多次规划选最短路径	M07
SDF 梯度方向错误	SDF 分辨率不足或边界处理错误	1. 提高 SDF 网格分辨率到 1-2cm 2. 检查 SDF 生成参数 3. 可视化梯度场	M04
Drake 求解器报 INFEASIBLE	约束矛盾或初始猜测太差	1. 放宽速度/加速度限制 2. 增加路径点数 3. 检查 IK 目标是否可达 (M03) 4. 用 OMPL 路径做初始猜测	M03, M07
轨迹优化后路径过长	平滑性权重过大 / 碰撞权重过小	1. 降低 smoothness_cost_weight 2. 增加 obstacle_cost_weight 3. 检查初始路径拓扑	M07
时间分配导致加速度爆炸	均匀时间步长对不等间距路径不适配	1. 改用弧长等分时间分配 2. 增加路径点密度 3. 降低总时间预设值	M08.7

向后指向: 本章讲的轨迹优化器在 CPU 上运行，典型延迟 50-500ms。对于静态环境、离线规划，这已经足够。但动态环境（人员走动、传送带物体变化）要求**实时重规划**——每个控制周期（10-100ms）重新规划一次。CPU 上的 OMPL + STOMP 无法在此时间预算内完成。M09（GPU加速规划）将介绍如何用 NVIDIA cuRobo 将全栈运动生成压缩到 30ms——GPU 上并行运行数百个优化种子，同时用 CUDA kernel 加速碰撞检测和 FK 计算。另一条路线是 VAMP 的 CPU SIMD 加速——在 Panda 上实现 35 微秒的采样规划。这是从"离线规划"到"实时反应"的代际跨越。

回顾本章的核心知识链：

M08.1: 建立了"采样找拓扑 + 优化提质量"的互补范式
M08.2: 设计了平滑性 + SDF 碰撞的代价函数框架（所有优化器的共同基础）
M08.3: CHOMP 用协变梯度在路径空间中做"正确的梯度下降"
M08.4: TrajOpt 用 SQP + CCD 提供工业级安全保证
M08.5: STOMP 用随机采样-加权替代梯度，处理不可微代价
M08.6: GPMP2 将运动规划统一到因子图框架，与 SLAM 数学同构
M08.7: Drake 提供联合路径-时间优化，时间分配策略影响加速度量级
M08.8: 场景驱动的方案选型，从焊接到码垛各有最优组合

这些知识构成了从"理论推导"到"工业部署"的完整桥梁——M09 将在此基础上加入硬件加速维度。

#	问题	期望答案要点
1	M07 中 RRT-Connect 找到的路径有什么典型缺陷？	路径锯齿状（不光滑）、可能绕远路、不考虑动力学约束（速度/加速度/力矩）、路径长度远非最优
2	签名距离场(SDF)是什么？给定空间中一点 \(x\)，\(\text{SDF}(x)\) 的正负值分别表示什么？	SDF 是到最近障碍物表面的带符号距离。SDF > 0 表示在障碍外（安全），SDF < 0 表示在障碍内（碰撞），SDF = 0 表示在表面上
3	非线性优化中，什么是"局部最优"？梯度下降为什么不能保证找到全局最优？	局部最优是在邻域内代价最小的点，但不一定是全局最小。梯度下降沿负梯度方向下降，但非凸函数可能有多个极小值，梯度为零不代表全局最优
4	什么是 SQP(Sequential Quadratic Programming)？它与普通梯度下降有何区别？	SQP 在每步将非线性问题近似为二次规划(QP)，然后精确求解 QP 得到搜索方向。比梯度下降收敛快（二阶收敛 vs 一阶收敛），且能处理约束
5	GTSAM 中的因子图由什么构成？变量和因子分别对应什么？	变量(Variable)是待优化的量（如位姿、关节角），因子(Factor)是变量间的约束或观测（如里程计、回环）。MAP推断等价于非线性最小二乘

项	含义	计算方法
\(\frac{\partial c}{\partial d}\)	代价对距离的导数	由 \(c(x)\) 的分段定义直接求导（见 Step 1）
\(\frac{\partial d}{\partial x} = \nabla \text{SDF}(x)\)	SDF 梯度	有限差分 or 解析（见 Step 2）
\(\frac{\partial x}{\partial q} = J_j(q)\)	控制点的 Jacobian	Pinocchio `computeFrameJacobian`（见 Step 3）

SLAM 概念	GPMP2 概念	物理含义
位姿 \(x_i\)	关节构型 \(q_t\)	待优化的量
里程计因子	GP 先验因子	路径光滑性（偏好匀速运动）
地标观测因子	碰撞因子	避障（远离障碍物的"观测"）
GPS 因子	起点/终点固定因子	边界条件
MAP 推断	路径优化	最小化负对数后验
Gauss-Newton/LM	同样!	求解算法完全复用