SLAM论文阅读：M-Loam：具有在线外参校准功能的多LiDAR系统的稳健里程表和建图

基于loam的多激光雷达slam

论文题目：

Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration

项目链接：

https://ram-lab.com/file/site/m-loam/

开源代码链接：

https://github.com/gogojjh/M-LOAM

视频展示：

https://www.bilibili.com/video/av756354450/

文章阅读：

摘要——结合多个激光雷达，使机器人能够最大限度地提高其对环境的感知意识，并获得足够的测量值，这有利于同时定位和映射(SLAM)。本文提出了一种实现多个激光雷达的鲁棒和同时的外在校准、测深和映射的系统。我们的方法从测量预处理开始，从原始测量中提取边缘和平面特征。在一个运动和外在初始化程序之后，一个基于滑动窗口的多激光雷达测深仪在机载上运行，通过在线校准细化和收敛识别来估计姿态。我们进一步开发了一种映射算法来构建全局地图和优化具有足够特征的姿态，以及一种捕获和减少数据不确定性的方法。我们在10个序列(4.60km总长度)上进行了广泛的实验，验证了我们的方法的性能，并将其与最先进的技术进行了比较。我们证明了所提出的工作是一个完整的、鲁棒的和可扩展的系统，用于各种多激光雷达设置。

1 介绍

1.1挑战

尽管它们在环境感知方面具有巨大的优势，但也有许多问题影响了使用多激光雷达设置的SLAM的发展。

1.1.1精确而灵活的外在校准：为一个新的机器人平台恢复多激光雷达转换是复杂的。在许多情况下，专业用户必须仔细校准人造环境中的传感器。这一要求增加了为野外机器人部署和维护多激光雷达系统的成本。

我们希望该系统能够在各种在线环境中自我校准外部功能。如[13]-[15]所示，受益于对外部运动和自我运动的同时估计，视觉惯性系统的工作范围已扩展到户外场景中的无人机和船只。这些方法在任务期间持续执行校准，以保证目标功能始终为“最佳”。然而，这一过程通常需要得到完全可以观察到的环境或运动约束。否则，由此产生的外功能可能变得次优或不可靠。因此，我们必须修复额外的东西，如果他们是精确的，这就产生了一个在线校准与收敛识别的需求。为了应对厘米级校准误差和外部系统随时间的意外变化，模拟多激光雷达系统的外在扰动也有好处

1.1.2 低姿态漂移：为了实时提供准确的姿态，最先进的(SOTA)激光雷达方法[16]通常通过两种算法解决：测测量和映射。这些算法被设计成以粗到细的方式估计姿态。基于原始的测深算法，需要一种在局部窗口内充分利用多激光雷达测量的方法。不断增加的约束条件有助于防止框架到框架配准的退化或失败。随后的映射算法在相对较低的频率下运行，并得到了大量的特征点和多次迭代，以获得更好的结果。然而，正如我们在第八节中所指出的那样，许多SOTA方法忽略了全局地图中的不确定点限制了准确性这一事实。为了尽量减少这种不利影响，我们必须开发一种方法来捕获地图点的不确定性和拒绝异常值。

1.2 贡献

为了解决这些挑战，我们提出了M-LOAM，一个用于多激光雷达外部校准、实时测度法和映射的鲁棒系统。在不需要人工干预的情况下，我们的系统可以从几个外部未校准的激光雷达开始，自动校准它们的外部图像，并提供准确的姿势以及一个全球一致的地图。我们之前的工作[5]提出了基于滑动窗口的测深法来将激光雷达点与高频IMU测量值相融合。这个框架激发了这篇论文，我们试图解决多激光雷达融合的问题。此外，我们引入了一种基于运动的方法[4]来初始化外部事物，并使用[19]中的工具来表示不确定的量。我们的M-loam设计做出了以下贡献：

1.2.1 计算所有关键状态的自动初始化，包括连续帧之间的运动以及后续阶段的附加功能。它可以在任意位置开始，而无需事先知道机械配置或校准对象（第六节）。

1.2.2 与一般收敛准则同时进行自校准的在线自校准。它能够以完全无监督的方式监控收敛性和触发终止（第VII-B节）。

1.2.3 基于滑动窗口的测距法，充分利用了来自多个激光雷达的信息。这种实现可以解释为小规模的帧到地图配准，这进一步减少了连续帧到帧测距法积累的漂移（第VII-C节）。

1.2.4 映射与两个阶段的方法，捕获和传播不确定的量从传感器噪声，退化姿态估计，和外在扰动。这种方法使映射过程具有不确定性，并帮助我们保持全球地图的一致性，并增强系统用于长期导航任务的鲁棒性（第八节）。

据我们所知，m-loam是第一个实现多激光雷达校准和单激光器的完整解决方案。该系统在手持设备和自动驾驶车辆上的广泛实验下进行了评估，涵盖了从室内办公室到室外城市道路的各种场景，其性能优于基于SOTA激光雷达的方法。关于不同平台上的校准，我们的方法达到了平移厘米和旋转十度的外在精度。对于不同尺度上的SLAM，M-LOAM已成功地应用于提供准确的姿势和地图结果。图中示1可视化每个阶段的M-LOAM的输出。为了使研究社区受益，我们公开发布了我们的代码、实现细节和多激光雷达数据集。

1.3 安排

