第3章碰撞检测基础：AABB与OBB包围盒原理、FCL库快速入门、在ROS中实现碰撞检测节点

碰撞检测，说白了就是回答一个问题：我的机械臂碰到东西了吗？

你想想看，避障算法的第一步，不是规划路径，而是知道“障碍物在哪”。如果连碰撞都检测不准，后面谈避障就是空中楼阁。这一章，我们就从最基础的包围盒原理讲起，再到FCL库的实战，最后在ROS里跑一个真正的碰撞检测节点。

3.1 包围盒：把复杂问题变简单

机械臂和障碍物的几何形状通常很复杂。直接拿三角网格去算碰撞，计算量太大，实时性根本扛不住。所以工业界普遍的做法是：用简单的几何体去包裹复杂的物体，这就是包围盒（Bounding Box）的由来。

我个人习惯把包围盒分为两大类：AABB 和 OBB。它们各有各的脾气。

3.1.1 AABB：轴对齐包围盒

AABB 的全称是 Axis-Aligned Bounding Box。它的特点是：盒子的边永远和世界坐标轴平行。不管物体怎么旋转，AABB 都会跟着重新计算，始终保持轴对齐。

它的好处很明显：

计算快：只需要比较每个轴上的最大最小值
内存小：存6个浮点数就够了（x_min, x_max, y_min, y_max, z_min, z_max）
实现简单：十几行代码就能搞定

但缺点也很致命：紧密性差。物体一旋转，AABB 会包进去大量空白空间，导致误报率升高。

我的经验： 在机械臂的快速预筛选阶段，我通常先用 AABB 做粗检测。如果 AABB 都没碰撞，那肯定没碰撞；如果 AABB 检测到碰撞，再进入更精细的检测。这种“粗筛+精检”的策略，能省下不少计算资源。

3.1.2 OBB：有向包围盒

OBB 的全称是 Oriented Bounding Box。它的特点是：盒子的方向可以跟随物体的旋转。说白了，就是给物体穿一件“合身的衣服”。

OBB 的紧密性比 AABB 好得多，尤其适合细长、旋转的物体——比如机械臂的连杆。

但代价也很明显：

计算复杂：需要做分离轴定理（SAT）检测，涉及向量投影
内存更大：除了位置，还要存旋转矩阵
更新麻烦：物体旋转后，OBB 需要重新计算方向

避坑指南： 我曾经在一个项目里，直接用 OBB 做全量检测，结果发现每帧计算时间飙到了 50ms 以上。后来改成“AABB 粗筛 + OBB 精检”的两级方案，计算时间降到了 5ms 以内。记住：不要用大炮打蚊子。

3.2 AABB 与 OBB 对比一览

特性	AABB	OBB
对齐方式	与世界坐标轴对齐	与物体自身方向对齐
紧密性	差（旋转后空隙大）	好（贴合物体形状）
计算速度	极快（仅比较数值）	较慢（需分离轴检测）
内存占用	6个float	6个float + 旋转矩阵
适用场景	静态物体、粗检测	旋转物体、精检测

3.3 FCL库：碰撞检测的瑞士军刀

自己手写碰撞检测算法？没必要。ROS 生态里有一个现成的库——FCL（Flexible Collision Library）。它封装了 AABB、OBB、球体、凸包等多种碰撞检测算法，接口简洁，性能优秀。

FCL 的核心概念就三个：

Collision Object：你要检测的物体，包含几何形状和位姿
Collision Request：检测参数，比如是否启用连续碰撞检测
Collision Result：检测结果，包含是否碰撞、碰撞点、法向量等

下面是一个简单的 FCL 使用示例：

#include <fcl/fcl.h>

// 创建两个球体
auto sphere1 = std::make_shared<fcl::Sphere>(1.0);
auto sphere2 = std::make_shared<fcl::Sphere>(1.0);

// 设置位姿
fcl::Transform3f tf1, tf2;
tf1.setIdentity();
tf2.setIdentity();
tf2.translation() = Eigen::Vector3f(1.5, 0, 0);

// 创建碰撞对象
fcl::CollisionObjectf obj1(sphere1, tf1);
fcl::CollisionObjectf obj2(sphere2, tf2);

// 执行检测
fcl::CollisionRequestf request;
fcl::CollisionResultf result;
fcl::collide(&obj1, &obj2, request, result);

// 输出结果
if (result.isCollision()) {
    std::cout << "碰撞发生！" << std::endl;
} else {
    std::cout << "安全距离内。" << std::endl;
}

  关键点： FCL 默认使用 AABB 做快速粗筛，只有粗筛通过后才会进入精确的 OBB 或网格碰撞检测。这个设计思路，和我们在实际项目中用的策略完全一致。

3.4 在ROS中实现碰撞检测节点

理论讲完了，咱们来点实战。下面是一个完整的 ROS 节点，它订阅机械臂的关节状态和障碍物的点云，然后输出碰撞检测结果。

