Costmap文献阅读——Layered Costmaps for Context-Sensitive Navigation

摘要

许多导航系统，包括无处不在的ROS导航堆栈，在单个成本图上执行路径规划，其中大部分信息存储在单个网格中。这种方法在生成最小长度的无碰撞路径方面非常成功，但是当成本图中的值超出已占用或空闲空间时，它在动态的、充满人员的环境中可能会遇到困难。
我们已经创建并实现了一种称为分层成本图的新方法，它通过将成本图数据的处理分离到语义上分离的层来工作。每一层跟踪一种类型的障碍或约束，然后修改用于路径规划的主成本图。我们展示了该算法如何与开源ROS导航堆栈集成，以及我们的方法如何比现有的单一方法更容易微调到特定的环境上下文。我们的设计还在实际使用中实现了更快的路径规划，并展示了比原始架构更清晰的关注点分离。新算法还可以表示复杂的代价值，以便为广泛的上下文创建导航行为。

1.Introduction

导航算法在过去的几十年里变得越来越复杂。它们可以处理大量传感器数据，以非常精确地跟踪障碍物和自由空间的位置。与正确的路径规划器相结合，他们可以非常熟练地引导机器人绕过环境。然而，许多这些导航算法都面临着同样的问题:算法优化基于寻找有效无碰撞路径的单一约束。
这种算法适用于许多用例，或者抽象的导航，如果所有重要的是从A点到b点，它对其他用例来说是不够的。
对于在人多的动态环境中运动的机器人，需要将更复杂的约束条件集成到优化中。从一个点移动到另一个点只是一个更大背景的一部分。机器人仅仅为了避免碰撞而绕过障碍物是不够的;机器人必须以不同的方式对待障碍，因为它是语义上的。例如，在大多数情况下，远离桌子几厘米是完全没问题的。然而，开车离一个人那么近在社会上是不受欢迎的。然而，如果导航算法平等地对待所有感知到的障碍，路径规划器就无法选择其中一条路径。

除了尊重人们的个人空间之外，还有许多其他情况，在这些情况下，选择最短的无碰撞路径可能不是最佳选择。考虑到人们经常在哪里的信息，避开可能的障碍的较长的路径可能是可取的。机器人还必须考虑进入潜在危险区域的效用，比如厨房，这是有效的路径，但需要付出代价。即使是像在走廊的右侧行驶这样简单的因素也需要考虑。机器人选择哪条路径将取决于对更大环境的额外信息。
关于路径规划器使用的环境的信息存储在成本映射中。在传统的成本图中，所有的数据都存储在单一的网格值中，我们称之为整体成本图。单片成本映射一直是流行的技术，因为它很简单，因为只有一个地方可以读取值和写入值。这样做的一个结果是丢失了关于成本映射中值的大量语义信息，这使得在周期之间正确维护成本映射变得更加困难.
在本文中，我们介绍了一种将额外的语义信息合并到成本图中的解决方案，该解决方案采用了一种称为分层成本图的新方法。以ROS导航框架为起点，我们展示了分层成本图复制了以前的功能
导航算法，同时增加了处理更多上下文的灵活性。图1显示了具有不同类型层的分层成本图的可能配置。我们将讨论算法和数据结构，以及它们在哪里改进了以前的方法。然后，我们将研究可以添加到成本图中的不同层，包括旧层和新层，以及它们整合的环境背景。

2. RELATED WORK

这项工作的重点是用于路径规划的基于网格的表示。现代成本图的直接祖先是占用网格，由Moravec等人在20世纪80年代在CMU开发[1,2]。所包含值的语义是直接的;每个单元格的值是存在障碍的概率，因此更新过程是贝叶斯规则的直接应用。Konolige[3]和Thrun[4]改进了概率模型，可以更好地定位障碍物。基于网格的成本图方法(网格值不是概率而是成本)已被证明是有用的，特别是当障碍物的位置更容易确定时(即不使用声纳)。在过去，这些方法主要关注二进制成本映射，其中单元要么被占用，要么是空闲空间。现在，更多复杂的成本被添加到成本映射中，导致成本映射中值的语义更加复杂。这些非致命成本的价值介于被占领和自由之间，通常代表软约束。
自动驾驶汽车使用这些值来优化行驶在正确的街道一侧和其他优先驾驶行为[5]。Gerkey和Agrawal[6]在成本图中表示不同类型的地形及其可穿越性和不同的成本。软约束也用于基于人机交互的约束。Sisbot等人的成本图系统考虑了人们的个人空间和视野，Kirby等人也模拟了社会行为，比如靠右通行。Svenstrup等人(bb1)和Scandolo和Fraichard等人(bb0)开发了更复杂的人为导航成本方法。

