基于ROS2/MoveIt!的工业机械臂控制系统开发全攻略

1. 系统架构设计

1.1 系统组成模块

[Vision System] --> [Perception Node]
          |             |
[Gazebo Sim] <--> [ROS2 Control] <--> [MoveIt! Planner]
          |             |
[Hardware Interface] --> [Real Arm]

1.2 技术栈选型

• 操作系统：Ubuntu 22.04 LTS；
• 机器人框架：ROS2 Humble Hawksbill；
• 运动规划：MoveIt2 + OMPL；
• 仿真环境：Gazebo 11 + Ignition；
• 视觉处理：OpenCV 4.5 + RealSense D435i；
• 开发语言：C++（核心模块） + Python（快速验证）。

2. 开发环境搭建

2.1 基础环境配置

# 安装ROS2 Humble
sudo apt install ros-humble-desktop
# 安装MoveIt2
sudo apt install ros-humble-moveit
# 创建工作空间
mkdir -p ~/arm_ws/src
cd ~/arm_ws/
colcon build --symlink-install

2.2 关键依赖安装

# 工业机械臂驱动包
sudo apt install ros-humble-industrial-core
# 视觉处理包
sudo apt install ros-humble-vision-opencv
# 深度相机驱动
sudo apt install ros-humble-realsense2

3. 机械臂运动学建模

3.1 URDF模型构建（示例：6轴机械臂）

<!-- arm_description/urdf/arm.urdf.xacro -->
<robot name="industrial_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="arm_joint" params="name parent child origin_xyz origin_rpy">
    <joint name="${name}_joint" type="revolute">
      <parent link="${parent}"/>
      <child link="${child}"/>
      <origin xyz="${origin_xyz}" rpy="${origin_rpy}"/>
      <axis xyz="0 0 1"/>
      <limit effort="100" velocity="1.0" lower="${-pi}" upper="${pi}"/>
    </joint>
  </xacro:macro>

  <!-- 基座 -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="0.15" length="0.1"/>
      </geometry>
    </visual>
  </link>

  <!-- 关节1 -->
  <xacro:arm_joint
    name="joint1"
    parent="base_link"
    child="link1"
    origin_xyz="0 0 0.1"
    origin_rpy="0 0 0"/>

  <!-- 后续关节定义（略） -->
</robot>

3.2 运动学参数配置（SRDF文件）

<!-- arm_moveit_config/config/arm.srdf -->
<robot name="industrial_arm">
  <group name="manipulator">
    <chain base_link="base_link" tip_link="tool0"/>
  </group>

  <end_effector name="gripper" parent_link="tool0" group="gripper"/>
</robot>

4. 核心运动规划实现

4.1 MoveIt!配置流程

# 初始化MoveIt!配置包
ros2 launch moveit_setup_assistant setup_assistant.launch

配置关键参数：

1. 规划组设置（Planning Groups）；
2. 碰撞矩阵（Collision Matrix）；
3. 被动关节（Passive Joints）；
4. 末端执行器（End Effectors）。

4.2 逆运动学求解（C++实现）

// arm_controller/src/ik_solver.cpp
#include <moveit/planning_interface/planning_interface.h>

class IKSolver {
public:
  bool computeIK(const geometry_msgs::msg::PoseStamped& target_pose,
                std::vector<double>& joint_values) {
    moveit::core::RobotStatePtr current_state =
        move_group->getCurrentState();

    bool found_ik = current_state->setFromIK(
        move_group->getRobotModel()->getJointModelGroup("manipulator"),
        target_pose.pose, 10, 0.1);

    if(found_ik) {
      current_state->copyJointGroupPositions(
          "manipulator", joint_values);
      return true;
    }
    return false;
  }
};

5. 任务规划器开发

5.1 行为树实现（Python版）

# task_planner/bt_nodes/assembly_task.py
from py_trees import Behaviour, Blackboard
from py_trees.common import Status

class PickPlaceTask(Behaviour):
    def __init__(self, name):
        super().__init__(name)
        self.blackboard = Blackboard()

    def update(self):
        # 1. 获取视觉目标位姿
        target_pose = self.blackboard.get("target_pose")

        # 2. 规划抓取路径
        if not self.plan_grasp(target_pose):
            return Status.FAILURE

        # 3. 执行抓取动作
        self.execute_grasp()

        # 4. 规划放置路径
        if not self.plan_place():
            return Status.FAILURE

        return Status.SUCCESS

    def plan_grasp(self, target_pose):
        # 调用MoveIt!规划服务
        return True

