datetime:2023/10/11 11:03

author:nzb

该项目来源于大佬的动手学ROS2

特别注意

ROS版本 Galactic to Humble中，nav2_recoveries to nav2_behaviors

Recovery To Behavior

1.Nav2导航框架介绍与安装

上一节我们对机器人导航过程中所用到的2D地图进行了介绍，本节我们就来正式的学习下Nav2导航框架，关于Nav2相关的更多资料，请访问Nav2中文网。

1.Nav2是什么

Nav2项目继承并发扬ROS导航栈的精神。该项目力求以安全的方式让移动机器人从A点移动到B点。Nav2也可以应用于其他应用，包括机器人导航，如下动态点跟踪，在这个过程中需要完成动态路径规划、计算电机的速度、避免障碍、恢复行为。

2.Nav2如何做到的

Nav2使用行为树调用模块化服务器来完成一个动作。动作可以是计算路径、控制力、恢复或任何其他与导航相关的操作。这些都是通过ROS Action服务器与行为树 (BT) 通信的独立节点。下图可以让你对Nav2的架构有一个很好的初步了解。

2.2 四个服务

再对上面的架构图进一步的进行解释，可以简单分为一大三小四个服务。

一大：

1. BT Navigator Server 导航行为树服务，通过这个大的服务来进行下面三个小服务组织和调用。

三小：

1. Planner Server，规划服务器，其任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。说白了就是在地图上找路。
2. Controller Server，控制服务器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。说白了就是根据找的路控制机器人走路。
3. Recovery Server，恢复服务器，恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。说白了就是机器人可能出现意外的时候想办法让其正常，比如走着走着掉坑如何爬出来。

为什么需要全局规划器和局部规划器，导航的运作流程如下：

• 由全局路径规划器基于静态地图规划出一条大致的路径，其中规划使用的地图包含建图时的障碍物，全局路径规划就会避开这些障碍物
• 而局部路径规划器就会把它分割成很多小段，再进行局部路径规划，在局部规划期间会对新增的障碍物信息也进行避障，达到动态避障。

通过规划路径、控制机器人沿着路径运动、遇到问题自主恢复三者进行不断切换完成机器人的自主导航（在这个过程还需要很多节点和数据的辅助，详见下面的组建部分）。

2.3 两大代价（成本）地图

在机器人导航的时候，仅仅靠一张SLAM建立的原始地图是不够的，机器人在运动过程中可能会出现新的障碍物，也有可能发现原始地图中某一块的障碍物消失了，所以在机器人导航过程中维护的地图是一个动态的地图，根据更新频率方式和用途不同，可以分为下面两种。

注意：不论是全局代价地图还是局部代价地图都是有很多个图层共同叠加而成的，你也可以自己定义图层。

2.3.1 全局代价地图 （Global Costmap）

全局代价地图主要用于全局的路径规划器。从上面结构图中其在可以看到在Planner Server中。

通常包含的图层有：

• Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。
• Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

2.3.2 局部代价地图（Local Costmap）

局部代价地图主要用于局部的路径规划器。从上面结构图中其在可以看到在Controller Server中。

通常包含的图层有：

• Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
• Inflation Layer：膨胀层，在障碍地图层上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

3.Nav2导航相关概念

该部分摘抄自Nav2中文网，中间删除了一些信息，全部文章请访问原文页面 。

3.1 动作服务器（Action Server）

动作服务器在第五章中 我们有简单介绍过，Nav2框架中大量的才用了这种通信方式，并将起分装成了行为树（关于行为树下面会介绍）的基础节点进行调用。

通过动作服务器通信，来计算路径规划、控制机器人运动和恢复。每个动作服务器都有自己独特的 nav2_msgs 格式的 .action 类型，用于与服务器进行交互。

3.2 生命周期节点和绑定

生命周期 (或被管理的，更正确的) 节点是ROS 2独有的。更多信息可以是 在这里 。它们是包含状态机转换的用于加载和卸载ROS 2服务器的节点。这有助于确定ROS系统启动和关闭的状态是否正常。

生命周期节点框架在整个项目中被广泛使用，所有服务器都使用它。如果可能的话，所有ROS系统最好使用生命周期节点。

上一节我们通过命令行来配置和激活map_server节点就是因为map_server节点采用了生命周期节点进行管理。

3.3 行为树

