核心概念

本页面面向机器人学新手，帮助大家熟悉移动机器人导航的核心概念，尤其是理解和使用 Nav2 所需的概念。

ROS 2

ROS 2 是 Nav2 使用的核心中间件。如果不熟悉，建议先阅读 ROS 2 文档。

动作服务器（Action Server）

与 ROS 一样，动作服务器（action server）是管理导航等长时间运行任务的常用机制。Nav2 大量使用动作（action），某些场景下甚至没有等价的话题（topic）接口。因此，ROS 2 开发者有必要深入理解动作服务器的工作方式。ROS 2 文档中提供了简单的 CLI 示例。

动作服务器与普通服务服务器（service server）类似，区别在于客户端请求的任务可能耗时较长。例如，升起推土机铲斗，或让机器人向右移动 10 米。

动作服务器和客户端允许我们在另一个进程或线程中启动长时间运行的任务，并获取表示结果的 future 对象。你可以阻塞等待动作完成，也可以定期检查状态，同时继续处理其他工作。由于任务耗时长，动作服务器还会向客户端持续提供反馈（feedback）。反馈内容不限，在 ROS 的 .action 文件中与请求和结果类型一起定义。

以推土机为例：请求是一个目标角度，反馈是剩余的转动角度，结果是表示成功或失败的布尔值加上最终角度。以导航为例：请求是一个目标位置，反馈是已用时间和距目标距离，结果是表示是否成功的布尔值。

反馈和结果既可以通过向动作客户端注册回调同步获取，也可以从共享的 future 对象异步获取。两种方式都需要客户端节点持续 spinning 以处理回调组（callback group）。

在 Nav2 中，动作服务器用于通过 NavigateToPose 动作消息与最高层的行为树（Behavior Tree, BT）导航器通信。BT 导航器也通过动作服务器与下级服务器通信，以计算路径规划、控制量（control efforts）和恢复行为（recoveries）。每个服务器在 nav2_msgs 中都有各自独立的 .action 类型。

生命周期节点与 Bond（Lifecycle Nodes and Bond）

生命周期节点（Lifecycle，更准确地说是 Managed 节点）是 ROS 2 独有的概念，更多原理可参阅此文。这种节点内置了状态机转换机制，用于管理 ROS 2 服务器的启动（bringup）和关闭（teardown），确保系统在启动和关闭过程中行为确定可控。同时也方便开发者以适合商业部署和调试的方式组织程序。

节点创建时处于未配置（unconfigured）状态，此时仅执行构造函数——构造函数中不应包含任何 ROS 网络设置或参数读取。通过启动系统或生命周期管理器（lifecycle manager），节点会被配置（configuring）并转入非活跃（inactive）状态。此后，再通过激活（activating）转换将节点激活。

激活状态下的节点可以正常处理消息并完全就绪。配置阶段触发 on_configure() 方法，在其中设置所有参数、ROS 网络接口，以及（安全相关系统的）所有动态分配内存。激活阶段触发 on_activate() 方法，激活 ROS 网络接口并初始化各种内部状态以开始处理信息。

关闭时，节点依次经历去激活（deactivating）、清理（cleaning up）、关闭（shutting down），最终到达结束（finalized）状态。在此过程中，网络接口停用、消息处理停止、内存释放，干净退出。

生命周期节点框架在本项目中应用广泛，所有服务器均采用。对任何 ROS 系统而言，尽量使用生命周期节点都是最佳实践。

在 Nav2 中，我们使用 LifecycleNode 的包装类 nav2_util::LifecycleNode。该包装类屏蔽了 LifecycleNode 对典型应用来说大部分不必要的复杂性。它还内置了用于生命周期管理器的 bond 连接，确保服务器在向上转换（transition up）后保持活跃。如果服务器崩溃，它会通知生命周期管理器并向下转换整个系统，以防严重故障扩散。详见 Eloquent 迁移指南。

行为树（Behavior Trees）

行为树（Behavior Tree, BT）在复杂机器人任务中越来越常见。它用树状结构来描述任务，为多步骤、多状态的应用提供了可扩展、易理解的框架。相比之下，有限状态机（FSM）在状态和转换数量增大后很快变得难以维护。

