自动化机械臂视觉跟踪和手眼校准

本文重点介绍了一款专为机器人教育而设计的具有动态跟踪功能的创客友好型机械臂

硬件组件

M5Stack ESP32 Basic Core IoT Development Kit

Raspberry Pi 4 Model B

Espressif ESP32S

Elephant Robotics myCobot 320 m5

引言

今天文章的重点是使用myCobot 320机械臂重新创建一个视觉跟踪案例，该案例由Elephant Robotics作为官方解决方案提供，使用户能够快速上手并逐步跟进，以查看如何重现结果并识别任何潜在问题。

设备

myCobot 320 M5Stack

myCobot 320 M5Stack是six-degree-of-freedom的机械臂，工作半径为350mm，末端执行器最大有效载荷为1kg，支持各种主流编程语言和操作系统，本文主要使用Python来控制机械臂。

从图像中我们可以看到，该装置由一个 myCobot 机械臂和一个用于捕获图像数据的相机组成。机械臂的具体参数如下。

照相机

你不需要使用和我一样的相机；关键是它可以安装在机械臂的末端执行器上，并且可以通过USB电缆获取图像数据。对于这个设置，我使用了myCobot Pro Camera Flange，这是大象机器人专门为myCobot改装的末端执行器相机。

环境设置

●操作系统：Windows 10

●编程语言：Python

●IDE：PyCharm

●库：Numpy、OpenCV、STag、pymycobot、json、time（推荐这些库的最新版本）

知识介绍

openCV

OpenCV（开源计算机视觉库）是一个用于计算机视觉和机器学习的开源软件库，广泛应用于图像处理、视频分析和目标检测。OpenCV为开发人员提供了一套丰富的工具，用于处理图像数据和实现复杂的视觉算法。在机械臂的背景下，OpenCV可用于视觉跟踪，其中相机实时捕捉目标，分析和提取目标的位置和运动轨迹。然后机械臂根据这些信息调整其运动，实现精确的对象抓取和操纵。这种视觉跟踪技术广泛应用于自动化、工业机器人和智能制造。

Stag

STag标记是一种二维条码系统，广泛用于机器视觉中的标记检测和空间定位。这些标记由黑白图案组成，通常呈方形，中心有独特的二进制图案，使它们能够被计算机视觉系统快速准确地识别。

手眼校准-手眼配置（Hand-Eye Calibration -- Eye-in-Hand Configuration）

手眼校准涉及确定相机（眼睛）和机械臂末端执行器（手）之间的精确空间和方向关系。在这种情况下，我们主要讨论的是"手眼"场景，它指的是本项目中遇到的情况。手眼校准对于建立相机相对于机械臂末端执行器的定位方式至关重要。在手眼配置中，相机安装在机械臂末端执行器上，因此视野和相机角度随着机械臂的移动而变化。目标是计算相机坐标系和机械臂末端执行器坐标系之间的变换。这使得机器人能够通过相机感知周围环境，并执行目标跟踪和精确抓取等任务。



手眼校准步骤

1.相机姿态变化：在手眼配置中，相机的视角随着机械臂的每次移动而变化。通过移动机械臂，可以捕捉校准对象的多个视点，从而产生相机不同姿态的数据。

2.数据收集：机械臂被移动到几个不同的位置，每次，它捕获校准板或特定物体的图像。末端执行器的姿态（由编码器提供）和物体的姿态（通过图像处理计算）被记录下来。

3、摄像机与末端执行器关系的求解：利用最小二乘等算法，计算摄像机与末端执行器之间的变换矩阵，建立它们之间的坐标变换关系。

用代码实现

该过程主要分为两部分：校准过程和跟踪运动模块。

校准过程

1.坐标转换

在手眼标定过程中，涉及不同坐标系之间的变换。关键坐标系如下：

●世界帧（W）：通常固定在环境中的参考帧。

●底架（B）：固定在机械臂底座的框架，用于表示手臂的姿势。

●末端执行器框架（E）：当相机安装在机械臂末端执行器上时，该框架代表手臂末端执行器的姿势。

●相机框架（C）：固定在相机上的框架，用于描述相机看到的物体的姿势。

在手眼标定中，目标是解决摄像机框架和机械臂末端执行器框架之间的变换矩阵。这使得摄像机检测到的物体的姿态被转换成机械臂末端执行器坐标，从而实现对目标物体的精确操作。

