第 3 章 碰撞检测与环境表示¶
规划器每秒钟要回答成千上万次“这个位形行不行”，回答的速度与正确性都由环境表示决定。规划场景（planning scene）是 MoveIt 侧唯一的环境真相来源：它装着机器人当前位形、世界里的物体、哪些 link 允许互碰，以及由传感器累积的占据地图。本章说明这三块内容的组织方式与更新成本，给出允许碰撞矩阵（allowed collision matrix, ACM）在运行时的实际形态，并用一组对照读数说明“逆解可用”与“位形合法”之间的距离。以下读数均取自运行环境 ROS 2 Humble + MoveIt 2 2.5.10 + FCL，样本臂为 moveit_resources 的 panda。
场景三块内容的汇总点，ACM 在检测入口过滤：
flowchart LR
robot["机器人当前位形"] --> scene["planning scene"]
world["世界物体"] --> scene
octo["传感器占据地图"] --> scene
scene --> fcl["FCL 碰撞检测"]
acm["ACM：允许碰撞矩阵"] -.->|"放行合法接触对"| fcl1. 场景的三块内容与两个服务¶
一份 moveit_msgs/PlanningScene 消息可以拆成三块：机器人状态（关节值 + 附着物体）、世界物体（碰撞物体表）、场景设置（ACM、link padding/scale、允许碰撞矩阵、octomap）。对外只有两个入口：
## 运行环境 ROS 2 Humble，move_group 已在跑
ros2 service type /get_planning_scene # moveit_msgs/srv/GetPlanningScene
ros2 service type /apply_planning_scene # moveit_msgs/srv/ApplyPlanningScene
/get_planning_scene 按请求里的 components 位掩码返回内容，/apply_planning_scene 接受 is_diff=true 的增量更新（新增、移动、删除物体都靠 CollisionObject.operation 区分：ADD / MOVE / REMOVE）。这两个服务就是所有场景编辑的统一入口——RViz 的交互对象、感知侧的物体注入、attach 操作，最后都走同一条通路。
物体最少要能回答三个问题：叫什么（id）、在哪（header 的坐标系 + 位姿）、什么形状（primitives 或 meshes 加各自的位姿）。写进场景后它就同时参与碰撞查询与最近距离计算，不需要任何额外的“注册”步骤。
2. 允许碰撞矩阵¶
默认假设是“任何两个 link 都可能相撞”，ACM 用来把物理上不可能相撞、或必须允许贴合的对儿排除掉。panda 样本的 panda.srdf 声明 34 对，运行时 ACM 的形态与之一致：
ACM 键=11，允许碰撞的矩阵单元=68（对称记两次，合 34 对）；SRDF 声明 34 对
样例键: ['panda_hand', 'panda_leftfinger', 'panda_link0']
panda_hand 的禁用邻居数=6
矩阵是 entry_names × entry_values，每行是一个布尔数组——true 表示这一对允许碰撞。因为对称，同一对记两次，所以 68 个单元对应 34 对。SRDF 里每一对都带一个 reason，两类语义不同：
<disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
<!-- 相邻 link 由关节连接，永远贴近：必须豁免 -->
<disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
<!-- 不相邻但受运动学约束不可能接触：豁免后省下查询代价 -->
Adjacent（及 Default）是机械性的，去掉就会让任何规划都失败；Never 是优化性的，去掉只增加代价。“规划总拒绝”的头号成因是这一表的反向问题：新加的末端工具或第三只手没有对应的豁免项，抓取姿态下工具与被抓物体必然重叠，于是所有目标位形都被判非法。检查方法是把位形交给合法性服务，让它直接报出冲突对：
## 返回体含 valid、contacts（每个带两个 body 名与深度），是排查 ACM 的第一手工具
ros2 service call /check_state_validity moveit_msgs/srv/GetStateValidity \
"{robot_state: {joint_state: {name: [panda_joint1], position: [0.0]}}, group_name: panda_arm}"
3. 按需取回与更新成本¶
场景消息可能很大，取多少由 components 决定。用同一批物体测三档（每次取回都含 ACM、物体名与几何）：
| 场景内容 | 取回耗时 | 序列化大小 |
|---|---|---|
| 空场景 | 1.3 ms | 1.3 KiB |
| 3 个 6 cm 方块 | 1.9 ms | 1.3 KiB |
| 203 个方块 | 18.8 ms（首次 28.7 ms） | 47.2 KiB |
203 个物体的场景取回约 19 ms、47 KiB，这还没算几何细节：只取名字（WORLD_OBJECT_NAMES）与同时取几何（WORLD_OBJECT_GEOMETRY）是两种量级的请求，前者只带 id 列表。规划回路里如果要高频同步场景，按需取组件是第一条原则；第二条是不把场景当查询数据库用——它是一次性快照，物体一多就退化成大对象搬运。
碰撞查询本身的代价则由位形与障碍的相对位置支配，不由物体总数线性支配：同一个 203 物体的场景，起点停在 ready（手在桌面上方远处）时重复 20 次合法性检查平均 0.33 ms/次；换成手伸进方块区的位形，同样亚毫秒返回，但会带出若干真实接触。差别不在耗时而在结果——这也说明“没报接触”不等于“查询很便宜所以没查”。
4. 图元、网格与近阶段检测¶
碰撞几何有两种载体。图元（shape_msgs/SolidPrimitive：BOX、SPHERE、CYLINDER、CONE、PLANE）有解析的距离查询；网格（shape_msgs/Mesh）需要三角级求交。近阶段碰撞检测（near-phase）指真正判断一对几何是否接触的那一步，它由插件承担，运行环境里同时装着两份实现：
libmoveit_collision_detection_fcl.so ← 默认
libmoveit_collision_detection_bullet.so ← 可选插件
判据级别的取舍（不做完整基准）：
| 表达 | 优点 | 代价与坑 |
|---|---|---|
| 图元 | 查询便宜、无预处理、体积可控 | 形状偏差大，过包会吃掉工作空间，欠包会漏检 |
| 凸网格（convex decomposition） | 贴合外形，近阶段仍可用凸-凸算法 | 需要离线分解工具，子块数量直接决定代价 |
| 三角网格（自碰撞模式） | 建模侧省事 | 近阶段按三角对枚举，代价高且对连续运动不稳定 |
实践中最常犯的是“直接套用视觉侧的精细网格做碰撞”：那类模型是数十万三角片的渲染网格，用它做碰撞会把近阶段变成三角对扫描。规则是渲染网格与碰撞网格分开，碰撞侧优先用图元或分解后的凸块。此外 MoveIt 的查询带接触 padding（padding_offset，默认 0.01 m 量级）与 link scale，判定裕度时应按“是否超出 padding”来读，而不是把返回的深度当作真实穿模量。
5. 附着物体与接触清单¶
抓到东西之后，物体必须继续参与碰撞检查，但它不再属于“世界”——它跟着末端走。这就是 attached object（附着物体）的职责：把世界物体转成机器人状态的一部分，绑定到某个 link 上。请求形态只有一个 link_name 与一组 touch_links：
from moveit_msgs.msg import AttachedCollisionObject, CollisionObject
aco = AttachedCollisionObject()
aco.object.id = "carried"
aco.link_name = "panda_hand" # 物体挂在这个 link 上，随它运动
aco.touch_links = ["panda_hand"] # 只豁免列出的 link 与物体的接触
touch_links 的语义很直白：把一个 5 cm 方块挂到 panda_hand，只列 panda_hand 为接触清单时，合法性检查报出的是它与基座链路相交——
带附着体查初始状态（touch_links=[panda_hand]）: valid=False
panda_link0 × carried 深度=0.0031 panda_link1 × carried 深度=0.0040
把 touch_links 扩到 [panda_hand, panda_leftfinger, panda_rightfinger]：
上两条例子不变，因为相交对象是 link0/link1，不在豁免名单里
接触清单不是“抓稳了”的声明，而是一张豁免表：只有列进去的 link 与被附着体之间的接触被忽略，其余 link 照常检查。放错了位置（本例中物体被放到基座附近）不会被这一机制掩盖。另有一条接口层面的事实：通过 /get_planning_scene 取回的 ACM 里不含被附着体的行（ACM 键数仍是 11）——附着体的豁免关系在 move_group 内部的 ACM 上维护，不随该组件回传，排查时不能靠读这条消息确认 attach 是否生效。
6. 占据地图的预算¶
没有传感器来源时，环境只由手工放置的物体组成。要把点云变成障碍，靠的是 octomap（八叉树占据地图）与它的更新插件。样本配置里的这一段就是全部旋钮：
## sensors_kinect_pointcloud.yaml：点云 → octomap 的更新器配置
sensors:
- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
point_cloud_topic: /camera/depth_registered/points
max_range: 5.0 # 超过 5 m 的点不入图：距离越远误差越大
point_subsample: 1 # 抽稀步长；大点云靠它压算力
padding_offset: 0.1 # 在物体外侧再扩 10 cm：把误差吃进安全侧
padding_scale: 1.0
max_update_rate: 1.0 # 每秒最多更新 1 次，规划侧不必追传感器帧率
filtered_cloud_topic: filtered_cloud
四条预算要一起看：分辨率决定内存与查询代价（该配置未显式给出分辨率，运行时会取 0.1 m 作为默认值）；max_range 决定哪些点可信；padding_offset 决定安全边界向外推多少；max_update_rate 决定地图刷新与规划的算力分配。octomap 分辨率并非越高越好：分辨率减半，内存与更新代价按立方增长，而噪声点还会被实体化成障碍——桌面操作里常见的是精细分辨率把工件的抖动放大成一片“不可进入区”，反而让规划失败。没有 3D 传感器插件被定义时，占据地图不会更新，场景只含手工物体，此时任何依赖地图的结论都不成立。
7. 校验先行：从逆解到规划的三道关¶
第 2 章留下的问题在这里收口：一个位姿的逆解返回成功，与“这个位姿可规划”之间隔着什么。同一个目标位姿（把一个 6 cm 方块的中心）分别走三条路：
| 路径 | 结果 | 读数 |
|---|---|---|
IK，avoid_collisions=False |
出解 | 往返 1.7 ms |
| 该解的合法性检查 | 非法 | 5 个接触：与 blkB 三处（最大 7.3 mm），自身两处（panda_link5×panda_hand 59.2 mm、panda_link7×panda_link5 15.7 mm） |
IK，avoid_collisions=True |
无解 | 返回无解码，2 s 预算烧满（往返 2001 ms） |
规划请求（OMPL RRTConnect，5 s 预算，5 次尝试，plan_only） |
拒绝 | 无轨迹，往返 5047 ms |
| 同一朝向、抬高到方块上方 40 cm | 成功 | 往返 55 ms，20 个航点 |
这张表把三道关摊开了。第一关是几何：IK 不看环境和自身，所以毫秒级返回的解可以既穿进物体又穿进自己的小臂。第二关是合法性：/check_state_validity 一次查询就能把接触对与深度列出来，这是唯一便宜的守门人。第三关是可达性：规划器要求的不只是目标位形合法，而是从起点到目标存在一条处处合法的连续路径——把目标抬高 20 cm（此时的新解虽与方块相交，但 /check_state_validity 判它合法）仍然被拒绝，抬高 40 cm 才在 55 ms 内出解。
另一个常见误解：碰撞感知 IK 不是绕障手段——打开 avoid_collisions 后，障碍附近的目标会返回无解并把超时烧满，因为该模式是在几何候选解上做一次过滤，而不是在障碍环境里搜索；搜索是规划器的职责（第 4 章）。高频的位姿跟踪需要绕障时，应使用 Servo 一类的实时通路（第 7 章），而不是把 IK 的超时调大。
8. 要点¶
- 规划场景是 MoveIt 侧唯一的环境真相，对外只有两个入口：
/get_planning_scene（按位掩码取组件）与/apply_planning_scene（is_diff增量，靠operation区分增删移动）。 - ACM 是
entry_names × 布尔行的对称矩阵；panda 样本运行时 11 键 / 68 单元，与 SRDF 的 34 对一一对应。Adjacent是机械豁免，Never是算力豁免。 - 新加工具后“规划总拒绝”，先用
/check_state_validity让服务把冲突对列出来，而不是先调规划器参数。 - 场景成本按取的组件数与相对位置支配：203 物体全量取回约 19 ms / 47 KiB，而远离障碍的位形检查只要 0.3 ms；高频同步必须按需取组件。
- 碰撞几何用图元或凸分解网格，渲染网格不入场；判定裕度按 padding 读，返回的深度不是真实穿模量。
touch_links是豁免表而不是“抓稳”声明：未列出的 link 与被附着体照常判碰；attach 的豁免行不通过/get_planning_scene的 ACM 回传，不能靠读那条消息确认。- 占据地图的四条预算（分辨率、
max_range、padding_offset、max_update_rate）必须一起看；未定义传感器插件时地图不更新，依赖地图的结论不成立。 - 从逆解到规划隔着三道关：几何（IK 不看环境与自身）、合法性（一次校验服务）、可达性（需要处处合法的连续路径）；同一目标抬高 20 cm 仍被拒、抬高 40 cm 才 55 ms 出解，就是这个差距的量。