ROS2 + MoveIt2 + IMU：三自由度人体手臂实时控制机械臂

灿灿221

601人浏览 · 2026-05-09 18:20:04

灿灿221 · 2026-05-09 18:20:04 发布

一、项目概述

用 3 个维特智能 WT61 IMU 传感器佩戴在人体上臂、前臂、手腕上，通过 ROS2 读取姿态数据，发布 TF 变换链，再由 MoveIt2 进行逆运动学求解，实现 6-DOF 机械臂末端实时跟随人手臂运动。

硬件：

6-DOF 机械臂
3 × WT61 IMU（RS485 Modbus 协议，ID: 0x51/0x52/0x53）
USB 转 RS485 适配器

软件栈： ROS2 Humble + MoveIt2 + wit_ros2_imu

二、系统架构

整个系统由 3 个进程组成，各自独立运行：

进程 1: IMU 驱动
  wit_ros2_imu 节点 → 读取 3 个 IMU 的加速度/陀螺仪/角度数据
  robot_state_publisher → 根据 arm.urdf 发布 TF 链

进程 2: MoveIt2 Demo
  move_group → 逆运动学求解引擎
  controller_manager → 轨迹执行管理
  robot_state_publisher → 机械臂 TF 链

进程 3: imu_to_moveit 控制节点
  监听 wrist TF → 低通滤波 → 工作空间检查 → MoveIt 规划 → 轨迹执行

两条 TF 链并行存在：

人体手臂：base_link → upper_arm → forearm → wrist（IMU 驱动发布）
机械臂：base_link → Link0 → Link1 → ... → Link5（MoveIt demo 发布）

控制节点通过 lookup_transform("base_link", "wrist") 获取人体手腕在 base_link 坐标系下的位姿，作为机械臂末端的目标。

三、人体手臂 URDF 建模

首先需要一个 URDF 来描述人体手臂的 3 段连杆结构，让 robot_state_publisher 能发布 TF：

<?xml version="1.0"?>
<robot name="human_arm">
  <link name="base_link"/>

  <!-- 上臂：0.3m -->
  <link name="upper_arm">
    <visual>
      <geometry><box size="0.08 0.05 0.3"/></geometry>
      <material name="red"><color rgba="1 0 0 1"/></material>
    </visual>
  </link>

  <!-- 前臂：0.27m -->
  <link name="forearm">
    <visual>
      <geometry><box size="0.08 0.05 0.27"/></geometry>
      <material name="green"><color rgba="0 1 0 1"/></material>
    </visual>
  </link>

  <!-- 手腕：0.12m -->
  <link name="wrist">
    <visual>
      <geometry><box size="0.08 0.05 0.12"/></geometry>
      <material name="blue"><color rgba="0 0 1 1"/></material>
    </visual>
  </link>

  <!-- 关节（全部 fixed，姿态由 IMU 驱动） -->
  <joint name="joint_shoulder" type="fixed">
    <parent link="base_link"/>
    <child link="upper_arm"/>
    <origin xyz="0 0 0"/>
  </joint>

  <joint name="joint_elbow" type="fixed">
    <parent link="upper_arm"/>
    <child link="forearm"/>
    <origin xyz="0 0 0.3"/>
  </joint>

  <joint name="joint_wrist" type="fixed">
    <parent link="forearm"/>
    <child link="wrist"/>
    <origin xyz="0 0 0.27"/>
  </joint>
</robot>

关键点： 关节类型设为 fixed，因为实际的姿态旋转由 IMU 驱动节点直接发布到 /tf，而不是由 joint_states 驱动。

四、IMU 驱动配置

使用 wit_ros2_imu 包，配置 3 个 IMU 的 Modbus ID：

ros2 launch wit_ros2_imu imu.launch.py \
  port:=/dev/imu_usb \
  baudrate:=115200 \
  protocol:=RS485_STD \
  modbus_ids:=[0x51,0x52,0x53]

这个 launch 文件会同时启动：

wit_ros2_imu 驱动节点 — 读取串口数据，发布 /imu/data/imu_51 等话题，并直接发布 TF 变换
robot_state_publisher — 根据 arm.urdf 发布静态 TF 链

IMU 安装位置：

