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

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

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

本章知识导航¶

M08 轨迹优化规划器 知识体系
│
├── §1 采样 vs 优化的互补关系 ⭐ ──── 为什么 RRT 路径不能直接用
├── §2 代价函数设计 ⭐⭐ ──── 平滑性 + SDF 碰撞 + 约束
├── §3 CHOMP ⭐⭐ ──── 协变梯度下降，软碰撞约束
├── §4 TrajOpt ⭐⭐⭐ ──── SQP + CCD，硬约束工业安全
├── §5 STOMP ⭐⭐ ──── 无梯度随机优化，不可微代价
├── §6 GPMP2 ⭐⭐⭐ ──── 因子图运动规划，SLAM 无缝过渡
├── §7 时间分配与 Drake 联合优化 ⭐⭐
├── §8 工业选型与 KOMO ⭐⭐
└── 累积项目：轨迹优化模块

前置知识桥接¶

回顾 M07：RRT-Connect 在构型空间中搜索可行路径——概率完备但路径质量差（锯齿、绕远路、无动力学约束）。本章的轨迹优化就是把这条"能用但不好用"的路径变成"平滑可执行"的轨迹。

回顾 M04：SDF（签名距离场）将空间中每个点映射到最近障碍物表面的带符号距离。SDF 的梯度 $\nabla \text{SDF}(x)$ 指向远离障碍物的方向——这是 CHOMP/TrajOpt 碰撞避障的核心输入。

预计阅读时间¶

模式	时间	适合人群
速查	30 分钟	只需了解四大优化器的定位差异和选型
精读	5-6 小时	需要理解 CHOMP/TrajOpt/STOMP/GPMP2 的数学框架
精读 + 实践	10 小时	需要在 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 轨迹优化规划器知识结构:

  ┌─── M08.1 采样 vs 优化互补 (基础概念, 5 min)
  │
  ├─── M08.2 代价函数设计 (数学基础, 30 min)
  │    ├── 平滑性代价 (矩阵 A)
  │    ├── 碰撞代价 (SDF 梯度) ←── M04 碰撞检测
  │    └── 约束代价
  │
  ├─── 四大经典优化器 (核心, 2-3 小时)
  │    ├── M08.3 CHOMP (协变梯度下降)
  │    ├── M08.4 TrajOpt (SQP + CCD)
  │    ├── M08.5 STOMP (无梯度 $\text{PI}^2$)
  │    └── M08.6 GPMP2 (因子图)
  │
  ├─── M08.7 时间分配 + Drake/Crocoddyl (进阶, 1 小时)
  │
  ├─── M08.8 工业选型 + 前沿进展 (工程, 1 小时)
  │
  └─── 下游: M09 (GPU 加速) → M10 (时间参数化) → M12 (执行)

预计阅读时间¶

模式	时间	覆盖内容
速查	30 分钟	四大优化器对比表 (M08.5)、选型决策流程 (M08.8)、API 速查表
速读	3 小时	全部 ⭐-⭐⭐ 内容 + 代码示例 + 工业案例
精读	8 小时	全部内容含 ⭐⭐⭐ 推导（SDF 梯度链式法则、协变梯度、SQP 线性化）
实战	15-20 学时	含 MoveIt2 CHOMP/STOMP 配置、TrajOpt 精读、Drake 练习

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$ 会使路径尽量短（接近直线），但直线可能穿过障碍。惩罚加速度允许路径弯曲绕过障碍，同时保证运动平滑。

不同阶数惩罚的物理含义：

惩罚阶数	数学表达	物理含义	路径特性
0 阶（路径长度）	$\sum \\|q_{t+1} - q_t\\|$	最短路径	可能穿过障碍，不光滑
1 阶（速度）	$\sum \\|q_{t+1} - q_t\\|^2$	最短路径（加权）	接近直线，与 0 阶类似
2 阶（加速度）	$\sum \\|q_{t+1} - 2q_t + q_{t-1}\\|^2$	最小加速度	光滑弯曲，CHOMP/TrajOpt 默认
3 阶（jerk）	$\sum \\|q_{t+2} - 3q_{t+1} + 3q_t - q_{t-1}\\|^2$	最小 jerk	加速度连续，更光滑

"不是X而是Y"句式：平滑性惩罚的选择不是"越高阶越好"——而是要匹配下游执行约束。如果时间参数化器（M10）支持 jerk 约束（如 Ruckig），3 阶惩罚是自然选择；如果只支持速度/加速度约束（如 TOTG），2 阶惩罚足够。过高阶数的惩罚矩阵条件数更大（$\kappa \propto N^{2k}$，$k$ 为阶数），增加优化难度但收益递减。

更高阶: Jerk 最小化

如果惩罚 jerk（加速度的导数 $\dddot{q}$），路径会更加平滑——加速度连续，电机不受冲击。这对应六次差分矩阵。cuRobo (M09) 默认使用 jerk 惩罚 + L-BFGS 优化。cuRobo V2 进一步切换到 B-spline 表示（4 阶 B-spline 天然保证 $C^2$ 连续），使 jerk 的光滑性由参数化本身保证而非惩罚项驱动。

碰撞代价: 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 体素网格。

两种方法的选型准则：

因素	有限差分（体素 SDF）	解析梯度
适用障碍类型	任意（mesh、点云）	仅简单几何原语
精度	受网格分辨率限制	任意精度
计算开销	6 次 SDF 查询/梯度	1 次最近点计算/梯度
内存占用	大（3D 体素网格）	小（几何参数）
GPU 友好度	好（规则内存访问）	极好（纯算术）

工程实践中，两种方法经常混合使用：已知的简单障碍（桌面、墙壁、工具架）用解析梯度——精确且无内存开销；未知的复杂障碍（来自深度相机的点云）用体素 SDF——通过 nvblox (M09) 实时生成 ESDF。cuRobo 的混合策略正是基于这一准则。

SDF 梯度在数值边界处的病态行为：

SDF 梯度在两个场景下会出现问题：

距障碍极近时：$\|x - x_{\text{nearest}}\| \to 0$，梯度方向可能因数值精度而跳变。工程对策：在 $d < \epsilon_{num}$（如 $10^{-6}$ m）时固定梯度方向为上一步的方向。
等距点（medial axis）：空间中到两个或更多障碍物表面等距的点，梯度方向不唯一。工程对策：选择任意一个最近表面点的梯度方向——因为优化器在下一步会重新计算梯度，一步的方向偏差不会导致发散。

跨领域类比：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 的收敛性分析与参数调优 ⭐⭐⭐¶

CHOMP 是一阶优化器（协变梯度下降），其收敛行为高度依赖参数选择。理解收敛性质对工程调优至关重要。

收敛速率分析：

CHOMP 的每步更新为 $\xi \leftarrow \xi - \eta A^{-1} \nabla \mathcal{J}$。由于平滑性代价 $\xi^T A \xi$ 是二次的，梯度为 $2A\xi$，在 $A$-度量下梯度下降的收敛速率由条件数 $\kappa(A)$ 决定：

\[ \text{收敛速率} \propto \frac{\kappa(A) - 1}{\kappa(A) + 1} \]

$A$ 的条件数随路径点数 $N$ 增大而增大（$\kappa(A) \propto N^4$，因为 $A$ 是四阶差分矩阵）。实际含义：路径点越多，纯平滑性优化收敛越慢。碰撞代价的非凸性使问题更加复杂——总代价函数是非凸的，梯度下降只能保证收敛到局部最优。

参数调优指南：

参数	偏大的效果	偏小的效果	推荐范围
`learning_rate` ($\eta$)	更快收敛但可能振荡/发散	更稳定但收敛慢	0.001-0.05
`smoothness_cost_weight`	路径更光滑但可能穿越障碍	路径更贴合障碍但可能不光滑	0.05-0.5
`obstacle_cost_weight`	更安全但路径可能绕远	路径更短但可能轻微碰撞	0.5-5.0
`collision_clearance` ($\epsilon$)	更大安全裕度但路径受限	更紧凑但安全余量小	0.05-0.3 m
`ridge_factor`	更稳定但偏离协变性	更忠于协变梯度但可能不稳定	0.001-0.1
`max_iterations`	更好的优化质量但时间长	更快但可能未收敛	100-500

调优流程（实践经验）：

先用中等参数（上表"推荐范围"中值）跑一次，观察收敛曲线
如果代价下降后振荡 → 减小 learning_rate 至原来的 1/3
如果收敛后仍碰撞 → 增大 obstacle_cost_weight 2 倍
如果路径过度绕远 → 减小 collision_clearance
如果优化后路径有高频波动 → 增大 smoothness_cost_weight

反事实推理：如果把 ridge_factor 设为 0 会怎样？$A$ 矩阵在某些边界条件下可能接近奇异（条件数达到 $10^8$ 以上），Cholesky 分解产生数值误差，协变更新方向偏离理论最优方向。ridge_factor 为 $A$ 的对角线加上一个小正数 $\lambda I$——这是 Levenberg-Marquardt 阻尼的思想在路径空间中的应用。代价是偏离纯协变梯度方向，但换来了数值稳定性。

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): 在梯度下降中注入随机动量，帮助逃逸局部极小。HMC 通过引入动量变量 $p$，在扩展空间 $(\xi, p)$ 中进行 Hamiltonian 动力学模拟——路径沿"等能面"运动，可以跳过浅的局部极小值 - 硬约束支持: 通过投影梯度法处理关节限位——每步更新后将超限关节投影回可行域 - 多分辨率优化: 先在粗分辨率（少路径点）上快速找到大致形状，再在细分辨率上精细优化——类似图像处理中的金字塔方法

CHOMP 的历史意义：

CHOMP 在 2009 年的提出改变了运动规划的研究范式——在此之前，运动规划几乎完全由采样方法（RRT/PRM 家族）主导。CHOMP 证明了连续优化在运动规划中的可行性和有效性，启发了后续的 TrajOpt (2013)、STOMP (2011)、GPMP2 (2018) 等一系列工作。虽然 CHOMP 本身在工业部署中不如 TrajOpt 常见（因为软约束的安全性不足），但它的协变梯度思想被广泛借鉴——cuRobo 的轨迹优化虽然用 L-BFGS 而非协变梯度，但其 jerk 惩罚矩阵的角色与 CHOMP 的平滑性矩阵 $A$ 完全相同。

