面试翻车现场

面试一家做仓储拣选机器人的公司，面试官问："MoveIt2的拾取放置（Pick and Place）做过吗？整个流程是怎样的？"

我说先规划到物体上方，然后下降抓取，再规划到放置位置。他追问："约束规划了解吗？比如要求机械臂在搬运过程中末端朝上，怎么实现？Planning Scene怎么和视觉系统联动？"

这几个问题暴露了我对MoveIt2高级功能的不足。面试官说："拾取放置是机械臂最基础的应用，但要做好需要考虑约束、碰撞、场景更新等很多细节。"

约束规划

约束规划是MoveIt2的高级功能之一。它允许你在规划时添加额外的限制条件，比如末端执行器的朝向、关节角度的范围、或者末端在某个平面内运动。

最常见的约束是方向约束（Orientation Constraint）。比如你搬运一杯水，要求末端在运动过程中保持朝上。你可以给MoveIt设置一个约束：末端Z轴和重力方向的夹角不超过30度。

设置约束的方式是通过Constraints消息。你可以指定位置约束（position constraints）和方向约束（orientation constraints），定义末端在运动过程中必须满足的几何条件。

from moveit_msgs.msg import Constraints, OrientationConstraint
constraint = OrientationConstraint()
constraint.header.frame_id = "base_link"
constraint.link_name = "tool0"
constraint.orientation.w = 1.0
constraint.absolute_x_axis_tolerance = 0.1
constraint.absolute_y_axis_tolerance = 0.1
constraint.absolute_z_axis_tolerance = 3.14
move_group.set_path_constraints(constraint)

约束规划的计算量比自由规划大很多，因为规划器需要在满足约束的解空间中搜索。如果约束太严格，可能找不到解。建议先放宽约束看看能不能规划成功，再逐步收紧。

Planning Scene高级操作

Planning Scene不只是添加几个碰撞体那么简单。在复杂应用中，你需要动态维护场景信息。

和视觉系统联动是最常见的需求。你用深度相机检测到桌面上的物体，把它们的位姿和形状更新到Planning Scene中。MoveIt在规划时就会自动避开这些物体。

具体实现：视觉节点检测到物体后，通过PlanningSceneInterface的add_collision_object()方法把物体添加到场景。物体可以是简单的几何体（立方体、圆柱体），也可以是从点云分割出来的mesh。

场景更新要注意几个问题。第一，物体的ID要唯一，重复添加同ID的物体会覆盖旧的。第二，物体形状要尽量精确——用简单几何体近似可以，但如果形状差太多，规划出来的轨迹可能在实际中碰撞。第三，要及时清理不再存在的物体，否则场景里会堆积很多"幽灵"障碍物。

附着物体（Attached Object）是另一个重要概念。当你夹爪抓住一个物体后，这个物体就变成了机械臂的一部分。你需要把它从普通碰撞体变成附着物体，这样它就不会和末端执行器发生碰撞检测了。

拾取放置流程

一个完整的Pick and Place流程是这样的。

第一步，感知。用视觉系统识别目标物体的位姿和形状，更新到Planning Scene中。

第二步，规划接近。规划机械臂从当前位置运动到物体上方一定高度（pre-grasp pose）。这个位置要考虑物体周围的障碍物。

第三步，规划下降。从pre-grasp pose垂直下降到抓取位置（grasp pose）。这一步通常用直线运动（笛卡尔空间规划），不用自由空间规划。

第四步，执行抓取。关闭夹爪，把物体设置为附着对象。

第五步，规划抬起。从grasp pose垂直抬起。同样用笛卡尔规划。

第六步，规划移动。从当前位置运动到放置位置上方。如果有方向约束（比如保持物体朝上），在这一步设置。

第七步，规划放置。下降到放置位置，打开夹爪，分离附着对象，抬起离开。

整个流程中任何一步失败都需要处理——回退到安全位置，或者重新规划。

笛卡尔路径规划

