基于因子图优化的多尺度深度感知同时定位和建图算法

丁承君; 马智辉; 张坦; 马腾飞; 李子健; 王逸桐; 景志凯

doi:10.12404/j.issn.1671-1815.2503143

科学技术与工程 ›› 2026, Vol. 26 ›› Issue (13) :5556-5565 DOI: 10.12404/j.issn.1671-1815.2503143

自动化技术、计算机技术

基于因子图优化的多尺度深度感知同时定位和建图算法

作者信息 +

Multi-scale Depth-aware Simultaneous Localization and Mapping Algorithm Based on Factor Graph Optimization

Author information +

文章历史 +

摘要

针对纯激光雷达即时定位与建图方法在大尺度复杂场景下存在精度衰减、鲁棒性欠佳的问题,提出一种基于因子图优化的紧耦合同时定位和建图技术(simultaneous localization and mapping,SLAM)方法。该方法通过三大核心设计提升性能:一是设计基于误差状态卡尔曼滤波并融合多传感器信息的组合惯导里程计因子;二是采用多尺度深度感知特征选择算法提取线面点特征,并通过先粗配准后精配准的方法优化帧间匹配,提升位姿变换精度;三是在因子图模型中整合组合惯导因子、激光惯性里程计因子,并引入具有旋转不变性的扫描上下文的回环检测因子,抑制累积误差。在KITTI公开数据集与校园自采数据集上的对比实验表明,所提算法相较于经典LIO-SAM算法,复杂场景下的绝对轨迹误差均值分别降低25.96%和23.01%,均方根误差分别降低25.52%和28.38%。实验验证了所提算法的高建图精度与鲁棒性,为室外复杂场景SLAM的工程应用提供了有效技术方案。

Abstract

In order to address the current limitations of pure laser SLAM under large-scale complex environments, particularly their lack of precision and poor robustness, a tightly-coupled SLAM method based on factor graph optimization has been proposed. The method enhances performance through three core designs. Firstly, designing an integrated inertial odometry factor based on the error-state Kalman filter (ESKF) that fuses multi-sensor information. Secondly, adopting a multi-scale depth-aware feature selection algorithm to extract line, plane, and point features, and optimizing inter-frame matching via coarse-to-fine registration to improve pose transformation accuracy. Thirdly, incorporating integrated inertial factors and laser-inertial odometry factors into the factor graph model, and introducing a rotation-invariant Scan Context-based loop closure detection factor to suppress cumulative errors. Comparative experiments on the public KITTI dataset and a self-collected campus dataset show that, compared with the classical LIO-SAM algorithm, the proposed algorithm reduces the mean absolute trajectory error (ATE) by 25.96% and 23.01% respectively, and the root mean square error (RMSE) by 25.52% and 28.38% respectively in complex scenarios. Experiments verify that the proposed algorithm achieves high mapping accuracy and robustness, and provides an effective technical solution for the engineering application of SLAM in outdoor complex scenarios.

Graphical abstract

关键词

激光SLAM / LIO-SAM算法 / ESKF / 特征提取 / 因子图优化

Key words

LiDAR SLAM / LIO-SAM algorithm / ESKF / feature extraction / factor graph optimization

引用本文

引用格式 ▾

丁承君, 马智辉, 张坦, 马腾飞, 李子健, 王逸桐, 景志凯. 基于因子图优化的多尺度深度感知同时定位和建图算法[J]. 科学技术与工程, 2026, 26(13): 5556-5565 DOI:10.12404/j.issn.1671-1815.2503143

DING Cheng-jun, MA Zhi-hui, ZHANG Tan, MA Teng-fei, LI Zi-jian, WANG Yi-tong, JING Zhi-kai. Multi-scale Depth-aware Simultaneous Localization and Mapping Algorithm Based on Factor Graph Optimization[J]. Science Technology and Engineering, 2026, 26(13): 5556-5565 DOI:10.12404/j.issn.1671-1815.2503143

登录浏览全文

4963

注册一个新账户忘记密码

同时定位和建图技术(simultaneous localization and mapping,SLAM)已成为移动机器人及自动驾驶领域的一种重要技术手段^[1],目前该技术主要分为基于视觉信息的方法和基于激光雷达信息的方法。作为感知环境和路径跟踪等功能的前置条件,需要在陌生环境下实时建立当前环境地图及获取自身位置信息,相比相机而言,雷达获得的信息更难受到环境光照和恶劣天气的影响,具有较强的鲁棒性。

