SDF-Loc: Signed Distance Field based 2D Relocalization and Map Update in Dynamic Environments

SDF-Loc：动态环境中基于符号距离场的二维重定位和地图更新

作者来自中国杭州阿里巴巴达摩院人工智能实验室。 {ximing.zmm, yimingchen, na.lmy}@alibaba-inc.com。

2019 American control conference (ACC), 2019

　　American Control Conference (ACC) 是控制领域世界级顶级会议。根据搜索结果，ACC的2023-2024年的影响因子（Impact Score）为1.11，h-Index为127，整体排名为9390。SCImago Journal Rank (SJR) 为0.575，这显示了其在学术界的影响力和认可度。因此，可以认为ACC是一个高水平的学术会议，对于控制和自动化领域的研究者来说，是一个非常重要的学术交流平台。

　　摘要：为了使自主机器人能够在给定区域执行长期导航，需要并发定位和地图更新算法。在本文中，我们通过为配备二维激光测距仪的机器人系统提供理论分析和算法设计来解决这个问题。本文的第一个关键贡献是我们提出了一种用于激光定位的混合符号距离场（SDF）框架。所提出的混合SDF集成了两种具有互补特性的方法，即欧几里德SDF（ESDF）和截断SDF（TSDF）。使用我们的框架，可以同时执行准确的姿态估计和快速地图更新。此外，我们引入了一种新颖的滑动窗口估计器，该估计器通过一致地利用具有扫描到扫描和扫描到地图数据关联的传感器和地图信息来获得更好的精度。真实实验结果表明，该算法可用于各种环境下的商业机器人的长期使用。实验还表明，我们的方法大大优于竞争方法。

一、引言

　　准确的全局位姿估计对于移动机器人有效地导航周围环境是必要的。在机器人领域，近20年来，这个定位问题得到了广泛的研究，特别是对于配备激光雷达或声纳等距离传感器的机器人[1][2][3][4][5]。大多数现有算法可以分为两类：未知环境中的定位和建图[2][3]或具有预先构建的定位地图的先前访问过的区域[4][5]。本文对后者做出了贡献，使移动机器人能够在不同条件下重复进行自主导航，无需人工干预。这克服了移动机器人长期自主化的一个关键挑战，并将使其低成本的商业用途成为可能。

　　为了实现这一目标，正确设计的算法应该满足以下所有要求。首先，针对现有定位图的位姿估计应该具有高精度，并且在不同的工作条件下也具有鲁棒性，例如，被移动物体包围、建筑材料的不同反射特性等。其次，虽然操作区域是预先访问和探​​索的，但在现实应用中，环境随着时间的推移而变化是不可避免的，例如室内空间中的家具重新布置。因此，在线地图验证和更新的能力决定了移动机器人随着时间的推移的性能。最后，为了广泛的现实应用和商业化，两者高精度位姿估计和地图更新组件应该能够在低成本平台上实时运行。

　　现有算法存在一定的缺陷。首先，现有的基于地图的算法要么需要像采用蒙特卡罗方法的算法一样进行大量计算[4][5]，要么需要精心调整的初始位姿估计以保证估计器收敛[3]。此外，我们注意到大多数算法没有明确地模拟操作环境随时间的变化（也没有相应的定位图）。相反，他们只是简单地概率性地拒绝与变化区域相对应的传感器测量结果[6]。对于较小的地图更改，这会导致估计精度降低，而对于较大的更改，这可能会导致定位失败。

　　为了解决这些限制，在本文中，我们提出了 SDFLoc，一种基于混合符号距离场的并发定位和地图更新算法。混合SDF方案集成了ESDF和TSDF，两者具有互补的特性。对于基于激光的定位问题，我们表明 ESDF 可以带来更好的估计精度和收敛特性（详细信息请参见第 IV 节），但在线更新以适应环境变化的计算成本很高。另一方面，TSDF 可以更新有效，但在数值优化方面不太稳健。这促使我们将它们结合在一起。

　　本文的主要特点和贡献可概括如下：

　　• ESDF 的新颖激光地图表示以及相应的扫描到地图优化功能。 ESDF 地图可以增量构建，也可以从其他地图格式转换而来，例如广泛使用的占用网格地图 [7]。

　　• 基于非线性优化的滑动窗口估计器，它以扫描到扫描和扫描到地图的方式由传感器和地图信息组成，以提高估计精度和收敛特性。

