应用于机器人搬运打磨领域的板状工件三维识别定位方法与流程

本发明属于计算机视觉领域，具体的说涉及一种基于点云的目标识别定位算法。

背景技术：

随着科学技术的发展和工业自动化水平的不断提高，机器人在各个行业中得到了广泛的应用，机器人作为工业生产系统中的一个标准部件，通过网络或者工业总线将生产线上各种设备的控制系统有效连接，形成一个综合控制系统，已成为现代生产装备的发展趋势。传统机器人工作是以“示教——再现”的方式进行，即先由操作人员引导机器人进行示教，或通过离线软件编程的方式进行示教，然后再由机器人再现示教内容。这种方式通常利用机械工具将作业目标重复的固定在示教时的位置和姿态，从而达到对目标定位的目的。这种作业目标的机械定位方式存在诸多缺点：(1)需要专门的夹具，必需随着作业目标的改变而变化；(2)接触式定位，易损伤作业目标；(3)适用的作业目标具有一定的局限性，比如很难用于柔性目标；(4)定位过程耗时长等等。

视觉定位是一种基于计算机视觉的非接触式定位技术，该技术的一般性原理是首先通过视觉传感器获取目标在空间中的信息；然后通过一系列算法识别目标并计算出目标在空间中的位置和姿态；进而结合离线标定模型计算出目标在机器人坐标系下的位置和姿态；最后通过闭环控制，引导机器人进行轨迹修整，完成预定作业。相比机械定位，视觉定位技术具有诸多优点：(1)柔性好，适用于多种作业目标；(2)非接触测量，对目标无损伤；(3)可以实时测量，反馈给机器人用于闭环控制等等。因此，视觉定位是机器人的一项关键单元技术，在机器人应用中具有举足轻重的地位。

本发明正是通过视觉传感器获取目标的三维点云信息，从而实现对大型板状工件的三维识别定位的一种方法，应用于机器人搬运打磨领域，为搬运打磨整套系统提供数据支撑。相比于已有的三维识别定位算法，该发明不仅可以高效准确的提供目标在三维空间中的信息，而且能够自动规划出机器人的打磨行走路径，在整个搬运打磨系统中是至关重要的一环。多年来，中国科学院沈阳自动化所结合实际工程应用背景，在光电信息处理和机器人智能制造领域，取得了一系列研究成果。本发明是在已有的科研成果基础上，大胆创新，经过反复验证，提出了一种全新的板状工件三维识别定位算法，在实际的工业生产中，具有重要意义。

技术实现要素：

一种应用于机器人搬运打磨领域的板状工件三维识别定位方法，其特征在于能够将在料箱中摆放的多垛大型板状工件逐一识别定位，计算出工件在三维坐标下4个自由度(x、y、z和Rz)的位置和姿态，建立工件的数据模型，为后续打磨等机器人操作规划路径，从而引导机器人以相应的位姿进行识别抓取和分类，根据规划的路径对工件进行打磨。

本发明为实现上述目的所采用的技术方案是：

应用于机器人搬运打磨领域的板状工件三维识别定位方法，其特征在于：在机器人末端设置结构光传感器，通过光传感器实时采集图像进行工件的三维识别定位；包括以下步骤：

点云的获取和数据预处理：通过控制机械手带动结构光传感器在不同类型的工件上逐行扫描，将结构光传感器获取的多行三维数据进行拼接，形成工件的点云数据；

模板制作和路径规划：每个类型选取一个工件的点云数据作为该型号工件的模型，通过扫描得到该工件的模板点云数据，用模板点云数据创建模板，该模板包括唯一描述该型号工件的模型信息。

实时检测：通过控制机械手带动结构光传感器扫描得到待测工件的点云数据，通过模板匹配得出待测工件信息，包括型号、位姿、打磨路径，将待测工件信息组成工件的实时场景模型信息发送给机器人，用于引导机器人抓取。

所述点云的获取和数据预处理包括以下步骤：

点云的获取：将结构光传感器固定于机器人手臂末端，根据传感器坐标系与机器人用户坐标系之间的转换关系，将传感器采集到的行扫描工件数据转换成机器人用户坐标系下的行点云数据；

数据预处理：将结构光传感器在工件上逐行扫描获取的行点云数据根据y坐标进行拼接，形成以机器人用户坐标系为基准的工件点云数据；对将其规范为等采样间隔的工件点云数据。