行为树 (BT) 在复杂的机器人任务中变得越来越普遍。它们是待完成任务的树形结构。行为树为定义多步或多状态应用程序创建了一个更具可扩展性和人类可理解性的框架。这与有限状态机 (FSM) 相反，后者可能有几十个状态和数百个状态过渡。

一个例子就是踢足球机器人。将足球比赛的逻辑嵌入FSM将具有挑战性，且容易出错因为有许多可能的状态和规则。此外，像从左侧、右侧或中间射门这样的建模选择尤其不清楚。使用行为树则可以为许多行为创建和重用基本原语，像 "kick" "walk" "go to ball" 。更多信息可以在 这本书 找到。强烈建议阅读第1-3章，以更好地理解术语和工作流程。大约需要30分钟。

Nav2项目使用 BehaviorTree CPP V3 作为行为树库。在 BT Navigator 中，创建了可以构建为行为树的节点插件。将节点插件加载到BT中，并且在解析该行为树的XML文件时，将关联注册的名称。此时，我们可以通过该行为树进行导航。

使用此库的一个原因是它能够加载子树。这意味着可以将Nav2行为树可以进行套娃操作。举个例子是在足球比赛中，使用Nav2行为树作为 "go to ball" 节点，将足球检测作为更大任务的一部分。此外，为BT提供了一个 NavigateToPoseAction 插件，因此可以从客户端应用程序通过通常的动作接口调用Nav2软件导航栈。

3.4 导航服务器

规划器和控制器是导航任务的核心。恢复器用于使机器人摆脱不良状态或尝试处理各种形式的问题，以使系统具有容错能力。在本节中，将分析有关它们的一般概念及其在Nav2项目中的用途。

3.4.1 规划器，控制器和恢复服务器

该项目中的三个Action Server是规划器、恢复器和控制器服务器。这些Action Server用于托管一个地图算法插件，以完成各种任务。它们还托管由该算法插件使用的环境表达，以计算其输出。

规划器（Planners）

规划器的任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。两个典型示例是计算一个到达目标位姿的规划（例如从当前位置到达一个目标位姿）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。规划器可以被编写为具有以下功能的工具：

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

Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。

控制器 （Controllers）

控制器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。控制器有权访问局部环境表达，以尝试计算要跟随的基准路径的可行控制工作。许多控制器会将机器人向前投射到空间中，并在每次更新迭代时计算局部可行路径。控制器可以被编写为具有以下功能的工具：

• 跟随路径
• 使用里程计坐标系中的检测器与充电站（桩）对接
• 登上电梯
• 与某个工具的接口

在Nav2中，控制器的一般任务是计算一个有效的控制工作以跟随全局规划路径。然而，有多个控制器类和局部规划器类。Nav2项目的目标就是所有控制器算法都可以作为此服务器中的插件，以用于一般研究和产业任务中。

恢复器 （Recovery）

恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。例子包括感知系统中会导致环境表达充满假障碍物的故障。这样就会触发清除成本地图恢复以允许机器人移动。

另一个例子就是机器人由于动态障碍物或控制不佳而卡住。在允许的情况下，倒退或原地旋转会允许机器人从卡住的位置移动到可以成功进行导航的自由空间中。

最后，在完全故障的情况下，可以实施恢复以引起操作员的注意以寻求帮助。这可以通过电子邮件、短信、Slack、Matrix等来完成。

3.4.2 航点跟随

航点跟随是导航系统的基本功能之一。它会告知系统如何使用导航程序到达多个目的地。

nav2_waypoint_follower 软件包含一个航路点跟踪程序，该程序具有特定任务执行程序的插件接口。如果需要让机器人前往给定位姿并完成像拍照、捡起盒子或等待用户输入之类的特定任务，这会非常有用。

关于机器人队管理器/调度器有两种思想流派：哑机器人+智能集中式调度器；智能机器人+哑集中式调度器。

在第一种思想中， nav2_waypoint_follower 软件包足以创造一个产品级的机器人解决方案。由于自主系统/调度器在分配任务时会考虑机器人的姿势、电池电量、当前任务等因素，机器人上的应用程序只需要关心手头的任务，而不用关心完成系统要求任务的其他复杂因素。在这种情况下，应该将发送至航点跟随者的请求视为1个工作单元（例如，仓库中的1次拣货、1个安全巡逻循环、1个过道等）来执行任务，然后返回给调度器以进行下一个任务或者要求充电。在这种思想流派中，航点跟随应用程序只是导航软件堆栈之上和系统自主应用程序之下的一个步骤。

