NAV2：ROS2 导航系统设计、架构与使用

最新推荐文章于 2025-07-31 21:17:56 发布

kuan_li_lyg

最新推荐文章于 2025-07-31 21:17:56 发布

阅读量712

点赞数 7

CC 4.0 BY-SA版权

文章标签：架构

本文链接：https://blog.csdn.net/weixin_46300916/article/details/147015477

系列文章目录

前言

移动机器人导航技术的发展使机器人能够在仓库、零售店和人行道上围绕行人进行操作。人们提出了各种导航解决方案，但很少有像 ROS（机器人操作系统）导航那样被广泛采用的。然而，ROS 导航系统未能跟上现代趋势。我们提出了新的导航解决方案--Navigation2，它建立在 ROS 导航的成功传统之上。Navigation2 使用行为树来协调导航任务，并采用了专为动态环境设计的新方法，适用于更广泛的现代传感器。它建立在 ROS2 的基础上，ROS2 是一个安全的消息传递框架，适用于安全关键型应用和程序生命周期管理。我们介绍了在校园环境中利用Navigation2与学生一起安全地进行马拉松比赛的实验，这是对Eppstein等人[1]提出的实验的扩展。Navigation2系统可在https://github.com/ros-planning/navigation2 免费获取，并有丰富的社区和说明。

一、导言

自第一台服务机器人问世以来，人们提出了许多移动机器人导航框架和系统[2][3]。这些框架为今天在工厂、零售店和人行道上推出的服务机器人奠定了基础。虽然提出的框架很多，但很少有框架能与 ROS 导航栈对不断发展的移动机器人行业的影响相媲美。

事实上，这个开源项目对公司、政府和研究人员都起到了推动作用。截至 2020 年 2 月，该项目已被引用 400 多次，谷歌学术（Google Scholar）收录的 1000 多篇论文都提到了它，GitHub 上有 1200 个分叉。该系统于 2010 年提出，提供了一个丰富且可配置的导航系统，经过重新配置后可用于各种机器人平台[1]。它提供了一个参考框架和一套基础算法实现，包括用于规划和控制的 A* 算法[4]和动态窗口法（DWA）[5]。然而，导航在高动态环境中存在感知和控制器问题，并且缺乏通用的状态估计器。此外，ROS1 在安全性和性能方面也有欠缺，因此不适合安全关键型应用。

在这项工作中，我们研究了当前的趋势和机器人系统，利用我们与流行机器人框架合作的集体经验，创建了一个新的导航系统。这项工作希望在 “导航 ”系统成功的基础上再接再厉，同时在更复杂的环境中支持更广泛的机器人形状和运动类型。

在本文中，我们提出了一个全新的、完全开源的导航系统--Navigation2，该系统在结构和算法上都进行了大量更新。Navigation2 使用可配置的行为树来协调规划、控制和恢复任务 [6]。其联合模型的结构是，每个行为树节点都会调用一个远程服务器，通过不断增加的算法实现来计算上述任务之一。每个服务器都实现了一个标准插件接口，以便在运行时轻松创建和选择新的算法或技术。

这种架构利用了多核处理器，并充分利用了 ROS2 的实时、低延迟功能[7]。ROS2 经过重新设计，具有工业级的安全性、可靠性和实时性，可用于下一代机器人。ROS2 还引入了管理节点（Managed Nodes）的概念，即生命周期状态可控的服务器，我们利用这一概念为系统中的每台服务器创建确定性行为。

我们提出的算法刷新侧重于模块化和在动态环境中平稳运行。这包括时空体素层（STVL）[8]、分层成本图[9]、定时弹性带（TEB）控制器[10]，以及用于状态估计的多传感器融合框架--机器人定位[11]。每种方法都支持全自主和非全自主机器人类型。然而，Navigation2 是专门设计的，所有组件均可在运行时配置，而且在许多情况下，已有多种备选算法可供使用。我们的导航系统是首个使用 ROS2 构建的同类系统，它能在高度动态的校园环境中安全运行。通过长时间的实验，我们发现该系统可以通过图 1 所示的 Tiago 和 RB-1 基座，在人流密集的区域稳健地导航，其距离超过马拉松比赛（总计 37.4 英里）。

二、相关工作