本文的其余部分组织如下。第二节回顾了相关文献。第三节阐述了这个问题，并提供了基本的概念。第四节概述了该系统。第五节介绍了激光雷达测量方面的预处理模块。第六节介绍了运动和外部初始化过程。第七节介绍了紧密耦合，基于滑动窗口的多激光雷达测深法(M-LO)与在线校准改进。第八节介绍了感知不确定性的多激光雷达映射算法，在第九节介绍了实验结果。在第十节中，我们对所提出的系统进行了讨论。最后，第十一章总结了本文，并介绍了未来可能的研究方向。

2 相关工作

关于SLAM和外部校准的学术工作是广泛的。在本节中，我们简要回顾了基于激光雷达的SLAM和多传感器系统的在线校准方法的相关结果。

2.1 基于激光雷达的SLAM

一般来说，许多SOTA是由迭代最近点(ICP)算法[20]-[23]开发而来的，其中包含的方法包括测量预处理、退化预测和传感器融合。这些工作已经推动了当前基于激光雷达的系统变得对大规模环境的快速、健壮和可行。

2.1.1 测量预处理：作为系统的前端，测量预处理将点云编码成一个紧凑的表示。我们将相关算法分类为密集方法或稀疏方法。作为一种典型的密集方法，SuMa[24]证明了利用超地图进行配准和回环检测的优点。它的扩展版本[25]将语义约束合并到原始的代价函数中。密集像素对应的匹配过程使该方法计算密集，因此，需要专用的硬件(GPU)来进行实时操作。这些方法不适用于我们的情况，因为我们必须经常在CPU上执行配准。

稀疏方法更倾向于从原始测量中提取几何特征，因此应该具有实时性能。Grant等人。提出了一个基于平面的配准，而Velas等人。将点云表示为一撮线段。与它们相比，loam[16]对传感器和周围环境的假设较少。它从边缘线和局部平面斑块中选择不同的点。最近，以下几种方法采用了地平面[17]、[28]、视觉检测、概率网格图、良好特性[31]或直接使用密集扫描仪[18]、[32]、[33]，以提高稀疏方法在噪声或无结构环境中稀疏方法的性能。为了限制在涉及到更多的激光雷达时的计算时间，我们提取了稀疏的边缘和平面特征，如loam。但不同的是，我们以一种更简洁和统一的方式来表示它们的剩余部分。

2.1.2 状态估计中的退化：特征的几何约束被表示为一个状态估计问题。针对退化问题，提出了不同的方法。张等人定义了一个等于信息矩阵的最小特征值的因子，以确定系统的退化。他们还提出了一种叫做解重映射的技术，以在良好条件的方向上更新变量。该技术被进一步应用于定位、配准、姿态图优化和传感器故障检查等任务。为了量化一个非线性系统的可观测性，Rong等人说计算了经验可观察性Gramian矩阵的条件数。由于我们的问题将目标函数线性化了，就像Zhang等人中所做的那样。，我们还引入了解决方案的重新映射来更新状态。此外，我们的在线校准方法采用退化因子作为外部校准的定量度量。

2.1.3 多传感器融合：利用多传感器来提高单激光雷达测距仪的运动跟踪性能是很有前景的。现有的大多数基于激光雷达的融合工作结合了视觉或惯性测量。处理多模态测量的最简单方法是松散耦合融合，其中每个传感器的姿态都是独立估计的。例如，激光雷达-IMU融合通常由扩展的卡尔曼滤波器(EKF)[39]-[41]完成。通过联合利用所有测量值来优化传感器姿态的紧密耦合算法在工程中越来越普遍。它们通常是由EKF或批处理优化来完成的。除了多模态传感器的融合外，另一种方法是探索多重激光雷达（单模型）的融合，这仍然是一个有待解决的问题。

多个激光雷达通过最大限度地提高传感覆盖范围来改进一个系统。受紧密耦合算法成功的启发，我们实现了一个滑动窗口估计器来优化多个激光雷达的状态。这种模式对多激光雷达融合具有很大的优点。

2.2 多传感器校准

精确的外在校准对任何多感量系统都很重要。传统的方法，在执行任务之前，必须运行一个特别的校准程序。每当结构上有轻微的扰动时，就需要重复这个繁琐的过程。估计这些参数的一个更灵活的解决方案是基于SLAM的组合技术。在这里，外肌被视为状态变量之一，并随传感器的姿势进行优化。该方案也适用于一些非平稳参数，如机器人运动学、IMU偏差和时间偏移量。

库默尔等人[52]开创了一个超图优化框架来校准机载激光扫描器。实验表明，在线对参数的校正得到了一致和准确的结果。泰希曼等人[53]同时提出了一种迭代的SLAM拟合管道来解决两个RGB-D相机的失真问题。为了恢复多相机系统的空间偏移，Heng等人[54]将这个问题表述为一个束调整，而欧阳等人他使用了阿克曼的汽车转向模型来约束额外的功能。如中所示，传感器间时间偏移的在线估计对以图像为中心的系统至关重要。秦等人[56]利用视觉特征的再投影误差来表述时间校准问题，而邱等人[58]提出了一种更一般的方法，通过分析传感器的运动相关性来校准异构传感器。

本文隐式地将多个激光雷达的时间与一个基于硬件的外部时钟进行同步，并明确地关注外在校准。我们的方法包括一个在线程序，以实现灵活的多激光雷达外部校准。为了监测估计外胚的收敛性，我们提出了一个一般的准则。此外，我们对外在扰动进行了建模，以减少其对长期导航任务的负面影响。

