CN113313815B - 一种机械臂抓取物体实时三维重建方法 - Google Patents

Abstract

本发明涉及一种机械臂抓取物体实时三维重建方法，包括以下步骤：(1)利用RGB‑D深度相机获取场景深度信息和彩色图像，基于深度信息进行实时背景分割，将背景从重建场景中剔除，仅保留动态物体；(2)针对步骤(1)保留的动态物体，通过对动态物体实时位姿解算，获取动态物体三维点云模型；(3)针对步骤(2)获取的动态物体三维点云模型，剔除动态物体三维点云模型中的机械爪，获得机械臂抓取物体的三维点云模型。本发明能实现机械爪与目标模型之间的完整分割，改善在机械爪影响下，三维重建模型精度，提升模型分割准确性，提升三维重建速度。

Description

一种机械臂抓取物体实时三维重建方法

技术领域

本发明涉及动态物体实时三维重建领域，具体涉及一种机械臂抓取物体实时三维重建方法。

背景技术

三维重建是指对三维物体建立合适的计算机模型并进行数字处理的过程，是计算机或智能机器人感知环境的最直接方式，在各行各业都用之广泛的应用，如何快速有效的获得精确模型，已成为学术和工业领域重要的研究课题。

随着三维重建研究领域的不断发展，三维模型的建立由传统的接触式方法逐渐发展为以视觉三维重建为代表的多种非接触式方法，视觉三维重建因其适用范围广、成本低等优势，成为这一领域的研究热点。根据图像数据来源，基于视觉的三维重建大体可分为多视图三维重建和基于视觉SLAM的三维重建。其中基于多视图的稠密三维重建算法运行时间较长，无法满足实时三维重建的需要，于是，能够进行实时三维重建的视觉SLAM的重建方法成为了研究的重点方向。

但与此同时，基于视觉的三维重建技术，又存在着种种制约，首先在重建项目的原理层面上来说，“静态世界假设”是目前基于视觉三维重建方法共同的依赖，假设中规定，建模场景内所有物体是绝对静止的，然而这样的场景在实际中难以达到，场景内其他物体产生的噪声对重建结果影响严重，使得三维重建技术难以在普通场景进行推广应用；其次，三维模型的构建需要实时解算相机位置与姿态的变化，在这过程中需要消耗大量的计算资源；最后，在重建物体与场景内其他物体产生互相遮挡，并且模型较难取得准确的先验信息时，如何精确的将目标物体准确的分割出来是目前的难点所在。

发明内容

本发明的技术解决的问题是：克服现有技术不足，提供动态场景实时视觉三维重建方法，实现机械爪与目标模型之间的完整分割，改善在机械爪影响下，三维重建模型精度，提升模型分割准确性，提升三维重建速度。

本发明的技术解决方案为一种机械臂抓取物体实时三维重建方法，包括以下步骤：

(1)利用RGB-D深度相机获取场景深度信息和彩色图像，基于深度信息进行实时背景分割，将背景从重建场景中剔除，仅保留动态物体；

(2)针对步骤(1)保留的动态物体，通过对动态物体实时位姿解算，获取动态物体三维点云模型；

(3)针对步骤(2)获取的动态物体三维点云模型，剔除动态物体三维点云模型中的机械爪，获得机械臂抓取物体的三维点云模型。

进一步的，所述步骤(1)中，利用RGB-D深度相机获取场景深度信息和彩色图像，基于深度信息进行实时背景分割，将背景从重建场景中剔除，仅保留动态物体的方法如下：

由RGBD相机采集得到的彩色图和深度图，但由于深度图采集传感器和彩色图采集传感器的视场和空间坐标系不同，需要对深度图和彩色图进行对齐操作，首先将深度图的像素点还原在深度坐标系下，再将深度空间的深度点映射到世界坐标系下，再将世界坐标系的深度点转换到彩色摄像头坐标系，并将彩色摄像头坐标系下的深度点映射到Z＝1的平面上。

接着根据深度图对彩色图进行分割，保证视场内只有被建模的物体，增强后续位姿估计的准确性。

进一步的，所述步骤(2)中，针对步骤(1)保留的动态物体，通过对动态物体实时位姿解算，获取动态物体三维点云模型，方法如下：

RGB-D相机通过红外结构光原理测量像素距离，通过同时获取的彩色图片可以达到每一个像素点的像素坐标，并可求得点在相机坐标系下的三维坐标：

ZP_uv＝KP

其中，Z为红外测量得到的像素距离，K为相机内参矩阵，P_uv为像素点的像素坐标(齐次坐标)，P为该点在相机坐标系下的三维坐标。

