SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——3.ros-navigation机器人自主避障导航..._kinetic-devel

SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——3.ros-navigation机器人自主避障导航

摘要                                            

通过前面的基础学习，本章进入最为激动的机器人自主导航的学习。在前面的学习铺垫后，终于迎来了最大乐趣的时刻，就是赋予我们的miiboo机器人能自由行走的生命。本章将围绕机器人SLAM建图、导航避障、巡航、监控等内容展开。本章内容：

1.在机器人上使用传感器

2.google-cartographer机器人SLAM建图

3.ros-navigation机器人自主避障导航

4.多目标点导航及任务调度

5.机器人巡航与现场监控

---

温馨提示：

本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》，感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习，购买链接如下：

点这里购买：《机器人SLAM导航核心技术与实战》购买链接

---

3.ros-navigation机器人自主避障导航

前面的学习教程打好了必须的基础，现在就正式开始探讨ROS系统最强大的特性之一，让我们的机器人能自主导航和避障。这都得益于开源社区和共享代码，使ROS拥有大量可用的导航算法。将这些导航算法集大成者，便是ros-navigation导航功能包集。了解更多ros-navigation的信息，请参考官方wiki教程：navigation - ROS Wiki。

3.1.机器人自主避障导航原理分析         

要分析导航功能包的原理，肯定需要先参考下面这张ros-navigation官方给出的系统框图，如图30。最中心的是move_base节点，是导航过程运动控制的最终执行机构，move_base订阅用户发布的导航目标move_base_simple/goal，并将实时运动控制信号cmd_vel下发给底盘以实现最终运动控制，move_base中的各种导航算法模块都是以插件的形式进行调用的，这样可以很方便的替换不同的算法以适应不同的应用，其中global_planner用于全局路径规划、local_planner用于局部路径规划、golobal_costmap是全局代价地图用于描述全局环境信息、local_costmap是局部代价地图用于描述局部环境信息、recovery_behaviors是恢复策略用于机器人碰到障碍后自动进行逃离恢复。然后是amcl节点，amcl节点利用粒子滤波算法实现机器人的全局定位，为机器人导航提供全局位置信息。再然后是map_server节点，map_server节点通过调用前面SLAM建图得到的地图为导航提供环境地图信息。最后就是要提供机器人模型相关的tf信息、里程计odom信息、激光雷达信息scan。

（图30）ros-navigation导航功能包集系统框图

机器人全局定位amcl粒子滤波算法：

amcl的全称是自适应蒙特卡洛粒子滤波，这里通过讲解粒子滤波、重要性采样、机器人绑架、自适应蒙特卡洛这几个概念来说明机器人全局定位的原理。由于amcl的数学理论比较复杂，限于篇幅这里不展开讲解，感兴趣的朋友可以参考《Probabilistic Robotics》这本书，里面有详细的推导过程，如图31。

（图31）概率机器人

粒子滤波，是一种思想，比如要计算一个矩形里面一个不规则形状的面积，这个问题不好直接计算，但是可以拿一把豆子均匀撒到矩形中，统计落在不规则形状中豆子的占比就能算出其面积了。在机器人定位问题中，我们在地图的任意位置撒上许多粒子点，然后通过传感器观测数据按照一定的评价方法对每个粒子点进行打分，评分高的粒子点表示机器人有更大的可能在此位置；在下一轮撒点时，就在评分高的粒子点附近多撒一些点，这样通过不断的迭代，粒子点就会聚拢到一个地方。这个粒子点聚集的地方，就是机器人位置的最优估计点。如图32，红色的粒子点慢慢聚拢到一团。

（图32）粒子滤波

重要性采样，在粒子滤波的迭代过程中，评分高的粒子点会被下一轮迭代时更加看重，这样不断迭代真实估计值附近的粒子点会越来越多。

