基于ROS2/MoveIt!的工业机械臂控制系统开发全攻略
1. 系统架构设计1.1 系统组成模块
-->
|<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> |
<--> <-->
|<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> |
--> 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-install2.2 关键依赖安装
# 工业机械臂驱动包
sudo apt install ros-humble-industrial-core
# 视觉处理包
sudo apt install ros-humble-vision-opencv
# 深度相机驱动
sudo apt install ros-humble-realsense23. 机械臂运动学建模
3.1 URDF模型构建（示例：6轴机械臂）
<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}"/>
<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>
<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文件）
<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配置关键参数：
[*]规划组设置（Planning Groups）；
[*]碰撞矩阵（Collision Matrix）；
[*]被动关节（Passive Joints）；
[*]末端执行器（End Effectors）。
4.2 逆运动学求解（C++实现）
// arm_controller/src/ik_solver.cpp#includeclass IKSolver {public:bool computeIK(const geometry_msgs::msg::PoseStamped& target_pose,<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> std::vector& 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.pyfrom py_trees import Behaviour, Blackboardfrom 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")<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> # 2. 规划抓取路径 if not self.plan_grasp(target_pose):<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>return Status.FAILURE<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> # 3. 执行抓取动作 self.execute_grasp()<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. 规划放置路径 if not self.plan_place():<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>return Status.FAILURE<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> return Status.SUCCESS def plan_grasp(self, target_pose): # 调用MoveIt!规划服务 return True5.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_raw6.2 目标检测与位姿估计（Python实现）
# vision_system/src/object_detector.pyimport cv2import 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(<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>gray, self.aruco_dict, parameters=self.aruco_params)<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> if ids is not None:<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>rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(<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> corners, 0.05, camera_matrix, dist_coeffs)<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>return tvec, rvec return None7. Gazebo仿真验证
7.1 仿真环境配置
model://ground_plane 0 0 0.75 0 0 0 true model://table<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> model://gear_part 0.3 0 0.8 0 0 07.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_node8. 实机部署注意事项
[*]硬件接口适配：
// 修改硬件接口驱动void write(const std::vector& commands) override {// 将关节角度转换为PWM信号for(size_t i=0; i
页:
[1]