ICRA 2021| 具有在线校准功能的高效多传感器辅助惯性导航系统

Efficient Multi-sensor Aided Inertial Navigation with Online Calibration

作者：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 算法的准确性和一致性。

I 引言

在线定位是自动驾驶汽车的基本先决条件。迄今为止，已经开发了许多算法来使用不同的传感器实现高精度一致的 3D 定位。多传感器融合通常用于实现这一目标，原因有很多，包括更可靠的数据结果、更大的覆盖范围、适用性和更低的设备成本，但计算成本更高。在所有可能的导航传感器中，IMU、相机、车轮编码器、GPS 和 3D LiDAR 很有吸引力，因为它们为 3D 运动估计提供了足够的信息，并且对商业产品具有良好的可访问性。虽然原则上融合所有这些传感器以实现良好的定位性能似乎是直接的，但很少有工作表明融合超过三种类型的传感器，因为它们具有不同的特性、计算量增加、异步性和校准问题。此外，准确的在线多传感器校准对于最佳传感器融合至关重要，因为它可能会在导航过程中随时间变化。因此，在这项工作中，我们开发了一种高效的多传感器辅助惯性导航系统 MINS，一种由多模态传感器辅助的 INS，包括相机、车轮编码器、GPS 和 3D LiDAR，同时在线校准所有涉及的传感器考虑到它们的异步性质，并实现强大的准确实时定位性能。

在融合这些多模态传感器时，LiDAR集成不能过分强调，因为它有点云携带的大量信息。由于 3D LiDAR 传感器每秒可以提供超过 200 万个点（例如，HDL-64E [4]），所有这些数据点的简单融合可能不是实时的。与视觉测量不同，在不同的 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 处理时间、校准收敛性和估计器一致性。所提出的方法也在大规模城市驾驶数据集上得到验证。

II 相关工作

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] 上，但尚不清楚这些数据驱动方法的效率和通用性如何。

III 基于MSCKF框架的多传感器辅助INS

在介绍我们的激光雷达集成方法之前，在我们前期工作[1]-[3]的基础上提出了基于MSCKF的多传感器辅助INS融合IMU、相机、GPS和车轮编码器的测量。具体地说，在时间t_k时，状态向量x_k由当前惯性状态x_Ik和n个在相机测量时间捕获的历史IMU位姿x_ck组成：

我们定义了x=x^田x~，其中x是真实状态，x^是它的估计，x~是误差状态。

A. IMU Kinematic Model

使用IMU线性加速度a_m和角速度w_m测量值，状态及时向前传播：

我们在零噪声假设下基于标准惯性运动模型 f(·) [29] 将状态估计和协方差从时间 t_k 传播到 t_k+1：

B. Camera Measurement Model

在与帧 x_Ck 相关联的图像窗口上检测和跟踪稀疏角点特征。由此产生的轴承测量值 z_k 由下式给出：

堆叠与单个特征对应的所有测量值并对特征位置执行线性边缘化（通过零空间投影）会产生残差 [5]：

然后，这可以直接用于 EKF 更新，而无需在状态中存储特征，从而节省大量计算量并限制状态大小。

C. Wheel Measurement Model

与 [1] 中每次轮式编码器读数进入时都向状态添加clones不同，我们整合测量以获得 2D 中两次clones时间之间的相对姿态测量（旋转 ^Ok+1 _Okθ 和平移 ^Okd_{Ok+ 1})，这不会增加状态向量。

D. GPS Measurement Model

除了视觉/车轮测量更新，每当 ENU 框架 {E} 中的新 GPS 测量可用时，我们使用它来更新状态，如 [2]。特别地，在时间步 k 处的 GPS 测量 ^Ep_gpsk 是：

由于传感器的异步性，GPS 测量时间与clones时间不完全匹配，因此我们使用两个边界 IMU 位姿的线性插值 [32] 来计算 GPS 测量时间的 IMU 位姿。

