myCobot Pro600六轴机械臂与3D深度视觉：物体精确识别抓取堆叠

Introduction

随着时代的进步，各种精密的机械臂，人形机器人不断的问世。我们即将迎来到处都是机器人的高科技时代。为了跟上时代的脚步，我最近入手了一台myCobot pro 600机械臂，我主要是想要用它来学习机械臂相关得控制以及机器视觉的项目，给以后的实践中在本文中，我将记录使用myCobot pro 600结合深度相机来实现物体得跟踪以及抓取。

接下来我会介绍我使用到的设备

Equipment

myCobot pro 600

myCobot pro 600是一款六个自由度的机械臂，它的工作半径最大达到600mm，它末端最大负载达到2kg，搭载的电机是谐波减速器。它还是一款以树莓派4B为核心控制主板的嵌入式机械臂搭配ubuntu20.0的系统。我选择他的原因有几个：

它不需要额外的电脑进行控制，可以直接使用树莓派进行控制
它的工作半径和末端负载能力刚好适合我的场景
大象机器人为它设计了一款可视化的控制软件robotflow，可以进行一些拖拽式的编程

SPECIFICATIONS

Degree of Freedom	6
Payload	2000g
Weight	8800g
Working Radius	600mm
Positioning Accuracy	±0.5mm
Working Temperature	-5℃~45℃
Working Lifespan	2000h
Power Input	DC 48V
JOINT ROTATION RANGE
Joint1	-180°~+180°
Joint2	-270°~+90°
Joint3	-150°~+150°
Joint4	-260°~+80°
Joint5	-168°~+168°
Joint6	-174°~+174°
Joint Maximum Speed	115°/s

FS820-E1-Depth camera

深度相机可以捕捉深度信息，可以获得物体的三维坐标，距离，尺寸和体积。深度相机使用多种算法技术来测量物体的深度，如结构光、时间飞行和立体视觉等。在机器视觉应用中，深度相机可以用于点云分割、物体识别和3D重建等任务。

Model	FS820-E1
Working Distance	0.3m-1.4m
FOV（H/V）	66°/44°
Accuracy(Z)	0.14mm@400mm； 1.53mm@700mm
Accuracy(XY)	4.88mm@700mm
Depth Resolution	1280*800
RGB Resolution	1920*1080
Power Interface	DC 12V/24V
Trigger Signal	HR10A-7P-6S
Data Interface	Gigabit Ethernet
Power Consumption	2.8W ~ 3.9W
Temperature	Storage：-10℃~55℃; Operation：0℃~45℃
Ingress Protection	IP41
Size	95mm45mm43mm
Weight	228g

Project

我使用机械臂有一段时间了，大部分的时间都是用来去执行一些路径，重复的做一些动作。如果只是这样的话，机械臂在很多方面都是不能够胜任人们的工作的。我们工作的时候手和眼睛是需要进行配合，当然机械臂我们要给他配置一个“眼睛”，就是深度相机。

我今天要记录的是用机械臂+相机来是想物体的抓取，不只是一个平面的抓取，而是一个能够根据获取的深度信息进行判断高度的抓取。

接下来我先简要介绍一下项目的结构。

Clear Task：首先我们要明确我的目标，做一个深度视觉的机械臂抓取木块的任务

Hardware preparation：机械臂使用的是mycobot pro 600，深度相机用的是FS820-E1

Hand-eye calibration：进行机械臂和相机之间的手眼标定，以建立它们之间的坐标系转换关系。通过手眼标定，可以确定机械臂末端执行器的位置和姿态与相机坐标系之间的转换关系，从而实现精确的视觉引导和抓取。

Object detection and recognition：确定被测物体为木块，训练算法识别木块，通过大量的数据让机器能够精准的识别出被测物体

Robotic arm path planning：识别到被测物体之后返回一个物体的三维坐标，给到机械臂执行抓取，要规划机械臂的运动轨迹达到抓取的点位

Test：完成上边测试之后，需要进行测试，不断的进行调整。

Execute：测试完成后，去执行任务

介绍完项目的结构，让我们开始实现Hand-eye calibration。

Hand-eye calibration

通常情况下，对于机械臂执行视觉类的抓取任务，手眼标定是必需的。手眼标定是一种将机械臂的坐标系与深度相机的坐标系进行关联的过程。通过手眼标定，可以确定机械臂末端执行器的位置和姿态与相机坐标系之间的转换关系，从而实现精确的视觉引导和抓取。

