原文地址：https://www.hrl.uni-bonn.de/papers/regier16iros.pdf
https://www.hrl.uni-bonn.de/HRL/publications
原文名称：Foresighted Navigation Through Cluttered Environments
摘要
在本文中，我们介绍了一种有效的机器人导航通过凌乱的室内环境。我们建议基于已经检测到的物体估计局部障碍物密度，并使用它们预测与机器人传感器尚未观测到的区域中的潜在障碍物相对应的穿越成本。通过考虑路径规划的预测成本，然后机器人能够以更具预见性的方式导航，并减少陷入混乱区域的风险。我们彻底评估我们的方法在模拟和真实世界的实验。实验结果表明，与不使用任何预测的标准方法相比，我们的方法能够使机器人有效地在包含杂乱区域的环境中导航，并且实现显著更短的完成时间。
简介
今天，移动机器人能够在实验室环境中定位和导航，这些环境整齐、静态，并且具有准确、最新的世界表示，例如平面图或预先记录的SLAM地图。然而，当机器人走出实验室时，他们面对的是一个不断变化的世界，充满了静态环境表象中未包含的成堆的物体和移动的障碍。特别地，在日常生活中在家庭和办公室环境中与人一起操作的服务机器人也需要通过杂乱的场景高效且鲁棒地导航，例如在玩具散落在地板上的儿童房间、工具铺设的车间、或在储藏室中。有堆垛的盒子。
通过杂波驱动是具有挑战性的，因为它需要准确地检测障碍物和精确的运动执行。机器人必须减速行驶，频繁转弯，当新的障碍物进入视野时重新规划一条无碰撞的路径，当它陷入死胡同时倒退，绕道而行，执行危险的机动，如向后行驶，并且当没有障碍物时甚至可能最终不得不放弃。能够找到一个合适的路径回到混乱的地区。因此，在给定的平面图上规划的最短路径不一定是最有效的路径，因为从一开始绕过杂乱区域可能更长，但执行起来更快、更安全。例如，考虑图1中描绘的场景。如果机器人走绿色的路径，它能够在不遇到障碍物的情况下快速稳定地向目标行驶，这比走红色的路径和潜在的卡住更有效。
如果世界是完全可观测的，并且杂波分布是已知的，那么机器人将能够通过推理安全距离、允许速度以及概率感测和运动执行误差来规划一条为效率而优化的路径。虽然解决这个问题已经具有挑战性，但当世界只能部分观察时，精确的规划是不可能的。从机器人的角度来看，由于传感器范围和遮挡的限制，杂波的真实分布对于机器人来说是未知的，因此它不能直接规划和优化规划的路径以提高效率。
传统的规划器只能考虑在给定环境地图中检测的对象，或者已经由传感器观测到的对象，因此这些规划器将向给定地图的自由空间中的所有未观测区域分配相等的遍历成本。然而，杂波通常不均匀传播。在儿童房里，玩具经常堆成一堆。在车间里，工具通常聚集在工作台周围。在储藏室里，箱子通常堆放在堆堆中。使用关于杂波分布的知识允许预测机器人尚未观测到的区域中的杂波区域，这使得机器人能够以更有远见的方式规划其路径。本文提出了一种根据已观测区域内物体信息预测视场外空间障碍物发生的方法。通过增加穿越预期障碍物的区域的成本，我们允许路径规划者避免混乱区域，这导致更有远见的导航。由于世界仅部分可观测，因此无法保证预测的障碍物实际存在，并且规划的路径在效率方面是最优的，但是如果有更容易和更安全的选择，则似乎有理由避免困难的导航挑战。
我们在机器人操作系统（ROS）中实现了我们的系统，并用Festo.actics公司的机器人机器人进行了仿真和实际实验。实验结果表明，该方法能够使机器人对意外目标做出有预见性的反应，并有效地进行导航。在与传统路径规划器的对比实验中，我们的方法在各种复杂场景中实现了显著缩短的完成时间。
相关工作
陆等。介绍了分层成本映射的概念，并在ROS（1）中实现了它。作者建议不要维护单个costmap，而是将costmap的信息分成具有不同语义的几个层。每一层都代表不同类型的障碍物或约束，例如静态地图、警示区和机器人不应该穿透的人的个人空间。这一概念已被广泛使用，例如用于路径规划或为了表示勘探进度。Marder Eppstein等人。[2]建议利用成本图表示法来引导PR2机器人穿过包含不同形状和大小的障碍物的杂乱的现实办公室环境。虽然他们的导航系统试图导航，甚至通过困难的障碍物领域，我们的方法预测场景背后的最初检测到的障碍物，并考虑采取迂回从一开始，如果导航通过杂波不看好。霍龙等。〔3〕考虑包含中等数量杂乱物体的3D环境。重点是寻找无碰撞上体配置的机器人穿越紧密通道。对于类人机器人，杂乱的环境特别具有挑战性，因为除了导航任务之外，机器人还必须遵守平衡约束。因此，他们必须选择自己的立足点，当移动通过杂乱地区。他们和STASSE（4）以及迈尔等人。〔5〕针对观测到的障碍物位置，提出了适合该任务的解决方案。约翰等。[6]提出了一种利用非参数贝叶斯模型从大数据集学习物体精确几何排列的方法。作者表明，他们的无人监督的方法能够学习如何设置表。在相关的方法中，Sudulthet等人。〔7〕还对视觉场景中物体的数量及其空间关系进行了论证。虽然作者对寻找几何排列中的可再现模式感兴趣，但我们不能期望在我们的场景中可以学习到典型的结构。相反，我们提出了一种基于已经观测到的杂乱对象预测环境未观测部分的遍历成本的有效方法。
当在拥挤的环境中如行人区与人类并排导航时，机器人必须适应人类的习惯，例如，关于人群流动或人类的个人空间。亨利等。〔8〕提出了一种在这种情况下模仿人类行为的学习方法。城市环境是高度杂乱环境的另一个例子，这些环境为导航带来了特殊的挑战，例如遍历性估计、交通规则以及与路人的交互[9]。虽然这些方法侧重于预测所观测世界的状态在未来将如何演变，但我们的工作侧重于预测与迄今未观测到的环境中的杂乱部分相对应的遍历成本。在未来工作中，将两种正交预测方法结合起来，以实现对运动障碍物和静态障碍物的更好预测，将是令人感兴趣的。
在这里，我们专注于国内和办公环境。基于检测到的对象，我们预测对应于潜在的障碍在接近但尚未观察到的区域的成本。通过绕过可能太杂乱而无法让机器人轻易通过的区域，我们的机器人避免了被卡住，并有效地导航到目标。
杂乱环境下路径规划的成本图
我们假设在环境中包含静态障碍的二维栅格地图。当机器人在环境中移动时，只要先前未知的物体出现在机器人的视野中，它就通过将单元标记为占用的或空闲来不断更新地图。机器人使用这张地图来定位自身，并预测与尚未用传感器观测到的区域内潜在的杂乱物体相对应的成本。
对于路径规划，将成本值分配给每个网格单元，并且基于A*的规划器找到到机器人给定目标位置的成本最低的路径。在下面，我们首先描述一个考虑到所有障碍物的安全距离的标准成本函数。然后，我们引入了成本函数的扩展，还考虑了局部环境的杂波密度。
A.标准成本函数
标准成本函数将所有被占用单元的成本设置为无穷大，此外，被占用单元被安全距离膨胀，并且它们的相邻单元也被分配了无穷大的成本。这样，机器人对所有障碍物保持安全距离R。虽然这会导致无碰撞路径的计算，但它可能导致不希望的行为，因为从一个房间到另一个房间的最短路径在进入房间时常常靠近墙壁和门柱。虽然有些情况需要挤过狭窄的通道或靠近障碍物移动，但机器人通常应该保持一定的间隙。表示对开放空间的这种偏好的常用方法是向与膨胀的障碍物相邻的区域增加成本，膨胀的障碍物随着到最近的障碍物的距离的增长而指数地衰减。这种方法允许策划者在路径长度和障碍物间隙之间进行权衡。由于这个成本项随着距离的增加而迅速失去影响，因此它只需要对障碍物附近的单元进行计算，而对于更远的单元可以忽略它。将这些成本项组合在一起产生以下成本函数：
其中r是机器人的安全距离，d1，…，dn是到附近障碍物的距离，Cmax定义了最高的可遍历成本，k是控制成本衰减的常量比例因子。图2中的红色曲线示出了具有两个障碍物的示例环境中的该成本函数。
B.杂乱环境的成本函数
在方程（1）中描述的代价函数只考虑最近的障碍，而忽略了所有其他障碍。然而，由于在有限的空间中移动很困难，并且传感器视图经常被阻塞，因此具有多个障碍物相互靠近的区域对机器人来说更具挑战性。增加因子k将促使机器人与所有障碍物保持较大距离，而不管杂波有多大，从而导致绕单个独立物体不必要的长弯路。相比之下，我们建议引入反映每个网格单元的局部环境中的杂波量的附加成本项。
新的成本函数应满足这些要求：
- ·成本值应随着机器人附近物体的数目成指数增加，以便规划者避免出现杂乱区域。
- 成本函数应该具有与原始函数EQ（1）相同的值域。
- 当距离达到无穷大时，代价函数应该接近零。
- ·如果在机器人附近只有一个物体，则成本函数应与公式(1)中的原始成本函数一致。
下列成本函数满足这些要求：
其中Ei与上述方程（1）中使用的指数衰减函数相同。在这个公式中，我们将1加到EI中，使得在EI 0中的遥远对象在乘法中变成中立元素。乘法之后，我们必须再次减去1，使得当距离趋于无穷大时，代价函数接近0。
除了上面定义的要求之外，函数还具有以下属性：
- 产品可以按增量计算
当观察到新的对象J时，允许有效地实现。
- 与原始函数等式(1)不同，在机器人能够穿越的区域中，它是连续可微的，这是某些规划算法所要求的。与原始函数一样，自由空间和障碍物之间的边界是不可微的。
图2显示了在包含两个障碍的简单环境中，标准成本函数（红色）和我们的方法（蓝色）的比较。
杂波密度与成本预测
在上面的章节中定义的成本函数可以应用于确定机器人已经观察到的区域中的成本。然而，我们还希望预测与邻近区域中的潜在物体相对应的成本，这些潜在物体尚未在视野内或被其他物体遮挡。为此，我们根据已经观测到的物体来估计局部障碍物密度，并增加在尚未可见的邻近区域中的细胞的遍历成本。为了有效地计算目标密度，我们计算在预测区域内的每个单元ci的半径Rj圆内观察到的占位密度。如图3所示，我们对具有不同半径的M圆圈进行这种计算。然后通过对所有M个密度估计进行求和，求出预测的密度ρi，用指数因子加权，该指数因子随R减小，
其中Tj是细胞总数和OI，J是CI周围的被占据细胞的计数，在半径为Rj的圆内。然后将每个CI的成本预测值定义为
在必须选择α的情况下，提高导航难度的杂波密度导致预测成本接近CMAX。在未来，我们将扩大我们的框架，包括战略，了解其最佳价值。
实验
我们实现了我们的方法在ROS和评估它在模拟（GasebO（10））和在现实世界的实验。使用Festo.actics公司的机器人进行实际实验，机器人具有固定在安装塔上的ASUS Xtion Pro Live，用于障碍物检测，以及水平安装在地面上方用于定位的Sick-S300激光扫描仪。为了检测物体，机器人使用来自RGBD摄像机的数据来构建环境的点云。地面以上的所有点都被视为障碍物。不能由给定环境表示中包含的已知障碍物解释的点被分类为对应于杂乱的物体。在我们的实验中，预测区域位于2.5米到5米左右的机器人的前半部分。计算职业密度的圆半径为0.5 m、1 m、1.5 m。
A. 路径规划与弹道执行
我们使用基于网格的A*规划器使用SEC中描述的预测成本来找到全局规划。然后，机器人使用ROS中包含的默认本地规划器执行计划轨迹。该轨迹展开规划器对机器人的控制空间中的速度进行采样，在短时间内模拟和评估机器人的路径，并向机器人发送最高额定速度命令（详见[11]）。通过在机器人的控制空间中进行采样和评估，规划器能够使机器人的速度适应环境施加的约束。特别地，机器人在杂乱的区域会开得慢一些，因为它在转弯或避开障碍物时必须遵守加速度限制。
B.定量评价
在没有修改的标准技术和我们使用修改的成本函数和预测的方法中，我们在一系列具有不同障碍物密度的仿真实验中评估了杂波量对完成时间的影响。我们在23×8平方米的矩形区域内随机取样物体。杂波区域被自由空间包围，使机器人有可能绕它旋转。作为杂波对象，我们使用长度从0.3到1.2米的盒子（参见图6的示例地图）。为了比较，我们将杂波区域中障碍物密度Dc的参数定义为出现在一平方米区域内的物体的平均数目。该机器人的任务是导航到一个目标点，距离28米，在竞技场的另一端，与杂乱的区域之间。在这种场景中，机器人不能被困在杂波区域，因为总有一条通向目标的可能路径，机器人可以在现场旋转，并驱动它返回的路径，并且模拟器提供无误差的传感器测量。对于每个DC，我们创建了20个随机杂乱的环境与各种对象。对于每个生成的环境，我们在具有不同的起始位置和目标位置的两次运行中对结果进行平均，并评估达到目标的平均运行时间。图4示出了与杂波区域的密度有关的总行驶时间的平均值和95%置信区间。
对于极值Dc=0和Dc∞，两种方法都给出了相同的定义：如果没有杂波障碍，两种方法都会在不受阻碍的情况下选择最短路径，因此旅行时间的均值和方差都很低。随着杂波密度的增加，完成时间通常增加，因为机器人必须走更长的路径以避免障碍。如果杂波太密集，机器人无法穿越，那么这两种方法将围绕整个区域计划绕行，再次导致类似的完成时间。当杂波足够稀疏以便机器人驱动通过而足够密集以致于阻碍机器人时，即当Dc值在0.3到0.8之间时，这些方法的表现不同。在这种情况下，机器人必须决定是通过杂乱区域行驶还是绕道绕行。通过杂波行驶会导致较短的轨迹，但是机器人必须在障碍物附近行驶得更慢，并且驾驶到死胡同的危险和不得不回溯增加。结果表明，我们的方法找到了一个很好的权衡，并实现较短的完成时间在所有情况下。对于标记为（*）和（**）的密度，在0.05和0.001水平上，根据配对t检验，差异有统计学意义。对于低于0.5的密度，通过杂波驱动通常是有利的选择，而对于0.5以上的密度，绕过障碍物组绕行往往更快。对于0.5左右的密度，完成时间强烈地依赖于杂波的几何分布，因为死点和迷宫状结构经常发生，使得很难预测哪个路径将通向目标。因此，均值和方差都增加了这两种方法。虽然我们的方法仍然表现较好的平均，差异没有统计学意义，由于高方差。平均而言，在新的传感器读数之后更新成本图对于我们的方法需要10.8±4.0ms，而对于标准方法需要5.9±2.3ms。
为了评估单个组件的贡献，我们分别报告图5中仅使用Sec的成本函数的规划器的结果。III- B（Green），仅使用来自SEC的预测方法的规划者。IV（黄色），和规划者使用两者的组合（蓝色）。为了更好的比较，我们将时间标准化，使标准方法匹配100%。
成本函数在SEC中的主要作用。III- B是为了增加通过在障碍物之间的狭窄通道挤压的成本。这种导航操作会减慢机器人的速度，而且是有风险的，因为当机器人的规划者再也找不到有效路径时，感测误差可能导致碰撞或陷阱。标准的低杂波密度的方法，因为规划师将保持一个更大的距离，以成组的障碍。然而，与杂波预测相结合，它减少了完成时间。
总的来说，我们的成本函数和杂波预测的组合是最好的。在某些情况下，由于成本函数增加了机器人与障碍物组之间的间隙，导致轨迹稍长，因此仅预测性能略好于组合。
在下一节中，我们将根据示例性情况更详细地讨论各个部分的影响。
定性评价
图6示出了典型的仿真结果。机器人的任务是从房间的左侧到右侧进行导航。房间的中心充满了机器人地图中没有包含的杂乱物体。在描述的情况下，机器人已经走了大约一半的目标。没有修正成本函数和预测（top）的标准方法试图沿着最短路径到达目标，从而导致机器人通过密集的杂波。由于机器人在转弯和躲避障碍时必须遵守加速度限制，机器人必须减速。相比之下，我们的方法预测在障碍物后面可能存在更多的物体，这些物体在驾驶时逐渐出现在它的视野里。该预测和修改的成本函数增加了朝向杂波区域中心以及在对象之间的成本，促使规划者选择更长、但更安全的绕过障碍区域的迂回路线。即使我们的方法的行进距离是11%长，机器人在72秒之后到达目标29%。
由于我们的算法预测了机器人尚未观测到的区域中的杂波，因此一旦机器人穿过先前未知的区域并观察实际场景，就必须基于实际测量来更新预测。图6还显示了机器人如何清除对它已经观察到为自由空间的区域的预测，例如，在中心顶部区域。
图7示出了我们的系统的各个组件对样本映射的影响。标准方法失去了时间，因为它必须减慢接近障碍物。预测激励策划者绕过密集的杂波，但它仍然通过狭窄的通道。在组合的方法中，成本函数还防止机器人通过狭窄的通道进入杂乱区域，因此组合方法产生最短的完成时间。在图8中，我们的方法避开了地图中心的稠密区域，并且喜欢顶部不那么密集的区域。
由于无法保证预测障碍物存在，在这种情况下，杂波避免策略导致不必要的迂回。然而，平均而言，预测的优势胜过多余弯道的风险。
真实世界实验
我们在图9所示的环境中使用RooToNo机器人进行了真实世界的实验。环境被围墙包围，并包含分隔走廊和多个房间的附加墙。另外，我们在环境的一侧放置了几个障碍物。当机器人能够访问包含墙壁的地图时，它事先不知道杂乱物体的数量或分布。标准方法计算通过杂乱区域的完整路径，其显示在图9的左侧。虽然机器人成功地在杂波中导航，但是机器人需要减速几次，以改变方向，避免碰撞，导致总导航时间为45.5秒。起初，附近杂乱无章，在杂乱地区增加的预测成本立即导致走廊周围重新铺设道路。该路径在图9的右侧示出。虽然导致较长的路径，总行程时间只有34秒，因为机器人能够驾驶更快，因为在正确的走廊上的自由空间。
结论
本文提出了一种通过包含多个杂乱物体区域的环境进行有效导航的新方法。我们的方法预测由机器人的传感器尚未观测到的区域中的潜在障碍物导致的遍历成本。该预测基于已经检测到的物体的估计障碍密度。通过直接考虑路径规划的预测遍历成本，机器人有远见地导航，并避免区域可能太杂乱，机器人难以通过。
我们在ROS中实现了我们的系统，并将预测的遍历成本表示为用于路径规划的新的成本映射层。正如我们在带有深度传感器的轮式机器人的实验中演示的那样，在几种情况下，所得的导航行为明显优于仅考虑检测对象的标准路径规划器生成的导航行为。
REFERENCES
[1] D. Lu, D. Hershberger, and W. Smart, “Layered costmaps for contextsensitive navigation,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots & Systems (IROS), Sept 2014, pp. 709–715.
[2] E. Marder-Eppstein, E. Berger, T. Foote, B. Gerkey, and K. Konolige,“The office marathon: Robust navigation in an indoor office environment,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2010.
[3] A. Hornung, M. Phillips, E. G. Jones, M. Bennewitz, M. Likhachev, and S. Chitta, “Navigation in three-dimensional cluttered environments for mobile manipulation,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2012.
[4] A. Orthey and O. Stasse, “Towards reactive whole-body motion planning in cluttered environments by precomputing feasible motion spaces,” in Proc. of the IEEE/RAS Int. Conf. on Humanoid Robots (Humanoids), 2013.
[5] D. Maier, C. Lutz, and M. Bennewitz, “Integrated perception, mapping, and footstep planning for humanoid navigation among 3D obstacles,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots & Systems (IROS), 2013.
[6] D. Joho, G. Tipaldi, N. Engelhard, C. Stachniss, and W. Burgard, “Nonparametric Bayesian models for unsupervised scene analysis and reconstruction,” in Proc. of Robotics: Science and Systems (RSS), 2012.
[7] E. B. Sudderth, A. Torralba, W. T. Freeman, and A. S. Willsky, “Describing visual scenes using transformed objects and parts,” Int. Journal of Computer Vision, vol. 77, no. 1–3, pp. 291–330, 2008.
[8] P. Henry, C. Vollmer, B. Ferris, and D. Fox, “Learning to navigate through crowded environments,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2010.
[9] R. Kummerle, M. Ruhnke, B. Steder, C. Stachniss, and W. Burgard, “A ¨ navigation system for robots operating in crowded urban environments,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA),2013.
[10] N. Koenig and A. Howard, “Design and use paradigms for Gazebo, an open-source multi-robot simulator,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots & Systems (IROS), 2004.
[11] B. P. Gerkey and K. Konolige, “Planning and control in unstructured terrain,” in Proc. of the ICRA Workshop on Path Planning on Costmaps,2008