前言

我們?cè)赮ouTube上看到有人使用機(jī)械臂實(shí)現(xiàn)物體跟蹤功能的視頻時(shí)，深受啟發(fā)，對(duì)這個(gè)項(xiàng)目產(chǎn)生了濃厚的興趣，并決定獨(dú)立開發(fā)一個(gè)類似的程序。

(資料圖片)

我們的目標(biāo)是開發(fā)一個(gè)能夠準(zhǔn)確識(shí)別和跟蹤物體的機(jī)械臂系統(tǒng)，以便在實(shí)際應(yīng)用中發(fā)揮作用，這個(gè)項(xiàng)目涉及到許多技術(shù)和算法，包括視覺識(shí)別、手眼協(xié)同和機(jī)械臂控制等方面。

機(jī)械臂的介紹 mycobot280-JetsonNano

操作使用的機(jī)械臂是myCobot280-Jetson Nano

這是一款大象機(jī)器人公司生產(chǎn)的小六軸機(jī)械臂，以JetsonNano為微處理器，ESP32為輔助控制，UR協(xié)作形結(jié)構(gòu)。myCobot280 JetsonNano，本體重量1030g， 負(fù)載250g，工作半徑280mm，設(shè)計(jì)緊湊便攜，小巧但功能強(qiáng)大，操作簡(jiǎn)單，能與人協(xié)同、安全工作。

Jetson Nano

Jetson Nano是英偉達(dá)推出的一款嵌入式人工智能計(jì)算機(jī)，它采用了NVIDIA Maxwell GPU和四核ARMCortex-A57處理器，性能強(qiáng)大。Jetson Nano支持多種人工智能框架和工具，如TensorFlow、PyTorch、Caffe和MXNet等。此外，Jetson Nano還具有多種輸入輸出接口，如HDMI、USB、GPIO等，方便開發(fā)人員進(jìn)行硬件連接和控制。

由于Jetson Nano具有強(qiáng)大的計(jì)算性能和專門為人工智能開發(fā)設(shè)計(jì)的特點(diǎn),支持多種深度學(xué)習(xí)框架，如TensorFlow、PyTorch和Caffe等，可以更方便地進(jìn)行人工智能應(yīng)用開發(fā),它成為了開發(fā)人員進(jìn)行人工智能應(yīng)用開發(fā)的理想平臺(tái)之一。

開發(fā)過程

下圖是項(xiàng)目的開發(fā)流程圖

相機(jī)捕捉目標(biāo)

在我開始開發(fā)之前，我首先進(jìn)行了一些調(diào)研和實(shí)驗(yàn)。我使用了一個(gè)相機(jī)來捕捉物體的圖像，并使用OpenCV庫來識(shí)別和跟蹤Aruco碼。嘗試過多種的方法，物體的識(shí)別需要讓機(jī)器進(jìn)行學(xué)習(xí)，我們要識(shí)別的目標(biāo)，這樣會(huì)增加項(xiàng)目開發(fā)的時(shí)間，最后決定用aruco碼來進(jìn)行識(shí)別，這樣可以快速捕捉到aruco碼，進(jìn)行下一步開發(fā)。

Aruco碼

下面是實(shí)現(xiàn)的代碼：

def show_video_v2(self):        # self.robot.init_robot()        xyz = np.array([0,0,0])        LIST = []        num_count = 0        list_len = 5        # cmax = [180, 80, 240]        # cmin = [130, -80, 200]        cmax = [150, -150, 300]        cmin = [-150, -250, 200]        while cv2.waitKey(1) < 0:            success, img = self.cap.read()            if not success:                print("It seems that the image cannot be acquired correctly.")                break            # transfrom the img to model of gray            gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)            # Detect ArUco marker.            corners, ids, rejectImaPoint = cv2.aruco.detectMarkers(                gray, self.aruco_dict, parameters=self.aruco_params            )            if len(corners) > 0:                if ids is not None:                    # get informations of aruco                    ret = cv2.aruco.estimatePoseSingleMarkers(                        # """https://stackoverflow.com/questions/53303730/what-is-the-value-for-markerlength-in-aruco-estimateposesinglemarkers"""                        corners, 0.025, self.camera_matrix, self.dist_coeffs                    )                    # rvec:rotation offset,tvec:translation deviator                    (rvec, tvec) = (ret[0], ret[1])                    (rvec - tvec).any()                    xyz = tvec[0, 0, :] * 1000                    rpy = rvec[0,0,:]                    camera = np.array([xyz[0], xyz[1], xyz[2]])                    if num_count > list_len:                        target = model_track(camera)                        print("target", target)                        for i in range(3):                            if target[i] > cmax[i]:                                target[i] = cmax[i]                            if target[i] < cmin[i]:                                target[i] = cmin[i]                        pose = np.array([-103, 8.9, -164])                        coord = np.concatenate((target.copy(), pose), axis=0)                        # q1 = math.atan(xyz[0] / xyz[2])*180/np.pimc.send_coords(coord,50,0)                        # print("target", coord)                        num_count = 1                    else:                        num_count = num_count + 1                    for i in range(rvec.shape[0]):                        # draw the aruco on img                        cv2.aruco.drawDetectedMarkers(img, corners)            cv2.imshow("show_video", img)

手眼標(biāo)定

