3d_navigation

https://github.com/ros-planning/3d_navigation

论文题目：Navigation in Three-Dimensional Cluttered Environments for Mobile Manipulation

摘要：杂乱环境中的无碰撞导航对于任何移动操作系统都是必不可少的。传统的导航系统依赖于从3D表示投影的2D栅格地图以获得效率。然而，这种方法防止了在投影3D配置在2D网格地图内发生冲突的情况下接近对象的导航，即使3D环境中实际上没有发生冲突。因此，当使用这种2D表示来规划移动操作机器人的路径时，能够解决的规划问题的数量是有限的，并且可能导致次优的机器人路径。本文提出了一种结合基于八叉树的三维世界表示和基于任意搜索的运动规划器求解三维路径规划的快速集成方法。我们的方法利用多层2D和3D表示的组合来提高规划速度，允许生成具有有界次优性的几乎实时的计划。我们在高度杂乱的环境中用两臂移动操作机器人PR2携带大物体给出了广泛的实验结果。使用我们的方法，机器人能够有效地规划和执行轨迹，同时运输对象，从而经常移动通过苛刻的，狭窄的通道。

简介：家庭辅助是个人机器人未来的一个重要领域。因此，在高度非结构化和动态的家庭环境中导航是实现任何高级任务的先决条件。目前最先进的导航方法使得室内的2D网格地图中的3维机器人基地能够快速规划和鲁棒执行[1]。在这些方法中，传感器数据可以是二维的，或者从3D向下投影到2D，并且投影足迹通常用于在2D网格地图上为机器人规划。

然而，这些方法通常不能产生运动到高度杂乱区域的目标位置或目标位置，其中机器人的2D投影在2D地图中处于冲突中——即使机器人的3D配置在3D世界中实际上是无冲突的。作为一个例子，图1显示了PR2试图拾取和移动一个洗衣筐离开桌子。它只能通过在桌子下面移动它的底部，用它的手臂在桌子上移动来完成这个任务。这样的配置是无法实现的，使用传统的导航规划技术。此外，当在2D地图中规划运动时，对于窄通道，可能找不到携带洗衣篮的PR2的路径。这是由于在2D地图中扩大的足迹需要考虑篮子和伸展的手臂。

在这种情况下，完全3D碰撞检查是一种可能的解决方案。然而，在具有多自由度机器人的典型室内环境中，这是昂贵的，并且会导致长的规划时间。使用更粗糙的环境表示可以加快碰撞检查。然而，缺乏高分辨率的信息可以防止机器人在杂波中达到目标。为了克服这些限制，我们提出了一个组合导航框架，它利用了完整的3D碰撞检查与机器人和环境的多层2D表示（图1）的结合。我们的方法开始与机器人增量构建3D占用地图在一个有效的八叉树为基础的表示。然后将该3D地图投影到多层2D表示中。基于机器人的当前配置及其可能持有的任何对象，自动计算不同层的对应投影足迹。

这种方法的优点是，当在任何层中都没有发现冲突时，可以避免在计划期间进行昂贵的3D冲突检查。我们应用全局规划器在任意时间点上基于分层表示的格子图上生成路径。本地计划者然后执行此路径并在执行期间验证它。

我们的小说贡献是多方面的。首先，我们提出了一个系统，能够高效的实时三维规划和导航。我们的方法特别适合于移动操作任务，例如，机器人需要在杂乱的环境中携带大型物体的任务。另一个重要贡献是将大规模室内环境的有效3D表示与快速实时运动规划相结合，在杂乱环境中使用噪声传感器数据操作的真实机器人上实现并验证。我们的方法为任意配置的不同形状的移动操作系统提供了有效的导航规划的第一个实现。我们通过在PR2移动操作机器人上的真实世界的实验来验证我们的方法。使用我们的方法，PR2能够在杂乱的室内环境中携带大的物体。它能够无碰撞、高效地导航，将物体传送到传统二维导航规划方法无法到达的部分工作空间。我们的框架基于并扩展了基于搜索的规划库（SBPL，[2]）的能力，可以作为开源软件1使用。

 

