3. 规划场景（Planning Scene）深入：场景的构建与更新、碰撞对象管理（添加/移除/附加）、允许碰撞矩阵（ACM）的灵活配置

规划场景，说白了就是机器人运动规划时的「世界模型」。它告诉规划器：周围有什么障碍物？机器人自身状态如何？哪些碰撞可以忽略？

我刚开始接触 MoveIt 时，觉得规划场景就是个简单的碰撞检测容器。后来在产线项目中，一个动态变化的装配环境让我彻底改变了看法——场景管理没做好，规划器就像蒙着眼睛走路，撞得鼻青脸肿。

3.1 场景的核心构成

一个完整的 PlanningScene 包含三大部分：

• 机器人状态：当前关节位置、末端执行器位姿
• 碰撞对象：环境中的障碍物、附加在机器人上的物体
• 允许碰撞矩阵：哪些物体对之间不检测碰撞

这三者缺一不可。我见过有人只更新了机器人状态，忘了更新碰撞对象，结果规划出来的轨迹直接穿模。

3.2 场景的构建与更新

构建场景有两种方式：

1. 从当前环境快照构建：适合静态场景
2. 增量更新：适合动态场景，只修改变化的部分

我个人习惯用增量更新，尤其是在产线场景中。你想想看，如果每次都要重建整个场景，计算开销太大了。

// C++ 示例：增量更新场景
moveit::planning_interface::PlanningSceneInterface psi;

// 添加一个障碍物
moveit_msgs::CollisionObject co;
co.id = "table";
co.header.frame_id = "world";
co.primitives.resize(1);
co.primitives[0].type = shape_msgs::SolidPrimitive::BOX;
co.primitives[0].dimensions = {1.0, 0.8, 0.1};

geometry_msgs::Pose pose;
pose.position.x = 0.5;
pose.position.y = 0.0;
pose.position.z = -0.05;
co.primitive_poses.push_back(pose);

co.operation = moveit_msgs::CollisionObject::ADD;
psi.applyCollisionObject(co);

嗯，这里要注意：operation 字段决定了是添加、移除还是修改。我踩过坑，忘记设置操作类型，结果物体一直没出现在场景里。

3.3 碰撞对象管理

3.3.1 添加与移除

碰撞对象的管理其实就三个动作：添加、移除、修改。但实际项目中，坑往往出在「附加」操作上。

操作	说明	典型场景
ADD	添加新物体到场景	放置一个工作台
REMOVE	从场景中移除物体	拿走已加工完的零件
APPEND	附加物体到机器人连杆	夹爪抓取工件
MOVE	移动物体位置	传送带上的工件

我曾经在项目中遇到一个问题：工件明明被夹爪抓住了，但规划时总是报碰撞。后来发现，工件还是独立的碰撞对象，没有附加到夹爪上。说白了，附加操作就是告诉规划器：「这个物体现在是机器人身体的一部分」。

3.3.2 附加与分离

附加操作是场景管理中最容易出错的环节。我建议你记住这个流程：

1. 先移除场景中的独立物体
2. 再附加到对应的机器人连杆
3. 更新场景

// 附加物体到夹爪
co.operation = moveit_msgs::CollisionObject::REMOVE;
psi.applyCollisionObject(co);

// 重新附加
co.operation = moveit_msgs::CollisionObject::APPEND;
co.attach_to_link = "gripper_link";
psi.applyCollisionObject(co);

3.4 允许碰撞矩阵（ACM）的灵活配置

允许碰撞矩阵，说白了就是一张「免检名单」。它告诉规划器：某些物体对之间不需要做碰撞检测。

为什么要用 ACM？我举个例子：机器人的两个连杆靠得很近，但永远不会碰撞。如果不配置 ACM，规划器每次都要检测它们，白白浪费计算资源。

3.4.1 ACM 的结构

ACM 本质上是一个二维矩阵，行和列都是场景中的物体（包括机器人连杆）。矩阵中的每个元素表示对应的物体对是否允许碰撞。

// ACM 配置示例
collision_detection::AllowedCollisionMatrix acm;

// 允许夹爪的两个手指之间不检测碰撞
acm.setEntry("gripper_left_finger", "gripper_right_finger", true);

// 允许工件和传送带之间不检测碰撞
acm.setEntry("workpiece", "conveyor", true);

3.4.2 动态调整 ACM

实际项目中，ACM 不是一成不变的。我记得有个项目，机器人需要抓取不同形状的工件。每个工件的碰撞特性都不一样，如果用一个固定的 ACM，要么太保守（规划失败），要么太激进（碰撞风险）。

我的做法是：根据当前任务动态调整 ACM。

// 动态调整 ACM
void updateACM(const std::string& object_id, bool allow_collision) {
    acm.setEntry(object_id, "robot_link", allow_collision);
    planning_scene->setAllowedCollisionMatrix(acm);
}

3.5 场景与 ACM 的协同工作

场景和 ACM 是配套使用的。你想想看，如果场景里添加了一个物体，但 ACM 没更新，规划器可能会认为这个物体和机器人有碰撞风险，导致规划失败。

我建议你遵循这个流程：

1. 更新场景（添加/移除/附加物体）
2. 同步更新 ACM（配置允许碰撞的物体对）
3. 触发规划

下面这张图展示了场景管理的核心流程：

3.6 实战中的避坑指南

最后，分享几个我踩过的坑：

• 场景和 ACM 不同步：我曾经在场景里添加了物体，但忘了更新 ACM，结果规划器认为物体和机器人有碰撞，一直规划失败。排查了半天才发现是 ACM 的问题。
• 附加操作顺序错误：先附加再移除，导致场景里出现两个相同的物体。规划器直接懵了。
• ACM 配置过于宽松：为了省事，把很多物体对都设为允许碰撞。结果规划出来的轨迹真的撞上了。嗯，ACM 不是万能药，该检测的还是要检测。