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

摘要

组合多个LIDARS使机器人能够最大化其对环境的感知意识，并获得足够的测量。本文提出了一种实现鲁棒和同步的外参标定，里程计和多个LIDAR的建图系统。

我们的方法从测量预处理开始，从原始测量中提取边缘和平面特征，在运动和外参初始化过程之后，基于滑动窗口的多激光雷达里程计将板载运行，以估计具有在线标定和收敛性检测的位姿。

我们进一步开发了一种建图算法来构造全局地图，并优化具有足够特征的Pose以及捕获和减少数据不确定性的方法。

介绍

如果没有手动干预，我们的系统可以从多个未知外参的激光雷达开始，自动校准其外参，并提供准确的姿势以及全局一致的地图。先前的工作（LIOM），启发了本文，我们尝试解决多激光雷达融合问题的地方。此外，我们介绍了一种基于运动的方法[4]以初始化外参的方法，并使用[19]中的工具来表示不确定性。本文贡献如下：

自动初始化，计算所有关键状态，包括连续帧之间的运动以及后续阶段的外参。它可以在任意位置开始，而无需任何先前的机械配置或校准对象知识(Section VI)
使用常规收敛准则的在线自标定与里程计同时执行，它具有以完全无监督的方式监控收敛性和触发终止的能力(Section VII-B)
基于滑动窗口的里程计，充分利用来自多个 LiDAR 的信息，该实现可以解释为小规模的帧到地图注册表，这进一步减少了连续帧到帧的帧累积的漂移(Section VII-C)
使用二阶段方法进行建图，捕获传感器噪声并传播不确定性，来消除姿势估计和外参估计的外部扰动，这种方法使建图过程能够了解不确定性，并有助于我们保持全局地图的一致性以及提高系统的鲁棒性以进行长期导航任务。

据我们所知，M-LOAM是第一个对多激光雷达外参标定和SLAM的完整解决方案，该系统在手持设备和自动驾驶汽车上进行了广泛的实验评估，从室内办事处到户外城市道路的各种情景，优于基于Sota Lidar的方法。关于各种平台上的校准，我们的方法实现了外参上的平移的厘米级精度和旋转的分数。对于不同尺度的SLAM，已成功应用M-LOAM以提供准确的姿势和地图结果。图1可视化每个阶段的M-Loam的输出。

问题描述

我们根据最大似然估计（Maximum Like- lihood Estimation， MLE）制定M-LOAM，MLE导致非线性优化问题，其中，高斯协方差的逆对残差函数进行加权。

在深入研究 M-LOAM 的细节之前，我们先介绍一些基本概念：

第 III-A 节介绍了符号
第 III-B 节介绍了 MLE
第 III-C 节描述了合适的模型来表示 $\mathbb{R}^{3}$中的不确定测量和 SE(3) 中的变换。
最后，第 III-D 节简要介绍了 MLE 在 M-LOAM 中具有近似高斯噪声的实现

符号约定

命名法如表I所示

我们考虑一个由一个主要激光雷达和多个辅助灵敏器组成的系统，主激光雷达作为base_frame，我们使用()$^{l^{1}} /()^{b}$来表示，对于其他辅助雷达，使用()$^{l^{i, i>1}}$来表示。

我们记$\mathcal{F}$为从原始激光雷达提取的有效特征，每个特征都表示为3D空间中的一个点：$\mathbf{p}=[x, y, z]^{\top}$。

状态向量，由平移和旋转部件组成，记为$\mathbf{x}=[\mathbf{t}, \mathbf{q}]$，其中$\mathbf{t}$是3x1向量，$\mathbf{q}$是汉密尔顿四元数，但在我们需要旋转向量的情况下，我们在SO(3)中使用3×3旋转矩阵$\mathbf{R}$.

第VIII节将不确定性与矢量空间上的构成相关联，我们使用SE（3）中的4×4变换矩阵T表示Pose:

\[ \mathbf{T}=\left[\begin{array}{cc} \mathbf{R} & \mathbf{p} \\ \mathbf{0}^{\top} & 1 \end{array}\right] \]

最大似然估计

我们为MLE问题制定了多激光雷达系统的姿势和外参估计[60] 式（1）：

\[ \hat{\mathbf{x}}_{k}=\underset{\mathbf{x}_{k}}{\arg \max } p\left(\mathcal{F}_{k} \mid \mathbf{x}_{k}\right)=\underset{\mathbf{x}_{k}}{\arg \min } f\left(\mathbf{x}_{\mathbf{k}}, \mathcal{F}_{k}\right) \]

其中，

$\mathcal{F}_{k}$表示第k帧的有效特征
$\mathbf{x}_{k}$表示待优化的状态
$f(\cdot)$表示目标函数

假设观测模型使用高斯噪声[3]来替换，那么式（1）变成非线性最小二乘(NLS)问题:

\[ \hat{\mathbf{x}}_{k}=\underset{\mathbf{x}_{k}}{\arg \min } \sum_{i=1}^{m} \rho\left(\left\|\mathbf{r}\left(\mathbf{x}_{k}, \mathbf{p}_{k i}\right)\right\|_{\mathbf{\Sigma}_{i}}^{2}\right) \]