相关工作：凌乱环境中的导航是移动机器人研究的一个问题[1 ]，[3 ]，[4 ]，[5 ]。然而，大多数解决这个问题的方法都集中于导航机器人以投影2D表示世界运动的2D投影足迹。因此，它们不适合我们在杂乱环境中进行移动操作的激励问题，因为它们将拒绝机器人的任何配置，其中机器人的2D投影足迹与2D环境表示冲突。Hornung和Bennewitz[6]最近提出了一种有效的类人机器人导航方法，该方法结合了开放空间中粗略的2D路径规划和障碍物附近的详细脚步规划。这种技术允许寻找基于2D网格的计划失败的解决方案。Vahrenkamp等人在模拟厨房环境中使用延迟碰撞检查和扩大的机器人模型来快速进行基于RRT的运动规划。〔7〕。作者集中在一小部分环境中的操纵任务。他们后来扩展了基地的粗略运动规划方法，同时进一步增加了更接近目标的规划器粒度[8]。然而，他们的方法并没有覆盖长的导航计划，通过杂乱的环境。此外，随机规划方法通常产生非最优解，并严重依赖于平滑来缩短最终路径。Marder Eppstein等人。〔1〕使用环境的紧凑3D表示，用于长时间运行的导航演示。这种表示允许对动态障碍物进行快速更新，但是仍然限制系统只在预测的2D环境表示中操作。SARZ等人用推车操作系统推车。〔9〕但导航又被限制在推车和机器人的2D投影不碰撞的目标上。这是不允许的，例如，可以在表下移动和存储购物车的操作。

为了在三维环境中进行有效的碰撞检测，过去已经提出了几种分层方法，如定向包围盒树[10]或球面树分层[11]、[12]。它们都依赖于3D空间的空间细分。与这些方法相比，我们的冲突检查首先在多层2D地图中执行，并且我们只在需要时才使用完整的3D冲突检查。刘等人。〔13〕最近提出了一种非圆机器人配置空间中增量映射的更新技术。在预计算步骤中用机器人的离散形状卷积地图之后，碰撞检查对应于单个查找。虽然地图可以有效地针对动态环境更新，但是机器人的运动学配置(例如，在拾取对象之后)的任何改变都需要完全重新计算。

Pr2机器人平台：我们在PR2上实现了我们的系统，它是一个具有全方位基础、铰接式传感器头和两个7自由度臂的移动操作系统。在整个工作中使用的传感器是北洋UTM-30LX2D激光测距仪中的定位基地和立体传感器中的头部。立体相机由纹理投影仪增强以提供密集数据[14 ]。校准的联合编码器提供关于机器人的运动学配置的精确本体感受数据。立体声传感器作为检测环境中障碍物的主要传感输入，用于我们的方法。PR2的密集立体对具有窄的55视场。为了让导航变得实用，我们实现了一种主动传感行为，其中机器人的传感器头总是指向当前驱动方向（可能指向侧面，甚至向后）。我们的方法的感测管道建立在为桌面操作应用程序开发的现有组件（15）上。PR2的立体声头的密集深度测量首先通过自滤波器进行。基于已知的机器人网格模型和本体感知，去除机器人身体或被操作或携带的物体上的点。PR2上的立体声和本体感觉数据之间的精确时间同步确保了有效的自滤波。然后，阴影滤波器去除立体感测固有的所有误差：从立体声对的相机之一遮挡的点和在关节的尖锐边缘和角落的遮蔽点。最后，基于RANSAC的地面平面检测将剩余的点标记为地面或非地面，以便插入到接下来详细描述的基于八叉树的3D占用图中。

环境表征：移动操作需要能够高效准确地表示和处理一个相当大的环境。表示必须是紧凑的，易于增量更新，以解释动态障碍和变化场景。
八叉树占据网格：对于机器人和环境之间的碰撞检查，需要一个完整的3D表示。然而，如果使用高分辨率来获得准确的结果，则完整的3D占用网格对计算内存要求提出了挑战。因此，我们使用基于八叉树的体积表示OctoMap[16]来有效地建立和存储分辨率为2.5cm的概率3D占用网格。

