# GIF-LIO：融合几何与强度的激光雷达-惯性里程计及退化约束状态估计
姚鹤凯 $^{ID}$、张雪涛 $^{ID}$（IEEE会员）、孙刚 $^{ID}$（IEEE研究生会员）、刘一莎 $^{ID}$（IEEE会员）、庄燕 $^{ID}$（IEEE高级会员）
摘要—在长走廊、隧道和开阔广场等感知环境复杂的场景中，鲁棒且精准的激光雷达里程计对于实现自主机器人的可靠导航与建图至关重要。本文提出了一种新型的融合几何与强度的激光雷达-惯性里程计（LIO）框架，能够在几何退化环境中实现精准定位。与现有方法不同，该方法的核心思路是将能够感知冗余退化的几何测量数据与互补的强度测量数据相融合，从而构建退化优化约束。这能确保系统状态沿信息量更高的方向更新，进而提升定位精度与系统鲁棒性。具体而言，本文提出了新型几何测量模型，用于减少扫描点与地图匹配中的冗余，并指示用于状态优化的非退化方向。此外，还利用概率自适应体素地图来获取更精准的几何测量结果与退化检测结果。当上述几何测量数据与互补的强度测量数据均可用时，便通过受限迭代扩展卡尔曼滤波器（CIEKF）进行联合更新，从而得到状态估计结果。通过在公开数据集上开展的对比实验表明，所提出的里程计在定位精度与地图一致性方面表现优异，尤其是在长隧道和平坦场地等退化环境中。
索引词—受限状态估计、几何-强度融合、激光雷达-惯性里程计（LIO）。
## 一、引言
激光雷达-惯性里程计（LIO）是当代机器人应用中导航与定位的核心技术 [^1]、[^2]、[^3]、[^4]、[^5]、[^6]、[^7]。然而，在具有结构自相似性或几何特征稀疏性的几何退化环境中，比如长隧道或开阔地形中，这些系统存在严重的脆弱性。此类环境中的退化几何测量会导致状态估计的累积漂移，甚至造成完全定位失败。因此，针对这类环境设计更具鲁棒性的LIO至关重要。
现有的抗退化LIO系统通常分为两类：1）能感知几何退化的优化方法 [^8]、[^9]、[^10]、[^11]，这类方法能够检测几何退化现象，并针对无信息方向构建状态更新的退化优化约束；2）基于激光雷达强度的信息增强方法 [^12]、[^13]、[^14]，激光雷达强度测量模型可为鲁棒的状态优化提供互补约束。不过，在具有挑战性的退化环境中，仍存在以下问题。
1) 增强强度的激光惯性里程计（LIO）系统[^12]、[^13]、[^14]通常具有较强的鲁棒性，它们能够整合互补的激光雷达强度信息，以缓解几何测量中可能出现的退化问题。然而，这类方法往往存在几何测量不精确的问题，从而限制了几何退化检测的质量。此类局限性会削弱强度信息的有效性，最终降低系统的鲁棒性和定位精度。
2) 对于能够感知几何退化的优化方法而言，关键步骤在于获取几何测量数据，并在状态估计中构建退化优化约束。但当前系统中的几何测量数据可能包含冗余且不准确的测量值，这会削弱这些约束的有效性。同时，所构建的优化约束也无法无缝应用于迭代扩展卡尔曼滤波（IEKF）里程计框架中。
鉴于上述问题，我们提出了一种新型的几何-强度融合LIO框架，该框架能够在几何退化环境中实现鲁棒且精准的定位。值得注意的是，信息精炼模块中提出的新型几何测量模型旨在减少扫描匹配过程中的冗余与不准确现象，同时还能为状态估计指明非退化方向。与此同时，信息增强模块能够选取沿退化方向的激光雷达强度图像特征，以构建互补的强度光度测量数据。
---
此外，我们采用了概率自适应体素映射框架，以实现具备不确定性感知能力的环境表征，从而获得更精确的扫描匹配几何测量结果与退化检测结果。主要贡献总结如下。
1) 我们提出了一种新型的几何-强度融合LIO框架，用于在退化环境中实现精准且鲁棒的定位。具体而言，信息精炼模块能够提供无冗余的几何测量数据并构建优化约束；信息增强模块则能在退化方向上提供互补的强度光度测量数据。此外，我们还利用概率体素图来提高几何测量与退化检测的准确性。
2) 我们提出了一种能够感知冗余与退化的几何测量模型，以减少扫描匹配过程中的冗余现象并构建优化约束。具体而言，借助经过筛选的激光雷达点云与退化方向信息，我们为扫描匹配残差设计了蒂霍诺夫正则化项，并利用该项对应的海森矩阵，在受限迭代扩展卡尔曼滤波（CIEKF）的联合更新过程中指示非退化方向。
3) 我们在公开数据集上对所提系统进行了评估，结果表明其性能显著优于现有最先进的方法。此外，在Enwide数据集上进行的消融实验证明，信息精炼模块确实能够提升退化环境中的定位精度。
本文的其余部分安排如下：第二节介绍了相关研究；第三和第四节分别详细阐述了所提出的框架与方法；第五节展示了实验结果与分析；第六节对全文进行总结。
### II. 相关工作
当前的LIO方法大致可分为两类：基于深度学习的方法和传统方法。借助数据驱动的范式，基于学习的方法能够提升在退化环境中的鲁棒性，尤其是在特征提取与融合方面。然而，这些方法通常涉及复杂的神经网络，导致计算复杂度高且内存消耗大。因此，它们往往无法满足工业和机器人导航应用中严格的实时性要求 [^15], [^16]。正因如此，现有方法仍严重依赖人工设计的几何特征和约束。相应地，本研究继续沿循传统的技术路线。
### A. 激光雷达-惯性里程计
点云配准是LIO系统的关键步骤。基于特征的方法是经典算法，例如LOAM [^17]，它通过提取点云中的边缘和平面来进行配准；LEGO-LOAM [^18]则通过引入地面分割来改进LOAM。不过，仅依靠几何特征提取与匹配的位姿估计在复杂环境或剧烈运动场景下容易失效。
将惯性测量单元（IMU）数据融入基于激光雷达的里程计中，能够提升定位的鲁棒性和精度。LIO-SAM [^19]是一种常用的基于优化的方法，它在因子图中利用激光雷达里程计因子和IMU预积分因子来优化系统状态 [^20]。不过，这类方法耗时较长。因此，基于滤波的方法愈发受到青睐。FAST-LIO2 [^1]重新定义了卡尔曼增益，并利用原始激光雷达点进行配准，从而实现了业界领先的定位精度和实时性能。类似地，Point-LIO [^21]提出了一种逐点处理的LIO框架，以提高里程计输出速率并增强在剧烈运动场景下的鲁棒性。为了实现更精准的点云配准，VoxelMAP [^22]设计了一种用于环境建模的新型概率体素地图。尽管通过与IMU数据融合提升了这些方法的鲁棒性，但由于依赖具有固有几何退化特性的激光雷达测量数据，激光雷达-惯性系统仍会出现漂移或失效现象。
### B. 抗几何退化里程计
为解决基于激光雷达的里程计系统的几何退化问题，人们提出了许多方法。视觉及其他传感器数据的融合能够提升SLAM系统的鲁棒性 [^23], [^24], [^25], [^26]。不过，这些方法需要额外的传感器配置，会增加系统的复杂性。
对于LIO系统而言，采用几何退化感知优化方法或基于激光雷达强度的信息增强方法能够提升系统的鲁棒性。退化检测与约束构建是几何退化感知优化的关键步骤[^11]。Zhang等人[^8]提出了退化因子与解映射方法，用以约束状态优化过程。在此基础上，Turcan等人[^9]提出了X-ICP算法，利用源点云与目标点云之间的对应关系来检测退化现象，同时沿退化方向添加硬约束以降低位姿估计误差。然而，不精确的几何配准限制了退化检测与约束构建的效果。此外，在某些极端退化条件下，仅依靠几何信息得出的约束并不充分。因此，融合激光雷达强度信息[^12]、[^13]、[^14]是一种可行的方法。不过，这些方法通常存在环境表征与几何测量不准确的问题，从而影响了几何退化检测的质量。此类局限进一步削弱了强度信息的效用，最终降低了系统的鲁棒性与定位精度。
尽管CoinLIO中的强度信息融合策略[^14]以及概率体素图的更新机制[^22]会带来额外的计算开销，但它们能在保证实时性的同时提升退化环境下的定位精度。受此启发，本文提出了一种新型的几何-强度融合LIO框架，该框架既能在几何退化环境中实现精准定位，又能维持实时性能。
---
通过融合冗余-退化感知的几何测量数据与互补的强度测量数据，可构建出退化优化约束。此外，利用概率自适应体素图来实现更精确的几何测量与退化检测。
### III. 系统概述
图1展示了所提出的几何-强度融合LIO框架，该框架旨在提升几何退化环境下的鲁棒性与定位精度。该系统采用了现有先进LIO方法[^1]中的预处理模块与紧耦合iEKF，并扩展了若干核心模块，整个系统包含以下部分。
1) 预处理模块：对原始的IMU测量数据与激光雷达扫描结果进行处理，利用先前的状态估计来预测先验状态，同时补偿运动畸变以生成无畸变的点云。
2) 信息精炼模块：通过最大化梯度流的熵[^25]来选择最具信息量的激光雷达点用于扫描-地图配准，随后计算几何残差、雅可比矩阵，并检测退化方向。在此基础上，计算Tikhonov正则化项并构建优化约束。
3) 信息增强模块：沿几何信息匮乏的方向生成强度图像并提取图像特征[^14]，以此构建互补的光度测量数据。
4) 迭代扩展卡尔曼滤波模块：利用优化约束对几何测量数据与光度测量数据进行联合优化，进而更新系统状态。
5) 概率体素映射模块：采用概率自适应体素映射技术[^20]，以提供用于精确扫描到地图几何测量及简并检测的平面特征。
### IV. 受简并约束的状态估计
本节沿用FAST-LIO2中的iEKF框架[^27]，并在其测量模型中加入来自激光雷达强度图像的光度残差项。不过，我们的系统中激光雷达与惯性测量单元（IMU）之间的外部参数（臂长）被视为已知量，因此无需在线估计。首先，我们介绍本文中使用的一些符号：G、I和L分别代表全局坐标系、IMU坐标系和激光雷达坐标系；全局坐标系G即为IMU的初始位姿，而从IMU坐标系到激光雷达坐标系的静态标定外部矩阵$^L\mathbf{T}_I = (^L\mathbf{R}_I, ^L\mathbf{p}_I) \in SE(3)$是已知的。以IMU坐标系作为参考机体坐标系，可得到连续的运动学模型。
$$ \begin{aligned}{{}^{G}\dot{\mathbf{p}}_{I}}&{{}={}^{G}\mathbf{v}_{I},\quad{}^{G}\dot{\mathbf{v}}_{I}={}^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-\mathbf{b}_{\mathbf{a}}-\mathbf{n}_{\mathbf{a}}\right)+{}^{G}\mathbf{g}}\\ {{}^{G}\dot{\mathbf{g}}}&{{}=\mathbf{0},\quad{}^{G}\dot{\mathbf{R}}_{I}={}^{G}\mathbf{R}_{I}\lfloor\omega_{m}-\mathbf{b}_{\omega}-\mathbf{n}_{\omega}\rfloor_{\wedge}}\\ \end{aligned} $$
$$ \dot{\mathbf{b}}_{\omega}=\mathbf{n}_{\mathbf{b}\omega},\quad\dot{\mathbf{b}}_{\mathrm{a}}=\mathbf{n}_{\mathbf{b a}} $$
其中，符号 $[\mathbf{a}]_\wedge$ 表示向量 $\mathbf{a} \in \mathbb{R}^3$ 在 $\mathcal{M} = SO(3) \times \mathbb{R}^{15}$ 上定义的斜对称矩阵，其定义为 $\mathbf{x} \doteq [^G\mathbf{R}_I^\top^G\mathbf{p}_I^\top^G\mathbf{v}_I^\top^G\mathbf{b}_\omega^\top^G\mathbf{g}^\top_I^\top]$；这里，$^G\mathbf{R}_I$ 和 $^G\mathbf{p}_I$ 分别表示惯性测量单元坐标系相对于全局坐标系的旋转矩阵和平移向量，$^G\mathbf{v}_I$ 和 $^G\mathbf{g}$ 是惯性测量单元在全局坐标系中的速度和重力加速度，而 $\mathbf{b}_a$ 和 $\mathbf{b}_\omega$ 则是加速度计和陀螺仪的偏差。借助这一连续运动学模型以及系统噪声，连续过程噪声协方差矩阵 $\mathbf{Q}_c$ 可表示为：
$$ Q_{c}=\mathrm{diag}\left(\sigma_{a}^{2}I_{3},\sigma_{\omega}^{2}I_{3},\sigma_{ba}^{2}I_{3},\sigma_{bg}^{2}I_{3}\right) $$
其中$\sigma_a$为加速度计白噪声密度，$\sigma_\omega$为陀螺仪测量白噪声密度，$\sigma_{ba}$和$\sigma_{bg}$为偏差随机游走谱密度。这些数值会根据激光雷达的特性进行重置。在实际实现中，连续过程噪声协方差矩阵$\mathbf{Q}_c$需要被离散化为采样时间$\Delta t$下的离散噪声协方差。例如，若$\sigma_a$为加速度计白噪声密度，则每样本的标准差为$=\sigma_a\sqrt{\Delta t}$，协方差为$=\sigma_a^2\Delta t$。
在初始化阶段，我们的系统采用静态初始化方式：初始旋转矩阵设为单位矩阵，初始位置、速度和方向均初始化为零；重力、陀螺仪及加速度计的偏差则通过前几帧IMU数据来估计。对于初始状态协方差矩阵，已知或手动指定的初始状态会被赋予较小的方差，而不确定的量则会被赋予较大的方差。
### A. 预处理
该模块具备前向预测和点云畸变校正功能。利用IMU数据递归预测状态，即可计算出当前激光雷达扫描结束时刻的预测状态$\hat{x}$及协方差矩阵$\hat{P}$。
由于激光雷达扫描中每个点是在不同时间戳采样的，因此运动畸变不可避免。我们的系统采用反向传播[^1]方法，将每个激光雷达点从各自的采样时间补偿到扫描结束时刻。系统中使用的离散运动学模型定义如下：
$$ \mathbf{x}_{i+1}=\mathbf{x}_{i}\boxplus(\Delta t\mathbf{f}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i})) $$
其中$\boxplus$为流形上的“加”运算封装符，$\Delta t$为IMU采样周期，i为IMU测量值的索引，输入向量$\mathbf{u}$、系统噪声$\mathbf{w}$以及函数$\mathbf{f}$的定义如下：
$$ \mathbf{w}\doteq\left[\begin{matrix}{\mathbf{n}_{\omega}^{T}}&{\mathbf{n}_{\mathrm{a}}^{T}}&{\mathbf{n}_{\mathrm{b}\omega}^{T}}&{\mathbf{n}_{\mathrm{b a}}^{T}}\\ \end{matrix}\right]^{T},\quad\mathbf{u}\doteq\left[\begin{matrix}{\omega_{m}^{T}}&{\mathbf{a}_{m}^{T}}\\ \end{matrix}\right]^{T} $$
$$ \mathbf{f}\left(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)=\left[\begin{matrix}{\mathbf{\omega}_{m_{i}}-\mathbf{b}_{\mathbf{\omega}_{i}}-\mathbf{n}_{\mathbf{\omega}_{i}}}\\ {^{G}\mathbf{v}_{I_{i}}}\\ {^{G}\mathbf{R}_{I_{i}}\left(\mathbf{a}_{m_{i}}-\mathbf{b}_{\mathbf{a}_{i}}-\mathbf{n}_{\mathbf{a}_{i}}\right)+{^{G}\mathbf{g}}_{i}}\\ {\mathbf{n}_{\mathbf{b}\mathbf{\omega}_{i}}}\\ {\mathbf{n}_{\mathbf{b}a_{i}}}\\ {\mathbf{0}_{3\times1}}\\ \end{matrix}\right]. $$
---
Fig. 1. Overview of the proposed system, consisting of five parts: a processing, an information refinement, an information augmentation, an iterated extended Kalman filter, and a probabilistic voxel map.
对于激光雷达传感器，假设一次激光雷达扫描S的结束时间表示为$t_{k}$，该扫描中单个点的采样时间表示为$L_{p_{i}}$，而$t_{i}$则表示该扫描中的采样时间。任意时刻t时激光雷达的姿态可表示为：
$$ {}^{G}\mathbf{T}_{L}(t)=\left[\begin{matrix}{{}^{G}\mathbf{R}_{L}(t)}&{{}^{G}\mathbf{p}_{L}(t)}\\ {\mathbf{0}}&{\mathbf{1}}\\ \end{matrix}\right]_{4\times4} $$
其中，$^G\mathbf{R}_L$和$^G\mathbf{p}_L$分别表示激光雷达坐标系相对于全局坐标系的旋转矩阵和平移向量。
为了补偿运动误差，我们需要将于$^L p_i$时刻采样的激光雷达点$t_i$投影到扫描结束时间$t_k$，具体过程如下：
$$ {^{L}}p_{k}={^{G}}\mathbf{T}_{L}^{\top}(t_{k}){^{G}}\mathbf{T}_{L}(t_{i}){^{L}}p_{i}={^{L_{t_{k}}}}\mathbf{T}_{L_{t_{i}}}{^{L}}p_{i}. $$
从$L_{t_k} \mathbf{T}_{L_{t_i}}$时刻到$t_i$时刻的相对姿态$t_k$可通过基于IMU数据的反向传播模型来求得，其定义如下：
$$ \mathbf{\check{x}}_{j-1}=\mathbf{\check{x}}_{j}\boxplus(-\Delta t\mathbf{f}(\mathbf{\check{x}}_{j},\mathbf{u}_{j},\mathbf{0})) $$
其中，$\mathbf{x}_j$是在反向传播过程中相对于$\mathbf{x}_j$的$\mathbf{x}_k$的估计值；反向传播过程从零姿态开始，状态则取自前向预测的状态$\hat{\mathbf{x}}_k$。按照上述步骤，激光雷达扫描的运动畸变将被消除。
### B. 信息精炼
对于基于滤波的LIO系统而言，测量模型可为状态估计提供约束条件。本节仅重点关注第IV-C节和第IV-E节中详述的整体测量数据中的几何部分。为了减少冗余的几何测量数据并抑制沿无信息方向的状态更新，我们提出了信息精炼模块。其主要功能如图2所示，具体描述如下。
通常，会利用去畸变后的点云$\mathcal{P}$来计算激光雷达扫描与地图之间的测量残差。对于每个
Fig. 2. Main functions of the proposed information refinement module. (a) Prune LiDAR points. (b) Detect degeneracy directions (c) Refine optimization directions.
有效的点$^L\mathbf{p}_i$而言，测量残差$\mathbf{z}_i^{geo}$被定义为该激光雷达点与对应平面特征之间的距离。综上所述，几何测量模型可表示为如下紧凑形式：
$$ \begin{aligned}\mathbf{0}&=\mathbf{h}_{i}\left(\mathbf{x},\mathbf{w}_{i}\right)\simeq\mathbf{h}_{i}\left(\widehat{\mathbf{x}},\mathbf{0}\right)+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\&=\mathbf{n}_{i}^{\top}\left(^{G}\hat{\mathbf{T}}_{L}{}^{L}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right)+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\&=\mathbf{z}_{i}^{geo}+\mathbf{J}_{i}^{geo}\delta\mathbf{x}+\mathbf{v}_{i}\\ \end{aligned} $$
其中，$\mathbf{h}$为激光雷达几何测量模型，$\widehat{\mathbf{x}}$表示预测的系统状态，其中包含$G\widehat{\mathbf{T}}_L$。$\mathbf{J}_i^{geo}$是在零点处评估得到的$\mathbf{h}_i(\widehat{\mathbf{x}} \otimes \delta \mathbf{x}, \mathbf{0})$关于误差状态$\delta \mathbf{x}$的雅可比矩阵。此外，$\mathbf{v}_i \in \mathcal{N}(\mathbf{0}, \mathbf{R}_i)$为高斯白测量噪声。另外，$\mathbf{n}_i$和$G\mathbf{q}_i$分别代表全局地图$\mathcal{M}$中相关平面特征的法向量和参考点，它们满足$\mathbf{n}_i^\top(G\mathbf{q}_i - G\mathbf{o}_i) = 0$关系，其中$G\mathbf{o}_i = 0$是同一平面上的另一个点。
然而，当来自相似方向的点过多时，上述激光雷达测量模型可能会产生冗余测量数据[^28]。这些冗余数据不仅会影响系统的实时性能，还会导致沿相似方向的测量误差累积，
---
尤其是在退化环境下更是如此。此外，该测量模型生成的雅可比矩阵无法在退化方向上提供足够的优化约束。下面给出简单的解释。
假设由姿态参数$\theta = \{R, t\}$引起的几何扫描-地图配准残差如下：
$$ r_{\theta}(P)=\sum_{(p,q)\in C_{P}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2} $$
其中，$C_P^Q$为对应关系集合，环境被建模为一组原子超平面$S = \{S_s\}_{s=1}^S$，每个超平面通过对应的点子集$P_s \subset P$来观测。
因此，总残差可以重新表示为对表面子集的求和。
$$ r_{\theta}(S)=\sum_{P_{s}\in S}\sum_{(p,q)\in C_{P_{s}}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2}. $$
为了独立于其基数来描述每个表面的贡献，定义了平均集合残差率。
$$ \bar{r}_{\theta}(S)=\sum_{P_{s}\in S}\frac{1}{|C_{P_{s}}^{Q}|}\sum_{(p,q)\in C_{P_{s}}^{Q}}\|r_{\theta}(p,q)\|_{2}^{2}. $$
定义：假设没有噪声，每个超平面表面 $S_{s}$ 会产生 $|P_{s}| - 1$ 个冗余残差。
也就是说，对于任意两点 $p_i, p_j \in P_s$…
$$ r_{\theta}(p_{i},q_{i})\parallel r_{\theta}(p_{j},q_{j}). $$
这意味着它们对应的残差向量是平行的且幅度相近。去除这些冗余点和残差并不会改变目标函数的最优值，因为它们对梯度方向和Hessian矩阵的贡献是相同的。更多证明和推导请参见 [^28]。
据此，我们提出了一种考虑冗余与简并的几何测量模型。首先，我们采用最小化冗余的点云采样（RMS）框架 [^28]，以获取能最大化信息的激光雷达点 $P_{RMS}$，从而通过梯度流熵最大化来实现扫描到地图的配准。接下来，利用预测状态、N个保留点以及全局体素地图，可以将式(9)中整个扫描到地图测量残差的代价函数重新表述为二次代价优化问题，而Hessian矩阵 $A'$ 可按下述方式分解：
$$ \begin{aligned}{\mathbf{A}^{\prime}}&{{}=\left(\sum_{i=1}^{N}\underbrace{\left[\begin{matrix}{{\left(^{L}\mathbf{p}_{i}\times\mathbf{n}_{i}\right)}}\\ {\mathbf{n}_{i}}\\ \end{matrix}\right]}_{A}\underbrace{\left[\begin{matrix}{{\left(^{L}\mathbf{p}_{i}\times\mathbf{n}_{i}\right)}^{\top}\mathbf{n}_{i}^{\top}}\\ \end{matrix}\right]}_{A^{\top}}\right)}\\ {}&{{}=\left[\begin{matrix}{\mathbf{A}_{r r}^{\prime}}&{\mathbf{A}_{r t}^{\prime}}\\ {\mathbf{A}_{t r}^{\prime}}&{\mathbf{A}_{t t}^{\prime}}\\ \end{matrix}\right]_{6\times6}.}\\ \end{aligned} $$
对于翻译组件 $\mathbf{A}_t'$，其特征值分解为 $\mathbf{A}'_t = \mathbf{U}_t \boldsymbol{\Sigma}_t \mathbf{U}_t^\top$；利用简并检测方法X-ICP [^9]，可以确定无信息量的简并方向集合 $\mathbf{V}_t$。用于扫描到地图配准的带Tikhonov正则化的代价函数如式(15)所示，
Fig. 3. Simplified optimization problem with Tikhonov regularization. The total loss, ordinary loss, and regularization loss are represented by the red surface, green surface, and blue surface, respectively. Without Tikhonov regularization, the solution $\dot{\mathbf{x}}$ (black point) may fall into the degenerate section, not the constrained optimal solution $\mathbf{x}^{*}$ (orange star).
能够改善在几何简并环境下ICP算法的表现。图3展示了一个简化的示例。
$$ \mathbf{E}(\mathbf{x})=\sum_{i=1}^{N}||\underbrace{\mathbf{n}_{i}^{\top}\left({}^{G}\mathbf{T}_{L}{}^{L}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right)}_{\mathbf{e}_{i}}||_{2}+\lambda_{D}||\mathbf{V}_{t}\mathbf{x}||_{2}^{2}. $$
为了最小化代价，关于 $\mathbf{J}_i$ 的雅可比矩阵 $\nabla_{\mathbf{E}(\mathbf{x})}$、梯度 $\mathbf{H}_{\mathbf{E}(\mathbf{x})}$ 以及Hessian矩阵 $\mathbf{x}$ 可以提供优化方向，其定义如下：
$$ \begin{aligned}\mathbf{J}_{i}&=\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{x}}\\\nabla_{\mathbf{E}(\mathbf{x})}&=\sum_{i=1}^{N}\left(2\mathbf{J}_{i}^{\top}\mathbf{e}_{i}\right)+2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\mathbf{x}\\\mathbf{H}_{\mathbf{E}(\mathbf{x})}&=\frac{\partial^{2}\mathbf{E}(\mathbf{x})}{\partial\mathbf{x}^{2}}=\sum_{i=1}^{N}\left(2\mathbf{J}_{i}\mathbf{J}_{i}^{\top}\right)+2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\end{aligned} $$
其中 $\lambda_{D}$ 是简并正则化参数。
在基于滤波的激光雷达里程计框架中，卡尔曼增益可以通过平衡先验模型和测量模型来提供更新方向。因此，我们设计了一个包含无信息方向的正则化信息精炼矩阵，旨在通过调整与测量相关的部分所对应的卡尔曼增益来提供优化方向。根据上述推导，正则化项 $\mathbf{H}_{(\mathbf{x})}$ 的Hessian矩阵 $\lambda_D\|\mathbf{V}_t\mathbf{x}\|_2^2$ 可视为第IV-E节中所用的信息精炼矩阵。若记 $\mathbf{L}(\mathbf{x}) = \lambda_D\|\mathbf{V}_t\mathbf{x}\|_2^2$，则 $\mathbf{L}(\mathbf{x})$ 的梯度和Hessian矩阵定义如下：
$$\begin{aligned}\nabla_{\mathbf{L}(\mathbf{x})}&=2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}\mathbf{x}\\ \mathbf{H}_{\mathbf{L}(\mathbf{x})}&=\frac{\partial^{2}\mathbf{L}(\mathbf{x})}{\partial\mathbf{x}^{2}}=2\lambda_{D}\mathbf{V}_{t}^{\top}\mathbf{V}_{t}.\end{aligned}$$
### C. 信息增强
由于简并现象源于约束不足，将几何测量数据与其他模态数据融合可以提高系统的鲁棒性。近年来，激光雷达强度信息越来越多地被整合到基于激光雷达的里程计系统中。
---
为了构建退化方向上的互补强度约束，我们采用了Coin-LIO中提出的强度测量模型[^14]。首先利用球面投影将输入的激光雷达点云转换为强度图像。由于激光雷达光束的仰角间距不均匀，会产生线性伪影，随后通过图像处理来消除这些伪影。为了在检测到的退化方向上构建几何互补残差$V_t$，需按如下方式计算图像块的补充分量$c_t$：
$$ \begin{aligned}\mathbf{s}_{i}&=\frac{\partial\Pi\left(^{L}\mathbf{p}_{j}\right)}{\partial^{L}\mathbf{p}_{j}}\cdot\mathbf{v}_{t,i}\in\mathbb{R}^{2}\quad\forall\mathbf{v}_{t,i}\in\mathbf{V}_{t}\\\mathbf{c}_{i}&=\frac{\mathbf{s}_{i}\cdot\mathbf{v}_{patch}}{\|\mathbf{s}_{i}\|_{2}^{2}}\end{aligned} $$
其中$\Pi(*)$表示球面投影模型。$\mathbf{v}_{patch}$是第二图像矩的最大特征值对应的特征向量，它代表了每个图像块中强度图像的信息方向。$\mathbf{s}_{i}$则表示由方向性点扰动引起的投影图像坐标偏移。
对于$\mathbf{V}_t$中的每个退化方向，会选取图像中对强度贡献最大的图像块$\mathbf{c}_i$来构建互补的光度约束；跟踪点的投影强度值$I_c(*)$与测量强度值$i_f$之间的光度残差为$\mathbf{z}^{pho} = I_c(\Pi(L\mathbf{p}_f)) - i_f$。光度测量模型的雅可比矩阵$\mathbf{J}^{pho}$及相关细节详见[^14]。
### D. 概率体素映射
基于上述讨论可知，信息精炼与信息增强均需要先进行退化检测。退化检测的关键步骤是将能够最大化信息的点与全局地图进行匹配，而点对平面匹配的准确性直接决定了退化检测及几何测量的质量。此外，激光雷达点测量噪声和位姿估计误差带来的不确定性也会影响几何约束和状态更新的准确性。因此，本文采用了概率体素映射方法[^22]，该方法在全局地图的平面特征中建模了激光雷达点测量噪声和位姿估计误差，还设计了用于精准实现点对平面匹配及计算测量噪声的概率方法。
给定先验激光雷达点$^G\mathbf{p}_i$、法线为$^G\mathbf{n}_i$且中心点为$^G\mathbf{q}_i$的平面，该点到平面的距离为$d_i$。
$$ d_{i}={}^{G}\mathbf{n}_{i}^{\top}\left({}^{G}\mathbf{p}_{i}-{}^{G}\mathbf{q}_{i}\right). $$
将$\delta_{\mathbf{n}_i}, \delta_{\mathbf{p}_i}, \delta_{\mathbf{q}_i}$、$^G\mathbf{n}_i$、$^G\mathbf{p}_i$的噪声$^G\mathbf{q}_i$代入式(19)，可得：
$$ \begin{align*}d_{i}&=\left({^{G}\mathbf{n}_{i}^{gt}}\boxplus\delta_{\mathbf{n}_{i}}\right)^{\top}\left[\left({^{G}\mathbf{p}_{i}^{gt}}+\delta_{G\mathbf{p}_{i}}\right)-{^{G}\mathbf{q}_{i}^{gt}}-\delta_{G\mathbf{q}_{i}}\right]\\&\approx\underbrace{\left({^{G}\mathbf{n}_{i}^{gt}}\right)^{\top}\left({^{G}\mathbf{p}_{i}^{gt}}-{^{G}\mathbf{q}_{i}^{gt}}\right)}_{=0}\\&\quad+\mathbf{J}_{G\mathbf{n}_{i}}\delta_{G\mathbf{n}_{i}}+\mathbf{J}_{G\mathbf{q}_{i}}\delta_{G\mathbf{q}_{i}}+\mathbf{J}_{G\mathbf{p}_{i}}\delta_{G\mathbf{p}_{i}}.\end{align*} $$
这意味着$d_i \sim \mathcal{N}(0, \Sigma_{\mathbf{w}_i})$，其中$\mathbf{w}_i = \mathbf{J}_{c_{\mathbf{n}_i}} \delta_{c_{\mathbf{n}_i}} + \mathbf{J}_{c_{\mathbf{q}_i}} \delta_{c_{\mathbf{q}_i}} + \mathbf{J}_{c_{\mathbf{p}_i}} \delta_{c_{\mathbf{p}_i}}$和$\mathbf{J}_{c_{\mathbf{n}_i}}, \mathbf{J}_{c_{\mathbf{q}_i}}, \mathbf{J}_{c_{\mathbf{p}_i}}$为对应的雅可比矩阵。若某点属于某个平面，则该点到平面的距离会在$3\sigma$范围内，此处$\sigma = \sqrt{\Sigma_{\mathbf{w}_i}}$。此外，式(9)中有效点对平面距离测量的噪声$\mathbf{v}_i$可表示为$\mathbf{J}_{c_{\mathbf{n}_i}} \delta_{c_{\mathbf{n}_i}} + \mathbf{J}_{c_{\mathbf{q}_i}} \delta_{c_{\mathbf{q}_i}} + \mathbf{J}_{c_{\mathbf{p}_i}} \delta_{c_{\mathbf{p}_i}}$。更详细的讨论可见[^22]。
```pseudo
// Algorithm 1: State Estimation
Input: Last estimation $\bar{\mathbf{x}}_{k-1}$, $\bar{\mathbf{P}}_{k-1}$;
```
激光雷达点$\mathcal{P}$、惯性测量单元输入$(\mathbf{a}_m, \omega_m)$；
几何地图$\mathcal{M}$、强度特征$\mathcal{F}$；
输出：当前估计值$\bar{\mathbf{x}}_k$、$\bar{\mathbf{P}}_k$；
1: $\hat{\mathbf{x}}_k$、$\hat{\mathbf{P}}_k \leftarrow$ 预测$(\bar{\mathbf{x}}_{k-1}, \bar{\mathbf{P}}_{k-1}, \mathbf{a}_m, \omega_m)$；
2: $\mathcal{P}_{un} \leftarrow$ 补偿运动$(\mathcal{P}, \mathbf{a}_m, \omega_m)$；
3: $\text{Img} \leftarrow$ 球面投影$(\mathcal{P})$；
4: 预处理（Img）；
5: $\mathcal{P}_{RMS} \leftarrow$ InforMaxSelection $(\mathcal{P}_{un})$;
6: $\kappa \leftarrow -1$, $\hat{\mathbf{x}}_k^{\kappa=0} \leftarrow \hat{\mathbf{x}}_k$;
7: repeat
8: $\kappa \leftarrow \kappa + 1$;
9: $\mathbf{H}^K \leftarrow \frac{\partial (\hat{\mathbf{x}}_k^{\kappa} \oplus \hat{\mathbf{x}}_k^{\tau}) \oplus \hat{\mathbf{x}}_k}{\partial \hat{\mathbf{x}}_k^{\tau}} | \hat{\mathbf{x}}_k^{\tau}=0$;
10: $\mathbf{P} \leftarrow (\mathbf{H}^{\kappa})^{-1} \hat{\mathbf{P}}_k(\mathbf{H}^{\kappa})^{-\top}$;
11: $\mathbf{z}^{geo}, \mathbf{J}^{geo} \leftarrow$ GeocMeasure $(\hat{\mathbf{x}}_k^K, \mathcal{P}_un, \mathcal{M})$;
12: $\mathbf{V}_t \leftarrow$ DedeneracyDetection $(\mathbf{J}^{geo})$;
13: $\mathbf{z}^{pho}, \mathbf{J}^{pho} \leftarrow$ PhcMeasure $(\hat{\mathbf{x}}_k^K, \mathcal{P}_img, \mathcal{F}, \mathbf{V}_t)$;
14: $\mathbf{H}_{L(x)} \leftarrow 2\lambda_D \mathbf{V}_t^{\top} \mathbf{V}_t$;
15: $\mathbf{J} \leftarrow [\mathbf{J}^{geo}, \mathbf{J}^{pho}];$ // 合并测量结果
16: $\mathbf{z} \leftarrow [\mathbf{Z}^{geo}, \mathbf{Z}^{pho}]$;
17: $\mathbf{K} \leftarrow (\mathbf{J}^{\top} \mathbf{R}^{-1} \mathbf{J} + \mathbf{H}_L(x) + \mathbf{P}^{-1})^{-1} \mathbf{J}^{\top} \mathbf{R}^{-1}$;
18: $\hat{\mathbf{x}}_k^{\kappa+1} \leftarrow \hat{\mathbf{x}}_k^{\kappa} \oplus (\mathbf{-Kz} - (\mathbf{I} - \mathbf{KH})(\mathbf{J})^{-1} \left( \hat{\mathbf{x}}_k^{\kappa} \oplus \hat{\mathbf{x}}_k^{\tau} \right))$;
19: until $\|\hat{\mathbf{x}}_k^{\kappa+1} \oplus \hat{\mathbf{x}}_k^{\kappa}\| < \epsilon$;
20: $\hat{\mathbf{x}}_k \leftarrow \hat{\mathbf{x}}_k^{\kappa+1}$; $\hat{\mathbf{P}}_k \leftarrow (\mathbf{I} - \mathbf{KH})\mathbf{P}$;
21: UpdateMapAndFeatures $(\mathcal{F}, \mathcal{M}, \hat{\mathbf{x}}_k, \mathcal{P}, \text{Img})$;
顺便说一下，经过剪枝后的点过于稀疏，因此不适合用于表示环境中的平面特征。所以，原始的无畸变激光雷达点被用于建图，而剪枝后的点则用于扫描与地图匹配以及简并检测。
### E. 受限状态更新
在获得几何残差 $\mathbf{z}^{geo}$、光度残差 $\mathbf{z}^{pho}$ 以及对应的雅可比矩阵后，可将这些测量结果组合起来构造如下的合并测量结果：
$$ \begin{aligned}r c l\mathbf{J}&=\left[\mathbf{J}_{1}^{\mathrm{e w}^{\top}},\ldots,\mathbf{J}_{m}^{\mathrm{g e o}^{\top}},\lambda\mathbf{J}_{1}^{\mathrm{p h o}^{\top}},\ldots,\lambda\mathbf{J}_{n}^{\mathrm{p h}\omega^{\top}}\right]^{\top}\\ \mathbf{z}&=\left[\mathbf{z}_{1}^{\mathrm{g e o}},\ldots,\mathbf{z}_{m}^{\mathrm{g e v}},\lambda\mathbf{z}_{1}^{\mathrm{p h o}},\ldots,\lambda\mathbf{z}_{n}^{\mathrm{p h o}^{\top}}\right]\end{aligned} $$
其中 $\lambda$ 是几何测量与光度测量之间的缩放因子。将状态先验与所有有效测量结果结合，即可得到状态估计问题：
$$ \min_{\delta\mathbf{x}}\left(\|\mathbf{x}_{k}\boxminus\widehat{\mathbf{x}}_{k}\|_{\widehat{\mathbf{P}}_{k}^{-1}}^{2}+\sum_{j=1}^{m+n}\|\mathbf{z}_{j}+\mathbf{J}_{j}\delta\mathbf{x}\|_{\mathbf{R}_{j}^{-1}}^{2}\right) $$
其中 $\|x\|_{\mathrm{M}}^{2} = x^{\top} \mathrm{M} x$，令 $\mathbf{R} = \text{diag}(\mathbf{R}_1, \ldots, \mathbf{R}_{m+n})$，且正常卡尔曼增益为 $\mathbf{K} = (\mathbf{J}^{\top} \mathbf{R}^{-1} \mathbf{J} + \mathbf{P}^{-1})^{-1} \mathbf{J}^{\top} \mathbf{R}^{-1}$。为了
---
通过信息精炼矩阵 $\mathbf{H}_{\mathrm{L}(x)}$ 提供优化方向，需要对卡尔曼增益中依赖于测量的部分 $\mathbf{K}$ 进行调整。由于我们的系统仅检测到平移方向上的简并现象，若将 $\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J}$ 的平移子矩阵记为 $\mathbf{G} = (\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J})_{3\times3}$，则需将 $\mathbf{G}$ 调整为 $\mathbf{G} + \mathbf{H}_{\mathrm{L}(x)}$。这可以用带有填充项 $\mathbf{H}_{\mathrm{L}(x)}^{*}$ 的简洁方程表示如下：
$$ \mathbf{K}=\left(\mathbf{J}^{\top}\mathbf{R}^{-1}\mathbf{J}+\mathbf{H}_{\mathbf{L}(\mathbf{x})}^{*}+\mathbf{P}^{-1}\right)^{-1}\mathbf{J}^{\top}\mathbf{R}^{-1}. $$
上述过程会重复直至收敛，之后便可利用更新后的状态将激光雷达点注册到概率体素图中。整个状态估计流程在算法1中进行了总结。
### V. 实验
为了评估所提方法的性能，我们针对几种最先进的开源LIO系统开展了定量与定性对比实验：Fast-LIO2 [^1]，一种实时高精度LIO系统；Point-LIO [^21]，一种逐点处理的LIO系统；VoxelMap [^22]，一种采用概率自适应体素图的LIO系统；Coin-LIO [^14]，一种适用于简并环境的先进LIO系统，它引入了激光雷达强度数据作为补充测量信息。
在本节中，我们定量评估了该方法与其他方法在定位精度方面的表现。绝对平移误差（ATE）的均方根误差（RMSE）被选作定位性能的评估指标，RMSE值越低表明定位精度越高。为了定性评估地图质量，我们采用了平均地图熵（MME）[^29]，这是一种用于衡量局部一致性的信息论指标。点的颜色根据MME值进行编码，数值越低表示一致性越高。
我们在FusionPortable V2 [^30]和Enwide [^14]数据集上开展实验。这两个数据集均使用128线Ouster激光雷达采集，借助Ouster驱动提供的元数据，可将激光雷达点投影为可靠的强度图像。此外，我们还在禁用了信息增强模块的NTU-viral数据集上进行了额外的对比实验。NTU-viral数据集是在南洋理工大学校园内利用DJI M600平台采集的多传感器无人机数据集，涵盖室内外环境，且具备精确的地面真实轨迹数据。所有实验均在机器人操作系统（ROS）环境下进行，所用计算机配备Intel i9-13900K CPU（主频5.80 GHz）和32 GB内存。由于基线方法与我们的方法采用类似的算法框架，因此二者共享的参数设置相同，其余参数则沿用基线方法中的设定。
### A. 通用环境下的实验
在本节中，我们在通用的FusionPortable V2数据集上开展实验。该数据集涵盖了多种运动模式和复杂环境 [^30]。实验中我们仅关注几何结构较为简单的场景。
1) 定位精度：利用工具箱计算得出的FusionPortable V2和NTU-viral数据集的定位精度结果分别列于表I和表II中。轨迹与地面真实轨迹的对比情况如图4和图5所示。从这些实验结果可以看出，所提出的系统在大多数场景下均展现出最优的定位精度。此外，借助用于几何测量的概率体素地图，VoxelMap的定位精度优于Fast-LIO2和Point-LIO中所采用的ikd-tree结构。虽然Coin-LIO利用激光雷达强度在FusionPortable V2数据集中构建互补测量值，但不准确的几何测量和强度噪声会降低定位精度。
2) 地图一致性：ugv-campus01和legged-grass00序列的建图结果如图6所示。所提方法构建的点云地图能够保留树木和墙壁的细微细节，同时减少了模糊和重影现象。此外，MME可视化结果显示暖色区域占比更高，这表明局部地图一致性更强。由于几何测量依赖于精确的扫描结果与地图的匹配度，更高的局部地图一致性能够提升匹配精度，进而提高退化检测精度和定位精度。
### B. 退化环境下的实验
尽管我们的系统在通用环境下表现最佳，但该方法的主要目的在于提升几何退化环境下的定位与建图性能。因此，本节我们在具有挑战性的Enwide数据集上开展实验。该数据集采集于典型的几何退化环境，其中所有序列均包含较长的几何退化路段，且能提供可靠的激光雷达强度信息。此外，为了验证所提出的信息精炼模块和概率体素地图的有效性，我们在该数据集上开展了消融研究。我们分别移除信息精炼模块和概率体素地图，相应记为“ours (w/o IR)”和“ours (w/o VM)”。
1) 定位精度：表III展示了定位精度结果，图7则显示了轨迹与真实值的对比情况。由于缺乏几何退化缓解机制，Fast-LIO2、Point-LIO和VoxelMap在一些严重退化的环境中（如隧道和跑道）无法正常工作。由于集成了激光雷达强度信息，Coin-LIO以及所提出的方法能够在所有序列中顺利运行。将未采用信息优化模块且未使用概率体素图的系统与本文所提系统相比，后者的定位精度更高。在几乎所有序列中，信息优化模块都能降低定位误差，尤其是在跑道和交叉口序列中。此外，可以看出利用概率体素图来处理几何测量数据能够提升定位精度。
---
(a)
(b)
Fig. 4. Trajectory comparison results on FusionPortable V2 dataset. The zoomed-in regions are shown in the red boxes. (a) ugv-campus01. (b) legged-grass00. TABLE I
ATE (RMSE, METERS) ON FUSIONPORTABLE V2 DATASET
| Sequence | Name | h-grass00 | h-underground00 | l-grass00 | u-campus01 | u-parking01 | u-parking02 | u-parking03 |
| Method | Fast-LIO2 | 0.467262 | 0.766680 | 0.372376 | 0.677929 | 0.483688 | 1.865421 | 1.393853 |
| Point-LIO | - | 1.800630 | 0.183620 | 0.566013 | 0.316874 | 0.845095 | 1.849024 |
| VoxelMap | 0.064231 | 0.827814 | 0.471301 | 0.468336 | 0.315034 | 0.410986 | 0.477338 |
| Coin-LIO | 0.663138 | 0.501824 | 0.318176 | 0.726021 | 0.323063 | 0.664624 | 0.831481 |
| Ours | 0.148206 | 0.448867 | 0.084614 | 0.443145 | 0.300281 | 0.404877 | 0.498481 |
注意：“-”表示系统运行失败或出现明显漂移现象。最佳结果以粗体显示，次佳结果则加下划线标注。
Fig. 5. Trajectory comparison results on NTU-viral dataset spms-01 sequence. The zoomed-in regions are shown in the red boxes.
TABLE II
ATE (RMSE, METERS) ON NTU-VIRAL DATASET
| Method | eee-01 | nya-02 | rtp-02 | spms-01 | sbs-01 |
| Coin-LIO | 0.181815 | 0.171741 | 0.146022 | 2.196588 | 0.184311 |
| Point-LIO | 0.188277 | 0.215078 | 0.363191 | 1.186950 | 0.191895 |
| Fast-LIO2 | 0.158396 | 0.177101 | 0.153968 | 1.827605 | 0.183802 |
| VoxelMap | 4.112456 | 0.381465 | 0.935816 | 1.910679 | 1.015116 |
| Ours | 0.177492 | 0.177734 | 0.128440 | 0.274400 | 0.180789 |
$^{*}$注意：最佳结果以粗体显示，次佳结果则加下划线标注。
2) 地图一致性：图8展示了RunwayS和IntersectionD序列的地图一致性可视化结果，而Enwide数据集中其他环境的建图结果则显示在图9中。这些对比结果表明，本文所提系统的地图一致性更高，尤其是在存在几何退化的长路段上。这些结果证实，信息优化模块能够在严重的几何退化场景下改善地图的局部一致性。
### C. 正则化参数分析
原则上，在一定程度上增大$\lambda_{D}$会沿退化方向引入额外的不确定性，进而降低系统在这些方向上的状态更新幅度。然而，由于退化检测和状态不确定性均会受到激光雷达类型及环境结构特征的影响，固定的$\lambda_{D}$值无法保证在每个序列中都能获得最优精度。不过为了实验的公平性，我们并未针对每个序列对$\lambda_{D}$值进行微调，而是统一将其设为20。
此外，为了评估退化正则化参数$\lambda_{D}$的影响，我们在多个Enwide序列中开展了敏感性分析，将$\lambda_{D}$的取值从0变化到100，步长为10。统计结果汇总于表IV中。此外，我们对所有序列的RMSE值进行了最小-最大归一化处理，图10展示了参数$\lambda_{D}$对应的平均归一化RMSE(m)变化情况。分析表明，不同环境下的最优$\lambda_{D}$值存在差异，说明其取值依赖于具体环境而非具有普适性。不过在大多数实验中，将该参数$\lambda_{D}$设置在10–30范围内时，能够获得相对较好的定位性能。
---
(a)
(b)
Fig. 6. Mapping comparison results on FusionPorable V2 dataset. (a) ugv-campus01. (b) legged-grass00.
(a)
(b)
Fig. 7. Trajectory comparison results of our method and Coin-LIO on Enwide dataset. (a) RunwayS. (b) IntersectionD.
取值范围在 10–30 之间时，定位性能相对较好。
### D. 实时性分析
为了评估实时性能，我们在Enwide数据集上测试了系统各模块的处理时间。如表V所示，该系统每次激光雷达扫描的平均处理时间为0.072秒（约13.9 Hz），这一数值超过了Ouster OS0-128传感器10 Hz的扫描速率。尽管与COIN-LIO相比，冗余最小化模块和体素地图模块会带来额外的0.021秒处理开销，但该系统仍能以令人满意的计算效率实现实时运行。
### E. 讨论与应用场景
在本研究中，这种考虑退化情况的公式主要针对激光雷达测量中的平移退化问题，因此主要适用于那些能够通过惯性测量单元提供高速率惯性旋转测量数据、且主要退化现象源于平移的场景。此外，实验结果表明，$\lambda_{D}$的最优值与场景结构密切相关，并非在不同场景下都具备普适性最优值。
---
TABLE III
ATE (RMSE, METERS) ON ENWIDE DATASET
| Method | Fast-LIO2 | Point-LIO | VoxelMap | Coin-LIO | ours (w/o IR) | ours (w/o VM) | ours |
| TunnelS | - | - | - | 0.776831 | 0.730282 | 0.736179 | 0.705223 |
| TunnelD | - | - | - | 0.505336 | 0.426861 | 0.489148 | 0.339451 |
| IntersectionS | 0.446384 | 0.183806 | 0.149982 | 0.328971 | 0.240054 | 0.457720 | 0.170944 |
| IntersectionD | - | - | 0.347427 | 1.650612 | 0.390446 | 3.689573 | 0.254946 |
| RunwayS | - | - | 3.210004 | 1.814010 | 0.580743 | 1.301178 | 0.539720 |
| RunwayD | - | - | - | 2.235924 | 2.081241 | 2.666165 | 1.641159 |
| FieldS | 0.213750 | 0.984559 | 0.191844 | 0.205698 | 0.172558 | 0.265523 | 0.205306 |
| FieldD | 12.011100 | - | 0.185444 | 0.526876 | 0.203913 | 0.385250 | 0.232325 |
| KatzenseeS | 4.141862 | 0.489176 | 0.245581 | 1.172257 | 0.309386 | 0.506755 | 0.315927 |
| KatzenseeD | 6.036565 | - | 0.196991 | 0.348262 | 0.179833 | 0.618113 | 0.147715 |
注：“-”表示系统运行失败或出现明显漂移现象。最佳结果以粗体显示，次佳结果则加下划线标注。IR代表信息精炼模块，VM代表概率体素映射模块。
(a)
(b)
Fig. 8. Mapping results of our method and Coin-LIO on Enwide dataset. (a) RunwayS. (b) IntersectionD.
TABLE IV
DISTRIBUTION OF RMSE FOR EACH SEQUENCE
| Sequence | Min | Max | Range | Mean | Std |
| FieldS | 0.192365 | 0.305410 | 0.113045 | 0.244995 | 0.036276 |
| IntersectionS | 0.244648 | 0.285907 | 0.041259 | 0.267306 | 0.013637 |
| KatzenseeS | 0.106669 | 0.119995 | 0.013326 | 0.11171 | 0.003985 |
| RunwayS | 0.706157 | 1.312839 | 0.606682 | 0.944527 | 0.197481 |
| TunnelD | 0.441583 | 0.512136 | 0.070553 | 0.471745 | 0.023941 |
| TunnelS | 0.655489 | 0.693984 | 0.674555 | 0.038495 | 0.011382 |
TABLE V
REAL-TIME PERFORMANCE ANALYSIS
| Computation Time | Average[s] | Max[s] | Min[s] |
| Total | 0.0726950581 | 0.1715988720 | 0.0591839850 |
| Rredundancy minimizing | 0.0170426966 | 0.0279797190 | 0.0138959010 |
| State Update Time | 0.0031382527 | 0.0051271680 | 0.0019688940 |
| Track Intensity Features | 0.0011082522 | 0.0026169430 | 0.0007360890 |
| Degeneracy Detection | 0.0000187596 | 0.0000513050 | 0.0000116240 |
| Calculate Regularization Matrix | 0.0000003700 | 0.0000026620 | 0.0000000220 |
| Update VoxelMap | 0.0360651181 | 0.1378541830 | 0.0268253270 |
实际上，$\lambda_{D}$的选择具有较大灵活性，其取值会因环境特征和几何条件的差异而产生明显变化。因此，开发自适应或场景感知型的$\lambda_{D}$调节策略，是未来研究的重要且极具前景的方向。
---
Fig. 9. Mapping results of our method on the Enwide dataset.
Fig. 10. Average normalized RMSE(m) variation with respect to parameter $\lambda_{D}$.
### VI. 结论
本文提出了一种几何-强度融合的LIO框架，能够在几何退化环境中实现鲁棒且精准的定位。信息精炼模块用于减少冗余且不准确的几何测量数据，这也为后续优化指明了方向。信息增强模块则会选取几何退化方向上的激光雷达强度图像特征，以此构建互补的光度测量数据。在公开数据集上的实验表明，与现有最先进方法相比，所提系统在精度和地图一致性方面表现更优，尤其在极度退化的环境中优势更为明显。
未来的工作还将探索深度学习技术的集成应用，以提升在几何约束较弱的退化环境中的SLAM性能。通过学习区分度极高的点云特征，系统便能在特征稀疏或重复的场景中保持定位能力。此外，研究人员还将致力于降低系统参数对环境的相关性，从而开发出更具适应性和通用性的SLAM解决方案。
## 参考文献
[^1]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “FAST-LIO2: Fast direct LiDAR-inertial odometry,” IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 2022.
[^2]: J. Lv, K. Hu, J. Xu, Y. Liu, X. Ma, and X. Zuo, "CLINS: Continuous-time trajectory estimation for LiDAR-inertial system," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2021, pp. 6657–6663.
[^3]: H. Gao, Q. Qiu, H. Liu, D. Liang, C. Wang, and X. Zhang, "ERPoT: Effective and reliable pose tracking for mobile robots using lightweight polygon maps," IEEE Trans. Robot., vol. 41, pp. 3799–3819, 2025.
[^4]: Q. Wu et al., “UA-LIO: An uncertainty-aware LiDAR-inertial odometry for autonomous driving in urban environments,” IEEE Trans. Instrum. Meas., vol. 74, pp. 1–12, 2025.
[^5]: L. Gong et al., “PreciseSLAM: Robust, real-time, LiDAR-inertial-ultrasonic tightly-coupled SLAM with ultraprecise positioning for plant factories,” IEEE Trans. Ind. Informat., vol. 20, no. 6, pp. 8818–8827, Jun. 2024.
[^6]: D. Liu, J. Wu, Y. Du, R. Zhang, and M. Cong, “SBC-SLAM: Semantic bioinspired collaborative SLAM for large-scale environment perception of heterogeneous systems,” IEEE Trans. Instrum. Meas., vol. 73, pp. 1–10, 2024.
[^7]: J. Wen, X. Zhang, H. Gao, J. Yuan, and Y. Fang, "CAE-RLSM: Consistent and efficient redundant line segment merging for online feature map building," IEEE Trans. Instrum. Meas., vol. 69, no. 7, pp. 4222–4237, Jul. 2020.
[^8]: J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in Proc. IEEE Int. Conf Robot. Autom. (ICRA), Aug. 2016, pp. 809–816.
[^9]: T. Tuna, J. Nubert, Y. Nava, S. Khattak, and M. Hutter, “X-ICP: Localizability-aware LiDAR registration for robust localization in extreme environments,” IEEE Trans. Robot., vol. 40, pp. 452–471, 2024.
[^10]: J. Hatleskog and K. Alexis, “Probabilistic degeneracy detection for point-to-plane error minimization,” IEEE Robot. Autom. Lett., vol. 9, no. 12, pp. 11234–11241, Dec. 2024.
[^11]: W. Chen et al., "P2D-DO: Degeneracy optimization for LiDAR SLAM with point-to-distribution detection factors," IEEE Robot. Autom. Lett., vol. 10, no. 2, pp. 1489–1496, Feb. 2025.
[^12]: H. Li, B. Tian, H. Shen, and J. Lu, “An intensity-augmented LiDAR-inertial SLAM for solid-state LiDARs in degenerated environments,” IEEE Trans. Instrum. Meas., vol. 71, pp. 1–10, 2022.
[^13]: Y. Zhang et al., “RI-LIO: Reflectivity image assisted tightly-coupled LiDAR-inertial odometry,” IEEE Robot. Autom. Lett., vol. 8, no. 3, pp. 1802–1809, Mar. 2023.
[^14]: P. Pfreundschuh, H. Oleynikova, C. Cadena, R. Siegwart, and O. Andersson, "COIN-LIO: Complementary intensity-augmented LiDAR inertial odometry," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2024, pp. 1730–1737.
[^15]: W. Hu, K. Feng, X. Ye, H. Ding, and L. Wen, “CODN-GS: Coupled optimization of depth and normal in 3D Gaussian splatting for scene reconstruction,” IEEE Trans. Autom. Sci. Eng., vol. 22, pp. 20746–20758, 2025.
[^16]: K. Feng, H. Xiao, J. Zhang, and Q. Ni, "A digital twin methodology for vibration-based monitoring and prediction of gear wear," Wear, vol. 571, Jun. 2025, Art. no. 205806.
[^17]: J. Zhang et al., “Loam: LiDAR odometry and mapping in real-time,” in Proc. Robotics, Sci. Syst., 2014, vol. 2, no. 9, pp. 1–9.
[^18]: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2018, pp. 4758–4765.
[^19]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIOSAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2020, pp. 5135–5142.
[^20]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017.
[^21]: D. He, W. Xu, N. Chen, F. Kong, C. Yuan, and F. Zhang, “Point-LIO: Robust high-bandwidth light detection and ranging inertial odometry,” Adv. Intell. Syst., vol. 5, no. 7, Jul. 2023, Art. no. 2200459.
[^22]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022.
[^23]: C. Zheng et al., “Fast-livo2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Rob., vol. 41, pp. 326–346, 2025.
[^24]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 5692–5698.
[^25]: J. Lin and F. Zhang, “R $^{3}$LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in Proc. IEEE Int. Conf Robot. Autom. (ICRA), Apr. 2022, pp. 10672–10678.
## [^26]: S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super odometry: IMU-centric LiDAR-visual-inertial estimator for challenging environments,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2021, pp. 8729–8736
---
[^27]: W. Xu and F. Zhang, “FAST-LIO: A fast, robust LiDAR-inertial odometry package by tightly-coupled iterated Kalman filter,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.
[^28]: P. Petracek, K. Alexis, and M. Saska, “RMS: Redundancy-minimizing point cloud sampling for real-time pose estimation,” IEEE Robot. Autom. Lett., vol. 9, no. 6, pp. 5230–5237, Jun. 2024.
[^29]: X. Hu et al., “MapEval: Towards unified, robust and efficient SLAM map evaluation framework,” IEEE Robot. Autom. Lett., vol. 10, no. 5, pp. 4228–4235, May 2025.
[^30]: H. Wei et al., “FusionPortableV2: A unified multi-sensor dataset for generalized SLAM across diverse platforms and scalable environments,” Int. J. Robot. Res., vol. 44, no. 7, pp. 1093–1116, Jun. 2025.
姚赫凯于2023年在中国大连理工大学获得人工智能专业工学学士学位，目前他正在该校智能机器人实验室攻读人工智能专业工程硕士学位。
孙刚（IEEE研究生会员）于2021年在中国大连理工大学获得自动化专业工学学士学位，同年在该校获得控制科学与工程专业工程硕士学位，目前他正在智能机器人实验室攻读人工智能专业博士学位。
他当前的研究兴趣包括激光雷达SLAM以及自主旋翼无人机的状态估计。
他目前的研究兴趣包括运动规划和感知感知规划。
他目前是中国大连理工大学人工智能学院、控制科学与工程学院智能机器人实验室的副教授。他目前的研究兴趣包括自主探索……
张雪涛（IEEE会员）于2015年在中国天津河北工业大学获得电子科学与技术专业学士学位，并于2020年在中国天津南开大学获得控制科学与工程专业博士学位。
自主旋翼无人机的相关研究涵盖导航、运动规划、视觉伺服以及状态估计。
刘艺莎（IEEE会员）分别于2005年和2011年在中国大连理工大学获得控制理论与控制工程专业学士和博士学位。
她目前是大连海事大学信息科学技术学院的教授。她目前的研究兴趣包括移动机器人及无人机的道路检测、路径规划、物体检测与跟踪，以及室外场景理解。
庄燕（IEEE高级会员）分别于1997年和2000年在中国沈阳东北大学获得控制理论与控制工程专业学士和硕士学位，并于2004年在中国大连理工大学获得控制理论与控制工程专业博士学位。
他目前是控制科学与工程学院及人工智能学院的教授。他目前的研究兴趣包括移动机器人的三维环境感知与建模、室外场景理解，以及机器人领域的机器学习。