其中，

$\rho(\cdot)$表示鲁棒性Huber损失[61]，用于处理outlier
$\mathbf{r}(\cdot)$表示残差函数
$\Sigma_i$表示协方差矩阵

迭代方法如高斯牛顿、LM等方法常用于解决NLS问题，这些方法通过计算目标函数相对于状态向量$\mathbf{x}_k$的Jacobian来进行局部线性化，即$\mathbf{J}=\partial f / \partial \mathbf{x}_{k}$。通过给定初始值，$\mathbf{x}_k$通过使用$\mathbf{J}$进行迭代优化，直到收敛到局部最优。

在最终的迭代中，状态的最小二乘协方差计算为$\boldsymbol{\Xi}=\boldsymbol{\Lambda}^{-1}$[62]，其中，$\boldsymbol{\Lambda}=\mathbf{J}^{\top} \mathbf{J}$称为信息矩阵。

不确定性表示

我们使用[19]中的工具来表示数据不确定性，首先，考虑噪声的激光点如下，式（3）：

\[ \mathbf{p}=\overline{\mathbf{p}}+\boldsymbol{\zeta}, \quad \boldsymbol{\zeta} \sim \mathcal{N}(\mathbf{0}, \mathbf{Z}) \]

其中,

$\bar{\mathbf{p}}$表示不含噪声的点
$\zeta \in \mathbb{R}^{3}$是零均值的高斯扰动变量，$\mathbf{Z}$是激光测量的噪声协方差
为了使得式（3）与转换矩阵(i.e., $\left.\mathbf{p}_{h}^{\prime}=\mathbf{T} \mathbf{p}_{h}\right)$更加紧凑，我们使用齐次坐标系来表示：

\[ \mathbf{p}_{h}=\left[\begin{array}{l} \overline{\mathbf{p}} \\ 1 \end{array}\right]+\mathbf{D} \boldsymbol{\zeta}=\overline{\mathbf{p}}_{h}+\mathbf{D} \boldsymbol{\zeta}, \quad \boldsymbol{\zeta} \sim \mathcal{N}(\mathbf{0}, \mathbf{Z}) \]

其中，

$\mathbf{D}$是将3x1向量转换为齐次坐标的矩阵

如[63]中的研究，LIDARS深度测量误差（也称为传感器噪声）主要受目标距离的影响，矩阵$\mathbf{Z}$被简单的设置为常值矩阵。

然后，我们定义SE(3)中受小扰动的随机变量：

\[ \mathbf{T}=\exp \left(\boldsymbol{\xi}^{\wedge}\right) \overline{\mathbf{T}}, \quad \boldsymbol{\xi} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Xi}) \]

其中，

$\overline{\mathbf{T}}$是不含噪声的变换矩阵
$\xi \in \mathbb{R}^{6}$是协方差为$\Xi$的小扰动变量

这种表示允许我们在状态空间中储存变换的均值作为$\overline{\mathbf{T}}$，并使用$\xi$作为扰动。

我们考虑$\Xi$包含两个部分的来源：

Degenerate Pose Estimatio，来自案例，例如在受限制的环境中缺乏几何结构[34]，它通常在其退化方向上不确定姿势[62]，[64]，现有工程诉诸基于模型和基于学习的[65]方法来估算ICP的背景下的姿势协方差。
Extrinsic Perturbation，该项始终存在，由于外参误差的存在[12]。这种扰动不利于多传感器系统的测量精度 [66]、[67] 但很难测量。

特别的，$\Xi$的详细计算在Section VIII

M-LOAM中的MLE问题

我们扩展了 MLE 以设计多个 M-LOAM 估计器，以从粗到细的方式解决机器人姿态和外参。最重要的步骤是将高斯噪声协方差$\Sigma$近似于现实的测量模型。

根据第 III-C 节中的（上述）讨论，我们确定了三个可能使地标不确定的误差源：传感器噪声、退化姿态估计和外参扰动。

帧到帧运动估计（部分VI-A）归结于传感器噪声，紧耦合的里程计（第VII-C部分）建立了局部地图用于位姿优化，因此我们应该传播位姿的不确定性到每一个地图点。尽管如此，如果涉及更多的雷达和滑动窗口，这种操作通常是耗时的（大约10ms-20ms）。为了保证里程计的实时性，我们不在此处计算的位姿不确定性。

因此，我们简单地设置$\Sigma=\mathbf{Z}$作为残差的协方差。在建图部分，我们有足够的时间来获得准确的姿势和全局地图，因此，我们考虑所有不确定性来源。第VIII部分解释了姿势不确定性如何影响建图精度和$\Sigma$的传播。

系统概览

我们制作三个假设来简化系统设计

Lidars是同步的，这意味着不同雷达之间的时间延迟几乎为零
该平台在校准初始化期间经历了足够的旋转和平移运动
主激光雷达的局部地图应与辅助LIDAR共享重叠的FOV，用于在改进中匹配以缩短校准阶段，这可以通过移动机器人来实现

