# 紧耦合激光雷达-视觉-惯性 SLAM与大规模体素占据地图构建
西蒙·博赫$^{1}$、塞巴斯蒂安·巴尔巴斯·莱纳$^{1}$、斯特凡·洛伊特内格$^{1,2,3}$
摘要—自主导航是移动机器人在现实世界中各类应用场景的核心需求之一。除了高精度的状态估计外，对三维环境的合理且全局一致的表征也不可或缺。我们提出了一种完全紧耦合的激光雷达-视觉-惯性 SLAM系统与三维地图构建框架，采用局部子图策略以实现对大规模环境的可扩展性。我们引入了一种新颖的、无需特征对应、具有固有概率特性的激光雷达残差表述方式，该表述仅基于占据场及其对应的梯度。这些残差可被添加到因子图优化问题中：既可作为实时估计的帧到地图因子，也可作为用于对齐各子图的地图到地图因子。实验验证表明，该方法不仅实现了业界领先的位姿精度，还能生成全局一致的体素占据子图，可直接用于导航或探索等下游任务。
## I. 引言
鲁棒且精准的状态估计是机器人系统自主导航的基础组件之一。但定位只是问题的一部分：对机器人所处三维环境的精准且全局一致的表征同样不可或缺。融合立体视觉、惯性测量单元（IMU）或激光雷达（LiDAR）等多种传感器源的同时定位与建图（SLAM）方法，已被证明在定位方面能取得优异性能。通过融合互补传感器以克服单一传感器在恶劣场景下的性能退化问题，可提升系统的鲁棒性。目前大多数先进的激光雷达-视觉-惯性（LVI）SLAM系统均采用特征或点云来表征三维世界，例如[^1]、[^2]、[^3]、[^4]。近期，面片也被应用于基于激光雷达的SLAM系统[^5]。尽管这些表征方式可能适用于状态估计与表面重建，但无法直接用于机器人路径规划等下游任务，因为这类任务需要显式表征已观测到的自由空间。
正因如此，近期学界开始研究将SLAM与体素映射相结合。为应对可能导致地图精度下降及一致性受损的位姿漂移问题，
Fig. 1: Horizontal slices of the occupancy fields and 3D reconstructions from Sequence Exp21 of the HILTI SLAM Challenge [^8]. All submaps are overlayed. Meshes of the surfaces (brown) are extracted from the occupancy field as zero-crossings. The blue area denotes free space extracted for gravity-aligned slices through the submaps. The estimated trajectory is shown in green.
近年来，子图方法受到了广泛关注。其思路是将环境划分为多个局部子图，由于局部漂移范围有限，这些子图能保持精准性与一致性。关于如何创建这些子图以及如何保证子图间的全局一致性，已有多种策略被提出，例如在[^6]或[^7]中有所探讨。
现有大多数方法都将SLAM与体素映射问题解耦，通过松耦合的优化问题来调整所有子图之间的相对位置与朝向。相比之下，我们的目标是构建一个完整的统一紧耦合问题，以确保三维地图表征始终保持一致，从而能够适配机器人应用场景。简而言之，本研究的主要贡献如下。
- 我们提出了一种紧密耦合的、基于优化的LVI-SLAM与占据图映射框架，该框架利用局部子图来确保全局一致性。
- 我们针对激光雷达误差项提出了一种新颖的残差公式，该公式直接作用于占据值和场梯度。这两者都可以从子图中高效查询，无需像迭代最近点（ICP）方法那样进行昂贵的数据关联步骤。这些残差可以通过两种方式添加到因子图中：要么为每个实时帧添加因子，要么在子图之间添加因子。
- 我们从定位精度的定量角度以及地图质量的定性角度对该方法进行了评估。结果表明，我们的系统能够为不同型号的激光雷达传感器生成全局一致的地图，同时实现业界领先的定位性能。
---
本文的其余部分结构如下：第II节简要概述相关工作，第III节阐述问题陈述与符号说明。第IV节勾勒出系统的整体架构，随后第V节和第VI节分别介绍映射与估计方法。第VII节给出了实验评估结果。
### II. 相关工作
在基于激光雷达的SLAM方法领域，LOAM [^9] 是最早取得突破且至今仍有影响力的研究成果之一。其核心思想是提取边缘和平面等几何特征，并在不同帧间进行匹配。这一基本概念被后续众多研究广泛采用。虽然LOAM也能以松散耦合的方式融合惯性测量单元（IMU）数据，但LIO-SAM [^10] 则构建了紧密耦合的位姿图优化模型，将边缘与平面误差残差与IMU误差残差融合在一起。此外，诸如FAST-LIO [^11] 或FAST-LIO2 [^12] 这类基于滤波的方法，也成功实现了激光雷达与IMU数据的高精度定位融合。其中FAST-LIO仍使用平面和边缘特征，而FAST-LIO2则通过直接处理原始点云实现了显著的加速。[^13] 提出了用于激光雷达扫描的滑动窗口光束法平差（BA）技术。
近年来，激光雷达-视觉-惯性SLAM系统越来越受到青睐。其中大量方法采用了LOAM的几何特征提取思路。LVI-SAM [^3] 将两个子系统——视觉-惯性（VI）子系统与激光雷达-惯性（LI）子系统——以紧密耦合的方式结合，以便在复杂场景中相互补充。LI子系统基于IMU预积分误差以及边缘和平面残差来构建因子图。VILENS [^1] 同样构建了一个优化问题，该问题包含视觉、惯性、腿部里程计以及基于激光雷达的线条和平面残差。另一类研究则采用基于多状态约束卡尔曼滤波器（MSCKF [^14]）的方法，来实现视觉、惯性和激光雷达数据的紧密耦合滤波融合。相关例子包括[^15] 和[^16]。
为了尽量减少长期场景中的漂移，最新研究通常采用子地图的概念。这一概念源自早期的SLAM研究，例如Atlas框架[^17]。在此背景下，可以推导出额外的因素来对齐子地图并消除漂移。[^1]利用局部点云子地图，并将ICP里程计测量结果加入因子图中。Wildcat[^5]是一种基于滑动窗口优化的激光雷达-惯性里程计系统，它通过构建局部曲面子地图并在位姿图优化中对齐这些子地图，从而实现了顶尖的鲁棒性和精度。上述大多数工作都采用基于特征或表面的三维表示方法。虽然这些方法或许足以满足高精度定位与三维重建的需求，但由于无法表示所观测到的自由空间，它们并不适用于导航。
已有多项研究探讨了基于体素图的子地图构建方法。[^18]基于OctoMap[^19]构建占据子地图，并通过标准的ICP配准方法对齐它们。Voxgraph[^6]利用视觉-惯性里程计提供位姿，以便将其整合到TSDF地图中。完成后会生成ESDF场，并基于ESDF值所对应的无对应关系的误差项，在后端位姿图优化中对齐子地图。新子地图以固定频率创建。Wang等人[^20]则采用占据图作为三维表示方法。他们使用Supereight2[^21]这种自适应分辨率映射方法，根据行驶距离来生成新子地图。子地图会根据视觉-惯性估计器的更新结果重新排布。后续研究[^7]通过评估新扫描数据的点云重叠情况来改进子地图创建过程，子地图的对齐则基于ICP算法。在本研究中，我们将采用子地图的概念。与[^6]、[^20]、[^7]不同，子地图的全局对齐并非与估计器分离，而是直接提供反馈。我们像[^6]中那样构建了无对应关系的残差项，但无需耗费大量资源来提取ESDF数据，因为我们直接利用了现有的占据信息。
### III. 预备知识
### A. 问题陈述
在本研究中，我们的目标是构建移动机器人周围环境的精确且全局一致的体素表示，以便用于后续任务，尤其是导航。我们以视觉-惯性SLAM系统OKVIS2[^22]为起点，同时采用自适应分辨率占据图映射框架Supereight2[^21]进行改进。由于视觉-惯性SLAM系统在局部具有一致性，但会随着时间推移积累较大的漂移，因此我们采用子地图策略来解决地图质量下降的问题。此外，为了提高估计器的精度并确保重叠地图的全局一致性，我们以紧耦合的方式将基于帧到地图以及地图到地图对齐因子的激光雷达约束集成到状态估计器中。
### B. 符号说明
在本研究中，将采用以下记号：坐标系记作 $\underline{\pmb{F}}_{A}$，在该参考系下表示的向量记为 $_{A}\mathbf{r}$。将点从参考系 $\underline{\pmb{F}}_{B}$ 变换到另一参考系 $\underline{\pmb{F}}_{A}$ 的刚体变换由 $\pmb{T}_{AB} \in SE(3)$ 表示，它可分解为旋转矩阵 $\mathbf{C}_{AB} \in SO(3)$ 和平移分量 $_{A}\mathbf{r}_{B}$。我们还用单位四元数形式 $\mathbf{C}_{AB}$ 来表示旋转 $\mathbf{q}_{AB}$。最常用的参考系包括：固定的世界参考系 $\underline{\pmb{F}}_{W}$、$\underline{\pmb{F}}_{C_i}$ 个摄像头对应的相机坐标系 $i = 1 \ldots N$、IMU传感器坐标系 $\underline{\pmb{F}}_{S}$、激光雷达传感器坐标系 $\underline{\pmb{F}}_{L}$ 以及地图坐标系 $\underline{\pmb{F}}_{M}$。此外，$[\cdot]^{\times}$ 表示一个三维向量的斜对称矩阵。
### IV. 系统概述
在本研究中，我们构建了一个完全紧耦合的系统，结合了体积建图与激光雷达-视觉-惯性（LVI）SLAM技术。图2给出了该系统的概览。系统输入包括IMU测量数据、双目相机帧以及
---
Fig. 2: High-level structure of the proposed tightly-coupled LVI-SLAM and submapping framework. Motion compensation for LiDAR point clouds is performed to formulate live frame-to-map factors and to update the current active submap. Upon submap completion, the most overlapping previous submap is determined and map-to-map factors are fused into the optimisation problem.
可能无序的激光雷达点云。通过利用状态估计结果和积分到各点时间戳的IMU测量数据，可进行运动补偿以抵消激光雷达传感器的动态移动影响。将测量数据转换到相应的地图坐标系后，便会更新当前活动子图的占据场。子图模块与激光雷达-视觉-惯性估计器之间的紧耦合通过两种方式实现：针对最新的双目帧所对应的状态，构建实时帧到地图的约束因子；当活动子图构建完成后，会找出重叠度最高的子图，并将地图到地图的约束因子添加到LVI状态估计器中。这些约束因子以及子图策略的相关细节将在后续章节中详细说明。
### V. 占据建图与子图划分
作为一种体积占据建图方法，我们采用了基于八叉树的多分辨率体积建图框架Supereight2 [^21]，专门用于适配各类激光雷达传感器。
### A. 动态激光雷达传感器的占据建图
与 [^20] 的方法不同，我们并未将激光雷达点云的距离图像投影作为建图系统的输入。为了应对传感器动态移动的情况，我们为Supereight2添加了基于射线的积分接口，
Fig. 3: Inverse sensor model used in Supereight2 [^21] for two measurements (1m, 5m). The log-odds occupancy probability along a ray measurement is expressed as a function of the difference $d_{r}$ between query points along the ray and the measured distance $z_{r}$. The occupancy values are clipped in front of the surface at a minimum $L_{min}$ reached at $3\sigma$ where $\sigma$ is a distance dependent uncertainty value. It grows linearly up to half the surface thickness $\tau(z_{r})$. For more details, see [^21].
该接口能够按射线逐条融合测量数据。我们采用较为常见的对数几率空间占据建图方法，配合简化的逆传感器模型，来近似描述传感器在测距或深度不确定性以及异常值概率方面的概率特性。在步骤k时，地图坐标系 $l^k(_{M}\mathbf{p})$ 中三维点 $M\mathbf{p}$ 的对数几率占据值 $\mathbf{\mathcal{F}}_M$ 可由下式给出：
$$ l^{k}\left(_{M}\mathbf{p}\right)=\log\frac{P_{\mathrm{o c c}}\left(_{M}\mathbf{p}|z_{r}\right)}{1-P_{\mathrm{o c c}}\left(_{M}\mathbf{p}|z_{r}\right)}, $$
其中 $z_{r}$ 是单条射线的测量距离，$l^{k}$ 遵循图3所示的沿射线的分段线性函数。
采用限幅权重后，我们应用与 [^21] 中相同的贝叶斯加法更新规则：
$$ \begin{aligned}\bar{L}^{k}\left(_{M}\mathbf{p}\right)&=\frac{\bar{L}^{k-1}\left(_{M}\mathbf{p}\right)w^{k-1}+l^{k}\left(_{M}\mathbf{p}\right)}{w^{k-1}+1}\\w_{k}&=\min\left\{w_{k-1}+1,w_{\max}\right\}\end{aligned} $$
累积的对数几率仍可表示为：
$$ \begin{array}{r}{\bar{L}^{k}\left(_{M}\mathbf{p}\right)=\bar{L}^{k}\left(_{M}\mathbf{p}\right)w_{k}.}\end{array} $$
为了保持八叉树表示的一致性并尽可能降低分辨率，我们采用了与 [^21] 中类似的向上传播和树剪枝策略。
### B. 子地图策略
与 [^7] 类似，我们的目标是利用现有的传感器信息作为生成新子地图的决策依据：我们会逐帧评估与已观测空间对应的测量数据占总测量数据的比例。如果该比例低于某个阈值 $\lambda_{\text{overlap}}$，就会按照 [^22] 中的方法，利用下一个生成的视觉关键帧来创建新子地图。每个子地图都锚定在对应关键帧的时间步长k处的IMU坐标系 $\mathcal{F}_S$ 中。所有子地图都会与其对应的关键帧位姿 $\mathcal{T}_{WS^k}$ 一同存储。
### VI. 激光雷达-视觉-惯性估计器
接下来，我们将介绍OKVIS2 [^22] 因子图的扩展内容。
### A. 因子图与优化问题
本文采用的因子图表示如图4所示。除了OKVIS2中已有的视觉、惯性和位姿图因子外，图中还添加了两种不同类型的激光雷达因子。对于实时优化窗口中的每个状态，都会添加相对于最后一个已完成的子地图的激光雷达因子。此外，还会添加地图间激光雷达因子，通过聚合不同子地图间的测量数据来约束子地图之间的关系。待估计的状态向量如下：
$$ \mathbf{x}=\left[_{W}\mathbf{r}_{S}^{T},\mathbf{q}_{W S}{}^{T},{}_{W}\mathbf{v}^{T},\mathbf{b}_{\mathrm{g}}^{T},\mathbf{b}_{\mathrm{a}}^{T}\right]^{T}, $$
其中，$w_{\mathbf{r}_S}$ 和 $\mathbf{q}_{WS}$ 分别表示固定世界坐标系中IMU传感器坐标系的位置和方向，$w_V$ 描述IMU相对于世界坐标系的速度。$\mathbf{b}_g$ 和 $\mathbf{b}_a$ 分别对应陀螺仪和加速度计的偏差。
---
● (many) landmarks ⚫ keyframe pose (with observ.) ⚫ non-key pose (with observ.) ● submap keyframe ● posegraph pose (no observ.) ● speed/biases ⚫ many reprojection errors ⚫ IMU error ⚫ posegraph errors ⚫ frame-to-map errors ⚫ map-to-map errors ⚫ pose prior ⚫ speed/bias prior ⚫ estimated variable ⚫ fixed parameters ⚫ active error term ⚫ inactive error term
Fig. 4: Optimisation Factor Graph of the LiDAR-Visual-Inertial Estimator. Left: The real-time estimator connects set of current keyframe states and non-keyframe states by IMU errors and visual reprojection errors. It also shows the active submap keyframe and the last completed submap keyframe. For every state in the optimisation window, we can formulate live LiDAR factors between every live frame and the last completed submap. Right: OKVIS2 connects keyframe poses through relative pose errors at a later stage. In addition to the live LiDAR factors, measurements between frames can be aggregated and map-to-map LiDAR factors can be added to the factor graph. Every map will be connected to the previous submap and optionally an older submap if the geometric overlap surpasses a threshold.
### B. 误差残差
为方便起见，在推导新的激光雷达因子之前，我们先对原始的OKVIS2因子进行简要总结。
1) 视觉重投影误差：OKVIS2 [^22] 采用第j个路标在第i个相机坐标系下、时间戳k时的2D重投影误差 $\mathbf{e}_{\mathrm{r}}^{i,j,k}$，以及其在图像中的对应观测值 $\tilde{\mathbf{z}}_{\mathrm{r}}^{i,j,k}$，计算公式如下：
$$ \mathbf{e}_{\mathrm{r}}^{i,j,k}=\tilde{\mathbf{z}}_{\mathrm{r}}^{i,j,k}-\mathbf{h}\left(\mathbf{T}_{S C_{i}}^{-1}\mathbf{T}_{S^{k}W^{}W}\mathbf{l}^{j}\right). $$
其中，h( $\cdot$) 表示相机投影函数。
2) 惯性测量单元误差：在构建惯性测量单元残差时，OKVIS2采用了 [^23] 中的惯性测量单元预积分方法。在时间步长k到n之间，惯性测量单元误差为：
$$ \mathbf{e}_{\mathrm{s}}^{k}=\tilde{\mathbf{x}}^{n}\left(\mathbf{x}^{k},\tilde{\mathbf{z}}_{\mathrm{s}}^{k,n}\right) 囗 \mathbf{x}^{n}, $$
其中，$\tilde{\mathbf{x}}^{n}$ 是根据当前状态 $\mathbf{x}^{k}$ 和惯性测量单元测量值 $\tilde{\mathbf{z}}_{\mathrm{s}}^{k,n}$ 得到的任意时间n处的预测状态。$\boxminus$ 执行常规的减法运算，但四元数除外（详见 [^22]）。
3) 相对位姿误差：此外，时间步长r和c之间的相对位姿误差 $\mathbf{e}_{p}^{r,c}$ 表示为：
$$ \mathbf{e}_{\mathbf{p}}^{r,c}=\mathbf{e}_{\mathbf{p},0}^{r,c}+\left[\begin{matrix}{s^{r}\mathbf{r}_{S^{c}}-s^{r}\tilde{\mathbf{r}}_{S^{c}}}\\ {\mathbf{q}_{S^{r}S^{c}}\boxminus\tilde{\mathbf{q}}_{S^{r}S^{c}}}\\ \end{matrix}\right]. $$
其中，$S^r\tilde{\mathbf{r}}_S^c$ 和 $({\tilde{\mathbf{q}}_S}^{r_S})^{c}$ 是在IMU坐标系 $\underline{\mathscr{F}}_S$ 中表示的名义相对位置和方向。关于常数 [^22] 以及误差雅可比矩阵和误差权重 $\mathbf{e}_{p,0}^{r,c}$ 的详细推导过程，请参阅 $\mathbf{W}_{p}^{r,c}$。
4) 基于子地图的激光雷达误差：如前一节所述，存在两种类型的激光雷达残差。在这两种情况下，误差残差都是基于两种状态来构建的。一方面，对于实时残差，这两种状态由当前帧和与上一个已完成的子地图相关联的关键帧给出；另一方面，对于地图间残差，这两种状态则是子地图锚定帧。不过，在误差残差的构建方式上，两者并无差异。
给定一个以关键帧位姿$T_{WS^a}$为锚点的已完成子地图，以及与另一个状态$\mathcal{P}_b$相关联的点云$T_{WS^b}$，我们为每个$S^b \mathbf{p} \in \mathcal{P}_b$构建残差如下：
$$ e_{1}^{a,b}\left(_{S^{a}}\mathbf{p}\right)=\frac{d}{\sigma}=\frac{L\left(_{S^{a}}\mathbf{p}\right)}{\sqrt{\frac{L_{\min}^{2}}{9}+\sigma_{z}^{2}\left|\nabla L\left(_{S^{a}}\mathbf{p}\right)\right|^{2}}}, $$
其中$S^a \mathbf{p} = T_{S^a S^b S^b} \mathbf{p}$且$T_{S^a S^b} = T_{WS^a}^{-1} T_{WS^b}$。这里的思路是：每个测量点都应位于3D地图中的表面上；该点到最近表面的距离d可以根据占据值$L(\cdot)$和占据梯度$\nabla L(\cdot)$推算得出，前提是假设其遵循与绘图传感器模型一致的线性行为。根据模型（图3），我们可以推导出距离d和地图不确定性：
$$ d=\frac{L}{|\nabla L|},\qquad\sigma_{\mathrm{m a p}}=\frac{L_{\mathrm{m i n}}}{3|\nabla L|}. $$
$L_{min}$是一个配置参数，表示占据概率的最小对数比值饱和度。结合传感器特定的测量不确定性$\sigma_{z}$，我们可以利用总不确定性在公式(8)中构建加权残差。
$$ \sigma=\sqrt{\sigma_{\mathrm{m a p}}^{2}+\sigma_{z}^{2}}. $$
此外，我们希望推导解析雅可比矩阵，为此对围绕线性化点$\delta\chi_T = [\delta\mathbf{r}, \delta\boldsymbol{\alpha}]$和$\overline{\mathbf{r}}$的位姿施加扰动$\overline{\mathbf{C}}$：
$$ \begin{aligned}&\mathbf{r}=\bar{\mathbf{r}}+\delta\mathbf{r},\\&\mathbf{C}=\operatorname{Exp}\left(\delta\boldsymbol{\alpha}\right)\overline{\mathbf{C}}.\\ \end{aligned} $$
我们进一步将误差状态定义为$\delta\chi = [\delta\chi_{T_{WS}}, \delta\chi_{sb}]$，其中$\delta\chi_{sb}$表示速度和偏差的附加扰动。由于激光雷达残差仅依赖于位姿状态，而与速度和偏差无关，因此我们可以利用链式法则，针对帧$\overrightarrow{F}_{S^{a}}$和$\overrightarrow{F}_{S^{b}}$的位姿计算雅可比矩阵：
$$ \frac{\partial e_{1}^{a,b}}{\delta\mathbf{x}_{T_{W S^{m}}}}=\frac{\partial e_{1}^{a,b}}{\partial_{S^{a}}\mathbf{p}}\frac{\partial_{S^{a}}\mathbf{p}}{\delta\mathbf{x}_{T_{W S^{m}}}}, $$
---
其中$m \in \{a, b\}$。公式(12)中的各个雅可比矩阵为：
$$ \begin{align*}\frac{\partial e^{a,b}_{1}}{\partial_{S^{a}}\mathbf{p}}&=\frac{\nabla L}{\sqrt{\frac{L^{2}_{\min}}{9}+|\nabla L|^{2}\sigma^{2}_{z}}}\\\frac{\partial_{S^{a}}\mathbf{p}}{\delta\mathcal{X}_{T_{WS^{a}}}}&=\mathbf{C}_{S^{a}W}\begin{bmatrix}-\mathbf{I}_{3}&\left[\mathbf{C}_{WS^{b}S^{b}}\mathbf{p}+_{W}\mathbf{r}_{S^{b}}-_{W}\mathbf{r}_{S^{a}}\right]^{\times}\end{bmatrix}\\\frac{\partial_{S^{a}}\mathbf{p}}{\delta\mathcal{X}_{T_{WS^{b}}}}&=\mathbf{C}_{S^{a}W}\begin{bmatrix}\mathbf{I}_{3}&-\left[\mathbf{C}_{WS^{b}S^{b}}\mathbf{p}\right]^{\times}\end{bmatrix}.\tag{13}\end{align*} $$
需要注意的是，这种优化的整体思路类似于点对面ICP算法。与常规的点对面ICP算法不同，由于可以直接从占据场中获取占据值和梯度，因此可以省略数据关联和法线计算这一耗时步骤。此外，异常值也会被自动排除，因为它们很可能位于空闲空间或未观测空间中，对应的梯度为零或无效。
5) 优化问题：上述所有因素都被整合到整体的最小化目标中：
$$ \begin{aligned}c\left(\mathbf{x}\right)&=\frac{1}{2}\sum_{i}\sum_{k\in\mathcal{K}}\sum_{j\in\mathcal{J}(i,k)}\rho\left(\mathbf{e}_{\mathrm{r}}^{i,j,k^{T}}\mathbf{W}_{\mathrm{r}}\mathbf{e}_{\mathrm{r}}^{i,j,k}\right)\\&+\frac{1}{2}\sum_{k\in\mathcal{P}\cup\mathcal{K}\backslash f}\mathbf{e}_{\mathrm{s}}^{k^{T}}\mathbf{W}_{\mathrm{s}}^{k}\mathbf{e}_{\mathrm{s}}^{k}+\frac{1}{2}\sum_{r\in\mathcal{P}}\sum_{c\in\mathcal{C}(r)}\mathbf{e}_{\mathrm{p}}^{r,c^{T}}\mathbf{W}_{\mathrm{r}}^{r,c}\mathbf{e}_{\mathrm{p}}^{r,c}\\&+\frac{1}{2}\sum_{k\in\mathcal{K}}\sum_{\mathbf{p}\in\mathcal{L}_{k}}e_{1}^{C,k^{2}}+\frac{1}{2}\sum_{b\in\mathcal{M}}\sum_{a\in\mathcal{A}_{b}}\sum_{\mathbf{p}\in\mathcal{L}_{b}}e_{1}^{a,b^{2}}.\quad(14)\end{aligned} $$
这里，集合$\mathcal{K}$包含了最新帧以及能在$\mathcal{J}(i,k)$中观测到可见地标的关键帧。$\mathcal{P}$包含所有位姿图帧，f表示当前帧。$\mathcal{C}(r) \subset \mathcal{P}$是与帧r相连的所有位姿图帧的集合。此外，集合$\mathcal{L}_k$表示与帧k相关的所有激光雷达测量数据的集合。$\mathcal{M}$是所有过往子地图的集合，而$\mathcal{A}_b$是通过地图间残差与子地图帧$\boldsymbol{T}_{WS^b}$相连的所有子地图帧的集合。C表示最后一个已完成的子地图帧。
### 七、实验结果
我们将在第七节B部分定量评估所提出的紧耦合激光雷达-视觉-惯性SLAM系统的精度。在第七节C部分，我们将定性地证明该系统能够生成全局一致的子图，这些子图可应用于机器人领域。最后，在第七节D部分，我们将简要分析所提系统的计算性能。
### A. 实现细节
所有实验均在配备3.6 GHz英特尔i7-11700K CPU和64 GB内存的设备上开展。我们采用了谷歌开发的非线性最小二乘优化框架Ceres [^25]，并实现了解析雅可比矩阵。作为创建新子图的阈值$\lambda_{overlap}$，我们选取了0.4这一数值。对于每个实时状态，我们会添加100个帧到地图的残差项；子图生成完成后，会再添加1000个地图到地图的残差项。为此，我们会从两个对应帧之间的聚合点云中随机采样点。子图的最大尺寸为15.36米，最高分辨率为3厘米。
### B. HILTI SLAM挑战赛
我们在HILTI 2022 SLAM挑战赛基准测试[^8]中对我们的方法进行了评估。Hilti-Oxford数据集是通过手持传感器设备采集的，该设备包含Alphasense五摄像头模块、惯性测量单元以及32线Hesai PandarXT-32激光雷达传感器。利用现有的校准数据集，我们使用Kalibr [^26]对多摄像头系统和惯性测量单元与摄像头的外部参数进行了自主校准。我们发现不同序列上的性能显著依赖于所使用的校准参数，因此我们也对惯性测量单元与摄像头的外部参数进行了在线校准。该挑战赛包含8个不同难度的序列，难度等级取决于复杂的环境（如光照不佳）或传感器套件的剧烈运动。在这8个序列中，有3个是在建筑工地采集的（Exp01、Exp02和Exp03），1个是在办公大楼的长走廊中采集的（Exp07），其余4个序列则是在牛津谢尔登剧院内外采集的（Exp09、Exp11、Exp15和Exp21）。挑战赛结束后，又有3个来自建筑工地的序列（Exp04、Exp05和Exp06）被添加到自动评估系统中。评估时提供了毫米级精度的稀疏真实轨迹数据。系统配备了自动评估工具，以绝对轨迹误差（ATE）作为评估指标。首先，使用SE(3) Umeyama对齐算法将估计轨迹与稀疏真实轨迹对齐。对于每一个真实控制点，系统会获取对应的估计值并给出0到10分的评分（误差小于1厘米得10分，误差超过10厘米得0分，更多细节见[^8]）。对于包含N个控制点的数据集j，其总评分$S_{j}$计算公式如下：
$$ S_{j}=\left(\frac{1}{10N}\sum_{i=0}^{N}s_{i}\right)\times100. $$
若总评分为100分，则意味着整个轨迹上的定位误差低于1厘米。
表一展示了采用相同参数时我们的方法在所有挑战序列上的最终评分。我们报告了包含因果估计以及最终捆绑调整的评估结果，并将其与作为基线的OKVIS2 [^22]的视觉-惯性SLAM（含回环闭合）性能进行对比。视觉-惯性基线算法使用了全部5个摄像头，且视觉前端参数与LVI情况一致。此外，我们还将其与在该基准测试中经过评估的其他最先进方法进行了比较，其中包括挑战赛当时的获胜算法Wildcat [^5]。
可以看出，无论是从因果角度还是最终优化评估来看，加入激光雷达因素都能显著提升OKVIS2的精度。此外，所提出的方法达到了最先进的定位精度。目前，最终得分为427.65分，在挑战赛排行榜上位列第7名，共涉及53份提交作品。
---
| Approach | Sensors | Sequence | Score | |
| C | L | V | exp01(e) | exp02(m) | exp03(h) | exp04(e) | exp05(e) | exp06(m) | exp07(m) | exp09(h) | exp11(m) | exp15(h) | exp21(e) |
| OKVIS2 [^22] | ✓ | ✓ | ✓ | 8.46 | 8.18 | 0.00 | 17.14 | 23.33 | 14.29 | 0.00 | 0.00 | 0.00 | 0.00 | 16.64 |
| OKVIS2 [^22] | | ✓ | ✓ | 30.77 | 20.00 | 0.00 | 47.14 | 56.67 | 34.28 | 31.67 | 13.75 | 28.00 | 12.22 | 136.41 |
| VILENS [^1] | ✓ | ✓ | ✓ | 69.23 | 49.09 | 40.00 | - | - | - | 16.67 | 5.00 | 68.00 | 17.78 | 325.77 |
| HKU $^{{*}}$ | ✓ | ✓ | ✓ | 84.62 | 70.00 | 44.71 | - | - | - | 31.67 | 7.50 | 92.00 | 24.44 | 48.00 |
| Wildcat [^5] | ✓ | ✓ | ✓ | 84.62 | 84.55 | 90.59 | - | - | - | 45.00 | 44.38 | 84.00 | 46.67 | 84.00 |
| Ours | ✓ | ✓ | ✓ | 43.08 | 17.27 | 32.35 | 57.14 | 61.67 | 17.14 | 0.00 | 16.25 | 32.00 | 51.11 | 62.00 |
| Ours | ✓ | ✓ | ✓ | 62.31 | 39.09 | 42.35 | 68.57 | 86.67 | 47.14 | 13.33 | 38.12 | 92.00 | 64.44 | 76.00 |
TABLE I: Evaluation scores on HILTI22 SLAM Challenge. Bold: best score, underlined: second best; C: causal evaluation (definition in [^22]); L, V, I: LiDAR, Visual and Inertial measurements are considered; (e/m/h) classifies the difficulty level of the sequence: easy, medium or hard; *based on [^24]. Sequences exp04, exp05 and exp06 greyed out as they are not included in the final score.
需要注意的是，VILENS和Wildcat为了获得报告中的分数，也采用了某种形式的离线批量或位姿图优化。为了更好地理解，可以这么说：这里报告的分数表示最终估计结果的平均ATE在1厘米（甚至更低）到3厘米之间。只有exp07实验例外，由于该场景是一条缺乏显著视觉特征或几何特征的长走廊，其平均ATE达到了约7厘米。
### C. 建图
1) HILTI数据集：图1展示了Exp21实验中创建的所有子图的叠加效果。图中显示了表面网格以及在0.2米高度处对占用场进行的切片展示。从定性上看，这表明我们能够生成精确的三维重建结果，并且得到的占用场可直接用于机器人导航，各子图之间不存在明显的不一致性。
2) 真实世界示例：为了定性展示该方法对不同类型激光雷达传感器的适应性，我们还处理了在Leica BLK2Fly无人机上采集的数据集。该数据集是在一栋办公楼周围的户外区域采集的。该无人机配备了五台摄像头和五个惯性测量单元，其中我们仅用一个进行状态估计。所用的激光雷达传感器是单束双轴旋转式激光雷达传感器。我们的建图方法避免了将其投影到非常稀疏的距离图像中。图5展示了重建结果、轨迹以及占用场的切片。
### D. 时间性能
我们在表II中进一步展示了Exp15实验中地图整合与图优化的时间性能。此处所说的LVI设置1指的是
Fig. 5: Outdoor Example: Reconstruction (brown), horizontal slice of the free space (blue) and estimated trajectory (green).
最初用于I节中所有序列评估的原始设置。在设置2中，我们将实时帧到地图的激光雷达残差数量减少到50个，地图到地图因素则减少到500个。此外，输入点云被下采样了3倍。结果显示，优化速率可从大约10 Hz提高到近15 Hz。通过下采样用于建图的输入点云，也能显著加快地图整合速度；整合速率可从10 Hz提升至25 Hz。需要注意的是，这种速度提升对定位精度的负面影响极小。
| Function | VI only | LVI Setup 1 | LVI Setup 2 |
| Optimisation | 49.0 $\pm$ 16.6 | 98.9 $\pm$ 30.9 | 79.3 $\pm$ 25.0 |
| Batch Integration | - | 98.4 $\pm$ 83.0 | 38.2 $\pm$ 34.5 |
| ATE (Causal) [cm] | 23.5 | 2.5 | 3.8 |
TABLE II: Per-frame average timings in ms and standard deviations for optimisation and map integration on Exp15.
### VIII. 结论
在现实世界中实现自主导航不仅需要高精度的定位，还需要准确且全局一致的3D环境模型。在本文中，我们提出了一种完全紧耦合的激光雷达-视觉-惯性系统，能够同时解决这两个问题。状态估计器的结果被用于把新获取的激光雷达数据整合到局部子图中。我们引入了一种新颖的无对应关系的残差公式，该公式仅利用占用场值和梯度，而这些数据可以高效查询。采用紧耦合方式时，这些激光雷达因子可以作为实时帧与地图之间的约束，或作为跨更长时间段的地图间约束，被加入优化问题中。实验表明，所提出的系统在定位方面达到了业界领先水平，同时能生成一致的占用子图。在未来的工作中，我们计划从多个方面进行改进，包括为激光雷达传感器建立更真实的不确定性模型，或者利用几何特性来智能地对输入的激光雷达扫描数据进行降采样。另一个我们将来要解决的挑战是提升系统在视觉前端容易跟踪失效的复杂场景中的鲁棒性。最后，我们还打算将当前的研究成果扩展为一个完整的探索框架，以便在现实世界中通过子图进行导航。
---
## 参考文献
[^1]: D. Wisth, M. Camurri, and M. Fallon, "Vilens: Visual, inertial, lidar, and leg odometry for all-terrain legged robots," IEEE Transactions on Robotics, vol. 39, no. 1, pp. 309–326, 2022.
[^2]: 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 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 10672–10678.
[^3]: 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). IEEE, 2021, pp. 5692–5698.
[^4]: P. Dellenbach, J.-E. Deschaud, B. Jacquet, and F. Goulette, "Cticp: Real-time elastic lidar odometry with loop closure," in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 5580–5586.
[^5]: M. Ramezani, K. Khosoussi, G. Catt, P. Moghadam, J. Williams, P. Borges, F. Pauling, and N. Kottege, "Wildcat: Online continuous-time 3d lidar-inertial slam," arXiv preprint arXiv:2205.12595, 2022.
[^6]: V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, and J. Nieto, “Voxgraph: Globally consistent, volumetric mapping using signed distance function submaps,” IEEE Robotics and Automation Letters, vol. 5, no. 1, pp. 227–234, 2019.
[^7]: Y. Wang, M. Ramezani, M. Mattamala, S. T. Digumarti, and M. Fallon, "Strategies for large scale elastic and semantic lidar reconstruction," Robotics and Autonomous Systems, vol. 155, p. 104185, 2022.
[^8]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022.
[^9]: J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and systems, vol. 2, no. 9. Berkeley, CA, 2014, pp. 1–9.
[^10]: 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). IEEE, 2020, pp. 5135–5142.
[^11]: W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3317–3324, 2021.
[^12]: 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.
[^13]: Z. Liu and F. Zhang, “Balm: Bundle adjustment for lidar mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3184–3191, 2021.
[^14]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE international conference on robotics and automation. IEEE, 2007, pp. 3565–3572.
[^15]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "Lic-fusion: Lidar inertial-camera odometry," in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 5848–5854.
[^16]: X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 5112–5119.
[^17]: M. Bosse, P. Newman, J. Leonard, M. Soika, W. Feiten, and S. Teller, “An atlas framework for scalable mapping,” in 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422), vol. 2. IEEE, 2003, pp. 1899–1906.
[^18]: B.-J. Ho, P. Sodhi, P. Teixeira, M. Hsiao, T. Kusnur, and M. Kaess, “Virtual occupancy grid map for submap-based pose graph slam and planning in 3d environments,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 2175–2182.
[^19]: A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: An efficient probabilistic 3d mapping framework based on octrees,” Autonomous robots, vol. 34, pp. 189–206, 2013.
[^20]: Y. Wang, N. Funk, M. Ramezani, S. Papatheodorou, M. Popović, M. Camurri, S. Leutenegger, and M. Fallon, “Elastic and efficient lidar reconstruction for large-scale exploration tasks,” in 2021 IEEE
国际机器人及自动化会议（ICRA）。IEEE出版社，2021年，第5035–5041页。
[^21]: N. Funk, J. Tarrio, S. Papatheodorou, M. Popović, P. F. Alcantarilla, and S. Leutenegger, “Multi-resolution 3d mapping with explicit free space representation for fast and accurate mobile robot motion planning,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 3553–3560, 2021.
[^22]: S. Leutenegger, “Okvis2: Realtime scalable visual-inertial slam with loop closure,” arXiv preprint arXiv:2202.09199, 2022.
[^23]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
[^24]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry," 2022.
[^25]: S. Agarwal, K. Mierle, and T. C. S. Team, "Ceres Solver," 3 2022. [Online]. Available:
[^26]: J. Maye, P. Furgale, and R. Siegwart, “Self-supervised calibration for robotic systems,” in 2013 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2013, pp. 473–480.