3. 整体成本图

整体成本图将所有数据存储在单个值网格中，构成了大多数成本图实现的基础，包括无处不在的ROS[11]导航堆栈。在本文中，我们重点研究ROS导航算法及其实现，因为它是一种广泛使用并运行在数十种机器人硬件上的算法1。它使用一个整体成本图进行全局规划，另一个用于局部规划。
单片成本图已被证明在计算最小长度无碰撞路径方面是有效的。将初始值写入成本映射是直接的，但是由于存储空间有限，更新过程存在问题，限制了可实现功能的类型，并影响了成本映射的效率和可扩展性。单片成本图方法的主要缺点如下。
1)更新步骤中的信息有限:
单片成本图的一个主要限制是成本图中的大部分信息存储在一个位置。考虑一个相对简单的例子，即传感器数据与全局成本图中已有的值之间存在冲突。传感器数据表明某一区域是畅通的，而成本图则表明有障碍物。更新成本映射的正确方法取决于数据的来源和其他语义信息。一种情况可能是，先前的值表示现在已经移动的人以前的职位。那么，正确的行为可能是用新的自由值覆盖costmap中的致死值，允许机器人通过新腾出的空间。然而，一个同样有效的场景是，成本图中的值来自静态图，创建静态图是为了包含传感器无法看到的障碍物，例如玻璃墙。在这种情况下，致死值应该留在成本图中。
在单一成本图中区分这两种情况是不可能的，因为它们在成本图中都表现为致命值。一旦成本图中的值被简化为成本图中的单个值，就会从数据中剥离成本图中值所表示的任何语义数据。
这对于正确处理三维障碍物数据也是有问题的。ROS实现的最初开发人员在使用像倾斜激光测距仪这样的三维传感器时遇到了这个问题。如果障碍物数据仅存储在单片成本图中，则可能通过清除观测值来不适当地移除不同高度的障碍物。因此，他们引入体素网格来跟踪额外的信息[12]。该解决方案适用于在这个用例中扩展单片成本映射的功能，但不能泛化。
随着成本图的数据源和类型的增加，有限的信息变得更加成问题。
考虑当存在多个非致命数据源时，每个数据源都具有单独的语义含义。如果在整体成本映射中将这些值加在一起，则对其中一个值的更改将导致需要重新计算每个单独的值。
2)固定更新区域:
单片成本图中缺乏语义信息也使得很难判断任何特定成本值在成本图中存在了多长时间。因此，如果更新的区域需要后处理或要发布到某些外部源，则没有既定的方法来确定最近更新的范围。处理此问题的一种无效方法是保守地估计覆盖整个可能被更新的区域的地图区域，这就是ROS实现所做的。在实践中，这意味着更新机器人周围大约6米× 6米的正方形，而不管实际更新了多少空间。（费时间，慢）
3） 特设更新过程：
缺乏维护和更新成本图的既定范例，导致实施采用特设方法。由于实践中使用的数据源数量相对较少，该方法迄今为止一直有效，但随着数据源数量的增加，该方法变得不可行。为了确保数据以正确的方式组合，每个数据源都需要了解其他数据源。
即使在之前的工作中定义了计算成本的有用算法，他们实际使用的将它们与完整的成本图集成的过程通常也是不透明的。如果没有关于成本图如何精确更新的信息，就不可能精确地复制结果。
4)语义固定解释:
除了限制成本图所包含的信息量外，整体成本图还限制了可以使用的信息类型。单片成本映射也只提供对成本映射中的值的单一解释。成本图的原始占用网格定义使用概率解释。或者，该值可以表示在某个位置的一些成本/惩罚。对于单片成本图，如何将概率数据源与基于成本的数据源结合起来是不明确的。
ROS成本图的实现存在额外的问题，因为它接受的唯一类型的信息是二进制障碍数据，即存在明确的障碍或明确的自由空间。增加非致命性成本并不符合其整体框架。由于成本映射中只有一种数据类型，因此信息在语义上是固定的。