手眼標(biāo)定是指在機(jī)器人領(lǐng)域中，確定機(jī)器人末端執(zhí)行器（例如機(jī)械手臂）相對(duì)于機(jī)器人基座坐標(biāo)系的位置和姿態(tài)。這個(gè)過程涉及到將機(jī)器人末端執(zhí)行器與相機(jī)進(jìn)行配對(duì)，然后通過捕捉執(zhí)行器在相機(jī)視野中的位置和姿態(tài)來確定它在機(jī)器人基座坐標(biāo)系中的位置和姿態(tài)。

手眼標(biāo)定通常涉及到在機(jī)器人末端執(zhí)行器和相機(jī)之間進(jìn)行一系列的運(yùn)動(dòng)，以便收集足夠的數(shù)據(jù)來計(jì)算出它們之間的變換矩陣。這個(gè)變換矩陣描述了機(jī)器人末端執(zhí)行器相對(duì)于相機(jī)的位置和姿態(tài)，從而可以用來控制機(jī)器人的運(yùn)動(dòng)，使其能夠準(zhǔn)確地執(zhí)行所需的任務(wù)。

在"eye-to-hand"手眼標(biāo)定中，相機(jī)被視為一個(gè)不動(dòng)的觀察者（"eye"），而機(jī)器人末端執(zhí)行器則被視為在相機(jī)視野中移動(dòng)的物體（"hand"）。機(jī)器人末端執(zhí)行器在相機(jī)周圍移動(dòng)時(shí)，會(huì)收集到一系列的圖像，這些圖像包含了機(jī)器人末端執(zhí)行器在不同位置和姿態(tài)下的圖像信息。通過分析這些圖像，可以計(jì)算出機(jī)器人末端執(zhí)行器相對(duì)于相機(jī)的位置和姿態(tài)，從而完成手眼標(biāo)定。

下面是處理坐標(biāo)之間轉(zhuǎn)換數(shù)據(jù)的代碼

#函數(shù)用于計(jì)算相機(jī)間的相似性def calculate_similarity(camera):    n = camera.shape[0]    total_similarity = 0    for i in range(n):        for j in range(i+1, n):            vector_a = camera[i]            vector_b = camera[j]            dot_product = np.dot(vector_a, vector_b)            norm_a = np.linalg.norm(vector_a)            norm_b = np.linalg.norm(vector_b)            similarity = dot_product / (norm_a * norm_b)            total_similarity += similarity    return total_similarity/n#函數(shù)用于計(jì)算相似性的變化率def similarity_change_rate(new_similarity):    global prev_similarity    if prev_similarity is None:        prev_similarity = new_similarity        return 0    else:        change_rate = (new_similarity - prev_similarity) / prev_similarity        prev_similarity = new_similarity        return change_rate#函數(shù)用于將旋轉(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#函數(shù)用于將歐拉角轉(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

機(jī)械臂控制

在這之后就是物體檢測(cè)和機(jī)械臂的控制，將識(shí)別到的物體的坐標(biāo)轉(zhuǎn)換成機(jī)械臂的運(yùn)動(dòng)指令，這里用到的是pymycobot庫來進(jìn)行對(duì)機(jī)械臂的控制。

#用于進(jìn)行視覺追蹤并計(jì)算目標(biāo)位置def Visual_tracking280(coord, camera):    pose_camera = camera[:3]    angle_camear = camera[3:]    r = CvtEulerAngleToRotationMatrix(angle_camear)    # r = np.array([[1, 0, 0],    #                  [0, 1, 0],    #                  [0, 0, 1]])    euler = np.radians(coord[3:])    R = CvtEulerAngleToRotationMatrix(euler)    offset = np.array([0, 0, -250])    Roff = np.array([[1, 0, 0],                     [0, -1, 0],                     [0, 0, -1]])    # Roff = np.array([[1, 0, 0],    #                  [0, 1, 0],    #                  [0, 0, 1]])    vector = pose_camera + offset    # print("R", R)    # print("r", r)    move_pos = np.dot(np.dot(R, r), Roff).dot(vector)    pos = coord[:3] + move_pos    # angle = np.array(CvtRotationMatrixToEulerAngle(np.dot(np.dot(R, r), Roff))) * 180/np.pi    angle =  coord[3:]    target = np.concatenate((pos, angle))    return target#根據(jù)相機(jī)坐標(biāo)計(jì)算目標(biāo)位置    def model_track(camera):    model_pos = np.array([-camera[0], -camera[2], -camera[1]])    camera_pos = np.array([-37.5, 416.6, 322.9])    target_pos = model_pos + camera_pos    # print("model_pos", model_pos)    # print("target_pos", target_pos)    return target_pos

最后整理一下項(xiàng)目的邏輯關(guān)系，

讓我們來看下效果如何。

總結(jié)：

在調(diào)試的過程中，我們發(fā)現(xiàn)跟蹤的效果并不是非常流暢和靈敏。我們通過控制檢測(cè)周期來調(diào)整流暢性，但是需要緩慢移動(dòng)被跟蹤的物體目標(biāo)，才能達(dá)到更好的效果。仍然還有一些不足的地方，在相機(jī)固定的情況下，機(jī)械臂的本體可能會(huì)遮擋相機(jī)的視野，導(dǎo)致沒有辦法進(jìn)行下一步跟蹤，想到的解決方案是相機(jī)換個(gè)位置不被遮擋的位置（坐標(biāo)換算那些都得重新計(jì)算）。如果你有更好的想法歡迎跟我們溝通！感謝你的耐心觀看。

審核編輯 黃宇