持续的地图更新：为了增量地映射环境，机器人不断地将最新的传感器信息插入到3D地图中。从感知管道输出的点云是SEC中描述的自滤波。有点属于地面平面标记。地图更新是使用来自传感器原点的光线投射来完成的。因此需要了解地面点，以便清除相应的区域，并将其标记为可通行的区域。我们应用概率地图更新[16]来确保当障碍物出现或消失(例如，被操纵的对象或环境中移动的人)时传感器噪声和动态更新的正确处理。机器人首先构建环境的初始3D地图，作为基本表示，然后可以在执行任务时通过基于机器人的传感器数据的增量更新来修改该基本表示。所得到的3D地图是运动规划框架的输入，该框架进一步处理地图以创建环境的多层表示，以提高运动规划的效率。现在我们将进一步详细描述这个过程。

多层二维障碍地图：移动操作机器人（如PR2）的几何结构可以被开发用于通过空间细分为多个3D体进行有效的碰撞检查。这种细分特别适用于机器人在载运物体的同时导航，因为在这种任务中，其几何结构将基本保持不变。如图1所示，PR2机器人可以以这种方式细分为三个部分：基座、带头部的脊柱和带有可能附加物体的臂。此外，我们还使用多层2D表示以紧凑的方式表示环境。该表示的每一层包含障碍物的投影表示，这些障碍物可以与该层关联的身体部分碰撞。因此，我们代表PR2的三个层的环境，每个层与上面提到的身体部分中的一个相关联。每个增量3D地图更新也更新这些向下投影的2D地图。通过在对应于层的最小高度和最大高度的范围内遍历所有离散层zi，针对层lj中的给定地图单元(x；y)更新2D投影。例如，对于用于机器人手臂的层，高度范围由手臂上的最高点和最低点以及潜在的附着对象确定。如果在3D地图中所有的Li，（x；y；Zi）是自由的，则（x；y）被标记为自由的。当一个（x；y；Zi）被占据时，（x；y）被认为是占据的。否则，（x；y）被标记为未知空间。因此，2D地图层中的被占用单元代表3D中可能的障碍。因此，自由单元保证在与该层对应的3D空间中不存在冲突。除了投影的2D地图之外，我们还具有对应于该层的机器人向下投影的足迹。每次请求一个计划，我们首先通过向下投影属于这个层的身体部分的网格的凸壳来更新每个层的2D足迹（参见图2）。

PR2机器人可分为以下几层（见图1）：箱形底座，从底座到头部的脊椎，以及带有要携带或操纵的附加物体的臂。虽然基座和脊椎层对于该机器人保持固定，但是臂部可以随着手臂移动而改变其高度和脚印。图2显示了三个图层的例子，即有足迹的投影栅格地图。这种表示是从PR2机器人的真实场景生成的，如图2（d）所示。显然，与脊柱层相比，手臂层具有非常少的障碍，因为手臂处于比环境中的大多数障碍（例如，椅子和桌子）更大的高度。以这种方式使用多个2D层允许运动规划人员避免昂贵的3D碰撞检查。

