CN109801337A - 一种基于实例分割网络和迭代优化的6d位姿估计方法 - Google Patents

Abstract

本发明涉及一种基于实例分割网络和迭代优化的6D位姿估计方法，包括以下步骤：1)将深度图像转换成HHA特征图和场景点云；2)将HHA特征图和彩色图像输入到增加空间变换网络后的实例分割网络中，进行任意位姿物体的实例分割获得物体种类识别结果以及掩膜分割结果；3)根据实例分割的结果，在场景点云上分割出目标点云；4)根据改进的4PCS算法和ICP算法将分割出的目标点云和目标CAD模型的模型点云进行匹配和位姿精修，从而得到精确的位姿估计结果。与现有技术相比，本发明具有准确识别、多种类物体识别、检测性能高、位姿匹配精度高等优点。

Description

一种基于实例分割网络和迭代优化的6D位姿估计方法

技术领域

本发明涉及机器人的环境感知技术领域，尤其是涉及一种基于实例分割网络和迭代优化的6D位姿估计方法。

背景技术

机器人的环境感知技术是机器人研究领域的重要科学问题，近年来随着计算机视觉和深度学习技术的发展，基于视觉的环境感知成为学术界和工业界关注的热点。机器人通过视觉信息的输入实现对所处环境、操作对象的感知，包括识别与状态估计等，进而实现交互并完成任务。其中，3D视觉技术的引入，使机器人能够获得更加丰富的信息，对于机器人环境感知问题的解决具有重要的推动作用。

本发明着眼于利用3D视觉技术和深度学习技术，实现对目标物体的位姿进行准确的估计，以便于实现机器人的抓取放置以及灵巧操作等任务，其应用领域可以是生活服务领域和工业生产领域等，相关研究主要有针对生活中的物体的姿态估计、工业生产线上的工件、零件的姿态估计等内容。

目前针对物体的姿态估计主要可以分为针对日常生活场景中的物体姿态估计和工业应用场景中的物体姿态估计。从现有的针对物体的姿态估计的专利来看，现有专利对目标物体的感知主要局限于二维层面，没有针对3D姿态估计的相关专利；针对工业场景中的目标物体的姿态估计，现存的专利应用场景都比较简单，往往无法适应复杂的工业应用场景，针对不同类型工件的鲁棒性和适应性不强。具体而言，现有技术主要有以下几个方面的局限：：

从应用场景上看，在现有基于视觉传感器的工业工件位姿估计方案中，有相当一部分的研究场景限定于传送带传送或平面工作台的场景，使用CCD摄像头，采集正对目标工件的平面图片，使用传统的图像处理与识别技术，例如灰度变换、图像二值化、边缘检测、模板匹配等，而且对盛放工件的平台背景颜色具有较高的要求，要求背景单一等等，从而识别出待测工件的简单轮廓，使用外接矩形的方法来对工件进行定位与进行工件平面旋转角度的判断。此类方案的工业应用前景有限，在背景杂乱或者工件堆叠的情况下，通过边缘检测、模板检测的传统的图像处理方案，是很难将视野中的工件识别的；与此同时，边缘检测等算法中存在一些需要人工调节的参数，对于不同的抓取对象即不同形状、不同尺寸、甚至是多种类型工件混杂的情况往往难以适应。

从姿态估计算法上看，现有的专利仍然局限传统的人工设计特征、提取特征的方法，进而将实际场景中的物体提取到的特征与模型或模板的特征做匹配。人工特征提取与模板匹配的方案一方面时间效率比较低，模板匹配过程中的搜索算法需要消耗大量的计算资源，另一方面，传统的特征提取策略要求待检测目标具有可区分的纹理特征，而这一点在工业领域的应用中往往难以满足，工业领域中的工件大多是少纹理特性的，传统的特征提取方法无法适应这一要求，当面对多种不同类型、不同形状的工业工件时，传统的方案同样是无法适应的。

综上所述，传统的解决方案或仅能得到工件的二维平面姿态，或是算法的准确性和鲁棒性不强，无法适应复杂的生活和生产应用场景，无法保证对各种形状、各种类型的物体都能准确识别和解算位姿，无法满足复杂的生活服务和工业生产要求。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于实例分割网络和迭代优化的6D位姿估计方法。

本发明的目的可以通过以下技术方案来实现：

一种基于实例分割网络和迭代优化的6D位姿估计方法，包括以下步骤：

