迷你无人车 Navigation 导航（5）— 基础框架学习

迷你无人车 Navigation 导航（5）— 基础框架学习

整个功能包整个功能包集合以move_base为核心，将里程计信息、传感器信息、定位信息、地图以及目标点输入给move_base，move_base经过规划后会输出速度指令

move_base包括三个关键部分：global_planner（全局规划器）、local_planner（局部规划器）和recovery_behaviors（恢复行为），这三个部分都是以插件的形式实现的，通过插件机制可以方便地切换不同算法实现的规划器

恢复行为会在机器人移动过程中出现了异常状态时被触发，目的是帮助机器人摆脱异常状态

move_base还包括了global_costmap（全局代价地图）和local_costmap（局部代价地图），规划器需要在代价地图上进行导航规划

odometry（里程计）

里程计的作用就是估计机器人运动的距离和速度，在ROS Navigation Stack中，里程计信息有两个作用，一个作用是提供给局部规划器，当局部规划器选取最优路径和判断机器人是否停止的时候会使用到里程计的速度信息，另一个作用就是将估计位姿信息用于定位

里程计信息一般从机器人底盘的轮式编码器获取，当然根据不同的机器人也可以选择使用视觉里程计，还可以使用扩展卡尔曼滤波对轮式里程计和IMU进行数据融合，得到更加准确的位姿估计

消息类型nav_msgs/Odometry中包括了机器人的位姿和速度以及各自的协方差

sensor（传感器）

传感器数据一般来自于激光雷达、IMU和深度相机，可以用于定位和避障

使用传感器需要设定传感器参考系与机器人参考系之间的坐标变换关系，也就是常说的tf变换，这样做是为了表示传感器感知到的环境与机器人参考系之间的关系

如果使用amcl算法，激光雷达数据会用来与静态地图进行匹配，修正机器人的位姿，得到更加准确的定位。激光雷达也可以感知到环境中障碍物的位置，通过将障碍物加入到代价地图中，实现避障。具体使用哪几种传感器，依赖于使用的机器人平台。理论上来说，使用的传感器种类越多，定位和避障的效果更好

tf

tf 是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓存并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换

机器人系统通常有许多随时间变化的三维参考系，例如世界参考系和机器人参考系。tf会随着时间的变化跟踪这些参考系

基于ROS Navigation Stack实现移动机器人的自主导航，必须维护一棵完整的tf树，即map->odom->base_link->sensor_link。实际上，我们所说的定位，就是维护map->base_link之间关系的过程

tf树中记录了机器人参考系与地图参考系之间的关系，也就得到了机器人在地图中的哪个位置，tf树也记录了传感器参考系与机器人参考系的关系，也就得到了感知的数据与机器人之间的关系

从上面这张图我们就可以直观地理解什么是tf变换了，由tf树帮助我们管理激光雷达与机器人底盘之间的坐标变换关系，当激光雷达感知到某个位置存在障碍物时，通过tf变换就能得到障碍物与机器人底盘之间的距离

map_server

map_server在ROS Navigation Stack中是可选的，其主要的作用就是给机器人导航提供地图

提供地图有两种方式，一种方式是通过SLAM提供实时地图，另一种方式是提供SLAM提前建立并保存或者通过其他方式制作的地图，常用的SLAM算法有gmapping和hector_slam等

一般在比较规则的场景，可以制作高精度地图提供给机器人，定位和规划都会有比较好的效果。通过SLAM提供实时地图，需要把实时地图以话题的形式发布。通过map_server提供的地图为pgm格式，通过加载yaml配置文件，将地图以话题的形式加载到系统中。在yaml文件中可以配置地图的分辨率，原点以及表示占据/空闲的概率

下面为yaml配置文件的内容：

地图的默认参考系为map。占据的概率occ = (255 – color_avg) / 255.0，其中color_avg为像素RGB的平均值

amcl（自适应蒙特卡洛定位）

amcl是ROS Navigation Stack中唯一指定的定位算法，全称为自适应蒙特卡洛定位，它是一种用于在二维环境中移动的机器人的概率定位系统

简单概括一下它的原理就是通过在全局地图中撒粒子，粒子可以理解为机器人的可能位姿。按照评价标准，例如激光雷达数据与地图的匹配程度给粒子打分，分数越高代表机器人在这个位置的可能性越大，经过粒子滤波器以后留下的就是分数高的粒子了。经过多次撒粒子，粒子就会集中到机器人位置可能性高的地方，称之为粒子收敛

自适应其实简单理解就是会根据粒子的平均分数或者粒子是否收敛来增加或减少粒子数，它能够有效地解决机器人绑架问题和粒子数固定问题

上图中红色的粒子簇即为amcl在全局地图中撒下的粒子，可以看到一开始的粒子簇集中在给定的起始位姿周围，此时的粒子簇还是比较分散的，随着机器人的移动，粒子簇逐渐收敛，定位效果还是不错的

amcl在ROS Navigation Stack中的作用就是输出map->odom的tf变换，来弥补里程计的漂移误差，它要求在机器人的定位系统中要存在里程计位姿估计，即odom->base_link的tf变换，并给定起始位姿和输入传感器数据

costmap_2d（代价地图）

costmap_2d功能包提供了一种二维代价地图的实现方案，该方案从实际环境中获取传感器数据，构建二维或三维的栅格占用地图，以及基于占用栅格地图和用户定义膨胀半径的二维代价地图。该包也支持基于map_server初始化代价地图，基于滚动窗口的代价地图，以及订阅和配置传感器话题

