激光雷达和IMU紧耦合SLAM算法

吴明月

激光雷达和IMU紧耦合SLAM算法

吴明月

（天津职业技术师范大学 汽车与交通学院，天津 300222）

近年来，无人驾驶领域成为广泛关注的热点方向，同时定位与地图构建（SLAM）技术是高精地图创建和无人车辆导航的基础，当下主流的激光SLAM算法基本能够满足应用的需求，但是在大范围场景建图的过程中仍然存在漂移的问题，且算法轻量化以及实时性方面依旧有着改进的空间。文章主要进行了激光雷达和惯性测量单元（IMU）紧耦合的同时定位与建图算法研究，前端部分主要对激光点云数据进行了去除畸变、特征提取，后端使用因子图融合IMU预积分因子、激光里程计因子和回环检测因子进行融合位姿输出。为了提高算法的实时性，文章使用iKD-Tree数据结构维护了一个局部地图，并使用Fast-GICP算法求解回环检测位姿变换。在Kitti公开数据集的测试表明，改算法在保证精度的同时提高了算法的实时性和鲁棒性。

激光雷达；因子图优化；IMU；紧耦合；SLAM算法

同时定位与地图构建（Simultaneous Localiza- tion And Mapping, SLAM）技术是无人车辆实现自主导航的基本前提，主要分为视觉SLAM和激光SLAM。相较于视觉SLAM，激光SLAM不受照明变化的影响，更适合直接捕获3D空间中环境的细节。但在大场景建图中仅使用激光雷达会导致较大的漂移，于是添加更多的传感器进行优化成为广泛的共识[1-2]。在过去二十年中，人们提出了许多基于激光雷达的SLAM方法。由ZHANG等[3]在2014年提出的实时状态估计和建图的激光里程计和建图（Lidar Odometry And Mapping, LOAM）方法是应用较广泛的方法之一。LOAM使用激光雷达和惯性测量单元（Inertial Measurement Unit, IMU），在Kitti公开数据集测试榜上一直被列为顶级的基于激光雷达识别方法。但LOAM仍存在一些局限性，因其核心是基于扫描匹配的方法，没有回环检测，在特征丰富的环境或者大范围建图中性能有所下降。2018年SHAN等[4]提出了一种可以部署到嵌入式平台，且面向复杂情况的轻量级优化地面雷达里程计和建图（Lightweight and Groud Optimized-Lidar Odometry And Mapping, Le GO-LOAM）算法，其添加了地面约束，且通过对地面点云的分割降低了运算量。但它是一种松耦合的惯性激光里程计，在大环境中其建图精度并不理想。2019年YE等[5]提出了一种激光-惯性里程计和建图（Laser Inertial Odoemtry and Mapping, LIOM）方法，在建图精度上相较于松耦合方法有明显提升。由于该算法基于滑动窗口进行优化，随着窗口参数的增加，影响了算法的实时性且误差会逐渐累积。2020年SHAN等[6]提出了基于佐治亚理工学院平滑与地图（Georgia Tech Smooth- ing And Mapping, GTSAM）优化库的惯导和激光雷达紧耦合激光雷达惯性里程计（Laser Inertial Odoemtry-Smoothing And Mapping, LIO-SAM）。通过定义观测量作为因子图因子构建优化模型，并采取了关键帧到局部地图匹配的方式，且支持添加全球定位系统Global Positioning SystemGPS数据作为绝对观测，因此，该算法建图精度得到了较大提升。但是随着图优化因子的增加会产生算法实时性降低的问题。基于此，本文算法使用了一种新的数据结构iKD-Tree进行局部地图的管理，同时使用Fast-GICP（Generalized Iterative Closest Point）算法对回环检测进行了优化。

1 算法框架

本文所研究的算法框架如图1所示。本文算法采用了激光里程计同IMU里程计紧耦合的方式，在数据预处理阶段，IMU数据参与点云去畸变。在激光里程计部分，使用特征点云进行匹配构建优化问题，IMU预积分通过与点云帧最优位姿融合作为迭代优化的初值求解位姿变换。后端基于因子图优化方法并采用了回环检测优化全局位姿，在全局最优位姿的基础上，融合IMU里程计预测位姿，最后实现以IMU频率输出车辆位姿信息。

图1 算法框架

2 关键技术

2.1 点云预处理

2.1.1 点云去畸变

