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

M04 碰撞检测与距离计算——从 GJK 算法到 GPU 并行碰撞查询¶

本章定位：碰撞检测是运动规划和控制的"安全基座"——没有可靠的碰撞检测，任何路径规划都是纸上谈兵。本章从碰撞检测的数学基础（GJK/EPA/SAT 算法）出发，深入精读 FCL 与 Coal（HPP-FCL）两大碰撞检测库，覆盖碰撞几何类型选择、自碰撞检测、可微距离计算、Pinocchio 碰撞集成、MoveIt2 碰撞管线，到 GPU 加速（cuRobo/VAMP）的完整知识链路。

与其他章节的关系：M01 的 Pinocchio 提供了运动链结构，M04 在这之上叠加几何碰撞层。M04 的碰撞检测能力直接服务于 M08（轨迹优化的障碍物代价）和 M07（采样规划的状态有效性检查）。碰撞距离的梯度计算则桥接到 M06（自动微分章节）的可微优化框架。

前置依赖：M01（Pinocchio 运动学/动力学）、M03（IK 求解器）、P01（URDF collision 几何声明）

下游章节：M08（轨迹优化——碰撞约束代价函数）、M07（采样规划——碰撞检查器）、M14（MoveIt2 碰撞检测管线）

建议用时：1.5 周（GJK 数学 2 天 + 库精读 3 天 + 碰撞管线集成 3 天 + GPU 加速 2 天）

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	URDF collision 标签：URDF 中 `<collision>` 和 `<visual>` 的区别是什么？为什么同一个 link 可以有不同的碰撞和视觉几何？	P01 URDF/Xacro
2	Pinocchio FK：调用 `forwardKinematics(model, data, q)` 后，如何获取某个 link 的世界坐标系位姿？`data.oMi` 和 `data.oMf` 的区别？	M01 §6 FK 深入
3	凸集：什么是凸集？给定两个凸多面体，如何判断它们是否相交？（提示：分离超平面定理）	线性代数 / 计算几何基础
4	Minkowski 和与差：给定集合 A 和 B，写出 Minkowski 差 $A \ominus B = \{a - b \mid a \in A, b \in B\}$ 的定义。为什么 $A \cap B \neq \emptyset \Leftrightarrow 0 \in A \ominus B$？	计算几何基础
5	模板特化：C++ 模板特化如何实现"同一接口、不同实现"？FCL 的 `BVHModel<BV>` 如何利用模板参数化不同的包围体类型？	v8 Ch13 模板特化

本章目标¶

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

推导 GJK 算法的核心循环——support function、simplex 迭代、Voronoi 区域判断——理解为什么 GJK 能高效计算任意凸形状的距离
区分 broadphase（宽相）与 narrowphase（窄相）两阶段架构的职责和数据结构（Dynamic AABB Tree / SAP / BVH）
使用 FCL 和 Coal 的 C++ API 构建碰撞检测管线，包括环境碰撞和自碰撞
选择合适的碰撞几何表示（球体 / 胶囊体 / 凸包 / 网格）并理解性能权衡
配置 Pinocchio + Coal 的碰撞检测集成，计算可微距离用于轨迹优化
理解 MoveIt2 的碰撞检测管线、SRDF 碰撞矩阵、ACM（Allowed Collision Matrix）的工程意义

1. 为什么碰撞检测是第一道门槛 ⭐¶

1.1 运动规划的安全性保证¶

考虑一个典型的 pick-and-place 任务：机械臂需要从 A 点抓取物体放到 B 点。路径规划器（RRT/PRM/BIT*）生成的路径必须满足一个硬约束——整条路径上没有碰撞。

碰撞类型	检测对象	后果
机械臂-环境碰撞	link vs 桌面/墙壁/障碍物	损坏工件或环境
自碰撞	link_i vs link_j	机械臂自身损坏
机械臂-被操作物体碰撞	link vs 抓取的物体	物体掉落或变形
末端执行器-障碍物碰撞	手爪 vs 周围物体	抓取失败

在采样规划中，碰撞检测是**最频繁调用**的函数。以 RRT-Connect 为例：

每次扩展树节点:
  1. 采样随机配置 q_rand        ← ~0.01 μs
  2. 找到最近节点 q_near         ← ~1 μs (kd-tree)
  3. 插值生成 q_new              ← ~0.1 μs
  4. 碰撞检查 isCollisionFree(q_new)  ← ~50-500 μs ← 瓶颈！

碰撞检查通常占规划总时间的 80-95%。一次典型的 7-DOF 机械臂规划可能需要 1000-10000 次碰撞检查。这意味着碰撞检测的性能直接决定了规划的实时性。

1.2 碰撞检测的计算挑战¶

N 体问题：如果场景中有 $N$ 个几何体，朴素的两两检查需要 $O(N^2)$ 次 narrowphase 检测。对于一个 7-DOF 机械臂（~10 个 link）在有 100 个障碍物的场景中：

\[\text{检测次数} = \binom{N_{\text{link}} + N_{\text{obs}}}{2} = \binom{110}{2} = 5995\]

每次 narrowphase 检测需要 ~1-10 μs（取决于几何复杂度），总计 ~6-60 ms——远超 1 ms 控制周期。这就是为什么需要 broadphase 加速。

跨领域类比：碰撞检测的两阶段架构（broadphase + narrowphase）类似于数据库查询的"索引 + 扫描"。索引（如 B-tree）快速排除大量不相关行，然后精确扫描剩余候选行。没有索引的全表扫描就像没有 broadphase 的 $O(N^2)$ 碰撞检测——在大规模场景下不可行。

1.3 碰撞检测在机械臂 pipeline 中的位置¶

URDF (collision geometry)
        │
        ▼
┌──────────────────────┐
│  碰撞检测管线 (M04)   │ ← 本章
│  ├── Broadphase       │
│  │   └── AABB Tree    │
│  ├── Narrowphase      │
│  │   └── GJK + EPA    │
│  └── 距离 + 梯度      │
└────────┬─────────────┘
         │
    ┌────┴────┐
    ▼         ▼
┌────────┐ ┌─────────┐
│ 采样规划│ │ 轨迹优化 │
│ (M07)  │ │ (M08)   │
│ 状态有  │ │ 碰撞代价 │
│ 效性检  │ │ + 梯度   │
│ 查(bool)│ │ (连续值) │
└────────┘ └─────────┘

注意两种使用模式的区别： - **采样规划**需要布尔结果（碰撞/不碰撞），调用频率极高，性能敏感 - **轨迹优化**需要连续距离值和梯度，用于代价函数反向传播

练习¶

⭐ 估算 Franka Panda（10 个 collision link）在有 50 个障碍物的场景中，朴素 $O(N^2)$ 碰撞检测需要多少次 narrowphase 调用。如果每次 narrowphase 耗时 5 μs，总碰撞检测耗时是多少？
⭐ 解释为什么轨迹优化（M08）需要碰撞距离的梯度，而采样规划（M07）只需要布尔碰撞结果。
⭐⭐ 在一个 1 kHz 控制循环中做在线碰撞检查（如变阻抗控制中的安全监控），碰撞检测的耗时预算是多少？这对碰撞几何的复杂度有什么限制？

2. 碰撞检测的两阶段架构 ⭐⭐¶

2.1 Broadphase——快速剪枝¶

Broadphase 的目标：用简单的几何近似（如 AABB）快速排除"肯定不碰撞"的对，将 $O(N^2)$ 的全量检测减少到 $O(N \log N)$ 或更少。

AABB（Axis-Aligned Bounding Box）：

原始几何体          AABB 近似
  ╱╲               ┌────┐
 ╱  ╲              │    │
╱    ╲             │    │
╲    ╱             │    │
 ╲  ╱              │    │
  ╲╱               └────┘
复杂形状            简单矩形框

AABB 的优势：相交测试极快（6 次比较即可），适合 SIMD 批处理。劣势：对旋转不友好——细长物体旋转后 AABB 会变得很松。

三种 Broadphase 数据结构：

数据结构	构建	查询	动态场景	用库
Dynamic AABB Tree	$O(N \log N)$	$O(\log N)$	优秀	FCL 默认
SAP (Sweep And Prune)	$O(N \log N)$	$O(N)$ 最坏	中等	Bullet
Static BVH	$O(N \log N)$	$O(\log N)$	需重建	静态环境

Dynamic AABB Tree 是 FCL 的默认 broadphase 算法。它维护一棵平衡二叉树，每个叶节点是一个几何体的 AABB，内部节点是子节点 AABB 的并集。查询时自顶向下——如果内部节点的 AABB 不与查询体相交，则整个子树可以跳过。

反事实推理：如果不做 broadphase 直接上 narrowphase 会怎样？ - 100 个物体的场景：$\binom{100}{2} = 4950$ 次 GJK 调用 - 每次 GJK ~5 μs → 总计 ~25 ms - 有 broadphase：只需 ~50 次 GJK（典型剪枝率 99%）→ ~0.25 ms - 性能差 100 倍！

2.2 Narrowphase——精确检测¶

Broadphase 筛出"可能碰撞"的对后，Narrowphase 做精确的几何相交/距离计算。核心算法有三个：

算法	用途	适用几何	输出
GJK	距离/相交测试	任意凸形状	距离 d 或"相交"
EPA	穿透深度	凸形状（已知相交）	穿透深度 + 方向
SAT	分离轴测试	凸多面体（顶点已知）	分离轴或"相交"

GJK 是最通用的——它只需要"support function"（给定方向返回最远点），任何凸形状都可以定义 support function。EPA 是 GJK 的扩展——当 GJK 判定相交后，EPA 继续展开 simplex 来计算穿透深度。SAT 对多面体最快但通用性差。

2.3 两阶段的代码结构¶

// FCL 的两阶段碰撞检测管线（伪代码）
class CollisionManager {
    // Broadphase: Dynamic AABB Tree
    DynamicAABBTreeCollisionManager broadphase_;

