CN111784770A - 基于shot和icp算法的无序抓取中的三维姿态估计方法 - Google Patents

Abstract

本发明涉及基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，以SHOT粗匹配方法为ICP精配准提供良好的初始位置，SHOT算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再对模型和场景点云对应的特征提取结果进行霍夫变换，剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。

Description

基于SHOT和ICP算法的无序抓取中的三维姿态估计方法

技术领域

本发明涉及三维姿态估计技术领域，主要涉及一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法。

背景技术

目前，基于图像的物体识别与姿态估计已经有很多研究成果，但是二维图像的成像过程是从三维空间映射到二维空间，这个过程会丢失大量的信息；而且一个无法忽视的事实是，最好的视觉系统应该是面向三维世界的。随着自动化程度的提高，机器人导航、工业零件检测及抓取等众多领域对计算机视觉系统的要求越来越高，因此基于二维图像的物体识别与姿态估计已经无法满足人类的要求。而目前三维点云数据的获取已经非常快捷，同时三维点云数据的采集不受光照影响，规避了二维图像遇到的光照、姿态等问题，因此基于点云数据的三维物体识别与姿态估计也引起了人们的重视。

在无序抓取的过程中，我们需要寻求一种稳健、快速，并且满足一定精度的姿态估计方法。传统的三维姿态估计一般分为两个阶段：粗匹配阶段和精配准阶段。传统的物体识别和三维姿态估计粗匹配方法，如Rusu提出的点特征直方图PFH(Radu Bogdan Rusu,NicoBlodow,Zoltan Csaba Marton,et al.Aligning Point Cloud Views using PersistentFeature Histograms[C]//2008 IEEE/RSJ International Conference on IntelligentRobots and Systems,September 22-26,2008,Acropolis Convention Center,Nice,France.IEEE,2008.)通过计算任意两个顶点间的法向量夹角和距离等特征，映射到直方图中获取统计信息，并求解空间变换关系。该算法耗时较长，无法达到实时性要求。随后，Rusu在保留PFH特征识别能力的基础上，提出了快速点特征直方图FPFH(Radu Bogdan Rusu,Nico Blodow,Michael Beetz.Fast Point Feature Histograms(FPFH)for 3Dregistration[C]//Robotics and Automation,2009.ICRA'09.IEEE InternationalConference on.IEEE,2009.)，相比PFH，提高了计算效率。

本发明针对工业实际应用环境的无序抓取提出基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，解决无序抓取过程中的物体位姿估计精度及实时性问题。

发明内容

为了解决无序抓取过程中的物体位姿估计问题，本发明提供了一种抗噪声能力强，鲁棒性高，精度较高的基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，用于工业现场三维物体识别和姿态估计。

本发明解决上述技术问题采用的技术方案是：

一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，包括下述步骤：

第一步，点云预处理：

1-1获取工件所在场景的三维点云数据及同时获取相应的二维图像；

1-2点云滤波：采用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景；

1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，获得场景点云；

第二步，应用SHOT物体识别和姿态估计算法：

2-1在场景和模型点云上，分别进行多线程法线估计，下采样提取点云关键点；

2-2在场景和模型点云上，分别构建局部参考系，获取工件区别于平面的部位的尺寸，局部参考系构建半径r与该尺寸一致，计算关键点在对应局参考系下特征计算半径内的拓扑特征，并将拓扑特征保存在直方图中；

2-3霍夫变换进行物体识别，将霍夫变换的区域划分间隔设定为与模型关键点提取下采样尺寸一致，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵T_s表示粗匹配姿态变换矩阵，它的左上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零；

第三步，应用ICP算法进行精确姿态估计：

kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H；根据构建的协方差矩阵利求解使目标函数E最小化的旋转矩阵R和平移向量T；

改进的ICP的目标函数E为：

其中，

为点对应之间距离的均值；p_i表示模型点云中的一点，q_i表示场景点云中的一点，q_i是p_i的最邻近点，N为点对的总数；

将求得的使目标函数E最小化的旋转矩阵R和平移向量T应用于模型点云，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵T_i；

第四步，获得最终的姿态估计结果：

将第二步得到的姿态变换矩阵T_s与第三步得到的姿态变换矩阵T_i相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。

在使用时，针对工业现场不同工件的不同特性，调整局部坐标系和特征描述子的计算半径，调整霍夫参数的区域划分间隔。

点云滤波的具体过程是：应用步骤1-1获取的不同种类工件的多张二维图像，使用LabelImg将工件最小外接矩形标注出来；采用YOLO-V3目标检测算法，Adam优化器，训练100个Epoch；调用训练好的模型检测工件边框，利用采集到的二维图像和三维点云各点间一一对应的关系，映射到三维点云。

场景点云关键点数量保持为模型点云关键点数量的3到4倍；特征计算半径设定为局部参考系构建半径的4/3。

将霍夫参数空间的插值间隔设定为与模型特征点提取下采样尺寸一致，将霍夫投票阈值设定为5.0。

第三步中，令

则目标函数E的最小化问题转化为寻找旋转矩阵R使Σ²最小化及求解平移向量T＝q_i-Rp_i；引入

改进的目标函数为：

采用SVD、单位四元数、对偶四元数、线性最小二乘或非线性最小二乘等方法求解使Σ²最小化的旋转矩阵R，进而求解T。

与现有技术相比，本发明的有益效果是：

本发明以SHOT粗匹配方法为ICP精配准提供良好的初始位置，SHOT算法中M矩阵的构建，特征值分解，进而构建局部参考系；计算对应参考系下的拓扑特征，并将结果保存在直方图中，再对模型和场景点云对应的特征提取结果进行霍夫变换，剔除错误点对，解算得到最终的粗配准姿态估计结果；ICP算法最近点的寻找，协方差矩阵的构建以及使目标函数最小化的旋转矩阵和平移向量的求解；根据工业场景中由于噪声、遮挡、和反光，造成工件点云分布不均匀、存在孔洞的问题，改进了目标函数，解决了传统ICP应用于由于工件反光，采集到存在孔洞的点云时，配准精度下降的问题。SHOT算法对噪声的鲁棒性高，具有旋转不变性，识别能力强，能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使系统能够满足抓取任务中的精度要求，二者相互作用提高位姿估计的精度和保证实时性。

本发明为了提高识别速度，在识别算法之前，需要对点云进行预处理，滤除大多数背景；传统的滤波算法，如直通滤波，灵活度不高，局限性较强；本发明针对面结构光三维相机获取的点云中的每一点，对应着相应的二维图像中的一个像素点，这一特点，开创性地采用了YOLO-V3目标检测算法，将检测到的目标工件在二维图像中框出，边框内像素点对应到三维点云，即完成了大多数背景的滤除。相较于传统的直通滤波算法，通用性强，鲁棒性高。利用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景，降低背景干扰，减少运算量；在不影响点云表征能力的基础上，应用体素滤波器对点云进行下采样，进一步减少运算量。

本申请在使用时，针对工业现场不同工件的不同特性，调整局部坐标系和特征描述子的计算半径，调整霍夫参数空间的插值间隔，方法的运行速度能够满足实时性要求。

附图说明

图1为一次识别结果可视化。

图2为原始的ICP算法和引入了改进的目标函数的ICP算法的精度对比图。

图3为SHOT算法和改进的ICP算法的位姿估计精度对比图。

具体实施方式

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

本发明基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于解决无序抓取过程中的物体位姿估计问题，包括下述步骤：

第一步，点云预处理：

1-1点云获取：利用Photoneo面结构光三维相机获取工件表面点云数据(同时获取了相应的二维图像)；

1-2点云滤波：使用面结构光相机采集工件二维图像(生产线上需要装配组装的每类工件都获取多张二维图像，本实施例中选择每类5000张)，使用LabelImg将工件最小外接矩形标注出来；采用YOLO-V3目标检测算法，Adam优化器，训练100个Epoch；调用训练好的模型检测工件边框，利用面结构光采集到的二维图像和三维点云各点间一一对应的关系，映射到三维点云，滤除大部分背景，降低背景干扰，减少运算量；

1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，进一步减少运算量；

第二步，应用SHOT物体识别和姿态估计算法：

2-1在场景和模型点云(模型点云为软件模拟生成或者在场景点云将工件部分完全截选构成模型点云)上，分别进行多线程法线估计，下采样提取点云关键点，下采样尺度的选取保证算法运行时间满足实际应用要求即可；为了提高识别成功率，使场景点云关键点数量保持为模型点云关键点数量的3到4倍；

2-2在场景和模型点云上，分别构建局部参考系，计算关键点在对应参考系下特征计算半径内的拓扑特征，并将特征保存在直方图中；局部参考系构建半径r的选取，获取工件区别于平面的部位(如凹陷、凸起、孔洞)的尺寸，将构建半径保持与之一致即可；将特征计算半径设定为参考系构建半径的4/3；

对于每一个关键点p，及以p为球心，半径为r的球体S_p内的点q，计算：

d_s＝||p-q||₂

M＝0,

M＝M+(r-d_s)(p-q)(p-q)^T

对矩阵M做特征值分解：

M＝VDV^-1,V＝[x⁺y⁺z⁺]

根据求得的特征向量构建局部参考系，其中V是由矩阵M的三个特征向量构成的矩阵，x⁺，y⁺，z⁺是取球体S_p中以点密度高的方向作为正方向的三个特征向量,分别对应着局部参考系的x，y，z轴。

在局部参考系下，在特征计算半径内，按照方位8个，俯仰2个，径向2个划分为32个区域；计算点q在局部参考系下的坐标，并计算坐标系原点到该点的向量的夹角θ，同时对处在边缘的点，对其俯仰、径向、和方位角以及到原点的欧式距离进行四线性插值，将结果保存在直方图中。

2-3霍夫变换进行物体识别，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景靠拢；用4×4矩阵T_s表示粗匹配姿态变换矩阵，它的坐上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零。将霍夫参数空间的插值间隔设定为与模型特征点提取下采样尺寸一致，将霍夫投票阈值设定为5.0。

第三步，应用ICP算法进行精确姿态估计：

3-1kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵；

令p_i表示模型点云中的一点，q_i表示场景点云中的一点，q_i是p_i的最邻近点，则：

其中，

为模型点云中心点，

为模型对应的场景点云(可能是一部分点)的中心点，N为点对的总数，矩阵H为构建的协方差矩阵。

3-2根据构建的协方差矩阵求解使目标函数最小化的旋转矩阵和平移向量；

本方法引入改进的ICP的目标函数为：

其中

为点对应之间距离的均值。

令

则该目标函数的最小化问题可以转化为：

(1)寻找旋转矩阵R使Σ²最小化。

(2)则平移向量T＝q_i-Rp_i。

为了求解(1)，可用的方法有SVD、单位四元数、对偶四元数、线性最小二乘、非线性最小二乘，这里以SVD为例求解：

对3-1构造的协方差矩阵H进行奇异值分解：

H＝UΛV^T

其中U为左奇异向量，V为右奇异向量，Λ的主对角线为奇异值，其余元素都为0。

则

R＝VU^T

再应用(2)：

T＝q_i-Rp_i

旋转矩阵R和平移向量T即为当前迭代中使目标函数最小化的结果。

用4×4矩阵T_i表示精配准姿态变换矩阵，它的坐上角的3×3矩阵为旋转矩阵R，最后一列的前三个元素为平移向量T，第四个元素为1，最后一行的前三个元素为零。

3-3将求得的变换关系(即旋转矩阵和平移矩阵)应用于模型点云，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；计算对应点之间的均方误差，若小于阈值，则迭代停止；实际上在程序实现时，迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵T_i；

实施例中差值阈值取值为1×e^-8。若大于差值阈值则回到步骤3-1继续迭代，直至满足终止条件。

第四步，获得最终的姿态估计结果：

将第二步得到的姿态变换矩阵T_s与第三步得到的姿态变换矩阵T_i相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。

利用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景，降低背景干扰，减少运算量，鲁棒性高，速度能够满足实时性要求；SHOT算法对噪声的鲁棒性高，具有旋转不变性，识别能力强；能够为ICP算法提供稳定、良好的初始位置；ICP算法进一步提高了位姿估计的精度，使系统能够满足抓取任务中的精度要求。

图1为一次识别结果可视化，待测对象为汽车发动机的某个工件，图中主视图窗口(左边的大窗口)中，白色点云即为应用最终的姿态变换矩阵将模型点云变换到场景中的效果；黑色和灰色内容为场景点云；模型视图窗口(右边的小窗口)中为显示的初始的模型点云(即没有经过姿态变换矩阵处理的点云)，工业现场工件表面光滑，会存在遮挡、反光等，工件点云中存在较大的孔洞。窗口中还包括载入模型、识别测试、无序抓取、停止及运行按钮，通过载入模型加载待测工件模型点云；识别测试，用于测试待测工件是否能够识别成功，完成三维姿态估计；无序抓取，将根据识别测试结果控制机械手进行相应的抓取动作。

图2为原始的ICP算法和引入了改进的目标函数的ICP算法的精度对比，横坐标为围成孔洞的点数量，纵坐标为均方误差。图中实验是在相同工件，相同场景下，在工件点云中人为制造不同大小的孔洞，在此基础上对原始的ICP算法和改进的ICP算法，在工件点云存在不同大小孔洞时的均方误差(RMSE)进行对比，图中。可见，随着孔洞的增大，改进的ICP算法相较于原始ICP算法配准精度显著提高，更适用于工业现场，且无论是大孔洞还是小孔洞，本申请的ICP算法均方误差均小于原始的ICP算法。

图3为上述实施例中，SHOT算法和改进的ICP算法的位姿估计精度对比，图中实验是在工业现场中，每次随机调整工件的位姿，采集工件点云，进行点云预处理。根据工件的特性(如存在凹陷或凸起的尺寸)，调整SHOT特征描述子的局部参考系构建半径、特征计算半径、霍夫参数空间的插值间隔，以及投票阈值；将ICP算法前后两次迭代之间的旋转角度差值的阈值设为1×e^-8，最大迭代次数限定为50次。在此基础上采用SHOT物体识别和初始配准算法与ICP精配准算法，估计工件位姿。进行100次独立实验，每次计算模型点云与目标点云(场景点云)之间点对应距离的平均值，以此作为度量误差的标准；计算100次实验的平均精度来观察整体效果。

由图3可以看出在SHOT初配准算法提供初始位姿的情况下，ICP总能使误差减小；横坐标表示实验次数，纵坐标表示精度，单位mm。针对初始位姿误差较大的情况，ICP减小误差的效果尤为明显。SHOT初始配准的平均精度(mean SHOT)为2.59543mm，ICP精配准的平均精度(mean ICP)为2.26341mm，由于拍摄位置和角度不同，模型点云和场景点云之间往往存在较大差异，实际误差要比实验求得小一些。

实验中100次实验总能使姿态估计问题收敛并达到较高的精度，证明该算法鲁棒性较强，精度较高，能够满足一般的无序抓取需求。本实施例中使用联想拯救者笔记本电脑，七代i5，2.5GHz，一次识别结果的检测时间为1200ms。

采用SHOT(直方图签名方法，Signature ofHistograms ofOrien-Tations,SHOT)物体识别和三维姿态估计方法，具有旋转不变性并对噪声具有鲁棒性，能够很好的抑制工业现场噪声的影响。为了进一步提高位姿估计的精度，需要在物体识别和三维姿态估计粗配准方法后，应用精配准算法，本方法采取迭代最近点(ICP)算法，并针对工业场景中存在一定噪声、遮挡、和反光，容易造成工件点云分布不均匀、存在孔洞的问题，修改了ICP的目标函数，使配准更精确，从而获得更加准确的姿态变换矩阵。

上面对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

本发明未述及之处适用于现有技术。

Claims (6)

1.一种基于SHOT和ICP算法的无序抓取中的三维姿态估计方法，该方法用于无序抓取过程中的物体位姿估计，包括下述步骤：

第一步，点云预处理：

1-1获取工件所在场景的三维点云数据及同时获取相应的二维图像；

1-2点云滤波：采用YOLO-V3目标检测算法，检测工件边框，映射到三维点云，滤除大部分背景；

1-3点云下采样：在不影响点云表征能力的基础上，对点云进行下采样，获得场景点云；

第二步，应用SHOT物体识别和姿态估计算法：

2-1在场景和模型点云上，分别进行多线程法线估计，下采样提取点云关键点；

2-2在场景和模型点云上，分别构建局部参考系，获取工件区别于平面的部位的尺寸，局部参考系构建半径r与该尺寸一致，计算关键点在对应局参考系下特征计算半径内的拓扑特征，并将拓扑特征保存在直方图中；

2-3霍夫变换进行物体识别，将霍夫变换的区域划分间隔设定为与模型关键点提取下采样尺寸一致，确定正确的特征匹配点对，根据特征匹配点对求解场景和模型点云之间的位姿变换关系，并将变换关系应用于模型点云，使模型点云向场景点云靠拢；用4×4矩阵T_s表示粗匹配姿态变换矩阵，它的左上角的3×3矩阵为旋转矩阵，最后一列的前三个元素为平移向量，第四个元素为1，最后一行的前三个元素为零；

第三步，应用ICP算法进行精确姿态估计：

kdtree搜索模型点云在场景点云中的最近点，根据找到的对应关系构建协方差矩阵H；根据构建的协方差矩阵利求解使目标函数E最小化的旋转矩阵R和平移向量T；

ICP的目标函数E为：

其中，

为点对应之间距离的均值；p_i表示模型点云中的一点，q_i表示场景点云中的一点，q_i是p_i的最邻近点，N为点对的总数；

将求得的使目标函数E最小化的旋转矩阵R和平移向量T应用于模型点云，返回搜索模型点云在场景点云中的最近点，进行下一次迭代；迭代的终止条件为计算前后两次迭代之间的旋转角度差值，差值小于差值阈值，则认为问题收敛，终止迭代；得到的姿态变换矩阵T_i；

第四步，获得最终的姿态估计结果：

将第二步得到的姿态变换矩阵T_s与第三步得到的姿态变换矩阵T_i相乘，得到最终的姿态变换矩阵；将得到的姿态变换矩阵转换成欧拉角和平移向量的形式，即为最终的姿态估计结果。

2.根据权利要求1所述的方法，其特征在于，在使用时，针对工业现场不同工件的不同特性，调整局部坐标系和特征描述子的计算半径，调整霍夫参数的区域划分间隔。

3.根据权利要求1所述的方法，其特征在于，点云滤波的具体过程是：应用步骤1-1获取的不同种类工件的多张二维图像，使用LabelImg将工件最小外接矩形标注出来；采用YOLO-V3目标检测算法，Adam优化器，训练100个Epoch；调用训练好的模型检测工件边框，利用采集到的二维图像和三维点云各点间一一对应的关系，映射到三维点云。

4.根据权利要求1所述的方法，其特征在于，场景点云关键点数量保持为模型点云关键点数量的3到4倍；特征计算半径设定为局部参考系构建半径的4/3。

5.根据权利要求1所述的方法，其特征在于，将霍夫参数空间的插值间隔设定为与模型特征点提取下采样尺寸一致，将霍夫投票阈值设定为5.0。

6.根据权利要求1所述的方法，其特征在于，第三步中，令

则目标函数E的最小化问题转化为寻找旋转矩阵R使Σ²最小化及求解平移向量T＝q_i-Rp_i；引入

改进的目标函数为：

采用SVD、单位四元数、对偶四元数、线性最小二乘或非线性最小二乘求解使Σ²最小化的旋转矩阵R，进而求解T。

Images