【仿真】UR机器人手眼标定与实时视觉追踪（单目）

这段代码实现了一个机器人视觉引导系统,主要功能包括:

1. 连接仿真环境,控制UR机器人。

2. 相机标定:

• 使用棋盘格图案进行相机内参标定

• 通过移动机器人采集多组图像

• 使用calibrateCamera函数计算相机内参

手眼标定:

• 采集机器人末端位姿和对应的棋盘格图像

• 使用calibrateHandEye函数计算相机相对机器人末端的变换矩阵

视觉引导:

• 实时检测棋盘格位置

• 计算棋盘格相对相机的位姿

• 结合手眼标定结果,计算机器人末端目标位置

• 控制机器人移动到目标位置

交互式跟踪:

• 在仿真器中实时检测棋盘格

• 控制机器人末端跟随棋盘格移动

主要使用了OpenCV进行图像处理和位姿估计,scipy进行旋转矩阵计算,以及仿真环境的API进行机器人控制。

这个系统展示了如何将机器视觉与机器人控制结合,实现基于视觉的机器人引导。它涵盖了相机标定、手眼标定、视觉检测和机器人控制等机器人视觉系统的核心组件。

仿真脚本分析

这些脚本包含了 机器人控制、相机设置以及深度图像处理的功能。每个脚本都有其特定的用途，如机器人运动控制、视觉处理和深度图像处理。

1. 机械臂线程脚本lua

移动回调函数：用于设置机器人关节的位置。-- 如果关节是动态启用的，设置目标位置 -- 否则，直接设置关节位置。

移动到指定配置的函数：-- 获取当前关节位置-- 使用 sim.moveToConfig 函数进行运动规划。

线程主函数：-- 获取机器人的 6 个关节句柄，-- 设置运动参数（速度、加速度、跃度）；-- 将角度值转换为弧度。

2. 机械臂自定义脚本python

仿真开始时初始化：导入必要的模块， 在 IK 插件环境中基于场景中机器人的运动学构建运动链和两个 IK 组（无阻尼和有阻尼）{获取场景中的对象[基座 末端 目标]，创建IK环境，创建无阻尼 IK 组，创建有阻尼 IK 组}

动作：IK求解器将机器人关节的运动与物理世界仿真同步。

3. 相机脚本lua

初始化：-- 获取深度相机对象,-- 添加深度视图,-- 获取彩色相机对象, -- 添加彩色视图.

4. 深度相机脚本

视觉传感器的回调函数，当有图像数据时被调用：-- 初始化返回值，-- 默认不触发其他处理，-- 初始化返回的数据包为空。-- 将深度图转换为工作图像，-- 对工作图像进行强度缩放{在保持图像最亮和最暗点不变的情况下，对图像进行均匀的强度调整，且这种调整不依赖于像素的深度信息。这通常用于简单的图像亮度标准化，以便后续的图像处理步骤（如特征检测、边缘识别等）能在更统一的条件下进行。}，- 将处理后的工作图像应用回视觉传感器图像。

手眼标定程序

函数

1. 连接到仿真器

创建远程API实例，获取仿真器sim接口,在加载场景前停止当前模拟（前提已打开仿真器），加载场景，开始模拟。返回客户端和sim实例。

2. 获取对象句柄

获取机器人基座句柄，获取机器人末端执行器句柄，获取摄像头的彩色图句柄，获取深度图句柄，获取机器人目标点句柄。返回以上句柄。

3. 将摄像头的彩色图转换为opencv的图像格式

从摄像头获取图像（返回图像和分辨率），将获取的图像数据转换为numpy数组（分辨率x,分辨率y，3），将图像上下翻转以符合OpenCV的图像表示方式

4. 设置对象的位姿矩阵

5. 获取对象的位姿矩阵

创建一个4x4的单位矩阵，获取并设置对象的位姿矩阵，返回位姿矩阵。

6. 计算位姿误差

计算平移误差，计算转转矩阵之间的误差，将旋转误差转换为角度，返回旋转误差和平移误差。

7. 等待机器人移动到位（目标点，获取位姿函数，获取线速度和角速度函数）

