一种半动态环境中的定位方法

在非静态环境中进行地图绘制和定位是机器人学的基本问题。之前的方法大多关注静态和高度动态的物体，但在半动态环境中，如果不考虑低动态物体（如停放的汽车和行人），可能会导致定位失败。我们提出了语义建图和终身定位方法，以识别非静态环境中的半动态物体，并提出了一个通用框架，将主流物体检测算法与建图和定位算法集成在一起。建图方法结合物体检测算法和SLAM算法来检测半动态物体，并构建只包含这些对象的语义地图。在导航期间，定位方法可以对静态和非静态对象进行分类观测，并评估半动态对象是否移动，以减少无效观测权重和位置波动。实验证明所提出的方法可以提高移动机器人在非静态环境下的定位精度。

01   引言

在过去几十年中，人们努力将SLAM的应用范围从实验室场景扩展到户外场景，但由于场景动态程度的差别。户外环境中，除了静态物体外，还有着更多的动态和半动态物体，这可能会导致定位系统产生波动，以对地图构建和定位系统提出了重要挑战。为此，研究人员开始打破静态世界的假设，但在识别高度动态和半动态物体方面仍然存在挑战。通过对动态物体进行建模和过滤，在高度动态的环境中实现了精确的地图和定位结果。然而，在包含许多半动态物体的环境中，地图和定位问题仍未解决，因为静态与半动态物体具有相似的特性。

为了解决这个问题，通过选择那些随时间变化较小的特征，并利用语义信息进行定位的方法证明是有效的，且已经得到广泛应用。因此，本文提出了一种相关方法，用于识别和记录环境中的静态和半动态物体，可以分为两部分：分别构建静态地图和半动态地图，并基于两者进行机器人定位。

本文的建图算法扩展了赫斯等人[15]引入的OccupancyGrid技术，用于记录环境的半动态部分。该算法能够检测环境中的静态和半动态物体，并生成两张地图：静态地图和半动态地图。在建图过程中，通过过滤动态物体，静态地图包含环境中的静态和半动态物体，而半动态地图只包含半动态物体。本文的定位算法同时使用两张地图优化粒子滤波算法。它允许对观测进行分类（静态或半动态物体），并确定半动态物体移动的距离。然后通过调整不同物体的权重，可以在半动态环境中获得更精确的定位结果。

我们的主要贡献如下：

我们提出了一种能够识别并记录环境半动态特征的语义建图方法；

我们构建了一个稳健的定位系统，可以根据观察结果确定物体类别；

我们引入了一个通用框架，结合主流的物体检测和SLAM算法，无论半动态物体位置如何变化，都能达到高精度定位的目标。

02   方法概述

本节介绍所提方法的体系结构。根据[29]的分类，本文稍作修改如下：

静态物体：保持固定位置的物体，如墙壁、货架；

半动态物体：在建图过程中保持静止，但在定位过程中可能移动的物体，如椅子、停放的汽车；

动态物体：指位置频繁变化的物体，例如行人和移动车辆。

图2展示了建图和定位框架。在建图过程中（红色虚线表示），选择一个对象检测算法（Yolov3
[37]）来识别半动态对象。在SLAM线程中，我们利用SLAM算法（Cartographer
[15]）生成静态地图，然后将激光雷达点投影到图像中。给出的半动态位置的全球坐标可以帮助创建半动态地图从静态地图。静态地图包含静态和半动态对象的信息，而半动态地图仅包含半动态对象的信息。此外，半动态地图是在线生成的，不会增加建图过程的复杂性。在定位过程中（蓝色虚线表示），同时将和加载到定位系统中。通过比较观察距离，我们可以获得与静态对象（黄色点）和半动态对象（红色点）对应的观察信息。同时从和中获取信息来定位机器人的姿态，取决于对象是否移动以及移动了多远。通过降低半动态物体观测值的权重，可以实现精确定位。

A：建图方法

1）静态地图更新： 基于物体出现在相同位置的可能性来实现对动态物体的识别。具体来说，如果某个区域的状态在占用和空闲之间发生变化，则该区域被视为动态区域。在本文中，采用概率方法将问题定义为：

2)半动态地图更新： 观测数据被分为静态对象观测数据和半动态对象观测数据。为此步骤，应用了Yolov3来获取分类信息。通过校准激光雷达与摄像头之间的外部参数，可以将激光雷达点投影到这些矩形框中。然而，由于激光雷达点在矩形框边缘可能对应于其他物体，因此无法提供准确的尺寸信息。为了解决这个问题，只采用矩形框中心的点来代表半动态对象的位置信息，从而排除了尺寸信息。有了位置信息，就可以在静态地图的基础上进行连接区域的计算，从而生成更精确的半动态地图。

图3中的蓝色圆点表示半动态位置。一个或多个相连的区域表示一个单一物体。为了补充半动态物体的维度，我们使用较低的阈值来获取二进制静态地图，然后计算相连区域。将半动态位置和相连区域结合起来生成半动态地图。在生成静态地图时，仅获取半动态位置，图3中的其他过程是离线的，不会增加建图过程的计算复杂度。有关更多实验结果，请参见第四部分。

B：定位方法

对于定位过程，我们使用蒙特卡洛方法通过使用odometry 和观察数据 进行递归验证来提出该想法。通过同时加载和，我们可以识别并减少与移动的半动态物体对应的观测值的权重。图4展示了所提定位方法的图形模型，其输入包括传感器观测数据、静态地图、半动态地图、odometry和时间的姿态。定位问题可以表示为：

通过将贝叶斯公式应用于方程2，可以表示为：

