经典文献阅读之--M-LOAM（多激光雷达在线标定方案）

Posted 2023-02-06

0. 简介

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

[港科大] 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​​​，我们使用来表示，对于其他辅助雷达，使用来表示。

我们记为从原始激光雷达提取的有效特征，每个特征都表示为3D空间中的一个点：。

状态向量，由平移和旋转部件组成，记为，其中是3x1向量，是四元数，但在我们需要旋转向量的情况下，我们在SO(3)中使用3×3旋转矩阵

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

2.1 最大似然估计

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

其中，

• 表示第k帧的有效特征
• 表示待优化的状态
• 表示目标函数

假设观测模型使用高斯噪声来替换，那么上式就会变成非线性最小二乘(NLS)问题:

其中，

• 表示鲁棒性Huber损失[61]，用于处理outlier
• 表示残差函数
• 表示协方差矩阵

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

在最终的迭代中，状态的最小二乘协方差计算为，其中，称为信息矩阵。

不确定性表示

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

其中,

• 表示不含噪声的点
• 是零均值的高斯扰动变量，是激光测量的噪声协方差
• 为了使得式（3）与转换矩阵(i.e., 更加紧凑，我们使用齐次坐标系来表示：

其中，

• 是将3x1向量转换为齐次坐标的矩阵

LIDARS深度测量误差（也称为传感器噪声）主要受目标距离的影响，矩阵被简单的设置为常值矩阵。

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

其中，

• 是不含噪声的变换矩阵
• 是协方差为的小扰动变量

这种表示允许我们在状态空间中储存变换的均值作为，并使用作为扰动。

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

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

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

3. 主要工作—在线标定

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

文中提到，本文还是依赖三个假设完成的系统设计，即：

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

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

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

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

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

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

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

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