基于目标工件点云的位姿估计方法研究

闫厚任，孔德杰，郭飞，张科航，谭文

（长春理工大学 光电工程学院，长春 130022）

近年来，机器人在制造领域各个环节的应用越来越广泛。为了提高机器人在应用中的生产效率，满足机械臂柔性装配等方面的需求，通过对任意位姿的目标工件进行三维重建，获取工件在空间中的位姿信息，再将该信息传递至机器人来实现机械臂对工件的准确抓取，从而提升工业生产的效率。 目前，应用最广泛的点云配准算法是迭代最近点算法ICP，但传统的ICP算法收敛速度较慢，且在点云数据集初始位置相差较大时，容易陷入局部最优解，为了改善这一问题，本文给出一种改进的点云配准算法［1］。

1 点云特征描述方法

点云特征描述是点云识别、配准的前提。特征点是指从点云中提取出的具有代表性和区分性的点集，用于描述点云的特征属性。 其关键在于从点云中提取所需目标的特征描述子，通过目标的特征描述子来识别物体，识别准确度依赖于所提取特征描述子的优劣［2］。

点云特征描述方法一般分为全局特征和局部特征两种类型。常见的全局特征有VFH、ESF等，局部特征有PFH、FPFH、SHOT 等。本文选择局部特征FPFH 作为特征描述子。

1.1 表面法线

在三维物体的描述中，表面法线是非常重要的属性之一。它不仅可以反映出三维表面的曲率变化，还可以获得三维表面的形状特征。 这里采用主成分分析法PCA，通过查询点的最近邻域构建协方差矩阵，从而计算出表面法线。 这个过程可以通过求取协方差矩阵的特征值和特征向量来完成。设待求表面法线的点云P中任意一点Pm，创建的协方差矩阵为：

式中，Pm为查询点所在的位置坐标；k为查询点最近邻域点的个数；为最近邻域点云的质心所在的位置坐标；为矩阵的第n个特征值和特征向量，可以用来表示点云的主要方向。

1.2 点特征直方图

点特征直方图PFH［3］是一种常用的点云局部特征描述方法，能够有效地描述点云中的局部几何特征。其利用表面法线和查询点k邻域中所有点的关系，通过几何特征处理k邻域中的点，并且用多维直方图表示几何特征。PFH 特征具有很强的抗干扰能力，即使在不同的噪声下也能表现良好。图1 为PFH 特征的影响区域图，图中pq表示查询点，虚线部分表示半径为r 的球体，将查询点与其所有邻域点全部相连，计算邻域中所有的点对关系，即可获得PFH 直方图。虽然PFH 特征描述方法在点云处理中具有广泛的应用，但是其计算复杂度比较高，为O(k2)。

图1 PFH 特征邻域图

设pa与pb为相连的两个点，两点的法线分别为na、nb，如图2 所示。α、ϕ、θ分别表示两向量的偏差角度。

图2 局部坐标系

则有局部坐标系为：

法线的偏差角度如下：

其中，两点之间的欧氏距离为d= (pb-pa)z。通过对每组点对进行四元组(α,ϕ,θ,d) 求解，可以将其特征信息进行压缩，从12 个值减少为4 个。

在PCL 点云库中，PFH 特征提取算法会从每组点对的四元组(α,ϕ,θ,d) 中取出前三元(α,ϕ,θ)，并将每个特征值分成五份，形成一个125 维的直方图，最终得到的PFH 特征向量即为一个125 维的向量。这个向量可以用于描述点云中每个点的几何特征信息。

1.3 快速点特征直方图

快速点特征直方图FPFH［4］描述子是一种改进的点云局部特征描述方法，它是在PFH 的基础上进行优化得到的，能够更快速、更准确地描述点云中的局部几何特征。通过引入点云中每个点与其法向量方向的偏差角度作为权重，来对PFH 进行优化，从而提高其对点云几何形状的描述精度。其计算复杂度从FPH 的O(k2)降低到了O(k)。FPFH 特征邻域图如图3 所示。

图3 FPFH 特征邻域图

FPFH 描述子算法步骤如下：

（1）通过计算查询点pq与k邻域之间的三元组(α,ϕ,θ) 来计算简化点特征直方图SPFH，其中三元组(α,ϕ,θ) 表示邻域中的每个点的位置关系。