所述模板制作和路径规划包括以下步骤：

在每个类型的工件选取一个工件点云数据中作为该型号工件的模板点云数据，向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系；

创建深度图像的金字塔，对每层金字塔利用canny算子提取深度图像边缘轮廓点，去除轮廓点上的噪声，计算边缘点上每个点的梯度方向；根据边缘点得到目标的形心及离散角度步长与尺度步长，并依据这些信息对轮廓点进行旋转与尺度变换获得工件轮廓模板；

在深度图像边缘轮廓点上，示教机器人行走边缘上的点，在邻近示教点设定距离内选取机器人运动的点，形成机器人运动路径；将得到的路径反向映射到点云的三维空间中形成三维路径。

所述创建深度图像的金字塔通过下式实现：

由若干个临近的像素的像素值计算均值构成金字塔上一层对应位置的像素值，表达式如下：

式中，Il^m(x,y)表示金字塔第l层深度图像中(x,y)像素点处的灰度值。

所述根据边缘点得到目标的形心及离散角度步长与尺度步长包括以下步骤：

形心(ox,oy)即为轮廓点的中心，其表达式如下：

式中，n为轮廓点数目，(xi,yi)为轮廓点像素坐标；

离散角度步长Δθ表达式如下：

式中，ε为最大允许距离，rmax为轮廓点与形心之间最大距离；

尺度步长表达式如下：

式中，Δsx、Δsy分别为第l层金字塔x、y方向上的尺度步长，(x,,y,)为距离形心最远的像素点坐标，θ为旋转角度即离散角度步长。

所述对轮廓点进行旋转与尺度变换获得工件轮廓模板包括以下步骤：

对于轮廓像素点坐标(xi,yi)，旋转角度为θ，x、y方向的尺度变换因子分为sx、sy时，旋转与尺度变换后的工件轮廓像素点坐标(xi,,yi,)为：

所述实时检测包括以下步骤：

将实时采集得到的待测工件点云数据向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系；

创建深度图像的金字塔，对每层金字塔利用sobel算子提取边缘，并计算各层金字塔边缘所有点的梯度和方向，得到实时图像轮廓点；依次将各工件轮廓模板与实时图像的轮廓点进行比较，找到与实时图像轮廓点最相似的工件轮廓模板，确定工件类型，计算工件轮廓模板位姿与实时深度图像中目标位姿的偏差，根据偏差得到实时深度图像中目标的形心和姿态；

将实时深度图像中目标的形心和姿态映射到三维空间中，将实时深度图像对应的待测工件信息包括型号、位姿、打磨路径组成工件的实时场景模型信息发送给机器人，用于引导机器人抓取。

所述找到与实时图像轮廓点最相似的模板包括以下步骤：

定义匹配分值：工件轮廓模板的轮廓像素点坐标为其梯度向量为ti、ui分别为x方向、y方向的梯度；实时图像轮廓像素点坐标为(x,y)，其梯度向量为d^s(x,y)＝(v(x,y),w(x,y))^T；v(x,y),w(x,y)分别为x方向、y方向的梯度；

对于实时图像(x,y)位置处，匹配分值s(x,y)的表达式如下：

选择匹配分值最大的工件轮廓模板。

本发明具有以下有益效果和优点：

1.本发明可以在料箱中交叉放置的多垛不同形状的工件分别进行三维空间的识别定位。

2.在对工件识别定位的同时，自动规划机器人打磨行走路径。计算出工件边缘是由多少条边构成，每条边是圆弧还是直线或者其他不规则线段，将这些边按照一定规则进行排序，分别给出每条边所对应的打磨路径。

3.在对工件定位的同时，识别出工件型号，引导机器人根据所属型号在打磨完成后，对工件进行分类放置。

附图说明

图1为搬运打磨系统框图；

图2为点云获取原理图；

图3为创建模板流程图；

图4为深度图像金字塔示意图；

图5为形心及尺度步长计算示意图；

图6为角度步长计算示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

如图1所示，本发明面向实际工程应用，采用建立三维数据模型，自动规划路径等技术手段，提出了一种应用于机器人搬运打磨领域的板状工件三维识别定位方法，不仅可以在复杂的场景中对多个待测目标逐一识别，并能够计算出目标在三维空间中的位姿，还能够规划出机器人对目标的打磨路径，引导机器人在对目标目标打磨完成后进行分类。该方法提供了机器人搬运打磨分类过程中的所需的必要信息，实现了机器人搬运打磨过程中的全自动化作业，能够适应工业现场复杂多变的环境。