以踢足球的机器人为例：用 FSM 嵌入足球比赛的逻辑会很困难且容易出错，因为可能的状态和规则太多，而且「从左侧、右侧或中央射门」这类策略选择也难以表达清楚。使用 BT，则可以将「踢球（kick）」「行走（walk）」「走向球（go to ball）」等基本原语封装为可复用的节点，灵活组合出各种行为。

更多信息可参阅这本书，强烈建议阅读第 1-3 章（约 30 分钟），以充分理解术语和工作流程。

行为树为导航逻辑提供了形式化结构，既能构建复杂系统，也能借助高级工具进行可验证的正确性分析。将应用逻辑集中在行为树中，配合独立的任务服务器（仅通过树传递数据），即可实现形式化分析。

本项目使用 BehaviorTree.CPP V4 作为行为树库。我们创建了节点插件，可在 BT Navigator 内部组装成树。节点插件在 BT 中加载，解析树的 XML 文件时通过注册名称关联到具体实现。此后即可遍历行为树执行导航。

选择该库的一个原因在于它支持加载子树——这意味着 Nav2 行为树可以作为子树嵌入到更高层级的 BT 中，将整个导航系统当作一个节点插件使用。例如在足球场景中，将 Nav2 行为树作为「走向球」节点，嵌入到包含球检测等更大任务的 BT 中。此外，我们还为 BT 提供了 NavigateToPoseAction 等插件，使得客户端应用可以通过常规动作接口调用整个 Nav2 技术栈。

设计复杂自主行为也可使用层次化 FSM（HFSM）等方案。我们选择行为树，是因为它在机器人及相关行业中普及度最高，也是用户需求最多的方案。不过，由于 Nav2 的独立任务服务器架构，未来推出 nav2_hfsm_navigator 包并不困难，取决于社区兴趣和贡献。

规划器和控制器是导航任务的核心。恢复行为用于帮助机器人摆脱困境或应对各类异常，使系统具备容错能力。平滑器可进一步提升规划路径的质量。本节介绍这些组件的一般概念及其在本项目中的作用。

规划器、控制器、平滑器、路线与行为服务器（Planner, Controller, Smoother, Route, and Behavior Servers）

本项目包含五个动作服务器：规划器（planner）、行为（behavior）、平滑器（smoother）、路线（route）和控制器（controller）服务器。

这些动作服务器承载各类任务的算法插件映射表，同时持有插件计算所需的环境表示（environmental representation）。

规划器、平滑器和控制器服务器在运行时会配置所使用算法的名称（别名）和类型。类型是已注册的 pluginlib 名称，名称则是对应任务的别名。例如，DWB 控制器使用的名称是 FollowPath，因为它跟随参考路径。此时 DWB 的所有参数都放在该命名空间下，如 FollowPath.<param>。

这三个服务器随后暴露与其任务对应的动作接口。当行为树 tick 相应的 BT 节点时，会调用动作服务器处理任务。服务器内部的回调按名称（如 FollowPath）调用所选算法，该名称映射到具体实现。这样，用户在行为树中使用的是算法类别而非具体实现。例如，你可以有 N 个控制器插件分别用于跟随路径、与充电器对接、避开动态障碍物或与工具交互。将所有插件放在同一个服务器中，可以共享单一的环境表示对象，避免代价高昂的重复创建。

行为服务器中，每个行为也有自己的名称，但每个插件还会暴露各自独立的动作服务器。这是因为可能创建的行为动作种类繁多，无法用一个简单接口统一覆盖。行为服务器还包含一个本地代价地图的订阅者，从控制器服务器接收实时更新以计算其任务——这样做同样是为了避免创建多个本地代价地图实例，因为复制代价地图在计算上开销很大。

路线服务器不像规划器或控制器服务器那样包含多种「路由算法」，而是使用一组插件通过导航图来计算路线。这些插件分别负责对图中的边评分、解析图文件，以及在必要时沿路线执行操作。路线服务器不进行自由空间规划，而是基于可生成的导航图来计算路线，导航图可以表示车道、允许机器人导航的区域、示教-重复（teach-and-repeat）路线、城市道路等。