4.分层成本图

A.数据结构和更新算法

为了抵消上一节介绍的限制，我们设计了分层成本图。数据结构仍然包含用于路径规划的二维代价网格。关键的区别在于如何填充这个主成本映射的值。分层的成本映射不是直接在网格中存储数据，而是维护一个有序的层列表，每个层都跟踪与特定功能相关的数据。然后将每一层的数据累积到主成本映射中，主成本映射需要两次遍历有序的层列表。
在第一次传递(updateBounds方法)中，对每个层进行轮询，以确定需要更新多少成本映射。这些层按顺序迭代，为每一层提供前一层需要更新的边界框(最初是一个空框)。每一层都可以根据需要展开边界框。第一次传递将产生一个边界框，该边界框确定需要更新多少主成本映射。在第二次传递期间，调用updateValues方法，在此期间，每个连续的层将更新主成本映射的边界框区域内的值。图2说明了使用一组复制基本单片成本图行为的层的更新算法。
有些层将维护它们自己版本的成本映射来缓存结果。这是数据结构维护数据语义信息的主要方式之一。例如，障碍物层保留与主成本图大小相同的私有成本图，以存储所有先前光线跟踪和标记步骤的结果。由于私有成本映射中的值只能被特定层访问，因此存储在其中的信息不会因为另一个数据源对其进行写入而丢失。这反过来又最小化了成本映射必须重新计算以前被覆盖的值的频率。
其他层不需要从一个周期到另一个周期保存那么多数据，并且会在每个回合用自己的数据更新主成本图，或者只是对其他层已经写入主成本图的数据进行操作。
图2中的示例显示了如何将前面用于生成成本图的特殊方法细化为一个整洁的、定义良好的流程。第六节更精确地说明了每一层如何改变总成本图。

图2 更新算法——在(a)中，分层成本图有三层和主成本图。障碍物和静态层维护自己的网格副本，而膨胀层则没有。为了更新成本映射，算法首先在每一层上调用updateBounds方法(b)，从有序列表中的第一层开始，如下所示。为了确定新的边界，障碍物层用新的传感器数据更新自己的成本图。结果是一个边界框，其中包含每个层需要更新的所有区域。接下来，每个层依次使用updateValues方法更新边界框中的主成本图，从静态层（c）开始，然后是障碍物层(d)和膨胀层(e)。

B.好处
分层成本图方法特别解决了单片成本图的局限性。
1)更新步骤更清晰:
在分层成本图方法中，不同类型的成本图信息被添加到不同的层中，使更新步骤更加清晰。
如果期望的行为包括处理静态障碍物、感应激光障碍物和感应声纳障碍物的能力，那么将这些障碍物存储在它们自己的层中可以大大简化簿记。每一层只需要与其他同类型的信息保持一致即可。
分层成本图还消除了竞争成本图信息源之间的争用。每一层只需要在该类型的新信息进入时更新。如果一个层基本上保持静态，则不需要在每次另一个层更新某个子区域时重新计算它。静态层只需要更新到主成本映射，更新可以移动到下一层。
这种更清晰的关注点分离还使成本映射的各个组件更容易调优。初始用户可以一次引入一个层，然后依次调试每个层。
2)动态更新区域:
与单片成本图中每轮更新都要更新的固定或未知区域不同，由于updateBounds在各层之间传递，分层成本图只更新各个层认为必要的地图区域。这为成本映射提供了额外的稳定性，保证只更新边界框内的值。此外，它还可以通过更新少量地图来提高效率。
3)有序更新过程:
与单片成本图中元素更新的未定义顺序不同，分层成本图具有明确的顺序。在我们的示例中，很明显，膨胀层将从障碍层和静态层中膨胀值，因为膨胀层位于有序列表中的其他两个层之后。此外，还显式地指定了层之间的交互。每个成本映射都可以配置为将前一个值和层的值组合为最大值、最小值或两者的其他一些数学函数。
4)灵活配置:
最后，也是最重要的，分层成本图方法的功能是无穷无尽的。实现与前一个实现相同的一组行为所需的层仅仅是个开始。机器人操作员想要多少层都可以添加到分层成本图中。其结果是，各个层可以实现任意复杂的逻辑来更新成本图，扩展成本图的语义可能性。每个层也可以有自己独立的数据表示，这样概率占用网格就可以存在于它们自己的层中，以及基于成本的层。

