前言

我們在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

最后整理一下項目的邏輯關系，

讓我們來看下效果如何。

總結：

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

審核編輯 黃宇