基于因子图的激光SLAM模型优化算法

摘 要：针对激光SLAM点云建图模型研究，设计一种因子图优化的SLAM模型方案。在前端模型中，激光里程计通过引入IESKF实现IMU与雷达点云数据的紧耦合构建IESKF-LIO。在SLAM后端模型中，为提高SLAM的鲁棒性和实时精度，构建多种因子进行位姿约束与补偿优化，同时在因子图融合过程中提出关键帧和增量式平滑建图，减少模型运算负担。在KITTI数据集中进行建图实验，实验结果验证了该模型较传统SLAM模型轨迹误差更低，建图效果更好。

关键词：SLAM；因子图；IESKF；回环检测

中图分类号：TP391.9" 文献标志码：A" 文章编号：1671-5276（2024）05-0167-04

Factor Graph-based Laser SLAM Model Optimization Algorithm

Abstract：For the study of laser SLAM point cloud mapping model， a factor graph optimized SLAM model scheme is designed. In the front-end module， the laser odometry realizes the tight coupling of IMU and radar point cloud data by introducing IESKF， thus IESKF-LIO being constructed. In the SLAM back-end model， a variety of factors are built for pose constraints and compensation optimization in order to improve the robustness and real-time accuracy of SLAM， meanwhile， key frames and incremental smooth mapping are proposed during the factor graph fusion process to reduce the computational burden of the model. Through the mapping experiments in KITTI data sets， the experimental results verify that the designed model has lower trajectory errors and better mapping effects than the traditional SLAM onel.

Keywords：SLAM；factor graph；IESKF；loopback testing

0 引言

室外场景下的语义类别较多，地面形体上各类别所呈现的高低起伏状态不具有一般规律性。传统激光SLAM获取的室外点云地图较为稀疏，获取点云不均匀和无规律性较强，同时所获点云信息仅能表示物体的一般特征，无法体现更深度的细节信息。

SLAM技术最早在1987年被提出［1］，经过30余年，SLAM技术得到了充分的发展。SMITH等［2］提出了EKF-SLAM模型，该模型在小规模和简单场景下具有较高精度的建图效果，但不适用于较大型场景下的建图作业。FAST-LIO是香港大学开发的实时建图模型，通过IESKF将激光点云特征与IMU数据进行融合，为了降低激光点云特征带来的计算量，提出依赖维度的卡尔曼增益公式［3］。CHANG等［4］提出一种融入IMUamp;ODO预积分的SLAM系统，在前端使用三维概率图，提高在特征有限的环境中点云匹配的效果。LeGO-LOAM［5］算法在LOAM算法框架的基础上，在特征提取部分针对地面特征进行优化，并增加了回环检测，但其仍然无法解决在结构化场景及空旷环境中雷达里程计失效的问题。为了解决激光雷达里程计失效的问题，研究者尝试使用激光雷达与其他传感器结合定位建图。LIO-SAM［6］算法则是在图优化的基础上实现紧耦合的激光雷达里程计，采用关键帧的滑动窗口模式减小建图计算量。付林等［7］提出结合惯性/高度传感器的激光SLAM算法，在点云初始化时引入惯性/高度传感器的姿态，有效提高了高程退化环境中定位精度。刘振宇等［8］提出一种融合激光雷达与惯性测量单元融合的算法，在算法中使用帧与局部地图匹配所得的距离值和IMU数据构建出联合优化函数，提高了原算法的精度。

针对室外环境的SLAM建图研究，本文以激光雷达点云数据为中心搭建多传感器数据融合的同步定位和建图方案。通过构建激光惯性里程计优化帧间里程数据、因子图融合等方式，实现对室外场景下的高效地形感知与建图算法搭建；同时通过仿真与对比实验，验证了比传统算法更为明显的优化效果。

1 基于优化卡尔曼滤波的LIO设计

自卡尔曼滤波算法被提出后，被广泛运用在控制系统和导航与定位等方面［9］。本文采用基于FAST-LIO框架的设计理念，使用紧密耦合的迭代扩展卡尔曼滤波器（IEKF）将LiDAR特征点与IMU数据融合在一起，引入误差状态卡尔曼滤波器实现对激光雷达和IMU融合位姿进行误差补偿。经过多次迭代，获得高精度的后验状态参数与协方差参数，在快速运动或非结构化环境中实现稳健导航和建图操作。

在融合IMU和激光雷达数据时，构建观测方程将测量结果结合起来，进一步需要求解出观测的协方差矩阵H，在得到观测方程的协方差矩阵后需要对协方差矩阵进行状态更新以减小误差传递，最后是获取增益矩阵Ki+1。不同于传统卡尔曼滤波协方差更新方程，本文采用的是FAST-LIO模型中IESKF模型的增益矩阵更新方式。

2 关键因子构建

2.1 因子图模型

因子图模型是用因子描述多变量复杂函数的二维图，属于概率模型（PGM）中的一种，可以表征全局函数和局部函数之间的关系，同时也可以表示各个变量与局部函数之间的关联，经常被用于多源数据融合的SLAM研究中。