图 2 展示了 M-LOAM 的流水线。系统从测量预处理（第 V 部分）开始，即从去噪点云中提取和跟踪边缘和平面特征

初始化模块（第VI部分）提供了所有必要的值，包括姿势和外参，用于启动后续的非线性优的M-LO。M-LO融合多激光雷达测量在滑动窗口内优化里程计和外参，如果外参已经收敛，我们跳过外参初始化以及细化步骤，然后进入纯里程计和建图阶段。

概率建图模块（第VIII节）构造了一种具有足够特征的全局地图，以消除里程计累计漂移。里程计和建图模块分别运行在隔离的线程上。

测量预处理

我们实施三个顺序步骤来处理LiDARS'raw测量，我们首先将点云分割成许多簇以去除嘈杂的对象，然后提取边缘和平面特征。为了将连续帧之间的特征关联起来，我们匹配了一系列的对应关系。 在本节中，每个 LiDAR 都是独立处理的。

噪声去除分割

通过了解LIDAR的垂直扫描角度，我们可以将原始点云投影到没有数据丢失的范围图像上,在图像中，每个有效点由像素表示。像素值记录了该点到原点的欧几里德距离。我们将[68]中提出的分割方法应用于将像素分组到多个集群中。

我们假设，如果两个相邻点的连接线大致垂直于（大于60度）激光束，则认为这两个相邻点属于相同的对象。我们采用广度第一搜索算法来遍历所有像素，确保恒定的时间复杂度，特别的，我们丢弃小集群，因为它们可能会在优化中提供不可靠的约束。

特征提取和匹配

我们有兴趣提取一般边缘和平面特征，我们遵循[16]选择根据其曲率从测量中选择一组特征点。该组提取的特征$\mathcal{F}$由两个子集组成：边缘子集（高曲率）$\mathcal{E}$和平面子集（低曲率）$\mathcal{H}$。我们进一步从$\mathcal{E}$收集曲率最高边缘点，从平面$\mathcal{H}$收集曲率最低的平面点，得到另外两组点$\hat{\mathcal{E}},\hat{\mathcal{H}}$。

下一步是确定两个连续帧之间的特征对应关系，()$^{l_{k-1}^{i}} \rightarrow()^{l_{k}^{i}}$，以构造几何约束：

对于边缘点集合$\hat{\mathcal{E}}^{l_{k}^{i}}$中的点，将从前一帧的边缘点集合$\mathcal{E}^{l_{k-1}^{i}}$中查找两个最近邻的边缘点以形成边缘线关联。
对于平面点集合$\hat{\mathcal{H}}^{l_{k}^{i}}$中的点，则从上一帧的边缘点集合$\mathcal{H}^{l_{k-1}^{i}}$中查找3个最近邻点作为平面关联。

初始化

优化多个LIDARS的状态是高度非线性的，需要给出初始估计值。本节介绍了我们的运动和外在初始化方法，不需要任何先前的传感器套件的机械配置。它还不涉及任何手动努力，使其对自主机器人特别有用。

Scan-Based Motion Estimation

在每个LIDAR的两个连续帧之间找到了相应的对应关系，我们通过最小化所有功能的残差误差来估计帧到帧变换。如图3所示，残差由边缘和平面对应关系制定，设$\mathbf{x}_k$为第k帧的相对变换，对于平面特征，对于平面点$\mathbf{p} \in \hat{\mathcal{H}}^{l_{k}^{i}}$，如果$\Pi$是关联的平面，那么该平面点对应的残差如下计算，式（6）：

\[ \mathbf{r}_{\mathcal{H}}\left(\mathbf{x}_{k}, \mathbf{p}, \Pi\right)=a \mathbf{w}, \quad a=\mathbf{w}^{\top}\left(\mathbf{R}_{k} \mathbf{p}+\mathbf{t}_{k}\right)+d \]

其中，

$a$是点到平面的距离
$[\mathbf{w}, d]$是平面的参数

对于边缘线特征点$\mathbf{p} \in \hat{\mathcal{E}}^{l_{k}^{i}}$，如果$L$是与之关联的边缘线，那么我们使用如式（6）的两个平面特征的组合来表示点与边缘线的残差：

\[ \mathbf{r}_{\mathcal{E}}\left(\mathbf{x}_{k}, \mathbf{p}, L\right)=\left[\mathbf{r}_{\mathcal{H}}\left(\mathbf{x}_{k}, \mathbf{p}, \Pi_{1}\right), \mathbf{r}_{\mathcal{H}}\left(\mathbf{x}_{k}, \mathbf{p}, \Pi_{2}\right)\right] \]

其中，

$\left[\mathbf{w}_{1}, d_{1}\right]$,$\left[\mathbf{w}_{2}, d_{2}\right]$分别是平面$\Pi_{1}$、$\Pi_{2}$的系数
$\mathbf{w}_{1}$恰好与点$\mathbf{p}$到直线$L$的投影方向一致
并且平面$\Pi_{2}$垂直于$\Pi_{1}$，满足s.t. $\mathbf{w}_{2} \perp \mathbf{w}_{1}$, and $\mathbf{w}_{2} \perp L$
上述定义与LOAM[16]有所不同，其中有两个好处，一是边缘残差为状态量提供了额外的约束，其次，残差可以使用向量表示，这允许我们乘以3x3的协方差矩阵