1)将深度图像转换成HHA特征图和场景点云；

2)将HHA特征图和彩色图像输入到增加空间变换网络后的实例分割网络中，进行任意位姿物体的实例分割获得物体种类识别结果以及掩膜分割结果；

3)根据实例分割的结果，在场景点云上分割出目标点云；

4)根据改进的4PCS算法和ICP算法将分割出的目标点云和目标CAD模型的模型点云进行匹配和位姿精修，从而得到精确的位姿估计结果。

所述的HHA特征图中的每个像素点均包括水平方向视差、距离地面高度以及法向量与重力间夹角的三个通道数值。

所述的空间变换网络包括定位网络、网格生成器以及采样器，所述的定位网络由多个全连接层或者卷积层再加一个回归层组成，其以输入特征为输入，其空间变换为2D仿射变换，其输出为空间变化T_θ的参数θ，所述的网格生成器以参数θ以及输入特征图中点的坐标集T_θ(G)为输入，以经过空间变换后的特征为输出，则有：

其中，Φ_x和Φ_y为采样器中的采样核函数k(·)的参数，为输入特征U在坐标(n,m)下第c个通道上的值，为输出特征在输出特征图中第i个点的坐标下第c个通道上的值，H'为输出特征V的高度，W'为输出特征V的宽度，H为输入特征U的高度，W为输入特征U的宽度，C为通道总数，为输入特征中点的坐标。

所述的步骤4)具体包括以下步骤：

41)采用改进的4PCS算法获取目标点云和模型点云之间的刚体转换矩阵T；

42)对目标点云进行点云滤波；

43)采用ICP算法对滤波后的点云进行迭代匹配，得到精确的位姿估计结果。

所述的步骤41)中，改进的4PCS算法以目标点云和模型点云作为输入，以输入两点云之间的刚体转换矩阵T为输出，并且在原4PCS算法中设置一个角度阈值∈，通过比较基中两个向量之间的点积判断角度是否近似相等，若角度差在角度阈值∈的范围内，则将其作为全等基，若否，则舍弃。

所述的步骤42)具体为：

采用刚体转换矩阵T对模型点云进行刚体变换，并对变换后的点云建立kd树，对目标点云中的每个点在kd树中找到与之距离最近的点，如果两点间的距离大于设置的阈值，则在目标点云中剔除该点。

所述的步骤43)具体包括以下步骤：

431)将滤波后目标点云的点集Qm、模型点云的点集Pn、初始旋转变换矩阵和平移变换矩阵作为ICP算法的输入，并设置距离误差δ；

432)根据当前旋转变换矩阵R和平移变换矩阵t，对点集Pn进行变换，并将变换后点集中的每个点在Qm中找出与之距离最近的点，构成点集Qn；

433)计算点集Pn在变换后点集与点集Qn之间的距离的均方根误差E(R,t)，通过奇异值分解求得使E(R,t)最小的旋转变换矩阵R和平移变换矩阵t；

434)当E(R,t)＜δ时，结束迭代，输出此时的旋转变换矩阵R和平移变换矩阵t，否则返回步骤432)。

所述的步骤432)中，均方根误差E(R,t)的计算式为：

其中，q_g为点集Qn的点，p_g为点集Pn中的点，n为点集中点的总数。

与现有技术相比，本发明具有以下优点：

1、本发明将实例分割网络和迭代优化方法进行了结合，实现了在背景复杂，物体杂乱堆叠、互相遮挡的情况下，对目标物体的识别和准确的位姿估计；

2、本发明同时实现了多种类物体识别和其位姿估计，不局限于单种类物体，而且不受限于物体表面纹理是否明显，算法的适应性和鲁棒性较强；

3、本发明针对现有的实例分割网络Mask R-CNN对于无纹理堆叠目标检测性能不强的缺点，除了将彩色图像作为网络的输入之外，还将深度图像转换成HHA特征图作为网络的输入，提高了网络的检测性能；

4、本发明对Mask R-CNN的结构进行了改进，引入了STN模块(Spatialtransformer networks)，解决了原有模型不具有旋转不变性的问题；

5、本发明对现有的4PCS算法进行了改进，主要包括寻找全等基方法的改进以及增加了过滤不匹配基的操作，提高了算法的运行速度以及最终输出的转换矩阵的准确程度；

6、本发明设计了离群点过滤算法，对分割出的点云进行了过滤操作，将不属于目标物体的点滤除，提高了最终的位姿匹配精度；

