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

技术特征：
1.基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：具体包括以下步骤：步骤s1：读取点云数据：读取模板点云数据与初始场景点云数据；步骤s2：数据预处理：对读取的初始场景点云数据进行处理，获得预处理后的场景点云数据；步骤s3：获取自适应最优邻域半径：针对模板点云数据与预处理后的场景点云数据，基于邻域特征熵，分别计算每个点的自适应最优邻域半径；步骤s4：计算法向量：依次计算模板点云数据和预处理后的场景点云数据中每个点的邻域协方差矩阵，进行特征值分解，取最小特征值对应的特征向量为该点的法向量；步骤s5：关键点提取：设置固定搜索半径，对模板点云数据以及预处理后的场景点云数据中每个点进行半径搜索得到邻域点，计算这些邻域点的权值以及邻域协方差矩阵，并进行特征值分解，设置特征值比值阈值，分别筛选出模板点云数据以及预处理后的场景点云数据的关键点；步骤s6：特征提取：利用步骤s3中的自适应最优邻域半径，计算每个邻域点的高斯权值，构建自适应高斯权快速点特征直方图（gwfpfh）；步骤s7：点云粗配准：根据步骤s6中的自适应高斯权快速点特征直方图（gwfpfh），在预处理后的场景点云数据中搜索与模板点云数据中特征相似的点作为匹配点对，并去除错误匹配点对，计算粗配准刚体变换矩阵，将模板点云数据进行粗配准变换；步骤s8：点云精配准：根据步骤s7中粗配准变换后的模板点云数据和预处理后的场景点云数据，计算初始精配准刚体变换矩阵，根据设置好的阈值以及最大迭代次数确定最终的精配准刚体变换矩阵；步骤s9：位姿估计：根据步骤s8中最终的精配准刚体变换矩阵的旋转矩阵计算预处理后的场景点云数据相对于模板点云数据的旋转角度，利用最终的精配准刚体变换矩阵的平移矩阵计算预处理后的场景点云相对于模板点云的平移距离。2.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s1中，所述模板点云数据采用离线提取算法，所述场景点云数据采用在线提取算法；场景点云数据由深度相机采集获取，包括场景物体点云的空间位置坐标。3.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s2中，数据预处理过程为：对模板点云数据和采集到的场景点云数据进行平面分割以及体素网格预处理，得到预处理后的场景点云数据。4.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s3中，获取自适应最优邻域半径的方式为：确定最小、最大邻域半径及间隔大小，根据邻域协方差矩阵分解得到的特征值计算每个邻域半径对应的邻域特征熵，最小的邻域特征熵对应的邻域半径为自适应最优邻域半径。5.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s5中，计算得到的法向量需要根据指向视点原则重新确定方向，从而保证所有的法向量指向一致。6.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s4中，关键点提取采用内部形状签名（iss）方法。
7.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s6中，特征提取具体过程为：a、利用步骤s3中获得的自适应最优邻域半径，确定模板点云数据与预处理后的场景点云数据中每个点的邻域范围；b、利用步骤s4中的法向量在关键点与其对应的邻域点处建立局部坐标系；c、利用局部坐标系，计算关键点与其邻域点的几何关系，得到简化的点特征直方图（spfh）；d、计算每个关键点以及其邻域点之间的距离均值和方差，高斯权函数，确定每个邻域点的高斯权值；e、根据邻域点的高斯权值对每个邻域点的简化的点特征直方图（spfh）进行加权，得到关键点的自适应高斯权快速点特征直方图（gwfpfh）。8.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s7中，利用随机采样一致性的算法去除错误的匹配点对，保留正确的匹配点对，并通过四元数法求得粗配准刚体变换矩阵。9.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s8中，精配准的具体过程为：a、采用k
‑
d tree算法，在预处理后的场景点云数据中搜索与模板点云数据中最近的点作为对应点；b、采用奇异值分解法求解初始精配准刚体变换矩阵，并基于初始精配准刚体变换矩阵将模板点云数据进行变换，得到新的模板点云；c、重复a、b步骤，当变换后的新的模板点云数据与预处理后的场景点云数据最近点之间的平均欧式距离小于设置的阈值或者迭代次数超过最大次数时，迭代终止，得到最终的精配准刚体变换矩阵。10.根据权利要求1所述的基于自适应高斯权快速点特征直方图的目标位姿估计方法，其特征在于：所述步骤s9中，旋转矩阵表示的是欧拉角，可以转化为弧度制表示三个方向的旋转角度，平移矩阵表示的是三个方向的平移距离。

技术总结
本发明公开了基于自适应高斯权快速点特征直方图的目标位姿估计方法，具体包括以下步骤：步骤S1：读取点云数据；步骤S2：数据预处理；步骤S3：获取自适应最优邻域半径；步骤S4：计算法向量；步骤S5：关键点提取；步骤S6：特征提取；步骤S7：点云粗配准；步骤S8：点云精配准；步骤S9：位姿估计。基于邻域特征熵，确定特征提取的自适应最优邻域半径，同时计算关键点和邻域点之间的距离均值和方差，构建新的特征描述符的高斯权函数，使得每个邻域点的权值设定能够更准确地描述邻域点对于关键点特征的影响。采用该方法使整个位姿估计过程精度和效率更高，鲁棒性更强。棒性更强。棒性更强。


技术研发人员：邵益平 朱宝昌 鲁建厦 周敏龙 佐富兴 朱婷婷 李亚云 周晓静 钮超晔
受保护的技术使用者：浙江工业大学
技术研发日：2021.09.09
技术公布日：2021/12/12