5.COMPARISONS

A. Implementation Specifics

虽然分层成本图的算法和数据结构是系统无关的，但由于平台的普遍性，我们专注于实现系统与ROS导航堆栈一起工作，以展示该方法的功能。分层的成本地图实现基本上保持了成本地图2d API的原样，并且像其他导航代码一样，是用c++实现的，每一层也是如此。
实现一个层非常简单。首先，必须创建一个扩展costmap 2d::Layer类的新类。这意味着实现initialize函数(其中层可以独立订阅ROS生态系统中的任何数据源)、updateBounds函数和updateCosts函数。通过简单的运行时参数更改，可以将独立编译的层插入到分层成本图中。
而以前的costmap类有特殊情况来处理是否有静态映射，或者是否在三维空间中跟踪障碍物，这些情况通过用不同的层配置全局和局部costmap来处理。
成本图的两种实现在Gazebo场景中进行了重复的模拟试验。所使用的机器人是PR2，在ROS导航[12]的初始基准测试中使用。在进行了数百次模拟后，我们发现两种执行方法所生成的路径在路径长度、完成时间和与障碍物的关系等方面并无明显差异。

B. Timing Comparison
我们分析的最关键的统计数据之一是成本图更新周期的平均运行时间。由于本地规划需要快速运行和调整以适应新的障碍，更新过程必须非常快。
我们希望达到的标准是更新频率至少为5 Hz(就像之前的实现所使用的那样)，将单个周期运行时间限制在0.2秒。单片成本映射能够超过一个或两个数量级，这取决于运行成本映射的环境和系统的具体情况。
分层成本图的实现也依赖于这些变量。最终，分层实现在某些场景下运行得更快，而在一些极端情况下运行得更慢。
在我们的模拟中，单片和分层实现的全局成本映射的平均更新时间分别为0.00166和0.00236秒，而对于本地成本映射，更新时间分别为0.00493和0.00463秒。使用单侧t检验，我们发现平均本地更新时间没有显著差异。使用分层成本图时，全局更新时间明显较慢(p<0.001)。然而，我们确定这是稀疏模拟环境的结果。在这些模拟中，机器人被放置在一个完全开放的环境中，除了机器人和目标之间有一个相对较小的障碍。因此，机器人的激光读数在大多数方向上延伸到最大距离，导致分层实现需要更新的大面积区域。

该区域需要每层更新，从而降低了整体更新速度。我们还在更混乱的环境中模拟了机器人，在这种环境中，机器人在设定的距离内四面被墙包围。由于墙壁离机器人非常近，更新区域要小得多。如图3所示，在这种情况下，分层成本图更快。随着更新区域的增长，单片实现的更新时间大致保持不变，而分层成本映射的更新时间随着要更新的单元数量的增加而增长。考虑到成本图系统是为在混乱、快速变化的环境中工作而设计的，分层成本图在这些环境中的速度更相关。
C. Navigating the Real World
除了全面的模拟测试外，我们还在真实环境中测试了PR2平台上的分层成本图。测试主要是在Willow Garage的办公环境中进行的。使用这些层来模拟单片成本图结构，PR2能够成功地复制以前实现的所有路径规划行为。然而，更令人兴奋的结果是，当我们修改这些层以获得单块成本图不可能实现的行为时。
首先，通过分离静态和障碍层，我们控制障碍层是否有能力覆盖静态地图。如第三节所述，单片成本地图导航可能不正确地清除部分静态地图，导致机器人规划穿过固体墙壁的路径。通过只允许障碍物层进行光线追踪并清除感知到的障碍物(而不是静态地图中的障碍物)，墙壁永远不会从主成本地图中清除，从而消除了令人尴尬的墙壁充电行为。
新层的引入也使以前不可能的新行为成为可能。我们对成本图的调查背后的激励用例是创建类似于第二节引用的工作的社会感知机器人导航。我们成功地将这样一个层集成到PR2的路径规划中。新层和我们创建的其他层的细节将在下一节中详细说明。

