一起自学SLAM算法：12.1 ros-navigation导航系统

连载文章，长期更新，欢迎关注：

---

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

        12.1 ros-navigation导航系统

        12.2 riskrrt导航系统

        12.3 autoware导航系统

        12.4 导航系统面临的一些挑战

第13章-机器人SLAM导航综合实战

---

可以说ros-navigation是ROS系统中最重要的组件之一，绝大部分自主移动机器人的导航功能都是基于ros-navigation导航系统实现的。下面将从原理分析、源码解读和安装与运行这3个方面展开讲解ros-navigation导航系统。

12.1.1 ros-navigation原理分析

从图11-24来看，导航系统就是以导航目标、定位信息和地图信息为输入，以操控机器人的实际控制量为输出。首先要知道机器人在哪，然后要知道机器人需要到达的目标在哪，最后就是寻找路径并利用控制策略开始导航。导航目标通常人为指定或者特定程序触发，这其实回答了问题“我将到何处去”。定位信息通常由SLAM或者其他定位算法提供，这其实回答了问题“我在哪”。而地图信息为导航起点和终点之间提供了障碍物描述，那么在此基础上就可以利用路径规划寻找路径并利用控制策略输出实际线速度和角速度控制量进行导航了。而ros-navigation导航系统的实现也遵循了这样的基本思路，其中所涉及的很多理论知识已经在第11章中讨论过了，这里就主要对其中比较特别的AMCL定位和Costmap代价地图两个概念进行介绍吧。

1.AMCL定位

在ros-navigation中提供了一种比SLAM定位更轻量级的方案，即AMCL[1]（Adaptive Monte Carlo Localization，自适应蒙特卡洛定位）定位方案。AMCL包含两种代码实现，即用于二维地图定位的代码实现amcl和用于三维地图定位的代码实现amcl3d，其中ros-navigation中已经默认集成了二维地图定位的amcl代码包。从图7-5就能看出单独定位问题比SLAM问题要简单，因为单独的定位问题是在环境地图已知的情况下估计机器人位姿，而SLAM问题在地图未知的情况下同时估计机器人位姿和地图。不过当SLAM载入已建立好的地图进入重定位模式时，可以认为SLAM重定位模式等价于单独定位问题。从某种意义上说，AMCL定位可以看成是Gmapping重定位模式的等价，虽然Gmapping中并没有重定位模式。

在第7章中已经讨论过，无论是SLAM问题还是单独定位问题都属于状态估计问题。对于单独定位问题来说，机器人位姿被看成随机变量，求解定位问题其实就是求解随机变量的概率分布，然后以的期望做为机器人位姿的估计值。其中贝叶斯估计方法是求解该状态估计的经典方法之一，另一种求解方法是优化方法。而贝叶斯估计又可以具体分为参数化实现和非参数化实现两种，参数化实现以卡尔曼滤波算法为代表，而非参数化实现以粒子滤波算法为代表。AMCL是MCL[2]（Monte Carlo Localization，蒙特卡洛定位）的改进版本，而MCL属于粒子滤波的范畴，因此AMCL也就属于粒子滤波的范畴了。

蒙特卡洛（Monte Carlo，MC）是一种将概率现象用统计试验方法来进行数值模拟的思想，基于蒙特卡洛思想衍生出了大量的优秀算法。比如为了求解矩形内某个不规则形状的面积，可以在矩形内均匀撒上很多大米粒，通过统计落在该不规则形状内大米粒的数量与矩形内所有大米粒的占比就可以求出该不规则形状的面积；还有11.5.1节中介绍的基于采样的强化学习方法也有了蒙特卡洛思想的身影；还有这里用于求解机器人定位问题的粒子滤波也体现了蒙特卡洛思想。在求解机器人定位问题的粒子滤波算法中，将机器人的待估计位姿量的概率分布用空间内的粒子来模拟，粒子点的分布密度近似代表的概率密度（也叫置信度），即粒子点聚集的地方说明机器人出现在该地方的置信度也就高，通过机器人观测方程和运动方程所提供的数据对粒子点的分布情况不断进行更新使粒子点最终收敛于某个很小的区域。由于更新粒子点分布的方法有很多，所以求解机器人定位问题的粒子滤波算法也有很多种，比如8.1节中的改进RBPF，还有这里讨论的MCL和AMCL，了解更多粒子滤波算法家族中的其他成员请参考文献[3]。