3 问题陈述

我们用最大似然估计(MLE)来表示M-LOAM。MLE导致了一个非线性优化问题，其中高斯协方差的逆加权残余函数。在深入研究M-loam的细节之前，我们首先介绍一些基本概念。第三节A节表示符号。第III-B节介绍了MLE，第III-C节描述了合适的模型来表示R3中的不确定测量值和SE（3）中的转换。最后，第三节D简要介绍了MLE在m-loam中近似高斯噪声的实现。

3.1 注释和定义

该命名法详见表一。我们考虑一个由一个主激光雷达和多个辅助激光雷达组成的系统。主激光雷达定义了基本帧，我们使用()l1/()b来表示它。辅助激光雷达的帧由()li、i>1表示。我们表示F表示从激光雷达原始测量中提取的可用特征。每个特征在三维空间中被表示为一个点：p=[x，y，z]T。状态向量，由平移部分和旋转部分组成，由x=[t，q]表示，其中t是一个3×1向量，q是汉密尔顿四元数。但在我们需要旋转一个向量的情况下，我们使用了李群SO（3）中的3×3旋转矩阵R。我们可以用罗德里格斯公式59]将q转换成R。第八节将不确定性与向量空间上的姿态联系起来，其中我们使用李群SE（3）中的4×4变换矩阵T来表示一个姿态。旋转矩阵和平移向量可以与变换矩阵作为T关联.

3.2 最大似然估计

我们将多激光雷达系统的姿态和外部估计表述为一个MLE问题[60]：

其中，Fk表示第k帧中的可用特征，xk是要优化的状态，而f（·）是目标函数。假设测量模型存在高斯噪声[3]，问题（1）成为一个非线性最小二乘(NLS)问题：

其中，ρ（·）是处理异常值的鲁棒Huber损失[61]，r（·）是残余函数，Σi是协方差矩阵。迭代方法，如高斯-牛顿或莱文伯格马夸特，可以用来解决这个NLS问题。这些方法通过计算雅可比矩阵的w.r.t来局部线性化目标函数。xk作为J=∂f/∂xk。给定一个初始预估值，xk被使用J迭代优化，直到收敛到一个局部最小值。在最后一次迭代中，状态的最小二乘协方差计算为Ξ=Λ−1[62]，其中Λ=JTJ称为信息矩阵。

3.3 不确定表示法

我们使用19]中的工具来表示数据的不确定性。我们首先表示一个有噪声的激光雷达点为

其中¯p是一个无噪声向量，ζ∈R3是一个均值为零的小的高斯扰动变量，Z是激光雷达测量值的噪声协方差。为了使（3）与变换矩阵兼容(即 =T )兼容，我们还用4×1齐次坐标来表示它：

其中D是一个将3×1向量转换为齐次坐标的矩阵。如[63]中所研究的那样，激光雷达的深度测量误差（也称为传感器噪声）主要受到目标距离的影响。Z被简单地设置为一个常数矩阵。然后我们根据SE（3）中定义一个具有小扰动的随机变量

1Z=diag（σ2x、σ2y、σz2），其中σx、σy、σz是激光雷达沿不同轴的深度噪声的标准差。我们参考获取特定激光雷达的手册。2（·）∧算符将ξ变成李代数s（3）的一个成员。指数映射将se（3）的元素与SE（3）中的变换矩阵相关联。同样地，我们也可以使用（·）∧和exp（·）将3×1向量φ与SO（3）中的旋转矩阵关联起来。19提供了详细的表达式。

其中¯T是无噪声变换，ξ∈R6是具有协方差Ξ的小摄动变量。这种表示允许我们将平均转换存储为¯T，并使用ξ进行向量空间上的扰动。我们认为，Ξ表示了两个实际的错误来源：

1. 退化姿态估计来自于在弱约束环境中缺乏几何结构等情况[34]。它通常会使姿态在它们退化的方向上变得不确定。现有的工作采用了基于模型和基于学习的[65]方法来估计ICP背景下的姿态协方差。或在长期运行过程中的振动、冲击和温度漂移。

2. 由于校准误差[12]，总是存在外部扰动，特别是宽基线传感器，如立体声相机，比标准传感器遭受更多的外部偏差。这种扰动对多传感器系统的测量精度有害，但难以测量。

Ξ的计算方法详见第八节。

4 在m-loam中具有近似高斯噪声的MLE

       我们将MLE扩展到设计多个M-loam估计器，以以粗到细的方式解决机器人的姿态和外部问题。最重要的步骤是将高斯噪声协方差Σ近似到实际的测量模型。基于第三节-C节的讨论，我们确定了三个误差源可能使地标不确定：传感器噪声、退化姿态估计和外在扰动。帧对帧的运动估计（第六至A节）大致受到传感器噪声的影响。紧密耦合的测深法（第七-C节）建立了姿态优化的局部图。我们应该把一些不确定性传播到每个地图绘制点。然而，如果涉及到更多的激光雷达和滑动窗口，该操作通常会耗时(约10ms−20ms)。为了保证实时测度，我们不计算姿态不确定性。我们简单地将Σ=Z设置为残差的协方差。在映射中，我们得到了足够的时间来实现精确的姿态和全局映射。因此，我们考虑了所有的不确定性源。第八节解释了姿态的不确定性如何影响映射精度和Σ的传播。

       图2中示。说明拟建M-loam系统完整管道的方框图。该系统从测量预处理开始（第五节）。初始化模块（第六节）通过校准优化来初始化后续基于非线性优化的多激光雷达测深法的值（第七节）。根据校准的收敛性，优化分为两个子任务：在线校准和纯里程测定。如果校准收敛，我们可以跳过外部初始化和细化步骤，进入纯测深和映射阶段。具有不确定性感知的多激光雷达地图（第八节）保持了一个全局一致的地图，以减少测深法漂移和消除噪声点。