6. THE LAYERS

除了允许分层成本图复制其他成本图的功能之外，它的主要优点是能够轻松地集成其他层，这些层将以与成本图的其他元素相同的方式处理。这些额外的层使成本图能够表示来自许多不同上下文的信息，并生成对这些上下文做出适当反应的运动。
A. Standard Layers
静态地图层:
为了执行全局规划，机器人需要一个超越其传感器的地图，以了解墙壁和其他静态障碍物的位置。静态映射可以用SLAM算法先验地生成，也可以从架构图中创建。当层接收到地图时，updateBounds方法将需要返回一个覆盖整个地图的边界框。然而，在随后的迭代中，因为它毕竟是静态的，所以边界框的大小不会增加。在实践中，静态映射是全局成本映射的底层，因此它直接将其值复制到主成本映射中，因为在它之前没有其他层被写入主成本映射中。如果机器人在使用生成的地图进行导航时运行SLAM，则分层成本地图方法允许静态地图层更新而不会丢失其他层中的信息。在单块成本映射中，整个成本映射将被覆盖。
障碍物层:
这一层收集来自高精度传感器(如激光和RGB-D相机)的数据，并将其放置在二维网格中。传感器与传感器读数之间的空间被标记为空闲，传感器读数的位置被标记为已占用。在每个循环的updateBounds部分，新的传感器数据被放置到层的成本图中，并且边界框扩展以适应它。
将障碍层的值与成本图中已有的值结合起来的精确方法可能会有所不同，这取决于对传感器数据的期望信任程度。以前，默认行为是用传感器数据覆盖静态地图数据。这在静态地图可能不准确的情况下是最有效的，并且在分层方法中仍然可用。但是，如果静态地图更可信，则可以将该层配置为只向主成本地图添加致命障碍。
体素层:
此层与障碍物层具有相同的功能，但以三维方式跟踪传感器数据。MarderEppstein等人所引入的三维体素网格允许更智能地清除障碍物，以反映可以看到障碍物的多个高度。
膨胀层：
如前所述，充气过程在每个致命障碍物周围插入一个缓冲区。机器人肯定会发生碰撞的位置标有致命成本，而周围区域的非致命成本很小。这些值确保机器人不会与致命的障碍物碰撞，并且不喜欢靠得太近。updateBounds步骤增加上一个边界框，以确保新的致命障碍物将被充气，并且可以充气到边界框中的前一个边界外的旧致命障碍也膨胀了。updateValues步骤直接在主成本图上操作，而不存储本地副本。

B. New Functionality

声纳层:
单片成本图能够处理声纳数据，但分层成本图增加了如何处理它的选项。将其中一层用于声纳读数，可以避免玻璃墙被激光观测清除的问题。此外，我们还可以使用该层来处理声纳数据，而不是高精度障碍物层。我们构建的声纳层实现了一个概率声纳模型，并使用贝叶斯逻辑更新成本图。
然后，我们可以设置一个截止概率，我们只将相对确定的数据写入主成本图。注意，这种方法允许我们保持概率的语义含义，而不必直接将它们与成本结合起来。
警戒区层:
这一层使我们能够以比空闲/占用更详细的方式指定机器人环境的区域。尽管没有障碍物，但大多数机器人都希望避免进入通往楼下的楼梯井。或者，机器人永远不应该进入某个人的办公室。在许多情况下，尽管机器人看起来可以导航，但操作员会想要限制机器人可以安全驾驶的地方。针对这些限制的一种实践技术是在静态地图上标记障碍物。这种技术可以工作，但是会从地图中删除其他应用程序(如AMCL)可能需要的信息。这一层还为我们提供了标记区域的能力，这些区域不一定是禁止的，但并不理想。在厨房中增加非致命成本可以确保机器人在没有其他选择的情况下不会靠近危险液体。这些区域也可以用于不被社会接受的地方，比如人与他们正在互动的物体之间的空间，比如电视，就像弗格森和利哈乔夫的[5]一样。
幽闭恐怖层:
膨胀层在致命障碍物周围增加了一个小缓冲区，但幽闭恐怖层增加了一个更大的缓冲区，以增加靠近障碍物的相对成本。因此，机器人更倾向于在广阔的开放空间中移动，尽可能远离障碍物，从而最大化与任何感知到的障碍物的间隙。这一层对于机器人相对于障碍物的确切位置有更多不确定性的场景很有用，并且驾驶进入障碍物的几率或成本很高。

