6.3 MoveIt2机械臂运动规划

6.3 MoveIt2 机械臂运动规划

本节摘要：如果说 Nav2 解决"轮子怎么滚到目的地"，MoveIt2 解决"关节怎么转到目标姿态"——后者要在三维空间里绕开自身与环境的碰撞，问题更稠密。本节讲清 MoveIt2 的三组件（规划场景、运动学、规划执行器），走通一次点到点规划的完整代码路径，并交代它如何消费第 5 章的 URDF 模型与 TF 树。导航与机械臂规划的思想互为镜像，对照着学收效翻倍。

规划的对象变了：从路径到轨迹

先做一个概念升级。Nav2 的全局规划输出是"路径"（空间上的一串点），MoveIt2 的输出是"轨迹"（每个关节角随时间的函数）——除了经过哪里，还包括何时经过、以什么速度。多了时间维度的原因很直接：机械臂的约束主要在动力学上，猛起猛停会甩飞末端负载。所以机械臂规划管线里永远有一个"时间参数化"步骤，把几何路径烹制成带时间戳的关节轨迹。

一、三组件：规划场景、运动学、规划执行器

**规划场景（planning scene）**是机器人的世界模型：从 URDF 读来的自身几何、从传感器或人工注入的环境障碍、当前关节状态。碰撞检查就在这里进行——候选轨迹的每个采样点都对场景做一遍自碰与环境碰检测。运动学模块负责两个方向的正逆解：正解是"给定关节角算末端位姿"，逆解是"给定目标位姿反求关节角"——逆解才是一切"我想让末端去这"需求的翻译官。规划执行器把前两者串起来：取场景、取目标、调规划算法（默认 OMPL 采样规划）、时间参数化、下发执行。

第 5 章的伏笔在此兑现：URDF 的质量决定规划上限。碰撞几何粗了（碰撞体包得太松），机器人不敢钻窄缝；惯量错了，时间参数化给出的速度就是执行不了的——5.1 节强调"三要素纪律"正是为此。

二、点到点规划的完整代码路径

以 Python 接口（moveit_py）为例，一次"把末端移到目标位姿"的最短代码：

from moveit.planning import MoveItPy

arm = MoveItPy(node_name='moveit_py')
arm_group = arm.get_planning_group('arm')

# 目标一：关节空间——每个关节转多少度，直觉直观
arm_group.set_start_state_to_current_state()
arm_group.set_joint_value_target({
    'joint1': 0.5, 'joint2': -1.0, 'joint3': 0.8})

# 目标二：位姿空间——只说末端要去哪，逆解自动翻译
# arm_group.set_pose_target([0.30, 0.10, 0.35])

plan_result = arm_group.plan()
if plan_result.error_code.value == 1:      # 1 即成功
    trajectory = plan_result.trajectory
    print(f'轨迹共 {len(trajectory.get_waypoints())} 个路点')
    arm.execute(trajectory)                 # 下发到控制器执行
else:
    print('规划失败：目标不可达或路径被堵')
​

两条目标设定路径各有适用场景：关节空间目标用于"摆姿势"（回零位、进料位），位姿空间目标用于"末端去哪"（对准工件）。失败时错误码值得读：不可达（逆解无解）与被堵（逆解有解但路径碰撞）是两种病，前者改目标，后者查场景。

# 命令行观测：轨迹由控制器执行，听它发布的状态
ros2 topic echo /joint_states --once        # 当前关节角
ros2 action list | grep follow              # 轨迹执行是标准动作接口
# follow_joint_trajectory —— 又一次见到 3.2 的三段式
​

图 6-3：从目标位姿到执行轨迹的规划管线

三、工程要点：场景维护与安全护栏

场景的真相维护是最容易被低估的工作。演示用的静态障碍手工注入即可；真实工作单元里障碍会动（传送带上的工件、人手），要么用深度相机把三维点云挂进体素层（Nav2 的体素层思想在这里再现），要么接受"场景滞后于现实"并靠低速与力控兜底。我更倾向后者起步——视觉场景维护的工程成本常被严重低估，先低速跑通，再逐步加感知。

安全护栏是产线刚需：执行前检查关节限位与速度上限（控制器层硬约束），目标点加"可达域白名单"，人与机器人共域时启用安全控制模式。这些不属于 MoveIt2 本身，但没有它们的 MoveIt2 部署不该离开实验室。

💡 关键直觉：导航与机械臂规划是同一个问题的两个投影——"在不碰任何东西的前提下，从当前状态到目标状态"。区别只在自由度与约束形状。带着这个视角学 MoveIt2，Nav2 里学过的代价、恢复、护栏概念全能平移过来。

四、规划失败的两种病：不可达与被堵

MoveIt2 的失败诊断值得单独成段，因为两类失败的治疗方向完全相反，误诊的代价是白费一轮调参。

不可达（逆解无解）：目标位姿超出了机械臂的工作空间，或落在了关节限位之外、逆解的奇异区里。症状是规划器在目标设定阶段就报错，甚至根本不进入搜索。治疗方向是改目标——往工作空间内部收，或拆分成两段运动。验证手段很直接：在 RViz 里用交互标记手动拖到目标位姿附近，能拖到就是可达，拖不到就别为难逆解器。

被堵（逆解有解但搜不过）：目标本身合法，但起点到目标之间的路径会撞——撞自己（关节构型穿模）、撞环境（没声明的障碍）、撞"以为的障碍"（碰撞体建得太胖）。症状是搜索超时或报"未找到有效路径"。治疗方向依次是：查场景里有没有多余障碍声明、查碰撞几何是否过松、换规划目标（中间加一个过渡位姿往往能绕开局部构型困境）。

区分两者的快速判据：先问目标能不能拖。能拖到（可达）却规划失败，就是被堵类；拖不到，就是不可达类。这个十秒钟的手动检查，能避免最长见的误诊——对着不可达的目标反复放宽碰撞容差，只会让真正的碰撞检查形同虚设。

另有一个工程提醒：采样规划器每次给出的轨迹可能不同（随机性本质），评估规划质量要看多次运行的稳定性而不是某一次的运气。发版前的回归测试里，同一目标至少连跑五次全成才算过关——单次成功在产线上不叫能力，叫侥幸。

本节要点回顾

• 轨迹比路径多时间维，时间参数化是规划管线的固定工序；
• 三组件分工：场景管碰撞事实、运动学管位姿翻译、规划执行器管搜索与下发；
• URDF 质量决定规划上限：碰撞体松紧与惯量真伪直接改变规划行为；
• 目标设定两路径：关节空间摆姿势、位姿空间去地点，失败码区分不可达与被堵；
• 场景维护从"接受滞后加低速"起步，安全护栏是离开实验室的前置条件。

到此，机器人"会做事"的三套系统齐了。下一章打开工具箱：这些系统出问题时，用什么看清它们。