基于自适应高斯权快速点特征直方图的目标位姿估计方法与流程

1.本发明属于三维点云处理技术领域，具体地说，本发明涉及基于自适应高斯权快速点特征直方图的目标位姿估计方法。


背景技术：

2.随着工业自动化、智能化的发展，企业对产品的生产效率和质量要求越来越高。传统的人工辅助生产模式，逐步显露出生产效率低、产品质量达标率低、产品质量浮动较大、人工成本高等诸多缺点。机器取代人工，成为了工业发展的趋势和必然选择。
3.视觉为人类的活动和生产提供了诸多的便利，使人类能够快速的感知环境并适应环境，机器视觉是通过照相机等设备来模仿并代替人眼，通过机器的自主感知来辅助工业生产。机器人视觉抓取系统将成为未来的趋势，其发展前景广阔，是当下的热门研究课题。通过视觉来识别被抓取物体并判断其位姿不仅可以提高生产线调试的效率，而且也提高了机器人抓取系统的灵活性以及适应性，使其对于复杂环境下的位姿估计与定位引导更具有优势。
4.在仓储环境、服务场所、工业生产等机器人需要按照用户命令抓取指定物体的应用场景中，精准的目标位姿估计能够为机械臂抓取、工件安装、物料搬运、质量检测等任务提供有效的信息支持，可以提高工业自动化水平。
5.当前技术条件下，位姿估计主要是通过对比物体的模板点云以及深度传感器获取的场景点云数据，提取点云特征描述符进行特征匹配，在场景点云数据中实现位姿估计。根据特征识别出物体后，用配准算法将场景点云数据与物体对应的模板点云数据进行匹配。目前，应用较广泛的点云特征描述符有：点特征直方图（pfh）、视点特征直方图(vfh)、方向直方图签名（shot）、快速点特征直方图（fpfh）等，其中，快速点特征直方图（fpfh）是点特征直方图（pfh）的改进版本，这种特征描述符只有33个维度，可以节省运算空间，且运算效率高，使用范围更广。然而，在计算关键点特征时，现有快速点特征直方图（fpfh）算法在选取的权重系数时，仅考虑了关键点和邻域点之间的绝对距离，使得权重系数差距过大，降低了快速点特征直方图（fpfh）特征描述符的鲁棒性；现有的快速点特征直方图（fpfh）特征描述符的计算方法没有考虑邻域半径的选取标准，采用手动多次调试的方法确定邻域半径，降低了位姿估计的效率与精度。
6.现有技术中，peng wu在论文“3d scene reconstruction based on improved icp algorithm”（microprocessors and microsystems，2020）中，提出用关键点和各邻域点之间的绝对距离以及平均距离来构造指数函数，并以此作为权重系数来计算关键点的快速点特征直方图（fpfh）特征，但是该方法的权重系数差距仍较大，并且该方法没有考虑到特征提取时邻域半径的选取标准。


技术实现要素：