7、本发明重新定义了姿态误差，可对对称的物体进行成功检测。

8、整个解决发明最终位姿估计的精度很高，其中距离误差为0.82mm，角度误差为0.91deg，其鲁棒性、准确性、实时性都能满足实际应用的要求。

附图说明

图1为本发明的方法流程图。

图2为HHA可视化效果图，其中，图(2a)为深度图，图(2b)为HHA特征图，图(2c)为水平视差图，图(2d)为距离地面高度图，图(2e)为法向量与重力夹角图。

图3为增加STN空间变换网络的Mask R-CNN网络示意图。

图4为STN网络结构。

图5为寻找近似全等的基示意图，其中，图(5a)为仿射变换前后比例不变的示意图，图(5b)为仿射变换后可能交点的示意图，图(5c)为寻找近似全等基的示意图。

图6为4PCS算法中的全等的基，其中，图(6a)为原始基示意图，图(6b)为未改进的4PCS算法找出的全等基示意图。

图7为实施例中本发明的准确率统计图，其中，图(7a)(7b)(7c)分别为x、y、z方向的位移误差，图(7d)(7e)(7f)分别为三个欧拉角方向的角度误差。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

三维空间中物体的位姿估计是机器人感知环境的重要一步，很多场景中都有对物体位姿进行估计的应用，例如机器人抓取、机器人避障寻迹、自动驾驶等。随着机器人与人工智能技术的不断发展，基于机器视觉的物体位姿估计方法将会有十分广泛的应用。

针对于工业生产线以及生活环境中工件产品或者生活物品的空间位姿检测，本发明提供了一套基于实例分割网络和迭代优化的三维空间物体位姿估计的方法。该方法对于散乱无序摆放或堆叠在平台上的物体进行精确的位姿估计。得到物体的精确位姿之后，便可结合机器人操控进行一系列的应用操作，比如进行物体分拣、物体分类、指定物体抓取等等。

如图1所示，本发明以彩色图像和深度图像作为输入，首先将深度图像转换成HHA特征图和点云；然后将HHA特征图和彩色图像输入到实例分割网络，实现任意位姿物体的实例分割；再根据实例分割的结果，在点云上分割出目标点云；最后利用改进的4PCS算法和ICP算法将分割出的目标点云和目标模型的点云进行匹配和位姿精修，从而得到精确的位姿估计结果。

具体包括以下步骤：

1、深度图像转换为HHA特征图：将深度图像转换为HHA特征图，作为实例分割网络的输入，提升模型的准确率。

深度图像转换为HHA特征图的原理如下：

HHA(Horizontal disparity,Height above ground,Angle with gravity)即将深度图像转换为三种不同的通道(水平差异，对地高度以及表面法向量的角度)。深度学习在彩色图上的应用已经相当成熟，但是在深度图上的应用还是比较少的。经过试验和相关调研，我们发现将深度图转换为HHA图后进行训练的模型有较高的准确率，因此我们将深度图先转换为HHA三个通道，然后再通过CNN提取特征。HHA三个通道分别为：

水平方向上视差：视差与深度呈倒数关系，因此水平方向上的视差计算如下描述所示。

距离地面高度：计算距离地面的高度首先要确定一个世界坐标系，然后得到世界坐标系到相机坐标系的旋转矩阵和平移向量，最后通过坐标变换得到距离地面的高度。

法向量与重力的夹角：重力的方向在工作区间内一般与所设的世界坐标系的z轴负方向相同，因此就是求法向量与世界坐标系z轴负方向之间的夹角。

1.1计算水平方向上的视差：

取深度图中的最小值为d_floor，取深度图中的最大值为d_ceil；

遍历整个深度图中所有的像素点D，同时新建一个视差图H，且其中h_floor＝1/d_ceil，h_ceil＝1/d_floor；

对于视差图图中的一个像素有：

H'[y,x]＝-1/D[y,x]