在多源SLAM全局优化过程中，基于贝叶斯模型构建系统的状态量和观测量SLAM的后验概率模型，假定各个因子都符合高斯分布，则其SLAM模型中求解最大后验概率问题则可以转化为非线性最小二乘问题。针对多种因子进行非线性求解的目标函数可以被定义为式（1）：

式中：hi表示第i个传感器模块的观测数据；γi表示第i的状态量下的传感器测量数据；hi（X）－γi表示各个因子的约束残差项；∑i f表示各个传感器的测量方差；状态变量X为待优化参数项。

2.2 改进最优描述子的快速回环因子设计

为有效解决大范围定位下的误差漂移问题，同时在保证回环精度前提下提升回环检测速度，本文基于经典Scan-Context回环模型进行改进，设计了一种在保证精度的同时实现快速回环的优化方案，通过联合Scan-Context描述子与Lidar-Iris特征图的方式构建并确定回环检测最优关键帧因子。整体的构建过程如下。

1）采用圆环分区的方式，将三维空间的点云信息转换为二维空间的矩阵。以当前帧为中心将所获点云转换为鸟瞰图，设置最大搜索距离为dmax，以dmax为圆半径将扫描区域划分为Nr个圆环和Ns个扇区，将每个环展开可以得到一个 Nr×Ns的二维图像。每个像素点Pi，j是第i个环第j个扇面对应的Bin值。本文定义每个像素点Pi，j的Bin值为当前像素区域内点云最大高度，这样利用Scan-Context就构成了一个Nr×Ns的二维矩阵，最终形式如图1所示。

将Scan-Context获取的环向量作为描述符，每个环向量都被用来构造KD-Tree，在KD-Tree中通过最近邻搜索选取与当前关键帧环键相似的环键对应的关键帧作为回环候选关键帧。

2）根据距离原点由近及远对矩阵的每一行向量ki计算环向量平均值，得到该帧下的描述符环向量K。环向量K是Nr×1维的矩阵，且具有旋转不变性，故可以采用KD-Tree的结构来计算两帧间环向量的相似度，从而快速地选取合格的候选回环帧。

3）在得到候选回环帧后，进一步利用Lidar-Iris对多个回环候选关键帧中的Bin值进行重新编码，生成Lidar-Iris图像。

4）将每个关键帧下得到的二值特征图存于历史数据库中。在步骤2）中获得候选回环关键帧后，通过计算当前关键帧和数据库中候选关键帧下Lidar-Iris特征图像之间的Hamming距离判断最优回环帧。如果距离计算结果最小且小于阈值，则判定该时刻下形成回环，进一步以当前帧为中心将KD-Tree中的最优回环帧与当前帧进行配准，得到回环下的位姿变换矩阵，最终完成回环的精匹配。

回环检测完成后，将得到的位姿变换矩阵作为新的约束传入因子图优化框架中，进一步更新传感器的位姿估计，消除里程计累计误差和漂移。

2.3 IMU预积分因子、 GPS环境因子构建

IMU预积分处理就是将在雷达两帧间获取的多组IMU加速度、角速度数据进行积分处理，从而得到机器在帧间的精准位姿变化信息，根据积分数据进一步得到IMU预积分因子。但两帧间获取的IMU数据为离散形式，不满足连续积分，因此要对积分公式进行离散化处理，可得到关键帧之间的离散IMU预积分测量模型。

虽然通过增加IMU预积分因子、回环检测因子和融合IMU位姿优化的激光惯性里程计因子可以使整体SLAM获得稳定的位姿和状态估计，但本研究主要是以室外环境为主，且多为较大场景结构或非结构化地形进行扫描，同时KITTI数据集下提供了较为精准的RTK-GPS轨迹真值。因此进一步考虑增加GPS约束，可以保证在室外环境下获得绝对位置精度补偿，更好地消除漂移问题。

3 基于滑动窗口的因子图优化模型

通过前端基于IESKF完成IMU/Lidar紧耦合融合优化构建了IESKF-LIO因子，可以实现局部地图模型下保持较高精度，进一步引入回环检测因子、IMU预积分因子和GPS项约束，基于上述4种因子约束项，在后端融合建图模块。

首先将构建的IESKF-LIO作为初始位姿，当设备在动态运行不断地获取局部地图观测信息时，通过固定尺寸的滑动窗口计算并更新少数与当前观测密切相关的点云数据，这样可以减少多元约束融合模型的计算量。在动态滑动窗口下，设计基于增量式平滑建图（ISAM）的因子图融合方式，保证了在因子图中每次加入新的观测时不必重新计算整个因子图的雅可比矩阵。最终的因子图多源融合框架如图2所示。融合因子包括激光惯性里程计因子、IMU预积分因子、GPS因子和回环检测因子。

4 试验与分析

针对运动位姿数据，本文通过使用EVO工具提供的两种常见评价指标包括相对轨迹误差（RPE）和绝对轨迹误差（ATE）进行性能分析。通过使用不同的算法在数据集上获得的地图轨迹和真值轨迹进行误差统计，可以非常直观地反映各种算法之间的轨迹精度和误差情况，从而更好地评价算法的全局一致性效果。

