引言
在現代科技的推動下，機器人在日常生活和工作場景中的應用越來越廣泛。本文將介紹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在更多領域展現其卓越的性能，為智能自動化帶來更多可能性。

審核編輯 黃宇