大象机器人水星MercuryX1轮式人形机器人基于物体标记建模的键盘点按操作！

引言
在现代科技的推动下，机器人在日常生活和工作场景中的应用越来越广泛。本文将介绍MercuryX1，这款先进的机器人如何通过其手臂末端的摄像头识别并确定键盘的键位，从而进行精确的打字操作。通过这一案例，我们将展示MercuryX1在自动化办公领域的潜力，以及其在提升效率和减少人为错误方面的显著优势。

接下里跟随我们的脚步，我们先简单的介绍一下使用到的产品。

Product
Mercury X1
水星Mercury X1是一款轮式人形机器人，整体由水星Mercury B1和高性能移动底座组合而成，拥有19个自由度。其单臂为7自由度的类人手臂结构机械臂。整机配备了英伟达Jetson Xavier主控。

移动底座具备丰富的感知能力，包括高性能激光雷达、超声波传感器和2D视觉传感器。其直驱电机驱动系统使其最大运行速度可达1.2m/s，最大爬坡高度为2CM，最大爬坡角度为15度。整机最大续航时间高达8小时。

此外，Mercury X1支持包括ROS、Moveit、Gazebo和Mujoco等主流仿真软件，提升了机器人智能的自主学习和快速迭代能力。

myCobot Pro Adaptive Gripper
是Mercury X1适配的自适应夹爪，提供较大的加持力，和标准的M8航空插头接口。

Camera Flange
适配于Mercury X1的摄像头模组可以安装在机械臂双臂的末端，并搭配夹爪使用。通过摄像头，实际获取物体的相关信息，并利用这些数据进行机器视觉识别算法处理。通过USB接口，数据被传输到Jetson Nano主控进行进一步处理。

技术要点
接下来介绍在项目中使用到的技术点。

pymycobot
pymycobot是Elephant Robotics专为其机械臂产品设计的控制库。通过该库，用户可以方便快捷地调用API，以实现对机器人的精确控制和操作。pymycobot提供了丰富的功能接口，简化了编程流程，使开发者能够专注于应用开发。

pymycobot · PyPI

OpenCV
OpenCV是一个开源的计算机视觉库。通过该库，用户可以方便快捷地调用各种图像处理和计算机视觉的API，以实现图像识别、对象检测和图像转换等操作。OpenCV提供了丰富的功能接口，简化了开发流程，使开发者能够专注于应用实现。

OpenCV - Open Computer Vision Library

Stag
STag是一种稳定的标记码，广泛应用于计算机视觉和机器人定位领域。通过STag，用户可以实现可靠的物体识别和追踪。STag提供了稳健的性能和易于集成的接口，简化了开发者在定位和识别任务中的工作。

GitHub - bbenligiray/stag: STag: A Stable Fiducial Marker System

Project
整个项目最主要的功能就机械臂运动控制，机械臂的手眼标定（坐标系的转化）和机器的视觉识别。我们先来介绍最重要的机器视觉识别。

机器视觉识别
想要让X1进行打字，那么它肯定得认识键盘，机器人咋可能自己就认识键盘呢，所以我们要教他认识键盘，并且告诉他那个键在哪个位置。这就用到了STag和OpenCV，STag的标记码能够确定键盘的位置，并且反馈坐标参数。

下面这段代码实现是，刷新相机界面获取实时画面，来检测STag码的位置

def stag_identify_loop(self):
       while True:
           self.camera.update_frame()  # 刷新相机界面
           frame = self.camera.color_frame()  # 获取当前帧
           (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)  # 获取画面中二维码的角度和id
           marker_pos_pack = self.calc_markers_base_position(corners, ids)  # 获取物的坐标(相机系)
           print("Camera coords = ", marker_pos_pack)
           cv2.imshow("按下键盘任意键退出", frame)
           # cv2.waitKey(1)
           # 按下键盘任意键退出
           if cv2.waitKey(1) & 0xFF != 255:
               break