接着，为减少对计算设备的压力和保证三维重建的实时性，我们对相机坐标系下的点云中的点进行采样，并对采样后的点分别计算点云法线。

因为3D重建是在实时采集的过程当中进行的，当前帧跟上一帧之间的变换不大，所以把相邻两帧得到的点云进行ICP配准，为减轻计算复杂度将传统点对点距离的计算方式改为只保留法线方向的距离，并采用优化的方式求解，优化的目标函数如下：

其中R,t是待求解量，p_i和q_i是原始点和目标点，n_i为指向目标点的法向量，k为点云点数。

为保证位姿解算准确，对于RGB图像通过光流法进行几何上的约束计算位姿：

其中，E_Optical为光流法优化目标函数，I₁，I₂为前后两帧RGB图，x，y为像素坐标。

最终优化方程为：

E_tr＝E_icp+wE_Optical

其中E_tr为最终优化项，w为设定权重值，可根据不同设备和环境做出适应性改变。

使用非线性最小二乘法求解E_tr最小，即可得到物体的精确位姿变化,最终通过解算出的旋转矩阵R和平移向量t可推出世界坐标系下的点云位置：

P＝RP_w+t

其中P_w为点在世界坐标系下的空间位置，最后得到了世界坐标系下的三维点云模型。

进一步的，所述步骤(3)中，针对步骤(2)获取的动态物体三维点云模型，剔除动态物体三维点云模型中的机械爪，获得机械臂抓取物体的三维点云模型，方法如下：

本步骤主要对步骤(2)获得的点云模型进行两次分割。

首先对原始点云模型进行点云颜色区域生长法分割：从一个生长点开始，将与该生长点性质相似的相邻像素或者区域与生长点合并，形成新的生长点，并重复此过程直到不能生长为止。生长点和相似区域的相似性判断依据为颜色信息。为提高效率，本发明采用从最小曲率值的点开始生长，减少了区域的总数，提高了整体运行速度。具体步骤为：

(1)设置一空的种子点序列和空的聚类数组，按照点的曲率值对点进行排序，找到最小曲率值点，并把它添加到种子点集；

(2)对于每个种子点，算法都会搜索周边的所有近邻点。计算每个近邻点与当前种子点的法线角度差，如果差值小于设置的阈值，则该近邻点被重点考虑，进行第二步测试；如果该近邻点通过了法线角度差检验，而且它的曲率小于我们设定的阈值，这个点就被添加到种子点集，即属于当前平面；

(3)设置最小点簇的点数为N_min，最大点簇为N_max；

(4)重复前三步，算法会生成点数在N_min和N_max之间的所有平面，并对不同平面加以区分；

(5)直到算法在剩余点中生成的点簇，不能满足最小点数N_min的要求，算法停止工作，这样，就通过区域生长实现了点云的分割。

再根据点云的空间位置进行二次分割，本步骤的最终目标在于解决完整取出被机械爪抓取的物体，且在实际中机械爪相对目标物体空间位置相对固定，因此采用比较不同聚类点云的质心高度对目标物体所属聚类进行判断。首先计算所获取的机械臂与物体点云的主方向，利用PCA主元分析法获得所有点云的三个主方向，获取质心，计算协方差矩阵，求取协方差矩阵的特征值，最大的特征值对应的特征向量即为主方向。再利用所获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重合，此时所得到的质心高度最低的聚类，即为目标物体所属的划分

第二次分割完成后，由于大部分的机械爪已经剔除，对点云模型整体进行滤波，去掉模型周围的离群点，并保留最终结果，即为细分割之后的目标点云。

本发明与现有技术相比的优点在于：

(1)本发明采用了最近点迭代和光流法相结合的方式，并融合了实时深度分割，在未增加系统整体运算规模、保证系统实时运行的基础上，提高了动态场景下相机的位姿解算精确度，实现了精细建模。此方式有效改善动态场景三维重建效果。

(2)本发明采用了颜色区域生长分割与空间坐标分割相结合的方式，将机械爪与目标模型进行两次分割，提高目标三维模型的精确度，并整体提升了三维点云模型分割效果。

总之，本发明采用的方法原理简洁，三维重建与分割效果良好，可达到对对机械臂抓取物体进行实时三维重建的目的。

附图说明

图1为本发明一种机械臂抓取物体实时三维重建方法流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示，本发明的一种机械臂抓取物体实时三维重建方法，具体实现步骤如下：

