基于ICP和NDT的激光点云匹配方法研究

朱 锋，吴长水，茅 健

（上海工程技术大学 机械与汽车工程学院，上海 201620）

0 引 言

自动驾驶技术是当前的研究热点，车辆定位技术是自动驾驶不可或缺的部分，良好的定位可以帮助车辆实现自动驾驶功能。同时定位与地图构建（SLAM）技术是车辆定位的关键技术之一，指的是搭载特定传感器的机器人或者车辆，在没有先验信息的环境中，通过自身运动过程建立环境地图，并估计自身的运动。根据传感器的不同，SLAM技术又可分为视觉SLAM和激光SLAM两大类。近年来，这2种SLAM技术取得了长足的进步，并逐渐在产品应用上落地。尤其是在自动驾驶领域，激光雷达的应用逐渐普及起来，三维激光SLAM技术将起到举足轻重的作用。

激光SLAM框架可以大致分为5个模块：传感器数据读取、前端里程计、后端优化、回环检测、建图。其中，前端里程计是激光SLAM的重要步骤，其功能是将激光雷达采集到的点云数据做相邻帧之间的匹配，从而得出相邻帧间位姿关系，再根据上一帧的位姿，来估计出当前帧的位姿。三维激光前端里程计帧间匹配方法主要分为直接匹配和和特征匹配两类。直接匹配法又可细分为迭代最近点ICP和正态分布变换NDT。

Chen等人提出的ICP算法，通过待匹配的2帧点云建立欧氏距离并使其最小化，不断迭代直至满足设定的终止条件，从而得到相对位姿变化。但激光点云数据往往较大，导致ICP算法耗时长；而且ICP算法需要提供一个较好的初值，否则最终迭代结果可能会陷入局部最优。Censi提出了PL-ICP算法（point-to-line ICP），该法精度较高，适用于2D激光SLAM，缺点是对初值更敏感，容易陷入局部极值。Low提出了PP-ICP算法（point-toplane ICP），该算法精度高且适用于3D激光SLAM。

Biber等人提出一种二维NDT的匹配方法，Magnusson等人在此基础上将二维NDT推广到三维匹配中，其核心思想是将点云数据投放到由小立方体组成的网格中，每个立方体中的点云转换成一个概率密度函数，然后求出点云之间的匹配关系。三维NDT算法初始化效果好，运行速度快，鲁棒性高，在3D激光SLAM使用较多。

Zhang等人提出的特征匹配算法，其思想是先计算曲率，并按曲率提取线特征和面特征，再和上一帧做匹配，最后求出相对位姿。LOAM方案在纯激光匹配算法中很具有代表性，长期霸榜于KITTI数据集前三位。LeGO-LOAM在LOAM的基础上对地面特征做了分割，减小了特征搜索范围，从而节省了计算资源；并加入了回环修正，精度也有所提升。LIO-mapping提出了一种LiDAR与IMU紧耦合融合方法，在LOAM的基础上加入IMU，基于滑动窗口方法，把雷达线／面特征、IMU预积分等的约束放在一起进行优化，得到良好的建图效果。LIO-SAM在LeGO-LOAM的基础上，先通过点云特征计算出相对位姿，再利用相对位姿、IMU预积分和GPS做融合，分2步完成，相比于直接一步做紧耦合，大大提高了效率。上述列举的方案中，前端激光里程计模块都是基于特征匹配得到的。

本文的研究对象是三维激光SLAM，针对前端激光里程计，用2种不同的点云匹配方法：迭代最近点ICP和正态分布变换NDT，在KITTI数据集上分别进行点云配准，同时也评估了这2种点云匹配方法的效率和精度。

1 前端激光里程计匹配方法

1.1 迭代最近点

迭代最近点（ICP）算法的核心思想，即通过待匹配的2帧点云建立欧氏距离并使其最小化，不断迭代直至满足设定的终止条件，从而得到相对位姿变化。图1为拟合示意图。

图1 拟合示意图Fig.1 Fitting schematic diagram

然后构建最小二乘问题，求，使得误差平方和达到最小：

观察上式等号右侧2项，第一项只和有关，第二项和，都有关。只要求得旋转矩阵，再令第二项为零即可求得平移。由2组点集的质心，，令：

计算有关的误差项，得：

对3×3矩阵进行SVD分解（奇异值分解），得：

当满秩时，解得：

1.2 正态分布变换

正态分布变换（NDT）核心思想是将点云数据投放到由小立方体组成的网格中，每个立方体中的点云转换成一个概率密度函数，然后通过数学手段求出点云之间的匹配关系。图2为通过概率刻画点云，左边为原始点云，右边为每个方块内点的概率。

图2 概率刻画点云Fig.2 Point cloud description with probability

根据预测的位姿，对点进行旋转和平移：

旋转和平移后的点与目标点集中的点在同一坐标系下，此时可计算各点的联合概率：

所有点的联合概率：

取对数，去除常数项，简化问题，则目标函数变为：

按照高斯牛顿法的求解流程，计算残差函数关于代求参数的雅可比，便可迭代优化。

2 基于数据集的实现