　　• 我们还提出了一种高效的在线地图更新方法，该方法利用 TSDF 框架合并新的半静态测量点，然后从更新的 TSDF 增量构建 ESDF。

　　• 最后，我们提供真实世界的实验结果来证明所提出的算法的准确性和有效性，以及与竞争方法的比较。图 1 显示了我们算法的代表性构建块。

图 1：我们提出的算法的代表性构建块。

利用先前的占用网格图 (a) 和原始激光扫描测量值 (b)，我们的方法估计机器人相对于全局地图的位姿 (c) 并执行动态地图更新 (d)。

请注意，这里的占用网格图仅用于可视化，位姿估计取决于所提出的 SDF 地图。

二.相关工作

　　在本节中，我们将简要回顾相关工作。基于激光的定位的现有工作可以按处理激光扫描和参数化激光地图的方法进行分类。一般来说，有三种类型的方法：基于稀疏特征参数化的方法[8][9][10]、基于密集表示的方法[11][3][2][12]以及最近引入的基于深度学习的端到端方法[13][14]。第一类算法旨在检测激光扫描中的可重复特征并对其进行几何参数化，包括但不限于直线[8]、角点[10]和斑点[9]。给定检测到的稀疏特征，将根据特征的几何信息[8]或其相应的描述符[9]跨帧执行数据关联步骤。完成此操作后，可以与这些特征联合估计机器人位姿，也可以根据离线计算的特征来估计机器人位姿。另一方面，密集算法无需预先选择信息即可处理整个激光扫描。占用网格建图[11]是当今最常用的密集方法之一，它通过网格单元呈现二维地图并计算每个单元的占用概率。在这个框架下已经开发了各种定位算法，其中大多数使用粒子滤波器估计[11][3]或非线性迭代优化[2]。最后，近年来，人们还提出了一些基于深度神经网络的算法来解决激光里程计[13]或扫描匹配[14]等问题。

　　最近，在上述算法中，基于密集的算法在商业应用中更受青睐。这是因为此类算法使用完整的激光扫描信息，从而提供了更好的准确性和鲁棒性保证。然而，占用网格框架[11]中的高精度实时姿态估计存在问题。基于粒子滤波器的方法[11][3]可以获得良好的精度，但其计算成本随着粒子数量的增加而线性增加。另一方面，可以采用基于非线性优化的方法[3][2]，但由于占用网格图表示的稀疏性，相应的估计器只有在提供良好的初始位姿估计时才能收敛。对于长期的机器人操作来说，这可能并不总是可行。 Fossel [12] 借鉴计算机视觉问题中的 TSDF 概念，提出了一种基于 TSDF 的定位算法，与传统的占用网格相比获得了更好的精度。最近的工作还表明 TSDF 可以直接用于绘图、导航和路径规划 [15] [16]。然而，TSDF 是高度压缩的地图表示，仅在结构表面附近保留有意义的值。因此，当初始位姿估计误差较大时，通过 TSDF 进行非线性迭代最小化仍有很大可能陷入局部极小解。为此，我们在本文中引入 ESDF[17][18]来提高定位性能。

　　机器人定位的另一个重要主题是处理动态环境，检测和拒绝移动物体以及将新出现的静态物体合并到地图中。第一个问题通常通过帧到帧密集数据关联来解决，通过最小化相对于刚性运动假设的几何一致性成本函数[19][20]。或者，可以使用卷积神经网络来检测和分类移动物体[21]。另一方面，为了更新定位图，[22]利用临时局部地图来跟踪由意外对象引起的观测，并使用局部地图和先前地图进行定位。 [23]通过在优化问题中引入时间作为参数，提出了一种称为动态位姿图SLAM的方法。具体来说，当机器人在同一区域多次操作时，位姿图将从每次运行中动态选择关键帧，以计算最能代表当前环境的地图。此外，Krajn ́ık [24] [25] 提出了一种时空占用网格方法，其中每个网格存储有关占用持久性和周期性的信息。基于此，这些方法可以预测不同时间戳的网格单元的概率。然而，在不同的环境条件下，占用持久性和周期性是不同的，并且这些算法对于现成的商业用途来说不够通用。

三．提出的方法

