🦾 MoveIt2 进阶:运动规划与抓取管线

1 运动规划问题定义

上一页我们只「用了」规划器,这一页要回答一个更根本的问题:规划器到底在解什么?运动规划的形式化定义是:给定机器人当前状态、目标状态、障碍物集合,找一条从起点到终点、全程无碰撞、满足运动学约束的轨迹(按时间排列的位形序列)。

1.1 构型空间 C-space:规划的「主战场」

机器人一条手臂若有 n 个关节,它的完整状态可以用一个 n 维向量 q = (q₁, q₂, …, qₙ) 表示。所有可能 q 组成的空间叫构型空间(Configuration Space,简称 C-space)。规划器不在三维物理空间里搜索,而是在 C-space 里搜索:

1.2 正/逆运动学在规划中的角色

• 正运动学(FK,Forward Kinematics):给定关节角 q,算出末端位姿。它是确定、唯一、快的,规划器每采样一个 q 都要用它把「关节态」映射到「物理空间」去做碰撞检测。
• 逆运动学(IK,Inverse Kinematics):给定末端位姿,反解关节角 q。它可能有 0/1/多解(冗余手臂有无数解),且可能无解(目标够不着)或遇到奇异位形。
• 分工:采样规划器主要在关节空间(C-space)采样,用 FK 检验碰撞,几乎不依赖 IK;只有当目标是「末端位姿」时,才在最后用 IK 把位姿目标转成关节角目标(或由规划器把 IK 作为目标采样器)。笛卡尔路径规划(第 3 节)则是反过来,密集调用 IK。

1.3 采样规划 vs 解析规划

MoveIt2 默认是采样规划(OMPL),并在规划后追加「轨迹简化 + 时间参数化」步骤,把采样出的锯齿轨迹变平滑、再配上速度时间戳——这就是 request_adapters 干的活(见第 2 节 yaml)。

2 OMPL 规划器详解

OMPL(Open Motion Planning Library)是 MoveIt2 默认的规划后端,一个与机器人无关的采样规划算法库。MoveIt2 通过 ompl_interface 插件把 OMPL 挂进来,自带 RRT / RRTConnect / RRTstar / PRM / PRMstar / BiTRRT / EST / KPIECE / LazyPRM / STRIDE / SPARS 等 20 余种算法(不同版本略有增减)。

2.1 常用算法:直觉与适用场景

2.2 planner_id 配置:ompl_planning.yaml

MoveIt2 的规划器在 ompl_planning.yaml 里声明。关键结构:planner_configs 定义「算法库」,每个规划组的 planner_configs 列出「本组可用哪些」,default_planner_config 指定默认项。

planning_plugin: ompl_interface/OMPLPlanner

request_adapters: >-
    default_planner_request_adapters/AddTimeParameterization
    default_planner_request_adapters/FixWorkspaceBounds
    default_planner_request_adapters/FixStartStateBounds
    default_planner_request_adapters/FixStartStateCollision
    default_planner_request_adapters/FixStartStatePathConstraints

start_state_max_bounds_error: 0.1

planner_configs:
  RRTConnectkConfigDefault:
    type: geometric::RRTConnect   # 算法类型,决定用哪个 OMPL 规划器
    range: 0.0                    # 单步扩展上限;0=按状态空间自动取默认
  RRTstarkConfigDefault:
    type: geometric::RRTstar
    range: 0.0
  BiTRRTkConfigDefault:
    type: geometric::BiTRRT
    range: 0.0
  PRMkConfigDefault:
    type: geometric::PRM
    max_nearest_neighbors: 10

arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - RRTConnectkConfigDefault
    - RRTstarkConfigDefault
    - BiTRRTkConfigDefault
    - PRMkConfigDefault
  planning_time: 5.0        # 单次规划时间上限(秒)
  planning_attempts: 10     # 最多尝试几次
  max_solutions: 10         # 最多保留几条候选解再选优

运行时可临时改规划器(不重启):RViz MotionPlanning 面板里直接选,或代码里 move_group.setPlannerId("RRTstarkConfigDefault")。

2.3 规划参数:时间、次数与质量

2.4 规划结果质量评估

「规划成功」≠「轨迹好用」。评估一条轨迹至少要盯三件事:

• 路径长度:在 C-space 里从起点到终点累计走过的距离,越短通常越快、越省能量;
• 平滑度:相邻关节角的「抖动」程度。采样规划常产出锯齿,MoveIt2 用 AddTimeParameterization 与轨迹简化缓解;
• clearance(间隙):轨迹上每一点离最近障碍的最小距离。间隙越大越安全;BiTRRT 正是为最大化间隙而生。

3 约束规划与笛卡尔路径

很多任务要求末端全程保持某种姿态或轨迹形状——端一杯水不能倾斜、插销要沿直线插入、焊接要贴面滑行。这就要用到 MoveIt2 的路径约束(Path Constraints)。

3.1 三类路径约束

注意区分两个层级:目标约束(goal constraints)只约束「终点」,路径约束(path constraints)约束「全程每一点」。方向约束既可做目标也可做路径,含义完全不同——这是新手常踩的坑。

3.2 笛卡尔路径 computeCartesianPath

当你需要末端沿一条直线(或给定路径点序列)运动时,关节空间规划会「乱绕」,要用笛卡尔路径规划:在笛卡尔空间按步长插值出密集途经点,对每个点解 IK,再串成轨迹。核心接口(C++ MoveGroupInterface):

// waypoints: 一串末端位姿(几何直线就取起点与终点间等距插值)
std::vector<geometry_msgs::msg::Pose> waypoints;
// ... 填充 waypoints ...

moveit_msgs::msg::RobotTrajectory trajectory;
// eef_step:末端每步移动距离(m);jump_threshold:允许关节最大跳变(0=禁用)
double fraction = move_group.computeCartesianPath(
    waypoints, eef_step, jump_threshold, trajectory);

// fraction ∈ [0,1]:= 1 表示走完全程;小于 1 表示中途卡住(奇异/碰撞/不可达)
if (fraction >= 0.999) {
    move_group.execute(trajectory);
} else {
    RCLCPP_WARN(node->get_logger(),
                "笛卡尔路径仅完成 %.1f%%,剩余段未解出", fraction * 100);
}

Python 侧(MoveIt2 官方绑定为 MoveItPy,前身是 ROS1 的 moveit_commander)做「规划 + 执行」的最小示例:

import rclpy
from rclpy.node import Node
from moveit_py import MoveItPy

rclpy.init()
node = Node("moveit_py_demo")

# 初始化 MoveItPy(会加载 moveit_config 包)
moveit_py = MoveItPy(node_name="moveit_py")
arm = moveit_py.get_planning_component("arm")

# 起点=当前状态,目标=命名位姿 "home"
arm.set_start_state_to_current_state()
arm.set_goal_state(configuration_name="home")

# 规划
plan_result = arm.plan()
if plan_result:
    # plan_result.trajectory 是 RobotTrajectory,可交给控制器执行
    moveit_py.execute(plan_result.trajectory, controllers=[])
else:
    node.get_logger().warn("规划失败,请检查目标/约束/碰撞")

3.3 人形机器人手臂常用约束

• 端平托盘:末端坐标系 Z 轴始终朝上(方向约束,夹角容差约 ±10°);
• 插销/插线:末端沿销孔轴线直线推进(位置约束绑定轴线 + 方向约束锁定指向);
• 双臂合抱搬运:两末端之间保持相对位姿约束(见第 7 节双臂协同);
• 避让躯干/头部:用关节约束限制肘部朝外,防止手臂扫过「脸」。

4 避障与碰撞检测

规划器「避障」靠的是碰撞检测,而 MoveIt2 的碰撞检测分两层:自碰撞(机器人自己打自己)与环境碰撞(机器人与周围物体)。

4.1 碰撞矩阵 ACM:允许碰撞矩阵

机器人的某些连杆天生就是贴着的(如相邻连杆、夹爪的两指),如果处处检测会误报大量「碰撞」。ACM(Allowed Collision Matrix,允许碰撞矩阵)声明「哪些连杆对之间不检查碰撞」,从而避免误报、加速检测。它由 Setup Assistant 采样生成,存于 SRDF 的 <disable_collisions> 段,并以 default 表示「默认允许、其它情况才查」。

<!-- SRDF 片段:声明 link1 与 link2 之间永不检查碰撞 -->
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
<!-- 或:默认允许,只有列出的「危险对」才检查 -->
<disable_collisions link1="base_link" link2="gripper_link" reason="Default"/>