E. Online Spatiotemporal Calibration

虽然我们已经简要介绍了不同的传感器集成模型来估计状态 (1)，但我们进一步扩展了我们的状态以包括传感器之间的传感器内参、外参和时间偏移，因此，可以执行完整的校准。我们省略了这些细节，感兴趣的读者可以参考[33]进行IMU-camera校准，[2]进行IMU-wheel校准，以及[1]进行IMU-GPS校准。

IV 高效的激光雷达测量系统更新

使用 LiDAR 测量有两个主要困难：实时处理和数据关联。由于 3D LiDAR 传感器提供大量数据点，因此几乎不可能实时跟踪所有点。与相机测量不同，找到不同扫描之间的点对应非常具有挑战性，因为这些点通常不代表相同的物理位置。为了解决这些问题，我们有效地从点云中提取平面块，并且由于它们包含主要结构信息，因此我们可以在扫描过程中有效地跟踪它们。为了描述平面patch (pp)，我们使用该平面的中心点 p 和 Hesse 法线 n：

利用MSCKF和pp表示，我们能够使用在局部窗口中收集的所有激光雷达测量值，有效地更新状态（1）。接下来，我们详细解释了从激光雷达点云中提取pp、合并pp、查找数据关联和执行更新/校准的方法。

A. Plane Patch Extraction

当一个新的激光雷达扫描进入时，我们稀疏地选择点并为每个选定点的邻近点来构建一个局部点云pi（参见图2左中的绿色和蓝色点）。

我们利用kd-tree快速找到邻近的点。一旦有了pi，我们利用[34]方法快速提取pp，并进一步扩展该方法来计算提取平面块的噪声协方差。我们首先通过减去云的中心点p_c：

现在，可以从该点云中提取平面，参数化为（通过沿z分量归一化）：

注意，当平面法线的z分量接近零时，这个表达式可以是奇异的，而通过选择不同的归一化轴可以很容易地解决这个问题。然后我们可以解决以下线性最小二乘：

然后可以计算平面法线n_z：=[a b 1]^T。请注意，计算出的平面通过了原点，因为d=0，因此p_c的中心点也成为了提取的pp的中心。为了避免从非平面或不良条件下提取pp，我们检查了点到平面的平均距离和nc的大小，然后在它们没有通过阈值时丢弃。

基于原始点测量噪声跟踪平面不确定性是很直接的：

请注意，有更稳健的方法来用更高的计算成本来解决这个问题，比如特征值分解。然而，计算速度是至关重要的，因此我们选择使用所提出的方法，因为解决方案可以立即从（15）中计算出来。

我们在一个新的点云中配置这个新的pp={p_c,n_c}，通过指定中心点p_c作为点的位置，n_c作为点的附加信息。请注意，由每个pp组成的局部点云pi也被跟踪到以后的合并步骤（IV-B）。平面斑点云(pppc)是新点云的名称，以区别于激光雷达点云。通过将pp视为点并构建pppc，我们可以方便地再次利用树树从选定的pp中搜索相邻的pp。

B. Plane Patch Merging

我们需要合并提取的pp。如果激光雷达是在一个结构化的环境中，特别是在一堵高墙附近，许多pp代表相同的平面，这是多余的。这种冗余不能被忽略，因为它可以显著增加计算量，特别是当从几个pppc具有相同的平面时。此外，如果点在同一平面上，提取p的点越多，就越准确、越一致。因此，我们试图在从激光雷达点云中提取它后，合并在同一平面上的pp。

为此，我们首先从pppc中稀疏地选择pp，并使用kd-tree找到每个相邻的pp。一旦找到相邻的pp，我们通过检查选定的pp_s={n_s，p_s}和每个相邻的pp_n={n_n,p_n}之间的以下剩余距离，迭代地检验“同一平面假设”：