机器人绑架，当机器人被突然从一个地方抱走到另一个地方，这个时候前一轮迭代得到的粒子点完全不能在新的位置上试用，这样继续迭代下去就会发生位置估计的错误。

自适应蒙特卡洛，自适应主要体现在两个方面。通过判断粒子点的平均分突变来识别机器人绑架问题，并在全局重新撒点来解决机器人绑架问题；通过判断粒子点的聚集程度来确定位置估计是否准确，在估计比较准确的时候降低需要维护的粒子点数目，这样来降低算法的计算开销。

代价地图costmap：

代价地图用于描述环境中的障碍物信息，代价地图利用激光雷达、声呐、红外测距等探测传感器的数据来生成，大致的原理是通过建立不同的图层Layer然后叠在一起，被填充的栅格点表示有障碍物，如图33，想要了解代价地图的更多细节可以阅读《Layered Costmaps for Context-Sensitive Navigation》这篇论文。机器人导航中用到了两个代价地图，全局代价地图global_costmap和局部代价地图local_costmap。

（图33）代价地图

路径规划：

机器人导航中核心的就是路径规划，路径规划是通过利用环境障碍物信息找到一条到达目标并且开销小的路劲。导航中会用到两种路径规划，即全局路径规划global_planner和局部路径规划local_planner。全局路径规划更像是一种战略性策略，需要考虑全局，规划处一条尽量短并且易于执行的路径。在全局路径的指导下，机器人在实际行走时还需要考虑周围实时的障碍物并制定避让策略，这就是局部路径规划要完成的事，可以说机器人的自主导航最终是由局部路径规划一步步完成的。ROS中推荐的全局路径规划器是global_planner，基于A*算法，如图34。

（图34）基于A*算法的全局路径规划

由于ROS中的局部路径规划器比较老了，所以我推荐大家使用比较新的局部路径规划器teb_local_planner。teb_local_planner是基于弹性时间带碰撞约束的算法，算法将动态障碍物、运行时效、路径平滑性等约束做综合考虑，在复杂环境下有更优秀的表现，如图35。

（图35）基于弹性时间带碰撞算法的局部路径规划

3.2.ros-navigation安装             

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

这里将建立一个叫catkin_ws_nav的ROS工作空间，专门用于存放机器人导航相关的功能包。关于创建ROS工作空间的操作，请参考前面相应部分内容，这里就不做讲解。

安装命令很简单，执行下面的命令编译安装就行了。

首先前往https://github.com/ros-planning/navigation，将分支切换到kinetic-devel然后下载源码到本地，将navigation-kinetic-devel.zip解压备用。将解压好的navigation-kinetic-devel文件夹拷贝到~/catkin_ws_nav/src/后，就可编译安装了。

#编译过程可能会遇到找不到Bullet等依赖问题，解决这些依赖简便办法
#先apt-get安装ros-navigation包，这样依赖会被自动装上 
sudo apt-get install ros-kinetic-navigation* 
#再apt-get卸载掉ros-navigation包 
sudo apt-get remove ros-kinetic-navigation ros-kinetic-navigation-experimental
 
cd  ~/catkin_ws_nav/
catkin_make

然后需要安装teb_local_planner，前往https://github.com/rst-tu-dortmund/teb_local_planner，将分支切换到kinetic-devel然后下载源码到本地，并解压源码包到~/catkin_ws_nav/src/后，就可以编译安装了。

#安装依赖
source ~/catkin_ws_nav/devel/setup.bash 
rosdep install teb_local_planner
 
cd  ~/catkin_ws_nav/
catkin_make  -DCATKIN_WHITELIST_PACKAGES="teb_local_planner"

值得一提的是，teb_local_planner源码中关于plugin的配置文件均已写好，直接编译源码就能完成plugin的注册及插入，非常方便。

3.3.ros-navigation使用              