与RBPF一样，MCL也属于SIR（sampling importance resampling）滤波器的范畴。因此MCL的原理也体现在重采样过程，也就是利用观测方程和运动方程所提供的数据来评估当前每个粒子点的重要性权重，然后依据每个粒子点的权重进行重采样来更新粒子点的分布。AMCL对MCL做了两方面的改进，一个改进是将MCL中的固定的粒子数量替换成了自适应的粒子数量，另一个改进是增加了MCL遭遇绑架后的恢复策略。

当粒子点比较分散时，粒子点总数可以设多一点；当粒子点比较聚集时，聚集在一起的粒子点会出现冗余，这时可以将粒子点总数可以设少一点来减少计算量以提高运行效率。AMCL中的粒子数量自适应有助于提高算法的运行效率，这在计算资源受限的机器中尤为重要。

所谓绑架问题，就是由于观测方程或者运动方程中的数据受噪声干扰或者某些偶然因素导致原本表征机器人真实位姿的粒子点被丢弃了，以后的更新过程也只是错误粒子点的收敛。简单点说就是在某次更新中正确粒子点被意外丢弃后，机器人位姿将永久丢失，即机器人遭遇了绑架。AMCL中引入了一个机制来监控绑架风险，以便在适当情况下启动恢复策略（即增加一些随机粒子点）。对于在真实环境中持续运行的机器人来说，机器人遭遇绑架是必然的，AMCL中引入绑架恢复策略对于机器人的工程应用非常必要。

 2.Costmap代价地图

导航控制策略的首要任务是避障，那么对障碍物的度量就成了关键问题。由于SLAM直接提供的地图格式种类繁多（比如特征地图、点云地图、几何地图、栅格地图、拓扑地图等），这些地图度量障碍物的能力参差不齐。虽然将这些地图格式转换成统一的栅格地图能一定程度上提高障碍物的度量能力，但是这种栅格地图也仅提供环境中静态障碍物的度量。我们知道机器人在导航避障时不仅要考虑SLAM地图所提供的静态障碍信息，还要考虑传感器（比如超声波、红外、深度视觉等）探测到的实时障碍信息，以及一些特殊的障碍信息（比如障碍物膨胀信息、人为划定的危险区域、行人或某些突变的动态语义信息等）。

为了解决各种复杂障碍物的度量问题，ros-navigation采用代价地图（Costmap）对障碍物进行统一度量。Costmap[4]采用多个独立的栅格化图层来维护障碍物信息，每个图层可以独立维护某个来源的障碍物信息，这些图层可以根据不同需求进行叠加形成特定的障碍描述层。Costmap的结构如图12-1所示，外部载入的静态栅格地图由Static Map图层维护、一些特定危险区域由Caution Zones图层维护、激光雷达之类的传感器扫描到的障碍物由Obstacles图层维护、超声波传感器扫描到的障碍物由Sonar图层维护、行人或特殊障碍物由Proxemics图层维护、膨胀效果由Inflation图层维护等，最终将这些图层叠加起来就得到了代价地图的主图层Master。

图12-1  Costmap的结构

Costmap是一个非常灵活的障碍度量方式，因为你可以根据需求创建特定的图层，然后在该图层上维护你需要关注的障碍物信息。假如你的机器人上原本只安装了激光雷达，那么激光雷达扫描到的障碍物放在Obstacles图层维护。如果你在机器人上添加了超声波，那么你只需要新建一个Sonar图层用于维护声波传感器扫描到的障碍物。每个图层都可以有自己的障碍更新规则（添加障碍、删除障碍、更新障碍点的置信度等），这极大提高了导航系统的可扩展性。

3.ros-navigation系统框架

到这里就可以分析ros-navigaion的系统框架了，如图12-2所示。ros-navigaion其实是一个功能包集，里面包含了大量的ROS功能包以及各种算法的具体实现节点。这些节点可以分为3类：必要节点、可选节点和机器人平台相关节点。节点move_base为必要节点，节点amcl和map_server为可选节点，sensor transforms、odometry source、sensor sources和base controller为机器人平台相关节点。其中最为核心的必要节点move_base通过插件机制（plugin）组织代码，这使得move_base中的global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors等算法能被轻易替换和改进。

图12-2  ros-navigation系统框架

