Woosik Lee, Yulin Yang, and Guoquan Huang
在公众号「」，后台回复「」，可直接下载。
为了方便大家更好地学习激光SLAM，我们邀请专家对激光进行深入分析SLAM详细讲解了关键算法和源代码实现，感兴趣的朋友可以加入我们一起学习。
本文设计了多功能多传感器辅助惯性导航系统 (MINS)，能有效融合 IMU、相机，车轮编码器，GPS 和 3D LiDAR 多模态测量和在线时空传感器校准。在工作 [1]-[3] 在此基础上，本工作主要关注基于滑动窗的高效率LiDAR集成。由于每个 3D LiDAR 大量扫描 3D 这对实时性能构成了巨大的挑战。我们提倡使用包含环境结构信息的平面图 LiDAR 点云中提取有效更新/校准系统。所提出的 LiDAR 平面patch该算法(包括提取、数据关联和更新)被证明是高效和一致的。广泛的 MonteCarlo 具有大规模城市驾驶场景的真实世界数据集已用于验证 MINS 算法的准确性和一致性。
在线定位是自动驾驶汽车的基本先决条件。到目前为止，许多算法已经开发出来，使用不同的传感器来实现高精度的一致性 3D 定位。多传感器集成通常用于实现这一目标，包括更可靠的数据结果、更大的覆盖范围、适用性和更低的设备成本，但计算成本更高。在所有可能的导航传感器中，IMU、相机，车轮编码器，GPS 和 3D LiDAR 因为它们很有吸引力，所以很有吸引力 3D 运动估计为商业产品提供了足够的信息和良好的可访问性。虽然原则上整合所有这些传感器以实现良好的定位性能似乎是直接的，但由于具有不同的特性、计算量增加、异步性和校准问题，很少有工作表明整合了三种以上的传感器。此外，准确的在线多传感器校准对最佳传感器集成至关重要，因为它可能会随着导航时间的推移而变化。因此，在这项工作中，我们开发了一种高效的多传感器辅助惯性导航系统 MINS，多模态传感器辅助的一种 INS，包括相机、车轮编码器，GPS 和 3D LiDAR，同时，考虑到其异步性能，所有涉及在线校准的传感器都实现了强大的准确实时定位性能。
在整合这些多模态传感器时，LiDAR由于云携带的调，因为它携带的信息有点云。由于 3D LiDAR 传感器每秒可以提供超过 200 一万点(例如，HDL-64E 所有这些数据点的简单集成可能不是实时的。不同的视觉测量，不同的 LiDAR 很难在扫描之间找到相关点，因为激光点击中可能没有完全相同的物理位置。因此，我们提倡使用平面块，它包含点云最重要的结构信息，以获得稳定的数据关联 LiDAR 点云中提取更新状态并校准 LiDAR 和 IMU 时空参数之间。特别是基于我们以前的工作 我们开发了[1]-[3] MINS，实时、一致、紧耦合、多传感器辅助 INS 高效的估计器 LiDAR 平面patch跟踪并在所有传感器之间校准在线时空。具体来说，这项工作的主要贡献包括:
? 我们设计了MINS，这是基于通用实时MSCKF [5] 多传感器辅助INS 可优化有效集成的估计器IMU、相机，车轮编码器，GPS 和3D LiDAR 的测量值。
? 我们开发了一种高效的产品 LiDAR 特征跟踪算法，以从 3D LiDAR 具有适当不确定性并和跟踪具有适当不确定性建模的平面块。这些平面patch实时在线校准 MSCKF 更新。
? 我们在现实模拟中进行了广泛的评估MINS，详细分析 LiDAR 处理时间、校准收敛性和估计器了处理时间、校准收敛性和估计器一致性。
MSF-EKF [6] 来自第一个未来 IMU、相机、GPS 在线空间外参校准与压力传感器的通用相对和绝对测量相结合。整个处理时间以几百毫秒为界。Hausman 等 [7] 还将 IMU、相机、GPS 和超宽带 (UWB) 测量在 EKF 外参校准应在框架内进行。然而，这两项工作都只能处理相对较小规模的测量，无法解决传感器之间的时间校准问题。Shen 等 [8] 使用 UKF 来集成 IMU、相机、GPS、2D LiDAR、不校准传感器参数和分析处理时间的压力高度计和磁力计 VINS Fusion [9] 使用松耦合graph formulation融合 IMU、相机、GPS、磁计和气压计。Suhr 等 [10] 基于粒子滤波的集成 IMU、相机、GPS、车轮以及符号道路标记图，而 Meng 等 [11] 在UKF 集成在框架下 IMU、GPS、距离测量仪 (DMI) 和 LiDAR。所有这些方法都假设了已知的完美校准，并且没有研究计算的复杂性分析。
最近，将会有一些研究工作 IMU、LiDAR 结合相机和在线校准。例如，我们以前的工作 LIC-Fusion 1.0 [12] 和 2.0 [13] 使用从 LiDAR 在线校准点云中提取的平面。然而，工作中考虑的传感器不包括车轮编码器和 GPS，仅限于通过 10 Hz 的 16 通道 LiDAR 实时性能显示，用于更新的平面数量较少。我们最近的工作 [1] 将 IMU、相机和 GPS [2]、[14]结合时空校准IMU、相机和车轮编码器进行时空校准和车轮参与校准，[15]集成MSCKFf框架内，车轮，2D激光雷达和预建的2D在线校准激光雷达地图。多传感器校准算法总结如表 I 所示。
虽然有很多关于 LiDAR 但我们只关注将要处理的文献 3D LiDAR 与其他传感器实时集成 SLAM 方法。特别是，Xu 等 [16] 使用information filter利用不反转大的测量协方差进行整合 IMU 和 LiDAR，[17]-[19] 的作者从 LiDAR 测量中提取 LOAM [20] 与其他传感器相比，特征和特征Zhang等 [21] 引入了快速平面分割和地图细化算法，节省计算时间，提高地图质量。Pathak 等[22] 从传入点云中提取平面，有效地找到相应的关系，从而提高效率。Shan 等 [23] 利用图优化 IMU 与 LiDAR 融合在一起，局部保持在滑动窗口 LiDAR 扫描以确保系统的实时性能 Maddern 等 [24] 和目视觉信息和 LiDAR 点云集成在滑动窗口的相机视场中，以改善图像视差估计。尽管最近的重要研究工作也集中在深度学习上 [25]-[28] 但这些数据驱动方法的效率和通用性尚不清楚。
在介绍我们的激光雷达集成方法之前，基于我们的早期工作[1]-[3]MSCKF多传感器辅助INS融合IMU、相机、GPS测量车轮编码器。具体来说，时间tk状态向量xk目前的惯性状态xIk在相机测量时间量时间捕获的历史IMU位姿xck组成：
我们定义了x=x^田x~，x是真实状态，x^是它的估计，x~是误差状态。
使用IMU线性加速度am和角速度wm测量值，状态及时向前传播：
在零噪声假设下，我们基于标准惯性运动模型 f(·) [29] 状态估计估计状态和协方差 tk 传播到 tk 1：
在与帧 xCk 在相关图像窗口上检测和跟踪稀疏角特征。轴承测量值 zk 由下式给出：
堆叠对应于单个特征的所有测量值，并对特征位置进行线性边缘化（通过零空间投影） [5]：
然后，它可以直接用于 EKF 在不存储状态特征的情况下进行更新，节省大量计算，限制状态大小。
与 [1] 每次轮式编码器读数进入时，都会向状态添加clones不同的是，我们整合测量以获得 2D 中两次clones时间之间的相对姿态测量(旋转 Ok 1 Okθ 和平移 OkdOk 1)，状态向量不会增加。
除视觉/车轮测量更新外，每当 ENU 框架 {E} 中的新 GPS 当测量可用性时，我们使用它来更新状态，例如 [2]。特别是，在时间上 k 处的 GPS 测量 Epgpsk 是：
由于传感器的异步性，GPS 测量时间与clones时间不完全匹配，所以我们使用两个边界 IMU 线性插值的位置 [32] 来计算 GPS 测量时间的 IMU 位姿。
虽然我们简要介绍了不同的传感器集成模型来估计状态 (1)，但我们进一步扩展了我们的状态以包括传感器之间的传感器内参、外参和时间偏移，因此，可以执行完整的校准。我们省略了这些细节，感兴趣的读者可以参考33]进行IMU-camera校准，[2]进行IMU-wheel校准，以及[1]进行IMU-GPS校准。
使用 LiDAR 测量有两个主要困难：实时处理和数据关联。由于 3D LiDAR 传感器提供大量数据点，因此几乎不可能实时跟踪所有点。与相机测量不同，找到不同扫描之间的点对应非常具有挑战性，因为这些点通常不代表相同的物理位置。为了解决这些问题，我们有效地从点云中提取平面块，并且由于它们包含主要结构信息，因此我们可以在扫描过程中有效地跟踪它们。为了描述平面patch (pp)，我们使用该平面的中心点 p 和 Hesse 法线 n：
利用MSCKF和pp表示，我们能够使用在局部窗口中收集的所有激光雷达测量值，有效地更新状态（1）。接下来，我们详细解释了从激光雷达点云中提取pp、合并pp、查找数据关联和执行更新/校准的方法。
当一个新的激光雷达扫描进入时，我们稀疏地选择点并为每个选定点的邻近点来构建一个局部点云pi（参见图2左中的绿色和蓝色点）。
我们利用kd-tree快速找到邻近的点。一旦有了pi，我们利用[34]方法快速提取pp，并进一步扩展该方法来计算提取平面块的噪声协方差。我们首先通过减去云的中心点pc：
现在，可以从该点云中提取平面，参数化为（通过沿z分量归一化）：
注意，当平面法线的z分量接近零时，这个表达式可以是奇异的，而通过选择不同的归一化轴可以很容易地解决这个问题。然后我们可以解决以下线性最小二乘：
然后可以计算平面法线nz：=[a b 1]T。请注意，计算出的平面通过了原点，因为d=0，因此pc的中心点也成为了提取的pp的中心。为了避免从非平面或不良条件下提取pp，我们检查了点到平面的平均距离和nc的大小，然后在它们没有通过阈值时丢弃。
基于原始点测量噪声跟踪平面不确定性是很直接的：
请注意，有更稳健的方法来用更高的计算成本来解决这个问题，比如特征值分解。然而，计算速度是至关重要的，因此我们选择使用所提出的方法，因为解决方案可以立即从（15）中计算出来。
我们在一个新的点云中配置这个新的pp={pc,nc}，通过指定中心点pc作为点的位置，nc作为点的附加信息。请注意，由每个pp组成的局部点云pi也被跟踪到以后的合并步骤（IV-B）。平面斑点云(pppc)是新点云的名称，以区别于激光雷达点云。通过将pp视为点并构建pppc，我们可以方便地再次利用树树从选定的pp中搜索相邻的pp。
我们需要合并提取的pp。如果激光雷达是在一个结构化的环境中，特别是在一堵高墙附近，许多pp代表相同的平面，这是多余的。这种冗余不能被忽略，因为它可以显著增加计算量，特别是当从几个pppc具有相同的平面时。此外，如果点在同一平面上，提取p的点越多，就越准确、越一致。因此，我们试图在从激光雷达点云中提取它后，合并在同一平面上的pp。
为此，我们首先从pppc中稀疏地选择pp，并使用kd-tree找到每个相邻的pp。一旦找到相邻的pp，我们通过检查选定的pps={ns，ps}和每个相邻的ppn={nn,pn}之间的以下剩余距离，迭代地检验“同一平面假设”：
残差（23）的第一个原始值的物理意义是平面法线的平行性，第二行是点到平面的距离。在测试了所有的ppn后，ppn的原始激光雷达点通过了测试，pps收集不重复，从收集的激光雷达点云创建一个新的平面补丁，使用的pps和ppn从云中删除。图中示。2（右）显示了如何针对单一pps案例处理补丁之间的合并。重复此操作，直到处理所有pps，根据环境的结构，整个合并步骤可以重复几次。
与前面的步骤不同，所有的操作都是在相同的参考帧内完成的，在需要状态信息的数据关联/更新过程中，pp必须将一帧转换为另一帧。然而，由于传感器之间的异步性，我们的状态在精确的激光雷达测量时间内没有一个IMU姿态来表示测量。
我们使用边界clone的线性插值[32]来表示测量时的IMU姿态，与每次激光雷达测量时添加clone相比，我们节省了大量的计算量。因此，我们首先收集所有可以找到边界位姿的pppc进行进一步步骤，如图3所示。
图3 为更新选择 LiDAR 测量。{I}和 {L}分别表示测量时的历史 IMU 位姿和 LiDAR 帧。在此示例中，收集了 tk 和 tk+1 处的 LiDAR 测量以进行更新，而在 tk−1 处则不是因为 IMU 位姿无法限制测量.
假设在 tk 的 LiDAR 帧{L} 中测量的 Lkpppc 是收集到的最旧的 pppc，我们想要找到 Lkppi ∈Lkpppc 的关联。对于每个收集到的 pppc，我们使用 kd-tree 找到最接近 Lk ppi 的 pp，并检查它们之间的残差。例如，用于测试 Lkppi 和 Lk+1ppj ∈ Lk+1pppc 的残差可以描述为：
如果残差小于阈值，我们认为两个 pp代表同一平面。请注意，我们在这里只检查残差；由于其与矩阵求逆相关的过程，Mahalanobisdistance检验可以在统计上更加一致但繁重。可能的错误关联将在下一个更新步骤中被过滤掉。我们在收集的 pppc 中迭代地找到 Lkpp 的所有关联。
虽然平面的 pp 表示可以提供快速的数据关联，但由于其非最小表示如果天真地用于更新，则存在一些与计算相关的问题。由于平面具有 3DOF，而 pp 使用 6DOF 向量 (p，n) 来表示这样的平面，这种过度参数化可能会显着增加计算负担和数值不稳定性。因此，我们将平面表示从平面patch (pp) 更改为最近点 (cp) [35]，这是平面的最小表示，并且可以制定紧凑的残差函数以执行有效的 MSCKF 更新。
具体来说，cp 可以被认为是位于平面上并且最接近被测帧原点的 3D 点。可以使用具有以下关系的 Hesse 法向量 n 和距离标量 d 来描述这种 cp 表示：
线性化：
如第 IV-C 节所述，可能存在错误关联，因此我们使用残差 (36) 执行以下 Mahalanobisdistance 测试：
如果Mahalanobisdistance小于阈值，我们认为关联是有效的，等式 (36) 可以直接用于 MSCKF 更新，而无需在状态中存储特征，从而节省大量计算量。从云中删除使用过的平面patch以避免信息的重用。
我们在真实世界数据集 KAIST Urban39 [36] 中进一步评估了 MINS，该数据集在具有 11.06 公里长轨迹的市区收集，并使用双目相机、IMU、车轮编码器、GPS 和 16 线 LiDAR 进行估计。基于 VIO，不同的传感器组合与校准一起进行测试，每种算法的结果轨迹如图 6 所示。与数据集提供的地面实况相比，每种算法的方向和位置的均方根误差 (RMSE) 为 总结在Table III中。
总的来说，VIO 出现了规模问题，结合额外的传感器能够解决这个问题。GPS-VIO 利用 GPS 的全球测量结果，在三个传感器组合中显示出最佳结果。LiDAR-VIO 的运行速度比实时速度快约 2 倍，尤其显示出良好的 z 轴估计结果。这是因为 LiDAR 以向下 45 度的角度安装，从道路表面提供大部分扫描，因此道路上提取的平面可以防止 z 方向漂移。所提出的MINS融合了所有传感器，并在实时运行时记录了所有校准参数收敛的最准确结果，显示了全局准确和局部精确定位性能。
在本文中，我们开发了一种高效且一致的基于 MSCKF 的多传感器辅助 INS（MINS），它融合了 IMU、相机、车轮、GPS 和 LiDAR 测量，并对所有传感器进行在线时空校准。特别是，我们一直主要专注于有效集成 LiDAR 测量，这是实时多传感器定位的瓶颈问题，因为它的数据量很大，并建议从点云中提取平面块并对其进行跟踪 扫描以形成 MSCKF 更新的运动约束。仿真结果表明我们的方法能够实时集成 64线 20 Hz LiDAR 以及校准收敛。所提出的 MINS 也在真实数据集中得到了验证，显示了其全局准确和局部精确的实时定位性能。将来，我们将研究如何有效地将闭环检测包含到系统中。
作者也是我们特邀嘉宾：
本文仅做学术分享，如有侵权，请联系删文。
在「计算机视觉工坊」公众号后台回复：，即可下载深度学习算法、3D深度学习、深度学习框架、目标检测、GAN等相关内容近30本pdf书籍。
在「计算机视觉工坊」公众号后台回复：，即可下载计算机视觉相关17本pdf书籍，包含计算机视觉算法、Python视觉实战、Opencv3.0学习等。
在「计算机视觉工坊」公众号后台回复：，即可下载独家SLAM相关视频课程，包含视觉SLAM、激光SLAM精品课程。
扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在
也可申请加入我们的细分方向交流群，目前主要有、、、、、等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。也请联系。
▲长按加微信群或投稿
▲长按关注公众号
：针对3D视觉领域的五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题