碰撞检测是运动规划中最频繁调用的子程序——RRT每次扩展节点要做碰撞检测，混合A*每次扩展运动基元要做碰撞检测，B样条优化每次迭代要做碰撞检测。说白了，碰撞检测的效率直接决定了整个规划算法的速度。
碰撞检测的问题定义很简单：给定两个几何体A和B，判断它们是否相交。但"简单"的前提是几何体的表示方式——如果是两个球体，距离公式一行代码搞定；如果是两个复杂的三角网格模型，计算量可能差几个数量级。
一、包围盒方法——快速粗筛
包围盒（Bounding Volume）是碰撞检测的第一道防线。用一个简单的几何体把复杂物体包起来，先判断包围盒是否相交——如果不相交，两个物体一定不相交，直接跳过精确检测。
常用的包围盒从简到复杂：
AABB（Axis-Aligned B Box）：轴对齐包围盒，用(xmin,xmax,ymin,ymax,zmin,zmax)表示。相交检测只需6次比较——极快。缺点是不能旋转，物体旋转后包围盒要重新计算。
OBB（Oriented Bounding Box）：有方向的包围盒，可以跟随物体旋转。比AABB更紧凑（贴合度更高），但相交检测需要做SAT（Separating Axis Theorem）——在15个候选分离轴上投影，计算量比AABB大5-10倍。
Bounding Sphere：包围球，用一个球心和半径表示。相交检测只需算一次距离。缺点是对细长物体（比如机械臂连杆）贴合度差。
# AABB相交检测——6次比较 def aabb_intersect(box_a, box_b): return (box_a.xmin <= box_b.xmax and box_a.xmax >= box_b.xmin and box_a.ymin <= box_b.ymax and box_a.ymax >= box_b.ymin and box_a.zmin <= box_b.zmax and box_a.zmax >= box_b.zmin)工程上的标准做法：建立包围盒层次树（BVH, Bounding Volume Hierarchy）。把复杂物体分解为子部件，每个子部件有自己的包围盒，父节点的包围盒是所有子节点的并集。检测时从根节点开始——如果根节点的包围盒不相交，整棵子树都不用检测。
二、GJK算法——精确碰撞检测
GJK（Gilbert-Johnson-Keerthi）算法是精确碰撞检测的经典方法。它的核心思想很巧妙：不直接判断两个物体是否相交，而是计算它们的闵可夫斯基差（Minkowski Difference）是否包含原点。
闵可夫斯基差的定义：A ⊖ B = {a - b | a ∈ A, b ∈ B}。如果A和B相交，则存在a ∈ A和b ∈ B使得a = b，即a - b = 0——原点在闵可夫斯基差中。
GJK不需要显式计算整个闵可夫斯基差（那是个体积很大的几何体），而是迭代地找一个包含原点的simplex（单纯形——点、线段、三角形或四面体）。如果找到了，两个物体相交；如果找不到（simplex无法包含原点），不相交。
# GJK算法核心循环 def gjk(shape_a, shape_b): direction = (1, 0, 0) # 初始搜索方向 simplex = [support(shape_a, shape_b, direction)] direction = -simplex[0] # 朝向原点 while True: new_point = support(shape_a, shape_b, direction) if dot(new_point, direction) < 0: return False # 不相交 simplex.append(new_point) if contains_origin(simplex, direction): return True # 相交GJK的关键操作是support函数：给定一个方向d，找到A中沿d方向最远的点和B中沿-d方向最远的点，两者之差就是闵可夫斯基差中沿d方向最远的点。对于凸形状，support函数可以在O(log N)时间内完成。
GJK的时间复杂度：迭代次数通常不超过10次（对3D凸形状），每次迭代调用一次support函数。总时间复杂度O(log N)——比暴力三角面片对比快得多。
三、EPA算法——碰撞深度计算
GJK只能告诉你"是否碰撞"，不能告诉你"碰了多少"。如果你需要碰撞深度（penetration depth）——两个物体重叠了多少——用EPA（Expanding Polytope Algorithm）。
EPA在GJK找到的simplex基础上工作：
- 从GJK的simplex（一个包含原点的四面体）开始
- 找到离原点最近的面
- 在这个面的法线方向上做support查询，得到新点
- 用新点扩展多面体（添加新面，删除被遮挡的旧面）
- 重复直到最近面的距离收敛
# EPA的简化流程 def epa(gjk_simplex, shape_a, shape_b): polytope = Polytope(gjk_simplex) while True: face = polytope.closest_face_to_origin() new_point = support(shape_a, shape_b, face.normal) if dot(new_point, face.normal) - face.distance < epsilon: return face.distance # 碰撞深度 polytope.expand(new_point, face)EPA的输出：碰撞深度（标量）和碰撞法线方向（向量）。这两个信息在物理仿真（计算接触力）和轨迹优化（计算排斥梯度）中很有用。
四、工程实践与开源库
FCL（Flexible Collision Library）：ROS/MoveIt2的标配碰撞检测库。支持AABB/OBB包围盒、GJK/EPA精确检测、BVH层次加速。C++实现，性能好。
Bullet Physics：游戏引擎和机器人仿真（PyBullet）中常用。碰撞检测部分也是GJK+EPA。
Drake：MIT的机器人仿真库，碰撞检测用自研的Signed Distance Function方法——对凸形状用GJK，对非凸形状分解为凸部分分别检测。
性能参考数据：两个包含1000个三角面片的模型，FCL用BVH+GJK的碰撞检测约0.01-0.1ms。如果用原始三角面片两两对比（1000×1000=100万次检测），需要几十毫秒。BVH加速比在100-1000倍。
五、面试实战
Q：GJK算法的核心思想是什么？A：通过闵可夫斯基差判断两个凸形状是否相交——如果闵可夫斯基差包含原点则相交。GJK迭代地构建包含原点的simplex，不需要显式计算整个闵可夫斯基差。
Q：GJK只能处理凸形状吗？A：是的，GJK要求输入是凸形状。非凸形状需要先做凸分解（Convex Decomposition），分解成多个凸部分，再对每一对凸部分分别用GJK检测。VHACD是常用的凸分解算法。
Q：碰撞检测怎么加速？A：三层加速。第一层AABB包围盒粗筛（6次比较）。第二层BVH层次树剪枝（减少需要检测的物体对数量）。第三层GJK精确检测（迭代次数少，每次O(log N)）。三层叠加后碰撞检测的加速比可达1000倍以上。
Q：你在项目中碰撞检测怎么做的？A：用FCL做碰撞检测。机械臂每个连杆用圆柱体包围盒，障碍物也用简化几何。BVH层次树在场景初始化时构建，机械臂移动时只更新连杆的包围盒位置。单次碰撞检测<0.05ms，规划一次调用约200-500次碰撞检测，总时间<20ms。
小结
碰撞检测三层架构：包围盒粗筛 → BVH层次树剪枝 → GJK/EPA精确检测。GJK通过闵可夫斯基差判断凸形状是否相交，EPA计算碰撞深度。FCL是机器人领域的标配库。
面试中碰撞检测的必考点：GJK的核心思想（闵可夫斯基差+support函数）、包围盒的层次和加速比、非凸形状的处理方法（凸分解）。
下一篇讲运动学约束规划——速度/加速度/加加速度限制的处理。
如果这篇文章对你有帮助，欢迎点赞、在看、转发三连。 你的支持是我持续更新的最大动力。
「机器人软件开发面试·从入门到精通」连载系列
上一篇：第230篇 机械臂运动规划——关节空间和笛卡尔空间的规划策略
下一篇预告：第232篇 运动学约束规划——速度/加速度/加加速度限制的处理
有任何问题欢迎评论区留言，我会尽量回复。