在图3中示出了一个例子，其中机器人的手臂在桌子上方，底座在桌子下面。当使用具有2D投影足迹的单个2D地图时，向下投影的表将与向下投影的臂碰撞。需要一个完整的3D碰撞检查来确认碰撞是否真的发生。然而，通过我们的方法，桌子只投射在脊椎层上（它不会与脊椎层碰撞），并且手臂和基础的足迹不会与之比较。因为没有一个脚印碰撞，所以不需要昂贵的全3D检查。因此，使用多层允许我们更频繁地声明机器人配置无冲突，而无需使用昂贵的3D冲突检查。但是我们也可以确定机器人确实处于3D碰撞中，而不必进行明确的3D碰撞检查。下面将对此进行解释。一些层具有这样的属性，对于相应足迹中的每个(x；y)单元以及层中的每个zi，(x；y；zi)是机器人的一部分。这适用于例如Pr2和高大脊柱的盒状基底。在这些情况下，如果障碍物的2D投影在机器人在适当层的足迹内，则无论z坐标如何，机器人在3D中肯定与物体发生碰撞，并且不需要3D检查。然而，对于某些层，例如臂层，由于它们不是盒状且经常改变形状，因此不能使用此属性。在某些情况下，我们仍然可以通过引入高障碍的概念来消除3D检查。如果层内的所有（X；Y；ZI）细胞被占据，则地图细胞（X；Y）被标记为高的障碍物。现在，当该层的2D占用空间与该单元格冲突时，3D配置将始终处于冲突中，并且不需要完整的3D冲突检查。请注意，我们应用我们的多层2D障碍地图排除全3D碰撞检查，而不是完全取代它们。为了保持任意机器人配置的完全灵活性，可能仍然需要针对3D碰撞测试其运动学配置。必要时，使用ODE仿真包[17]中的冲突模型表示来执行完整的3D冲突检查。

规划导航框架：3D地图和多层2D障碍地图作为输入到我们的规划和导航框架。我们应用全局规划器在位置和平面取向空间（x；y；thy）中构造一个计划以达到目标。这个计划然后由当地规划者执行。为了定位导航过程中的机器人，我们目前依赖于基于激光数据的2D蒙特卡罗定位。这种定位使用静态地图，其中包含墙壁和环境的其他静态部分，结合来自车轮和位于机器人上的IMU的里程计信息。

全局定位：基于搜索的格子图规划 ARA*

执行结果：

在执行过程中，来自全局规划器的级联的离散运动原语必须转换为机器人基地的电机命令。此外，计划的有效性必须在执行时检查，因为障碍位置可能会改变。为此，局部规划器计算沿着路径到达下一个(x；y；θ)姿态所需的全向速度，并执行单个轨迹展开，在更新的障碍地图中检查是否有碰撞。如在全球规划师中，碰撞检查首先在2D中执行，并且仅在需要时在3D中进行。在预测到碰撞的情况下，本地规划器首先尝试减小速度命令，以尽可能接近目标姿态。例如，当与桌子对接时，完全离散的前向运动可能与桌子碰撞，而较短、较慢的接近运动则更接近桌子。如果环境已经改变或者机器人已经偏离其路径，则机器人停止并且再次调用全局规划器以找到绕过障碍物的新路径。

实验。我们进行了大量的实验来评估我们的方法，并证明其对于实时移动操作任务的可用性。

A离线运动规划测试

第一组实验被设计为显示我们的方法与传统技术相比的优越性能。我们产生了多个规划问题，其中机器人被配置在具有扩展臂的构型中用于操作。这种配置需要使用一个大的2D投影足迹用于传统的运动规划。我们评估了以下三种不同的方法：

1)单层2D：传统的导航方法，只使用机器人的一个2D投影足迹和2D投影环境地图。

2)单层3D：一种导航方法，其中，当(单个)2D足迹与2D投影地图冲突时，总是执行3D冲突检查。

3)多层3D：我们的方法使用多层2D占用网格，只有在绝对必要时才执行3D冲突检查。

我们在两个不同的场景中进行了实验。第一种情况在图6中示出。它包括两把椅子和一张桌子之间的狭窄通道，机器人只能靠在桌子底下侧向移动才能通过。对于第二种情况，椅子被移除以使规划问题变得更容易。注意，环境是一个真正的杂乱的办公空间，用于生成环境表示的所有传感器数据都来自一个真正的PR2机器人。

手动选择六个不同的状态（图6），并在这些状态的开始和目标的每个组合之间进行规划。这导致总共60个不同的规划问题在这两种情况下。规划始于“＝10，允许继续”直到1分钟或5分钟过去。在图5中示出了每个规划方法的成功率。注意，图6中只有两个配置具有没有冲突的2D足迹，这使得传统的单层2D方法仅能在四种情况下获得成功。

