基于IMU点云特征跟踪一致性的ICP配准技术

邢 静，闫克丁，高俊钗

(1.西安培华学院智能科学与信息工程学院，陕西 西安 710125；2.西安工业大学电子信息工程学院，陕西 西安710021)

0 引言

对于采用飞行时间(time of flight,TOF)相机测绘的机器人，为了得到全场景的三维地图，将机器人在不同位姿采集的点云进行配准确定点云之间的位姿关系，然后通过位姿变换关系，将点云转换到统一坐标系中，实现点云的拼接。当点云非重叠区域较大时，由于场景中存在相似区域和平滑区域，点云之间的配准存在局部极值情况，同时场景的数据量很大，使得配准的复杂度较高，所以需要研究一种准确、快速的点云配准技术。

目前，点云配准的方法有很多，根据原理不同，主要分为2类。一种是基于关键点特征的配准，以采样一致性初始配准算法(sample consensus initial alignment,SAC-IA)[1]和三维形状上下文算法 (3D shape contexts，3DSC)[2]为代表。这2种算法存在关键点提取、特征表示的过程，计算准确度都比较高，但复杂度也都比较高。另一种是基于模型优化的配准，以迭代最近点算法(iterative closest point,ICP)[3]和正态分布变换算法 (normal distributions transform，NDT)[4]为代表。这2种算法直接对点云进行处理，计算复杂度都比较低。但由于是基于随机点对匹配的计算，所以容易陷入局部最优而不是全局最优。如果有一个较好的初始值，就会在一定程度上解决这个问题，所以需要对点云进行初始的粗配准，为模型优化配准提供一个较好的迭代初始值。

针对测绘机器人点云配准过程中 ICP 配准技术对初始位置敏感的问题，宗晓萍等[5]提出一种基于FPFH[6]特征的ICP点云配准改进算法，但该算法存在参数需要经验来进行调整的问题，而且对于相似的点云特征，容易出现误匹配；石峰源等[7]提出一种基于PCA的点云配准策略，但该算法对存在较大边界非重叠区域时点云的PCA主成分特征向量会发生改变，较大的不一致性使得算法会失效；荆路等[8]提出了一种基于尺度不变特征变换(scale-invariant feature transform,SIFT)[9]特征点结合ICP的点云配准方法，但该算法特征点提取与特征匹配严重影响了点云配准的效率。

为在有较大非重叠区域的情况下保持ICP配准算法的准确性，并加快配准的速度，本文结合惯导(inertial measurement unit,IMU)和关键点特征，设计了一种三维点云跟踪配准的变换矩阵计算方法，快速准确估计三维点云配准初始变换矩阵；基于稳定的关键点跟踪匹配及矢量一致性判断去除误匹配进行ICP迭代，进而高精度地动态反向解算机器人的位置和姿态，实现点云配准拼接。

1 点云配准算法设计

本文提出的基于IMU点云跟踪一致性的ICP配准方法如图1所示。首先，采用NARF[10]检测当前待配准的第k帧点云的关键点，以有效减少点云匹配数量，并增强点云的稳定性和可区别性；然后，按照IMU确定的2幅点云变换姿态对待配准的第k帧关键点进行变换，与第k-1帧关键点一起进行相同的分区网格化，在第k帧分区网格按照蒙特卡罗方法选取不超过20个关键点，计算特征作为匹配样本，在关键点邻域3σ范围内采用KDTree搜索算法在第k-1帧匹配最近点，并采用匹配点之间矢量的一致性对点云关键点匹配进行验证，剔除误匹配，利用SVD算法计算匹配点之间的初始变换矩阵；最后，以初始变换矩阵作为迭代初始值，对所有点云关键点进行跟踪匹配，及矢量一致性剔除误匹配的ICP配准迭代优化，判断是否满足终止条件，完成点云的精配准。

图1 IMU关键点特征跟踪一致性的ICP配准技术

2 基于IMU的NARF关键点特征跟踪一致性初始匹配

由于点云数据量较大，而且存在平滑的区域，点云跟踪会存在误跟踪匹配，所以检测3D点云上具有稳定性、区别性的点集：关键点。同时为了关键点旋转、平移表示的唯一性，便于跟踪，对关键点进行特征提取表示。另外，为了加速关键点匹配，采用IMU进行跟踪匹配。

2.1 基于NARF的关键点检测

关键点检测的方法有Harris3D、SIFT3D和NARF (normal aligned radial feature)[10]等，由于NARF关键点检测方法可以适用于深度图像，而本文TOF相机直接获取的是深度图像，所以本文采用NARF算法进行关键点提取。由于边缘上的点具备可区别性和显著性的特点，是潜在的关键点，所以先进行边缘检测。另外，由于要进行后续关键点的识别追踪，所以在不同视角的关键点需要具备唯一性，但很多边缘点在邻域内不具备唯一性，所以将方向和曲率显著性的边缘点作为关键点。