    void registerObject(CollisionObject* obj) {
        broadphase_.registerObject(obj);
    }

    void collide(CollisionResult& result) {
        // Step 1: Broadphase——AABB 相交测试
        broadphase_.collide(
            &result,
            [](CollisionObject* a, CollisionObject* b,
               CollisionResult* r) {
                // Step 2: Narrowphase——GJK/EPA 精确测试
                fcl::collide(a, b, request, *r);
                return r->isCollision();
            }
        );
    }
};

2.4 BVH——加速单个 mesh 的碰撞¶

BVH (Bounding Volume Hierarchy) 不是 broadphase 数据结构——它用于加速**单个 mesh 内部**的碰撞检测。当两个 mesh 各有数千个三角形时，BVH 把每个 mesh 内部的三角形组织成层次树，自顶向下剪枝避免全量三角形-三角形检测。

包围体类型（精度 vs 速度 trade-off）：

类型	紧密度	相交测试	适用场景
AABB	松	最快（6次比较）	一般用途
OBB	紧	中等（SAT 15轴）	细长物体
RSS	较紧	较快	距离查询
OBBRSS	紧	中等	FCL 默认混合
KDOP	可调	可调	特殊场景

FCL 的 BVHModel<BV> 是**模板化的 BVH 树**——BVHModel<AABB> / BVHModel<OBBRSS> 等，这是 v8 Ch13 模板特化在几何库中的应用。

// FCL BVH 模板使用
auto mesh_model = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();
mesh_model->beginModel();
// 添加三角形
for (const auto& tri : triangles) {
    mesh_model->addTriangle(tri.v0, tri.v1, tri.v2);
}
mesh_model->endModel();
// endModel() 自动构建 BVH 树

练习¶

⭐⭐ 画出 Dynamic AABB Tree 的结构示意图：5 个几何体，标注叶节点的 AABB 和内部节点的合并 AABB。解释查询一个新物体时如何自顶向下剪枝。
⭐⭐ 对比 AABB 和 OBB（Oriented Bounding Box）的紧密度。画出一个细长圆柱体在旋转 45 度后，AABB 和 OBB 的包围体积差异。
⭐⭐⭐ 估算 Dynamic AABB Tree 在以下场景下的 broadphase 效率：7-DOF 机械臂（10 个 link）在有 200 个立方体障碍物的仓储场景中。如果 AABB 重叠率是 5%，narrowphase 需要调用多少次？

3. GJK 算法——凸形状距离计算的核心 ⭐⭐⭐¶

3.1 问题定义¶

给定：两个凸形状 A 和 B（可以是球、盒子、圆柱、凸多面体、任何凸形状）。

求：A 和 B 之间的最短距离 $d(A, B)$。如果 $d = 0$，则 A 和 B 相交。

这个问题在 1988 年被 Gilbert、Johnson、Keerthi 优雅地解决——GJK 算法至今仍是所有碰撞检测库的核心。

3.2 Minkowski 差——将两体问题转化为单体问题¶

GJK 的核心洞察是利用 Minkowski 差 将两个凸体的距离问题转化为一个凸体与原点的距离问题：

\[A \ominus B = \{a - b \mid a \in A, b \in B\}\]

关键性质： - 如果 A 和 B 都是凸集，那么 $A \ominus B$ 也是凸集 - $A \cap B \neq \emptyset \Leftrightarrow 0 \in A \ominus B$ - $d(A, B) = \min_{\mathbf{p} \in A \ominus B} \|\mathbf{p}\|$

  A (三角形)      B (正方形)      A ⊖ B (凸多边形)
    ╱╲             ┌──┐              ╱╲
   ╱  ╲            │  │           ╱╱    ╲╲
  ╱____╲           └──┘          ╱╱______╲╲
                                 ╲╲      ╱╱
                                  ╲╲____╱╱

  d(A,B) = Minkowski差中离原点最近的点的距离

跨领域类比：Minkowski 差将"两个物体之间的距离"转化为"一个物体与原点的距离"，这类似于控制论中"把跟踪问题转化为调节问题"——通过坐标变换把"跟踪目标轨迹"变成"将误差调节到零"。同样的数学简化思想。

3.3 Support Function——GJK 的唯一接口¶

GJK 算法不需要知道凸形状的具体表示（顶点列表、隐式方程等），它只需要一个**support function**：

\[S_{A \ominus B}(\mathbf{d}) = \arg\max_{\mathbf{p} \in A \ominus B} \mathbf{p} \cdot \mathbf{d} = S_A(\mathbf{d}) - S_B(-\mathbf{d})\]

直觉：support function 返回凸体在方向 $\mathbf{d}$ 上"最远的点"。

各种几何体的 support function：

几何体	$S(\mathbf{d})$	复杂度
球 (中心 $c$, 半径 $r$)	$c + r \cdot \mathbf{d}/\\|\mathbf{d}\\|$	$O(1)$
胶囊 (两端点 $p_1, p_2$, 半径 $r$)	$\arg\max(p_1 \cdot \mathbf{d}, p_2 \cdot \mathbf{d}) + r \cdot \mathbf{d}/\\|\mathbf{d}\\|$	$O(1)$
长方体 (半尺寸 $h$)	$(\text{sign}(d_x) \cdot h_x, \text{sign}(d_y) \cdot h_y, \text{sign}(d_z) \cdot h_z)$	$O(1)$
凸多面体 ($N$ 个顶点)	$\arg\max_{v \in \text{vertices}} v \cdot \mathbf{d}$	$O(N)$
圆柱 (轴 $\mathbf{a}$, 半径 $r$, 高 $h$)	解析公式	$O(1)$

本质洞察：GJK 的优雅之处在于它**不关心凸体的内部结构**——只要你能提供 support function，任何凸体都能做碰撞检测。这是"基于接口编程"在几何算法中的体现：算法依赖抽象接口（support function），而非具体实现（顶点列表）。

3.4 GJK 核心循环¶

GJK 迭代地在 Minkowski 差 $A \ominus B$ 上构建一个 simplex（2D: 线段/三角形, 3D: 线段/三角形/四面体），逐步逼近离原点最近的点。

# GJK 算法核心（伪代码）
def gjk(A, B):
    # 初始方向
    d = initial_direction()  # 例如 (1, 0, 0)

    # 获取第一个 support 点
    simplex = [support(A, B, d)]

    # 新的搜索方向: 从 simplex 指向原点
    d = -simplex[0]

    while True:
        # 获取新的 support 点
        new_point = support(A, B, d)

        # 关键检查: 新点是否越过了原点？
        if dot(new_point, d) < 0:
            # 新点没有越过原点
            # → 原点不在 Minkowski 差中
            # → A 和 B 不相交，距离 > 0
            return distance_to_origin(simplex)

        # 将新点加入 simplex
        simplex.append(new_point)

        # 更新 simplex 和搜索方向
        # (Voronoi 区域判断——GJK 最复杂的部分)
        simplex, d, contains_origin = update_simplex(simplex)

        if contains_origin:
            return 0  # 相交！

def support(A, B, d):
    """Minkowski 差的 support function"""
    return support_A(d) - support_B(-d)

算法直觉：想象你站在 Minkowski 差的边界上，想要找到离原点最近的点。GJK 就像"走台阶"——每一步都朝着原点的方向走，用 simplex（三角形或四面体）来"围住"离原点最近的区域。如果原点被围住了 → 相交。如果新台阶走不过原点 → 不相交，当前 simplex 上的最近点就是答案。

3.5 Simplex 更新与 Voronoi 区域¶

simplex 更新（update_simplex）是 GJK 最难理解也最容易出 bug 的部分。它需要判断原点位于 simplex 的哪个 Voronoi 区域，然后丢弃不再需要的顶点。

以 3D 的四面体 simplex 为例，原点可能在： - 4 个顶点区域之一（退化为 0-simplex） - 6 条边区域之一（退化为 1-simplex） - 4 个面区域之一（退化为 2-simplex） - 四面体内部（包含原点，相交！）

每种情况的处理方式不同。这就是为什么 GJK 的工业实现（FCL/Coal/libccd）代码看起来很复杂——核心循环只有十几行，但 simplex 更新的分支判断有数百行。

⚠️ 编程陷阱：GJK 实现中的 Voronoi 区域判断

错误描述：初学者常省略某些边界情况（如 simplex 退化为共线的三个点）

现象：在特定几何体配置下返回错误的距离，或无限循环不收敛

根本原因：3D Voronoi 区域有 15 种情况（4 顶点 + 6 边 + 4 面 + 1 内部），遗漏任一种都会导致错误

正确做法：直接使用 libccd/FCL/Coal 的成熟实现，不要自行实现 3D GJK。如果需要学习，从 2D GJK 开始（只有 7 种情况）

3.6 EPA——穿透深度计算¶

GJK 判定两个凸体相交后，EPA（Expanding Polytope Algorithm） 接手计算穿透深度和穿透方向。

GJK 结束时的状态:
  simplex 包含原点（四面体）

EPA 算法:
  1. 以 simplex 为初始 polytope
  2. 找到 polytope 上离原点最近的面
  3. 在该面的法方向上取新的 support 点
  4. 如果新点到面的距离 < epsilon → 收敛，返回穿透深度
  5. 否则用新点扩展 polytope，回到步骤 2

EPA 的收敛速度通常很快（3-10 次迭代），因为每次扩展都在"最薄弱"的方向上改进 polytope。

EPA 的数学推导——为什么它能找到穿透深度：

EPA 求解的是以下优化问题：给定 Minkowski 差 $C = A \ominus B$，已知 $0 \in C$（GJK 已确认），求原点到 $C$ 边界的最短距离：

\[d_{\text{pen}} = \min_{\mathbf{p} \in \partial C} \|\mathbf{p}\|\]