ros-navigation功能包集是一个很强大的导航框架，支持几乎所有的移动机器人。我们只需要按照适当的配置就可以将ros-navigation应用到我们自己的机器人。为了更好的管理配置文件和启动节点，我们需要先为miiboo机器人建立一个功能包miiboo_nav，用于专门存放我们机器人的导航配置与启动文件。miiboo_nav功能包文件结构，如图36。不难发现，miiboo_nav是一个不包含任何可执行源码的功能包，里面只包含config和launch。config中存放导航中各算法模块的参数配置，launch中存放启动导航所需的各种节点的启动文件。下面依次介绍这些文件的作用。

（图36）miiboo_nav功能包文件结构

机器人全局定位AMCL算法配置amcl.launch.xml：

导航中用到的全局定位AMCL算法功能包包含很多可以配置的参数，这里建立配置文件amcl.launch.xml对这些参数进行配置，内容如图37。配置参数分为3类：粒子滤波参数、雷达模型参数、里程计模型参数。由于参数比较多，关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：amcl - ROS Wiki。

（图37）全局定位AMCL算法配置amcl.launch.xml

代价地图公有参数配置costmap_common_params.yaml：

全局代价地图和局部代价地图公有的参数被放置在costmap_common_params.yaml这里面，这样只需要载入这个配置文件就能完成共有参数的配置，避免了在全局代价地图和局部代价地图配置文件中重复配置一些相同参数的冗余，内容如图38。配置参数分为2类：机器人形状和代价地图各Layer图层。机器人形状可以用多边形或圆形描述，我们的miiboo机器人是矩形的，所以选多边形描述。代价地图各Layer图层包括：静态层static_layer（由SLAM建立得到的地图提供数据）、障碍层obstacle_layer（由激光雷达等障碍扫描传感器提供实时数据）、全局膨胀层global_inflation_layer（为全局代价地图提供膨胀效果）、局部膨胀层global_inflation_layer（为局部代价地图提供膨胀效果）。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：costmap_2d - ROS Wiki。

（图38）代价地图共有参数配置costmap_common_params.yaml

全局代价地图参数配置global_costmap_params.yaml：

全局代价地图是以插件的形式组建的，在costmap_common_params.yaml中定义的各Layer图层都可以通过插件的形式放入全局代价地图中，各Layer图层的组合可以根据需求自由选择，如图39。我们的miiboo机器人的全局代价地图只用了static_layer和global_inflation_layer两个图层。

（图39）全局代价地图参数配置global_costmap_params.yaml

局部代价地图参数配置global_costmap_params.yaml：

局部代价地图和全局代价地图类似，就不展开讲解了，如图40。我们的miiboo机器人的局部代价地图只用了obstacle_layer和local_inflation_layer两个图层。

（图40）局部代价地图参数配置local_costmap_params.yaml

全局路径规划器参数配置global_planner_params.yaml：

全局路径规划器也是以插件的形式被使用，这里主要对全局路径规划器GlobalPlanner插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图41。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：

global_planner - ROS Wiki。

（图41）全局路径规划器参数配置global_planner_params.yaml

局部路径规划器参数配置teb_local_planner_params.yaml：

局部路径规划器也是以插件的形式被使用，这里主要对局部路径规划器TebLocalPlannerROS插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图42。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：

teb_local_planner - ROS Wiki。

（图42）局部路径规划器参数配置teb_local_planner_params.yaml

导航核心控制参数配置move_base_params.yaml：

导航核心控制由move_base节点最终实现，move_base在整个导航框架中处于核心地位，全局代价地图、局部代价地图、全局路径规划、局部路径规划、恢复策略等等都将在这里得到具体实现的调用。配置内容如图43。不难发现，最开始两行便对调用的全局路径规划器和局部路径规划器进行了申明，然后就是一些运动控制方面的参数。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：move_base - ROS Wiki。

（图43）导航核心控制参数配置move_base_params.yaml

启动导航所需的各种节点的启动文件miiboo_nav.launch：

