摘要
许多导航系统，包括无处不在的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等人所引入的三维体素网格允许更智能地清除障碍物，以反映可以看到障碍物的多个高度
