使用myCobot 280 Jeston Nano進(jìn)行物體精確識(shí)別追蹤

前言

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

我們的目標(biāo)是開(kāi)發(fā)一個(gè)能夠準(zhǔn)確識(shí)別和跟蹤物體的機(jī)械臂系統(tǒng)，以便在實(shí)際應(yīng)用中發(fā)揮作用，這個(gè)項(xiàng)目涉及到許多技術(shù)和算法，包括視覺(jué)識(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和四核ARM Cortex-A57處理器，性能強(qiáng)大。Jetson Nano支持多種人工智能框架和工具，如TensorFlow、PyTorch、Caffe和MXNet等。此外，Jetson Nano還具有多種輸入輸出接口，如HDMI、USB、GPIO等，方便開(kāi)發(fā)人員進(jìn)行硬件連接和控制。

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

開(kāi)發(fā)過(guò)程

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

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

在我開(kāi)始開(kāi)發(fā)之前，我首先進(jìn)行了一些調(diào)研和實(shí)驗(yàn)。我使用了一個(gè)相機(jī)來(lái)捕捉物體的圖像，并使用OpenCV庫(kù)來(lái)識(shí)別和跟蹤Aruco碼。嘗試過(guò)多種的方法，物體的識(shí)別需要讓機(jī)器進(jìn)行學(xué)習(xí)，我們要識(shí)別的目標(biāo)，這樣會(huì)增加項(xiàng)目開(kāi)發(fā)的時(shí)間，最后決定用aruco碼來(lái)進(jìn)行識(shí)別，這樣可以快速捕捉到aruco碼，進(jìn)行下一步開(kāi)發(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.pi
                        mc.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è)過(guò)程涉及到將機(jī)器人末端執(zhí)行器與相機(jī)進(jìn)行配對(duì)，然后通過(guò)捕捉執(zhí)行器在相機(jī)視野中的位置和姿態(tài)來(lái)確定它在機(jī)器人基座坐標(biāo)系中的位置和姿態(tài)。

手眼標(biāo)定通常涉及到在機(jī)器人末端執(zhí)行器和相機(jī)之間進(jìn)行一系列的運(yùn)動(dòng)，以便收集足夠的數(shù)據(jù)來(lái)計(jì)算出它們之間的變換矩陣。這個(gè)變換矩陣描述了機(jī)器人末端執(zhí)行器相對(duì)于相機(jī)的位置和姿態(tài)，從而可以用來(lá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)下的圖像信息。通過(guò)分析這些圖像，可以計(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庫(kù)來(lái)進(jìn)行對(duì)機(jī)械臂的控制。

#用于進(jìn)行視覺(jué)追蹤并計(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)系，

讓我們來(lái)看下效果如何。

總結(jié)：

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

審核編輯 黃宇