在config目录中将各个导航模块的参数配置好后，就可以在一个启动文件中将导航所需的各种节点进行启动，并载入相关配置。启动文件miiboo_nav.launch就是来完成这个任务的，如图44。不难发现，第一个启动项是map_server，将SLAM建图中得到的地图进行载入后发布到ROS中供导航中需要的模块使用；第二个启动项是amcl，amcl开始运行后将为导航中的模块提供机器人在地图中的全局位置；第三个启动项是move_base，在这里将载入代价地图公有参数、全局代价地图参数、局部代价地图参数、move_base参数、全局路径规划参数、局部路径规划参数。

（图44）启动导航所需的各种节点的启动文件miiboo_nav.launch

到这里，我们已经准备好了导航的各个配置文件和启动文件，现在就正式来进行导航。这里需要特别提醒，我们假设已经在SLAM建图过程得到了一张比较好的环境地图，并保存到了正确的路径。启动导航分为3步：启动机器人上所有传感器、启动导航所需各个节点、发送导航目标点。

首先，启动机器人上所有传感器，打开终端，通过下面的命令直接启动就行了。

source ~/catkin_ws/devel/setup.bash 
roslaunch miiboo_bringup miiboo_all_sensor.launch 

然后，启动导航所需各个节点，打开终端，通过下面的命令直接启动就行了。

source ~/catkin_ws_nav/devel/setup.bash 
roslaunch miiboo_nav miiboo_nav.launch

最后，发送导航目标点。这里有两种方式来下发导航目标点，方法一是通过rviz交互界面，方法二是通过手机APP交互界面。下面对两种方法分别介绍。

方法一：

在PC端打开rviz，打开一个新终端，运行rviz启动命令。

rosrun rviz rviz

订阅/map、/scan、/tf等信息，并观察机器人的初始位置是否正确，如果机器人的初始位置不正确，需要用[2D Pose Estimate]按钮手动给定一个正确的初始位置，如图45。操作方法很简单，先点击[2D Pose Estimate]按钮，然后将鼠标放置到机器人在地图中实际应该的位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。

（图45）在rviz中手动给定机器人初始位置

初始位置设置正确后，就可以用[2D Nav Goal]按钮手动指定导航目标点了，如图46。操作方法很简单，先点击[2D Nav Goal]按钮，然后将鼠标放置到地图中任意的想让机器人到达的空白位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。这样机器人就会开始规划路径并自动导航到该指定目标点。

（图46）在rviz中手动指定导航目标点

方法二：

直接打开手机上的miiboo机器人APP，就可以看到地图和机器人在地图中的位置了。和方法一中的一样，需要观察机器人的初始位置是否正确，如果机器人的初始位置不正确，用[Set Pose]按钮手动给定一个正确的初始位置，如图47。操作过程和方法一类似，就不展开了。

（图47）在miiboo机器人APP中手动给定机器人初始位置

初始位置设置正确后，就可以用[Set Goal]按钮手动指定导航目标点了，如图48。操作过程和方法一类似，就不展开了。

（图48）在miiboo机器人APP中手动指定导航目标点

后记                                                      

如果大家对博文的相关类容感兴趣，或有什么技术疑问，欢迎加QQ技术交流群（117698356）

参考文献

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

 购书链接：https://item.jd.com/13041503.html

下载更多资料：www.xiihoo.com

QQ技术讨论群： 117698356

B站视频教程：https://space.bilibili.com/66815220

Github源码：https://github.com/xiihoo/Books_Robot_SLAM_Navigation

Gitee源码（国内访问速度快）：https://gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

序

前言

编程基础篇

第1章 ROS入门必备知识

1.1 ROS简介 2

1.1.1 ROS的性能特色 2

1.1.2 ROS的发行版本 3

1.1.3 ROS的学习方法 3

1.2 ROS开发环境的搭建 3

1.2.1 ROS的安装 4

1.2.2 ROS文件的组织方式 4