5 系统综述

我们做了三个假设来简化系统设计：

激光雷达是同步的，这意味着不同激光雷达之间的时间延迟几乎为零。

在校准初始化期间，该平台经历了足够的旋转和平移运动。

主激光雷达的局部图应与辅助激光雷达共享重叠的FOV，以便进行特征匹配，以缩短校准阶段。这可以通过移动机器人来实现。至少90°，实际180°。

图中示2展示了M-loam的管道。该系统从测量预处理（第五节）开始，其中从去噪点云中提取和跟踪边缘和平面特征。初始化模块（第六节）提供了所有必要的值，包括姿势和外部，以引导随后基于非线性优化的M-LO。M-LO融合多激光雷达测量，以优化滑动窗口内的测深和推断。如果外部值已经收敛，我们将跳过外部初始化和细化步骤，并进入纯测深法和映射阶段。概率映射模块（第八节）构造了一个具有足够特征以消除测距漂移的全局映射。测程和映射在两个分离的线程中同时运行。

6 测量预处理过程

我们实施了三个顺序步骤来处理激光雷达的原始测量结果。我们首先将点云分割成许多集群，以去除有噪声的物体，然后提取边缘和平面特征。为了关联连续帧之间的特征，我们匹配了一系列的对应关系。在本节中，每个激光雷达都被独立处理。

6.1 除噪分割

了解激光雷达的垂直扫描角度，我们可以将原始点云投影到范围图像上而无需数据丢失。在图像中，每个有效点都由一个像素表示。像素值记录从一个点到原点的欧几里得距离。我们应用68]中提出的分割方法将像素分组成许多集群。如果水平或垂直方向上的两个相邻点的连接线大致垂直（>60度），我们假设该方向上的两个相邻点属于同一物体。我们使用广度优先搜索算法来遍历所有像素，确保一个恒定的时间复杂度。我们丢弃小集群，因为它们可能在优化中提供不可靠的约束。

6.2 特征提取和匹配

我们感兴趣的是提取一般的边缘和平面特征。我们根据其曲率从测量中选择一组特征点。提取的特征集F组由两个子集组成：边缘子集（高曲率）E和平面子集（低曲率）H。E和H都由部分最具代表性的特征组成。我们进一步收集了曲率最高的E的边缘点，以及曲率最低的H的平面点。这些点形成了两个新的集合：ˆE和ˆH。下一步是确定两个连续帧之间的特征对应关系，()lik−1→()lik，以构造几何约束。对于Eˆlik中的每个点，选择来自Elik−1的两个最近邻作为边对应。对于点Hlik，形成一个平面的离Hlik−1最近的三个点被选择作为平面对应点。

7 初始化

优化多个激光雷达的状态是高度非线性的，需要初始预估。本节介绍了我们的运动和外部初始化方法，它不需要任何传感器的套件机械配置。它也不涉及任何手动的努力，这使它对自主机器人特别有用。

7.1 基于扫描的运动估计

通过发现每个激光雷达的连续两帧之间的对应关系，我们通过最小化所有特征的残差误差来估计帧对帧的变换。如图3所示。残差由边和平面对应来表示。设xk是激光雷达在第k帧处的两次扫描之间的相对变换。关于平面特征，对于一个点p∈ˆHlik，如果Π是相应的平面斑块，则平面残差计算为：

其中a是点到平面的欧几里得距离，[w，d]是Π的系数向量。然后，对于边缘点p∈Eˆlik，如果L是相应的边线，我们使用（6）来定义边缘残差为两个平面残差的组合：

其中[w1、d1]和[w2、d2]为Π1和Π2的系数，w1与L到p的投影方向重合，Π2垂直于Π1，在w2⊥w1和w2⊥L的条件下。上述定义loam[16]的定义不同，并且有两个好处。首先，边缘残差为状态提供了额外的约束条件。此外，残差被表示为向量，而不是标量，允许我们乘以一个3×3协方差矩阵。我们最小化所有剩余误差的和，以得到MLE作为：

其中，rF（·）的雅各比行列式，详见附录A。

实际实践中，滚动快门扫描使激光雷达运动后点是倾斜的。在解决了增量运动xk之后，我们通过将点转换为最后一帧()lik−1来纠正它们的位置。让tk−1和tk分别是激光雷达扫描的开始和结束时间。对于在t∈p(tk−1，tk]捕获的点，它被转换为：

其中，旋转和平移为线性插值：

7.2 多台激光雷达系统的校准

通过对齐两个传感器的运动序列，得到了初始的提取量。这就是解决手工校准问题AX=XB，其中A和B是两个传感器的历史转换，X是它们的外部转换。当机器人移动时，以下第i激光雷达的方程式应该适用于任何k

根据[14]，原始问题被分解为旋转和平移分量。我们实现了这个方法来在线初始化附加内容。

1)旋转初始化：我们通过使用四元数来重写（11）作为一个线性方程：

其中，⊗是四元数乘法算子，Q1（·）和Q2（·）是左右四元数乘法的矩阵表示[59]。叠加（13）从多个时间间隔，我们形成了一个过度确定的线性系统作为