由于传统机械式激光雷达的一帧数据是其旋转一周内形成的所有数据，在运动场景下，采集的原始激光点云数据存在着畸变，不能真实反映环境信息，因此，需要进行点云去畸变。

因为IMU可以高频率输出位姿信息，所以采用IMU提供的点云起始时刻到结束时刻的位姿信息对该帧点云进行运动补偿。

2.1.2 特征提取

为提高计算效率，需要进行激光点云的特征提取。本文沿用了LOAM[7]方法，通过计算曲率和距离变化得到角点和面点。

2.2 IMU预积分

无人车辆搭载的六轴IMU可以通过加速度计和陀螺仪获得原始加速度测量值a和角速度测量值w，但这些值中包含了大量的噪声，考虑IMU的零偏不稳定性噪声和测量噪声之后，定义IMU的状态为

2.3 激光里程计

2.3.1 初值计算

作为基于优化方案的点云匹配，初始值是非常重要的，一个好的初始值会帮助优化问题快速收敛，且避免局部最优解的情况。在系统的初始化阶段，使用磁力计提供的位姿信息作为优化的先验，在接收到IMU预积分提供的位姿增量后，加到上一帧激光里程计最佳位姿上去，作为当前帧迭代优化的先验位姿估计。

2.3.2 基于iKD-Tree的局部地图

以关键帧位置形成的点云建立iKD-Tree，根据最后一个关键帧的位姿，进行最近邻搜索，提取半径50 m内的关键帧。

根据查询的结果，把这些点的位置存进一个点云结构中，最近10 s的关键帧也保存下来。为避免关键帧过多，做一个下采样，确认每个下采样后的点索引，根据筛选出来的关键帧进行局部地图构建。

本模块采用了增量式iKD-Tree进行局部地图的创建和管理。除了有效的最近邻搜索外，新的数据结构还支持增量地图更新，以最小的计算成本进行动态重新平衡和对iKD-Tree进行降采样操作。iKD-Tree通过维护范围信息和惰性标签进行删除操作，采取并行重建的方式降低延迟，保证了主线程的实时性和准确性，相较于KD-Tree更为高效。由于延迟降低，该方法支持直接将关键帧对应的点云加入点云地图，避免了点云转换的操作，节约时间的同时，提高了复杂环境中扫描配准的鲁棒性。

2.3.3 点云配准

使用当前帧与构建的局部地图进行点云配准，遍历当前帧并取出一个角点，将该点从当前帧通过初始的位姿变换到地图坐标系。在角点局部地图里面寻找距离当前点比较近的5个点，计算找到的5个点的均值并构建协方差矩阵，进行特征值分解验证这5个点是否是一条直线。然后根据点的均值往两边拓展构成这条线的两个端点，最大特征值对应的特征向量对应的就是直线的方向向量。求整个点到构建的两个点形成直线的距离和方向，即残差与残差方向，计算残差关于待求变量的雅可比矩阵，使用高斯牛顿下降法进行迭代优化，得到帧间位姿变换。点面约束基本同理。

2.4 基于Fast-GICP的回环检测

回环检测通过周期性匹配当前帧和历史帧的特征点云，来确认无人车辆是否经过已经走过的位置，从而得到回环相对位姿关系进行全局位姿优化。根据当前关键帧与历史帧间的欧式距离进行检索，即将关键帧列表保存于iKD-Tree中，以半径搜索当前关键帧的相邻帧，并根据采样时间判断是否为相邻时刻的关键帧。形成回环后，使用历史帧周围25帧点云构建局部地图，与当前关键帧进行Fast-GICP算法匹配求解位姿变换。

Fast-GICP是一种体素化的广义迭代最近点算法，用于快速、准确地进行三维点云配准。该方法扩展了GICP方法的体素化，避免了代价昂贵的最近邻搜索，同时保持了回环检测算法的精度。因其通过聚集体素中每个点的分布来估计体素分布，使回环检测算法的实时性得到提升。

2.5 基于因子图的全局位姿优化

本算法基于因子图优化进行融合位姿输出。首先将激光里程计因子和回环检测因子加入融合框架当中得到点云帧间最佳位姿，再将优化后的位姿信息结合预积分因子纠正IMU零偏，最后使用IMU预积分得到的位姿同点云帧间最佳位姿融合得到IMU频率的位姿估计。因子图优化模型位姿融合输出如图2所示。

图2 位姿融合输出