步骤1、利用RGB-D深度相机获取场景深度信息和彩色图像，通过基于深度信息的实时背景分割技术，将背景从重建场景中剔除。由RGBD相机采集得到的彩色图和深度图，但由于深度图采集传感器和彩色图采集传感器的视场和空间坐标系不同，需要对深度图和彩色图进行对齐操作，首先将深度图的像素点还原在深度坐标系下，再将深度空间的深度点映射到世界坐标系下，再将世界坐标系的深度点转换到彩色摄像头坐标系，并将彩色摄像头坐标系下的深度点映射到Z＝1的平面上。根据深度图对彩色图进行分割，保证视场内只有被建模的物体，增强后续位姿估计的准确性。

步骤2、利用动态物体实时位姿解算技术，获取动态物体三维点云模型。RGB-D相机通过红外结构光原理测量像素距离，通过同时获取的彩色图片可以达到每一个像素点的像素坐标，并可求得点在相机坐标系下的三维坐标：

ZP_uv＝KP

其中，Z为红外测量得到的像素距离，K为相机内参矩阵，P_uv为像素点的像素坐标(齐次坐标)，P为该点在相机坐标系下的三维坐标。

接着，为减少对计算设备的压力和保证三维重建的实时性，我们对相机坐标系下的点云中的点进行采样，并对采样后的点分别计算点云法线。

因为3D重建是在实时采集的过程当中进行的，当前帧跟上一帧之间的变换不大，所以把相邻两帧得到的点云进行ICP配准，为减轻计算复杂度将传统点对点距离的计算方式改为只保留法线方向的距离，并采用优化的方式求解，优化的目标函数如下：

其中R,t是待求解量，p_i和q_i是原始点和目标点，n_i为指向目标点的法向量，k为点云点数。

为保证位姿解算准确，对于RGB图像通过光流法进行几何上的约束计算位姿：

其中，E_Optical为光流法优化目标函数，I₁，I₂为前后两帧RGB图，x，y为像素坐标，x+Δx、y+Δy为前一帧x，y处的像素点在后一帧图片上的像素坐标。

最终优化方程为：

E_tr＝E_icp+wE_Optical

其中E_tr为最终优化项，w为设定权重值，可根据不同设备和环境做出适应性改变。

使用非线性最小二乘法求解E_tr最小，可得到物体精确位姿变化,通过解算出的旋转矩阵R和平移向量t可推出世界坐标系下的点云位置：

P＝RP_w+t

其中P_w为点在世界坐标系下的空间位置。进而得到了世界坐标系下的三维点云模型。

步骤3、对点云模型进行两次分割，并细化分割结果。

首先对原始点云模型进行点云颜色区域生长法分割，首先，设置一空的种子点序列和空的聚类数组，按照点的曲率值对点进行排序，找到最小曲率值点，并把它添加到种子点集；其次，对于每个种子点搜索周边的所有近邻点，计算每个近邻点与当前种子点的法线角度差，如果差值小于设置的阈值，则该近邻点被重点考虑，进行第二步测试，如果该近邻点通过了法线角度差检验，且曲率小于设定的阈值，这个点就被添加到种子点集，即属于当前平面；最后设置最小点簇的阈值为N_min，最大点簇阈值为N_max，并重复前述步骤，直到在剩余点中生成的点簇不能满足最小点簇N_min的要求，第一次分割完成。

接着根据点云的空间位置进行二次分割，本步骤的最终目标在于解决完整取出被机械爪抓取的物体，且在实际中机械爪相对目标物体空间位置相对固定，因此采用比较不同聚类点云的质心高度对目标物体所属聚类进行判断。首先计算所获取的机械臂与物体点云的主方向，利用PCA主元分析法获得所有点云的三个主方向，获取质心，计算协方差矩阵，求取协方差矩阵的特征值，最大的特征值对应的特征向量即为主方向。再利用所获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重合，此时所得到的质心高度最低的聚类，即为目标物体所属的划分。

第二次分割完成后，由于大部分的机械爪已经剔除，对点云模型整体进行滤波，去掉模型周围的离群点，并保留最终结果，即为细分割之后的目标点云。

由此可见，本发明能够针对机械臂抓取的物体，改善三维重建效果，提高三维点云分割精细程度。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，且应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (2)

1.一种机械臂抓取物体实时三维重建方法，其特征在于，包括以下步骤：

(1)利用RGB-D深度相机获取场景深度信息和彩色图像，基于深度信息进行实时背景分割，将背景从重建场景中剔除，仅保留动态物体；