A. 导航系统

Dervish 是最早在办公环境中实施有效导航系统的机器人之一 [14]。它利用声纳环对环境进行粗略的概率表示，在办公室送货机器人竞赛的走廊场景中进行导航。不久之后，[2] 和 [15] 又推出了博物馆机器人 RHINO 和 MINERVA。这些机器人主要使用概率占位网格在环境中定位，并采用不同的规划方法。为了避开障碍物，使用了 DWA [5]。这些机器人依靠二维环境表示进行定位和导航，这可能会导致与平面外的障碍物发生碰撞。

[1]提出了一种导航系统和环境表示法，利用倾斜二维激光扫描仪的三维信息来导航环境。许多早期技术，如 A* 和 DWA，都用于规划和避障。随着时间的推移，倾斜式二维激光扫描仪已让位于随处可见的廉价三维深度相机和激光扫描仪。针对二维倾斜激光配置所显示的几何形状、低精度和效果，该方法的一些表述是专门设计的。此外，随着稀疏多光束激光扫描仪的引入，纯粹的光线投射方法已无法有效清除自由空间，尤其是在动态场景中。STVL 为众多、稀疏和长距离传感器提供了一种更具可扩展性的替代方法，可用于替代此类技术[8]。

2013 年提出了一项室外机器人导航实验[19]。这项工作的重点是制图、定位和简单的动态障碍物检测方法，用于在德国 7 公里长的街道上导航。我们的工作侧重于模块化导航框架，利用行为树和扩展实验构建多种类型的应用，以展示其成熟度。虽然我们在实验中没有直接考虑动态障碍物的检测，但通过启用特定用例检测器的 Navigation2，所有必要的接口都已存在。我们实施了一种优化的本地轨迹规划和三维环境建模技术，该技术在动态障碍物存在的情况下功能高效，无需明确检测，但如果有此类信息，则可对其进行增强。

B. 模型

行为树（BT）作为有限状态机（FSM）的替代品，已成功应用于机器人任务规划[20]。随着状态和转换的数量不断增加，将复杂行为和多步骤任务建模为 FSM 对于现实世界中的任务来说会变得非常复杂。[20]成功演示了使用行为树为足球比赛中的运动和射门战术建模，而使用 FSM 则难以实现。

行为树还被大量应用于机器人任务执行、机器人操纵和复杂的移动操纵器任务领域[6]。此外，行为树可以通过广泛的可重用基元轻松修改，并在运行时加载，无需编程逻辑。因此，我们采用行为树作为导航框架的主要结构。

三、导航 2 系统的设计

导航 2 系统的设计具有高度的可配置性和未来扩展性。虽然马拉松实验展示了导航 2 的一种配置，但我们工作的目的是支持各种类型的机器人（差分式、整体式、阿克曼式、腿式），以适应各种环境和应用。此外，这种支持不仅针对研究和教育，也针对生产机器人。设计考虑到了机器人产品的需求，包括安全性、保障性和确定性，同时又不失上述通用性。

A. 可靠性

对于在人类附近移动速度超过 2 米/秒的专业移动机器人来说，有许多安全和可靠性问题必须解决。Navigation2 是在实时元操作系统 ROS2 的基础上设计的，以满足功能安全标准和确定性要求。ROS2 是流行的机器人中间件 ROS 的第二代产品，基于数据分发服务（DDS）通信标准[7]。DDS 用于关键基础设施，包括飞机、导弹系统和金融系统。它从 DDS 标准中获得了同样严格的消息传递保证，使应用程序能够选择消息传递策略。ROS2 利用 DDS 的安全功能，允许用户在没有专用网络的情况下，在机器人内部和云服务器之间安全地传输信息。这些功能为工业级移动机器人创建了一个可行的通信框架。

此外，ROS2 还引入了管理节点（也称为生命周期节点）的概念。托管节点实现了一种服务器结构，从实例化到销毁都有清晰的状态转换[21]。该服务器在程序启动时创建，但会等待外部刺激，通过一个确定的调用过程进行转换。在关机或出错时，服务器通过状态机从激活状态过渡到最终状态。每个状态转换都有明确的责任，包括管理内存分配、网络接口和开始处理任务。Navigation2 中的所有服务器都使用托管节点进行确定性程序生命周期管理和内存分配。

