本文档属于 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>` 如何利用模板参数化不同的包围体类型？	02_C++基础与进阶/C++ 模板与泛型编程

本章目标¶

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

推导 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）的工程意义

本章知识导航¶

碰撞检测这个领域初看是"一堆算法和库的名字"，但它其实有一棵清晰的知识树。树根是**一个问题**：给定两个几何体（或一整个场景），快速判断它们是否相交、相距多远、以及（用于优化时）距离如何随关节角变化。所有内容都挂在这个根上。

                  「两个几何体碰了吗？相距多远？」
                              │
        ┌─────────────────────┼─────────────────────┐
        ▼                     ▼                     ▼
   规模问题 N²            单对几何问题          可微/连续问题
   (§2 两阶段)           (§3 narrowphase)      (§7 梯度/CCD)
        │                     │                     │
   ┌────┴────┐         ┌──────┼──────┐        ┌─────┴─────┐
   ▼         ▼         ▼      ▼      ▼        ▼           ▼
broadphase  BVH      GJK    EPA    SAT     距离梯度      CCD
(AABB树)  (单mesh)  (距离) (穿透)  (多面体)  (链式法则)  (扫掠/TOI)
   §2.1     §2.4     §3.4   §3.6   §2.2新    §7.3        §7.5
        │                     │
        └──────────┬──────────┘
                   ▼
            几何表示的选择 (§4)
       球 → 胶囊 → 凸包 → mesh → SDF
                   │
        ┌──────────┴──────────┐
        ▼                     ▼
   工程库 (§5,§6,§8)      性能加速 (§9)
   FCL/Coal/MoveIt2       SIMD/GPU/学习型

三条主线，读者应清楚自己在哪条线上：

主线	解决什么	核心章节	关键产出
规模线	$N$ 个物体如何避免 $O(N^2)$	§2（broadphase + BVH）	候选对列表
几何线	两个凸体的精确相交/距离	§3（GJK/EPA/SAT）	距离 $d$ 或穿透深度
应用线	怎样接入规划/优化/实时控制	§4-§9（表示、库、梯度、GPU）	可调用、可微、可加速的碰撞接口

读者读完每一节都应能回答："我刚学的这个东西在上面这棵树的哪个位置？它的输入是什么、输出给谁用？" 例如 GJK 的输入是两个 support function、输出是距离，它的输出喂给 §7 的距离梯度，再喂给 M08 的轨迹优化。

前置知识桥接¶

本章建立在几个前置章节之上。这里用 2-3 行重新激活每个前置要点，让你不必翻回去也能跟上：

回顾 M01（Pinocchio 运动学）：forwardKinematics(model, data, q) 计算每个关节/frame 的世界位姿 data.oMi[joint] / data.oMf[frame]（$\in SE(3)$）。本章把碰撞几何体"挂"在这些位姿上——FK 先算出 link 在哪，碰撞检测再判断 link 之间碰没碰。FK 是碰撞检测的上游。
回顾 M01 §7（几何雅可比）：$J(q)$ 把关节速度 $\dot q$ 映射到 link 的空间速度 $(v, \omega)$。在 §7 我们会用它把"几何距离对最近点的梯度"链式传播成"距离对关节角的梯度"。雅可比是碰撞梯度的桥梁。
回顾线性代数（凸集与分离超平面）：两个凸集不相交 $\Leftrightarrow$ 存在一个超平面把它们分开。这条定理是 GJK（§3）和 SAT（§2.2）的共同数学根基——GJK 在 Minkowski 差上找原点的最近点，SAT 直接枚举候选分离轴。
回顾 02_C++基础（模板特化）：模板用类型参数实现"同一接口、不同实现"。FCL 的 BVHModel<BV> 用 BV 参数化包围体类型（§2.4），是这一机制在几何库中的典型落地。

如果跳过本章会怎样¶

场景 1（规划器永远找不到解）：你在 M07 写了一个 RRT，但碰撞检查器要么太慢（每次 500 µs，规划 10 秒还没结果），要么把 collision mesh 配得比 visual 大一圈，导致所有采样都"碰撞"，规划器报 Unable to find valid plan。不理解 §2 的两阶段和 §4 的几何选型，你无法定位这是"性能问题"还是"几何配置问题"。
场景 2（轨迹优化不收敛）：你在 M08 用 CHOMP/TrajOpt 做避障优化，但碰撞代价的梯度方向在迭代间反复横跳，优化器在障碍物边缘震荡。不理解 §7.3 的"碰撞距离分段光滑、最近特征切换处梯度不连续"，你会误以为是优化器 bug，实则是碰撞距离的可微性陷阱。

预计阅读时间¶

模式	用时	路径
精读（推导 + 手算 + 跑代码）	1.5 周	全章，重点手推 §3 GJK/EPA 和 §7 梯度链
速读（理解架构 + 选型）	1 天	§1-§2 + §4 选型表 + §5 库对比 + §9 加速概览
速查（查 API/陷阱/故障）	按需	末尾 API 速查表 + 故障排查手册 + 各节 ⚠️ 陷阱框

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 $\mu$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 $\mu$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 $\mu$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.2.1 SAT（分离轴定理）——多面体相交的另一条路 ⭐⭐⭐¶

上表把 SAT 和 GJK 并列，但全章一直在讲 GJK，SAT 只是一个名字。这一小节把它讲透——因为它揭示了碰撞检测背后那条共同的数学根基（分离超平面），理解了 SAT 你会更深刻地理解 GJK 在做什么。

动机：GJK 在 Minkowski 差上迭代搜索，需要循环、需要 simplex 更新、需要终止判断——对于**顶点数很少的凸多面体**（典型如长方体只有 8 个顶点、6 个面），这套迭代机制显得"重"。能不能不迭代，直接一次性判定？SAT 给出了肯定的回答。

分离轴定理（Separating Axis Theorem）：两个凸集 $A$、$B$ 不相交，当且仅当存在一根轴 $\mathbf{a}$，使得 $A$ 和 $B$ 在 $\mathbf{a}$ 上的投影区间不重叠。这根轴 $\mathbf{a}$ 称为**分离轴**，它的法平面就是分离超平面。

       投影到分离轴 a 上:

   A 的投影    B 的投影
   ├────┤      ├──────┤        ← 不重叠 → 存在分离轴 → 不相交
   ────────────────────────► a

   A 的投影  B 的投影
   ├────┼──┤                   ← 重叠 → 这根轴不是分离轴
   ────────────────────────► a

本质洞察：SAT 和 GJK 求解的是**同一个问题的两面**。分离超平面定理说"凸集不相交 ⟺ 存在分离超平面"。GJK 从 Minkowski 差出发去**构造**那个最近点（最近点的支撑平面就是分离超平面）；SAT 则**枚举**所有可能的分离轴方向去**验证**。一个是构造性的（迭代逼近），一个是验证性的（有限枚举）——殊途同归。

关键问题：要测多少根轴？ 凸集之间有无穷多个方向，不可能全测。SAT 的精髓在于：对于**凸多面体**，如果存在分离轴，那么必然存在一根分离轴属于以下**有限集合**之一：

$A$ 的所有面法向量（每个面定义一个候选轴）
$B$ 的所有面法向量
$A$ 的边与 $B$ 的边的所有叉积 $\mathbf{e}_A^i \times \mathbf{e}_B^j$（边-边接触情形）

为什么是这三类？因为 Minkowski 差 $A \ominus B$ 也是凸多面体，它的每个面要么来自 $A$ 的面、要么来自 $B$ 的面、要么来自 $A$ 的边与 $B$ 的边"扫"出的面。分离超平面如果存在，必然平行于 $A \ominus B$ 的某个面——于是分离轴必然是上述三类法向量之一。这是一条可以严格证明的结论，不是工程近似。

⚠️ 概念误区：以为"测两个多面体的面法向量就够了"

新手想法："分离超平面要么贴着 A 的某个面，要么贴着 B 的某个面，测面法向量不就行了？"

实际上：当两个多面体是**边-边接触**（如两根细棍交叉）时，分离超平面既不平行于 A 的任何面、也不平行于 B 的任何面，而是同时"卡"在 A 的一条边和 B 的一条边上。这种情况的分离轴是两条边方向的叉积 $\mathbf{e}_A \times \mathbf{e}_B$。漏掉边-边叉积这一类，SAT 会把实际分离的物体误判为相交。

正确做法：三类轴一个都不能少。对两个长方体，面法向量各 3 个（盒子只有 3 个独立面法向，因为对面平行）+ 边方向各 3 个（同理只有 3 个独立边方向）的叉积 $3 \times 3 = 9$ 个 = 共 15 根轴——这正是 §2.4 包围体表格里"OBB 相交测试 SAT 15 轴"的来历。

盒子-盒子的 15 轴 SAT（OBB 相交测试的核心）：

\[\underbrace{3}_{A \text{ 面法向}} + \underbrace{3}_{B \text{ 面法向}} + \underbrace{3 \times 3}_{A,B \text{ 边叉积}} = 15 \text{ 根候选分离轴}\]

// OBB-OBB 相交测试（SAT 15 轴，简化骨架）
bool obbIntersect(const OBB& a, const OBB& b) {
    // a.axes[0..2], b.axes[0..2] 是各自的 3 个局部坐标轴（即面法向）
    Eigen::Vector3d axes[15];
    int n = 0;
    for (int i = 0; i < 3; ++i) axes[n++] = a.axes[i];   // A 面法向 ×3
    for (int i = 0; i < 3; ++i) axes[n++] = b.axes[i];   // B 面法向 ×3
    for (int i = 0; i < 3; ++i)                          // 边-边叉积 ×9
        for (int j = 0; j < 3; ++j) {
            Eigen::Vector3d cr = a.axes[i].cross(b.axes[j]);
            if (cr.norm() > 1e-9) axes[n++] = cr.normalized();
            // 平行边叉积为零向量，跳过（退化情形由面法向覆盖）
        }
    for (int k = 0; k < n; ++k) {
        // 在 axes[k] 上投影两个盒子，比较半径之和与中心距投影
        double rA = projectRadius(a, axes[k]);
        double rB = projectRadius(b, axes[k]);
        double dist = std::abs((b.center - a.center).dot(axes[k]));
        if (dist > rA + rB) return false;   // 找到分离轴 → 不相交，立即返回
    }
    return true;   // 15 轴都无法分离 → 相交
}

注意**短路返回**：一旦找到一根分离轴就立即返回"不相交"——这是 SAT 在"实际不相交"场景下极快的原因（平均只需测几根轴）。最坏情况（紧贴或相交）才会测满 15 轴。

SAT vs GJK——为什么工业库默认 GJK 而非 SAT？

维度	SAT	GJK
适用几何	仅凸多面体（需顶点/边/面拓扑）	任意凸体（只需 support function）
球/胶囊/圆柱	❌ 无面法向，不适用	✅ 解析 support function
输出	相交/分离（+ 最小平移向量 MTV）	距离 + 最近点（分离时）或交由 EPA 算穿透
距离查询	❌ 只能判相交，不能给分离距离	✅ 直接给分离距离
轴数随复杂度	$O(F_A + F_B + E_A E_B)$，高面数 mesh 爆炸	迭代次数与面数弱相关（3-10 次）
实现复杂度	中（但边-边情形易漏）	高（simplex/Voronoi），但已有成熟库

反事实推理：如果用 SAT 替代 GJK 做机械臂的凸包碰撞会怎样？ - 一个 link 凸包典型有 50-200 个面、上百条边 → 边-边叉积达 $100 \times 100 = 10^4$ 根轴 - 每根轴都要投影所有顶点求区间 → 单次检测 $\sim 10^4 \times 200$ 次点积，远慢于 GJK 的几次迭代 - 且 SAT 不给分离距离，轨迹优化（§7）要的距离梯度无从谈起 - 结论：SAT 只在**低面数固定几何**（盒子、AABB/OBB 包围体本身的相交测试）上有优势——这正是它在 BVH 包围体层（§2.4）而非 mesh narrowphase 层的定位。

所以本章后续 narrowphase 全程用 GJK：它通用、给距离、且对高面数几何更友好。SAT 退居为**包围体层**（OBB 树节点的快速剪枝）的专用工具。这就是为什么 §2.4 的包围体表里 OBB 用 SAT，而 §3 的 mesh 距离用 GJK——它们服务于不同的层次。

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> 等，这是 C++ 模板特化在几何库中的典型应用。

// 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 树

2.4.1 BVH 是怎么建起来的——自顶向下分裂 ⭐⭐⭐¶

上面 endModel() 一行就"自动构建 BVH 树"，但这棵树到底怎么建？理解构建过程才能理解为什么 BVH 能加速、以及什么时候它会退化。

BVH 构建有两条经典路线，FCL/Coal 默认用**自顶向下（top-down）递归分裂**：

build(triangles):
  1. 计算包住所有三角形的包围体 BV（叶子层会换成更紧的 BV）
  2. 若三角形数 <= 阈值(如1) → 建叶节点，返回
  3. 选一根"分裂轴"——通常取三角形质心分布方差最大的坐标轴
  4. 按质心在该轴上的投影，用中位数把三角形分成左右两半
  5. 对左半、右半分别递归 build()
  6. 当前节点的 BV = 左子 BV ∪ 右子 BV

        根 BV (包住全部 1000 三角形)
        /                    \
   左 BV(~500)            右 BV(~500)
    /      \               /      \
 ...      ...           ...      ...
  │        │             │        │
[叶:三角形] [叶]        [叶]      [叶]

为什么按"质心方差最大的轴"分裂？ 因为我们希望左右两半的包围体**尽量不重叠**——重叠越小，查询时"两边都得进"的概率越低，剪枝效率越高。沿三角形最"铺得开"的方向切，得到的两半在空间上分得最开。这与 kd-tree 选分裂维度的思路一致。

跨领域类比：BVH 的自顶向下分裂和**快速排序**几乎是同一套骨架——选一个 pivot（分裂轴上的中位数）、按 pivot 划分成两组、递归处理两组。区别在于快排划分的是"数值大小"、目标是有序；BVH 划分的是"空间位置"、目标是两组的包围体重叠最小。连复杂度都一样：理想划分 $O(N \log N)$，最坏退化 $O(N^2)$。

本质洞察：BVH 加速的本质是**把"测试一次就能排除一批"变成可能**。线性遍历 $N$ 个三角形，每个三角形提供 1 bit 信息（碰/不碰）。BVH 让一次"查询体 vs 内部节点 BV"的测试，在不相交时一次性排除整棵子树（可能上百个三角形）——用一次 $O(1)$ 的 BV 测试换取 $O(\text{子树大小})$ 的三角形测试。信息论上，这是把"逐个排除"升级为"成批排除"。

BVH 的遍历——双树同步下降：两个 mesh 各有一棵 BVH，碰撞检测时**同步遍历两棵树**：

// 两棵 BVH 的碰撞遍历（递归骨架）
void traverse(const BVNode& a, const BVNode& b, Result& res) {
    if (!overlap(a.bv, b.bv)) return;          // 两节点 BV 不相交 → 整对子树剪掉
    if (a.isLeaf() && b.isLeaf()) {
        // 两个叶子 → 真正的三角形-三角形相交测试
        if (triTriIntersect(a.tri, b.tri)) res.addContact(a.tri, b.tri);
        return;
    }
    // 启发式：展开"更大"的那个节点，让 BV 更快收紧
    if (b.isLeaf() || (!a.isLeaf() && a.bv.size() > b.bv.size())) {
        traverse(a.left,  b, res);
        traverse(a.right, b, res);
    } else {
        traverse(a, b.left,  res);
        traverse(a, b.right, res);
    }
}

注意这里有两层 BV 测试在协同：节点级 BV（AABB/OBB）做粗剪枝，只有当两个叶子的 BV 都相交，才落到最底层做**三角形-三角形**精确相交。一个 $1000 \times 1000$ 三角形的 mesh 对，朴素全量是 $10^6$ 次三角形测试；BVH 在典型场景下把它降到几十到几百次。

⚠️ 编程陷阱：把 BVH（单 mesh 内部加速）和 broadphase（多物体之间加速）搞混

错误描述：初学者看到"AABB 树"既出现在 broadphase 又出现在 BVH，以为是同一个东西

现象：试图用一棵 BVH 管理整个场景的所有物体，或试图用 broadphase 的 Dynamic AABB Tree 加速单个 mesh 内部的三角形

根本原因：两者层次不同——broadphase 的树，叶子是"整个物体的 AABB"，用于在 $N$ 个物体间剪枝；BVH 的树，叶子是"一个三角形"，用于在单个 mesh 的成千上万三角形间剪枝。一个管"物体之间"，一个管"三角形之间"

正确做法：场景级用 broadphase（§2.1，物体动就更新），mesh 级用 BVH（§2.4，mesh 不变形就只建一次）。两层串联：broadphase 筛出候选物体对 → 对每对调 narrowphase → 若几何是 mesh 则进入 BVH 遍历

2.4.2 Dynamic AABB Tree 的"胖盒子"——为什么动态场景不用每帧重建¶

回到 §2.1 的 broadphase。机械臂每个控制周期都在动，link 的 AABB 时刻在变。如果每帧都重建整棵树（$O(N \log N)$），broadphase 自己就成了瓶颈。Dynamic AABB Tree 用一个巧妙的工程技巧避免频繁重建——胖 AABB（fat AABB）：

树里存的不是物体的**紧贴 AABB**，而是向外扩了一圈裕量、并沿运动方向"拉长"的**胖 AABB**
物体小幅移动时，紧贴 AABB 仍待在胖 AABB 内部 → 树**不需要更新**
只有当紧贴 AABB **越出**胖 AABB 边界时，才把该叶子从树里删除并重新插入

   胖 AABB (broadphase 实际存储)
   ┌─────────────────┐
   │  ┌────┐         │   物体在胖盒子内移动 → 树不动
   │  │物体│ → → →   │
   │  └────┘  ┌────┐ │
   │          │物体│ │   物体仍在胖盒子内 → 仍不更新
   │          └────┘ │
   └─────────────────┘
              ┌────┐
              │物体│       物体越出 → 删除+重插该叶子
              └────┘

这把"每帧 $O(N \log N)$ 重建"降为"每帧仅 $O(\log N)$ 处理少数越界叶子"。查询复杂度约为 $k \cdot \log N$（$k$ 为实际相交对数）。代价是胖 AABB 比紧贴 AABB 松，broadphase 会多放过一些"假阳性"候选对给 narrowphase——这是典型的**用一点精度换大量更新成本**的工程权衡。Bullet 的 btDbvtBroadphase、Box2D 的 b2DynamicTree 都采用这一设计，FCL 的 DynamicAABBTreeCollisionManager 同源。

本质洞察：胖 AABB 体现了一个反复出现的系统设计原则——用空间松弛换时间稳定。紧贴 AABB 精确但"敏感"（动一点就要更新树），胖 AABB 松弛但"迟钝"（动很多才更新）。在高频更新的动态场景里，"迟钝"恰恰是优点——它把昂贵的结构调整摊薄到偶发的越界事件上。同样的思想也出现在缓存行预取、数据库的页分裂裕量中。

练习¶

⭐⭐ 画出 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.5.1 2D GJK 的 7 种 Voronoi 情况——把"最难的部分"讲透 ⭐⭐⭐¶

上面说"2D GJK 只有 7 种情况"，但没有展开。这一小节把这 7 种情况逐一列清——它是理解 update_simplex 的钥匙，也是 3D 那 15 种情况的"降维预演"。掌握 2D 的 7 种，3D 的 15 种就只是同一套逻辑的机械扩展。

Voronoi 区域是什么？ 给定一个 simplex（2D 中是点、线段或三角形），把整个平面按"离 simplex 的哪个特征（顶点/边/内部）最近"划分成若干区域，每个区域就是一个 Voronoi 区域。update_simplex 要做的事就是：判断原点落在当前 simplex 的哪个 Voronoi 区域，据此（a）丢掉对"逼近原点"无贡献的顶点，（b）确定下一步的搜索方向。

情况按 simplex 的维度分组：

1-simplex (线段 AB):                2-simplex (三角形 ABC):
   ┌── 顶点 A 区域                      ┌── 顶点 A/B/C 区域 (3种)
   │   A●━━━━━━●B                       │       A
   │   ↑ 边 AB 区域 ↑                   │      ╱●╲
   └── 顶点 B 区域                      │     ╱   ╲     边 AB/BC/CA 区域 (3种)
                                        │    B●━━━━●C
   2D 共: 顶点A + 顶点B + 边AB           │   三角形内部 = 包含原点! (1种)
        = 3 种情况                       └── 共 3+3+1 = 4 种情况

   总计 3 + 4 = 7 种 Voronoi 情况

逐情况分析（以原点 $O$ 为查询点，最新加入的点记为关键点）：

#	simplex	原点所在区域	动作（保留谁 / 新方向）
1	线段 AB	顶点 A 一侧	退化为 `{A}`，方向 = $-A$
2	线段 AB	顶点 B 一侧	退化为 `{B}`，方向 = $-B$
3	线段 AB	边 AB 中间	保留 `{A,B}`，方向 = 垂直 AB 指向原点
4	三角形 ABC	顶点区域	退化为单点，方向指向原点
5	三角形 ABC	边 AB/BC/CA 区域	退化为对应边，方向垂直该边指向原点
6	三角形 ABC	三角形内部	包含原点 → 相交！返回碰撞
7	（任意）	新点未越过原点	提前终止：不相交，当前最近点即答案

实际实现里有一个关键的**剪枝优化**：每次只可能新增一个点，所以**不必检查所有区域**。例如 2-simplex 的情况，因为上一步原点已经在 AB 这条边的"外侧"才会加入新点 C，所以原点**不可能**落在"顶点 A 区域"或"顶点 B 区域"（否则上一步就该丢弃 A 或 B 了）。这把要检查的区域数从 7 个砍到每步 2-3 个。这也是为什么工业实现里 doSimplex2 / doSimplex3 看似在做大量分支，实则每次只走其中一条路径。

def update_simplex_2d(simplex, d):
    """2D simplex 更新——返回 (新simplex, 新方向, 是否含原点)"""
    if len(simplex) == 2:           # 线段 AB（A 是最新加入点）
        A, B = simplex[1], simplex[0]
        AB, AO = B - A, -A
        if dot(AB, AO) > 0:         # 情况 3：原点在边 AB 投影范围内
            d_new = triple_cross(AB, AO, AB)   # 垂直 AB 指向原点
            return [B, A], d_new, False
        else:                       # 情况 1：原点在 A 外侧
            return [A], AO, False   # 丢弃 B
    else:                           # 三角形 ABC（A 是最新加入点）
        A, B, C = simplex[2], simplex[1], simplex[0]
        AB, AC, AO = B - A, C - A, -A
        abc = cross_2d(AB, AC)      # 三角形朝向
        # 检查边 AC 区域
        if cross_2d(AC, abc) * dot(... ) > 0:   # 伪代码：原点在 AC 外侧
            return [C, A], perp_toward_origin(AC, AO), False
        # 检查边 AB 区域
        if cross_2d(abc, AB) * dot(...) > 0:     # 原点在 AB 外侧
            return [B, A], perp_toward_origin(AB, AO), False
        # 都不在外侧 → 情况 6：原点在三角形内部
        return simplex, None, True   # 相交！

本质洞察：Voronoi 区域判断的"难"不在数学，而在**完备性**——必须保证每个原点位置都被恰好一个分支捕获，不重不漏。2D 的 7 种、3D 的 15 种，本质是"把空间按 simplex 特征做无缝划分"。一旦理解它是一个**完备的空间划分**而非"一堆 if 判断"，代码里那些看似随意的点积/叉积符号判断就有了清晰的几何含义：每个判断都在回答"原点在这个特征的哪一侧"。

跨领域类比：2D→3D 的 Voronoi 情况扩展（7→15），与有限元从三角形单元到四面体单元的扩展是同构的。三角形有 3 顶点 + 3 边 + 1 内部 = 7 个特征；四面体有 4 顶点 + 6 边 + 4 面 + 1 内部 = 15 个特征。相似之处在于"按单纯形的子特征做穷举"这一组合结构；不同之处在于 GJK 用它判断原点归属，有限元用它做基函数插值——同样的组合骨架，不同的用途。

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 $\mu$s	解析公式更快
总耗时 (凸包-凸包, 100 顶点)	~2-5 $\mu$s	FCL 实现
总耗时 (凸包-凸包, 500 顶点)	~10-30 $\mu$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 $\mu$s
MoveIt2 规划 (10-100 Hz)	凸包 (默认)	平衡精度和性能
高精度装配任务	三角网格	需要精确的碰撞表面
GPU 轨迹优化 (cuRobo)	SDF + 胶囊体	GPU 并行友好
腿足 WBC 自碰撞	球体	最多 512 对，需极快

反事实推理：如果在 1 kHz 控制循环中使用三角网格做碰撞检测，会怎样？ - 单次碰撞检查 ~50 $\mu$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 模板设计（C++ 模板特化在碰撞检测中的应用）：

// 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 $\mu$s	0.08 $\mu$s	1.9x
凸包-凸包 (50 顶点)	3.2 $\mu$s	0.8 $\mu$s	4.0x
凸包-凸包 (500 顶点)	28 $\mu$s	2.1 $\mu$s	13.3x
mesh-mesh (1k 三角形)	85 $\mu$s	12 $\mu$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。

7.5.1 保守前进（Conservative Advancement）——CCD 的主流算法 ⭐⭐⭐¶

"检测扫掠体积"听起来简单，但**精确构造**一个旋转运动的扫掠体积是非凸、非解析的难题（旋转的盒子扫出来的形状很复杂）。工业界真正在用的是另一套思路——保守前进（Conservative Advancement, CA），由 Brian Mirtich 提出，FCL/Coal 的 ContinuousCollisionRequest 底层就是它。

核心问题：两个物体从时刻 $t=0$ 运动到 $t=1$，求**首次接触时刻（Time of Impact, TOI）** $t^* \in [0,1]$，使得它们恰好接触。若整段无接触则 $t^* > 1$。

保守前进的直觉：不去构造扫掠体积，而是**反复"安全地往前跳"**：

保守前进迭代:
  t = 0
  repeat:
    1. 在时刻 t 用 GJK 算当前最短距离 d(t)
    2. 估计两物体相互接近的最大速度上界 v_max
       （由相对线速度 + 相对角速度 × 最大半径 给出的上界）
    3. 安全步长 Δt = d(t) / v_max
       —— 在 Δt 内两物体最多接近 d(t)，绝不会穿透
    4. t ← t + Δt
  until d(t) < ε (接触) 或 t >= 1 (整段无碰撞)
  return t   # 即 TOI

   d(t0)            d(t1)      d(t2)
  ├──────┤        ├───┤      ├─┤
  t0─────►t1──────►t2────────►t3 ... → 收敛到 TOI
  每步跳的距离 = 当前间隙 / 最大接近速度（保证不跳过接触）

本质洞察：保守前进的精妙在于"用静态距离查询拼出动态接触时刻"。它从不显式构造扫掠体积，而是反复问 GJK"现在离多远"，再用一个速度上界把"距离"换算成"安全时间"。每一步都**保守**（宁可少跳、不可跳过接触），所以绝不会漏掉碰撞——这就是名字里"conservative"的含义。它把一个困难的连续问题，化归为一串它已经会解的离散距离查询。

跨领域类比：保守前进与数值积分里的**自适应步长**（如 RKF45）思想一致——都是"当前局部状况好（距离大/误差小）就大步走，状况危险（距离小/误差大）就小步走"。区别在于自适应积分用局部误差估计控制步长，保守前进用"距离 / 最大速度"控制步长，目标是绝不越过那个关键事件（接触 / 精度阈值）。

⚠️ 概念误区：以为"离散碰撞检测把时间步取小一点就等于 CCD"

新手想法："我把轨迹插值得足够密，每隔 1ms 查一次碰撞，不就不会穿过了吗？"

实际上：固定步长的离散检测，对**高速薄物体**永远存在漏检风险——子弹穿纸的经典问题。无论步长多小，只要物体在一个步长内移动超过障碍物厚度，就可能"这一帧在左边、下一帧在右边"，中间的穿透完全不被采样到。把步长取小只是降低概率，不能消除。

正确做法：用真正的 CCD（保守前进），它给出的是**整个连续区间**的保证，而非离散采样点的保证。在 M07 采样规划的 motion validator 里，相邻两个 milestone 之间就应该用 CCD（或足够保守的离散细分 + 物体最小厚度约束）来保证边的有效性，而不是简单插几个点查一下。

7.5.2 胶囊体距离的解析公式——补上 §4.2 欠的那笔账¶

§4.2 说"胶囊距离 = 线段距离 - 两半径，其中线段距离有解析公式"，但没给公式。这里补上——它是机械臂 link 碰撞检测里用得最多的解析式（cuRobo、足式 WBC 都靠它），值得讲清。

胶囊体 = 一条线段（轴）"膨胀"半径 $r$。两个胶囊的最短距离，等于它们两条轴线段的最短距离再减去两个半径：

\[d(\text{capsule}_A, \text{capsule}_B) = d_{\text{seg}}(\overline{P_1 P_2},\ \overline{Q_1 Q_2}) - r_A - r_B\]

所以关键是**两条 3D 线段的最短距离** $d_{\text{seg}}$。设线段 $A(s) = P_1 + s(P_2 - P_1)$、$B(t) = Q_1 + t(Q_2 - Q_1)$，$s, t \in [0, 1]$。令 $\mathbf{u} = P_2 - P_1$、$\mathbf{v} = Q_2 - Q_1$、$\mathbf{w}_0 = P_1 - Q_1$，要最小化 $\|A(s) - B(t)\|^2$。对 $s, t$ 求偏导置零，得线性方程组：

\[\begin{cases} (\mathbf{u}\cdot\mathbf{u})\, s - (\mathbf{u}\cdot\mathbf{v})\, t = -\,\mathbf{u}\cdot\mathbf{w}_0 \\ (\mathbf{u}\cdot\mathbf{v})\, s - (\mathbf{v}\cdot\mathbf{v})\, t = -\,\mathbf{v}\cdot\mathbf{w}_0 \end{cases}\]

记 $a = \mathbf{u}\cdot\mathbf{u}$、$b = \mathbf{u}\cdot\mathbf{v}$、$c = \mathbf{v}\cdot\mathbf{v}$、$d = \mathbf{u}\cdot\mathbf{w}_0$、$e = \mathbf{v}\cdot\mathbf{w}_0$，则无约束解为：

\[s^* = \frac{b e - c d}{a c - b^2}, \qquad t^* = \frac{a e - b d}{a c - b^2}\]

⚠️ 编程陷阱：直接用上面的无约束解，忽略 $s, t \in [0, 1]$ 的截断和平行退化

错误描述：算出 $s^*, t^*$ 直接代回求距离，不做 clamp

现象：当最近点落在线段端点之外（如两段平行错开、或一段很短），返回的距离偏小甚至错误；两段平行时 $ac - b^2 = 0$ 直接除零得 NaN

根本原因：(1) 无约束极值点可能在 $[0,1]$ 之外，此时真正的最近点在端点；(2) 平行时分母为零，几何上对应"无穷多对等距最近点"

正确做法：(1) 先判 $ac - b^2 \approx 0$（平行），退化为"点到线段"距离；(2) 把 $s^*, t^*$ clamp 到 $[0,1]$，clamp 后还要把固定一个参数、对另一个重新求极值并再次 clamp（标准做法见 Ericson《Real-Time Collision Detection》§5.1.9 的 ClosestPtSegmentSegment）。绝不要自己从零写——直接用 Coal/FCL 的 Capsule 距离或 Ericson 的参考实现

理论-工程桥接：正因为胶囊距离是**纯解析**（解一个 $2 \times 2$ 线性系统 + 几次 clamp，无迭代），它比凸包的 GJK（迭代 3-10 次）快一个量级。这就是 §4.3 选型表里"1 kHz 控制循环 / 腿足 WBC 自碰撞"清一色用球/胶囊的根本原因——不是它们更"准"，而是它们的距离有 $O(1)$ 闭式解，能塞进微秒级预算。几何选择的背后，永远是"这个几何的距离查询有没有解析解"这个工程问题。

练习¶

⭐⭐ 推导碰撞代价 $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 的轨迹优化代价函数。
⭐⭐ 实现两条 3D 线段的最短距离函数 closestPtSegmentSegment，处理 $s,t$ 的 clamp 与平行退化两种边界情况。构造一组平行错开的线段，验证你的实现不返回 NaN，并与"端点到线段"的暴力解对比。
⭐⭐⭐ 用保守前进手算一个一维 TOI：物体 A 从 $x=0$ 以速度 $1$ 向右，物体 B 静止在 $x=10$（半宽各 $1$）。从 $t=0$ 起按 $\Delta t = d(t)/v_{\max}$ 迭代，列出前 4 步的 $t$ 与 $d(t)$，观察它如何收敛到真实 TOI $=8$ 而绝不越过。

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 $\mu$s	x86
Coal 串行	单核 CPU (Nesterov)	~10-100 $\mu$s	x86
VAMP	SIMD 向量化 (AVX2/AVX-512)	~1-5 $\mu$s	x86 多核
cuRobo	GPU 并行 (CUDA)	~0.1-1 $\mu$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 障碍物场景下的查询耗时。

9.5 Neural Collision Checking——学习型碰撞检测 ⭐⭐⭐⭐¶

传统碰撞检测基于精确的几何计算（GJK/EPA），但在某些场景下存在瓶颈：

复杂环境频繁更新：仓储场景中障碍物频繁变化，每次更新 BVH 树有重建开销
点云场景：从深度相机获取的障碍物是点云而非几何原语，传统方法需要先做表面重建
高维配置空间：双臂系统（14-DOF）的碰撞检查在高维空间中尤其昂贵

Neural Collision Checking 的核心思想是：用神经网络学习一个从配置空间到碰撞概率的映射 $f_\theta(q) \in [0, 1]$，一次前向传播代替完整的 GJK 计算。

代表性工作：

方法	输入	输出	速度	精度
NeuralCBF (Dawson et al., 2023)	$q$ + 环境嵌入	安全值函数	~5 $\mu$s (GPU)	依赖训练数据
Config-Space SDF	$q$	到碰撞边界的距离	~10 $\mu$s	近似
ClearanceNet	$q$ + 点云	最小距离估计	~20 $\mu$s	保守估计

与传统方法的关系：Neural Collision Checking 适合做 broadphase 的"快速初筛"——用神经网络快速排除明确安全/碰撞的配置，只对不确定的配置调用精确的 GJK。这种混合策略可以将总碰撞检测时间降低 3-5 倍，同时保持精确方法的安全性保证。

本质洞察：Neural Collision Checking 不应该 单独用于安全关键场景——因为神经网络的预测没有形式化的正确性保证。它的正确用法是"快速初筛 + 精确验证"的两阶段架构，与传统 broadphase/narrowphase 的思想一脉相承。

9.6 GPU 碰撞加速的未来方向¶

随着 GPU 算力持续增长，碰撞检测的 GPU 加速正在从"专用工具"走向"通用基础设施"：

cuCollision 概念：将完整的 broadphase + narrowphase 管线移植到 GPU，包括 BVH 树构建和 GJK 求解
Warp (NVIDIA)：NVIDIA 的可微物理仿真框架，内置 GPU 碰撞检测，Python 优先 API
硬件光线追踪加速：RTX GPU 的 RT Core 原本为光线追踪设计，研究者正在探索将其用于碰撞射线投射（Ray-Based Collision）——从 link 表面发射射线检测碰撞距离，利用硬件加速达到亚微秒级

练习¶

⭐⭐⭐ 调研一篇 Neural Collision Checking 的论文，分析其训练数据如何生成（提示：通常是用精确碰撞检测器标注大量配置空间样本）。
⭐⭐⭐⭐ 设计一个"Neural Broadphase + GJK Narrowphase"的混合碰撞检测管线。估算相比纯 GJK 管线的加速比。

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 和 BVH 是同一个东西	broadphase 管"物体之间"（叶子=物体 AABB），BVH 管"单 mesh 三角形之间"（叶子=三角形）；两层串联	§2.4.1
SAT 测两个多面体的面法向量就够了	还必须测"边-边叉积"，否则边-边接触的分离会漏判；盒子共 15 轴	§2.2.1
工业库该用 SAT，因为它一次判定不迭代	SAT 不给分离距离、对高面数 mesh 轴数爆炸、不支持球/胶囊；GJK 才是 narrowphase 默认	§2.2.1
动态场景每帧都要重建 broadphase 树	胖 AABB 让物体小幅移动不触发更新，仅越界叶子才删除重插，$O(\log N)$/帧	§2.4.2
GJK 的 Voronoi 判断是"一堆 if"	它是对空间的完备无缝划分（2D 7 种 / 3D 15 种），每个判断在问"原点在特征哪一侧"	§3.5.1
用 GJK 的 simplex 距离就能当穿透深度	simplex 是 Minkowski 差的极粗内接近似；穿透深度需 EPA 迭代逼近真实边界	§3.6
碰撞距离函数处处可微	它分段光滑，最近特征切换处梯度不连续（次梯度），优化会震荡	§7.3
把时间步取够小就等于 CCD	固定步长对高速薄物体永远有漏检风险；CCD（保守前进）给的是连续区间保证	§7.5.1
collision mesh 应该和 visual mesh 一样	collision 故意更简单（球/胶囊/凸包），否则碰撞检测慢 10-100 倍	§10.2
MoveIt Setup Assistant 生成的 ACM 总是安全的	默认仅采样约 1 万配置，7 维空间极稀疏，极端配置可能漏；须验证 + 运行时监控	§6.5
`min_distance` 是负数说明 Coal 出错了	负值是正常的穿透深度（绝对值），由 EPA 给出	§3.6

本章小结¶

知识点	核心要点	难度
两阶段架构	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 $\mu$s) / cuRobo (GPU, ~0.1 $\mu$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
高速运动下偶发穿透（离散检测漏检）	固定步长离散碰撞检查跳过了薄障碍物（子弹穿纸）	1. 改用 CCD/保守前进做边检查 2. 估计单步最大位移 vs 障碍物最小厚度 3. milestone 间用 TOI 而非插值采样	§7.5.1
两个明明分开的细长物体被判相交	SAT/OBB 测试漏了边-边叉积轴	1. 确认 OBB 测试覆盖全 15 轴（含 $3 \times 3$ 边叉积） 2. 检查平行边叉积为零时是否正确跳过 3. 优先用 Coal 的成熟实现	§2.2.1
GJK 在某些位姿不收敛/返回错距离	自实现的 Voronoi 区域判断遗漏边界情况	1. 不要自写 3D GJK，换 libccd/Coal 2. 若学习用，先验证 2D 全 7 种情况 3. 检查 simplex 退化（共线/共面）	§3.5, §3.5.1

术语速查表¶

术语	英文 / 缩写	一句话定义
宽相 / 窄相	broadphase / narrowphase	两阶段碰撞架构：宽相用 AABB 粗剪枝出候选对，窄相做精确相交/距离
轴对齐包围盒	AABB (Axis-Aligned Bounding Box)	边平行于坐标轴的最小包围盒，相交测试仅 6 次比较
有向包围盒	OBB (Oriented Bounding Box)	可任意朝向的包围盒，比 AABB 紧，相交测试用 SAT 15 轴
包围体层次	BVH (Bounding Volume Hierarchy)	把单个 mesh 的三角形组织成包围体树，加速 mesh 内部碰撞
动态 AABB 树	Dynamic AABB Tree	broadphase 数据结构，用胖 AABB 支持动态场景增量更新
胖包围盒	fat AABB	向外扩裕量的 AABB，物体小幅移动不触发树更新
分离轴定理	SAT (Separating Axis Theorem)	凸集不相交 ⟺ 存在一根投影不重叠的分离轴
GJK 算法	Gilbert–Johnson–Keerthi	在 Minkowski 差上迭代构造 simplex，求凸体间距离/相交
EPA 算法	Expanding Polytope Algorithm	GJK 判相交后，扩展 polytope 逼近边界求穿透深度/方向
Minkowski 差	Minkowski difference $A \ominus B$	$\{a-b\}$；$A\cap B\neq\varnothing \Leftrightarrow 0\in A\ominus B$
支撑函数	support function	给定方向返回凸体最远点；GJK 唯一需要的几何接口
单纯形	simplex	GJK 维护的 0/1/2/3 维顶点集（点/线段/三角形/四面体）
Voronoi 区域	Voronoi region	按"离 simplex 哪个特征最近"对空间的划分；2D 7 种 / 3D 15 种
凸分解	convex decomposition	把非凸 mesh 拆成多个凸块（V-HACD / CoACD），使 GJK 可用
签名距离场	SDF (Signed Distance Field)	体素网格存到最近表面的距离，查询 $O(1)$，GPU 友好
连续碰撞检测	CCD (Continuous Collision Detection)	检测运动区间全程是否碰撞，避免高速穿透
保守前进	conservative advancement	CCD 主流算法：用静态距离查询迭代逼近首次接触时刻
首次接触时刻	TOI (Time of Impact)	两运动物体恰好接触的时刻 $t^*\in[0,1]$
允许碰撞矩阵	ACM (Allowed Collision Matrix)	标记哪些 link 对跳过碰撞检测的布尔矩阵（SRDF 配置）
安全裕度	security / safety margin	把碰撞体外膨胀的距离，使近接触提前被报告
激活距离	activation distance	碰撞代价开始非零的距离阈值
距离下界	distance_lower_bound	Coal 在无碰撞时给出的距离保证下界

API 速查表¶

以 Coal（曾名 HPP-FCL）/ FCL / Pinocchio 的核心调用为主。签名以概念为准，具体随版本微调，请以目标版本头文件为准。

// ── Coal 几何与查询 ───────────────────────────────────────────
coal::Box(x, y, z);  coal::Sphere(r);  coal::Capsule(r, lz);  // 基本凸体
coal::Transform3s tf;  tf.setTranslation(coal::Vec3s(...));    // 位姿

coal::CollisionRequest creq;
creq.security_margin = 0.005;          // 安全裕度（>0 时近接触按碰撞报告）
coal::CollisionResult cres;
coal::collide(geomA, tfA, geomB, tfB, creq, cres);
bool hit = cres.isCollision();

coal::DistanceRequest dreq;
dreq.enable_nearest_points = true;     // 要最近点对
coal::DistanceResult dres;
coal::distance(geomA, tfA, geomB, tfB, dreq, dres);
double d   = dres.min_distance;        // d<0 时绝对值为穿透深度
auto   pA  = dres.nearest_points[0];   // link A 上最近点（世界系）
auto   pB  = dres.nearest_points[1];

// ── FCL 等价接口（命名几乎一致，类型后缀 d 表示 double）────────
fcl::Boxd, fcl::Sphered; fcl::CollisionObjectd(geom, tf);
fcl::collide(&objA, &objB, request, result);
fcl::distance(&objA, &objB, dist_request, dist_result);
auto mesh = std::make_shared<fcl::BVHModel<fcl::OBBRSSd>>();  // 模板化 BVH
mesh->beginModel(); mesh->addTriangle(v0,v1,v2); mesh->endModel();

// ── Pinocchio + Coal 自碰撞 / 距离 ───────────────────────────
pinocchio::urdf::buildGeom(model, urdf, pinocchio::COLLISION, geom_model);
geom_model.addAllCollisionPairs();
pinocchio::srdf::removeCollisionPairs(model, geom_model, srdf);  // 应用 ACM
pinocchio::GeometryData geom_data(geom_model);
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
bool col = pinocchio::computeCollisions(model, data, geom_model, geom_data, q, false);
pinocchio::computeDistances(model, data, geom_model, geom_data, q);
double dij = geom_data.distanceResults[pair_idx].min_distance;

// ── 距离梯度所需雅可比（见 §7.3）─────────────────────────────
pinocchio::computeJointJacobians(model, data, q);              // 先缓存
pinocchio::getJointJacobian(model, data, joint, pinocchio::LOCAL_WORLD_ALIGNED, J);

函数 / 字段	作用	关键陷阱
`collide(...)`	布尔/接触查询，比 distance 快	`security_margin>0` 会把近接触判为碰撞
`distance(...)`	距离 + 最近点	`min_distance<0` 是穿透深度，非错误
`enable_nearest_points`	启用最近点输出	不开则 `nearest_points` 无效
`addAllCollisionPairs()`	加入全部 link 对	必须随后 `removeCollisionPairs` 应用 ACM
`getJointJacobian(LOCAL_WORLD_ALIGNED)`	取世界对齐雅可比	用 `LOCAL` 会与世界系最近点坐标系不一致（§7.3）
`BVHModel<BV>`	模板化 mesh 碰撞树	`endModel()` 才真正建树；之后改三角形需重建

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
M06 自动微分与代码生成	把 §7.3 的手写距离梯度升级为 AD 自动求导	距离-雅可比链式法则、碰撞距离的分段光滑性（决定能否放进 AD tape）
M07 采样规划	把本章碰撞检查器封装为 `StateValidityChecker` 与 motion validator	broadphase/narrowphase 性能（决定规划实时性）、CCD（边有效性，§7.5.1）
M08 轨迹优化	把 §7.4 的安全裕度代价 + §7.3 的距离梯度接入优化器	碰撞代价 $c(d)$、激活距离、梯度震荡的平滑处理
M14 MoveIt2 碰撞管线	展开 §8 的 PlanningScene / CollisionEnv / ACM 工程细节	ACM 构建与验证（§6.5）、pluginlib 后端切换

向后预告：本章把碰撞距离梯度"手写"了出来（§7.3 那段几十行的雅可比拼装）。你可能会觉得繁琐且易错——确实如此。M06 会展示如何用 CppAD/CasADi 让编译器自动生成这段梯度代码，但前提是你理解本章讲的"距离在最近特征切换处不可微"这一限制，否则 AD 出来的梯度在穿透区会失效。先理解手写版的边界，才能正确使用自动版。

研究实践建议¶

入门（⭐ / ⭐⭐，先建立可用的碰撞检测能力）： - 先用 Pinocchio + Coal 跑通自碰撞与环境距离查询（§6.3），把"FK → 更新几何位姿 → 查询"这条链打通，这是后续一切的基础。 - 几何表示从胶囊体起步（§4.2）——它对机械臂 link 近似好、距离有解析解、调试直观。先别碰高面数 mesh。 - 用 §6.5 的脚本为你的机器人生成并**验证** ACM，养成"任何自动生成的碰撞配置都要独立验证"的习惯。

进阶（⭐⭐⭐，深入算法与可微优化）： - 手算一遍 2D GJK（§3.7 示例）和 7 种 Voronoi 情况（§3.5.1），再去读 libccd 的 ccdGJKDist()——有了手算基础，150 行 C 代码会变得清晰。 - 实现 §7.3 的距离梯度并用有限差分验证。重点体会"最近特征切换"如何让梯度跳变——这是连接 M04 与 M08 的最关键的一道坎。 - 用保守前进（§7.5.1）实现一个简单的 motion validator，对比"密集插值采样"在高速场景下的漏检。

研究级（⭐⭐⭐⭐，面向前沿）： - 复现或评测 Coal 的 Nesterov 加速 GJK（Montaut et al. RSS 2022）相对原版 FCL 的加速比，理解"利用问题结构加速收敛"的迁移性。 - 探索学习型碰撞检测（§9.5）的"神经初筛 + GJK 精验"两阶段架构，但牢记神经网络无形式化安全保证——它只能放在 broadphase 位置，绝不能单独用于安全关键判定。 - 关注硬件光线追踪（RT Core）用于碰撞射线投射（§9.6）的新方向——这是 GPU 碰撞从"通用算力"走向"专用单元"的潜在拐点。

几何体	\(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)\)

主线	解决什么	核心章节	关键产出
规模线	\(N\) 个物体如何避免 \(O(N^2)\)	§2（broadphase + BVH）	候选对列表
几何线	两个凸体的精确相交/距离	§3（GJK/EPA/SAT）	距离 \(d\) 或穿透深度
应用线	怎样接入规划/优化/实时控制	§4-§9（表示、库、梯度、GPU）	可调用、可微、可加速的碰撞接口

数据结构	构建	查询	动态场景	用库
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)\)	需重建	静态环境

#	simplex	原点所在区域	动作（保留谁 / 新方向）
1	线段 AB	顶点 A 一侧	退化为 `{A}`，方向 = \(-A\)
2	线段 AB	顶点 B 一侧	退化为 `{B}`，方向 = \(-B\)
3	线段 AB	边 AB 中间	保留 `{A,B}`，方向 = 垂直 AB 指向原点
4	三角形 ABC	顶点区域	退化为单点，方向指向原点
5	三角形 ABC	边 AB/BC/CA 区域	退化为对应边，方向垂直该边指向原点
6	三角形 ABC	三角形内部	包含原点 → 相交！返回碰撞
7	（任意）	新点未越过原点	提前终止：不相交，当前最近点即答案

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

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

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

方法	输入	输出	速度	精度
NeuralCBF (Dawson et al., 2023)	\(q\) + 环境嵌入	安全值函数	~5 \(\mu\)s (GPU)	依赖训练数据
Config-Space SDF	\(q\)	到碰撞边界的距离	~10 \(\mu\)s	近似
ClearanceNet	\(q\) + 点云	最小距离估计	~20 \(\mu\)s	保守估计