本案例展示了如何利用視覺(jué)系統(tǒng)提升機(jī)械臂的抓取精度，成功實(shí)現(xiàn)了人形機(jī)器人的雙臂抓取不在局限于單臂抓取。

引言
如今市面上已經(jīng)有了許多不同類型的人形機(jī)器人，涵蓋了服務(wù)行業(yè)和醫(yī)療行業(yè)等各個(gè)領(lǐng)域。這些機(jī)器人以其智能化和自動(dòng)化的特性逐漸融入我們的日常生活和工作中。然而，盡管現(xiàn)有的人形機(jī)器人在特定應(yīng)用場(chǎng)景中展現(xiàn)出了巨大的潛力，如何進(jìn)一步提升其操作精度、適應(yīng)性和多功能性，仍然是機(jī)器人技術(shù)發(fā)展的關(guān)鍵挑戰(zhàn)。針對(duì)這一需求，本案例探討了通過(guò)整合視覺(jué)系統(tǒng)與機(jī)械臂技術(shù)，來(lái)提升人形機(jī)器人在復(fù)雜環(huán)境中的自主操作能力，特別是在精確抓取和操作任務(wù)中的應(yīng)用。

目標(biāo)：

通過(guò)結(jié)合openc算法＆STag標(biāo)記碼視覺(jué)系統(tǒng)和Mercury X1輪式人形機(jī)器人，實(shí)現(xiàn)對(duì)各種形狀和大小的物品進(jìn)行精準(zhǔn)抓取，提高分揀效率和準(zhǔn)確度，實(shí)現(xiàn)雙手合作充分發(fā)揮人形機(jī)器人的作用。

產(chǎn)品
Mercury X1
Mercury X1是由Elephant Robotics開(kāi)發(fā)的一款先進(jìn)的人形機(jī)器人，特別設(shè)計(jì)用于應(yīng)對(duì)各種自動(dòng)化任務(wù)。它擁有19個(gè)自由度，（單臂7個(gè)自由度）使其在執(zhí)行任務(wù)時(shí)具有極高的靈活性和適應(yīng)性。Mercury X1配備了輪式移動(dòng)底座，由高性能直驅(qū)電機(jī)驅(qū)動(dòng)，能夠在復(fù)雜環(huán)境中穩(wěn)定移動(dòng)，并具備高達(dá)8小時(shí)的電池續(xù)航能力，適合個(gè)人和商業(yè)應(yīng)用。

這款機(jī)器人采用高性能主控制器系統(tǒng)，配置了NVIDIA Jetson Xavier提供強(qiáng)大的計(jì)算支持，以處理視覺(jué)測(cè)距、傳感器融合、定位與地圖構(gòu)建、障礙檢測(cè)和路徑規(guī)劃等復(fù)雜算法。此外，Mercury X1的移動(dòng)基座裝備了LiDAR、超聲波傳感器和2D視覺(jué)系統(tǒng)，能夠?qū)崿F(xiàn)高感知的環(huán)境交互。

myCobot Pro Adaptive Gripper
自適應(yīng)夾爪，它可以拾起任何形狀的任何物體并且不會(huì)松開(kāi)。使用它來(lái)完成一系列完整的應(yīng)用，并快速投入生產(chǎn) - 無(wú)需機(jī)器人專業(yè)知識(shí)。它是高度靈活和可靠的機(jī)器人單元的關(guān)鍵。

myCobot Pro Camera Flange
使用USB-B數(shù)據(jù)線能夠獲取到圖像的相機(jī)模組。

接下來(lái)，我們將探討這些技術(shù)在實(shí)際應(yīng)用中是如何被集成到機(jī)器人中，并展示其在具體任務(wù)中的表現(xiàn)。

技術(shù)概覽
OpenCV
OpenCV是用于實(shí)現(xiàn)圖像處理和計(jì)算機(jī)視覺(jué)的開(kāi)源庫(kù)，它在本次的案例中扮演了至關(guān)重要的角色，沒(méi)有它就不能完成這個(gè)項(xiàng)目。機(jī)器人的攝像頭通過(guò)OpenCV分析收集到的視覺(jué)數(shù)據(jù)，識(shí)別和定位物體。OpenCV的算法使機(jī)器人能夠識(shí)別物體形狀、大小和精確坐標(biāo)，這些信息對(duì)于精確抓取和操作至關(guān)重要。

