碰撞检测是运动规划中最频繁调用的子程序——RRT每次扩展节点要做碰撞检测混合A*每次扩展运动基元要做碰撞检测B样条优化每次迭代要做碰撞检测。说白了碰撞检测的效率直接决定了整个规划算法的速度。碰撞检测的问题定义很简单给定两个几何体A和B判断它们是否相交。但简单的前提是几何体的表示方式——如果是两个球体距离公式一行代码搞定如果是两个复杂的三角网格模型计算量可能差几个数量级。一、包围盒方法——快速粗筛包围盒Bounding Volume是碰撞检测的第一道防线。用一个简单的几何体把复杂物体包起来先判断包围盒是否相交——如果不相交两个物体一定不相交直接跳过精确检测。常用的包围盒从简到复杂AABBAxis-Aligned B Box轴对齐包围盒用(xmin,xmax,ymin,ymax,zmin,zmax)表示。相交检测只需6次比较——极快。缺点是不能旋转物体旋转后包围盒要重新计算。OBBOriented Bounding Box有方向的包围盒可以跟随物体旋转。比AABB更紧凑贴合度更高但相交检测需要做SATSeparating 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算法——精确碰撞检测GJKGilbert-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——两个物体重叠了多少——用EPAExpanding 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的输出碰撞深度标量和碰撞法线方向向量。这两个信息在物理仿真计算接触力和轨迹优化计算排斥梯度中很有用。四、工程实践与开源库FCLFlexible Collision LibraryROS/MoveIt2的标配碰撞检测库。支持AABB/OBB包围盒、GJK/EPA精确检测、BVH层次加速。C实现性能好。Bullet Physics游戏引擎和机器人仿真PyBullet中常用。碰撞检测部分也是GJKEPA。DrakeMIT的机器人仿真库碰撞检测用自研的Signed Distance Function方法——对凸形状用GJK对非凸形状分解为凸部分分别检测。性能参考数据两个包含1000个三角面片的模型FCL用BVHGJK的碰撞检测约0.01-0.1ms。如果用原始三角面片两两对比1000×1000100万次检测需要几十毫秒。BVH加速比在100-1000倍。五、面试实战QGJK算法的核心思想是什么A通过闵可夫斯基差判断两个凸形状是否相交——如果闵可夫斯基差包含原点则相交。GJK迭代地构建包含原点的simplex不需要显式计算整个闵可夫斯基差。QGJK只能处理凸形状吗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篇 运动学约束规划——速度/加速度/加加速度限制的处理有任何问题欢迎评论区留言我会尽量回复。