【仿真】UR机器人相机标定、立体标定、手眼标定、视觉追踪（双目）

实现在CoppeliaSim环境中进行手眼标定和目标追踪的一个例子。它主要涉及到机器人、机器视觉和控制算法的编程，使用了Python语言。接下来对该代码的主要类和方法进行解析：

1. 导入相关库

• 用于与CoppeliaSim模拟器通过ZeroMQ接口通信。

• 包含Rotation类，用于执行各种旋转和方向变换操作。

• OpenCV库，用于执行各种图像处理和计算机视觉任务。

• 用于执行各种数学和矩阵运算。

• 用于进行图像和数据的可视化。

• 用于创建和操作迭代对象的库。

• 用于实现与时间相关的功能。

2. 定义全局变量

• 分别用于定义场景路径、等待间隔、超时时间、接受的旋转误差、接受的平移误差、接受的停止速度阈值。

3. 定义主要的方法和步骤

• 连接到CoppeliaSim模拟器。

• 获取模拟器中对象的句柄。

•  从摄像头获取图像，并将其转换为OpenCV的BGR格式。

• 设置从一个坐标帧到另一个坐标帧的变换矩阵。

• 获取从一个坐标帧到另一个坐标帧的变换矩阵。

• 计算实际位姿与目标位姿之间的误差。

• 等待机器人移动到目标位姿，直到误差足够小或超时。

• 设置目标位姿并等待机器人移动完成。

4. 实施手眼标定和目标追踪

• 使用二维码检测来定位目标，然后进行相机标定和手眼标定计算。

• 计算机器人末端和摄像头之间的相对位姿。

• 实现了一系列的图像获取和处理，利用OpenCV库进行棋盘角点的提取。

• 通过实验获取数据，计算相机内部参数和两个相机之间的位姿。

• 实现了基于不同位姿下的图像集合进行手眼标定。

• 完成了在模拟器中交互式地跟踪目标的过程。

5. 保存与加载数据

• 使用方法来压缩和保存相关数据，以便后续的分析和使用。

本代码的特点在于它结合了机器人控制、图像处理、手眼标定和交互式目标追踪等多个领域的技术，是一套完整的解决方案。

视频演示

---

源码解析

函数

1. 通过ZeroMQ远程API连接到CoppeliaSim：创建客户端实例，获取CoppeliaSim API的实例，停止当前模拟，加载模拟场景，开始模拟，返回客户端和sim实例

2. 获取对象句柄： 获取机器人基座的句柄，获取机器人末端执行器的句柄，获取第一个相机的句柄，获取第二个相机的句柄，获取目标球的句柄

3. 定义从CoppeliaSim视觉传感器获取图像并转换为OpenCV格式的BGR图像的函数：从指定相机获取图像数据，将获取的图像数据转换为NumPy数组并重塑为正确的形状，将图像在垂直方向上翻转，以符合OpenCV的图像显示方式

4. 定义设置给定对象在某个参考框架下的位姿（位置和姿态）的函数：设置对象的变换矩阵，以定义其位置和方向

5. 定义获取给定对象在某个参考框架下的位姿的函数：初始化为4x4的单位矩阵，获取对象的变换矩阵并重塑，组成位姿矩阵

6. 定义计算两个位姿之间误差的函数:计算平移误差,计算旋转误差,将旋转误差转换为角度

7.定义等待机器人移动到目标位姿的函数:记录开始时间,初始化误差值为无穷大,计算已经经过的时间,如果超时则返回失败,等待一定时间,获取当前位姿,获取当前速度,计算线速度均值,计算角速度均值,计算误差,如果误差小于阈值且速度接近零，则返回成功

8. 定义设置目标位姿并等待机器人移动完成的函数:设置目标位姿,等待机器人移动到目标位姿并返回结果

主程序

########## 定义场景路径、等待间隔、超时时间以及接受的误差

设置NumPy打印选项 

定义场景路径 

定义等待时间间隔，单位秒   

定义超时时间，单位秒

接受的旋转误差阈值

接受的平移误差阈值

接受的停止速度阈值

连接到CoppeliaSim并开始模拟

获取必要的句柄

########### 设置图像的检测、摄像机标定参数，并检测棋盘格角点

初始化棋盘格检测所需的终止条件:设置寻找亚像素角点的精确度和最大迭代次数,设置相机标定时需要固定的参数

设置棋盘格的尺寸和单位长度:棋盘格的行和列，示例尺寸，根据实际需要调整,单位长度，以米为单位,初始化棋盘格的三维坐标,设置棋盘格上每个点的x，y坐标，z坐标默认为0

从第一个相机获取BGR图像

获取图像的高度和宽度

将图像从BGR转换为灰度图

寻找棋盘格角点

对角点坐标进行精确化

在图像上绘制并显示棋盘格角点

获取机器人末端执行器的原始位姿

定义平移和旋转参数

构建平移和旋转矩阵

设置末端执行器到目标位姿并等待动作完成

显示相机1获取的图像

将末端执行器复位到原始位姿

显示相机1获取的图像

显示相机2获取的图像

########### 构建多个位姿变换矩阵，以便在手眼标定实验中使用

定义平移和旋转的范围

根据定义的范围生成一系列变换矩阵

计算生成的变换矩阵数量

########### 使用生成的位姿变换矩阵收集用于手眼标定的数据

初始化相机1、2图像点和对象点的列表，用于相机标定

定义机器人末端执行器的位姿列表、相机1拍摄的图像列表、相机2拍摄的图像列表