4.2 自碰撞 vs 环境碰撞

• 自碰撞(self-collision):机器人不同连杆之间的碰撞。用简化碰撞几何 + ACM 检查,Setup Assistant 生成。
• 环境碰撞(environment collision):机器人与外部物体(桌、箱、人、工件)的碰撞。外部物体通过 PlanningScene 加入,用 collision_object 或 attached_collision_object(抓在手上的物体)表示。

4.3 Octomap:点云 → 八叉树 → 规划场景

真实场景里障碍物是动态、未知的,靠深度相机/激光雷达的点云来「看」。MoveIt2 的感知避障链路:

Octomap 的优势是内存友好 + 多分辨率:空旷区用粗大格子,障碍边缘用细密格子,适合大范围场景。配置要点是 sensors_3d.yaml(声明点云话题、更新频率、过滤范围)与 octomap 参数(体素分辨率,典型 0.05 m)。

4.4 动态障碍与规划重试

5 抓取管线:MoveIt Task Constructor

「抓起一个东西」不是一个规划动作,而是一连串有依赖关系的子任务:先规划到抓取点、再闭夹、再抬升、再搬运、再放置。若把整件事塞进一次 MoveGroup 规划,成功率极低且难调试。MoveIt Task Constructor(MTC) 就是为此设计的:把复杂操作拆成有向图上的多个 Stage(阶段),每个 Stage 可独立求解、可回溯,前一个 Stage 的成功结果作为后一个的起点。

5.1 MTC 核心概念

• Task(任务):一张由 Stage 组成的图,定义「这件事怎么做」;
• Stage(阶段):图上的节点,分三大类——Generator(生成候选解,如抓取姿态)、Propagator(单进单出,如沿路运动)、Connector(连接两个状态,如两段轨迹的拼接);
• Interface State:在 Stage 之间流动的「机器人状态快照」,携带当前位形、夹爪状态、附着的物体;
• Solution:从起始 Stage 走到终止 Stage 的一条完整路径,即一套可执行的抓取方案。

5.2 抓取流程的典型 Stage

MTC 官方仓库自带 demo/scripts/pickplace.py 与 C++ 版 Pick and Place 示例,可作为起点改写。配置好后的启动方式:

# 官方 Pick and Place 示例(以 panda 为例,不同版本包名略有差异)
ros2 launch moveit_task_constructor_demo pick_place_demo.launch.py
# 或跑 Python 脚本版
python3 pickplace.py

5.3 感知集成与夹爪配合

• 感知:抓取姿态由感知给出——物体位姿估计(如 04-12 的 6D 位姿估计)后,在物体坐标系里离线生成 Grasp 数据库(类似 GraspIt!/GraspGenerator 的思路),运行时按位姿变换到当前物体位置;MTC 的 GenerateGraspPose 就是把抓取库「铺」到物体上。
• 夹爪:抓取流程里夹爪开合是独立于手臂规划的动作,通常由夹爪控制器直接下发开/合指令,并在场景里同步 attached_collision_object(物体随末端一起运动,才能正确检测「抬着箱子时会不会撞」)。

6 MoveIt2 与真实机器人

仿真里 MoveIt2 规划的轨迹「凭空」执行;真机上轨迹必须经 ros2_control 下发给关节模组,并由硬件接口读回编码器反馈。

6.1 集成链路:controller manager + joint_trajectory_controller

MoveIt2 规划轨迹(MoveGroup / MoveItPy)
        │  发布 moveit_msgs/msg/JointTrajectory
        ▼
joint_trajectory_controller(ros2_control)
        │  每个控制周期(如 100 Hz)插值下发位置/速度
        ▼
hardware_interface(真机驱动:CAN/EtherCAT 或仿真插件)
        │  写入电机命令 / 读回编码器
        ▼
关节模组(伺服电机 + 减速器)

真机配置示例(ros2_controllers.yaml):

controller_manager:
  ros__parameters:
    update_rate: 100   # 控制循环 Hz

arm_controller:
  ros__parameters:
    type: joint_trajectory_controller/JointTrajectoryController
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
    command_interfaces:
      - position           # 位置控制接口(也可加 velocity/effort)
    state_interfaces:
      - position
      - velocity
    state_publish_rate: 50.0
    action_monitor_rate: 20.0
    allow_partial_joints_goal: false
    constraints:
      stopped_velocity_tolerance: 0.01
      goal_time: 0.0

