点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！
摘要：针对煤矿井下巷道、地下洞穴等结构退化场景中，基于激光－惯性紧耦合的同步定位与建图（Simultaneous Localization and Mapping，SLAM）系统，易因特征稀疏、几何约束不足出现配准失效、位姿漂移与建图一致性差等问题，本文提出一种面向井下退化环境的自适应特征与约束增强SLAM方法。该方法以多残差融合迭代误差状态卡尔曼滤波（Iterative Error State Kalman Filter，IESKF）紧耦合框架为核心，整合点平面几何、点云配准、特征分布及回环检测四类残差，构建 维观测方程与23维误差状态向量的全局约束体系，通过场景自适应噪声调整策略，实现多场景下的最优状态估计。前端提出双尺度梯度－熵值（Dual Scale Gradient Entropy，DSGE）特征提取方法，通过双尺度体素邻域联合分析结构梯度突变与空间分布熵值，筛选高区分度边缘与平面特征。同时依靠自适应初始化联合动态权重几何配准（Dynamic Weight Coupling Consistency Constrained Iterative Closest Point，DWCC-ICP）策略，融合IMU预积分与IESKF最优估计生成高质量初始位姿，结合动态权重模型提升配准鲁棒性。后端构建轻量化回环检测模块，基于XYZ－强度四维直方图构建全局描述子，高效修正长程累积漂移。系统通过“前端鲁棒感知－后端全局优化－闭环反馈修正”协同架构，将IMU零偏、LiDAR-IMU外参及最优位姿反馈至前端，实现全链路协同优化。在SubT-MRS公开数据集、自制教学楼长廊及山西樟村煤矿巷道的实验结果表明，相较于FAST-LIO2、FAST-LIO-LC、PV-LIO等主流算法，本文方法位置估计精度提升 ，姿态稳定性优化 ；长廊场景建图横向、纵向相对误差分别低至 与 ；消融实验表明，DSGE与DWCC-ICP模块在建图最小误差、平均误差、均方根误差及标准差上较基准算法分别提升 、 、 和 ，轻量化回环检测模块在最小误差与标准差上分别提升 、 ，多残差IESKF紧耦合框架在最小误差与平均误差上分别提升 、 ，各模块协同作用显著强化了系统在退化环境下的定位精度与运行稳定性。本文方法在极端退化环境下展现出更强的鲁棒性与工程实用性，有效提升了全局建图一致性，为煤矿井下无人开采装备的精准定位、三维地图构建与自主导航提供可靠技术支撑。
作者：金智新等人(太原理工大学 山西省煤矿智能装备工程研究中心)
1 引言
煤矿智能化以及煤炭产业数字化转型，是培育煤炭新质生产力、推动行业高质量发展的核心关键所在[1]。而同步定位与建图（Simultaneous Localization and Mapping，SLAM）技术通过同步构建环境地图与实时定位，为煤矿机器人提供厘米级空间基准和动态场景语义理解能力，是实现无人化开采装备协同调度、数字孪生系统虚实映射等的基础支撑[2]。然而在煤矿井下非结构退化环境中，结构单一、环境复杂以及光照条件差等难题，导致煤矿井下SLAM技术研究仍面临严峻挑战。激光雷达具有不受粉尘遮挡以及光照条件变化影响的特点，适用于井下低光照、高粉尘环境定位与建图[3]。
现有的激光雷达SLAM系统，在井下环境中面临着诸多难题。首先是环境几何结构退化问题突出，长直巷道、弱特征岩壁等场景下点云分布均匀，导致有效特征提取困难、配准约束信息不足。传统依赖局部几何特征的配准算法，易因特征误匹配破坏配准的几何一致性，进而引发显著的横向漂移[4]。再者，传感器自身退化带来的影响不容忽视，剧烈振动与温湿度波动等复杂工况会导致惯性测量单元（Inertial Measurement Unit，IMU）零偏产生时变漂移，加剧误差累积，而激光雷达（Light Detection and Ranging，LiDAR）与IMU外参也易受安装扰动与环境变化影响发生时变[5-6]，传统后端优化框架对这类时变特性进行静态建模，难以维持长时间尺度下的状态估计一致性。
激光SLAM技术的发展为井下定位建图提供了重要基础。经典激光SLAM以LOAM为奠基性工作[7]，为后续方法提供了“特征提取－运动补偿－帧到地图优化”的核心范式。其衍生版本A-LOAM简化优化流程提升实时性，LeGO-LOAM则通过高效的地面分割与特征提取，进一步优化了轻量化与地面适配能力。MULLS[9]提出扫描线无关的多度量线性最小二乘ICP算法，通过提取多类几何特征，在KITTI基准测试中实现顶尖的实时性与低漂移。SuMa[10]引入表面元地图表示，利用surfel的稳定性估计过滤动态干扰，支持大规模环境建模。SuMa++[11]则融合语义分割，通过语义约束优化与动态目标过滤，进一步提升了动态场景下的地图一致性。这些方法在城市道路、结构化室内等特征丰富环境中，均实现了厘米级建图精度与可靠位姿估计。
然而，这类基于激光雷达的SLAM方法均高度依赖于丰富的几何特征，在结构退化场景中暴露出显著局限性。为解决纯激光SLAM在剧烈运动、特征短暂缺失场景下的稳定性问题，激光-IMU紧耦合成为主流技术路线。该路线的核心是通过高频IMU数据补偿激光点云畸变，提升位姿估计的鲁棒性。根据状态估计核心框架的差异，可分为基于滤波和基于优化的两条技术主线。基于滤波的路线以卡尔曼滤波及其变体为核心，代表性成果为FAST-LIO系列。FAST-LIO系列算法[12-13]采用迭代扩展卡尔曼滤波（iEKF）框架，凭借高效的地图管理与滤波融合策略，成为嵌入式平台部署的优选方案。DLIO[14]作为退化环境适配的改进方法，采用“运动校正与先验生成一体化”的紧凑架构，其精度与计算效率均优于FAST-LIO2。此外，Super-LIO[15]作为滤波类紧耦合的轻量化拓展，提出紧凑八体素地图与启发式引导KNN搜索（HKNN），进一步完善了滤波类方法在实时性与鲁棒性上的平衡。基于优化的紧耦合路线以因子图优化为核心，代表性成果包括LIO-SAM、LIOM等。LIOM[16]作为优化类紧耦合的先驱工作，首次实现激光-IMU原始测量的联合优化，提出旋转约束地图精修策略，提升了全局一致性。LIO-SAM[17]进一步构建了基于因子图的紧耦合框架，将多重约束统一纳入后端优化，有效平衡了全局一致性与实时性。LIO-Fusion[18]基于因子图紧耦合框架融合多重定位及轮速计数据，通过全局与局部强化策略，实现LiDAR退化场景下高精度位姿估计。
尽管上述方法在常规结构化、半结构化环境中实现了精度与实时性的平衡，但在煤矿井下、长直隧道等极端结构退化场景中，仍面临特征稀疏、约束不足、漂移累积等问题。为突破这些局限，学术界围绕特征增强、配准优化、多约束融合等方向提出一系列改进方法。薛光辉等[3]提出了一种融合FPFH和ICP算法的二阶段点云特征提取及配准的方法，实现了非结构化退化环境的高精度全局一致性地图构建。Yang等[19]提出退化场景补偿的LiDAR-IMU紧耦合SLAM方法，基于滑动窗口因子图优化实现全局一致建图。薛光辉等[20]提出融合动态加权鲁棒主成分分析的特征提取方法，结合基于快速稳定三角形全局描述符的闭环检测有效克服了轨迹漂移问题。黄晨烜等[21]基于因子图构建非结构化状态约束松弛的里程计因子、UWB因子和回环因子，通过对全部因子进行并行全局优化，实现高精度的全局定位及建图。刘其鑫等[22]提出融合激光反射强度特征的SLAM算法，通过ORB算子提取强度图特征并跟踪匹配，丰富退化环境下特征数量。Shang等[23]构建三级子图架构，通过多子图融合与地图反馈机制，提升长距离退化场景下的建图一致性与实时性。Wang等[24]基于深度学习检测交叉口、室等显著区域实现回环检测，通过傅里叶-梅林变换估计初始位姿避免虚假回环。Fan等[25]提出高维多约束SLAM框架，通过YOLO检测与OCR识别隧道百米标实现回环检测，在地铁隧道建图中轨迹一致性得到提高。吴唯同等[26]基于IESKF框架融合激光雷达与毫米波雷达，在滤波更新阶段通过截断奇异值分解减轻激光定位退化带来的影响，提升异步传感器更新时的精度。李小波等[27]提出融合退化因子与置信度融合机制的EKF算法，在地图隧道及煤矿巷道中均实现了高精度的定位建图。Sun等[28]融合自适应强度特征提取与强度辅助进行回环检测，克服了现有方法对几何特征的依赖。Dong等[29]通过设计强度边缘上下文描述符优化回环检测，有效克服纯几何SLAM的约束不足问题。
然而，现有改进方法仍存在特征约束单一、场景适配性不足、时变特性建模不充分等问题，难以完全满足煤矿井下极端退化环境的定位建图需求。为此，本文提出一种面向煤矿井下结构退化环境的激光-IMU紧耦合SLAM方法，以融合四维残差与场景自适应噪声调整的IESKF紧耦合框架为核心，提出了双尺度梯度-熵值（DSGE）特征提取器，设计了自适应初始化联合动态权重的几何配准（DWCC-ICP）策略，构建轻量化回环检测模块与闭环反馈机制，实现不同退化场景下的最优状态估计；本文在公开数据集、自制长廊场景及真实煤矿井下巷道中开展系统性实验验证，结果表明所提方法相较于主流算法，在保持实时性的同时显著提升结构退化环境下的位姿估计精度与建图一致性，为煤矿井下无人化装备精准定位与场景建模提供可靠技术方案。
2 算法原理
2.1 系统概述
本文提出的激光-IMU紧耦合SLAM系统采用“前端鲁棒感知-后端全局优化-闭环反馈修正”协同架构如图1所示），实现了高精度位姿估计与全局一致地图构建。在前端里程计模块中，系统首先通过异步时间同步机制对齐LiDAR与IMU数据流，并基于IMU预积分结果采用前向传播机制消除点云运动畸变；随后，DSGE（双尺度梯度-熵值）特征提取器筛选高区分度的边缘与平面特征；最终通过DWCC-ICP自适应配准模块为后端提供高精度的位姿观测输入。
后端优化以多残差IESKF紧耦合框架为核心，构建 维观测方程与23维误差状态向量，通过迭代线性化与协方差传播实现状态最优估计。同时搭载轻量化回环检测模块，基于四维直方图构建全局描述子并快速检索闭环帧，以迭代最近点算法（Iterative Closest Point，ICP）验证生成残差修正累积漂移。
系统通过多通道反馈机制维持长期一致性，将IMU零偏、外参估计结果及最优位姿实时反馈至前端模块，有效提升在结构退化环境下的鲁棒性与定位精度。
2.2 前端里程计
2.2.1 数据预处理与时间同步
激光雷达与IMU的数据采集存在天然异步性，核心表现为采样频率差异与设备间时钟相位差及漂移，直接破坏数据时空一致性，需通过针对性时间同步策略解决。
同步过程采用以激光帧为基准的异步对齐机制。针对数据传输延迟引发的时间戳偏移问题，通过统计历史传输延迟均值对IMU时间戳进行预补偿修正；同时引入滑动窗口检测策略，剔除时间戳跳变及无效数据帧。若单帧内有效IMU数据少于3个，则标记该帧为低置信度，在后续滤波中增大协方差权重。
由于激光雷达单帧扫描存在一定时间跨度，期间传感器的平移和旋转运动会导致同一帧内不同时刻采集的点云产生拉伸、扭曲畸变。本文基于IMU预积分结果采用前向传播机制进行逐点补偿，如图2所示将每个激光点从采集时刻的传感器坐标系统一转换至帧起始时刻的坐标系，从而消除运动带来的畸变影响。具体流程如下：
首先基于同步后的IMU序列进行预积分计算，获取从帧起始时刻到任意点采集时刻的相对位姿变换。因LiDAR与IMU存在安装偏差，需先将原始点云从LiDAR系经外参转换至IMU坐标系，补偿完成后逆变换转回LiDAR系。随后在IMU坐标系下，利用相对位姿将原始点云投影至起始时刻坐标系，再经过逆外参变换得到LiDAR坐标系下的无畸变点。
2.2.2 双尺度梯度及熵值特征提取
双尺度梯度-熵值（DSGE）特征提取的核心目标是从去畸变后的三维点云中，自适应捕获不同空间尺度下的局部几何结构突变与分布无序性，生成兼具区分性与抗干扰性的高维特征描述子，为后续几何配准、紧耦合位姿优化提供可靠的几何语义支撑。
（1）邻域构建
点云的邻域构建作为特征提取的基础，效率与精度至关重要。因此DSGE特征提取前，本文采用体素哈希索引与半径球查询的混合策略进行邻域构建，如图3所示。
体素哈希索引的核心是通过“空间离散化 + 哈希映射”实现目标点的快速定位。空间离散化是指将三维点云空间按固定大小的体素划分，每个体素对应一个空间分区。其中体素大小的选择影响到了特征提取的效率，体素过小则体素数量激增，哈希表开销增大，体素过大则分区内点过多，失去筛选意义。为了选择适中的体素大小，本文采用长直走廊数据进行了不同体素下的对比实验，表1展现了不同体素下的单帧体素数量、特征提取耗时以及邻域查询准确率。
基于实验结果以及实时性准确性需求，本文选择 体素尺寸进行空间离散化，并为每个体素分配唯一哈希键并构建哈希表，表中存储每个体素内的点索引。通过哈希键可 时间复杂度定位目标体素，避免遍历全部点云。
在此基础上，采用半径球查询进行精筛，以目标点为球心，设置适配双尺度特征提取的邻域半径，筛选球内所有点作为最终邻域点集。
（2）双尺度梯度计算
双尺度梯度的核心是通过细尺度保留局部精细几何结构，精准刻画边缘突变强度与平面平缓程度，通过粗尺度平滑传感器噪声并捕捉全局空间趋势，结合协方差矩阵特征值分解实现几何结构量化。本文通过在多场景中进行多次实验，根据建图精度以及计算效率，选用 细尺度以及 粗尺度。双尺度梯度计算具体步骤如下：
（2.1）为构建双尺度表征，先对去畸变点云执行双尺度体素下采样。
（2.2）基于前期体素哈希索引，快速定位目标点所在体素及邻域体素，初步筛选候选点；再以目标点为球心、邻域半径为半径执行球查询，为避免冗余，最终保留最多20个邻域点，形成局部邻域点集 。
对细尺度点云的邻域点集 ，计算3D协方差矩阵 ，刻画局部点云的空间分布特征：
其中 为目标点 的局部邻域点集， 为邻域点数量（需满足 以保证矩阵非奇异）； 为邻域点集的中心点坐标； 为邻域内的单个点； 为点 相对于中心点的中心化向量。
（2.3）对协方差矩阵 执行特征值分解，得到3个非负特征值 。
边缘结构： （一个特征值主导）
平面结构： （两个特征值主导）
无序结构： （三个特征值主导）
细尺度梯度 为最大特征值与中间特征值的比值，量化局部结构突变程度：
其中当 时，由于分母过小可能导致数值过大所引发的计算崩溃等问题，因此给一个固定值，但基本上不会出现此类情况。
（2.4）双尺度梯度融合粗尺度梯度 和上述计算步骤一致，最终双尺度梯度通过加权求和：
其中， 为融合后的最终梯度值， 和 分别为细尺度梯度和粗尺度梯度的权重值。
（3）局部信息熵计算
局部信息熵量化邻域点分布的无序性，与梯度特征形成互补，二者协同实现复杂场景下的特征区分。
（3.1）邻域距离统计
对邻域点集 ，计算每个邻域点 与 的欧氏距离 ，统计距离的最小值、最大值，得到距离范围 ，若 说明邻域点高度集中，熵值设为0。
（3.2）构建距离分布直方图
将距离范围 均匀划分为 个区间，统计每个区间内的距离数量，形成距离分布直方图 ，其中 为第 个区间的距离计数。
（3.3）信息熵计算与归一化
基于直方图 计算香农信息熵，量化距离分布的无序性：
其中， 为第 个区间的概率。
为使熵值在统一尺度下可比，采用理论最大熵进行归一化：
其中， 为原始信息熵值； 为归一化后的信息熵值。
归一化后， 代表邻域点高度有序，即平面和边缘结构， 代表邻域点杂乱分布，即噪声以及结构退化区域。
结合双尺度梯度 与归一化熵值 设置自适应阈值实现特征分类。当 且 ，对应局部结构突变强、分布有序的线状边缘特征。当 且 对应局部结构平缓、分布有序的平面特征。当 ，判定为噪声或结构退化区域，直接剔除。其中 作为边缘特征梯度阈值取25-35之间，在巷道长廊等结构退化及壁面光滑场景中，该范围阈值可有效筛选高质量边缘特征。平面特征阈值 取1.5以下，该阈值内的梯度值极低平面特征质量高，熵阈值 则取 ， 为阈值松弛量用以放宽阈值约束，本文中两个阈值松弛量分别取6及0.3。
为了验证DSGE特征提取方法的有效性，选择了曲率法，主成分分析方法（Principal Component Analysis，PCA）以及固有形状特征签名方法（Intrinsic Shape Signatures，ISS）进行对比。四种方法的点云地图如图4所示，指标量化结果如表2所列。在地下洞穴场景中，本文方法和PCA方法没有出现明显的漂移以及结构失真等问题；ISS方法存在局部的轨迹漂移，导致了部分结构错位，如红圈内的部分；曲率方法出现了明显的漂移情况，建图精度大大降低。而在室内长廊场景下，基于曲率的特征提取方法因整体结构彻底失真而建图失败；ISS方法存在建图精度不足的问题，在图中表现为电梯房建模失真以及右侧走廊倾斜；PCA方法虽能完成建图，但单帧提取耗时长达 ，且也存在右侧长廊墙壁倾斜导致的精度不足的问题。表3给出了DSGE提取的边缘与平面特征点梯度信息，其中边缘特征点梯度值达315.3，平面特征点仅为0.997，二者差异显著，充分印证了DSGE方法提取特征点的可靠性与有效性。
2.2.3 自适应初始化几何配准
几何配准是前端里程计的核心环节，其精度直接决定位姿估计的可靠性。针对结构退化环境下传统配准易因初始值偏差导致发散、几何约束不足导致配准漂移的问题，本文提出自适应初始化 动态权重几何配准（DWCC-ICP）策略，通过多源初始位姿优化、几何一致性动态权重两大核心设计，实现鲁棒高效的点云配准，为后端IESKF提供高精度位姿观测。
（1）自适应初始位姿优化
传统配准多采用单位矩阵或单一传感器提供初始位姿，易因运动剧烈或传感器噪声导致初始偏差过大。本文提出多源信息融合的自适应初始位姿生成方法，融合IMU预积分位姿与后端输出的最优位姿，通过姿态插值与位置平滑优化初始估计。
通过IMU预积分，获取到当前帧的姿态 和位置 ，提供高频运动先验。同时记录上一帧后端输出的位姿 ， 及对应的时间戳 ，提供配准一致性约束。
姿态作为SO(3)流形上的元素，采用球面线性插值（SLERP）避免欧式插值导致的姿态失真，插值公式为：
其中 为插值后的姿态信息， 为时间自适应权重，由当前帧与上一帧的时间差 动态计算， ， 为时间常数，确保权重不极端。
位置采用加权融合策略并添加异常值检测避免运动突变导致的偏差：
若 ，则调整 ，优先信任历史配准结果，避免初始位置跳变。
（2）动态权重几何配准
基于DSGE提取的边缘、平面特征，设计动态权重配准模型，融合距离一致性、特征类型、几何结构三重约束，为不同特征点对分配自适应权重，提升配准精度。以DSGE提取的边缘与平面特征作为源特征点云 ，以局部地图特征作为目标点云 ，以两者的加权距离最小化为目标求解变换矩阵 ：
其中 为优化目标，旨在 空间中寻找变换 ，使得后续求和项最小； 为源点 经过变换 后的坐标； 为残差向量； 为第 个源点； 为第 个目标点； 为第 个匹配点对的权重系数。
动态权重 融合距离因子、特征因子、几何因子三重约束，实现多维度自适应调整：
其中 分别为对应因子的权重系数； 为距离因子，是一种基于匹配点对欧式距离的高斯衰减函数，用以抑制远距离误匹配； 为特征因子，给予边缘点更高的权重，用以强化结构约束； 为几何因子，基于曲率一致性与法向量一致性，筛选几何相似的匹配点对。
最后采用高斯-牛顿法迭代求解变换矩阵 ，每次迭代通过SVD分解最小二乘问题更新位姿。为平衡配准精度与实时性，采用双重收敛判定条件。位姿增量收敛：平移增量 ，旋转增量 ；残差收敛：加权残差均值 ， 为匹配点对距离。满足任意条件即停止迭代，输出最终配准位姿 ，作为前端里程计的位姿估计结果，用于后端IESKF更新。
为验证本文配准方法的效果，采用ICP配准方法进行对比，结果见图5。可以看到，ICP算法未能有效建立可靠的点云对应关系，导致帧间累积漂移不断叠加，前后帧点云无法精准对齐，最终出现显著的墙体重复建模、几何形态失真的现象。而本文算法通过多源位姿先验融合生成高质量初始猜测，结合动态权重模型强化有效匹配对的贡献，有效抑制误匹配与漂移累积。
2.3 后端优化
2.3.1 轻量化回环检测
长距离行驶中，前端里程计的累计漂移会严重破坏地图一致性与位姿估计精度，尤其在结构退化环境中，单一帧间约束难以抑制漂移累积。回环检测通过识别已遍历场景并构建全局约束，成为修正漂移的核心手段。本文提出关键帧 + 低维全局特征 + ICP验证的轻量化回环检测方案，强化结构退化环境下的闭环鲁棒性，为后端多残差IESKF提供可靠的全局约束。
（1）关键帧筛选
关键帧筛选的核心目标是在保证场景代表性的前提下最小化帧库规模，为后续闭环检测提供可靠的匹配基础。如图6所示，针对结构退化环境中运动模式单一、场景重复度高的特点，筛选策略基于“运动增量双阈值 + 滑动窗口帧库管理”设计，平衡轻量化与鲁棒性。
关键帧的判定基于当前帧与上一关键帧的位姿变化量，采用“平移增量 + 旋转增量”的双阈值逻辑。平移阈值 根据采集设备每分钟移动速度 动态确定，取 ，确保关键帧间距与运动速度自适应匹配。本文中采集设备由人手持，行进速度约为 ， 取 ，同时旋转阈值 取30度。满足以上任一条件即判定为关键帧，且仅保留最近50个关键帧，当新关键帧加入导致帧库超限时，自动剔除最早的非闭环关联关键帧。
（2）低维全局特征提取
为平衡匹配效率与鲁棒性，采用XYZ + 强度4维全局直方图作为关键帧特征描述子，该特征维度低、计算快，且对结构退化环境的低特征密度场景具有强适应性。
首先统计关键帧点云在XYZ和强度维度的取值范围；对每个维度均匀划分为若干个分箱（bin，即离散化的统计区间），统计每个bin内的点云数量，形成4维 × bin数的特征直方图；对直方图进行归一化处理，消除点云密度差异的影响。
（3）闭环验证与残差生成
闭环候选帧搜索阶段，基于全局特征的相似性度量实现高效筛选，采用直方图余弦距离作为相似度评价指标。设当前关键帧特征直方图为 ，候选关键帧特征直方图为 ，余弦距离定义为：
余弦距离 的取值范围为 ，值越小表明两帧的特征相似度越高。通过设定距离阈值，筛选距离小于该阈值的候选帧，若存在多个符合条件的候选帧，则选择距离最小的帧作为最优候选。为解决特征匹配可能引入的虚假闭环问题，需通过ICP配准进行二次验证，进一步校验位姿一致性。配准参数按如下配置：最大迭代次数50次，收敛阈值 ，配准残差阈值 。若ICP配准过程收敛，且最终输出的配准残差小于设定阈值，则判定为有效闭环；反之，则判定为虚假闭环并予以剔除。
回环残差通过计算当前帧与闭环帧的相对位姿差并转换为6维李代数形式构建，随后接入后端多残差IESKF紧耦合框架完成优化，实现轨迹漂移校正。
2.3.2 多残差IESKF紧耦合框架
激光-IMU紧耦合的核心挑战在于结构退化环境下单一约束易失效、线性化误差累积及多源信息可靠性的动态变化。本文提出多残差融合的迭代误差状态扩展卡尔曼滤波（IESKF）紧耦合框架。以IMU预积分作为过程模型完成状态预测，在观测更新步通过误差状态建模降低非线性影响，整合点平面几何残差、点云配准残差、特征分布残差及回环残差4类残差形成多维度约束，结合场景自适应噪声调整策略，实现结构退化环境下的高精度、鲁棒性状态估计，为SLAM系统输出一致的位姿、速度及传感器零偏信息。
图7展现了紧耦合框架的结构图，框架以“误差状态迭代优化 + 多残差互补约束 + 场景自适应调整”为核心，设计遵循“状态分层建模 → 多残差统一融合 → 自适应权重调整 → 迭代误差修正”的逻辑，兼顾估计精度、实时性与鲁棒性。
IESKF的核心优势在于通过名义状态结合误差状态的分层建模，将非线性系统优化转化为误差状态的线性更新，将线性化误差降至二阶小量。
名义状态向量定义系统名义状态 ，包含传感器核心状态与外参等24维数据，直接表征系统真实状态：
其中 为机体系相对于导航系的姿态旋转矩阵； 为导航系下的位置向量； 为导航系下的速度向量； 为陀螺与加速度计零偏； 为导航系下的重力向量； 为LiDAR与IMU的外参。
误差状态向量定义误差状态 ，表征名义状态与真实状态的偏差，用于线性化更新：
其中， 为 旋转群的李代数，表示为IMU、LiDAR坐标系相对于世界坐标系的横滚、俯仰、偏航角度估计偏差； 为IMU坐标系在世界系下的位置估计偏差和线速度估计偏差； 为陀螺及加速度计零偏误差； 为世界系下重力向量 的估计偏差； 为IMU、LiDAR的外参误差。
需要注意的是，误差状态的重力向量误差定义在二维切空间上，故而只有两维，因此误差状态维数要比名义状态少一维。
该框架整合4类残差形成全局观测约束，所有残差统一映射至误差状态空间，构建观测方程 （其中 为堆叠残差， 为全局雅可比矩阵， 为误差状态， 为零均值高斯噪声），各残差细节如表4所示。全局雅可比矩阵由各残差的雅可比矩阵分块堆叠而成：
四类残差的基础权重通过噪声协方差矩阵设计实现，定义有效权重：
其中， 为第 类残差实际参与IESKF优化的最终权重，决定该残差对状态估计的贡献程度； 为第 类残差的基础权重系数，反映该类残差在系统中的基准重要性； 为第 类残差的方差，由滑动窗口进行估计，当方差较小时最终权重较大，在IESKF优化时更加信任该残差； 为滑动窗口大小； 为第 类残差在第 的观测值。四类残差的基础权重如表5所示。
为适配结构退化、转弯等不同场景下残差可靠性的动态变化，设计噪声协方差自适应调整策略。当处于结构退化场景时，降低特征分布残差协方差以及回环残差协方差至原来的二分之一，从而强化横向约束，提升全局约束权重，抑制单一约束漂移。当处于转弯场景时，降低IMU过程噪声，同时调整点平面几何协方差，状态降低至原来的 倍 倍。当处于无回环场景时，设回环残差的噪声协方差矩阵 （ 为6阶单位矩阵），使其权重趋近于0，避免虚假约束干扰。
IESKF通过迭代线性化与误差状态修正，逐步降低非线性系统的线性化误差，核心流程如下：
（1）基于IMU预积分结果预测名义状态 与误差状态协方差 进行状态预测：
其中 为状态转移函数，表征从 时刻的系统状态，结合 时刻的IMU输入，推导 时刻预测状态的物理规律； 为 时刻IMU的输入； 为误差状态转移矩阵； 为噪声输入矩阵； 为IMU过程噪声协方差矩阵。
（2）基于预测的名义状态 ，多源观测信息通过数值微分或解析法计算对应雅可比矩阵。
（3）以残差最小化为目标，通过多次循环完成误差状态修正、名义状态更新与协方差更新，迭代次数通常设为3~5次。为平衡估计精度与实时性，设置双重收敛判定条件，当全局残差向量的L2范数小于所设定的阈值或误差状态向量小于所设定的阈值时，终止迭代。
（4）为保证状态估计的物理合理性，对更新后的重力向量进行归一化处理，并限制IMU零偏与LiDAR-IMU外参在物理合理范围，保证状态估计稳定收敛。
针对长直巷道、弱纹理等结构退化场景，额外设计残差一致性检测和外参在线优化两项强化策略。通过滑动窗口统计各残差的方差，若某残差方差异常突增超过其历史均值两倍时，则临时降低其权重至原值的 ，避免异常残差主导估计结果；将LiDAR与IMU的外参纳入误差状态，通过点平面几何残差与点云配准残差联合优化，补偿离线标定误差。
后端输出的IMU零偏、LiDAR-IMU外参及最优位姿三类优化结果反哺至前端IMU预积分和DWCC-ICP环节，用以提升系统在井下极端退化环境的整体鲁棒性和全局建图一致性。
3 实验结果与讨论
为评估所提算法的精度与鲁棒性，选取SubT-MRS数据集的FinalChallenge_UGV1序列开展验证实验。该序列采集于地下洞穴及狭长通道复合场景，存在结构重复、特征稀疏、粉尘模拟噪声等典型退化特性，可全面检验算法在非结构化环境下的位姿估计精度与全局一致性。同时自制数据集使用上海炙微科技有限公司研发的EDU-SCAN三维激光雷达视觉一体化扫描仪在学校教学楼走廊采集。
所有实验均在Dell Precision 3551便携式工作站上运行，硬件配置为：Intel Core i7-10875H处理器（主频2.30GHz，8核16线程），运行内存32GB。算法在Ubuntu20.04系统上基于 实现，采用OpenMP多线程并行加速，实验中固定启用3个并行线程以保证实时性与稳定性。本文所有参数均基于地下洞穴、室内走廊、煤矿巷道等多场景数据联合调试确定，充分适配不同场景的几何结构与噪声特性，使所提方法在各类退化环境下具备一定的跨场景泛化性与鲁棒性。
3.1 公开数据集验证与分析
采用FAST-LIO2、FAST-LIO-LC以及PV-LIO算法与本文算法对FinalChallenge_UGV1序列的激光雷达数据进行轨迹精度分析，利用evo工具得到了两种算法的轨迹图以及 、 、 三轴的位置偏差与横滚角、俯仰角和航向角的姿态偏差，结果见图8与表6。
结合实验可视化结果与量化数据可知，以FAST-LIO2为框架进行改进的本文算法，在X、Y、Z轴上的误差分别为 ， ， ，相较于FAST-LIO2的 ， ， ，分别实现 ， ， 的精度提升。相较于FAST-LIO-LC与PV-LIO，本文算法除了在Z轴方向平均误差较大外，X、Y轴的平均误差均优于两种算法。姿态平均误差层面，本文算法的横滚角（ ）和俯仰角（ ）较FAST-LIO2的 ， 分别优化 ， ，仅航向角误差（ ）略高于FAST-LIO2的 。相比于FAST-LIO-LC和PV-LIO，本文算法虽然在横滚角以及俯仰角平均误差上略差一些，但航向角误差（ ）要远小于PV-LIO的 和FAST-LIO-LC的 。
图9与图10分别展示了对比算法与本文算法的绝对位姿误差与相对位姿误差，计算间隔为1帧，量化数据见表7。
由实验数据可知，与FAST-LIO2相比，本文算法在绝对位姿误差（APE）中的最小误差、平均误差、均方根误差和标准差上分别降低了 ， ， 和 ；APE最大误差略有升高（从 升至 ），此变化主要受极端场景下单一帧位姿波动影响，不影响本文算法的整体精度优势；与其他两个方法相比，本文算法在绝对位姿误差上都取得了不错的优势。相对位姿误差（RPE）方面，本文算法与FAST-LIO2的各项指标总体接近，两者在最大误差、最小误差及平均误差等维度无显著差异，表明本文算法在局部运动估计精度上保持了与对比算法相当的水平。与FAST-LIO-LC以及PV-LIO相比，除却最大误差（ ）低于FAST-LIO-LC的 外，其他指标均要优于这两种算法。
为验证各模块的有效性，本文针对DSGE特征提取与DWCC-ICP配准模块、回环检测模块以及多残差IESKF紧耦合框架开展消融实验。由于本文算法基于FAST-LIO2框架实现，且在绝对位姿误差上具有显著优化效果，因此选取FAST-LIO2作为基准方法，以绝对位姿误差为评价指标完成消融实验，实验结果如表8所示。
由表8可见，DSGE+DWCC-ICP特征与配准策略在最小误差、平均误差、均方根误差及标准差上较FAST-LIO2分别提升 ， ， 和 ；轻量化回环检测模块在最小误差与标准差上分别提升 ， ；多残差IESKF紧耦合框架对最大误差与均方根误差的优化效果不明显，但在最小误差与平均误差上分别提升 和 。
消融实验结果表明，本文所设计的各模块均能有效提升系统定位精度与稳定性，验证了所提方案的设计合理性与整体优越性。
3.2 自制数据集验证与分析
为了验证本文算法在退化环境中的鲁棒性，选取了学校教学楼走廊作为实验场景。图11(b) 展示了走廊的真实环境，该场景包括行人、重复的门框、重复的梁、弱纹理及光照条件差等典型退化情况。实验采用AVIA激光雷达以回扫方式采集数据，雷达测距精度为 （ ），具体信息和实验设备如图11(a)所示。
采集场景尺寸图见图12，其中两处红色圆点处为急拐弯地点，该地区角度变化大，单一传感器易失效。尤其是2处需要进行360度转身进行回扫，在短时间内角度变化大于180度，极其考验算法在此情况下的鲁棒性。
基于自制数据集，选用FAST-LIO2、FAST-LIO-LC、PV-LIO以及本文算法进行点云地图建图效果测试。为保证对比实验的公平性，本文采用点云降采样策略，将各算法的单帧输入点云数量统一控制在5000-6000范围内。图13以及图14展示了四种方法的建图效果，可以看出FAST-LIO2在2处由于大角度变化导致回扫时约束不足从而建图失败。而本文算法、PV-LIO以及加入回环检测的FAST-LIO-LC算法则能在保证不漂移的情况下建图成功。从局部放大图图14进一步分析（因FAST-LIO2在大角度转向场景下建图失败，后续局部分析不再纳入）：在电梯井区域，FAST-LIO-LC虽引入回环检测，但受限于结构退化场景下约束信息不足、回环描述子区分度有限等问题，仍出现墙面重复建模的现象；而PV-LIO与本文算法均实现了精准的几何建模，无冗余结构与边界错位。在阳台区域，三种算法的整体建图效果相似，但细节还原度存在差异，PV-LIO在电路管道等细节建图上要略逊色于其余两种算法。
为量化评估建图精度与建图效率两大核心性能指标，将三种算法的建图结果导出为PCD格式并导入CloudCompare软件，从横向尺寸、横向误差、横向相对误差、纵向尺寸、纵向误差、纵向相对误差及耗时共七个维度开展量化对比分析。从表9可以看出，在横向尺寸上本文算法实现了厘米级精度，相对误差为 ，优于PV-LIO的 以及FAST-LIO-LC的 。在纵向误差上，本文算法与PV-LIO接近，领先于FAST-LIO-LC的 。而其中FAST-LIO-LC算法纵向相对误差过大，主要是由于在2处角度短时间内变化过大导致回扫时约束不足、回环检测失效等问题引起的。值得注意的是本文算法在可接受计算开销提升精度的前提下，单帧耗时要比PV-LIO低 。同时在表10中列出了各模块的平均耗时，可以看出DWCC-ICP耗时最高，达到了16.52毫秒，而轻量化回环检测的轻量化特性得到了印证，只有0.93毫秒的耗时。
3.3 巷道真实环境验证
为验证所提算法在煤矿井下真实退化环境中的可靠性与实用性，本文在山西樟村煤矿开展实地数据采集，并与FAST-LIO2、FAST-LIO-LC及PV-LIO算法进行建图效果对比，实验结果如图15所示。由图可见，在井下弱纹理、粉尘干扰及结构重复的复合退化场景下，FAST-LIO2出现明显的轨迹漂移，导致点云地图结构错位、全局一致性严重破坏；而本文算法、FAST-LIO-LC与PV-LIO均能稳定输出结构完整、无明显失真与漂移的全局一致点云地图。
为进一步对比各算法的建图精细度，图16展示了局部区域的建图细节。可以看出，FAST-LIO2在二次扫描区域因几何约束不足出现轨迹跳变，巷道壁面发生重复建模与异常旋转，严重影响地图精度。FAST-LIO-LC虽借助回环检测缓解了部分漂移，但仍存在轻微轨迹偏移，导致局部建模精度不足。相比之下，PV-LIO与本文算法均未出现约束失效与轨迹漂移问题，巷道轮廓规整、结构还原精准，充分验证了本文算法在煤矿井下极端退化环境中具备更强的鲁棒性与更高的建图可靠性。
4 结论
本文针对结构退化环境中激光-IMU紧耦合SLAM面临的特征稀疏、噪声干扰及累积漂移等核心问题，提出了一套以多残差IESKF紧耦合状态估计框架为核心的完整解决方案。通过理论建模与工程实现的深度结合，系统验证了算法在复杂退化场景下的高精度与高鲁棒性，主要结论如下：
（1）前端里程计通过DSGE特征提取与DWCC-ICP几何配准的协同设计，有效突破了结构退化环境的特征瓶颈。DSGE特征提取采用体素哈希 + 半径球邻域构建策略，结合双尺度梯度与局部信息熵量化，实现抗噪性边缘与平面特征筛选；DWCC-ICP配准融合IMU预积分与后端输出最优的位姿优化初始值，通过动态权重提升配准稳定性，为后端提供高精度帧间位姿观测。
（2）后端多残差IESKF紧耦合框架整合点平面几何、点云配准、特征分布及回环四类残差，结合场景自适应噪声调整策略，显著降低了单一约束失效风险。轻量化回环检测通过关键帧双阈值筛选、构建基于XYZ-强度四维直方图的全局描述子，有效修正长距离行驶后的累积漂移。
（3）闭环反馈机制通过“前端鲁棒感知-后端全局优化-闭环反馈修正”协同架构，将IMU零偏、LiDAR-IMU外参及最优位姿反馈至前端，实现全链路协同优化。实验验证表明，算法在SubT-MRS公开数据集、教学楼走廊自制数据集及山西樟村煤矿真实巷道环境中，整体效果优于FAST-LIO2、FAST-LIO-LC等对比算法，位置精度提升 ，姿态稳定性优化 ，长廊场景建图横向、纵向相对误差分别低至 与 ，有效缓解了退化场景下的建图失效问题。
本文仅做学术分享，如有侵权，请联系删文。
。
添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。