本发明包括以下几个部分：1、点云的获取和数据预处理；2、模板制作和路径规划；3、实时检测。本发明应用于机器人搬运打磨领域，提供了工件从上料到打磨直至分类下料整个过程中所用到的工件数据模型，为实现机器人全自动化作业提供了感知功能。

点云的获取和数据预处理。点云数据获取采用结构光三角测量原理，所用的结构光传感器为线结构光传感器(下简称为传感器)，将结构光传感器固定于机器人机械手末端，通过机械手末端在料框上匀速运动，带动传感器在料框中摆放的多垛板状工件上方扫描，将机械手在不同位置上传感器获取的三维数据进行拼接，形成完整的场景点云数据。包括以下步骤：

将传感器固定于机器人手臂末端，对传感器和机器人手臂末端进行标定，标定的目的在于建立传感器坐标系与机器人用户系之前的对应关系，从而将传感器采集到的点云数据转换到机器人用户坐标系下。

数据预处理。将传感器在不同位置上获取的点云数据根据y坐标进行拼接，形成以机器人用户坐标系为基准的完整的点云场景数据。对场景点云数据进行重采样和滤波，去除点云噪声并将点云规范为等采样间隔的点云数据。

模板制作和路径规划。通过在各类型号工件中每种工件选取一个作为该型号工件的模型，通过数据扫描得到模板点云数据，用模板点云数据创建模板，该模板可以唯一描述该型号工件的模型信息。包括以下步骤：

将得到的模板点云数据向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系。

创建深度图像的金字塔，对每层金字塔利用canny算子提取深度图边缘轮廓点，去除轮廓点上的噪声，计算边缘点上每个点的梯度方向，根据边缘点计算目标的形心及离散角度步长与尺度步长，并依据这些信息对轮廓点进行旋转与尺度变换获得模板。

在深度图边缘轮廓点上，通过人机交互方式，示教机器人行走路径上的点，在示教点附近搜索最合适机器人运动的点，用这些搜索到的点形成机器人运动路径。

将得到的路径通过模板制作和路径规划步骤一反向映射到点云的三维空间中。

实时检测。扫描得到场景中的数据，通过模板计算出工件的型号、位姿、打磨路径等信息。组成工件的实时场景模型信息发送给机器人，引导机器人将工件抓取、打磨并将不同工件分类放置。包括以下步骤：

将实时采集得到的点云数据向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系。

与模板制作和路径规划中同样方法创建深度图像金字塔，利用sobel算子计算各层金字塔的梯度和方向，对梯度和幅值进行非极大值抑制，最后得到实时图像轮廓点。依次将各个事先做好的模板与实时图像的轮廓点进行比较，找到与实时深度图像轮廓点最相似的模板，确定目标型号，计算模板位姿与实时深度图像中目标位姿的偏差，根据偏差算出实时深度图像中目标的形心和姿态。

将实时深度图像中目标的形心和姿态映射到三维空间中，用工件的型号、位姿、打磨路径等信息组成工件的实时场景模型信息发送给机器人，引导机器人将工件抓取、打磨并将不同工件分类放置。

创建深度图像金字塔，由四个临近的像素的像素值计算均值构成金字塔上一层对应位置的像素值，计算表达式如下：

式中，I(x,y)表示图像中(x,y)像素点处的灰度值，l表示图像金字塔层数。

计算形心及离散角度，尺度和步长。形心(ox,oy)即为轮廓点的中心，其表达式如下：

式中，n为轮廓点数目，(xi,yi)为轮廓点像素坐标。

尺度步长表达式如下：

式中，Δsx、Δsy分别为第l层金字塔x、y方向上的尺度步长，(x,,y,)为距离形心最远的像素点坐标，θ为旋转角度。

角度步长表达式如下：

式中，ε为最大允许距离，rmax为轮廓点与形心之间最大距离，其表达式为：

对于轮廓像素点坐标(xi,yi)，旋转角度为θ，x、y方向的尺度变换因子分为sx、sy时，旋转、尺度变换后的轮廓像素点坐标(xi,,yi,)为：

为了找到与实时深度图像轮廓点最相似的模板，定义匹配分值。模板图像的轮廓像素点坐标为其梯度向量为实时图像轮廓像素点坐标为(x,y)，其梯度向量为d^s(x,y)＝(v(x,y),w(x,y))^T。对于实时图像(x,y)位置处，匹配分值s(x,y)的表达式如下：