其中 $\partial C$ 是 $C$ 的边界。这个距离就是穿透深度，对应方向就是穿透方向（分离两个物体所需的最小位移方向）。

EPA 的核心思想是从 simplex 出发，逐步**从内部逼近** $\partial C$：

Step 1：初始 polytope = GJK 结束时的 simplex（包含原点的四面体）。此 polytope 是 $C$ 的一个内接凸包。

Step 2：找到离原点最近的面 $F$。设 $F$ 的法向量为 $\mathbf{n}_F$，原点到 $F$ 的距离为 $d_F$。

Step 3：在方向 $\mathbf{n}_F$ 上查询 support function，获得新点 $\mathbf{v} = S_C(\mathbf{n}_F)$。

Step 4：如果 $\mathbf{v} \cdot \mathbf{n}_F - d_F < \epsilon$，说明 polytope 的这个面已经"碰到" $C$ 的真实边界——$d_F$ 就是穿透深度。

Step 5：否则，用 $\mathbf{v}$ 扩展 polytope（替换面 $F$ 及其可见面为新的三角形），回到 Step 2。

EPA 迭代过程 (2D 截面示意):

 迭代 1:                迭代 2:                迭代 3 (收敛):
   ╱╲                    ╱─╲                   ╱──╲
  ╱  ╲                  ╱   ╲                 ╱    ╲
 ╱ ●  ╲     →          ╱ ●   ╲     →        ╱  ●   ╲
 ╲    ╱                ╱      ╲             ╱       ╲
  ╲  ╱                ╱________╲           ╱_________╲
   ╲╱                                     几乎贴合 Minkowski 差边界
                                          d_pen = 原点到最近面的距离

反事实推理：如果不用 EPA 而直接用 GJK 的 simplex 距离估算穿透深度，会怎样？ - GJK 的 simplex 是 Minkowski 差的一个极粗糙的内接近似（只有 4 个顶点） - 原点到 simplex 面的距离可能远小于真实穿透深度（特别是对于不规则凸体） - 穿透方向也可能不准确，导致碰撞响应推离方向错误 - 结论：EPA 的迭代是必要的——用 3-10 次 support function 调用换取精确的穿透信息

EPA 的伪代码实现：

def epa(A, B, initial_simplex):
    """EPA 算法——从 GJK 的 simplex 出发计算穿透深度"""
    # 初始化 polytope（以 simplex 的面为初始三角形列表）
    polytope = build_polytope(initial_simplex)

    for _ in range(MAX_EPA_ITERATIONS):
        # 找到离原点最近的面
        closest_face, distance, normal = find_closest_face(polytope)

        # 在最近面法向方向上取新的 support 点
        new_point = support(A, B, normal)

        # 收敛检查
        new_distance = dot(new_point, normal)
        if new_distance - distance < EPA_TOLERANCE:
            # 穿透深度 = distance
            # 穿透方向 = normal
            return distance, normal

        # 扩展 polytope：删除可见面，用新点创建新面
        remove_visible_faces(polytope, new_point)
        add_new_faces(polytope, new_point)

    # 未收敛——返回当前最佳估计
    return distance, normal

⚠️ 编程陷阱：EPA 的 polytope 面管理

错误描述：扩展 polytope 时，只删除了"最近面"而未删除其他对新点可见的面

现象：polytope 变为非凸，后续迭代产生错误的"最近面"，最终穿透深度不正确

根本原因：新点可能同时对多个面可见——所有可见面都必须删除，替换为以新点为顶点的新三角形

正确做法：用面法向量和新点的点积判断可见性：$\mathbf{n}_F \cdot (\mathbf{v} - \mathbf{p}_F) > 0$ 的面都需要删除

3.7 GJK 收敛性证明——为什么 GJK 一定能终止¶

理解 GJK 的收敛性有助于设置合理的迭代上限和终止条件。

定理：GJK 在有限步内终止，且终止时返回正确的距离或相交判定。

证明思路：

关键观察：每次迭代中，simplex 上离原点最近的点 $v_k$ 满足：

\[\|v_{k+1}\| \leq \|v_k\|\]

即距离序列 $\{\|v_k\|\}$ 单调递减（或当 simplex 包含原点时终止）。

为什么单调递减？ 新加入的 support 点 $w_{k+1}$ 满足：

\[w_{k+1} \cdot d_k \geq v_k \cdot d_k = -\|v_k\|^2 / \|d_k\|\]

其中 $d_k = -v_k$ 是搜索方向。新 simplex 上的最近点 $v_{k+1}$ 不会比 $v_k$ 更远，因为 $v_k$ 本身就在新 simplex 上（作为旧 simplex 的子集）。

终止条件：当 $w_{k+1} \cdot d_k < \|v_k\|^2 - \epsilon$ 时，新点无法改进距离估计——此时 $\|v_k\|$ 就是距离的近似值。

跨领域类比：GJK 的收敛行为类似于梯度下降——每步选择一个"最有希望"的方向（support 方向 $d_k$），每步都缩小距离。区别在于 GJK 在有限维 simplex 上搜索，因此在有限步内精确收敛，而梯度下降在连续空间中只能渐近收敛。

2D GJK 完整手算示例：

设 $A = \{(0,0), (4,0), (2,3)\}$（三角形），$B = \{(6,1), (8,1), (7,3)\}$（三角形）。

计算 Minkowski 差 $A \ominus B$：

$A \ominus B$ 的顶点 = $\{a - b \mid a \in \text{vertices}(A), b \in \text{vertices}(B)\}$

\[= \{(0,0)-(6,1), (0,0)-(8,1), (0,0)-(7,3),$$ $$\quad (4,0)-(6,1), (4,0)-(8,1), (4,0)-(7,3),$$ $$\quad (2,3)-(6,1), (2,3)-(8,1), (2,3)-(7,3)\}\]

\[= \{(-6,-1), (-8,-1), (-7,-3), (-2,-1), (-4,-1), (-3,-3),$$ $$\quad (-4,2), (-6,2), (-5,0)\}\]

$A \ominus B$ 的凸包包含这些点的子集。原点 $(0,0)$ 显然不在这些点构成的凸包中（所有 x 坐标都为负），因此 $A$ 和 $B$ 不相交。

GJK 迭代：

迭代 0:
  d₀ = (1, 0)  (初始搜索方向)
  support(d₀): A 中最远 = (4,0), B 中最远 = (8,1)
  w₀ = (4,0)-(8,1) = (-4,-1)
  simplex = {(-4,-1)}
  v₀ = (-4,-1),  d₁ = -v₀ = (4,1)

迭代 1:
  support(d₁): A 中最远 = (4,0), B 中最远 = (6,1)
  w₁ = (4,0)-(6,1) = (-2,-1)
  检查: w₁·d₁ = (-2)(4)+(-1)(1) = -9 < 0 → 新点没越过原点

  simplex = {(-4,-1), (-2,-1)}
  最近点: simplex 上离原点最近的点在线段 (-4,-1)→(-2,-1) 上
  投影: 两点 y 分量相同 = -1, x 范围 [-4,-2]
  最近点 v₁ = (-2,-1),  ||v₁|| = √5 ≈ 2.24

  终止检查: w₁·d₁ = -9, ||v₁||² = 5
  因为 |w₁·d₁/||d₁||| < ||v₁|| → 距离确认

  d(A,B) ≈ ||v₁|| = √5 ≈ 2.24

（实际 GJK 实现会继续迭代直到更精确地收敛，但核心过程如上所示。）

3.8 GJK 的性能特性¶

特性	值	说明
典型迭代次数	3-10	对大多数凸体对
每次迭代	2 次 support + simplex 更新	O(1) 对简单几何，O(N) 对 N 顶点 mesh
总耗时 (球-球)	~0.1 μs	解析公式更快
总耗时 (凸包-凸包, 100 顶点)	~2-5 μs	FCL 实现
总耗时 (凸包-凸包, 500 顶点)	~10-30 μs	需 hill-climbing 优化
Coal vs FCL	Coal 快 2-15x	Nesterov 加速 (Montaut et al. RSS 2022)

3.9 GJK+EPA 在 Coal 中的完整调用流程¶

理解 GJK 和 EPA 的理论后，让我们看看在 Coal 中如何实际调用完整的距离/穿透计算：

#include <coal/collision.h>
#include <coal/collision_data.h>
#include <coal/distance.h>
#include <coal/distance_data.h>
#include <coal/math/transform.h>
#include <coal/shape/geometric_shapes.h>

// 创建两个凸体
auto box = std::make_shared<coal::Box>(1.0, 0.5, 0.3);
auto sphere = std::make_shared<coal::Sphere>(0.2);

// 设置位姿
coal::Transform3s tf_box = coal::Transform3s::Identity();
coal::Transform3s tf_sphere = coal::Transform3s::Identity();
tf_sphere.setTranslation(coal::Vec3s(0.8, 0, 0));

// 距离查询（GJK 路径）
coal::DistanceRequest dist_req;
dist_req.enable_nearest_points = true;
coal::DistanceResult dist_res;
coal::distance(box.get(), tf_box, sphere.get(), tf_sphere,
               dist_req, dist_res);

if (dist_res.min_distance > 0) {
    // GJK 路径: 不相交，返回距离和最近点
    std::cout << "距离: " << dist_res.min_distance << std::endl;
    std::cout << "点A: "
              << dist_res.nearest_points[0].transpose() << std::endl;
    std::cout << "点B: "
              << dist_res.nearest_points[1].transpose() << std::endl;
} else {
    // EPA 路径: 相交，返回穿透深度
    // 注意: min_distance 为负值，绝对值是穿透深度
    std::cout << "穿透深度: " << -dist_res.min_distance << std::endl;
}

// 碰撞查询（只要布尔结果，更快）
coal::CollisionRequest coll_req;
coal::CollisionResult coll_res;
coal::collide(box.get(), tf_box, sphere.get(), tf_sphere,
              coll_req, coll_res);