　　如前所述，本文的重点是在预先访问过的区域中执行并发定位和地图更新。因此，在本文中，我们假设预先构建的定位地图可用。我们不对生成定位图的建图算法施加任何限制，因为可以应用不同的最先进算法，例如[2][23]。这是因为我们不需要拥有完美的先验地图，因为我们可以执行概率地图更新以随着时间的推移细化地图细节。我们系统的概述如图2所示。由于大多数地图算法都会生成占用网格图，为了使我们的算法方便使用，我们首先描述将占用网格图转换为ESDF的管道。接下来，我们首先介绍非线性优化成本函数的基本公式，包括扫描到扫描项和扫描到地图项，来描述我们的滑动窗口估计器。随后我们将介绍这两个术语的数学细节。最后，我们描述了我们提出的在线地图验证和更新方法。

图2：SDF-Loc系统概览

A. 地图表示

　　使用占用网格图进行非线性优化的主要问题是其在相应梯度图中的稀疏性。具体来说，非线性优化方法需要计算相对于不同位置轴上的地图的梯度。但由于占用网格稀疏，这些元素中的大多数将完全相同为零。因此，当初始姿态估计不高精度时（这在结构变化较大或运动物体较多的环境中很常见），无法达到全局最优收敛。为了解决这个问题，我们建议使用ESDF。 ESDF 也是一个基于网格的地图，其中每个网格都包含到最近障碍物的欧几里德距离。我们注意到 TSDF 是另一种常用的基于 SDF 的表示，它计算每个单元到短半径内障碍物的投影距离。很明显，与占用网格和 TSDF 相比，ESDF 由于其更密集的地图表示而不会遇到“梯度消失问题”。

　　当提供占用网格图时，我们提出的方法的第一步是将其转换为 ESDF 表示。我们通过实验发现，许多现成的建图算法对于一些常见的材料（例如玻璃和镜子）表现不佳。因此，在执行占用网格图到 ESDF 地图的转换之前，我们首先应用预处理滤波器来删除病态地图区域。请注意，并非所有建图算法都需要此步骤，但它将有助于将所提出的算法无缝连接到最广泛使用的开源实现。

　　具体来说，我们根据以下标准进行预处理：所有自由空间单元应形成连通分量，并且未知空间单元应通过占用空间单元与自由空间单元分开。为了呈现过滤器的详细信息，我们引入以下符号“占用”、“空闲”和“未知”来表示相应的单元格。对于占用的单元格，我们计算空闲的百分比其周围的细胞。如果小于给定阈值，则该占用小区不可信，我们将其设置为未知。另一方面，对于每个自由单元，我们应用连通分量分析来拒绝不满意的自由单元，并将其设置为未知。一旦预处理完成，我们建议按照[26]的思想执行快速占用网格图到 ESDF 地图的转换。完整的过程如算法1所示，典型的地图转换案例如图3所示。

图 3：从占用网格地图到 ESDF 地图的代表性地图转换案例。

(a) 预处理步骤之前的占用网格图。与镜子对应的区域用红色圆圈显示。 (b) 未经预处理的 ESDF 图。

(c) 预处理的占用网格图。 (d) 从预处理的占用网格地图转换而来的 ESDF 地图。

B. 估计公式

　　在基于激光的定位中，我们寻求随着时间的推移估计相对于地图坐标系的二维激光位姿。从数学上讲，在时间戳 k 处，2D 位姿 pk 为：

　　其中xk和yk表示地图框中表达的位置元素，θk是航向角。通过将时间戳 k 处的激光测量值表示为 sk，将先验建图表示为 M，计算 pk 的概率算法可以表示为

　　其中 L(·) 用于计算扫描到建图差异作为位姿 pk 的函数，以建图 M 和扫描 sk 为条件。 [3]是这样做的代表性算法。然而，该方程仅依赖于单次激光扫描的信息来解决高度非线性的优化问题。因此，所使用的信息量不够，不可避免地会导致局部最小解（验证细节请参见实验部分）。

　　为了提高估计精度，我们建议使用基于滑动窗口的算法，其在时间戳k处的状态为：

　　其中 N 是滑动窗口大小。我们将估计算法表述为：

　　其中 I(·) 是一个函数，表示基于相应位姿的两次扫描之间的差异。通过引入扫描到扫描约束以及滑动窗口优化器，可以解决定位问题并提高性能。

　　接下来，我们将详细介绍激光帧到帧成本函数 I(·) 和帧到地图成本函数 L(·)。

C. 成本函数

　　我们首先提出帧到地图的成本函数。由于 ESDF 地图的每个单元代表到最近表面的距离，因此我们直接寻求最小化所有激光测量点的距离值之和。因此，我们将成本函数表述为：

　　其中 K 是扫描点数，mi k 是地图坐标系中表示的距离测量值，

