3D深度视觉与机械臂无序抓取

今天我记录使用myCobot320 M5跟FS820-E1深度相机进行一个无序抓取物体的分享。

为什么会选择深度相机和机械臂做一个案例呢？

2D相机（最常见使用的相机）可以捕捉二维图像，也就是在水平和垂直方向上的像素值。它们通常用于拍摄静态场景或移动的物体，并且无法提供深度信息。在机器视觉应用中，2D相机可以用于图像分类、目标检测和识别等任务。

相比之下，深度相机可以捕捉深度信息，因此可以获得物体的三维信息。这些相机使用各种技术来测量物体的深度，如结构光、时间飞行和立体视觉等。在机器视觉应用中，3D相机可以用于点云分割、物体识别和3D重建等任务。

2D相机捕获到的信息已经满足不了一些特殊的情况，所以换上深度相机获得的更多的信息，比如说物体的长宽高。

让我们开始今天的主题。

FS820-E1

相机的环境搭建

首先我要搭建FS820-E1的开发环境，使用的是相机提供的RVS进行开发。可以利用RVS中的视觉算子写成节点(Node)快速搭建抓取功能。

RVS工作界面

实时采集数据

在左上角窗口资源中，找到TyCameraResource 算子添加到算子图中的 ResourceGroup 中，在算子列表搜索TyCameraAccess，trigger 算子分别添加到算子图中，并根据需要调整算子参数。然后点击运行和属性面板Trigger->ture即可查看可视化数据。没有报错能正常显示即可进行下一步。

TyCameraResource 算子

● start 以及 stop 分别用于开启、关闭资源算子的线程。auto_start 也是用于开启资源算子，如果勾选，则仅在打开 RVS 软件后第一次进入运行状态时自动开启资源线程。

● reset：在打开资源线程后如果需要更改属性参数，需要选中该选项进行重置。

TyCameraAccess 算子

● 打开cloud、rgb、depth可视化属性，将 cloud_color设置为-2，代表真实颜色

myCobot 320-M5Stack

myCobot 320 是面向用户自主编程开发的实践型机器人，产品最大有效臂展 350mm，最大负载 1KG，重复定位精度 ±0.5mm。

环境搭建

需要python 编译环境以及控制机器人的库pymycobot

pip install pymycobot --upgrade

ps:使用的PC的显卡最好是1060 2G独显以上，因为需要锻炼图片识别等操作，显卡的性能越好运行得越快

无序抓取

接下来是实现机械臂的无序抓取，无论物体处于何种姿态都能过精准的抓到。下图是整体的算子图，也就是unstacking.xml工程文件

手眼标定

用棋盘格来进行手眼标定

准备：

● 准备棋盘格，算好棋盘格行列数，以及棋盘格边长(mm)

● 手眼标定分为眼在手上(eye in hand)、眼在手外(eye to hand)。根据不同情况将标定板和相机固定好。这里选择eye to hand

数据录制

点击左上角加载，打开unstacking_runtime/HandEyeCalibration/HandEyeCalibration.xml

在属性面板正确填写标定板的行列数，和标定板格子的单位长度,和数据保存的文件路径

启动相机工程和机械臂开始进行标定.

标定前确保相机能完整识别完整的棋盘格，以及标定过程中，棋盘格是固定的，不能发生移动。运行完成会得到18组数据。

计算标定结果

positional error 在 0.005（5 毫米）以内，则比较理想

坐标系转换

此操作旨在将点云所处的坐标系——相机 rgb 镜头坐标系转换至机器人坐标系，这一转换涉及相机外参及手眼标定结果。

步骤：

● 1）在算子图中右键选择在此处导入Group XML，导入RVSCommonGroup 中的HandToEye_Depth2Robot.group.xml。需要注意的是，除了该文件之外 ，还有HandInEye_Depth2Robot.group.xml。

● 2）加载手眼标定数据组的pose端口与HandToEye_Depth2Robot组的rgb2robot 端口连接。

● 3）拖入 LoadCalibFile 算子，用于加载标定文件，finshed 端口连接至HandToEye_Depth2Robot组的start端口；extrinsic_pose端口与rgb2depth 端口连接；start端口与InitTrigger端口finished端口连接。具体连接如下：

点击 Group，找到 rgb2tcp 算子，在属性面板的 pose 属性处，粘贴手眼标定的结果。

● 5）通过前述步骤，我们已经获取了相机 rgb 镜头转机器人坐标系的矩阵 rgb2robot 和相机深度镜头转机器人坐标系的矩阵 depth2robot，此处我们将相机深度镜头坐标系下点云转换至机器人坐标系下。

● 6）首先拖入 Transform 算子，type 属性选择“PointCloud”，将 depth2robot 端口连接至该算子的pose 输入端口，将 LoadLocalData 算子组的 pointcloud 端口连接到本算子的同名输入端口。

AI训练

采集训练图像

打开 unstacking_runtime/MaskRCNN/ty_ai_savedata.xml，内容基本与录制 RGB 图像一致，在这里我们只需要调整 EmitSring 中的 string 参数，设置为我们想要的路径即可。点击 Capture 录制图像。当然数据越多那是越好，越稳定。

标注训练模型

目前为已录制好的 RGB 标注，我们推荐使用 labelme 这款软件，本文档提供一种 labelme 的安装方法。

● 1.按照官网安装pip

https://pip.pypa.io/en/stable/installation/

● 2.安装PyQt5

pip install PyQt5

● 3.安装labelme

pip install labelme

标注前准备

首先确定任务目标，明确在检测过程中什么物体需要被检测，什么物体不需要被检测，从而有针对性的进行标注。

给定的标注条件无需过分苛刻，不要按照人的思维去考虑，而是按照自己主观设定的标注思路是否便于落实代码。