提供了物體的坐標(biāo)給機(jī)器人，就可以實(shí)現(xiàn)精準(zhǔn)的抓取。

S-Tag標(biāo)記碼技術(shù)
S-Tag標(biāo)記碼是一種高度可靠的標(biāo)識(shí)系統(tǒng)，設(shè)計(jì)用于在視覺(jué)上挑戰(zhàn)的環(huán)境中提供準(zhǔn)確的標(biāo)記識(shí)別。這些標(biāo)記被用于標(biāo)識(shí)Mercury X1機(jī)器人操作環(huán)境中的物體和位置。即使在光線不足或視線受阻的情況下，S-Tag也能確保機(jī)器人通過(guò)其攝像頭系統(tǒng)準(zhǔn)確識(shí)別目標(biāo)物體。

https://youtu.be/vnHI3GzLVrY

通過(guò)這些技術(shù)的應(yīng)用，Mercury X1大象人形機(jī)器人能夠執(zhí)行復(fù)雜的任務(wù)，如自主導(dǎo)航、物體識(shí)別和精確操控，這些都是現(xiàn)代自動(dòng)化和智能系統(tǒng)不可或缺的能力。

pymycobot
pymycobot是用于控制Mercury X1機(jī)器人機(jī)械臂和末端執(zhí)行器（如夾爪）的Python庫(kù)。它允許開(kāi)發(fā)者精確控制機(jī)械臂的角度、坐標(biāo)以及運(yùn)動(dòng)模式，包括差補(bǔ)模式和刷新模式。此庫(kù)為機(jī)器人提供了高度的靈活性和可定制性，使得機(jī)器人能夠執(zhí)行復(fù)雜的抓取和操控任務(wù)，并適應(yīng)各種操作需求。

項(xiàng)目實(shí)現(xiàn)
使用前準(zhǔn)備
首先的確保機(jī)械臂的零位正確，可以通過(guò)下述方法校準(zhǔn)零位：

1）使用放松指令釋放關(guān)節(jié)電機(jī)（注意！放松后需要扶住關(guān)節(jié)防止機(jī)械臂下墜損壞！）

ml.release_all_servos()
mr.release_all_servos()

2）拖動(dòng)機(jī)械臂回到零位

可以通過(guò)以下細(xì)節(jié)確定零點(diǎn)的位置

3) 發(fā)送指令鎖閉電機(jī)

ml.focus_all_servos()
mr.focus_all_servos()

4）檢查零點(diǎn)是否正確

輸入獲取關(guān)節(jié)角度指令查詢當(dāng)前位置

ml.get_angles()
mr.get_angles()

如果返回的關(guān)節(jié)角度逼近[0, 0, 0, 0, 90, 0]，則視為零點(diǎn)正確

5）零點(diǎn)校準(zhǔn)

如果在零位讀取到的關(guān)節(jié)角度與[0, 0, 0, 0, 90, 0]相差很大，則需要校準(zhǔn)關(guān)節(jié)零位

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

校準(zhǔn)完畢后讀取關(guān)節(jié)信息，返回為[0, 0, 0, 0, 90, 0]則表示校準(zhǔn)成功

ml.get_angles()
mr.get_angles()

這樣就準(zhǔn)備ok了，接下來(lái)可以繼續(xù)我們的功能部分的實(shí)現(xiàn)了。

相機(jī)和夾爪的安裝
相機(jī)和夾爪的安裝方式與視覺(jué)識(shí)別的手眼矩陣相對(duì)應(yīng)，已經(jīng)提前做好了一個(gè)關(guān)于Mercury X1 相機(jī)夾爪的手眼標(biāo)定的數(shù)值，如果更改的話需要重新進(jìn)行手眼標(biāo)定。

首先將機(jī)械臂移動(dòng)回零點(diǎn)：

ml.over_limit_return_zero()
mr.over_limit_return_zero()

