0. 简介

对于激光SLAM而言，目前越来越多的工作开始集中在工业应用等场景，比如说我们这篇文章《Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration》。文中提到多个激光雷达的组合使得机器人能够最大限度地感知周围环境并获得足够的测量信息，这对于同时定位和建图（SLAM）是很有帮助的。然后文中指出在M-LOAM方法中可以在线标定得到鲁棒且实时的外参矩阵。对应的代码已经在Github上开源了。

[video(video-1K0JxXXg-1664086516393)(type-bilibili)(url-https://player.bilibili.com/player.html?aid=756354450)(image-https://img-blog.csdnimg.cn/img_convert/6d80b88ca2bd11a81cf74c03ac866921.jpeg)(title-[港科大] M-LOAM: 多激光雷达SLAM和标定)]

1. 文章贡献

文中指出本文提出了一个能够利用多激光雷达获得鲁棒与实时的外参标定、里程计与建图效果的系统。我们的系统首先提取点云中的边缘与平面点。经过外参初始化后，我们使用一个基于滑窗的多激光雷达里程计来同时估计位姿与在线标定，其中包含了标定结果的收敛性判断。本文主要贡献有四点：

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

作者也提到，M-LOAM是第一个完整的解决方案，多激光雷达校准和SLAM的完整解决方案。该方法实现了外参上的平移的厘米级精度和旋转的分数。对于不同尺度的SLAM，已成功应用M-LOAM以提供准确的姿势和地图结果。

2. 先验知识

文中在第三章一开始就指出，本文主要围绕了最大似然估计（Maximum Like- lihood Estimation， MLE）制定M-LOAM，MLE导致非线性优化问题，其中，高斯协方差的逆对残差函数进行加权。下面是本文的一些参数的命名。 文中考虑该框架由一个主要激光雷达和多个辅助激光雷达组成，主激光雷达的tf作为作为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}$

我们使用SE(3)中的4×4变换矩阵T表示Pose则为

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

2.1 最大似然估计

文中也对MLE问题，进行了详细的解释，并制定了多激光雷达系统的姿势和外参估计式：

$\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)$ 表示目标函数

假设观测模型使用高斯噪声来替换，那么上式就会变成非线性最小二乘(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}$ ，其中， $\boldsymbol{\Lambda}=\mathbf{J}^{\top} \mathbf{J}$ 称为信息矩阵。

不确定性表示

首先，考虑噪声的激光点如下：

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

其中,

• $\overline{\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向量转换为齐次坐标的矩阵

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，例如在受限制的环境中缺乏几何结构，它通常在其退化方向上不确定性。
• Extrinsic Perturbation，该项始终存在，由于外参误差的存在。这种扰动会对多传感器系统的测量精度造成不良影响,但很难测量。

作者也提到了文中有 $\Xi$ 的详细推导，这里就不一一展开了，等有需要再好好分析这部分推导

3. 主要工作---在线标定

这部分对应了M-LOAM文中的主要工作，我们这里还是主要集中来看一下外参标定的部分。下图为M-LOAM系统的完整流程的框图。系统从测量预处理开始。初始化模块为随后基于非线性优化的多激光雷达里程计初始化值，并进行校准优化。根据标定的收敛性，将优化分为在线标定和纯里程计两个子任务。如果校准收敛，我们可以跳过外部初始化和细化步骤，进入纯里程计和建图阶段。 文中提到，本文还是依赖三个假设完成的系统设计，即：

1. 激光雷达是同步的，这意味着不同激光雷达之间的时间延迟几乎为零。
2. 在校准初始化期间，平台经历充分的旋转和平移运动。
3. 主激光雷达的局部地图应与辅助激光雷达共享一个重叠的视场，以便在精细化过程中进行特征匹配，以缩短标定阶段。这可以通过移动机器人来实现。

下面我们将从odom的四个阶段来进行阐述,这里我们直接应用这篇文章的分类：

3.1 外参粗校准（Calibration of Multi-LiDAR System）

在尚未获得外参初始值的阶段，M-LOAM通过LOAM算法里的提取edge特征与planar特征并进行两两帧匹配的方式，算出每个激光雷达的两两帧之间的相对运动，累积得到每个激光雷达在各自的开机坐标系下的位姿。

随后构建手眼标定问题 $AX=XB$ ，其中 $A$ 和 $B$ 是要标定的两个激光雷达各自在各自的开机坐标系下的位姿，而X是这两个雷达的外参。

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

$\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}}$

$\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}}$

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

M-LOAM这里通过堆叠外参中旋转的约束，构建 $AX=0$ 的等式，通过对矩阵 $A$ 的SVD分解来获得外参中的旋转部分。当求得旋转之后，通过堆叠外参中的平移约束可以构建与求解最小二乘问题，即可求得位移。

3.2 外参细标定

通过粗标定获得激光雷达之间的外参初始值后，构建优化问题。

1. 首先构建Pr LiDAR的Local map，方法是将滑窗内点云都合并到Pivot帧（Pivot定义见优化变量节）的坐标系下。

2. 随后为优化问题添加特征关联因子。

   • 对于滑窗内每个不是Fixed的帧的Pr LiDAR的点云的特征点（Edge point 或Planer point)，将其直接通过该帧的初始Pose与Pr LiDAR的Local Map关联（K-d Tree)。则优化时会更新该帧的pose。
   • 对于滑窗内每个不是Fixed的帧的Au LiDAR的点云的特征点（Edge point 或Planer point)，将其通过Pr LiDAR的初始Pose以及该Au LiDAR到Pr Lidar的外参，与Pr LiDAR的Local Map关联。则优化时不仅会更新该帧的Pr LiDAR的pose，还会更新该Au LiDAR与Pr LiDAR的外参。

3. 执行优化。

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

$\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$ 是滑动窗口中的状态数量

3.3 外参收敛性判断

这个问题可以转化为如何判断优化结果是否出现退化（即优化的结果在某一个或多个维度约束不够）：如果最后优化结果没有出现退化，则可以认为优化收敛。 M-LOAM判断优化是否收敛，采用了与LOAM作者Ji Zhang在16年发表的论文《On degeneracy of optimization-based state estimation problems》一致的思想。在这篇文章中，作者提出了利用退化因子lambda来判断优化的结果是否发生退化。其中lambda是 $J^TJ$ 的最小特征值，而 $J$ 是优化问题的Jacobian。

当外参细标定的过程中，每一帧得到的外参优化结果都会进行退化因子的判断，若退化因子大于特定的阈值，则认为当前帧的标定结果收敛，将其保存到set中。若set中的外参结果数量超过阈值，则认为外参已经标定完成。将set中的所有外参取平均，作为传感器最后固定的外参。此时进入纯里程计阶段。

具体流程图作者在文中呈现如下：

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

剩下的还有很多内容，这里就不全部展开讲了，感兴趣的同学可以做更近一步的了解

4. 参考链接

https://blog.csdn.net/u013019296/article/details/117538703

https://jishuin.proginn.com/p/763bfbd785e3

http://epsilonjohn.club/2022/05/23/M_LOAM%E8%AE%BA%E6%96%87%E9%98%85%E8%AF%BB/