1.2.3 ROS网络通信配置 5

1.2.4 集成开发工具 5

1.3 ROS系统架构 5

1.3.1 从计算图视角理解ROS架构 6

1.3.2 从文件系统视角理解ROS架构 7

1.3.3 从开源社区视角理解ROS架构 8

1.4 ROS调试工具 8

1.4.1 命令行工具 9

1.4.2 可视化工具 9

1.5 ROS节点通信 10

1.5.1 话题通信方式 12

1.5.2 服务通信方式 15

1.5.3 动作通信方式 19

1.6 ROS的其他重要概念 25

1.7 ROS 2.0展望 28

1.8 本章小结 28

第2章 C++编程范式

2.1 C++工程的组织结构 29

2.1.1 C++工程的一般组织结构 29

2.1.2 C++工程在机器人中的组织结构 29

2.2 C++代码的编译方法 30

2.2.1 使用g++编译代码 31

2.2.2 使用make编译代码 32

2.2.3 使用CMake编译代码 32

2.3 C++编程风格指南 33

2.4 本章小结 34

第3章 OpenCV图像处理

3.1 认识图像数据 35

3.1.1 获取图像数据 35

3.1.2 访问图像数据 36

3.2 图像滤波 37

3.2.1 线性滤波 37

3.2.2 非线性滤波 38

3.2.3 形态学滤波 39

3.3 图像变换 40

3.3.1 射影变换 40

3.3.2 霍夫变换 42

3.3.3 边缘检测 42

3.3.4 直方图均衡 43

3.4 图像特征点提取 44

3.4.1 SIFT特征点 44

3.4.2 SURF特征点 50

3.4.3 ORB特征点 52

3.5 本章小结 54

硬件基础篇

第4章 机器人传感器

4.1 惯性测量单元 56

4.1.1 工作原理 56

4.1.2 原始数据采集 60

4.1.3 参数标定 65

4.1.4 数据滤波 73

4.1.5 姿态融合 75

4.2 激光雷达 91

4.2.1 工作原理 92

4.2.2 性能参数 94

4.2.3 数据处理 96

4.3 相机 100

4.3.1 单目相机 101

4.3.2 双目相机 107

4.3.3 RGB-D相机 109

4.4 带编码器的减速电机 111

4.4.1 电机 111

4.4.2 电机驱动电路 112

4.4.3 电机控制主板 113

4.4.4 轮式里程计 117

4.5 本章小结 118

第5章 机器人主机

5.1 X86与ARM主机对比 119

5.2 ARM主机树莓派3B+ 120

5.2.1 安装Ubuntu MATE 18.04 120

5.2.2 安装ROS melodic 122

5.2.3 装机软件与系统设置 122

5.3 ARM主机RK3399 127

5.4 ARM主机Jetson-tx2 128

5.5 分布式架构主机 129

5.5.1 ROS网络通信 130

5.5.2 机器人程序的远程开发 130

5.6 本章小结 131

第6章 机器人底盘

6.1 底盘运动学模型 132

6.1.1 两轮差速模型 132

6.1.2 四轮差速模型 136

6.1.3 阿克曼模型 140

6.1.4 全向模型 144

6.1.5 其他模型 148

6.2 底盘性能指标 148

6.2.1 载重能力 148

6.2.2 动力性能 148

6.2.3 控制精度 150

6.2.4 里程计精度 150

6.3 典型机器人底盘搭建 151

6.3.1 底盘运动学模型选择 152

6.3.2 传感器选择 152

6.3.3 主机选择 153

6.4 本章小结 155

SLAM篇

第7章 SLAM中的数学基础

7.1 SLAM发展简史 158

7.1.1 数据关联、收敛和一致性 160

7.1.2 SLAM的基本理论 161

7.2 SLAM中的概率理论 163

7.2.1 状态估计问题 164

7.2.2 概率运动模型 166

