本发明涉及智能机器人领域，尤其是涉及一种基于rgbd相机的堆叠工件姿态识别及拾取方法。
背景技术：
随着机器人与人工智能技术如火如荼的发展，工业流水线也越来越向着自动化的方向发展，国外的工业4.0战略和我国的“中国制造2025”，都对机器人与人工智能技术在工业领域的应用提出了很高的要求。
在某些工业生产线中，我们需要对杂乱无章堆叠的、形态各异的工业零件进行分类和拾取，传统的工业生产线上依靠人力进行分拣的方式效率低、人力成本较高，越来越多的企业希望通过机器人去替代人力，实现工业零件的分类和拾取。
目前存在一些针对工件抓取的专利，但现存的针对工件抓取的专利来看，存在着或多或少的缺陷，往往无法适应复杂的工业应用场景，针对不同类型工件的鲁棒性和适应性不强。
从目前传感器的选用来看，主流是激光传感器和以工业摄像头为代表的视觉传感器。众所周知，虽然激光传感器能够获得高精度的数据，但鉴于其价格昂贵，从成本考虑，基于激光传感器的工件抓取解决方法并不适合在工业界的大规模推广。相比之下，以普通摄像机为代表的视觉传感器，成本更低，以此为传感器的工业工件抓取的解决方法更适应工业界的应用。
在现有基于视觉传感器的工业工件抓取方法中，有相当一部分针对的是平面空间中的视觉定位，即仅使用ccd摄像头，采集正对目标工件的平面图片，使用传统的图像处理与识别技术，例如灰度变换、图像二值化、边缘检测、模板匹配等，而且对盛放工件的平台背景颜色具有较高的要求，要求背景单一等等，从而识别出待测工件的简单轮廓，使用外接矩形的方法来进行定位与平面旋转角度的判断。此类方法的工业应用前景有限，在背景杂乱或者工件堆叠的情况下，通过边缘检测、模板检测的传统的图像处理方法，是很难将视野中的工件进行识别的；与此同时，边缘检测等算法中存在一些需要人工调节的参数，对于不同的抓取对象即不同形状、不同尺寸、甚至是多种类型工件混杂的情况往往难以适应。
另有基于双目视觉的工件抓取解决方法，利用双目视觉的解决方法，在一定程度上克服了单目视觉的弊端，可以在三维空间内对待抓取工件的位姿进行估计，但目前基于双目视觉的解决方法，其本质仍然是在单张的单目视觉图片中进行模板匹配，然后将双目摄像头的信息加以融合，以此仍然无法很好地解决工件混乱堆叠的问题。另外，采用双目视觉的方法，需要进行双目标定，双目标定的误差对最终工件识别和位姿估计的误差产生很大的影响。
综上所述，传统的解决方法或价格昂贵，或仅能得到工件的二维平面姿态，或是算法的准确性和鲁棒性不强，无法适应复杂的工业应用场景，无法保证对各种形状、各种类型的工件都能准确识别和解算位姿，无法满足工业生产的需求。
技术实现要素：
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于rgbd相机的堆叠工件姿态识别及拾取方法。
本发明的目的可以通过以下技术方法来实现：
一种基于rgbd相机的堆叠工件姿态识别及拾取方法，包括以下步骤：
1)对rgbd相机内参数进行标定，并且进行机器人与相机的手眼标定；
2)根据预先获得的待抓取工件的3d模型进行训练，模拟全方位摄像头进行全视角扫描，生成用以匹配的2d模型；
3)利用rgbd相机获取待识别工件的rgb图像和深度图像，将rgb图像与训练好的2d模型进行匹配，获取出待抓取工件的轮廓信息及粗略位姿信息；
4)获取待抓取工件在图像像素坐标系中的二维位置信息以及在相机坐标系下精确的六自由度位姿；
5)根据手眼标定和相机坐标系下六自由度位姿进行坐标转换获取待抓取工件在机器人坐标系下的六自由度位姿；
6)根据机器人坐标系下的六自由度位姿，控制六轴机器人对待抓取工件进行拾取。
所述的步骤3)具体为：
采用两级多尺度金字塔穷举算法分别根据姿态范围和图像尺寸将rgb图像与2d模型进行匹配，框选出待抓取工件的轮廓信息，包括抓取工件的重心在像素坐标系中的二维位置信息；
所述的步骤4)具体包括以下步骤：
41)根据待抓取工件在图像像素坐标系中的重心点通过相机参数进行坐标变换，同时结合深度图像的深度信息，获得其在摄像机坐标系中xyz轴的物理坐标；
42)以重心点作为参考点获取待抓取工件的边界角点在摄像机坐标系中的物理坐标，确定待抓取工件的中轴线，通过平面投影和深度信息采用最小二乘法拟合出中轴线的直线方程，得到摇摆、俯仰和偏转三个方向的角度，最终获取待抓取工件在摄像机坐标系下的六自由度位姿量。
所述的步骤5)中，通过手眼标定获取机器人坐标系到相机坐标系的变换矩阵具体为：
将标定板固定在机器人的末端执行器上，摄像头位置固定，控制末端执行器移动位置，改变标定板在摄像机坐标系中的位置，并记录末端执行器的位姿以及标定板在摄像头坐标系下的位姿，进而利用标定算法得获取机器人坐标系到相机坐标系的变换矩阵。
所述的待抓取工件为圆柱状的金属轴承。
所述的3d模型通过手动测量获取或通过solidworks绘制得到。
所述的步骤4)还包括以下步骤：
根据待抓取工件的自身位置信息以及堆叠的不同工件之间的相对位置关系，将匹配识别出的轮廓图内二维区域与参考模板在该姿态下的二维视图进行对比，遍历该区域内的像素，获取工件未被覆盖区域与轮廓区域总像素的比值，得到近似表示工件可视度的分数值，对可视度的分数值进行排序将堆叠的工件进行排序识别，实现从工件堆由上至下的识别和抓取。
与现有技术相比，本发明具有以下优点：
一、利用rgb图像与深度信息配合来获取检测区域内工件的信息；
二、使用图形学算法，通过对给定工件的cad模型进行训练，实现从rgb图像中识别出给定的工件，得到其二维位置信息；
三、利用相机参数以及2中获得的位置信息，解算出工件在空间中的六自由度位姿，实现配合机械臂的抓取；
四、识别匹配的处理时间较短，可以实现快速的姿态定位与抓取，在实际工业生产中能得到较高的效率；
五、识别匹配具有较高的精度，并且能够通过判断深度信息来进行排序定位，优先识别匹配散乱堆砌的工件堆中表面便于抓取的工件，实现成堆工件的有序识别；
六、识别匹配的过程具有较强的抗干扰性；
七、采用廉价的rgbd相机作为工件抓取解决方法的传感器，价格低廉，有利于在工业界的大规模推广；
八、基于cad模型的匹配思路使其在实际应用中拥有很高的适应性和迁移性，能够适应各种形状的工件，并可随时调整。
附图说明
图1为本发明的方法流程图。
图2为本发明的框架流程图。
图3为模拟全视角扫描图。
图4为摄像头获得的模拟图像。
图5为工件的坐标参数示意图。
图6为匹配过程框架图。
图7为图像生成流程图。
图8为像素坐标系、物理坐标系及其相互关系图。
图9为轴承角点示意图。
图10为轴承抓取点计算示意图。
图11为手眼标定示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
针对以上问题和需求，本发明提出一种基于rgbd相机的堆叠工件姿态识别及拾取方法，对已经加工完成但未进行分类整理的工业零件进行姿态识别，应用场景包括零件堆叠状态下的拾取与分类、零件单个传输过程中的定位拾取等。检测给定区域内符合指定模型的工件，将其识别并进行姿态的估计，得到工件质心的空间坐标，以及工件在检测区域内相对于指定模型姿态的俯仰角度以及旋转角度。通过和机械臂的配合工作，实现工件的定位抓取以及分类的功能。
如图1所示，本发明提供一种基于rgbd相机的堆叠工件姿态识别及拾取方法，该方法包括如下步骤：
(1)rgbd相机内参数标定，机器人与相机的手眼标定。
(2)利用预先获得的待抓取工件的cad模型进行训练，得到用于匹配的模型。
(3)利用rgbd相机获取待识别工件的rgb图像和深度图像。
(4)将二维彩色图像与训练好的模型进行匹配，框选出工件的轮廓，得到工件在二维平面上的像素位置以及在相机坐标系下粗略的六自由度位姿，并根据工件自身位置及堆叠工件的空间分布情况，对识别出的工件进行打分。
(5)利用rgbd相机的相机参数将二维位置信息映射到三维，找到三维点云中对应的坐标点，利用最小二乘法进行拟合，确定工件在三维空间中精确的六自由度位置和姿态信息。
(6)利用已得到的六自由度位姿信息，控制六轴机器人进行拾取。
本发明的详细技术说明将分为以下四部分：
1.使用cad模型进行目标工件的识别模板训练；
2.对rgb图像进行匹配识别，筛选出符合度最高的轮廓区域，同时获得待检测目标粗略的六自由度位姿；
3.基于匹配得到的结果，结合深度图精确计算待检测目标的六自由度位姿；
4.手眼标定技术介绍及实际操作步骤。
(1)3dcad模型的训练
该步骤的目的，目标工件实际的位姿状态存在许多种情况，首先我们要知道目标工件的具体形状，即需要预先给定3d的模型，再来判断摄像头视野中的二维平面在三维平面中的具体姿态，所以需要先通过对3d模型进行训练，模拟空间中全方位摄像头视角产生的二维视图，从而得到二维的样板形状；然后进行平面的形状匹配。
设计的算法与实现：基于3d的cad模型，将该模型假设作为一个球形空间中的圆心，球面为各个角度的虚拟摄像头对其进行图像采集，遍历每个角度的摄像头可以得到各个角度的不同姿态。如图3和图4所示，图3表示对3d模型进行全视角的二维视觉记录，若此时虚拟相机所处位置如图3红色标记点所示，则此时对应的2d工件模型为图4所示。
对3d模型进行全视角扫描时，为减少扫描生成的二维模型的大小，缩短训练的时间，提高后期匹配的效率，可对虚拟扫描相机的位姿进行限制，根据球面坐标系的定义，从经度(longitude)、纬度(latitude)和距离(distance)三个维度进行限制：longitude∈[λmin,λmax]，
(2)rgb图像匹配得到对象轮廓
根据(1)中所述算法可得到待检测三维对象在不同虚拟相机视角下的2d匹配模板，接下来利用该模板在rgb图像中匹配得到对象轮廓，本方法采用两级多尺度金字塔的穷举算法，寻找待检测对象的轮廓。
所谓两级多尺度金字塔，是指将(1)中训练得到的多姿态2d模板进行金字塔式的划分。第一级金字塔根据2d模板的姿态范围(指经度、纬度和距离的范围)进行划分，多尺度指划分为每个尺度所覆盖的姿态范围是不同的，这里以最大尺度为level4为例，则level4节点所覆盖的姿态范围为longitude∈[λmin,λmax]，
以上阐述了第一级根据姿态范围所划分的金字塔，接下来阐述第二级金字塔，第二级金字塔根据图像的尺寸进行划分，指将图像分成不同级别的尺寸，比如原始图像(尺度level1)是600×400，level2图像尺寸为300×200，level3图像尺寸为150×100，以此类推，进行多个尺度的划分。
以上阐述了两级多尺度金字塔式划分的含义，在此基础上对待检测对象进行穷举搜索。在根据姿态范围划分的金字塔上，从最高级level向level1进行遍历，按照自顶而下的搜索方向，直到找到level1中能够成功匹配的叶子节点；在根据图像尺寸划分的金字塔中，为了需求更好的速度和满足更高的精度，从金字塔最高级开始，即从较小的尺寸进行搜索，可以匹配比较鲜明的边缘特征，为后面在较大尺寸上进行搜索节约时间，从而整体上提高了搜索速度，也提高了准确性。
经过以上所算法，能够在二维rgb图像中检测出待识别对象的轮廓信息，并且能够得到工件的重心在图像坐标系中的坐标。以上为接下来六自由度位姿的计算做好准备。
(3)结合深度图计算工件的六自由度位姿
(31)位置信息的计算
在对工件实现抓取的过程中，我们需要确定工件的三个维度的信息，本方法中确定出工件重心的x、y、z三个维度的位置信息。通过步骤(2)的算法，我们已经能够确定工件重心在像素坐标系中的二维位置信息，接下来阐述如何利用所得二维位置信息得到工件在相机坐标系下的三维位置坐标。
首先介绍本方法基于rgbd相机采集的彩色数据流和深度数据流所涉及相机模型及其所涉及的几个坐标系，三维世界中的物体，其图像信息经过摄像机的采集最终成为二维像素坐标点呈现出来，其经历的流程如图7所示。
(311)图像像素坐标系
数字图像在计算机内存储为m×n的数组，m行n列的图像中的每一个元素(称为像素，pixel)的数值即是图像点的灰度值。如图所示，像素坐标系的坐标原点o0在图像的左上角，u轴为水平方向，v轴为垂直方向，该坐标系的基本单位为像素，矩阵下标为(m,n)的元素存储第(m,n)个像素点的值(如深度、灰度、颜色值等)。
(312)图像物理坐标系
图像物理坐标系，顾名思义，即以物理单位表示的图像坐标系，根据采集图像的视觉传感器的不同，基本单位是米或毫米等；以摄像机光轴和图像平面的交点作为图像物理坐标系的原点o1，x轴/y轴平行于摄像机坐标系的xc轴/yc轴，如下图说明了像素坐标系、物理坐标系及其相互关系。
假设dx和dy分别为单位像素的物理宽度值和物理高度值，物理坐标系的坐标原点在像素坐标系中的坐标为(u0,v0)，则我们可以找到物理坐标系中的任意一点(x,y)与像素坐标系中的坐标(u,v)的对应关系：
(313)摄像机坐标系
摄像机坐标系，即固结在摄像机上的坐标系，其坐标原点在光心，z轴沿光轴方向，xc轴/yc轴分别平行于成像平面，同时与图像物理坐标系的x轴和y轴平行。由此，根据相似三角形，图像物理坐标系中的点(x,y)与相机坐标系中的点(xc,yc,zc)的对应关系可以用如下公式表示为：
坐标点从像素坐标系到摄像机坐标系的变换参数称为摄像机内部参数。摄像机内部参数是摄像机的固有参数，不会随着摄像机空间位置的变化而变化，在使用过程中可以通过标定获得或直接读取出厂数据。由以上两公式可以得到：
z坐标——通过rgbd相机采集的深度图像获取
本方法采用的rgbd相机可以同时采集到彩色图像数据流和深度图像数据流，并且内置算法已经将彩色数据和深度数据对齐，也就是说彩色图像像素坐标系中(x0，y0)坐标确定的像素其在物理世界中相机坐标系下的深度就是深度图像中(x0，y0)坐标确定的值。
xy坐标——通过相机参数矩阵从二维映射到三维
工件重心点在相机坐标系下xy方向的坐标通过相机模型的参数来得到，正如上文介绍的各个坐标系坐标之间的变换关系，我们可以得到图像像素坐标系到相机坐标系的变换公式为：
通过模板匹配得到待抓取工件的重心在图像像素坐标系中的坐标，通过以上公式，结合事先标定好的摄像头内参数，进而计算得到工件重心在摄像机坐标系中的物理坐标。
(32)姿态信息的计算
在工件抓取的过程中，除了需要上文提到的xyz三个位置坐标信息，还需要摇摆(roll)、俯仰(pitch)和偏转(yaw)三个方向的角度信息，这三个角度信息分别表征了待抓取物体绕坐标系z轴、y轴和x轴旋转的角度，只有获得这三个方向的角度信息，才能为机器人末端执行器设计合理的姿态，对工件进行抓取。
本方法设计中，以圆柱形的轴承工件为例，由于该工件为旋转对称性物体，因此我们在计算的过程中只需计算轴承绕z轴的旋转角度roll和绕y轴的旋转角度picth即可。
如前文所述，在获取待抓取工件的位置信息时，确定了工件重心所在位置作为整个工件位置的代表，然而，当我们需要确定工件的角度信息时，以圆柱形轴承为例，我们至少需要一条线的信息，以这条线段的角度来代表整个轴承的角度。
我们以轴承的重心点作为参考点，根据轴承的尺寸，在重心参考点的基础上增加一定的偏置量，得到轴承工件柱形结构四个边界角点的三维位置坐标，并通过映射矩阵将四个边界角点映射到相机坐标系下，得到四个边界角点的实际物理坐标。进一步的，分别将四个角点中上面两个角点和下面两个角点去平均值，从而得到轴承工件中轴线上的两个点，相当于确定了一条经过轴承工件中轴线上的一条线段。如图9所示，点abcd即为确定的轴承工件的四个角点：
在计算轴承绕z轴的旋转角度roll时，将轴承中轴线两端点的坐标投影到xoy平面，利用反正切计算该条投影直线与摄像头x轴方向的夹角，即可得到轴承绕摄像机坐标系z轴的旋转角度。
在计算轴承绕摄像机坐标系y轴的旋转角度时，采用最小二乘法计算中轴线方程的拟合值，进而利用轴承中轴线与中轴线上端点到xoy平面的垂线段构成的直角三角形，计算轴承绕摄像机坐标系y轴的旋转角度。具体算法实现如图10所示。
在利用最小二乘法拟合中轴线方程时，首先需要为中轴线方程选定一个参考坐标系，选取经过轴承中轴线并且与xoy平面垂直的平面作为坐标系所在的平面，以轴承垂直投影所在的直线为横轴，以深度坐标作为纵轴，以此确定的坐标系作为中轴线方程的参考坐标系。在具体拟合的过程中，我们需要用到深度摄像头采集到的深度数据，自变量为轴承在xoy平面上的投影，按照像素步长为1进行取值，因变量为每一像素步长所对应的像素的深度数据，进而利用最小二乘法，拟合出以深度数据为表征的中轴线方程。利用拟合出的中轴线方程，可以得到相对准确的轴承圆柱部分的长度，取投影长度与圆柱长度的比值的反余弦，即可得到轴承工件绕摄像机坐标系y轴的旋转角度。
由于利用rgbd相机采集到的深度数据有一定的噪声，若直接利用原始数据中轴承圆柱部分上下两端点所对应的深度数据值，会导致很大的误差，利用最小二乘法拟合中轴线直线方程，能够在很大程度上减少误差，增加姿态计算的准确性。
(33)手眼标定及工件在机器人坐标系下的位姿
经过以上步骤，我们得到了轴承工件在摄像机坐标系下的六自由度位姿量，若想利用六轴机器人对工件实现抓取，需要得到机器人坐标系到相机坐标系的变换矩阵，也就通常意义上说的手眼标定。
在推导过程中，我们会用到四个坐标系，分别是基础坐标系，机械手坐标系，相机坐标系，以及标定物坐标系，如图11所示。
其中basehcal表示基础坐标系到标定物坐标系的转化关系，包括旋转矩阵和平移向量；camhtool表示相机坐标系到机械手坐标系的转化关系；camhcal可以由相机标定求出；basehtool可以由机器人系统中得出。
在具体标定过程中，将标定板固定在机器人的末端执行器上，摄像头位置固定，通过示教器控制末端执行器移动位置，从而改变标定板在摄像机坐标系中的位置。如此反复进行，记录末端执行器的位姿以及标定板在摄像头坐标系下的位姿，进而利用标定算法计算得到机器人坐标系到相机坐标系的变换矩阵。
实施效果：
本实施例中识别及抓取的目标工件为金属轴承，而且为质量不均匀分布工件；轴承的cad三维模型可以通过手动测量，通过solidworks绘制得到。通过使用该三维模型进行训练，测量出实际检测中摄像头距离抓取平台的高度，从而得到训练模型。应用该训练模型即可对轴承的姿态进行实时的检测。
对单个轴承的姿态检测结果如下
在结果中，绘制出识别得出的工件轮廓，同时标识出计算得到的重心，以及该重心点在摄像头坐标系下的三维空间坐标，同时给出工件相对与机械臂的空间坐标系下绕z轴旋转的角度(gamma)以及绕x轴旋转的角度(beta)。得到的这两个角度对于此圆柱形的抓取部位来说，足够满足使用夹具进行抓取的条件，因此在此不再给出绕y轴旋转的角度。并且整个识别的过程通过计时器统计在1s以内，能够满足高速度的工业生产中实时识别以及工件抓取的要求。
从结果看出在光线满足一定要求的环境中，在复杂背景的平台上也能实现较为准确的识别，体现出本系统的抗干扰性。
本方法针对堆叠工件的复杂情况的抓取问题，不同于以往的简单无堆叠平面工件抓取；本方法算法所用数据源为rgbd相机采集到的彩色图像和深度数据，而非普通单目工业相机、双目立体视觉相机的数据；本方法在匹配算法方面，利用待抓取工件的cad模型进行训练，对各种工件均有适应性；本方法对于工件的姿态估计是指工件的六自由度位姿，包括三个位置参数和三个角度参数；本方法在工件位姿计算的过程中，采用最小二乘法拟合工件中轴线方程，提高了位姿估计的准确性。
通过rgb摄像头采集到的二维图像，通常使用形状匹配进行工件轮廓的选取时偶尔会选取到非表面的工件，即被遮盖的工件。此时我们需要对所有识别出来的工件轮廓进行一个排序，排序的依据是计算出轮廓内区域工件的可见度分值。即通过遍历区域内的像素与该姿态模板下的二维图进行对比，判断未被覆盖区域占总轮廓内区域面积的一个比值。该分值为0到1之间的小数，例如工件被遮挡部分超过一半，则该分值低于0.5，通过设定最低分值阈值来选取高于该阈值的识别结果进行排序，从分值最高的工件进行抓取。这样解决了复杂工件堆叠时模板匹配不理想的问题。同时与深度信息图中的深度坐标相配合，实现在堆叠工件中从上往下的依次识别与抓取。