我们的方法（多层3D）总是成功地生成运动计划，即使在最混乱的场景中也是如此。它比单层3D方法性能要好得多，并且在短时间内和长期内都能找到更多的解决方案（参见图5）。对于我们的方法，第一个解决方案总是在最多7.3秒内找到。表I比较了我们的方法和单层3D在32次规划尝试中的成功之处。从单层2D的数据被省略，因为它只能在两种配置之间进行规划。结果表明，使用多层表示可以显著提高运动规划器的性能，使其能够快速找到初始解。将解细化为完全最优解（“=1:0”）需要花费更多的时间，但在实践中，次优解已经导致机器人的有效导航行为。表II显示了从更难的28个计划尝试中聚集的数据，我们的方法（多层3D）成功了。在表I和II中，2D和3D碰撞检查的数量是指必须检查的运动原语的数量。运动原语除了起始状态和结束状态之外还可以具有多个中间位置。我们的每个原语可能需要多达10个碰撞检查个别机器人的位置。每个运动原语的冲突检查的实际数量可能更少，因为我们在一个中间位置发生冲突时立即中止。用我们的方法检查3D碰撞的运动原语的数目非常少，因为具有多层使得膨胀（当机器人接近障碍物时，它惩罚了成本函数）在保持搜索远离障碍物和3D方面更有信息。碰撞检查。另外，从图2中可以看出，在机器人的臂层中很少有障碍物不被分类为高障碍。然而，随着规划师接近“1.0”，即使我们的方法最终不得不做更多的3D碰撞检查。这就是为什么，平均来说，在一些更困难的情况下，我们只能在5秒内达到“6.28”，而在许多情况下，在达到最佳解决方案之前，需要10,000或100,000次碰撞检查。

B对接机器人与真实机器人

第二组实验包括在类似的杂乱的办公室环境中，使用真正的PR2机器人重复对接和解锁桌子。这是机器人在需要拾取或放置物体在桌子中央时必须执行的动作。注意，使用传统的2D导航方法无法实现需要机器人到达桌子中间的配置。我们评估了这组实验，只为我们的方法来运动规划与多层表示。

从图7中可以看到从表中拾取大对象的典型对接操作。在我们的实验中，所有目标配置达到无碰撞，而机器人对接和解除对接表12次。

C搬运大型物体时的导航

最后，我们在一组实验上评估了我们的方法，该实验针对一个困难的场景，涉及在极其杂乱的环境中自主导航，同时执行一个目标图7。与一个桌子对接，拿起一个大对象。左：目标配置（阴影绿色）和由此产生的计划（蓝色）。在这种情况下，机器人经常不得不穿过狭窄的区域，迫使它向障碍物的侧面移动。洗衣篮的位置是预先提供给机器人的。机器人首先计划在篮前的一个球门的路径。然后用一个预定义的手臂动作抬起篮子。使用包围柱从机器人的传感器数据中滤出篮子。机器人最后的导航目标是在另一张桌子旁边，它必须再次放下篮子。我们使用多个组合从六个目标和开始状态的集合进行多次试验。机器人成功地完成了每一项任务。图8示出了执行此任务时的一系列快照，而图9中示出了完整的计划路径。在这个场景中，足迹的预计算时间是3.46s，而完整的计划时间是3.16s。为了捡起洗衣筐，机器人最初不得不把它的底座移到桌子下面。后来，狭窄的通道用椅子迫使它在桌子的旁边移动，篮子在桌子上。最后，PR2走近第二张桌子放下篮子。注意，在此场景中，机器人使用传统的2D导航规划方法无法到达工作空间的大部分。

结论与未来工作

本文提出了一种利用移动操作机器人在办公室或家庭等三维、杂乱环境中进行有效导航的新方法。我们的集成方法结合了一个有效的八叉树为基础的3D表示和任何时候基于搜索的运动规划。为了在规划期间进行有效的冲突检查，我们的方法利用多层2D环境表示，允许生成几乎实时的有界次优计划。我们展示了我们的框架在PR2移动操作机器人在详尽的真实世界实验中的性能。机器人能够高效且无碰撞地导航，将两只胳膊的洗衣篮从一张桌子带到另一张桌子，穿过一间杂乱不堪的房间。机器人还成功地在桌子上用伸出的手臂执行对接和解锁操作，从而通过移动桌子下面的底座来最大化其操作空间。我们的方法平均在2秒内找到初始解决方案，而初始解决方案的最高规划时间约为7秒。这强调了我们的方法在移动操作场景中导航的效率和实用性。未来工作的自然扩展是将较大的物体（如带有铰接基元的椅子或手推车）在杂乱的环境中移动，例如将它们存储在桌子下面。

 

