水星Mercury X1轮式人形机器人结合openc算法＆STag标记码视觉系统实现精确抓取！

本案例展示了如何利用视觉系统提升机械臂的抓取精度，成功实现了人形机器人的双臂抓取不在局限于单臂抓取。

引言
如今市面上已经有了许多不同类型的人形机器人，涵盖了服务行业和医疗行业等各个领域。这些机器人以其智能化和自动化的特性逐渐融入我们的日常生活和工作中。然而，尽管现有的人形机器人在特定应用场景中展现出了巨大的潜力，如何进一步提升其操作精度、适应性和多功能性，仍然是机器人技术发展的关键挑战。针对这一需求，本案例探讨了通过整合视觉系统与机械臂技术，来提升人形机器人在复杂环境中的自主操作能力，特别是在精确抓取和操作任务中的应用。

目标：

通过结合openc算法＆STag标记码视觉系统和Mercury X1轮式人形机器人，实现对各种形状和大小的物品进行精准抓取，提高分拣效率和准确度，实现双手合作充分发挥人形机器人的作用。

产品
Mercury X1
Mercury X1是由Elephant Robotics开发的一款先进的人形机器人，特别设计用于应对各种自动化任务。它拥有19个自由度，（单臂7个自由度）使其在执行任务时具有极高的灵活性和适应性。Mercury X1配备了轮式移动底座，由高性能直驱电机驱动，能够在复杂环境中稳定移动，并具备高达8小时的电池续航能力，适合个人和商业应用。

这款机器人采用高性能主控制器系统，配置了NVIDIA Jetson Xavier提供强大的计算支持，以处理视觉测距、传感器融合、定位与地图构建、障碍检测和路径规划等复杂算法。此外，Mercury X1的移动基座装备了LiDAR、超声波传感器和2D视觉系统，能够实现高感知的环境交互。

myCobot Pro Adaptive Gripper
自适应夹爪，它可以拾起任何形状的任何物体并且不会松开。使用它来完成一系列完整的应用，并快速投入生产 - 无需机器人专业知识。它是高度灵活和可靠的机器人单元的关键。

myCobot Pro Camera Flange
使用USB-B数据线能够获取到图像的相机模组。

接下来，我们将探讨这些技术在实际应用中是如何被集成到机器人中，并展示其在具体任务中的表现。

技术概览
OpenCV
OpenCV是用于实现图像处理和计算机视觉的开源库，它在本次的案例中扮演了至关重要的角色，没有它就不能完成这个项目。机器人的摄像头通过OpenCV分析收集到的视觉数据，识别和定位物体。OpenCV的算法使机器人能够识别物体形状、大小和精确坐标，这些信息对于精确抓取和操作至关重要。

提供了物体的坐标给机器人，就可以实现精准的抓取。

S-Tag标记码技术
S-Tag标记码是一种高度可靠的标识系统，设计用于在视觉上挑战的环境中提供准确的标记识别。这些标记被用于标识Mercury X1机器人操作环境中的物体和位置。即使在光线不足或视线受阻的情况下，S-Tag也能确保机器人通过其摄像头系统准确识别目标物体。

https://youtu.be/vnHI3GzLVrY

通过这些技术的应用，Mercury X1大象人形机器人能够执行复杂的任务，如自主导航、物体识别和精确操控，这些都是现代自动化和智能系统不可或缺的能力。

pymycobot
pymycobot是用于控制Mercury X1机器人机械臂和末端执行器（如夹爪）的Python库。它允许开发者精确控制机械臂的角度、坐标以及运动模式，包括差补模式和刷新模式。此库为机器人提供了高度的灵活性和可定制性，使得机器人能够执行复杂的抓取和操控任务，并适应各种操作需求。

项目实现
使用前准备
首先的确保机械臂的零位正确，可以通过下述方法校准零位：

1）使用放松指令释放关节电机（注意！放松后需要扶住关节防止机械臂下坠损坏！）

ml.release_all_servos()
mr.release_all_servos()

