基于6D光笔的工业机器人高精度示教方法

High-precision Industrial Robot Teaching Method Based on 6D Light Pens

摘要

针对目前工业机器人视觉示教方法存在精度较低、对光照敏感和示教范围易受限等问题，提出一种基于6D光笔的工业机器人高精度示教方法。光笔设有三个红外发光标记球，通过固定于机器人末端的红外双目相机捕捉光笔标记球即可获得其3D位置坐标和3D姿态信息。采取了两种措施提高6D光笔的视觉示教精度，一是提出一种基于红外发光标记球的手眼标定方法，有效提高了相机坐标系到机器人工具坐标系的转换精度；二是提出了一种相机视野自适应跟踪方法，使机器人能够通过固定在末端的相机自动跟踪光笔的位置，从而保证光笔标记球始终处于相机视野中心区域，有效提高了示教精度。最后将捕捉到的示教轨迹转换到机器人基坐标系下实现轨迹的编程跟踪。实验结果表明，利用该光笔得到的单点最大误差为0.63 mm，姿态最大误差为2.1432°，示教轨迹最大误差为0.73 mm，所提出的示教方法能够实现机器人高精度、高效率示教。

Abstract

Aiming at the problems of poor precision， sensitivity to light and restricted teaching range in current industrial robot vision teaching methods， a high-precision industrial robot teaching method was proposed based on a 6D light pen. The light pen was equipped with three infrared-emitting marker balls， and infrared binocular camera mounted on the robot's end-effector captured the marker balls to obtain their 3D position coordinates and 3D pose information. Two measures were implemented to enhance the vision teaching precision of the 6D light pens. Firstly， a hand-eye calibration method was proposed based on infrared light-emitting marker balls， which significantly improved the transformation precision from the camera coordinate system to the robot tool coordinate system. Secondly， an adaptive camera tracking method was proposed， allowing the robot to automatically track the position of the light pen through the camera mounted on the end-effector， ensuring that the marker balls remain centered in the camera's field of view， thus effectively improving the teaching precision. Finally， the captured teaching trajectory was transformed into the robot base coordinate system to achieve trajectory programming and tracking. Experimental results show that the maximum single-point error of the light pens is as 0.63 mm， the maximum pose error is as 2.1432°， and the maximum trajectory error is as 0.73 mm. The proposed teaching method may achieve high-precision and high-efficiency teaching for robots.

Graphical abstract

关键词

机器人示教 / 6D光笔 / 红外双目相机 / 手眼标定

Key words

robot teaching / 6D light pen / infrared binocular camera / hand-eye calibration

Author summay

杨权印，男，2000年生，硕士研究生。研究方向为机器人加工技术。E-mail： [e-mail]。徐金亭^*（通信作者），男，1980年生，教授、博士研究生导师。研究方向为复杂曲面高性能数控加工理论与方法、机器人加工技术及装备。E-mail： [e-mail]。

梁继煌，男，2000年生，硕士研究生。研究方向为机器视觉、机器人控制技术。E-mail：[e-mail]

吴海彬^*（通信作者），男，1973年生，教授、博士研究生导师。研究方向为机器人控制技术、人机共融技术。E-mail： [e-mail]。

0 引言

工业机器人是人工智能和自动化制造领域的核心装备，它可以帮助人们完成高精度重复性作业任务，同时也可以在极端环境下进行工作，因此广泛应用于焊接、码垛、搬运等环境中^［1］。在机器人执行新的生产任务前，通常需要对机器人进行轨迹示教^［2］。但目前工业机器人传统的示教方法存在示教过程繁杂、技术难度高和受限于不同作业环境等问题^［3］。

为提高工业机器人的示教效率和灵活性，研究人员通过各种传感器（如视觉传感器、触觉传感器^［4］和位置跟踪器^［5-6］等）实现对机器人的快速示教。其中采用与视觉传感器结合的示教工具进行机器人示教的方法具有硬件成本低、操作便捷和精度较高的优点。倪自强等^［7］使用双目视觉系统拍摄带有棋盘格的示教工具完成机器人示教。应再恩等^［8］设计了一种贴有特征标记圆的示教工具，可以通过示教工具实现复杂的轨迹示教。上述方法使用的标记物均为平面标记物，容易受到光照强度和相机拍摄角度的影响。LE等^［9］通过相机拍摄示教工具上的特征标记物（贴有二维码的31面体）来计算示教工具的位姿信息，能够有效减小相机拍摄角度对示教精度的影响，但无法解决不同强度的可见光导致的测量误差。

不同于对可见光较为敏感的标记物图案，LIN等^［10］设计了一种带有发光点的示教笔，这种发光点设计虽有利于相机的快速捕捉，但难以抵抗较强可见光对标记物造成的影响。ZHANG等^［11］采用Kinect v2传感器与球杆型示教笔对机器人进行视觉示教，能够快速获得红外视野内特征物的深度信息，从而降低可见光对测量精度的影响。然而这种方法不仅因为传感器自身精度较低无法实现高精度轨迹获取，且由于传感器位置固定而限制了示教笔的有效示教范围。

为解决目前机器人视觉示教方法存在的不足，本文提出了一种基于6D光笔的机器人高精度示教方法，主要贡献如下：

1）针对当前视觉示教方法的精度易受可见光和相机拍摄角度影响的问题，设计了基于三个红外发光标记球的6D光笔（简称光笔）。采用红外发光标记球作为标记物可以避免不同强度的可见光对系统精度的影响，使得红外双目相机可以在不同角度下快速精确地捕捉到光笔的位姿信息，有效提高了示教精度。