我们最小化所有残差项的总和以获得MLE：

\[ \hat{\mathbf{x}}_{k}=\underset{\mathbf{x}_{k}}{\arg \min } \sum_{\mathbf{p} \in \hat{\mathcal{F}}^{i \atop k}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{k}, \mathbf{p}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{p}}}^{2}\right) \]

\[ \mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{k}, \mathbf{p}\right)=\left\{\begin{array}{ll} \mathbf{r}_{\mathcal{E}}\left(\mathbf{x}_{k}, \mathbf{p}, L\right) & \text { if } \mathbf{p} \in \hat{\mathcal{E}}^{i_{k}^{i}} \\ \mathbf{r}_{\mathcal{H}}\left(\mathbf{x}_{k}, \mathbf{p}, \Pi\right) & \text { if } \mathbf{p} \in \hat{\mathcal{H}}^{i_{k}^{i}} \end{array}\right. \]

特别的，关于$\mathbf{r}_{\mathcal{F}}(\cdot)$的雅克比在Appendix A进行详细讨论。

在实践中，在带滚动快门扫描的LiDars运动后，点存在一些倾斜畸变，在求出增量运动$\mathbf{x}_k$后，我们将这些点转换到前一帧扫描的最后时刻（即第k帧扫描的起始）以矫正畸变。

令$t_{k-1},t_{k}$表示激光扫描的起始和结束，对于每一个扫描点$\mathbf{p}$，都可以进行转换如下：

\[ \mathbf{p}^{l_{k-1}^{i}}=\mathbf{R}_{k}^{\tau} \mathbf{p}+\mathbf{t}_{k}^{\tau}, \quad \tau=\frac{t-t_{k-1}}{t_{k}-t_{k-1}} \]

其中，旋转和平移部分使用se(3)进行插值？参考文献[5]

\[ \mathbf{R}_{k}^{\tau}=\exp \left(\boldsymbol{\phi}_{k}^{\wedge}\right)^{\tau}=\exp \left(\tau \boldsymbol{\phi}_{k}^{\wedge}\right), \quad \mathbf{t}_{k}^{\tau}=\tau \mathbf{t}_{k} \]

Calibration of Multi-LiDAR System

通过对准两个传感器的运动序列来获得初始外参，即这被称为手眼标定问题$\mathbf{A X}=\mathbf{X B}$，其中$\mathbf{A},\mathbf{B}$是两个传感器的位姿增量，$\mathbf{B}$是他们的外参。

当机器人移动时，第i个激光雷达的第k帧时刻，有：

式（11）：

\[ \mathbf{R}_{l_{k}^{i}}^{l_{k-1}^{i}} \mathbf{R}_{l^{i}}^{b}=\mathbf{R}_{l^{i}}^{b} \mathbf{R}_{b_{k}}^{b_{k-1}} \]

式（12）：

\[ \left(\mathbf{R}_{l_{k}^{i}}^{l_{k-1}^{i}}-\mathbf{I}_{3}\right) \mathbf{t}_{l^{i}}^{b}=\mathbf{R}_{l^{i}}^{b} \mathbf{t}_{b_{k}}^{b_{k-1}}-\mathbf{t}_{l_{k}^{i}}^{l_{k-1}^{i}} \]

上面两式实际上是根据[14]，将情况下原始问题$\mathbf{A X}=\mathbf{X B}$分解为旋转和平移部分。我们实现了此方法可在线初始化外参。

旋转初始化

通过使用四元数，可以将式（11）重写成如下式（13）：

\[ \begin{aligned} & \mathbf{q}_{l_{k}^{i}}^{l_{k-1}} \otimes \mathbf{q}_{l^{i}}^{b}=\mathbf{q}_{l^{i}}^{b} \otimes \mathbf{q}_{b_{k}}^{b_{k-1}} \\ \Rightarrow &\left[\mathbf{Q}_{1}\left(\mathbf{q}_{l_{k-1}^{i}}^{l_{k}}\right)-\mathbf{Q}_{2}\left(\mathbf{q}_{b_{k}}^{b_{k-1}}\right)\right] \mathbf{q}_{l^{i}}^{b}=\mathbf{Q}_{k}^{k-1} \mathbf{q}_{l^{i}}^{b} = 0 \end{aligned} \]

其中，$\otimes$表示四元数乘法，$\mathbf{Q}_{1}(\cdot)，\mathbf{Q}_{1}(\cdot)$表示四元数的左乘、右乘等价矩阵[59]。

通过多次时间间隔，可以将式（13）就行堆叠，形成超定线性方程组：

式（14）：

\[ \left[\begin{array}{c} w_{1}^{0} \cdot \mathbf{Q}_{1}^{0} \\ \vdots \\ w_{K}^{K-1} \cdot \mathbf{Q}_{K}^{K-1} \end{array}\right]_{4 K \times 4} \mathbf{q}_{l^{i}}^{b}=\mathbf{Q}_{K} \mathbf{q}_{l^{i}}^{b}=\mathbf{0}_{4 K \times 4} \]

