大象機(jī)器人水星MercuryX1輪式人形機(jī)器人基于物體標(biāo)記建模的鍵盤(pán)點(diǎn)按操作！

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

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

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

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

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

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

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

技術(shù)要點(diǎn)
接下來(lái)介紹在項(xiàng)目中使用到的技術(shù)點(diǎn)。

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

pymycobot · PyPI

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

OpenCV - Open Computer Vision Library

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

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

Project
整個(gè)項(xiàng)目最主要的功能就機(jī)械臂運(yùn)動(dòng)控制，機(jī)械臂的手眼標(biāo)定（坐標(biāo)系的轉(zhuǎn)化）和機(jī)器的視覺(jué)識(shí)別。我們先來(lái)介紹最重要的機(jī)器視覺(jué)識(shí)別。

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

下面這段代碼實(shí)現(xiàn)是，刷新相機(jī)界面獲取實(shí)時(shí)畫(huà)面，來(lái)檢測(cè)STag碼的位置

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

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

def draw(frame, arm):
   global per_right_corners, per_left_corners
   # 將圖片灰度
   imGray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
   # 獲取圖片中二維碼的角點(diǎn)
   (corners, ids, rejected_corners) = stag.detectMarkers(imGray, 11)
   # 通過(guò)角點(diǎn)獲取，二維碼相對(duì)于相機(jī)的位移、旋轉(zhuǎn)向量
   marker_pos_pack = calc_markers_base_position(corners, ids, marker_size, mtx, dist)
   if arm == "left":
       # 獲取當(dāng)前機(jī)械臂末端坐標(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)
       # 通過(guò)手眼矩陣獲取二維碼相對(duì)于機(jī)械臂基座的坐標(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)前機(jī)械臂末端坐標(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)
       # 通過(guò)手眼矩陣獲取二維碼相對(duì)于機(jī)械臂基座的坐標(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
   # 通過(guò)角點(diǎn)獲取，二維碼相對(duì)于相機(jī)的位移、旋轉(zhuǎn)向量
   rvecs, tvecs = solve_marker_pnp(corners, marker_size, mtx, dist)
   # 畫(huà)出坐標(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ù)兩個(gè)交點(diǎn)之間連線的角度獲取偏轉(zhuǎn)角
   r = np.arctan(y / x)
   r = abs(r)
   # 獲取兩個(gè)角點(diǎn)之間的距離
   size = abs(x / np.cos(r))
   # 左臂攝像頭兩個(gè)按鍵x軸之間的距離
   left_x_dis = size * 1.3
   # 左臂攝像頭兩個(gè)按鍵y軸之間的距離
   left_y_dis = size * 1.35
   # 右臂攝像頭兩個(gè)按鍵x軸之間的距離
   right_x_dis = size * 1.3
   # 右臂攝像頭兩個(gè)按鍵y軸之間的距離
   right_y_dis = size * 1.35
   # 按鍵框的半徑
   rad = int(size / 2)
   # 左臂攝像頭x軸與第一個(gè)字母的偏移距離
   left_add_x = [size * 1.25]
   # 左臂攝像頭y軸各行與第一個(gè)字母的偏移距離
   left_add_y = [-size * 2, -size * 2.3, -size * 3]
   # 右臂攝像頭x軸與第一個(gè)字母的偏移距離
   right_add_x = [size * 1.3]
   # 右臂攝像頭y軸各行與第一個(gè)字母的偏移距離
   right_add_y = [size * 4.1, size * 2.1, size * 1]
   # 獲取按鍵框的中心點(diǎn)
   tray_frame = Path(corners[0][0])
   tray_frame_center_plot = tray_frame.vertices.mean(axis=0)

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

   # 左臂鍵盤(pán)布局
   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"]
   ]
   # 右臂鍵盤(pán)布局
   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"],
   ]
   # 左臂點(diǎn)按的字母
   left_control = ["q", "w", "e", "r", "t",
                   "a", "s", "d", "f", "g",
                   "z", "x", "c", "v", ","]
   # 左臂點(diǎn)按的字母
   right_control = ["y", "u", "i", "o", "p",
                    "h", "j", "k", "l",
                    "b", "n", "m", "."]

這樣X(jué)1就能夠知道鍵盤(pán)是什么，以及相對(duì)應(yīng)的鍵位在哪里了，接下來(lái)就要解決的是機(jī)械臂的手眼標(biāo)定的問(wèn)題了，需要將目標(biāo)的物體的坐標(biāo)系和機(jī)械臂末端的坐標(biāo)系轉(zhuǎn)化到同一個(gè)坐標(biāo)系當(dāng)中去。

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

使用相機(jī)捕獲圖像，并檢測(cè)STag標(biāo)記，獲取到標(biāo)記碼的三維坐標(biāo)。調(diào)用 solve_marker_pnp 計(jì)算標(biāo)記在相機(jī)坐標(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)換為歐拉角，以便于進(jìn)一步的計(jì)算和分析，組合平移向量和旋轉(zhuǎn)向量，得到目標(biāo)坐標(biāo)。

cv2.Rodrigues 函數(shù)用于在旋轉(zhuǎn)向量和旋轉(zhuǎn)矩陣之間進(jìn)行轉(zhuǎn)換。這個(gè)函數(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

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

機(jī)械臂的運(yùn)動(dòng)控制
當(dāng)我們有了目標(biāo)物體的坐標(biāo)之后，就到我們的X1閃亮登場(chǎng)了，開(kāi)始執(zhí)行運(yùn)動(dòng)，這里我們用到pymycobot來(lái)控制機(jī)械臂運(yùn)動(dòng)。

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

就是將目標(biāo)的坐標(biāo)傳遞給機(jī)械臂去運(yùn)動(dòng)，就能實(shí)現(xiàn)打字了，我們一起來(lái)看看運(yùn)動(dòng)的效果如何。

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

審核編輯 黃宇