引言
在現(xiàn)代科技的推動下，機器人在日常生活和工作場景中的應(yīng)用越來越廣泛。本文將介紹MercuryX1，這款先進的機器人如何通過其手臂末端的攝像頭識別并確定鍵盤的鍵位，從而進行精確的打字操作。通過這一案例，我們將展示MercuryX1在自動化辦公領(lǐng)域的潛力，以及其在提升效率和減少人為錯誤方面的顯著優(yōu)勢。

接下里跟隨我們的腳步，我們先簡單的介紹一下使用到的產(chǎn)品。

Product
Mercury X1
水星Mercury X1是一款輪式人形機器人，整體由水星Mercury B1和高性能移動底座組合而成，擁有19個自由度。其單臂為7自由度的類人手臂結(jié)構(gòu)機械臂。整機配備了英偉達Jetson Xavier主控。

移動底座具備豐富的感知能力，包括高性能激光雷達、超聲波傳感器和2D視覺傳感器。其直驅(qū)電機驅(qū)動系統(tǒng)使其最大運行速度可達1.2m/s，最大爬坡高度為2CM，最大爬坡角度為15度。整機最大續(xù)航時間高達8小時。

此外，Mercury X1支持包括ROS、Moveit、Gazebo和Mujoco等主流仿真軟件，提升了機器人智能的自主學(xué)習(xí)和快速迭代能力。

myCobot Pro Adaptive Gripper
是Mercury X1適配的自適應(yīng)夾爪，提供較大的加持力，和標(biāo)準(zhǔn)的M8航空插頭接口。

Camera Flange
適配于Mercury X1的攝像頭模組可以安裝在機械臂雙臂的末端，并搭配夾爪使用。通過攝像頭，實際獲取物體的相關(guān)信息，并利用這些數(shù)據(jù)進行機器視覺識別算法處理。通過USB接口，數(shù)據(jù)被傳輸?shù)絁etson Nano主控進行進一步處理。

技術(shù)要點
接下來介紹在項目中使用到的技術(shù)點。

pymycobot
pymycobot是Elephant Robotics專為其機械臂產(chǎn)品設(shè)計的控制庫。通過該庫，用戶可以方便快捷地調(diào)用API，以實現(xiàn)對機器人的精確控制和操作。pymycobot提供了豐富的功能接口，簡化了編程流程，使開發(fā)者能夠?qū)Ｗ⒂趹?yīng)用開發(fā)。

pymycobot · PyPI

OpenCV
OpenCV是一個開源的計算機視覺庫。通過該庫，用戶可以方便快捷地調(diào)用各種圖像處理和計算機視覺的API，以實現(xiàn)圖像識別、對象檢測和圖像轉(zhuǎn)換等操作。OpenCV提供了豐富的功能接口，簡化了開發(fā)流程，使開發(fā)者能夠?qū)Ｗ⒂趹?yīng)用實現(xiàn)。

OpenCV - Open Computer Vision Library

Stag
STag是一種穩(wěn)定的標(biāo)記碼，廣泛應(yīng)用于計算機視覺和機器人定位領(lǐng)域。通過STag，用戶可以實現(xiàn)可靠的物體識別和追蹤。STag提供了穩(wěn)健的性能和易于集成的接口，簡化了開發(fā)者在定位和識別任務(wù)中的工作。

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

Project
整個項目最主要的功能就機械臂運動控制，機械臂的手眼標(biāo)定（坐標(biāo)系的轉(zhuǎn)化）和機器的視覺識別。我們先來介紹最重要的機器視覺識別。

機器視覺識別
想要讓X1進行打字，那么它肯定得認(rèn)識鍵盤，機器人咋可能自己就認(rèn)識鍵盤呢，所以我們要教他認(rèn)識鍵盤，并且告訴他那個鍵在哪個位置。這就用到了STag和OpenCV，STag的標(biāo)記碼能夠確定鍵盤的位置，并且反饋坐標(biāo)參數(shù)。

下面這段代碼實現(xiàn)是，刷新相機界面獲取實時畫面，來檢測STag碼的位置

def stag_identify_loop(self):
       while True:
           self.camera.update_frame()  # 刷新相機界面
           frame = self.camera.color_frame()  # 獲取當(dāng)前幀
           (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)  # 獲取畫面中二維碼的角度和id
           marker_pos_pack = self.calc_markers_base_position(corners, ids)  # 獲取物的坐標(biāo)(相機系)
           print("Camera coords = ", marker_pos_pack)
           cv2.imshow("按下鍵盤任意鍵退出", frame)
           # cv2.waitKey(1)
           # 按下鍵盤任意鍵退出
           if cv2.waitKey(1) & 0xFF != 255:
               break