在ROS Navigation Stack中，代价地图分为全局代价地图和局部代价地图，全局代价地图使用基于map_server初始化代价地图，也就是Static Map Layer（静态地图层），局部代价地图为基于滚动窗口的代价地图

代价地图还包括Obstacle Map Layer（障碍物层）和Inflation Layer（膨胀层），有时候根据应用场景的需要也可以加入用户自定义层，用户自定义层可以用插件来实现。障碍物层是将传感器感知到的障碍物加入到代价地图中

上面这张图为ROS wiki上的一个示例，可以看到图中灰色部分即为静态地图，红色部分为传感器感知到的障碍物，蓝色部分为膨胀层。红色多边形表示机器人的形状

move_base

move_base是整个ROS Navigation Stack的最顶层，它将各个功能模块组合起来，通过SimpleActionServer接收目标点并完成导航任务

move_base支持任何遵循在nav_core包中指定的nav_core::BaseGlobalPlanner接口的全局规划器和任何遵循在nav_core包中指定的 nav_core::BaseLocalPlanner 接口的局部规划器

move_base执行的流程是通过状态机来控制是执行规划行为还是恢复行为，当规划失败就会执行恢复行为，如果所有恢复行为也都失败就会退出导航并报告问题

规划的流程是通过回调函数接收目标点，同时会有一个线程不断地跑全局规划器，然后局部规划器以一定频率对规划出来的路径进行跟踪，计算出速度指令，最终到达目标点就重置规划状态

nav_core

nav_core功能包为用于导航的机器人行为提供了通用的接口，其中定义了三个抽象类，分别是BaseGlobalPlanner、BaseLocalPlanner和RecoveryBehavior

从上图可以清楚地看到move_base的整个导航流程以及各部分使用的nav_core功能包定义的接口

通过阅读源码我们可以知道，BaseGlobalPlanner中定义了纯虚函数makePlan()，makePlan()用于实现规划全局路径

BaseLocalPlanner中定义了纯虚函数setPlan()、isGoalReached()和computeVelocityCommands()，setPlan()用于实现设定局部规划器跟踪的路径，isGoalReached()用于实现判断是否到达目标点，computeVelocityCommands()用于实现计算安全的速度指令

RecoveryBehavior定义了纯虚函数runBehavior()，runBehavior()用于实现执行恢复行为

global_planner（全局规划器）

对于move_base的全局规划器，我们主要了解global_planner功能包

global_planner功能包实现了两种路径规划算法：A*和dijkstra，并实现了OrientationFilter类，用于对路径进行优化

global_planner功能包的GlobalPlanner继承了nav_core:: BaseGlobalPlanner，实现了makePlan()函数。makePlan()函数的基本流程就是输入起点和终点，读取代价地图，使用A*或者dijkstra算法进行规划，调用OrientationFilter类中的方法对路径进行优化，将规划好的路径以话题的形式发布

local_planner（局部规划器）

局部规划器是move_base中最复杂的部分，实现局部规划器要继承nav_core包的BaseLocalPlanner类

常用的局部规划器是dwa_local_planner，dwa_local_planner提供了在二维平面进行局部路径规划的动态窗口法的实现。动态窗口法主要是在速度空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动

局部规划器的一个基本流程就是读取局部代价地图，设定要跟踪的全局路径，对全局路径进行分段，根据分段的全局路径的坐标进行局部规划，计算机器人每个周期（采样周期）内的线速度、角速度，使之尽量符合全局最优路径，并实现实时避障

以DWA局部规划器为例，当规划全局路径的线程plan Thread在控制循环中输出新的路径时，新的路径将通过setPlan()传递给DWAPlannerROS，直接保存为global_plan_。如果isGoalReached()返回false，程序继续执行调用computeVelocityCommands()。在computeVelocityCommands()中会调用getLocalPlan()对全局路径进行分段，并调用dwaComputeVelocityCommands()使用DWA算法计算最优速度，最后在move_base中发布速度指令。当机器人到达目标位姿且速度小于停止速度时，isGoalReached()返回true

recovery_behaviors（恢复行为）

上图是move_base默认的恢复行为的执行流程，可以理解为机器人通过旋转底盘，来清除代价地图中的障碍物，经过清除以后如果可以找到可走的路径，就继续导航，否则认为目标点不可达并报告终止导航任务

从源码来看，move_base中包括了clear_costmap_recovery、move_slow_and_clear和rotate_recovery这三个有关恢复行为的功能包

• clear_costmap_recovery实现的是将给定区域内的障碍物从代价地图中清除
• move_slow_and_clear实现的是清除代价地图中的障碍物，并在用户给定的限制速度下移动一定的距离，这可能会导致机器人发生碰撞
• rotate_recovery实现的是通过使机器人旋转一周来清除代价地图中的障碍物

问题

1、自适应蒙特卡洛定位和里程计定位的区别？

2、代价地图的设置，自车膨胀与障碍物膨胀的区别？

3、全局规划在导航过程中是否只是执行一次？

4、全局规划与局部规划的关系，局部规划如何对全局路径进行分段？

5、不同恢复行为的触发时机是什么，不同恢复行为的优缺点？

6、如何自定义实现全局路径规划/局部路径规划的插件？

参考

理解ROS Navigation Stack，看完这篇你就知道啦！

Setup and Configuration of the Navigation Stack on a Robot

ROS: global_planner 整体解析

Navigation包 Global_planner全局路径规划源码详细解析

move_base代码学习一

base_local_planner源码解读