　　F(·) 是柯西 M 估计量

　　其中 τ 是鲁棒估计参数。求解方程。 5 需要计算欧几里德距离值 D(mi k) 及其相应的梯度，在地图坐标 (mix, miy) 处进行评估。这可以通过在最新 ESDF 的四个网格单元上使用双线性插值来近似，该 ESDF 将从 TSDF 增量更新（有关详细信息，请参阅第 III-E 节）。

 　　另一方面，为了引入帧到帧成本函数，我们提出了连续时间激光测距 flo 方程[19]：

 　　其中 xi = (vx, vy, wθ) 表示位置速度和旋转速度，βi 是激光扫描 s 中激光点 si 的角度，kα 是代表激光束角分辨率的激光固有参数，Ri α 和 Ri t是 si 关于范围和时间的导数。

　　我们注意到，在上式中，ρ(xi) 与 xi 呈线性关系。因此，如果我们用Δt表示两次连续扫描之间的时间间隔，δ=(dx,dy,dθ)是它们之间的相对变换，我们可以写成：

 　　第一个近似方程符合线性运动假设。对于在连续激光扫描之间的短时间间隔（例如 75 毫秒）内在 2D 平面上移动的机器人，这种假设的误差很小。因此，对于连续帧中的所有激光点，可以通过使用鲁棒成本函数最小化 ρ(δ) 来制定帧到帧成本。

D. 滑动窗口优化

　　通过制定帧到帧和帧到图成本函数，我们可以重写估计问题方程 4 为：

 　　我们通过应用 Levenberg Marquardt 方法来解决这个非线性最小化问题，该方法在每次迭代时迭代地线性化上述成本函数。当获得新的位姿时，我们建议从滑动窗口中边缘化最旧的位姿，并且最旧的位姿将用于地图更新。

　　必须注意的是，典型的一致滑动窗口估计器必须在最小化问题中引入先验边缘化因子，以避免信息丢失，无论是信息形式[27]还是协方差形式[28][29]。然而，在我们的系统中，由于全局地图的存在，我们可以简单地删除最旧的帧，因为它是有条件独立于给定离线地图的其他帧的。

E. 动态地图更新

　　在更新地图之前，我们必须指出环境中存在不同样式的动态对象。与[6]类似，我们根据对象的动态将对象分为三类：

　　• 静态物体：在环境中不会改变其位姿的物体，如墙壁、固定桌子等。它们可以用作定位的有力线索。

　　• 半静态物体：以相对较低的频率改变位姿的物体，例如重新布置的家具。在本文中，我们进一步假设这些物体在激光视场中时不会改变其位置，但之后它们可以移动。这些对象应该在定位地图中动态更新。

　　• 动态物体：被观察时会改变其位置的物体，例如移动的人、移动的手推车等。它们既不用于定位也不用于地图更新。他们应该被归类为异常值。

　　通过计算相应单元上的欧式距离残差值可以识别静态物体和半静态物体。如果距离大于预设阈值Rthre，则将相应的测量标记为半静态测量，否则标记为静态测量。动态物体可以通过距离流量计算简单地检测和拒绝。地图更新中仅使用半静态测量。

　　Bresenhams 射线追踪算法[30]用于将这些半静态测量投影到网格单元中，并将潜在受影响的网格单元推入向量中。为了进一步加快更新过程，我们使用类似于[31]和[16]的方法。对于激光扫描中的每个点，我们将其位置投影到相应的网格单元，并将其与建图到同一网格单元的所有其他点分组。

　　与深度传感器不同，激光扫描仪通常具有非常大的视场角，例如超过180度，导致SDF 的投影误差较大。这里我们假设局部平面物体，误差可以表示为：

　　其中 d 是测量距离，γ 是激光射线与激光扫描仪中观察到的表面之间的角度。事实上，γ 可以在 0 到 π/2 之间变化，从而导致最大误差：rmax = d。

　　为了解决这个问题，我们首先将每次激光扫描中的半静态测量投影到网格单元，但仅当激光扫描次数超过 Nthre 时才更新 TSDF。对于某些网格单元，只有在有多个不同入射角的激光扫描测量时才需要更新TSDF值，以减轻小入射角带来的误差。一旦更新了 TSDF，就会应用类似于[16]的快速方法的 2D 修改来增量更新 ESDF。有关地图更新过程的更多详细信息可以在算法 2 中找到。

四．实现和实验

A. 实现和设置