bool is_collision = coll_res.isCollision();

Coal 的 safety margin 功能——轨迹优化的利器：

Coal 支持在**碰撞查询**中设置安全裕度，等效于"膨胀"碰撞几何体。注意这不是 DistanceRequest 字段；距离查询仍返回真实最近距离，安全区间应由外层比较 distance - d_safe 或 hinge loss 实现：

// 安全裕度: 将碰撞体向外膨胀 5mm
coal::CollisionRequest safe_req;
safe_req.security_margin = 0.005;
// 此时 collide() 会把进入 5mm 安全区的几何对也报告为接触/近接触。
// 若需要连续代价，仍应使用 distance() 结果在外层计算 distance - d_safe。

练习¶

⭐⭐ 手工计算两个 2D 三角形的 GJK 迭代过程：$A = \{(0,0), (2,0), (1,2)\}$，$B = \{(3,0), (5,0), (4,2)\}$。画出 Minkowski 差 $A \ominus B$，验证原点不在其中。
⭐⭐ 实现球体、长方体、胶囊体的 support function（C++ 或 Python），验证对单位球 support(d) 的输出是 d 方向的单位向量。
⭐⭐⭐ 精读 libccd 的 GJK 实现（src/ccd.c 中 ccdGJKDist()，约 150 行）。配合 GJK 论文，标注代码中 support function 调用、simplex 更新、Voronoi 区域判断的位置。

4. 碰撞几何类型与性能权衡 ⭐⭐¶

4.1 几何类型谱系¶

从简单到复杂，碰撞检测的几何类型形成一个"精度-性能"谱系：

简单 ◄──────────────────────────────────────────► 复杂
快速                                              慢速
粗糙                                              精确

球体 → 胶囊体 → OBB → 凸包 → 三角网格 → 点云/SDF
~0.1μs  ~0.2μs  ~0.5μs ~2-5μs  ~10-50μs   ~1-100μs

4.2 各类型详解¶

球体 (Sphere)：最简单的碰撞几何。

检测：d(球A, 球B) = ||c_A - c_B|| - r_A - r_B
优势：O(1)，无需 GJK
劣势：对非球形物体（如 link）包围很松
用途：腿足 WBC 中的 link 近似、快速预筛

胶囊体 (Capsule)：两个球 + 连接圆柱。机械臂 link 的最佳近似。

检测：d(胶囊A, 胶囊B) = d(线段A, 线段B) - r_A - r_B
     其中线段距离有解析公式
优势：O(1)，比球体紧密得多
劣势：对不规则形状仍然较松
用途：机械臂 link 的默认碰撞表示（cuRobo 使用此方案）

凸包 (Convex Hull)：从 mesh 生成的最小凸包围体。

检测：GJK + EPA
优势：比 AABB/OBB 紧密，适合大多数 link 形状
劣势：需要 GJK 迭代（~2-5 μs）
用途：FCL/Coal 的常用碰撞几何

三角网格 (Triangle Mesh)：原始 CAD 导出的 STL/OBJ mesh。

检测：BVH + 三角形-三角形测试
优势：最精确
劣势：慢（10-50 μs），内存大
用途：高精度场景（如手术机器人）

签名距离场 (SDF)：将空间离散为体素，每个体素存储到最近表面的距离。

检测：查表 + 三线性插值
优势：查询 O(1)，GPU 友好
劣势：预计算耗时，内存大，精度受分辨率限制
用途：cuRobo 的 GPU 碰撞检测管线

4.3 选型决策表¶

场景	推荐几何类型	理由
1 kHz 控制循环碰撞监控	球体/胶囊体	必须 <10 μs
MoveIt2 规划 (10-100 Hz)	凸包 (默认)	平衡精度和性能
高精度装配任务	三角网格	需要精确的碰撞表面
GPU 轨迹优化 (cuRobo)	SDF + 胶囊体	GPU 并行友好
腿足 WBC 自碰撞	球体	最多 512 对，需极快

反事实推理：如果在 1 kHz 控制循环中使用三角网格做碰撞检测，会怎样？ - 单次碰撞检查 ~50 μs - 7 link x 5 障碍物 = 35 次检查 → 1.75 ms - 已经超过 1 ms 控制周期！ - 结论：实时控制只能用 O(1) 的简单几何（球/胶囊/OBB），三角网格只适合离线规划。

4.4 凸分解——非凸 mesh 的处理¶

很多机械臂 link 不是凸的（如有凹槽、孔洞的法兰盘）。GJK 只能处理凸体，因此需要**凸分解**——将非凸 mesh 分解为多个凸部分。

工具	算法	Stars	典型凸块数	质量
V-HACD	体素化 + 层次近似	~1.2k	10-30	良好
CoACD	学习增强凸分解	~800	5-15	最佳 (SIGGRAPH 2022)
Blender	手动分割	—	可控	取决于操作者

CoACD (SarahWeiii/CoACD) 是目前最推荐的凸分解工具——它使用学习方法引导分解方向，生成更少但更紧密的凸块。

练习¶

⭐⭐ 对 Franka Panda 的 link5（有复杂形状），分别用球体、胶囊体、凸包近似。计算三种近似与原始 mesh 的"体积过估比"（近似体积 / 原始体积）。
⭐⭐ 用 CoACD 对一个非凸 mesh 做凸分解，对比 V-HACD 的结果。比较凸块数量和总碰撞检测耗时。
⭐⭐⭐ 实现一个简单的"球体近似生成器"：给定 URDF，为每个 link 自动生成包围球体参数（中心、半径），输出为新的碰撞 URDF。

5. FCL 与 Coal 深度精读 ⭐⭐¶

5.1 FCL——事实标准碰撞检测库¶

GitHub: flexible-collision-library/fcl | Stars ~1.4k | License: BSD-3 | C++14

FCL (Flexible Collision Library) 是 Pan et al. (ICRA 2012) 提出的通用碰撞检测库，被 MoveIt2 和 DART 作为默认碰撞后端。

FCL 的核心 API：

#include <fcl/fcl.h>

// 1. 定义碰撞几何
auto box_geom = std::make_shared<fcl::Boxd>(1.0, 0.5, 0.3);
auto sphere_geom = std::make_shared<fcl::Sphered>(0.2);

// 2. 创建碰撞对象（几何 + 位姿）
fcl::CollisionObjectd box(box_geom,
    fcl::Transform3d(Eigen::Translation3d(0, 0, 0)));
fcl::CollisionObjectd sphere(sphere_geom,
    fcl::Transform3d(Eigen::Translation3d(2, 0, 0)));

// 3. 碰撞检查
fcl::CollisionRequestd request;
request.enable_contact = true;
fcl::CollisionResultd result;
fcl::collide(&box, &sphere, request, result);

if (result.isCollision()) {
    for (const auto& contact : result.getContacts()) {
        std::cout << "penetration: " << contact.penetration_depth
                  << std::endl;
        std::cout << "contact pos: "
                  << contact.pos.transpose() << std::endl;
    }
}

// 4. 距离查询
fcl::DistanceRequestd dist_request;
dist_request.enable_nearest_points = true;
fcl::DistanceResultd dist_result;
fcl::distance(&box, &sphere, dist_request, dist_result);

std::cout << "min distance: " << dist_result.min_distance
          << std::endl;

FCL 的 BVHModel 模板设计（v8 Ch13 模板特化的机器人应用）：

// BVHModel 用模板参数选择包围体类型
template <typename BV>
class BVHModel : public CollisionGeometry<typename BV::S> {
    std::vector<BVNode<BV>> bvs;     // BVH 树节点
    std::vector<Triangle> tri_indices; // 三角形索引
    std::vector<Vector3<S>> vertices;  // 顶点

    // 不同 BV 类型在 BVHModel 内部共享代码
    // 差异通过 BV::fit() 和 BV::overlap() 特化
};

// 使用：BVHModel<AABB> 最快，BVHModel<OBBRSS> 最紧
auto mesh_aabb = std::make_shared<fcl::BVHModel<fcl::AABBd>>();
auto mesh_obb  = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();

5.2 Coal——Pinocchio 生态的高性能碰撞¶

GitHub: coal-library/coal（曾用名 humanoid-path-planner/hpp-fcl）| Stars ~290 | License: BSD-2 | C++17

Coal 是 INRIA/LAAS 维护的 FCL fork，为 Pinocchio 生态优化。与 FCL 的核心差异：

维度	FCL	Coal (HPP-FCL)
GJK 实现	libccd 委托	自研 GJK，2-15x 更快
Nesterov 加速	❌	✅ (Montaut et al. RSS 2022)
距离梯度	❌	✅ 提供距离/最近点信息，部分版本暴露导数或支持 AD 标量
安全裕度	有限	✅ safety margin API
接触面片	❌	✅ contact patch 计算
Pinocchio 集成	需手动	原生集成
模板标量化	❌	✅ 部分 API/版本支持 CppAD/CasADi 标量

Coal 的可微距离：

回顾 M01 §4：Pinocchio 的标量参数化使得同一套运动学代码可以用 double / CppAD::AD<double> / casadi::SX 运行。Coal 在 Pinocchio 生态中提供距离结果、最近点信息，以及随版本演进的导数/标量模板接口；工程上常用这些几何信息结合雅可比手写链式梯度，也可以在目标版本支持时放入 AD tape。这里必须注意：碰撞距离是**分段光滑**的，最近特征切换时梯度会不连续，所以不能把"可用于优化"理解成"全局处处可微"。

为什么 Pinocchio 生态不用原版 FCL？

FCL 不提供距离梯度——距离函数在碰撞表面是分段连续但不处处可微的，FCL 没有暴露 subgradient 接口
Pinocchio 的全模板标量类型需要碰撞检测也支持模板化 Scalar——Coal 在部分几何/距离 API 上做了这层适配
Coal 对齐了 Pinocchio 的 Model/Data 分离模式

