本文档属于 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.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 推断**——如果你学过基础课程中学过的 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 的增量更新: 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" 更像是 "先验正则化"，
   而非 "数据驱动学习"。不需要训练数据。

练习¶

[编程] 用 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 的输入。

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$ 使约束满足。
[跨章综合] 综合 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 参数化，最新进展将轨迹优化嵌入基元生成——在离线阶段为常见场景预优化一组高质量基元，在线阶段用图搜索快速拼接。这结合了轨迹优化的质量和图搜索的速度。

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: 场景驱动的方案选型，从焊接到码垛各有最优组合

跨章综合练习 ⭐⭐⭐¶

题目 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推断等价于非线性最小二乘

项	含义	计算方法
\(\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	同样!	求解算法完全复用

方法	平滑性项	碰撞项	约束	优化策略
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 在 MoveIt2 中的使用 ⭐⭐¶

CHOMP 的可视化教学价值 ⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

历史背景 ⭐⭐¶

SQP 数学框架 ⭐⭐⭐¶

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

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

Tesseract 工业栈中的 TrajOpt ⭐⭐¶

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

CHOMP vs TrajOpt 核心对比 ⭐⭐¶

Tesseract 的工程亮点 ⭐⭐¶

⚠️ 常见陷阱¶

练习¶

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

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

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

PI^2 的信息论推导 ⭐⭐⭐⭐¶

STOMP 在 MoveIt2 中的配置 ⭐⭐¶

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

⚠️ 常见陷阱¶

练习¶

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

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

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

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

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

GTSAM 代码示意 ⭐⭐¶

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

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

⚠️ 常见陷阱¶

练习¶

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

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

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

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

时间分配策略 ⭐⭐¶

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

⚠️ 常见陷阱¶

练习¶

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

焊接场景 ⭐⭐¶

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

码垛场景 ⭐⭐¶

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

选型决策流程 ⭐⭐¶

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

练习¶

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

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

GPMP2 on Lie Groups ⭐⭐⭐⭐¶

Optimization-based Motion Primitives ⭐⭐⭐¶