Besl等^[2]于1992年提出的迭代最近点理论(iterated closest points, ICP)作为三维点云配准的基础,是一种精准配准算法,但对于点云数据较为稀疏场景效果较差。Segal等^[3]提出的GICP理论(generalized-ICP)针对稀疏点云场景中的误差积累问题提出一种基于点、直线、平面的配准原则。Koide等^[4]基于概率配准理论提出VGICP框架(voxelized GICP),通过融合GICP算法与多尺度体素空间离散策略保证了该算法在提升配准精度的前提下显著提升运算效率。在此基础上CT-ICP^[5]在算法中添加了帧间逻辑约束,令起始状态接近上一帧的末尾状态,从而并行实现姿态估计和误差矫正进程,Vizzo等^[6]提出的Kiss-ICP在提高计算精度的同时大幅降低了计算所用资源,虽然这种方法能够降低计算负荷,但连续帧之间存在较大的重叠区域导致误差累计较大,这一问题在稀疏场景和复杂大场景中表现明显。

Zhang等^[7]提出的LOAM是一种实时的里程计与建图方法,它提取边缘和平面特征用纯激光雷达进行建图构建出高精度的SLAM框架。通过计算获取的点云信息得到线、面特征,实现对系统的位姿估计。LOAM中帧间匹配与帧到地图匹配的分离结构已被许多后续工作采用,例如LeGO-LOAM^[8],该算法考虑帧间匹配时地面带来的约束,使用松耦合方式组合了IMU数据,从而提高了激光里程计的精度,并引入闭环检测提高了系统鲁棒性。由于松耦合架构对于传感器信息的利用并不充分,刘振宇等^[9]通过加入滑动窗口优化点云数据和IMU数据的融合,提高了位姿估计的准确率。姜文飞等^[10]提出一种改进的扩展卡尔曼滤波算法提升位姿匹配效果,并融合深度相机信息提升建图效果及定位精度。Shan等^[11]在2020年提出了紧耦合的激光SLAM算法LIO-SAM,首次将因子图框架引入作为后端优化,这使得系统能够将不同来源的绝对和相对测量值作为因子纳入图模型,从而减小误差的传播。但是在密集重复纹理区域中,相似的几何特征易引起误匹配,点云的稀疏性更加剧了该问题的产生。FAST-LIO采用求解卡尔曼增益技术和增量KD-Tree(k-dimensional tree)结构大大降低了计算负担^[12]。Chen等^[13]提出的DLIO采用一种仅由时间参数化的解析方程方法来构建连续时间轨迹以实现精确的运动校正,通过同时进行运动校正和先验生成,显著提高了建图效率。IG-LIO 提出了一种基于增量GICP的紧密耦合激光雷达惯性里程计算法^[14],将GICP约束和惯性约束整合到统一的估计框架中,引入曲面特征点提取^[15],并利用增量体素地图来表示周围环境的概率模型,显著减少协方差估计和最近邻搜索计算时间。但是以上研究都存在着在几何特征较弱的大型场景中位姿估计效果不佳甚至精度降低严重^[16]的问题,且其在计算资源有限的平台上消耗大量资源用于局部地图的构建^[17],降低了计算效率。

针对LIO-SAM在全局轨迹绘制和回环检测方面的不足,现采用GTSAM(georgia tech smoothing and mapping)库实现因子图优化问题的建模。该算法通过对激光雷达、IMU、RTK等传感器数据的处理进而对车辆进行状态估计并建立地图信息。所提算法相对LIO-SAM算法主要的改进部分有以下内容:提出基于ESKF的组合惯导里程计因子,将高频IMU数据和GNSS全局定位信息通过紧耦合的方式优化估计的车辆位姿,从而实时修正位姿信息,降低定位误差;改进点云特征提取部分,通过多尺度深度感知的特征选择算法能够在大尺度场景中更准确地提取环境特征,提升建图准确性;通过改进点云帧间匹配部分获得更精确的空间位姿变换关系,避免在初始位姿不准确时算法陷入局部最优解的情况。通过在公开数据集和自采数据集上进行实验,验证所提算法。

1 算法原理

本文算法框架如图1所示,首先将GNSS与IMU信息融合得到组合惯导里程计因子,并将经过修正的IMU预积分数据用于矫正激光点云的运动畸变。之后对点云数据进行特征点提取与特征点配准,从而得到局部地图和激光惯性里程计信息,并进行回环检测。其次在因子图优化器中输入组合惯导里程计因子、激光惯性里程计因子和回环检测因子,结合组合惯导给出的先验位姿得到全局地图和轨迹信息。