REFERENCES
[1] E. Marder-Eppstein, E. Berger, T. Foote, B. P. 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.
[2] M. Likhachev, http://www.ros.org/wiki/sbpl, 2010.
[3] S. Thrun, M. Bennewitz, W. Burgard, A. B. Cremers, F. Dellaert,D. Fox, D. Hahnel, C. Rosenberg, N. Roy, J. Schulte, and D. Schulz, ¨“Minerva: A second-generation museum tour-guide robot,” in Proc. Of the IEEE Int. Conf. on Robotics & Automation (ICRA), 1999.
[4] I. R. Nourbakhsh, C. Kunz, and T. Willeke, “The mobot museum robot installations: A five year experiment,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2003.
[5] R. Kummerle, D. H ¨ ahnel, D. Dolgov, S. Thrun, and W. Burgard, ¨ “Autonomous driving in a multi-level parking structure,” in Proc. Of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2009.
[6] A. Hornung and M. Bennewitz, “Adaptive level-of-detail planning for efficient humanoid navigation,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2012.
[7] N. Vahrenkamp, T. Asfour, and R. Dillmann, “Efficient motion planning for humanoid robots using lazy collision checking and enlarged robot models,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2007.
[8] N. Vahrenkamp, C. Scheurer, T. Asfour, J. J. Kuffner, and R. Dillmann, “Adaptive motion planning for humanoid robots,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2008.
[9] J. Scholz, S. Chitta, B. Marthi, and M. Likhachev, “Cart pushing with a mobile manipulation system: Towards navigation with moveable objects,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), Shanghai, China, 2011.
[10] S. Gottschalk, M. C. Lin, and D. Manocha, “OBBTree: A hierarchical structure for rapid interference detection,” in Proc. of ACM SIGGRAPH, 1996.
[11] K. Steinbach, J. Kuffner, T. Asfour, and R. Dillmann, “Collision and self-collision detection for humanoids based on sphere tree hierarchies.” in Proc. of the IEEE-RAS Int. Conf. on Humanoid Robots (Humanoids), 2006.
[12] S. Quinlan, “Efficient distance computation between non-convex objects,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 1994.
[13] B. Lau, C. Sprunk, and W. Burgard, “Incremental updates of configuration space representations for non-circular mobile robots with 2d, 2.5d, or 3d obstacle models.” in Proc. of the European Conf. on Mobile Robots (ECMR), 2011.
[14] K. Konolige, “Projected texture stereo,” in Proc. of the IEEE Int. Conf. on Robotics & Automation (ICRA), 2010.
[15] M. Ciocarlie, K. Hsiao, E. G. Jones, S. Chitta, R. B. Rusu, and I. A. Sucan, “Towards reliable grasping and manipulation in household environments,” in ISER, New Delhi, India, December 2010.
[16] K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems,” in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and Modeling for Mobile Manipulation, 2010, software available at http://octomap.sf.net/.
[17] “Open dynamics engine,” http://www.ode.org.
[18] M. Likhachev and D. Ferguson, “Planning long dynamically-feasible maneuvers for autonomous vehicles,” in Int. Journal of Robotics Research (IJRR), 2009.
[19] M. Pivtoraiko and A. Kelly, “Generating near minimal spanning control sets for constrained motion planning in discrete state spaces,” in Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), 2005.
[20] M. Likhachev, G. Gordon, and S. Thrun, “ARA*: Anytime A* search with provable bounds on sub-optimality,” in Proc. of the Conf. on Neural Information Processing Systems (NIPS), 2003.