实时语义SLAM：激光+IMU+GPS/MAV

这项工作中，提出了一种利用语义来全局实时定位机器人方法，这种方法仅使用以自身为中心的三维语义标记的LiDAR、IMU以及从卫星或空中机器人获得的自顶向下的RGB图像即可完成。此外，在运行时，我们的方法构建了一个全局配准的环境语义地图。

图片

01  介绍

定位是移动机器人的一个基本问题。从自动驾驶汽车[1]到探索型微型飞行器（MAV）[2]，机器人需要知道自己在哪里。这个问题对于多机器人系统来说更具有挑战性。在这种环境下，有效的协作通常假设共享对全局地图的理解[3]，而且也需要考虑到每个智能体所拥有的独特信息，这是一个重要的内容。

该问题最常见的解决方案是GPS[4]或设置的初始配置[5]。然而，GPS通常不可靠或不可用，基于初始配置统一构建地图的方法容易随时间推移而漂移，并且对初始对准中的小误差很敏感。其他还有使用视觉惯性里程计增强机器人-机器人检测的方案[6]，将机器人定位在共同的移动框架[7]内，但如果机器人不在彼此的视野内或距离太远，这类方法就会失败。

图片

如果机器人能够在彼此的地图上进行定位，相对定位问题就可以巧妙地解决。如果机器人在相似的空间中移动，则该问题相当于具有相似传感模式[8]、甚至不同传感器[9]的异构机器人的闭环问题。但是，如果机器人的轨迹永远不会重叠，那么环路闭合就无法完成了。

因此，我们建议将机器人的局部测量值与卫星或空中MAV获取的图像的更大的全球地图进行比对。因为俯视图与大多数机器人可用的以自我为中心的数据非常不同[12]，所以这种方法比传统的已知地图定位方法更具挑战性[10]，[11]。所以，在这些鸟瞰图中直接应用基于特征的配准方法来定位机器人是无效的。此外，卫星视图可能是在不同时间拍摄的，这意味着配准时必须能够应对季节变化。因此，我们必须有效地比较卫星视角和自我中心视角。

近年来，使用人工神经网络（ANN）的图像语义分割已成为一项成熟的技术[13]。地图语义是跨视图配准的理想选择，因为在给定充分变化的训练数据的情况下，它们对透视和季节变化是不变的。此外，无论是手动还是使用另外的ANN，都比较容易从鸟瞰图生成粗语义航空图[14]。同时，激光雷达的光束密度增加了，价格和重量也降低了，所以把它们放在小型移动机器人上现在是可行的。这两种传感技术的融合可以生成密集的语义点云，我们认为这是解决跨视图定位问题的理想方式。

最近的工作使用了图像语义进行跨视图定位[15]，[16]，但通常不用或很少使用深度信息，或对环境进行强有力的结构假设。与文献[3]等几何方法在没有足够几何结构的环境中失败相比，不同的是我们引入了一种方法，该方法将这两种信息源结合起来，以实现更稳健的语义交叉定位系统。此外，我们的系统完全是环境不可知的，只假设一些预先确定的类集。

我们这项工作的贡献如下：

1） 我们提出了一个实时的跨视图SLAM框架，该框架使用语义点云来定位到俯视图。在环境未知但有边界的情况下，我们的方法也能够估计航空地图的比例。

2）我们使用SemanticKITTI[17]以及我们自己在包括农村和城市环境在内的各种地点的数据集验证了我们提出的方法。此外，还使用推断的、有噪声的、真实的分割数据进行了验证。

3）我们发布了我们的数据集和建图代码，以便其他研究团体在该领域进一步开展工作。

02  相关工作

2.1 图像匹配