1.1 基于ESKF的组合惯导里程计因子

实时动态差分定位(real time kinematic, RTK)作为卫星导航增强体系的核心技术,但在实际应用场景中由于遮挡场景会出现失效问题,以及在振动场景下存在周跳和相位模糊度丢失问题,因此常将惯性测量单元(inertial measurement unit, IMU)引入来补偿RTK失效时的定位误差。IMU作为一种能够测量物体的加速度、角速度和磁场强度的传感器装置被广泛应用于提供车辆的姿态及运动信息。但现实因素导致测量值与真值之间存在误差,因此假设IMU测得的角速度和加速度为

$\stackrel{\sim }{\mathit{\omega }}$

、

$\stackrel{\sim }{\mathit{\alpha }}$

,真值为ω、α,存在以下关系。

(1)

$\stackrel{\sim }{\mathit{\omega }}=\mathit{\omega }+\overline{\mathit{b}}+\stackrel{—}{\mathit{\eta }}$

(2)

$\stackrel{\sim }{\mathit{\alpha }}={\mathit{Q}}_{\mathit{i}\mathit{w}}(\mathit{g}-\mathit{\alpha })+\stackrel{—}{\mathit{b}}+\stackrel{—}{\mathit{\eta }}$

式中:Q_iw为从世界坐标系到IMU坐标系的旋转矩阵;g为地球重力加速度;

$\stackrel{—}{\mathit{b}}$

、

$\stackrel{—}{\mathit{\eta }}$

、

$\underset{—}{\mathit{b}}$

、

$\underset{—}{\mathit{\eta }}$

分别为加速度计与陀螺仪的高斯噪声和零偏误差。在实际应用中,IMU测量值常常包含外部噪声干扰,即所提到的高斯噪声和零偏误差。高斯噪声是指在测量过程中由于环境干扰、器件自带噪声等因素引起的随机测量误差。零偏误差是指被观测物体在完全静止的情况下,IMU传感器的输出不为零的现象。

为了处理系统的噪声及干扰,减少定位的不确定性,一般选择引入卡尔曼滤波器通过估计线性动态系统的状态来实现对定位估计值的优化。但由于在实际应用中,很多系统的观测模型都是非线性的,直接使用卡尔曼滤波器来估计系统的状态会产生较大的误差,因此提出引入误差状态卡尔曼滤波器(error state Kalman filter,ESKF)优化系统状态的预测值和误差状态预测。

车辆状态估计流程如图2所示。系统状态可分解为名义状态与误差状态的叠加。其中,名义状态表征状态量的预测估值,而误差状态反映该估值与真实状态间的偏差量。通过将这两个状态分量进行矢量合成,即可重构出完整的系统真实状态。系统状态估计流程可构建为多源数据融合的递推过程。首先基于RTK设备与IMU的初始观测值构建系统状态估计与误差状态的初始参数。通过IMU数据的惯性解算推算速度、姿态等运动参数,进而获得完整的动力学状态描述。随后结合惯性解算结果、历史状态估计及实时控制输入,执行误差状态卡尔曼滤波(ESKF)的预测更新过程,实现误差状态的先验预测。在此阶段同步更新误差状态协方差矩阵以量化预测阶段的不确定性累积。系统持续监测GNSS观测通道状态:在GNSS信号缺失时,维持惯性导航解算结果作为位姿输出;当获取有效GNSS观测值时,通过卡尔曼增益矩阵对预测状态与观测值进行最优融合,完成状态估计的校正。经量测更新后得到IMU/GNSS融合的最优位姿估计值,实现多源传感器数据的协同定位。

1.2 基于特征点匹配的激光惯性里程计因子

1.2.1 多尺度深度感知的特征选择算法

提出一种多尺度深度感知的特征提取算法,在提取关键点时通过设定距离阈值筛选当前帧附近满足距离阈值要求的关键帧,距离阈值的设置和当前帧的位置相关。当前帧的位置距激光雷达较远时,距离阈值较大,需要计算的临近点越多;当前帧的位置距激光雷达较近时,距离阈值较小,需要计算的临近点越少。将经过处理的点云通过多尺度深度感知的特征选择算法计算单帧去畸变后的点云中每个激光点的曲率,表达式为

(3)