2）拖动机械臂回到零位

可以通过以下细节确定零点的位置

3) 发送指令锁闭电机

ml.focus_all_servos()
mr.focus_all_servos()

4）检查零点是否正确

输入获取关节角度指令查询当前位置

ml.get_angles()
mr.get_angles()

如果返回的关节角度逼近[0, 0, 0, 0, 90, 0]，则视为零点正确

5）零点校准

如果在零位读取到的关节角度与[0, 0, 0, 0, 90, 0]相差很大，则需要校准关节零位

for i in range(1,7):
 ml.set_servo_calibration(i)
 mr.set_servo_calibration(i)

校准完毕后读取关节信息，返回为[0, 0, 0, 0, 90, 0]则表示校准成功

ml.get_angles()
mr.get_angles()

这样就准备ok了，接下来可以继续我们的功能部分的实现了。

相机和夹爪的安装
相机和夹爪的安装方式与视觉识别的手眼矩阵相对应，已经提前做好了一个关于Mercury X1 相机夹爪的手眼标定的数值，如果更改的话需要重新进行手眼标定。

首先将机械臂移动回零点：

ml.over_limit_return_zero()
mr.over_limit_return_zero()

安装先装摄像头（注意方向）然后再安装夹爪，也需要注意方向。

视觉算法模块
然后我们会使用到camera_detect 这个功能包，封装了一些关于相机使用的方法，接下来我会介绍一些接口,针对对STag码的识别，使用起来非常的方便，不需要自己再次进行手眼标定的计算了。

obj = camera_detect(Camera_ID, mark_size, mtx, dist, direction)
参数：
camera_ID: 相机ID
mark_size: stag码边长mm
direction: 0：左臂 1：右臂
功能： 相机和机械臂初始化

obj.camera_open_loop()
功能： 显示相机画面
obj.stag_identify_loop()
功能： 显示相机画面,识别并打印stag码的相机坐标

coords = obj.stag_robot_identify(direction)
参数：
direction: 0：左臂 1：右臂
return: stag码的机械臂坐标
功能： 计算stag码的机械臂坐标

obj.vision_trace(catch_mode, direction)
参数：
catch_mode: 0:水平抓取，1:竖直抓取
direction: 0：左臂 1：右臂
功能： 显示相机画面,识别并打印stag码的相机坐标

以下是调用camera_detect包的使用代码,非常的干净简洁。

import numpy as np
from pymycobot import Mercury
# 导入视觉识别stag包
from camera_detect import camera_detect

if __name__ == "__main__":
   camera_params = np.load("camera_params.npz")  # 相机配置文件
   mtx, dist = camera_params["mtx"], camera_params["dist"]
   ml = Mercury("/dev/left_arm")  # 设置左臂端口
   mr = Mercury("/dev/right_arm")  # 设置右臂端口
   arm = 0         # 0左臂，1右臂
   catch_mode = 0  # 0水平抓取，1竖直抓取
   # 摄像头id需要根据实际更改
   left_camera_id = 3
   right_camera_id = 6
   stag_size = 32
   # 新建左右臂视觉识别对象
   ml_obj = camera_detect(left_camera_id, stag_size, mtx, dist, arm)
   mr_obj = camera_detect(right_camera_id, stag_size, mtx, dist, arm)
   # 左右臂移动至水平观察位，进行抓取
   mr_obj.vision_trace(catch_mode, mr)
   ml_obj.vision_trace(catch_mode, ml)

正常的手眼标定的步骤如下：

1数据采集：采集若干组手眼数据，包括机器人末端执行器在不同位置的姿态（即位置和方向）和相机看到的特征点的位姿。

2 建立运动方程：为了确定相机坐标系与机器人末端执行器坐标系之间的变换关系

3 求解变换矩阵 ：得到的是描述相机坐标系和机器人末端执行器（手）坐标系之间的空间变换关系的值。

下面的代码是解变换矩阵的例子。