跨视图定位也可以称为是图像匹配的问题[12]。在给定表示全局地图的图像数据库和表示局部观察的查询图像时，该问题可以被简化为找到一些描述子，使得来自不同视图的相同位置的图像在某些可能的空间位置更接近。早期的工作如文献[18]、[19]使用局部特征描述子来实现这一目的。Majdik等人[11]将这些特征与谷歌街景的模拟图像相匹配，以与四旋翼飞机在城市环境中飞行的图像相匹配。然而，这些方法由于查询图像和数据库图像之间的视点差异更为极端而失败。

这种限制导致了最近对孪生网络（siamese networks）的兴趣[20]，[21]。这些网络在给定每个视点的权重共享的两个视点的潜在表示之间执行互相关，以便确定两个视点之间的距离。整个网络在已知的图像对上进行训练，损失函数激励潜在空间中的共同表示。尽管这些网络实现了令人印象深刻的准确性，但它们在处理图像对时速度相对较慢，因此必须同时快速查询许多可能的状态，所以无法在机器人应用中实时运行。

2.2 基于视觉的状态估计

图像匹配算法提供了一种将图像与所提供的现有图像数据库进行比较的方法，但它们并不明确地估计机器人或传感器的位置。定位需要一个带有姿态标签的图像数据库或一个完整的航空地图，如文献[22]所示。在这项工作中，作者将全球地图表示为空中帧中的一系列边缘，然后在粒子过滤器框架中与地面图像中的边缘进行匹配。然而，通过将全局地图缩减为一组边缘，该方法丢弃了大量潜在有用的信息。其他工作如文献[23]使用立体图像生成RGBD图像，然后从空中透视图渲染该图像，并使用Chamfer匹配与已知地图进行比较。然而，这种方法未能解决由季节变化或环境中的动态物体（如人或汽车）等因素引起的图像变化问题。

为了更好地应对季节变化和更极端的视点角度变化，最近的工作越来越强调使用语义进行定位。Castaldo等人[15]对地面图像进行分段，并使用与地面的单应性将这些语义投射到自顶向下的视图中。然后，他们为分割的图像设置语义描述符，并与已知地图进行比较，在一组相机位置上生成热图。然而，它们不利用时间或深度信息，导致在大规模定位系统中收敛缓慢。他们的投影方法也在单应性假设不成立的情况下失败，例如机器人前面的区域不接近平面的情况。在一项类似的工作中，Gawel等人[16]为空中和地面图像生成了语义图表示，然后他们在其上构建描述符，以匹配各种合成数据集。然而，该方法通过将语义分割减少到类块的质心来最小化地利用几何信息，因此在类少且种类最少的环境中失败。

2.3 基于激光雷达的状态估计

在存在深度信息的情况下，在帧之间投影数据变得更容易。Wolcott等人[24]通过从各种角度渲染点云并最大化状态空间上给定相机图像的相互信息，将相机定位在已知的高清点云中。Gawel等人[3]通过从两个角度构建点云地图并使用几何描述符进行匹配，来交叉定位地面和空中机器人。Barsan等人[25]通过使用具有LiDAR强度扫描自上而下视图的孪生网络实现厘米级定位。虽然这些方法准确且强大，但需要相关环境的预先存在的点云地图，而我们只需要一张航空图像。

图片

与基于视觉的方法一样，最先进的LiDAR方法利用了环境的语义结构。在早期的工作中，Matei等人[26]将封闭建筑模型拟合到从飞行器收集的点云上，用于创建地面水平预测，与地面图像进行比较。Senlet等人[27]通过使用ANN分割建筑物并比较捕获局部建筑布局的散列描述符，将局部自顶向下视图与大型卫星地图相匹配。Tian等人[28]使用了一种类似的混合方法，该方法也对建筑物进行分段，但他们使用孪生网络来生成建筑物描述符。与我们的类似，Yan等人[29]使用OpenStreetMap中的简单自上而下的环境视图进行基于3D激光雷达的定位。作者构建了一个用于匹配的4位描述符，其中包含了关于交叉点和建筑间隙的信息。虽然这种方法效率很高，可以实时运行，但为了将3D点云压缩到4位，它必须丢弃大量数据。这些方法都假定在定位环境的结构上具有很强的先验条件，并且在一般（即非城市）环境中失败。