7.2.3 概率观测模型 171

7.2.4 概率图模型 173

7.3 估计理论 182

7.3.1 估计量的性质 182

7.3.2 估计量的构建 183

7.3.3 各估计量对比 190

7.4 基于贝叶斯网络的状态估计 193

7.4.1 贝叶斯估计 194

7.4.2 参数化实现 196

7.4.3 非参数化实现 202

7.5 基于因子图的状态估计 206

7.5.1 非线性最小二乘估计 206

7.5.2 直接求解方法 206

7.5.3 优化方法 208

7.5.4 各优化方法对比 218

7.5.5 常用优化工具 219

7.6 典型SLAM算法 221

7.7 本章小结 221

第8章 激光SLAM系统

8.1 Gmapping算法 223

8.1.1 原理分析 223

8.1.2 源码解读 228

8.1.3 安装与运行 233

8.2 Cartographer算法 240

8.2.1 原理分析 240

8.2.2 源码解读 247

8.2.3 安装与运行 258

8.3 LOAM算法 266

8.3.1 原理分析 266

8.3.2 源码解读 267

8.3.3 安装与运行 270

8.4 本章小结 270

第9章 视觉SLAM系统

9.1 ORB-SLAM2算法 274

9.1.1 原理分析 274

9.1.2 源码解读 310

9.1.3 安装与运行 319

9.1.4 拓展 327

9.2 LSD-SLAM算法 329

9.2.1 原理分析 329

9.2.2 源码解读 334

9.2.3 安装与运行 337

9.3 SVO算法 338

9.3.1 原理分析 338

9.3.2 源码解读 341

9.4 本章小结 341

第10章 其他SLAM系统

10.1 RTABMAP算法 344

10.1.1 原理分析 344

10.1.2 源码解读 351

10.1.3 安装与运行 357

10.2 VINS算法 362

10.2.1 原理分析 364

10.2.2 源码解读 373

10.2.3 安装与运行 376

10.3 机器学习与SLAM 379

10.3.1 机器学习 379

10.3.2 CNN-SLAM算法 411

10.3.3 DeepVO算法 413

10.4 本章小结 414

自主导航篇

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

11.1 自主导航 418

11.2 环境感知 420

11.2.1 实时定位 420

11.2.2 环境建模 421

11.2.3 语义理解 422

11.3 路径规划 422

11.3.1 常见的路径规划算法 423

11.3.2 带约束的路径规划算法 430

11.3.3 覆盖的路径规划算法 434

11.4 运动控制 435

11.4.1 基于PID的运动控制 437

11.4.2 基于MPC的运动控制 438

11.4.3 基于强化学习的运动控制 441

11.5 强化学习与自主导航 442

11.5.1 强化学习 443

11.5.2 基于强化学习的自主导航 465

11.6 本章小结 467

第12章 典型自主导航系统

12.1 ros-navigation导航系统 470

12.1.1 原理分析 470

12.1.2 源码解读 475

12.1.3 安装与运行 479

12.1.4 路径规划改进 492

12.1.5 环境探索 496

12.2 riskrrt导航系统 498

12.3 autoware导航系统 499

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

12.5 本章小结 500

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

13.1 运行机器人上的传感器 502

13.1.1 运行底盘的ROS驱动 503

13.1.2 运行激光雷达的ROS驱动 503

13.1.3 运行IMU的ROS驱动 504

13.1.4 运行相机的ROS驱动 504

13.1.5 运行底盘的urdf模型 505

13.1.6 传感器一键启动 506

13.2 运行SLAM建图功能 506

13.2.1 运行激光SLAM建图功能 507

13.2.2 运行视觉SLAM建图功能 508

13.2.3 运行激光与视觉联合建图功能 508

13.3 运行自主导航 509

13.4 基于自主导航的应用 510

13.5 本章小结 511

附录A Linux与SLAM性能优化的探讨

附录B 习题