CHOMP 的可视化教学价值 ⭐¶

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

如何在 MoveIt2 中观察 CHOMP 迭代：

在 CHOMP 配置中设置 animate_path: true 和 animate_endeffector_segment: true
在 RViz 中打开 Planned Path 的 Loop Animation 选项
观察路径在每步迭代中的形状变化——初始路径（直线或 OMPL 锯齿）在碰撞代价场的推力下逐渐远离障碍物
如果启用了 HMC（use_stochastic_descent: true），偶尔会看到路径"跳跃"——这是随机动量注入帮助逃逸局部最优

这种可视化对理解轨迹优化的物理直觉非常有价值——SDF 梯度就像"力场"，碰撞代价场是"势能场"，路径是在势能场中被推动的弹性绳。平滑性代价对应绳子的弹性——限制绳子不会被推得太弯曲。

⚠️ 常见陷阱¶

概念误区: 认为 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 独立栈
教学价值	★★★★★	★★★★☆

TrajOpt 的参数调优与工程最佳实践 ⭐⭐⭐¶

TrajOpt 的 SQP 框架比 CHOMP 的梯度下降有更多可调参数。以下是工程中最常用的调优策略。

信赖域参数调优：

参数	说明	推荐值	调优建议
初始信赖域 $\rho_0$	第一步允许的最大更新幅度	0.5-2.0	初始解质量好 → 小值；差 → 大值
最大信赖域 $\rho_{max}$	信赖域上限	10.0	过大可能导致线性化误差
最小信赖域 $\rho_{min}$	低于此值则终止	1e-4	过小则可能在数值噪声中振荡
缩放因子（成功时）	$\rho \leftarrow \min(\rho \times 2, \rho_{max})$	2.0	保守场景可用 1.5
缩放因子（失败时）	$\rho \leftarrow \rho \times 0.5$	0.5	极保守场景可用 0.25

碰撞约束参数：

参数	说明	推荐值
安全裕度 $\delta_{safe}$	要求的最小碰撞距离	0.02-0.10 m
初始惩罚系数 $\mu_0$	L1 惩罚的初始权重	10-100
最大惩罚系数 $\mu_{max}$	超过此值报告失败	$10^6$
CCD 分辨率	相邻路径点间的连续碰撞检测步数	5-20

TrajOpt 失败时的诊断流程：

TrajOpt 不收敛?
│
├── 信赖域缩到最小? → 初始路径太差或约束不可行
│   ├── 解法 1: 用 OMPL 提供更好的初始路径
│   ├── 解法 2: 放宽安全裕度 δ_safe
│   └── 解法 3: 检查目标构型 IK 是否可达
│
├── 惩罚系数达到 μ_max? → 某些约束根本不可满足
│   ├── 解法 1: 可视化碰撞点，检查是否是球体近似过大
│   ├── 解法 2: 检查自碰撞排除表
│   └── 解法 3: 增加路径点数（路径更灵活）
│
└── 收敛但路径绕远? → 平滑性权重和碰撞权重不平衡
    ├── 解法: 降低 smoothness_weight / 增大碰撞惩罚
    └── 检查: 路径拓扑是否正确（是否绕障碍错误的一侧）

反事实推理：如果 TrajOpt 不用 CCD 而只用离散碰撞检查会怎样？在一个实际工业案例中，焊接机器人的焊枪需要穿过车身内部的窄通道（宽度仅 2 cm）。离散碰撞检查在路径点处无碰撞，但路径点间的轨迹穿过了车身薄钢板（厚 0.8 mm）。TrajOpt 的 CCD 检测到了这个"穿隧"碰撞并产生了约束——迫使优化器生成更保守的路径。这正是 CCD 在安全关键场景中不可替代的原因。

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 任务流水线——把"简化 → 约束检查 → 时间参数化 → 运动规划 → 输出验证"串成有向无环图。

Task Composer 的工程价值：在工业部署中，运动规划不是一个孤立的函数调用——它是一条包含多个步骤的管线：路径简化、碰撞验证、时间参数化、控制器格式转换等。每个步骤可能成功或失败，失败时需要 fallback 策略（如切换到不同的规划器或降低约束）。Task Composer 将这些步骤和 fallback 逻辑编排为 DAG——声明式定义而非硬编码 if-else 分支。这使得管线的修改和调试比传统的过程式代码简单得多。

Tesseract vs MoveIt2 的技术栈对比：

维度	Tesseract	MoveIt2
设计目标	工业部署（确定性、安全性）	通用研究+教学+部署
碰撞检测	Bullet (CCD)	FCL (DCD)
轨迹优化	TrajOpt (硬约束 SQP)	CHOMP/STOMP (软约束)
管线编排	Task Composer (DAG)	PlanningPipeline + Adapters
用户基数	较小（工业自动化公司）	极大（ROS 生态）
许可证	Apache-2.0	BSD-3-Clause

跨领域类比：Tesseract 之于 MoveIt2，类似于 Tesseract OCR 之于 Easyocr——前者更成熟/更适合特定场景（工业/英文），后者更灵活/更社区友好。名字的巧合只是偶然。

⚠️ 常见陷阱¶

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

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

概念误区: 认为 TrajOpt 的硬约束比 CHOMP 的软约束"总是更好"

💡 TrajOpt 的硬约束保证约束满足——但代价是初始解质量要求更高。
   当初始路径深入障碍物内部时，TrajOpt 的 QP 子问题可能不可行（INFEASIBLE），
   必须依赖 L1 惩罚方法逐步松弛，收敛更慢。
   而 CHOMP 的软约束天然允许初始路径穿过障碍——
   碰撞代价梯度会把路径"推出"障碍物，即使初始解很差也能收敛。
   正确选择: 初始解质量好（有 OMPL 预处理） → TrajOpt（更安全）；
            初始解质量差或不确定 → CHOMP 先优化再碰撞验证。

练习¶

[精读] 阅读 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)：不需要梯度的轨迹优化方法。

算法：路径积分（$\text{PI}^2$）的运动规划变体 ⭐⭐⭐¶

STOMP 源自强化学习中的路径积分方法 ($\text{PI}^2$, 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}$（平滑性矩阵的逆）作为协方差——生成的噪声天然光滑。不会产生"锯齿状"扰动。

$\text{PI}^2$ 的信息论推导 ⭐⭐⭐⭐¶

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

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

STOMP 的收敛性与采样效率分析 ⭐⭐⭐¶

STOMP 作为零阶方法，其收敛性质与 CHOMP/TrajOpt 有本质区别。

采样数 $K$ 对收敛速度的影响：

STOMP 每步的更新方向 $\delta\xi$ 是 $K$ 个扰动的加权平均。当 $K \to \infty$ 时，这个平均收敛到代价函数的负梯度方向——即等价于梯度下降。但有限 $K$ 时，更新方向有方差：

\[ \text{Var}(\delta\xi) \propto \frac{1}{K} \cdot \text{Var}(\epsilon_k \cdot \exp(-S_k/\lambda)) \]

$K$ 越大方差越小，更新方向越准确，但每步计算量线性增大。工程经验：$K = 10$ 是一个好的折中——足够探索多个方向，计算量可控。

温度参数 $\lambda$ 的工程选取：

$\lambda$ 值	行为	适用场景
$\lambda \ll \overline{S}$	近乎贪心：只保留最低代价扰动	代价面单调的简单场景
$\lambda \approx \overline{S}$	平衡探索与利用	大多数场景（推荐）
$\lambda \gg \overline{S}$	所有扰动等权：纯随机搜索	极度非凸的代价面（罕见）

其中 $\overline{S}$ 是扰动路径代价的平均值。实践中，先用中等 $\lambda$（如 MoveIt2 默认的 10.0）跑一次，观察代价收敛曲线。如果代价快速下降后停滞——可能是 $\lambda$ 太小，陷入局部最优，试增大 $\lambda$。如果代价下降缓慢且波动大——$\lambda$ 太大，降低 $\lambda$ 使更新方向更确定。

STOMP vs CHOMP 的实证对比（来自 MoveIt2 社区基准测试）：

场景	CHOMP (200步)	STOMP (K=10, 50步)	胜出
简单桌面（3 box）	85 ms, 95% 成功	120 ms, 93% 成功	CHOMP（梯度利用效率高）
复杂杂乱（20 box）	450 ms, 78% 成功	350 ms, 85% 成功	STOMP（随机探索帮助逃逸）
窄通道	600 ms, 60% 成功	500 ms, 72% 成功	STOMP（梯度在窄通道失效）
不可微代价	不收敛	200 ms, 90% 成功	STOMP（唯一选择）

本质洞察：STOMP 的零阶特性在简单凸场景中是劣势（梯度信息白白浪费），但在复杂非凸场景中反而成为优势——梯度可能指向局部最优的"最速下降"方向，而随机扰动有概率跳出局部最优探索全局。这与优化理论中"随机优化在非凸问题上的优势"完全一致。决策原则：代价面接近凸时选 CHOMP/TrajOpt，代价面高度非凸或不可微时选 STOMP。

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。

编程陷阱: STOMP 的温度参数 $\lambda$ 和扰动幅度不匹配

⚠️ 温度参数 λ 和噪声协方差 R 必须协调。
   错误做法: λ 很小（接近贪心）但噪声方差很大。
   现象: 每步只保留一条极端扰动路径，更新方向方差极大，
        路径在迭代间剧烈跳变，代价不下降反而振荡。
   根本原因: λ 小意味着指数加权接近 argmin（只保留最优样本），
            但大方差噪声使"最优样本"每步都在完全不同的方向上。
   正确做法: λ 应与扰动路径代价的标准差同量级。
            MoveIt2 默认 exponentiated_cost_sensitivity = 10.0
            通常足够，只在代价量级变化大时需要调整。

练习¶