def eyes_in_hand_calculate(self, pose, tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr):

        tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr = map(np.array, [tbe1, Mc1, tbe2, Mc2, tbe3, Mc3, Mr])
        # Convert pose from degrees to radians
        euler = np.array(pose) * np.pi / 180
        Rbe = self.CvtEulerAngleToRotationMatrix(euler)
        Reb = Rbe.T
        
        A = np.hstack([(Mc2 - Mc1).reshape(-1, 1), 
                    (Mc3 - Mc1).reshape(-1, 1), 
                    (Mc3 - Mc2).reshape(-1, 1)])
        
        b = Reb @ np.hstack([(tbe1 - tbe2).reshape(-1, 1), 
                            (tbe1 - tbe3).reshape(-1, 1), 
                            (tbe2 - tbe3).reshape(-1, 1)])
        
        U, S, Vt = svd(A @ b.T)
        Rce = Vt.T @ U.T
        
        tce = Reb @ (Mr - (1/3)*(tbe1 + tbe2 + tbe3) - (1/3)*(Rbe @ Rce @ (Mc1 + Mc2 + Mc3)))
        
        eyes_in_hand_matrix = np.vstack([np.hstack([Rce, tce.reshape(-1, 1)]), np.array([0, 0, 0, 1])])
        
        return eyes_in_hand_matrix

2.数据收集

通过将机械臂移动到不同的位置，收集有关机械臂末端执行器的各种位置和相机观察的数据。

在您的代码中，机械臂的姿势是通过调用"ml.get_coords（）"方法获得的，而相机的位置数据是通过"stag_identify（）"函数收集的，该函数识别标记对象。

def reg_get(self, ml):
    for i in range(30):
        Mc_all = self.stag_identify()  
    tbe_all = ml.get_coords()  
    ...
    return Mc, tbe

3.坐标变换矩阵

根据每个位置的数据，可以导出两种转换：

●Ai是机械臂末端执行器在不同位置的变换矩阵，代表末端执行器的运动。

●Bi是相机在相机坐标系中观察到的物体的变换矩阵，代表相机的运动。

这些变换矩阵通过视觉系统和机械臂系统（使用'get_coords'）获得。

4.求解校准矩阵

根据校准模型：

●Ai代表机械臂末端执行器的运动（从世界框架到末端执行器框架）。

●Bi表示相机的运动（在相机坐标系中看到的物体的运动）。

●Xce是待求解的手眼校准矩阵，代表相机和机械臂末端执行器之间的刚体变换。

通过收集Ai和Bi的多个位置，可以使用最小二乘法来求解Xce。虽然代码中没有显示这部分逻辑，但通常可以使用SVD分解等方法来解决。

保存收集到的数据并计算结果后，就可以实现后续的跟踪功能。

[[0.9825202432037423, 0.03775722308035847, 0.1822864882543945, -21.50838594386444], [-0.04022441808787263, 0.9991420672993772, 0.009855229181470597, -0.6545263884052905], [-0.1817579926285262, -0.017015330087522124, 0.9831960692850951, 59.71321654600654], [0.0, 0.0, 0.0, 1.0]]

5. 视觉跟踪

手眼校准的输出是一个刚体变换矩阵，用于描述相机和机械臂的末端执行器之间的空间关系。该矩阵构成了机械臂视觉控制和操作的基础。利用这个矩阵，机械臂可以将视觉系统感知到的物体位置转换为自己的坐标系。前面提到的 STag 代码是使用 OpenCV 算法识别的。

def stag_robot_identify(self, ml):
        marker_pos_pack = self.stag_identify()
        target_coords = ml.get_coords() 
        while (target_coords is None):
            target_coords = ml.get_coords()
        # print("current_coords", target_coords)
        cur_coords = np.array(target_coords.copy())
        cur_coords[-3:] *= (np.pi / 180)  
        fact_bcl = self.Eyes_in_hand(cur_coords, marker_pos_pack, self.EyesInHand_matrix) 
        
        for i in range(3):
            target_coords[i] = fact_bcl[i]
        
        return target_coords

基于从识别出的代码中返回的坐标，机械臂相应地移动，沿末端执行器的 XYZ 轴进行运动，以实现跟踪目标。

def vision_trace_loop(self, ml):
    mc.set_fresh_mode(1)
    time.sleep(1)

    ml.send_angles(self.origin_mycbot_horizontal, 50) 
    self.wait() 
    time.sleep(1)

    origin = ml.get_coords()  
        while 1:
        target_coords = self.stag_robot_identify(ml) 
        target_coords[0] -= 300  
        self.coord_limit(target_coords)  
        print(target_coords)
        for i in range(3):
            target_coords[i+3] = origin[i+3]  
        ml.send_coords(target_coords, 30)

总结

总体来说，在运行这段代码的时候，可能还是会出现一些小插曲，有些功能没有完全解释清楚。使用过许多手眼校准方法后，我发现这是更直接的自动校准方法之一，尽管它缺乏一些精度，但可以通过优化来改进。总的来说，这个案例值得探索，特别是对于那些对机械臂和视觉系统有一定了解的人来说！