5.3 Coal 性能优势——Nesterov 加速 GJK¶

Montaut et al. (RSS 2022) 将 Nesterov 加速梯度下降的思想引入 GJK 算法，将收敛速度从线性提升到超线性。

传统 GJK 在每次迭代中沿"simplex 到原点"的方向搜索——这是一个"最速下降"方向，会导致锯齿形路径。Nesterov 加速引入"动量项"——搜索方向是当前方向和历史方向的加权组合——避免锯齿，加速收敛。

测试场景	FCL GJK	Coal GJK (Nesterov)	加速比
球-球	0.15 μs	0.08 μs	1.9x
凸包-凸包 (50 顶点)	3.2 μs	0.8 μs	4.0x
凸包-凸包 (500 顶点)	28 μs	2.1 μs	13.3x
mesh-mesh (1k 三角形)	85 μs	12 μs	7.1x

跨领域类比：Coal 对 GJK 的 Nesterov 加速，类似于 SLAM 后端从梯度下降升级到 Gauss-Newton。两者的核心思想都是"利用问题结构加速收敛"——GJK 的 simplex 结构对应最小二乘的 $J^TJ$ 近似 Hessian。

5.4 碰撞库选型决策¶

你的使用场景?
│
├── MoveIt2 规划 → FCL（默认）或 Coal（如果用 Pinocchio 辅助计算）
├── Pinocchio 生态 MPC/轨迹优化 → Coal（必选，因为需要距离梯度）
├── Tesseract 工业规划 → Bullet（TrajOpt 依赖）
├── 物理仿真需求 → Bullet（完整物理）或 DART（集成）
├── 学 GJK 算法 → libccd（300行C代码最纯粹）
└── GPU 加速（批量碰撞） → cuRobo 自研 kernel

练习¶

⭐⭐ 用 FCL 构建一个简单场景：一个 fcl::Boxd (桌面) + 一个 fcl::Cylinderd (障碍物柱子) + Franka Panda 的 link 碰撞几何。检测机械臂在不同 $q$ 下是否与桌面/柱子碰撞。
⭐⭐ 对比 FCL 和 Coal 在同一组测试（100 对随机凸包做距离查询）中的速度。用 std::chrono 测量并绘制柱状图。
⭐⭐⭐ 精读 libccd 的 ccdGJKDist()（约 150 行 C 代码）。配合 GJK 论文，标注代码中 support function 调用、simplex 更新的位置。

6. 自碰撞检测与碰撞矩阵 ⭐⭐¶

6.1 自碰撞的特殊性¶

自碰撞 (self-collision) 是机器人 link 之间的碰撞。与环境碰撞有三个关键区别：

维度	环境碰撞	自碰撞
永久排除	无	相邻 link 在零位重叠——必须排除
对数	link x 障碍物	$\binom{n_{\text{link}}}{2}$ link 对
配置工具	无特殊	SRDF `<disable_collisions>`
MoveIt 工具	Planning Scene	MoveIt Setup Assistant ACM

6.2 碰撞矩阵 (ACM)¶

Allowed Collision Matrix (ACM) 是一个布尔矩阵，标记哪些 link 对可以跳过碰撞检测。

SRDF 中的碰撞矩阵定义：

<!-- panda.srdf -->
<robot name="panda">
  <!-- 相邻 link 对——始终重叠，必须禁用 -->
  <disable_collisions link1="panda_link0" link2="panda_link1"
                      reason="Adjacent" />
  <disable_collisions link1="panda_link1" link2="panda_link2"
                      reason="Adjacent" />

  <!-- 永远不会碰撞的远距 link 对 -->
  <disable_collisions link1="panda_link0" link2="panda_link4"
                      reason="Never" />
</robot>

MoveIt Setup Assistant 通过采样大量随机配置来自动检测哪些 link 对"永远不碰撞"或"始终碰撞"，然后生成 <disable_collisions> 列表。

6.3 Pinocchio + Coal 的自碰撞检测实现¶

#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/parsers/srdf.hpp>

// 1. 加载几何模型（从 URDF 的 collision 标签）
pinocchio::GeometryModel geom_model;
pinocchio::urdf::buildGeom(model, "panda.urdf",
    pinocchio::COLLISION, geom_model);

// 2. 添加所有碰撞对（排除相邻 link）
geom_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model, geom_model,
    "panda.srdf");

// 3. 创建几何数据
pinocchio::GeometryData geom_data(geom_model);

// 4. 碰撞检查
Eigen::VectorXd q = pinocchio::randomConfiguration(model);
pinocchio::updateGeometryPlacements(model, data, geom_model,
    geom_data, q);
bool is_collision = pinocchio::computeCollisions(model, data,
    geom_model, geom_data, q, false);

// 5. 距离计算
pinocchio::computeDistances(model, data, geom_model,
    geom_data, q);
for (size_t i = 0; i < geom_model.collisionPairs.size(); ++i) {
    double dist = geom_data.distanceResults[i].min_distance;
}

6.4 碰撞对优化策略¶

对于 7-DOF Panda，全量 link 对数 = $\binom{10}{2} = 45$。去除相邻和永远不碰撞的对后，典型检测对数为 15-25 对。

优化策略	方法	效果
SRDF 排除	MoveIt Setup Assistant 自动分析	减少 50-70% 对数
距离阈值	只对距离 < threshold 的对做精检	减少 30-50% narrowphase
分级检测	先球体粗检 → 凸包精检	减少精检耗时
GPU 批量	cuRobo 的 self-collision kernel	千个对并行

6.5 自碰撞矩阵配置实战——从零构建 ACM ⭐⭐¶

MoveIt Setup Assistant 自动生成的 ACM 是一个很好的起点，但在实际工程中你经常需要**手动调整**碰撞矩阵。以下是从零构建和调试 ACM 的完整流程。

Step 1：理解 ACM 的禁用理由分类

MoveIt Setup Assistant 在禁用碰撞对时会标注理由（reason 属性），理解这些理由对手动调整至关重要：

reason 值	含义	手动调整建议
`Adjacent`	相邻 link，物理上始终接触	永远不要启用——会产生大量误报
`Never`	采样 10000 个随机配置后从未碰撞	通常安全禁用，但极端配置可能遗漏
`Default`	用户手动禁用	需要工程师确认合理性
`Always`	所有采样配置下始终碰撞	检查碰撞几何是否过大

Step 2：手动生成 ACM 的 Python 脚本

当 MoveIt Setup Assistant 不适用时（如自定义机器人、非标准 URDF），可以用 Pinocchio 脚本自动生成 ACM：

import pinocchio as pin
import numpy as np

def generate_acm(urdf_path, srdf_output, n_samples=10000):
    """从 URDF 自动生成碰撞矩阵"""
    model = pin.buildModelFromUrdf(urdf_path)
    geom_model = pin.buildGeomFromUrdf(
        model, urdf_path, pin.COLLISION)
    data = model.createData()
    geom_data = pin.GeometryData(geom_model)

    # 添加所有碰撞对
    geom_model.addAllCollisionPairs()
    n_pairs = len(geom_model.collisionPairs)
    print(f"总碰撞对数: {n_pairs}")

    # 统计每对在随机配置下的碰撞次数
    collision_count = np.zeros(n_pairs, dtype=int)

    for i in range(n_samples):
        q = pin.randomConfiguration(model)
        pin.updateGeometryPlacements(
            model, data, geom_model, geom_data, q)
        pin.computeCollisions(
            model, data, geom_model, geom_data, q, False)

        for j in range(n_pairs):
            if geom_data.collisionResults[j].isCollision():
                collision_count[j] += 1

    # 分类
    adjacent_pairs = []    # 始终碰撞
    never_pairs = []       # 从未碰撞
    sometimes_pairs = []   # 偶尔碰撞（需要检测）

    for j in range(n_pairs):
        cp = geom_model.collisionPairs[j]
        name1 = geom_model.geometryObjects[cp.first].name
        name2 = geom_model.geometryObjects[cp.second].name
        rate = collision_count[j] / n_samples

        if rate > 0.99:
            adjacent_pairs.append((name1, name2, "Always"))
        elif rate == 0:
            never_pairs.append((name1, name2, "Never"))
        else:
            sometimes_pairs.append(
                (name1, name2, f"Sometimes({rate:.2%})"))

    print(f"始终碰撞: {len(adjacent_pairs)} 对")
    print(f"从未碰撞: {len(never_pairs)} 对")
    print(f"偶尔碰撞: {len(sometimes_pairs)} 对（必须检测）")

    # 生成 SRDF
    with open(srdf_output, 'w') as f:
        f.write('<?xml version="1.0" ?>\n')
        f.write('<robot name="robot">\n')
        for name1, name2, reason in adjacent_pairs + never_pairs:
            f.write(f'  <disable_collisions link1="{name1}" '
                    f'link2="{name2}" reason="{reason}" />\n')
        f.write('</robot>\n')

    return sometimes_pairs  # 返回需要运行时检测的对

Step 3：验证 ACM 的安全性

生成的 ACM 必须经过验证——错误禁用一个实际会碰撞的 link 对可能导致机器人自损。