兩個STag碼是為了確定位置，在用draw在圖中畫出鍵盤的位置,在這里需要考慮兩個手的情況，當(dāng)然如果單臂也是能完成的但是我們是要模擬人打字的，左手負責(zé)左邊的區(qū)域，右手負責(zé)右邊的區(qū)域。

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)
   # 通過角點獲取，二維碼相對于相機的位移、旋轉(zhuǎn)向量
   marker_pos_pack = calc_markers_base_position(corners, ids, marker_size, mtx, dist)
   if arm == "left":
       # 獲取當(dāng)前機械臂末端坐標(biāo)
       stag_cur_coords = np.array(ml.get_base_coords())
       stag_cur_bcl = stag_cur_coords.copy()
       stag_cur_bcl[-3:] *= (np.pi / 180)
       # 通過手眼矩陣獲取二維碼相對于機械臂基座的坐標(biāo)
       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]
       # 存入二維碼的三維坐標(biāo)
       keyboard_coords[","] = stag_coord
   else:
       # 獲取當(dāng)前機械臂末端坐標(biāo)
       stag_cur_coords = np.array(mr.get_base_coords())
       stag_cur_bcl = stag_cur_coords.copy()
       stag_cur_bcl[-3:] *= (np.pi / 180)
       # 通過手眼矩陣獲取二維碼相對于機械臂基座的坐標(biāo)
       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]
       # 存入二維碼的三維坐標(biāo)
       keyboard_coords["."] = stag_coord
   # 通過角點獲取，二維碼相對于相機的位移、旋轉(zhuǎn)向量
   rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
   # 畫出坐標(biāo)系
   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
   # 根據(jù)兩個交點之間連線的角度獲取偏轉(zhuǎn)角
   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)

隨后就是通過算法來確定每個鍵位的坐標(biāo)如何計算的問題了。將圈選出來的鍵位存入數(shù)組當(dāng)中，規(guī)定左手負責(zé)的區(qū)域，規(guī)定右手負責(zé)的區(qū)域

   # 左臂鍵盤布局
   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就能夠知道鍵盤是什么，以及相對應(yīng)的鍵位在哪里了，接下來就要解決的是機械臂的手眼標(biāo)定的問題了，需要將目標(biāo)的物體的坐標(biāo)系和機械臂末端的坐標(biāo)系轉(zhuǎn)化到同一個坐標(biāo)系當(dāng)中去。

手眼標(biāo)定-眼在手中
1. 標(biāo)記檢測

使用相機捕獲圖像，并檢測STag標(biāo)記，獲取到標(biāo)記碼的三維坐標(biāo)。調(diào)用 solve_marker_pnp 計算標(biāo)記在相機坐標(biāo)系中的位置和方向。

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. 坐標(biāo)轉(zhuǎn)換

將標(biāo)記的旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣，再轉(zhuǎn)換為歐拉角，以便于進一步的計算和分析，組合平移向量和旋轉(zhuǎn)向量，得到目標(biāo)坐標(biāo)。

cv2.Rodrigues 函數(shù)用于在旋轉(zhuǎn)向量和旋轉(zhuǎn)矩陣之間進行轉(zhuǎn)換。這個函數(shù)將旋轉(zhuǎn)向量轉(zhuǎn)換為旋轉(zhuǎn)矩陣，或者將旋轉(zhuǎn)矩陣轉(zhuǎn)換為旋轉(zhuǎn)向量。

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

#歐拉角和旋轉(zhuǎn)矩陣的相互轉(zhuǎn)換
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

#合并旋轉(zhuǎn)和平移向量
cam_coords = tvec + rvec
target_coords = cam_coords

他們本身不是一個世界的人，現(xiàn)在強行轉(zhuǎn)化到一個世界里，就能夠互相知道在哪里了！這樣就能夠直接獲取到鍵盤鍵位的result了 。

機械臂的運動控制
當(dāng)我們有了目標(biāo)物體的坐標(biāo)之后，就到我們的X1閃亮登場了，開始執(zhí)行運動，這里我們用到pymycobot來控制機械臂運動。

   r是右手的控制，l是左手的控制
   mr = Mercury("/dev/ttyACM0")
   ml = Mercury("/dev/ttyTHS0")
   #發(fā)送角度位置和速度給機械臂，
   mr.send_angles(mr_pos, sp)
   ml.send_angles(ml_pos, sp)
   #發(fā)送坐標(biāo)和速度給機械臂
   mr.send_coords(mr_pos, sp)
   ml.send_coords(ml_pos, sp)

就是將目標(biāo)的坐標(biāo)傳遞給機械臂去運動，就能實現(xiàn)打字了，我們一起來看看運動的效果如何。

總結(jié)
通過本文，我們詳細介紹了Mercury X1輪式人形機器人在打字任務(wù)中的應(yīng)用實例。Mercury X1憑借其19自由度的靈活結(jié)構(gòu)、豐富的感知能力和高性能的控制系統(tǒng)，展示了在自動化辦公領(lǐng)域的巨大潛力。結(jié)合適配的攝像頭模組和先進的機器視覺算法，Mercury X1能夠精準(zhǔn)識別并操作鍵盤，顯著提升了工作效率和準(zhǔn)確性。隨著技術(shù)的不斷進步，我們期待Mercury X1在更多領(lǐng)域展現(xiàn)其卓越的性能，為智能自動化帶來更多可能性。

審核編輯 黃宇