基于视觉的移载协作机器人机床上下料末端纠偏算法

Vision-Based End Correction Algorithm for Collaborative Robot Machine Tool Loading and Unloading

摘要: 针对复合机器人在机床上下料中的定位方法，本文主要关注识别机床卡盘和末端作业面的定位。首先分析了复合机器人在机床上下料过程中的工作原理及其基本结构，包括AGV (自动导引车)和协作机器人的功能及特点。然后，详细阐述了如何使用视觉系统和纠偏算法来识别机床卡盘和末端作业面，进而实现了复合机器人在机床上下料作业过程中的精确定位。接着提出了一种基于视觉系统和纠偏算法的复合机器人定位方法，该方法可以有效地识别机床卡盘和末端作业面。最后，对文中所提出的定位方法进行了实验验证，并与现有的定位方法进行了比较。实验结果表明，本文提出的定位方法能够有效地提高复合机器人在机床上下料过程中的定位精度和工作效率。总之，本研究为复合机器人在机床上下料中的应用提供了一种有效的定位方法，具有较高的实用价值。

Abstract: Based on the positioning method of composite robots in machine tool stacking and unloading, this paper focuses on the positioning of machine tool dishes and end-end operation surfaces. Firstly, based on the working principle and basic structure of composite robots in the process of machine tool stacking and unloading, including the functions and characteristics of AGV (automatic guided vehicle) and assistant robot, we have elaborately elaborated on how to identify machine tool dish and end-end operation surfaces through visual systems and end correction algorithm. In this way, we can achieve accurate positioning of composite robots in the process of machine tool stacking and unloading. Then this paper proposed a positioning method based on visual systems and end correction algorithm, which can effectively identify machine tool dish and end-end operation surfaces. Finally, conducted experiments to verify the proposed positioning method and compare it with existing positioning methods. The experimental results show that the proposed positioning method can effectively improve the positioning accuracy and work efficiency of composite robots in the process of machine tool stacking and unloading. In summary, this research provides a valuable positioning method for the application of composite robots in machine tool loading and unloading and has a high practical value.

1. 引言

随着技术的升级换代，国家提出了“中国制造2025”、“工业4.0”等相关政策。制造业的自动化、智能化程度都不断的提高。近年来，珠三角地区甚至是泛大湾区的制造业企业纷纷提出机器人换人的计划，以求降低生产成本，提高生产效率，同时提高生产的弹性 [1] 。

机床作为工业母机，在工业化进程中具有举足轻重的地位，将需要机床加工的工件移动到待加工位和从待加工位将工件移走统称为机床上下料工序。目前比较常见的机床上下料方式有机械手、气缸组合、物流小车、工业机器人等。在国内制造业中，普遍采用专用设备或者人工进行机床上下料作业，这在单一产品线，以及产能不高的情形是适用的。然而，社会在不断向前发展，科学技术也在不断进步，产品也不断升级换代，像传统那样使用专机或人工进行机床上下料工序就不能满足大批量生产的需求了。因此亟需研发一种能够克服专机或人工机床上下料结构复杂、柔性不够、效率低下、容易发生工伤事故、劳动强度大等缺点的新方法 [2] [3] 。

针对上述情况，本文提出一种采用移载协作机器人配合视觉识别方法 [4] [5] ，视觉的使用方式均采用市面成熟的设备 [6] [7] ，该方法应用在机床上下料工序中可以有效改善上述使用AGV+协作机器人进行机床卡盘特征不清晰，机床内部空间小的问题 [8] 。因为AGV的移动特性，系统中的IO使用的是无线IO [9] [10] ，经实践证明，该方法可以显著提升定位点的精度，同时，在医疗领域也是有类似的应用[9]，多数的坐标系转换方法原理相同，但是在具体场景中的使用差别较大 [11] [12] 。

2. 设备的组成和工作流程

这个系统的整体组成包括以下五个部分：1. 机床(5台)是主要的生产设备，负责进行加工操作。2. AGV (自动导引小车)在机床之间运输零件，协助生产流程的进行。3. 协作机器人可以与工作人员一起工作，执行一些特定的任务，提高生产效率。4. 检测台负责产品的质量检查，确保产品质量符合要求。5. 自动化仓库负责存储原材料、半成品和成品，提高存储和检索的效率和精度。这个系统通过自动化和智能化技术，提高了生产效率和质量，降低了人工成本，是现代制造业的重要组成部分。

2.1. 核心组件

核心组件由三部分组成见图1，分别为：1. 机床，这是系统的核心部分，负责进行主要的生产工作。这些机床可以独立操作，也可以通过网络连接进行协同生产。2. AGV (自动导引小车)：AGV用于在机床之间运输零件，以及从仓库中提取和送回原材料和工具。它能够自主导航，减少人为干预，提高生产效率。3. 协作机器人：协作机器人与工作人员一起在生产环境中工作，它们可以执行一些重复性、危险或繁琐的任务，例如装载和卸载机床，进行质量检测等。

Figure 1. Machine tool collaborative robot and AGV

图1. 机床协作机器人及AGV

本文讨论的工件放置目标点机床卡盘如下图2所示，所用工具头两个夹爪，配一套视觉摄像头和光源，体积较大，机床门较小，内部空间狭窄，无法进行有效拍摄卡盘的位置特征，此卡盘特征较为复杂。而且在加工工件后，会有油污，无法正常识别。

Figure 2. The gripper is in the position of the machine tool chuck

图2. 夹爪在机床卡盘位置

2.2. 辅助组件

辅助组件由两部分组成，分别是：1. 检测台：检测台如下图3，用于对生产出的产品进行质量检查。它可能包括一系列传感器和测试设备，用于检查产品的尺寸、形状、材料等特性。2. 自动化仓库：仓库用于存储原材料、半成品和成品。自动化仓库使得物品的存取更加高效和精确，它通过自动化的机械系统和计算机管理系统，可以快速地存储和检索物品。

Figure 3. The gripper is located at the entrance and exit of the automated warehouse

图3. 夹爪在自动化仓库出入口位置

2.3. 工作流程

先简单介绍了整套设备的工作流程，重点介绍机床取放位置的流程，机床部分的操作是本文设计的重点。

2.3.1. 总体工作流程

这个工作流程涉及到自动化控制系统和机器人。下面是大致的描述如下图4：1. 总控系统：总控系统是整个流程的核心，它负责发出任务指令。这些指令可能包括对机器人的动作指令，例如移动到特定的工位，执行特定的操作等。2. 系统控制IO：系统控制的IO (输入/输出)设备是连接总控系统和机器人的桥梁。机器人通过读取这些IO来判断下一步的工作任务。3. 机器人：机器人根据系统控制的IO来判断需要前往的工位和工作任务。然后，它会按照指令移动到指定的工位，执行相应的操作。具体步骤如下：1. 总控系统生成并发送任务指令。这些指令可能包括工位的编号，需要执行的任务等。2. 系统控制IO接收这些指令，并将其传输给机器人。3. 机器人读取系统控制的IO，根据其中的指令来判断下一步的工作任务。4. 机器人根据判断结果，移动到对应的工位。这个过程可能涉及到机器人的路径规划，导航等复杂技术。5. 到达工位后，机器人执行总控系统发出的任务指令。6. 任务完成后，机器人返回原位或进行下一步的操作，等待新的任务指令。以上就是一个基本的自动化工作流程。在实际的工业环境中，可能还需要考虑其他因素，例如机器人的安全性、工作效率等。

Figure 4. General route chart

图4. 总体工作流程图

2.3.2. 机床位置的取放流程

1. 机床位置的取放流程如下图5：确定取放物品的位置：首先，需要确定要取放的物品在机床的哪个位置。这通常会涉及到对机床的内部结构和物品的特性有一定的了解。选择适当的工具：根据要取放的物品的特性和位置，选择适当的工具。这些工具可能包括机械臂、抓取器、滑块或者其他专门的设备。确定最佳路径：在确定了工具之后，需要确定最佳的路径来达到目标位置。这可能涉及到对机床的空间使用、安全限制和其他因素的理解。执行取放操作：使用所选的工具，按照确定的最佳路径进行取放操作。这个过程可能需要高度精确的控制和协调，以确保物品的安全和准确放置。确认操作完成：在完成取放操作后，需要确认操作是否成功。这可能涉及到检查物品是否被正确放置，工具是否已经返回安全位置，以及是否有任何异常情况发生。2. 其他位置的流程与其类似：虽然不同位置的机床可能会有其特定的挑战和需求，但是基本的取放流程大致相同。确定物品的位置、选择适当的工具、确定最佳路径、执行操作和确认操作完成是通用的步骤。3. 校准流程的提取：校准流程通常会涉及到一些特殊的步骤，例如设置基准点、进行测量、比较数据和进行修正等。这些步骤可以从取放流程中提取出来，因为它们都涉及到对工具和操作的高精度控制。在某些情况下，校准流程可能会在每次取放操作之前或之后进行，以确保工具的精确度和安全性。

Figure 5. Pick and place route chart

图5. 取放流程图

综上所述：1. 确定目标和任务：明确工作的目标和任务，以便确定工作流程和优先级。2. 规划工作步骤：根据目标和任务，规划完成工作所需的具体步骤。3. 制定时间表：根据每个步骤所需的时间和优先级，制定一个详细的时间表。4. 准备工具和材料：根据工作需要，准备必要的工具和材料。5. 执行工作步骤：按照时间表和规划的步骤执行工作。6. 监控进度：根据时间表和实际进展，监控工作的进度和质量。7. 调整和优化：根据实际情况和进展，对工作流程进行必要的调整和优化。8. 完成和评估：完成工作后，对工作的结果进行评估，并根据评估结果进行必要的反馈和改进。

3. 算法解析及实现

3.1. 通用的直角坐标系转换算法

设有两个直角坐标系XOY和X'O'Y'，它们之间的夹角为θ (逆时针为正)，点P与原点之间的向量记为r，如下图图6。设P在两个坐标系中的坐标分别为(x, y)和(x', y')，那么有：

$\{\begin{array}{c}x=r\cos \left(\alpha +\theta \right)=r\cos \left(\alpha \right)\cos \left(\theta \right)-r\sin \left(\alpha \right)\sin \left(\theta \right)\\ y=r\sin \left(\alpha +\theta \right)=r\sin \left(\alpha \right)\cos \left(\theta \right)+r\cos \left(\alpha \right)\sin \left(\theta \right)\end{array}$ (1)

即：

$\{\begin{array}{c}x=x\text{'}\cos \theta -y\text{'}\sin \theta \\ y=y\text{'}\cos \theta +x\text{'}\sin \theta \end{array}$ (2)

Figure 6. The rectangular coordinate system X'O'Y' rotates relative to XOY

图6. 直角坐标系X'O'Y'相对于XOY旋转

再加上平移如下图7，假设O'在XOY坐标系中的位置为(x0, y0)，那么坐标转换公式为：

$\{\begin{array}{c}x=x\text{'}\cos \theta -y\text{'}\sin \theta +x_0\\ y=y\text{'}\cos \theta +x\text{'}\sin \theta +y_0\end{array}$ (3)

同样的，从XOY坐标系转到X'O'Y'坐标系的公式为：

$\{\begin{array}{c}x\text{'}=\left(x-x_0\right)\cos \theta +\left(y-y_0\right)\sin \theta \\ y\text{'}=-\left(x-x_0\right)\sin \theta +\left(y-y_0\right)\cos \theta \end{array}$ (4)

Figure 7. Rotation and translation of rectangular coordinate system X'O'Y' relative to XOY

图7. 直角坐标系X'O'Y'相对于XOY旋转及平移

3.2. 实际问题描述

如下图8所示，5台机床左下角均贴有该标签，上面是一个条形码，下面是一个十字架。AGV移动到机床前停止后，机器人会带相机运动到拍照点。通过IO命令打开视觉光源，机器视觉通过拍照，识别出来条形码的数值1~7，其中1~5为机床位置，6为检测位置，7为立体库出入口位置。

机器视觉从十字光标获得该标签值的坐标(x, y)和旋转值θ。因AGV的每次移动误差，第一次校准值(x0, y0, θ0)和后面每次过去的值(xn, yn, θn)均有偏差。

Figure 8. Machine identification label

图8. 机床识别标签

问题描述：

以一台机床为例，第一次校准时，获得十字标签的坐标和旋转量，通过机器视觉的转换，将获得值转换为机器人坐标系下。然后示教机床卡盘的机器人末端坐标。获得量均为协作机器人坐标系下的值。

每个位置点都需要校准后才能正常运行。

1. 十字光标通过相机拍照后转换为机器人坐标系坐标。

2. 机床校准的卡盘坐标通过机器人示教后获得。

已知机床标签校准值(tx, ty, tθ)，机床卡盘坐标(Px, Py)，第n次机床标签纸(tnx, tny, tnθ)。

求每次即第n次的机床卡盘坐标(Pnx, Pny)，如下图9所示。

Figure 9. Target coordinates and cross label coordinates in the robot coordinate system

图9. 机器人坐标系下的目标坐标和十字标签坐标

3.3. 坐标转换算法推导(0 < 旋转角度 < 90)

通过以下描述将实际问题转换为通用的直接坐标系转换，如下图10所示：

1. (Px, Py)在以(tx, ty)为原点的坐标系XOY中的坐标为(x, y) = (Px − tx, Py − ty)。

2. (Pnx, Pny)在以(tnx, tny)为原点的坐标系XOY^*中的坐标为(x^*, y^*) = (Pnx − tnx, Pny − tny)。

3. 旋转角度因为相机的镜像会取差值的负值θ = −ac = tθ− tnθ。

4. 坐标系XOY^*相对于坐标系XOY原点的偏移(x0, y0) = (tnx − tx, tny − ty)。

Figure 10. Corresponding to rotation and translation of coordinate system

图10. 坐标系旋转平移对应

得到如下变量描述：

$\{\begin{array}{c}\begin{array}{c}\left(x,y\right)=\left(Px-tx,Py-ty\right)\\ \left(x^{\ast },y^{\ast }\right)=\left(Pnx-tnx,Pny-tny\right)\end{array}\\ \begin{array}{c}\theta =-ac=t\theta -tn\theta \\ \left(x0,y0\right)=\left(tnx-tx,tny-ty\right)\end{array}\end{array}$ (5)

第N次机床卡盘坐标(Pnx, Pny)的算法：

此坐标也是机器人末端坐标，设：

$\{\begin{array}{c}ax=Px-tnx\\ ay=Py-tny\\ ac=tn\theta -t\theta \end{array}$ (6)

1. 通过公式3可得：

$\{\begin{array}{c}x=x\ast \cos \theta -y\ast \sin \theta +x0\\ y=x\ast \sin \theta +y\ast \cos \theta +y0\end{array}$ (7)

2. 带入公式5所描述变量：

$\{\begin{array}{c}Px-tx=\left(Pnx-tnx\right)\cos \theta -\left(Pny-tny\right)\sin \theta +\left(tnx-tx\right)\\ Py-ty=\left(Pnx-tnx\right)\sin \theta +\left(Pny-tny\right)\cos \theta +\left(tny-ty\right)\end{array}$ (8)

3. 两边分别减去(tnx − tx)和(tny − ty)：

$\{\begin{array}{c}Px-tnx=\left(Pnx-tnx\right)\cos \theta -\left(Pny-tny\right)\sin \theta \\ Py-tny=\left(Pnx-tnx\right)\sin \theta +\left(Pny-tny\right)\cos \theta \end{array}$ (9)

4. 使用ax和ay分别替代(Px − tnx)和(Py − tny)，公式两边分别乘以cosθ和sinθ：

$\{\begin{array}{c}\begin{array}{c}ax\ast \cos \theta =\left(Pnx-tnx\right)\cos {\theta }^2-\left(Pny-tny\right)\sin \theta \ast \cos \theta \\ ax\ast \sin \theta =\left(Pnx-tnx\right)\cos \theta \ast \sin \theta -\left(Pny-tny\right)\sin {\theta }^2\end{array}\\ \begin{array}{c}ay\ast \cos \theta =\left(Pnx-tnx\right)\sin \theta \ast \cos \theta +\left(Pny-tny\right)\cos {\theta }^2\\ ay\ast \sin \theta =\left(Pnx-tnx\right)\sin {\theta }^2+\left(Pny-tny\right)\cos \theta \ast \sin \theta \end{array}\end{array}$ (10)

5. 公式10中第1行和第4行相加，第3行和第2行相减：

$\{\begin{array}{c}ax\ast \cos \theta +ay\ast \sin \theta =\left(Pnx-tnx\right)\\ ay\ast \cos \theta -ax\ast \sin \theta =\left(Pny-tny\right)\end{array}$ (11)

6. 调换一下两边变量：

$\{\begin{array}{c}Pnx=ax\ast \cos \theta +ay\ast \sin \theta +tnx\\ Pny=ay\ast \cos \theta -ax\ast \sin \theta +tny\end{array}$ (12)

7. 带入ac = −θ后得到：

$\{\begin{array}{c}Pnx=ax\ast \cos \theta -ay\ast \sin \theta +tnx\\ Pny=ay\ast \cos \theta +ax\ast \sin \theta +tny\end{array}$ (13)

3.4. 坐标转换算法推导(旋转角度 = 0)

当ac = 0时，从公式13可得：

$\{\begin{array}{c}Pnx=ax+tnx\\ Pny=ay+tny\end{array}$ (14)

带入公式6可得：

$\{\begin{array}{c}Pnx=Px\\ Pny=Py\end{array}$ (15)

当ac = 0时，按带角度旋转的公式推导并不正确，正确的如下：

$\{\begin{array}{c}Pnx=Px+\left(tnx-tx\right)\\ Pny=Py+\left(tny-ty\right)\end{array}$ (16)

如果角度不变，几乎是在没有移动位置的时候进行的，按角度推导也是正确的。

3.5. 算法实现的代码

Decompose p [0] = target_s [weizhi]

该机器人指令是分解数组，target_s [weizhi]为不同机床卡盘的目标位置，机器人的末端坐标格式[x, y, z, rx, ry, rz]，将该位置的六个值放置到数组p [0]中，该算法主要去头两个值，即卡盘坐标的x和y。其中p [0] = x，p [1] = y。

ax = p [0] − markp. x [weizhi]

ay = p [1] − markp.y [weizhi]

以上两条指令中的markp为第n次相机拍照的机床标签，包括4个值，其中weizhi是条形码的值，代表不同的机床。x和y分别为标签十字架转换成机器人坐标系的相应的值。C为十字架的旋转角度。

可得ax，ay为第n次标签坐标和机床卡盘坐标的差值。

ac = trot − markp.c [weizhi]

该机器人指令中的markp.c [weizhi]为校准时保存的十字架的旋转角度，trot为第n次拍照的十字架的旋转角度。

$px=ax\ast \cos \left(ac\right)-ay\ast \sin \left(ac\right)+tx$

$py=ax\ast \sin \left(ac\right)+ay\ast \cos \left(ac\right)+ty$

根据公式13可以得到以上的第n次目标机床的x，y值。

Point target = Trans (px, py, p [2], p [3], p [4], p [5] − ac)

将分解指令中相关的值修改后放到以上指令中，得到机床卡盘最终的六个机器人坐标系值，机器人走到该位置及可以准确到走到卡盘位置。

4. 实验结果

TPWrite 0, “num:%d, case:%d, dpx:%.2f, dpy:%.2f, dtx:%.2f, dty:%.2f, dtrot:%.2f”

, n/4, weizhi, px-p [0], py-p [1], tx-markp.x [weizhi], ty-markp.y [weizhi], ac

该机器人程序指令，是向显示端0，即示教器日志打印以下数值，这些数值是相关点的差值，方便参考偏差评估。上面一行是各个值的输出格式描述，下面的值是机器人存取的相关值进行计算得到。

不同的值描述如下：

$\{\begin{array}{c}\begin{array}{c}\text{weizhi}=1-5\\ \Delta x1=px-p\left[0\right]\\ \Delta y1=px-p\left[1\right]\end{array}\\ \begin{array}{c}\Delta x2=tx-\text{markp}\text{.x}\left[\text{weizhi}\right]\\ \Delta y2=ty-\text{markp}\text{.y}\left[\text{weizhi}\right]\\ \Delta \theta =trot-\text{markp}\text{.c}\left[\text{weizhi}\right]\end{array}\end{array}$ (17)

1) weizhi代表从不同的操作点，五个机床分别为weizhi = 1到weizhi = 5，检测点weizhi = 6，立体库出入点weizhi = 8，本文仅关注机床的上下料，故序号为1~5。该值由视觉相机从标签的条形码中读取。

2) Px为经过坐标变换后的机床卡盘的位置的x坐标，p [0]为在校准的时候示教的机床卡盘的位置x坐标，故∆x1为每次在该机床时候，程序打印出来校准指和实际值的偏差。

3) Py为经过坐标变换后的机床卡盘的位置的y坐标，p [1]为在校准的时候示教的机床卡盘的位置y坐标，故∆y1为每次在该机床时候，程序打印出来校准指和实际值的偏差。

4) tx为校准时候从视觉相机读取的标签十字位置的坐标x，markp.x [weizhi]为本次相机读取的标签十字位置的坐标x。代表每次AGV移动的偏差。

5) ty为校准时候从视觉相机读取的标签十字位置的坐标y，markp.y [weizhi]为本次相机读取的标签十字位置的坐标y。代表每次AGV移动的偏差。

trot为校准时候从视觉相机读取的标签十字位置的在z即竖直方向上的旋转角度，markp.c [weizhi]为本次相机读取的标签十字位置的在z即竖直方向上的旋转角度。代表每次AGV移动的偏差。

Table 1. First-round bias after calibration

表1. 校准后第一轮偏差值

Table 2. Second-round bias after calibration

表2. 校准后第二轮偏差值

Table 3. Third round bias after calibration

表3. 校准后第三轮偏差值

Table 4. Fourth round bias after calibration

表4. 校准后第四轮偏差值

经过多次的测试发现见上表1、表2、表3、表4，偏差在10 mm以内可以正常抓取，当角度和x，y偏差超过10 mm或10度以后，需要进行重新校准才能正常工作。中间发现一个较为重大的问题，是因为AGV有跷跷板结构，在位置偏差较大后，机器人会出现前倾，导致后续数据持续紊乱，在循环10次后就会出现误差超过10 mm现象，需要重新校准。

5. 结论

在机床上下料应用中，一般方案使用导轨方式移载机器人进行上下料。该方案仅需增加机器人外部轴即可精确实现机器人末端抓取精度在0.3 mm左右。但是此方案配置导轨需定制，不方便进行柔性改造.现有的AGV移栽协作机器人方案的难点在于AGV移栽精度在 + − 10 mm左右，若没有基于视觉的纠偏算法，经过现场验证，末端精度通过机器人连杆的延长放大，可以到达20~40 mm，根本无法完成工件抓取.机床物料工件抓取需要末端精度小于2 mm，才能配合。通过本文所使用的纠偏算法，可以使机器人末端精度达到1 mm左右，精确完成机器人上下料工作。为移载协作机器人在机床上下料领域打下坚实基础。同时也可以扩展到移载协作机器人的其他场景应用，如超市货架上下料，加油站改造等。

本文成功地提出了一种基于十字标进行机床卡盘位置坐标转换的方法，有效地解决了机床内部空间狭小和定位特征不清晰的问题。通过在机床卡盘附近设置十字标，可以将机床的实际坐标转换为对应的坐标系，为后续的加工操作提供准确的坐标信息。此外，本文还对该方法进行了理论分析和实验验证，结果表明该方法对机床上下料的自动化、智能化具有较高的实用价值和参考性。对其他行业中的类似场景同样具有较高的参考价值，如在医疗领域中的血管试剂的机器人取放。

致谢

李晨杰完成了协作机器人的夹具，视觉的安装调试，验证了算法，通过代码对算法进行了实现。对算法提供了理论并实际验证。唐渝伟对协作机器人的相关数据格式进行深度讲解，并共同探讨该算法，并进行了实际验证。彭卓然对立体仓库和AGV进行设置，调试并提供了稳定的测试环境和相关接口。周昊晖对机床进行设置调试，并提供了稳定的测试环境和相关接口。余杰先设计了该协作机器人并进行集成，提供了实验的基础。张旭提供了相关的撰写思路，李树强和李腾飞代表羽承公司对该文章进行了资助并有意引入该算法到相关的医疗设备中。

参考文献