def validate_acm(model, geom_model, srdf_path,
                 n_validation=50000):
    """验证 ACM 不会遗漏真正的碰撞"""
    data = model.createData()
    geom_data = pin.GeometryData(geom_model)

    # 加载 SRDF 禁用碰撞对
    geom_model_full = pin.GeometryModel(geom_model)
    geom_model_full.addAllCollisionPairs()

    geom_model_acm = pin.GeometryModel(geom_model)
    geom_model_acm.addAllCollisionPairs()
    pin.removeCollisionPairs(model, geom_model_acm, srdf_path)

    missed_collisions = 0
    for i in range(n_validation):
        q = pin.randomConfiguration(model)
        pin.updateGeometryPlacements(
            model, data, geom_model_full,
            pin.GeometryData(geom_model_full), q)

        # 用完整碰撞对检测
        full_collision = pin.computeCollisions(
            model, data, geom_model_full,
            pin.GeometryData(geom_model_full), q, False)

        # 用 ACM 裁剪后的碰撞对检测
        acm_collision = pin.computeCollisions(
            model, data, geom_model_acm,
            pin.GeometryData(geom_model_acm), q, False)

        if full_collision and not acm_collision:
            missed_collisions += 1
            print(f"警告: 配置 {i} 被 ACM 遗漏!")

    print(f"遗漏率: {missed_collisions}/{n_validation} "
          f"= {missed_collisions/n_validation:.4%}")
    # 可接受阈值: 0%（任何遗漏都不可接受）

⚠️ 思维陷阱：认为"MoveIt Setup Assistant 生成的 ACM 总是安全的"

新手想法："工具自动生成的配置不需要人工验证"

实际上：MoveIt Setup Assistant 默认只采样约 10000 个随机配置。对于某些极端但可达的配置（如手臂完全折叠），10000 个采样可能**完全没覆盖到**。特别是 7-DOF 冗余机械臂的配置空间是 7 维的，10000 个点在 7 维空间中极其稀疏。

正确做法：(1) 增加采样数到 50000 以上，(2) 手动测试工作任务中的极端配置，(3) 用上述验证脚本做独立验证，(4) 在实际运行中加入运行时碰撞监控作为最后一道防线

Step 4：为末端执行器添加碰撞对

当机械臂安装了自定义夹爪时，需要为夹爪的碰撞几何添加与臂身的碰撞对。这通常不在标准 URDF 中，需要手动配置：

<!-- 自定义夹爪碰撞配置 -->
<robot name="panda_with_gripper">
  <!-- 保留原有的 Panda ACM -->
  <disable_collisions link1="panda_link0" link2="panda_link1"
                      reason="Adjacent" />
  <!-- ... 其他原有 Panda 碰撞对 ... -->

  <!-- 夹爪与 link7 相邻，禁用 -->
  <disable_collisions link1="panda_link7" link2="gripper_base"
                      reason="Adjacent" />

  <!-- 夹爪与 link6 永远不碰撞（经验证），禁用 -->
  <disable_collisions link1="panda_link6" link2="gripper_base"
                      reason="Never" />

  <!-- 注意：不要禁用 gripper_base vs panda_link3/4/5 -->
  <!-- 这些对在某些极端配置下可能碰撞 -->
</robot>

练习¶

⭐⭐ 用 MoveIt Setup Assistant 为 Franka Panda 生成 SRDF，统计被禁用的碰撞对数和保留的对数。
⭐⭐ 用 Pinocchio + Coal 实现 Franka Panda 的自碰撞检测。验证零位无自碰撞，然后手动设置 $q$ 使两个 link 相撞，确认能检出。
⭐⭐⭐ 实现分级自碰撞检测：先用球体近似快速排除，再对球体重叠的对用凸包精检。对比与直接用凸包的耗时差异。
⭐⭐⭐ 用上述 Python 脚本为 UR5e 生成 ACM 并进行验证。统计禁用对数，与 MoveIt Setup Assistant 的结果对比。

7. 距离计算与梯度——为轨迹优化服务 ⭐⭐⭐¶

7.1 从布尔到连续——两种碰撞检测需求¶

需求方	需要的信息	数据类型	用途
采样规划 (RRT/PRM)	"碰撞/不碰撞"	`bool`	状态有效性检查
轨迹优化 (CHOMP/TrajOpt)	距离值 + 距离梯度	`double` + `Vector`	碰撞代价 + 反向传播

轨迹优化需要**连续**的碰撞距离函数 $d(q)$ 及其梯度 $\nabla_q d(q)$，才能通过梯度下降将轨迹推离障碍物。

7.2 碰撞距离的链式求导¶

碰撞距离 $d(q)$ 是以下复合函数：

\[d(q) = d_{\text{geom}}(T_{\text{link}_A}(q), T_{\text{link}_B}(q))\]

其中 $T_{\text{link}}(q)$ 是 link 的世界坐标系位姿（由 FK 给出），$d_{\text{geom}}$ 是两个几何体之间的几何距离（由 GJK 给出）。

链式法则给出梯度：

\[\frac{\partial d}{\partial q} = \frac{\partial d_{\text{geom}}}{\partial T_A} \frac{\partial T_A}{\partial q} + \frac{\partial d_{\text{geom}}}{\partial T_B} \frac{\partial T_B}{\partial q}\]

其中 $\frac{\partial T}{\partial q}$ 就是关节雅可比（回顾 M01 §7），$\frac{\partial d_{\text{geom}}}{\partial T}$ 可以由 Coal 返回的距离/最近点信息、版本支持的导数接口，或工程中手写的最近点梯度共同构造。

本质洞察：碰撞距离梯度的计算本质上是"几何微分"和"运动学微分"的融合——GJK 告诉你"哪两个点最近"，雅可比告诉你"关节角变化如何影响这两个点的位置"。M06（自动微分与代码生成）会深入讲解如何用 CppAD/CasADi 自动化这个过程。

7.3 距离梯度的解析推导 ⭐⭐⭐¶

碰撞距离梯度的推导是连接碰撞检测（M04）与轨迹优化（M08）的核心桥梁。让我们完整推导从关节角 $q$ 到碰撞距离 $d$ 的梯度链。

问题设定：机器人 link A 上有碰撞几何体 $G_A$，link B 上有碰撞几何体 $G_B$（B 可以是另一个 link 或环境障碍物）。GJK 计算出两者之间的最短距离 $d$ 以及对应的最近点对 $(p_A^*, p_B^*)$。

三层链式法则：

\[\frac{\partial d}{\partial q} = \underbrace{\frac{\partial d}{\partial (p_A^*, p_B^*)}}_{\text{几何梯度}} \cdot \underbrace{\frac{\partial (p_A^*, p_B^*)}{\partial (T_A, T_B)}}_{\text{刚体变换}} \cdot \underbrace{\frac{\partial (T_A, T_B)}{\partial q}}_{\text{运动学雅可比}}\]

第一层——几何梯度：

当 $d > 0$（不相交）时，距离函数的梯度方向是从 $p_A^*$ 指向 $p_B^*$ 的单位向量：

\[\frac{\partial d}{\partial p_A^*} = -\hat{\mathbf{n}}, \quad \frac{\partial d}{\partial p_B^*} = +\hat{\mathbf{n}}, \quad \hat{\mathbf{n}} = \frac{p_B^* - p_A^*}{\|p_B^* - p_A^*\|}\]

这有直观的物理意义：如果 $p_A^*$ 沿着 $\hat{\mathbf{n}}$ 方向移动（朝向 $B$），距离减小；如果 $p_B^*$ 沿着 $\hat{\mathbf{n}}$ 方向移动（远离 $A$），距离增大。

第二层——刚体变换：

最近点 $p_A^*$ 在世界坐标系中的位置由 link A 的位姿 $T_A \in SE(3)$ 决定：

\[p_A^* = T_A \cdot p_A^{\text{local}}\]

其中 $p_A^{\text{local}}$ 是最近点在 link A 局部坐标系中的坐标（由 GJK 的 support function 确定）。

对 $T_A$ 的微分（使用 $SE(3)$ 的切空间参数化）：

\[\delta p_A^* = \delta R_A \cdot p_A^{\text{local}} + \delta t_A = [\omega_A]_\times R_A p_A^{\text{local}} + v_A\]

其中 $\omega_A$ 是角速度，$v_A$ 是线速度，$[\cdot]_\times$ 是反对称矩阵。

第三层——运动学雅可比：

link A 的速度（线速度 + 角速度）由关节速度通过**几何雅可比** $J_A(q)$ 给出：

\[\begin{bmatrix} v_A \\ \omega_A \end{bmatrix} = J_A(q) \cdot \dot{q}\]

回顾 M01 §7，这个雅可比可以通过 pinocchio::computeJointJacobian() 获取。

组合三层：

\[\frac{\partial d}{\partial q} = -\hat{\mathbf{n}}^T \cdot J_A^{\text{point}}(q) + \hat{\mathbf{n}}^T \cdot J_B^{\text{point}}(q)\]

其中 $J_A^{\text{point}}(q)$ 是 link A 上 $p_A^*$ 点的线速度雅可比（3 x nv 矩阵），可以通过以下方式计算：

\[J_A^{\text{point}} = J_{A,v} - [p_A^* - t_A]_\times J_{A,\omega}\]

这里 $J_{A,v}$ 和 $J_{A,\omega}$ 分别是 link A 的线速度和角速度雅可比部分。

Coal 的实现方式：

Coal 通过 computeDistances() 同时返回距离值和最近点对。结合 Pinocchio 的雅可比计算，完整的距离梯度可以这样获得：