2）由于红外相机难以通过拍摄彩色标定件进行手眼标定，故提出了一种基于红外发光标记球的手眼标定方法，可以简化手眼标定流程并提高标定精度。

3）提出了一种相机视野自适应跟踪方法，使机器人能够通过固定在末端的相机自动跟踪光笔的位置，从而确保光笔始终出现在相机视野中心区域，扩大了视觉示教的有效范围并提高了示教的精度。

1 基于6D光笔的机器人示教系统

基于6D光笔的机器人示教系统如图1所示，主要由工业机器人、红外双目相机、机器人末端工具和红外光笔组成。其中红外双目相机安装在机器人末端工具上方，可以跟随机器人一起移动。光笔由红外发光标记球和红宝石探针组成，红外发光标记球的内部均装有850 nm的红外发光LED，可以清晰地被红外双目相机捕捉。标记球通过红外相机在不同角度拍摄到的图片中均呈现为圆形，且边缘有明显的跳跃特征，灰度值变化大，经过简单的图像分析处理后便可以检测和定位。

本文提出的机器人视觉示教方法流程图见图2，主要分为前置标定阶段、视觉示教阶段和轨迹复现阶段。

1）前置标定阶段。示教开始前需要进行前置标定，通过光笔探针坐标系标定得到探针与标记球的坐标系转换关系，再基于红外发光标记球对红外双目相机进行手眼标定得到相机坐标系到工具坐标系的手眼标定矩阵。

2）视觉示教阶段。通过标定得到转换矩阵后即可进入视觉示教阶段。首先，相机开启拍摄功能，操作人员手持光笔控制末端探针沿着指定的轨迹移动，通过球心定位方法计算光笔的标记球在相机坐标系下的准确三维坐标，随后通过标定得到的转换矩阵计算探针在机器人基坐标系下的位姿信息，从而得到光笔探针的示教轨迹。在示教的过程中，同时启用相机视野自适应跟踪功能，实时计算光笔在相机视野中的位置，并判断光笔是否在相机视野中心区域内，若光笔偏离该区域，则机器人末端工具能够根据光笔的位置带动双目相机进行跟随移动，且保证光笔始终处于双目相机视野中心区域。

3）轨迹复现阶段。操作人员使用光笔完成示教后进入轨迹复现阶段，将得到的示教轨迹进行滤波处理后发送给机器人，机器人便可通过该轨迹进行复现。

2 双目视觉下的球心定位方法

为了快速准确地捕捉红外标记球并实时计算其球心在相机坐标系下的三维坐标，采用一种基于双目视觉的球心定位方法。该方法主要分为亚像素圆心提取算法和双目立体视觉测量方法。首先使用亚像素圆心提取算法快速拟合并计算标记球在图像中的二维圆心坐标，再通过双目立体视觉测量原理计算标记球的实际三维坐标。

如图3a所示，在相机拍摄的原始图像中，特征圆的信息不明显且存在干扰信息，因此需要对图像进行二值化操作以凸显特征圆信息，二值化后的图像如图3b所示，可见此时图像中只存在特征圆信息。随后通过Canny边缘检测和轮廓查找的方法提取图像中特征圆的轮廓并记录其像素坐标，以便后续椭圆拟合操作，得到的图像如图3c所示。最后使用基于最小二乘的椭圆拟合算法拟合所提取的特征圆轮廓，并计算特征圆的圆心坐标，该算法的计算过程如下：

假设P_j(x_j,y_j)(j=\mathrm{1,2},\dots ,n)是椭圆上的n个待测像素点，根据最小二乘法对待测像素点进行椭圆方程拟合，可得到目标函数为

F(A,B,C,D,E)={\displaystyle \sum _{j=\mathrm{1}}^n}(x_j^{\mathrm{2}}+A{x}_j{y}_j+B{y}_j^{\mathrm{2}}+C{x}_j+D{y}_j{+E)}^{\mathrm{2}}（1）

其中，A\mathrm{、}B\mathrm{、}C\mathrm{、}D\mathrm{、}E为椭圆拟合系数。在该函数中要使F值最小，需要满足：

\frac{\partial F}{\partial A}=\frac{\partial F}{\partial B}=\frac{\partial F}{\partial C}=\frac{\partial F}{\partial D}=\frac{\partial F}{\partial E}=\mathrm{0}（2）

根据式（1）、式（2）可以建立方程求解出A\mathrm{、}B\mathrm{、}C\mathrm{、}D\mathrm{、}E的值，进一步可得到椭圆的长轴a、短轴b以及中心坐标(x_{\mathrm{0}},y_{\mathrm{0}})：

\left.\begin{array}{l}{x}_{\mathrm{0}}=\frac{\mathrm{2}BC-AD}{A^{\mathrm{2}}-\mathrm{4}B}\\ y_{\mathrm{0}}=\frac{\mathrm{2}D-AD}{A^{\mathrm{2}}-\mathrm{4}B}\\ a=\sqrt[]{\frac{\mathrm{2}(ACD-B{C}^{\mathrm{2}}-D^{\mathrm{2}}+\mathrm{4}BE-A^{\mathrm{2}}E)}{(A^{\mathrm{2}}-\mathrm{4}B)(B-\sqrt[]{A^{\mathrm{2}}+(\mathrm{1}-B^{\mathrm{2}})+\mathrm{1}})}}\\ b=\sqrt[]{\frac{\mathrm{2}(ACD-B{C}^{\mathrm{2}}-D^{\mathrm{2}}+\mathrm{4}BE-A^{\mathrm{2}}E)}{(A^{\mathrm{2}}-\mathrm{4}B)(B+\sqrt[]{A^{\mathrm{2}}+(\mathrm{1}-B^{\mathrm{2}})+\mathrm{1}})}}\end{array}\right\}（3）