拾取放置中经常需要直线运动，这就是笛卡尔路径规划（Cartesian Path Planning）。

和自由空间规划不同，笛卡尔规划要求末端沿着指定的直线路径运动。MoveIt通过compute_cartesian_path()方法实现。你给一系列 waypoints（路径点），它会计算出末端沿着这些点直线运动的关节轨迹。

笛卡尔规划的成功率不如自由空间规划高，因为直线路径可能被障碍物挡住。如果返回的轨迹不完整（fraction小于1.0），说明路径被挡住了，你需要调整路径或者绕障。

常见问题排查

"规划总是失败"——先检查Planning Scene。在Rviz的MotionPlanning插件里看看场景中的碰撞体是否正确。很多时候规划失败是因为场景里有"幽灵"障碍物（之前添加的但没清理的碰撞体）。用get_planning_scene服务查看当前场景。

"规划成功但执行时碰撞"——Planning Scene和真实环境不一致。视觉检测有延迟或者精度不够，导致场景中的障碍物位置和实际有偏差。解决办法是在障碍物周围加一层安全余量（padding），或者在执行前再做一次碰撞检查。

"规划时间太长"——检查规划算法的参数。OMPL的默认规划时间是5秒，对于简单场景可以缩短到1秒。如果场景很复杂（很多障碍物），采样规划器需要更多时间找到通过的路径。可以考虑用PRM算法（适合重复规划相似场景）或者减小规划精度要求。

"逆运动学找不到解"——目标位姿可能超出了机械臂的工作空间，或者在奇异点附近。先用set_joint_value_target设置关节角度试试，如果关节角度能到达，说明是IK求解的问题。可以增大kinematics_solver_attempts或者换一个IK求解器。

"约束规划找不到解"——约束太严格了。比如要求末端完全水平（容差0度），这在数值上几乎不可能满足。给一定的容差范围，比如±5度。另外，约束规划对初始状态敏感，如果当前状态就违反约束，规划器可能直接放弃。

拾取放置的安全策略

实际部署拾取放置系统时，安全策略不能忽视。

首先是速度控制。机械臂在接近物体和放置物体时应该降低速度，避免碰撞时造成损坏。MoveIt支持通过max_velocity_scaling_factor参数降低规划速度。在接近物体的最后阶段，速度应该降到正常速度的20%以下。

其次是力矩监控。如果机械臂有得力矩传感器反馈，可以在运动过程中监控关节力矩。力矩突然增大可能意味着碰撞，这时候应该立即停止运动。ros2_control的JointTrajectoryController支持配置力矩限制，超过阈值自动停机。

最后是工作空间限制。通过set_workspace()方法限定机械臂的运动范围，防止它运动到超出物理限制的位置。在调试阶段，先用仿真验证所有动作的安全性，再上实车测试。

补充一个拾取放置调试的实用技巧：在正式跑流程之前，先用Rviz的MotionPlanning插件手动拖拽机械臂到各个关键位姿（pre-grasp、grasp、pick-up），检查每个位姿是否可达、有没有碰撞。这一步虽然简单，但能帮你提前发现很多位姿设置不合理的问题。我见过不少人直接跑自动流程，结果在第一步下降时就撞了，浪费大量时间排查是位姿问题还是规划问题。手动验证一遍关键位姿，能省下很多调试时间。

面试中怎么聊

面试官问MoveIt2进阶，你可以说："约束规划通过Constraints消息限制末端运动方向，计算量大但能实现特殊要求。Planning Scene需要和视觉系统联动，动态更新障碍物信息。拾取放置的完整流程包括感知、接近、下降、抓取、抬起、移动、放置七步，每步都要处理失败情况。笛卡尔规划用于直线运动，成功率不如自由规划但轨迹可控。实际项目中，场景维护和约束设置是最影响系统稳定性的环节。"

---

上一篇：第137篇 MoveIt2入门——机械臂运动规划的ROS2方案

下一篇预告：第139篇 ROS2调试技巧——常见错误的排查思路和工具