// 完整的距离梯度计算
Eigen::VectorXd computeCollisionDistanceGradient(
    const pinocchio::Model& model,
    pinocchio::Data& data,
    const pinocchio::GeometryModel& geom_model,
    pinocchio::GeometryData& geom_data,
    const Eigen::VectorXd& q,
    size_t collision_pair_idx) {

    // 1. FK + 几何位置更新
    pinocchio::forwardKinematics(model, data, q);
    pinocchio::updateGeometryPlacements(
        model, data, geom_model, geom_data, q);

    // 2. 距离计算（获取最近点对）
    pinocchio::computeDistances(
        model, data, geom_model, geom_data, q);

    const auto& dr = geom_data.distanceResults[collision_pair_idx];
    Eigen::Vector3d p1 = dr.nearest_points[0];  // link A 上最近点
    Eigen::Vector3d p2 = dr.nearest_points[1];  // link B 上最近点
    double dist = dr.min_distance;

    if (dist <= 1e-10) {
        throw std::runtime_error(
            "closest-point distance gradient below is only valid for d > 0; "
            "use EPA/contact normal or a signed safety-margin gradient in penetration.");
    }

    // 3. 距离方向向量
    Eigen::Vector3d n_hat = (p2 - p1) / dist;

    // 4. 获取两个 link 的雅可比
    const auto& cp = geom_model.collisionPairs[collision_pair_idx];
    auto joint_A = geom_model.geometryObjects[cp.first].parentJoint;
    auto joint_B = geom_model.geometryObjects[cp.second].parentJoint;

    Eigen::Matrix<double, 6, Eigen::Dynamic> J_A(6, model.nv);
    Eigen::Matrix<double, 6, Eigen::Dynamic> J_B(6, model.nv);
    J_A.setZero();
    J_B.setZero();

    // 注意：computeJointJacobian 返回 LOCAL 坐标系的雅可比，
    // 与下方世界系的接触点 p1/p2 不一致。
    // 正确做法：先调用 computeJointJacobians 缓存所有关节雅可比，
    // 再用 getJointJacobian 取 LOCAL_WORLD_ALIGNED 坐标系的雅可比。
    pinocchio::computeJointJacobians(model, data, q);
    pinocchio::getJointJacobian(model, data, joint_A,
                                pinocchio::LOCAL_WORLD_ALIGNED, J_A);
    pinocchio::getJointJacobian(model, data, joint_B,
                                pinocchio::LOCAL_WORLD_ALIGNED, J_B);

    // 5. 点雅可比 = 线速度雅可比 - [r]x * 角速度雅可比
    Eigen::Vector3d r_A = p1 - data.oMi[joint_A].translation();
    Eigen::Vector3d r_B = p2 - data.oMi[joint_B].translation();

    Eigen::Matrix<double, 3, Eigen::Dynamic> J_pA =
        J_A.topRows(3) - pinocchio::skew(r_A) * J_A.bottomRows(3);
    Eigen::Matrix<double, 3, Eigen::Dynamic> J_pB =
        J_B.topRows(3) - pinocchio::skew(r_B) * J_B.bottomRows(3);

    // 6. 距离梯度 = (J_pB - J_pA)^T * n，返回 nv x 1 列向量
    Eigen::VectorXd grad = (J_pB - J_pA).transpose() * n_hat;
    return grad;  // (nv x 1) 向量
}

工程边界：上面的最近点公式只适用于 $d > 0$ 的分离状态。进入穿透区后，min_distance <= 0 仍然把梯度置零会让优化器停在碰撞内部；正确做法是切到 EPA 给出的穿透方向/contact normal，或把约束写成带安全裕度的 signed-distance/hinge 代价，并保证碰撞内仍有把轨迹推出去的梯度。

⚠️ 概念误区：认为"碰撞距离函数处处可微"

新手想法："既然 GJK 给出了连续的距离值，那距离函数就是光滑的"

实际上：碰撞距离函数在**最近点跳变**时不可微。当两个凸体的相对位置变化时，最近点对可能突然从一条边跳到另一条边（对应 Voronoi 区域切换）。在这些跳变点上，距离函数连续但梯度不连续（存在次梯度 subgradient）。

工程影响：轨迹优化中，如果碰撞距离的最近点在相邻优化迭代间频繁跳变，梯度方向会振荡，导致优化器不收敛。

正确做法：(1) 使用安全裕度平滑化（见下一节），(2) 在最近点跳变时用加权平均或次梯度过渡，(3) 用有限差分回归测试检查目标版本的最近点/导数行为是否满足优化器需求

7.4 安全裕度与激活距离¶

在实际工程中，碰撞代价通常不是简单的距离函数，而是带有**安全裕度 (safety margin)** 和**激活距离 (activation distance)** 的平滑函数：

代价函数 c(d):

c(d) │
     │╲
     │ ╲
     │  ╲
     │   ╲
     │    ╲___________
     │
     └──┬──┬──────────── d
        0  d_safe  d_act

d < 0:          碰撞！代价很高
0 < d < d_safe: 太近，高代价
d_safe < d < d_act: 平滑衰减
d > d_act:      不关心（代价=0）

典型参数： - d_safe (安全裕度): 5-20 mm - d_act (激活距离): 50-100 mm

cuRobo 使用的碰撞代价函数：

\[c(d) = \begin{cases} \frac{1}{d_{\text{act}}^2}(d - d_{\text{act}})^2 & \text{if } d < d_{\text{act}} \\ 0 & \text{otherwise} \end{cases}\]

7.5 连续碰撞检测 (CCD)¶

离散碰撞检测（单点 $q$ 的碰撞查询）有一个致命缺陷——穿越问题：

时刻 t:   臂不碰撞          时刻 t+dt: 臂不碰撞
     │                           │
     │    ┌──┐                   │         ┌──┐
     │    │  │                   │         │  │
     │    └──┘                   │         └──┘
     ●──○──○──                   ──○──○──●
   (障碍物在中间)             (臂已经穿过障碍物!)

连续碰撞检测 (CCD) 通过检测两个时刻之间的**扫掠体积 (swept volume)** 来保证轨迹全程无碰撞。FCL 和 Coal 都支持 CCD。

练习¶

⭐⭐ 推导碰撞代价 $c(d) = (d - d_{\text{act}})^2 / d_{\text{act}}^2$ 的梯度 $\frac{\partial c}{\partial d}$。验证在 $d = 0$ 和 $d = d_{\text{act}}$ 处的梯度值。
⭐⭐⭐ 用 Pinocchio + Coal 计算 Franka Panda 某个 link 与一个球体障碍物的距离。用有限差分 $\frac{d(q+\epsilon e_i) - d(q-\epsilon e_i)}{2\epsilon}$ 验证 Coal 给出的距离梯度的正确性。
⭐⭐⭐ （跨章综合题）结合 M01 的雅可比计算和 M04 的碰撞距离，实现一个完整的"碰撞距离对关节角的梯度"计算。这将直接用于 M08 的轨迹优化代价函数。

8. MoveIt2 碰撞检测管线 ⭐⭐¶

8.1 MoveIt2 的碰撞检测架构¶

MoveIt2 的碰撞检测通过 pluginlib 实现可插拔——默认用 FCL，也可以切换到 Bullet 或自定义后端。

MoveIt2 碰撞检测栈:

PlanningScene
  ├── CollisionWorld (环境碰撞体)
  │   └── CollisionEnv (pluginlib)
  │       ├── CollisionEnvFCL     ← 默认
  │       ├── CollisionEnvBullet  ← Tesseract 偏好
  │       └── CollisionEnvDistanceField ← SDF 版本
  │
  ├── CollisionRobot (机器人碰撞体)
  │   └── 从 URDF collision 标签加载
  │
  └── AllowedCollisionMatrix (ACM)
      └── 从 SRDF 加载

8.2 PlanningScene 碰撞检查 API¶

#include <moveit/planning_scene/planning_scene.h>

auto robot_model = ...;  // 从 URDF/SRDF 加载
planning_scene::PlanningScene scene(robot_model);

// 添加碰撞物体
moveit_msgs::msg::CollisionObject obstacle;
obstacle.id = "table";
obstacle.primitives.push_back(shape_msgs::msg::SolidPrimitive());
obstacle.primitives[0].type =
    shape_msgs::msg::SolidPrimitive::BOX;
obstacle.primitives[0].dimensions = {1.0, 0.8, 0.02};
obstacle.primitive_poses.push_back(table_pose);
obstacle.operation = moveit_msgs::msg::CollisionObject::ADD;
scene.processCollisionObjectMsg(obstacle);

// 碰撞检查
collision_detection::CollisionRequest request;
request.contacts = true;
request.max_contacts = 10;
collision_detection::CollisionResult result;

moveit::core::RobotState state(robot_model);
state.setJointGroupPositions("panda_arm", q);
state.update();

scene.checkCollision(request, result, state);

8.3 OMPL 状态有效性检查¶

MoveIt2 的规划器通过 StateValidityChecker 接口调用碰撞检测——这个函数在采样规划中每次扩展都被调用，是性能热路径：

// MoveIt2 内部的 OMPL 状态有效性检查
class MoveItStateValidityChecker
    : public ompl::base::StateValidityChecker {
public:
    bool isValid(const ompl::base::State* state) const override {
        // 1. 将 OMPL 状态转换为 MoveIt RobotState
        // 2. 调用 PlanningScene::isStateColliding()
        // 3. 返回 !colliding
        return !planning_scene_->isStateColliding(robot_state);
    }
};

练习¶

⭐⭐ 在 MoveIt2 中手动添加一个 Box 障碍物到 PlanningScene，用 Franka Panda 规划避障轨迹。
⭐⭐ 对比 MoveIt2 的 FCL vs Bullet 碰撞后端在同一场景的规划性能。

9. GPU 加速碰撞检测——cuRobo 与 VAMP ⭐⭐⭐¶

9.1 为什么需要 GPU¶

当碰撞检测成为瓶颈（占规划时间 80-95%）时，GPU 并行是突破性能墙的关键手段。

方案	方法	7-DOF 碰撞检查	平台
FCL 串行	单核 CPU	~50-500 μs	x86
Coal 串行	单核 CPU (Nesterov)	~10-100 μs	x86
VAMP	SIMD 向量化 (AVX2/AVX-512)	~1-5 μs	x86 多核
cuRobo	GPU 并行 (CUDA)	~0.1-1 μs (批量)	RTX 3060+

9.2 VAMP——CPU SIMD 加速¶

VAMP (Thomason et al., ICRA 2024) 通过 AVX2/AVX-512 SIMD 指令集实现微秒级碰撞检查：

将多个碰撞对的球体近似同时加载到 SIMD 寄存器
一条指令同时处理 4/8 对碰撞检查
7-DOF Panda RRT-Connect 中位解 35 微秒
VAMP-MR 扩展到多臂协同规划（AAAI 2026 WoMAPF workshop oral）