计算椭圆长短轴的比值，若比值接近1则判断该轮廓为特征圆，拟合该轮廓并计算其圆心坐标，图像处理结果如图3d所示。经验证，该方法可快速准确地定位红外双目相机视野中标记球的位置，并计算出标记球的亚像素级像素圆心坐标。

得到红外标记球的圆心坐标后，需要通过双目立体视觉测量算法计算标记球在相机坐标系下的三维坐标。该算法原理如图4所示，经过极线校正后左右相机成像平面平行且共面，并具有相同的焦距f，其中O_{\mathrm{l}}{x}_{\mathrm{l}}{y}_{\mathrm{l}}{z}_{\mathrm{l}}为左相机坐标系，O_{\mathrm{r}}{x}_{\mathrm{r}}{y}_{\mathrm{r}}{z}_{\mathrm{r}}为右相机坐标系，O_{\mathrm{l}}、O_{\mathrm{r}}分别为左右相机的光学中心，O_{\mathrm{l}}在成像平面的投影为(C_x,C_y)。基于左相机坐标系下的测量点P(X_{\mathrm{c}},Y_{\mathrm{c}},Z_{\mathrm{c}})在左右相机平面上的投影点分别为p_{\mathrm{l}}(u_{\mathrm{l}},v_{\mathrm{l}})和p_{\mathrm{r}}(u_{\mathrm{r}},v_{\mathrm{r}})，其中L_{\mathrm{b}}为双目相机基线的长度，双目视觉的视差d_{\mathrm{s}}=u_{\mathrm{l}}-u_{\mathrm{r}}。

根据图4所示的几何关系，测量点P与视差d_s的关系可以表示为^［12］

\left.\begin{array}{l}{Z}_{\mathrm{c}}=\frac{L_{\mathrm{b}}f}{d_{\mathrm{s}}}\\ X_{\mathrm{c}}=\frac{(u_{\mathrm{l}}-C_x)}{f}{Z}_{\mathrm{c}}\\ Y_{\mathrm{c}}=\frac{(v_{\mathrm{l}}-C_y)}{f}{Z}_{\mathrm{c}}\end{array}\right\}（4）

由式（4）可以计算出空间任意一点在左相机坐标系下的三维坐标，结合上文所述的亚像素圆心提取算法，可以分别计算出光笔的三个红外标记球的球心在左相机坐标系下的三维坐标。

3 视觉示教系统的坐标系转换

示教系统的坐标系转换关系如图5所示，其中机器人基坐标系O{X}_{\mathrm{B}}{Y}_{\mathrm{B}}{Z}_{\mathrm{B}}和工具坐标系O{X}_{\mathrm{T}}{Y}_{\mathrm{T}}{Z}_{\mathrm{T}}的转换矩阵为{\mathit{H}}_{\mathrm{B}}^{\mathrm{T}}，光笔的标记球坐标系O{X}_{\mathrm{O}}{Y}_{\mathrm{O}}{Z}_{\mathrm{O}}和相机坐标系O{X}_{\mathrm{C}}{Y}_{\mathrm{C}}{Z}_{\mathrm{C}}的转换矩阵为{\mathit{H}}_{\mathrm{C}}^{\mathrm{O}}，探针坐标系O{X}_{\mathrm{E}}{Y}_{\mathrm{E}}{Z}_{\mathrm{E}}和标记球坐标系的转换矩阵为{\mathit{H}}_{\mathrm{O}}^{\mathrm{E}}，相机坐标系和工具坐标系的转换矩阵为{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}，探针坐标系和机器人基坐标系的转换矩阵为{\mathit{H}}_{\mathrm{B}}^{\mathrm{E}}，光笔的标记球坐标系和机器人基坐标系的转换矩阵为{\mathit{H}}_{\mathrm{B}}^{\mathrm{O}}。可以看出各个坐标系构成了运动闭合链，则光笔探针坐标系到机器人基坐标系的转换关系矩阵{\mathit{H}}_{\mathrm{B}}^{\mathrm{E}}可表示为

{\mathit{H}}_{\mathrm{B}}^{\mathrm{E}}={\mathit{H}}_{\mathrm{B}}^{\mathrm{T}}{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}{\mathit{H}}_{\mathrm{C}}^{\mathrm{O}}{\mathit{H}}_{\mathrm{O}}^{\mathrm{E}}（5）

3.1 光笔探针坐标系标定

在设计光笔时，红外标记球与探针的几何关系已经确定，但在加工和装配过程中通常会产生一定的误差，因此，为提高示教系统的精度，需要校准探针和红外标记球的相对位置，从而准确获得光笔的探针坐标系与标记球坐标系之间的转换矩阵。如图6所示，建立标记球坐标系O{X}_{\mathrm{O}}{Y}_{\mathrm{O}}{Z}_{\mathrm{O}}和探针坐标系O{X}_{\mathrm{E}}{Y}_{\mathrm{E}}{Z}_{\mathrm{E}}。第i个位姿下标记球坐标系到相机坐标系的旋转矩阵和平移矩阵可分别表示为{}_i{}^{}{}_{\mathrm{C}}^{\mathrm{O}}和{}_i{}^{}{}_{\mathrm{C}}^{\mathrm{O}}（i=\mathrm{1,2},\dots ,N）。