H[y,x]＝(H'[y,x]-h_floor)/(h_ceil-h_floor)

1.2计算距离地面的高度：

建立一个高度图H，旋转矩阵为平移向量为

H[y,x]＝p.z

1.3计算法向量与重力的夹角：

建立一个夹角图A，首先计算深度图中每一点上的法向量，计算点云中一点p₀的法向量

计算完上述三个通道之后，为了计算和存储方便，分别将三个通道的值线性变换到0～255之间，将其可视化结果如图2所示。

2、Mask R-CNN的改进与应用：通过在Mask R-CNN的ROIAlign层后增加STN模块，提升网络的旋转不变性性能；利用改进的Mask R-CNN网络，对物体进行识别以及分割出其掩膜。

Mask R-CNN的特征提取网络的骨干网络是卷积神经网络(CNN)，由于卷积神经网络(CNN)所提取的特征不具有旋转不变性，训练得到的模型对物体旋转较为敏感。针对六自由度的物体的位姿估计问题，目标物体的方向必然是在全空间范围内的，为此，本发明在原有Mask R-CNN网络的基础上，增加空间变换网络(Spatial Transformer Network，STN)，从而解决旋转不变性的问题。

具体而言，如图3所示，本发明在Mask R-CNN原有的ROIAlign Layer之后插入空间变换网络STN，Spatial Transformer Network是一个可微模块，根据输入的特征对其进行相应的空间变化，输出变换后的特征。输入特征U经过Spatial Transformer Network模块的定位网络、网格生成器以及采样器后输出特征V。

具体地，记定位网络的输入为特征U∈R^H×W×C，其中W,H,C分别为长、宽和通道数，网络的输出为空间变化T_θ的参数θ，参数θ的个数由空间变换的类型决定，本方案所采用的空间变换为2D仿射变换，则：

定位网络内部可以由一些全连接层或者卷积层再加一个回归层组成。

网格生成器本质上就是在输入特征中选取需要进行空间变化的点，记SpatialTransformer Network的输出特征为V∈R^H'×W'×C，其中W',H',C分别为输出特征的长、宽和通道数，输出特征的通道数和输入特征的通道数相同，不能改变，并且空间变换T_θ将分别作用于输入U的各个通道以保证每个通道上的变换一致。并记点集其中为输出特征图中点的坐标，由定位网络输出的参数θ和G我们就可以在输入特征中确定需要进行空间变换的点的集合T_θ(G)：

其中是输入特征中点的坐标。

采样器输入网格生成器生成的点集T_θ和输入特征U，最终输出经过空间变换后的特征V，具体如公式所示：

其中Φ_x和Φ_y是采样核函数k()的参数，表示输入特征U在坐标(n,m)下第c个通道上的值，Vi^c表示输出特征在坐标下第c个通道上的值。理论上可以使用任何采样核函数，只要可以对和求导，因为网络训练需要对上式求导。

3、4PCS算法的改进及应用：对4PCS算法进行改进，并用其获得物体的初始位姿；

4PCS算法是用来解决最大公共点集问题的方法。算法的输入是两个点云和距离参数δ，输出是两点云之间刚体转换矩阵T。4PCS算法基于的原理是在点云的仿射变换和刚体运动后，点云中定义的共面点集的比例是恒定的。

在4PCS算法中一个关键的方法是使用了宽基，曲线之间基于宽基的对齐是比使用窄基更加稳定的。其主要的步骤是基于RANSAC方法来进行对齐。具体来说，首先我们得到两个点云P和Q作为输入；每次循环首先会从点集P中挑选共面的宽基B，具体实现时，先从点集P中随机选取3个点，然后在剩下的点中选取第四个点构成共面的四点，第四个点的选取尽可能使得每个点之间的距离最大(因为我们要使用宽基)，并且与前3个点近似共面(显然由于噪声的存在，完全共面并不现实)。

在点集P中选取好宽基B后，算法下一步会在点集Q中通过4-points的仿射不变性找出所有与宽基B“全等”的基，构成集合U。选取全等基的方法首先使用基B中的点先定义两个仿射无关的比例，如图5(a)所示。假设在点集Q中找到两点q₁和q₂，并且|||q₁-q2||-||a-b||≤δ，则点q₁，q₂有可能与点a，b对应，则直线ab与cd相交的点e的对应点可能为：

e₁＝q₁+r₁(q₂-q₁) (4)

或者

e₁＝q₂+r₁(q₁-q₂) (5)

同理也可以根据c，d的对应点(设为q₃，q₄)求得e的对应点：

e₂＝q₃+r₁(q₄-q₃) (6)

或者

e₂＝q₄+r₁(q₃-q₄) (7)

则当e₁≈e₂时，q₁,q₂,q₃,q₄就是我们所要找的一组与点a,b,c,d近似“全等”的基，如图5(c)中的q₅,q₃,q₄,q₁。

在Q中找出所有与基B近似全等的基后，对于U中的每个基Ui，利用最小二乘的思想计算B到Ui的刚体变换Ti。得到刚体变换Ti后，我们将点集P进行变换Ti，然后对变换后的点集中的点在Q中查找最近点，统计最近点距离小于δ的个数Si，最大Si对应的Ti就是最优的刚体变换T。

仔细研究4PCS算法，可以发现从点集Q中提取的基与B并不是全等的，如图6所示，将线段q₁q₂绕交点转动一定角度后便不再与原基全等，但是4PCS仍然会找出点q₁′,q₂′,q₃,q₄作为与p₁,p₂,p₃,p₄全等的基。这一缺点会导致4PCS算法需要更多的求解时间，并且还有可能影响最终的匹配结果。因此，我们对4PCS算法进行了改进。具体是我们设置一个允许的角度阈值∈，然后通过比较基中两个向量之间的点积来判断角度是否近似相等，如果角度差在所设阈值范围内，则将其作为全等基，否则将其舍弃。

4、点云滤波的设计和ICP算法的应用：设计点云滤波算法将分割点云的离群点滤除，最后应用ICP算法得到最终的输出位姿。

经过改进的4PCS算法输出的刚体变换T的精度不高，所以设计的匹配模块在改进的4PCS算法后面通过点云滤波算法和ICP迭代算法来提升最终匹配的精度。

具体包括以下步骤：

41、点云滤波算法的设计

实际要匹配的两个点云并不是完全重合的，分割得到的点云在与CAD模型生成的点云进行匹配的时候存在许多离群点，将这些离群点滤去有助于提高最终的匹配精度。针对此问题我们设计了点云滤波算法，该算法的输入是两个点集P和Q，初始刚体变换T，以及允许的距离误差δ，其中点集P是由物体的CAD模型转换过来的，因此不对其进行滤波，只对点集Q进行离群点滤除。

具体方法是，首先使用T对点集P进行刚体变换，然后对变换后的点集建立kd树，建立好kd树后，对点集Q中的每个点在kd树中找到与之距离最近的点，如果两点间的距离大于所设的参数δ，则在点集Q中去除该点。

42、ICP算法的应用

在滤去离群点后，为了提升匹配精度，我们使用ICP算法对滤去离群点后的点云进行迭代匹配。ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复选择对应关系点对，计算最优刚体变换，直到满足正确配准的收敛精度要求。

ICP算法的输入包括两个点集Pn和Qm，初始旋转变换R和平移变换t，以及距离误差δ，具体步骤如下：

步骤1：根据当前R和t，对点集Pn进行变换，并将变换后点集中的每个点在Qm中找出距离最近的点，构成点集Qn；

步骤2：计算Pn变换后点集和Qn之间的距离的均方根误差：

通过奇异值分解求得使E(R,t)最小的R和t；

步骤3：如果E(R,t)＜δ，结束迭代，输出R和t；否则跳转至步骤1。

实施例：

为验证本方案的效果，本发明分别进行了物体识别实验和位姿估计实验，分别用来评估实例分割网络的识别效果和最终输出位姿的准确度。

为了验证物体识别的效果，我们分别在一个已有“Shelf&Tote”Benchmark数据集和我们自己采集的数据集上进行了实验，其中“Shelf&Tote”Benchmark数据集中的物体都是纹理特征比较丰富的，而我们自己采集的数据集中的物体都是缺少纹理信息的，而且存在大量同类物体堆叠混杂的情况。

无论是“Shelf&Tote”Benchmark数据集还是自采的数据集，都有较好的识别结果。

为了评估本方法的性能，定义位姿误差如下：

在姿态误差的基础上，我们定义了算法的准确率，它是指正确检测出目标并正确估计目标位姿的样本数占总样本数的比例，“准确”的判断标准如下：

k_m*d＞m (9)

其中k_m为系数，d为目标物体对应的模型的直径，若满足式(9)，记为正确检测目标并正确估计位姿。

我们在自采数据集上进行了实验，并与现有的LINEMOD算法做了比较。我们将系数k_m从5％变化到15％，统计本方法和LINEMOD算法在测试集上的运行的准确率，统计结果如表1所示。从表中可以看出，在k_m＝13％时本方案的准确率已经接近100％在k_m＝9％时，此时肉眼可以看出明显的匹配误差，本方案的准确率达到了98.10％，比LINEMOD方法高了大约9个百分点。

表1算法准确率对比

为了进一步测试算法计算出的位姿精度，取k_m＝9％时本方案准确检测的例子，将位姿转换为X,Y,Z,r,p,y六个直观的变量，并与ground truth比较，得到在距离和角度上的误差的频率直方图如图7所示。从图中可以看出在本方案正确检测和估计目标位姿的情况下，在X,Y,Z方向下的位置误差大部分分布在0-1mm之间，其距离精度在1mm左右；在r,p,y三个角度下的角度误差大部分分布在0-1deg之间，其角度精度在1deg左右。统计图7中的数据，可以算出距离误差和角度误差的的均值和方差为：

e_d＝0.82±0.21mm (10)

e_a＝0.91±0.29deg (11)

Claims (8)

1.一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，包括以下步骤：

1)将深度图像转换成HHA特征图和场景点云；