其中，K是约束的数量，而wk−1k是定义为残余四元数的角轴表示中的角度的稳健权值：

其中ρ0（·）是Huber损失的导数，ˇqbli是目前估计的外在旋转，q∗是q的逆。根据kqblik=1，我们使用SVD找到了（14）的封闭解。为了实现3自由度旋转的完全可观测性，需要足够的运动激发。在足够的约束下，QK的空空间应该为1。这个意味着我们只有一个零的奇异值。否则，由于一个或多个轴上的简并运动，QK的空空间可能大于一个。因此，我们需要通过检查是否实现该条件来确保第二个小的奇异值σmin2足够大。我们设置了一个阈值σR，并在使用σmin2>σR时终止旋转校准。不断多的数据使QK行迅速增长。为了限制计算时间，我们使用长度为K=300的优先级队列[69]来增量地存储历史约束。消除具有小旋转的约束。

1. 平移初始化：一旦旋转校准完成，我们从（通过合并所有可用数据作为12）构建一个线性系统

其中，通过应用最小二乘方法得到tbli。然而，如果机器人的运动是平面的，那么在z−轴上的平移是不可观测到的。在这种情况下，我们设置了tz=0，并通过删除tbli的z−组件来重写（16）。与[4]不同的是，我们的方法不能通过利用地平面来初始化tz，并且必须在细化阶段恢复它（第VII-B节）。

8 紧密耦合的多激光雷达测深法与校准改进

以初始猜测为输入，我们提出了一个紧密耦合的M-LO来优化滑动窗口内的所有状态。这一过程的灵感来自于最近在多传感器系统、图中实现的束调整、基于图的形成和边缘化的成功。

8.1 构想

滑动窗口中的完整状态向量定义为：

其中，xk是世界帧中主传感器在不同时间戳下的状态，xbli表示从主激光雷达到辅助激光雷达的外参，N+1是窗口中的状态数。为了建立数据关联来约束这些状态，我们构建了局部映射。

图中示。4可视化基于图形的形式。我们使用p来索引窗口的枢轴状态，并将xp设置为局部映射的原点。通过从枢轴框架到其他帧的相对转换，映射是通过与第一个N个帧的特征连接来构造的也就是Flik，k∈[1，N]。第一激光雷达的局部特征图由局部边缘图和局部平面图组成，用Mli表示。我们将X分为三组：Xf、Xv和Xe。Xf=[x1、···、xp]被视为固定、准确的状态。Xv=[xp+1、···、xN+1]被认为是在优化过程中递归更新的变量。Xe=[xbl2，···，xblI]是外参。它们的设置取决于在线校准的收敛性。我们将滑动窗口内所有剩余误差的和最小化，以获得一个地图估计为

其中，rpri(X)是第VII-E节中定义的状态边缘化的前一项，而FM(X)是基于映射的残余误差的和。其雅各比亚式文字（18）见附录A。与第六-A节中的帧对帧估计不同，所提出的滑动窗口估计器共同优化了窗口中的所有状态。由于局部映射提供了密集和可靠的对应关系，这种方法产生了更可靠的结果。如果传感器被精确地校准了，还使用了来自其他激光雷达的约束。根据校准的收敛性，我们将这个问题分为两个子任务：在线校准（变量Xe）和纯里程法（固定Xe）。在每项任务中，fM(X)的定义都是不同的，我们在第VII-B节和第VII-C节中介绍了细节。

图中示4。具有在线校准（左）和纯里程计（右）的滑动窗口估计器（p=3，N=6）的图形模型的插图。MbF(MliF)是基地(i)激光雷达的本地地图。它由基础(i)激光雷达从窗口中的第一个N个帧捕获的转换和合并的特征点组成。请注意，附加函数在在线校准中是优化的，而它们在纯里程计中是固定的。

8.2 通过在线校准进行优化

我们利用基于地图的测量量来细化粗初始化结果。在这里，我们将校准视为一个配准问题。fM(X)分为两个功能，关于 。对于Xv中的状态，这些约束是由最新帧中主传感器的特征之间的对应关系构造起来的，即Fbk，k∈[p+1，N+1]和那些主要的局部地图，即 。对于Xe中的状态，这些约束是由第p帧处的辅助激光雷达特征之间的对应关系建立起来的。翻转和地图Mb。

使用[16]中的方法可以找到Fbk和Mb之间的对应关系。KD-Tree用于建立地图中的快速索引。

1)对于每个边点，我们在特定区域内的局部边图中找到一组其最近点的集合。这个集合用S表示，然后计算出它的协方差。与最大特征值相关联的特征向量意味着相应的边线的方向。通过计算S的中心点，也确定了这条线的位置。

2)对于每个平面点，通过求解ws+d=0、∀s∈s等线性系统，得到了局部平面图中相应平面的系数。同样地，我们找到了Flip和Mb之间的对应关系。最后，我们将目标定义为在线校准的所有测量残差的总和

其中，x−1pxk定义了从枢轴帧到第k帧的相对转换。

8.3 采用纯测度法进行优化

一旦我们通过满足收敛准则(第七-D节)完成在线校准，然后用给定精确推断的纯测度法进行优化。在这种情况下，我们不优化外测，并利用所有可用的基于地图的测量来改进单激光雷达测深。我们将所有激光雷达和局部映射的特征之间的约束纳入到成本函数中