此外，由于 BT 节点本质上是调用动作的简单插件，可以创建新的 BT 节点来调用其他类型的动作服务器。建议尽量使用已有的服务器。如果确实因为插件或动作接口需要新服务器，该框架也能支持。新服务器应采用与已有服务器类似的类型和插件接口，并需要创建相应的 BT 节点插件来调用。通过广泛使用服务器和插件模式，Nav2 仓库本身无需 fork 或修改。

如果你发现需要对 pluginlib 定义或动作类型提供新的接口，请提交 ticket，看看能否在现有接口中解决。

规划器（Planners）

规划器的任务是计算一条满足特定目标的路径。根据术语和算法的不同，路径也可称为路线（route）。两个典型场景是点到点规划（从当前位置到目标）和完全覆盖规划（规划覆盖所有自由空间的路径）。规划器可访问全局环境表示及缓冲到其中的传感器数据。规划器可以实现以下功能：

计算最短路径
计算完全覆盖路径
计算沿稀疏或预定义路线的路径

Nav2 中规划器的常规任务是计算从当前位姿到目标位姿的有效且尽可能最优的路径，同时也支持多种规划类别和路线类型。

控制器（Controllers）

控制器——在 ROS 1 中称为局部规划器（local planner）——负责跟随全局规划路径或完成局部任务。控制器可访问局部环境表示，尝试计算出机器人底盘可执行的可行控制量。许多控制器会在空间中向前投影机器人位姿，在每次迭代中计算一条局部可行路径。控制器可以实现以下功能：

跟随路径
使用里程计坐标系（odometric frame）中的探测器与充电站对接
进入电梯
与工具交互

Nav2 中控制器的常规任务是计算有效控制量以跟随全局规划路径，同时也支持多种控制器和局部规划器。本项目的目标是让所有控制器算法都能作为该服务器的插件，覆盖常见的研究和工业任务。

行为（Behaviors）

恢复行为是容错系统的中流砥柱。其目标是处理系统未知的或失败的状况并自主恢复。例如，感知系统故障导致代价地图充满虚假障碍物时，会触发清除代价地图（clear costmap recovery）行为，让机器人恢复正常移动。

又比如机器人因动态障碍物或控制不佳而被卡住时，后退或原地旋转可帮助它脱离困境，移动到可以正常导航的自由空间。

最后，在完全失败的情况下，可以触发恢复行为来通知操作员介入，通过电子邮件、短信、Slack、Matrix 等渠道求助。

值得一提的是，行为服务器不仅能容纳恢复行为，还能承载任何需要共享代价地图、TF 缓冲区等昂贵资源的行为。每个行为可以有各自的 API。

平滑器（Smoothers）

规划器搜索路径时采用的最优性标准通常是简化过的，因此对路径的额外细化往往能带来改善。为此引入了平滑器，通常用于减少路径的锯齿并平滑突变转向。由于平滑器可访问全局环境表示，它还能增大路径与障碍物及高代价区域之间的距离。

与规划器内置的平滑相比，独立平滑器的优势在于可以灵活组合不同的规划器和平滑器，或者对平滑过程进行精细控制（例如仅平滑路径的特定部分）。

Nav2 中平滑器的一般任务是接收路径并返回优化后的版本。不过，针对不同的输入路径，优化的标准和方法各不相同，因此可以注册多种平滑器插件到该服务器。

路线（Route）

路线服务器是一个专门的规划器，它使用导航图而不是自由空间代价地图来计算路线。路线被计算为从起点到终点、经过预定义导航图中节点集和有向边集的最优路径。导航图可以用来表示车道、允许机器人导航的区域、示教-重复路线、城市道路等。

机器人足迹（Robot Footprints）

在代价地图中，机器人足迹可设置为半径为 robot_radius 的圆形；如果机器人不是圆形，则可用点向量 footprint 表示任意多边形。这也可以随时间通过代价地图的 ~/footprint 话题进行调整，该话题会根据需要随时间更新多边形，以应对机器人状态的变化，例如机械臂的移动、拾取托盘，或其他改变机器人形状的动作。该多边形随后将被规划器和控制器自动使用。

航点跟随（Waypoint Following）

