基于改进GICP 配准的激光-惯性SLAM 算法

徐晓苏，高佳誉，周 帅，姚逸卿

（1.微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2.东南大学 仪器科学与工程学院，南京 210096）

同步定位与建图（Simultaneous Localization and Mapping,SLAM）是机器人导航和自主控制的核心技术之一[1]，也是自动驾驶技术的关键。激光-惯性SLAM 系统结合了激光雷达和惯性传感器的优势，能够实现高精度的自主定位和地图构建[2,3]。激光点云配准通常被建模为一个非线性优化问题，配准的性能对定位精度有决定性的影响[4]。多线激光雷达每帧扫描中包含的数据点数量庞大，而点云配准的过程通常涉及最近邻搜索，计算量随扫描点数量的增长呈指数级增长，对算法的实时性带来了很大的挑战。

Shan 等[5]提出的LeGO-LOAM（Lightweight and Ground-Optimized Lidar Odometry and Mapping）通过提取线、面、地面特征，采用两步优化法实现位姿计算，达到了较好的精度和效率。周治国等[6]引入了权重函数来平衡匹配过程中线面特征的提取。Shan 等[7]改进了LeGO-LOAM，提出了一种激光雷达与惯性测量单元（Inertial Measurement Unit,IMU）紧耦合的框架，提高了精度和效率，命名为LIO-SAM（Lidar Inertial Odometry via Smoothing and Mapping）。以上基于特征的SLAM 算法能够减少参与配准的点云规模，并能够利用同类点之间的配准提高定位精度，但对特征的强依赖性使得系统容易在开放的非结构化环境中失效。因此，近年来研究人员提出了一系列基于直接配准的SLAM 系统。HDL_GRAPH_SLAM[8]是一种基于正态分布变换（Normal Distributions Transform,NDT）配准的算法[9]，该算法利用高斯分布重构点云，进行点到网格的配准，但其对网格分辨率十分敏感，难以适用多种场景。FAST-LIO2[10]和FASTER-LIO[11]直接对所有点执行点到面的配准，但统一的点面配准模型比较粗糙，且没有回环检测环节。广义迭代最近点[12]（Generalized Iterative Closest Point,GICP）配准算法不需要对点云进行特征分类，利用马氏距离整合了其他变种的ICP，在配准过程中充分考虑了点云的局部特征，构造了面-面配准模型，具有更高的配准精度，但这种算法高度依赖最近邻搜索，计算量十分庞大。因此，Koide 等[13]提出了利用CPU 多线程进行加速的Fast-GICP 和利用体素关联的方式搜索最近邻的VGICP（Voxelized GICP），VGICP 的计算效率更高，但体素化的操作同样对分辨率敏感。Chen 等[14]提出了在GICP 过程中用NanoFLANN 搜索最近邻的方法，提高了效率同时保证了精度。基于直接配准法的SLAM 算法更加鲁棒，不依赖结构化特征，环境适应性更强，但往往需要耗费更高的算力，难以达成精度和效率的平衡，在更高的效率下追求精度是近年来激光SLAM 的主要发展方向。

本文受到文献[14]的启发，对GICP 算法提出了进一步的改进，作为SLAM 的配准模型，平衡定位精度和效率。通过在背景点云中提取平面信息降低配准规模，设计了一系列剪枝模型，减少错误匹配的概率，并在迭代解算过程中加入了法向量作为目标，提高对旋转变换的约束。此外，本文针对改进后的算法设计了一个高效的子地图模型。最后，用因子图融合了各项因子进行全局优化，提高了系统定位精度。

1 算法整体框架

基于改进GICP 配准的激光-惯性SLAM 系统的整体框架如图1 所示。订阅传感器节点获取激光点云和IMU 原始数据进行点云预处理。利用IMU 数据对激光点云进行畸变校正，后文统一将畸变校正后的点云称为背景点云。接着对背景点云进行体素降采样，以获取少量待配准点云。将帧间IMU 预积分增量作为点云配准的先验初值，以背景点云为基础，提取降采样后待配准点云的局部平面信息。根据位姿邻近关系提取关键帧点云集合，构成子地图，作为目标点云进行配准。通过最小化平面间的马氏距离及法向量约束迭代求解方程获取激光里程计数据。根据激光里程计帧间变化进行关键帧选取。最后，根据关键帧组成的激光里程计因子、回环因子和IMU 预积分因子进行低频的全局优化，获取全局一致的点云地图和轨迹。