# 将旋转矩阵转为欧拉角
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


# 坐标转换为齐次变换矩阵，（x,y,z,rx,ry,rz）单位rad
def Transformation_matrix(coord):
   position_robot = coord[:3]
   pose_robot = coord[3:]
   # 将欧拉角转为旋转矩阵
   RBT = CvtEulerAngleToRotationMatrix(pose_robot)
   PBT = np.array([[position_robot[0]],
                   [position_robot[1]],
                   [position_robot[2]]])
   temp = np.concatenate((RBT, PBT), axis=1)
   array_1x4 = np.array([[0, 0, 0, 1]])
   # 将两个数组按行拼接起来
   matrix = np.concatenate((temp, array_1x4), axis=0)
   return matrix

机械臂抓取
我们来控制机械臂的双臂，会使用到pymycobot库当中的Mercury,我们为了能够直观明了，每一条手臂控制的时候都需要创建一个对象。

from pymycobot import Mercury

ml = Mercury('/dev/left_arm')
mr = Mercury('/dev/right_arm')

控制机械臂末端运动到coords的坐标。
send_base_coords(coords, speed)
功能：：控制机械臂末端运动到指定坐标
参数：
coords: ： 坐标值列表 [x,y,z,rx,ry,rz],length6
速度(int): 1 ~ 100

下面是识别目标物体进行抓取的代码。

if __name__ == "__main__":
   # 设置摄像头id                  
   camera = UVCCamera(5, mtx, dist)
   # 打开摄像头
   camera.capture()
   # 设置左臂观察点
   origin_anglesL = [-44.24, 15.56, 0.0, -102.59, 65.28, 52.06, 23.49]
   # 设置夹爪运动模式
   ml.set_gripper_mode(0)
   # 设置工具坐标系
   ml.set_tool_reference([0, 0, Tool_LEN, 0, 0, 0])
   # 将末端坐标系设置为工具        
   ml.set_end_type(1)
   # 设置移动速度                 
   sp = 40
   # 移动到观测点                  
   ml.send_angles(origin_anglesL, sp)
   # 等待机械臂运动结束    
   waitl()
   # 刷新相机界面  
   camera.update_frame()
   # 获取当前帧
   frame = camera.color_frame()
   # 获取画面中二维码的角度和id
   (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)
   # 获取物的坐标(相机系)
   marker_pos_pack = calc_markers_base_position(corners, ids, MARKER_SIZE, mtx, dist)
   # 获取机械臂当前坐标
   cur_coords = np.array(ml.get_base_coords())
   # 将角度值转为弧度值       
   cur_bcl = cur_coords.copy()
   cur_bcl[-3:] *= (np.pi / 180)
   # 通过矩阵变化将物体坐标(相机系)转成(基坐标系)
   fact_bcl = Eyes_in_hand_left(cur_bcl, marker_pos_pack)
   target_coords = cur_coords.copy()
   target_coords[0] = fact_bcl[0]
   target_coords[1] = fact_bcl[1]
   target_coords[2] = fact_bcl[2] + 50
   # 机械臂移动到二维码上方
   ml.send_base_coords(target_coords, 30)
   # 等待机械臂运动结束
   waitl()
   # 打开夹爪              
   ml.set_gripper_value(100, 100)
   # 机械臂沿z轴向下移动
   ml.send_base_coord(3, fact_bcl[2], 10)
   # 等待机械臂运动结束
   waitl()
   # 闭合夹爪                                                        
   ml.set_gripper_value(20, 100)

   # 等待夹爪闭合       
   time.sleep(2)

   # 抬起夹爪
   ml.send_base_coord(3, fact_bcl[2] + 50, 10)

单独启动左手的demo

下面是单独启动右手的demo。

总结
如果拥有一台Mercury X1你会用他来做些什么呢？发挥他人形机器人最关键的特性，如果你有什么好的想法，想要实现的欢迎在下方留言和我们沟通，你的点赞和留言是对我们最大的支持！

审核编辑 黄宇