安裝先裝攝像頭（注意方向）然后再安裝夾爪，也需要注意方向。

視覺(jué)算法模塊
然后我們會(huì)使用到camera_detect 這個(gè)功能包，封裝了一些關(guān)于相機(jī)使用的方法，接下來(lái)我會(huì)介紹一些接口,針對(duì)對(duì)STag碼的識(shí)別，使用起來(lái)非常的方便，不需要自己再次進(jìn)行手眼標(biāo)定的計(jì)算了。

obj = camera_detect(Camera_ID, mark_size, mtx, dist, direction)
參數(shù)：
camera_ID: 相機(jī)ID
mark_size: stag碼邊長(zhǎng)mm
direction: 0：左臂 1：右臂
功能： 相機(jī)和機(jī)械臂初始化

obj.camera_open_loop()
功能： 顯示相機(jī)畫(huà)面
obj.stag_identify_loop()
功能： 顯示相機(jī)畫(huà)面,識(shí)別并打印stag碼的相機(jī)坐標(biāo)

coords = obj.stag_robot_identify(direction)
參數(shù)：
direction: 0：左臂 1：右臂
return: stag碼的機(jī)械臂坐標(biāo)
功能： 計(jì)算stag碼的機(jī)械臂坐標(biāo)

obj.vision_trace(catch_mode, direction)
參數(shù)：
catch_mode: 0:水平抓取，1:豎直抓取
direction: 0：左臂 1：右臂
功能： 顯示相機(jī)畫(huà)面,識(shí)別并打印stag碼的相機(jī)坐標(biāo)

以下是調(diào)用camera_detect包的使用代碼,非常的干凈簡(jiǎn)潔。

import numpy as np
from pymycobot import Mercury
# 導(dǎo)入視覺(jué)識(shí)別stag包
from camera_detect import camera_detect

if __name__ == "__main__":
   camera_params = np.load("camera_params.npz")  # 相機(jī)配置文件
   mtx, dist = camera_params["mtx"], camera_params["dist"]
   ml = Mercury("/dev/left_arm")  # 設(shè)置左臂端口
   mr = Mercury("/dev/right_arm")  # 設(shè)置右臂端口
   arm = 0         # 0左臂，1右臂
   catch_mode = 0  # 0水平抓取，1豎直抓取
   # 攝像頭id需要根據(jù)實(shí)際更改
   left_camera_id = 3
   right_camera_id = 6
   stag_size = 32
   # 新建左右臂視覺(jué)識(shí)別對(duì)象
   ml_obj = camera_detect(left_camera_id, stag_size, mtx, dist, arm)
   mr_obj = camera_detect(right_camera_id, stag_size, mtx, dist, arm)
   # 左右臂移動(dòng)至水平觀察位，進(jìn)行抓取
   mr_obj.vision_trace(catch_mode, mr)
   ml_obj.vision_trace(catch_mode, ml)

正常的手眼標(biāo)定的步驟如下：

1數(shù)據(jù)采集：采集若干組手眼數(shù)據(jù)，包括機(jī)器人末端執(zhí)行器在不同位置的姿態(tài)（即位置和方向）和相機(jī)看到的特征點(diǎn)的位姿。

2 建立運(yùn)動(dòng)方程：為了確定相機(jī)坐標(biāo)系與機(jī)器人末端執(zhí)行器坐標(biāo)系之間的變換關(guān)系

3 求解變換矩陣 ：得到的是描述相機(jī)坐標(biāo)系和機(jī)器人末端執(zhí)行器（手）坐標(biāo)系之間的空間變換關(guān)系的值。

下面的代碼是解變換矩陣的例子。

# 將旋轉(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


# 將歐拉角轉(zhuǎn)為旋轉(zhuǎn)矩陣
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


# 坐標(biāo)轉(zhuǎn)換為齊次變換矩陣，（x,y,z,rx,ry,rz）單位rad
def Transformation_matrix(coord):
   position_robot = coord[:3]
   pose_robot = coord[3:]
   # 將歐拉角轉(zhuǎn)為旋轉(zhuǎn)矩陣
   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]])
   # 將兩個(gè)數(shù)組按行拼接起來(lái)
   matrix = np.concatenate((temp, array_1x4), axis=0)
   return matrix