2)将HHA特征图和彩色图像输入到增加空间变换网络后的实例分割网络中，进行任意位姿物体的实例分割获得物体种类识别结果以及掩膜分割结果；

3)根据实例分割的结果，在场景点云上分割出目标点云；

4)根据改进的4PCS算法和ICP算法将分割出的目标点云和目标CAD模型的模型点云进行匹配和位姿精修，从而得到精确的位姿估计结果。

2.根据权利要求1所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的HHA特征图中的每个像素点均包括水平方向视差、距离地面高度以及法向量与重力间夹角的三个通道数值。

3.根据权利要求1所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的空间变换网络包括定位网络、网格生成器以及采样器，所述的定位网络由多个全连接层或者卷积层再加一个回归层组成，其以输入特征为输入，其空间变换为2D仿射变换，其输出为空间变化T_θ的参数θ，所述的网格生成器以参数θ以及输入特征图中点的坐标集T_θ(G)为输入，以经过空间变换后的特征为输出，则有：

其中，Φ_x和Φ_y为采样器中的采样核函数k(·)的参数，为输入特征U在坐标(n,m)下第c个通道上的值，V_i ^c为输出特征在输出特征图中第i个点的坐标下第c个通道上的值，H'为输出特征V的高度，W'为输出特征V的宽度，H为输入特征U的高度，W为输入特征U的宽度，C为通道总数，为输入特征中点的坐标。