9.3 cuRobo——GPU 并行碰撞¶

cuRobo (Sundaralingam et al., ICRA 2023) 使用 GPU 并行处理碰撞检测的两个关键模块：

ESDF (Euclidean Signed Distance Field)——环境碰撞： - 将环境表示为 3D 体素网格，每个体素存储到最近表面的距离 - GPU 并行计算 ESDF（比传统 CPU 快 10x） - 碰撞查询 = 在 ESDF 中查表 + 三线性插值（$O(1)$） - 梯度 = 中心差分或解析（SDF 的梯度就是到表面的方向向量）

球体近似——自碰撞： - 每个 link 用 2-5 个球体近似 - 总检测对数 ~100-500 - GPU 核函数一次处理所有对 - 支持批量轨迹优化（同时检查 1024 条候选轨迹）

cuRobo 端到端性能：

场景	平台	端到端耗时
桌面 pick-and-place	RTX 4090	~30 ms
复杂遮挡场景	RTX 4090	~80 ms
比 CPU 规划器 (OMPL) 快	—	60x

9.4 GPU vs CPU 的选择¶

你的碰撞检测性能需求?
│
├── 单次碰撞检查 <10 μs（实时控制）
│   └── 球体/胶囊体 + Coal (CPU 已足够)
│
├── 采样规划 <100 ms（MoveIt2 级）
│   └── FCL/Coal (CPU) → 如果不够则 VAMP (SIMD)
│
├── 轨迹优化 <30 ms（cuRobo 级）
│   └── cuRobo (GPU) → 需要 RTX 3060+
│
└── 批量验证 1000+ 配置
    └── cuRobo (GPU) 最优 → 一次 kernel 处理所有

练习¶

⭐⭐ 查阅 VAMP 论文 (ICRA 2024)。解释 VAMP 如何将碰撞检查的数据布局优化为 SIMD 友好格式。
⭐⭐⭐ 使用 cuRobo 的 Python API 在桌面场景中生成无碰撞轨迹。对比 OMPL (CPU) 和 cuRobo (GPU) 的规划耗时。
⭐⭐ 对比 FCL broad-phase 的 SAP vs DynamicAABBTree 在 1000 障碍物场景下的查询耗时。

10. 可视化与调试技巧 ⭐¶

10.1 碰撞检测的常见调试场景¶

问题	调试方法
规划器找不到解（所有采样都碰撞）	在 RViz 中可视化 collision geometry，检查是否比 visual 大很多
自碰撞误报（零位报碰撞）	检查 SRDF 的 `<disable_collisions>`，用 MoveIt Setup Assistant 重新生成
碰撞检查太慢	profile broadphase vs narrowphase 占比，考虑简化碰撞几何
轨迹优化碰撞梯度方向错误	用有限差分验证解析梯度，检查坐标系约定

10.2 RViz 碰撞可视化¶

在 RViz 中同时显示 visual mesh 和 collision mesh 是调试碰撞问题的第一步。通过 RobotModel display 的 Collision Enabled 选项可以切换显示。

⚠️ 思维陷阱：认为"collision mesh 和 visual mesh 应该一样"

新手想法："为什么我的 collision mesh 看起来和实际形状不同？"

实际上：collision mesh 通常比 visual mesh 简单得多（球/胶囊/简化凸包）。这是**有意为之**——复杂的 visual mesh 用于显示，简单的 collision mesh 用于快速碰撞检测。如果 collision mesh 和 visual mesh 完全一样（高精度三角网格），碰撞检测会慢 10-100 倍。

练习¶

⭐ 在 RViz 中同时显示 Franka Panda 的 visual mesh 和 collision mesh。找到 collision mesh 比 visual mesh"大"的 link，解释为什么这样设计。
⭐⭐ 编写一个调试工具：输入关节角 $q$，输出所有碰撞对的距离值。用这个工具找到 Franka Panda 的一个自碰撞配置。

累积项目：碰撞检测模块 ⭐⭐¶

本章新增模块¶

mini-manip/
├── src/
│   ├── load_panda.cpp              ← M01
│   ├── dynamics_benchmark.cpp      ← M02
│   └── collision_checker.cpp       ← M04 新增
├── include/
│   ├── dynamics_interface.hpp      ← M02
│   └── collision_checker.hpp       ← M04 新增
└── CMakeLists.txt                  ← M04 更新: 添加 Coal 依赖

练习¶

⭐ 实现 CollisionChecker 类：加载 Panda URDF + SRDF，提供 isCollisionFree(q) 和 minDistance(q) 接口。
⭐⭐ 将 CollisionChecker 集成到 M02 的 benchmark 框架中，测量不同碰撞几何类型的耗时。
⭐⭐⭐ （跨章综合题）结合 M01 的 FK、M02 的性能测量、M04 的碰撞检测，实现一个完整的"给定起始配置和目标配置，用线性插值检查路径上是否有碰撞"的 motion validator。这是 M07 采样规划的前置组件。

本章小结¶

知识点	核心要点	难度
两阶段架构	Broadphase (AABB Tree) + Narrowphase (GJK/EPA)	⭐⭐
GJK 算法	Support function + Simplex 迭代 + Voronoi 区域	⭐⭐⭐
碰撞几何选型	球(快) → 胶囊 → 凸包 → mesh → SDF(精)	⭐⭐
FCL vs Coal	Coal 快 2-15x (Nesterov) + 可微距离 (Pinocchio 生态独有)	⭐⭐
自碰撞与 ACM	SRDF disable_collisions + MoveIt Setup Assistant	⭐⭐
可微碰撞距离	距离梯度 = 几何梯度 x 雅可比（链式法则）	⭐⭐⭐
MoveIt2 碰撞管线	PlanningScene + CollisionEnv (pluginlib) + ACM	⭐⭐
GPU 加速	VAMP (SIMD, ~5 μs) / cuRobo (GPU, ~0.1 μs 批量)	⭐⭐⭐

延伸阅读¶

资源	难度	说明
Gilbert et al. (1988) "A Fast Procedure for Computing Distance..."	⭐⭐⭐	GJK 原始论文，IEEE J-RA
van den Bergen (2003) "Collision Detection in Interactive 3D Environments"	⭐⭐	碰撞检测教材，GJK/EPA/SAT 全覆盖
Ericson (2004) "Real-Time Collision Detection"	⭐⭐	最权威的碰撞检测工程教材，第4-9章
Pan et al. (2012) "FCL: A General Purpose Library..."	⭐⭐	FCL 设计论文，ICRA 2012
Montaut et al. (2022) "Collision Detection Accelerated..."	⭐⭐⭐	Coal 的 Nesterov 加速 GJK，RSS 2022
Sundaralingam et al. (2023) "cuRobo..."	⭐⭐⭐	GPU 并行碰撞 + 轨迹优化，ICRA 2023
Thomason et al. (2024) "VAMP: Vector-Accelerated Motion Planning"	⭐⭐⭐	SIMD 碰撞加速，ICRA 2024
libccd 源码 (~300 行 C)	⭐⭐	最清爽的 GJK/EPA 实现，学习 GJK 数学的最佳代码

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
MoveIt2 规划始终失败 ("Unable to find valid plan")	碰撞几何比 visual 大，或 ACM 配置不当	1. 在 RViz 显示 collision mesh 检查大小 2. 检查 SRDF 的 disable_collisions 3. 手动检查起/终点是否碰撞	§6, §8
自碰撞检测在零位报碰撞	相邻 link 的 collision 几何重叠但未在 SRDF 中排除	1. 重新运行 MoveIt Setup Assistant 2. 手动添加 disable_collisions 3. 检查 collision mesh 的 origin offset	§6.2 ACM
Coal 距离计算返回负值	两个几何体已穿透，负值是穿透深度	1. 确认 d<0 表示穿透是正常行为 2. 如需穿透方向用 EPA 3. 检查位姿更新是否遗漏	§3.6 EPA
碰撞检查耗时 >1 ms	碰撞几何太复杂（高面数 mesh）或 broadphase 效率低	1. 简化碰撞几何（凸分解/球近似） 2. 检查 broadphase 类型 3. 减少碰撞对数	§4, §9
轨迹优化碰撞梯度振荡	距离函数连续但最近特征切换处梯度不连续/不可微	1. 添加安全裕度 d_safe 平滑过渡 2. 使用 hinge loss 代替硬约束 3. 检查 activation distance	§7.3

几何体	\(S(\mathbf{d})\)	复杂度
球 (中心 \(c\), 半径 \(r\))	\(c + r \cdot \mathbf{d}/\\|\mathbf{d}\\|\)	\(O(1)\)
胶囊 (两端点 \(p_1, p_2\), 半径 \(r\))	\(\arg\max(p_1 \cdot \mathbf{d}, p_2 \cdot \mathbf{d}) + r \cdot \mathbf{d}/\\|\mathbf{d}\\|\)	\(O(1)\)
长方体 (半尺寸 \(h\))	\((\text{sign}(d_x) \cdot h_x, \text{sign}(d_y) \cdot h_y, \text{sign}(d_z) \cdot h_z)\)	\(O(1)\)
凸多面体 (\(N\) 个顶点)	\(\arg\max_{v \in \text{vertices}} v \cdot \mathbf{d}\)	\(O(N)\)
圆柱 (轴 \(\mathbf{a}\), 半径 \(r\), 高 \(h\))	解析公式	\(O(1)\)

数据结构	构建	查询	动态场景	用库
Dynamic AABB Tree	\(O(N \log N)\)	\(O(\log N)\)	优秀	FCL 默认
SAP (Sweep And Prune)	\(O(N \log N)\)	\(O(N)\) 最坏	中等	Bullet
Static BVH	\(O(N \log N)\)	\(O(\log N)\)	需重建	静态环境