机器人抓取流程介绍与实现——机器人抓取系统基础系列（七）

---

机器人抓取系统基础系列文章目录
1. UR机械臂的ROS驱动安装官方教程详解——机器人抓取系统基础系列（一）
2. MoveIt控制机械臂的运动实现——机器人抓取系统基础系列（二）
3. 机器人（机械臂）的相机选型与安装——机器人抓取系统基础系列（三）
4. Realsense相机驱动安装及其ROS通讯配置——机器人抓取系统基础系列（四）
5. 机器人的手眼标定——机器人抓取系统基础系列（五）
6. 机器人夹爪的选型与ROS通讯——机器人抓取系统基础系列（六）
7. 机器人抓取流程介绍与实现——机器人抓取系统基础系列（七）

---

---

前言

本文介绍了机器人抓取的流程和具体实施过程，主要包括物体的定位、位姿估计、坐标系转换、抓取系统硬件与抓取实施。不仅提供了关键步骤的示例函数和代码，也提供了整个基于ROS的抓取系统运行步骤。为机器人相关工作人员提供参考。

---

一、抓取路线介绍

机器人的抓取实施的主要流程包括 [1]：

1）相机发布视觉相关信息；
2）订阅视觉信息并计算物体在像素坐标系下的信息（如位置，关键点，角度等）；
3）根据物体的初步信息计算物体在相机坐标系下的物体位姿和抓取位姿；
4）将相机坐标系下的抓取位姿转换为机器人坐标系下的抓取位姿，进一步转换为机器人末端执行位姿，然后规划路径；
5）机器人通过运动规划算法控制末端执行器的抓取。

本文假设读者已经根据前述系列教程完成了基础配置工作，包括机械臂的ROS驱动配置、相机的驱动与通讯配置、夹爪的通讯配置和手眼标定相关工作。接下来，就是具体物体的识别和抓取规划实现了。

基于视觉的抓取估计分为2D抓取和6D抓取，2D抓取适用于垂直水平面的特定角度抓取（需要确定2D位置和1个抓取角），6D抓取则实现任意角度抓取（需要确定3D位置和3D姿态）。本文为便于说明抓取的实施流程，以简单的2D抓取为例。

二、物体的定位

根据相关资料 [2, 3]，基于视觉的抓取检测包含三个子任务：物体定位，物体位姿估计，抓取位姿估计。对于具体的抓取任务，这三个子任务可以通过不同的组合方式来实现抓取。

第一步的物体定位又分为三种方法：
• 只定位不识别：获取目标物体的区域，但不知道目标物体的类别；
• 目标检测：获取目标物体的区域，同时识别出物体的类别；
• 目标实例分割：识别物体的同时，获得像素级/点云级物体区域。

以下表格为DeepSeek总结的三种物体定位方法的具体区别，根据表格内容，以下为简单选择建议：
• 追求极致速度和简单场景（单一物体）：只定位不识别。
• 需要区分抓取多种物体且定位要求不极致：目标检测 (性价比之选)。
• 需要极高抓取精度、处理复杂堆叠/遮挡、抓取特定部位：目标实例分割 (性能最优，但成本/计算量最高)。

给定机器人抓取场景，目前总能找到合适的技术方案来定位目标物体的位置，进而执行后续的物体位姿估计以及机器人抓取位姿估计等任务。

以目标检测为例，流行的算法如YOLO [4] 就可以获得物体的类别和位置信息（可以是四个识别框的角点，也可以是识别框的中心点、长、宽和角度）。

三、物体位姿和抓取位姿估计

对于桌面上的物体位姿估计，如中心对称物体（如圆形橡皮）为例，YOLO本身的输出信息就足够确定其物体位姿。比如其物体位置由YOLO识别的中心点确定，物体角度可以假设。

对于非中心对称物体（如长方形橡皮），除了YOLO确定的抓取中心点位置之外，还需要确定其抓取角度。比如可以再通过该物体分割区域最小外接矩形的角度来指定其抓取角度。

需要指出的是，YOLO的位置信息只有2D位置，而且是像素坐标系下的位置信息（单位为像素）。在实际的抓取任务中，我们需要先将其位置信息转换为相机坐标系下的位置信息（单位为米）。其中Z坐标上的值可以根据深度计算，也可以设定特定的数值（如果抓取高度已知）。

然后物体位姿就是在相机坐标系下的位姿信息和物体的角度定义的，其定义可以通过以下函数实现。