一、点云的获取和数据预处理

点云数据获取采用结构光三角测量原理，原理如图2所示，所用的结构光传感器为线结构光传感器(下简称为传感器)，将结构光传感器固定于机器人机械手末端，通过机械手末端在料框上匀速运动，带动传感器在料框中摆放的多垛板状工件上方扫描，将机械手在不同位置上传感器获取的三维数据进行拼接，形成完整的场景点云数据。

步骤一：将传感器固定于机器人手臂末端，对传感器和机器人手臂末端进行标定，标定的方法为在料箱中建立传感器坐标系，将机器人用户坐标系与传感器建立的坐标系重合，标定的目的在于建立传感器坐标系与机器人用户系之前的对应关系，从而将传感器采集到的点云数据转换到机器人用户坐标系下。机器人匀速从料箱上方带动传感器扫描而过，获取每一时刻激光线照射部位的点云数据。

步骤二：数据预处理。将传感器在不同位置上获取的点云数据根据y坐标进行拼接，形成以机器人用户坐标系为基准的完整的点云场景数据。对场景点云数据进行重采样和滤波，去除点云噪声并将点云规范为等采样间隔的点云数据，采集到的点云数据。

二、模板制作和路径规划

步骤一：将得到的模板点云数据向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系。

步骤二：模板制作。模板制作流程如图3所示，主要分为四个部分：

(1)创建深度图像的金字塔，

如图4所示，由四个临近的像素的像素值计算均值构成金字塔上一层对应位置的像素值，计算表达式如下：

式中，I(x,y)表示图像中(x,y)像素点处的灰度值，l表示图像金字塔层数。

(2)对每层金字塔利用canny算子提取深度图边缘轮廓点，去除轮廓点上的噪声。，

(3)计算形心及离散角度、尺度步长。如图5所示，形心(ox,oy)即为轮廓点的中心，其表达式如下：

式中，n为轮廓点数目，(xi,yi)为轮廓点像素坐标。

尺度步长表达式如下：

式中，Δsx、Δsy分别为第l层金字塔x、y方向上的尺度步长，(x,,y,)为距离形心最远的像素点坐标，θ为旋转角度。

如图6所示，角度步长表达式如下：

式中，ε为最大允许距离，rmax为轮廓点与形心之间最大距离，其表达式为：

(4)轮廓点旋转、尺度变换获得模板。对于轮廓像素点坐标(xi,yi)，旋转角度为θ，x、y方向的尺度变换因子分为sx、sy时，旋转、尺度变换后的轮廓像素点坐标(x’i,y’i)为：

步骤三：在深度图边缘轮廓点上，通过人机交互方式，示教机器人行走路径上的点，在示教点附近搜索最合适机器人运动的点，用这些搜索到的点形成机器人运动路径。

步骤四：将得到的路径通过模板制作和路径规划步骤一反向映射到点云的三维空间中。

三、实时检测

步骤一：将实时采集得到的点云数据向坐标系x轴和y轴组成的平面做投影，设定Z方向上的有效检测范围，将有效范围内的Z值对应于z∈[0,255]之间，创建点云的深度图像，建立点云三维空间和点云深度图之间一一对应的映射关系。

步骤二：创建深度图像金字塔，方法如制作模板步骤二所示，利用sobel算子计算各层金字塔的梯度和方向，得到实时图像轮廓点。依次将各个事先做好的模板与实时图像的轮廓点进行比较，找到与实时深度图像轮廓点最相似的模板，采用匹配分值判定目标与模板的相似性，匹配分值的计算方法为：

假设模板图像的轮廓像素点坐标为其梯度向量为实时图像轮廓像素点坐标为(x,y)，其梯度向量为d^s(x,y)＝(v(x,y),w(x,y))^T。对于实时图像(x,y)位置处，匹配分值s(x,y)的表达式如下：

根据匹配到的模板确定目标型号，计算模板位姿与实时深度图像中目标位姿的偏差，根据偏差算出实时深度图像中目标的形心和姿态。

步骤三：将实时深度图像中目标的形心和姿态映射到三维空间中，用工件的型号、位姿、打磨路径等信息组成工件的实时场景模型信息发送给机器人，引导机器人将工件抓取、打磨并将不同工件分类放置。