其中为归一化常数。为了进一步减少移动物体的权重，应根据和计算观测值的分类。方程3可进一步定义为：

我们的方法主要改进在于同时使用两张地图对半动态物体和静态物体的观测进行分类。通过比较每个激光点与两张地图之间的最近距离来计算分类，设代表点到静态地图中相邻网格的距离，代表点i到半动态地图中相邻网格的距离。如果-的差小于一个阈值，则当前激光点对应于半动态物体，否则为静态物体。定位过程中给出的结果反映了半动态物体（红色激光点）和静态物体（黄色激光点）。根据半动态物体移动的距离，相应观测的权重会相应地减少。减少的比例可以定义为，该函数的结果在0到1之间，表示当前观测的权重应减少多少。在本工作中，这些半动态物体的位置被假设遵循高斯分布。用F表示递减比率，其定义如下：

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

其中，表示观测模型。在本文中，选择似然观测模型来计算当前位置的权重。通过同时使用静态地图和半动态地图，可以使姿态估计更加准确。计算当前姿态权重的算法总结在算法1中。

该框架的三个主要组件——目标检测、地图构建和定位算法——都可以被其他相应的主流算法替换。

03实验

为了验证本文所提出的方法，我们使用一台机器人进行了实验测试。该机器人（如图6所示）配备了一台16束Velodyne激光雷达（在地图构建和定位过程中仅使用一束激光束）、摄像头、惯性测量单元和编码器。

该停车库被选为实验场景，因为每个个体运行时停车库都会发生显著变化。它由四个房间组成，地图框架中的左侧房间通常比其他房间停放更多的车辆。机器人从左侧房间开始，绕着停车库移动，然后回到原地。由于实验场景的特殊性，很难直接获得真实姿态。我们在地图构建过程中记录了机器人的姿态作为真实姿态，并在其他时间构建的地图上记录了定位过程中的姿态。通过比较未使用和使用所提方法的定位精度，验证了所提方法的有效性。在发生改变的环境中使用所提方法的定位精度也与在未改变的环境中不使用所提方法的定位精度进行了比较。

A. 停车场场景中的地图

对于建图过程，我们让机器人每隔几天进行一次数据采集。在单次运行中收集的数据用于构建.，该文件是通过使用静态对象的位置和速度信息在线下构建。

最终构建的和如图5所示。在这些静态地图中，由于车辆的移动，某些区域显得较为明显，而其他部分则始终保持静态。包含环境中大部分半动态物体，缺失的半动态物体对应于较少被观察到的区域，这对定位精度影响较小。通过应用逆观测模型，在地图构建过程中可以自动去除高度动态的物体，如移动的行人和车辆。

B. 在非静态环境中的定位

我们比较了在未修改和修改的半动态环境中使用和不使用我们的算法的定位精度（图7ABCD）。此外，我们还展示了在使用纯静态地图的情况下，原始蒙特卡洛定位方法的定位精度（图7E）。所有半动态对象，如停放的汽车，都是人工移除的。在建图过程中选择的姿态作为真值，在定位过程中记录的姿态也使用在其他日子构建的地图中。算法Evo（[38]）被用于离线评估我们的方法的精度。图7展示了定位精度的定性结果，表I列出了姿态误差的定量比较。MCL+-和MCL++分别表示在未修改和修改的环境中使用蒙特卡洛算法时，是否使用了我们的方法（第一个加减号指示定位过程中环境是否改变，第二个加减号指示定位过程中是否使用了我们的方法）。因此，MCL--和MCL-+分别表示在未修改的环境中使用蒙特卡洛算法时，是否使用了我们的方法。MCL表示使用蒙特卡洛算法，但不使用我们的方法，使用纯静态地图。纯静态地图如图8所示，其中半动态对象从地图中人工移除。此外，图7ABCDE分别对应表1中的MCL+-、MCL++、MCL--、MCL-+和MCL。

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

我们还进行了实验来评估算法在静态环境中的表现。如图7C和7D所示，我们的算法也可以应用于未改变的环境中，与传统的蒙特卡洛定位方法相比，其精度相当（0.327米对0.336米，0.065米对0.064米），这证明了所提方法在具有静态和半动态物体的环境中的通用性。

此外，我们还进行了实验，测试原始蒙特卡洛定位算法在修改后的环境中的定位性能，但将半动态物体从地图中人工移除。因此，地图仅包含环境的静态物体（如图8所示）。机器人利用环境中的静态物体来确定自身的位置。如图7E所示，由于所有半动态信息都被丢弃，因此其定位精度较低，是合理的。

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

C.一些棘手的情况

在获得半动态位置的过程中，可能会出现一些半动态位置击中错误位置的情况（如图9A和B所示）。但是，错误的位置不会影响生成，因为它没有对应的连接区域。

在计算连通区域的过程中，某些静态区域可能会被误认为是半动态区域。如图9C所示，柱子被误认为是半动态物体，然而，这些物体（柱子和墙）的静态特性使得该区域的重量保持不变。因此，错误的连通区域不会影响定位精度。

04   总结与未来

本文提出了一种新颖的语义地图生成方法和能够处理半动态环境的定位方法。通过融合摄像头和激光雷达，所提出的方法可以在地图构建期间自主检测和标记半动态物体，利用这些关键信息，能够构建用于后续导航的2D半动态地图，并设计了基于后验分布的姿态估计。我们还对配备了摄像头、激光雷达和IMU的移动平台在地下停车场进行了多项评估。结果表明，我们的方法在大多数情况下都能正常工作。基于此工作，我们的框架可以轻松扩展到3D模式。

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