IMU 0x51 → 上臂（距肩关节 0.15m）
IMU 0x52 → 前臂（距肘关节 0.135m）
IMU 0x53 → 手腕（距腕关节 0.06m）

五、控制节点核心实现

5.1 为什么不用 pymoveit2

最初尝试使用 pymoveit2 库（一个 Python 封装的 MoveIt2 接口），但发现其内部的 JointState 订阅器存在 QoS 不匹配问题 — 发布者用的是 reliable + volatile，而 pymoveit2 订阅者用的是 best_effort，导致永远收不到关节状态消息。

解决方案： 完全绕过 pymoveit2，直接使用 MoveIt2 的原生 ROS2 接口：

/plan_kinematic_path（GetMotionPlan 服务）— 发送规划请求
/execute_trajectory（ExecuteTrajectory Action）— 执行轨迹

5.2 控制循环流程

核心控制循环以 20Hz 频率运行，每帧执行以下步骤：

def control_loop(self):
    # 1. 如果正在执行轨迹，跳过（防止并发）
    if self.is_executing:
        return

    # 2. 从 TF 获取手腕位姿
    transform = self.tf_buffer.lookup_transform("base_link", "wrist", Time())

    # 3. 转换为 PoseStamped
    target_pose = PoseStamped()
    target_pose.pose.position.x = transform.transform.translation.x
    # ... y, z, orientation

    # 4. 低通滤波平滑
    target_pose = self.smooth_target_pose(target_pose)

    # 5. 工作空间边界检查
    if not self.check_workspace_bounds(target_pose):
        return

    # 6. 构建 MoveIt 规划请求
    request = GetMotionPlan.Request()
    # ... 设置目标约束

    # 7. 异步发送规划请求
    future = self.plan_client.call_async(request)
    future.add_done_callback(lambda f: self.plan_callback(f, target_pose))

5.3 构建 MoveIt 规划请求

这是最关键的部分 — 如何告诉 MoveIt "把末端移到这个位置"：

request = GetMotionPlan.Request()
request.motion_plan_request.group_name = "lll"  # 规划组名
request.motion_plan_request.num_planning_attempts = 5  # 尝试 5 次求解
request.motion_plan_request.allowed_planning_time = 1.0  # 最多规划 1 秒
request.motion_plan_request.max_velocity_scaling_factor = 0.5  # 速度缩放
request.motion_plan_request.max_acceleration_scaling_factor = 0.5

# 起始状态：当前关节角度
request.motion_plan_request.start_state.joint_state = self.current_joint_state

# 目标约束：位置 + 姿态
constraint = Constraints()
constraint.name = "target_pose"

# 位置约束：用一个半径 1mm 的球体定义目标点
pos_constraint = PositionConstraint()
pos_constraint.header.frame_id = "base_link"
pos_constraint.link_name = "Link5"  # 末端连杆
volume = SolidPrimitive()
volume.type = SolidPrimitive.SPHERE
volume.dimensions = [0.001]  # 1mm 精度
pos_constraint.constraint_region.primitives.append(volume)
pos_constraint.constraint_region.primitive_poses.append(target_pose.pose)
pos_constraint.weight = 1.0
constraint.position_constraints.append(pos_constraint)

# 姿态约束
orient_constraint = OrientationConstraint()
orient_constraint.header.frame_id = "base_link"
orient_constraint.link_name = "Link5"
orient_constraint.orientation = target_pose.pose.orientation
orient_constraint.absolute_x_axis_tolerance = 0.01  # 容差
orient_constraint.absolute_y_axis_tolerance = 0.01
orient_constraint.absolute_z_axis_tolerance = 0.01
orient_constraint.weight = 1.0
constraint.orientation_constraints.append(orient_constraint)

request.motion_plan_request.goal_constraints.append(constraint)

5.4 异步规划与执行

规划和执行都是异步的，避免阻塞控制循环：

# 规划回调
def plan_callback(self, future, target_pose):
    result = future.result()
    if result.motion_plan_response.error_code.val == 1:  # SUCCESS
        trajectory = result.motion_plan_response.trajectory.joint_trajectory

        # 安全检查：遍历轨迹每个点
        for point in trajectory.points:
            if not self.check_joint_limits(point.positions):
                return  # 关节超限，丢弃
            if self.detect_singularity(point.positions):
                return  # 奇异点，丢弃

        self.execute_trajectory(trajectory)