文献[30]、[31]等一般利用环境的3D标记点云图。在文献[30]中，通过将投影的点云与来自机器人的以自我为中心的图像进行比较来进行配准，而在文献[31]中，作者使用随机游走描述符（random walk descriptors）进行匹配。与这些工作相比，我们的框架只需要单个卫星或鸟瞰环境，可以适用于更多应用。

03  方法

我们的方法由两个主要组件组成：基于ICP的LiDAR SLAM系统全景SLAM联合[32]（UPSLAM）-和基于粒子过滤器的语义定位器，如图2所示。这里我们详细介绍了这两个系统的目的和实现。

3.1 定位（Localization）

受蒙特卡洛机器人定位方法的成功启发[29]，[22]，[10]，我们采用粒子过滤器作为定位管道的主干。粒子过滤器特别擅长处理多模态状态分布，这在机器人定位问题中经常出现。然而，这是以计算成本为代价的，我们通过采用定制的优化策略来降低计算成本。

1） 问题阐述

对于2D地图定位，我们的系统状态x 由元组（p\in SE(2) ；s \in [s_{min},s_{max}] ）组成，其中是机器人在地图上的方向和位置位为米），（单位为像素/米）是地图比例，在其边界上有一定的优先级。此外，我们还有输入u \in SE(3) ，即机器人从UPSLAM或任何其他里程计源的最后一帧（以米为单位）的刚体变换。然后我们就剩下了定义运动模型P(x_t | x_{t-1},u_{t-1}) 。此外，在每个处，我们从LiDAR和相机获得语义扫描。为了定义我们的粒子过滤器，我们还必须定义测量模型P(z_t|x_t) 。

2）运动模型

我们不假设访问机器人的控制输入，而是依赖UPSLAM的帧到帧运动估计。由于UPSLAM在3D中操作，我们首先将帧到帧的运动投影到局部x-y水平面上，我们将该操作表示为\mathtt{proj}(u)\in SE(2) 。此外，我们只使用UPSLAM的初始ICP解进行运动估计，忽略了环路闭合的姿态优化，以避免里程计中的不连续性。最后，我们假设每个估计具有恒定协方差的正态分布噪声，尺度噪声收敛于对数空间中。总之：

图片

为了鼓励自然收敛，我们还用距离起始位置的倒数来缩放\sum_s 。一旦位姿中的尺度方差下降到阈值以下，我们就修正。

3）测量模型

我们的测量值z是一个语义点云，我们可以将其表示为具有相关标签的机器人框架的点位置列表：z=\{(p_1,l_1),(p_2,l_2),...,(p_n,l_n)\}。然后我们可以将这些点投影到地平面上。此外，对于任何给定的粒子状态，我们可以在自顶向下的空间地图L中查询机器人框架中任何点的期望类。然后，计算具有姿势d的特定粒子的成本的简单方法是：

图片

为了通过扩大局部最小值来提高收敛性，我们选择了一个价值函数。我们不是以二分法评估成本，而是惩罚特定类别的点与航空地图上同一类别的最近点之间的（阈值）距离。然后价值函数为

图片

最后，我们通过倒置C和归一化来计算ad-hoc概率。我们还引入了每类加权因子\alpha _l 。即，

图片

其中γ是引入慢收敛的正则化常数。所有粒子的概率最终被归一化，使得它们在所有粒子上的总和为1。我们注意到，这是一种特殊的度量，在实践中，常数通过实验来调整。然而，这对于基于蒙特卡洛的定位方法（如[29]）来说并不罕见。

4）性能优化