7.本发明提供基于自适应高斯权快速点特征直方图的目标位姿估计方法，克服了现
有位姿估计技术中采用固定邻域半径，邻域点权重系数差距过大，导致精度和效率过低、鲁棒性差的问题。
8.为了实现上述目的，本发明采取的技术方案为：基于自适应高斯权快速点特征直方图的目标位姿估计方法，具体包括以下步骤：步骤s1：读取点云数据：读取模板点云数据与初始场景点云数据；步骤s2：数据预处理：对读取的初始场景点云数据进行处理，获得预处理后的场景点云数据；步骤s3：获取自适应最优邻域半径：针对模板点云数据与预处理后的场景点云数据，基于邻域特征熵，分别计算每个点的自适应最优邻域半径；步骤s4：计算法向量：依次计算模板点云数据和预处理后的场景点云数据中每个点的邻域协方差矩阵，进行特征值分解，取最小特征值对应的特征向量为该点的法向量；步骤s5：关键点提取：设置固定搜索半径，对模板点云数据以及预处理后的场景点云数据中每个点进行半径搜索得到邻域点，计算这些邻域点的权值以及邻域协方差矩阵，并进行特征值分解，设置特征值比值阈值，分别筛选出模板点云数据以及预处理后的场景点云数据的关键点；步骤s6：特征提取：利用步骤s3中的自适应最优邻域半径，计算每个邻域点的高斯权值，构建自适应高斯权快速点特征直方图（gwfpfh）；步骤s7：点云粗配准：根据步骤s6中的自适应高斯权快速点特征直方图（gwfpfh），在预处理后的场景点云数据中搜索与模板点云数据中特征相似的点作为匹配点对，并去除错误匹配点对，计算粗配准刚体变换矩阵，将模板点云数据进行粗配准变换；步骤s8：点云精配准：根据步骤s7中粗配准变换后的模板点云数据和预处理后的场景点云数据，计算初始精配准刚体变换矩阵，根据设置好的阈值以及最大迭代次数确定最终的精配准刚体变换矩阵；步骤s9：位姿估计：根据步骤s8中最终的精配准刚体变换矩阵的旋转矩阵计算预处理后的场景点云数据相对于模板点云数据的旋转角度，利用最终的精配准刚体变换矩阵的平移矩阵计算预处理后的场景点云相对于模板点云的平移距离。
9.优选的，所述步骤s1中，所述模板点云数据采用离线提取算法，所述场景点云数据采用在线提取算法；场景点云数据由深度相机采集获取，包括场景物体点云的空间位置坐标。
10.优选的，所述步骤s2中，数据预处理过程为：对模板点云数据和采集到的场景点云数据进行平面分割以及体素网格预处理，得到预处理后的场景点云数据。
11.优选的，所述步骤s3中，获取自适应最优邻域半径的方式为：确定最小、最大邻域半径及间隔大小，根据邻域协方差矩阵分解得到的特征值计算每个邻域半径对应的邻域特征熵，最小的邻域特征熵对应的邻域半径为自适应最优邻域半径。
12.优选的，所述步骤s4中，计算得到的法向量需要根据指向视点原则重新确定方向，从而保证所有的法向量指向一致。
13.优选的，所述步骤s5中，关键点提取采用内部形状签名（iss）方法。
14.优选的，所述步骤s6中，特征提取具体过程为：a、利用步骤s3中获得的自适应最优邻域半径，确定模板点云数据与预处理后的场
景点云数据中每个点的邻域范围；b、利用步骤s4中的法向量在关键点与其对应的邻域点处建立局部坐标系；c、利用局部坐标系，计算关键点与其邻域点的几何关系，得到简化的点特征直方图（spfh）；d、计算每个关键点以及其邻域点之间的距离均值和方差，构建高斯权函数，确定每个邻域点的高斯权值；e、根据邻域点的高斯权值对每个邻域点的简化的点特征直方图（spfh）进行加权，得到关键点的自适应高斯权快速点特征直方图（gwfpfh）。
15.优选的，所述步骤s7中，利用随机采样一致性的算法去除错误的匹配点对，保留正确的匹配点对，并通过四元数法求得粗配准刚体变换矩阵。
16.优选的，所述步骤s8中，精配准的具体过程为：a、采用k
‑
d tree算法，在预处理后的场景点云数据中搜索与模板点云数据中最近的点作为对应点；b、采用奇异值分解法求解初始精配准刚体变换矩阵，并基于初始精配准刚体变换矩阵将模板点云数据进行变换，得到新的模板点云；c、重复a、b步骤，当变换后的新的模板点云数据与预处理后的场景点云数据最近点之间的平均欧式距离小于设置的阈值或者迭代次数超过最大次数时，迭代终止，得到最终的精配准刚体变换矩阵。
17.优选的，所述步骤s9中，旋转矩阵表示的是欧拉角，可以转化为弧度制表示三个方向的旋转角度，平移矩阵表示的是三个方向的平移距离。
18.采用以上技术方案的有益效果是：1、本发明提出了基于自适应高斯权快速点特征直方图的目标位姿估计方法，构建了基于高斯权函数的特征描述符，解决了权重系数差距过大的问题，克服了鲁棒性差的问题；建立了自适应最优邻域半径选取标准，实现了高精度的位姿估计；2、本发明在保证精度的情况下，采用邻域自适应的特征提取算法，相较手动调整邻域的方法，特征提取所需时间减少约25%，提升了真实场景中目标的实时位姿估计效率。
附图说明
19.图1是本发明基于自适应高斯权快速点特征直方图的目标位姿估计方法流程图；图2是实施例1模板点云；图3是实施例1场景点云；图4是实施例1模板点云与场景点云偏差示意图；图5是实施例1模板点云与场景点云降采样后偏差示意图；图6是实施例1模板点云关键点；图7是实施例1场景点云关键点；图8是实施例1模板点云与场景点云的特征匹配点对；图9是实施例1模板点云与场景点云的粗配准结果示意图；图10是实施例1模板点云和场景点云的精配准结果示意图；图11是对比例1在场景点云中精配准的托盘示意图；
图12是实施例2模板点云；图13是实施例2场景点云；图14是实施例2模板点云与场景点云偏差示意图；图15是实施例2模板点云与场景点云降采样后偏差示意图；图16是实施例2模板点云关键点；图17是实施例2场景点云关键点；图18是实施例2模板点云与场景点云的特征匹配点对；图19是实施例2模板点云和场景点云的精配准结果示意图；图20是实施例2模板点云和场景点云的精配准结果示意图。
具体实施方式
20.下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，目的是帮助本领域的技术人员对本发明的构思、技术方案有更完整、准确和深入的理解，并有助于其实施。
21.如图1至图20所示，本发明是基于自适应高斯权快速点特征直方图的目标位姿估计方法，基于邻域特征熵，确定特征提取的自适应最优邻域半径，同时计算关键点和邻域点之间的距离均值和方差，构建新的特征描述符的高斯权函数，使得每个邻域点的权值设定能够更准确地描述邻域点对于关键点特征的影响。采用该方法使整个位姿估计过程精度和效率更高，鲁棒性更强。
22.以下用具体实施例对具体工作方式进行阐述：实施例1：基于自适应高斯权快速点特征直方图的目标位姿估计方法，具体包括以下步骤：步骤s1：读取点云数据：读取托盘模板点云数据和kinect v2采集到的初始场景点云数据，数据为ply格式，提取点云三维坐标信息，模板点云数据中共有78894个点，初始场景点云数据中共有103995个点，读取结果如图2所示；步骤s2：数据预处理：采用pcdownsample函数对模板点云数据和初始场景点云数据进行体素网格预处理，获取预处理后的场景点云数据；步骤s3：获取自适应最优邻域半径：最小邻域半径r_min=0.006m，最大邻域半径r_max=0.01 m，半径间隔0.001 m，计算不同邻域半径对应的特征熵，取最小特征熵对应的半径为特征提取的自适应最优邻域半径；步骤s4：计算法向量：采用主成分分析（pca）计算点云法向量，设置邻域点个数k=8，取邻域的中心点坐标作为视点方向，确定法向量的指向；步骤s5：关键点提取：采用iss方法获取模板点云数据与预处理后的场景点云数据的关键点，邻域半径为0.05m，两个阈值分别设置为0.8和0.4；步骤s6：特征提取：利用步骤s3中获得的自适应最优邻域半径确定模板点云数据与预处理后的场景点云数据中每个点的邻域范围；计算关键点与其邻域点的几何关系，得到简化的点特征直方图（spfh）；计算每个关键点与其邻域点之间的距离均值和方差，构建高斯权函数，将关键点与邻域点的简化的点特征直方图（spfh）利用高斯权函数加权计算，得到关键点的自适应高斯权快速点特征直方图（gwfpfh），然后在预处理后的场
景点云数据中找到距离模板点云数据最近的特征描述符（gwfpfh）作为特征匹配点对；步骤s7：点云粗配准：利用随机采样一致性的算法，随机选取三对步骤s6中确定的特征匹配点对，计算它们的粗配准刚体变换矩阵，基于粗配准变换矩阵与模板点云数据的原始坐标信息得到转换后的模板点云数据的坐标信息，计算转换后的模板点云数据与预处理后的场景点云数据的距离并与距离阈值0.05进行比较，小于阈值的匹配点对为最终的特征匹配点对。最后计算剩余匹配点之间的旋转矩阵r和平移矩阵t，模板点云数据根据粗配准刚体变换矩阵进行刚体变换完成粗配准；步骤s8：点云精配准：利用k
‑
d tree算法，在预处理后的场景点云数据中搜索与步骤s7中变换后的模板点云数据中的对应点；利用奇异值分解法计算对应点间的旋转矩阵r0和平移矩阵t0，并将模板点云数据进行刚体变换，设置对应点之间的平均欧式距离阈值0.0001，设置最大迭代次数80次，满足迭代终止条件后结束精配准，获得最终的刚体变换矩阵r
n
=[0.987，5.824，
‑
0.159；0.005，0.999，0.033；0.159，
‑
0.033，0.987]和t
n
=[0.173；
‑
0.002；0.141]，点云最终配准偏差rms=0.0099；步骤s9：位姿估计：利用dcm2angle函数将r
n
换算成弧度制的角度，得到r
x
=0.042、r
y
=5.249、r
z
=0.023三个方向的角度偏转量；t
n
表示t
x
=0.173、t
y
=
‑
0.002、t
z
=0.141三个方向的偏移量，完成位姿估计。
[0023]
本发明将目标位姿估计流程分为离线部分与在线部分，离线部分针对模板点云数据完成相关操作，在线部分先对场景点云数据进行处理与特征提取，然后对模板点云数据与预处理后的场景点云数据进行粗配准和精配准运算，得到刚体变换矩阵，并计算模板点云数据和预处理后的场景点云数据之间的位姿偏差。
[0024]
如图1所示为本发明基于自适应高斯权快速点特征直方图的目标位姿估计方法流程图；图2为实施例1中用kinect v2采集的托盘模板点云数据；图3为实施例1中用kinect v2采集的初始场景点云数据；如图4所示，将实施例1中场景点云数据的和模板点云数据放在同个坐标系中比较其位置关系；如图5所示，为经过预处理后的场景点云数据的和模板点云数据关系；对模板点云数据以及预处理后的场景点云数据提取关键点，模板点云数据关键点如图6所示，场景点云数据关键点如图7所示，可以看出，相较图2的模板点云数据与图3的场景点云数据，点的数量明显减少，加快后续特征提取的计算效率；计算托盘模板点云数据及场景点云数据关键点的自适应高斯权快速点特征直方图（gwfpfh）特征描述符，并进行特征匹配，结果如图8所示，连线为对应匹配点之间的连线。根据特征匹配点对，利用随机采样一致性的方法对预处理后的场景点云数据和模板点云数据进行粗配准，结果如图9所示。利用icp算法对粗配准变换后的模板点云数据和预处理后的场景点云数据进行精配准，如图10所示。最后根据精配准得到的刚体变换矩阵完成位姿估计。图11所示为对比例1中采用固定半径0.008m进行特征提取得到的精配准结果，特征提取的耗时有所增加，并且精度有所降低，实施例1中点云配准的精度更高，位姿估计结果更为准确。
[0025]
对比例1：对比例1与实施例1的区别在于，采用固定邻域半径进行特征提取，采用fpfh作为
特征描述符。
[0026]
基于自适应高斯权快速点特征直方图的目标位姿估计方法，结合实施例1分析本发明提出的邻域自适应和高斯权快速点特征直方图相比于现有邻域手动调整的优势与有效益之处。
[0027]
为排除其它因素干扰，对比例1所用数据集和实施例1中相同，对比例1中步骤s1、步骤s2、步骤s4、步骤s5、步骤s7至步骤s9和实施例1中一致，关键点检测参数、误匹配点对去除距离阈值均与实施例1相同，步骤s3中特征提取的邻域半径设置为0.008m，在线特征提取用时分别为4.785s，而实施例1在线特征提取用时为3.586s，且精度有所提高。
[0028]
对比例1采用固定邻域半径，为保证所有点取到有效半径，需采用较大半径，增加了特征提取的时间，同时计算特征描述符时采用的权值计算公式可以导致邻域点间的权重系数差距过大，影响特征描述符的鲁棒性；实施例1获取自适应最优邻域半径，保证每个点得到最优邻域范围及最经济的计算时间，并且采用高斯权改进的快速点特征直方图，使特征描述符能够更加准确地描述点云特征。
[0029]
实施例2：基于自适应高斯权快速点特征直方图的目标位姿估计方法，具体包括以下步骤：步骤s1：读取点云数据：读取kinect数据集中的模板点云数据“dragonstandright_0”和初始场景点云数据“dragonstandright_24”，数据为ply格式，仅包含三维坐标信息，模板点云数据有41841个点，初始场景点云数据有34836个点；步骤s2：数据预处理：采用pcdownsample函数对模板点云数据和初始场景点云数据进行体素网格预处理，获取预处理后的场景点云数据；步骤s3：获取自适应最优邻域半径：最小邻域半径r_min=0.006m，最大邻域半径r_max=0.012m，半径间隔0.001m，计算不同邻域半径对应的特征熵，取最小特征熵对应的半径为特征提取的自适应最优邻域半径；步骤s4：计算法向量：采用主成分分析（pca）求取点云法向量，设置邻域点个数k=8，取邻域的中心点坐标作为视点方向，确定法向量的指向；步骤s5：关键点提取：采用iss方法获取模板点云数据与预处理后的场景点云的关键点，邻域半径为0.005m，两个阈值分别设置为0.8，0.4；步骤s6：特征提取：利用步骤s3中获得的自适应最优邻域半径确定模板点云数据与预处理后的场景点云数据中每个点的邻域范围；计算关键点与其邻域点的几何关系，得到简化的点特征直方图（spfh）；计算每个关键点与其邻域点之间的距离均值和方差，构建高斯权函数，将关键点与邻域点的简化的点特征直方图（spfh）利用高斯权函数加权计算，得到关键点的自适应高斯权快速点特征直方图（gwfpfh），然后在预处理后的场景点云数据中找到距离模板点云数据最近的特征描述符（gwfpfh）作为特征匹配点对。
[0030]
步骤s7：点云粗配准：利用随机采样一致性算法去除误匹配点对，计算匹配点之间的旋转矩阵r和平移矩阵t，模板点云数据根据粗配准刚体变换矩阵进行刚体变换完成粗配准；步骤s8：点云精配准：利用k
‑
d tree算法，在预处理后的场景点云数据中搜索与步骤s7中变换后的模板点云数据中的对应点，利用奇异值分解法计算对应点间的旋转矩阵r0和平移矩阵t0，并将模板点云数据进行刚体变换，设置对应点之间的平均欧式距离阈值
0.0001，设置最大迭代次数80次，满足迭代终止条件后结束精配准，获得最终的刚体变换矩阵r
n
=[1.942，0.003，
‑
0.413；
‑
0.001，2.000，0.004；0.413，
‑
0.050，1.942]和t
n
=[0.001；0.003；
‑
0.004]，点云最终配准偏差rms=0.0018，迭代次数67次。
[0031]
步骤s9：位姿估计：利用dcm2angle函数将r
n
换算成弧度制的角度，得到r
x
=0.086、r
y
=24.418、r
z
=0.2445三个方向的角度偏转量；t
n
表示t
x
=0.001、t
y
=0.003、t
z
=
‑
0.004三个方向的偏移量，完成位姿估计。
[0032]
以上结合附图对本发明进行了示例性描述，显然，本发明具体实现并不受上述方式的限制，只要是采用了本发明的方法构思和技术方案进行的各种非实质性的改进；或未经改进，将本发明的上述构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。