
碰撞检测这件事说简单也简单说坑也真不少。我最早接触3D碰撞检测是在做一个机械臂抓取仿真项目当时用包围盒粗略判断结果机械臂末端经常穿模——明明显示已经碰到零件了实际物理引擎里还差好几毫米。后来换成FCLFlexible Collision Library才把这个问题彻底解决。FCL是开源社区里做碰撞检测的老牌库支持包围盒、凸包、网格等多种碰撞体类型还能做连续碰撞检测和距离查询。这篇内容就是把我这几年用FCL踩过的坑、总结的套路完整梳理一遍从环境搭建到代码跑通再到实际项目里的调优经验尽量让刚接触碰撞检测的朋友少走弯路。不管你是做机器人仿真、游戏物理、还是3D打印前的模型干涉检查这套流程都能直接拿去用。1. 为什么碰撞检测值得单独拎出来做1.1 碰撞检测到底在解决什么问题很多人第一次听到碰撞检测会觉得这不就是判断两个东西有没有碰到吗能有多复杂。但真正上手做3D项目就会发现这件事的复杂度远超想象。举个最直观的例子一个由50万个三角面片组成的机械零件模型要和另一个30万面的装配体做干涉检查如果老老实实拿每个三角形去和对方每个三角形做相交测试计算量是50万乘以30万也就是1500亿次三角形对测试。这个量级放在任何单机上都是灾难。碰撞检测的核心任务就是在保证判断准确性的前提下把这种暴力计算的规模压下来。FCL做的事情本质上分两层第一层用简单的几何体比如AABB包围盒、OBB有向包围盒、球体、胶囊体做快速粗筛把明显不可能碰撞的物体对直接排除第二层才对可能碰撞的物体做精确的三角形级别检测。这个粗筛加精测的两阶段思路是所有成熟碰撞检测库的通用套路。FCL的全称是Flexible Collision Library最早由北卡罗来纳大学教堂山分校的Gamma研究组开发后来在机器人领域被广泛采用MoveIt、Gazebo这些机器人框架底层都用它做碰撞检测。它的优势在于支持的碰撞体类型丰富、API相对清晰、性能经过大量实际项目验证。当然它也不是没有缺点文档偏少、编译依赖有点绕这些后面会细说。1.2 哪些场景真的需要FCL不是所有3D项目都需要上FCL这种级别的库。如果你只是做个简单的网页3D展示用Three.js自带的包围盒判断就够了。但下面这几类场景FCL的价值就体现出来了机器人运动规划机械臂在规划路径时需要实时判断当前姿态下是否和周围环境、自身其他连杆发生碰撞。这个判断每秒钟可能要做上千次对性能要求极高。装配干涉检查在CAD领域检查两个零件装配到一起时是否会有物理干涉这是产品设计阶段的刚需。3D打印前的模型校验打印前要确认模型壁厚是否足够、是否有自相交、支撑结构是否和模型本体冲突。物理仿真引擎游戏或仿真软件里物体之间的碰撞响应需要先检测出碰撞点和碰撞法线FCL能提供这些几何信息。抓取规划机器人抓取物体时要判断夹爪闭合过程中是否会和物体或其他障碍物碰撞。我自己的项目主要集中在机器人抓取和装配仿真这两块所以后面的例子也会围绕这些场景展开。但底层原理是通用的你换成游戏或者3D打印场景思路完全一样。1.3 FCL和其他碰撞检测方案的对比市面上做碰撞检测的方案不少选型时容易挑花眼。我把几个主流方案拉出来对比一下方便你根据自己项目情况做判断方案碰撞体类型连续检测依赖复杂度适用场景FCLAABB/OBB/球/胶囊/凸包/网格支持中等依赖Eigen、libccd机器人、装配、仿真Bullet同上更丰富支持中等游戏、物理仿真ODE基本几何体部分支持低简单物理仿真Three.js内置包围盒/球不支持极低网页展示自研AABB树需自己实现需自己实现低特定优化场景选FCL的核心理由通常是你在做机器人相关开发生态里大家都用它和MoveIt、ROS集成方便或者你需要凸包和网格级别的精确检测但又不想引入Bullet那么重的物理引擎。如果你纯粹做游戏Bullet可能更合适因为它碰撞响应那一套是现成的。FCL只负责检测不负责响应碰撞之后物体怎么弹开、怎么变形得你自己处理。2. 环境搭建那些文档里不会写的坑2.1 依赖安装的先后顺序很关键FCL的编译依赖不算特别多但顺序搞错了会浪费很多时间。它主要依赖三个东西Eigen线性代数库、libccd凸体碰撞检测库、以及可选的OctoMap用于八叉树场景表示。我建议的安装顺序是先把Eigen装好再装libccd最后编译FCL。在Ubuntu环境下最省事的做法是用apt装依赖然后源码编译FCL# 安装基础依赖 sudo apt-get update sudo apt-get install -y cmake g git sudo apt-get install -y libeigen3-dev libccd-dev # 如果需要OctoMap支持 sudo apt-get install -y liboctomap-dev # 下载FCL源码 git clone https://github.com/flexible-collision-library/fcl.git cd fcl mkdir build cd build # 配置编译选项 cmake .. -DFCL_BUILD_TESTSOFF -DCMAKE_BUILD_TYPERelease # 编译安装 make -j$(nproc) sudo make install这里有几个细节值得说。第一-DCMAKE_BUILD_TYPERelease一定要加Debug模式下FCL的性能会差好几倍因为大量模板代码没有内联优化。第二-DFCL_BUILD_TESTSOFF可以跳过测试用例编译省不少时间除非你要改FCL源码本身。第三如果你的项目对C标准有要求FCL 0.6以上版本需要C110.7版本开始需要C14编译前确认一下你的编译器版本。提示如果你在编译时遇到undefined reference to ccdXXX这类链接错误八成是libccd没装好或者版本不匹配。FCL 0.6对应libccd 2.0FCL 0.7对应libccd 2.1版本对不上会出各种奇怪问题。2.2 CMake工程里怎么正确链接FCL装完FCL只是第一步在自己的项目里正确链接它才是真正容易翻车的地方。FCL的CMake配置文件做得不算特别友好很多人第一次用会碰到找不到头文件或者链接失败的问题。下面是我验证过能用的CMakeLists.txt写法cmake_minimum_required(VERSION 3.10) project(fcl_demo) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # 查找FCLREQUIRED表示找不到就报错 find_package(fcl REQUIRED) # 查找Eigen3 find_package(Eigen3 REQUIRED) add_executable(fcl_demo main.cpp) # 链接FCL和Eigen target_link_libraries(fcl_demo fcl Eigen3::Eigen )如果你是用apt装的FCLsudo apt-get install libfcl-devfind_package一般能直接找到。但如果是源码编译安装到自定义路径可能需要设置CMAKE_PREFIX_PATH环境变量指向你的安装目录。我遇到过最坑的情况是系统里同时存在apt装的旧版FCL和源码编译的新版FCLCMake找到了旧版导致API对不上编译报错。解决办法是在CMakeLists里显式指定FCL_DIR或者干脆把旧版卸干净。2.3 验证安装是否成功的最小测试装完之后别急着写复杂代码先跑一个最小测试确认环境没问题。下面这段代码创建两个球体判断它们是否碰撞#include fcl/fcl.h #include iostream int main() { // 创建两个球体半径分别为1.0和0.8 auto sphere1 std::make_sharedfcl::Spheredouble(1.0); auto sphere2 std::make_sharedfcl::Spheredouble(0.8); // 球1在原点球2在x轴方向距离1.5的位置 fcl::Transform3double tf1 fcl::Transform3double::Identity(); fcl::Transform3double tf2 fcl::Transform3double::Identity(); tf2.translation() fcl::Vector3double(1.5, 0, 0); // 创建碰撞对象 auto obj1 fcl::CollisionObjectdouble(sphere1, tf1); auto obj2 fcl::CollisionObjectdouble(sphere2, tf2); // 碰撞请求和结果 fcl::CollisionRequestdouble request; fcl::CollisionResultdouble result; // 执行碰撞检测 fcl::collide(obj1, obj2, request, result); if (result.isCollision()) { std::cout 发生碰撞 std::endl; std::cout 碰撞点数量: result.numContacts() std::endl; } else { std::cout 没有碰撞 std::endl; } return 0; }两个球半径加起来是1.8球心距离1.5所以应该发生碰撞。如果这段代码能编译运行并输出发生碰撞说明环境基本没问题。如果编译报错说找不到fcl/fcl.h检查一下头文件路径如果链接报错检查库路径和链接顺序。3. 碰撞体选型精度和性能的平衡术3.1 六种碰撞体的适用边界FCL支持的碰撞体类型挺多但每种都有明确的适用场景选错了要么精度不够要么性能拉胯。我把常用的几种列出来结合我的实际使用经验说说各自的门道球体Sphere是最简单的碰撞体只需要一个半径参数。它的碰撞检测速度极快因为球和球之间的距离判断就是圆心距和半径和的比较。但球体对形状的拟合能力很差一个长条形的物体用球体包络会浪费大量空间导致误报。我一般只在物体本身接近球形或者对精度要求不高的粗筛阶段用球体。AABB轴对齐包围盒是另一个极端它用一个和坐标轴对齐的长方体包住物体。AABB的优点是计算简单、更新快物体旋转时只需要重新计算包围盒的八个顶点。缺点是当物体旋转到45度左右时AABB会明显偏大包络不紧凑。AABB适合做第一层粗筛或者物体基本不旋转的场景。OBB有向包围盒解决了AABB旋转后包络不紧凑的问题它允许包围盒跟着物体一起旋转。OBB的包络比AABB紧得多但碰撞检测算法复杂不少用的是分离轴定理SAT。我在机械臂项目里对连杆用OBB效果比AABB好很多误报率明显下降。胶囊体Capsule可以理解成两端是半球、中间是圆柱的形状。它特别适合表示机器人连杆、人体骨骼这类细长且两端圆润的物体。胶囊体的碰撞检测有解析解速度快且精度不错是机器人领域用得很多的碰撞体。凸包Convex是用物体的凸包络作为碰撞体。对于凸多面体或者接近凸的物体凸包能提供很好的包络精度。FCL内部用GJK算法做凸体之间的碰撞检测效率不错。但要注意凸包只能表示凸形状凹形状用凸包会丢失细节。网格BVH Model是最精确的碰撞体直接用原始三角网格做检测。FCL会为网格构建BVH包围体层次结构树来加速查询。网格碰撞检测精度最高但性能开销也最大而且要求网格是封闭的流形否则可能出现检测异常。3.2 我的选型决策流程实际项目里怎么选碰撞体我总结了一个简单的决策流程。首先看物体形状如果物体本身是球或接近球直接用球体如果是细长杆状优先考虑胶囊体如果是块状且接近凸多面体用凸包如果形状复杂且凹才考虑网格。然后看精度要求如果只是做粗筛或者对误报容忍度高用AABB或球体如果要求精确检测用OBB、凸包或网格。最后看性能预算实时性要求高的场景比如每秒上千次检测尽量用简单碰撞体离线检查场景比如装配干涉检查可以用网格追求精度。我做过一个对比测试同一个机械臂模型用不同碰撞体做自碰撞检测性能差异很明显碰撞体类型单次检测耗时微秒相对精度适用阶段球体约2低粗筛AABB约5中低粗筛OBB约15中精筛胶囊体约8中高精筛凸包约30高精筛网格约200最高最终校验这个数据是在我的开发机上跑的具体数值会因硬件和模型复杂度不同而变化但相对关系是稳定的。可以看到网格检测比球体慢了整整两个数量级所以千万别一上来就用网格先用简单碰撞体筛掉大部分情况只对少数可疑对做网格检测。3.3 多碰撞体组合的实战技巧一个复杂物体往往不是单一碰撞体能完美表示的。比如一个机械臂连杆主体是圆柱形两端有法兰盘末端还有夹爪。这时候可以用多个碰撞体组合来表示一个物体FCL支持把多个碰撞体打包成一个CollisionObject。具体做法是创建多个基础碰撞体分别设置它们相对于物体坐标系的变换然后一起传给碰撞对象。这样检测时FCL会分别检测每个子碰撞体只要有一个碰撞就算整体碰撞。这种组合方式能在精度和性能之间取得很好的平衡——用简单碰撞体覆盖大部分体积关键部位用精确碰撞体。注意多碰撞体组合时子碰撞体之间的相对变换要算准。我踩过一次坑法兰盘的碰撞体位置偏了5毫米导致检测结果一直不对排查了大半天才发现是变换矩阵写错了。建议组合完成后先用可视化工具确认一下碰撞体位置。4. 完整代码实战从零跑通一个碰撞检测程序4.1 场景设计两个机械零件的干涉检查光讲理论没意思直接上一个能跑的完整例子。这个例子的场景是一个底座零件和一个待装配的轴检查轴插入底座的过程中是否会发生干涉。底座用一个带孔的方块表示轴用一个圆柱表示。为了简化我们用凸包和胶囊体来近似这两个零件。先定义场景和碰撞体#include fcl/fcl.h #include iostream #include vector using namespace fcl; // 创建一个表示底座的碰撞体用凸包近似带孔方块 std::shared_ptrConvexdouble createBaseConvex() { // 底座外形是一个长方体用8个顶点定义凸包 std::vectorVector3double vertices { {-0.1, -0.1, 0.0}, {0.1, -0.1, 0.0}, {0.1, 0.1, 0.0}, {-0.1, 0.1, 0.0}, {-0.1, -0.1, 0.05}, {0.1, -0.1, 0.05}, {0.1, 0.1, 0.05}, {-0.1, 0.1, 0.05} }; auto convex std::make_sharedConvexdouble(); convex-setVertices(vertices); return convex; } // 创建一个表示轴的碰撞体用胶囊体近似圆柱 std::shared_ptrCapsuledouble createShaftCapsule() { // 半径0.02长度0.15 return std::make_sharedCapsuledouble(0.02, 0.15); }这里底座用凸包是因为它是个凸多面体凸包能精确表示。轴用胶囊体是因为圆柱形物体用胶囊体近似精度足够而且检测速度快。如果你追求更高精度轴也可以用圆柱体或者网格但性能会下降。4.2 碰撞检测主循环的写法接下来是主检测逻辑。我们要模拟轴从上方逐渐插入底座的过程每一步都做碰撞检测int main() { // 创建碰撞体 auto base_convex createBaseConvex(); auto shaft_capsule createShaftCapsule(); // 底座固定在原点 Transform3double base_tf Transform3double::Identity(); CollisionObjectdouble base_obj(base_convex, base_tf); // 轴从上方逐渐下降 double start_z 0.2; // 起始高度 double end_z 0.0; // 终止高度 double step 0.01; // 步长 for (double z start_z; z end_z; z - step) { Transform3double shaft_tf Transform3double::Identity(); shaft_tf.translation() Vector3double(0, 0, z); CollisionObjectdouble shaft_obj(shaft_capsule, shaft_tf); CollisionRequestdouble request; CollisionResultdouble result; collide(base_obj, shaft_obj, request, result); std::cout 轴高度 z z 米, 碰撞状态: (result.isCollision() ? 碰撞 : 安全) std::endl; if (result.isCollision()) { std::cout 碰撞点数量: result.numContacts() std::endl; // 输出第一个碰撞点的位置 auto contact result.getContact(0); std::cout 碰撞位置: ( contact.pos[0] , contact.pos[1] , contact.pos[2] ) std::endl; break; // 检测到碰撞就停止 } } return 0; }这段代码的逻辑很直白轴从z0.2米的高度开始每次下降0.01米检测一次碰撞直到检测到碰撞或者降到z0。运行结果会告诉你轴下降到哪个高度时开始和底座干涉。4.3 编译运行和结果解读把上面的代码保存成main.cpp配合前面给的CMakeLists.txt编译运行mkdir build cd build cmake .. make ./fcl_demo正常的话你会看到类似这样的输出轴高度 z 0.2 米, 碰撞状态: 安全 轴高度 z 0.19 米, 碰撞状态: 安全 ... 轴高度 z 0.07 米, 碰撞状态: 碰撞 碰撞点数量: 1 碰撞位置: (0.02, 0, 0.05)这个结果告诉我们轴下降到0.07米高度时开始和底座碰撞。碰撞点在(0.02, 0, 0.05)正好是底座上表面边缘的位置。这个信息在实际项目里很有用——你可以根据碰撞点位置调整装配路径或者修改零件尺寸避免干涉。提示如果你的输出里碰撞高度和预期不符先检查碰撞体的尺寸参数和变换矩阵。我遇到过最常见的问题是单位不统一比如模型是毫米单位但代码里按米算结果差了一千倍。4.4 连续碰撞检测的补充上面的例子是离散检测每一步检测一个静态位置。但实际中物体是连续运动的如果运动速度快、步长太大可能出现在两个采样点之间穿过了障碍物但没被检测到的情况这叫隧穿。FCL提供了连续碰撞检测CCD来解决这个问题。连续碰撞检测需要提供物体的运动信息包括起始变换、终止变换以及运动时间。FCL会计算在这段时间内两个物体是否会发生碰撞以及碰撞发生的时间点。用法上把CollisionRequest里的enable_contact和连续检测标志打开然后调用continuousCollide函数。连续检测比离散检测慢一些但对高速运动的物体是必要的。我在做机械臂快速运动规划时就吃过离散检测的亏——机械臂末端快速移动时偶尔会穿过薄壁障碍物。后来改用连续检测虽然单次检测耗时增加了约30%但再也没出现过漏检。5. 性能调优让检测速度再快一个量级5.1 宽相和窄相的分工FCL内部把碰撞检测分成宽相broad phase和窄相narrow phase两个阶段。宽相负责快速排除明显不相交的物体对窄相负责对可能相交的物体对做精确检测。理解这个分工对性能调优很重要。宽相常用的数据结构是动态AABB树Dynamic AABB Tree。它把所有物体的AABB组织成一棵树查询时从根节点开始快速排除不相交的分支。如果你的场景里物体很多比如上百个宽相能筛掉90%以上的物体对窄相只需要处理剩下的少数对。窄相就是具体的碰撞检测算法球体用距离判断凸体用GJK算法网格用BVH树遍历。窄相的性能主要取决于碰撞体复杂度和物体对数量。调优的基本思路是先优化宽相减少进入窄相的物体对数量再优化窄相降低单个物体对的检测耗时。5.2 几个立竿见影的优化手段第一合理设置碰撞体复杂度。网格碰撞体虽然精确但BVH树的构建和遍历都很耗时。如果模型有几十万面片考虑先做网格简化或者用凸包分解把凹模型拆成多个凸块。我一般会把网格面片数控制在1万以内超过就做简化。第二利用碰撞矩阵过滤。很多物体对根本不需要检测比如机械臂相邻连杆之间、固定不动的环境物体之间。FCL本身不提供碰撞矩阵但你可以在调用检测前自己过滤。维护一个布尔矩阵标记哪些物体对需要检测能省掉大量无用计算。第三批量检测用BroadPhaseCollisionManager。如果你要检测多个物体之间的碰撞别一个个手动配对调用collide用FCL的管理器类。它内部会构建AABB树自动做宽相筛选。用法是创建管理器、注册所有物体、调用collide做批量检测。第四注意变换更新的开销。物体运动后需要更新它的变换这个操作会触发AABB重新计算。如果物体很多且频繁运动更新开销不可忽视。一个技巧是只在物体确实移动了才更新微小抖动可以忽略。5.3 实测性能数据与调优效果我在一个包含20个机械臂连杆和50个环境物体的场景里做过调优测试数据如下优化阶段单帧检测耗时相对提升优化前全网格逐个配对约45毫秒基准用AABB粗筛约12毫秒3.75倍换用凸包胶囊体约4毫秒11倍加碰撞矩阵过滤约1.5毫秒30倍用管理器批量检测约0.8毫秒56倍从45毫秒降到0.8毫秒提升了50多倍。这个提升主要来自三个方面碰撞体简化、无效对过滤、批量检测。其中碰撞体简化贡献最大因为网格检测实在太慢了。注意优化不能牺牲正确性。每次优化后都要用一组已知结果的测试用例验证确保没有漏检。我一般会保留一个金标准测试集优化前后都跑一遍对比结果。6. 踩坑记录那些让我加班到深夜的问题6.1 网格不封闭导致的检测异常有一次做3D打印模型校验用网格碰撞体检测模型自相交结果死活检测不出问题但切片软件明明报了自相交警告。排查了很久才发现那个模型的网格不是封闭的有几十个边界边。FCL的网格碰撞检测假设输入是封闭流形非封闭网格会导致BVH树构建异常检测结果不可靠。解决办法是在用网格碰撞体之前先检查网格是否封闭。可以用FCL的isManifold相关接口或者自己写个简单的边界边检查。如果网格不封闭要么修复网格要么改用凸包分解的方式。6.2 浮点数精度引发的边界误判碰撞检测里大量涉及浮点数比较精度问题很容易导致边界情况误判。我遇到过一个案例两个物体刚好相切理论上不算碰撞但浮点误差导致检测结果时有时无每次运行结果都不一样。FCL提供了一些容差参数比如碰撞请求里的security_margin和break_distance。合理设置这些参数能缓解边界抖动。我的经验是对于精密装配场景容差设小一点比如1e-6避免漏检对于一般场景容差可以设大一点比如1e-4减少边界抖动。6.3 变换矩阵的坐标系陷阱这个坑我踩过不止一次。FCL里的变换矩阵是物体坐标系到世界坐标系的变换但很多建模软件导出的模型本身带有自己的坐标系偏移。如果你直接把模型顶点读进来不做坐标系转换碰撞体位置就会偏。正确的做法是先确认模型顶点在哪个坐标系下然后计算从模型坐标系到世界坐标系的变换把这个变换设置给碰撞对象。如果模型有嵌套结构比如机械臂的连杆树每个连杆的变换要逐级累乘。6.4 多线程使用的注意事项FCL本身不是线程安全的多个线程同时调用同一个碰撞对象的检测函数可能出问题。如果你的项目需要并行检测有两种做法一是每个线程用独立的碰撞对象副本二是加锁串行化。第一种性能更好但内存开销大第二种简单但可能成为瓶颈。我在一个需要实时检测的项目里用了线程池每个线程持有独立的碰撞对象副本检测任务分发到不同线程。这样做的代价是内存占用翻了几倍但换来了接近线性的性能提升。7. 从检测到应用把结果用起来7.1 碰撞点信息的实际用途检测出碰撞只是第一步碰撞点信息才是真正有价值的东西。FCL的CollisionResult里包含碰撞点位置、碰撞法线、穿透深度等信息。这些信息可以用来做很多事情碰撞响应根据碰撞法线和穿透深度计算反弹力或分离向量。路径调整在运动规划中根据碰撞点位置调整路径避开障碍。装配力分析在装配仿真中根据碰撞点分布分析受力情况。可视化标注在3D界面上高亮显示碰撞位置方便调试。我做的机械臂抓取项目里就是根据碰撞点法线判断夹爪是否正对物体表面如果法线偏差太大就调整抓取姿态。7.2 距离查询的妙用除了碰撞检测FCL还提供距离查询功能能计算两个物体之间的最近距离。这个功能在避障场景里特别有用——你不仅要知道有没有碰撞还要知道离碰撞还有多远。距离查询的用法和碰撞检测类似把CollisionRequest换成DistanceRequest调用distance函数。返回结果里有最近距离值和最近点对。在运动规划里可以用距离值做代价函数引导规划器远离障碍物。7.3 和可视化工具的集成光看数字很难直观判断碰撞检测对不对配合可视化工具会高效很多。我一般用RVizROS的可视化工具或者自己写个简单的OpenGL窗口把碰撞体和检测结果画出来。碰撞体用半透明线框显示碰撞点用红色小球标注一眼就能看出问题。如果你不想搭可视化环境也可以用FCL自带的导出功能把碰撞体导出成OBJ文件用MeshLab之类的工具查看。这个方法简单但不够实时适合调试阶段用。8. 写在最后的一些个人体会用FCL这几年最大的感受是碰撞检测这件事难的不是调库而是想清楚你的场景到底需要什么精度、什么性能。我见过太多项目一上来就用最精确的网格检测结果性能扛不住又回头做优化白白浪费时间。正确的做法是先明确需求再选碰撞体最后才是写代码。另外碰撞检测的结果一定要验证。我养成了一个习惯每写一个检测逻辑都构造几个已知结果的测试用例比如两个明确相交的物体、两个明确分离的物体、两个刚好相切的物体跑一遍确认结果符合预期。这个习惯帮我提前发现了很多问题省下了大量调试时间。最后说个实用的小技巧如果你的项目对实时性要求极高可以考虑把碰撞检测放到单独的线程或者进程里主线程只管发检测请求和收结果。这样即使检测偶尔慢一下也不会卡住主循环。我在一个VR项目里就是这么做的效果不错。