可以发现ros-navigation系统框架就是图11-24所示导航通用框架更具体的一种实现形式，map_server节点扮演地图供应者的角色，amcl节点扮演定位信息提供者的角色，传感器驱动节点（sensor sources）和里程计节点（odometry source）分别扮演障碍信息反馈和运动反馈的角色，而底盘控制节点（base controller）扮演执行器的角色。下面从定位、障碍物度量、路径规划和策略恢复这几个方面，对ros-navigation系统框架展开进一步的分析。

（1）定位

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）障碍物度量

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）路径规划

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（4）恢复策略

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

12.1.2 ros-navigation源码解读

上面讨论完ros-navigation的原理，现在就来解读ros-navigation的源码，其代码框架如图12-3所示。代码围绕节点move_base来组织，导航目标点通过话题/move_base_simple/goal输入，地图数据通过话题/map输入，各个传感器数据通过相应的传感器话题输入，里程计数据通过话题/odom输入，而控制量通过话题/cmd_vel输出。同时还要为节点move_base提供必须的tf关系，包括动态tf关系（map->odom->base_link）以及传感器之间的静态tf关系（比如base_link->base_footprint、base_link->laser_link、base_link->imu_link等）。如果ros-navigation采用amcl进行全局定位，那么动态tf关系map->odom由amcl维护，amcl实质上是通过map->base_link与odom->base_link之间的差值来修正map->odom的漂移；如果ros-navigation不采用amcl进行全局定位，那么动态tf关系map->odom则由其他提供全局定位的算法（比如在线的SLAM、重定位的SLAM、UWB、二维码定位等）维护。而动态tf关系odom->base_link由机器人平台里程计维护，该里程计有多种形式，比如轮式里程计、轮式里程计与IMU融合后的里程计、视觉里程计等都是可以的。由于里程计在节点move_base中有不同的用途，因此机器人平台里程计节点需要将里程计数据分别发布到tf关系和/odom话题之中。传感器之间的静态tf关系可以由机器人机械模型urdf提供，也可以由用户手动提供。节点move_base除了topic访问接口外还提供了service和action访问接口，导航目标除了可以由话题/move_base_simple/goal输入还可以通过action接口输入，话题/map主要用于输入实时更新的在线地图，而离线地图则更适合通过service接口输入。可以发现move_base仅仅搭建了一个虚拟的壳体以及各种标准化接口，壳体的各个算法实现部分则通过插件机制（plugin）从外部导入。

图12-3  ros-navigation代码框架

ros-navigation是一个强大的功能包集，除了包含必要功能包move_base外，还包含诸多可选功能包以及各种插件和工具。所以在解读ros-navigation的具体代码之前，有必要了解一下ros-navigation的功能包组织结构，如表12-1所示。amcl和map_server为可选功能包，move_base为必要功能包，nav_core为插件接口组件，navfn、global_planner和carrot_planner为全局路径规划插件，base_local_planner和dwa_local_planner为局部路径规划插件，costmap_2d为代价地图插件，rotate_recovery、move_slow_and_clear和clear_costmap_recovery为恢复策略插件，voxel_grid、fake_localization和move_base_msgs为其他类型功能包。

表12-1  ros-navigation中的功能包

 1.可选功能包

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.必要功能包

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.插件接口组件

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

4.全局路径规划插件

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

5.局部路径规划插件

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

6.代价地图插件

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

7.恢复策略插件

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

8.其他

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

12.1.3 ros-navigation安装与运行

学习完ros-navigation导航系统的原理及源码之后，大家肯定迫不及待想亲自安装运行一下ros-navigation体验一下真实效果。在第1章中已经声明过，本书在Ubuntu18.04和ROS melodic环境下进行讨论。不管是使用X86主机、X86主机虚拟机还是ARM主机，一旦装好Ubuntu18.04系统后，就可以在该系统上安装ROS melodic发行版了。所以，下面的讨论假设Ubuntu18.04和ROS melodic环境已经准备妥当了。

1.ros-navigation安装

ros-navigation的安装有两种方法，方法1是直接通过apt-get安装编译好的ros-navigation库到系统中，方法2是下载ros-navigation源码手动编译安装。由于后续可能需要对ros-navigation中的代码做修改来改进算法，所以这里就采用方法2进行安装。

首先，需要准备好ROS工作空间，关于ROS工作空间的构建，在1.2.2节中已经讨论过了，因此这里不再赘述。