其中，

$K$表示约束的数量
$w_{k}^{k-1}$是鲁棒性权重，定义为残差四元素的角度轴表示中的角度？

\[ w_{k}^{k-1}=\rho^{\prime}(\phi), \quad \phi=2 \arctan \left(\left\|\mathbf{q}_{x y z}\right\|, q_{w}\right) \]

\[ \mathbf{q}=\left(\check{\mathbf{q}}_{l^{i}}^{b}{ }\right)^{*}\otimes\left(\mathbf{q}_{l_{k}^{i}}^{l_{k-1}^{i}}\right)^{*} \otimes \check{\mathbf{q}}_{l^{i}}^{b} \otimes \mathbf{q}_{b_{k}}^{b_{k-1}} \]

其中，

$\rho^{\prime}(\cdot)$是Huber loss的微分
$\check{\mathbf{q}}_{l^{i}}^{b}$是当前估计的旋转外参
$\mathbf{q}^{*}$是$\mathbf{q}$的逆

令$\| \mathbf{q}_{l^{\prime}}^{b}\|=1$，我们可以使用SVD来获取式（14）的close form解。

对于3-DOF旋转的全部可观察性，需要足够的运动激励，在足够的约束下，$Q_k$的零空间的秩应为1，也就是说，我们只有1个零奇异值（4个变量，3个维度，剩下一个为0）。

否则，由于在一个轴或多个轴上的运动退化会导致$Q_k$的零空间>1。因此，我们需要确保第二小的奇异值$\sigma_{min2}$足够大以确保条件满足。我们设置了阈值$\sigma_{\mathbf{R}}$，and terminate the rotation calibration if σmin2> σR. ???

越来越多的数据会使得$Q_k$行数迅速增长，为了维持计算时间，我们使用优先级队列[69]，长度k = 300逐渐存储历史约束，并删除小旋转的约束。

平移初始化

一旦旋转校准完成，我们通过式（12）将所有可用数据结合，构造一个线性系统：

式（16）：

\[ \left[\begin{array}{c} \mathbf{R}_{l_{1}^{i}}^{l_{0}}-\mathbf{I}_{3} \\ \vdots \\ \mathbf{R}_{l_{K}^{i}}^{l_{K-1}^{i}}-\mathbf{I}_{3} \end{array}\right]_{3 K \times 3} \mathbf{\mathbf { t }}_{l^{i}}^{b}=\left[\begin{array}{c} \hat{\mathbf{R}}_{l^{i}}^{b} \mathbf{t}_{b_{1}}^{b_{0}}-\mathbf{t}_{l_{1}^{i}}^{l_{0}^{i}} \\ \vdots \\ \hat{\mathbf{R}}_{l^i}^{b} \mathbf{t}_{b_{K}}^{b_{K-1}}-\mathbf{t}_{l_{K}^{i}}^{l^{i}_{K-1}} \end{array}\right]_{3 K \times 1} \]

其中，

$\mathbf{t}_{l^{i}}^{b}$通过使用最小二乘法获取

然而，如果机器人只在平面上运动，z轴方向上的平移是不可观的，在这种情况下，我们设置$t_z=0$然后重写式（16）以移除$\mathbf{t}_{l^{i}}^{b}$中的Z轴分量。

不像文献[4]，我们的方法不能通过地面来初始化$t_z$，因此需要在后续的refinement阶段来恢复$t_z$.(Section VII-B)

具备传感器外参细化的紧耦合多激光雷达里程计

将最初的猜测作为输入，我们提出了一个紧密耦合的M-LO，以优化滑动窗口内的所有状态，该过程的灵感来自多传感器系统的BA，Graph-Based和marginalization的启发[5],[15],[70].

问题构造

滑动窗口中的全部状态向量如下定义：

$$ \[\begin{aligned} \mathcal{X} &= [\mathcal{X}_{f},\qquad \mathcal{X}_{v}, \qquad \qquad \qquad \mathcal{X}_{e}] \\ &= [\mathbf{x}_{1}, \cdots, \mathbf{x}_{p}, \mathbf{x}_{p+1}, \cdots, \mathbf{x}_{N+1}, \mathbf{x}_{l^{2}}^{b}, \cdots, \mathbf{x}_{l^{l}}^{b}] \end{aligned}\]

\[ \begin{aligned} \mathbf{x}_{k} &=\left[\mathbf{t}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}\right], \quad k \in[1, N+1] \\ \mathbf{x}_{l^{i}}^{b} &=\left[\mathbf{t}_{l^{i}}^{b}, \mathbf{q}_{l^{i}}^{b}\right], \quad i \in[1, I], \end{aligned} \]

其中，

$\mathbf{x}_k$是主激光雷达在世界坐标系下不同时间戳的状态
$\mathbf{x}_{l^{i}}^{b}$表示主激光雷达到第i辅助激光雷达的外参
$N+1$是滑动窗口中的状态数量