B. 模块化和可重构

要创建一个能在多种环境下与各种机器人配合使用的导航系统，Navigation2 必须具有高度模块化的特点。同样，我们的设计方法也倾向于采用不仅模块化，而且易于在运行时重新配置和选择的解决方案。为实现这一目标，Navigation2 创建了两种设计模式：行为树导航器和特定任务异步服务器，如图 2 所示。每个特定任务服务器都是一个托管算法插件的 ROS2 节点，算法插件是在运行时动态加载的库。

行为树导航器使用行为树来协调导航任务；它激活并跟踪规划器、控制器和恢复服务器的导航进度。行为树插件使用 ROS2 动作接口调用规划器、控制器和恢复服务器。通过修改以 XML 格式存储的行为树，可以创建独特的导航行为。BT 可通过不同的控制流和条件节点类型重新配置，无需编程。更改导航逻辑只需创建一个新的行为树标记文件，并在运行时加载即可。我们利用 BT 行动节点状态和控制流节点创建了一个上下文恢复系统，特定服务器的故障将触发独特的响应。

利用 ROS2，导航器中的行为树节点可以调用其他处理器内核中长期运行的异步服务器。多核处理器的使用大大增加了导航系统可有效利用的计算资源。Navigation2 BT 导航器利用动态库加载 BT 节点插件。这种模式允许创建可重复使用的原始节点，并在运行时加载行为树 XML，而无需链接到导航器本身。这些节点不仅能利用多个处理器内核，还能以任何支持 ROS2 客户端库的语言调用其他 CPU 上的远程服务器。

特定任务服务器的设计目的是让每个服务器都能承载 ROS2 服务器、环境模型和运行时选定的算法插件。它们是模块化的，因此可以同时运行其中的一个或多个服务器来计算动作。ROS2 服务器是 BT 导航节点的入口点。该服务器还负责处理取消、抢占或新信息请求。这些请求会被转发给算法插件，以便它们通过访问环境模型来完成任务。

总之，我们可以创建可配置的行为树，其结构和节点可在运行时加载。每个 BT 节点负责管理与远程服务器的通信，远程服务器上有一个用各种语言编写的算法，可在运行时通过插件接口加载。

C. 支持功能扩展

利用 Navigation2 的模块性和可配置性，在设计阶段提出了一些商业功能扩展。本小节将重点介绍关键示例以及实现这些示例的设计考虑因素。行为树最常用于对复杂或多步骤任务建模，其中导航到某个位置是更大任务的一个节点。选择 BT 库 BehaviorTree.CPP2 是因为它很受欢迎，而且支持子树，使 Navigation2 可以用于更大的 BT 任务。任务复杂的用户可以使用所提供的 “Navigation2 BT 节点 ”包装器，将 Navigation2 用作其任务的子树。

许多服务机器人会自动停靠充电或操作电梯，无需人类协助[22]。传统的导航框架在制定时没有考虑到这类任务。虽然由于缺乏供应商标准化而没有实现这些扩展，但我们提供了轻松实现这些扩展的指导。每个服务器都支持多种算法插件；停靠控制器算法可以加载到控制器服务器中，并从 BT 中调用以停靠机器人。可以添加一个 BT 操作节点，利用供应商提供的其他物联网应用程序接口调用电梯[23]。

四、Navigation2 的实现

本节将对 Navigation2 中提供的恢复、规划和控制器服务器逐一进行研究。每个服务器都可以由用户定制或替换，但我们将讨论在实验中提供、推荐和使用的特定算法插件和服务器。每个服务器都包含一个与其运行相关的环境模型、一个连接 ROS2 的网络接口和一组运行时定义的算法插件。所提供的插件见表一。

A. 行为树导航器

行为树导航器是 Navigation2 最高级别的组件，它承载着用于实现导航行为的树。Navigation2 基于 BehaviorTree.CPP，这是一个开源的 C++ 库，支持类型安全的异步操作、可组合树和用于开发的日志/档案基础设施。图 2 所示的顶层节点（BT 导航服务器）是加载和运行用户定义的 BT 的地方。每个节点都会调用下面的服务器来完成任务。