航点跟随是导航系统的一项基本功能。它告诉系统如何依次导航到多个目的地。

nav2_waypoint_follower 包含一个航点跟随程序，带有针对特定任务执行器的插件接口。如果你需要到达某个位置并完成特定任务（如拍照、拾取箱子或等待用户输入），它会非常有用。它也是一个很好的示例，展示了如何在应用中使用 Nav2。

不过，它的用途不仅限于示例应用。对于车队管理者/调度器（fleet managers/dispatchers），有两种思路：

简单机器人 + 智能集中式调度器
智能机器人 + 简单集中式调度器

第一种方案中，nav2_waypoint_follower 完全足以构建生产级的机载解决方案。由于自主系统/调度器在分配任务时会考虑机器人的位姿、电池电量、当前任务等因素，机器人上的应用只需要关注手头的任务，而不必操心完成该任务所需的其他系统复杂性。在这种情况下，你应该将对航点跟随器的请求视为 1 个单位的工作（例如仓库中的 1 次拣选、1 次安保巡逻循环、1 条通道等），完成一个任务后返回调度器获取下一个任务或请求充电。在这种思路下，航点跟随应用只比导航高一个层次，低于系统自主应用。

第二种方案中，nav2_waypoint_follower 只是一个不错的示例应用/概念验证。你确实需要机器人上的航点跟随/自主系统承担更多责任，才能构建稳健的解决方案。在这种情况下，你应该使用 nav2_behavior_tree 软件包创建自定义的应用层行为树，利用导航来完成该任务。这可以包括诸如在任务中检查充电状态以便返回充电桩的子树，或在更复杂的任务中处理超过 1 个单位的工作等。很快将推出一个 nav2_bt_waypoint_follower（名称可能调整），让你更容易创建这种应用。在这种思路下，航点跟随应用与系统自主性联系更紧密，在许多情况下，它本身就是系统自主性。

两者没有优劣之分，具体取决于你的机器人要完成什么任务、在什么类型的环境中运行，以及有哪些云资源可用。通常，对于特定的业务场景，这种区别非常明显。

nav2_waypoint_follower 还支持 GPS 航点跟随——当全局定位由 robot_localization 的 navsat_transform 节点提供时即可使用（也可由 Fuse 或其他来源提供）。 nav2_waypoint_follower 提供了一个名为 /follow_gps_waypoints 的动作服务器，可直接接受 GPS 坐标形式的目标，将其转换为全局坐标系中的笛卡尔目标，然后作为笛卡尔航点执行。

状态估计（State Estimation）

根据社区标准，在导航项目中需要提供 2 个主要的坐标变换。 map 到 odom 的变换由定位系统（localization、mapping、SLAM）提供，odom 到 base_link 的变换由里程计（odometry）系统提供。

注意：使用导航系统没有要求你的机器人必须使用激光雷达，也没有要求使用基于激光雷达的避障、定位或 SLAM。不过，我们确实提供了使用激光雷达的成熟实现方案的说明和支持。使用视觉或深度定位系统，配合其他传感器进行避障，同样可以取得成功。唯一的要求是，无论选择哪种实现方案，都要遵循下面的标准。

标准（Standards）

REP 105 定义了导航以及更广泛的 ROS 生态所需的坐标系和约定。应始终遵循这些约定，以便使用社区中丰富的定位、里程计和 SLAM 项目。

简而言之，REP-105 规定，你至少必须为你的机器人构建一个包含完整 map -> odom -> base_link -> [传感器坐标系] 的 TF 树。 TF2 是我们在 ROS 2 中使用的时变坐标变换库，用于表示和获取时间同步的变换。全局定位系统（GPS、SLAM、Motion Capture 动作捕捉）的工作是至少提供 map -> odom 变换。然后，里程计系统负责提供 odom -> base_link 变换。相对于 base_link 的其余变换应该是静态的，并在你的 URDF 中定义。

全局定位：定位与 SLAM（Global Positioning: Localization and SLAM）

全局定位系统（GPS、SLAM、动作捕捉）的工作是至少提供 map -> odom 变换。我们提供 amcl，这是一种基于粒子滤波器的自适应蒙特卡洛定位（Adaptive Monte-Carlo Localization）技术，用于在静态地图中定位。我们还提供 SLAM Toolbox 作为默认的 SLAM 算法，用于定位并生成静态地图。