遍历所有预设的变换矩阵：{计算目标位姿，移动机器人末端执行器到目标位姿，如果移动失败，则跳过此次循环。获取当前机器人末端执行器的位姿，从相机1获取图像，并找到棋盘格角点，显示找到的角点，从相机2获取图像，并找到棋盘格角点，显示找到的角点，将找到的角点和对应的对象点分别存入列表，将图像存入相应列表}

计算成功捕获棋盘格角点的图像数量

########### 对两个相机分别进行标定获取内参和畸变系数

分别对两个相机进行标定，获取相机内参矩阵和畸变系数

输出相机内参矩阵和畸变系数

########### 进行立体标定获取两相机之间的旋转和平移矩阵

进行立体标定，获取两个相机间的相对位姿关系（）

从仿真场景获取两个相机间的位姿关系

########### 初始化手眼标定所需的矩阵列表

初始化手眼标定所需的数据结构：{机器人夹持器到基座的旋转矩阵列表, 标定板到相机的旋转矩阵列表,机器人夹持器到基座的平移向量列表,标定板到相机的平移向量列表  }

############ 遍历采集的相机1每个图像点、对象点、末端执行器位姿以及图像

遍历每组图像点、对象点、末端执行器位姿以及图像:{ 使用solvePnP求解标定板相对于相机的位姿,在图像中绘制坐标轴并显示，构造标定板到相机的变换矩阵，将求解得到的旋转矩阵和平移向量添加到列表中 }

使用cv2.calibrateHandEye进行相机1手眼标定，求解夹持器到相机1的变换矩阵

输出夹持器到相机1的变换矩阵

定义OpenGL到OpenCV的坐标系统转换矩阵

从场景获取末端执行器到相机1的位姿，并进行坐标系统转换

根据立体标定结果计算夹持器到第二个相机的变换矩阵

从场景获取末端执行器到相机2的位姿，并进行opengl到opencv坐标系统转换

将所有标定数据保存到压缩文件中，以备后用

########### 为机器人对目标进行跟踪做准备

定义一系列位姿变换，用于测试不同的目标位姿

定于存储所有生成的变换矩阵列表

根据上述定义的范围生成一系列变换矩阵

向棋盘格中心的位姿变换

向后（z负向）的位姿变换

棋盘格基座的获取

获取并输出棋盘格的原始位姿

将机器人末端执行器和棋盘格复位到原始位姿

将棋盘格复位到原始位姿

########### 在相机1视角下，机器人对目标进行跟踪

初始化变换矩阵和图像列表，用于存储相机1当前位姿、下一位姿和各位姿对应的图像：存储当前机器人末端执行器的位姿、存储计算得到的下一步的机器人末端执行器位姿、存储相机1拍摄的图像列表

遍历所有预先定义的位姿变换矩阵【实现了在相机1视角下对棋盘格位姿跟踪和视觉处理的自动化实验，为机器人视觉系统的测试和校准提供了重要的数据支持】：{ 计算目标物体（棋盘格）的新位姿，移动棋盘格到新位姿、如果移动失败，则跳过此次循环，获取相机1图像并转换为灰度图像，寻找棋盘格角点，打印旋转和平移误差。精细化角点位置，使用solvePnP求解物体位姿，计算棋盘基座到相机的变换矩阵，应用向目标中心的位姿变换得到棋盘中心到相机的变换矩阵，可视化并显示旋转轴。获取机器人末端执行器当前位姿，计算新的末端执行器位姿，以便于下一次移动，添加当前末端执行器位姿到列表，添加计算得到的下一步位姿到列表，添加图像到列表，移动机器人末端执行器到新的位姿，拍摄并显示移动后的图像}

将机器人末端执行器和棋盘格复位到原始位姿

########### 在相机2视角下，机器人对目标进行跟踪

初始化变换矩阵和图像列表，用于存储相机2当前位姿、下一位姿和各位姿对应的图像：存储当前机器人末端执行器的位姿、存储计算得到的下一步的机器人末端执行器位姿、存储相机2拍摄的图像列表

遍历所有预先定义的位姿变换矩阵【实现了在相机2视角下对机器人位姿跟踪和视觉处理的自动化实验，为机器人视觉系统的测试和校准提供了重要的数据支持】：{计算目标物体（棋盘格）的新位姿、移动棋盘格到新位姿、如果移动失败，则跳过此次循环、获取相机2的图像、转换为灰度图像、寻找棋盘格角点、打印旋转和平移误差、精细化角点位置、使用solvePnP求解物体位姿、计算物体到相机的变换矩阵、应用向目标中心的位姿变换、可视化并显示旋转轴、获取机器人末端执行器当前位姿、计算新的末端执行器位姿，以便于下一次移动、添加当前末端执行器位姿到列表、添加计算得到的下一步位姿到列表、添加图像到列表、移动机器人末端执行器到新的位姿、拍摄并显示移动后的图像}

将收集的位姿和图像数据存储为压缩文件，方便后续使用

将机器人末端执行器和棋盘格复位到原始位姿

########### 在相机2视角下，机器人对目标进行交互式跟踪模拟

交互式跟踪模拟：{从相机2获取BGR图像、将图像转换为灰度图、

检测棋盘格角点、如果没有找到角点，继续下一次循环、通过子像素角点检测精细化角点位置、使用solvePnP算法求解物体位姿、如果solvePnP求解失败，继续下一次循环、初始化物体到相机的变换矩阵、根据旋转向量更新旋转矩阵、更新平移向量、应用向目标中心的位姿变换、获取当前机器人末端执行器的位姿、根据得到的相机位姿计算新的机器人末端执行器位姿、将机器人末端执行器移动到新的位姿}

作者陈晓永：智能装备专业高级工程师，软件工程师。机器人自动化产线仿真动画制作 

The End