我建议你把这个节点作为碰撞检测的“骨架”，后续可以根据实际需求往里加逻辑。

3.4.1 节点架构

先看一张整体流程图，理清数据流：

3.4.2 核心代码实现

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/PointCloud2.h>
#include <fcl/fcl.h>
#include <moveit/robot_model_loader/robot_model_loader.h>

class CollisionDetector {
public:
    CollisionDetector(ros::NodeHandle& nh) : nh_(nh) {
        // 订阅关节状态和障碍物点云
        joint_sub_ = nh_.subscribe("/joint_states", 1, 
            &CollisionDetector::jointCallback, this);
        cloud_sub_ = nh_.subscribe("/obstacle", 1, 
            &CollisionDetector::cloudCallback, this);
        
        // 发布碰撞状态
        collision_pub_ = nh_.advertise<std_msgs::Bool>("/collision_status", 1);
        
        // 加载机器人模型
        robot_model_loader::RobotModelLoader loader("robot_description");
        kinematic_model_ = loader.getModel();
        
        ROS_INFO("碰撞检测节点已启动");
    }

private:
    void jointCallback(const sensor_msgs::JointState::ConstPtr& msg) {
        // 更新机械臂各连杆的位姿
        // 这里省略了具体的运动学计算
        // 实际项目中会调用 moveit 的 RobotState 来更新
        
        // 执行碰撞检测
        checkCollision();
    }
    
    void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
        // 将点云转换为 FCL 支持的几何形状
        // 通常做法：对点云进行下采样，然后生成凸包或球体
        // 这里只做示意
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*msg, *cloud);
        
        // 生成障碍物的 FCL 碰撞对象
        // obstacle_obj_ = ...;
    }
    
    void checkCollision() {
        // 遍历所有连杆，检测与障碍物的碰撞
        for (auto& link : links_) {
            fcl::CollisionObjectf link_obj(link.shape, link.transform);
            fcl::CollisionObjectf obs_obj(obstacle_shape_, obstacle_transform_);
            
            fcl::CollisionRequestf request;
            fcl::CollisionResultf result;
            fcl::collide(&link_obj, &obs_obj, request, result);
            
            if (result.isCollision()) {
                std_msgs::Bool msg;
                msg.data = true;
                collision_pub_.publish(msg);
                ROS_WARN_THROTTLE(1, "碰撞检测到！连杆: %s", link.name.c_str());
                return;
            }
        }
        
        // 无碰撞
        std_msgs::Bool msg;
        msg.data = false;
        collision_pub_.publish(msg);
    }
    
    ros::NodeHandle nh_;
    ros::Subscriber joint_sub_;
    ros::Subscriber cloud_sub_;
    ros::Publisher collision_pub_;
    robot_model::RobotModelPtr kinematic_model_;
    // 其他成员变量...
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "collision_detector");
    ros::NodeHandle nh;
    CollisionDetector detector(nh);
    ros::spin();
    return 0;
}

我曾经踩过的坑： 在点云转 FCL 几何体时，如果直接用原始点云生成凸包，计算量会非常大。我的做法是：先对点云进行体素滤波下采样，再用凸包算法生成简化模型。这样精度损失不大，但速度能快 10 倍以上。

3.4.3 调参建议

碰撞检测节点有几个关键参数，我列个表给你参考：

参数	推荐值	说明
点云体素大小	0.02~0.05 m	太小则计算慢，太大则精度差
碰撞检测频率	50~100 Hz	机械臂运动快时用高频，慢时用低频
包围盒类型	AABB + OBB 两级	粗筛用 AABB，精检用 OBB
安全距离	0.01~0.03 m	留一点余量，防止误触

我的习惯： 在调试阶段，我会把碰撞检测结果可视化出来——用 RViz 显示包围盒和碰撞点。这样一眼就能看出是算法问题还是参数问题。调试效率能提升不少。

嗯，这一章的内容就到这里。AABB 和 OBB 的原理、FCL 的用法、ROS 节点的实现，这三块你掌握之后，碰撞检测的基础就算打牢了。后面几章，我们会在这个基础上，加上避障算法，让机械臂真正学会“绕路走”。

📚 课程目录

第3章 碰撞检测基础：AABB与OBB包围盒原理、FCL库快速入门、在ROS中实现碰撞检测节点

3.1 包围盒：把复杂问题变简单

3.1.1 AABB：轴对齐包围盒

3.1.2 OBB：有向包围盒

3.2 AABB 与 OBB 对比一览

3.3 FCL库：碰撞检测的瑞士军刀

3.4 在ROS中实现碰撞检测节点

3.4.1 节点架构

3.4.2 核心代码实现

3.4.3 调参建议

第3章碰撞检测基础：AABB与OBB包围盒原理、FCL库快速入门、在ROS中实现碰撞检测节点