在第二种思想中， nav2_waypoint_follower 软件包是一个不错的示例应用程序/概念证明，但确实需要机器人上的航点跟踪/自主系统来承担更多任务以制定健壮的解决方案。在这种情况下，应该使用 nav2_behavior_tree 软件包创建自定义应用程序级别的行为树，以使用导航来完成任务。这可以包含子树，例如在任务中检查充电状态以返回停泊坞，或者在更复杂的任务中处理1个以上的工作单元。很快，将会有一个 nav2_bt_waypoint_follower (名称有待调整)，它将允许用户更容易地创建此应用程序。在这个思想流派中，航点跟随应用程序与自主系统的联系更加紧密，或者在很多情况下，航点跟随应用程序就是自主系统本身。

这两种思想流派并不能简单地说谁比谁更好，谁更好很大程度上取决于机器人正在完成何种任务、处于何种类型的环境中以及有何种可用的云资源。通常，对于既定的业务案例，这种区别非常明显。

3.5 状态估计（重要组件）

Nav2中，默认进行状态估计的组件是AMCL (Adaptive Monte Carlo Localization)自适应蒙特卡洛定位。nav2中对应的功能包是nav2_amcl。

根据ROS社区标准，在导航项目中，需要提供两个主要的坐标转换。 map 到 odom 的坐标变换由定位系统 (定位，建图，SLAM)提供， odom 到 base_link 的坐标转换由里程计系统提供。

注解：无需在机器人上使用LIDAR即可使用导航系统。不需要使用基于激光雷达的防撞、定位或SLAM系统。但是，Nav2确实可以提供说明和支持使用激光雷达对这些系统进行尝试和真实实现。使用基于视觉或深度传感器的定位系统和使用其他传感器来避免碰撞可以同样成功。唯一的要求就是在选择具体实现方式时遵循REP-105标准。

REP-105标准

REP 105 定义了导航和更大的ROS生态系统所需的框架和约定。应始终遵循这些约定，以利用社区中丰富的定位、里程计和SLAM项目。

简而言之，REP-105至少必须为机器人构造一个包含map -> odom -> base_link -> [sensorframes] 的完整 的TF树。TF2是 ROS 2中的时变坐标变换库，Nav2使用TF2来表达和获取时间同步的坐标变换。全球定位系统 (GPS、SLAM、动作捕捉Motion Capture) 的工作是至少要提供 map-> odom 的坐标转换。然后，里程计系统的作用是提供 odom -> base_link 的坐标转化。关于 base_link 的其余坐标转换应该是静态的，并应在 URDF 中定义。

3.6 全局定位: 定位与SLAM

全局定位系统 (GPS、SLAM、运动捕捉) 的工作是至少提供 map -> odom 的坐标转换。Nav2项目提供的 amcl 是一种基于粒子过滤器的自适应蒙特卡罗定位技术，用于静态地图的定位。Nav2还提供用于定位和生成静态映射的SLAM工具箱作为默认的SLAM算法。

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

3.7 里程计（Odometry）

里程计系统的作用是提供 odom -> base_link 的坐标转换。里程计可以来自许多数据源，包括激光雷达、车轮编码器、VIO和IMU。里程计的目标是提供基于机器人运动的平滑和连续的局部坐标系。全局定位系统会相对全局坐标的坐标变换进行更新，以解决里程计的漂移问题。

这个 Robot Localization 通常用于这种融合。它将采用各种类型的 N 个传感器，并为TF和话题提供连续平滑的里程计。一个典型的移动机器人装置可能有来自车轮编码器或IMU的里程计以及融合在这个工作区内的视觉。

这样平滑输出就可用于精确运动的航行位置推算和在全局位置更新之间准确地更新机器人的位置。

3.8 环境表达（建模）

环境表征是机器人感知环境的方式。它还充当各种算法和数据源的中心定位工具，以将它们的信息组合到一个空间中。这样，控制器、规划器和恢复器就可以使用该空间来安全有效地计算它们的任务。

成本地图和图层

当前的环境表达是一个成本地图。成本地图是包含来自未知、空闲、占用或膨胀成本的单元格的规则2D单元格网格。然后搜索该成本地图以计算全局计划或采样以计算局部控制工作。

各种成本地图图层被实现为pluginlib插件，以将信息缓冲到成本地图中。这包括来自LIDAR、RADAR、声纳、深度传感器、图像传感器等的信息。最好在传感器数据输入到成本地图之前进行处理，但这取决于开发人员。