機(jī)械臂抓取
我們來(lái)控制機(jī)械臂的雙臂，會(huì)使用到pymycobot庫(kù)當(dāng)中的Mercury,我們?yōu)榱四軌蛑庇^明了，每一條手臂控制的時(shí)候都需要?jiǎng)?chuàng)建一個(gè)對(duì)象。

from pymycobot import Mercury

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

控制機(jī)械臂末端運(yùn)動(dòng)到coords的坐標(biāo)。
send_base_coords(coords, speed)
功能：：控制機(jī)械臂末端運(yùn)動(dòng)到指定坐標(biāo)
參數(shù)：
coords: ： 坐標(biāo)值列表 [x,y,z,rx,ry,rz],length6
速度(int): 1 ~ 100

下面是識(shí)別目標(biāo)物體進(jìn)行抓取的代碼。

if __name__ == "__main__":
   # 設(shè)置攝像頭id                  
   camera = UVCCamera(5, mtx, dist)
   # 打開(kāi)攝像頭
   camera.capture()
   # 設(shè)置左臂觀察點(diǎn)
   origin_anglesL = [-44.24, 15.56, 0.0, -102.59, 65.28, 52.06, 23.49]
   # 設(shè)置夾爪運(yùn)動(dòng)模式
   ml.set_gripper_mode(0)
   # 設(shè)置工具坐標(biāo)系
   ml.set_tool_reference([0, 0, Tool_LEN, 0, 0, 0])
   # 將末端坐標(biāo)系設(shè)置為工具        
   ml.set_end_type(1)
   # 設(shè)置移動(dòng)速度                 
   sp = 40
   # 移動(dòng)到觀測(cè)點(diǎn)                  
   ml.send_angles(origin_anglesL, sp)
   # 等待機(jī)械臂運(yùn)動(dòng)結(jié)束    
   waitl()
   # 刷新相機(jī)界面  
   camera.update_frame()
   # 獲取當(dāng)前幀
   frame = camera.color_frame()
   # 獲取畫(huà)面中二維碼的角度和id
   (corners, ids, rejected_corners) = stag.detectMarkers(frame, 11)
   # 獲取物的坐標(biāo)(相機(jī)系)
   marker_pos_pack = calc_markers_base_position(corners, ids, MARKER_SIZE, mtx, dist)
   # 獲取機(jī)械臂當(dāng)前坐標(biāo)
   cur_coords = np.array(ml.get_base_coords())
   # 將角度值轉(zhuǎn)為弧度值       
   cur_bcl = cur_coords.copy()
   cur_bcl[-3:] *= (np.pi / 180)
   # 通過(guò)矩陣變化將物體坐標(biāo)(相機(jī)系)轉(zhuǎn)成(基坐標(biāo)系)
   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
   # 機(jī)械臂移動(dòng)到二維碼上方
   ml.send_base_coords(target_coords, 30)
   # 等待機(jī)械臂運(yùn)動(dòng)結(jié)束
   waitl()
   # 打開(kāi)夾爪              
   ml.set_gripper_value(100, 100)
   # 機(jī)械臂沿z軸向下移動(dòng)
   ml.send_base_coord(3, fact_bcl[2], 10)
   # 等待機(jī)械臂運(yùn)動(dòng)結(jié)束
   waitl()
   # 閉合夾爪                                                        
   ml.set_gripper_value(20, 100)

   # 等待夾爪閉合       
   time.sleep(2)

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

單獨(dú)啟動(dòng)左手的demo

下面是單獨(dú)啟動(dòng)右手的demo。

總結(jié)
如果擁有一臺(tái)Mercury X1你會(huì)用他來(lái)做些什么呢？發(fā)揮他人形機(jī)器人最關(guān)鍵的特性，如果你有什么好的想法，想要實(shí)現(xiàn)的歡迎在下方留言和我們溝通，你的點(diǎn)贊和留言是對(duì)我們最大的支持！

審核編輯 黃宇