---

機器人抓取系統基礎系列文章目錄
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}}?{\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}}?{\mathbf{T}}_{\text{ToolEnd}}^{\text{Tool0}}={\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}?{\mathbf{T}}_{\text{ToolEnd}}^{\text{Object}}$$
移項可得如下公式：
$${\mathbf{T}}_{\text{Tool0}}^{\text{RobotBase}}={\mathbf{T}}_{\text{Object}}^{\text{RobotBase}}?{\mathbf{T}}_{\text{ToolEnd}}^{\text{Object}}?({\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