3 实验结果与分析

Kitti数据集测评如下：

3.1 实验环境搭建

硬件配置为CPU-Interi5/16 G、运行内存为250 G SSD的笔记本电脑；操作系统为 Linux ubuntu 20.04；对应的机器人操作系统（Robot Operating System, ROS）为Noetic；测试数据集选用Kitti数据集。

3.2 iKD-Tree实时性评定

在实时性方面，iKD-Tree较KD-Tree降低了26%的耗时，如表1所示。

表1 iKD-Tree 与KD-Tree耗时对比

3.3 回环检测配准算法评定

Fast-GICP算法较ICP（Iterative Closest Point）算法匹配得分更高，这意味着回环检测位姿变换求解的鲁棒性更好，且计算速度比ICP算法快了一个数量级，如表2所示。

整体来看，本文算法提高了算法的实时性和鲁棒性。

表2 Fast-GICP与ICP对比

4 结论

针对单一激光传感器SLAM算法在大范围建图过程中存在的轨迹漂移的问题，本文基于因子图优化的激光雷达和IMU紧耦合的SLAM算法进行研究，在LIO-SAM算法的基础上使用iKD-Tree和Fast-GICP方法对算法后端进行了改进。实验证明，该算法在大场景建图中相比于LIO-SAM有着更低的耗时，后期还应该进一步进行实车部署，对算法性能进行完善。

[1] 蔡英凤,陆子恒,李祎承,等.基于多传感器融合的紧耦合SLAM系统[J].汽车工程,2022,44(3):350-361.

[2] 周治国,曹江微,邸顺帆.3D激光雷达SLAM算法综述[J].仪器仪表学报,2021,42(9):13-27.

[3] ZHANG J,SINGH S.LOAM:Lidar Odometry andMapping in Real-time[J].Robotics:Science and Systems, 2014,2(9):1-9.

[4] SHAN T,ENGLOT B.LeGO-LOAM: Lightweight and Ground-optimized Lidar Odometry and Mapping on Variable Terrain[C]//2018 IEEE/RSJ InternationalConference on IR0S.Piscataway:IEEE,2018:4758-4765.

[5] YE H,CHEN Y,LIU M.Tightly Coupled 3D LidarInertial Odometry and Mapping[C]//2019 International Conference on Robotics and Automation.Piscataway: IEEE,2019:3144-3150.

[6] SHAN T,ENGLOT B,MEYERS D,et al.LIO-SAM: Tightly Coupled Lidar Inertial Odometry Vias Mooth- ing and Mapping[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems.Pisca- taway:IEEE,2020:5135-5142.

[7] ELSEBERG J,MAGNENAT S,SIEGWART R,et al. Comparison of Nearest-neighbor­-search Strategies and Implementations for Efficient Shape Registration [J].Journal of Software Engineering for Robotics,2012, 12:1-12.

Tightly Coupled SLAM Algorithm for Lidar and IMU

WU Mingyue

( School of Automobile and Transportation,Tianjin University of Technology and Education, Tianjin 300222, China )

In recent years, the field of unmanned driving has become a hot topic of widespread concern,and the same time,simultaneous localization and mapping(SLAM) technology is the basis of high-precision map creation and unmanned vehicle navigation, and the current mainstream laser SLAM algorithm can basically meet the needs of applications, but there is still a drift problem in the process of large-scale scene mapping, and there is still room for improvement in algorithm lightweight and real-time.In this paper, the simultaneous localization and mapping algorithm of lidar and inertial measurement unit (IMU) tightly coupled is mainly studied, and the front-end part mainly removes distortion and feature extraction of laser point cloud data, and the back-end uses factor map to fuse IMU pre-integration factor, laser odometry factor and loopback detection factor for fusion pose output. In order to improve the real-time performance of the algorithm, this paper uses the iKD-Tree data structure to maintain a local map, and uses the Fast-generalized iterative closest point (GICP) algorithm to solve the loopback detection pose transformation. The test of Kitti's public dataset shows that the proposed algorithm improves the real-time and robustness of the algorithm while ensuring accuracy.

Lidar;Factor graph optimization;IMU;Tight coupled;SLAM algorithm

U469.5

A

1671-7988(2023)21-21-04

10.16638/j.cnki.1671-7988.2023.021.005

吴明月（1995－），男，硕士研究生，研究方向为激光SLAM，E-mail:1423513196@qq.com。