C. Human-Robot Interaction Layers
如第二节所见，在成本图中添加更复杂成本的主要动机之一是为了建模人机交互引入的约束。
邻近层:
人与机器人之间的空间关系，以及确保机器人不违反预期关系的方法的研究一直在稳步上升。最常见的方法是将高斯分布或多个高斯分布的混合物添加到成本图中，如Kirby等人所示。
这些调整在被检测到的人周围创造了一个区域，使更接近人们的路径成本更高，同时尊重他们的近距离关注。我们创建了一个邻近层，实现了Kirby的混合高斯模型。利用被检测人的位置和速度(从人的腿部激光扫描中提取)，该层将每个人的高斯值写入该层的私有成本图，然后将其添加到主成本图中。生成的值根据两个不同的参数，振幅和方差进行缩放。一般来说，当你增加这些参数时，最优路径会离人更远。然而，正如[13]中所讨论的，在最优路径变为最短路径之前，这些参数的变化幅度是有限的。这意味着调整参数试图让机器人走得更远，结果恰恰相反，这在社会上是次优的。[13]的结果在Gazebo模拟器中使用完全模拟的路径进行了复制。
我们也开始基于第二节中提到的更复杂的邻近模型来实现层。有些模型假设的信息比人的位置和方向更多，我们的层实现假设它们与足够强大的传感器功能配对，以检测诸如头部和身体姿势之类的东西。
走廊层:
在某些文化中，有走在小路右侧的习俗，就像许多国家的司机都走在路的右侧一样。
我们实现了一个层，它决定机器人是否在走廊上，并动态地增加走廊左侧的成本，使机器人更喜欢右边。我们在最近的用户研究[14]中使用了一个类似的模型，其中层改变了成本，使机器人更喜欢在走廊的对面导航，因为离它最近的人(通常是右边)。这一层的添加不仅可以有效地将机器人移动到走廊的一侧，而且还可以使人在交互过程中表现得更有效。
车辙层:
如果机器人的目标是避免社交入侵，并尽量减少意外障碍，一个有效的策略可能包括模仿人类的交通模式。这一层可以减少人们走过的路径的成本，从而使机器人也能选择最优路径。你也可以扭转成本的极性，在人们经常聚集的地区增加价值，以尽量减少社会混乱。

7. DISCUSSION

在本文中，我们讨论了新的分层成本图模型相对于以前的单片模型的好处。由于其效率和可扩展性，它的实现已被用作从Hydro开始的所有已发布版本的ROS的默认导航算法，其源代码可以在github.com/rosplanning/navigation上找到。所有附加层的代码都可以从wiki.ros.org/costmap2d链接到。此外，基于基于插件的层结构，我们希望创建额外成本图规则的新发展将作为层来实现，并在ROS导航框架内进行测试，从而允许更开放的算法行为交换和更容易进行的比较。
分层成本图和相关层为广泛的附加机器人行为提供了可能性。
随着更多的层被纳入规划算法，机器人将更加了解环境的不同方面，并在导航时考虑到这些背景。实践的当前状态只是忽略额外的上下文，或者一次处理一个上下文。虽然分层成本图确实可以集成上下文，但我们预测，未来的挑战将是找到一种动态管理层集合的方法，以确保在正确的时间对正确的上下文进行优先级排序。近距离行为决定了个人空间应该得到尊重，但机器人的路径究竟应该降低多少效率，这是一个悬而未决的问题。问题的一半是设计成本图层，使数学上最优的路径是期望的距离。
另一半则是一个没有明确答案的社会问题，即如何平衡机器人的需求与人类的需求。虽然我们不能提供这样一个问题的具体答案，但我们相信，拥有一个高度可定制的数据结构来定制机器人的行为将使回答这样一个问题变得容易得多。