# 执行轨迹
def execute_trajectory(self, trajectory):
    goal = ExecuteTrajectory.Goal()
    goal.trajectory.joint_trajectory = trajectory
    self.is_executing = True
    future = self.execute_client.send_goal_async(goal)
    future.add_done_callback(self.execute_callback)

# 执行完成后释放标志位
def execute_result_callback(self, future):
    self.is_executing = False  # 允许下一次规划

5.5 MultiThreadedExecutor

由于使用了异步回调（规划回调、执行回调），必须使用多线程执行器，否则回调会阻塞定时器：

def main(args=None):
    rclpy.init(args=args)
    node = ImuToMoveit()
    executor = MultiThreadedExecutor(num_threads=2)
    executor.add_node(node)
    executor.spin()

六、安全防护机制

6.1 奇异点检测

基于关节极限接近度的简化奇异点检测。当轨迹中任意关节角度接近极限的 2% 范围内时，判定为奇异点，丢弃整条轨迹：

def detect_singularity(self, joint_positions):
    for i, name in enumerate(self.current_joint_state.name):
        min_limit, max_limit = self.joint_limits[name]
        range_limit = max_limit - min_limit
        margin = range_limit * 0.02  # 2% 边界
        if joint_positions[i] < min_limit + margin or \
           joint_positions[i] > max_limit - margin:
            return True
    return False

6.2 工作空间边界检查

防止 IMU 抖动导致目标超出机械臂可达范围：

def check_workspace_bounds(self, pose):
    x, y, z = pose.pose.position.x, pose.pose.position.y, pose.pose.position.z
    if not (-1.0 <= x <= 1.0): return False
    if not (-1.0 <= y <= 1.0): return False
    if not (-0.5 <= z <= 1.5): return False
    return True

6.3 目标位姿低通滤波

IMU 数据存在高频抖动，直接使用会导致机械臂震颤。一阶低通滤波：

def smooth_target_pose(self, new_pose):
    alpha = 0.3  # 越小越平滑，但延迟越大
    smoothed.pose.position.x = alpha * new_pose.pose.position.x + \
                                (1 - alpha) * self.last_target_pose.pose.position.x
    # ... y, z 同理
    return smoothed

七、踩坑记录

问题	原因	解决方案
pymoveit2 收不到 JointState	QoS 策略不匹配（reliable vs best_effort）	弃用 pymoveit2，直接调用 MoveIt 原生服务 `/plan_kinematic_path`
关节状态全为 0.0	硬件接口 RealRobotHardwareInterface 不更新状态	切换为 `mock_components/GenericSystem`
IMU TF 不发布	用 `ros2 run` 启动不会加载 robot_state_publisher	必须用 `ros2 launch wit_ros2_imu imu.launch.py`
error_code=-7 (CONTROLLER_FAILED)	上一条轨迹还没执行完就发了新请求	添加 `is_executing` 标志位，执行期间跳过控制循环
error_code=-4 (PLANNING_FAILED) 频繁	IMU 断连 / 目标超出工作空间 / 奇异点	三重防护：边界检查 + 奇异点检测 + 位姿平滑
机械臂进入奇异点卡死	奇异点检测阈值太严（10%），几乎所有轨迹被拒	逐步调整到 2%，平衡安全和可用性
多个 robot_state_publisher 冲突	arm_ws 和 wit_ros2_imu_ws 各有一个 RSP，同名冲突	给 IMU 的 RSP 节点重命名为 `imu_robot_state_publisher`
colcon build 找不到包	默认只搜索当前目录的 src/	使用 `--base-paths Src --install-base install` 指定路径

八、启动方式

# 终端 1：IMU 驱动
cd ~/imu/wit_ros2_imu_ws
source install/setup.bash
ros2 launch wit_ros2_imu imu.launch.py

# 终端 2：MoveIt demo
cd ~/imu/arm_ws
source install/setup.bash
ros2 launch arm_mov demo.launch.py

# 终端 3：控制节点
cd ~/imu/arm_ws
source install/setup.bash
ros2 launch arm_mov imu_to_moveit.launch.py