6.2 真实机械臂部署流程

6.3 伺服 servoing:绕过「规划」的实时控制

规划适合「点到点」;而遥操作、力引导、人机协作需要机器人实时跟随外部输入(手柄、手部追踪、力矩)。moveit_servo 提供高频率(约百 Hz 级)的末端速度伺服:输入「末端期望速度/增量」,直接解出关节速度下发,不做完整规划。它常与视觉遥操作、临场感控制配合,是实现「人手带着机械臂走」的关键模块。

6.4 常见失败与调试

7 人形机器人场景

人形机器人的手臂规划与桌面机械臂最大的不同:双臂、双足浮动基座、全身平衡。这里只谈手臂层面的落地现状,整机平衡与步态见本站其它章节。

7.1 双臂协同规划(dual-arm)

双臂协同通常有两种做法:

• 两个独立 MoveGroup:左右臂各一个规划组,各自规划,再用相对位姿约束把两末端「绑」在一起(如合抱箱子时两掌心相对、距离固定)。简单,但难保证两臂轨迹时序同步。
• 合并为一个大规划组:把左右臂关节并入一个 14-DoF 组,一次规划出双臂联合轨迹;时序天然同步,但 C-space 维数翻倍、规划更慢,且碰撞矩阵更复杂。

实际工程里常用「主子臂」模式:主臂走笛卡尔/关节规划,从臂用相对约束跟随,兼顾成功率与同步性。

7.2 宇树 H1:整机 URDF 与双臂规划

宇树 H1 是双臂人形机器人,官方 unitree_ros 仓库提供了 h1_description(URDF 与 MJCF 两种描述),以及 H1-2 型号描述;同时社区有基于 H1 的 ROS2 SDK 与 MoveIt 配置实践。在 H1 上跑 MoveIt2 的典型路径是:用官方 URDF 经 Setup Assistant 生成 moveit_config,建 left_arm / right_arm 规划组,再接入宇树的关节控制接口。注意:H1 双臂的 MoveIt 配置多由社区维护,官方公开资料以描述文件与 SDK 为主,具体型号的规划组命名与控制器需以对应仓库为准(约)。

7.3 智元(稚晖君团队)开源实践

智元机器人(AgiBot)在 2024 年 10 月开源了 灵犀 X1 的软硬件资料(官方称「0 元购」软硬件全套图纸与代码),含推理代码仓库 AgibotTech/agibot_x1_infer 等,以及自研开源通信中间件 AimRT。这套资料为「整机 + 双臂操作」的学习与二次开发提供了从模型到算法的完整参考。手臂层面的规划仍可复用 MoveIt2 生态(约,具体以各仓库 README 为准)。

8 性能与进阶资源

继续深入的方向与官方入口(均为核实过的官方文档,详见文末「参考来源」):

• MoveIt2 官方文档:moveit.picknik.ai —— 概念、教程、API 一体,首选;
• MoveIt Task Constructor:官方仓库 moveit/moveit_task_constructor 与 Pick and Place with MTC 教程;
• OMPL:ompl.kavrakilab.org —— 算法库官网与各类规划器的形式化描述;
• ros2_control:control.ros.org —— 控制器与硬件接口文档;
• servo:Realtime Arm Servoing 教程。

9 本节自测

10 参考来源

• MoveIt 2 官方文档
• MoveIt2 · Planning Scene Monitor(规划场景与感知同步)
• OMPL 官方文档(采样规划算法库)
• MoveIt Task Constructor 官方文档
• MoveIt2 · Pick and Place with MoveIt Task Constructor 教程
• MoveIt Task Constructor GitHub 仓库(demo/scripts/pickplace.py)
• ros2_control 官方文档
• joint_trajectory_controller 用户文档
• MoveIt2 · Realtime Arm Servoing(servo)教程
• MoveIt2 官方教程仓库(moveit2_tutorials)
• 宇树官方 unitree_ros 仓库(h1_description:URDF 与 MJCF)
• 智元机器人 AgibotTech 开源组织(灵犀 X1 开源资料、AimRT 中间件)
• 本站 04-10 · ROS2 实战进阶:URDF 与 MoveIt2(本页前置)

资料整理更新至 2026-08;版本与参数以各官方文档当前版本为准,本文中未能逐一核实之处均标注「约」。