第3章 MoveIt核心概念
运动规划组 · 规划场景 · 运动规划器 · 碰撞检测机制
好，咱们直接进入正题。这一章要聊的四个概念，是MoveIt的骨架。你想想看，不管你是做简单的机械臂示教，还是搞移动抓取，最后都得跟它们打交道。我个人习惯把这一章当作“MoveIt世界观”的入门——搞懂了这些，后面的代码和配置才不会觉得玄乎。
1. 运动规划组 (Planning Group)
说白了，规划组就是你把机器人身上哪些关节“打包”在一起，告诉MoveIt：“嘿，这几个关节是一伙的，你规划的时候一起考虑。” 比如一个六轴机械臂，从基座到末端，所有转动关节组成一个规划组。我刚开始用MoveIt时，总以为规划组就是URDF里的一个link集合——其实没那么简单。
每个规划组都有对应的运动学求解器、规划算法和参考坐标系。你可以定义多个规划组，比如“arm_group”、“gripper_group”，甚至“whole_body”。
2. 规划场景 (Planning Scene)
规划场景就是MoveIt对“当前世界”的快照。它包含机器人自身状态、周围障碍物、附加物体（比如抓起来的零件）以及各种约束。你可以把它理解成一个“世界模型”，规划器在这个模型里做碰撞检测和路径搜索。
我记得有一次在项目里，机械臂旁边临时放了一个料架。我没有更新规划场景，结果规划出来的路径直接穿过了料架……嗯，从那以后我每次改变环境都会调用 applyPlanningScene 刷新场景。
PlanningSceneInterface 可以添加/移除物体、设置颜色、附加物体到机器人上。例如：
moveit::planning_interface::PlanningSceneInterface psi;
moveit_msgs::CollisionObject co;
co.id = "table";
co.primitives.push_back(shape);
co.primitive_poses.push_back(pose);
psi.applyCollisionObject(co);3. 运动规划器 (Planner)
规划器就是那个负责“找路”的算法模块。MoveIt默认集成了OMPL（Open Motion Planning Library），里面有一大堆算法：RRT、RRTConnect、PRM、LazyPRM等等。我个人偏爱RRTConnect，因为它双向搜索，在复杂空间里效率很高。
你可能会问：“这么多算法，我该选哪个？” 我的建议是——先跑默认的RRTConnect，如果规划时间太长或者路径太绕，再试试PRM或者LazyPRM。每个规划器都有自己的参数，比如最大迭代次数、规划时间限制。
| 规划器 | 特点 | 适用场景 |
|---|---|---|
| RRTConnect | 双向快速扩展，收敛快 | 大多数机械臂规划 |
| PRM | 概率路线图，适合多次查询 | 静态环境，重复规划 |
| LazyPRM | 延迟碰撞检测，构建更快 | 高维空间，障碍物稀疏 |
| EST | 基于膨胀空间树 | 狭窄通道场景 |
4. 碰撞检测机制
碰撞检测是MoveIt的“安全底线”。它基于FCL（Flexible Collision Library）库，可以检测机器人自身碰撞（自碰撞）以及机器人与环境的碰撞。默认情况下，MoveIt会检查所有规划组内的link对，但你可以通过“允许碰撞矩阵”（ACM）来跳过某些永远不会碰撞的link对，提升性能。
我见过不少新手一上来就把碰撞检测开到最严，结果规划慢得不行。其实你可以通过 setPlanningScene 设置碰撞检测的精度，或者用 getRobotModel() 获取link对，手动排除那些明显碰不到的link。
- 离散碰撞检测 — 检查路径上的离散点，速度快，但可能漏掉中间碰撞。
- 连续碰撞检测 — 在连续运动轨迹上检测，更安全，但计算量大。
- 自碰撞检测 — 默认开启，防止机器人自己打自己。
举个例子，如果你有一个七轴协作臂，自碰撞检测尤其重要。我曾经调试时忘了开启自碰撞，结果规划出的路径让两个连杆差点“亲上”，还好仿真里发现了。
planning_scene_rviz_plugin 可视化碰撞检测结果。把碰撞的link显示为红色，一目了然。我每次调参都会打开这个插件，比看日志快多了。
好了，这四个概念就像MoveIt的四根柱子。规划组定义“谁动”，规划场景定义“在哪动”，规划器决定“怎么动”，碰撞检测保证“安全动”。你想想看，是不是所有MoveIt应用都绕不开它们？