两个STag码是为了确定位置，在用draw在图中画出键盘的位置,在这里需要考虑两个手的情况，当然如果单臂也是能完成的但是我们是要模拟人打字的，左手负责左边的区域，右手负责右边的区域。

def draw(frame, arm):
   global per_right_corners, per_left_corners
   # 将图片灰度
   imGray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
   # 获取图片中二维码的角点
   (corners, ids, rejected_corners) = stag.detectMarkers(imGray, 11)
   # 通过角点获取，二维码相对于相机的位移、旋转向量
   marker_pos_pack = calc_markers_base_position(corners, ids, marker_size, mtx, dist)
   if arm == "left":
       # 获取当前机械臂末端坐标
       stag_cur_coords = np.array(ml.get_base_coords())
       stag_cur_bcl = stag_cur_coords.copy()
       stag_cur_bcl[-3:] *= (np.pi / 180)
       # 通过手眼矩阵获取二维码相对于机械臂基座的坐标
       stag_fact_bcl = Eyes_in_hand(stag_cur_bcl, marker_pos_pack, "left")
       stag_coord = stag_cur_coords.copy()
       stag_coord[0] = stag_fact_bcl[0]
       stag_coord[1] = stag_fact_bcl[1]
       stag_coord[2] = stag_fact_bcl[2]
       # 存入二维码的三维坐标
       keyboard_coords[","] = stag_coord
   else:
       # 获取当前机械臂末端坐标
       stag_cur_coords = np.array(mr.get_base_coords())
       stag_cur_bcl = stag_cur_coords.copy()
       stag_cur_bcl[-3:] *= (np.pi / 180)
       # 通过手眼矩阵获取二维码相对于机械臂基座的坐标
       stag_fact_bcl = Eyes_in_hand(stag_cur_bcl, marker_pos_pack, "right")
       stag_coord = stag_cur_coords.copy()
       stag_coord[0] = stag_fact_bcl[0]
       stag_coord[1] = stag_fact_bcl[1]
       stag_coord[2] = stag_fact_bcl[2]
       # 存入二维码的三维坐标
       keyboard_coords["."] = stag_coord
   # 通过角点获取，二维码相对于相机的位移、旋转向量
   rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
   # 画出坐标系
   cv2.drawFrameAxes(frame, mtx, dist, rvecs, tvecs, 50)
   draw_img = frame

   x1 = corners[0][0][0][0]
   y1 = corners[0][0][0][1]
   x2 = corners[0][0][1][0]
   y2 = corners[0][0][1][1]

   x = x1 - x2
   y = y1 - y2
   # 根据两个交点之间连线的角度获取偏转角
   r = np.arctan(y / x)
   r = abs(r)
   # 获取两个角点之间的距离
   size = abs(x / np.cos(r))
   # 左臂摄像头两个按键x轴之间的距离
   left_x_dis = size * 1.3
   # 左臂摄像头两个按键y轴之间的距离
   left_y_dis = size * 1.35
   # 右臂摄像头两个按键x轴之间的距离
   right_x_dis = size * 1.3
   # 右臂摄像头两个按键y轴之间的距离
   right_y_dis = size * 1.35
   # 按键框的半径
   rad = int(size / 2)
   # 左臂摄像头x轴与第一个字母的偏移距离
   left_add_x = [size * 1.25]
   # 左臂摄像头y轴各行与第一个字母的偏移距离
   left_add_y = [-size * 2, -size * 2.3, -size * 3]
   # 右臂摄像头x轴与第一个字母的偏移距离
   right_add_x = [size * 1.3]
   # 右臂摄像头y轴各行与第一个字母的偏移距离
   right_add_y = [size * 4.1, size * 2.1, size * 1]
   # 获取按键框的中心点
   tray_frame = Path(corners[0][0])
   tray_frame_center_plot = tray_frame.vertices.mean(axis=0)

