本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

MoveIt2碰撞检测模块：FCL与Octomap的避障机制

导读：机械臂如何"看见"障碍物并自动绕开？MoveIt2 的碰撞检测模块是保证运动安全的关键。本文详解碰撞检测原理、精度控制与效率优化技巧。

原理简析

碰撞检测需要解决两类问题：自碰撞（机器人自身部件之间碰撞）和环境碰撞（机器人与外部障碍物碰撞）。

MoveIt2 使用 FCL（Flexible Collision Library）作为默认几何碰撞检测库，结合 Octomap 实现三维环境感知。整个检测由 PlanningScene 统一调度，规划器每采样一个新状态都会调用碰撞检测验证。

flowchart TB A(["碰撞检测请求"]) --> B{"自碰撞检测"} B --> C["遍历链接对 ACM过滤"] C --> D{"碰撞?"} D -->|"是"| E["标记碰撞"] D -->|"否"| F["环境碰撞检测"] F --> G["Octomap 体素查询"] G --> H{"碰撞?"} H -->|"是"| E H -->|"否"| I(["返回安全"])

碰撞检测流程：
1. 收到规划请求，获取当前机器人状态
2. 通过 ACM（Allowed Collision Matrix）过滤不需检测的链接对
3. 调用 FCL 检测剩余链接对的自碰撞
4. 查询 Octomap 体素，检测与环境障碍的碰撞
5. 返回碰撞结果与最近距离

实操步骤

碰撞检测架构

flowchart TB A["Planning Scene 规划场景"] --> B["FCL 几何碰撞检测"] A --> C["Octomap 环境感知"] A --> D["ACM 允许碰撞矩阵"] B --> E["自碰撞结果"] C --> F["环境碰撞结果"] D --> B E --> G(["综合结果"]) F --> G

碰撞检测优化参数

参数	说明	调优建议
contact_distance	碰撞容差（最近距离阈值）	一般 0.01~0.05m，越大越保守
octomap_resolution	Octomap 体素分辨率	0.01m 精细，0.05m 快速
acm_enabled	ACM 启用开关	必须启用，过滤相邻连杆对
max_contacts	最大接触点数	影响检测精度，常用 50

避坑点：contact_distance 设过大会导致"看似没碰撞也报碰撞"；设过小则可能在动态环境中漏检。建议静态场景 0.01m，动态场景 0.05m。
来自 linuxros.cn · linuxROS

代码实现

配置 Planning Scene Monitor

# planning_scene_monitor.yaml
planning_scene_monitor:
  robot_description: "robot_description"
  scene_topic: "/monitored_planning_scene"
  collision_object_topic: "/collision_object"
  attached_collision_object_topic: "/attached_collision_object"
  octomap_frame: "world"
  octomap_resolution: 0.05

C++ 碰撞检测示例

// collision_check.cpp
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

void CollisionCheckExample(
    const moveit::core::RobotModelPtr& robot_model)
{
    // 创建 Planning Scene
    auto scene = std::make_shared<planning_scene::PlanningScene>(robot_model);

    // 添加碰撞物体
    moveit_msgs::msg::CollisionObject obj;
    obj.id = "box";
    obj.header.frame_id = "world";
    obj.primitives.resize(1);
    obj.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
    obj.primitives[0].dimensions = {0.1, 0.1, 0.1};
    obj.primitive_poses.resize(1);
    obj.primitive_poses[0].position.x = 0.4;
    obj.primitive_poses[0].position.y = 0.0;
    obj.primitive_poses[0].position.z = 0.2;
    obj.operation = obj.ADD;
    scene->processCollisionObjectMsg(obj);

    // 碰撞检测
    collision_detection::CollisionRequest req;
    req.verbose = true;
    req.distance = true;        // 计算最近距离
    req.max_contacts = 50;

    collision_detection::CollisionResult res;
    scene->checkCollision(req, res);

    if (res.collision) {
        RCLCPP_INFO(rclcpp::get_logger("collision"),
                    "发生碰撞！最近距离: %f", res.distance);
    } else {
        RCLCPP_INFO(rclcpp::get_logger("collision"),
                    "路径安全，最近距离: %f", res.distance);
    }
}

设置允许碰撞矩阵（ACM）

// acm_example.cpp
void ACMExample(moveit::core::RobotState& state,
                planning_scene::PlanningScene& scene)
{
    // 允许某些链接对碰撞（如夹爪与工件）
    collision_detection::AllowedCollisionMatrix& acm =
        scene.getAllowedCollisionMatrixNonConst();
    acm.setEntry("gripper_left_finger", "workpiece", true);
    acm.setEntry("gripper_right_finger", "workpiece", true);

    // 检查（带 ACM）
    collision_detection::CollisionRequest req;
    collision_detection::CollisionResult res;
    scene.checkCollision(req, res, state);
}

注意：setEntry 第一个参数是 link 名称，对大小写敏感；MoveIt Setup Assistant 在生成 SRDF 时会自动把相邻连杆对加入 ACM，但用户自定义对象需要手动配置。

常见问题解决

Q1：检测速度太慢怎么办？
A：启用 ACM 过滤不需检测的链接对（典型可减少 70% 检测量）；降低 Octomap 分辨率（0.05~0.1m）；启用 collision_detection::DistanceRequest 仅算最近距离。

Q2：明明没碰撞却报碰撞？
A：检查 URDF 中碰撞体是否过大；适当减小 contact_distance；查看 acm 是否漏配某些链接对。

Q3：Octomap 不准确？
A：调整传感器点云发布频率（≥10Hz）；检查 octomap_resolution 与点云密度匹配；启用 OctomapUpdate 增量更新而非全量重建。

总结

碰撞检测是 MoveIt2 安全运行的基础。FCL 负责几何碰撞检测，Octomap 负责环境感知，ACM 负责过滤豁免对。通过合理配置 ACM 和容差参数，可以在保证安全的同时显著提升规划效率。下期讲解控制器模块，搞懂轨迹执行与 ros2_control 的对接。