[编程] 在 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 推断——如果你学过基础课程中学过的 GTSAM，这几乎就是换了变量名的 SLAM 后端优化。

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

SLAM 因子图 (回顾 GTSAM 基础章节):
  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 与其他优化器的性能对比 ⭐⭐⭐¶

维度	GPMP2	CHOMP	TrajOpt
底层框架	GTSAM (因子图 MAP 推断)	独立实现	Tesseract (SQP)
矩阵结构	稀疏（因子图对应稀疏 Hessian）	带状（有限差分矩阵）	稀疏（QP 约束矩阵）
求解器	Gauss-Newton / LM	Cholesky 回代	QP (OSQP/qpOASES)
增量更新	支持（iSAM2）	不支持	不支持
连续碰撞	GP 插值检查	不支持	CCD（Bullet）
适合人群	SLAM/GTSAM 背景的研究者	运动规划入门者	工业部署工程师

GPMP2 的独特优势在于增量更新——当环境变化时，只需修改受影响的碰撞因子，iSAM2 自动重新消元受影响的变量子集。这比 CHOMP/TrajOpt 从头重新优化快一个数量级。

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

GPMP2 的杀手锏：当障碍物移动时，可以用 iSAM2（回顾 GTSAM 基础中的增量优化——iSAM2 通过 Bayes 树维护因子图的稀疏分解，只重新消元受影响的变量）增量更新因子图——只重新优化受影响的变量，而非从头求解。

// 障碍物移动时增量更新
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" 更像是 "先验正则化"，
   而非 "数据驱动学习"。不需要训练数据。

编程陷阱: 碰撞因子方差 $\sigma_{\text{obs}}$ 设置不当导致 Hessian 病态

⚠️ 碰撞因子的方差 σ_obs 控制因子的"硬度"。
   错误做法: 将 σ_obs 设得极小（如 1e-6）以"强制"无碰撞。
   现象: GTSAM 的 Gauss-Newton/LM 报 "indeterminate linear system"
        或迭代发散，因为 Hessian 中碰撞因子贡献的对角元素达到
        1/σ²_obs = 10^12，与 GP 先验因子的贡献量级差异过大。
   根本原因: 因子图 MAP 推断的 Hessian 是所有因子信息矩阵之和。
            极小的 σ 导致碰撞因子的信息矩阵极大，Hessian 条件数
            爆炸——和 Ceres 中权重过大导致条件数爆炸的问题完全一致。
   正确做法: σ_obs 设为 0.01-0.1 量级（与 SDF 的 ε 同量级），
            通过调整 GP 先验方差 Q_c 来平衡光滑性和避障。

练习¶

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

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

动机：几何路径 $\ne$ 可执行轨迹 ⭐⭐¶

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 的输入。

几何路径到可执行轨迹的完整数据流 ⭐⭐¶

将本章的输出（平滑几何路径）放到完整的运动生成管线中：