这些方法还可能产生其他输出，包括位置话题、地图或其他元数据，但它们必须提供该变换才有效。多种定位方法可以使用 robot localization 融合在一起，下文将详细讨论。

里程计（Odometry）

里程计系统负责提供 odom -> base_link 变换。里程计可以来自多种来源，包括激光雷达、RADAR、轮式编码器、VIO 和 IMU。里程计的目标是基于机器人运动提供一个平滑且连续的局部坐标系。全局定位系统将更新相对于全局坐标系的变换，以校正里程计漂移。

Robot Localization 通常用于这种融合。它将接收 N 个各种类型的传感器，并向 TF 和话题提供连续平滑的里程计。典型的移动机器人配置会将来自轮式编码器、IMU 和视觉的里程计以这种方式融合。

平滑的输出随后可用于航位推算（dead-reckoning），以实现精确运动，并在两次全局位置更新之间准确更新机器人的位置。

环境表示（Environmental Representation）

环境表示是机器人感知其环境的方式。它也是各种算法和数据源将信息汇总到单一空间的核心位置。控制器、规划器和恢复行为则利用这个空间来安全高效地完成各自的任务。

代价地图与图层（Costmaps and Layers）

当前的环境表示是代价地图（costmap）。代价地图是一个规则的 2D 网格，每个单元格标记为未知（unknown）、空闲（free）、占用（occupied）或膨胀（inflated）状态及相应代价。规划器搜索这张代价地图来计算全局路径，控制器则对其采样来计算局部控制量。

各种代价地图图层作为 pluginlib 插件实现，将信息缓冲到代价地图中。这包括来自激光雷达、RADAR、声纳、深度相机等传感器的信息。在将传感器数据输入代价地图图层之前进行预处理是推荐做法，但最终取决于开发者。

还可以创建代价地图图层，利用相机或深度传感器检测和跟踪场景中的障碍物，用于避障。此外，也可以创建图层，根据特定规则或启发式方法动态修改底层代价地图。最后，它们还可以用于将实时数据缓冲到 2D 或 3D 世界中，用于二值障碍物标记。

代价地图过滤器（Costmap Filters）

想象一下，你正在标注一个地图文件（或任何图像文件），以便根据地图中标注的位置触发特定动作。例如，可以标注禁入区以避免在其中规划路径，或者为标记区域内的像素设定最大速度限制。这种标注地图称为「过滤器掩码（filter mask）」。如同覆盖在表面上的掩膜，它可以与主地图大小、位姿和比例相同，也可以不同。过滤器掩码的核心作用是在地图上划定区域，附加特定功能或改变机器人行为。

代价地图过滤器是一种基于代价地图图层的方法，将过滤器掩码中标注的空间相关行为变化应用到 Nav2 技术栈中。代价地图过滤器被实现为代价地图插件。这些插件被称为「过滤器（filter）」，因为它们根据过滤器掩码上标记的空间标注来过滤代价地图。为了制作过滤后的代价地图并改变机器人在标注区域中的行为，过滤器插件读取来自过滤器掩码的数据。这些数据被线性转换为过滤器空间中的特征图。有了这个转换后的特征图，结合地图/代价地图、任何传感器数据和当前机器人坐标，过滤器可以更新底层代价地图，并根据机器人所在位置改变其行为。例如，借助代价地图过滤器可以实现以下功能：

机器人永远不会进入的禁入/安全区域。
速度限制区域：机器人进入这些区域后，最大速度会被限制。
工业环境和仓库中机器人移动的首选车道。

其他形式（Other Forms）

还存在各种其他形式的环境表示。包括：

梯度图（gradient maps），与代价地图类似，但表示表面梯度以检查可通行性
3D 代价地图，以 3D 方式表示空间，但同时也需要 3D 规划和碰撞检测
网格地图（mesh maps），与梯度图类似，但带有多个角度的表面网格
「矢量空间（vector space）」，接收传感器信息并使用机器学习检测要跟踪的单个物体和位置，而不是缓冲离散点