简单地实现，方程3的计算成本很高，因为它相当于对一系列（非凸）最小化的结果求和。我们通过预计算航空语义图的TDF来优化这一点。计算的实现很简单，大约需要一分钟，但每个地图只需要执行一次。该映射编码从每个点到该类最近点的阈值距离，将等式3的最小化转化为简单的固定时间查找。此外，我们不是对所有点求和，而是首先将语义LiDAR扫描离散化为极性片段，并计算每个片段中每个类的点数。局部类TDF以相同的方式呈现。然后，我们可以通过对这两个图像的元素乘积求和来近似方程3，从而得到每个类的内积运算，这可以有效地执行。我们发现，对于100×25像素的极坐标图，每个粒子大约需要500µs的时间，几乎所有这些时间都用于查找极坐标TDF。该过程如图3所示。

图片

此外，在极坐标表示中旋转地图相当于索引移动，速度非常快，这是我们在初始化期间利用的一个事实。我们在地图道路上随机采样点，因为我们有很强的先验知识，知道我们正在开始一条道路。对于每一个点，我们初始化在s_{min} 和s_{max} 之间均匀变化比例的k_s 粒子（依据我们的实验，为1和10像素/米）。对于每个粒子，我们均匀地采样k_{\theta} 可能的取向，这可以非常有效地利用折射率偏移来实现。我们选择最佳方向并将其用作初始粒子。

为了进一步加快我们的算法，我们在CPU内核上并行处理每个粒子的成本计算。我们还根据高斯混合模型（GMM）拟合粒子分布的协方差椭圆面积之和自适应地选择粒子数量。我们预计通过将粒子权重计算移动到GPU上可以获得较大的性能提高，但这将作为未来的工作。

3.2 语义分割

1） 卫星图像分割

我们训练了完全卷积网络（FCN）[33]的两个简单修改的版本，该网络具有在ImageNet上预训练的ResNet-34[34]主干，以分割卫星图像。图像直接从Google Earth获取。我们使用4个类别：道路、地形、植被和建筑。为了分析卫星图像，将256×256像素RGB图像作为输入传送到图像分割网络。该网络是在表1中数据集的三张手动标记的卫星图像上训练的。这些图像被随机缩放、旋转、裁剪和翻转，以生成更多的训练样本。卫星图像的随机缩放也允许模型更好地概括从多个高度收集的图像。我们网络的示例输出和训练数据如图5所示。尽管我们的模型仅在3幅图像上进行训练，但表现良好，这可能令人惊讶，但每幅图像都包含许多对象实例，此外，训练图像与测试图像来自同一城市，需要最小的泛化。

2） 扫描分割

我们使用图2中绿色框中显示的两个不同Pipeline，根据数据集生成语义点云。通过使用这些不同的分割方法进行测试，我们表明，如果最终输出是逐点标记的点云，我们的方法可以在不同的传感系统之间很好地扩展应用。

PC：对于KITTI数据集，我们使用与卫星分割相同的FCN结构（也是经过预处理的ImageNet），但对LiDAR扫描进行操作，该扫描表示为具有X、Y、Z和深度通道的2D极坐标网格图（64，2048）（我们不使用强度）。我们在SemanticKITTI上训练，使用{10}和{00；02; 09}作为验证和测试分割，剩余数据在训练分割中。我们还将额外的vehicle和Other类别添加到用于卫星分割的类别集合中，作为自上而下投影过程的一部分，将vehicle变换为road。

RGB：对于我们自己的Morgantown和UCity数据集，我们使用了与KITTI（Ouster OS-1）不同的LiDAR传感器，因此无法使用SemanticKITTI来训练数据。相反，我们使用在Cityscapes上训练的HRNets[13]对校准到LiDAR的RGB图像进行分割[35]。使用我们计算的外显性，我们可以将LiDAR点云投影到相机帧上，并为RGB分割中的每个点分配适当的类。

图片

3.3 建图