本文是基于KITTI数据集实现的，使用的数据包为kitti_2011_10_03_drive_0027_synced.bag，对其中的激光雷达点云数据，分别添加了前端里程计的算法ICP和NDT，算法流程见图3和图4，测试算法并得到了里程计的轨迹，最后和gnss轨迹做数据对比分析。

图3 ICP算法流程Fig.3 The flowchart of ICP algorithm

图4 NDT算法流程图Fig.4 The flowchart of NDT algorithm

里程计子模块功能设计如下：

（1）点云匹配。用pcl库设置匹配参数，从接收到一帧点云开始，和地图进行匹配，如果是第一帧数据，那么就认为是地图，供下一帧匹配使用。本文要把里程计轨迹和gnss轨迹做对比，所以把初始时刻gnss和imu给出的位姿作为里程计的初始位姿。

（2）提取关键帧。如果把每一帧匹配好的点云都加入地图，会使地图太大，所以要提取关键帧，即每隔一段距离取一帧点云，用关键帧来拼接成地图。

（3）滑窗。关键帧会不断增加，地图会一直累加，那么会导致很多不必要的计算量，所以把时间靠前的关键帧给剔除，把和当前帧一定距离范围内的关键帧找出来拼接即可。

（4）点云滤波。在匹配之前需要滤波，对点云稀疏化。这里点云滤波是直接采用了pcl库中的voxel_filter，其基本原理就是把三维空间划分成等尺寸的立方体格子，在一个立方体格子内最多只留一个点。滤波格子大小决定了匹配的效率和精度，格子越小，点越多，精度越高，但是速度越慢，反之速度加快，精度下降；可适当调整参数，对比效果。

（5）位姿预测。点云匹配有一个特性，对位姿的预测值比较敏感，所以在载体运动时，不能以其上一帧的位姿作为这一帧的预测值，可以使用IMU预测，也可以使用运动模型预测，此处采用运动模型来做位姿预测。假如当前帧是第帧，那么用第2帧位姿和第1帧位姿就可以计算一个位姿变化量，在1帧位姿基础上累加这个位姿变化量，就可认为是第帧的预测值。

3 实验结果分析

本文中用到的计算机配置：Intel i7-9750H处理器，16 G内存，系统环境为Ubuntu18.04和ROS Melodic1.14。使用精度评估工具evo，把对应的gnss数据和激光里程计数据进行分析处理。采用的评价指标如下：

（1）绝对位姿误差（absolute pose error，）。用于比较估计轨迹和参考轨迹并计算整个轨迹的统计数据，此处估计轨迹为激光里程计，参考轨迹为gnss。

（2）相对位姿误差（relative pose error，）。相对位姿误差比较运动过程中的姿态增量，可以给出局部精度，如slam系统每米的平移或者旋转漂移量。

将ICP算法和NDT算法分别在KITTI数据集上进行仿真实验，得出了的效果如图5所示。图5（a）、图5（b）中的蓝色轨迹是gnss轨迹，绿色轨迹为激光里程计。对比2种算法，可以发现激光里程计均出现了一定程度上的漂移。具体来说，图5（a）中的漂移比较严重，图5（b）中的漂移比较轻。下面将给出数据做定量分析。

图5 KITTI测试效果图Fig.5 KITTI test effect diagram

图6和图7分别展示了ICP和NDT的位姿误差分析图，将图中数据整理到表1和表2中。在表1、表2中，max表示最大误差，表示平均误差，表示误差中位数，min表示最小误差，表示均方根误差，表示标准差。

图6 ICP方法的绝对位姿误差和相对位姿误差Fig.6 APE and RPE of ICP

图7 NDT方法的绝对位姿误差和相对位姿误差Fig.7 APE and RPE of NDT

表1 绝对位姿误差对比Tab.1 The comparison of APE

表2 相对位姿误差对比Tab.2 The comparison of RPE

在绝对位姿误差中，ICP比NDT的最大误差和均方根误差都要大很多；在相对位姿误差对比中，ICP的误差比NDT的误差也大很多。根据图5（a）和图5（b），在运动开始时没有旋转，对于平移，ICP和NDT精度几乎相当；在第三个转弯处，ICP出现了较大的漂移误差；在第七个转弯处，ICP累积漂移更大，此处由于没有较好的初值，最终迭代结果陷入了局部最优，因而轨迹出现了失真。由上可以得出：在角度变化时，NDT的鲁棒性明显优于ICP；且ICP需要提供一个较好的初值，否则最终迭代结果可能会陷入局部极值。

4 结束语

本文利用三维激光点云中2种匹配算法：迭代最近点（ICP）和正态分布变换（NDT）算法，实现了前端激光里程计的功能。为了比较ICP和NDT算法的效率和精度，在KITTI数据集上进行了仿真实验。实验中，用gnss轨迹作为真值，激光里程计作为估计值，给出了2种算法的绝对位姿误差和相对位姿误差，结果表明NDT算法具有更好的效率和精度，具体表现为如下2点：

（1）对于前端激光里程计，NDT的匹配精度优于ICP。

（2）由于ICP需要较好的初值且易受旋转影响，ICP更适用于精匹配，NDT则更适用于粗匹配。

本文实现了基本的激光里程计功能，但是对里程计来讲，漂移是不可避免的。后续的工作，是增加其他约束来消除累计误差，从而构建高精地图。