Lifelong Localization in Semi-Dynamic Environment阅读笔记（翻译）

半动态环境下的终身定位

摘要

非静态环境中的映射和定位是机器人技术的基本问题。以前的大多数方法主要关注环境中的静态和高动态对象，在半动态场景中可能会出现定位失败，而不考虑动态性较低的对象，例如停放的汽车和停止的行人。在本文中，我们介绍了语义映射和终身定位方法来识别非静态环境中的半动态对象。我们还提出了一个通用框架，可以将主流对象检测算法与SLAM算法相结合。 该映射方法结合了对象检测算法和SLAM算法来检测半动态对象，并构造出只包含环境中的半动态对象的语义图。 在导航过程中，定位方法可以对静态和非静态对象分别对应的观测进行分类，并评估那些半动态对象是否发生了移动，以减少无效观测和定位波动的权重。

引言

在过去的几十年中，已经努力将其应用从实验室场景扩展到室外场景，其中主要区别在于动态程度。除了静态对象，在室外环境中更常见的动态和半动态对象可能会导致定位系统的波动，这对SLAM系统提出了关键的挑战。为了解决这个复杂而又有希望的问题，研究人员开始打破静态世界假设，不可避免的障碍是识别环境中的高度动态和半动态对象。通过建模和过滤出动态对象，在高度动态的环境中实现了精确的SLAM结果、。但是，在包含许多半动态对象的环境中，由于静态对象和半动态对象的动态特性相似，SLAM仍然是悬而未决的问题。

为了解决这个问题，选择了随时间变化较小的特征。利用语义信息进行定位的方法已被广泛应用并被证明是有效的。因此，在本文中，我们提出了一种相关的方法来识别和记录环境中的静态和半动态对象，该方法可以分为两个部分：分别建图静态对象和半动态对象，以及同时使用静态地图和半动态对象进行机器人定位。本工作中的映射算法扩展了Hess等人引入的占用网格技术记录环境的半动态部分。它能够检测环境中的静态和半动态对象，并生成两个地图：静态地图和半动态地图。在映射过程中过滤掉动态对象后，静态地图同时包含环境中的静态和半动态对象，而半动态地图只包含半动态对象。

这项工作中的定位算法通过同时使用两个地图来优化粒子滤波算法。它允许观察分类（静态或半动态对象），以及确定半动态对象移动的距离。通过调整不同物体的权重，可以在半动态环境中获得更精确的定位结果。

贡献：

（1）我们提出了一种语义建图方法，可以识别和记录环境的半动态特征。

（2）我们构建了一个强大的定位系统，可以根据观察结果确定对象类别（动态，半动态，静态）。

（3）引入了一个通用框架，可以结合主流目标检测和 SLAM 算法来实现高定位精度，而不受半动态目标位置变化的影响。

相关工作

在非静态环境中进行SLAM是一项具有挑战性的任务，近年来受到了广泛关注。通过对环境的静态部分进行建模并过滤掉动态对象，尝试了几种解决方案。Burgard等人提出了一种距离过滤器，用于丢弃与动态对象相对应的观测值。H¨ahnel等人提出了一种基于特征的方法，使用联合概率数据关联粒子滤波器来跟踪和映射人。Wang等人开发了基于特征的方法来过滤扫描配准过程中的动态对象。H¨ahnel等人    使用期望最大化（EM）算法离线识别环境的静态和动态部分。这些基于识别和过滤动态对象的方法在高动态环境中具有很强的鲁棒性。然而，它们丢弃了有价值的观测数据，而且无法处理移动频率较低的半动态对象。

为了解决这个问题，提出了维护多个地图来表示环境的方法。Wolf等人维护了两张分别代表动态和静态对象的地图。Krajnık等人通过一个单独的SLAM过程在飞行中构建了新地图，这确保了新地图与之前的地图一致。新构建的地图被集成到一个时空模型中，该模型表示环境动力学。Gallagher等人为每个物体绘制了单独的地图。通过激光观测检测到的动态对象保存在带有位置和方向的列表中。尽管这些方法利用了动态信息并构建了一个能够更好地表示当前环境的地图，但它们仍然与以前的方法具有相同的静态假设限制。