（2）对于每个查询点，再次查找其邻域点的k邻域，计算每个邻域点的三元组(α,ϕ,θ)，并将其与查询点的三元组结合起来进行加权平均，权重wk表示第二个邻域中第k个点在第一个邻域中的权重。计算公式如下：

与PFH 相比，FPFH 算法在计算查询点和邻域点之间的关系时，并没有完全将二者相连。因此，FPFH 算法的计算值较少。由于采用了加权的方法，使得其在计算时涵盖了邻域点的邻域值，并且某些点可能会存在于多个邻域中，会存在重复计算的情况。

在PCL 点云库中，FPFH 特征提取算法将每个特征值分成11 份，形成一个33 维的直方图。因此FPFH 特征向量是一个33 维的向量，用于描述点云中每个点的几何特征信息。

2 点云配准

2.1 点云配准原理

目标工件点云和模板点云存在相似的部分，并且两个点云位于不同的坐标系中。本文选取的目标工件均为刚体，利用点云配准使目标工件点云和模板点云对齐，并获取位姿变换矩阵。

将目标工件点云中的一点P1和模板点云中的一点P1′ 作为一个匹配点对，两者的旋转变换矩阵为R，平移变换为t，可得：

设α、β、γ分别为沿着x轴、y轴、z轴的旋转角，tx、ty、tz分别为沿着x轴、y轴、z轴的平移距离，共有六个未知数α、β、γ、tx、ty、tz。 因此，需要不少于三个匹配点才能求取位姿变换矩阵，且三个点不共线。由于噪声的存在，通过一个匹配点对无法得到精确的变换矩阵，因此需要使用多个匹配点对进行求解。对于已知匹配点对的情况下，可以通过最小二乘法来求解变换矩阵，最小化点云之间的距离误差。

设目标工件点云和模板点云分别为Pi(i=1,2,…,n)、Qi(i= 1,2,…,n)。为了计算旋转平移变换的最优解，使目标工件点云和模板点云间的欧式距离之和为最小，则有如下关系式：

2.2 选取模板点云

目标工件的位姿估计可以通过计算目标工件点云和模板点云的旋转平移变换来实现。选择模板点云可以利用CAD 软件建立目标工件的三维模型，并将其转换为点云数据。 也可以选取工件某一个位姿下的目标点云作为模板点云。

在CAD 软件中建立三维模型时，利用Solid-Works 软件构造工件实际尺寸的三维模型，所构造模型的坐标系原点位置即为相机视觉系统坐标系原点（选取彩色相机坐标系为视觉系统的坐标系），并且模型的坐标系原点放置在工件的下底面中心位置。

在PCL 点云库中，需要将CAD 模型转换为点云支持的文件格式，例如obj 或ply 格式。为此，首先需要使用SolidWorks 软件将CAD 模型转换为ply 格式的文件。然后，将该文件导入到点云转换程序中，并设置点云生成的合适密度。 需要注意的是，点数过多可能会降低点云配准的计算速度，点数过少则可能会导致配准结果错误。 因此，需要根据具体情况选择适当的点密度，可以通过体素滤波降采样降低点的采样密度，进而提高配准的效率和结果。

选取某一个位姿下的目标点云作为模板点云时，将该工件点云作为机械臂的初始抓取位姿。

2.3 SAC-IA 采样一致性粗配准

为了使点云配准的方向趋于正确，使旋转平移后的点云之间有较好的初始位置，避免陷入局部最优解的情况［5］，使用SAC-IA 采样一致性算法获取初始的目标点云位姿。 如果直接用ICP 迭代最近点算法，会导致计算量过大，而且可能会得不到正确的转换参数。SAC-IA 算法步骤如下［6］：

（1）从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的FPFH 特征，采样点两两之间的距离应该满足大于预先给定最小距离阈值d。

（2）在目标点云Q中查找与点云P中采样点具有相似FPFH 特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点。

（3）计算对应点之间刚体变换矩阵，然后通过求解对应点变换后与采样点距离的误差来判断当前配准结果的好坏。

（4）重复以上三个步骤，达到迭代次数后，选择距离误差和最小的对应点对，作为粗配准的最优结果。

为了测试采样一致性粗配准算法的有效性，使用多个不同位姿下的工件点云作为目标点云，分别进行与模板点云的配准，并通过配准结果和均方误差判断粗配准的位置是否可以作为进行精配准的初始位姿。