图1 算法框架Fig.1 Overall frame of the system

2 改进的GICP 配准算法

2.1 标准GICP 算法介绍

GICP 是一种广义的ICP 点云配准算法，是ICP 算法的扩展和改进。考虑每个点的局部特征，构造局部平面，用马氏距离替代欧式距离描述局部平面之间的差距，整合了点对点的ICP 和点对面的ICP 算法。

其中，Pi表示激光点的坐标。在配对完美的情况下，正确的变换矩阵T*与配对点云之间存在以下关系：

对于刚性变换T，定义变换残差di=bi-T⊙ai，则对应的服从高斯分布：

2.2 基于背景点的平面特征提取

在多线激光雷达被广泛应用的背景下，单帧激光点云的数量通常在30000 到60000 之间，密集的点云会带来爆炸性的计算量，因此，大多数SLAM 算法出于实时性考虑，会对原始点云做降采样处理。但GICP算法的配准精度随点云数量下降而下降，主要原因是，算法通过最近邻搜索收集待配准点周围的k个点（k通常取10～20）来计算协方差，而经过大量降采样后的点云难以准确描述每个点真实的局部特征。GICP 算法频繁使用昂贵的最近邻搜索，精度与效率的矛盾尤为凸显。

SLAM 中采用的GICP 算法通常在降采样后的点云中搜索最近邻，但为了保证精度，不会进行分辨率过大的降采样。针对这个问题，本文提出了基于背景点的平面特征提取方法，仅对降采样后的点做协方差计算，但在降采样前的背景点云中搜索最近邻，故计算量仍由降采样后的点云数量决定，并且保证了不破坏局部平面特征。此外，为了减少孤立点对配准结果的影响，在计算最近邻时，根据近邻点对应的近邻距离，剔除距离过于分散的采样点，不让其参与特征提取。本文用更高效的NanoFLANN 算法代替普通KDTree 进行最近邻搜索。

标准GICP 算法仅提取配准点Pi所在局部平面的协方差Ci作为特征，本文在此基础上增加法向量ni和曲率σi特征，用于提高后续匹配的精度。

设点Pi的k个邻近点Pj,j=1,2…k，点Pi协方差矩阵通过式(6)(7)计算：

其中，μi表示均值。对协方差矩阵进行奇异值分解（Singular Value Decomposition,SVD）后具有以下形式：

其中，λ1i、λ2i、λ3i表示按升序排列的特征值；Qi表示特征向量组成的矩阵 。本文用描述该点的曲率，σi越小，局部表面越平坦。对一帧示例点云进行计算后，根据经验，取0.05 作为阈值，曲率分布情况如图2 所示。

图2 曲率分布情况Fig.2 Distribution of curvature

图2 中红点表示曲率较大的点，蓝点表示曲率较小的点，从图中可以看出，利用该参数能达到与LeGOLOAM 系列特征分割类似的效果。

GICP 认为真实世界中的表面至少是分段可微的，假设点云表征的环境是局部平面，每个点可以提供一个沿法线方向的约束，因此，可以将点云的原始协方差用局部平面的形式替代。已知协方差矩阵对称，将SVD 分解后的特征值矩阵替换为代表平面的特征矩阵，则替换后的协方差矩阵具有以下形式：

其中，κ是一个小量，通常取0.001。

同时，提取最小特征值对应的特征向量作为局部平面的法向量ni构成特征用于后续配准。

2.3 错误配对剪枝

匹配的前提是将源点云通过变换矩阵投影到目标点云坐标系下，通过最近邻搜索获取与投影后的离源点最近的目标点形成配对关系{i,j}，配对点分别表示为。可想而知，错误的配对数量越多，对收敛结果的影响越大，因而本文设计了一系列剪枝方案以减少配对错误的概率，以下条件之一生效则剔除该配对关系：

（a）配对点之间的距离超过阈值εd：

这是最基本的剪枝思路，两点间欧式距离差距过大，形成配对点的概率较低，本文εd取3.0 m。

（b）配对点对应的曲率差距超过阈值εσ：

