使用myCobot 280 Jeston Nano进行物体精确识别追踪

前言

我们在YouTube上看到有人使用机械臂实现物体跟踪功能的视频时，深受启发，对这个项目产生了浓厚的兴趣，并决定独立开发一个类似的程序。

我们的目标是开发一个能够准确识别和跟踪物体的机械臂系统，以便在实际应用中发挥作用，这个项目涉及到许多技术和算法，包括视觉识别、手眼协同和机械臂控制等方面。

机械臂的介绍 mycobot280-JetsonNano

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

这是一款大象机器人公司生产的小六轴机械臂，以JetsonNano为微处理器，ESP32为辅助控制，UR协作形结构。myCobot280 JetsonNano，本体重量1030g， 负载250g，工作半径280mm，设计紧凑便携，小巧但功能强大，操作简单，能与人协同、安全工作。

Jetson Nano

Jetson Nano是英伟达推出的一款嵌入式人工智能计算机，它采用了NVIDIA Maxwell GPU和四核ARM Cortex-A57处理器，性能强大。Jetson Nano支持多种人工智能框架和工具，如TensorFlow、PyTorch、Caffe和MXNet等。此外，Jetson Nano还具有多种输入输出接口，如HDMI、USB、GPIO等，方便开发人员进行硬件连接和控制。

由于Jetson Nano具有强大的计算性能和专门为人工智能开发设计的特点,支持多种深度学习框架，如TensorFlow、PyTorch和Caffe等，可以更方便地进行人工智能应用开发,它成为了开发人员进行人工智能应用开发的理想平台之一。

开发过程

下图是项目的开发流程图

相机捕捉目标

在我开始开发之前，我首先进行了一些调研和实验。我使用了一个相机来捕捉物体的图像，并使用OpenCV库来识别和跟踪Aruco码。尝试过多种的方法，物体的识别需要让机器进行学习，我们要识别的目标，这样会增加项目开发的时间，最后决定用aruco码来进行识别，这样可以快速捕捉到aruco码，进行下一步开发。

Aruco码

下面是实现的代码：

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)

手眼标定

手眼标定是指在机器人领域中，确定机器人末端执行器（例如机械手臂）相对于机器人基座坐标系的位置和姿态。这个过程涉及到将机器人末端执行器与相机进行配对，然后通过捕捉执行器在相机视野中的位置和姿态来确定它在机器人基座坐标系中的位置和姿态。

手眼标定通常涉及到在机器人末端执行器和相机之间进行一系列的运动，以便收集足够的数据来计算出它们之间的变换矩阵。这个变换矩阵描述了机器人末端执行器相对于相机的位置和姿态，从而可以用来控制机器人的运动，使其能够准确地执行所需的任务。

在"eye-to-hand"手眼标定中，相机被视为一个不动的观察者（"eye"），而机器人末端执行器则被视为在相机视野中移动的物体（"hand"）。机器人末端执行器在相机周围移动时，会收集到一系列的图像，这些图像包含了机器人末端执行器在不同位置和姿态下的图像信息。通过分析这些图像，可以计算出机器人末端执行器相对于相机的位置和姿态，从而完成手眼标定。

下面是处理坐标之间转换数据的代码

#函数用于计算相机间的相似性
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
#函数用于计算相似性的变化率
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

#函数用于将旋转矩阵转换为欧拉角
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

机械臂控制

在这之后就是物体检测和机械臂的控制，将识别到的物体的坐标转换成机械臂的运动指令，这里用到的是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

#根据相机坐标计算目标位置
    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

最后整理一下项目的逻辑关系，

让我们来看下效果如何。

总结：

在调试的过程中，我们发现跟踪的效果并不是非常流畅和灵敏。我们通过控制检测周期来调整流畅性，但是需要缓慢移动被跟踪的物体目标，才能达到更好的效果。仍然还有一些不足的地方，在相机固定的情况下，机械臂的本体可能会遮挡相机的视野，导致没有办法进行下一步跟踪，想到的解决方案是相机换个位置不被遮挡的位置（坐标换算那些都得重新计算）。如果你有更好的想法欢迎跟我们沟通！感谢你的耐心观看。

审核编辑 黄宇