4.根据权利要求1所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的步骤4)具体包括以下步骤：

41)采用改进的4PCS算法获取目标点云和模型点云之间的刚体转换矩阵T；

42)对目标点云进行点云滤波；

43)采用ICP算法对滤波后的点云进行迭代匹配，得到精确的位姿估计结果。

5.根据权利要求4所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的步骤41)中，改进的4PCS算法以目标点云和模型点云作为输入，以输入两点云之间的刚体转换矩阵T为输出，并且在原4PCS算法中设置一个角度阈值∈，通过比较基中两个向量之间的点积判断角度是否近似相等，若角度差在角度阈值∈的范围内，则将其作为全等基，若否，则舍弃。

6.根据权利要求4所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的步骤42)具体为：

采用刚体转换矩阵T对模型点云进行刚体变换，并对变换后的点云建立kd树，对目标点云中的每个点在kd树中找到与之距离最近的点，如果两点间的距离大于设置的阈值，则在目标点云中剔除该点。

7.根据权利要求4所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的步骤43)具体包括以下步骤：

431)将滤波后目标点云的点集Qm、模型点云的点集Pn、初始旋转变换矩阵和平移变换矩阵作为ICP算法的输入，并设置距离误差δ；

432)根据当前旋转变换矩阵R和平移变换矩阵t，对点集Pn进行变换，并将变换后点集中的每个点在Qm中找出与之距离最近的点，构成点集Qn；

433)计算点集Pn在变换后点集与点集Qn之间的距离的均方根误差E(R,t)，通过奇异值分解求得使E(R,t)最小的旋转变换矩阵R和平移变换矩阵t；

434)当E(R,t)＜δ时，结束迭代，输出此时的旋转变换矩阵R和平移变换矩阵t，否则返回步骤432)。

8.根据权利要求7所述的一种基于实例分割网络和迭代优化的6D位姿估计方法，其特征在于，所述的步骤432)中，均方根误差E(R,t)的计算式为：

其中，q_g为点集Qn的点，p_g为点集Pn中的点，n为点集中点的总数。