残差（23）的第一个原始值的物理意义是平面法线的平行性，第二行是点到平面的距离。在测试了所有的pp_n后，pp_n的原始激光雷达点通过了测试，pp_s收集不重复，从收集的激光雷达点云创建一个新的平面补丁，使用的pp_s和pp_n从云中删除。图中示。2（右）显示了如何针对单一pp_s案例处理补丁之间的合并。重复此操作，直到处理所有pp_s，根据环境的结构，整个合并步骤可以重复几次。

C. Data Association

与前面的步骤不同，所有的操作都是在相同的参考帧内完成的，在需要状态信息的数据关联/更新过程中，pp必须将一帧转换为另一帧。然而，由于传感器之间的异步性，我们的状态在精确的激光雷达测量时间内没有一个IMU姿态来表示测量。

我们使用边界clone的线性插值[32]来表示测量时的IMU姿态，与每次激光雷达测量时添加clone相比，我们节省了大量的计算量。因此，我们首先收集所有可以找到边界位姿的pppc进行进一步步骤，如图3所示。

图3  为更新选择 LiDAR 测量。{I}和 {L}分别表示测量时的历史 IMU 位姿和 LiDAR 帧。在此示例中，收集了 tk 和 tk+1 处的 LiDAR 测量以进行更新，而在 tk−1 处则不是因为 IMU 位姿无法限制测量.

假设在 t_k 的 LiDAR 帧{L} 中测量的 ^Lkpppc 是收集到的最旧的 pppc，我们想要找到 ^Lkpp_i ∈^Lkpppc 的关联。对于每个收集到的 pppc，我们使用 kd-tree 找到最接近 ^Lk ppi 的 pp，并检查它们之间的残差。例如，用于测试 ^Lkpp_i 和 ^Lk+1ppj ∈ ^Lk+1pppc 的残差可以描述为：

如果残差小于阈值，我们认为两个 pp代表同一平面。请注意，我们在这里只检查残差；由于其与矩阵求逆相关的过程，Mahalanobisdistance检验可以在统计上更加一致但繁重。可能的错误关联将在下一个更新步骤中被过滤掉。我们在收集的 pppc 中迭代地找到 ^Lkpp 的所有关联。

D. Plane Measurement Update and Calibration

虽然平面的 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以避免信息的重用。

VI 实验分析

我们在真实世界数据集 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融合了所有传感器，并在实时运行时记录了所有校准参数收敛的最准确结果，显示了全局准确和局部精确定位性能。

VII 总结

在本文中，我们开发了一种高效且一致的基于 MSCKF 的多传感器辅助 INS（MINS），它融合了 IMU、相机、车轮、GPS 和 LiDAR 测量，并对所有传感器进行在线时空校准。特别是，我们一直主要专注于有效集成 LiDAR 测量，这是实时多传感器定位的瓶颈问题，因为它的数据量很大，并建议从点云中提取平面块并对其进行跟踪 扫描以形成 MSCKF 更新的运动约束。仿真结果表明我们的方法能够实时集成 64线 20 Hz LiDAR 以及校准收敛。所提出的 MINS 也在真实数据集中得到了验证，显示了其全局准确和局部精确的实时定位性能。将来，我们将研究如何有效地将闭环检测包含到系统中。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「计算机视觉工坊」公众号后台回复：深度学习，即可下载深度学习算法、3D深度学习、深度学习框架、目标检测、GAN等相关内容近30本pdf书籍。

下载2

在「计算机视觉工坊」公众号后台回复：计算机视觉，即可下载计算机视觉相关17本pdf书籍，包含计算机视觉算法、Python视觉实战、Opencv3.0学习等。

下载3

在「计算机视觉工坊」公众号后台回复：SLAM，即可下载独家SLAM相关视频课程，包含视觉SLAM、激光SLAM精品课程。

重磅！计算机视觉工坊-学习交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有ORB-SLAM系列源码学习、3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、深度估计、学术交流、求职交流等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~