为了建立这些状态之间的数据关联，我们建立了局部地图

图4对Graph-base的构造进行了可视化，我们使用$p$来索引滑动窗口中的枢轴状态，并设置$\mathbf{x}_p$作为局部地图的原点（起始）。

利用从枢轴帧到其他帧的相对变换，通过连接前N帧（如$\mathcal{F}^{l_{k}^{i}}, k \in[1, N]$）的特征点来构造局部地图。第i个激光雷达的局部特征地图由局部边缘地图和局部平面地图组成，记为$\mathcal{M}^{l^{i}}$。

我们分割全状态向量$\mathcal{X}$为3个部分：$\mathcal{X}_{f},\mathcal{X}_{v},\mathcal{X}_e$：

$\mathcal{X}_{f}=[\mathbf{x}_1,\dots,\mathbf{x}_p]$是由已经固定的准确状态组成
$\mathcal{X}_{v}=\left[\mathbf{x}_{p+1}, \cdots, \mathbf{x}_{N+1}\right]$被考虑作为在优化过程中迭代更新的变量
$\mathcal{X}_{e}=\left[\mathbf{x}_{l^{2}}^{b}, \cdots, \mathbf{x}_{l^I}^{b}\right]$是外参向量，它们的设置取决于在线校准的收敛性。
我们最小化滑动窗口内所有残差的总和以获得 MAP 估计为：

\[ \hat{\mathcal{X}}=\underset{\mathcal{X}}{\arg \min }\left\{\left\|\mathbf{r}_{p r i}(\mathcal{X})\right\|^{2}+f_{\mathcal{M}}(\mathcal{X})\right\} \]

其中，

$\mathbf{r}_{p r i}(\mathcal{X})$是VII-E节中定义的边缘化状态的先验项
$f_{\mathcal{M}}(\mathcal{X})$是基于地图的残差的总和，其式（18）的雅克比矩阵推导见Appendix A

本文呈现的滑动窗口估计器与帧到帧估计不同，该估计器联合优化了窗口中的所有状态。这种方法，输出更准确的结果，因为局部地图提供了密集和可靠的对应关系。如果传感器精确标定，还使用来自其他雷达的约束。根据标定的收敛性，我们将问题分为两个子任务：

在线标定online calibration (variable $\mathcal{X}_e$)
纯里程计pure odometry (fixed $\mathcal{X}_e$)

在每个任务中，$f_{\mathcal{M}}(\mathcal{X})$的定义是不同的，我们在第 VII-B 和 VII-C 节中介绍了详细信息

考虑在线标定的优化

我们利用基于地图的测量来改进粗略初始化结果，在这里，我们将标定问题视为配准问题，$f_{\mathcal{M}}(\mathcal{X})$分为两个函数，分别相对于$\mathcal{X}_{v}$和$\mathcal{X}_{e}$。

对于$\mathcal{X}_{v}$中的状态，这些约束由主传感器的最新帧的特征如$\mathcal{F}^{b_{k}}, k \in[p+1, N+1]$，与主激光雷达的局部地图$\mathcal{M}^{b}$之间的关联构成。

对于$\mathcal{X}_{e}$中的状态，这些约束由第i辅助激光雷达的第p帧的特征$\mathcal{F}^{l_{p}^{i}}$与局部地图$\mathcal{M}^{b}$的特征关联构成。

$\mathcal{F}^{b_{k}}$与局部特征地图$\mathcal{M}^{b}$的关联使用[16]中的方法进行寻找，其中特征地图$\mathcal{M}^{b}$使用了KD-TREE进行索引。

对于边缘点，我们在局部边缘地图中的指定范围查找与之最近邻的点集合，记为$\mathcal{S}$，然后计算点集的方差。点集的最大特征值对应的特征向量表示了与该边缘点所关联的直线的方向。通过计算点集的均值，即可确定边缘点对应的边缘线。
对于平面点，通过求解线性系统$\mathbf{w s}+d=0, \forall \mathbf{s} \in \mathcal{S}$，可以得到在局部平面点云地图中与之关联的平面的系数，同样的，我们寻找$\mathcal{F}^{l_{p}^{i}}$和$\mathcal{M}^{b}$的关联。

最后，我们将目标定义为在线标定的所有测量残差的总和，式（19）：

\[ \begin{aligned} f_{\mathcal{M}}(\mathcal{X}) &=f_{\mathcal{M}}\left(\mathcal{X}_{v}\right)+f_{\mathcal{M}}\left(\mathcal{X}_{e}\right) \\ &=\sum_{k=p+1}^{N+1} \sum_{\mathbf{p} \in \mathcal{F}^{b_{k}}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{p}^{-1} \mathbf{x}_{k}, \mathbf{p}\right)\right\|_{\Sigma_{\mathrm{p}}}^{2}\right) +\sum_{i=2}^{I} \sum_{\mathbf{p} \in \mathcal{F}^{l_{p}^{i}}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{l^{i}}^{b}, \mathbf{p}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{p}}}^{2}\right) \end{aligned} \]

其中，