第一步

需要准备一个标定板：选择黑白相间的方格为标定板，棋盘格的几何结构非常规则，方格之间的边缘和角点位置可以被精确地计算和模型化。这使得标定算法可以准确地估计相机与棋盘格之间的几何关系。

第二步

固定深度相机和机械臂

我要做的是眼在手外，就是eye to hand的模式，相机的位置不会随着机械臂的改变而改变。

第三步

匹配特侦点

将标定板在相机视野范围内进行多次的旋转移动，捕获这些图片以提供丰富的图片信息，对每个标定板图像，使用角点检测算法来提取标定板上的取方格交叉点的坐标特征点。

用的是相机自带的RVS（robot vision suite)软件，里面提前编写好捕获图片，提取特征点，匹配特侦点的方法。

第四步

计算标定坐标值

得到了20张不同角度，不同位置的特侦点的数据，接下来使用RVS提供的算法在程序中进行计算得出eye to hand标定的值。记录下手眼标定的结果等到后面进行机械臂坐标系和相机坐标系转化的关键。

到这里手眼标定就结束了，接下来我们来训练机器识别被测物体。

Object detection and recognition

首先得采集我们的被测物体，木块。我们用一个开源软件labelme，LabelMe是一个流行的开源图像标注工具，用于创建和编辑图像标注数据集。旨在简化图像标注的过程，并为机器学习和计算机视觉任务提供标注数据。

采集被测物图片

像前边标定一样，采集的图片越多数据越精准。将他保存在指定的路径中。

安装labelme

在python中安装依赖库，以便后续的使用

pip install PyQt5
pip install labelme

标注过程

打开labelme，找到我们保存采集图片的路径，然后点击Create Polygons，为木块绘制红色的边框，完成标记后给标记结果明明“wooden block”，一直重复这个步骤为木块绘制边框，直至标记完所有采集的图片。

这是人为标记的木块，要实现机器识别就得让机器学会检测该标记的物体。接下来要训练AI 模型，RVS软件中有这个算法，我们只需要将标记好的图片提交到AI训练功能，最后会生成一个Train output文件夹，在这里可以获取标记的权重文件。

AI 推理

AI推理它使得训练好的模型能够应用到实际场景中，使用先前经过训练的参数和权重来处理新的输入数据，并生成相应的输出结果。

1）使用 FilterBoxList 算子（重命名为"点云高度排序"）来筛选木块并按照木块列表的 Z 轴坐标值进行排序。这个算子的属性值需要按照下面的要求进行调整。

2）使用 FindElement 算子来获取适合抓取的平面。在算子属性中选择类型为"Plane"，并通过调整 distance_threshold 属性来选择合适的平面。可以打开点云可视化属性来查看所选的平面。

3）使用 MinimumBoundingBox 算子（重命名为"获得外包框"）来获取平面的中心点坐标。在算子属性中选择类型为"ApproxMVBB"，并给该算子提供一个 ref_pose，这个 ref_pose 连接到先前提到的"TowardsDownPose"算子，它表示绕着 X 轴旋转 180°，使 Z 轴朝下，以便机器人抓取。可以打开"GetBoxCube"属性面板中的 box 和 box_pose 可视化属性来显示计算出的平面中心点。

总结起来，这个操作的目标是从点云中筛选出木块并按照高度进行排序，然后找到适合抓取的平面，并计算该平面的中心点坐标。这个过程可能是为了进一步在机器人操作中使用这些信息，例如进行抓取任务或路径规划。

被测物体的识别和检测就完成了。

再结合手眼标定，就能识别出木块的时候会得到木块的坐标信息。这样就获取到了木块相对于机械臂的坐标。

下图是RVS中的仿真模拟3D环境。最左边带坐标的是深度相机的坐标系的标志。

Robotic arm path planning

在上一步操作中，我们得到了resultPose，也就是木块相对于机械臂的坐标，有了坐标就可以对机械臂进行控制，将机械臂的末端运行到被测物体坐标的上方。

下面是部分实现路径规划的代码

import _thread
import socket
import json
import time
import sys
import math
import copy
import numpy as np
from RoboFlowSocket import RoboFlowSocket