def pose3d(center3d, object_angle):"""根据物体的位置和角度定义物体的位姿参数:如下center3d: 相机坐标系下的物体3D位置object_angle: 物体的角度返回: 相机坐标系下的物体位姿"""center_pose = PoseStamped()center_pose.header.frame_id = "camera_color_frame"center_pose.header.stamp = rospy.Time.now()center_pose.pose.position = center3droll = 0pitch = 0yaw = math.radians(object_angle) q = quaternion_from_euler(roll, pitch, yaw)center_pose.pose.orientation = Quaternion(*q)return center_pose

对于形状规则的物体，其抓取位姿可以根据其物体位姿直接定义，只需要设置特定的对应关系即可。而对于一些不规则物体或者工具类物体，其抓取位姿还需要考虑其具体形状和任务需求。

本文的重点在于说明整个抓取Pipeline的工作流程，选择了形状规则的物体如长方形橡皮为抓取对象。对于长方形的橡皮，只需要保证执行器的末端位姿（即抓取位姿）和橡皮的位姿重合即可。

四、坐标系转换与运动规划

1 相机坐标系下的物体位姿${\mathbf{T}}_{\text{Object}}^{\text{Camera}}$到机器人坐标系的物体位姿${\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}$转换原理如以下公式所示。
$${\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}={\mathbf{T}}_{\text{Object}}^{\text{Camera}}\cdot {\mathbf{T}}_{\text{Camera}}^{\text{RobotBase}}$$
其中，${\mathbf{T}}_{\text{Camera}}^{\text{RobotBase}}$为提前手眼标定好的结果，存储在特定文件中，使用时将其标定结果发布出去即可，发布指令如下，参考基础系列博客（五）。

roslaunch easy_handeye publish.launch

在位姿转换时，可以直接使用TF坐标转换函数：

def transformCoor(msg, max_retries=3):tfBuffer = get_tf_buffer()  # Initializing TF system...for attempt in range(max_retries):try:# 使用最新可用变换（不指定特定时间）transform = tfBuffer.lookup_transform("base_link", "camera_color_frame",rospy.Time(0),  # 最新可用时间rospy.Duration(1.0))# 应用变换到目标坐标系pose_in_camera = PoseStamped()pose_in_camera.pose = msg.posepose_in_camera.header.frame_id = "camera_color_frame"pose_in_camera.header.stamp = rospy.Time.now()# 使用transform进行坐标变换pose_in_base = tf2_geometry_msgs.do_transform_pose(pose_in_camera, transform)return pose_in_base.poseexcept (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:rospy.logwarn(f"TF转换尝试 {attempt+1}/{max_retries} 失败: {str(e)}")rospy.sleep(0.1)  # 短暂等待后重试rospy.logerr("坐标变换失败，达到最大重试次数")return None

2 机器人坐标系下的物体位姿${\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}$到机器人末端的可执行位姿${\mathbf{T}}_{\text{Tool0}}^{\text{RobotBase}}$的转换过程如下所示。

根据上图中机器人抓取系统的坐标关系，可得坐标系之间的关系如下：
$${\mathbf{T}}_{\text{Tool0}}^{\text{RobotBase}}\cdot {\mathbf{T}}_{\text{ToolEnd}}^{\text{Tool0}}={\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}\cdot {\mathbf{T}}_{\text{ToolEnd}}^{\text{Object}}$$
移项可得如下公式：
$${\mathbf{T}}_{\text{Tool0}}^{\text{RobotBase}}={\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}\cdot {\mathbf{T}}_{\text{ToolEnd}}^{\text{Object}}\cdot ({\mathbf{T}}_{\text{ToolEnd}}^{\text{Tool0}}{)}^{-1}$$
其中：
${\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}$为上一步求出的物体在机器人坐标系下的表示；
${\mathbf{T}}_{\text{ToolEnd}}^{\text{Object}}$为抓取位姿到物体位姿的关系，为提前设定的；
${\mathbf{T}}_{\text{ToolEnd}}^{\text{Tool0}}$为夹爪末端坐标系和机械臂末端的关系，由夹爪的长度和安装方式决定；

实际执行时，我们可以专门定义一个坐标转换函数，如下所示，其中eelink表示tool0：

def transform_end(pos):# eelink_to_robot, object_to_robot, toolend_to_object, toolend_to_eelinkrealEnd = Pose()object_to_robot = tf.transformations.quaternion_matrix([pos.orientation.x, pos.orientation.y, pos.orientation.z, pos.orientation.w]) # the order of xyzwobject_to_robot[:, 3] = [pos.position.x, pos.position.y, pos.position.z, 1]toolend_to_object = tf.transformations.quaternion_matrix([0, 0, 0, 1]) # the order of xyzwtoolend_to_object[:, 3] = [0, 0, 0, 1]toolend_to_eelink = tf.transformations.quaternion_matrix([0, 0, 0, 1]) # the order of xyzwtoolend_to_eelink[:, 3] = [0.0, 0.0, 0.22, 1]eelink_to_robot = numpy.dot( numpy.dot(object_to_robot, toolend_to_object), numpy.linalg.inv(toolend_to_eelink))realEnd.position.x = eelink_to_robot[0, 3]realEnd.position.y = eelink_to_robot[1, 3]realEnd.position.z = eelink_to_robot[2, 3]q = tf.transformations.quaternion_from_matrix(eelink_to_robot)realEnd.orientation.x = q[0]realEnd.orientation.y = q[1]realEnd.orientation.z = q[2]realEnd.orientation.w = q[3]return realEnd