$\mathbf{x}_{p}^{-1} \mathbf{x}_{k}$表示从中枢坐标系到第k帧的变换
$\mathbf{x}_{p}$作为局部地图的起点状态

纯里程计的优化

一旦我们通过满足收敛标准完成在线校准（VII-D部分），然后就可以在给定准确外参的情况下进行纯里程计的优化。在这种情况下，我们没有优化外参，并利用基于地图的测量来改善单激光的里程计。

我们将所有 LiDAR 和局部地图的特征之间的约束合并到成本函数中：

\[ \begin{aligned} f_{\mathcal{M}}(\mathcal{X}) &=f_{\mathcal{M}}\left(\mathcal{X}_{v}\right) \\ &=\sum_{k=p+1}^{N+1} \sum_{\mathbf{p} \in \mathcal{F}^{b_{k}}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{p}^{-1} \mathbf{x}_{k}, \mathbf{p}\right)\right\|_{\Sigma_{\mathbf{p}}}^{2}\right) \\ &+ \sum_{i=2}^{I} \sum_{k=p+1}^{N+1} \sum_{\mathbf{p} \in \mathcal{F}^{l^{i}_{k}}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{p}^{-1} \mathbf{x}_{k} \mathbf{x}_{l^{i}}^{b}, \mathbf{p}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{p}}}^{2}\right) \end{aligned} \]

其中，

$\mathbf{x}_{p}^{-1} \mathbf{x}_{k} \mathbf{x}_{l^{i}}^{b}$表示第k帧下，从主激光雷达的局部地图坐标系（pivot frame）到辅助激光雷达坐标系的变换。

外参标定收敛性管理

在以无人监督的方式工作在线标定时，判断标定是否收敛是有意义的，收敛后，我们就可以固定外参。这对我们的系统有益，因为里程计和建图都是从辅助雷达获得更多几何约束，以便更准确地进行位姿估计。

如[34]中得出，作为信息矩阵的最小特征值的退化因子λ揭示了基于优化的状态估计问题的条件。通过这项工作的影响，我们使用λ表示是否包含足够的约束来获取准确的外参。

更新外参和收敛性监视器的详细流水线总结如算法1所示：

算法采用式（19）定义的函数$f_{\mathcal{M}}(\cdot)$，以及使用当前的外参作为输入，并返回优化后的外参。

在第4行，我们从cost function的信息矩阵中计算$\lambda$，
在5-7行，如果$\lambda$大于阈值，则对外参进行更新。
在第8行，我们使用的外参候选值来检查收敛性。
在第9-10行，收敛性判断条件满足，因此触发停止标定。然后计算$\mathcal{L}$的采样均值，作为输出的外参结果，并且采样协方差作为标定的协方差。

边缘化

我们应用边缘化技术来删除滑动窗口中的最旧变量状态，边缘化是将历史约束作为目标融合的过程，这是维持里程计和标定结果一致性的重要步骤。

在我们的系统中，$\mathbf{x}_{p}$是在每次优化后被边缘化的唯一的状态，通过应用Schur Complement，我们得到关于保留状态的信息矩阵$\mathbf{A}_{rr}^{*}$和残差$\mathbf{g}_{r}^{*}$。因此，由边缘化得到的先验残差项可写成：$\left\|\mathbf{r}_{p r i}\right\|^{2}=\mathbf{g}_{r}^{* \top} \boldsymbol{\Lambda}_{r r}^{*-1} \mathbf{g}_{r}^{*}$，Appendix B会给出更加详细的数学推导。

考虑不确定性的多激光雷达建图

我们首先回顾了典型 LiDAR SLAM 系统的建图模块的管道 [16]-[18]，以里程计先验作为输入，算法构建全局地图并使用足够的约束来对pose关键帧进行精细化调整。这通过最小化所有基于地图的残差项之和来实现，式（21）：

\[ \hat{\mathbf{x}}_{b_{k}}^{w}=\underset{\mathbf{x}_{b_{k}}^{w}}{\arg \min } \sum_{i=1}^{I} \sum_{\mathbf{p} \in \mathcal{F}_{k}^{l^{i}_{k}}} \rho\left(\left\|\mathbf{r}_{\mathcal{F}}\left(\mathbf{x}_{b_{k}}^{w} \mathbf{x}_{l^{i}}^{b}, \mathbf{p}\right)\right\|_{\boldsymbol{\Sigma}_{\mathbf{p}}}^{2}\right) \]

其中,

$\mathcal{F}^{l_{k}^{i}}$是第k帧点云的特征点
$\mathcal{G}_{\mathcal{F}}^{w_{k}}$是全局地图
$\mathbf{x}_{b_{k}}^{w} \mathbf{x}_{l^{i}}^{b}$表示第k帧时刻第i个激光雷达的状态

我们使用Section VII-B中的方法查找$\mathcal{F}^{l_{k}^{i}}$和$\mathcal{G}_{\mathcal{F}}^{w_{k}}$之前的特征关联。在求解式（21）之后，求解的位姿将用于将特征点转换到世界坐标系，然后添加到全局地图上。为了降低计算和内存复杂性，地图也使用体素滤波器[71]进行降采样。但是，优化的精度取决于地图质量。图5展示了使用不确定的pose得到的带有噪声的地图点。我们认为，三个不确定性来源使地图点嘈杂：传感器噪声，退化姿态估计和外参扰动。