当光笔探针触碰相机坐标系下一固定点时，探针在相机坐标系下的坐标为{\mathit{P}}_{\mathrm{t}\mathrm{i}\mathrm{p}}，在标记球坐标系下坐标为{\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}。光笔以不同姿态触碰该点，且触碰方向均匀分布，则有：

{\mathit{P}}_{\mathrm{t}\mathrm{i}\mathrm{p}}={}_i{}^{}\mathit{R}{}_{\mathrm{C}}^{\mathrm{O}}{\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}+{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}（6）

进一步可得

({}_i{}^{}\mathit{R}{}_{\mathrm{C}}^{\mathrm{O}}-{}_{i+\mathrm{1}}{}^{}\mathit{R}{}_{\mathrm{C}}^{\mathrm{O}}){\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}={}_{i+\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}-{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}（7）

由此可得N-\mathrm{1}组形式为\mathit{A}\mathit{X}=\mathit{b}的方程，其中\mathit{A}={}_i{}^{}\mathit{R}{}_{\mathrm{C}}^{\mathrm{O}}-{}_{i+\mathrm{1}}{}^{}\mathit{R}{}_{\mathrm{C}}^{\mathrm{O}}，\mathit{b}={}_{i+\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}-{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}，\mathit{X}={\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}。可通过最小二乘法拟合求解{\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}，其求解公式如下：

\mathit{X}=({\mathit{A}}^{\mathrm{T}}{\mathit{A})}^{-\mathrm{1}}{\mathit{A}}^{\mathrm{T}}\mathit{b}（8）

最后计算得到探针在标记球坐标系下的三维坐标{\mathit{P}}_{\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{b}\mathrm{e}}后，将标记球坐标系绕坐标系中心顺时针旋转45°后作为探针坐标系的姿态，由此可以得到探针坐标系与标记球坐标系之间的转换矩阵。

3.2 基于红外发光标记球的手眼标定方法

除了完成光笔探针坐标系标定，还需标定机器人基坐标系与红外双目相机的转换矩阵系统中各坐标系的转换关系。该标定过程称为手眼标定（{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}为手眼标定矩阵），其原理是通过获取标定件（代表为高精度棋盘格）在机器人基坐标系和相机坐标系下的对应位姿数据点集进行求解^［13-15］，但红外双目相机难以通过棋盘格等彩色标记物完成手眼标定，因此本文使用主动发光的红外标记球作为标定件，由固定在机器人末端的红外相机实时捕捉其轮廓并计算相应的位姿信息，从而简化了针对红外相机的手眼标定流程并提高了标定精度，具体流程如下：

首先将光笔固定在工作台面上，控制机器人末端工具以N个不同的位姿带动相机拍摄光笔的红外标记球并计算第i个位姿下标记球坐标系到相机坐标系的转换矩阵{}_i{}^{}{}_{\mathrm{C}}^{\mathrm{O}}（i=\mathrm{1,2},\dots ,N），接着通过机器人示教器读取该位姿下工具坐标系到机器人基坐标系的转换矩阵{}_i{}^{}{}_{\mathrm{B}}^{\mathrm{T}}。标定过程中，标记球坐标系在机器人基坐标系下的转换矩阵{\mathit{H}}_{\mathrm{B}}^{\mathrm{O}}固定不变，满足

{\mathit{H}}_{\mathbf{B}}^{\mathbf{O}}={}_i{}^{}\mathit{H}{}_{\mathbf{B}}^{\mathbf{T}}{\mathit{H}}_{\mathbf{T}}^{\mathbf{C}}{}_i{}^{}\mathit{H}{}_{\mathbf{C}}^{\mathbf{O}}（9）

将式（9）改写成如下矩阵形式：

\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{B}}^{\mathrm{O}}& {\mathit{t}}_{\mathrm{B}}^{\mathrm{O}}\\ \mathrm{0}& \mathrm{1}\end{array}\right]=\left[\begin{array}{cc}{}_i{}^{}{\mathit{R}}_{\mathrm{B}}^{\mathrm{T}}& {}_i{}^{}{\mathit{t}}_{\mathrm{B}}^{\mathrm{T}}\\ \mathrm{0}& \mathrm{1}\end{array}\right]\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}& {\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}\\ \mathrm{0}& \mathrm{1}\end{array}\right]\left[\begin{array}{cc}{}_i{}^{}{\mathit{R}}_{\mathrm{C}}^{\mathrm{O}}& {}_i{}^{}{\mathit{t}}_{\mathrm{C}}^{\mathrm{O}}\\ \mathrm{0}& \mathrm{1}\end{array}\right]（10）

式中：{\mathit{R}}_{\mathrm{B}}^{\mathrm{O}}、{\mathit{t}}_{\mathrm{B}}^{\mathrm{O}}分别为标记球坐标系到机器人基坐标系下的旋转矩阵和平移矩阵；{}_i{}^{}{}_{\mathrm{B}}^{\mathrm{T}}、{}_i{}^{}{}_{\mathrm{B}}^{\mathrm{T}}分别为第i个位姿下机器人末端工具坐标系到机器人基坐标系的旋转矩阵和平移矩阵；{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}、{\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}分别为相机坐标系到工具坐标系的旋转矩阵和平移矩阵；{}_i{}^{}{}_{\mathrm{C}}^{\mathrm{O}}、{}_i{}^{}{}_{\mathrm{C}}^{\mathrm{O}}分别为第i个位姿下标记球坐标系到相机坐标系的旋转矩阵和平移矩阵。