图 3 显示的是传统的导航 2 BT，也用于我们的实验。按照从左到右的逻辑流程，一个策略以 1 Hz 的速度勾选全局规划器动作。如果全局规划器未能找到路径，后备节点就会前进到下一个子节点，以清除全局环境中的先前障碍。清除环境模型可能有助于解决感知系统的潜在故障。然后，父序列节点会勾选控制器或类似的清除后备行为。如果本地回退动作和后续序列节点返回失败，根回退节点就会勾选最后一个子节点，尝试更积极的恢复行为。首先清理所有环境，然后让机器人原地旋转，调整局部障碍物的方向，最后等待动态障碍物让路。

B. 恢复

恢复行为用于缓解完全导航故障。它们从行为树的叶子中调用，由恢复服务器执行。按照惯例，这些行为从保守行为到激进行为依次排列。

但需要注意的是，恢复行为既可以是特定于其子树（如全局规划器或控制器）的行为，也可以是子树中仅包含系统故障恢复的系统级行为。实验中使用的恢复行为如下：

清除成本图：在感知系统故障时清除成本图层的恢复。旋转：用于清除空闲空间，将机器人从潜在的局部故障（如机器人认为自己被困住而无法后退）中引导出来。等待：在遇到人类交通或收集更多传感器数据等基于时间的障碍时进行恢复等待。

C. 感知

随着深度传感技术的范围和分辨率不断提高，用于机器人环境建模的世界表示法也在不断改进。为了充分利用这些改进，我们采用了分层成本计算方法，允许用户根据不同的传感器模式、分辨率和速率限制自定义层级结构。分层成本计算图允许多个数据源和算法对单个成本计算图进行连贯更新。每一层都可以扩展或修改其继承的成本计算图，然后将新信息转发给规划人员和控制人员。

虽然成本图是二维的，但 STVL 会保留环境的三维表示法，以便将障碍物投射到规划空间中。该层利用基于时间的测量持久性，在存在动态障碍物的情况下保持对世界的精确观察。与光线投射方法相比，它能更好地利用二维和三维激光扫描仪、深度摄像头和雷达等众多高分辨率长距离传感器进行扩展[8]。图 4 显示了机器人在中央楼梯间规划路线的情况，以及它使用 STVL 表示体素网格的环境视图。地图（黑色显示）和静态地图的膨胀（彩色梯度）也显示在局部和全局代价图中。

用于实验的成本图如下：

静态图层：使用由 SLAM 管道提供或从磁盘加载的静态地图，并初始化占位信息。
膨胀层：通过卷积机器人的碰撞足迹，以指数衰减方式膨胀成本地图中的致命障碍物。时空
体素层：通过激光和 RGBD 摄像头的传感器模型，维护随时间衰减的时空三维稀疏体积体素网格。

D. 全局规划器和控制器

全局规划器的任务是计算通往目标的最短路径，控制器则使用本地信息计算最佳本地路径和控制信号。全局规划器和控制器是委托给特定任务异步服务器的插件。为了提高性能，相关的全局和局部成本地图被放置在各自的服务器中。

通过将 TEB 本地规划器与用于动态 3D 感知的 STVL 相结合，采用了新的算法，以便在高度动态的环境中顺利导航。TEB 控制器能够接收物体检测和轨迹，以告知控制器环境中的其他限制因素。它还适用于差分、全向和阿克曼式机器人。DWB 是 DWA 的一种实现方式（DW “B ”为增量命名），也是一种高度可配置的基于评分的控制器。不过，由于其在动态场景中的性能问题，本实验并未使用它。实验中使用的控制器如下

A* 规划器：使用 A* 扩展的导航功能规划器，假定粒子为 2D 整体onomic 粒子。TEB 控制器：使用定时弹性带进行时间最优点对点非线性模型预测控制。

E. 状态估计

在状态估计方面，Navigation2 遵循 ROS 转换树标准，利用社区提供的许多现代工具。其中包括机器人定位（Robot Localization），这是一种使用扩展卡尔曼滤波器（Extended Kalman Filters）或无符号卡尔曼滤波器（Unscented Kalman Filters）的通用传感器融合解决方案[11]。它用于提供来自 N 个任意来源的平滑基本里程测量，通常包括车轮里程测量、多个 IMU 和视觉里程测量算法。