2.4 优化迭代最近点精配准算法

ICP（Iterative Closest Point，迭代最近点）［7］算法采用不断迭代的方式，用最小二乘法得到两个点云之间位置关系的最优解［8］。其对点云初始位置的要求较为严格，为了提高点云配准的成功率和精度，从而获得更准确的位姿信息，需要先进行粗配准，再使用ICP 算法进行优化。

ICP 算法的核心思想是找到两个点云中距离最近的对应点对，通过反复迭代，使得点对间的欧氏距离最小，并计算最佳的旋转矩和平移关系矩阵。该算法步骤为：

（1）将粗配准后的两片点云P′（经过坐标变换后的源点云）和Q，作为精配准的初始点集。

（2）对源点云P′ 中的每一个点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对。

（3）初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对。

（4）计算旋转矩阵R和平移向量t，使函数的值最小。

（5）设定某一阈值ε和最大迭代次数Nmax，将上一步得到的刚体变换作用与源点云P′，得到新位置点云P″，计算P″ 和Q的距离误差，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P″ 和Q，继续重复上述步骤，直到满足收敛条件。得到最终的旋转矩阵R与平移矩阵t。

本文使用SVD（奇异值分解）法［9］求解对应点集间的均方误差，求解步骤如下。

首先，计算两个点云的质心：

假设最优旋转矩阵和平移向量为R、t，则Qi与的质心相同，对点云P与Q去中心化得：

将式（9）与式（10）代入式（6）可知：

将式（12）展开，则有：

其中，U、V为3×3 的正交矩阵；Λ 为3×3 的对角矩阵。 令X=VUT，当det (X) = 1，则R=X=VUT，为最优旋转矩阵，此时。

2.5 Trimmed ICP 精配准

Trimmed ICP 算法由Chetverikov 等人［7］提出，是一种改进的点云精配准算法，它旨在解决两片点云之间不能完全重合并且包含大量离群点，以及传统的ICP 算法对于非重叠部分的点对匹配效果不佳等问题。该算法利用截断最小二乘（Least Trimmed Squares）方法，对匹配点对之间的欧式距离进行排序，选取一定百分比的匹配点对参与目标方程的优化求解，该方法可以有效降低无关点对配准结果的影响，提高配准算法的鲁棒性［10］。

其算法原理如下：

对于模板点云P中点的个数不等于目标点云Q中点个数的两个点集，模板点云中的一些点在目标点集中可能没有对应关系。假设可以匹配的点的最小保证率为σ，那么可以配对的点数量为NP0=σN，当σ= 1 时就是ICP。

（1）对于P中的每个点，找出Q中最近的点，并计算各个点对之间的距离。

（3）满足约束条件（与ICP 相同）则退出程序，否则令sum′=sum 并继续下一步。

（4）利用（2）中选定的前Np0个对应点对，采用最小二乘方法计算最佳变换(R,t)。

（5）利用(R,t) 变换待配准点云P并循环。

重复上述步骤，直到满足如下三个条件之一时，停止迭代并得出配准结果。

（1）达到最大迭代次数。

（3）两次迭代值几乎不变。

3 实验结果及分析

为验证本文算法，采用深度相机Kinect v2 分别采集长方体工件和内六角螺丝工件的点云数据进行对比实验。实验平台为Windows10 系统，CPU 主频2.3 GHz，内存16 G，利用Visual Studio C++编程进行点云配准实验。

3.1 长方体目标工件

利用SolidWorks 建立工件实际尺寸的三维模型，并导出为点云文件。 由于目标工件是对称的长方体，且目标点云为工件点云的一部分，如果将目标点云与工件完整点云进行配准，可能会得到不同的位姿信息。 因此，将模型转换的点云沿z= 0 的平面分割，利用平面上部的点云进行配准。模板点云如图4 所示。

图4 分割后的点云模板

为了验证采样一致性粗配准算法的可行性，将长方体工件摆放了6 个不同的位姿，分别进行目标点云与模板点云的配准，工件位置摆放方式如图5 所示。

图5 长方体工件摆放方式图

首先计算两点云的法向量与FPFH 特征值，设置FPFH 搜索半径为0.1 m。在提取匹配点对后，进行SAC-IA 粗配准。其中第一个位姿粗配准后的结果如图6 所示。

图6 长方体工件SAC-IA 粗配准结果