为了更加清晰直观地体现本文构建算法的合理性，将针对KITTI数据集下07序列地图，利用A-Loam、Lego-Loam、Lio-Sam和本文融合多因子图模型算法方案进行对比分析。

在KITTI_07数据集下进行对比仿真，图3所示是4种算法的轨迹运行情况的热图误差对比分析。其中参考值是KITTI数据集提供的轨迹真值，彩色轨迹表示模型输出轨迹，轨迹颜色越靠近红色表示与真值误差越大，越靠近蓝色表示与真值轨迹越贴合，误差越小（因本刊为黑白印刷，有疑问之处请向作者咨询）。从图3中可以看出，对比真实轨迹下的绝对误差热图可以发现，本文所建的建图模型与其他3种相比，具有更小的误差最大值和误差真值分布区间，表明了整体轨迹定位精度控制效果最优。

针对点云建图的模型评估指标如图4所示，主要包括轨迹误差标准差（Std）、方均根误差（RMSE）、最小轨迹偏移量（Min）、轨迹误差中值（Median）、平均值（Mean）和最大值（Max），其中的标准差、方均根误差和平均值皆取3次实验的平均值，最大最小值取3次试验的最值，各项指标的具体数值记录在表1中。可以看出本文构建的模型在多个误差指标中皆具有不同程度的优越性。本文所建模型的方均根误差和误差均值皆控制在0.5m左右，反映本文所建模型轨迹稳定性更高，防数据畸变效果更好。

5 建图效果展示

本实验的地图环境为KITTI_07地图模型，由于本文所建算法是基于多因子融合的方式进行数据融合，针对激光惯性里程计使用优化卡尔曼滤波方式进行构建，理论中可以明显改善点云畸变问题，同时减少当前帧扫描点云下的偏移误差累积，进而提高局部到全局地图的融合精度。由于针对建图效果没有定量的指标分析评估手段，本小节仅通过地图可视化分析建图效果，如图5所示。

从图5中可以看出在本文所用算法构建的点云地图与数据集提供的真实地图基本重合。由于优化了点云扫描当前帧的去畸变效果，在动态建图扫描中融合了多种因子进行补偿纠正，在局部的弯道路径中建图效果表现较好，未出现点云重影或回环图像不重合等现象，实现了较高精度的建图效果。

6 结语

本文主要针对点云三维建图进行相关研究。首先通过使用紧耦合的迭代误差状态卡尔曼滤波（IESKF）将雷达特征点云数据与IMU数据融合，构建激光惯性里程计获得更精准的初始位姿信息。进一步使用优化因子图方式实现多传感器数据融合，所建融合因子包括：基于迭代扩展卡尔曼滤波融合的激光惯性里程计因子、IMU预积分因子、基于关键帧的回环因子与增加置信度评价的GPS因子。最后通过对上述整体模型在两种KITTI数据集上进行对比仿真分析，结果表明本文所建融合模型的建图效果较传统方案有较高的精度和较为明显的优化效果。

参考文献：

［1］ SMITH R C，CHEESEMAN P. On the representation and estimation of spatial uncertainly［J］. International Journal of Robotics Research，1987，5（4）：56-68.

［2］ SMITH R，SELF M，CHEESEMAN P. Estimating uncertain spatial relationships in robotics［C］//Proceedings of 1987 IEEE International Conference on Robotics and Automation. Raleigh，NC，USA： IEEE，2003：850.

［3］ XU W，ZHANG F. FAST-LIO：a fast，robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter［J］. IEEE Robotics and Automation Letters，2021，6（2）：3317-3324.

［4］ CHANG L，NIU X J，LIU T Y. GNSS/IMU/ODO/LiDAR-SLAM integrated navigation system using IMU/ODO pre-integration［J］. Sensors，2020，20（17）：4702.

［5］ SHAN T X，ENGLOT B. LeGO-LOAM：lightweight and ground-optimized lidar odometry and mapping on variable terrain［C］//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Madrid，Spain： IEEE，2018：4758-4765.

［6］ SHAN T X，ENGLOT B，MEYERS D，et al. LIO-SAM：tightly-coupled lidar inertial odometry via smoothing and mapping［C］//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS） ，Las Vegas，NV， USA： ［s.n.］，2020：5135-5142.

［7］ 付林，郑佳楠，何洪磊，等. 面向大型室内场景的无人机三维激光雷达解耦SLAM方法［J］. 电子测量技术，2022，45（13）：96-103.

［8］ 刘振宇，惠泽宇，郭旭，等. 基于滑动窗口优化的激光雷达惯性测量单元紧耦合同时定位与建图算法［J］. 科学技术与工程，2022，22（21）：9167-9175.

［9］ 袁艳，李峰，王东. 基于开关卡尔曼滤波的叶轮故障振动信号特征提取［J］. 机械制造与自动化，2024，53（2）：67-70.