M08 输出 (平滑路径) 的后续处理:

  平滑关节角序列 [q₀', q₁', ..., q_N']
       │
       ├── 直接送 M10? ← 需要先检查路径质量
       │
       ├── 碰撞验证 (M04 FCL):
       │   for each pair (q_i, q_{i+1}):
       │     check_collision(interpolate(q_i, q_{i+1}, s))
       │   如果碰撞 → 回到 M08 增大碰撞权重重新优化
       │
       ├── 关节限位检查:
       │   for each q_i, joint j:
       │     assert q_min[j] <= q_i[j] <= q_max[j]
       │   如果超限 → 路径有误, 检查 IK 或约束设置
       │
       └── 路径简化 (可选):
           remove_redundant_waypoints(path, tolerance)
           减少路径点数 → M10 计算更快

这个数据流强调了一个关键点：M08 的输出不能直接执行——它还需要碰撞验证、限位检查和时间参数化。工业级系统（如 Tesseract 的 Task Composer）将这些步骤自动化为 DAG 管线。

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）做最终时间参数化之前，轨迹优化器内部也需要初步的时间分配——这影响加速度惩罚的量级和碰撞检测的时序。

为什么时间分配影响优化质量？

平滑性代价 $\mathcal{J}_{\text{smooth}} = \sum \|\ddot{q}_t\|^2$ 中的加速度是用有限差分近似的：

\[\ddot{q}_t \approx \frac{q_{t+1} - 2q_t + q_{t-1}}{\Delta t^2}\]

$\Delta t$ 出现在分母中。如果 $\Delta t$ 很小（路径点密集），同样的位移差 $q_{t+1} - 2q_t + q_{t-1}$ 会产生极大的加速度值——平滑性代价变得过大，主导整个优化过程，碰撞代价被忽略。反之，$\Delta t$ 很大时平滑性代价太小，路径不够光滑。

定量分析：对 Panda 7-DOF，路径长度约 1.5 rad 总位移，$N = 50$ 个路径点。如果 $T_{total} = 2$ s，则 $\Delta t = 0.04$ s，加速度量级约 $1.5 / (50 \times 0.04^2) \approx 19$ rad/s$^2$——合理。如果 $T_{total} = 0.1$ s，则 $\Delta t = 0.002$ s，加速度量级约 $1.5 / (50 \times 0.002^2) \approx 7500$ rad/s$^2$——荒谬。

反事实推理：如果忽略时间分配对加速度量级的影响，直接用极短的 $T_{total}$ 会怎样？优化器为了降低巨大的加速度代价，会把所有路径点压成一条直线（最小化位移差），完全忽略碰撞代价——路径直接穿过障碍物。这是一个初学者经常遇到但难以诊断的 bug：CHOMP 输出穿障碍的直线路径，实际原因是 $\Delta t$ 设太小导致平滑性代价淹没碰撞代价。

策略 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 在每个控制周期重新规划短期轨迹。

MPC vs 经典轨迹优化的根本区别：

维度	经典轨迹优化 (CHOMP/TrajOpt)	MPC
规划频率	一次性（离线规划）	每个控制周期（在线重规划）
Horizon	完整路径（$s \in [0, 1]$）	短期窗口（$N_{mpc}$ 步，通常 10-50）
状态反馈	无（开环执行规划结果）	有（闭环——实际状态作为下次规划起点）
鲁棒性	低（模型误差累积）	高（持续修正偏差）
计算需求	一次性大计算	每周期小计算（但总量可能更大）

跨领域类比：经典轨迹优化之于 MPC，就像预先写好的演讲稿之于即兴演讲。演讲稿（经典规划）在安静环境中完美——但如果观众提问打断（障碍物移动），演讲者需要即兴应答（MPC 重规划）。最好的方式是两者结合：先准备稿件框架（轨迹优化），然后根据现场反应灵活调整（MPC）。这也是 cuRobo 实时重规划架构（M09.5）的设计哲学。

Contact-Implicit MPC 的特殊价值：在装配任务（peg-in-hole、齿轮啮合）中，接触状态（接触/不接触、滑动/粘滞）是离散变量——传统轨迹优化需要预先指定接触序列。Contact-Implicit MPC 不需要预指定——它在优化中自动发现最优接触序列。这使得机器人能够自主发现"推、滑、插"的操作策略，而非由工程师手动编排。代价是计算量大（每步需要求解混合整数规划或互补约束问题），目前仅适合 10-30 Hz 的低频控制。

⚠️ 常见陷阱¶

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

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

编程陷阱: Drake KinematicTrajectoryOptimization 的碰撞约束采样密度不足

⚠️ Drake 的 AddPathPositionConstraint(min_distance, s) 只在指定的
   路径参数 s 处检查碰撞——不是连续检查。
   错误做法: 只在起点和终点（s=0 和 s=1）添加碰撞约束。
   现象: 优化成功，起终点无碰撞，但路径中段穿过障碍。
   根本原因: NLP 优化器只在约束点满足约束，中间可自由穿越。
            B-spline 的控制点不等于路径上的实际点——
            即使控制点无碰撞，曲线段仍可能穿过障碍。
   正确做法: 在 s ∈ [0, 1] 上均匀采样 15-30 个检查点
            （如示例中的 np.linspace(0, 1, 20)），
            窄通道场景需更密（30-50 点）。
            优化后仍需做最终碰撞验证（FCL 离散检查）。

练习¶

[思考] Crocoddyl 的 DDP 可以同时优化路径和力矩。这相比 CHOMP + TOPP-RA 的两阶段方法有什么优势？有什么劣势？（提示：考虑问题规模和求解速度。）
[工程] 用 Drake 的 KinematicTrajectoryOptimization 对 Panda 7-DOF 规划一条避障轨迹（桌面上放置 2 个立方体障碍）。分别用 TOPP-RA 和 Drake 内置的时间优化（AddDurationCost）对比最终轨迹的总时间和最大关节速度。讨论两种时间分配策略在什么场景下各有优势。
[跨章综合] 对比 M08.7 中 Drake 联合优化与 M08.3 CHOMP + M10 TOPP-RA 两阶段方法的数据流差异。画出两种方案的完整管线图（从 URDF 到可执行轨迹），标注每一步的输入/输出格式和典型延迟。在什么场景下联合优化比两阶段方法有决定性优势？

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 组合在固定环境中最高效。

涂胶/喷涂场景 ⭐⭐¶

需求: 沿复杂 3D 曲面均匀涂覆, 涂层厚度一致
约束: 末端速度均匀 (涂量 ∝ 1/v), 喷枪姿态法向对齐
安全要求: 中等 (无人操作区域)

选型方案:
  路径生成: 离线 CAD/CAM 生成涂覆路径 (非 OMPL)
  姿态约束: 多组约束 IK (M03 pick-ik, 每个路径点保持法向)
  路径优化: Drake (联合路径+姿态+时间优化)
  时间参数化: TOPP-RA (弧长参数化 + 自定义恒速约束)
  理由: 涂胶核心需求是速度均匀而非时间最优;
        Drake 可以同时优化路径形状和速度曲线;
        TOPP-RA 的自定义约束可精确控制末端速度。

不同工业场景的典型管线对比 ⭐⭐¶

场景	核心需求	规划方案	优化方案	时间参数化	关键约束
焊接	安全 + 恒速	PRM*	TrajOpt (CCD)	TOPP-RA (恒速)	碰撞硬约束 + 末端速度
装配	精度 + 接触	RRT-Connect	CHOMP	Ruckig	位置精度 + 力限制
码垛	节拍	PRM*	STOMP	Ruckig	速度最大化
涂胶	均匀性	CAD/CAM	Drake	TOPP-RA	末端速度均匀
人机协作	安全	OMPL	STOMP	Ruckig (在线)	ISO/TS 15066 限速
检测/拍照	覆盖	PRM*	CHOMP	TOTG	视野覆盖 + 关节限位

本质洞察：工业场景选型的核心不是"哪个算法最先进"——而是"哪个算法最匹配约束类型"。焊接需要硬碰撞约束 → TrajOpt；码垛需要速度最大化 → Ruckig；涂胶需要恒速约束 → TOPP-RA。算法选型应从约束类型出发，而非从算法性能出发。

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），适合学术研究但不推荐用于工业部署。

四大优化器的收敛行为深入对比 ⭐⭐⭐¶

理解四种方法在不同场景下的收敛行为，是做出正确选型决策的关键。

收敛速度与初始解质量的关系：

方法	好初始解（OMPL预处理后）	差初始解（直线穿障碍）	无初始解（冷启动）
CHOMP	20-50 步收敛	100-200 步，可能卡局部最优	可以，但拓扑不保证正确
TrajOpt	5-15 步收敛	15-30 步，信赖域自动缩小	可以，CCD 提供安全保证
STOMP	30-60 步收敛	50-100 步，随机探索有概率逃逸	可以，但慢
GPMP2	10-30 步收敛	30-100 步，LM 阻尼自适应	可以，GP 先验提供正则化

关键工程洞察：TrajOpt 在差初始解情况下的鲁棒性最强——信赖域策略在线性化不准确时自动缩小步长，而不是像 CHOMP 那样沿可能错误的梯度方向走大步。这就是为什么 TrajOpt 在工业部署中可以不依赖 OMPL 初始化，直接从直线路径出发。代价是需要 QP 求解器，实现复杂度更高。

局部最优的逃逸策略比较：

轨迹优化都是非凸优化——所有方法都可能陷入局部最优。四种方法的逃逸策略各不相同：

方法	逃逸策略	有效性	计算代价
CHOMP	HMC 随机动量注入	中等（依赖噪声幅度）	低（每步额外一次采样）
TrajOpt	多起点 SQP（不同初始路径）	高（覆盖多种拓扑）	高（线性倍增）
STOMP	随机扰动天然具备（$\text{PI}^2$ 的探索性）	中等	中等（$K$ 个扰动路径）
GPMP2	iSAM2 增量更新 + 多初始化	中等	低（增量更新高效）

跨领域类比：轨迹优化器的局部最优逃逸问题，类似于深度学习中的损失面导航。CHOMP 的 HMC 类似于 SGD 的随机性帮助跳出尖锐极小值；TrajOpt 的多起点类似于多次随机初始化训练；STOMP 的随机扰动类似于高温退火。但与深度学习不同，轨迹优化的"全局最优"有明确的物理含义（最短/最光滑/最安全的路径），局部最优可能意味着路径绕了远路或穿过了不必要的狭窄通道。

2024-2025 轨迹优化前沿进展 ⭐⭐⭐¶

轨迹优化领域在 2024-2025 年出现了几个重要趋势，值得关注。

趋势 1：GPU 并行多种子优化

cuRobo（M09 将详细讨论）的核心策略是在 GPU 上并行运行 512 个轨迹优化种子（每个种子是不同的初始路径），然后选择代价最低、约束满足的最优解。这种"暴力并行"策略在 RTX 4090 上实现了约 15 ms 的轨迹优化延迟——比 CPU 上单次 TrajOpt SQP 还快，同时因为多种子覆盖了更多拓扑，成功率从 90-95% 提升到 99%。

cuRobo V2（2026 年初发布）进一步引入了 B-spline 轨迹表示和动力学感知优化，使其从纯运动学规划扩展到动力学可行的轨迹生成，并开始支持高自由度人形机器人的全身运动生成。

趋势 2：可微仿真器驱动的端到端优化

传统轨迹优化的碰撞梯度来自 SDF，但 SDF 对复杂几何（如铰接物体、布料、绳索）支持有限。2024-2025 年的新方向是用可微仿真器（如 Warp、DiffTaichi、MuJoCo MJX）提供碰撞和接触的梯度——使轨迹优化器能直接处理接触丰富的操作任务。

工具	可微碰撞支持	适用场景
Warp (NVIDIA)	粒子/mesh 可微碰撞	cuRobo 后续集成
DiffTaichi	自定义可微物理	研究原型
MuJoCo MJX	XLA 可微仿真	RL + 轨迹优化混合
Drake + AutoDiff	接触互补约束梯度	精密装配

趋势 3：学习+优化混合方法

纯优化方法（CHOMP/TrajOpt/STOMP）从零求解每个问题实例；纯学习方法（运动策略网络）一次推理但泛化性有限。2024-2025 年出现了混合范式——用学习模型提供高质量初始解，再用优化器精细调整：

MotionPolicy Transformer：预训练 Transformer 输出初始轨迹 → TrajOpt 精细优化（初始解质量好 → 优化步数从 30 步降到 5 步）
L2O (Learning to Optimize)：学习优化器的超参数（步长、信赖域大小）以加速收敛
Neural Motion Planning：神经网络直接预测无碰撞轨迹，在高度杂乱或部分可观测环境中，某些学习方法已经在成功率上超过 cuRobo

这些进展不意味着经典优化器会被替代——经典方法在约束满足、安全保证、可解释性上的优势在工业部署中不可替代。更可能的演化方向是"学习提供初始猜测 + 优化保证约束满足"的协同架构。

"不是X而是Y"句式：2024-2025 年的轨迹优化进展不是"更好的优化算法"——CHOMP/TrajOpt/STOMP 的数学框架在过去十年基本稳定。真正变化的是计算范式（GPU 并行化）、梯度来源（可微仿真器替代手写 SDF 梯度）、和初始化策略（学习模型替代随机/直线初始化）。理解这三个维度的变化，比追逐单一新算法更有价值。

选型决策流程 ⭐⭐¶

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

特殊情况:
  接触丰富操作 → Drake (联合优化)
  SLAM 背景/增量更新 → GPMP2 (因子图)
  学术研究/原型验证 → KOMO/rai 或 Drake
  GPU可用且需要实时 → cuRobo (并行多种子)
  学习+优化混合 → MotionPolicy + TrajOpt

全栈规划管线延迟分解 ⭐⭐¶

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

完整规划管线 (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$ 使约束满足。
[跨章综合] 综合 M05（QP/NLP 建模）+ M06（自动微分）+ M08（本章），设计一个"可微轨迹优化器"原型：(1) 用 CasADi 构建 7-DOF 机械臂的轨迹优化 NLP，包含碰撞约束（M08.2 的 SDF 代价）和动力学约束（M05 的力矩限）；(2) 用 CasADi 自动微分提供梯度（替代 M08.3 CHOMP 的手写协变梯度）；(3) 对比 CHOMP 手写梯度和 CasADi 自动梯度的开发效率和运行性能。

M08.8b 前沿进展：Differentiable Trajectory Optimization ⭐⭐⭐⭐¶

CasADi + AD 驱动的可微轨迹优化¶

传统轨迹优化器（CHOMP/TrajOpt/STOMP）的代价函数和梯度需要手工设计——这限制了对新约束类型的快速实验。可微轨迹优化（Differentiable Trajectory Optimization）的核心思想是：用自动微分框架自动推导所有梯度，让研究者只需定义代价函数的前向计算。

CasADi + Ipopt 的组合是当前最实用的可微轨迹优化框架。与 CHOMP 等方法的对比：

维度	CHOMP/TrajOpt（手写梯度）	CasADi + Ipopt（自动梯度）
新约束开发速度	慢（需手推梯度）	快（只需前向计算）
运行性能	快（手工优化）	中（符号展开开销）
碰撞梯度	SDF 梯度（手写）	SDF 可微或链式法则
动力学约束	需手动组装	Pinocchio CasADi 自动生成
适用场景	成熟工业部署	研究原型、快速迭代

GPMP2 on Lie Groups ⭐⭐⭐⭐¶

GPMP2 的经典版本在 $\mathbb{R}^n$ 空间定义 GP 先验。但机械臂的构型空间包含旋转关节（$S^1$ 拓扑），直接在 $\mathbb{R}^n$ 上定义 GP 会在 $\pm \pi$ 边界处产生不连续性。GPMP2 on Lie Groups（Mukadam et al., WAFR 2018）将 GP 先验推广到 Lie 群上——用 SE(3) 的对数映射定义差异、用指数映射做插值，确保了旋转空间中的拓扑正确性。

本质洞察：可微轨迹优化不是"更好的 CHOMP"——它改变的是工程工作流。当你需要快速试验一个新的代价函数（如工具朝向约束、可操作度最大化、能耗最小化），手写梯度需要数天开发+调试，CasADi 只需定义前向计算然后让 AD 自动搞定梯度。这种开发效率的差异在研究阶段尤为关键。

Optimization-based Motion Primitives ⭐⭐⭐¶

运动基元（Motion Primitives）是预计算的短轨迹片段库，在线阶段通过拼接基元快速生成完整轨迹。传统基元用样条或 DMP 参数化，最新进展将轨迹优化嵌入基元生成——在离线阶段为常见场景预优化一组高质量基元，在线阶段用图搜索快速拼接。这结合了轨迹优化的质量和图搜索的速度。

运动基元方法的三种范式：

范式	代表方法	离线计算	在线计算	适用场景
DMP 基元	Dynamic Movement Primitives	从示教录制基元	参数调整	模仿学习
格点基元	State Lattice / Motion Primitive Library	穷举离散化构型空间生成基元	图搜索 + 拼接	固定环境
优化基元	TrajOpt 预优化 + 在线选择	对典型场景预优化高质量基元	最近基元匹配 + 局部修正	重复性工业任务

反事实推理：如果不用运动基元而每次从头规划会怎样？在码垛场景中，机器人每次拾放都规划完整路径（RRT-Connect + STOMP + TOPP-RA）——即使环境和目标几乎相同，也需要 200+ ms。用运动基元库：首次规划时预优化并存储高质量轨迹片段，后续拾放只需从库中检索最近的基元（< 1 ms）并微调（10 ms）——总时间从 200+ ms 降到 10 ms 级别。这在高节拍码垛（每秒 2+ 个箱子）中是决定性的优势。

本章常见误解汇总¶

误解	正确理解
"轨迹优化可以替代采样规划"	轨迹优化是局部优化器，需要采样规划提供拓扑正确的初始解
"CHOMP 保证无碰撞"	CHOMP 用软约束处理碰撞，不保证消除碰撞。优化后需碰撞验证
"TrajOpt 是最好的优化器"	TrajOpt 在安全关键场景最佳，但实现复杂度高、需 QP 求解器
"STOMP 因为没有梯度所以很差"	STOMP 在不可微代价场景中是唯一选择，在非凸场景中探索性更强
"GPMP2 是用 GP 做机器学习"	GPMP2 的 GP 是路径先验（正则化），不是数据驱动学习
"轨迹优化器输出就是轨迹"	输出是几何路径（无时间信息），还需 M10 的时间参数化
"GPU 加速让所有优化器都快"	GPU 加速的核心是碰撞检测和批量 FK，算法逻辑本身不并行
"代价函数越复杂越好"	代价函数应简洁——过多项导致梯度冲突、调参困难
"更多路径点总是更好"	路径点过多增加优化维度、减慢收敛，且不一定提升路径质量
"Drake 在所有场景都比 CHOMP/TrajOpt 好"	Drake 适合联合优化和接触丰富操作，但在 MoveIt2 生态内集成不如 CHOMP/STOMP 方便

术语速查表¶

术语	英文	一句话定义
轨迹优化	Trajectory Optimization	在代价函数指导下优化路径形状（平滑性、碰撞、约束）
协变梯度	Covariant Gradient	在由平滑性矩阵定义的黎曼度量下的梯度方向
签名距离场	Signed Distance Field, SDF	空间中每点到最近障碍表面的带符号距离
序贯二次规划	Sequential Quadratic Programming, SQP	逐步线性化非线性约束，求解 QP 子问题的优化方法
信赖域	Trust Region	限制每步更新幅度的约束，保证线性化近似质量
连续碰撞检测	Continuous Collision Detection, CCD	检测路径点之间（非仅路径点处）的碰撞
路径积分	Path Integral, $\text{PI}^2$	STOMP 的数学基础——用采样-加权替代梯度计算
因子图	Factor Graph	GPMP2 的底层表示——变量和约束的二部图
L1 惩罚	L1 Penalty	TrajOpt 将硬约束转化为代价的方法，推动精确约束满足
增广拉格朗日	Augmented Lagrangian	KOMO 使用的优化方法，交替更新路径和乘子
B-spline	B-spline	Drake 和 cuRobo V2 使用的轨迹参数化，天然保证连续性

API 速查表¶

API/工具	头文件/模块	核心调用
MoveIt2 CHOMP	`chomp_motion_planner`	YAML 配置 `chomp:` 参数
MoveIt2 STOMP	`stomp_motion_planner`	YAML 配置 `stomp:` 参数
Tesseract TrajOpt	`trajopt_sco/solver_interface`	`BasicTrustRegionSQP::optimize()`
GPMP2	`gpmp2` (GTSAM 扩展)	`GaussNewtonOptimizer::optimize()`
Drake TrajOpt	`pydrake.all.KinematicTrajectoryOptimization`	`Solve(prog)`
Pinocchio FK/Jacobian	`pinocchio/algorithm/kinematics.hpp`	`computeFrameJacobian()`

版本信息速查¶

工具	推荐版本	许可证	关键依赖
MoveIt2 CHOMP	MoveIt2 Humble+ 内置	BSD-3-Clause	MoveIt2
MoveIt2 STOMP	MoveIt2 Humble+ 内置	BSD-3-Clause	MoveIt2
Tesseract TrajOpt	最新 main 分支	Apache-2.0	Bullet, Eigen, Boost
GPMP2	0.3+	BSD-3-Clause	GTSAM 4.x
Drake	最新 nightly 或 stable	BSD-3-Clause	Eigen, SNOPT/IPOPT
KOMO/rai	最新 main 分支	MIT	独立
Pinocchio	3.x	BSD-2-Clause	Eigen, Boost
CasADi	3.6+	LGPL-3.0	独立
cuRobo	0.8+ (V2)	以仓库 LICENSE 为准	CUDA, PyTorch

研究实践建议¶

层次	建议
入门（1-2 周）	在 MoveIt2 中配置 OMPL → STOMP 链式管线，观察路径优化效果
进阶（2-4 周）	精读 CHOMP 的协变梯度推导，手动实现 2D 版本
深入（1-2 月）	用 Tesseract 栈部署 TrajOpt，在工业场景（焊接/装配）中对比
研究（2+ 月）	用 CasADi + Pinocchio 构建可微轨迹优化器，集成自定义约束

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)	增广拉格朗日学术框架	学术研究参考

本章常见误解汇总¶

误解	正确理解	相关章节
"轨迹优化可以替代采样规划"	轨迹优化依赖初始路径——没有 RRT 给的拓扑正确初值，优化器可能收敛到碰撞轨迹	§1
"CHOMP 保证无碰撞"	CHOMP 用软约束处理碰撞——优化后必须做独立的硬碰撞检测验证	§3
"TrajOpt 的 SQP 保证全局最优"	SQP 只找局部最优；不同初始路径可能收敛到不同局部最优	§4
"STOMP 很慢因为是随机方法"	STOMP 的随机采样可以并行化；GPU 上的 STOMP 变体可以很快	§5
"GPMP2 只是另一个优化器"	GPMP2 将运动规划统一到因子图框架，可以用 iSAM2 做增量重规划	§6
"均匀时间分配足够好"	路径点间距不等时，均匀时间导致加速度爆炸——应用弧长等分	§7
"所有场景用 STOMP 就行"	安全关键场景（焊接/装配）必须用 TrajOpt 的硬约束；STOMP 无安全保证	§8

术语速查表¶

术语	英文	一句话定义
CHOMP	Covariant Hamiltonian Optimization for Motion Planning	在路径空间用协变梯度下降做轨迹优化
TrajOpt	Trajectory Optimization via SQP	用序列二次规划 + 连续碰撞检测做轨迹优化
STOMP	Stochastic Trajectory Optimization for Motion Planning	用随机采样-加权替代梯度做无梯度轨迹优化
GPMP2	Gaussian Process Motion Planner 2	用 GP 先验 + 因子图 (GTSAM) 做运动规划
SDF	Signed Distance Field	空间中每点到最近障碍物的带符号距离
CCD	Continuous Collision Detection	连续碰撞检测——检查运动过程中而非仅起终点的碰撞
SQP	Sequential Quadratic Programming	每步将非线性问题近似为 QP 并精确求解
KOMO	K-Order Markov Optimization	Toussaint 的增广拉格朗日学术框架
ESDF	Euclidean Signed Distance Field	欧几里德符号距离场，voxel 网格上的 SDF

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

机械臂全栈项目进度: - 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	⭐⭐	集成配置指南
Ichter et al. (2018) "Learning Sampling Distributions for Robot Motion Planning"	⭐⭐⭐	学习增强的采样规划——OMPL 初值优化
Fishman et al. (2023) "Motion Policy Networks" (MPiNets)	⭐⭐⭐	神经网络直接输出轨迹，替代 RRT+优化
Chi et al. (2023) "Diffusion Policy"	⭐⭐⭐⭐	扩散模型生成多模态运动规划
Tesseract Motion Planning 文档 (`tesseract-robotics/tesseract_planning`)	⭐⭐⭐	TrajOpt 的工业开源实现

轨迹优化的本质——一个统一的思考框架 ⭐⭐⭐¶

学完本章后，你应该能用以下框架分析任何新出现的轨迹优化方法：

三个核心问题：

搜索空间是什么？ 路径点序列 (CHOMP/STOMP) / B-spline 控制点 (Drake) / GP 后验 (GPMP2)
碰撞处理是软还是硬？ 软约束 (CHOMP/STOMP/GPMP2) 性能好但不保证安全 / 硬约束 (TrajOpt) 安全但可能不可行
搜索方向怎么来？ 梯度 (CHOMP/TrajOpt) 需要可微的代价函数 / 随机采样 (STOMP) 不需要梯度但收敛慢

当你遇到新的优化器时（如 cuRobo 的 L-BFGS 变体、Aligator 的 ProxDDP），用这三个问题快速定位它的能力和局限：

问题	cuRobo (M09)	Aligator	你的新方法?
搜索空间	B-spline 控制点	状态+控制序列	?
碰撞处理	软约束 (球体近似)	硬约束 (ProxDDP)	?
搜索方向	梯度 (L-BFGS)	梯度 (DDP 变体)	?
特殊优势	GPU 并行数百种子	约束 DDP + 接触	?

本质洞察：轨迹优化不是"选一个方法用到底"——它是一个工程决策问题，需要根据约束类型（软/硬）、计算预算（CPU/GPU）、安全要求（工业/研究）和场景复杂度（开放/窄通道）做出权衡。本章的核心价值不是教你"CHOMP 怎么用"，而是教你"面对新场景时如何选择和组合工具"。

首尾呼应：回顾本章开头 M07 RRT-Connect 的锯齿路径——现在你知道了四种"把锯齿路径变光滑"的方法，每种方法的数学原理、工程特性和适用场景。更重要的是，你知道了"为什么不存在万能的优化器"——因为碰撞处理的软/硬、搜索方向的确定性/随机性是两个无法同时最优的维度。工程的艺术在于权衡。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
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	GTSAM 基础章节
链式管线输出不稳定	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
CHOMP 在窄通道中振荡不收敛	SDF 梯度在窄通道两侧方向相反，导致路径来回摆动	1. 降低步长 $\eta$ 2. 增加平滑性权重 3. 用 OMPL 穿过窄通道后再优化	M08.3, M07
STOMP rollout 全部碰撞	噪声方差过大或障碍物包围	1. 降低噪声标准差 2. 增加 rollout 数量 3. 用 OMPL 初始化（而非直线）	M08.5
TrajOpt SQP 信赖域过小导致不收敛	碰撞约束梯度突变	1. 增大初始信赖域 2. 放宽碰撞安全裕度 3. 增加路径点密度	M08.4
GPMP2 GTSAM 后端报 IndeterminateLinearSystem	GP 先验方差与碰撞因子方差差异过大	1. 增大 GP 先验方差 $\sigma_\text{GP}^2$ 2. 增大碰撞因子方差 $\sigma_\text{obs}^2$ 3. 增大 GTSAM LM damping	M08.6
链式管线 OMPL → STOMP 输出路径抖动	OMPL 的随机路径拓扑变化导致 STOMP 收敛到不同局部最优	1. 固定 OMPL 随机种子 2. 对 OMPL 结果做一次快速 shortcutting 3. 多次规划取最优	M07, M08.5
Drake 轨迹优化 SNOPT 返回 INFEASIBLE	速度/加速度限制与目标位姿不兼容	1. 放宽速度限制 2. 增加 B-spline 控制点数 3. 增加允许的执行时间	M08.7

工业部署建议¶

双通道验证：轨迹优化输出必须经过独立的碰撞检测验证（FCL/Coal），不能仅依赖优化器的碰撞代价
超时保护：为轨迹优化设置硬超时（如 2 秒），超时后返回 OMPL 原始路径（安全但不光滑）
监控指标：记录每次优化的迭代次数、最终碰撞距离、路径长度，用于生产环境的质量监控
场景预测试：新场景上线前，用 100+ 随机起终点做批量测试，确认成功率 > 95%

向后指向: 本章讲的轨迹优化器在 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: 场景驱动的方案选型，从焊接到码垛各有最优组合
M08.8b: 可微轨迹优化和 2024-2025 前沿趋势——GPU 并行、可微仿真、学习+优化混合

跨章综合练习 ⭐⭐⭐¶

题目 1：综合 M04（碰撞检测）+ M07（OMPL）+ M08（轨迹优化），实现完整的"采样→优化"管线：

用 M04 的 FCL/Coal 为桌面场景生成 SDF（3 个 box 障碍物）
用 M07 的 OMPL RRT-Connect 为 Franka Panda 规划从起始到目标的路径
实现简化版 CHOMP：用 SDF 梯度做碰撞代价，用有限差分矩阵做平滑性代价，做 50 次梯度下降迭代
可视化优化前后的路径对比（关节空间和笛卡尔空间）
测量路径长度、最大加速度、碰撞距离的改善

题目 2：综合 M05（QP）+ M08（轨迹优化），理解 TrajOpt 的 SQP 内部：

将 M08.4 的 TrajOpt 数学框架手动展开为 QP 子问题
用 M05 的 ProxQP 求解一步 SQP
比较你的手动 SQP 与 MoveIt2 CHOMP 的结果差异

轨迹优化调试的系统化方法 ⭐⭐¶

轨迹优化出问题?
│
├── 1. 优化后仍碰撞
│   ├── 检查 SDF 分辨率 (应 ≤ 2cm)
│   ├── 增大碰撞权重 (CHOMP) / 减小安全裕度 (TrajOpt)
│   ├── 优化后做硬碰撞检测验证
│   └── 检查 SDF 是否覆盖所有障碍物
│
├── 2. 优化器不收敛
│   ├── 检查初始路径质量 (是否穿过障碍物)
│   ├── 增加迭代次数
│   ├── 调整步长 / 信赖域大小
│   └── 用 OMPL 提供更好的初始路径
│
├── 3. 优化后路径质量差 (仍然绕远)
│   ├── 检查初始路径的拓扑 (是否绕了障碍物错误的一侧)
│   ├── 轨迹优化不改变拓扑——需要更好的采样规划初值
│   └── 多次规划选最短路径
│
├── 4. 优化时间过长
│   ├── 减少路径点数
│   ├── 降低 SDF 分辨率
│   ├── 减少迭代次数 (接受次优解)
│   └── 考虑 GPU 加速 (M09)
│
└── 5. MoveIt2 链式管线不工作
    ├── 检查 STOMP/CHOMP 插件是否正确安装
    ├── 检查 response adapter 配置
    ├── 查看 MoveIt2 日志 (ros2 launch --log-level=debug)
    └── 确认 OMPL 阶段确实输出了可行路径

跨领域类比：轨迹优化的调试逻辑与深度学习训练的调试非常相似——loss 不下降时，你会检查学习率（步长）、数据质量（初始路径）、网络架构（优化器选择）、正则化（平滑性权重）。同样的系统化思维可以直接迁移到轨迹优化调试中。

API 速查表¶

MoveIt2 CHOMP 参数¶

参数	含义	推荐值
`planning_time_limit`	最大优化时间	5.0 s
`max_iterations`	最大迭代次数	100
`obstacle_cost_weight`	碰撞代价权重	10.0
`smoothness_cost_weight`	平滑性代价权重	1.0
`ridge_factor`	Hessian 正则化	0.001
`collision_clearance`	碰撞安全距离	0.02 m

MoveIt2 STOMP 参数¶

参数	含义	推荐值
`num_iterations`	最大迭代次数	40
`num_rollouts`	每次迭代的随机 rollout 数	20
`exponentiated_cost_sensitivity`	代价敏感度 (温度)	10.0
`max_velocity_scaling`	最大速度缩放	1.0

Drake TrajOpt API¶

from pydrake.all import (
    KinematicTrajectoryOptimization,
    BsplineTrajectory,
    AddPositionConstraint,
    AddDurationConstraint,
)
# 创建 B-spline 轨迹优化
trajopt = KinematicTrajectoryOptimization(
    plant, num_positions, num_control_points=20,
    spline_order=4)
# 添加约束
trajopt.AddDurationCost(1.0)
trajopt.AddPathLengthCost(1.0)
trajopt.AddPositionBounds(lb, ub)
trajopt.AddVelocityBounds(v_lb, v_ub)
# 求解
result = Solve(trajopt.prog())

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

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
M09 GPU加速	直接承接	cuRobo 的 GPU 轨迹优化是本章 CPU 优化的加速版
M10 时间参数化	下游	本章输出几何路径，M10 赋予时间参数使其可执行
M14 MoveIt2集成	工程应用	本章的 CHOMP/STOMP 在 MoveIt2 中作为链式管线使用

研究实践建议¶

入门级¶

在 MoveIt2 中配置 OMPL → STOMP 链式管线，体验"采样+优化"互补效果
用 CHOMP 的 2D 可视化示例理解协变梯度下降

进阶级¶

实现一个简单的 CHOMP：固定端点轨迹 + 弧长参数化 + SDF 碰撞代价 + 梯度下降
对比 TrajOpt vs STOMP 在有窄通道的场景中的成功率和轨迹质量

研究级¶

评估 GPMP2 的增量重规划 (iSAM2) 在动态环境中的能力
探索 Drake KinematicTrajectoryOptimization 的联合路径-时间优化
研究 学习增强的轨迹初始化——用 MPiNets/Diffusion Policy 生成初值替代 RRT
实现 Riemannian CHOMP——在 SE(3) 流形上做协变梯度下降，解决末端笛卡尔约束

常见选型错误与纠正¶

错误决策	后果	正确做法
"直接用 STOMP 不用 OMPL 初值"	从直线初始路径出发可能收敛到碰撞拓扑	用 OMPL 提供拓扑正确的初值
"CHOMP 碰撞权重设为 1000"	平滑性完全被忽略，路径呈锯齿状	碰撞权重 / 平滑性权重保持在 5-20 范围
"优化后不做碰撞验证就执行"	2-5% 概率路径仍碰撞	必须做独立的 FCL/Coal 碰撞验证
"所有场景用同一套参数"	窄通道场景可能完全失败	按场景复杂度分级设置参数
"用 TrajOpt 做人机协作"	TrajOpt 离线优化延迟太高（400ms+）	动态环境用 cuRobo (30ms) 或 GPMP2 增量

从 M07 到 M08 的知识递进检验¶

回答以下问题验证你的理解是否正确：

RRT-Connect 找到的路径为什么不能直接执行？（答：锯齿状导致 jerk 无穷大，电机震荡、减速器磨损）
CHOMP 和 TrajOpt 对碰撞的处理有什么本质区别？（答：CHOMP 用代价函数惩罚碰撞[软约束]，TrajOpt 用 QP 不等式禁止碰撞[硬约束]）
STOMP 不需要梯度，为什么还需要 SDF？（答：STOMP 需要评估碰撞代价——SDF 提供距离值作为代价，但不需要 SDF 的梯度方向）
GPMP2 如何实现增量重规划？（答：通过 GTSAM 的 iSAM2 增量因子图更新——新增/删除碰撞因子后只更新受影响的变量，而非重新求解整个因子图）
为什么 cuRobo 的 GPU 并行优化能缓解局部最优问题？（答：并行运行数百个不同初始路径的优化种子，取最终代价最小的结果。更多种子覆盖更多拓扑，降低陷入差局部最优的概率）
CHOMP 的"协变"梯度和"普通"梯度有什么区别？（答：协变梯度用平滑性矩阵 $A^{-1}$ 作为度量，使更新方向自然保持光滑；普通梯度可能产生锯齿状更新，需要更多后处理）
为什么 SDF 分辨率必须小于机器人最小 link 半径的一半？（答：分辨率过大时，小于一个体素的障碍物的 SDF 值被平滑化，优化器"看不到"该障碍物，导致碰撞）

四大优化器深度对比决策表 ⭐⭐⭐¶

以下是基于实际工程经验的综合对比，帮助你在具体场景中做出选择：

算法特性对比¶

维度	CHOMP	TrajOpt	STOMP	GPMP2
碰撞处理	软约束 (代价函数)	硬约束 (SQP 不等式)	软约束 (代价函数)	软约束 (因子)
碰撞检测	SDF + 梯度	SDF + CCD	SDF (无需梯度)	SDF + GP 插值
需要梯度	是 ($\nabla \text{SDF}$)	是 (Jacobian)	否	是
初始路径	直线/OMPL	OMPL (推荐)	直线/OMPL	直线
收敛速度	中 (一阶)	快 (二阶 SQP)	慢 (随机)	中 (GTSAM LM)
安全保证	无 (需后验检查)	有 (硬约束+CCD)	无	无
动力学约束	无 (仅几何)	可扩展	可扩展	可扩展 (因子)
并行化	困难	困难	容易 (多rollout)	中等
MoveIt2 集成	✅ 官方插件	❌ (Tesseract)	✅ 官方插件	❌ (独立)

典型工业场景选型¶

场景	首选	备选	理由
焊接 (精度要求高、安全关键)	TrajOpt	Drake	需要硬碰撞约束 + CCD 保证焊枪不碰工件
码垛 (简单路径、高节拍)	OMPL + STOMP	OMPL + CHOMP	无窄通道，STOMP 不需要 SDF 梯度、调参少
装配 (窄间隙、高精度)	TrajOpt	CHOMP	窄间隙需要硬约束保证无碰撞
人机协作 (动态环境)	cuRobo (M09)	GPMP2 (增量)	需要实时重规划
学术研究 (SLAM 背景)	GPMP2	KOMO	因子图框架与 SLAM 同构，复用 GTSAM 知识
快速原型 (不确定需求)	OMPL + STOMP	CHOMP	STOMP 调参少、不需要 SDF 梯度

2025 轨迹优化前沿趋势 ⭐⭐⭐¶

趋势 1：GPU 并行轨迹优化

cuRobo (NVIDIA, 2023) 将轨迹优化搬到 GPU 上——并行运行数百个优化种子（不同初始路径），取最优结果。这解决了局部最优问题（多初值覆盖更多拓扑），同时利用 GPU 的并行性将总延迟压缩到 30 ms。详见 M09。

趋势 2：学习增强的轨迹优化

2024-2025 年的多项工作用神经网络预测"好的初始路径"，替代 RRT 作为轨迹优化的初值：

工作	方法	优势
Motion Policy Networks (MPiNets)	点云输入 → 直接输出轨迹	亚秒级端到端，无需 RRT
M$\pi$Nets + 优化	神经网络初值 → STOMP 后处理	结合学习速度和优化安全性
Diffusion Policy for Planning	扩散模型生成轨迹分布	多模态解（绕左/绕右）

反事实推理：如果不用 RRT 而是用神经网络生成初始路径，轨迹优化会怎样？ - 优势：网络推理比 RRT 快 10-100 倍（~10 ms vs ~200 ms），且输出路径已经接近光滑 - 风险：网络可能在训练分布外的场景失败，产生碰撞初值——此时 TrajOpt 的 CCD 硬约束是最后的安全网 - 结论：学习初值 + 优化后处理是当前最有前景的方向，但安全关键场景仍需传统碰撞检查

趋势 3：可微仿真与轨迹优化融合

Drake、MuJoCo MJX、Genesis 等可微仿真器使得轨迹优化可以穿透接触事件——优化器不仅优化自由运动，还优化接触力和接触模式。这对装配、插入等接触丰富操作至关重要。

本质洞察：轨迹优化的本质是"在约束满足的前提下，找到最优的运动方案"。不同的优化器（CHOMP/TrajOpt/STOMP/GPMP2）提供了不同的约束处理方式和搜索策略。未来的趋势是：(1) GPU 加速搜索空间的覆盖范围，(2) 学习模型提供更好的初始猜测，(3) 可微仿真将搜索空间扩展到接触域。但核心数学框架——代价函数 + 约束 + 梯度/采样——不会改变。

四大优化器的数学统一视角 ⭐⭐⭐⭐¶

从优化理论的角度，四大优化器可以用统一的数学框架理解：

\[\min_{\xi} \underbrace{\mathcal{J}_{\text{smooth}}(\xi)}_{\text{平滑性先验}} + \underbrace{\mathcal{J}_{\text{obs}}(\xi)}_{\text{碰撞代价}} \quad \text{s.t.} \quad \underbrace{c(\xi) \leq 0}_{\text{约束（可选）}}\]

方法	平滑性项	碰撞项	约束	优化策略
CHOMP	$\xi^T A \xi$	$\int \text{body}(\nabla \text{SDF}) \,ds$	无 (软)	协变梯度下降
TrajOpt	速度/加速度惩罚	$\text{SDF}(x) \geq d_{safe}$	硬	SQP
STOMP	$\xi^T R^{-1} \xi$	任意 $c(\xi)$	无 (软)	随机采样-加权
GPMP2	GP 先验 $\mathcal{N}(\mu, K)$	SDF 似然	无 (软)	MAP 推断 (LM)

关键差异只在两个维度：(1) 碰撞处理是软约束还是硬约束，(2) 搜索方向是基于梯度还是随机采样。理解这两个维度，你就能在任何新的优化器出现时快速定位它的能力和局限。

版本信息速查¶

工具	版本	安装	许可证
MoveIt2 CHOMP	Humble/Jazzy/Kilted	`apt install ros-${DISTRO}-moveit-chomp-*`	BSD-3
MoveIt2 STOMP	Humble/Jazzy/Kilted	`apt install ros-${DISTRO}-moveit-planners-stomp`	BSD-3
TrajOpt (Tesseract)	0.23.x	源码编译	Apache-2.0
GPMP2	0.3.x	源码编译 (GTSAM 依赖)	BSD-2
KOMO (rai)	最新	源码编译	MIT
Drake	v1.52+	`pip install drake`	BSD-3
cuRobo	0.7.x	`pip install curobo`	NVIDIA License
Aligator	0.x	源码编译	BSD-3
Crocoddyl	2.1.x	`conda install crocoddyl`	BSD-3

版本注意：MoveIt2 的 CHOMP/STOMP 插件 API 在 Humble、Jazzy、Kilted 之间有细微差异（如参数名称、适配器接口）。部署前请核对你使用的 ROS2 发行版对应的 MoveIt2 版本文档。Tesseract (TrajOpt) 独立于 MoveIt2 维护，版本号和 API 独立演进。Drake 的 KinematicTrajectoryOptimization 在 2025 年增加了 B-spline 阶数选择和更多约束类型支持。

符号表¶

符号	含义	首次出现
$\xi$	轨迹（路径点序列），$\xi = [q_0, q_1, \ldots, q_N]$	§2
$A$	平滑性矩阵（有限差分加速度的内积矩阵）	§2, §3
$\mathcal{J}_{\text{smooth}}$	平滑性代价 $= \xi^T A \xi$	§2
$\mathcal{J}_{\text{obs}}$	碰撞代价（SDF 积分）	§2
$\text{SDF}(x)$	签名距离场值（正=安全，负=碰撞）	§2
$\nabla \text{SDF}$	SDF 梯度（指向远离障碍方向）	§2, §3
$d_j(q_t)$	第 $j$ 个 link 在配置 $q_t$ 下到最近障碍的距离	§4
$\delta_{\text{safe}}$	安全距离裕度	§4
$\Delta\xi$	SQP 中的路径增量	§4
$H_k$	SQP 第 $k$ 次迭代的 Hessian 近似	§4
$R^{-1}$	STOMP 的噪声协方差逆（也是平滑性矩阵）	§5
$K(s, s')$	GPMP2 的 GP 核函数	§6

CHOMP/TrajOpt/STOMP 性能实测对比 ⭐⭐⭐¶

以下数据来自 MoveIt2 Kilted 环境，Franka Panda 7-DOF，桌面抓取场景（1 个目标物 + 3 个障碍物 box）：

指标	OMPL RRT-Connect	OMPL → CHOMP	OMPL → STOMP	OMPL → TrajOpt (Tesseract)
端到端延迟	120 ms	280 ms	350 ms	400 ms
路径长度 (关节空间弧长)	3.2 rad	2.1 rad	1.9 rad	1.8 rad
最大 jerk	$\infty$ (折点)	高	中	低
碰撞安全	✅ (离散检查)	⚠️ (软约束)	⚠️ (软约束)	✅ (CCD 硬约束)
成功率 (100 次测试)	98%	92%	95%	88%
MoveIt2 集成	✅ 原生	✅ 插件	✅ 插件	❌ (Tesseract)

关键发现：

路径质量：轨迹优化后路径长度减少 35-44%，但延迟增加 130-230%——这是质量和速度的权衡
成功率：TrajOpt 成功率最低，因为硬约束在某些场景下导致不可行——但它的成功结果是最安全的
实际部署：多数工业场景选择 OMPL + STOMP（成功率高、调参少），安全关键场景选择 OMPL + TrajOpt

反事实推理：如果不做轨迹优化，直接把 RRT-Connect 路径发给控制器会怎样？ - 路径长度 3.2 rad vs 优化后 1.8 rad → 执行时间增加 ~78% - 折点处的 jerk 无穷大 → 谐波减速器寿命缩短（每 10 万次冲击减少 5-10% 寿命） - 路径绕远 → 产线节拍时间增加，年产量下降 - 结论：轨迹优化不是"锦上添花"——它是工业部署的必需步骤

MoveIt2 链式管线配置实战 ⭐⭐¶

在 MoveIt2 中配置 OMPL → STOMP 链式管线的关键步骤：

# moveit_configs/config/planning_pipelines/ompl_stomp.yaml
# 注意: 精确字段名以 MoveIt2 当前版本文档为准
planning_plugin: ompl_interface/OMPLPlanner
request_adapters:
  - default_planner_request_adapters/AddTimeOptimalParameterization
  - default_planner_request_adapters/FixWorkspaceBounds
  - default_planner_request_adapters/FixStartStateBounds
  - default_planner_request_adapters/FixStartStateCollision
  - default_planner_request_adapters/FixStartStatePathConstraints
# STOMP 作为后处理器
response_adapters:
  - stomp_moveit/StompSmoothingAdapter

调参指南：

参数	CHOMP 推荐值	STOMP 推荐值	说明
最大迭代	50-100	30-60	STOMP 通常更快收敛
碰撞权重	5-20	N/A (自适应)	CHOMP 需手动调权重
路径点数	50-100	20-50	STOMP 对路径点数不敏感
噪声标准差	N/A	0.01-0.1 rad	STOMP 的随机扰动幅度
优化后验证	必须	推荐	软约束优化器必须做硬碰撞验证

⚠️ 编程陷阱：CHOMP/STOMP 优化后不做碰撞验证就执行

错误做法：直接将 CHOMP/STOMP 的输出发给 JointTrajectoryController

现象：极低概率下（约 2-5%），优化后的路径仍存在碰撞——因为软约束允许"几乎满足"

正确做法：在 MoveIt2 的 response adapter 中添加碰撞验证步骤；如果验证失败，用 OMPL 的原始路径（安全但不光滑）作为 fallback

根本原因：CHOMP/STOMP 使用连续的碰撞代价函数，在代价梯度为零的配置（障碍物边界的鞍点）处可能收敛到非零碰撞解

CHOMP 与 STOMP 的互补使用策略 ⭐⭐¶

在实际部署中，CHOMP 和 STOMP 可以互补使用：

CHOMP 优先：当场景 SDF 梯度可用且连续时，CHOMP 的一阶梯度收敛更稳定
STOMP fallback：当 CHOMP 因 SDF 梯度不连续（如尖角障碍）而振荡时，切换到 STOMP
并行竞速：MoveIt2 支持并行规划管线——同时运行 CHOMP 和 STOMP，取先完成且通过碰撞验证的结果

并行竞速策略:
│
├── Pipeline 1: OMPL → CHOMP (基于梯度)
│   └── 如果成功 → 使用 CHOMP 结果
│
├── Pipeline 2: OMPL → STOMP (基于采样)
│   └── 如果 CHOMP 失败 → 使用 STOMP 结果
│
└── 取先完成且通过碰撞验证的结果

跨领域类比：这种并行竞速策略类似于 TRAC-IK 的设计——TRAC-IK 同时运行 KDL 数值迭代和 SQP 两个求解器，取先收敛的结果。在运动规划中，同一思想被推广到规划器级别。

延伸阅读：Ratliff et al., "CHOMP: Covariant Hamiltonian Optimization for Motion Planning", IJRR, 2009；Kalakrishnan et al., "STOMP: Stochastic Trajectory Optimization for Motion Planning", ICRA, 2011。这两篇奠基论文分别建立了梯度优化和随机采样两大轨迹优化范式，至今仍是理解本章所有算法的必读文献。

#	问题	期望答案要点
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推断等价于非线性最小二乘

惩罚阶数	数学表达	物理含义	路径特性
0 阶（路径长度）	\(\sum \\|q_{t+1} - q_t\\|\)	最短路径	可能穿过障碍，不光滑
1 阶（速度）	\(\sum \\|q_{t+1} - q_t\\|^2\)	最短路径（加权）	接近直线，与 0 阶类似
2 阶（加速度）	\(\sum \\|q_{t+1} - 2q_t + q_{t-1}\\|^2\)	最小加速度	光滑弯曲，CHOMP/TrajOpt 默认
3 阶（jerk）	\(\sum \\|q_{t+2} - 3q_{t+1} + 3q_t - q_{t-1}\\|^2\)	最小 jerk	加速度连续，更光滑

项	含义	计算方法
\(\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）

参数	偏大的效果	偏小的效果	推荐范围
`learning_rate` (\(\eta\))	更快收敛但可能振荡/发散	更稳定但收敛慢	0.001-0.05
`smoothness_cost_weight`	路径更光滑但可能穿越障碍	路径更贴合障碍但可能不光滑	0.05-0.5
`obstacle_cost_weight`	更安全但路径可能绕远	路径更短但可能轻微碰撞	0.5-5.0
`collision_clearance` (\(\epsilon\))	更大安全裕度但路径受限	更紧凑但安全余量小	0.05-0.3 m
`ridge_factor`	更稳定但偏离协变性	更忠于协变梯度但可能不稳定	0.001-0.1
`max_iterations`	更好的优化质量但时间长	更快但可能未收敛	100-500

参数	说明	推荐值	调优建议
初始信赖域 \(\rho_0\)	第一步允许的最大更新幅度	0.5-2.0	初始解质量好 → 小值；差 → 大值
最大信赖域 \(\rho_{max}\)	信赖域上限	10.0	过大可能导致线性化误差
最小信赖域 \(\rho_{min}\)	低于此值则终止	1e-4	过小则可能在数值噪声中振荡
缩放因子（成功时）	\(\rho \leftarrow \min(\rho \times 2, \rho_{max})\)	2.0	保守场景可用 1.5
缩放因子（失败时）	\(\rho \leftarrow \rho \times 0.5\)	0.5	极保守场景可用 0.25

参数	说明	推荐值
安全裕度 \(\delta_{safe}\)	要求的最小碰撞距离	0.02-0.10 m
初始惩罚系数 \(\mu_0\)	L1 惩罚的初始权重	10-100
最大惩罚系数 \(\mu_{max}\)	超过此值报告失败	\(10^6\)
CCD 分辨率	相邻路径点间的连续碰撞检测步数	5-20

\(\lambda\) 值	行为	适用场景
\(\lambda \ll \overline{S}\)	近乎贪心：只保留最低代价扰动	代价面单调的简单场景
\(\lambda \approx \overline{S}\)	平衡探索与利用	大多数场景（推荐）
\(\lambda \gg \overline{S}\)	所有扰动等权：纯随机搜索	极度非凸的代价面（罕见）

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

方法	平滑性项	碰撞项	约束	优化策略
CHOMP	\(\xi^T A \xi\)	\(\int \text{body}(\nabla \text{SDF}) \,ds\)	无 (软)	协变梯度下降
TrajOpt	速度/加速度惩罚	\(\text{SDF}(x) \geq d_{safe}\)	硬	SQP
STOMP	\(\xi^T R^{-1} \xi\)	任意 \(c(\xi)\)	无 (软)	随机采样-加权
GPMP2	GP 先验 \(\mathcal{N}(\mu, K)\)	SDF 似然	无 (软)	MAP 推断 (LM)

符号	含义	首次出现
\(\xi\)	轨迹（路径点序列），\(\xi = [q_0, q_1, \ldots, q_N]\)	§2
\(A\)	平滑性矩阵（有限差分加速度的内积矩阵）	§2, §3
\(\mathcal{J}_{\text{smooth}}\)	平滑性代价 \(= \xi^T A \xi\)	§2
\(\mathcal{J}_{\text{obs}}\)	碰撞代价（SDF 积分）	§2
\(\text{SDF}(x)\)	签名距离场值（正=安全，负=碰撞）	§2
\(\nabla \text{SDF}\)	SDF 梯度（指向远离障碍方向）	§2, §3
\(d_j(q_t)\)	第 \(j\) 个 link 在配置 \(q_t\) 下到最近障碍的距离	§4
\(\delta_{\text{safe}}\)	安全距离裕度	§4
\(\Delta\xi\)	SQP 中的路径增量	§4
\(H_k\)	SQP 第 \(k\) 次迭代的 Hessian 近似	§4
\(R^{-1}\)	STOMP 的噪声协方差逆（也是平滑性矩阵）	§5
\(K(s, s')\)	GPMP2 的 GP 核函数	§6

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

本章知识导航¶

前置知识桥接¶

预计阅读时间¶

M08.0 前置自测¶

本章目标¶

知识导航¶

预计阅读时间¶

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

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

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

两种范式的互补性 ⭐¶

MoveIt2 的工程实践 ⭐⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

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

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

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

⚠️ 常见陷阱¶

练习¶

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

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

历史背景 ⭐⭐¶

算法推导 ⭐⭐⭐¶

协变梯度的严格推导 ⭐⭐⭐⭐¶

完整算法流程 ⭐⭐¶

CHOMP 的收敛性分析与参数调优 ⭐⭐⭐¶

CHOMP 在 MoveIt2 中的使用 ⭐⭐¶

CHOMP 的可视化教学价值 ⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

历史背景 ⭐⭐¶

SQP 数学框架 ⭐⭐⭐¶

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

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

Tesseract 工业栈中的 TrajOpt ⭐⭐¶

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

CHOMP vs TrajOpt 核心对比 ⭐⭐¶

TrajOpt 的参数调优与工程最佳实践 ⭐⭐⭐¶

Tesseract 的工程亮点 ⭐⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

算法：路径积分（\(\text{PI}^2\)）的运动规划变体 ⭐⭐⭐¶

\(\text{PI}^2\) 的信息论推导 ⭐⭐⭐⭐¶

STOMP 的收敛性与采样效率分析 ⭐⭐⭐¶

STOMP 在 MoveIt2 中的配置 ⭐⭐¶

四大优化器对比总表 ⭐⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

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

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

碰撞因子: 似然函数 ⭐⭐¶

GTSAM 代码示意 ⭐⭐¶

GP 先验因子的完整推导 ⭐⭐⭐⭐¶

GPMP2 与其他优化器的性能对比 ⭐⭐⭐¶

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

⚠️ 常见陷阱¶

练习¶

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

动机：几何路径 \(\ne\) 可执行轨迹 ⭐⭐¶

几何路径到可执行轨迹的完整数据流 ⭐⭐¶

Crocoddyl/Drake 中的联合优化 ⭐⭐⭐¶

Drake 联合轨迹优化代码 ⭐⭐⭐¶

时间分配策略——影响优化质量的隐藏参数 ⭐⭐¶

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

⚠️ 常见陷阱¶

练习¶

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

焊接场景 ⭐⭐¶

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

码垛场景 ⭐⭐¶

涂胶/喷涂场景 ⭐⭐¶