碰撞检测优化：FCL与Bullet引擎对比、碰撞矩阵配置、自碰撞与场景碰撞过滤、性能调优

各位工程师朋友，咱们接着聊。上一章我们把运动学规划的路子捋顺了，但规划出来的轨迹能不能用，还得看一个关键环节——碰撞检测。说白了，就是检查机械臂在运动过程中，会不会跟周围环境或者自己“打起来”。

这一块，我踩过的坑可不少。刚开始做项目时，总觉得碰撞检测嘛，不就是调个库嘛。结果真到现场，机械臂慢得像蜗牛，或者干脆检测不到障碍物，差点把价值几十万的夹具给撞了。嗯，从那以后，我再也不敢小看这个环节了。

核心观点：碰撞检测是MoveIt规划管道的“守门员”。它决定了规划出的路径是“纸上谈兵”还是“真能落地”。优化得好，规划速度快、安全性高；优化得不好，要么卡死，要么撞车。

一、两大碰撞引擎：FCL vs Bullet

MoveIt默认用的是FCL（Flexible Collision Library），但很多人不知道，它其实也支持Bullet引擎。我个人习惯，在仿真阶段用FCL，因为它轻量、启动快；但到了真机调试，尤其是需要更精细的连续碰撞检测时，我会切到Bullet。

为什么会这样？咱们看个对比：

特性	FCL	Bullet
核心算法	GJK + EPA（离散检测）	GJK + 连续碰撞检测（CCD）
性能特点	轻量，内存占用低，速度快	功能丰富，精度高，但计算开销大
适用场景	静态环境、低动态场景	高速运动、需要“穿透检测”的场景
配置复杂度	简单，开箱即用	需要额外配置物理参数

我在项目中遇到过这样一个情况：用FCL做规划，明明显示无碰撞，但机械臂一跑起来，末端工具就蹭到了旁边的支架。后来排查发现，是因为FCL默认只做离散碰撞检测——它只检查规划路径上的几个离散点，如果机械臂在两个检查点之间穿过了障碍物，它就检测不到。

换成Bullet的连续碰撞检测（CCD）后，问题就解决了。它会计算物体在时间步内的运动轨迹，确保不会“漏检”。当然，代价就是规划时间从0.5秒涨到了2秒。所以，这里有个权衡：你是要速度，还是要安全？

我的建议：如果环境是静态的，且机械臂运动速度不快（比如<0.5m/s），用FCL完全够用。如果环境有动态障碍物，或者机械臂跑得很快，请务必开启Bullet的CCD。

二、碰撞矩阵配置：给碰撞检测“上规矩”

碰撞矩阵（Collision Matrix），说白了就是一张“谁跟谁不能碰”的表格。MoveIt里通过AllowedCollisionMatrix（ACM）来管理。默认情况下，所有物体之间都会进行碰撞检测，但这会带来大量不必要的计算。

你想想看，机械臂的两个连杆之间，如果距离很远，有必要每帧都检测它们会不会碰撞吗？没必要。所以，我们要手动配置碰撞矩阵，告诉MoveIt：“这两个物体永远不会碰撞，别检测了。”

配置方式有两种：

通过SRDF文件静态配置：在.srdf文件中定义<disable_collisions>标签。适合那些“永远不可能碰撞”的固定对，比如相邻连杆。
通过API动态配置：在代码中调用setEntry()方法。适合运行时根据场景动态调整。

举个例子，我在调试一个六轴机器人时，发现规划速度很慢。一查日志，发现MoveIt在检测“基座”和“末端执行器”的碰撞——这两个部件一个在地上，一个在天上，怎么可能撞上？于是我在SRDF里加了一行：

<disable_collisions link1="base_link" link2="tool0" reason="Never" />

就这么一行，规划时间直接降了30%。

注意：配置碰撞矩阵时，千万别“一刀切”。我曾经为了省事，把“自碰撞”全关了，结果机械臂在奇异点附近自己把自己拧成了麻花。嗯，那次教训很深刻。

三、自碰撞与场景碰撞过滤

碰撞检测分两大类：自碰撞（机器人自己碰自己）和场景碰撞（机器人碰环境物体）。

自碰撞是必须保留的，因为机械臂在运动过程中，连杆之间确实可能发生干涉。但我们可以通过碰撞过滤来减少计算量。比如，相邻连杆（link1和link2）之间永远不可能碰撞，因为它们是物理连接的。这些对可以直接过滤掉。

场景碰撞则要复杂一些。我记得有一次做抓取任务，场景里有一个透明的玻璃罩。FCL默认把玻璃罩当成一个实心体，导致规划出来的路径都绕着它走，根本抓不到里面的零件。后来我手动把玻璃罩的碰撞模型改成了一个“空心壳”，问题才解决。

这里有个技巧：为场景中的物体创建简化的碰撞模型。比如，一个复杂的桌子，可以用几个长方体代替；一个圆柱形障碍物，可以用一个胶囊体代替。这样既能保证碰撞检测的准确性，又能大幅提升性能。

避坑指南：我曾经在场景里放了一个带纹理的STL模型，结果碰撞检测直接卡死。后来发现，FCL对高面数的网格模型处理效率很低。解决办法是：用convex decomposition（凸分解）把复杂模型拆成多个凸包，或者直接用基本几何体（box、sphere、cylinder）代替。

四、性能调优：让碰撞检测“飞”起来

性能调优，说白了就是“在保证安全的前提下，尽可能少干活”。我总结了几个实战经验：

开启碰撞检测缓存：MoveIt支持缓存碰撞检测结果。如果连续两次查询的机器人状态相同，直接返回缓存结果，不用重新计算。这个功能默认是关闭的，建议在ompl_planning.yaml中开启：enable_cached_collision_detection: true。
降低检测频率：在规划过程中，不需要每毫秒都做碰撞检测。可以设置一个“步长”，比如每0.01秒检测一次。对于低速运动，这个频率完全够用。
使用AABB包围盒加速：FCL和Bullet都支持AABB（轴对齐包围盒）的快速碰撞剔除。简单说，就是先用一个“大盒子”把物体包起来，如果两个大盒子都不相交，那里面的物体肯定不相交。这一步计算量极小，能过滤掉90%以上的碰撞对。

下面这张图，是我自己总结的碰撞检测优化流程，你可以参考一下：

最后，说一个我常用的“杀手锏”：在规划前先做一次“快速碰撞检查”。如果机械臂的起始状态或目标状态本身就处于碰撞中，那就不用浪费时间去规划了，直接报错。这个检查可以在planning_scene中通过isStateColliding()方法实现，几乎不花时间，但能避免很多无意义的计算。

一个小技巧：在调试阶段，可以开启MoveIt的display_collision_contacts可视化功能。这样你就能在RViz里看到哪些地方发生了碰撞，以及碰撞的深度。对于排查“为什么规划失败”特别有用。

嗯，关于碰撞检测的优化，今天就聊这么多。记住一句话：碰撞检测不是越精确越好，而是“够用就好”。在安全的前提下，尽可能减少计算量，这才是工程化的思维。