我们使用了[32]中描述的UPSLAM建图功能，但在此简要概述了我们的相关更改。UPSLAM使用迭代最近点（ICP）来估计LIDAR传感器的运动，并根据附加到关键帧的全景深度图的集合来表示整体几何结构，这些关键帧彼此连接以形成姿势图。对于这项工作，我们扩展了UPSLAM，另外集成从图像中提取的语义标签，以形成语义全景。值得注意的是，UPSLAM根本不使用扫描匹配的语义，它只是使用估计的刚体变换来集成语义信息。因此，因为我们不需要每次扫描的语义数据，所以我们可以以低于LiDAR的速度运行我们的推断，而无需删除ICP的数据，以提高地图质量。样本深度、正常和语义全景如图4所示。

除了对粒子过滤器运动模型使用UPSLAM自运动估计，我们还计算了每次更新时后验粒子过滤器估计的协方差和均值。一旦协方差下降到阈值\sum _t 以下，我们使用相对于the overhead map的估计位置作为姿态图中的先验因子，将相应的全景锚定到全局图。最终生成的姿态图如图2所示。通过这种方式，我们允许图优化也基于全局定位对全局地图进行地理参考。我们的实验表明，添加这些语义边可以消除漂移，有效地在每个关键帧创建全局地图的语义回环包。这使建图功能能够处理更大的轨迹，而无需循环，同时保持全局一致。

04  评估数据集

我们首先在SemanticKITTI[17]上评估我们的系统，这是一个来自KITTI里程计基准[36]的LiDAR扫描的手动逐点标记数据集。在这项工作中，我们对里程数据集的00、02和09序列进行了测试。

除了SemanticKITTI，我们还在Philadelphia和Morgantown收集了自己的数据集，以便在更多样的环境中进行测试。这些数据集是使用Ouster OS-1-64激光雷达和4台PointGrey Flea3 RGB相机收集的。摄像头与Ouster硬件同步，并使用文献[37]中介绍的方法进行校准。表1概述了我们用于评估的数据集。我们还使用了一个额外的数据集ucity'，它遵循与ucity相同的轨迹，但在几个月前就收集完成了。此外，我们还启动kitti2 50秒，kitti9 10秒，以避免UPSLAM在数据集开始时出现故障的区域。

图片

图片

05  结果

5.1 推理性能

我们首先评估了推理网络的性能，其结果如表2所示。对于LiDAR网络，测试集为KITTI 00、02和09。对于空中网络，模型在每个训练图像的3/4上进行训练；最后的1/4用于评估。我们注意到，这并不能很好地测试其他城市的通用性，但稳健的多城市航空分割不是本工作的重点。然而，我们强调，无论是在激光雷达扫描还是航空图像中，我们的定位器在分割数据中都能够很好地处理显著的真实噪声。

5.2 精准性

我们对KITTI 00、02和09进行了测试，通过对每个轨迹进行5次运行，既使用了主动尺度估计，也使用了固定的地面真实尺度。

图片

图6显示了自动检测到收敛后整个数据集的像素位置误差。为了方便运行，我们向系统提供了地面真实比例尺。除了kitti9之外，所有运行的所有参数都是固定的，对于kitti9，我们将角过程噪声增加了10倍，因为UPSLAM的估计值更高。平均误差分别为2.0、9.1和7.2米。对于后来的数据集，误差集中在几个局部区域，这些区域通常是UPSLAM困难的区域（通常在几何结构较少的树的隧道状区域中），因此运动先验没有那么准确。我们将这些结果与表3中的[20]和[29]进行了比较。通过结合更丰富的语义数据，我们能够在kitti0上以10倍的优势超过文献[29]。Kim等人[20]获得了与我们相当的定位精度，尽管使用了单个RGB相机而不是LiDAR，并且需要在卫星图像上的网格上预计算深度嵌入。

图片

KITTI上的这些实验完全实时执行，包括分割、定位和建图，运行在AMD Ryzen 9 3900X CPU和NVidia GeForce RTX 2080 GPU上。这种级别的计算可以很容易地安装在车辆或中等大小的地面机器人上。我们还预计，通过进一步优化我们的方法，例如将粒子权重函数移动到GPU，我们可以在计算资源更加受限的平台上实时运行。