3 运动规划
在实际抓取过程中，我们可能还会设置接近位姿、抓取位姿、拾起位姿、home位姿等，只需要让机器人在这些位姿之间移动并配合夹爪的开合即可完成抓取任务。

运动规划使用MoveIt实现，在默认配置下，使用OMPL提供的RRTConnect算法来完成机械臂的运动规划任务。当然可以选择其他算法，也可以用自定义规划算法。

五、抓取实施

本文是在前述系列教程的基础上撰写的，在前述系列教程中，我们使用的机械臂为UR5e，相机为Realsense相机，夹爪为Rochu夹爪，控制系统使用的是ROS。整个硬件系统的如下图所示，其中，黄色线路表示气体驱动线路，紫色线路表示信号传输线路。

根据前述系列博客和本文的内容，在抓取和操作实施时，需要启动的节点依次如下所示：

1 Realsense相机相关节点启动：
发布包括/camera/color/image_raw话题在内的所有图像相关话题 [5]，关于Realsense相机的ROS通讯配置参考基础系列博客（四）。

# 启动相机节点并发布相关图像话题
roslaunch realsense2_camera rs_camera.launch
# 如果需要深度图与彩色图像素精确匹配时（如RGB-D融合、点云生成、SLAM），对齐深度
roslaunch realsense2_camera rs_camera.launch align_depth:=true

2 视觉计算节点启动：
接收从相机传过来的RGB图像并做初步处理，获取像素坐标系中的物体位置，角度等信息。

# 根据任务需求自定义视觉处理算法，本案例中使用基于YOLO的视觉处理算法
rosrun ultralytics_ros yolo_ros_node.py

3 夹爪通讯相关节点启动：
基础系列博客（六）详细说明了夹爪的ROS通讯控制。

# 启动夹爪的通讯节点，等待接收夹爪控制指令
roslaunch serial_msgs gripper_control.launch 

4 机械臂相关节点启动：
启动UR5e工作所需要的所有相关节点，其配置过程参考基础系列博客（一）。

# 本文使用的是ur5e机械臂
roslaunch ur_robot_driver ur5e_work_all.launch

5 手眼标定结果发布节点启动：
发布手眼标定计算出的标定结果，基础系列博客（五）详细说明了手眼标定的原理和实操步骤。

# 本文使用easy_handeye标定功能包
roslaunch easy_handeye publish.launch

6 抓取或操作规划器节点启动：
启动抓取或者操作规划器，执行物体位姿计算与坐标系转换，发布路径点和夹爪控制指令。

# 根据任务需要自定义抓取或者操作规划器
rosrun ur_work TaskPlanner.py

上述节点中，需要自定义完成的是视觉计算节点和抓取规划器节点，在完成这些节点的定义后按照上述顺序依次启动节点，即可完成机械臂的抓取或操作任务。

当然，以上所有节点可以写在launch文件中，以减少启动窗口，在测试初期可以分别启动以测试不同节点的运行情况。

---

总结

以上就是今天要讲的内容，本文主要讲了机器人抓取流程和抓取实现，首先简单介绍了抓取的路线，然后重点讲解物体的识别和定位、物体的位姿和抓取位姿估计、抓取实施时的坐标系转换，最后介绍了抓取系统和抓取实施需要启动的节点。为机器人相关工作人员提供参考。

Reference

[1] 视觉机械臂自主抓取全流程：https://blog.csdn.net/knightsinghua/article/details/125328920?spm=1001.2014.3001.5501
[2] Du G, Wang K, Lian S, et al. Vision-based robotic grasping from object localization, object pose estimation to grasp estimation for parallel grippers: a review[J]. Artificial Intelligence Review, 2021, 54(3): 1677-1734
[3] 机器人抓取中物体定位算法概述：https://blog.csdn.net/dsoftware/article/details/105936927?spm=1001.2014.3001.5502
[4] Ultralytics YOLO 文件：https://docs.ultralytics.com/zh/
[5] Realsense-ROS1通讯配置：https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy