CN204585232U - 抓取在线工件的机器人位姿及运动轨迹定位系统 - Google Patents

Abstract

本实用新型公开了一种抓取在线工件的机器人位姿及运动轨迹定位系统，包括用于实时拍摄工件的相机、用于抓取工件的机器人系统、用于根据相机所拍摄图像得到机器人系统抓取坐标的图像处理器、工作台，相机固定在一相机支架上，相机支架和机器人系统分别设置于工作台的两侧；图像处理器分别与相机和机器人系统中的机器人控制装置相连；工作台上设有工件传输带，工件在工件传输带上运动；机器人系统的末端设有用于夹紧及旋转工件的末端执行器；所述相机的光轴与工件运动的方向垂直，相机用于获取工件的轴侧图像。本实用新型采用单相机获取输送运动中不确定位姿的工件信息，引导机器人采用智能地对工件进行准确抓取，具有抓取精度高、结构简单的优点。

Description

抓取在线工件的机器人位姿及运动轨迹定位系统

技术领域

本实用新型机器人运动控制以及视觉定位领域，特别涉及一种抓取在线工件的机器人位姿及运动轨迹定位系统。

背景技术

目前，工业机器人在各工业领域中获得了广泛应用，其可代替人工做出可重复的精确的运动。机器人要完成三维工件的智能抓取，必须沿特定轨迹、采用相应的位姿及运动轨迹进行运动。目前在静态三维工件的抓取中，工业机器人抓取位姿及运动轨迹的定位有在线示教、离线编程；在运动工件的在线实时抓取中，目前有单目视觉引导系统、双目视觉引导系统以及结构光与视觉结合的引导系统。然而，现有工业机器人三维位姿及运动轨迹定位技术，存在多方面的不足：

1)在线示教不需标定机器人的工具坐标系，机器人的位姿、运动轨迹及夹紧参数依靠示教工程人员的经验目测获取，系统精度较低、在线编程繁琐、占用生产时间，难以满足当今快速而多品种变换的柔性生产需求，特别是不能满足自动抓取位姿不确定的在线输送工件的需求。

2)为了获得真实的机器人位置空间模型必须对机器人进行标定。常用的标定设备有三坐标测量机、关节臂测量机和激光跟踪仪，但用于对工作场景中的机器人标定受限制，位置标定误差仍较大。

3)单目视觉系统通常用于二维平面上的信息提取，无法获取工件在Z轴(深度方向)上的信息，传递给机器人的只有平面上的位置和角度量。相机光轴必须与工件平面保持严格垂直，深度位置只能靠经验值人为预先设定固定值。

4)结构光与单相机相结合获取工件信息的方式，需进行图形学解码运算。为保证解算精度，必须保持相机与工件相对位置固定，难以适用于运动工件的抓取，而且涉及三维点云计算，运算复杂耗时。

5)双目立体视觉系统，利用两个相机呈固定角度从不同视角拍摄工件图像，计算工件的三维坐标。要经过双目视觉标定、深度信息提取等过程，运算比较复杂，耗时较长，成本较高，而且鲁棒性较差。

因此，对于工业机器人自动抓取在线运动的三维工件，有发展工件位姿及运动轨迹在线快速识别装置的需求，以改善上述缺点与不足。

实用新型内容

本实用新型的目的在于克服现有技术的缺点与不足，提供一种抓取在线工件的机器人位姿及运动轨迹定位系统，该系统基于单目视觉系统，适用于利用工业机器人智能、快速地抓取制造车间最常见的生产输送线上已知形状而位姿不确定的中小型工件(可以不同形状、规格、材质的多种工件混合)。

本实用新型的目的通过以下的技术方案实现：抓取在线工件的机器人位姿及运动轨迹定位系统，包括用于实时拍摄工件的相机、用于抓取工件的机器人系统、用于根据相机所拍摄图像得到机器人系统抓取坐标的图像处理器、工作台，所述相机固定在一相机支架上，相机支架和机器人系统分别设置于工作台的两侧；所述图像处理器分别与相机和机器人系统中的机器人控制装置相连；所述工作台上设有工件传输带，工件在工件传输带上运动；所述机器人系统的末端设有用于夹紧及旋转工件的末端执行器；所述相机的光轴与工件运动的方向垂直，相机用于获取工件的轴侧图像。本实用新型采用单相机获取输送运动中不确定位姿的工件信息并计算得到其三维坐标，引导机器人采用正确的末端执行器位姿和运动轨迹智能地对工件进行准确抓取。

优选的，在所述工件传输带的两端设有用于探测工件是否进入相机视场的光电传感器，该光电传感器与相机外接触发头连接。从而可以通过传感器信号触发相机拍照。

本实用新型与现有技术相比，具有如下优点和有益效果：

(1)使机器人智能化自动工作，抓取精度高。本实用新型采用单相机获取输送运动中不确定位姿的工件信息并计算得到其三维坐标，引导机器人采用正确的末端执行器位姿和运动轨迹智能地对工件进行准确抓取。

(2)可多种目标混合识别，适应多品种柔性化生产需求。本实用新型设计单目视觉获取工件轴侧图像，根据不同的已知形状物体找出其特征，可适应于不同形状、规格、材质的多种工件混合输送场合的工件识别和机器人抓取引导。

(3)适应性广，应用条件宽泛。本实用新型对识别对象工件无材质、形状、尺寸的特定限制(但以中小型物体为佳)，对工件随传送带运动的速度以及在传 送带上的位置偏差大小也有比较大的适应性。

(4)结构简单，成本低廉。本实用新型采用单个普通相机，配用普通PC机，与常规工业机器人组成智能识别与抓取系统，无需其他辅助器件和软件平台。

附图说明

图1是本实用新型系统硬件总体结构示意图。

图2-1是本实用新型平面点在垂直方向上的深度定位模型原理图。

图2-2是本实用新型空间点在垂直方向上的深度定位模型原理图。

图2-3是本实用新型平面点在水平方向上的X轴向定位模型原理图。

图2-4是本实用新型空间点在水平方向上的X轴向定位模型原理图。

图3(a)是本实施例被测工件为立方体的示意图。

图3(b)是本实施例被测工件为其他规则体的示意图。

具体实施方式

下面结合实施例及附图对本实用新型作进一步详细的描述，但本实用新型的实施方式不限于此。

实施例1

参阅图1，本实施例抓取在线工件的机器人位姿及运动轨迹定位系统包括相机支架1、工业相机2、被测工件3、工件传输带4、末端执行器5、机器人系统6、工作台7、图像处理器8、工作台控制装置9、位移传感器10以及机器人控制装置11。工业相机2安装在相机支架1上，相机光轴与垂线呈适当下倾角度安装，使其能够获取输送带上工件的轴侧图像以及初始时的机器人末端执行器所在位置。相机与工业机器人分别安装在运动平台两侧，保证机器手臂不遮挡相机视野。光电传感器安装在传送带侧旁，用于探测工件是否进入相机视场。图像处理器分别与相机和机器人系统中的机器人控制装置相连，用于根据相机所拍摄图像得到机器人系统抓取坐标。末端执行器用于夹紧、旋转及放置工件。

下面对本实施例所述系统的工作过程进行具体说明。

S1、调整相机的安装位置和安装角度，使得工件进入相机视场后相机能获取工件的轴侧图像。

S2、将标定板放置于工件传输带上，且位于相机的视场中心，对相机-工件 传输带平面-末端执行器进行几何标定，标定得到相机的内参数f和外参数α、h、h₁。

S3、依据相机的安装空间位置和已知的工件形状尺寸，建立帧坐标系uo"v中工件特征图像坐标与绝对坐标系XYZ中工件三维位姿坐标之间的转换数学模型。

该数学模型具体包括四个模型，分别是绝对坐标系中的平面点在垂直方向上的深度定位模型、绝对坐标系中的空间点在垂直方向上的深度定位模型、绝对坐标系中的平面点在水平方向上的X轴向定位模型与绝对坐标系中的空间点在水平方向上的X轴向定位模型。根据上述模型，再结合工件的先验知识，从而将所有相机中成像的点的三维坐标求出。上述4个数学模型，本领域技术人员可以进行推导得到。具体推理过程如下：

(3-1)建立坐标系：绝对坐标系XYZ以相机镜头的光轴中心在工作台平面的垂直投影M为原点，以过相机镜头光轴线的垂直平面为参照，垂直于该平面的方向X轴，垂直于X轴的方向为Y方向，坐标表示为(X,Y),以长度为单位；像平面坐标系xo'y原点在CCD成像平面的中心，坐标表示为(x,y)；帧坐标系uo"v原点在图像平面的左上角，坐标表示为(u,v)，以像素为单位，o'在坐标系uo"v下的坐标为(u₀,v₀)；下面所述平面点为绝对坐标系XYZ中XMY平面上的点。

(3-2)建立绝对坐标系中的平面点在垂直方向上的深度定位模型。

参见图2-1，设绝对坐标系中的平面点P(0，Y，0)，则：

$\mathrm{\β}=\mathrm{\α}-\mathrm{\γ};\mathrm{\γ}=\arctan \frac{y}{f};Y=\frac{h}{\tan \mathrm{\β}}=\frac{h}{\tan (\mathrm{\α}-\arctan \frac{y}{f})}---\left(1.1\right)$

因为从图像上得到的并不是y值，而是像素的个数v，所需换算公式是：y＝(v-v₀)×d_y(其中d_y是像素的间距),f_y＝f/d_y。则如图2-1所示，绝对坐标系中的平面点P到原点M的距离为： 

$\mathrm{MP}=Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}---\left(1.2\right)$

其中，f_y＝f/d_y，d_y是像素的间距，f为通过标定得到的相机的内参数；v为平面点P在帧坐标系uo"v中的纵坐标。

(3-3)建立绝对坐标系中的空间点在垂直方向上的深度定位模型。

参见图2-2，因为在单目视觉的二维图像上的一个点P'往往要对应于三维空间中的许多点，设空间点P₁与平面点P都对应于成像平面的P'点，空间点P₁到绝 对坐标系中XMY平面的实际物理高度P₁T＝h₁，已知,工业相机镜头中心点O到绝对坐标系中XMY平面的实际物理高度OM＝h，则：

$\tan \mathrm{\β}=\frac{P_{1}T}{\mathrm{TP}}=\frac{\mathrm{OM}}{\mathrm{MP}}---\left(1.3\right)$

$\mathrm{TP}=\frac{P_{1}T}{\mathrm{OM}}\×\mathrm{MP}=\frac{h_1}{h}\×Y---\left(1.4\right)$

所以P1在XMY平面上的投影点T与原点M之间的距离为：

$\mathrm{MT}=\mathrm{MP}-\mathrm{TP}=Y-\frac{h_1}{h}\×Y=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h})---\left(1.5\right)$

其中，v为空间点P1在帧坐标系uo"v中的纵坐标。

(3-4)建立绝对坐标系中的平面点在水平方向上的X轴向定位模型。

参见图2-3，设一平面点Q(X,Y)，则：

$\mathrm{\β}=\mathrm{\α}-\mathrm{\γ};\mathrm{\γ}=\arctan \left(\frac{y}{f}\right)---\left(1.6\right)$

$\mathrm{OP}=\frac{\mathrm{OM}}{\sin \mathrm{\β}}=\frac{h}{\sin (\mathrm{\α}-\mathrm{\γ})}=\frac{h}{\sin (\mathrm{\α}-\arctan \frac{y}{f})}---\left(1.7\right)$

换成像素坐标：

$\mathrm{OP}=\frac{h}{\sin [\mathrm{\α}-\arctan (\frac{v-v_0}{f}\×d_y)]}=\frac{h}{\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}---\left(1.8\right)$

$P^{\′}O=\sqrt{P^{\′}{O}^{\′2}+O^{\′}{O}^2}=\sqrt{f^2+y^2}=\sqrt{f^2+{(v-v_0)}^2\×{d_y}^2}---\left(1.9\right)$

根据相似定理得：

$\frac{P^{\′}{Q}^{\′}}{P^{\′}O}=\frac{\mathrm{PQ}}{\mathrm{PO}}---\left(1.10\right)$

则

$\mathrm{PQ}=\frac{P^{\′}{Q}^{\′}}{P^{\′}O}\×\mathrm{PO}---\left(1.11\right)$

从图像上得到的并不是x值，而是像素的个数u，所需换算公式是：x＝(u-u₀)×d_x(其中d_x是像素的间距),d_x和d_y为摄像机内部参数,分别表示CCD两相邻感光点间的x方向距离和y方向距离,由于采用的是线性CCD摄像头,x方向和y方向是均匀相等的,故d_x＝d_y。

所以由式(1.11)得到平面点Q在XMY平面上X轴向的坐标为：

横轴坐标为： 

$\mathrm{PQ}=\frac{P^{\′}{Q}^{\′}}{P^{\′}O}\×\mathrm{PO}=\frac{(u-u_0)\×h}{\sqrt{{f_y}^2+{(v-v_0)}^2}\×\sin [\mathrm{\α}+\arctan (\frac{v_0-v}{f}\×d_y)]}---\left(1.12\right)$

其中，u、v为平面点Q在帧坐标系uo"v中的坐标。

(3-5)建立绝对坐标系中的空间点在水平方向上的X轴向定位模型。

因为在单目视觉的二维图像上的一个点Q'往往要对应于三维空间的许多的点，如图2-4所示，空间点Q₁与平面点Q都对应于成像平面中的Q'点，空间点Q₁到绝对坐标系中XMY平面的实际物理高度Q₁N＝h1，已知，相机镜头的光轴中心点O到绝对坐标系中XMY平面的实际物理高度OM＝h。

在OMQ平面中，由相似得

$\frac{Q_{1}N}{\mathrm{NQ}}=\frac{\mathrm{OM}}{\mathrm{MQ}}---\left(1.13\right)$

$\mathrm{QM}=\sqrt{{\mathrm{PM}}^2+{\mathrm{PQ}}^2}=\sqrt{X^2+Y^2}---\left(1.14\right)$

$\mathrm{NQ}=\frac{Q_{1}N\×\mathrm{QM}}{\mathrm{OM}}=\frac{h_1}{h}\×\sqrt{X^2+Y^2}---\left(1.15\right)$

$\mathrm{NM}=\mathrm{MQ}-\mathrm{NQ}=\sqrt{X^2+Y^2}\×(1-\frac{h_1}{h})---\left(1.16\right)$

则空间点Q1在XMY平面的垂直投影点N(X_e、Y_e)，由相似得：

$\frac{\mathrm{EN}}{\mathrm{NM}}=\frac{\mathrm{PQ}}{\mathrm{MQ}}---\left(1.17\right)$

$X_e=\mathrm{EN}=\frac{\mathrm{PQ}\×\mathrm{NM}}{\mathrm{MQ}}=X\×(1-\frac{h_1}{h})=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{u_0-u}{\mathrm{fx}})}\×(1-\frac{h1}{h})---\left(1.18\right)$

同理可得： 

$Y_e=\mathrm{EM}=\frac{\mathrm{PM}\×\mathrm{NM}}{\mathrm{MQ}}=Y\×(1-\frac{h_1}{h})=\frac{h}{\tan (\mathrm{\α}+\arctan \frac{v_0-v}{f_y})}\×(1-\frac{h_1}{h})---\left(1.19\right)$

其中，u、v为空间点Q1在帧坐标系uo"v中的坐标；f_x＝f/d_x，d_x是像素的间距，f为通过标定得到的相机的内参数。

S4、依据机器人在绝对坐标系中的空间位置，建立工件在绝对坐标系XYZ中三维位姿坐标与在机器人坐标系X₁Y₁Z₁中坐标之间的转换数学模型。这一模型同样为本领域技术人员通过推导即可得到。

具体方法是： 

绝对坐标系XYZ与机器人坐标系X₁Y₁Z₁的Z轴方向一致，通过平移与绕Z轴旋转来实现两个坐标系间的空间转换，其中转换数学模型是：设绝对坐标系XYZ为坐标系A，机器人坐标系X₁Y₁Z₁为坐标系B，则：

B＝Trans(ΔX,ΔY,ΔZ)Rot(Z,θ)A

其中Trans(ΔX,ΔY,ΔZ)为平移的齐次变换，其数学模型为：

$\mathrm{Trans}(\mathrm{\ΔX},\mathrm{\ΔY},\mathrm{\ΔZ})=\left[\begin{array}{cccc}1& 0& 0& \mathrm{\ΔX}\\ 0& 1& 0& \mathrm{\ΔY}\\ 0& 0& 1& \mathrm{\ΔZ}\\ 0& 0& 0& 1\end{array}\right]$

式中，元素ΔX,ΔY,ΔZ分别表示沿各自坐标轴X,Y,Z的移动量；

其中Rot(Z,θ)为绕Z轴进行转动的旋转算子，数学模型为：

$\mathrm{Rot}(Z,\mathrm{\θ})=\left[\begin{array}{cccc}\cos \mathrm{\θ}& -\sin \mathrm{\θ}& 0& 0\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

式中，元素θ为绕Z轴旋转的角度，上述参数ΔX、ΔY、ΔZ、θ通过标定求解出实际值。

S5、工件传送带以速度V携载工件平移运动，当探测到工件进入相机视场后相机获取工件的轴侧图像，提取该轴侧图像中工件的图像特征点。

例如图3(a)、(b)中的A、B、C、D、A'、B'、C'、D'。

S6、将目标点的图像坐标(u_i,v_i)和内外参数代入式(1.12)和式(1.2)，求出帧坐标所对应的世界坐标系中的平面点的坐标位置。例如可得到立方体C、D、C`、D`特征点相对于摄像机的垂直投影点建立的坐标系的三维坐标。

将目标点的图像坐标(u_i,v_i)和内外参数代入式(1.18)和式(1.19)，求得有一定高度的目标物相对于摄像机的坐标位置。例如可得到在空间中的A、B、A`、B`特征点的三维坐标。

根据各特征点在绝对坐标系中的三维坐标以及步骤S4建立的数学模型，得到机器人抓取时的工件在机器人坐标系X₁Y₁Z₁中的三维位姿及空间坐标,将此数据传输给机器人控制器，进而控制末端执行器抓取在线工件。

S7、为了更进一步提高抓取的精度，机器人控制器还实时采集当前末端执行器的位置，然后计算此实际位置和目标位置的偏差，根据此偏差反馈一运动 控制信号控制末端执行器运动，形成一闭环控制回路，直至末端执行器精密抓取工件。

上述实施例为本实用新型较佳的实施方式，但本实用新型的实施方式并不受上述实施例的限制，其他的任何未背离本实用新型的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本实用新型的保护范围之内。

Claims (2)

1.抓取在线工件的机器人位姿及运动轨迹定位系统，其特征在于，包括用于实时拍摄工件的相机、用于抓取工件的机器人系统、用于根据相机所拍摄图像得到机器人系统抓取坐标的图像处理器、工作台，所述相机固定在一相机支架上，相机支架和机器人系统分别设置于工作台的两侧；所述图像处理器分别与相机和机器人系统中的机器人控制装置相连；所述工作台上设有工件传输带，工件在工件传输带上运动；所述机器人系统的末端设有用于夹紧及旋转工件的末端执行器；所述相机的光轴与工件运动的方向垂直，相机用于获取工件的轴侧图像。

2.根据权利要求1所述的抓取在线工件的机器人位姿及运动轨迹定位系统，其特征在于，在所述工件传输带的两端设有用于探测工件是否进入相机视场的光电传感器，该光电传感器与相机外接触发头连接。