记录开始时间，初始化旋转误差和平移误差为无穷大。进入循环：{计算已经过去的事件，检查是否超时如果超时返回失败和上一误差记录，等待一段时间，获取当前位姿，获取线速度和角速度，计算线速度的平均值，计算角速度的平均值，计算当前位姿和目标位姿之间的误差。如果旋转误差平移误差和速度都小于预设的阈值，则返回成功}

8. 设置目标位姿并等待移动到位

设置目标位姿，等待机器人移动到目标位姿并返回结果。

主程序

设置numpy打印选项，保留小数点后三位，不使用科学计数法

指定场景文件路径

设置 等待间隔时间、超时时间、接受的旋转误差范围、接受的平移误差范围、接受的停止速度

---------- 连接仿真器

连接到仿真器

获取句柄

---------- 设置视觉参数，例如停止条件、棋盘格大小等，并且执行了棋盘格角点的检测和显示结果

设置寻找角点的终止条件

设置相机校正的标志

设置棋盘格大小为10x7  

设置棋盘格大小 0.025米

创建存放对象点的数组 10x7x3

设置对象点的x和y坐标 70x2，z为0

---------- 获取相机的彩色图像，并转换为灰度图，然后检测棋盘格角点，并显示检测结果

获取彩色图像,获取图像的高度和宽度，将彩色图转换为灰度图，检测棋盘格角点，优化角点的位置。显示检测到的棋盘格角点。

---------- 获取机器人末端执行器和摄像头当前的位姿，并且执行了位姿的调整与更新，展示了调整后的图像

获取机器人末端执行器当前的位姿，打印当前位姿，定义位姿的调整参数并进行位姿调整{定义平移调整参数 z移动负值，绕x旋转小角度}，创建平移矩阵，计算旋转矩阵，更新位姿变换矩阵并计算末端目标位姿，设置机器人的目标位姿并等待直到达到目标位姿。

获取BGR图片反转RGB并显示

重置机器人到初始位姿，再次获取BGR图片翻转为RGB并显示

---------- 生成的变换矩阵

定义变换范围：x轴、y轴、z轴平移范围、绕x、y轴旋转的角度范围，z轴设置不旋转。

定义存储生成的矩阵列表，遍历平移范围和角度范围所有的可能组合{ 定义平移4X4矩阵，定义旋转4X4矩阵，根据欧拉角设置旋转矩阵的R[:3,:3]，合并旋转和平移矩阵，添加到变换矩阵列表}

---------- 每个位姿变换矩阵尝试获取相应的图像并进行处理

初始化用于相机标定的列表：存储图像中检测到的角点，存储对象的世界坐标系中的点，存储末端执行器的位姿，存储捕获的图像。

遍历每个位姿变换矩阵尝试获取相应图像并进行处理：

计算目标变换，设置模拟环境中的位姿并等待运动完成，如果没有返回值继续执行下一位姿。 获取当前摄像机BGR图像，获取末端执行器的位姿，将图像转换为灰度图，检测棋盘角点（如果没返回值继续下一位姿），打印位移和旋转误差，精确化角点位置，在图像中绘制检测到的角点并将BGR转换为RGB以显示。储存数据（图像中检测到的角点加入列表，对象在世界坐标系中的点加入列表，末端执行器位姿加入列表，BGR图像加入列表）

位移和旋转误差及图像

 0.0 6.4243363e-06

0.0 3.3219899e-06

…… 很多图（略）

0.0 1.6598583e-06

---------- 根据收集的数据进行相机标定

输出相机的内参矩阵 

输出相机的畸变系数

---------- 初始化用于手眼标定的列表

对每个标定点，计算解算体位姿，并准备进行手眼标定：

使用solvePnP求解相机位姿，如果求解成功（绘制并显示坐标轴，计算目标相对于相机的位姿变换矩阵，将这次的位姿变换加入手眼标定的列表中）

列表包括：

机器人手爪（或末端执行器）相对于机器人基座的旋转矩阵的集合。每个矩阵代表在不同的手爪位置或姿态下，手爪相对于基座的旋转关系。

机器人手爪相对于机器人基座的平移向量的集合。与旋转矩阵集合对应，每个向量代表在不同的手爪位置或姿态下，手爪相对于基座的平移关系。