曲率差距过大意味着配对点周围的局部特征可能有较大的差异，剪枝后可以降低角点与平面点配对的概率，本文εσ取1.3。

（c）配对点法向量内积差距超过阈值εn：

法向量内积差距过大常常意味两个配对点云不在同一平面上，没有配对价值，本文εn取0.9，也就是在两点法向量夹角小于25°时才形成有效匹配。

2.4 基于马氏距离和法向量的迭代优化

与LeGO-LOAM 系列的迭代求解过程类似，每一次迭代，都将重新计算一系列配对点{Pi,Pj}∈c，c表示配对点集合。标准GICP 算法将两点之间的马氏距离作为目标函数，本文在此基础上，增加法向量作为约束量，以提高对旋转变换的约束。

定义向量η=(PT,nT)T，包含点的三维坐标和局部平面对应的法向量，定义符号⊕进行如式(13)的运算：

其中，η′ 表示变换后的向量。则六维残差向量eij可表示为：

目标函数被表示为：

最后利用Gauss-Newton 算法计算迭代增量，求解变换关系。定义六维状态向量x为：

其中，θroll、θpitch、θyaw分别表示横滚角、俯仰角、航向角；tx、ty、tz分别表示三轴的平移。

每次迭代求解线性方程：

其中，[ξ]×表示根据向量ξ得到的反对称阵。

本节针对激光点云配准性能与配准效率平衡的问题，研究并改进了GICP 配准算法。提出了用背景点云提取降采样点云的局部特征的机制，设计了剪枝模型以减少点云配对出错的概率，最后采用马氏距离和法向量同时作为约束迭代求解位姿，提高定位精度。

3 基于改进的GICP 配准的激光SLAM

本文遵循以下符号及坐标系定义：L表示激光坐标系，W表示世界坐标系，B表示为IMU 坐标系。在激光-惯性系统中，第一帧激光坐标系被定义为世界坐标系，B系与L系通过旋转平移变换重合。移动机器人的状态χ为：

3.1 IMU 预积分

在激光-惯性系统中，IMU 频率远高于激光雷达的频率，对两帧激光数据之间的IMU 进行预积分可以很好地描述移动机器人从时刻i到j的运动增量，假设两帧激光分别处于i、j时刻，期间存在多帧时间间隔为Δt的IMU 数据。本文用文献[15]给出的预积分公式计算i、j时刻间的旋转与速度和位置的增量

其中，RbW表示从B系到W系的旋转矩阵，表示重力加速度矢量。

IMU的帧间预积分增量能够为激光点云的配准提供良好的初值，使配准算法能够更快收敛。

3.2 基于子地图匹配的激光里程计

在大型场景中，当前帧到前一帧的配准容易积累误差，因此，本文应用更加全局一致的当前帧到子地图的配准来抑制误差的累积。子地图由关键帧点云组成，根据IMU 预积分得到初值，用KD-Tree 搜索距当前帧一定范围内的关键帧集合，并抽取关键帧位姿对应的点云构成子地图集合。

子地图内所有的点以W系为参考坐标系，所以要将L系下的关键帧点云PL投影至W系：

其中，PW表示世界坐标系下的关键帧点云，表示对应的旋转矩阵。

此外，根据2.2 节的讨论，每个点都需要计算三种特征参数，分别是法向量n、曲率σ、协方差矩阵C，这些参数同样参与子地图构建过程。但对数量庞大的子地图点云，逐个进行最近邻搜索计算特征显然会耗费大量算力，难以保证系统实时性，所以，本文通过旋转变换将关键帧点云的特征直接由L系投影到W系。变换公式为：

直接投影的方法使得每个输入系统的激光点只需进行一次特征计算，极大地提高了系统的运行效率。

本文采用第3 节提出的改进后GICP 算法进行点云配准，将IMU 预积分后的结果作为初值，将降采样后的去畸变点云作为待配准点云，子地图点云作为目标点云，提取平面特征，进行配准，迭代求解位姿从而获得激光里程计信息。

3.3 全局优化

全局优化问题可以被描述为一个估计最大后验概率的问题，本文用因子图建模，将关键帧所在时刻的机器人状态建模为因子图中的节点，当前帧与前一关键帧间的位姿变化超过某一阈值则被判定为关键帧，本文设定旋转阈值为0.1 rad、平移阈值为1 m。