为了克服静态世界假设的局限性，已经报道了旨在找到代表动态的模型的方法。Biber等人提出了一个模型，该模型同时在多个时间尺度上表示环境。Montesano等。有选择地区分和利用静态和动态信息来预测碰撞并提高导航性能。Meyer-Delius等开发了一个隐马尔可夫模型来表示每个网格的动态，并对占用率随时间的变化进行精确建模。Akai等人设计了一个观测模型，其中偏离一定阈值距离的物体被视为动态障碍。但是，如果在地图过程中动态对象相对接近静态对象的位置，则此假设可能会失败。Li等人引入了一种粒子滤波方法来解释移动的动态障碍物，并使用了主动定位系统来降低动态。丁等人设计了一个变化检测模块来显示每个单元格中存在变化的可能性。但是，这些方法要求机器人在同一条路线上运行多次，以从环境的动态中收集数据。

与传统特征相比，语义特征对环境变化具有不变性。肖等人将语义信息与基于特征的动态 SLAM 系统相结合。他们提出了一种选择跟踪算法来消除动态对象并提高鲁棒性和准确性。贝斯科斯等人利用 RGBD 相机和 Mask R-CNN 去除 RGB-D 相机中对动态物体的观察，这在高动态场景中优于标准视觉 SLAM 基线的准确性。布拉施等人在姿态估计管道中使用了语义信息。维内特等人将场景理解引入到大规模语义场景重建中。他们还提出了一种语义融合方法，可以更有效地处理动态对象。于等人将语义分割网络与移动一致性相结合，以提高定位精度。王等人将 LiDAR数据投影到图像中，并将 RGB 图像的 2D 分割信息应用于 3-D LiDAR点。斯坦伯格等人使用语义分割来替换特征描述符。陈等人提出了一种基于语义特征的姿态优化方法，在估计机器人姿态的同时调整树的位置。以前基于语义信息的方法已成功应用于动态场景，但忽略半动态特性是主要限制。在本文中，我们提出了一个包含映射方法的系统，该方法利用对象检测算法生成语义图。以下定位算法可应用于半动态环境。

方法

地图中物体分类：

（1）静态物体：永远不会改变位置的物体，如墙壁、架子。

（2）半动态对象：在映射过程中是静态的，但在定位过程中可能会移动的对象，如椅子、停放的汽车。

（3）动态物体：经常改变位置的物体，如移动的人、移动的汽车。

图中给出了映射和本地化框架的概述。在映射过程中 (红色虚线)，选择对象检测算法(Yolov3 )来识别半动态对象。在SLAM线程中，我们利用SLAM算法 (Cartographer) 生成静态地图，然后将LiDAR点投影到图像中。在全局坐标中给定的半动态位置以后可以促进从静态地图ms创建半动态地图md。静态地图ms包含静态和半动态对象的信息，而半动态地图md仅包含半动态对象的信息。此外，半动态地图是离线生成的，而不会增加映射过程的复杂性。在定位过程中 (蓝色虚线)，ms和md同时加载到定位系统。通过比较观察的距离，我们可以获得对应于静态物体 (黄色点) 和半动态物体 (红色点) 的观察。合并了来自ms和md的信息，以定位机器人的姿势，具体取决于物体是否移动以及移动了多远。精确定位可以通过减少与半动态对象相对应的观测权重来实现。

A.建图方法

（1）静态地图更新: 根据对象出现在同一位置的概率来实现对动态对象的识别。具体来说，如果某个区域的状态在占用和自由之间变化，则该区域是动态的。本文利用概率方法将问题定义为:

其中 z1:t 是从时间1到t的传感器数据，st−1是先前的静态图，st是当前的静态图。去掉一些不必要的项，我们可以得到一个只能记录环境静态部分的逆观测模型。如果网格的状态从未知变为占用或保持占用，则将记录该网格。

（2）半动态地图更新：观测zt为观测筛选为静态对象观测zts或半动态对象观测ztd。对于这一步，使用Yolov3来获取分类信息。通过校准激光雷达和相机之间的外参，可以将激光雷达点投影到这些边界框中。但是，它无法提供准确的尺寸信息，因为边界框边缘的激光雷达点可能对应于其他对象。为了解决这个问题，只采用包围盒中心的点来表示半动态对象的位置信息，而不包括尺寸信息。利用位置信息，在静态地图上计算连接区域后，可以得到更精确的半动态地图。