5.2 状态机实现（C++版）

// task_planner/include/state_machine.h
#include <smacc2/smacc2.hpp>

class AssemblySM : public smacc2::SmaccStateMachineBase {
public:
  using SmaccStateMachineBase::SmaccStateMachineBase;

  struct Orthogonal : smacc2::Orthogonal<Orthogonal> {};

  struct StateIdle : smacc2::SmaccState<StateIdle, AssemblySM> {
    smacc2::Transition onEvent() override {
      return transit<StatePick>();
    }
  };

  // 后续状态定义（略）
};

6. 视觉伺服系统集成

6.1 深度相机标定

# 相机内参标定
ros2 run camera_calibration cameracalibrator \
  --size 8x6 --square 0.0245 image:=/camera/color/image_raw

6.2 目标检测与位姿估计（Python实现）

# vision_system/src/object_detector.py
import cv2
import numpy as np

class ObjectDetector:
    def __init__(self):
        self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
        self.aruco_params = cv2.aruco.DetectorParameters_create()

    def detect_pose(self, img):
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        corners, ids, _ = cv2.aruco.detectMarkers(
            gray, self.aruco_dict, parameters=self.aruco_params)

        if ids is not None:
            rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
                corners, 0.05, camera_matrix, dist_coeffs)
            return tvec[0][0], rvec[0][0]
        return None

7. Gazebo仿真验证

7.1 仿真环境配置

<!-- arm_gazebo/worlds/assembly.world -->
<world>
  <include>
    <uri>model://ground_plane</uri>
  </include>

  <model name="assembly_table">
    <pose>0 0 0.75 0 0 0</pose>
    <static>true</static>
    <include>
      <uri>model://table</uri>
    </include>
  </model>

  <!-- 添加目标物体模型 -->
  <include>
    <uri>model://gear_part</uri>
    <pose>0.3 0 0.8 0 0 0</pose>
  </include>
</world>

7.2 完整仿真流程

# 启动仿真环境
ros2 launch arm_gazebo assembly_world.launch.py

# 启动MoveIt!规划场景
ros2 launch arm_moveit_config moveit_rviz.launch.py

# 启动控制节点
ros2 run arm_controller arm_control_node

8. 实机部署注意事项

1. 硬件接口适配：

// 修改硬件接口驱动
void write(const std::vector<double>& commands) override {
  // 将关节角度转换为PWM信号
  for(size_t i=0; i<commands.size(); ++i){
    pwm_signals[i] = angle_to_pwm(commands[i]);
  }
  // 通过CAN总线发送
  can_bus.send(pwm_signals);
}

2.安全机制实现：

• 紧急停止按钮监控；
• 关节限位物理保护；
• 碰撞检测算法。

9. 完整代码结构

arm_ws/
├── src/
│   ├── arm_description/        # URDF模型
│   ├── arm_moveit_config/      # MoveIt!配置
│   ├── arm_controller/         # 控制算法（C++）
│   ├── task_planner/           # 任务规划（C++/Python）
│   ├── vision_system/          # 视觉处理
│   └── arm_gazebo/             # 仿真环境
└── colcon.meta

10. 运行与调试指南

10.1 关键调试命令

# 查看规划场景
ros2 run rviz2 rviz2 -d `ros2 pkg prefix arm_moveit_config`/share/arm_moveit_config/launch/moveit.rviz

# 记录规划数据
ros2 bag record /move_group/display_planned_path

# 性能分析
ros2 topic hz /joint_states

10.2 常见问题解决

1. 规划失败处理：

• 检查碰撞矩阵配置；
• 调整规划时间参数（moveit_resources）；
• 验证URDF模型完整性。

2.视觉定位偏差：

• 重新标定手眼关系；
• 检查时间同步（使用approximate_time同步策略）；
• 优化目标检测算法鲁棒性。

11. 扩展功能建议

1. 添加力控传感器接口；
2. 实现动态障碍物避让；
3. 集成数字孪生系统；
4. 开发HMI操作界面（Qt/ROS2）。

通过本教程的系统学习，开发者可以掌握：

1. ROS2/MoveIt!生态系统核心组件；
2. 工业机械臂全流程开发方法论；
3. 视觉伺服系统的集成技巧；
4. 复杂机器人系统的调试方法。

建议按照以下顺序学习：

1. 完成Gazebo仿真验证；
2. 部署到真实硬件；
3. 扩展自定义功能模块；
4. 优化系统性能参数。

（注：实际开发中需根据具体机械臂型号调整运动学参数和硬件接口）