此外，我们在图7中对ucity和morg的结果进行了定性检测。我们注意到，通过将全局边缘合并到建图位姿图中，我们不仅能够在自顶向下的图像中全局定位机器人，还可以提高全局地图的一致性。在长循环过程中，UPSLAM本身由于漂移而无法找到回环，从而导致全局不一致的建图。从语义定位器中加入我们的全局边缘可以缓解这种漂移，使UPSLAM能够成功地找到回环。

图片

图片

5.3 收敛性

我们通过在整个数据集中以偶数间隔开始运行，对UCity、Morgantown和KITTI 0和2数据集进行收敛分析。我们不分析kitti9，因为它相对较短。我们将正确的收敛速度定义为检测到的收敛的分数，其中收敛后前20秒的平均误差小于10米。图8显示了正确收敛到整体检测收敛的频率。我们忽略粒子过滤器在运行结束前未收敛的运行，这发生在数据集中剩余时间小于收敛时间的开始时间。除固定比例情况下正则化项γ加倍外，所有运行均使用相同的参数。

图片

图片

根据这些结果，我们得出结论，在估计尺度时，收敛更困难，这与我们所说的一致。我们在两种情况下使用相同数量的粒子，使得样品更可能收敛。ucity对于我们的系统来说显然是非常困难的，我们将其归因于环境更接近于一个统一的网格，其结构不那么独特，可以帮助定位。为了改进定位，我们在几个月前拍摄的相同轨迹（ucity）的数据集上运行定位器，并使用生成的地图细化原始卫星分割。这一过程的结果如图10所示。使用这一新的细化建图ucity ref，我们能够显著提高正确的收敛速度，这表明尽管先前的运行提前了几个月，但能够为建图添加有用的关联信息。Kim等人[20]和Yan等人[29]在表3中获得了差不多的时间，但无法估计尺度。对于kitti9，由于数据集较短，我们仅从开头就开始。注意，这些工作只计算从数据集开始的收敛时间，并且计算从不同点开始的平均时间。

图片

最后，图9中对我们系统的估计规模的分析表明，在大多数情况下，我们的方法可以在真实值的大约0.05倍内准确估计规模。

5.4 消融实验

注意到地图细化对于提高ucity的收敛性非常有帮助，我们调研了我们的定位器在多大程度上依赖于道路形状和轨迹，以及它在多大限度上使用了来自建筑物和树木的额外线索。我们计算了树木和建筑物的成本，并在图11中展示了这些发现。对于所有运行，我们假设了固定的已知尺度。我们得出结论，对于具有复杂道路结构的环境和轨迹，简单地利用轨迹并将其与道路模式匹配就足够了。然而，对于更像曼哈顿假设的结构，建筑物和树木等局部细节变得更为相关。有趣的是，对于ucity来说，仅仅移除树木比建筑和树木都稍微差一些。我们设想，这是因为在密集的城市环境中，两边几乎都有建筑物，因此它们在道路之外增加的结构很少，在噪音的情况下可能是有害的。然而，对于ucity来说，这些树显然提供了强有力的有效线索。

图片

06  总结

我们提出了一种在卫星或自上而下的无人机图像中进行实时语义SLAM系统。此外，我们还展示了在各种环境中使用不同传感器收集的多个数据集的定量和定性结果。在所有这些实验中，我们的系统运行稳定且准确。未来，我们计划在异构机器人团队上部署我们的系统，以实现分散建图和更高级别的自动化。

精彩推荐

1. 视觉3D目标检测，从视觉几何到BEV检测
2. 万字综述 | 自动驾驶多传感器融合感知
3. 两万字 | 视觉SLAM研究综述与未来趋势讨论
4. 清晰讲解激光雷达与相机标定的时间戳同步问题
5. 末流985研一在读，视觉SLAM方向快坚持不下去了，大家可不可以给点建议...
6. 基于SLAM的机器人自主定位导航全流程