前言

我們在YouTube上看到有人使用機械臂實現(xiàn)物體跟蹤功能的視頻時，深受啟發(fā)，對這個項目產(chǎn)生了濃厚的興趣，并決定獨立開發(fā)一個類似的程序。

(資料圖片)

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

機械臂的介紹 mycobot280-JetsonNano

操作使用的機械臂是myCobot280-Jetson Nano

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

Jetson Nano

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

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

開發(fā)過程

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

相機捕捉目標

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

Aruco碼

下面是實現(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)

手眼標定

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

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

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

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

#函數(shù)用于計算相機間的相似性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ù)用于計算相似性的變化率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

機械臂控制

在這之后就是物體檢測和機械臂的控制，將識別到的物體的坐標轉(zhuǎn)換成機械臂的運動指令，這里用到的是pymycobot庫來進行對機械臂的控制。

#用于進行視覺追蹤并計算目標位置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ù)相機坐標計算目標位置    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

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

讓我們來看下效果如何。

總結(jié)：

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

審核編輯 黃宇