目标物体（通常是一个已知图案，如棋盘格）相对于相机的旋转矩阵的集合。每个矩阵代表在不同的相机位置或姿态下，目标物体相对于相机的旋转关系。

目标物体相对于相机的平移向量的集合。与旋转矩阵集合对应，每个向量代表在不同的相机位置或姿态下，目标物体相对于相机的平移关系。

……

（很多图）略

利用前面求得的各个位姿数据，通过cv2.calibrateHandEye函数计算工具末端点（Tool Center Point, TCP）在相机标系中的位姿。cv2.calibrateHandEye函数通过分析机器人手爪与标定物在机器人基座和相机视野中的多个不同位置和姿态下的相对变换，然后求解得到一个从相机坐标系到机器人手爪坐标系（或反向）的稳定且准确的空间变换关系，使得在执行精确任务（如精准抓取、目标跟踪等）时能够有效地利用视觉信息。

将位姿从OpenGL坐标系转换到OpenCV坐标系。定义从opengl坐标系到opencv坐标系的变换矩阵，从仿真场景中获取并输出从机械臂末端到相机的位姿变换矩阵。

------------初始化另一系列位姿变换矩阵用于进一步的实验

定义平移和旋转范围，x,y的平移，z平移设定为0，绕x、y、z轴的旋转角度范围，初始化变换矩阵列表。根据范围生成所有可能得变换矩阵（创建平移矩阵，初始化旋转矩阵为单位矩阵，设置旋转矩阵的前三行三列为由欧拉角zyx构成的旋转矩阵，计算最终的变换矩阵，实现旋转后平移的效果。将变换矩阵添加到列表中）

定义两个特定的变换矩阵：定义棋盘坐标系向棋盘中心平移的变换矩阵、定义向后（z负向）平移的变换矩阵

获取初始目标框架位置：获取棋盘格基座句柄，获取棋盘格基坐标系相对机器人基座的位姿记为目标框架初始位姿。重置机器人末端执行器和棋盘初始位姿。

-----------对每一个位姿变换矩阵，尝试移动棋盘并获取图像

尝试移动棋盘到新的位姿，如果设置失败则跳过此次循环。获取相机图像，转换为灰度图，寻找棋盘格角点，如果未找到角点跳过此次循环。打印旋转和平移误差，对角点进行亚像素级精细化，使用solvePnP求解相机位姿，如果失败则跳过此次循环。 构造世界到相机的变换矩阵（初始化4x4齐次矩阵，设置旋转部分，设置平移部分，进一步调整至中心位置），可视化并显示结果。

计算新的末端执行器位置（从场景获取末端执行器在机器人基座标系下的位姿X相机在末端执行器下的位姿X棋盘格中心坐标系在相机坐标系下的位姿X向后移动的平移矩阵X相机坐标系在棋盘格中心坐标系下的位姿）

移动机器人末端执行器到新的计算位置。实现实时跟踪

获取并显示移动后的图像

重置机器人臂和棋盘到初始位姿

----------- 交互式跟踪棋盘仿真器中棋盘的动作

获取摄像机当前的BGR图像，将BGR图像转换为灰度图像，寻找棋盘格角点，如果没有找到角点，继续下一次循环，对角点坐标进行精细化，使用solvePnP求解相机位姿，如果求解失败，继续下一次循环，根据求解结果构造从世界坐标系到相机坐标系的变换矩阵，计算棋盘中心到棋盘原点的位姿变换，获取机械臂末端相对于基座的当前位姿，计算新的机器人臂位姿，使摄像头对准棋盘中心，设置机器人臂到新的位姿并等待移动完成。

交互式实时追踪棋盘格

实现了在仿真环境中对棋盘进行实时追踪的功能。通过不断捕获摄像头的图像，并识别出棋盘格角点的位置，进而计算出棋盘相对于摄像头的位姿变换。此后，根据预先通过手眼标定计算得到的机器人末端执行器到相机的变换关系，以及棋盘中心相对于世界坐标系的位姿变换，计算出机器人末端执行器应该达到的新位姿，并通过仿真器的API命令机器人移动到这个新位姿。通过这样的过程，机器人末端的摄像头能实时调整位置以紧随棋盘移动，实现了对棋盘的动态追踪。

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

The End