其中，x−1pxkxbli是从枢轴帧中的主激光雷达到第k帧中的辅助激光雷达的转换。

8.4 监测校准的一致性

在以无监督的方式进行在线校准时，需要决定校准是否收敛。收敛之后，我们修复了外数。这对我们的系统是有益的，因为里程计和映射都给出了更多的几何约束，从辅助激光雷达，以获得更精确的姿态。如[34]导出的，信息矩阵的最小特征值退化因子λ揭示了基于优化的状态估计问题的条件。受这项工作的激励，我们使用λ来指示我们的问题是否包含足够的约束来精确的推断。在算法1中总结了更新提取物和监测收敛性的详细管道。该算法以（19）中定义的函数fM（·）以及当前外部输出作为输入，并返回优化的外部输出。在第4行中，我们从代价函数的信息矩阵中计算λ。在第5-7行中，如果λ大于阈值，则会更新外参。在第8行中，我们使用候选外参的数量来检查收敛性。在第9-10行上，满足收敛准则，从而触发终止。然后我们计算L的采样平均值作为得到的外参，采样协方差作为校准协方差。

8.5 边缘化

我们应用边缘化技术来删除滑动窗口中最古老的变量状态。边缘化是一个将历史约束作为先验纳入目标的过程，这是保持测深法和校准结果一致性的必要步骤。在我们的系统中，xp是每次优化后唯一被边缘化的状态。通过应用舒尔补充，我们得到了线性信息矩阵Λ∗rr和剩余gr∗关于其余的状态。先前的残差表示为krprik2=gr∗>Λ∗−1rrgr∗。附录B提供了一些数学基础。

9 具有不确定性感知性的多激光雷达映射

我们首先回顾了典型的激光雷达激光器系统的映射模块的管道。采取之前作为输入，该算法构造一个全局映射，并在足够的约束下细化姿态。这是通过最小化所有基于地图的残差误差的和来实现的

其中Flik是感知第k时刻点云时的输入特征，GwkF是全局映射，xwbkxbli表示第i激光雷达的状态。我们使用第七-B节中的方法来找到Flik和GFwk之间的对应关系。在求解（21）之后，得到的姿势用于将当前特征转换为世界框架，并将其添加到地图中。为了降低计算和内存复杂度，还使用体素网格过滤器缩小地图尺寸[71]。然而，优化的精度取决于地图的质量。图中示。5可视化由不确定姿态变换的噪声地图点（地标）的发生。我们认为，不确定性的三个来源会使地图点产生噪声：传感器噪声、退化姿态估计和外在扰动。

在下一节中，我们传播激光雷达点的不确定性和在地图点上提出（表示为变换的矩阵）。因此，每个映射点都被建模为一个独立同分布的高斯变量。然后，我们提出了一种不确定性感知的方法来提高多激光雷达映射算法的鲁棒性和精度。

图中示。5.映射过程和噪声映射点的发生。黑色曲线表示历史姿势。蓝色曲线表示主激光雷达的当前位置。紫色曲线显示从主激光雷达到辅助激光雷达的外参。使用姿态和外参，将输入特征（红点和绿点）转换并添加到全局地图（黑点）。嘈杂的姿势使新的地图点不确定。

9.1 不确定性的传播

继续第三节-C节的初步工作，我们现在计算Ξ。通过解决NLS问题，对映射姿态（21）进行了优化。我们直接计算信息矩阵的逆比。Ξxwbk=Λ−1，作为协方差。外在协方差的设置取决于一个特定的情况。我们通常将外在协方差定义为

其中ξlbi是外部分子的扰动变量，Ξcalib是根据算法1计算的校准协方差，α是一个尺度参数，允许我们增加协方差的大小。如果一个多激光雷达系统最近已经被校准，我们设置了α=1，而如果该系统已经被使用了很长一段时间而没有重新校准，激光雷达上应该有小的外部偏差，α设置为更大。它与时间和外部影响以及温度漂移有隐式的关系。给定主传感器的平均姿态、平均外置态及其协方差，然后我们计算其他激光雷达的平均姿态和协方差即{Twlik，Ξwlik}。这是一个关于复合两个有噪声的姿态的问题。我们按照四阶近似来计算它们。然后，我们需要通过一个有噪声的变换来传递一个点的高斯不确定性，以产生一个新的地标y∈GFwk+1，其均值和协方差为{¯y，Σ}。通过将一个点转化为世界框架，我们就有了

其中我们保持指数映射的一阶近似。如果我们乘以方程，只保留第一阶项，我们就有了

这里：

操作 将4×1列转换为4×6矩阵：

所有的扰动变量都体现在R9中的θ中。协方差Θ=diag(Ξwlik，Z)表示传感器读数、估计姿态和外部图像的组合不确定性。由H线性变换，y为高斯变换，平均¯y和协方差Σ为

我们遵循[72]来使用跟踪，即tr（Σ），来量化协方差的大小。

9.2 不确定性感知操作

原来的映射算法与三个步骤集成，以提高其性能和鲁棒性。在最后一节中，我们证明了所有映射点的协方差都是通过考虑三个误差源来传播的。首先，问题（21）与（27）中的传播协方差集成。该操作使代价函数考虑到姿态的不确定性和外部扰动。因此，停留在原点附近的点应该有很高的重量。此外，鉴于外参协方差，主激光雷达往往比辅助激光雷达更值得信赖。