#旋转矩阵和欧拉角之间的转换
def CvtRotationMatrixToEulerAngle(pdtRotationMatrix):
    pdtEulerAngle = np.zeros(3)

    pdtEulerAngle[2] = np.arctan2(pdtRotationMatrix[1, 0], pdtRotationMatrix[0, 0])

    fCosRoll = np.cos(pdtEulerAngle[2])
    fSinRoll = np.sin(pdtEulerAngle[2])

    pdtEulerAngle[1] = np.arctan2(-pdtRotationMatrix[2, 0], (fCosRoll * pdtRotationMatrix[0, 0]) + (fSinRoll * pdtRotationMatrix[1, 0]))
    pdtEulerAngle[0] = np.arctan2((fSinRoll * pdtRotationMatrix[0, 2]) - (fCosRoll * pdtRotationMatrix[1, 2]), (-fSinRoll * pdtRotationMatrix[0, 1]) + (fCosRoll * pdtRotationMatrix[1, 1]))

    return pdtEulerAngle

def CvtEulerAngleToRotationMatrix(ptrEulerAngle):
    ptrSinAngle = np.sin(ptrEulerAngle)
    ptrCosAngle = np.cos(ptrEulerAngle)

    ptrRotationMatrix = np.zeros((3, 3))
    ptrRotationMatrix[0, 0] = ptrCosAngle[2] * ptrCosAngle[1]
    ptrRotationMatrix[0, 1] = ptrCosAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] - ptrSinAngle[2] * ptrCosAngle[0]
    ptrRotationMatrix[0, 2] = ptrCosAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] + ptrSinAngle[2] * ptrSinAngle[0]
    ptrRotationMatrix[1, 0] = ptrSinAngle[2] * ptrCosAngle[1]
    ptrRotationMatrix[1, 1] = ptrSinAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] + ptrCosAngle[2] * ptrCosAngle[0]
    ptrRotationMatrix[1, 2] = ptrSinAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] - ptrCosAngle[2] * ptrSinAngle[0]
    ptrRotationMatrix[2, 0] = -ptrSinAngle[1]
    ptrRotationMatrix[2, 1] = ptrCosAngle[1] * ptrSinAngle[0]
    ptrRotationMatrix[2, 2] = ptrCosAngle[1] * ptrCosAngle[0]

    return ptrRotationMatrix
    
# 抓取位置的路径规划
def compute_end_effector_pose(current_pose, tool_pose):
    # 从位姿矩阵中提取旋转矩阵和平移向量
    current_rotation = current_pose[:3,:3]
    current_translation = current_pose[:3,3]
    tool_rotation = tool_pose[:3,:3]
    tool_translation = tool_pose[:3,3]

    # 计算工具坐标系在基坐标系下的位姿
    new_rotation = np.dot(current_rotation, tool_rotation)
    new_translation = np.dot(current_rotation, tool_translation) + current_translation

    # 组合旋转矩阵和平移向量得到新的位姿矩阵
    new_pose = np.eye(4)
    new_pose[:3,:3] = new_rotation
    new_pose[:3,3] = new_translation

    return new_pose

我们来一起看看效果如何。

我预留了一段距离，机械臂末端到木块的距离。这一段距离是给机械安装上夹爪的距离。我在桌面上放了几张白纸，那是因为我使用的被测物体跟我放置的桌面的颜色相似度较高，为了能够快速识别被测物体。

遇到的问题

1 最一开始在考虑机械臂跟深度相机如何建立联系,两个处理器之间建立联系就有点复杂，最后用了python的socket的库来实现上位机对myCobot pro 600的控制。

2 在测试的时候会有个别的坐标出现了偏差，后来检测原因是因为，在做完手眼标定之后，机械臂和深度相机的位置一定是要固定住的，不能再标定完之后再去做改变。

3 被测物的识别的快慢的问题。在图片中我们可以看到桌面上放了几张白纸，那是因为我们的被测物体跟桌面的背景色的纹理有些相似，所以不能快速的识别出来。（是为了提高识别率从才放的白纸）机器终究还是机器，抵不过人类的眼睛。

总结

目前的项目还不够完善，后续我考虑加上末端的执行器将木块抓起来。但是这个也不能真正的运用到实际场景当中使用，是不需要我们来抓木块。换个角度，如果说他是一堆零件堆在那，用机械臂将一大堆的零件进行分类，给人们提供一个帮助那就很有用了。因为在一堆杂乱的零件中找零件是一件非常痛苦的事情，如果有遇到过这个问题的就能明白我在说什么了。

如果你有什么想法，或者有趣的建议欢迎在下方评论留言。你要是觉得喜欢这篇文章给予我一个点赞或者评论吧！