可以使用相机或深度传感器创建代价地图层来检测和跟踪场景中的障碍物，以避免碰撞。此外，可以创建层来基于一些规则或启发式算法来改变基础成本图。最后，它们可用于将实时数据缓冲到2D或3D世界中，以进行障碍物的二值化标记。

成本地图过滤器

想象一下，您正在注释地图文件 (或任何图像文件)，以便根据注释地图中的位置执行特定操作。

通过使用成本地图过滤器可以实现以下功能：

• 机器人永远不会进入的禁区/安全区。
• 限速区，机器人进入这些区域的最大速度将受到限制。
• 机器人在工业环境和仓库中移动的首选通道。

其他形式的环境表示

存在各种其他形式的环境表征。包括:

• 梯度图，类似于成本地图，但梯度图会表达表面梯度以检查可穿越性
• 3D成本图，以3D形式表示空间，但这样就也需要3D规划和碰撞检测
• 网格图，类似于梯度图，但具有多个角度的表面网格
• Vector space ，接收传感器信息并使用机器学习算法来检测要跟踪的单个物品和位置，而不是对离散点进行缓冲区计算

4.Nav2下载安装

看完了第三节的是不是似懂非懂，接下来我们将通过实验进一步来对照着学习。

Nav2的安装有两种方式，一种是通过二进制直接安装，另外一种是通过源码的方式进行安装，这里依然推荐源码的方式，毕竟我们还是要看一看Nav2的源码的。

4.1 apt安装

安装nav2

 sudo apt install ros-humble-nav2-*

4.2 源码安装

将下面的源码克隆到fishbot_ws的src目录下：

git clone https://ghproxy.com/https://github.com/ros-planning/navigation2.git -b humble

安装依赖

这里我们使用rosdepc进行依赖的安装，rosdepc指令找不到可以先运行下面的一键安装命令，选择一键配置rosdep即可。

wget http://fishros.com/install -O fishros && . fishros

接着在fishbot_ws下运行下面这个命令进行依赖的安装。

rosdepc install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y

编译

这里有一个新的命令--packages-up-to，意思是其所有依赖后再编译该包

colcon build --packages-up-to navigation2

4.3 测试是否安装成功

如果是源码编译请先source下工作空间后再使用下面指令查看是否安装成功；

ros2 pkg list | grep navigation2

能看到下面的结果即可

navigation2

5.源码功能包拆解

包含代码的功能包及其功能见下面列表

#==============控制器及其实现相关功能包======================#
nav2_controller　｜　控制器
nav2_dwb_controller | DWB控制器，Nav2控制器的一个实现
nav2_regulated_pure_pursuit_controller | 纯追踪控制器，Nav2控制器的一个实现
nav2_constrained_smoother

#==============规划器及其实现相关功能包======================#
nav2_planner | Nav2规划器
nav2_navfn_planner　｜　navfn规划器，Nav2规划器的一个实现
nav2_smac_planner | smac规划器，Nav2规划器的一个实现

#=====================恢复器==============================#
nav2_recoveries | Nav2恢复器

#=====================行为树节点及其定义====================#
nav2_bt_navigator |　导航行为树
nav2_behavior_tree | 行为树节点插件定义

#=====================地图和定位===========================#
nav2_map_server　｜　地图服务器
nav2_costmap_2d　｜　2D代价地图
nav2_voxel_grid | 体素栅格
nav2_amcl | 自适应蒙特卡洛定位。　　状态估计，输入地图、激光、里程计数据，输出机器人map和odom之间的位姿关系。

#=====================通用插件系统管理等====================#
nav2_bringup | 启动入口
nav2_common　｜　公共功能包
nav2_msgs　｜　通信相关消息定义
nav2_util | 常用工具
nav2_lifecycle_manager |节点生命周期管理器　
nav2_rviz_plugins | RVIZ插件

#=====================核心定义============================#
nav2_core　｜　Nav2核心包
navigation2 | nav2导航汇总配置

#=====================应用================================#
nav2_waypoint_follower | 路点跟踪

#=====================测试=================================#
nav2_system_tests | 系统测试

上面的每个节点都有自己的参数可以配置，下一节我们就对个个节点的配置进行介绍并进行适配

6.总结

本节我们对Nav2进行了详细的介绍和安装，下一节我们就开始为FishBot配置Nav2，然后开始尝试让Nav2控制Fishbot自主的移动起来。