标注过程

● 终端输出labelme，打开软件点击OpenDir,选择我们标注的路径(在3.2.1采集训图像Emit算子string路径)

● 点击Create Polygons，为木块绘制红色的边框

● 完成后会弹出命名框，第一次请命名 wooden block，后续同类直接选择

● 当图像内所有箱子标注完成后，点击 Save 进行保存，默认当前文件夹，默认名称，随后选择 Next Image 切换到下一个图像

训练AI模型

开unstacking_runtime/MaskRCNN/ty_ai_train.xml，这里只需要调整 data_directory 和classnames _filepath 路径。点击 start_train按钮即开始训练。

最终会生成一个 train output 文件夹在这个文件夹中有命名为 model fial,pth是所需要的权重文件。

AI推理

1）拖入一个 Emit 算子，type 属性选择“pose”，重命名为“抓取参考Pose”，将 pose_roll 输入入“3.141592654”。这个算子在后续的算子中使用。将该算子中 pose 端口与计算抓取点组down_pose 端口连接

2）双击展开计算抓取点组，需要预先使用 MaskRCNN 网络对数据进行训练，将其中的AIDetectGPU 算子的 type 更改为MaskRCNN 并对应修改其余配置文件参数。由于 AI 推理算子在正式运行前需要初始化运行一次，所以需要在算子前额外添加一个 Trigger(type 为 InitTrigger)。

3）AI 推理算子会获得目标在 2D 图像中的位置区域（即掩码图，对应的是 obj_list 端口），之后我们需要将这些位置区域转换到 3D 点云中，这一环节对应的是 计算抓取点 组中的 ProjectMask 算子。对于 ProjectMask 算子，不仅需要给入 AI 推理算子获得的 obj_list,还需要给入 2D 图对应的点云、2D图采图时所用的 rgb 镜头坐标系同点云坐标系的转换矩阵、相机 rgb 镜头的内参。这里已经将点云转换到了机器人坐标系，所以需要输入 rgb 镜头到机器人坐标系的转换矩阵。相机的 rgb 镜头内参可以直接从相机参数文件中读取。算子运行完成后，会获得所有检测目标的点云列表。

机械臂定位抓取

定位识别

根据 AI 推理后的流程，已经获得了在机器人坐标系下所有检测目标的点云列表。接下来要获得它的点云中心坐标。

1）双击展开 计算抓取点 组中 寻找目标 组。需要先筛选木块，并按照木块列表的 Z 轴坐标值进行筛选，筛选出最上层的木块，并对上层木块进行排序。因此这里使用 FilterBoxList 算子，重命名为“点云高度排序”，该算子的属性值调整如下：

2）获取平面，使用 FindElement，type 选择“Plane”，获得点云中适合抓取的平面。调整算子属性distance_threshold 来调整所选取的平面。打开 cloud 可视化属性来查看选取的平面。

3）获取平面中心点，使用 MInimumBoundingBox 算子，重命名为“获得外包框”，type 属性选择“ApproxMVBB”获得一个方便机器人抓取的坐标中心点。这里需要给该算子一个 ref_pose，这里连接在3.3.4进行AI推理中提到的“TowardsDownPose”，表示绕着 X 轴旋转 180°，使 Z 轴朝下，便于机器人抓取。打开“GetBoxCube”属性面板 box 和 box_pose 可视化属性即可显示计算出的平面中心点。

4）调整木块方向，使用AdjustBoxNode算子，该算子的作用是，选择长度大于宽度的物体，将物体位姿进行改变，这里选择yaw选择90°

这样就能够获取到坐标了

机械臂的抓取

在完成上述操作后，已经获得了目标点坐标，需要通过机器人和RVS软件建立连接并进行 tcp通讯。进行实际抓取。

1）编写TCP通讯代码(RobotControl_Elephant.py)，以下部分为截取，该代码实现RVS软件和机械臂的TCP通讯

#CAPTURE
print("***get pose***%s"%time.asctime())
capture_cmd = "GET_POSES \n"
capture_bytes=bytes(capture_cmd,encoding="utf-8")
sock_rvs.send(capture_bytes)
#recv CAPTURE
data = sock_rvs.recv(socket_buf_len)
print("---------------------------接收的数据----------------------------")
print(data)
print("***data end***%s"%data[-1:])

print("***capture_receive***%s"%time.asctime())
if int(data[-1:]) == 1:
    print("***received CAPTURE result***\n")
if int(data[-1:]) == 2:
    print("***All finished!***"
    #P_FLAG = bool(1-P_FLAG)
    #print("切换拍照位")
    continue
    #break

2）将目标点进行调整坐标⽐例，将 ScalePose 算⼦的 type 设置为 Normal ，分别调整 pose 的（ X 、Y 、Z ）和（ Roll 、Pitch 、Yaw）⽐例。scale_rpy ：修改 pose 中 r p y 的单位。设：57.2957795 。即从将弧度切换为⻆度。

3）最后，将ScalePose的 finished 和pose_list端口连接到最外层算子组的 MirrorOutput 端口, 并连接回 HandEyeTCPServer算子。至此，项目文件的编辑已经完成。

效果展示

完成以上步骤，在unstacking.xml工程下，点击运行，同时运行RobotControl_Elephant.py文件，识别到多个木块选取其中一个木块位姿就会发送给机械臂进行夹取。

总结

总的来说这只是深度相机的一小点功能，后续甚至考虑将这几个物体叠在一起又或者其他的不规则形状来体现出它性能的强大。提前训练好模型，就能实现想要的效果。你期待我用它来做些什么呢？欢迎在地下留言，你们的点赞和关注将是我更新的动力！