$\mathit{c}=\frac{1}{\left|\mathit{S}\right|‖{{\mathit{X}}^{\mathit{L}}}_{(\mathit{k},\mathit{m})}‖}‖\frac{\mathit{　}}{\mathit{　}}{\sum }_{\mathit{n}\in \mathit{S},\mathit{n}\ne \mathit{m}}^{}[{{\mathit{X}}^{\mathit{L}}}_{(\mathit{k},\mathit{m})}-{{\mathit{X}}^{\mathit{L}}}_{(\mathit{k},\mathit{n})}]‖$

式(3)中:X^L_(k,m)为该激光点在雷达坐标系下的欧式距离;m和n分别为目标激光点的深度和目标激光点邻域点的深度;k为激光雷达扫描一圈所需时间;S为该帧点云的激光点数量,其值与深度值R_i存在关系为

(4)$S=\left\{\begin{array}{ll} 10, & R_{i} \leqslant 10 \\ 8, & 10<R_{i} \leqslant 20 \\ 6, & 20<R_{i} \leqslant 30 \\ 4, & 30<R_{i} \end{array}\right.$

如图3所示,激光点的特征曲率值反映了该点位置的几何特征。依据计算得到的特征曲率值,对照预先设置好的曲率阈值,对激光点进行分类将其划分为角点与平面点。

如图4所示为车辆在单行道路及开放交会路口行驶过程中提取的角点及平面点,黄色点云为角点,紫色点云为平面点。可以观察到,这些特征点云在不同代表路况下均匀分布,既反映了真实道路环境的三维样貌,又降低了点云数据量,有效减少了后续特征点云配准的计算量。

1.2.2 特征点云配准

通过上述特征提取算法提取点云特征后,将获取到的多帧点云数据进行时空对齐,利用特征匹配建立相邻帧间的最优变换模型实现对车辆的位姿估计,并将准确的位姿变换加入后端因子图中。采用正态分布变换(normal distributions transform, NDT)算法实现点云粗配准,为后续迭代最近点(ICP)算法提供较精确的初值,从而在提升算法计算效率的同时确保算法匹配精度。NDT算法通过将目标点云空间离散化处理,得到多分辨率体素栅格,在体素栅格中构建局部几何结构多元正态分布模型,从而用概率密度函数来描述空间点集的几何特征。首先计算目标点云X= {x₁, x₂, …,x_Nx}中每个体素栅格的均值μ和协方差Σ,计算该体素单元的正态分布参数,公式为

(5)

$\left\{\begin{array}{l}{\mathit{\mu }}_{\mathit{i}}=\frac{1}{{\mathit{N}}_{\mathit{x}}}\sum _{\mathit{i}=1}^{{\mathit{N}}_{\mathit{x}}}{\mathit{x}}_{\mathit{i}}\\ {\mathit{\Sigma }}_{\mathit{i}}=\frac{1}{{\mathit{N}}_{\mathit{x}}-1}\sum _{\mathit{i}=1}^{{\mathit{N}}_{\mathit{x}}}({\mathit{x}}_{\mathit{i}}-{\mathit{\mu }}_{\mathit{i}}\left)\right({\mathit{x}}_{\mathit{i}}-{\mathit{\mu }}_{\mathit{i}}{)}^{\mathit{{\rm T}}}\end{array}\right.$

式(5)中:i为对应体素栅格的编号;N_x为对应栅格内点云总数;μ_i为均值位置矢量;x_i为单激光点的位置矢量;Σ_i为协方差矩阵。根据先验的待配准点云的变换关系将其投影到目标点云坐标系下,待配准点云集合为Y = {y₁, y₂, …,y_Ny},变换关系式为

(6)

$\mathit{y}\mathit{\text{'}}{\mathit{ }}_{\mathit{i}}=\mathit{T}(\mathit{p},{\mathit{y}}_{\mathit{i}})=\mathit{R}{\mathit{y}}_{\mathit{i}}+\mathit{t}$

式(6)中:T为待配准点云变换到目标点云的刚体变换算子;p为变换参数的集合,包括旋转矩阵R和平移向量t,即p={R,t}。获得新点云集合Y'后求出经过投影后的正态分布,将新点云集合的联合概率密度作为目标函数,通过梯度下降法迭代优化直到满足收敛条件并输出,点云集合的联合概率密度表达式为

(7)

$\begin{array}{l}\prod _{\mathit{i}=1}^{{\mathit{N}}_{\mathit{x}}}\mathit{f}[\mathit{X},\mathit{T}(\mathit{p},{\mathit{y}}_{\mathit{i}})\left]\mathit{f}\right(\mathit{X},\mathit{y}\mathit{\text{'}}{\mathit{ }}_{\mathit{i}})=\frac{1}{\sqrt[3]{2\mathit{\pi }}\sqrt{\left|\mathit{\Sigma }\right|}}\times \\ \mathit{　}\mathit{　}\mathit{e}\mathit{x}\mathit{p}\left[-\frac{(\mathit{y}\mathit{\text{'}}{\mathit{ }}_{\mathit{i}}-{\mathit{\mu }}_{\mathit{i}}{)}^{\mathit{{\rm T}}}{\mathit{\Sigma }}_{\mathit{i}}^{-1}(\mathit{y}\mathit{\text{'}}{\mathit{ }}_{\mathit{i}}-{\mathit{\mu }}_{\mathit{i}})}{2}\right]\end{array}$

将点云粗配准得到的位姿变换矩阵代入ICP算法中,并将其作为目标点云X中点集x_i对应待配准点云Y中点集y_i的最近邻变换,计算此时误差函数E(R,t)为

(8)

$\mathit{E}(\mathit{R},\mathit{t})=\frac{1}{\mathit{n}}\sum _{\mathit{i}=1}^{\mathit{n}}{‖\frac{\mathit{　}}{\mathit{　}}{\mathit{y}}_{\mathit{i}}-(\mathit{R}{\mathit{x}}_{\mathit{i}}+\mathit{t})‖}^{2}$

对目标点云使用上述位姿变换矩阵可得新的点云集X¹,计算其与待配准点云集Y的平均距离d为

(9)

$\mathit{d}=\frac{1}{\mathit{n}}\sum _{\mathit{i}=1}^{\mathit{n}}{‖\frac{\mathit{　}}{\mathit{　}}{\mathit{x}}_{\mathit{i}}^{1}-{\mathit{y}}_{\mathit{i}}‖}^{2}$

若平均距离大于设定阈值,将新的点云集X¹作为目标点云进行奇异值分解(singular value decomposition, SVD),使得误差函数E(R, t)通过不断迭代最小化,直到目标点云与待配准点云的平均距离小于阈值则停止迭代。

1.3 回环检测因子

SLAM系统的单端感知系统产生航迹漂移的原因在于惯性导航里程计的推算误差与点云配准残差的累计效应。通过引入闭环约束,能够有效抑制系统由于长时间运行产生的航迹漂移误差,并基于历史位姿和当前位姿的几何相似性消除点云配准残差累计造成的局部地图形变,从而修复大场景建图过程中的空间连续性断裂。如图5所示,蓝色轨迹为参考轨迹,红色轨迹为SLAM系统估计的轨迹,在有回环检测时可以消除漂移误差造成的起点P₁终点P^* ₁不重合问题。

扫描上下文(Scan Context)的回环检测因子是一种基于激光点云的全局描述符技术,其核心思想是通过对激光雷达点云的几何特征进行全局编码,将三维点云数据转化为一个二维矩阵表示,从而实现对场景的快速相似性判断。其关键帧点云划分如图6所示。

其次对历史帧的全局描述符通过最近邻搜索进而得到两个全局描述符深度图的距离。最新帧与历史候选帧的Scan Context描述符分别为I^q和I^c,

${\mathit{c}}_{\mathit{j}}^{\mathit{q}}$

和

${\mathit{c}}_{\mathit{j}}^{\mathit{c}}$

分别为最新帧与历史帧描述符的第j列向量。根据式(10)计算判断这两帧点云Scan Context描述符的相似性。

(10)

$\mathit{d}({\mathit{I}}^{\mathit{q}},{\mathit{I}}^{\mathit{c}})=\frac{1}{{\mathit{N}}_{\mathit{s}}}\sum _{\mathit{j}=1}^{{\mathit{N}}_{\mathit{s}}}\left(1-\frac{{\mathit{c}}_{\mathit{j}}^{\mathit{q}}{\mathit{c}}_{\mathit{j}}^{\mathit{c}}}{\Vert {\mathit{c}}_{\mathit{j}}^{\mathit{q}}\Vert \Vert {\mathit{c}}_{\mathit{j}}^{\mathit{c}}\Vert }\right)$

通过判断最新帧和历史帧的距离阈值确定这两帧点云是否产生回环。但如果车辆处于同一位置但姿态不同,距离函数的计算结果相差较大,因此回环检测失效。针对该问题,通过将描述符I^c按列平移,旋转历史候选帧点云,得到该描述符的N_s个子描述符。计算最新帧描述符与这些子描述符的距离,并与设定的阈值进行比较,以此找到旋转后的最优匹配,根据式(11)选取距离最短的子描述符并判断该历史候选帧点云是否回环。

(11)

$\mathit{D}({\mathit{I}}^{\mathit{q}},{\mathit{I}}^{\mathit{c}})=\underset{\mathit{n}\in \left[{\mathit{N}}_{\mathit{s}}\right]}{\mathit{m}\mathit{i}\mathit{n}}\mathit{d}({\mathit{I}}^{\mathit{q}},{\mathit{I}}_{\mathit{n}}^{\mathit{c}})$

通过引入一致性判断来解决车辆位姿不一致问题,根据式(2)计算最新帧和历史帧的附近n帧平均余弦值。

(12)

$\mathit{p}({\mathit{I}}^{\mathit{q}},{\mathit{I}}^{\mathit{c}})=\frac{1}{\mathit{n}}\sum _{\mathit{k}=1}^{\mathit{n}}\mathit{D}({\mathit{I}}_{\mathit{i}-\mathit{k}}^{\mathit{q}},{\mathit{I}}_{\mathit{j}-\mathit{k}}^{\mathit{c}})$

式(12)中:

${\mathit{I}}_{\mathit{i}-\mathit{k}}^{\mathit{q}}$

为将最新帧I^q按列平移(i-k)行;

${\mathit{I}}_{\mathit{j}-\mathit{k}}^{\mathit{c}}$

为将历史帧I^c按列平移(j-k)行;若计算得余弦值小于设定阈值则认为此时这两帧点云构成回环。

2 因子图优化

采用GTSAM库实现后端因子图优化问题的建模。SLAM后端的常用形式包括因子图优化和扩展卡尔曼滤波(EKF)等方法。基于因子图优化的方法能够在复杂场景下提高位姿估计和局部地图构建的精度和鲁棒性。相比EKF等方法,因子图优化通过图结构构建变量间的约束关系并将SLAM问题转化为非线性优化问题求解,将位姿或地图特征作为图中的节点,将观测或运动约束作为因子节点,从而优化问题。

图优化框架如图7所示,其非线性求解的函数表达式为

(13)

${\mathit{X}}^{\mathit{*}}=\mathit{a}\mathit{r}\mathit{g}\mathit{m}\mathit{i}\underset{\mathit{x}}{\mathit{n}}\sum _{\mathit{i}=1}^{\mathit{N}}{‖\frac{\mathit{　}}{\mathit{　}}{\mathit{\lambda }}_{\mathit{i}}\left({\mathit{X}}_{\mathit{i}}\right)-{\mathit{b}}_{\mathit{i}}‖}_{2}^{2}$

式(13)中:λ_i(X_i)为第i个因子处对预测数据的多变量偏微分;b_i为第i个因子处的观测值;X^*为目标最优位姿变换;x为待优化参数状态变量。

3 实验结果及分析

3.1 实验平台与数据集

构建的自动驾驶实验平台使用基于Nvidia Orin与英飞凌TC297的优控智行EAORA04G-D款域控制器作为计算平台,搭载速腾聚创Helios 32激光雷达,测量频率为10Hz,垂直视场角为-16°~+10°。使用维特HWT-905-CAN款IMU,测量频率为100 Hz,以及深圳北天BT-B159款RTK移动站接收经差分校正的GNSS数据,测量频率为10 Hz。采用以下两组数据集进行验证。

(1)KITTI数据集中序列号2011_09_30_ drive_0018(简称为KITTI05)和2011_09_30_drive_0027(简称为KITTI07),其中05序列轨迹总长度为2 205.576 m,录制环境复杂,障碍物密集且具有多个回环路径,07序列轨迹总长度为694.697 m,场景相对简单,障碍物较少且回环单一。

(2)校园自采数据集。通过本研究构建的实验平台在河北工业大学校园内采集,实验平台如图8所示。设计两类开阔场景进行实验,卫星地图为Google地图网站获得。第一种为回环特征较简单的闭环道路,运动轨迹为环绕化工楼一圈。第二种为模拟车辆白天通勤场景,环境更加复杂且动态障碍物较多,由于此场景建图范围较大,更易造成建图的累计误差从而产生漂移现象。

3.2 全局轨迹与定位精度分析

采用开源评估工具evo系统在KITTI05和KITTI07数据集上对本文算法生成的轨迹和其他经典算法轨迹进行对比,如图9和图10所示。通过观察全局轨迹的局部细节可以发现,在05序列这种复杂城市道路场景中,A-LOAM算法产生轨迹漂移误差较大,LeGO-LOAM算法在终点处回环失败,LIO-SAM算法和本文算法与真实里程计偏差较小。在场景较为简单的07序列上各算法轨迹误差较小,而在弯道处本文算法相较其他算法显示出了更好的效果,里程计信息更接近真实里程。

各算法在KITTI数据集05和07序列的绝对轨迹误差(absolute trajectory error, ATE)统计数据如表1所示。ATE用于评估SLAM算法轨迹与地面真实轨迹之间的全局误差,反映系统在整个轨迹中的绝对定位精度。在05序列中,本文算法相较于其他经典算法的优势明显,全局轨迹的最大与最小误差均与地面轨迹真值差距最小,相比LIO-SAM算法平均误差减小了47.26%,最小误差减小了49.16%,均方根误差减小了44.11%;在07序列上相比LIO-SAM算法平均误差减小了4.66%,最小误差减小了3.57%,均方根误差减小了6.94%。

对比本文算法与LIO-SAM算法的绝对位姿轨迹误差(absolute pose trajectory error, APE)曲线如图11所示,可以发现在KITTI数据集05序列中的(0,30)、(75,90)、(120,170)、(210,260)等时间内,本文算法的绝对轨迹误差明显优于LIO-SAM算法,这证明该算法稳定性得到了明显优化。以上数据充分证明了本文算法在长时间的动态复杂大范围场景下充分利用了多传感器信息与环境特征信息,具有更高的鲁棒性,在场景较为简单、回环检测较少的场景中也能表现出一定的提升。

3.3 校园自采数据集实验

为了验证本文算法在实际场景中的建图效果,使用自建自动驾驶平台在河北工业大学内进行数据采集,设计的第一种回环特征较简单实验场景轨迹总长度为820.75 m,采集时间约为913 s;第二种模拟通勤实验场景轨迹总长度为1 479.32 m,采集时间约为1 225 s。将实验结果保存为pcd点云图,两个场景的建图效果如图12和图13所示。可以观察到在场景1中建筑物立体细节得到准确复现,窗框与立柱的几何特征保持完整,道路两侧的路缘石和绿化带边界无间隙、无断裂,路径上的树木特征明显,枝叶的层次结构得到完整保留。在场景2中交叉路口处不同方向采集的道路标线,交通环岛几何结构特征完整,未出现地图漂移重叠情况,经过回环检测修正后的地图有效抑制了点云层叠误差,实现了无缝融合局部地图的效果。

本文算法与LIO-SAM算法在自建数据集上的绝对轨迹误差统计数据如表2所示,通过ATE评价验证本文算法的精度和有效性。使用GNSS定位数据作为参考轨迹对算法轨迹进行评估,GNSS数据包括车辆在行驶过程中的精确坐标以及各时刻对应的航向角,两种实验场景的卫星图及全局轨迹对比图如图14和图15所示。

在场景对比实验中,本文算法较LIO-SAM算法展现出显著性能优化,在场景1中ATE均值降低了0.219 6 m,改善率达23.01%,均方根误差对应降低了28.38%;在场景2中ATE均值降低了0.391 3 m,改善率达29.32%,均方根误差对应降低了17.74%。本文算法具有更好的均方根表现,在各类代表性场景下具有更优的定位精度及鲁棒性。

4 结论

本文算法针对LIO-SAM算法在全局轨迹绘制和回环检测方面表现不佳的问题进行改进,提出一种具有更高精度和更高鲁棒性的SLAM算法。通过在KITTI数据集的复杂城市道路场景和校园自采数据集上进行实验验证,可以得到以下结论。

(1)引入基于ESKF的组合惯导里程计因子,通过紧耦合的方式融合多传感器信息并估计误差来优化车辆位姿估计,从而降低定位误差,保证建图的连贯性和准确性。

(2)改进点云特征提取和特征匹配环节,根据点云稀疏性特点设计一种多尺度深度感知的特征选择算法提高建图准确性,避免特征点丢失现象;并帧间匹配部分提高计算效率。

(3)在KITTI数据集的复杂城市道路场景和校园自采数据集上进行实验验证,实验结果表明本文算法的轨迹评价指标明显优于其他经典算法,具有较高的建图精度和鲁棒性,且在面对大尺度复杂场景时仍具有较好表现,具备实际工程应用价值。

参考文献

原文顺序 | 出版日期 | 本文引用

[1]	余联想, 郑明魁, 欧文君, 等. 多传感器融合的移动机器人室外激光SLAM算法优化与系统实现[J]. 电子测量与仪器学报, 2023, 37(2): 48-55.

[2]	Yu Lianxiang, Zheng Mingkui, Ou Wenjun, et al. Optimization and system implementation of outdoor LiDAR SLAM algorithm for mobile robots with multi-sensor fusion[J]. Journal of Electronic Measurement and Instrumentation, 2023, 37(2): 48-55.

[3]	Besl P J, Mckay H D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 1992, 14(2): 239-256.

[4]	Segal A, Hhnel D, Thrun S. Generalized-ICP[J]. Robotics: Science and Systems V, 2009, 2(4): 435-442.

[5]	Koide K, Yokozuka M, Oishi S, et al. Voxelized GICP for fast and accurate 3D point cloud registration[C]// 2021 IEEE International Conference on Robotics and Automation(ICRA). Xi’an: IEEE, 2021: 11054-11059.

[6]	Dellenbach P, Deschaud J, Jacquet B, et al. CT-ICP: real-time elastic LiDAR odometry with loop closure[C]// 2022 International Conference on Robotics and Automation (ICRA). Philadelphia: ICRA, 2022: 5580-5586.

[7]	Vizzo I, Guadagnino T, Mersch B, et al. KISS-ICP: in defense of point-to-point ICP-simple, accurate, and robust registration if done the right way[J]. IEEE Robotics and Automation Letters, 2023, 8(2): 1029-1036.

[8]	Zhang J, Singh S. Low-drift and real-time lidar odometry and mapping[J]. Auton Robot, 2017, 41(2): 401-406.

[9]	Shan T, 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: IEEE, 2018: 4758-4765.

[10]	刘振宇, 惠泽宇, 郭旭, 等. 基于滑动窗口优化的激光雷达惯性测量单元紧耦合同时定位与建图算法[J]. 科学技术与工程, 2022, 22(21): 9167-9175.

[11]	Liu Zhenyu, Hui Zeyu, Guo Xu, et al. Tightly coupled simultaneous localization and mapping algorithm for LiDAR inertial measurement unit based on sliding window optimization[J]. Science Technology and Engineering, 2022, 22(21): 9167-9175.

[12]	姜文飞, 张小俊, 王金刚. 地下车库的雷达与相机融合即时定位与建图[J]. 科学技术与工程, 2022, 22(6): 2334-2342.

[13]	Jiang Wenfei, Zhang Xiaojun, Wang Jingang. Radar and camera fusion real-time positioning and mapping of underground garage[J]. Science Technology and Engineering, 2022, 22(6): 2334-2342.

[14]	Shan T, 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: IEEE, 2020: 5135-5142.

[15]	Xu W, Zhang F. FAST-LIO: a fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman kilter[J]. IEEE Robotics and Automation Letters, 2022, 6(2): 3317-3324.

[16]	Chen K, Nemiroff R, Lopez B. Direct LiDAR-inertial odometry: lightweight LIO with continuous-time motion correction[C]// 2023 IEEE International Conference on Robotics and Automation (ICRA). London: IEEE, 2023: 3983-3989.

[17]	Chen Z J, Xu Y, Yuan S H, et al. iG-LIO: an incremental GICP-based tightly-coupled LiDAR-inertial odometry[J]. IEEE Robotics and Automation Letters, 2024, 9(2): 1883-1890.

[18]	柴立平, 王维栋, 李晨阳, 等. 基于曲面特征的激光/IMU融合同步定位与建图方法[J]. 激光与光电子学进展, 2025, 62(4): 197-204.

[19]	Chai Liping, Wang Weidong, Li Chenyang, et al. Fusion SLAM method for LiDAR and IMU using surface features[J]. Laser & Optoelectronics Progress, 2025, 62(4): 197-204.

[20]	李兆强, 张岳, 熊福力, 等. 基于优化地面分割与回环检测的激光同时定位与建图算法[J]. 光学学报, 2024, 44(20): 203-215.

[21]	Li Zhaoqiang, Zhang Yue, Xiong Fuli, et al. Laser simultaneous location and mapping algorithm based on optimized ground segmentation and closed-loop detection[J]. Acta Optica Sinica, 2024, 44(20): 203-215.

[22]	唐浩, 黎东, 王成, 等. 基于图优化的激光SLAM点云整体配准方法[J]. 激光与光电子学进展, 2024, 61(10): 281-289.

[23]	Tang Hao, Li Dong, Wang Cheng, et al. Global registration method for laser SLAM point clouds based on graph optimization[J]. Laser & Optoelectronics Progress, 2024, 61(10): 281-289.