AMR机械臂协同作业系统 - 第3节
3. MoveIt2框架入门
3. MoveIt2框架入门：架构、运动规划组、规划场景与碰撞检测
好，咱们进入第三章。前两章我们把ROS2的基础和机械臂的模型搞定了，现在该让机械臂真正“动起来”了。这一章，我们来啃MoveIt2这块硬骨头。
说实话，我第一次接触MoveIt时，感觉它像个黑盒子。点个“Plan”按钮，机械臂就自己规划路径了。但做项目不能只停留在表面，你得知道它内部怎么运作的。今天我就带你把MoveIt2的底裤扒开看看。
3.1 MoveIt2架构：核心组件与数据流
MoveIt2不是个单一的程序，它是一套框架。说白了，它把运动规划、碰撞检测、逆运动学求解这些功能，打包成一个个独立的模块，然后通过ROS2的通信机制串起来。
我习惯把MoveIt2的架构分成三层：用户接口层、核心处理层和底层驱动层。
核心组件速览：
- MoveGroup节点：用户最常打交道的接口，提供“规划+执行”的集成服务。
- Planning Scene Monitor：负责维护规划场景的实时状态，包括机器人自身和周围物体。
- Planning Pipeline：规划流水线，串联规划器、逆运动学求解器、碰撞检测等。
- Robot Model Loader：加载URDF/SRDF，构建机器人的运动学模型。
数据流是怎么走的？嗯，这里我画了张图，你看一眼就明白了。
你看，用户通过RViz或代码发起规划请求，MoveGroup节点收到后，从Planning Pipeline里调用规划器，同时从Planning Scene Monitor获取当前场景信息。规划完成后，轨迹发回给MoveGroup，再下发给底层驱动执行。
我的经验：刚开始调试时，我总搞不清规划失败是场景问题还是规划器问题。后来我养成了一个习惯：先单独测试Planning Scene Monitor，用moveit_visual_tools把场景发布出来看看。如果场景里物体位置不对，规划器再牛也没用。
3.2 运动规划组：定义你的机械臂
运动规划组（Planning Group）是MoveIt2里最核心的概念之一。它定义了你要控制哪几个关节，以及这些关节的运动约束。
举个例子，一个6轴机械臂，你可以把全部6个关节定义成一个规划组叫"arm_group"。也可以把前3个关节定义成"position_group"，后3个定义成"orientation_group"。为什么这么干？我在做码垛项目时，就遇到过需要固定末端姿态、只调整位置的情况，这时候分开定义就方便多了。
规划组的定义在SRDF文件中。看这个例子：
<!-- 在robot.srdf中定义规划组 -->
<group name="arm_group">
<joint name="joint_1"/>
<joint name="joint_2"/>
<joint name="joint_3"/>
<joint name="joint_4"/>
<joint name="joint_5"/>
<joint name="joint_6"/>
</group>
<group name="gripper_group">
<joint name="gripper_left_finger_joint"/>
<joint name="gripper_right_finger_joint"/>
</group>定义好规划组后，MoveIt2会自动帮你做几件事：
- 计算运动学（正解和逆解）
- 生成关节空间和笛卡尔空间的路径
- 检查关节限位和速度/加速度约束
注意：规划组里的关节顺序很重要。MoveIt2默认按SRDF里定义的顺序排列关节。如果你在代码里传关节角度时顺序搞错了，机械臂会做出很诡异的动作。我曾经因为这个，让机械臂差点撞到旁边的传感器支架，还好我按了急停。
3.3 规划场景：让机器人“看见”世界
规划场景（Planning Scene）是MoveIt2对环境的“认知”。它包含两部分：
- 机器人自身状态：当前各关节角度、末端执行器位姿
- 环境物体：工作台上的障碍物、传送带、甚至其他机器人
为什么要单独搞个规划场景？你想想看，如果机械臂不知道桌上有杯水，它规划路径时直接穿过去，那不就打翻了？规划场景就是用来避免这种问题的。
在代码中添加障碍物很简单：
# 创建一个规划场景对象
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
# 定义一个桌子
table = CollisionObject()
table.id = "table"
table.header.frame_id = "world"
# 桌子的形状：长方体
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [1.0, 0.8, 0.05] # 长宽高
# 桌子的位置
table_pose = Pose()
table_pose.position.x = 0.5
table_pose.position.y = 0.0
table_pose.position.z = -0.025
table.primitives.append(box)
table.primitive_poses.append(table_pose)
table.operation = CollisionObject.ADD
# 发布到规划场景
scene = PlanningSceneInterface()
scene.apply_collision_object(table)这段代码我闭着眼都能写出来，因为项目里几乎每次都要加障碍物。注意那个frame_id，一定要和你的机器人基座坐标系一致，不然物体位置会飘。
3.4 碰撞检测：别让机械臂“打架”
碰撞检测是MoveIt2的看家本领。它要回答一个问题：这条路径上，机器人会不会撞到东西？
MoveIt2使用FCL（Flexible Collision Library）作为底层碰撞检测引擎。检测方式分两种：
| 检测类型 | 说明 | 性能 |
|---|---|---|
| 自碰撞检测 | 检查机器人自身连杆之间是否碰撞 | 快，默认开启 |
| 环境碰撞检测 | 检查机器人与障碍物是否碰撞 | 较慢，取决于障碍物数量 |
在实际项目中，我遇到过一个问题：碰撞检测太灵敏了。有时候机械臂明明离障碍物还有5厘米，MoveIt2却说会碰撞。这是因为碰撞检测用的是“包围盒”近似，不是精确的网格模型。
避坑指南：我曾经在一个精密装配项目里，因为碰撞检测的包围盒太大，导致规划出来的路径都绕远路，效率极低。后来我把每个连杆的碰撞检测精度从APPROXIMATE改成了EXACT，路径就顺畅多了。当然，代价是规划时间变长了。你需要根据项目需求做权衡。
你可以通过配置参数来控制碰撞检测的行为：
# 在moveit_config的config目录下，修改planning_scene_monitor.yaml
planning_scene_monitor:
publish_planning_scene: true
publish_geometry_updates: true
publish_state_updates: true
publish_transforms_updates: true
# 碰撞检测配置
collision_detection:
use_collision_map: true
collision_map_safety_radius: 0.01 # 安全半径，单位米这个collision_map_safety_radius参数很关键。设大了安全但路径绕，设小了路径短但风险高。我个人习惯设0.01米，也就是1厘米的余量，对于大多数工业场景够用了。
好了，这一章的内容就到这。MoveIt2的架构、规划组、场景和碰撞检测，是后续所有操作的基础。你把这些搞明白了，后面学运动规划算法、抓取策略时，就会轻松很多。