图3中的蓝点表示半动态位置。一个或多个连接区域表示单个对象。为了补充半动态对象的维度，使用较低的阈值来获得二值静态图，然后计算连通区域。 半动态位置和连接区域的整合产生了半动态地图。构建静态地图时只获取半动态位置，图3中的其他过程是离线的，不会增加映射过程中的计算复杂度。

B.定位方法

定位过程的图形模型

对于定位过程，蒙特卡洛方法用于通过递归使用里程计ut和观察zt来验证所提出的想法。我们通过同时加载 ms 和 md 来提高定位精度，这可以识别和减少对应于移动的半动态对象的观察权重。图4说明了所提出的定位方法的图形模型，其中传感器观测值 zt、静态地图 ms、半动态地图 md、里程计 ut、时间 t-1 的姿势作为输入。定位问题可以表示为：

我们方法的主要改进是同时使用两个地图对对应于半动态对象和静态对象的观察进行分类。可以通过比较每个 LiDAR 点与两个地图之间的最近距离来计算分类。令dsi表示静态地图中点 i 到最近网格的距离，令ddi表示半动态地图中点i到最近网格的距离。如果ddi和dsi之差小于阈值，则当前 LiDAR 点对应于半动态对象，否则，LiDAR 点对应于静态对象。 定位过程中的给定结果反映了半动态对象（红色 LiDAR 点）和静态对象（黄色 LiDAR点）。分类后，半动态对象对应的观测权重根据它们移动的距离而减少。 递减率可以定义为 f(zt, ms, md,  dsi, d) 函数的结果在0和1之间，表示当前观察的权重应该减少多少。 在这项工作中，这些半动态对象的位置应该遵循高斯分布。令 F 表示递减率 f(zt,ms,md, dsi , ddi )，可定义为：

其中是一个比较小的距离，本文取0.1。是物体被认为是半动态的阈值，在本文中取0.3。 因此，当前位置的最终权重可以计算为：

其中 p(zt|xt,ms,md) 是观察模型。 在本文中，选择似然观测模型来计算当前位置的权重。 通过使用静态地图和半动态地图，姿势估计可以更准确。算法1总结了当前姿态权重的计算算法。

其中一个主要优点是，该框架中的三个组件，即对象检测，建图和定位算法可以被其他相应的主流算法代替。（可替换性）

实验

为了验证本文提出的方法，已经使用真实机器人进行了实验测试。这项工作中使用的机器人（图6）配备了16线Velodyne LiDAR（在测绘和定位过程中仅使用单线）、相机、惯性测量单元和编码器。

停车场被选为实验场景，因为停车场每次运行都会发生显着变化。它由四个房间组成，地图框中的左侧房间通常比其他房间显示更多的汽车。机器人从左边的房间开始，绕着停车场转了一圈，回到了原来的地方。由于实验场景的特殊性，很难直接得到ground truth。我们将机器人在建图过程中的姿势记录为地面实况，并且在定位过程中的这些姿势也使用其他日子构建的地图记录下来。通过比较没有和有该方法的定位精度，证明了该方法的有效性。还将在改变的环境中使用所提出的方法的定位精度与在未改变的环境中不使用所提出的方法的定位精度进行比较。