图6 中模板点云为绿色，目标工件点云为红色。目标工件在6 个不同位姿下的SAC-IA 粗配准结果如表1 所示，均方误差MSE 表示迭代结束后所有对应点距离平方和的平均值。

表1 长方体工件SAC-IA配准结果

从表1 中可以计算出粗配准的平均MSE 为0.608 5 mm2，可以作为进行精配准的初始位姿。

使用SAC-IA 粗配准结果作为配准初始位置。分别使用ICP 算法与Trimmed ICP 算法进行精配准，长方体工件其中一个位姿的配准结果如图7、图8 所示，分别为ICP 算法和Trimmed ICP算法配准后的结果。 其中红色表示目标点云，绿色表示模板点云。

图7 长方体工件ICP 算法配准后的结果

图8 长方体工件Trimmed ICP 算法配准后的结果

从图7、图8 可以看出，ICP 算法配准之后，两片点云已经大致重合，Trimmed ICP 算法配准之后，两片点云的各个部位均得到了较好的融合，绿色的模板点云基本配准到红色的目标点云的对应位置。 为了验证算法的有效性和优劣，采用均方误差（MSE）作为点云配准精度的评价指标。表2 统计了6 个不同位姿下精配准ICP 算法和Trimmed ICP 算法的用时与误差。

表2 长方体工件精配准结果对比

从表2 中可以看出，相对于ICP 算法，Trimmed ICP 算法无论是配准速率还是配准精度都有了明显的改善。在6 组点云数据实验中，该算法配准时间平均降低了91%，MSE 降低了17.0%，配准效率得到了提升。

3.2 内六角螺丝目标工件

将内六角螺丝摆放12 个不同的位姿，选取第一个位姿下的工件点云作为模板点云，分别进行其余11 个位姿下目标点云与模板点云的配准，工件位置摆放方式如图9 所示。

图9 内六角螺丝工件摆放方式图

进行采样一致性粗配准，计算FPFH 特征值，设置FPFH 搜索半径为0.1 m，在提取匹配点对后进行粗配准。内六角螺丝工件在11 个不同位姿下的SAC-IA 粗配准结果如表3 所示。

表3 内六角螺丝工件SAC-IA配准结果

从表3 中可以计算出粗配准的平均MSE 为0.562 602 73 mm2，可以作为进行精配准的初始位姿。

使用SAC-IA 粗配准结果作为初始配准位置，分别进行ICP 精配准和Trimmed ICP 精配准，内六角螺丝工件其中一个位姿下的配准结果如图10、图11 所示。 分别为ICP 算法配准后的结果、Trimmed ICP 算法配准后的结果。 其中红色表示待配准螺丝工件点云，绿色表示模板点云。

图10 内六角螺丝工件ICP 算法配准后的结果

图11 内六角螺丝工件Trimmed ICP 算法配准后的结果

从图10、图11 中可以看出，ICP 算法配准之后，两片点云仍然大致重合，Trimmed ICP 算法配准之后，两片点云的各个部位均得到了较好的融合，绿色的模板点云基本配准到红色的目标点云的对应位置。 同时，仍然采用均方误差（MSE）作为点云配准精度的评价指标。 表4 统计了11 个不同位姿下精配准ICP 算法和Trimmed ICP 算法的用时与误差数据。

表4 内六角螺丝工件精配准结果对比

从表4 中可以看出，相对于ICP 算法，Trimmed ICP 算法无论是配准速率还是配准精度都有了明显的改善。在11 组点云数据实验中，该算法配准时间平均降低了78.2%，MSE 降低了18.0%。由上述分析可知，SAC-IA 粗配准和Trimmed ICP精配准能够在配准结果有效的基础上，在误差允许的范围内，提高配准精度，并大幅提升配准的收敛速度。

4 结论

本文结合三维点云，完成了对长方体工件和内六角螺丝工件在不同摆放方式下的位姿估计方法研究。通过计算FPFH 特征值提取目标点云和模板点云的特征点对，完成了点云配准实验。实验中，SAC-IA 粗配准之后，在优化后的传统ICP 精配准基础上，进行了Trimmed ICP 精配准，并统计了不同工件不同摆放方式下两种配准方法的用时和误差。上述实验表明，结合SAC-IA粗配准与Trimmed ICP 精配准的点云配准方法具有相对较高的配准精度和更快的收敛速度，配准效率有了显著提升。