　　我们使用 C++ 来实现源代码，其中 Ceres 求解器 [32] 用于我们提出的非线性优化框架。核心源代码是无ROS的，但我们的实验中也实现了ROS接口，用于可视化占用网格图。为了评估我们提出的方法，我们在四个不同的室内地方进行了实验。 [2]用于离线制图，地图网格大小为5cm。如前所述，值得注意的是，所提出的算法并未对映射算法的选择施加任何限制，并且由于其开源实现，我们使用了[2]。四种环境的计算占用网格图如图 4 所示，图 5 中还提供了示例图像。

图 4：三个办公室地点、当地餐厅、酒店和大厅的占用网格图。黑色代表占用空间，白色代表自由空间，灰色代表未知空间。

图5：室内场景的四种典型图像，办公室（左上）、餐厅（右上）、酒店（左下）、大厅（右下）

B. 结果

　　1) 收敛性和准确性：我们首先将所提出的方法与 [7]（称为 AMCL）进行比较，其中是一种带有 KLDsampling [33] 的蒙特卡洛定位方法 [7]。基于蒙特卡罗的算法广泛应用于机器人和自动驾驶应用。我们的实验中使用了在线开源实现[34]。对于定量评估，我们使用[35]中描述的指标来比较位姿误差（过渡和旋转元素）。第一个实验是评估优化收敛的能力。具体来说，我们测试了当输入的姿态估计误差较大时，不同的方法是否可以收敛（生成误差较小的姿态估计）。为此，我们手动将位置和旋转误差添加到所提出的方法和[34]的初始姿态估计中，并测试了两者的最大允许误差。为了解耦过渡和旋转的影响，我们在输入姿态估计中将它们分开。例如，为了评估过渡组件，我们仅向其自身添加误差，而旋转元素保持不变。

　　另外，由于我们稍后将展示滑动窗口优化器的必要性和性能，因此在本次测试中，我们不断将滑动窗口大小设置为1，以重点关注地图表示和ESDF扫描到地图成本函数。表一显示所提出的算法大幅优于竞争方法。在所有四个测试环境中，所提出的算法都可以应对较大的初始误差。这表明，所提出的地图公式和成本函数更适合定位的目的。

表 I：收敛性评估：仍然可以运行的两种方法的最大允许附加误差（产生可接受的定位性能）。

　　当两种算法都提供相同的合理初始位姿估计时，我们还比较了所提出的算法和 AMCL 的总体性能。我们将最大粒子数设置为 400，该值可能随时间而变化。另外，我们将滑动窗口大小设置为7，滑动窗口大小的详细分析可以在下一节中找到。实验结果见表二。我们的方法在定位精度和运行时间方面显然表现更好。我们注意到，这是通过混合 SDF 地图表示以及滑动窗口估计器公式来实现的。一方面，混合SDF图表示使其更适合非线性优化，从而获得更好的收敛性。另一方面，滑动窗口估计器始终结合帧到帧和帧到地图约束，导致更好的紧密耦合信息融合。

表 II：定位误差和运行时间

　　2）滑动窗口公式：我们还进行了实验来评估所提出的算法在不同滑动窗口大小下的性能。需要注意的是，当滑动窗口大小等于 1 时，帧到帧约束将无法有效使用。因此，估计器公式变得与 [3] 类似，但具有不同的地图表示。准确率和运行时间的实验结果如图6所示。我们首先注意到，当窗口大小为1时，获得最大误差。这表明需要更多的帧来进行联合优化以及帧与帧之间的约束。另外，由于论文篇幅有限，我们没有给出针对[3]的实验结果，而我们的测试也间接表明我们提出的公式可以更好地处理基于激光定位的问题。此外，我们注意到，当滑动窗口位姿数量增加时，精度会增加，但存在一个“截止”数：当使用 7 个位姿进行优化时，引入更多位姿会导致精度增益减少。

图 6：办公场景中不同滑动窗口大小（N = 1 ∼ 10）的定位误差和运行时间。

　　3）地图更新：虽然很难直接评估地图更新性能，但我们仍然能够比较有和没有地图更新的定位精度。这将显示进行地图更新过程的重要性。为此，我们进行了两项实验：1）重新布置家具的室内定位，2）移动大型平面物体的办公室定位。结果如表III所示。我们可以清楚地发现，当进行地图更新时，它可以更好地表征当前的环境条件并带来更好的定位精度。这也表明，要部署现实世界的机器人系统，该模块绝对是必要的，因为动态场景的变化是不可避免的。