俯视图（A、B、C）显示了使用每次运行中收集的数据构建的静态地图。 (a', b', c') 是区域 (a, b, c) 的放大视图，其中半动态对象（汽车）经常变化。底部面板（D、E、F）显示了使用来自静态地图和半动态位置的信息构建的半动态地图。

A. 停车场场景中的地图绘制

对于映射过程，我们引导机器人每隔几天收集一次数据。 在单独运行期间收集的数据用于构建 ms。 md 是通过使用 ms 和半动态对象的位置离线构造的。

最终构造的ms和md如上图所示。在这些静态地图中，由于汽车的移动，某些区域是不同的，而其他部分始终是静态的。md包含环境中的大多数半动态对象，而缺少的半动态对象对应于很少观察到的区域，这些区域对定位精度的影响较小。通过应用逆观察模型，在建图过程中自动删除了高度动态的物体，例如移动的行人和汽车。

B.非静态环境中的定位

比较了在更改和未更改的半动态环境中使用和不使用我们的算法的定位精度。此外，我们还介绍了原始蒙特卡洛定位的定位精度，而无需使用纯静态地图的方法。所有半动态物体，如停放的汽车，都被人为移除。映射过程中的姿势被选为地面真相，并且定位过程中的姿势也使用其他日子构建的地图记录。算法Evo [38] 用于离线评估我们方法的准确性。图7显示了定位精度的定性结果，表I列出了姿态误差的定量比较。在不使用我们的方法和使用我们的方法的情况下，使用蒙特卡洛算法在改变的环境中MCL − 和MCL均值 (第一个正负下标表示定位过程的环境是否发生了变化，第二个正负下标表示定位过程是否使用我们的方法)。因此，在不使用我们的方法和使用我们的方法的情况下，使用蒙特卡洛算法在未改变的环境中表示mcl-和mcl-均值。MCL * 意味着使用蒙特卡洛算法而不使用我们的方法，使用纯静态图。纯静态地图如图8所示，其中半动态对象被人为地从地图中删除。此外，图7ABCDE分别对应于表I中的MCL − 、MCL *。

在图 7A 和 7B 中，我们的算法显着降低了变化环境中的偏差，从而降低了平均误差（0.083m 对 0.129m）和最大误差（0.336m 对 0.516m）。根据图 5 所示的地图和机器人的路线，它在前 300 秒和最后 200 秒内遇到了更多的半动态物体，并且在这些地方定位精度波动很大（图 7A），这证明了原始的蒙特卡洛算法在改变的环境中性能低下。相反，应用我们的算法后结果更加准确（图 7B）。

       我们还开发了实验来评估算法在静态环境中的性能。在图7C和7D中，我们的算法也可以在不变的环境中应用，与传统的蒙特卡罗定位方法相比，精度相对相同（0.327m对0.336，0.065m对0.064）。这证明了所提方法在静态和半动态对象环境中的通用性。

此外，我们还进行了实验以测试原始蒙特卡罗定位算法在改变的环境中的定位性能，但半动态对象被人为地从地图中移除。因此，地图仅包含环境的静态对象（图 8）。机器人利用环境中的静态物体来定位姿势。定位精度如图 7E 所示。由于丢弃了所有半动态信息，因此它具有较低的准确性是合理的。

另外，md是离线获取的。 因此，机器人在定位过程中只需要比较两张地图之间的距离，计算复杂度可以忽略不计。 需要注意的是，蒙特卡洛算法的绝对精度不是本文的重点。 我们的目标是将我们的方法引入主流的建图和定位算法，以在不牺牲效率的情况下获得更准确的结果。实验结果证明了我们的方法在静态和半动态环境中具有更好的鲁棒性、更高的准确性和多功能性。

C. 一些棘手的情况

在获取半动态位置的过程中，可能会有一些半动态位置击中错误的位置 (图9A和B)。但是，错误的位置不会影响md生成，因为它没有相应的连接区域。（可以改进的点）

在计算连通面积的过程中，一些静态区域会被错误地视为半动态区域。在图9C中，支柱被误认为是半动态物体，尽管如此，这些物体 (支柱和墙壁) 的静态特性使该区域的重量恒定。因此，错误的连接区域不会危及定位精度。

结论和未来的工作

本文介绍了一种新的语义地图生成方法和一种可以处理半动态环境的定位方法。通过融合相机和激光雷达，所提出的方法可以在映射过程中自动检测和标记半动态对象。利用这些关键信息，构建二维半动态地图以供以后导航，并设计基于后验分布的姿态估计。我们还在一个配备了摄像头、LiDAR 和 IMU 的移动平台的地下停车场进行了多项评估。结果表明，我们的方法能够在大多数情况下工作。基于这项工作，我们的框架可以很容易地扩展到 3D 模式。

在进一步的扩展中，我们将研究使用其他类型的地图和不同的定位算法，例如点云地图和 NDT 定位算法。