在优化的最后一次迭代中，Ξxwbk等于新信息矩阵的逆。其次，重新传播变换后每个点的不确定性。如果tr（Σ）大于一个阈值，我们将滤掉异常值。最后，我们修改了原始的体素网格过滤器，以概率的方式缩小全局映射。修改后的滤波器根据每个立方体的协方差进行采样点。让 把立方集中的第i点，M是立方集中的点数。一个立方体的采样均值和协方差为

其中，w是阈值，和 是一个标准化的权重。

10 实验

我们在三个平台上进行了模拟和真实的实验，以测试m-loam的性能。首先，我们在所有提出的平台上校准多激光雷达系统。将该算法与SOTA方法进行了比较，并引入了两个评价指标。其次，我们演示了M-LOAM在涵盖室内环境和室外城市道路的各种场景下的SLAM性能。此外，为了评估m-loam对外在误差的敏感性，我们在不同外在扰动水平下的手持设备和车辆上进行了测试。最后，我们提供了一个研究，以全面评估M-loam的性能和计算时间与不同的激光雷达组合。

10.1 实施情况的详细信息

我们使用PCL库[71]来处理点云和谷神星求解器[73]来解决非线性最小二乘问题。在没有指定的实验中，我们的算法是在一个具有[email protected]和32GBRAM的桌面上执行的。测试了三个具有不同多激光雷达系统的平台：一个模拟机器人、一个手持设备和一辆车辆。

实际平台上的激光雷达与以ns级精度触发的外部GPS时钟同步。

1/模拟机器人(SR)建立在露台上。两个16束激光雷达安装在一个移动机器人上进行测试。我们建造了一个封闭的模拟矩形房间。我们使用来自[75]的方法来设置激光雷达配置，以最大限度地扩大传感覆盖范围。我们以0.5米/秒的平均速度移动机器人。提供了地面真实的细节和姿势。

2/真实手持设备(RHD)用于手持测试，如图所示。6.它的配置类似于SR。除了两个VLP-16s4之外，我们还安装了一台用于数据收集的迷你计算机(IntelNUC)和一个用于记录测试场景的相机(mvBlueFOX-MLC200w)。我们使用这个设备在校园里收集数据，平均速度为2米/秒。

3/真实车辆(RV)是自主物流交通的车辆。我们在这个平台上进行了实验，以证明我们的系统在大规模的、具有挑战性的户外环境中也表现良好。如图所示。7、4个RS-激光雷达-16s5分别刚性安装在顶部、前、左和右位置。我们开车以3米/秒的平均速度穿过城市道路。地面真实姿态是由一个耦合的激光雷达-GPS编码器定位系统中得到的。

表二显示出了经验设置的参数。σR、λcalib和Lcalib是校准过程中的收敛阈值。设置最后两个参数需要一个初步的培训过程，详见补充材料[77]。p和N分别是测程中的局部图和滑动窗口的大小。w是映射中不确定点的阈值，α是外在协方差的尺度。对于在第九-D节中注入大扰动的情况，我们设置了α=10。否则，则为α=1。

图中示。6.(a)室内测试的真正手持设备。左右两侧分别安装了两个VLP-16s。附加的摄像机用于记录测试场景。(b)校准点云由左（红色）和右（粉红色）激光雷达的点组成。

图中示。7.(a)大规模户外测试的真正车辆。四个RS-16分别安装在上、前、左和右位置。(b)校准的点云由来自顶部（红色）、前（绿色）、左（蓝色）和右（粉红色）激光雷达的点组成。

10.2 校准过程中的性能

1)评估指标：我们引入了两个指标来评估来自不同方面的激光雷达校准结果：

/与地面真值比较误差(EGT)根据旋转和平移计算地面真值与估计值之间的距离：

/提出了平均映射熵(MME)来测量点云的紧凑度。它已经被探索为一个标准的指标来评估注册的质量，如果地面上的真相是不可用的。给定一个校准的点云，归一化平均映射熵为：

其中m是点云的大小，Cpi是pi周围局部半径r内的采样协方差。对于每个校准情况，我们选择10个包含许多平面的连续点云帧，并计算它们的平均MME值进行评估。

由于完美的地面真实在真实应用中是未知的，我们使用“PS-Calib”[12]作为“相对地面真实”来计算EGT。PS-校准是一种被充分理解的、基于目标的校准方法，它应该与我们的方法具有相似或优越的精度[80]。另一个度量标准是MME，它以一种无监督的方式计算分数。它可以解释为点云紧性的信息理论度量。