a.深度图像边缘检测。在深度图像中，遍历每个深度图像点，通过在5×5近邻区域寻找有深度变化的位置进行边缘检测。将pi邻域内的点按照与pi的距离进行排序，d0为与pi距离最近的点的距离，d24为与pi最远的点距离。计算pi右边的点平均距离dm为

(1)

对pi邻域内距离上非负的凸点按照式(2)进行边缘点评判。

(2)

取si,j>0.8作为边缘候选点，并进行非极大值抑制，就可以得到物体的边缘。

b.确定关键点。遍历深度图每个边缘点pi，计算边缘点及邻域点方向与权重平面。将边缘点pi邻域内的点，进行PCA主成分分析，可以得到1个主方向v作为边缘点方向，以及曲率值λ作为边缘点权重w。取api为垂直于pi与原点连线的平面，则邻域点方向为主方向v在平面ap上的投影，权重w为1-(1-(1-λ))3。

根据主方向v，计算第k帧边缘点pi的显著性R1i,k，以及该边缘点处表面的变化情况R2i,kl，按照式(3)给出该边缘点为关键点的可能性Ii,k。

Ii,k=R1i,kR2i,kl

(3)

对可能性值Ii,k进行平滑滤波，并进行非最大值抑制找到的显著性点，即为NARF关键点。为了提高计算效率，限制关键点数量不超过100个。

2.2 点云关键点的FPFH特征提取

点云关键点特征主要有局部特征和全局特征，局部特征有法线等几何形状特征的描述。全局特征有视点特征直方图(VFH)和形状描述子3DSC。全局特征对于背景、噪声点过多的情况，匹配效果很不好，而且计算复杂度较高；局部特征的计算复杂度相对较低，关键点的FPFH特征提取方法如下：

a.关键点特征坐标系建立与SPFH计算。对于已知关键点pi，利用pi和它近邻关键点pm之间的对应描述其邻域特征。为了计算关键点pi和邻域关键点pm对应的法线Ni和Nm以及它们之间的相对偏差，在关键点上定义1个固定的局部坐标系，如图2所示。

图2 关键点局部坐标系

在局部坐标系中，使用图2中uvw坐标系，对于每一个关键点pi，计算这个关键点pi和它的邻域关键点pm之间的1个元组：3个特征值f1、f2和f3，即SPFH。关键点SPFH特征值的计算式为

(4)

将3个特征值中的每个特征值按照数值分为11个区间，每点对之间只取1个特征值，这样得到有33个区间的直方图，统计每个区间中点云的个数，得到点云关键点的SPFH直方图。

b.关键点FPFH特征计算。确定每个邻域关键点的m邻域，使用邻域点的SPFH值来计算关键点pi的最终FPFH直方图，即

(5)

权重wi为中心点pi和其邻域点pm之间的距离。计算关键点的邻近点SPFH值，从而得到关键点的最终FPFH值。

2.3 基于IMU关键点特征跟踪的加速初始配准

ICP迭代优化算法易陷入局部极小值，所以在迭代之前，初始配准非常重要。在有遮挡和噪声干扰的情况下，三维点云初始配准方法主要有SAC-IA方法和4PCS方法等，但这些方法搜索匹配点时范围较大，使得计算复杂度都比较高，效率较低。所以本文结合IMU测量对关键点特征进行跟踪，加速初始配准。

假设第k帧和第k-1帧关键点云数据集分别为P={pi∈R3,i=1,…,Np}和Q={qi∈R3,

i=1,…,Nq}，惯导在2帧之间确定的初始相对位姿参数为R和T。则配准初始值估计算法的主要流程如下：

a.第k帧的P关键点集经过IMU测量的相对位姿变换得到点集P′，变换到第k-1帧的坐标系中。

b.对P′中关键点集深度值按照方位进行网格化，分割为4×4的16个网格。

c.按照蒙特卡洛方法在P′中任取10个网格中不在同一条直线上的20个关键点。对于选取的20个关键点，计算特征作为匹配样本，在其位置邻域1σ～3σ范围内，采用KD树搜索关键点集Q中的近邻匹配点。

d.对近邻匹配的关键点，按照关键点位移变化方向及幅值的一致性进行匹配验证，剔除误匹配点。

e.如果匹配点小于3对或在同一条直线上，转到c继续选取关键点；如果找到3对及其以上不在同一条直线上的匹配点，则转到f。

f.任取3对不在同一条直线上的匹配点Pii和qii，将对应点之间的欧氏距离作为点对之间的权重值，表示为ωii，采用SVD矩阵分解的方法，得到1个最优的变换矩阵，使得如式(6)中目标函数取得最小值。