本系统应用三种因子：激光帧间因子、预积分因子、回环因子。构成的因子图如图3 所示。

图3 因子图Fig.3 Factor graph

1）激光帧间因子

激光帧间因子L（χ）由激光里程计相邻关键帧间的变化定义：

其中，i、i+1分别表示相邻关键帧的编号。

2）IMU 预积分因子

预积分因子I（χ）由3.1 节介绍的预积分增量定义：

3）回环因子

回环因子用于消除定位的累积误差，不是本文的研究重点，采用LeGO-LOAM 系列基于欧氏距离的回环检测方案，对检测到的回环帧{i,j}进行ICP 配准，以前一帧为目标点云，后一帧为待配准点云，纠正后一帧的轨迹。本文对回环因子F（χ）定义如下：

本节阐述了改进的GICP 配准模型在激光-惯性SLAM 系统中的应用，设计了一个高效的子地图模型，能够节省系统运行时间，并介绍了IMU 预积分与全局优化模型。

4 实验结果与分析

为验证算法的精度和效率，本文进行了公共数据集实验和实测实验。实验平台为具有16 G 运行内存、AMD Ryzen 7 4800H 处理单元的个人电脑，操作系统为Ubuntu18.04，ROS 版本为melodic。通过与LIOSAM、LeGO-LOAM 算法对比，验证本文算法的优越性能。评估精度采用的指标为绝对轨迹误差（Absolute Trajectory Error,ATE），第i帧ATE 定义为：

其中，S表示估计位姿Ni到真值Mi的转移矩阵。本文用均方根误差（Root Mean Squared Error,RMSE）来统计ATE，RMSE 的定义为：

其中，trans(EATE,i)表示三轴的平移误差。

4.1 改进效果对比实验

为了验证改进GICP 配准模型的定位精度优于原始GICP 模型，采用KITTI 数据集00 序列进行实验。KITTI 数据集包含城市、乡村、高速公路等场景采集的真实数据，配置了64 线的Velodyne 激光雷达和采样频率为100 Hz 的IMU。00 是一个典型城市场景序列，包含大量直线和弯道，全长3723.737 m，改进前后的算法定位轨迹对比如图4 所示。

图4 改进前后算法定位轨迹对比Fig.4 Comparison of trajectories between the original and proposed algorithms

图4 中的original 表示改进前的算法轨迹，proposed 表示改进后的算法轨迹，与下文统一。从图中可以直观地看出，改进后的轨迹更加贴近真值。用原始GICP 模型进行配准的SLAM 轨迹均方根误差为10.971 m，改进后的算法均方根误差为3.294 m，改进后精度提升了69.98%，说明本文提出的改进模型与原始的GICP 模型相比精度更高。

4.2 大场景数据集实验

本文选取KITTI 里程计分类的00、01、02 序列验证算法在大场景环境下的定位精度。00 序列已在4.1节中介绍。01 是一个高速公路场景，平均时速超过70 km/h，以直线行驶为主，全长为2451.551 m。02 序列是里程计分类中轨迹最长的序列，全长5067.233 m。图5-7 为本文算法、LIO-SAM、LeGO-LOAM 算法与真值的轨迹对比，表1 为三种算法在三个序列中的RMSE 对比。

表1 各算法在KITTI 数据集中RMSE 对比（单位：m）Tab.1 RMSE for algorithms on the KITTI dataset (Unit: m)

图5 KITTI00 序列轨迹Fig.5 KITTI00 sequence trajectory

从表1 可以看出，本文算法在三个大型场景中的表现比LeGO-LOAM、LIO-SAM 算法更优。图5 显示在00 序列中，本文算法与LeGO-LOAM 效果相似，在一些直线行驶的路段比LeGO-LOAM更贴近轨迹真值。LIO-SAM 表现相对不佳，主要由于该序列下的IMU 数据有一定缺失与跳变，而本文算法在IMU 表现不稳定的情况下仍然能够完成全局定位，鲁棒性更好。图6 显示在01 序列中，LeGO-LOAM 算法的的轨迹误差很大，这是由于在高速场景下缺乏IMU 辅助造成的，仅靠激光雷达自身无法区分高速公路上高度同质化的特征，造成了退化。LIO-SAM 与本文算法在IMU 的辅助下表现更佳，且本文算法能更加充分地表征环境，因此全程的轨迹都比LIO-SAM 更贴合真值。在图7 的长距离序列02 中，本文算法与其他两种算法相比也有更高的精度。在三个序列中，本文算法的平均轨迹均方根误差相比LIO-SAM 算法降低62.42%，相比LeGO-LOAM 算法降低了88.44%。

