3. 碰撞检测基础：AABB与OBB包围盒原理，以及如何用FCL库进行快速碰撞检测
各位好，欢迎来到第三章。今天聊一个绕不开的话题——碰撞检测。
做机械臂抓取，最怕什么？怕撞。撞工件、撞夹具、撞自己。我刚开始做机器人规划那会儿，就吃过这个亏。仿真里跑得挺好，一上真机，咔一声，末端撞到料框边缘了。后来一查，碰撞检测模型太粗糙，根本没检测到那个边角。
所以这一章，咱们把碰撞检测的基础打扎实。重点讲两种最常用的包围盒——AABB和OBB，以及一个工业级的碰撞检测库FCL怎么用。
3.1 为什么需要包围盒？
直接拿机械臂的三角网格模型去做碰撞检测，行不行？行，但太慢了。一个机械臂模型动辄几万个三角形，两两求交，算到你怀疑人生。
说白了，就是用一种简单的几何体，把复杂的物体“包”起来。先拿包围盒做粗检测，如果包围盒都碰不上，那里面精细的网格肯定碰不上。这招叫“粗筛+精检”，是工程上最常用的套路。
核心思想：用简单的几何体代替复杂模型，先做快速排除，再做精确判断。
3.2 AABB包围盒
AABB，全称Axis-Aligned Bounding Box，轴对齐包围盒。什么意思？就是包围盒的每条边，都跟世界坐标系的轴平行。
你想想看，一个长方体，它的六个面分别平行于X、Y、Z轴。这就是AABB。
3.2.1 AABB的表示
AABB的表示方式很简单，只需要两个点：
- 最小值点：
min = (x_min, y_min, z_min) - 最大值点：
max = (x_max, y_max, z_max)
整个包围盒就是这两个点确定的对角线区域。
3.2.2 AABB的碰撞检测
两个AABB是否相交？判断方法极其简单——在三个轴向上分别做投影，看是否重叠。
如果三个轴都重叠，则相交；只要有一个轴不重叠，就不相交。
// 两个AABB的碰撞检测
bool AABBOverlap(const AABB& a, const 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需要重新计算，而且会变大——因为旋转后的物体在轴向上投影范围会扩大。这一点在机械臂关节运动时要特别注意。
3.3 OBB包围盒
OBB，全称Oriented Bounding Box，有向包围盒。跟AABB最大的区别是：OBB可以旋转，它的方向跟物体的实际姿态一致。
说白了，AABB是“死”的，OBB是“活”的。
3.3.1 OBB的表示
OBB需要三个要素：
- 中心点：
center = (cx, cy, cz) - 三个轴向量：
u, v, w（单位向量，互相垂直） - 三个半边长：
half_x, half_y, half_z
你看，OBB比AABB多存了一个旋转信息。所以它更精确，但也更复杂。
3.3.2 OBB的碰撞检测
OBB的碰撞检测用的是分离轴定理（SAT）。原理是：如果能找到一条直线，使得两个OBB在这条直线上的投影不重叠，那么这两个OBB就不相交。
理论上，最多需要检测15条轴：
- 物体A的三个轴（3条）
- 物体B的三个轴（3条）
- A的每个轴与B的每个轴的叉积（3×3=9条）
但实际上，大部分情况下检测前6条轴就能出结果了。
避坑指南：我曾经在项目里自己手写OBB碰撞检测，结果发现浮点数精度问题导致误判。两个OBB明明挨着但没碰到，程序却报了碰撞。后来加了一个很小的容差阈值（比如1e-6）才解决。所以，做碰撞检测一定要留余量。
3.4 AABB vs OBB：怎么选？
这个问题，我经常被问到。直接看对比表：
| 对比维度 | AABB | OBB |
|---|---|---|
| 计算速度 | 极快（仅需比较6个值） | 较慢（需要SAT，最多15条轴） |
| 存储空间 | 小（2个点，6个float） | 大（中心+3轴+3半长，10个float） |
| 旋转适应性 | 差（旋转后需重算，且会膨胀） | 好（随物体旋转，保持紧凑） |
| 紧密度 | 一般（对细长物体浪费空间大） | 好（贴合物体实际形状） |
| 适用场景 | 静态物体、快速粗筛 | 运动物体、需要精确碰撞 |
我的建议是：粗筛用AABB，精检用OBB。或者，如果场景里大部分物体是静止的，直接用AABB就够了。
3.5 FCL库：把碰撞检测交给专业的人
手写碰撞检测？可以，但没必要。工业界有现成的库——FCL（Flexible Collision Library）。
FCL是ROS里默认的碰撞检测库，支持AABB、OBB、三角形网格、点云等多种几何体的碰撞检测和距离计算。
3.5.1 FCL的核心概念
FCL里几个关键概念，你得先搞清楚：
- CollisionObject：碰撞对象，包含几何体和变换矩阵
- CollisionRequest：碰撞请求，设置检测参数（如是否返回接触点）
- CollisionResult：碰撞结果，包含是否碰撞、接触点、法线等信息
- DistanceRequest：距离请求，用于计算两个物体之间的最近距离
- DistanceResult：距离结果
3.5.2 用FCL做碰撞检测
直接上代码。这是我在项目里实际用过的模板：
#include <fcl/fcl.h>
// 1. 创建几何体——这里用AABB包围盒
auto box_geom = std::make_shared<fcl::Boxd>(0.5, 0.3, 0.2); // 长宽高
// 2. 创建碰撞对象，绑定几何体和位姿
fcl::CollisionObjectd box_obj(box_geom);
box_obj.setTranslation(fcl::Vector3d(1.0, 0.0, 0.5)); // 位置
box_obj.setRotation(fcl::Matrix3d::Identity()); // 姿态
box_obj.computeAABB(); // 更新AABB
// 3. 创建第二个物体
auto sphere_geom = std::make_shared<fcl::Sphered>(0.2);
fcl::CollisionObjectd sphere_obj(sphere_geom);
sphere_obj.setTranslation(fcl::Vector3d(1.2, 0.1, 0.5));
sphere_obj.computeAABB();
// 4. 执行碰撞检测
fcl::CollisionRequestd request;
fcl::CollisionResultd result;
fcl::collide(&box_obj, &sphere_obj, request, result);
// 5. 输出结果
if (result.isCollision()) {
std::cout << "碰撞了！" << std::endl;
// 获取接触点
auto contacts = result.getContacts();
for (auto& c : contacts) {
std::cout << "接触点: " << c.pos.transpose() << std::endl;
}
} else {
std::cout << "安全距离内" << std::endl;
}我的习惯：在机械臂的每个连杆上都挂一个FCL的CollisionObject，然后在规划循环里批量检测。注意，每次关节运动后，要调用computeAABB()更新包围盒。这一步很多人会忘，结果检测结果一直是旧的。
3.5.3 FCL支持的其他功能
FCL不止能做碰撞检测，还能做：
- 距离计算：两个物体之间最近的距离是多少？用
fcl::distance() - 连续碰撞检测：物体从A点运动到B点，中间会不会撞？用
fcl::continuousCollide() - 网格碰撞：直接对三角网格做碰撞检测，用
fcl::BVHModel
3.6 本章知识体系
下面这张图，把本章的核心逻辑串起来了：
3.7 工程实践建议
最后，分享几点我在实际项目里踩过的坑：
- 不要迷信OBB：OBB虽然紧凑，但更新成本高。如果场景里大部分物体是静止的，用AABB就够了。我见过有人把所有物体都转成OBB，结果帧率掉了一半。
- FCL的线程安全：FCL的CollisionObject不是线程安全的。如果你在多线程里同时读写同一个对象，记得加锁。或者，每个线程维护一份独立的拷贝。
- 容差设置：机械臂运动到目标位置时，往往跟工件贴得很近。这时候碰撞检测容易误报。我习惯在FCL的CollisionRequest里设置一个
enable_contact和max_contact_count，然后根据接触点的距离做二次判断。 - 性能监控：碰撞检测是规划循环里的瓶颈。我建议每次检测后都打印一下耗时，如果超过1ms，就要考虑优化了——比如减少检测对的数量，或者降低检测频率。
曾经踩过的坑：有一次，我把机械臂的基座也加入了碰撞检测，结果每次规划都报基座自碰撞。查了半天，发现是基座的包围盒跟它自己的第一个连杆的包围盒有重叠。后来把基座从碰撞检测列表里排除掉，问题就解决了。所以，自碰撞检测要小心处理相邻连杆。