2)校准结果：所有平台的多激光雷达系统都通过我们的方法进行校准。为了初始化外部元素，我们用足够的旋转和翻译手动移动这些平台。表三报告了由此产生的外部案例，其中测试了两个模拟案例（相同的外部案例、不同的运动）和两个真实案例。由于空间有限，我们仅演示车辆上顶部激光雷达和前激光雷达之间的校准。我们的方法由“提议(Ini.+Ref.”表示）”，并与离线多激光雷达校准方法[4]（“自动校准”）进行比较。虽然自动校准遵循类似的初始化细化过程来获得外的，但它在几个方面与我们的算法不同。例如，自动校准仅在细化中使用平面特征。而且它假设激光雷达的视图应该有很大的重叠区域。基于手眼的首字母。）”）、未校准（“无校准”）、CAD（“CAD模型”）和地面真相（“GT”）供参考。

我们的基于手眼的方法成功地初始化了所有情况下的旋转偏移（<9度），但无法恢复SR和RV上的平移偏移（>0.3m）。模拟机器人和车辆都必须进行长距离的平面运动才能初始化，由于运动估计的漂移，x−和−平移恢复较差。平面运动也导致z−平移不可观测到。但我们可以在6自由度内移动RHD，并快速收集丰富的约束条件。因此，其初始化结果很好。关于在线细化，我们的算法优于自动校准，并在EGT(<3度和<0.07m)和MME指标方面展示了与PS-Calib的类似性能。基于这些结果，我们得出结论，初始化阶段可以提供粗的旋转估计，并需要改进精确的提取。

我们明确地展示了RHD上的测试。在图中。8、我们在标定点云的不同帧上绘制所有MME值，其中结果与表III一致。无论r被设置为0.3m或0.4m，我们的方法总是有一个比自动标定更好的分数。图中示。9说明了校准过程，传感器套件的轨迹如图所示。10.该过程分为三个顺序阶段：第1阶段（外部初始化，第VI-B节）、第二阶段（外部细化测深，第VII-B节）和第3阶段（纯测深和绘图，第VII-C节）。

阶段1开始在不了解旋转偏移的情况下恢复旋转偏移。当QK的第二个小奇异值大于一个阈值时，它就会退出。然后计算出平移分量。第二阶段执行非线性优化以共同细化外功能。如果没有足够的环境限制，这个过程可能会持续很长时间。然而，我们基于滑动窗口的边缘化方案可以确保一个有界复杂的程序不断更新附加功能。用简并因子（第VII-D节）和候选的数量来监测收敛条件。收敛后，我们关闭校准，进入第IX-C节中评估的阶段3。

我们还评估了我们的细化方法对不同水平的初始猜测的敏感度：CAD模型以及粗糙的旋转和平移初始化。定量的结果可在补充材料中找到。

10.3 SLAM的性能

我们评估了由SR、RHD和RV平台收集的模拟序列和真实序列上的M-LOAM。多激光雷达系统采用我们的在线方法进行校准（第七-B节）。详细的提取内容见表三的第一行、第三行、和第四行。

12 结论及未来的工作

在本文中，我们提出了一个完整的多激光雷达外部校准和鲁棒解决方案。这种方法包含了几个理想的特征，包括对噪声去除、运动和外部初始化的快速分割、具有收敛识别的在线外部校准、紧密耦合的M-LO和具有不确定性感知的多激光雷达映射。我们进行了广泛的实验，涵盖从室内办公室到室外城市道路的评估。我们的方法针对不同平台的多激光雷达系统。它产生的平移精度厘米和旋转十度，可与基于SOTA目标的方法相媲美。对于SLAM，该系统在中尺度（>150m）场景中通常达到低于40cm，在大型城市道路中达到几米的定位精度（>3.2km）。为了社区的利益，我们将我们的实现成为开源。

未来的研究有几个方向。在我们的系统中添加一个环闭合模块是可取的，这有助于纠正累积的漂移并保持全局映射[84]。另一个研究方向涉及以对象为中心的SLAM。最近，这个社区面临的两个挑战正在增加。一方面，广泛使用的低级几何特征对视点变化不具有代表性和敏感。另一方面，基于激光雷达的目标检测器中的数据稀疏性和阻塞是主要的瓶颈。一个可能的解决方案是开发一种SLAM方法，它可以使用对象级特性来优化自我运动和动态对象的运动。有人提出对相机或视觉惯性系统进行试验，而激光雷达的工作很少。最后，将我们的校准和不确定性建模方法扩展到各种模式的传感器，如IMUs[5]、雷达[9]和事件摄像机[88]，是很有前途的。例如，我们可以传播IMU噪声模型来预测姿态的不确定性，或者所提出的收敛准则可以用于多模态传感器的外在校准。

附录：

A. 残差的雅可比矩阵

状态向量被定义为x=[t，q]。我们利用罗德里格斯公式[59]将q转换为旋转矩阵R：

1)rH的雅各比亚式书：（8）中的剩余部分被重写为

利用左扰动：Rexp（δφ∧）≈R(I+δφ∧)，旋转和平移的雅可比亚式计算为

四元数在哪里更新是根据

在线校准的fM中的残差的2)雅各比式：（19）中的目标函数有两个项：fM(Xv)和fM(Xe)。对于第一项，雅各比亚人是由

其中，k∈[p+1，N+1]。由于第二项与（8)的形式相同，雅各比亚人是由(33）作为

3)纯光学测量中的残差的雅各比亚：（20）中残差的雅各比亚计算为

B. 边缘化

滑动窗口估计器需要映射几种状态，优化后添加新状态。对于整个状态向量X，我们表示Xm为边缘状态集，Xr表示为剩余状态集。通过在初始点对代价函数（18）进行线性化和扩展，得到了正态方程：ΛδX=−g，其中Λ=PJ>Σ−1J是信息矩阵，g=J>Σ−1r。通过用块矩阵来表示这个方程，我们就有了

由此产生的Λ∗rr和gr∗编码了边缘化状态的依赖性。将线性残差进行下一个优化，我们可以保持Xr的一致性。由于谷神星求解器[73]使用雅各比矩阵来更新变量，在实现边缘化时，我们使用雅各比矩阵重写信息矩阵。在得到Λ∗rr后，我们用特征值和特征向量分解它：

在下一个优化中，先前的残余 等于 ，其中∆Xr是当前状态和初始状态之间的“距离”。