(6)

W为所有对应点之间距离的总和，即

(7)

3 基于关键点跟踪一致性的ICP精准配准优化算法

点云数据量较大，而且噪声干扰较大，不但加大ICP迭代优化的计算复杂度，而且降低其配准准确度，针对这个问题，采用关键点跟踪的ICP优化求解变换矩阵。采用关键点跟踪矢量的一致性剔除误匹配，则ICP精配准优化估计算法的主要流程如下：

a.将计算出来的旋转变换矩阵R′和T′作为ICP迭代的初始值，将第k帧的关键点云数据集P变换到第k-1帧的坐标系中为Pnn。

b.在点云数据Pnn的关键点位置邻域1σ～3σ范围内，根据深度值欧氏距离最小，采用KD树跟踪搜索关键点云数据Q中的匹配点。

c.对匹配关键点利用位移变化方向和幅值的一致性进行匹配验证，剔除误匹配点。

d.计算匹配点的误差，如果小于误差阈值，则输出当前的位姿，结束；如果大于误差阈值，则转向e。

e.采用SVD矩阵分解的方法，利用匹配关键点计算P和Q的位姿关系R″和T″，转向a。

重复迭代上述过程，直到目标函数的取值达到最小阈值或是达到迭代上限时，停止迭代。

4 实验验证

4.1 配准精度评价

配准精度采用2个参数来评价：误差统计参数和均方根误差参数。误差统计参数的表达式为：

(8)

(9)

H(pii,qii)为经过ICP精准配准后，匹配点对(pii,qii)之间的距离；τ为设定的精度阈值；N为匹配点对的个数；S(pii,qii)为当前点对满足配准精度的要求；μ为配准点对中没有达到配准精度要求的数据点所占比率。

均方根误差参数ξ，用点对之间距离的均方根误差来表示，均方根误差可以很好地体现出匹配点对之间距离的整体情况，其表达式为

(10)

4.2 仿真配准验证

红颜色是目标点云，绿颜色是待配准点云，将本文设计的点云配准算法和ICP算法分别用于图3中的绿颜色的Bunny兔，蓝颜色是配准后的点云，ICP算法用时为2.1 s左右，本文设计算法用时1.5 s，提高了配准的效率。在配准效果上，本文算法与ICP算法的对比如图3所示。

图3 本文算法与ICP算法结果对比

从图3中可以看出，本文算法与ICP算法相比，配准效果明显有所提高，2个不同位姿下的Bunny兔基本上完全重合在了一起。

4.3 数据集验证

基于NYUv2数据集构建的三维彩色点云局部地图如图4所示。

从图4中可以看出，图4a和图4b存在相同的区域，即书架部分。提取只有空间信息的三维点云图如图5a和图5b所示。

图5a场景中，点云中点的个数为30多万，图5b场景中，点云个数为40多万。由于点云数量巨大，将每个点的空间位置特性都考虑到是十分耗时的，会造成拼接效率低的问题。

图4 三维彩色点云图

图5 经过点云类型转换后的点云图

对于基于NYUv2数据集上建立的2个局点云地图，给定1个接近变换矩阵的值作为惯导的测量值，结合惯导的关键点跟踪后估计得到的初始变换矩阵为R′T′，即

(11)

将这个变换矩阵作为ICP精准配准的初始值，通过关键点跟踪一致性配准，得到的变换矩阵R″T″为

(12)

将变换矩阵作用于数据集P，与数据集Q最终拼接后的点云地图及其彩色三维地图如图6所示。

图6 三维点云拼接结果

从拼接结果来看，重叠的书架区域拼接效果良好，整体场景比较自然。

本文设计的算法在NYUv2数据集上求得配准点对中没有达到配准精度要求的数据点所占比率μ=0.058。经过计算求得均方根误差ξ=3.4×10-3m。综合未达到配准精度要求所占的数据点占比以及匹配点对的均方根误差来看，本文设计的算法是有效且精准的。

5 结束语

本文设计的基于IMU点云特征跟踪一致性的ICP配准技术，采用初始配准与精配准相结合的方法，既保证了配准的效率，又保证了配准的精度。结合IMU惯导测量数据的关键点跟踪，限定了匹配的区域，避免了贪婪的穷尽搜索匹配复杂度高的问题，有效地提高了匹配的正确率和效率。基于跟踪矢量的一致性剔除误匹配，增强了配准矩阵计算的鲁棒性，提高了配准矩阵计算的精度。通过实验测试，选取了经典ICP算法与本文方法进行对比分析。由实验可知，本文算法配准效果较好，不仅有效地避免ICP算法陷入局部最小，同时获得了较优的配准效率与精度，综合提升了点云的配准性能。