将式（10）展开计算可得

\left.\begin{array}{l}{\mathit{R}}_{\mathrm{B}}^{\mathrm{O}}={}_i{}^{}\mathit{R}{\mathit{R}}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_i{}^{}\mathit{R}{\mathit{R}}_{\mathrm{C}}^{\mathrm{O}}\\ {\mathit{t}}_{\mathrm{B}}^{\mathrm{O}}={}_i{}^{}\mathit{R}{\mathit{R}}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_i{}^{}\mathit{t}{\mathit{t}}_{\mathrm{C}}^{\mathrm{O}}+{}_i{}^{}\mathit{R}{\mathit{R}}_{\mathrm{B}}^{\mathrm{T}}{\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}+{}_i{}^{}\mathit{t}{\mathit{t}}_{\mathrm{B}}^{\mathrm{T}}\end{array}\right\}（11）

由于标记球在机器人基坐标系下的{\mathit{t}}_{\mathrm{B}}^{\mathrm{O}}固定不变，进一步可得

{}_{i-\mathrm{1}}{}^{}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}+{}_{i-\mathrm{1}}{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}+{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}=

{}_i{}^{}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}+{}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}+{}_i{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}（12）

为简化计算，在每两组测量的过程中只对机器人末端工具进行平移操作，使机器人末端工具坐标系到机器人基坐标系的旋转矩阵相同（即{}_{i-\mathrm{1}}{}^{}{}_{\mathrm{B}}^{\mathrm{T}}={}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}），此时可得

{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}({}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}-{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}})=({}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}{)}^{\mathrm{T}}({}_i{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}-{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}})（13）

将测量得到的样本数据进行拼接，将{}_{i-\mathrm{1}}{}^{}{}_{\mathrm{C}}^{\mathrm{O}}-{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}拼接成的矩阵记为{\mathit{P}}_{N/\mathrm{2}}，将({}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}{)}^{\mathrm{T}}({}_i{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}-{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}})拼接成的矩阵记为{\mathit{T}}_{N/\mathrm{2}}，可得

{\mathit{P}}_{N/\mathrm{2}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}={\mathit{T}}_{N/\mathrm{2}}^{\mathrm{T}}（14）

由式（8）可以解算出手眼标定矩阵中旋转矩阵{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}，随后式（12）可写作：

({}_{i-\mathrm{1}}{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}-{}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}){\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}={}_i{}^{}\mathit{R}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_i{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}-

{}_{i-\mathrm{1}}{}^{}{}_{\mathrm{B}}^{\mathrm{T}}{\mathit{R}}_{\mathrm{T}}^{\mathrm{C}}{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{C}}^{\mathrm{O}}+{}_i{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}-{}_{i-\mathrm{1}}{}^{}\mathit{t}{}_{\mathrm{B}}^{\mathrm{T}}（15）

再次通过最小二乘法求解平移矩阵{\mathit{t}}_{\mathrm{T}}^{\mathrm{C}}，至此得出了手眼标定矩阵{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}。得到手眼标定矩阵后可通过式（5）计算出光笔探针在机器人基坐标系下的位姿{\mathit{H}}_{\mathrm{B}}^{\mathrm{E}}。

4 相机视野自适应跟踪方法

由于相机拍摄得到的视野范围有限，当操作人员需示教的轨迹超出相机视野时，需要手动修正机器人末端来调整相机位置，操作较为不便。另外，相机成像时视野边缘的畸变程度相较于视野中心的畸变程度更大^［16］，此时需要对相机进行畸变校正，但仍无法完全消除畸变的影响。由此可知，在示教过程中标记球靠近视野边缘时，相机畸变会对标记球的捕捉精度产生较大影响。针对上述问题，本文提出了一种相机视野自适应跟踪方法，可以根据捕捉的标记球位置实时自动调整机器人位姿，以确保标记球在相机视野范围中心区域，从而扩大示教范围并提高示教精度，具体方法如下：

1）如图7a所示，在红外双目相机的左相机视野内设置视野有效范围，有效范围中心如图中P_{\mathrm{0}}(u_{\mathrm{0}},v_{\mathrm{0}})所示（将该点作为双目相机视野中心），标记球越靠近P_{\mathrm{0}}则越接近视野的中心位置，球心坐标计算精度越高。

2）通过三个标记球像素坐标值计算其重心P_{\mathrm{1}}(u_{\mathrm{1}},v_{\mathrm{1}})并作为标记球的中心位置，即图中绿色标记点，以绿色标记点为中心，框定标记球最佳拍摄范围，当左相机的有效范围中心处于该范围内时，表示标记球中心已经处于双目相机的视野中心区域。当P_{\mathrm{0}}在视野最佳范围以外时，可以计算出两点之间的向量{\mathit{P}}_{\mathrm{0}}{\mathit{P}}_{\mathrm{1}}，可得

\left.\begin{array}{l}\mathrm{\Delta }u=u_{\mathrm{1}}-u_{\mathrm{0}}\\ \mathrm{\Delta }v=v_{\mathrm{1}}-v_{\mathrm{0}}\\ d=\sqrt[]{{(\mathrm{\Delta }u)}^{\mathrm{2}}+{(\mathrm{\Delta }v)}^{\mathrm{2}}}\end{array}\right\}（16）

式中：\mathrm{\Delta }u、\mathrm{\Delta }v为点P_{\mathrm{0}}和P_{\mathrm{1}}之间的水平像素距离和垂直像素距离；d为两点间的像素距离。

3）得到向量{\mathit{P}}_{\mathrm{0}}{\mathit{P}}_{\mathrm{1}}后，需要机器人带动相机沿着{\mathit{P}}_{\mathrm{0}}{\mathit{P}}_{\mathrm{1}}的方向在机器人基坐标系的X、Y方向进行移动，移动距离通过自适应调整比率\lambda确定。\lambda表示机器人末端移动距离与P_{\mathrm{0}}和P_{\mathrm{1}}之间的像素距离的比值，需通过先验测试对不同机器人型号、相机视野大小等进行调整。机器人移动的距离计算公式如下：

\left.\begin{array}{c}{m}_X=\lambda \mathrm{\Delta }u\\ m_Y=\lambda \mathrm{\Delta }v\end{array}\right\}（17）

式中：m_X为机器人沿X轴移动的距离；m_Y为机器人沿Y轴移动的距离。

4）为了使机器人末端移动更加平滑，需要实时对末端的运动速度进行规划，使用PID算法求解机器人末端的线速度v，其表达式如下：

\left.\begin{array}{l}\frac{v}{\lambda }=k_{\mathrm{p}}e(t)+k_{\mathrm{i}}{\int }_{\mathrm{0}}^{t}e(t)\mathrm{d}t+k_{\mathrm{d}}\frac{\mathrm{d}e(t)}{\mathrm{d}t}\\ e(t)=d(t)\end{array}\right\}（18）

式中：k_{\mathrm{p}}、k_{\mathrm{i}}、k_{\mathrm{d}}为PID控制参数；e(t)输入误差；d(t)为t时刻下P_{\mathrm{0}}与P_{\mathrm{1}}之间的像素距离。

5）求解得到机器人末端线速度v后，通过上位机将机器人末端需要移动的距离和线速度发送给机器人，机器人末端快速执行偏移操作，使P_{\mathrm{0}}移动至视野最佳范围内，如图7b所示，随后重复步骤2）~步骤5）。

5 实验

如图8所示，为验证本文提出的视觉示教方法的有效性，搭建相应的示教实验平台，由ABB IRB120机器人、红外双目相机、单点实验板、马鞍型轨迹板、光笔、光笔底座和棋盘格组成。其中，红外双目相机由两个带有850 nm红外滤光片的相机组成，相机参数如表1所示。本章共设计了三个实验，分别为：①设计两组单点示教实验分别用于验证本文提出的手眼标定方法和相机视野自适应跟踪方法。②设计光笔姿态测量实验，使用Leica-AT960MR绝对激光跟踪仪辅助测量光笔的姿态变化，用于验证该示教系统的姿态精度。③设计连续轨迹示教评估实验，由于激光跟踪仪是具有高精度和高稳定性的测量设备，因此将激光跟踪仪获取轨迹数据作为示教轨迹的基准；接着通过本文提出的视觉跟踪示教系统在实验室环境内采集同样的轨迹与基准轨迹进行对比，并将轨迹数据发送给机器人进行轨迹复现；最后，为验证系统在不同光照环境下的稳定性，设计了光照对比实验进行分析。

5.1 单点示教对比实验

使用本文提出的方法进行手眼标定实验时，需要将所设计的光笔通过图8所示的底座固定在实验平台上。首先，由第3.2节所述的手眼标定矩阵解算方法结合采集的数据得出相应的手眼标定矩阵{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}为

{\mathit{H}}_{\mathrm{T}}^{\mathrm{C}}=

\left[\begin{array}{cccc}-\mathrm{0.135}\mathrm{48}& \mathrm{0.784}\mathrm{24}& -\mathrm{0.605}\mathrm{48}& \mathrm{74.122}\mathrm{58}\\ -\mathrm{0.988}\mathrm{34}& -\mathrm{0.064}\mathrm{17}& \mathrm{0.138}\mathrm{03}& \mathrm{66.716}\mathrm{85}\\ \mathrm{0.069}\mathrm{39}& \mathrm{0.617}\mathrm{12}& \mathrm{0.783}\mathrm{79}& -\mathrm{322.710}\mathrm{99}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]

接下来，使用光笔在实验板上标记的9个固定点位置（如图8红点所示）采集各自在机器人基坐标系下的三维坐标，其中每个位置的光笔均以不同姿态连续采集50组数据，将50组三维坐标的平均值作为该点的测量坐标。最后，使用示教器控制机器人末端工具记录前述固定点的真实坐标，将该值和测量坐标之间的距离作为该点的误差，即本方法的实验误差。

为验证该方法的有效性，使用图8中的棋盘格进行手眼标定，标定过程需使用红外光源对棋盘格进行照明，光照时保持标定板在视野中呈现清晰的特征信息，并使用TSAI法^［17］求解出相应的手眼标定矩阵。重复上述实验流程后，与本文的手眼标定方法进行对比实验。实验结果如图9所示，计算结果如表2所示。

由表2可得，本文的手眼标定方法平均误差为0.508 mm，使用棋盘格标定法得到的平均误差为0.960 mm，相较于使用棋盘格进行标定，本文标定方法的精度提高了47.1%。当使用棋盘格进行标定时，由于光照不均匀容易对标定精度造成影响，而直接使用红外发光标记球进行手眼标定具有更高的精度且简化了标定流程。

随后，为验证相机视野自适应跟踪方法对整体示教精度的影响，仍使用图8中的实验板进行实验。首先采用本文提出的相机视野自适应跟踪方法，操作人员每次分别对实验板上的9个点按标号顺序连续采集，采集过程中机器人时刻跟随光笔移动，点位标号越小表示越靠近相机的视野中心，标号越大则表明越靠近相机视野边缘。一共重复进行10次采集，并记录所测得的三维点位（基于机器人基坐标系），分别取其平均值作为9个点的测量坐标，计算测量坐标和真实坐标的距离作为位置误差。然后保持相机固定不动，重复上述步骤得到固定示教方法下的位置误差，并与使用本文跟踪示教方法得到的误差进行对比，实验结果如图10所示，数据对比如表3所示。

由测得的数据可知，两种方法得到的最大误差均小于1 mm。在相机固定示教的过程中，当示教点在相机视野中心时，两者得到的位置误差接近。而当示教点远离视野中心并接近视野边缘时，位置误差逐渐增大，最大误差为0.896 mm。使用本文提出的方法后，位置误差基本保持稳定，且整体测量的精度也有所提高，最大误差为0.601 mm。由此可见，使用跟踪示教的方法可以保证光笔标记球始终处于相机视野中心区域，从而可提高示教的精度，并且能够扩大视觉示教系统的有效示教范围。

5.2 光笔姿态测量实验

为验证光笔在示教过程中姿态的精度，将Leica-AT960MR绝对激光跟踪仪采集的数据作为基准，并与本文光笔所采集的数据进行对比分析姿态误差。如图11所示，首先标定得到激光跟踪仪坐标系和机器人基坐标系的转换矩阵，随后，将本文设计的光笔和激光跟踪仪的T-probe固连在一起，此时光笔和T-probe的探针坐标系均转换到机器人基坐标系下。接着分别通过光笔和T-probe采集8组机器人基坐标系下的姿态数据进行对比，得到的姿态误差如表4所示，其中\mathrm{\Delta }{E}_x、\mathrm{\Delta }{E}_y、\mathrm{\Delta }{E}_z分别为光笔与激光跟踪仪在机器人基坐标系下测量姿态的x、y、z轴旋转角误差。

由表4可知，测量得到的光笔姿态最大误差为2.1432°，能够满足实际需求。

5.3 连续轨迹示教评估

为了评估光笔获取连续轨迹示教点位的精度，使用一个固定在工作台上用于模拟两根圆管相交焊缝的3D打印工件进行实验，其表面的马鞍型轮廓即代表焊接轨迹。实验过程如下：首先操作人员手持图12a中的6D光笔，使用其末端探针顺时针采集轨迹的三维数据，并将数据转化到机器人基坐标系下。随后使用图12b中激光跟踪仪的T-probe按上述方法采集其末端在机器人基坐标系下的三维数据，并与光笔所采集的数据进行对比。由于操作过程中存在抖动等影响精度的问题，故需要对轨迹进行卡尔曼滤波处理以去除噪声，并使用B样条进行轨迹拟合，两种情况下的轨迹对比如图13所示。

此外，将激光跟踪仪采集的轨迹数据作为标准值与本文提出的视觉示教系统所获得的最终轨迹进行对比，从而验证本系统的精确性。将两组测量的轨迹按照每隔2 mm的距离计算得到插补点，比较各组对应插补点的距离作为本实验的误差，计算得到的最大误差为0.734 mm，平均误差为0.618 mm，均能达到亚毫米级的精度。

将得到的光笔轨迹数据发送给ABB机器人，并对机器人进行速度规划，即设置机器人末端初始移动速度和加速度均为0，机器人末端沿着轨迹的运动速度设置为20 mm/s。最终，机器人可以按照马鞍型轨迹执行轨迹复现任务，其过程如图14所示。

实验结果表明，使用本文基于6D光笔的视觉示教系统可以进行较为复杂的轨迹示教，且具有较高的复现精度，可以满足实际使用需求。

为了验证本系统在不同光照下的稳定性，使用型号为GM1020的照度计对光照进行定量描述。首先测量上述轨迹复现实验环境下可见光的光照强度为67 lx，随后通过可见光的光源分别提供弱（153 lx）、中（376 lx）、强（742 lx）三种不同强度的光照。在三种光照环境下重新采集马鞍型示教轨迹的数据进行对比，得到各自的最大误差与平均误差，实验过程如图15所示，其中图15a和图15b右上角为该光照环境下相机拍摄的图像。可见不同光照下得到的圆形特征未发生明显变化，实验对比结果如表5所示。

由对比实验结果可得，在不同强度的光照环境下红外相机拍摄得到的图像无明显区别，且计算得到的误差也无明显的变化，因此，该系统在不同强度的可见光环境下均有较高的稳定性。

表6列举了几种现有的视觉示教研究成果（以引用文献中示教笔上的特征标记物为区分）进行对比，可以得出本文方法的示教精度更高，且不会受到可见光的影响。由于本文设计光笔在倾角过大的情况下可能会出现标记球重叠的现象，因此与使用多面体和球杆型作为特征标记物的示教笔时相比，示教人员在使用6D光笔的过程中允许的示教姿态范围仍会更小。

6 结论

1）提出了一种基于6D光笔的机器人高精度示教方法，该方法使用红外双目相机捕捉光笔的红外发光标记球，不易受到相机拍摄角度的限制，且可以有效避免不同强度的可见光对示教精度的影响。

2）提出了一种基于红外发光标记球的手眼标定方法，可以有效降低手眼标定的难度并提高手眼标定的精度。

3）提出了一种相机视野自适应跟踪方法，可以使光笔在移动过程中始终处于相机视野中心区域，提高了视觉示教的精度，同时扩大有效示教范围。

4）在搭建的实验平台上验证了所提系统能够实现机器人对于复杂轨迹的高精度示教再现，并且操作简单，稳定性高，相较于传统的示教方法有较大的优势。

本文提出的相机视野自适应跟踪方法为减轻相机边缘畸变对示教精度的影响和扩大视觉示教范围提供了新的思路。然而这种方法并未考虑到相机快速移动中发生抖动或机器人末端移动存在误差的问题，可能会影响示教点位的精度，未来将对此进行进一步的研究。

参考文献

[1] 吴超群， 赵松， 雷艇. 曲线焊缝的机器人焊接轨迹规划与高频控制［J］. 中国机械工程， 2023， 34（14）： 1723-1728.

[2] WU Chaoqun， ZHAO Song， LEI Ting. Robot Welding Trajectory Planning and High Frequency Control for Curved Seams ［J］. China Mechanical Engineering， 2023， 34（14）： 1723-1728.

[3] 葛吉民， 邓朝晖， 王水仙， 等. 基于点云的机器人焊缝自动化磨削系统与方法［J］. 中国机械工程， 2024， 35（7）： 1253-1262.

[4] GE Jimin， ZHENG Chaohui， WANG Shuixian， et al. Automated Grinding System and Method for Robotic Weld Seams Based on Point Cloud［J］. China Mechanical Engineering， 2024， 35（7）：1253-1262.

[5] 曾超， 杨辰光， 李强， 等. 人-机器人技能传递研究进展 ［J］. 自动化学报， 2019， 45（10）： 1813-1828.

[6] ZENG Chao， YANG Chenguang， LI Qiang， et al. Research Progress on Human-Robot Skill Transfer ［J］. Acta Automatica Sinica， 2019， 45（10）： 1813-1828.

[7] CHANG H T， CHANG J Y. Sensor Glove Based on Novel Inertial Sensor Fusion Control Algorithm for 3-D Real-time Hand Gestures Measurements ［J］. IEEE Transactions on Industrial Electronics， 2019， 67（1）： 658-666.

[8] TOGIA T， GKOURNELOS C， ANGELAKIS P， et al. Virtual Reality Environment for Industrial Robot Control and Path Design ［J］. Procedia CIRP， 2021， 100： 133-138.

[9] PAN Y， CHEN C， LI D， et al. Augmented Reality-based Robot Teleoperation System Using RGB-D Imaging and Attitude Teaching Device［J］. Robotics and Computer-Integrated Manufacturing， 2021， 71： 102167.

[10] 倪自强， 王田苗， 刘达 ． 基于视觉引导的工业机器人示教编程系统 ［J］. 北京航空航天大学学报， 2016， 42（3）： 562-568.

[11] NI Ziqiang， WANG Tianmiao， LIU Da. Vision Guide Based Teaching Programming for Industrial Robot［J］． Journal of Beijing University of Aeronautics and Astronautics， 2016， 42（3）： 562-568.

[12] 应再恩， 平雪良， 李正洋， 等.基于视觉跟踪的机器人复杂轨迹模拟再现［J］.机械设计与研究， 2014， 30（01）： 39-41.

[13] YING Zaien， PING Xuelian， LI Zhengyang， et al. The Simulation and Reconstruction of the Complex Robot Trajectories Based on Visual Tracking［J］. Mechanical Design and Research， 2014， 30（1）： 39-41.

[14] LE T S， TRAN Q V， NGUYEN X L， et al. Solpen： an Accurate 6-DOF Positioning Tool for Vision-guided Robotics［J］. Electronics， 2022， 11（4）： 618.

[15] LIN H I， LIN Y H. A Novel Teaching System for Industrial Robots ［J］. Sensors， 2014， 14（4）： 6012-6031.

[16] ZHANG H D， LIU S B， LEI Q J， et al. Robot Programming by Demonstration： a Novel System for Robot Trajectory Programming Based on Robot Operating System［J］. Advances in Manufacturing， 2020， 8： 216-229.

[17] LIU L， ZHU P， GUAN J， et al. A Binocular Reconstruction Method Fused with Laplacian Image Information for Pavement Texture Evaluation［J］. Measurement， 2021， 185： 110039.

[18] 毛成林， 于瑞强， 宋爱国. 一种结合TCP标定的深度相机手眼标定方法 ［J］. 仪器仪表学报， 2023， 44（3）： 280-286.

[19] MAO Chenglin， YU Ruiqiang， SONG Aiguo. A Hand-eye Calibration Method for Depth Camera Combined with TCP Calibration ［J］. Chinese Journal of Scientific Instrument， 2023， 44（3）： 280-286.

[20] XU J， LI Q， WHITE BAI. A Novel Hand-eye Calibration Method for Industrial Robot and Line Laser Vision Sensor ［J］. Sensor Review， 2023， 43（4）： 259-265.

[21] WU L， ZANG X， BAI M， et al. Simple and High‐precision Hand-eye Calibration for 3D Robot Measurement Systems ［J］. Advanced Intelligent Systems， 2023， 5（12）： 2300307.

[22] PAN B， YU L， WU D， et al. Systematic Errors in Two-dimensional Digital Image Correlation due to Lens Distortion ［J］. Optics and Lasers in Engineering， 2013， 51（2）： 140-147.

[23] TSAI R Y， LENZ R K. A New Technique for Fully Autonomous and Efficient 3D Robotics Hand-eye Calibration［J］. IEEE Transactions on Robotics and Automation， 1989， 5（3）： 345-358.

基金资助

国家重点研发计划(2018YFB1308603)

福建省科技重大专项(2024HZ026020)