# PG-LIO：用于鲁棒激光雷达-惯性导航的光度-几何融合技术 里程计 Nikhil Khedekar 和 Kostas Alexis 摘要—激光雷达-惯性里程计（LIO）广泛应用于精确的状态估计与建图，这是自主机器人不可或缺的需求。传统的LIO方法通常依赖于利用激光雷达采集的几何结构来构建约束条件。因此，在缺乏几何结构的情况下，这类方法往往会变得条件不良（即退化）并导致失效。要使LIO能够更广泛地部署，就必须具备应对此类情况的鲁棒性。针对这一问题，我们提出了PG-LIO——一种实时LIO方法，它融合了激光雷达采集的光度与几何信息，以及来自惯性测量单元（IMU）的惯性约束条件。这些多模态信息被整合到基于滑窗优化的因子图中，以实现实时运算。我们在多个数据集上对PG-LIO进行了评估，这些数据集既包含几何结构良好的场景，也包含几何特征相似的场景。在几何结构复杂的环境中，我们的方法能达到与最先进的LIO相当的定位精度；而在退化场景下，其定位精度更是显著提升，甚至优于那些同样融合了强度信息的算法。值得注意的是，在平均速度为7.5米/秒（最高速度10.8米/秒）的条件下，通过一条几何特征相似的隧道时，在1公里的人工操控飞行轨迹上，我们的系统仅产生$\sim 1$米的偏移量。为了回馈社区，我们还将开源我们的源代码。 索引词—激光雷达-惯性里程计 ## 一、引言 由于激光雷达传感器具有高精度且应用范围广泛，它已被广泛用于里程计估计。由于机械旋转式激光雷达的方位角旋转速率通常较低，因此这类传感器通常会搭配IMU使用。激光雷达-惯性系统在各类机器人中得到了广泛应用[^1]：只要这些机器人的尺寸、重量、功耗及计算能力能够满足相关要求。 利用激光雷达几何数据的最常用方法是迭代最近点（ICP）算法[^2]。该算法的核心是通过非线性优化问题，迭代最小化已识别对应点之间的距离度量值。虽然该方法在几何结构良好的环境中表现优异，但现实世界环境往往存在几何特征相似的情况（如隧道、田野），这会导致问题变得条件不良（即退化）。 缓解这一问题一直是众多研究的重点，这些研究从不同角度入手，包括信息分析、约束优化和多模态融合 [^3] [^4] [^5] [^6] [^7] [^8] [^9]。近期的一项研究 [^9] 将退化问题归类为数据缺失问题，因此要解决这一问题，我们需要从某些来源获取额外数据。虽然已有一些方法会融合额外的传感器，比如视觉或热成像相机 [^3]、[^4]、[^6]、[^8]，或是雷达 [^10]、[^11]，但引入额外传感器通常会增加复杂性、重量和功耗，而后者对微型飞行器而言尤为关键。相反，在仅保留激光雷达和惯性测量单元这套传感设备的前提下，我们利用强度信息来应对此类场景。现代激光雷达通常已经能提供强度数据。不过，由于传感器分辨率低、采样离散、存在明显的卷帘效应、投影模型复杂，且强度值受各种环境、传感器和采样特性影响，因此强度信息的利用并非易事。 基于上述背景，本文提出PG-LIO——一种实时、紧耦合的激光雷达惯性里程计方法，它能在因子图中融合光度信息、几何信息以及惯性线索。其中的光度因子采用了基于归一化互相关（NCC）的代价函数，该函数在处理违反亮度恒定假设的激光雷达强度图像时，能更好地适配直接光度误差计算任务。此外，我们还提出了一种可自校准的投影模型，用以修正机械旋转式激光雷达在将点云投影为图像时产生的偏差。几何因子则用于最小化点到平面的误差，而在激光雷达单次方位旋转内采样的惯性测量数据会被作为预积分因子加入其中。我们在多个数据集上对PG-LIO进行了评估，结果显示：在几何条件良好的场景中，其性能与最先进的激光雷达惯性里程计方法相当；而在几何特征相似的环境中，其精度相比同样利用强度信息的先进激光雷达惯性里程计方法COIN-LIO [^12]有显著提升。我们也会公开源代码，以供业界参考使用。 本文其余部分的结构如下：第二节综述相关研究，第三节详细介绍我们的方法，第四节展示评估结果，第五节给出结论。 ### 二、相关工作 本节我们将回顾激光雷达-惯性里程计领域中关于强度信息利用/辅助的相关文献，若想了解更全面的激光雷达惯性里程计研究概况，可参考[^13]。 虽然强度信息已在激光雷达里程计中被应用 [^14] [^15] [^16]，但在激光雷达-惯性里程计中的应用却相对较少。[^17]中的作者专注于固态激光雷达，除了基于几何曲率的边缘点和平面点 [^18]外，他们还会从点云的平面区域中提取强度边缘点。他们维护着一个由强度边缘点构成的地图。 --- 并在该地图中查找点对线对应关系，这一过程与其几何子系统类似。 在先进的几何LIO基础上，[^19]和[^20]中的研究为几何地图中的点添加了反射率信息。当寻找最近邻点时，系统会查询这些信息，并利用它们来构建代价函数以最小化反射率差异。不过，强度梯度通常具有局部性，而地图的低密度（0.2米）无法记录这些细节。此外，他们还提出了一种用于激光雷达的投影模型，以解决光束间距不均匀的问题。然而，该模型并未考虑投影过程中与距离相关的误差偏差。 COIN-LIO [^12]基于[^20]发展而来，它采用从强度图像中提取的图块进行处理。为了能够利用与距离相关的强度数据而非预处理后的反射率数据，他们首先提出了一种滤波方法以改善图像的亮度一致性。此外，他们还提出了基于退化特性的有效图块选择机制，从而实现两种模态间互补信息的融合。与现有工作相比，我们的方法有两个关键优势使其性能超越现有技术：a) 采用更接近激光雷达实际结构的投影模型；b) 采用了对强度噪声更具容忍度的新因子公式。 ### III. 方法概述 ### A. 符号说明 设W、I、L分别代表世界坐标系、惯性测量单元坐标系和激光雷达坐标系。$(\cdot)_{t}$用于表示时刻t时的量$(\cdot)$。初始化时，世界坐标系与惯性测量单元坐标系重合，即$W = I_{0}$。坐标系A中的点$\mathbf{p}$表示为$\mathbf{p}_{A}$。从坐标系B到坐标系A的旋转由$\mathbf{R}_{AB}$表示。齐次变换$\mathbf{T}_{WI} = \begin{bmatrix} \mathbf{R}_{WI} & \mathbf{p}_{WI} \\ \mathbf{0}_{1 \times 3} & 1 \end{bmatrix}$可将惯性测量单元坐标系中的点$\mathbf{p}_{I}$转换到世界坐标系，即$\mathbf{p}_{W} = \mathbf{T}_{WI} \mathbf{p}_{I}$。假设惯性测量单元与激光雷达已同步，且二者之间存在预先标定的固定外参变换$\mathbf{T}_{IL}$。时刻t的状态$\mathbf{x}_{t}$表示为： $$ \mathbf{x}_{t}=\left(\mathbf{T}_{W I_{t}},\mathbf{v}_{W,t},\mathbf{b}_{I,t}\right) $$ 其中$\mathbf{v}_{W,t}$表示惯性测量单元在世界坐标系中的速度，$\mathbf{b}_{I,t} = \left[\mathbf{b}_{a,t} \quad \mathbf{b}_{g,t}\right]^{\top}$表示加速度计和陀螺仪的偏差。此外，我们用$\mathbf{g}_{W} = g\hat{\mathbf{g}}_{W}$表示世界坐标系中的重力矢量。时刻t的加速度计和陀螺仪测量值分别由$\tilde{\mathbf{a}}_{t}$和$\tilde{\omega}_{t}$表示。 ### B. 整体架构 我们的方法基于因子图，融合了几何、光度及惯性子系统的因子，并通过滑动窗口进行平滑处理以保证实时性。在时刻t，因子图中涉及的变量包括滑动窗口长度w内的机器人状态$\mathbf{x}_i \forall i \in [t-w, t]$以及重力方向$\hat{\mathbf{g}}_W$。由于我们假设了$W = I_0$，因此重力方向是必不可少的组成部分。所维护的因子图如图1所示。 总体而言，输入的点云数据会通过惯性测量单元的测量结果进行去畸变处理，以消除运动带来的失真。 ![Image](images/67b609ca6b0f6c96d447fb2b939a70dc25ef20f4b534a9580481a1e182bb1c2e.jpg)
Fig. 1. Architecture of the maintained factor graph. Factors are represented by squares while variables are represented with circles.
处理后的数据被送入几何与光度子系统。各子系统生成的因子随后被加入因子图，并借助基于[^21]的增量固定滞后平滑器进行优化。 ### C. 初始化 我们假设系统在初始阶段保持静止 $N_s$ 秒（通常为 $N_s = 0.5$ 秒，在IMU测量频率为100 Hz的情况下会产生 $N = 50$ 个样本）。$\mathbf{b}_g$ 被初始化为该时间段内陀螺仪测量的平均值。受 [^22] 的启发，我们设定 $W = I_0$；由于重力加速度大小 $g \approx 9.81 \, \mathrm{m/s^2}$ 在全球范围内都能被高精度测定（参见 [^23]），因此我们仅估计其在W坐标系中的方向。由于 $\mathbf{b}_a$ 通常较小，我们将 $\hat{\mathbf{g}}_W$ 初始化为加速度计测量值的平均值所对应的方向 $\boldsymbol{\mu}_a$。随后，$\mathbf{b}_a$ 被初始化为加速度计测量平均值与重力之间的差值。总结如下： $$ \mathbf{b}_{g,0}=\frac{1}{N}\sum_{i}\tilde{\omega}_{i} $$ $$ \boldsymbol{\mu}_{a}=\frac{1}{N}\sum_{i}\tilde{\mathbf{a}}_{i} $$ $$ \hat{\mathbf{g}}_{W,0}=\frac{\boldsymbol{\mu}_{a}}{|\boldsymbol{\mu}_{a}|} $$ $$ \mathbf{b}_{a,0}=\boldsymbol{\mu}_{a}-g\mathbf{\hat{g}}_{W,0} $$ 几何子系统和光度子系统都在W坐标系中维护各自的地图，因此它们在初始化时直接使用未经去畸变处理的原始点云。对于几何子系统而言，这相当于初始化一个iVox [^24] 地图；而对于光度子系统来说，这只是提取图像块特征并将其存储在数组中而已。 ### D. 惯性子系统 在点云采集期间采样的惯性测量数据会被预积分，以生成IMU预积分因子。 --- [^25] 中的IMU因子不仅约束两个状态之间的相对变换关系，还约束它们相对于W坐标系中重力的绝对状态，并假设重力方向严格垂直向下，即 $\mathbf{g}_W = [0, 0, -1]^\top$。由于我们的W坐标系未必与重力方向对齐，IMU因子会产生强烈的校正梯度，这与光度因子和几何因子不相符。当系统绕横滚轴或俯仰轴的旋转激励很小时，这一问题会更加突出。由于重力加速度大小的估计通常具有很高的精度（参见 [^23]），我们仅对 $S^2$ 的方向进行估计。预积分因子从 [^25] 修改而来，以便将重力方向也作为待估计的变量。 ### E. 几何子系统 在 $[t_s, t_e]$ 期间采集的传入点云首先通过利用此期间的IMU测量数据传播前一状态来进行去畸变处理，得到 $t_e$；随后再利用已知的外参将其转换到 $I_e$ 坐标系中。为提高计算效率，点云会被下采样，然后用于针对整体式地图生成点对面几何因子 [^24]。如果优化后的位姿在平移或旋转方面与用于更新地图的位姿相差较大，那么新的点云就会被转换到W坐标系中以更新地图。这些步骤的详细说明如下。 a) 去畸变：IMU的测量周期为 $\Delta t$ 秒。如同 [^25] 中的假设，我们认为时间t时的IMU测量值 $\tilde{\omega}_t$、$\tilde{a}_t$ 在 $[t, t+\Delta t]$ 时间段内代表恒定的角速度和线性加速度。因此，利用 $\mathbf{x}_t$、$\tilde{\omega}_t$ 和 $\tilde{a}_t$，我们可以得到 $\mathbf{x}_{t+\Delta t}$，其表达式为： $$ \begin{aligned}\mathbf{R}_{WI_{t+\Delta t}}&=\mathbf{R}_{WI_{t}}\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{t}-\mathbf{b}_{g,t}-\boldsymbol{\eta}_{\mathrm{gd},\mathrm{t}}\right)\Delta t\right)\\\mathbf{v}_{W,t+\Delta t}&=\mathbf{v}_{W,t}+g\hat{\mathbf{g}}_{W}\Delta t\\&+\mathbf{R}_{WI_{t}}\left(\tilde{\mathbf{a}}_{t}-\mathbf{b}_{a,t}-\boldsymbol{\eta}_{\mathrm{ad},\mathrm{t}}\right)\Delta t\end{aligned} $$ $$ \begin{aligned}\mathbf{p}_{WI_{t+\Delta t}}&=\mathbf{p}_{WI_{t}}+\mathbf{v}_{W,t}\Delta t+\frac{1}{2}g\hat{\mathbf{g}}_{W}\Delta t^{2}\\&+\frac{1}{2}\mathbf{R}_{WI_{t}}\left(\tilde{\mathbf{a}}_{t}-\mathbf{b}_{a,t}-\boldsymbol{\eta}_{ad,t}\right)\Delta t^{2}\end{aligned} $$ 其中 $\eta_{gd}$、$\eta_{ad}$ 表示离散时间噪声的协方差。更多细节请参阅 [^25]。 方程6用于根据IMU测量数据在 $[t_s, t_e]$ 期间传播状态。此外，方程6还用于获取激光雷达采样时刻的状态。这些状态随后被用来将每个点去畸变到扫描结束时的IMU坐标系下，具体过程如下： $$ \mathbf{T}_{L_{e}L_{t}}=\mathbf{T}_{IL}^{-1}\mathbf{T}_{WI_{e}}^{-1}\mathbf{T}_{WI_{t}}\mathbf{T}_{IL} $$ $$ \mathbf{p}_{I_{e}}=\mathbf{T}_{I L}\mathbf{T}_{L_{e}L_{t}}\mathbf{p}_{L_{t}} $$ 成对的$(t,\mathbf{T}_{L_{e}L_{t}})$会被存储起来，供光度子系统使用。去歪斜处理还会为待优化的新状态$(\mathbf{x}_{e})$提供一个先验。 b) 下采样：现代激光雷达采集的点云数据量特别大，且包含大量冗余信息。为了实现实时操作，我们参照[^19]的做法，先丢弃点云中每4个点中的3个。为了进一步减少计算量，通常会采用体素网格下采样方法[^19]。不过，由于该方法会用体素内的质心来替代采样点，可能会导致边缘处的点被平均化[^26]。因此，我们采用了一种下采样策略[^26]：只有当一个体素内的点数少于20个，且新点与体素内所有点的距离大于0.1米时，才会将该点加入体素。 c) 几何因子：下采样后的点云中的每个点都会利用先验$\mathbf{T}_{WI_e}$变换到世界坐标系下，以便在地图中寻找对应点。如果地图上距离最近的5个点满足以下条件，则判定该对应点有效：它们之间的距离不超过$d_1$、不共线（通过确保点分布的最大特征值小于第二大特征值的3倍来验证），且位于其所代表平面的$d_2$阈值范围内。每个有效对应点会为优化过程贡献一个代价e和一个雅可比矩阵J。 $$ \boldsymbol{e}=\mathbf{n}_{W}\cdot\left(\mathbf{T}_{W I_{e}}\mathbf{p}_{I_{e}}-\mathbf{q}_{W}\right) $$ $$ \mathbf{J}=\mathbf{n}_{W}^{T}\begin{bmatrix}-\mathbf{R}_{WI_{e}}\mathbf{p}_{I_{e}}^{\times}&\mathbf{R}_{WI_{e}}\end{bmatrix} $$ $$ =\left[\begin{pmatrix}\mathbf{p}_{I_{e}}\times\mathbf{n}_{I_{e}}\end{pmatrix}^{T}\quad\mathbf{n}_{I_{e}}^{T}\right] $$ 其中$\mathbf{p}_{I_e}$是下采样点云中的一个点，$\mathbf{n}_W$和$\mathbf{q}_W$分别是地图上对应平面上的法线和点。为了提高计算效率，这些项都会经过白化处理，再使用Huber核进行加权[^27]，最后合并为因子图中的单个稠密因子。 与[^12]中的方法类似，我们利用$\mathbf{J}^\top \mathbf{J}$右下角（平移部分）的特征向量方向所对应的投影定位贡献，来确定需要采集光度特征的方向。 d) 地图管理：本研究采用一种整体式地图[^24]，该地图会按空间间隔进行更新。由于所用激光雷达的水平视场角为$360^{\circ}$，因此当满足以下任一条件时就会更新地图：1）当前位姿与用于更新地图的任意位姿之间的距离超过阈值（2米）；2）当前位姿与最近位姿之间的横滚或俯仰变化幅度达到$30^{\circ}$。 ### F. 光度子系统 去歪斜后的点云和原始点云会被用来生成强度图像。经过去除线条伪影、提升亮度一致性的预处理步骤后，会将地图中的特征投影到当前图像中，以生成基于NCC的 Patch 因子。持续被跟踪的特征会被保留，而失去跟踪的特征则会被删除。新特征会根据几何子系统识别出的退化方向来初始化。 1) 图像生成与预处理：由于输入的点云是有序排列的，因此每个点的强度信息可以很容易地组成图像。这种方法被称为基于ID的投影（PBID）[^20]。我们采用[^12]提出的滤波方法去除线条伪影，并利用亮度图来规整图像中的亮度[^12]。具体来说，先通过垂直方向的高通滤波器分离出线条信号，再通过水平方向的低通滤波器处理，最后从原始图像中减去该信号。亮度值会在较大区域内取平均值。 --- ![Image](images/e1cbb5ac4c529f6d37f7a26837b5b28348f3a7557e8451096a6ffd7ef04a1e9f.jpg)
Fig. 2. LiDAR Projection model used in the Photometric Subsystem. The model explicitly accounts for the $\theta_{a}$ offset in the direction of the ray from the origin to the projection center and the ray from the projection center to the sampled point p.
窗口用于计算曝光一致的图像。随后，该图像会经过 $3 \times 3$ 高斯核平滑处理，以降低噪声。 2) 投影模型：特征中的每个点都会通过投影函数 $\Pi_n(\cdot)$ 被投影到图像中。在 [^12] 中用于将点云中的三维点 $\mathbf{p}_{L_t} = [x, y, z]^\top$ 投影为二维图像坐标 $[u, v]^\top$ 的投影模型如下： $$ \begin{bmatrix}u\\ v\end{bmatrix}=\Pi(\mathbf{p}_{L_{t}})=\begin{bmatrix}f_{x}\operatorname{atan2}(y,x)+c_{x}\\ f_{y}\operatorname{arcsin}(\frac{z}{R})+c_{y}\end{bmatrix} $$ 其中，$f_x = -w/2\pi$、$f_y = -h/\theta_{fov}$、$c_x = w/2$、$c_y = h/2$、$R = \sqrt{L^2 + z^2}$、$L = \sqrt{x^2 + y^2} - n$ 分别表示图像的宽度和高度，$\theta_{fov}$ 是垂直视场角，n 则是激光雷达原点到投影中心的距离。该模型忽略了激光雷达构造中的一个重要方面：位于投影中心的激光头与从原点到激光头的射线之间存在方位角偏移 $\theta_a$。对于激光头在编码器角度 $\theta_e$ 和仰角 $\phi$ 下测得的距离 R，其对应的笛卡尔坐标计算如下： $$ x=R\cos(\theta_{e}+\theta_{a})\cos(\phi)+n\cos(\theta_{e}) $$ $$ y=R\sin(\theta_{e}+\theta_{a})\cos(\phi)+n\sin(\theta_{e}) $$ $$ z=R\sin(\phi) $$ 每个激光头的 $\theta_a$ 值可能不同，且取决于具体的传感器类型；例如，在 Ouster 传感器中，$\theta_a$ 的取值可处于 $[-11^\circ, +11^\circ]$ 范围内（参见 [^28]）。这一现象在图 2 中有直观展示。因此，当利用公式 12 中的模型将点投影到图像时，随着测量距离从 0.3 米变化到 20 米，u 值可能会相差数个像素。这种差异在图 3 中对 $\theta_a = 11^\circ$、$n = 0.02767$、$\theta_e = 30^\circ$、$w = 1024$ 的情况均有体现。虽然 [^20] 中提出的“基于真实角度的校正投影（CPBRA）”方法将这种偏差建模为每个激光环的恒定偏移量，但如图 3 所示，该偏移量会随点的距离而变化。我们转而针对输入点云中的每个点计算偏差偏移量，从而生成一张查找表： ![Image](images/ab27e3faab3ea86f28c5690ecf42abc3a1b32984a3a3925c73fe09f92a1bb791.jpg)
Fig. 3. Variation of projected pixel location u with the sampled range r for the projection model in Equation 12.
该查找表中存储的是用于校正投影位置、使其精确落在 PBID 位置的偏差值。与 CPBRA 不同，该方法考虑了采样距离的影响。最终的投影函数如下所示： $$ \begin{bmatrix}u\\ v\end{bmatrix}=\Pi_{n}(\mathbf{p}_{L_{t}})=\Pi(\mathbf{p}_{L_{t}})+\begin{bmatrix}b_{u}\\ 0\end{bmatrix} $$ 其中，$b_{u}$ 是从生成的查找表中获取的偏差偏移量。对于无效点，查找表中会缺少对应值，此时会通过插值填补每行中的首个有效值。此外，通过插值激光头的仰角（参见 [^12]），v 值可被进一步细化至亚像素级精度。 3) 亮度一致性：强度值是环境参数、激光雷达参数、距离、表面反射率以及入射角的函数（参见 [^29]）。若不进行繁琐的传感器专属校准，要获得亮度一致的强度图像尤为困难（参见 [^30]）。尽管 [^12] 中提出的滤波方法能使强度图像的一致性明显优于原始强度图像，但我们发现这种一致性还不足以支撑“亮度恒定”的假设，也就是说，不能认为连续采集的两个点具有相同的滤波后强度值。 在存在未知仿射亮度变化的情况下，NCC 已被广泛应用（参见 [^31]），因此我们采用它来构建相关因子。具体而言，我们的因子是基于 [^32] 实现的 NCC（公式 9.11）（参见 [^33]）。由于篇幅有限，此处仅简要回顾相关细节，有兴趣的读者可查阅 [^33] 以获取详细研究内容。 4) NCC概述：给定图像$\mathbf{I}$以及在采样位置$\mathbf{\bar{\mathbf{I}}} = \mathbf{I}(\mathbf{X}) \in \mathbb{R}^M$处的强度向量$\mathbf{X} \in \mathbb{R}^{M \times 2}$，可定义归一化函数$\Psi : \mathbb{R}^M \to \mathbb{R}^M$如下： $$ \Psi(\mathbf{\bar{I}})=\frac{\mathbf{\bar{I}}-\mu_{\mathbf{\bar{I}}}}{\sigma_{\mathbf{\bar{I}}}} $$ $$ \mu_{\mathbf{\bar{I}}}=\frac{\mathbf{1}^{\top}\mathbf{\bar{I}}}{\mathrm{M}} $$ $$ \sigma_{\mathbf{\bar{I}}}=\left\|\mathbf{\bar{I}}-\mu_{\mathbf{\bar{I}}}\right\| $$ --- 该函数具有将向量转换为零均值且方差为单位值的有用特性。随后，NCC被定义为两个向量$\overline{S}$和$\overline{T}$的相似性度量，表达式如下： $$ E_{N C C}(\overline{{\mathbf{S}}},\overline{{\mathbf{T}}})=\Psi(\overline{{\mathbf{S}}})^{\top}\Psi(\overline{{\mathbf{T}}}) $$ 该度量值的范围限定在$[-1,1]$内。直观而言，相似的向量会具有$E_{NCC} > 0$。为了用于最小二乘法，可定义误差函数如下： $$ E_{Z N S S D}=||\Psi(\overline{{\mathbf{S}}})-\Psi(\overline{{\mathbf{T}}})||^{2} $$ $$ =2-2E_{N C C}(\overline{\mathbf{S}},\overline{\mathbf{T}}) $$ 该误差函数在[^0]和[^4]范围内有界，其值超过2时显然属于异常值。$\Psi()$是可微分的，相关内容见[^33]，其雅可比矩阵为 $$ \mathbf{J}_{\Psi}=\frac{\partial}{\partial\mathbf{\bar{I}}}\Psi(\mathbf{\bar{I}}) $$ $$ =\underbrace{\frac{\mathbf{I}-\Psi(\mathbf{\bar{I}})\Psi(\mathbf{\bar{I}})^{\top}}{\sigma_{\mathbf{\bar{I}}}}}_{Variance norm.}\underbrace{\left(\mathbf{I}-\frac{\mathbf{1}\mathbf{1}^{\top}}{\mathrm{M}}\right)}_{Zero mean} $$ 5) 地图管理：地图中的特征以点块形式存在。首先从滤波后的强度图像中提取梯度较高的候选点块，随后根据其对方位检测的局部贡献度进行评分并筛选，具体过程如[^12]所述。一旦某个点块在光度因子中被标记为异常值，就会被从地图中移除。 6) 光度因子：将地图中的所有点块投影到当前图像中，以生成基于NCC的误差。 $$ e=\Psi(\mathbf{I}_{j}(\Pi_{n}(\Gamma(\mathbf{P}(\mathbf{X})))))-\Psi(\mathbf{I}_{k}(\mathbf{X})) $$ 其中$\mathbf{I}_k$是首次提取该特征时的原始图像，$\mathbf{P}$表示对应的三维点$\mathbf{p}_W$，$\Gamma(\cdot)$是每个点对应的齐次变换矩阵，$\Pi(\cdot)$是三维到二维的投影函数，$\mathbf{I}_j$则是当前强度图像。通过链式法则可推导出雅可比矩阵如下： $$ \mathbf{J}=\frac{\partial e}{\partial\mathbf{T}_{W I_{e}}} $$ $$ =\mathbf{J}_{\Psi}\mathbf{J}_{I}\mathbf{J}_{\Pi_{n}}\mathbf{J}_{\Gamma} $$ $$ \mathbf{J}_{\Pi_{n}}=\frac{\partial\Pi_{n}(\mathbf{p}_{L_{t}})}{\partial\mathbf{p}_{L_{t}}}=\begin{bmatrix}\frac{-f_{x}y}{x^{2}+y^{2}}&\frac{f_{x}x}{x^{2}+y^{2}}&0\\ \frac{-f_{y}zx}{R^{2}(L+n)}&\frac{-f_{y}zy}{R^{2}(L+n)}&\frac{f_{y}L}{R^{2}}\end{bmatrix} $$ $$ \mathbf{J}_{\Gamma}=\frac{\partial(\mathbf{T}_{L_{e}L_{t}}^{-1}\mathbf{T}_{I L}^{-1}\mathbf{T}_{W I_{e}}^{-1}\mathbf{p}_{W})}{\partial\mathbf{T}_{W I_{e}}} $$ $$ \begin{aligned}=\mathbf{R}_{L_{t}I_{e}}\left[(\mathbf{T}_{WI_{e}}^{-1}\mathbf{p}_{W})^\times\quad-\mathbf{I}_{3\times3}\right]\end{aligned} $$ $J_I$仅仅是采样位置的图像梯度。直到采样步骤的最后一步，每个点都是单独处理的——只有在最后的NCC步骤中，它们才会组合成一个点块。[^12]中提出的畸变校正图用于获取每个$\mathbf{T}_{L_e L_t}$对应的正确$\mathbf{p}_W$。通过跟踪投影中的深度差异并设置$E_{NCC}$的最小阈值来排除异常值。每个单独点块的成本项
TABLE I
NEWER COLLEGE DATASET
ABSOLUTE TRAJECTORY ERROR (RMSE) (m) / RELATIVE ERROR (%)
MethodQuad-HardCloisterStairsPark
Length (m)234.81428.7957.042396.20
KISS-ICP [^26]0.324 / 1.880.297 / 2.07$\times$ / 32.482.871 / 1.06
MD-SLAM [^15]19.639 / 12.360.360 / 2.730.340 / 6.2196.797 / 23.03
Du and Beltrame [^16]18.506 / 16.43259.544 / 19.274$\times$ / 26.121$\times$ / 42.717
LIO-SAM [^34]0.299 / 2.3800.145 / 1.032$\times$ / 5122.3201.566 / 2.064
FAST-LIO2 [^19]0.049 / 0.260.078 / 0.23$\times$ / 3497.220.310 / 0.59
RI-LIO [^20]0.237 / 1.040.285 / 1.33$\times$ / 16877.2889.289 / 5.00
COIN-LIO [^12]0.046 / 0.290.078 / 0.280.102 / 0.740.287 / 0.54
Ours0.054 / 0.350.097 / 0.550.077 / 0.780.331 / 0.61
会被合并为一个单一的光度因子，再添加到因子图中。 ### 四、评估 我们在一组相关数据集上评估了所提出的方法，结果不仅表明其在几何条件良好的场景下可与最先进方法相媲美，更重要的是证明了其在几何退化环境中具有更高的鲁棒性和精度。表I和表II中[^12]、[^15]、[^19]、[^20]、[^26]、[^34]对应的数值均由[^12]报告，因此按照其惯例，我们也报告了均方根误差（RMSE）、绝对轨迹误差（ATE）以及以米为单位的相对误差（RE），其中相对误差是在10米增量下由开源evo工具包[^35]评估得出的。 ### A. Newer College数据集 为了展示我们方法的总体性能，我们首先在Newer College数据集的部分序列上评估该方法[^36]，并将结果列于表I中。该数据集由手持式Ouster OS0-128激光雷达在校园的不同区域采集数据构成。在所有序列中，我们的方法与最先进的LIO方法表现相当。图4展示了Park序列的累积配准点云，以及估计轨迹和真实轨迹的可视化结果。 ### B. ENWIDE数据集 其次，我们在ENWIDE数据集上测试了我们的方法[^12]，结果见表II。该数据集包含多个序列，均由手持式OS0-128激光雷达在几何结构复杂的环境中采集数据，其中既有慢速运动序列（后缀为S），也有动态运动序列（后缀为D）。真实位置数据是通过安装在激光雷达上的棱镜，由徕卡全站仪获取的。 在对比的几种方法中，COIN-LIO是唯一一种在任何序列中都不会完全失效的方法。除了TunnelS和RunwayD之外，我们的方法在所有序列中的ATE和RE指标上均显著优于COIN-LIO [^12]。在TunnelS场景中，这是由于估计器未能收敛到正确的重力与加速度计偏差值，进而导致在隧道末端转弯时出现较大误差。而在RunwayD场景中，我们的方法在某一特别灵敏的动作中对旋转的估计出现偏差，从而破坏了地图匹配效果。虽然[^12]在同一时刻也出现了类似的匹配中断现象，但它随后又会在轨迹后续部分重新恢复匹配状态。 --- ![Image](images/43c0023ccd7aeed7e5a2b45e4e5972683a7a19a6afb7acf69381501fd386684d.jpg)
Fig. 4. Top-Down Orthographic view of the accumulated registered pointcloud for the Park sequence of the Newer College Dataset. Inset: Close-up isometric view of the initial courtyard showcasing the accuracy of the reconstruction.
TABLE II
ENWIDE DATASET - ABSOLUTE TRAJECTORY ERROR (RMSE) (m) / RELATIVE ERROR (%)
MethodTunnelSTunnelDIntersectionSIntersectionDRunwaySRunwayDFieldSFieldDKatzenseeSKatzenseeD
Length (m)251.58179.71279.28388.47333.57357.14232.70287.91242.88177.20
KISS-ICP [^26]$\times$ 144.41$\times$ 68.11$\times$ 65.69$\times$ 64.84$\times$ 113.45$\times$ 124.64$\times$ 54.84$\times$ 70.70$\times$ 66.23$\times$ 76.80
MD-SLAM [^15]$\times$ 88.16$\times$ 80.76$\times$ 90.87$\times$ 87.89$\times$ 97.73$\times$ 91.13$\times$ 96.03$\times$ 84.86$\times$ 93.92$\times$ 91.29
Du and Beltrame [^16]$\times$ 58.084$\times$ 56.086$\times$ 60.812$\times$ 57.449$\times$ 67.906$\times$ 63.978$\times$ 72.548$\times$ 69.480$\times$ 93.92$\times$ 74.207
LIO-SAM [^34]$\times$ 2565.621$\times$ 2662.983$\times$ 2022.878$\times$ 2362.314$\times$ 2334.174$\times$ 3984.588$\times$ 2196.344$\times$ 1999.9685.588 / 2.673$\times$ 1485.377
FAST-LIO2 [^19]$\times$ 316.12$\times$ 81.31124.73 / 29.2823.800 / 28.11$\times$ 53.64$\times$ 59.840.163 / 0.579.209 / 16.081.122 / 4.311.02 / 2.38
RI-LIO [^20]$\times$ 70.32$\times$ 63.02$\times$ 49.94$\times$ 188.83$\times$ 52.18$\times$ 79.161.721 / 2.4424.851 / 25.894.79 / 4.34$\times$ 154.19
COIN-LIO [^12]0.743 / 1.600.487 / 1.590.466 / 1.251.912 / 1.691.033 / 1.892.437 / 2.980.232 / 0.850.581 / 1.830.412 / 0.990.592 / 1.61
Ours0.635 / 2.090.425 / 1.450.279 / 0.800.473 / 0.840.491 / 1.003.273 / 1.080.274 / 0.600.206 / 0.880.260 / 0.470.355 / 0.64
由于更接近初始位置，因此ATE值更低。图5展示了所提出方法在IntersectionD序列上相较于COIN-LIO在精度上的提升效果。 ### C. 几何自相似的自行车隧道 最后，我们在一架人工操控的空中机器人采集的数据上评估所提出的方法，该机器人以高速飞行于几何自相似的自行车隧道内。该隧道每500米设有一个休息站，各休息站之间由直径为8米的恒定半圆形截面的直线段连接。隧道壁上混合着普通墙面和装饰性壁画，为强度图像提供了纹理线索。这架空中机器人属于[^37]的变体，搭载了Ouster OS-0-128 Rev7激光雷达以及VectorNav VN100惯性测量单元。机器人从第一个休息站起飞，穿过隧道抵达下一个休息站，再返回起始位置降落，整个轨迹长度达到1047米。飞行过程中的平均速度为7.5米/秒，最高速度可达10.8米/秒。由于[^15]、[^19]、[^20]、[^26]、[^34]在ENWIDE数据集的简单序列中均表现不佳，因此本实验中我们只将我们的方法与COIN-LIO进行对比。图6展示了我们的方法和COIN-LIO生成的累积配准点云。虽然两种方法都能大致还原从第一个休息站到第二个休息站的隧道长度，但COIN-LIO在回程中视觉特征较少的区域丢失了特征信息。相比之下，所提出的方法仅产生了$\sim 1$米的小偏移。尽管这种偏移极小且在重建的地图中不易察觉，但两种方法都存在轻微的z轴偏移现象。 --- ![Image](images/71a4aacf9e5516220e7cfbef8c353b4ca8acc54205f22354feb42d812f13dd5b.jpg)
Fig. 5. Qualitative comparison demonstrating our improved accuracy over COIN-LIO [^12] on the IntersectionD sequence of the ENWIDE dataset. While the section marked in the yellow dashed box shows a lower overall drift achieved by our method, the rest of the map is also more sharp. Additionally, the section in the blue dashed box highlights the overlap of our trajectory against ground truth.
![Image](images/f97cb7bfabdb053152626d1f2d54e276503a558668871af4923be8293b50e10e.jpg)
Fig. 6. Comparison against COIN-LIO [^12] in a 500 m long geometrically self-similar section of a bicycle tunnel collected from an aerial robot manually piloted at an average speed of 7.5 m/s and maximum speed of 10.8 m/s. The total length of the trajectory is 1047 m which is accurately reconstructed by our method.
### V. 结论 本文提出了一种实时、紧密耦合的LIO方法，该方法融合了激光雷达采集的光度与几何信息，以及来自惯性测量单元（IMU）的惯性数据。实验表明，在几何条件良好的环境中，该方法的精度可与最先进的LIO相媲美；而在几何退化环境中，利用强度信息的先进LIO算法相比，本方法的精度更高。为了推动研究发展，相关代码将开源发布。 --- ## 致谢 我们衷心感谢韦斯特兰郡议会为本研究提供进入菲林斯达尔自行车隧道的权限。 ## 参考文献 [^1]: K. Ebadi, L. Bernreiter, H. Biggie, G. Catt, Y. Chang, A. Chatterjee, C. E. Denniston, S.-P. Deschênes, K. Harlow, S. Khattak, L. Nogueira, M. Palieri, P. Petráček, M. Petrlík, A. Reinke, V. Krátky, S. Zhao, A.-a. Agha-mohammadi, K. Alexis, C. Heckman, K. Khosoussi, N. Kottege, B. Morrell, M. Hutter, F. Pauling, F. Pomerleau, M. Saska, S. Scherer, R. Siegwart, J. L. Williams, and L. Carlone, “Present and future of slam in extreme environments: The darpa subt challenge,” IEEE Transactions on Robotics, vol. 40, pp. 936–959, 2024. [^2]: P. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239–256, Feb. 1992. [^3]: N. Khedekar, M. Kulkarni, and K. Alexis, “MIMOSA: A Multi-Modal SLAM Framework for Resilient Autonomy against Sensor Degradation,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Kyoto, Japan: IEEE, Oct. 2022, pp. 7153–7159. [^4]: S. Khattak, H. Nguyen, F. Mascarich, T. Dang, and K. Alexis, “Complementary Multi-Modal Sensor Fusion for Resilient Robot Pose Estimation in Subterranean Environments,” in 2020 International Conference on Unmanned Aircraft Systems (ICUAS). Athens, Greece: IEEE, Sep. 2020, pp. 1024–1029. [^5]: S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super odometry: Imu-centric lidar-visual-inertial estimator for challenging environments,” in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021, pp. 8729–8736. [^6]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping," in 2021 IEEE International Conference on Robotics and Automation (ICRA). Xi'an, China: IEEE, May 2021, pp. 5692–5698. [^7]: T. Wen, Y. Fang, B. Lu, X. Zhang, and C. Tang, "LIVER: A Tightly Coupled LiDAR-Inertial-Visual State Estimator With High Robustness for Underground Environments," IEEE Robotics and Automation Letters, vol. 9, no. 3, pp. 2399–2406, Mar. 2024. [^8]: C. Zheng, W. Xu, Z. Zou, T. Hua, C. Yuan, D. He, B. Zhou, Z. Liu, J. Lin, F. Zhu, Y. Ren, R. Wang, F. Meng, and F. Zhang, "Fast-livo2: Fast, direct lidar-inertial-visual odometry," 2024. [Online]. Available: [^9]: T. Tuna, J. Nubert, P. Pfreundschuh, C. Cadena, S. Khattak, and M. Hutter, “Informed, constrained, aligned: A field analysis on degeneracy-aware point cloud registration in the wild,” 2025. [Online]. Available: [^10]: M. Nissov, N. Khedekar, and K. Alexis, “Degradation resilient lidar-radar-inertial odometry,” in 2024 IEEE International Conference on Robotics and Automation (ICRA), 2024, pp. 8587–8594. [^11]: C. Noh, W. Yang, M. Jung, S. Jung, and A. Kim, “Garlio: Gravity enhanced radar-lidar-inertial odometry,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Atlanta, May. 2025. [^12]: P. Pfreundschuh, H. Oleynikova, C. Cadena, R. Siegwart, and O. Andersson, “Coin-lio: Complementary intensity-augmented lidar inertial odometry,” in 2024 IEEE International Conference on Robotics and Automation (ICRA), 2024, pp. 1730–1737. [^13]: D. Lee, M. Jung, W. Yang, and A. Kim, "Lidar odometry survey: recent advancements and remaining challenges," Intelligent Service Robotics, vol. 17, no. 2, pp. 95–118, 2024. [^14]: T. Guadagnino, X. Chen, M. Sodano, J. Behley, G. Grisetti, and C. Stachniss, “Fast Sparse LiDAR Odometry Using Self-Supervised Feature Selection on Intensity Images,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 7597–7604, Jul. 2022. [^15]: L. Di Giammarino, L. Brizi, T. Guadagnino, C. Stachniss, and G. Grisetti, “Md-slam: Multi-cue direct slam,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2022, pp. 11047–11054. [^16]: W. Du and G. Beltrame, “Real-Time Simultaneous Localization and Mapping with LiDAR Intensity,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). London, United Kingdom: IEEE, May 2023, pp. 4164–4170. [^17]: H. Li, B. Tian, H. Shen, and J. Lu, “An Intensity-Augmented LiDAR-Inertial SLAM for Solid-State LiDARs in Degenerated Environments,” IEEE Transactions on Instrumentation and Measurement, vol. 71, pp. 1–10, 2022. [^18]: J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time,” in Proceedings of Robotics: Science and Systems, Berkeley, USA, July 2014. [^19]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "Fast-lio2: Fast direct lidar-inertial odometry," IEEE Transactions on Robotics, vol. 38, no. 4, pp. 2053–2073, 2022. [^20]: Y. Zhang, Y. Tian, W. Wang, G. Yang, Z. Li, F. Jing, and M. Tan, "RI-LIO: Reflectivity Image Assisted Tightly-Coupled LiDAR-Inertial Odometry," IEEE Robotics and Automation Letters, vol. 8, no. 3, pp. 1802–1809, Mar. 2023. [^21]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, "iSAM2: Incremental smoothing and mapping using the Bayes tree," The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, Feb. 2012. [^22]: T. Lupton and S. Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions,” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61–76, Feb. 2012. [^23]: G. Balmino, N. Vales, S. Bonvalot, and A. Briais, “Spherical harmonic modelling to ultra-high degree of bouguer and isostatic anomalies,” Journal of Geodesy, vol. 86, no. 7, pp. 499–520, 2012. [^24]: C. Bai, T. Xiao, Y. Chen, H. Wang, F. Zhang, and X. Gao, “Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels,” IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 4861–4868, Apr. 2022. [^25]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “Imu preintegration on manifold for efficient visual-inertial maximum-a-posterior estimation,” in Proceedings of Robotics: Science and Systems, Rome, Italy, July 2015. [^26]: I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, “KISS-ICP: In Defense of Point-to-Point ICP – Simple, Accurate, and Robust Registration If Done the Right Way,” IEEE Robotics and Automation Letters, vol. 8, no. 2, pp. 1029–1036, Feb. 2023. [^27]: P. J. Huber, “Robust Estimation of a Location Parameter,” The Annals of Mathematical Statistics, vol. 35, no. 1, pp. 73 – 101, 1964. [Online]. Available: [^28]: "Ouster Sensor Documentation," , [Accessed 11-04-2025]. [^29]: R. A. Hewitt and J. A. Marshall, “Towards intensity-augmented SLAM with LiDAR and ToF sensors,” in 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hamburg, Germany: IEEE, Sep. 2015, pp. 1956–1961. [^30]: K. Viswanath, P. Jiang, and S. Saripalli, "Reflectivity is all you need! Advancing lidar semantic segmentation," 2024. [Online]. Available: [^31]: O. J. Woodford and E. Rosten, “Large scale photometric bundle adjustment,” in Proceedings of the 31st British Machine Vision Conference (BMVC). BMVA Press, 2020. [Online]. Available: [^32]: R. Szeliski, Computer Vision: Algorithms and Applications, ser. Texts in Computer Science. Cham: Springer International Publishing, 2022. [^33]: O. J. Woodford, “Least squares normalized cross correlation,” 2022. [Online]. Available: [^34]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Las Vegas, NV, USA: IEEE, Oct. 2020, pp. 5135–5142. [^35]: M. Grupp, “evo: Python package for the evaluation of odometry and slam.” , 2017. [^36]: L. Zhang, M. Camurri, and M. Fallon, “Multi-Camera LiDAR Inertial Extension to the Newer College Dataset,” arXiv:2112.08854 [cs], Dec. 2021. [^37]: P. D. Petris, H. Nguyen, M. Dharmadhikari, M. Kulkarni, N. Khedekar, F. Mascarich, and K. Alexis, “RMF-Owl: A Collision-Tolerant Flying Robot for Autonomous Subterranean Exploration,” in 2022 International Conference on Unmanned Aircraft Systems (ICUAS). Dubrovnik, Croatia: IEEE, Jun. 2022, pp. 536–543.