# D-LIOM：紧密耦合的激光雷达-惯性里程计与建图系统 王钟 $^{®}$、张林 $^{®}$（IEEE高级会员）、沈颖 $^{®}$、周毅聪 $^{®}$（IEEE高级会员） 摘要—基于激光雷达与惯性传感器融合的同时定位与建图技术，是诸多自动化应用中的关键技术。近年来，基于几何特征的方法不断发展，通过紧密耦合的估计方式取得了令人瞩目的成果。然而，这类基于特征的技术与激光雷达的扫描机制密不可分，依赖于稳定的特征检测，因此难以适配多激光雷达系统。另一方面，少数“直接”解决方案会将原始点云与已构建的概率地图进行配准，这种方式计算效率更高且易于扩展。但现有的直接方法均为松散耦合模式，无法修正惯性测量单元（IMU）的偏差，因此仅适用于二维场景。针对这一问题，我们提出了D-LIOM——一种紧密耦合的激光雷达-惯性里程计与建图框架。在D-LIOM中，激光雷达扫描数据会直接与概率子图进行配准，同时整合激光雷达里程计、IMU预积分以及重力约束，在子图的时间窗口内构建局部因子图，从而实现系统的实时高精度位姿估计。此外，为消除时间累积误差，我们采用回环检测机制，基于投影后的二维子图的相互匹配来调整稀疏位姿图，确保D-LIOM能在大规模场景中稳定运行。另外，为提升其对不同传感器组合的适应性，D-LIOM支持多激光雷达输入，并能借助通用的六轴IMU完成初始化。大量实验表明，D-LIOM在建图效果、定位精度以及时间效率方面均显著优于现有最先进的方法。最后，为确保研究结果完全可复现，我们已将所需的数据和代码开源发布。相关介绍视频也可在官网查看。 关键词—激光雷达-惯性里程计、同步定位与建图、回环检测、数据融合。 稿件于2021年12月5日收稿，2022年3月10日修订，2022年4月14日录用。2022年4月19日发表，2023年9月8日发布当前版本。本研究部分得到了国家重点研发计划项目（项目编号2020YFB2103900）、国家自然科学基金项目（项目编号61973235和61972285）、上海市科技创新行动计划项目（项目编号）、上海市教委曙光计划项目（项目编号21SG23）、上海市重大科技专项（项目编号2021SHZDZX0100）以及中央高校基本科研业务费的支持。负责审稿并批准本稿件发表的副编辑为方璐博士。（通讯作者：张林。） ## 一、引言 同步定位与建图（SLAM）是工业自动化、自动驾驶以及测绘领域的关键技术[^1]。在一些无GPS信号的场景下，通常会使用激光雷达和惯性测量单元等主动式传感器来构建SLAM系统。由于激光雷达具备探测距离远、精度高、视角广且不受光照影响等优势，因此在SLAM系统设计中得到了广泛应用[^2] [^3] [^4] [^5]。然而，运动畸变以及稀疏点云之间关系建立的困难，会对激光雷达SLAM系统的发展产生显著负面影响。充分利用惯性测量单元的瞬时运动测量能力，自然有助于姿态估计并提升建图性能。因此，近年来已有多项研究致力于开发激光雷达-惯性SLAM系统[^6] [^7] [^8] [^9] [^10] [^11]。¹ 关于数据融合：通常而言，SLAM系统包含前端和后端两部分。前端负责实时估计载体状态，后端则用于以较低频率消除前端的漂移现象。在激光雷达-惯性SLAM系统的前端，核心问题便是数据融合方式。根据融合过程中是否对惯性测量单元偏差进行估计，现有相关方法可分为松耦合与紧耦合两类。例如，松耦合方法通常仅利用惯性测量单元的数据来提供初始值估计，或者借助广泛应用的卡尔曼滤波器来进行数据融合[^6]、[^7]、[^11]。这些技术在概念上简单且易于实现，但并未充分整合两种传感器的数据。另一方面，一些新兴的紧耦合方法则借助卡尔曼滤波器[^8]、[^12]、[^13]或联合优化[^9]、[^10]、[^14]、[^15]来同时估计系统状态与惯性测量单元偏差。相较于松耦合方法，紧耦合方法由于能够在线估计惯性测量单元偏差，因此能取得更好的效果。 关于扫描配准：激光雷达-惯性SLAM系统前端的另一个重要课题，是如何将新获取的点云与先前的扫描结果进行配准。根据所采用的配准策略，现有方法大致可分为基于特征的方法和直接法两类。自2014年张和辛格基于线特征和面特征点提出LOAM以来[^6]，实际上所有前沿解决方案，如LeGO-LOAM[^7]、LINS[^8]、LIOM[^9]以及LIO-SAM[^10]，均是基于此方法构建的。 --- LOAM中描述的特征。在这类基于特征的技术中，会构建先前扫描与当前扫描之间的特征点相关性以实现配准。尽管这些基于特征的方法能够直接建立点之间的关联以辅助扫描间配准，但它们仍存在若干缺陷。首先，由于激光雷达扫描得到的点云较为稀疏，LOAM中对线点或者面点的提取实际上与传感器的扫描模式高度相关。若要将该算法应用于新型传感器，就必须重新设计相应的特征提取模块。例如，Lin等人在“LOAM-livox”中将LOAM扩展至Livox激光雷达[^16]。其次，由于这种耦合机制，每种激光雷达的特征提取都必须单独进行，因此难以应用于多激光雷达系统。另一方面，有些方法无需提取特征，它们通过将原始点云进行变换以获得最大的地图概率，从而以“直接”方式完成配准。例如，Kohlbrecher等人[^17]、Olson等人[^18]以及Hess等人[^11]采用了类似的扫描匹配策略，利用高斯-牛顿法将扫描数据与地图进行配准，在二维场景下取得了不错的效果。与基于特征的方法相比，直接配准方案的一个显著优势在于它们能适配任何类型的激光传感器，且易于扩展到多激光雷达系统。此外，直接配准方法构建的概率图可无缝用于路径规划等上层应用。然而在三维场景下，由于系统对位姿初始值较为敏感，现有基于直接配准的松散耦合方案仍无法取得令人满意的结果。 关于回环检测：在激光雷达-惯性导航系统的后端，亟需高效且自动化的回环检测来消除累积误差。近年来，计算机视觉技术的迅速普及催生了越来越多的专为3D激光雷达SLAM设计的扫描间回环检测算法。例如，Scan Context[^19]和Iris[^20]通过构建圆形投影图像并从中提取位置特征来实现扫描间的位置识别。还有一些研究试图引入深度学习技术来端到端地学习位置特征[^21]。不过这些算法都有一个前提假设：即发生回环的两次扫描之间的水平角度必须足够接近，否则回环检测的成功率会大幅下降。除了这些扫描间回环检测研究外，还有几种技术旨在从SLAM的系统层面减少累积误差。例如，Hess等人在[^11]中通过暴力式的扫描与子图匹配来建立回环约束；LOAM系列算法[^6]、[^8]、[^9]则依靠低频的扫描与地图配准来消除累积误差；LIO-SAM[^10]则通过经验性距离阈值来检测回环闭合。然而，所有这些系统层面的设计都无法有效检测回环闭合，这会导致系统在大规模场景中长时间运行时，累积误差仍难以被及时消除。 惯性测量单元（IMU）偏差。从另一个角度来看，直接法并不依赖特定的扫描模式，且相比基于特征的方法具有更好的可扩展性。然而，现有的直接法均为松耦合方式，这严重限制了它们在三维场景中的性能。因此，本文提出通过直接配准来实现实时紧耦合位姿估计。此外，在长期SLAM中，可靠且快速的回环检测至关重要，而从稀疏点云中直接提取特征极为困难。一个自然的思路是：利用由多帧点云累积而成的相对密集的地图来构建位置特征，这应该是可行的。为此，我们提出了一种基于子图间匹配的回环检测策略。最终，我们便得到了名为D-LIOM的紧耦合直接激光雷达-惯性里程计与建图框架。 总而言之，一方面，由于对……的估计，紧耦合融合能够实现更高的精度。 在开发D-LIOM的前端时，难点在于如何恰当地将紧耦合优化与直接配准相结合。我们认为，对IMU偏差的合理估计会辅以精确的点云配准，二者可以随时间交替进行以实现深度融合。为实现这一目标，首先采用直接方式将时空同步输入的激光雷达数据配准到三维概率子图中，而非通过特征提取和匹配来完成。随后，为了修正IMU偏差，我们将IMU预积分结果与从直接配准得到的激光雷达里程计信息结合，在子图对应的时间窗口内构建局部因子图。此外，由于可以利用优化后的激光雷达里程计和IMU预积分结果来显式估计重力方向，为了限制横滚角和俯仰角的漂移，我们还进行了在线重力估计，并将其作为先验因子引入局部因子图。最终，通过对因子图进行联合优化，便可得到优化的状态量和偏差，进而对输入的扫描数据进行校正，并为下一轮更新预测初始状态。在D-LIOM的后端，得益于在线重力估计结果，我们能够将三维、与重力对齐的子图精确投影到二维水平面上。进一步借助图像特征点的检测与匹配技术，我们便能高效检测出回环闭合点，并通过子图间匹配建立回环约束。接着，通过对全局位姿图进行优化，即可及时消除累积误差。最后同样重要的是，为了提高对不同传感器配置的适应性，我们的D-LIOM既支持在静态和动态环境下使用常见的六轴IMU进行初始化，也因其直接配准特性而支持多激光雷达输入。 为了评估所提出的D-LIOM的性能，我们在自行采集的数据集上开展了大量实验：包括使用手持设备在校园环境中采集的数据集、由无人机采集的公共室内数据集[^22]，以及由车载平台在复杂城市环境中采集的公共数据集[^23]。实验结果表明，当载体快速旋转或系统运行于大规模场景中时，D-LIOM的性能显著优于现有最先进的方法，不仅能生成更精确的地图结果，还能实现更高的定位精度，同时具备很高的计算效率。 --- 所提出的D-LIOM的特点及我们的贡献可总结如下： 1) 在前端，我们提出了一种紧密耦合的直接激光雷达-惯性里程计方案。该方案无需进行特征提取与匹配，而是直接将扫描数据注册到概率子图中；同时将惯性测量单元（IMU）预积分结果、激光雷达里程计数据以及估计的重力先验信息整合为一个局部因子图，从而能够实时低漂移地联合估计相关状态。 2) 在后端，我们提出了一种通过子图间匹配实现的鲁棒且高效的回环检测方法。该方法大幅降低了计算开销，同时具备极高的精确度和召回率，使得后端能够以低延迟优化全局位姿图，并确保长期建图的一致性。 3) 为适配多种传感器配置，D-LIOM支持在载体静止或运动状态下利用常见的六轴IMU完成初始化，从而使估计出的状态与重力方向对齐，便于D-LIOM与上层应用无缝衔接。此外，得益于直接配准的固有优势，D-LIOM能够支持多激光雷达输入，充分利用具有互补视角的数据，且不会造成明显的效率损失。 4) 为了评估D-LIOM的性能，我们开发了手持设备用于采集数据，通过一系列全面实验充分验证了D-LIOM的有效性。最后，为使我们的研究成果可复现并惠及学术界，所有相关代码和数据均已在线开源发布。 $^{2}$ ### 二、相关工作 在本节中，我们将从点云与其先前扫描数据的配准方式这一角度，综述与我们的D-LIOM高度相关的工作。 ### A. 直接配准方法 迄今为止，基于直接配准的激光雷达-惯性SLAM系统大多针对二维场景设计，且均采用松耦合方式利用IMU信息。一个典型的例子是Kohlbrecher等人提出的二维SLAM系统[^17]，该系统依靠高精度激光雷达通过将二维点云直接与已构建的概率图进行配准来实现增量式建图。为使该二维SLAM系统适配在三维空间运动的平台（如无人机），Kohlbrecher等人借助九轴IMU的方向测量结果，将观测到的点投影到二维平面上。在另一项类似的研究[^11]中，Hess等人提出了著名的Cartographer系统，它整合了直接激光雷达SLAM的一些典型技术与技巧，通过持续构建子图并以直接方式将扫描数据配准到子图中，从而实现快速准确的前端递归。在后端，Hess等人通过将扫描数据逐个配准到子图来构建约束，进而形成稀疏位姿图以供优化。通过反复利用并独立估计IMU观测值，Cartographer能够依据六轴IMU预测近似位姿。得益于IMU信息的一定程度融合以及后端引入的稀疏位姿调整机制，Cartographer目前仍是最先进的二维激光雷达SLAM方案。然而，在Cartographer的三维版本（简称Carto3D）中，一方面，IMU的松耦合递归精度不足，导致其前端配准性能下降；另一方面，其后端的暴力匹配精度和回环检测率均不够高，致使系统随时间推移会产生较大的漂移。 ### B. 基于特征的方法 与直接在扫描数据和现有地图之间进行概率配准的方法不同，大量方案试图通过从激光点云中提取特征点、线条或平面来估计位姿。在这些基于特征的方法中，Zhang和Singh提出的LOAM堪称里程碑式成果[^6]。在LOAM的前端，通过为激光束的曲率设定阈值来提取线特征和面特征。利用这些特征，通过优化点到线以及点到面的误差项来实现高频的扫描间配准。在LOAM的后端，则以较低频率将新扫描的特征点与已构建地图中的特征点进行配准，从而消除累积误差。为了适配嵌入式硬件，Shan等人对LOAM前端的特征点提取过程进行了优化，最终形成了LeGO-LOAM[^7]，它提升了帧间匹配的速度并增强了配准的鲁棒性。然而，当载体高速运动时，上述两种方案的性能会显著下降，原因在于它们对IMU数据的融合较为松散。 为了实现对IMU信息的更深入融合，近年来学界对LOAM特征[^6]与IMU数据之间的紧耦合融合研究兴趣日益浓厚。根据所采用的融合策略，这些紧耦合方法大致可分为基于滤波的方法和基于优化的方法。 基于滤波的方案通常借助卡尔曼滤波器来融合IMU信息。例如，Qin等人提出的LINS[^8]便基于迭代误差状态卡尔曼滤波器，并利用LeGO-LOAM[^7]来提取扫描特征。其前端会递归预测载体的位姿，同时利用激光雷达观测数据来更新IMU偏差，进而提升相对定位精度。与基于滤波的LINS类似，也有多项研究尝试在统一框架内融合激光雷达、IMU和摄像头数据[^24] [^25] [^26]。在[^24]中，Zuo等人提出了类似于MSCKF[^27]（一种基于误差状态卡尔曼滤波器的视觉惯性里程计框架）的LIC-Fusion框架，利用激光雷达测量数据和摄像头观测数据来更新载体状态及IMU偏差。在近期推出的LIC-Fusion 2.0[^25]中，引入了滑动窗口内的点对面跟踪技术，从而对LIC-Fusion[^24]进行了扩展。尽管这些基于滤波的方案计算效率较高，但由于线性化精度较低带来的不利影响，其定位误差会随时间迅速累积。 --- 另一类紧密耦合的方案则采用联合优化的方法来实现有效的数据融合。在[^9]中，Ye等人提出了LIOM算法，该算法将数据融合问题转化为一个多约束优化问题，其中包含了帧间相对运动项、IMU预积分项以及局部滑动窗口内的激光特征点误差项，从而实现了激光雷达与IMU的深度融合。为了消除累积误差，LIOM还借鉴了LOAM中的后端“建图”机制[^6]。尽管局部窗口内的计算负载有限，但LIOM仍受到待优化变量维度庞大的制约。此外，仅依靠后端将扫描数据与局部地图匹配，仍难以消除长期累积的误差。最近，Shan等人在其先前工作LeGO-LOAM[^7]的基础上，提出了LIO-SAM[^10]，该方法采用“平滑与建图”技术，通过对稀疏平滑信息矩阵进行分解[^14]、[^15]，为完整的SLAM问题提供了高效解决方案。LIO-SAM通过构建两个子图来解决SLAM问题：在前端，LIO-SAM利用IMU预积分结果与经地图配准校正后的关键帧来建立约束，进而优化状态；在后端，LIO-SAM则构建包含关键帧因子、GPS因子以及回环闭合因子的因子图，以消除累积误差。通过分解优化问题，LIO-SAM通过将关键帧与地图匹配并基于经验距离阈值触发回环检测，修正了前端的递推误差，从而取得了迄今为止最佳的激光雷达-惯性融合效果。然而，正如我们的实验结果所显示的那样，LIO-SAM的回环检测成功率受经验阈值的严重影响，这大大降低了其在大规模场景中的实际性能。 ### 三、方法论 ### A. 符号说明 为使后续讨论更加清晰，此处预先定义一些用到的符号。 空间坐标系：我们用$(\cdot)^{w}$表示世界坐标系，用$(\cdot)^{b}$表示机体坐标系（IMU坐标系），用$(\cdot)^{l}$表示激光雷达坐标系。假设世界坐标系的z轴垂直于水平面，且初始节点的航向角被用作偏航角的参考。 上标与下标：右下角的标记表示该状态量的所属对象或参考时间，右上角的标记表示参考坐标系，左上角的标记则根据具体语境表示某些特殊属性。 状态与变换：旋转矩阵R或四元数q可无差别地用于表示三维旋转；p和v分别指代载体的三维空间位置与速度；g代表重力加速度。为了表达简洁，我们用T来表示旋转R与平移t的复合变换。 对于某个特定节点$n_k$（其与对应的激光雷达扫描数据具有相同的时间戳k），其所关联的待估计状态被定义为一个元组$(p_{b_k}^w, v_{b_k}^w, q_{b_k}^w, a_b, \omega b)$，其中$^a b$和$\omega b$分别为受随机游走过程影响的加速度计与陀螺仪测量值的偏差。 通用数学符号：$\|\cdot\|$表示对运算变量取$l_2$范数；Exp表示将$\mathfrak{so}(3)/{\mathfrak{se}}(3)$中的元素映射到特殊正交群/欧几里得群$\mathbb{SO}(3)/\mathbb{SE}(3)$中，满足如下关系： $$ \begin{align*}\mathrm{Exp}:\mathfrak{so}(3)&\ni\phi\to\exp(\lfloor\phi\rfloor_{\times})\in\mathbb{SO}(3)\\\mathfrak{se}(3)&\ni\xi\to\exp(\lfloor\xi\rfloor_{\times})\in\mathbb{SE}(3).\end{align*} $$ 给定$\phi = [\phi_x, \phi_y, \phi_z]^T$、$\rho \in \mathbb{R}^{3 \times 1}$和$\xi = [\phi^T, \rho^T]^T$，算子$\lfloor \cdot \rfloor_\times$会生成对应向量的矩阵，如下所示： $$ \left\lfloor\phi\right\rfloor_{\times}=\begin{bmatrix}0&-\phi_{z}&\phi_{y}\\ \phi_{z}&0&-\phi_{x}\\ -\phi_{y}&\phi_{x}&0\end{bmatrix}\mathrm{a n d}\left\lfloor\boldsymbol{\xi}\right\rfloor_{\times}=\begin{bmatrix}\left\lfloor\phi\right\rfloor_{\times}&\boldsymbol{\rho}\\ \mathbf{0}^{T}&0\end{bmatrix}. $$ ### B. 框架概述 我们的D-LIOM整体框架如图1所示。为了帮助大家清晰理解该框架的功能，在此先对其关键模块做简要介绍。 首先，所有输入数据在流入后续步骤前都需要经过预处理。多个激光雷达的点云会与主激光雷达在时间维度和空间维度上对齐，同时也会获取两次扫描之间的惯性测量单元（IMU）预积分结果。在初始化阶段，我们会估算相关状态并将系统与世界坐标系对齐。系统初始化完成后，预处理后的数据会被送入前端——即紧耦合的直接激光雷达-惯性导航里程计模块。通过结合激光里程计、IMU预积分以及重力先验这些因素，前端能够实时输出高精度的位姿估计结果。最后，后端会接收前端生成的子图，通过子图间的匹配来检测回环闭合，进而构建并优化全局位姿图以消除累积误差。 ### C. 测量数据预处理 1) IMU预积分：IMU的原始加速度和角速度读数在任何时刻都来自其自身的坐标系b。IMU的物理测量模型如下： $$ \tilde{a}_{b}^{b}=R_{b}^{w T}(a_{b}^{w}-g^{w})+{}^{a}b+{}^{a}n, $$ $$ \tilde{\omega}_{b}^{b}=\omega_{b}^{b}+{}^{\omega}b+{}^{\omega}n, $$ 其中，$\tilde{a}_b^b$和$\tilde{\omega}_b^b$分别表示加速度计和陀螺仪的测量值；$^a b$和$\omega b$是第III-A节中定义的加速度和角速度偏差，而$^a n$和$\omega n$则分别代表加速度计和陀螺仪的测量噪声。 如果已知载体在世界坐标系中的初始状态，我们就可以根据牛顿运动定律，通过对IMU读数积分来理论推算出t时刻的状态。然而，当我们更新历史状态的估计值时，所有相关状态都必须重新积分，这一过程极其耗时且不切实际。为解决这一问题，我们采用了“IMU预积分”[^28]技术，该技术巧妙地建立了相对运动与原始IMU数据之间的关联。 --- ![Image](images/0ec4169bfec823dfda730d52414e265d41f0957046528b12de8e95a110299853.jpg)
Fig. 1. Framework overview of D-LIOM. In "Measurement Preprocessing," the inputs from multiple LiDARs and an IMU are synchronized to the timestamp of the primary LiDAR, and the IMU pre-integration is used to predict the initial value of the relative motion between two scans. In "System Initialization," all the starting states are roughly estimated and the system is aligned with the world frame. The front-end LiDAR-Inertial odometry receives the result of scan-to-submap matching, IMU pre-integration, and gravity estimation, constructs a factor graph within the time window of the submap, solves it in real time, estimates states, and updates the IMU biases. The back-end integrates the result of submap-to-submap loop detection, constructs a global pose graph, jointly adjusts the poses of submaps and nodes, and eliminates accumulated errors.
根据Forster的研究结果，IMU预积分、真实状态与噪声之间的关系满足以下公式： $$ \begin{align*}\Delta\tilde{\boldsymbol{R}}_{b_{j}}^{b_{i}}&\approx\boldsymbol{R}_{b_{i}}^{w^{T}}\boldsymbol{R}_{b_{j}}^{w}\mathrm{Exp}(\delta\phi_{b_{j}}^{b_{i}}),\\\Delta\tilde{\boldsymbol{v}}_{b_{j}}^{b_{i}}&\approx\boldsymbol{R}_{b_{i}}^{w^{T}}(\boldsymbol{v}_{b_{j}}^{w}-\boldsymbol{v}_{b_{i}}^{w}-\boldsymbol{g}^{w}\Delta t_{ij})+\delta\boldsymbol{v}_{b_{j}}^{b_{i}},\\\Delta\tilde{\boldsymbol{p}}_{b_{j}}^{b_{i}}&\approx\boldsymbol{R}_{b_{i}}^{w^{T}}\left(\boldsymbol{p}_{b_{j}}^{w}-\boldsymbol{p}_{b_{i}}^{w}-\boldsymbol{v}_{b_{i}}^{w}\Delta t_{ij}-\frac{1}{2}\boldsymbol{g}^{w}\Delta t_{ij}^{2}\right)+\delta\boldsymbol{p}_{b_{j}}^{b_{i}},\end{align*} $$ 其中，$\delta\phi_{b_i}^{b_j}$、$\delta\boldsymbol{v}_{b_i}^{b_j}$和$\delta\boldsymbol{p}_{b_i}^{b_j}$是预积分测量的高斯噪声。 2) 激光雷达数据同步与去畸变：由于激光雷达是通过逐点扫描来采集数据的，因此需要从时间和空间两个维度对各个激光雷达的点进行同步。也就是说，首先要根据统一的时间基准对不同激光雷达的点云进行对齐；随后将每个激光雷达坐标系下的点云转换到同一空间参考系中。同时，还需要考虑每个点的畸变校正问题。 时间同步：假设我们有一台主激光雷达和几台辅助激光雷达，每个激光雷达点都有对应的时间戳（所有现代多线激光雷达都具备这一属性）。如图1中的“激光雷达同步”模块所示，我们以主激光雷达当前扫描和上次扫描的时间戳为参考，提取出所有时间戳落在各辅助激光雷达时间间隔内的数据点，再按照时间戳将它们与主激光雷达的扫描点一起重新排序，从而得到融合后的点云。 空间同步与去畸变：在上一步的时间同步中，我们已经得到了按时间顺序排列的点云。同时，通过预先积分上次扫描到本次扫描时间间隔内的惯性测量单元读数，我们可以得到从上次扫描到本次扫描的相对运动近似值，进而对点云进行去畸变处理。假设通过惯性测量单元预积分估算出的相对运动以及上次扫描时的状态为$\boldsymbol{T}_{bt}^{bi}$。应用三维变换规则，时间戳为t的激光雷达点$\boldsymbol{p}_{t}^{l_1} \in \mathbb{R}^{3 \times 1}$转换到坐标系$b_i$下的表达式可写为$\boldsymbol{P}_{t}^{b_i} = \boldsymbol{T}_{bt}^{b_i} \boldsymbol{T}_l^b[\boldsymbol{p}_{t}^{l_1T}, 1]^T$。 ### D. 系统初始化 初始化时需要执行两项任务：一是粗略估计载体的初始状态（包括位置、速度、旋转角和惯性测量单元偏差），二是确定重力方向，从而将所有状态对齐到世界坐标系下。 考虑到初始化时载体可能处于静止或运动状态，接下来的两个小节将分别讨论这两种情况下的初始化方案。 1) 静态初始化：在静止状态下，惯性测量单元（IMU）测得的加速度应与重力加速度方向相反。假设重力加速度的大小已知且恒定为G，那么IMU相对于世界坐标系的相对位姿即可确定。 --- 通过在短时间内将加速度均值向量与重力反向向量对齐，就能得到世界坐标系下的相对位姿$R_{b_0}^w$。此外，该时间段内角速度测量的平均值可视为陀螺仪偏差的合理估计值。自然地，位置、速度和加速度计偏差均被初始化为0。因此，静止状态下的初始化过程可按照如下方式完成： $$ \begin{align*}\hat{\boldsymbol{R}}_{b_{0}}^{w}&=\mathrm{Rot}(\hat{\boldsymbol{g}}^{b_{0}},-\boldsymbol{g}^{w}),\\\hat{\boldsymbol{v}}_{b_{0}}^{w}&=\mathbf{0},\hat{\boldsymbol{p}}_{b_{0}}^{w}=\mathbf{0},{}^{a}\hat{\boldsymbol{b}}_{b_{0}}=\mathbf{0},\\{}^{\omega}\hat{\boldsymbol{b}}_{b_{0}}&=\mathrm{Avg}\left(\sum_{t}\tilde{\boldsymbol{\omega}}\right),\hat{\boldsymbol{g}}^{b_{0}}=\mathrm{Avg}\left(\sum_{t}\tilde{\boldsymbol{a}}\right),\end{align*} $$ 其中，Avg($\cdot$)用于计算时间窗口内测量值的均值，Rot($\cdot$)则返回两个向量之间的旋转矩阵，其计算公式如下： $$ \begin{align*}\boldsymbol{c}&=\hat{\boldsymbol{g}}^{b_{0}}\times(-\boldsymbol{g}^{w})=\boldsymbol{g}^{w}\times\hat{\boldsymbol{g}}^{b_{0}},\\\mathrm{Rot}(\hat{\boldsymbol{g}}^{b_{0}},-\boldsymbol{g}^{w})&=\boldsymbol{I}+\left\lfloor\boldsymbol{c}\right\rfloor_{\times}+\left\lfloor\boldsymbol{c}\right\rfloor_{\times}^{2}\frac{1+(\hat{\boldsymbol{g}}^{b_{0}}\cdot\boldsymbol{g}^{w})}{\left\|\boldsymbol{c}\right\|^{2}}.\end{align*} $$ 2) 动态初始化：受Mur-Artal和Tardøs提出的方法[^29]以及Qin等人针对视觉-惯性SLAM设计的研究成果[^30]的启发，本部分我们将介绍一种载体运动时的激光雷达-惯性系统初始化方法。 陀螺仪偏差初始化：理想情况下，若惯性测量单元数据中没有偏差，角速度的预积分结果应接近激光雷达里程计估算出的旋转量。基于这一假设，我们建立约束方程来估计初始陀螺仪偏差。 为实现上述目标，第一步是重新获取两次激光雷达扫描之间的相对变换关系。 用于获取两次激光雷达扫描之间的相对变换关系。 对于扫描间匹配，“迭代最近点”（ICP）算法[^31]可被用于此目的。 “正态分布变换”（NDT）[^32]是两种常用的传统方法。 ICP通过迭代方式建立点与点之间的对应关系， 而NDT则通过构建概率体素来优化点云之间的变换参数。 相比之下，ICP对初始对齐更为敏感，而NDT由于具备概率特性，对噪声的鲁棒性更强。 因此，我们采用NDT来获得相对变换的合理估计。 相较于初始对齐方法，NDT对噪声的鲁棒性更强， 这得益于其概率特性。因此，我们采用NDT方法。 当物体以微小旋转运动时，满足以下方程： 旋转差的最小值（式(6)）是一个单位四元数， 因此结合式(7)，式(6)所表达的目标函数可进一步转化为， 陀螺仪偏差$\delta b_{\omega}^{*}$可通过以下方式确定： $$ \delta\boldsymbol{b}_{\omega}^{*}=\arg\min_{\delta\boldsymbol{b}_{\omega}}\sum_{k}\|\hat{\boldsymbol{q}}_{l_{0}}^{b_{k}}\otimes\hat{\boldsymbol{q}}_{b_{k+1}}^{l_{0}}\otimes\Delta\tilde{\boldsymbol{q}}_{b_{k}}^{b_{k+1}}\|^{2}, $$ 其中，$\Delta\tilde{\boldsymbol{q}}_{b_k}^{b_{k+1}}$是第k次扫描与第$(k+1)$次扫描之间预积分得到的旋转四元数，$\hat{\boldsymbol{q}}_{l_0}^{b_k}$和$\hat{\boldsymbol{q}}_{b_{k+1}}^{l_0}$是通过NDT配准计算得到的旋转量，运算符“$\otimes$”表示两个四元数的复合旋转。 利用式(11)和式(12)的结果，通过建立窗口内多次激光雷达扫描与IMU读数构成的超定方程组(10)，便可粗略估计陀螺仪偏差。 $$ \begin{array}{r}{\Delta\boldsymbol{q}_{b_{k}}^{b_{k+1}}\approx\Delta\tilde{\boldsymbol{q}}_{b_{k}}^{b_{k+1}}\otimes\left[\begin{matrix}{1}\\ {\frac{1}{2}J_{b_{\omega}}^{\Delta\tilde{\boldsymbol{q}}}\delta\boldsymbol{b}_{\omega}}\\ \end{matrix}\right].}\end{array} $$ 旋转差的最小值((6))是一个单位四元数。 $$ \hat{\boldsymbol{q}}_{l_{0}}^{b_{k}}\otimes\hat{\boldsymbol{q}}_{b_{k+1}}^{l_{0}}\otimes\Delta\tilde{\boldsymbol{q}}_{b_{k}}^{b_{k+1}}=\begin{bmatrix}1\\ \mathbf{0}\end{bmatrix}, $$ 结合式(7)，由式(6)表达的目标函数可进一步转化为： $$ \begin{bmatrix}1\\ \frac{1}{2}J^{\triangle\tilde{\boldsymbol{q}}}_{\boldsymbol{b}_{\omega}}\delta\boldsymbol{b}_{\omega}\end{bmatrix}=\Delta\tilde{\boldsymbol{q}}^{b_{k+1}-1}_{b_{k}}\otimes\hat{\boldsymbol{q}}^{b_{k+1}}_{l_{0}}\otimes\hat{\boldsymbol{q}}^{l_{0}}_{b_{k}}\otimes\begin{bmatrix}1\\ \mathbf{0}\end{bmatrix}. $$ 此外，仅考虑虚部时，我们有： $$ \begin{array}{r}{J_{b_{\omega}}^{\Delta\tilde{q}}\delta b_{\omega}=2(\Delta\tilde{q}_{b_{k}}^{b_{k+1}}{}^{-1}\otimes\hat{q}_{l_{0}}^{b_{k+1}}\otimes\hat{q}_{b_{k}}^{l_{0}})_{2:4}.}\end{array} $$ 根据Forster等人的结论 [^28]，雅可比矩阵 $J_{b_\omega}^{\Delta\tilde{q}}$ 的形式如下： $$ \begin{array}{r}{\boldsymbol{J}_{\boldsymbol{b}_{\omega}}^{\Delta\tilde{q}}=\displaystyle\sum_{i=0}^{j-1}(-\Delta\tilde{q}_{b_{i+1}}^{b_{j}}\mathrm{J}_{r}((\tilde{\boldsymbol{\omega}}_{i}-{}^{\omega}\boldsymbol{b}_{0})\Delta t)\Delta t),}\end{array} $$ 其中下标i和j表示第k次与第 $(k+1)$ 次激光雷达扫描之间的惯性测量单元读数索引。而右雅可比矩阵 $J_{r}(\phi)$ 可通过下式计算： $$ \mathrm{J}_{r}(\phi)=\boldsymbol{I}-\frac{1-\cos(\|\phi\|)}{\|\phi\|^{2}}+\frac{\|\phi\|-\sin(\|\phi\|)}{\|\phi\|^{3}}\lfloor\phi\rfloor_{\times}^{2}. $$ 因此，利用式(11)和式(12)的结果，通过基于窗口内的多次激光雷达扫描与惯性测量单元读数建立超定方程组(10)，便可粗略估计陀螺仪偏差。 姿态、速度和重力的初始化：如上所述，每次扫描相对于前一次扫描的变换可通过NDT匹配得到 [^32]。若以窗口内的首次扫描为参考，则窗口内所有扫描均可变换到该首次扫描的参考坐标系中。 用 $\boldsymbol{g}^{l_0}$ 表示首次扫描坐标系中的重力矢量。窗口内的变量，包括各节点的速度 $\boldsymbol{v}_k, k \in [i, j]$ 以及重力 $\boldsymbol{g}^{l_0}$，均需进行估计。残差定义为预积分结果（$\tilde{p}$、$\tilde{v}$）与通过NDT匹配得到的相对里程计数据之间的误差，据此应满足以下方程： $$ \begin{aligned}&\begin{bmatrix}\delta\boldsymbol{p}_{b_{k+1}}^{b_{k}}\\\delta\boldsymbol{v}_{b_{k+1}}^{b_{k}}\end{bmatrix}\\ &=\begin{bmatrix}\tilde{\boldsymbol{p}}_{b_{k+1}}^{b_{k}}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}(\hat{\boldsymbol{p}}_{b_{k+1}}^{l_{0}}-\hat{\boldsymbol{p}}_{b_{k}}^{l_{0}}-\hat{\boldsymbol{R}}_{b_{k}}^{l_{0}}\boldsymbol{v}_{b_{k}}^{b_{k}}\Delta t_{k}-\frac{1}{2}\boldsymbol{g}^{l_{0}}\Delta t_{k}^{2})\\\tilde{\boldsymbol{v}}_{b_{k+1}}^{b_{k}}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}(\hat{\boldsymbol{R}}_{l_{0}}^{b_{k+1}}\boldsymbol{v}_{b_{k+1}}^{b_{k+1}}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\boldsymbol{v}_{b_{k}}^{b_{k}}-\boldsymbol{g}^{l_{0}}\Delta t_{k})\end{bmatrix}.\end{aligned} $$ 在初始化阶段，由激光雷达数据与惯性测量单元预积分得到的相对运动可视为相同，即： --- $$[\delta p_{b_{k+1}}^{b_{k}},\delta v_{b_{k+1}}^{b_{k}}]^{T}=0\text{. Thus, (13) can be further transformed into a linear equation,}$$ $$ \begin{aligned}&\begin{bmatrix}-I\Delta t_{k}&0&-\frac{1}{2}\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\Delta t_{k}^{2}\\\boldsymbol{-I}&\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\hat{\boldsymbol{R}}_{b_{k+1}^{l_{0}}}&-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\Delta t_{k}\end{bmatrix}\begin{bmatrix}\boldsymbol{v}_{b_{k}}^{b_{k}}\\\boldsymbol{v}_{b_{k+1}}^{b_{k+1}}\\\boldsymbol{g}^{l_{0}}_{l^{0}}\end{bmatrix}\\ &=\begin{bmatrix}\Delta\tilde{\boldsymbol{p}}_{b_{k+1}}^{b_{k}}-\boldsymbol{p}_{c}^{b}+\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\hat{\boldsymbol{R}}_{b_{k+1}}^{l_{0}}\boldsymbol{p}_{c}^{b}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}(\hat{\boldsymbol{p}}_{b_{k+1}}^{l_{0}}-\hat{\boldsymbol{p}}_{b_{k}}^{l_{0}})\\\Delta\tilde{\boldsymbol{v}}_{b_{k+1}}^{b_{k}}\end{bmatrix}.\end{aligned} $$ 此外，对于特定位置而言，重力矢量的模长G通常是已知的，因此我们也将约束条件 $\|g^{l_0}\| = G$ 加入式(14)中。 通过这种方式，利用窗口内多个节点观测到的数据建立公式(14)，便可估算出每个节点的速度以及相对于第一个节点坐标系的重力。此外，通过根据估算出的$g^{l_0}$和相对NDT变换来计算每个节点相对于第一个节点的位姿，就能将所有节点的位置和旋转变换到世界坐标系下。 ### E. 前端：直接激光雷达-惯性里程计 前端的目标是同时构建高精度的局部子图，并实时准确地进行位姿估计，这属于典型的SLAM问题。为实现这一目标，本部分将首先介绍SLAM问题的建模方法，随后阐述如何将观测数据融入该模型。 1) 基于二次优化的SLAM：首先，我们简要回顾如何将SLAM问题从其概率模型转化为对应的二次优化问题。本部分的符号遵循Thrun等人定义中的惯例[^33]。 假设载体在时间戳t时的位姿为$\boldsymbol{x}_t = (\boldsymbol{p}_t^{wT}, \boldsymbol{v}_t^{wT}, \boldsymbol{q}_t^{wT})^T$，所创建的地图为$\mathcal{M}$，那么完整SLAM问题中需要估计的状态便包含所有涉及的位姿和地图，即$\boldsymbol{y}_{0:t} = [\boldsymbol{x}_0^T, \boldsymbol{x}_1^T, \ldots, \boldsymbol{x}_t^T, \mathcal{M}^T]^T$。对于时间戳为t的特定节点，我们用$\boldsymbol{y}_t = [\boldsymbol{x}_0^T, \mathcal{M}^T]^T$表示其与地图的组合状态。根据贝叶斯定律，$\boldsymbol{y}_{0:t}$的后验概率可表示为： $$ \begin{aligned}&P(\boldsymbol{y}_{0:t}|\boldsymbol{z}_{1:t},\boldsymbol{u}_{1:t})\\ &=\eta P(\boldsymbol{y}_{0})\prod_{t}[P(\boldsymbol{x}_{t}|\boldsymbol{x}_{t-1},\boldsymbol{u}_{t})\prod_{i}P(^{i}\boldsymbol{z}_{t}|\boldsymbol{y}_{i},^{i}c_{t})],\\ \end{aligned} $$ 其中$z_{1:t}$表示所有测量值，$^i z_t$是时间戳t时的第i个测量值，$^i c_t$编码了$^i z_t$与$\mathcal{M}$中地标的对应关系，$u_{1:t}$是控制输入，$\eta$是归一化系数，而$P(y_0)$则表示载体初始状态$(x_0)$及地图的先验分布。 通常，SLAM过程会伴随控制和观测输入。 $$ \boldsymbol{x}_{t}=\mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{x}_{t-1})+\boldsymbol{^{x} n}, $$ $$ \begin{array}{l}i z_{t}=\mathrm{h}(\boldsymbol{y}_{t},^{i}c_{t})+^{z}\boldsymbol{n},\end{array} $$ 其中$f(\cdot)$和$h(\cdot)$一般为非线性函数，而$x_n$和$z_n$分别是状态和观测的高斯噪声，其协方差矩阵分别为$\Lambda_t$和$\Sigma_t$。 我们的目标是估算出能使公式(15)的概率达到最大的所有状态。这个最大后验概率（MAP）问题可以等价转化为最大化对数后验概率的函数，即： $$ \begin{align*}\boldsymbol{y}^{*}_{0:t}&=\arg\max_{\boldsymbol{y}_{0:t}}\log P(\boldsymbol{y}_{0:t}|\boldsymbol{z}_{1:t},\boldsymbol{u}_{1:t})\\&=\arg\max_{\boldsymbol{y}_{0:t}}\log P(\boldsymbol{x}_{0})+\sum_{t}[\log P(\boldsymbol{x}_{t}|\boldsymbol{x}_{t-1},\boldsymbol{u}_{t})\\&\quad+\sum_{i}\log P(^{i}\boldsymbol{z}_{t}|\boldsymbol{y}_{t},^{i}c_{t})].\end{align*} $$ 进一步利用公式(16)和(17)，公式(18)可等价表示为： $$ \begin{align*}\boldsymbol{y}_{0:t}^{*}=&\arg\min_{\boldsymbol{y}_{0:t}}\boldsymbol{x}_{0}^{T}\boldsymbol{\Lambda}_{0}\boldsymbol{x}_{0}\\&+\sum_{t}(\boldsymbol{x}_{t}-\mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{x}_{t-1}))^{T}\boldsymbol{\Lambda}_{t}^{-1}(\boldsymbol{x}_{t}-\mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{x}_{t-1}))\\&+\sum_{t}\sum_{i}(^{i}\boldsymbol{z}_{t}-\mathrm{h}(\boldsymbol{y}_{t},^{i}\boldsymbol{c}_{t}))^{T}\boldsymbol{\Sigma}_{t}^{-1}(^{i}\boldsymbol{z}_{t}-\mathrm{h}(\boldsymbol{y}_{t},^{i}\boldsymbol{c}_{t})),\end{align*} $$ 在实际应用中，无法精确计算公式(19)中的$f(\cdot)$和$h(\cdot)$。一个可行的替代方案是在当前估计值处对它们进行泰勒展开。也就是： $$ \mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{x}_{t-1})\approx\mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{\mu}_{t-1})+\boldsymbol{G}_{t}(\boldsymbol{x}_{t-1}-\boldsymbol{\mu}_{t-1}), $$ $$ \mathrm{h}(\boldsymbol{y}_{t},^{i}\boldsymbol{c}_{t})\approx\mathrm{h}(\boldsymbol{\mu}_{t},^{i}\boldsymbol{c}_{t})+^{i}\boldsymbol{H}_{t}(\boldsymbol{y}_{t}-\boldsymbol{\mu}_{t}), $$ 其中$G_t$和$^iH_t$分别是$f(\cdot)$和$h(\cdot)$的雅可比矩阵，而$\mu_t$是状态向量$y_t$的当前估计值。因此，将公式(20)和(21)代入公式(19)可得： $$ \begin{aligned}\boldsymbol{y}_{0:t}^{*}&=\arg\min_{\boldsymbol{y}_{0:t}}\boldsymbol{x}_{0}^{T}\boldsymbol{\Lambda}_{0}\boldsymbol{x}_{0}\\&\quad+\sum_{t}\boldsymbol{x}_{t-1:t}^{T}(-\boldsymbol{G}_{t}^{T},\mathbf{1})^{T}\boldsymbol{\Lambda}_{t}^{-1}(-\boldsymbol{G}_{t}^{T},\mathbf{1})\boldsymbol{x}_{t-1:t}\\&\quad-2\boldsymbol{x}_{t-1:t}^{T}(-\boldsymbol{G}_{t}^{T},\mathbf{1})^{T}\boldsymbol{\Lambda}_{t}^{-1}[\mathrm{f}(\boldsymbol{u}_{t},\boldsymbol{\mu}_{t-1})-\boldsymbol{G}_{t}\boldsymbol{\mu}_{t-1}]\\&\quad+\sum_{i}\boldsymbol{y}_{t}^{T i}\boldsymbol{H}_{t}^{T}\boldsymbol{\Sigma}_{t}^{-1i}\boldsymbol{H}_{t}\boldsymbol{y}_{t}\\&\quad-2\boldsymbol{y}_{t}^{T i}\boldsymbol{H}_{t}^{T}\boldsymbol{\Sigma}_{t}^{-1}[\boldsymbol{i}\boldsymbol{z}_{t}-\mathrm{h}(\boldsymbol{\mu}_{t},\boldsymbol{i}\boldsymbol{c}_{t})+\boldsymbol{}^{i}\boldsymbol{H}_{t}\boldsymbol{\mu}_{t}],\quad(2)\end{aligned} $$ 其中$x_{t-1:t}$将时间戳t-1和t时的位姿合并为一个向量。 进一步，公式(22)中的所有二次项可汇总为矩阵A，线性项可按相同方式整理为向量b。这样，完整的SLAM问题便可表述为一个标准的二次优化问题： $$ \delta\boldsymbol{y}_{0:t}^{*}=\underset{\boldsymbol{y}_{0:t}}{\arg\min}\left(\boldsymbol{y}_{0:t}^{T}\boldsymbol{A}\boldsymbol{y}_{0:t}+2\boldsymbol{y}_{0:t}^{T}\boldsymbol{b}\right). $$ 最终，这个二次优化问题可以通过数值优化方法高效求解，例如高斯-牛顿算法或列文伯格-马夸特算法 [^34], [^35]。 --- 在我们的场景中，IMU预积分本质上对应于SLAM系统的控制输入，相关内容已在第III-C1节中详细介绍。接下来，我们将重点探讨两种测量输入的因子构建，即激光雷达里程计和重力先验。 ## 2) 激光雷达里程计因子 子图表示：在二维激光雷达SLAM中，二维空间通常表示为占据栅格地图 [^18], [^33]。在三维情况下，它可自然扩展为占据体素地图（OVM） [^36]。在OVM中，每个元素会缓存该体素的状态 $(o)$，而其存储的值 $odd(v) = \frac{P(o=1)}{P(o=0)}$ 则是该体素被占据的概率 $(o=1)$ 与为空的概率 $(o=0)$ 之比。 当产生新的观测值z时，该比值会更新为 $odd(v|z) = \frac{P(o=1|z)}{P(o=0|z})$。应用贝叶斯法则，我们有： $$ P(o=1|z)=\frac{P(z|o=1)P(o=1)}{P(z)}, $$ $$ P(o=0|z)=\frac{P(z|o=0)P(o=0)}{P(z)}, $$ 从而得到： $$ o d d(v|z)=\frac{P(o=1|z)}{P(o=0|z)}=\frac{P(z|o=1)}{P(z|o=0)}o d d(v). $$ 进一步对式(25)取对数，可得： $$ \log odd(v|z)=\log\frac{P(z|o=1)}{P(z|o=0)}+\log odd(v). $$ 此外，观测值只有命中（1）或未命中（0）两种状态，即： $$ C_{m i s s i n g}=\log\frac{P(z=0|o=1)}{P(z=0|o=0)}, $$ $$ C_{h i t t i n g}=\log\frac{P(z=1|o=1)}{P(z=1|o=0)}. $$ 由于 $C_{missing}$ 和 $C_{hitting}$ 均为常量，因此对于体素的概率更新而言，只需按照式(26)进行加减运算即可。 为了节省存储空间并高效更新地图，我们将每个子图表示为八叉树结构，初始时仅维护虚拟的空间划分，只有在观测到真实数据时才会分配实际内存。 扫描点与子图的匹配：得益于IMU偏差的实时校正，我们能够从IMU读数推断出可靠的初始位姿。因此，扫描点与子图的配准是在局部范围内进行的。在此，我们给出一种三维扫描点与子图的匹配方法。 给定一个三维点 $p^w$，可直接查询其在地图中的对应概率 $\mathrm{M}(p^w)$，而其梯度 $\nabla\mathrm{M}(p^w) = \begin{bmatrix} \frac{\partial M}{\partial x}, \frac{\partial M}{\partial y}, \frac{\partial M}{\partial z} \end{bmatrix} | p^w$ 可通过地图上与其相邻的六个体素来近似计算。 假设当前估计位姿对应的李代数表示为 $\hat{\boldsymbol{\xi}} = [\hat{\boldsymbol{\phi}}^{T}, \hat{\boldsymbol{\rho}}^{T}]^{T} \in \mathfrak{se}(3)$。我们将变换后的点与已知地图之间的概率差异视为残差项： $$ \mathrm{R e s}(\hat{\pmb{\xi}})=\sum_{i}[1-\mathrm{M}(\pmb{T}_{i}(\hat{\pmb{\xi}}))]^{2}, $$ 其中，$\text{Res}(\cdot)$ 用于计算关于操作参数的残差，$\text{M}(\cdot)$ 返回对应体素的概率，而 $\boldsymbol{T}_i(\hat{\boldsymbol{\xi}})$ 则表示利用相关参数 $\boldsymbol{p}_i^l$ 对激光雷达点 $\hat{\boldsymbol{\xi}}$ 进行变换，即 $\boldsymbol{T}_i(\hat{\boldsymbol{\xi}}) = \text{Exp}(\hat{\boldsymbol{\xi}})\left[\boldsymbol{p}_i^l \quad 1\right]^T$。需注意，$\boldsymbol{T}_i(\hat{\boldsymbol{\xi}})$ 的结果是一个齐次坐标，属于 $\mathbb{R}^{4 \times 1}$ 中的向量，其最后一个元素为1。当 $\text{M}(\cdot)$ 或 $\nabla\mathbf{M}(\cdot)$ 作用于它时，会将其隐式转换为非齐次坐标。 若存在近似的初始值，便可在该值附近确定最优的 $\delta\xi$ 以最小化 $\text{Res}(\hat{\xi})$。也就是说： $$ \delta\boldsymbol{\xi}^{*}=\arg\min_{\delta\boldsymbol{\xi}}\sum_{i}[1-\mathrm{M}(\boldsymbol{T}_{i}(\hat{\boldsymbol{\xi}}+\delta\boldsymbol{\xi}))]^{2}. $$ 对式(29)进行一阶泰勒展开并应用链式法则，可得： $$ \delta\pmb{\xi}^{*}=\arg\min_{\delta\pmb{\xi}}\sum_{i}\left[1-\mathrm{M}(\pmb{T}_{i}(\hat{\pmb{\xi}}))-\bigtriangledown\check{\mathrm{M}}(\pmb{T}_{i}(\hat{\pmb{\xi}}))\frac{\partial\hat{\pmb{T}_{i}}(\pmb{\xi})}{\partial\pmb{\xi}}\delta\pmb{\xi}\right]^{2}, $$ 在这里，$(\cdot)$ 会将元素 0 添加到对应的 $1 \times 3$ 向量中，而 $\frac{\partial \hat{T}_i(\xi)}{\partial \xi} = \frac{\partial \hat{T}_i(\xi)}{\partial \xi} |\xi$ 则是对符号的略微滥用。(30)式在关于 $\delta \xi$ 的偏导数为 0 时取得最小值。此时求解 $\delta \xi$ 就能得到对应的高斯-牛顿最小化方程。 $$ \delta\boldsymbol{\xi}^{*}=\boldsymbol{H}^{-1}\sum_{i=1}\left[\bigtriangledown\check{\mathrm{M}}(\boldsymbol{T}_{i}(\hat{\boldsymbol{\xi}}))\frac{\partial\hat{\boldsymbol{T}_{i}}(\boldsymbol{\xi})}{\partial\boldsymbol{\xi}}\right]^{T}[1-\mathrm{M}(\boldsymbol{T}_{i}(\hat{\boldsymbol{\xi}}))], $$ 其中 $$ \begin{align*}\boldsymbol{H}&=\left[\bigtriangledown\check{\mathrm{M}}(\boldsymbol{T}_{i}(\hat{\boldsymbol{\xi}}))\frac{\partial\hat{\boldsymbol{T}_{i}}(\boldsymbol{\xi})}{\partial\boldsymbol{\xi}}\right]^{T}\left[\bigtriangledown\check{\mathrm{M}}(\boldsymbol{T}_{i}(\hat{\boldsymbol{\xi}}))\frac{\partial\hat{\boldsymbol{T}_{i}}(\boldsymbol{\xi})}{\partial\boldsymbol{\xi}}\right],\\\frac{\partial\hat{\boldsymbol{T}_{i}}(\boldsymbol{\xi})}{\partial\boldsymbol{\xi}}&=\begin{bmatrix}\boldsymbol{I}&\frac{\partial\mathrm{Exp}(\phi)\boldsymbol{p}_{i}^{l}}{\partial\phi}\big|_{\hat{\phi}}\\\mathbf{0}^{T}&\mathbf{0}^{T}\end{bmatrix}.\tag{32}\end{align*} $$ 而 $\frac{\partial\mathrm{Exp}(\phi)\boldsymbol{p}_{i}^{l}}{\partial\phi}$ 的推导过程详见第六节。 因此，优化后的激光雷达里程计可以转换为局部因子图中的单位因子。该单位因子相对于节点位姿的雅可比矩阵是单位矩阵，其对应的协方差可通过 $\mathrm{Var}(\hat{\boldsymbol{\xi}})=\sigma^{2}\cdot\boldsymbol{H}^{-1}$ 计算，其中 $\boldsymbol{H}$ 可由(32)式算出，$\sigma$ 则与激光雷达 [^37] 的精度相关。 3) 重力先验因子：系统初始化后，我们可以大致将状态对齐到世界坐标系。但估计出的重力可能会随时间产生漂移。为此，我们考虑利用优化后的状态来动态估计重力，并将其作为约束参与后续节点的优化。通过这种方式，有望使系统在长期运行过程中保持较低的横滚角和俯仰角漂移。实验将证实这一策略对提升所提出方法的召回率起着关键作用。 --- 循环检测策略以及循环约束的建立也会因此得以加速。 与系统初始化阶段的策略（第三节D2小节）类似，我们选取局部窗口中的几个优化节点来在线估计重力。但与前者不同的是，此时每个节点的速度、位姿和惯性测量单元偏差都已被合理估计。因此，我们将它们在(14)式中视为已知量。这样一来，只有重力矢量未知，待求解的方程可转化为： $$ \begin{aligned}&\begin{bmatrix}-\frac{1}{2}\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\Delta t_{k}^{2}\\-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\Delta t_{k}\end{bmatrix}\begin{bmatrix}\boldsymbol{g}^{l_{0}}\end{bmatrix}\approx\\ &\begin{bmatrix}\Delta\tilde{\boldsymbol{p}}_{b_{k+1}}^{b_{k}}-\boldsymbol{p}_{l}^{b}+\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\hat{\boldsymbol{R}}_{b_{k+1}}^{l_{0}}\boldsymbol{p}_{l}^{b}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}(\hat{\boldsymbol{p}}_{b_{k+1}}^{l_{0}}-\hat{\boldsymbol{p}}_{b_{k}}^{l_{0}})+I\Delta t_{k}\hat{\boldsymbol{v}}_{b_{k}}^{b_{k}}\\\Delta\tilde{\boldsymbol{v}}_{b_{k+1}}^{b_{k}}+I\hat{\boldsymbol{v}}_{b_{k}}^{b_{k}}-\hat{\boldsymbol{R}}_{l_{0}}^{b_{k}}\hat{\boldsymbol{R}}_{b_{k+1}}^{l_{0}}\hat{\boldsymbol{v}}_{b_{k+1}}^{b_{k+1}}\end{bmatrix},\end{aligned} $$ 其中 $g^{l_0}$ 是窗口内首次扫描帧中的重力，$p_l^b$ 是离线标定的激光雷达与惯性测量单元之间的外部平移量，带帽符号（$\cdot$）则表示由窗口内的优化状态计算得出的量。 假设第k个节点估计出的重力为 $g^{b_k}$。节点 $n_k$ 的残差项可表示为： $$ \mathrm{R e s}(\boldsymbol{R}_{b_{k}}^{w},\boldsymbol{g}^{b_{k}})=\frac{^{r p}\boldsymbol{R}_{b_{k}}^{w}\boldsymbol{g}^{b_{k}}}{\left\|^{r p}\boldsymbol{R}_{b_{k}}^{w}\boldsymbol{g}^{b_{k}}\right\|}-\frac{\boldsymbol{g}^{w}}{\left\|\boldsymbol{g}^{w}\right\|}, $$ 其中 $^{rp}R_{b_k}^{w}$ 表示仅包含 $R_{b_k}^{w}$ 横滚角和俯仰角分量的旋转矩阵，其对应的旋转向量为 $\boldsymbol{\theta}$。那么，残差关于 $\boldsymbol{\theta}$ 的偏导数可通过(公式见附录A)计算得出。 $$ \begin{array}{r}{\frac{\partial\mathrm{R e s}\left(R_{b_{k}}^{w},g^{b_{k}}\right)}{\partial\theta}=-\mathrm{E x p}(\theta)\lfloor g^{b_{k}}\rfloor_{\times}\mathrm{J}_{r}(\theta).}\end{array} $$ 最终，重力因子会作为节点的单位因子。其相对于节点旋转的雅可比矩阵可通过(35)式计算，而其协方差则可按照 [^38] 中的建议，通过数值方法合理估计。 ### F. 后端：子图到子图的循环检测 为确保长期建图的一致性，我们在后端开发了一套全局位姿图优化流程。本节中，我们并未直接对所有子图进行暴力扫描匹配，而是提出了一种基于子图间匹配的快速循环检测与约束构建策略。 由于三维激光点云较为稀疏，设计鲁棒的位置特征十分困难。不过，通过累积多次扫描数据，有望更轻松地提取稳定的位置特征。为此，我们考虑从已构建的子图中提取位置特征，而非基于单次扫描，再利用特征点检测与匹配技术来识别回环并构建几何约束。 1) 位置特征提取与匹配：得益于前端紧耦合优化，子图内各节点的相对位姿相当精确。此外，借助重力先验因子，每个子图都能与重力方向对齐。因此，我们采用将子图投影到二维水平面的方式寻找回环，而非直接在三维空间中操作。 具体而言，根据估算出的重力方向，所有三维子图首先被投影到水平面，从而得到对应的二维子图集合。接着，借助特征点检测技术，可为每个二维子图建立视觉特征对应的描述符。常用的视觉特征检测算子包括SIFT [^39]、SURF [^40]和ORB [^41]。其中SIFT是一种设计精良的算子，但计算复杂度较高，会导致回环检测出现严重延迟。近年来，尽管ORB算子在视觉SLAM中应用广泛，但在我们的场景中，生成的子图包含大量噪声点，容易引发过多误判。最终实验表明，只有SURF能在精度与速度之间取得可接受的平衡。 关键点检测完成后，即可为每个点计算特征描述符，进而通过对应的描述符集合识别每个子图。进行回环检测时，我们利用“近似最近邻快速库”（FLANN）[^42]来匹配成对子图的描述符集合。一旦匹配对的数量超过特定阈值，便认为这两个子图之间存在回环闭合。 2) 快速回环约束构建：与仅专注于位置识别的定性回环检测方法不同 [^19] [^20] [^21]，我们采用的投影二维子图包含精确的空间信息。也就是说，根据匹配对可得到三维刚体变换 $(o x_{s_i}^s, o y_{s_i}^s, \gamma)$。这一三维刚体变换能为更精确的扫描与子图配准提供合理的初始值，从而大幅提升回环约束构建的计算效率。此外，得益于在线重力估计，横滚角和俯仰角已有合理近似值。因此，唯一缺乏近似估计的变量就是 $o^s z_{s_i}^s$。接下来，我们将先介绍三维刚体变换的建立过程，再探讨如何估算 $o^s z_{s_i}^s$ 的合理值。 子图间的变换估计：若子图的重力估计足够准确，则二维子图间的变换会大致符合三维刚体变换关系。 $$ \begin{bmatrix}x^{s_{j}}\\ y^{s_{j}}\end{bmatrix}=\begin{bmatrix}\cos(\gamma)&-\sin(\gamma)\\ \sin(\gamma)&\cos(\gamma)\end{bmatrix}\begin{bmatrix}x^{s_{i}}\\ y^{s_{i}}\end{bmatrix}+\begin{bmatrix}^{o}x_{s_{i}}^{s_{j}}\\ ^{o}y_{s_{i}}^{s_{j}}\end{bmatrix}. $$ 在公式(36)中存在三个待确定的未知数，即$(^o x_{s_i}^s, o_{y_{s_i}^j}, \gamma)$。理论上，至少需要两对成功匹配的点。然而，由于实际环境的复杂性，所生成的子图往往包含噪声，这会导致直接匹配对中出现异常值。为解决这一问题，我们借助RANSAC算法——一种常用的鲁棒随机采样一致性估计算法，迭代地估算3自由度变换参数[^43]。 图2给出了一个示例，用以说明二维子图间变换的概念。如图所示，特征 --- ![Image](images/9d11a663787c7b716d20409e112a88a8917cdc7d564d45b72840797192b30399.jpg)
Fig. 2. Projected submap-to-submap matching. 2D projected submaps with a loop closure are shown in sub-figures (a) and (b). The ends of each colored line segment are corresponding SURF points, and each colored line segment represents a pair of matched points. The 3-DoF transformation between these 2D submaps can be estimated using the matched feature pairs. By registering (a) to (b), we can get the transformed version of (a) as shown in (c).
![Image](images/ac0e13c3c0ead7d17ef9628f1fddcf21740edfa3395f6483764661a3259fd334.jpg)
Fig. 3. Steps of determining the 5-DoF transformation from a node $n_{k}$ in submap $s_{i}$ to submap $s_{j}$. With the estimated 2D transformation between the 2D submap i (whose frame is $h_{i}$) to 2D submap j (whose frame is $h_{j}$), we can first transform $n_{k}$ to $h_{i}$ and then map it to $h_{j}$. Finally, because the transformation between $h_{j}$ and $s_{j}$ is known, the 5-DoF transformation of $n_{k}$ in $s_{j}$ can be obtained (only the vertical shift is left undetermined).
子图(a)与子图(b)之间的配对点由不同颜色的线段表示，这些线段的端点即为对应的SURF特征点[^40]；而子图(c)则是利用匹配点对估算出的3自由度变换，将子图(a)转换后得到的结果。 节点到子图的约束构建：为建立闭环约束，需将上述得到的子图间约束转换为节点到子图的约束，以便利用近似的3D初始值进行更精确的匹配。 假设如前所述，已获得从第i个二维子图到第j个二维子图的3自由度变换$T_{hi}^{h_j}$。用$T_s^h$表示子图坐标系到其水平投影坐标系的变换，该变换在子图构建过程中已确定。因此，通过$T_{n_k}^{s_j}$即可求得子图i中的采样节点$n_k$与子图j之间的变换$T_{n_k}^{s_j} = T_{h_j}^{s_j} T_{h_i}^{h_j} T_{s_i}^{h_i} T_{n_k}^{s_i}$。图3展示了这一变换的具体步骤。 通过这种方式，便可基于投影后的二维子图间约束，建立起从节点$n_k$到子图$s_j$的近似约束。此时，仅剩$o^{s_j s_j}$尚未获得初始值信息。对$o^{s_j s_j}$的搜索需要在较大范围内进行。借鉴Hess等人的方法[^11]，我们采用分支定界算法来快速搜寻$o^{s_j s_j}$。相比之下，他们的情况需要确定4自由度参数($o^{x_{s_i, s_j}}$、$o^{y_{s_j}}$、$o^{z_{s_j}}$、$\gamma$)。得益于通过子图匹配估算出的5自由度变换的初始值，我们的计算效率得以大幅提升，最终可得到6自由度的初始变换$\hat{T}_{n_k}^{s_j}$。 最后，利用第III-E2节所述的方法，在确定6自由度变换$n_k$后，即可将节点$s_j$处的扫描数据精确配准到子图$\hat{T}_{n_k}^{s_j}$上。此外，借助全局稀疏位姿图的联合优化，所有节点和子图的位姿都能轻松调整。如此一来，系统的累积误差便能及时消除，从而确保系统能够长时间稳定运行。 ### 四、实验 在本部分中，我们将通过大量实验回答以下三个问题： DLIOM的建图与定位性能如何？ - D-LIOM的前端能否实时运行？其后端能否在低延迟条件下消除累积误差？ - 为何引入的闭环检测策略与重力先验因子对D-LIOM而言至关重要？ 为此，我们在从不同平台收集的多种规模场景的数据集上，首先从定性和定量两个角度探讨了D-LIOM在定位与建图方面的性能，接着对该框架的时间效率进行定量分析，最后针对专门设计的模块开展消融实验。 ### A. 实验方案 1) 实现方式：D-LIOM的所有模块均采用C++实现。在前端，D-LIOM借助因子图优化工具箱GTSAM来构建并优化局部因子图；在后端，D-LIOM利用OpenCV提取并匹配2D子图的特征点，再通过Ceres-Solver优化全局位姿图。我们利用流行的机器人操作系统（ROS）的通信机制来实现进程间的消息传输。实验在一台配备两颗2.4 GHz英特尔至强E5-2620V3 CPU和32 GB内存的工作站上进行。 2) 数据集：实验所用数据集来自多种场景，包括小型室内建筑、中型校园区域以及大型复杂城市环境。此外，测试案例涉及三种常见的应用平台，即手持设备、无人机和地面行驶车辆。我们认为这些数据集极具挑战性，足以检验所提框架的鲁棒性、通用性和灵活性。后续部分将逐一介绍这些数据集。 自行采集的数据集：如图4(a)所示，我们自主研发的数据采集设备由帧率为10 Hz的16线ROBOSENSE激光雷达和能以400 Hz频率输出线性加速度及角速度读数的消费级IMU组成。我们采用[^44]中建议的方法来校准激光雷达与IMU之间的外参。IMU的内参及其噪声统计参数则由Woodman的方法[^45]提供。 借助该手持设备，我们在同济大学校园内的结构化空间和非结构化区域采集了多组序列数据，随后建立的数据集被命名为“TONGJI数据集”。数据的所有具体特征 --- ![Image](images/534f3a3af7c212e94ff0c145911cf994e57450a132f3af36210c4e848555cc50.jpg) ![Image](images/77f09196aaf71f5cc8418e16b7cb0f90e51b2d59f6d54fb4710242f25652c828.jpg)
(b) UAV-mounted
![Image](images/9e7e40b2c0dc7f2c2661be8381bf8a0a4e3b175e8c57fee70d518d9feb03a6fb.jpg)
(a) Our handheld device
(c) Vehicle-mounted
Fig. 4. (a) Our self-developed handheld device is composed of a 16-line RO-BOSENSE LiDAR with a frame rate of 10 Hz and a built-in consumer-grade IMU with a frame rate of 400 Hz. (b) The UAV-mounted platform of the VIRAL dataset [^22]. (c) The vehicle-mounted platform of the Complex Urban Dataset [^23].
TABLEI
DETAILS OF THE TONGJI DATASET. "TRAJ. LEN.," "LIN. VEL.," AND "ANG. VEL." ARE ABBREVIATIONS OF TRAJECTORY LENGTH, LINEAR VELOCITY, AND ANGULAR VELOCITY, RESPECTIVELY
NameTraj. Len. (m)Lin. Vel. (m/s)Ang. Vel. (°/s)Area ( $10^{4}m^{2}$)Scans
TJ-13014.685.33167.6616.775090
TJ-23004.975.54184.4813.015562
TJ-33170.734.9999.3812.286516
TJ-43500.255.18120.8819.896392
TJ-51300.983.39145.023.344452
TJ-6134.832.85223.45$7.99 \times 10^{-4}$440
TJ-7117.362.99232.45$7.57 \times 10^{-4}$383
均列于表I中。其中，序列TJ-1、TJ-2、TJ-3和TJ-4是在骑行过程中采集的，而相对较短的TJ-5则是在步行时采集的。TJ-6和TJ-7采集时的最大角速度分别达到$223^\circ/s$和$232^\circ/s$，采集过程中伴随一系列剧烈的旋转动作。现已可以在网上公开获取所采集的TONGJI数据集。$^2$ 公开数据集：除了自行采集的数据集外，我们还利用两个具有参考值的公开数据集开展实验验证，分别是Complex Urban Dataset [^23]和VIRAL [^22]。 Complex Urban Dataset [^23]是由车载平台（图4(c)）采集的，其主要传感器包括两台16线Velodyne激光雷达、一台AHRS IMU、GPS以及若干摄像头。我们在“Urban-09”和“Urban-10”这两个代表性序列上测试了我们的框架，因为这些序列包含高层建筑、多车道道路、大量动态目标以及零星的GPS数据。 VIRAL [^22] 的采集平台是一架 DJI M600 UAV（图 4(b)），该平台配备了两个 Ouster OS1-16-gen-1 LiDARs 和一个 VectorNav-VN100 IMU。尽管 VIRAL 所覆盖的空间物理范围相对较小，但由于双 LiDARs 提供了互补的视角，VIRAL [^22] 在一定程度上仍然是验证多 LiDAR 系统性能的一个合理选择。
TABLE II
TRAITS OF D-LIOM AND ITS COMPETITORS
IMU Biases EstimationDynamic InitializationMulti-LiDAR InputLoop Detection
LOAM [^6]✗✗✗✗
LIOM [^9]✓✓✗✗
Carto3D [^11]✗✗✓✓
LIO-SAM [^10]✓✗✗✓
D-LIOM (Ours)✓✓✓✓
### B. 定性分析 1) 与同类方法的特性对比：首先，我们从四个方面对D-LIOM与现有最先进框架（Carto3D [^11]、LOAM [^6]、LIOM [^9]以及LIO-SAM [^10]）的特性进行定性比较： 是否估计惯性测量单元的偏差？ 是否启用基于六轴惯性测量单元的动态初始化功能？ 是否支持多激光雷达输入？ • 是否具有自动闭环检测功能？ 如表II所示，在受评估的框架中，只有我们提出的D-LIOM具备这四项突出优势。一方面，支持六轴惯性测量单元的自动初始化以及多激光雷达输入功能，使D-LIOM能够很好地适配不同的传感器配置；另一方面，惯性测量单元偏差的在线估计与自主闭环检测功能也能提升D-LIOM的定位与建图性能。 2) 快速旋转条件下的重建：通过融合惯性测量单元信息，我们的D-LIOM能够实时校正扫描数据中的畸变。同时，初始位姿的递归值使得扫描配准更加精确，从而在快速运动场景下提升建图性能。为了验证这一点，我们利用自主研发的手持设备（见图4(a)）采集了若干包含剧烈旋转动作的序列数据（这些序列中出现的最大角速度超过了$223^{\circ}/s$），并分别用LIO-SAM [^10]和所提出的D-LIOM重建相应的三维结构。如图5顶部行所示，两个位置的快速运动轨迹以蓝色曲线表示。中间行展示的是由最先进的LIO-SAM算法恢复的三维结构，而最后一行呈现的则是D-LIOM建立的三维结构。结果表明，在快速旋转条件下，D-LIOM不仅改善了轮廓的清晰度，还提升了窗户、柱子等结构细节的精细度。至于LIO-SAM，虽然它能大致还原空间结构，但结果显得较为模糊，这说明LIO-SAM的建图精度实际上更差。 此外，我们也尝试在这些复杂数据集上评估Carto3D [^11]、LOAM [^6]和LIOM [^9]，但遗憾的是，它们均无法输出合理的结果，因此此处不再赘述。 3) 结构化与非结构化区域的建图：直接配准机制使D-LIOM能够很好地适应各种环境；同时，及时且准确的闭环检测功能也让D-LIOM能够构建一致的地图。为了 --- ![Image](images/85a948ca66c2cdd6c4e0c0b7918427540065530aa26a7c8fc87cf60f6e9ecc4a.jpg)
Fig. 5. Reconstruction with fast rotation. The acquisition trajectories of TJ-6 and TJ-7 are depicted in the top row. The reconstruction results of LIO-SAM [^10] on the experimented data sequences are shown in the middle row, while our results are shown in the last row. By comparison, it can be found that when the carrier moves with rapid rotation, the restored structure by D-LIOM exhibits much sharper outlines and finer details.
验证这两点，本部分将研究D-LIOM在结构化与非结构化区域中的建图性能。 TONGJI数据集中的几个具有挑战性的序列（TJ-1至TJ-4）被用以通过D-LIOM构建地图。如图6所示，D-LIOM生成的地图能够很好地还原真实的空间结构。无论是结构化场景还是非结构化场景，都能从宏观结构（如道路、桥梁、建筑轮廓等）或微观局部细节（如路灯、梯子、树干等）中准确重建出来。此外，子图间的回环检测功能使D-LIOM能够有效消除累积误差，从而实现长期建图的一致性。如图6中局部放大图“G”所示，可以发现回环检测后重建的建筑轮廓十分清晰，这表明回环闭合前后的结果高度一致。由于我们已经验证D-LIOM在这四个复杂序列（TJ-1、TJ-2、TJ-3和TJ-4）上能够取得令人满意的建图效果，因此这些对应的轨迹可作为定性比较的近似参考。据此，我们在这几个序列上评估了最先进的竞争对手算法（Carto3D [^11]、LOAM [^6]、LIOM [^9]以及LIO-SAM [^10]），将它们估算出的轨迹与D-LIOM估算的轨迹对齐，并将结果绘制在图7中。需要注意的是，LIOM [^9]仅在TJ-3序列上运行成功，而LOAM [^6]在TJ-2和TJ-4上的定位误差过大，因此相应的轨迹未在对应子图中展示。 从图7可以看出，基于直接配准的Carto3D框架，以及基于特征的LOAM、LIOM和LIO-SAM算法，其长期定位结果通常较差。可能的原因如下： - 尽管LOAM和LIOM通过将新扫描帧的特征与过去的多帧特征进行配准来减轻累积误差的不利影响，但在大规模场景中运行时，它们仍不可避免地会出现漂移现象。 - 由于关键点检测率降低且特征提取不稳定，基于特征的解决方案在非结构化场景中表现不佳。相比之下，概率性直接配准方法更为稳健。 - Carto3D结果不理想的根本原因在于其无法正确检测回环。虽然Carto3D的后端会对扫描数据与子图进行暴力匹配，但缺乏合适的相对位姿初始值，导致其回环检测率仍然很低。 4) 大规模复杂城市场景中的定位：在本小节中，为了了解我们的框架在超大尺度场景中的性能，我们在Complex Urban数据集[^23]中的两个具有挑战性的序列“Urban-09”和“Urban-10”上对D-LIOM进行了测试。这些序列是由搭载两台倾斜激光雷达和惯性测量单元的车辆采集的。每个序列的采集时间约为1小时，覆盖面积近2平方公里。 由于城市中高层建筑的遮挡，“Urban-09”和“Urban-10”中的GPS测量结果并不稳定且存在意外误差，因此我们在此仅将其作为近似参考。采用Umeyama [^46]提出的方法进行轨迹对齐后，我们将D-LIOM的定位结果绘制在图8中。如图所示，尽管定位位置与GPS测量值之间存在一些较大偏差，但大多数位置的轨迹与GPS结果基本一致，这表明即便仅配置主动传感器，D-LIOM仍能实现可与GPS媲美的高定位精度。 值得指出的是，我们也尝试在这两组具有挑战性的序列上测试Carto3D [^11]、LOAM [^6]、LIOM [^9]以及LIO-SAM [^10]，但它们均未能产生有意义的结果。可能的原因是该载具平台上安装的激光雷达倾斜角度较大，而载体主要沿水平方向运动，导致缺乏稳定的特征点，这对基于特征的系统产生了致命影响。相比之下，基于直接概率匹配的D-LIOM则不受此限制。 ### C. 定量分析 为了对D-LIOM进行深入定量分析，接下来我们首先探究其在室内数据集VI-RAL [^22]以及自行采集的数据集上的定位精度（见表I），随后评估其时间效率。 1) 绝对定位误差：尽管源自空中平台的VIRAL [^22]数据集存在一些局限，比如空间范围较小且运动速度较慢，但它也有几个值得作为基准来评估激光雷达-惯性SLAM系统的特殊特性。其一是 --- ![Image](images/7b8d7ba30aec7d0e4c9891648f92a995b9c92b3f6f8c827894ac89915b22ac4c.jpg)
Fig. 6. Mapping in structured and unstructured areas by D-LIOM. Left: mapping around man-made structures (top) and along a natural river (bottom). Middle: A $\sim$ F are partially enlarged drawings of the left, with clearly restored buildings, bridges, gardens, flagpoles, and bushes. Right: mapping for a hybrid scene with a combination of structured and unstructured areas. In G, it can be found that the building reconstructed after the loop was detected has a sharp outline, demonstrating that the results before and after the loop closure were in good agreement.
![Image](images/3ddc7933d1701c01e8459f4b325c486c9d759b1928d08b843752921b41cf66b8.jpg)
Fig. 7. Trajectories estimated by D-LIOM and its counterparts in four medium-sized areas. Evidently, only our D-LIOM has shown good localization results on all test sequences. Other methods either fail or can't eliminate the accumulated errors in the long-term SLAM.
![Image](images/c6cdae847a33f03ac96923d64b6da66d475e9faffa040a893c7002a4be245318.jpg)
Fig. 8. Localization results of D-LIOM in complex urban environments. Despite the fact that GPS is not very reliable under the shelter of towering structures, the trajectories aligned with GPS show that our D-LIOM can nevertheless produce globally consistent localization results when running for a long time in large-scale scenes.
VIRAL配备了两个视角互补、分别水平与垂直安装的激光雷达；其二是它具备厘米级的定位真值。因此，我们在VIRAL数据集上，从绝对定位误差的角度，将我们的D-LIOM与两种最先进的竞争对手LIOM [^9]及LIO-SAM [^10]进行了比较。 由于LIOM和LIO-SAM仅支持单激光雷达输入，因此我们利用VIRAL的水平激光雷达数据来研究
TABLE III
ABSOLUTE POSITIONING ERRORS (M). "OURS (H)" AND "OURS (HV)" MEAN D-LIOM WITH THE HORIZONTAL LiDAR INPUT OR BOTH THE HORIZONTAL AND VERTICAL LiDARS. OVER EACH TEST SEQUENCE, THE RESULT WITH THE SMALLEST ERROR IS IN BOLD. "W-AVG" MEANS THE WEIGHTED AVERAGE ERROR OF THE RELEVANT SEQUENCES
DataPosesLIOM [^9]LIO-SAM [^10]Ours (H)Ours (HV)
eee0166161.060.100.230.09
eee0255120.720.080.240.08
eee0329901.030.120.110.10
nya0176752.240.090.140.10
nya0276791.970.120.140.10
nya0376973.000.420.150.10
sbs0156231.670.110.160.12
sbs0260621.810.120.150.11
sbs0354342.000.100.190.15
w-avg1.820.150.170.10
LIOM、LIO-SAM以及我们的D-LIOM的性能。此外，由于D-LIOM对多激光雷达输入有强大支持，我们也利用VIRAL的水平与垂直激光雷达数据对其进行了测试。为便于描述，仅使用水平激光雷达的D-LIOM记为“Ours (H)”，采用双激光雷达数据的D-LIOM记为“Ours (HV)”。结果汇总于表III中。 如表III所示，D-LIOM在VIRAL [^22]上的定位精度比基于特征的LIOM [^9]高出整整一个数量级。而与基于特征的LIO-SAM [^10]相比，当仅使用水平激光雷达时，D-LIOM的定位精度略低。这一现象的可能原因是D-LIOM的精度与概率子图的分辨率相关。当测绘范围较小且载体运动速度较慢时（VIRAL是来自慢速无人机采集的室内数据集），直接优化点到线及点到面误差项的基于特征的方法有可能获得更精细的匹配结果。当系统输入为具有互补视角的多激光雷达数据时，我们的D-LIOM将加权定位精度提升了 ---
TABLE IV
REVISITING ERRORS (M) OF COMPETING LiDAR-INERTIAL SLAM FRAMEWORKS ON SELF-COLLECTED TONGJI DATASET
DataCarto3DLOAMLIOMLIO-SAMD-LIOM
TJ-114.7090.33failed1.080.81
TJ-239.97305.76failed27.870.65
TJ-334.89129.8022.8114.160.59
TJ-452.76380.10failed40.210.58
5厘米，超过了LIO-SAM [^10]。毫不奇怪，这种互补视角对于搭载在无人机上的激光雷达-惯性SLAM系统而言至关重要。当载体靠近地面时，单个激光雷达便能在水平和垂直方向上提供充足的观测数据，从而不再存在这一限制。 2) 重访误差分析：除了探究VIRAL [^22]上的绝对定位精度外，我们还通过在TONGJI数据集上对比D-LIOM与现有最先进算法在更复杂环境中的表现，验证了D-LIOM的优越性能。为自动生成基准真值，我们采用NDT匹配方法对重访位置周围的点云进行配准[^32]，并将成功配准点对之间的相对位姿作为参考值。随后，将每种激光雷达-惯性SLAM算法估算出的这两个节点间的相对位姿与基准真值之间的差异，定义为重访误差。为了便于比较，我们选取了Carto3D [^11]、LOAM [^6]、LIOM [^9]以及LIO-SAM [^10]作为对比对象。相关结果如表IV所示。 如表IV所示，我们的D-LIOM在所有测试序列中均取得了优异的结果。首先，与松耦合方案[^6]、[^11]相比，D-LIOM的性能优势显著；其次，与紧耦合的LIO-SAM [^10]和LIOM [^9]相比，D-LIOM的重访误差明显更小。其根本原因在于我们的回环检测机制能够有效消除长期建图中的累积误差，而依赖经验距离阈值的LIO-SAM回环检测机制此时容易失效。最后，与基于直接配准的Carto3D [^11]相比，D-LIOM的改进主要体现在两方面：一是通过实时优化修正了惯性测量单元偏差；二是基于子图匹配的回环检测不仅提高了回环闭合的检测率，还提供了更合理的相对位姿初始值，进而为后续回环约束的建立创造了便利条件。 3) 时间开销：与定位及建图精度一样，时间开销也是评估SLAM系统性能的关键指标。为了定量分析所提出的D-LIOM的处理速度，我们分别测量了LOAM [^6]、LIOM [^9]、LIO-SAM [^10]以及我们的D-LIOM在处理16线激光雷达和64线激光雷达单次扫描时，“里程计”模块和“建图”模块所耗费的时间。需要注意的是，我们的框架实际上在前端就完成了部分建图工作（子图构建），而在后端的全局位姿图优化阶段，仅间隔性地执行回环检测和全局优化操作。因此，在统计D-LIOM的“建图”时间时，我们将所有后端处理时间相加后再取每帧的平均值。
TABLE V COMPARISON OF TIME COSTS (MS)
LOAMLIOMLIO-SAMD-LIOM
16 linesOdometry55.6684.725.927.5
Mapping166.6140.6124.133.5
64 linesOdometry116.21181.147.254.5
Mapping117.7236.9158.060.1
TABLE VI
REVISITING ERRORS (M) OF D-LIOM, D-LIOM $_{W_{oG}}$ (D-LIOM WITHOUT THE GRAVITY FACTOR), AND D-LIOM $_{W_{oL}}$ (D-LIOM WITHOUT THE SUBMAP-TO-SUBMAP LOOP DETECTION)
TJ-1TJ-2TJ-3TJ-4
D-LIOM $_{W\circ G}$7.140.650.5911.45
D-LIOM $_{W\circ L}$12.1419.813.6861.37
D-LIOM0.810.650.590.58
根据表V可知，在前端“里程计”环节，D-LIOM耗时远少于LOAM和LIOM，且处理效率可与LIO-SAM相媲美。在后端，得益于我们的回环检测策略，D-LIOM的效率与前端接近，这一特性对于大规模场景下的长期建图至关重要。 此外，在使用多激光雷达时，我们的D-LIOM也能通过便捷的下采样操作高效运行。相比之下，基于特征的方案——LOAM [^6]、LIOM [^9]以及LIO-SAM [^10]——必须分别提取每个激光雷达扫描数据的特征后再进行下采样。由于这类方案在“里程计”环节的主要效率瓶颈在于特征提取与匹配，因此它们的效率必然会有所下降。 最后，另一点值得提及的是，我们的D-LIOM和Carto3D [^11]均基于直接配准方式，因此在前端具有相近的效率。不过在后端，我们基于子图匹配的回环检测机制一方面能够减少大量不必要的处理工作；另一方面，子图匹配的副产品（即相对位姿的初始值）还能进一步提升从扫描数据到3D子图的回环配准效率。 ### D. 消融实验 在此，我们验证了所引入的重力先验因子以及提出的回环检测策略对D-LIOM而言的必要性。 1) 重力先验因子：为了验证重力先验因子对D-LIOM的作用，我们在保持其余设置不变的情况下，对比了D-LIOM与禁用重力因子的D-LIOM $W_{oG}$在TJ-1~TJ-4这四个序列上的重访误差。 如表VI所示，当禁用重力因子时，D-LIOM $_{W_{o}G}$在TJ-1和TJ-4上的定位性能明显变差。由此可以推断，引入重力先验因子能够使我们的D-LIOM在重力方向上的漂移量降低，从而确保2D子图投影的几何精度，最终保障回环闭合的正确检测并提升整体性能。 ---
TABLE VII PRECISION AND RECALL OF OUR LOOP DETECTION
Sample numberMetric
true-positivefalse-positivefalse-negativePrecisionRecall
231231190.94%95.45%
![Image](images/1a67523c4910b93a22ffc3e12189838b16c26623aed73a4be2c7da505de89fbe.jpg)
Fig. 9. Typical failure cases of the submap-to-submap loop detection.
2) 回环检测模块：我们通过对比启用S2SLD的D-LIOM与在后端禁用S2SLD的D-LIOM $_{W_{oL}}$，来展示框架中基于子图到子图的回环检测机制（简称S2SLD）对结果的影响。D-LIOM $_{W_{oL}}$与D-LIOM在TJ-1~TJ-4上的重访误差分别列于表VI的第二行和第三行。可以看出，启用S2SLD后D-LIOM的性能有了显著提升，这验证了S2SLD的有效性。 此外，为了更深入地了解S2SLD，我们分析了两个常用的评估其性能的指标：精确率和召回率。假设正确检测到的回环闭合记为真阳性（TP），错误识别的回环闭合记为假阳性（FP），未检测到的回环闭合记为假阴性（FN）。那么精确率和召回率的定义如下： $$ Precision=\frac{TP}{TP+FP}, $$ $$ Recall=\frac{TP}{TP+FN}. $$ 在TJ-1~TJ-4上，我们按照第III-F节所述的方法匹配所有子图以检测回环闭合。相关样本数量及这两个指标的数值列于表VII中。结果显示，所提出的S2SLD分别达到了90.94%和95.45%的极高精确率和召回率，从而确保了D-LIOM的长期建图一致性。 ### E. 失效案例研究 尽管D-LIOM（S2SLD）的回环检测在召回率和精确度方面取得了相当高的水平，但仍存在一定概率的误检或漏检现象。图9展示了典型的假阴性及假阳性回环闭合误判示例。可以看出，这些误判主要源于特征点的不匹配。在假阴性示例中，这种不匹配会导致估计出的相似变换出现较大偏差；而在假阳性示例中，子图之间高度相似的结构导致了S2SLD的误判。实际上，仅使用低层特征时，这类情况难以避免。通过提取高层信息（如语义物体），有望提升S2SLD的性能。不过也需注意的是，相较于假阴性而言，假阳性对D-LIOM的负面影响相对较小。一旦检测到错误的回环闭合，在建立回环约束时，它会在精确的扫描与子图配准阶段被进一步过滤掉。 ### F. 关于潜在改进的讨论 D-LIOM的潜在改进主要体现在两个方面。目前，前端忽略了IMU与激光雷达之间的时间差；若能显式估计这一时间偏移量，激光雷达-惯性里程计的准确性有望得到提升。此外，如上所述，子图间的回环检测有时会因特征点不匹配而受影响；通过寻找更鲁棒的特征，预计能在一定程度上缓解这种负面影响。 ### V. 结论 在本文中，我们结合了直接配准与紧耦合联合优化的优势，提出了一种多功能、鲁棒且精确的激光雷达-惯性SLAM框架D-LIOM。框架前端会同时对重力因子、IMU预积分因子以及来自直接配准的激光雷达里程计因子进行优化，从而实时估计载体的状态并校正IMU偏差。在D-LIOM的后端，通过将对齐重力的子图投影到二维水平面，能够高效地检测回环闭合并通过子图间匹配来建立回环约束，进而迅速消除系统漂移。此外，为了提升该框架对不同传感器配置的适应性，我们还提出了一种初始化方法，支持由多个激光雷达和一个通用六轴IMU组成的激光雷达-惯性SLAM系统。与现有最先进的算法相比，D-LIOM取得了卓越的成果：当载体高速运动且在大规模环境中工作时，它能生成更精确的地图结构，同时具备更高的定位精度。 ## 附录 假设向量$\boldsymbol{a} \in \mathbb{R}^{3 \times 1}$左乘旋转矩阵$\boldsymbol{R} \in \mathbb{SO}(3)$（其对应的旋转向量为$\boldsymbol{\alpha} \in \mathfrak{so}(3)$），则可以按如下方式求得$\boldsymbol{R}\boldsymbol{a}$关于$\boldsymbol{\alpha}$的雅可比矩阵： $$ \begin{align*}\frac{\partial\mathrm{Exp}(\alpha)\boldsymbol{a}}{\partial\alpha}&=\lim_{\delta\alpha\to0}\frac{\mathrm{Exp}(\alpha+\delta\alpha)\boldsymbol{a}-\mathrm{Exp}(\alpha)\boldsymbol{a}}{\delta\alpha}\\&=\lim_{\delta\alpha\to0}\frac{(\mathrm{Exp}(\alpha)\mathrm{Exp}(\mathrm{J}_{r}(\alpha)\delta\alpha)-\mathrm{Exp}(\alpha))a}{\delta\alpha}\\&=\lim_{\delta\alpha\to0}\frac{(\mathrm{Exp}(\alpha)(\boldsymbol{I}+\lfloor\mathrm{J}_{r}(\alpha)\delta\alpha\rfloor_\times)-\mathrm{Exp}(\alpha))a}{\delta\alpha}\\&=\lim_{\delta\alpha\to0}\frac{\mathrm{Exp}(\alpha)\lfloor\mathrm{J}_{r}(\alpha)\delta\alpha\rfloor_\times a}{\delta\alpha}\\&=\lim_{\delta\alpha\to0}-\frac{\mathrm{Exp}(\alpha)\lfloor\boldsymbol{a}\rfloor_\times\mathrm{J}_{r}(\alpha)\delta\alpha}{\delta\alpha}\\&=-\mathrm{Exp}(\alpha)\lfloor\boldsymbol{a}\rfloor_\times\mathrm{J}_{r}(\alpha),\end{align*} $$ 其中$J_{r}(\alpha)$为右雅可比矩阵，其表达式如式(12)所示。 --- ## 参考文献 [^1]: C. Cadena et al., “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Trans. Robot., vol. 32, no. 6, pp. 1309–1332, Dec. 2016. [^2]: L. Li, Z. Li, S. Liu, and H. Li, “Motion estimation and coding structure for inter-prediction of LiDAR point cloud geometry,” IEEE Trans. Multimedia, to be published, doi: 10.1109/TMM.2021.3119872. [^3]: M. Krivokuca, E. Miandji, C. Guillemot, and P. Chou, “Compression of plenoptic point cloud attributes using 6-D point clouds and 6-D transforms,” IEEE Trans. Multimedia, to be published, doi: 10.1109/TMM.2021.3129341. [^4]: C. Lv, W. Lin, and B. Zhao, “Voxel structure-based mesh reconstruction from a 3D point cloud,” IEEE Trans. Multimedia, vol. 24, pp. 1815–1829, 2021. [^5]: D. V. Nam and K. Gon-Woo, “Solid-state LiDAR based-SLAM: A concise review and application,” in Proc. IEEE Int. Conf. Big Data Smart Comput., Jeju Island, South Korea, 2021, pp. 302–305. [^6]: J. Zhang and S. Singh, “LOAM: LiDAR odometry and mapping in real-time,” in Proc. Int. Conf. Robot. Sci. Syst., CA, USA, 2014, pp. 1–9. [^7]: 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., Madrid, Spain, 2018, pp. 4758–4765. [^8]: C. Qin et al., “LINS: A LiDAR-inertial state estimator for robust and efficient navigation,” in Proc. IEEE Int. Conf. Robot. Autom., Paris, France, May 2020, pp. 8899–8906. [^9]: M. L. Haoyang Ye and Y. Chen, “Tightly coupled 3D LiDAR inertial odometry and mapping,” in Proc. IEEE Int. Conf. Robot. Autom., Montreal, Canada, May 2019, pp. 3144–3150. [^10]: T. Shan et al., “LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, USA, 2020, pp. 5135–5142. [^11]: W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2D LiDAR SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., Stockholm, Sweden, May 2016, pp. 1271–1278. [^12]: J. Sola, “Quaternion kinematics for the error-state Kalman filter,” Nov. 2017, arXiv:1711.02508. [^13]: 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. [^14]: M. Kaess, A. Ranganathan, and F. Dellaert, "iSAM: Incremental smoothing and mapping," IEEE Trans. Robot., vol. 24, no. 6, pp. 1365–1378, Dec. 2008. [^15]: M. Kaess et al., “iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in Proc. IEEE Int. Conf. Robot. Autom., Shanghai, China, 2011, pp. 3281–3288. [^16]: J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV,” in Proc. IEEE Int. Conf. Robot. Autom., Paris, France, May 2020, pp. 3126–3131. [^17]: S. Kohlbrecher, O. Von Stryk, J. Meyer, and U. Klingauf, “A flexible and scalable SLAM system with full 3D motion estimation,” in Proc. IEEE Int. Symp. Saf. Secur. Rescue Robot., Kyoto, Japan, 2011, pp. 155–160. [^18]: E. B. Olson, “Real-time correlative scan matching,” in Proc. IEEE Int. Conf. Robot. Autom., Kobe, Japan, 2009, pp. 4387–4393. [^19]: G. Kim and A. Kim, “Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Madrid, Spain, 2018, pp. 4802–4809. [^20]: Y. Wang et al., “LiDAR iris for loop-closure detection,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, USA, 2020, pp. 5769–5775. [^21]: X. Chen et al., “OverlapNet: A siamese network for computing LiDAR scan similarity with applications to loop closing and localization,” Auton. Robot., vol. 46, no. 1, pp. 61–81, Aug. 2021. [^22]: T.-M. Nguyen et al., “NTU VIRAL: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint,” Int. J. Robot. Res., Dec. 2021, doi: 10.1177/02783649211052312. [^23]: J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, and A. Kim, “Complex urban dataset with multi-level sensors from highly diverse urban environments,” Int. J. Robot Res., vol. 38, no. 6, pp. 642–657, Apr. 2019. [^24]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “LIC-Fusion: LiDAR-Inertial-Camera odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Macau, China, 2019, pp. 5848–5854. [^25]: X. Zuo et al., “LIC-Fusion 2.0: LiDAR-inertial-camera odometry with sliding-window plane-feature tracking,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, USA, 2020, pp. 5112–5119. [^26]: J. Lin, C. Zheng, W. Xu, and F. Zhang, “R $^{2}$ LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robot. Autom. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021. [^27]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. Robot. Autom., Rome, Italy, Apr. 2007, pp. 3565–3572. [^28]: 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. [^29]: R. Mur-Artal and J. D. Tardós, “Visual-inertial monocular SLAM with map reuse,” IEEE Robot. Autom. Lett., vol. 2, no. 2, pp. 796–803, Apr. 2017. [^30]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^31]: P. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 14, no. 2, pp. 239–256, Feb. 1992. [^32]: P. Biber and W. Strasser, “The normal distributions transform: A new approach to laser scan matching,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, USA, 2003, pp. 2743–2748. [^33]: S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics (Intelligent Robotics and Autonomous Agents), 2nd ed., Cambridge, MA, USA: MIT Press, 2005. [^34]: K. Levenberg, “A method for the solution of certain problems in least square,” Quart. Appl. Math., vol. 2, no. 2, pp. 164–168, 1944. [^35]: D. W. Marquardt, “An algorithm for least-squares estimation of nonlinear parameter,” J. Appl. Math., vol. 11, no. 2, pp. 431–441, 1963. [^36]: H. Armin, M. W. Kai, B. Maren, S. Cyrill, and B. Wolfram, "OctoMap: An efficient probabilistic 3D mapping framework based on octrees," Auton. Robot., vol. 34, pp. 189–206, 2013. [^37]: B. Antoni, G. Yolanda, and O. Gabriel, “On the use of likelihood fields to perform sonar scan matching localization,” Auton. Robot., vol. 26, pp. 203–222, 2009. [^38]: E. A. Wan and R. van der Merve, “The unscented kalman filter for nonlinear estimation,” in Proc. IEEE Adapt. Syst. Signal Proc. Commun. Control Sympo., Lake Louise, Canada, 2000, pp. 153–158. [^39]: D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” Int. J. Comput. Vis., vol. 60, no. 2, pp. 91–110, Jan. 2004. [^40]: H. Bay, T. Tuytelears, and L. Van Gool, “SURF: Speeded up robust features,” in Proc. Eur. Conf. Comput. Vis., Graz, Austria, 2006, pp. 404–417. [^41]: E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, "ORB: An efficient alternative to SIFT or SURF," in Proc. IEEE Int. Conf. Comput. Vis., Barcelona, Spain, 2011, pp. 2564–2571. [^42]: M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration,” in Proc. Int. Conf. Comput. Vis. Theory Appl., Lisboa, Potugal, 2009, pp. 331–340. [^43]: M. A. Fischler and R. C. Bolles, “Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography,” Commun. ACM, vol. 24, no. 6, pp. 381–395, Jun. 1981. [^44]: J. Lv, J. Xu, K. Hu, Y. Liu, and X. Zuo, “Targetless calibration of LiDAR-IMU system based on continuous-time batch estimation,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Las Vegas, USA, 2021, pp. 9968–9975. [^45]: O.J. Woodman, “An introduction to inertial navigation,” Univ. Cambridge, Comput. Lab., Tech. Rep. UCAM-CL-TR-696, Aug. 2007. [^46]: S. Umeyama, “Least-squares estimation of transformation parameters between two point patterns,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 13, no. 4, pp. 376–380, Apr. 1991.