在下一节中，我们将激光测量点和位姿求解的不确定性传播到地图点上。结果，每个地图点都被建模为高斯变量。然后，我们提出了一种考虑不确定性的方法来提高多激光雷达建图算法的鲁棒性和准确性。

不确定性传播

继续在第III-C节中描述，我们现在计算协方差$\Xi$，mapping的位姿通过求解式（21）的NLS问题得到，我们直接计算信息矩阵的逆，即$\mathbf{\Xi}_{\mathbf{x}_{b_{k}}^{w}}=\boldsymbol{\Lambda}^{-1}$作为协方差。

外参协方差的设置取决于特定的情况，我们通用的把外参协方差定义如下：

\[ \boldsymbol{\Xi}_{\mathbf{x}_{l^{i}}^{b}}=\alpha \cdot \boldsymbol{\Xi}_{\text {calib }}, \quad \boldsymbol{\xi}_{l^{i}}^{b} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{\Xi}_{\mathbf{x}_{l^{i}}^{b}}\right) \]

其中，

$\boldsymbol{\xi}_{l^{i}}^{b}$是外参的扰动变量
$\boldsymbol{\Xi}_{\text {calib }}$是通过算法1计算得到的标定协方差
$\alpha$是缩放系数，用于控制协方差的量级

如果多激光雷达系统最近被标定过，我们设置$\alpha=1$，如果系统使用了很长时间且没有进行重新标定，激光雷达之间的外参会存在小的偏差，$\alpha$将被设置为更大的值。它具有与时间和外部影响的隐含关系，以及温度漂移。

给定主激光雷达的pose均值以及外参的均值和协方差，然后计算其他雷达pose的均值和协方差，如$\left\{\mathbf{T}_{l_{k}^{i}}^{w}, \boldsymbol{\Xi}_{l_{k}^{i}}^{w}\right\}$，这是一个关于两个含有噪声的复合pose的问题，我们遵循[19]中的4阶近似来计算他们。

然后，我们需要通过一个包含噪声的位姿变换来传播高斯不确定性，以产生新的地图点（landmark），如$\mathbf{y} \in \mathcal{G}_{\mathcal{F}}^{w_{k+1}}$，其均值和方差为$\{\overline{\mathbf{y}}, \boldsymbol{\Sigma}\}$，利用将点进行坐标变换的公式，可得：

\[ \begin{aligned} \mathbf{y} \triangleq \mathbf{T}_{l_{k}^{i}}^{w} \mathbf{p}_{h} &=\exp \left(\boldsymbol{\xi}_{e l_{k}^{i}}^{w^{\wedge}}\right) \overline{\mathbf{T}}_{l_{k}^{i}}^{w}\left(\overline{\mathbf{p}}_{h}+\mathbf{D} \boldsymbol{\zeta}\right) \\ & \approx\left(\mathbf{I}+\boldsymbol{\xi}_{l_{k}^{i}}^{w^{\wedge}}\right) \mathbf{\mathbf { T }}_{l_{k}^{i}}^{w}\left(\overline{\mathbf{p}}_{h}+\mathbf{D} \boldsymbol{\zeta}\right) \end{aligned} \]

其中，我们只保留指数映射$\exp(\cdot)$的一阶近似。

进一步的，如果我们展开等式，并仅保留一阶项，我们有：

\[ \mathbf{y} \approx \mathbf{h}+\mathbf{H} \boldsymbol{\theta} \]

其中，

\[ \mathbf{h}=\overline{\mathbf{T}}_{l_{k}^{i}}^{w} \overline{\mathbf{p}}_{h}, \quad \mathbf{H}=\left[\left(\overline{\mathbf{T}}_{l_{k}^{i}}^{w} \overline{\mathbf{p}}_{h}\right)^{\odot} \quad \overline{\mathbf{T}}_{l_{k}^{i}}^{w} \mathbf{D}\right] \]

\[ \boldsymbol{\theta}=\left[\boldsymbol{\xi}_{l_{k}^{i}}^{w \top}, \boldsymbol{\zeta}^{\top}\right]^{\top}, \quad \boldsymbol{\theta} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Theta}), \quad \boldsymbol{\Theta}=\operatorname{diag}\left(\boldsymbol{\Xi}_{l_{k}^{i}}^{w}, \mathbf{Z}\right) \]

特别的，操作符$\odot$表示将4x1的向量转换为4x6的矩阵：

\[ \left[\begin{array}{l} \varepsilon \\ \eta \end{array}\right]^{\odot}=\left[\begin{array}{cc} \eta \mathbf{I} & -\boldsymbol{\varepsilon}^{\wedge} \\ \mathbf{0}^{\top} & \mathbf{0}^{\top} \end{array}\right], \quad \boldsymbol{\varepsilon} \in \mathbb{R}^{3}, \quad \eta=1 \]