然而，局部滤波姿势不足以考虑综合里程漂移，因此导航仍然需要全局定位解决方案。用于实验的全局定位解决方案如下：

SLAM 工具箱：这是一种基于图形的可配置 SLAM 系统，使用二维姿势图和典型扫描匹配生成地图和序列化文件，用于多会期映射[24]。该系统用于创建静态地图，为实验做准备。AMLL：自适应蒙特卡洛定位的一种实现方法，它使用粒子滤波器，利用全向或差分运动模型[18]，在给定的占位网格中定位机器人。在实验过程中，它被用作主要的定位工具。

F. 实用工具

Navigation2 还包括辅助测试和运行的工具。它包括生命周期管理器，用于协调导航器和各种服务器的程序生命周期。该管理器将引导每个服务器经历受管节点生命周期：非活动、活动和最终完成。

生命周期管理器可设置为在系统启动时将所有提供的服务器转换为活动状态。或者，它也可以等待激活系统的信号。Navigation2 可让用户通过图形用户界面激活、管理和指挥系统。该图形用户界面包括控制生命周期管理器、发布导航命令、选择要导航的航点以及实时取消当前任务。对于自主应用程序，所有这些命令都可以通过 ROS2 服务器接口和示例来实现。

G. 质量保证

系统范围内的集成测试用于质量保证目的，有助于避免随着时间的推移出现倒退。除了基本的单元测试、代码样式精简和内存静态分析外，持续集成管道还使用模拟测试来仿真整个机器人软件栈。如图 5 所示，三维机器人模拟器 Gazebo [25] 被用于在各种虚拟场景（包括静态和动态环境）中测试社区的贡献。

通过这种系统测试，维护人员不仅可以监控测试轨迹上的导航故障，还可以监控具有独特运动运动学特性的各种机器人平台上各种算法的性能分析。

五、实验与分析

A. 实验概述

为了展示导航 2 的鲁棒性，我们进行了比 [1] 中提出的更广泛的实验，即 “超级马拉松”（比标准马拉松距离更远）。这项测试使用图 1 所示的两个专业机器人（Tiago 和 RB-1 base），证明了我们的系统在没有人类辅助的情况下长期运行的稳健性和可靠性。这些机器人在充满人类的大学环境中以接近工业的速度运行。

实验在胡安-卡洛斯国王大学电信工程师技术学院进行。图 7 显示了该环境的地图和实验中使用的轨道。机器人绕着中央楼梯间（周围的航点 3、11、13、14）、人流量较大的桥梁和走廊（航点 4-10），在无人干预的情况下通过狭窄的门道回到实验室（航点 1）。如图 6 所示，机器人与学生共处一个环境，学生有时会自发地挡住机器人的去路。我们通过这组航点定义了一条 300 米的实验路线。

两个机器人的特点相似，如表 II 所示。两者都使用差速转向，直径相似，但高度不同。它们都使用不同范围和分辨率的 RGBD 摄像头和安全激光扫描仪进行感知和避障。出于功能安全考虑，两个机器人的最大速度均设定为 0.45 米/秒，低于其最大允许速度。我们使用了两个不同的机器人来展示我们的系统在多个硬件供应商之间的可移植性，只需进行简单的重新配置即可。Tiago 使用外部计算机，因为 ROS2 不支持其内部操作系统。两家机器人供应商都为用户提供了支持的驱动程序和接口。我们在它和 ROS2 之间创建了一个自定义桥接器，支持数据的连续流动。这是支持变换和里程测量高频数据要求所必需的。

机器人每秒保存一次以下信息：

时间戳：当前时间
距离：运动距离。
recovery_executed：记录的已执行恢复。
vel_x：瞬时线速度。
vel_theta: 瞬时角速度。

本实验中使用的所有软件、配置和数据均可在 marathon3 资源库中找到。该资源库包含重现实验的说明以及在实验中收集到的上述数据。我们为实验数据集收集了以下信息：带有协方差的定位姿态、指令的瞬时速度、是否触发了恢复行为、导航时间和距离、激光扫描读数、几何变换、惯性传感器数据和地图。这些数据可在存储库中公开获取，以供实验验证。

B. 分析

