4、碰撞检测原理：包围盒算法（AABB/OBB）、距离计算与GJK算法简介
各位好，我是老张。今天咱们聊一个绕不开的话题——碰撞检测。你想想看，机械臂在干活的时候，周围全是设备、工件，甚至还有操作员。要是撞上了，轻则停机，重则毁件伤人。所以，碰撞检测就是避障系统的“眼睛”和“神经”。
这一章，我会把常用的碰撞检测方法掰开揉碎了讲。从最简单的包围盒，到更精确的GJK算法，咱们一步步来。我当年刚入行时，就因为选错了碰撞检测方法，导致机械臂在调试时频频误报，后来才发现是包围盒类型没选对。嗯，这里面的坑，我帮你先踩一遍。
4.1 包围盒算法：让碰撞检测“快”起来
碰撞检测最朴素的想法，就是把两个物体的三角面片挨个算距离。但机械臂模型动辄几万个面，实时算根本扛不住。所以，工业界普遍的做法是先给物体套个“盒子”，用盒子之间的碰撞来快速过滤掉大部分不可能相撞的情况。
说白了，就是用简单几何体去近似复杂物体。常用的有两种：AABB 和 OBB。
4.1.1 AABB（轴对齐包围盒）
AABB 的全称是 Axis-Aligned Bounding Box。它的特点是：盒子的每条边都平行于世界坐标系的轴。你想象一个长方体，无论物体怎么旋转，这个盒子始终“站得笔直”。
它的好处是计算极快。判断两个AABB是否相交，只需要比较它们在X、Y、Z三个轴上的投影区间有没有重叠。如果三个轴都有重叠，那就相交；只要有一个轴不重叠，那就肯定没碰上。
AABB 相交检测核心逻辑：
两个AABB相交，当且仅当它们在三个轴上的投影区间都重叠。
代码实现也非常简单，我贴一段伪代码，你一看就懂：
// 判断两个AABB是否相交
bool AABBOverlap(AABB a, AABB b) {
// 检查X轴
if (a.max.x < b.min.x || a.min.x > b.max.x) return false;
// 检查Y轴
if (a.max.y < b.min.y || a.min.y > b.max.y) return false;
// 检查Z轴
if (a.max.z < b.min.z || a.min.z > b.max.z) return false;
// 三个轴都有重叠，则相交
return true;
}
但是，AABB 有个明显的缺点：它不随物体旋转。如果机械臂的连杆是斜着的，AABB 会包进去大量空白区域，导致“假阳性”碰撞。说白了，就是没撞上，但盒子说撞上了。这在避障中会引发不必要的急停。
我曾经踩过的坑： 在高速码垛场景中，用了AABB做碰撞检测。结果机械臂在接近目标点时频繁误报，导致节拍损失了15%。后来换成OBB，问题才解决。所以，如果你的机械臂运动姿态变化很大，AABB要慎用。
4.1.2 OBB（有向包围盒）
OBB 的全称是 Oriented Bounding Box。它和AABB最大的区别是：OBB 可以跟着物体一起旋转。盒子的方向由物体的主轴方向决定，所以它包裹得更紧实，空白区域更少。
判断两个OBB是否相交，比AABB复杂一些。常用的方法是“分离轴定理”（Separating Axis Theorem, SAT）。简单说，就是找一条轴，让两个OBB在这条轴上的投影不重叠。如果能找到这样一条轴，它们就不相交。
对于两个OBB，最多需要检查15条轴（每个OBB的3个面法向量，加上9个边叉积）。虽然计算量比AABB大，但精度高得多。
| 特性 | AABB | OBB |
|---|---|---|
| 方向 | 固定与世界坐标轴对齐 | 随物体旋转 |
| 紧密度 | 差，空白区域多 | 好，包裹更紧 |
| 相交检测速度 | 极快（3次比较） | 较慢（最多15条轴） |
| 适用场景 | 静态物体、粗略过滤 | 动态物体、高精度需求 |
| 更新开销 | 低（只需更新位置） | 高（需更新旋转矩阵） |
我的个人习惯： 在项目中，我会先用AABB做第一层粗筛，快速排除掉90%以上的不相干物体。对于剩下的候选对，再用OBB做精确检测。这样既保证了速度，又兼顾了精度。
4.2 距离计算：不只是“撞没撞”
碰撞检测只回答“是或否”，但在避障规划中，我们更想知道“有多远”。距离信息是生成排斥力、计算安全裕度的基础。
对于包围盒，距离计算通常转化为“点到盒子的距离”或“盒子到盒子的距离”。
- 点到AABB的距离： 把点投影到AABB的六个面上，取最近距离。如果点在盒子内部，距离为0。
- 点到OBB的距离： 先把点变换到OBB的局部坐标系，然后按AABB的方法计算。
- 两个包围盒之间的距离： 可以用GJK算法（后面会讲）或者直接计算两个凸体之间的最近点对。
我记得有一次做力控打磨，需要机械臂末端以恒定力接触工件。距离信息直接决定了力的大小。如果距离算不准，力控就会震荡。那段时间我天天盯着距离函数调参，最后发现是包围盒的膨胀系数设得太大了。
4.3 GJK算法简介：精确碰撞检测的利器
GJK 算法（Gilbert-Johnson-Keerthi）是计算两个凸体之间距离的经典算法。它不依赖包围盒，而是直接操作物体的顶点集。它的核心思想是：通过迭代搜索，找到两个凸体之间的“最近点对”。
GJK 算法的精髓在于“明可夫斯基差”（Minkowski Difference）。简单说，就是把两个凸体A和B，通过A - B的方式组合成一个新的凸体。如果这个新凸体包含原点，那么A和B就相交了。否则，原点到新凸体的距离就是A和B之间的距离。
下面这张图展示了GJK算法的核心流程：
GJK 算法的优势在于：
- 通用性强： 只要物体是凸体，就能用。对于凹体，可以先分解成多个凸体。
- 效率高： 迭代次数通常很少，平均3-5次就能收敛。
- 精度高： 直接计算最近点对，误差可控。
但GJK也有它的局限性。它只能处理凸体。对于非凸体，比如带孔洞的工件，需要先做凸分解。另外，GJK的初始支撑点选择会影响收敛速度。我个人的经验是，用上一次迭代的最近点作为下一次的初始猜测，能显著加速。
避坑指南： 我曾经在调试一个抓取场景时，发现GJK算法偶尔会陷入死循环。后来排查发现，是因为浮点数精度问题导致支撑点计算不准确。解决方案是在判断“原点是否在单纯形内”时，加一个很小的容差（比如1e-6）。
4.4 小结：如何选择碰撞检测方法？
说了这么多，到底该怎么选？我总结一下：
- 如果你追求极致速度，且物体姿态变化不大： 用AABB做粗筛就够了。
- 如果你需要高精度，且物体姿态多变： 用OBB做精确检测。
- 如果你需要精确的距离信息，且物体是凸体： 用GJK算法。
- 如果你面对的是复杂场景： 混合使用。先用AABB粗筛，再用OBB或GJK精检。