图6 KITTI01 序列轨迹Fig.6 KITTI01 sequence trajectory

图7 KITTI02 序列轨迹Fig.7 KITTI02 sequence trajectory

4.3 实测实验

本文在校园中进行了实测实验，测试了算法在64线Ouster 激光雷达和ZED2 相机中400 Hz 的IMU 配置下的性能表现，以中海达iRTK20 定位结果为真值，以松灵公司生产的bunker 底盘为载体，搭建了如图8所示的实验平台，底盘的最大速度为1.5 m/s。

图8 实验平台Fig.8 Experiment platform

实验场景包括操场、停车场、校园直道，与KITTI数据集相比，校园环境相对低速，操场场景比较空旷、停车场路线迂回曲折，校园直道场景几何特征丰富。多种算法的轨迹对比结果如图9-11 所示，精度对比如表2 所示。

表2 各算法在实测数据中RMSE 对比（单位：m）Tab.2 RMSE for algorithms in real test (Unit: m)

图9 操场轨迹Fig.9 The trajectory of the playground

图10 停车场轨迹Fig.10 The trajectory of the parking lot

图11 校园直道轨迹Fig.11 The trajectory of the straight road

从图9-11 可以看出，三种算法的轨迹在x、y轴上与真值比较接近，在z轴上有一定的区分，这与z轴尺度与x、y轴相比较小有关。在操场序列中，LIO-SAM算法的高程偏差很大，在平坦的操场地面上行驶时，最大与最小高程差距达到1 m 以上。LeGO-LOAM 由于分割地面点云并单独配准的机制，在平地上表现良好。本文的算法虽然没有单独对地面进行分割，但利用背景点云提取局部平面特征的机制能更好地表征全局地面，且基于法向量夹角的误匹配剪枝能够剔除当前帧与局部子地图对应点法向量差距过大的配对，比普通的点面配准更加鲁棒，所以在z轴上表现良好。停车场序列场景紧凑，弯道较多且转弯半径较小，LeGO-LOAM 由于缺乏IMU 辅助，定位效果不佳，LIO-SAM 算法与本文算法精度相似。校园直道序列全程无回环，主要考验算法的里程计性能，相比之下，本文算法的精度优于其他两种算法。从表2 可以看出，在三个场景中，本文算法的平均轨迹均方根误差比LIO-SAM 算法降低24.77%，比LeGO-LOAM 算法降低了21.02%。

4.4 效率对比实验

为了验证本文算法对降采样分辨率的包容度，对LIO-SAM 和本文算法在不同分辨率下的单帧处理时间和对应的精度进行了对比实验。以校园直道数据为例，对比了两种算法配准加优化的单帧处理时间，如图12 所示。事实上，本文对LIO-SAM 的时间统计并没有包含特征提取过程，即使在这样的情况下，本文算法在不同分辨率下仍然比LIO-SAM 节约了大约一半的时间，平均效率提高48.12%。

图12 单帧处理时间对比Fig.12 Comparison of single-frame processing time

在不同降采样分辨率下，两种算法RMSE 的对比如表3 所示。

表3 各算法在不同降采样分辨率下RMSE对比（单位：m）Tab.3 RMSE for algorithms at different downsample resolutions (Unit: m)

从表3 可以看出，本文算法相对于LIO-SAM，对降采样分辨率的包容度更大，可以在更高的效率下达到更高的精度。

5 结论

本文提出了一种基于改进GICP 配准的激光-惯性SLAM 算法。该算法对标准GICP 进行改进，利用背景点云提取降采样点云的局部特征，经过一系列剪枝后，使用马氏距离和法向量作为约束迭代求解位姿。同时，专门针对改进的GICP 模型提出了一种高效的子地图构建方法。最后在KITTI 数据集上的多个序列和实际校园场景中进行实验验证，实验结果表明：相比于LIO-SAM 和LeGO-LOAM，本文所提算法具有更好的精度、鲁棒性和效率。