表 III 显示了实验结果。在动态校园环境中，机器人在不到 23 小时的时间内成功航行了 37 英里。实验期间的平均线速度为 0.37 米/秒。在测试过程中，两个机器人都没有发生碰撞或需要紧急停止的危险情况。在整个实验过程中，机器人在没有直接辅助的情况下进行操作，从未出现过无法完成导航任务的情况。

图 8 显示了机器人在开课时刻在拥挤的走廊上所面临情况的延时面板。在这段时间里，许多学生都在快速走向自己的目的地，并频繁地使用手机。这段视频展示了机器人是如何与几组学生互动的。走近时，机器人首先放慢速度，绕过第一幅图中的前两个人。第三个人停顿了一下，给了机器人继续前进的权利--在第二幅图中。然后，在绕过第 1 至第 3 面图中的第一组人之后，第 3 至第 4 面图中的一个新的人走过场景，机器人修正轨迹，从他们的路径上绕开。图 5 显示机器人在没有碰撞或停顿的情况下离开了复杂场景。

在极少数情况下，当学生占据了机器人正在导航的航点的准确位置时，机器人会提供被动辅助。此时，要求学生移动，以便机器人继续前进。在 22.8 小时的实验过程中，这种被动辅助的时间影响不到 1 分钟。这是为马拉松实验构建的应用程序（使用导航 2）中的一个错误。该应用程序没有考虑占据目标姿势的问题，但这并不代表导航 2 本身的问题或故障模式。

虽然恢复的总次数似乎很高，但触发恢复行为通常并不是性能不佳的指标，因为它是容错系统的一部分。恢复行为表明，由于路径可能受阻或传感器测量错误，机器人遇到了一些困难。系统会自动执行所需的恢复操作，以克服特定的挑战，从而呈现出强大的系统。

大多数恢复行为都是在两种情况下发生的。第一种情况发生在拥挤的空间，机器人无法计算出通往目标的路线。在这种情况下，机器人会清除其环境表征（通常是本地成本图）中的障碍物。如果这还不够，通常情况下，路径会被学生挡住，等待恢复会暂停导航，直到路径通畅为止。第二种情况是定位信心不足。在周围有很多人的情况下，具有重复特征的长走廊区域偶尔会出现问题。在动态环境中，如果有足够多的光线能照射到静态地图上，AMLL 就能很好地发挥作用，但是，长时间的多次变化会降低定位质量。在这种情况下，机器人的自旋恢复功能有助于重拾定位信心，继续前进。

该实验的视频集锦见脚注4。

六、局限性

实验表明，Navigation2 可以在没有人类干预的情况下，有效地在狭窄空间内绕行数英里，但它也存在一些局限性，将在今后的工作中加以解决。目前，所使用的 A* 规划器无法为非圆形非人体工学机器人创建可行路径。对于本实验中使用的差动驱动机器人来说，这通常不会造成问题，因为它们可以原地旋转到新的航向。但是，对于复杂几何机器人或类似汽车的机器人来说，这就造成了一个问题，即由于路径不可行而无法移动。目前正在进行扩展，以支持任意形状的非人体工学机器人。

此外，TEB 控制器在计算速度指令时可考虑动态障碍物，但在这些实验中并未使用明确的障碍物检测。在没有这些明确检测的情况下，我们的系统能够在存在许多动态障碍物的情况下成功导航。不过，检测和预测模型将进一步确保机器人在人类和其他代理周围安全运行。

七、结论

事实证明，在长时间测试中，Navigation2 导航系统可以在有学生在场的情况下可靠地在校园环境中导航。在我们的演示中，两个不同的工业级机器人使用我们的系统，在没有任何人工干预或碰撞的情况下进行了超过一个马拉松的导航。该系统利用行为树来协调导航算法，使其具有高度可配置性，并充分利用多核处理器--ROS2 的工业级可靠通信使其得以实现。我们使用 STVL 和 TEB 等现代算法在大型动态环境中进行感知和控制，同时继续发扬 ROS 导航栈的传统。

在动态障碍物跟踪和规划方面还存在不足，这些问题正在开发中，并将在今后的工作中加以解决。这项工作在许可授权下开源，我们预计会有更多的扩展和算法使用我们的框架，并随着时间的推移进一步改进。