随后就是通过算法来确定每个键位的坐标如何计算的问题了。将圈选出来的键位存入数组当中，规定左手负责的区域，规定右手负责的区域

   # 左臂键盘布局
   left_keyboard_txt = [
       ["q", "w", "e", "r", "t", "y", "u", "i", "o", "p"],
       ["a", "s", "d", "f", "g", "h", "j", "k", "l"],
       ["z", "x", "c", "v", "b", "n", "m"]
   ]
   # 右臂键盘布局
   right_keyboard_txt = [
       ["m", "n", "b", "v", "c", "x", "z"],
       ["l", "k", "j", "h", "g", "f", "d", "s", "a"],
       ["p", "o", "i", "u", "y", "t", "r", "e", "w", "q"],
   ]
   # 左臂点按的字母
   left_control = ["q", "w", "e", "r", "t",
                   "a", "s", "d", "f", "g",
                   "z", "x", "c", "v", ","]
   # 左臂点按的字母
   right_control = ["y", "u", "i", "o", "p",
                    "h", "j", "k", "l",
                    "b", "n", "m", "."]

这样X1就能够知道键盘是什么，以及相对应的键位在哪里了，接下来就要解决的是机械臂的手眼标定的问题了，需要将目标的物体的坐标系和机械臂末端的坐标系转化到同一个坐标系当中去。

手眼标定-眼在手中
1. 标记检测

使用相机捕获图像，并检测STag标记，获取到标记码的三维坐标。调用 solve_marker_pnp 计算标记在相机坐标系中的位置和方向。

def calc_markers_base_position(corners: NDArray, ids: T.List, marker_size: int, mtx: NDArray, dist: NDArray) -> T.List:
   if len(corners) == 0:
       return []
   rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
   res = []
   for i, tvec, rvec in zip(ids, tvecs, rvecs):
       tvec = tvec.squeeze().tolist()
       rvec = rvec.squeeze().tolist()
       rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
       Rotation = cv2.Rodrigues(rotvector)[0]
       Euler = CvtRotationMatrixToEulerAngle(Rotation)
       cam_coords = tvec + rvec
       target_coords = cam_coords
   return target_coords

2. 坐标转换

将标记的旋转向量转换为旋转矩阵，再转换为欧拉角，以便于进一步的计算和分析，组合平移向量和旋转向量，得到目标坐标。

cv2.Rodrigues 函数用于在旋转向量和旋转矩阵之间进行转换。这个函数将旋转向量转换为旋转矩阵，或者将旋转矩阵转换为旋转向量。

rotvector = np.array([[rvec[0], rvec[1], rvec[2]]])
Rotation = cv2.Rodrigues(rotvector)[0]

#欧拉角和旋转矩阵的相互转换
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

#合并旋转和平移向量
cam_coords = tvec + rvec
target_coords = cam_coords

他们本身不是一个世界的人，现在强行转化到一个世界里，就能够互相知道在哪里了！这样就能够直接获取到键盘键位的result了 。

机械臂的运动控制
当我们有了目标物体的坐标之后，就到我们的X1闪亮登场了，开始执行运动，这里我们用到pymycobot来控制机械臂运动。

   r是右手的控制，l是左手的控制
   mr = Mercury("/dev/ttyACM0")
   ml = Mercury("/dev/ttyTHS0")
   #发送角度位置和速度给机械臂，
   mr.send_angles(mr_pos, sp)
   ml.send_angles(ml_pos, sp)
   #发送坐标和速度给机械臂
   mr.send_coords(mr_pos, sp)
   ml.send_coords(ml_pos, sp)

就是将目标的坐标传递给机械臂去运动，就能实现打字了，我们一起来看看运动的效果如何。

总结
通过本文，我们详细介绍了Mercury X1轮式人形机器人在打字任务中的应用实例。Mercury X1凭借其19自由度的灵活结构、丰富的感知能力和高性能的控制系统，展示了在自动化办公领域的巨大潜力。结合适配的摄像头模组和先进的机器视觉算法，Mercury X1能够精准识别并操作键盘，显著提升了工作效率和准确性。随着技术的不断进步，我们期待Mercury X1在更多领域展现其卓越的性能，为智能自动化带来更多可能性。

审核编辑 黄宇