(2)针对步骤(1)保留的动态物体，通过对动态物体实时位姿解算，获取动态物体三维点云模型；方法如下：通过RGB-D深度相机通过红外结构光原理测量像素距离，求得点在相机坐标系下的三维坐标；为减少对计算设备的压力和保证三维重建的实时性，对相机坐标系下的点云中的点进行采样，并对采样后的点分别计算点云法线，采用三维点云法线方向上的ICP配准加二维RGB图像光流法进行联合结算，在保证实时性的同时，精确求解空间中物体运动姿态，最终推出世界坐标系下的点云位置，获取动态物体三维点云模型，提高了机械臂抓取物体实时三维重建精度；

(3)针对步骤(2)获取的动态物体三维点云模型，剔除动态物体三维点云模型中的机械爪，获得机械臂抓取物体的三维点云模型；方法如下：

待重建的机械臂抓取目标物体与机械爪紧密接触，二者同时出现在相机视场范围内；在进行目标物体点云数据获取时，同时会对机械爪进行点云重建；

3.1)对步骤2)获取的动态物体三维点云模型进行聚类，首先，确定区域生长的初始种子点，先计算所有点的曲率，再依据点的曲率值对点进行排序，把曲率值最小的点设为种子点，曲率最小的点位于平坦区域，把种子点设在此处，能够减少区域的总数，提高效率；其次，在确定出始种子点后，搜索种子点的邻域点，对比每一个邻域点与当前种子点之间的颜色差异，如果小于设定阈值，则将该邻域点加入到当前区域，检查每一个邻域点的曲率值，小于设定曲率阈值的点成为新的种子点，并删除之前种子点；最后，从新的种子点继续增长，重复之前步骤，直到没有新的种子点产生，完成了第一次颜色聚类；

在第一次聚类基础上，对比相邻区域的平均颜色，如果两个邻域的平均颜色小于设定阈值则将两个区域合并，再检查每一个区域点的数量，如果区域中点的数量少于设定的阈值则将该区域与最近邻区域合并，完成最终聚类；

3.2)对3.1)聚类结果进行判别，判别出待重建目标和机械爪，机械爪抓取到目标物体后，将目标物体放置在相机前方适当距离，并将物体进行旋转，此过程中机械爪位置一直位于目标物体上方，因此通过比较不同聚类点云的质心高度对目标物体所属聚类进行判断；在进行比较之前，由于所获取点云的主方向不一定与相机坐标系的坐标轴重合，因此不便直接对点云数据的某一几何维度进行比较；为了保证判断结果的准确性，首先计算所获取的机械臂与物体点云的主方向，利用主成分分析法获得所有点云的三个主方向，获取质心，计算协方差矩阵，求取协方差矩阵的特征值，最大的特征值对应的特征向量即为主方向；再利用所获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重合，此时所得到的质心高度最低的聚类，即为目标物体所属的划分；

3.3)利用两次分割算法，对小于预定体积的目标物体点云进行有效分割；机械爪对于体积较小的目标物体遮挡程度较高，同时相机传感器很难在目标物体与机械爪接触的部分准确的获取目标物体的颜色信息，造成部分机械爪残留；采取两次分割的方法实现体积小于预定体积的目标物体的三维重建效果；在第一次分割中，提高颜色阈值和最小聚类的点云数量，降低分割的精细度，实现对大部分机械爪点云的去除，同时不产生过多的聚类数目，降低分割结果判断难度，提高算法鲁棒性；在第二次分割中，降低区域生长算法的颜色阈值和最小聚类的点云数目，进一步将机械爪和目标物体分开，剔除大部分机械爪；根据残留机械爪点云数量远远少于目标物体点云的先验信息，通过比较不同聚类的点云数目对聚类结果进行筛选，保留点云数目最多的聚类结果，获得目标物体点云模型。

2.根据权利要求1所述的机械臂抓取物体实时三维重建方法，其特征在于：所述步骤(1)中，利用RGB-D深度相机获取场景深度信息和彩色图像，通过基于深度信息的实时背景分割技术，将背景从重建场景中剔除，仅保留动态物体，方法如下：

利用RGB-D深度相机获取场景深度信息和彩色图像信息，得到深度图和彩色图；基于深度信息进行静态背景分割，将背景从重建场景中剔除，仅保留动态物体，在此基础上再进行动态物体的三维重建；首先对RGB-D深度相机采集到的数据进行深度提取，对深度信息进行预处理，将深度图与彩色图对齐；然后根据先验信息设定分割深度距离大小，改变深度图的局部深度分布；最后依照处理后的深度图对彩色图进行像素级别的分割；为保证能够完全剔除背景，深度分割模块将设定距离之外的深度赋值为无穷远，在保证精确性的基础上又保证实时性。

Images