然后，安装ros-navigation的依赖库，网上介绍了很多装依赖库的方法，但后续过程往往还是会出现缺少依赖的错误，这里介绍一种彻底解决依赖问题的巧妙方法。先用apt install的方式将ros-navigation及其关联包都装上，这样系统在安装过程中会自动装好相应的依赖。然后用apt remove将ros-navigation卸载但保留其依赖，这样就巧妙的将所需依赖都装好了。

#安装ros-navigation及其关联功能包，依赖也会随之安装上
sudo apt install ros-melodic-navigation*
#卸载ros-navigation但保留其依赖
sudo apt remove ros-melodic-navigation

接下来，就可以下载ros-navigation的源码到工作空间编译安装了。由于ros-navigation属于功能包集，其中包含了多个功能包，建议新建一个专门的工作空间来维护。

#切换到工作空间目录
cd ~/catkin_ws/src/
#下载ros-navigation源码
git clone https://github.com/ros-planning/navigation.git
cd navigation
#查看代码版本是否为molodic，如果不是请使用git checkout命令切换到对应版本
git branch
#编译
cd ~/catkin_ws/
catkin_make

2.ros-navigation在实际机器人运行

ros-navigation导航系统的强大功能是依靠多功能包协同，需要配置和启动一系列不同的功能包程序才能真正将自主导航跑起来。下面首先介绍使用ros-navigation导航系统涉及的各种配置，然后利用这些配置启动各个功能包将自主导航跑起来。

（1）机器人平台相关节点的配置与启动

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（2）地图供应节点的配置与启动

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（3）全局定位节点的配置与启动

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

（4）导航核心节点的配置与启动

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

12.1.4 路径规划改进

ros-navigation中虽然集成了一些不同的路径规划插件供用户在不同场景下选择使用，其中可选的全局路径规划插件包括navfn、global_planner和carrot_planner，可选的局部路径规划插件包括base_local_planner和dwa_local_planner。不过这些插件还远不能满足研究和工程应用时的多样化需求，此时用户可以选择其他第三方路径规划插件加载到move_base中使用，或者用户根据nav_core的接口规范自己开发所需的路径规划插件。其中SBPL_Lattice_Planner和srl_global_planner为比较典型的第三方全局路径规划插件，teb_local_planner为比较典型的第三方局部路径规划插件，下面就具体介绍一下这3个第三方路径规划插件。

1.基于图搜索的全局路径规划SBPL_Lattice_Planner

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.基于采样的全局路径规划srl_global_planner

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

3.基于弹性带的局部路径规划teb_local_planner

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

12.1.5 环境探索

自主导航通常是在已知环境地图的条件下进行环境交互的行为，也就是从环境的一个地点移动到另一个地点。SLAM则通常是在人为操控的条件下进行环境交互的行为，也就是为未知环境构建地图模型。自主导航和SLAM都是在受限条件下进行环境交互，自主导航的受限条件是环境地图必须已知，SLAM的受限条件是构建未知环境地图的过程需要人为操控。如果将自主导航与SLAM结合起来，就可以让机器人与环境交互的过程真正自主化，即所谓的环境探索。在机器人的环境探索过程中，SLAM为自主导航提供实时更新的地图，自主导航则为SLAM提供操控，这样就能让机器人在完全未知的环境中自主构建地图并利用该地图自主导航，环境探索涉及3个基本问题：建图、定位和路径规划。从某种意义上说，自主导航问题是环境探索问题的一种特殊情况，也就是将环境探索问题中的建图、定位和路径规划之中的建图部分替换成了已知地图。另外就是自主导航问题的导航目标点是从外部获取的某个确定值，而环境探索问题的导航目标点是探索算法根据当前状态临时生成的。

由于环境探索与自主导航非常相似，所以我们可以在ros-navigation导航系统上稍做修改就能实现环境探索。也就是将ros-navigation中可选功能包map_server部分更改成直接从外部获取实时地图数据，至于是否将可选功能amcl部分更改成由SLAM提供全局定位则无所谓，因为SLAM全局定位可能不如amcl全局定位稳健。另外就是将ros-navigation中从外部接收导航目标点的部分更改成从探索程序中自动生成导航目标点。下面介绍两个比较典型的探索程序的实现，即frontier_exploration和rrt_exploration。

1.frontier_exploration

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

2.rrt_exploration

（先占个坑，有时间再来补充详细内容，大家可以直接看文后的参考文献）

源码仓库

• Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation

• Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.