ros之navigation

本文讲解下ros的navigation模块。

一 概述

Navigaiton的功能实现起始点到目标点的导航。功能主要分为全局规划和局部规划，全局规划搜索出全局路径，局部规划实现机器跟随全局路径进行导航控制。

Navigation输入为目标点、当前位姿、地图信息、传感器数据，输出为全局路径、局部轨迹及速度控制指令。

二 ARCH

2.1 NAV1

2.1.1 软件框图

voxel_grid：体素滤波。

map_server：地图服务器，导入及保存地图。

coatmap_2d：代价地图，为全局规划、局部规划提供障碍物信息。

move_base：整个navigation模块的控制类，实现整体的控制流程。

localization：定位模块，fake_localization给仿真器用, amcl蒙特卡洛定位（机器人知道地图信息的情况下如何利用传感器信息确定自己的位置）。

nav_core:提供全局规划、局部规划、恢复行为的基础类，定义了通用接口。

2.1.2 软件流程

move_base为整个navigation模块的控制类，实现整体的控制流程。其定义了一个节点，订阅位姿及目标点信息，输出轮子控制速度。

global_planner:为全局规划器，提供dijstra和A*算法。

Local_planner:为局部规划器，提供DWA及TEB算法。

2.1.3 move_base类

Move_base构造函数中，global_planner单独定位一个线程，进行全局路径计算。如下图所示。

Move_base构造函数中，定义一个action服务，其回调函数executeCb进行整体流程控制。

内部用一个三状态的状态机进行流程控制。

数据流转采用move_base类内定义的vetctor数据。

2.2 NAV2

由一个节点变为四个节点，每个节点为一个action server。这样做的好处是内部各模块去耦合。

2.2.1 behavior tree

状态机改为行为树。实际项目中navigation模块状态十分复杂，用状态机容易流程混乱。行为树原理请看如下博客行为树入门教程 - 知乎 (zhihu.com)

详细的行为树遍历 — Navigation 2 1.0.0 文档 (fishros.org)

2.2.2生命周期管理

加入生命周期管理，就是将各个模块定义了各种状态，在状态切换时，会调用响应的注册函数，进行上下文设置。

[ROS2] 使用LifecycleNode管理节点起停等状态_ros2 关闭节点-CSDN博客

三 全局规划

3.1 Dijkstra

Dijkstra算法详解 通俗易懂 - 知乎 (zhihu.com)

3.2 A*

我们假设某个人要从A点到达B点，而一堵墙把这两个点隔开了，如下图所示，绿色部分代表起点A，红色部分代表终点B，蓝色方块部分代表之间的墙。

起始点，周围所有不是障碍物的点，都有一个权值。这个权值，代表从起始点到此点的开销与此点到目标点预估开销的和（栅格边长为10）。

则选取权值最小的点作为路径的下一个点。

按以上原理迭代，则就可以找出到达目标点的最优路径。如下图所示。

3.3 搜索效率

A* 算法搜索效率受距离估算影响。距离估算值越接近当前顶点到终点的实际值，A* 算法的搜索效率也就越高；反过来，如果距离估算值与实际值相差较大，那么该算法的效率可能会比狄克斯特拉算法的还要低。如果差距再大一些，甚至可能无法得到正确答案。不过，当距离估算值小于实际距离时，是一定可以得到正确答案的（只是如果没有设定合适的距离估算值，效率会变差）。

四 局部规划

4.1 DWA

【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）-CSDN博客

用角速度，线速度作为坐标，形成一个二维坐标系。以当前机器角速度，线速度为中心，制定一个变化窗口范围，在窗口范围内进行离散采用。用采样得到角速度和线速度值，来预测机器未来轨迹；并对轨迹进行评分，从而选取评分最高的角速度和线速度，作为机器下时刻的速度。

4.2 TEB

从零开始实现移动机器人路径规划算法-TEB - 知乎 (zhihu.com)