SLAM导航机器人零基础实战系列：（六）SLAM建图与自主避障导航——4.多目标点导航及任务调度
摘要
通过前面的基础学习，本章进入最为激动的机器人自主导航的学习。在前面的学习铺垫后，终于迎来了最大乐趣的时刻，就是赋予我们的miiboo机器人能自由行走的生命。本章将围绕机器人SLAM建图、导航避障、巡航、监控等内容展开。本章内容：
2.google-cartographer机器人SLAM建图
温馨提示：
本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》，感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习，购买链接如下：
4.多目标点导航及任务调度
通过前面的学习，我们已经可以通过点击地图的方式来命令机器人运动到目标点。其实，ros-navigation导航框架就是为我们提供了一个最基本的机器人自动导航接口，即单点导航。然而，在实际的机器人应用中，机器人往往要完成复杂的任务，这些复杂的任务都是由一个个基本的任务组合而成的。一般的，机器人通过状态机的形式将一个个基本任务组合在一起来进行复杂任务的调度实现。
4.1.状态机
这里我们只讨论有限状态机,也称为FSM(Finite State Machine)，其在任意时刻都处于有限状态集合中的某一状态。当其获得一个输入条件时，将从当前状态转换到另一个状态，或者仍然保持在当前状态。任何一个FSM都可以用状态转换图来描述，图中的节点表示FSM中的一个状态，有向加权边表示输入条件时状态的变化。如图49，以一个上班族的生活场景来举例说明状态机的状态转换图。矩形框表示FSM中的一个状态，有向边表示在输入条件下的状态转换过程。
（图49）有限状态机FSM举例
4.2.多目标点巡航
机器人多目标点巡航，特别是按特定巡逻路径进行巡航是很实用的功能。这里将利用前面学到的ros-navigation单点导航、状态机、状态机任务调度的知识。我们来编写一个应用功能包实现机器人多目标点巡航。
到这里，我们慢慢清楚了miiboo机器人编程的框架思路，我们将传感器相关的底层驱动包放在~/catkin_ws/工作空间统一管理，将基于google-cartographer的SLAM建图程序包放在~/catkin_ws_carto/工作空间统一管理，将基于ros-navigation的导航程序包放在~/catkin_ws_nav/工作空间统一管理，将高层应用功能包放在~/catkin_ws_apps/工作空间统一管理。miiboo机器人编程的框架思路，如图50。
（图50）miiboo机器人编程的框架思路
这里将建立一个叫catkin_ws_apps的ROS工作空间，专门用于放置日后开发的各种应用层功能包。关于创建ROS工作空间的操作，请参考前面相应部分内容，这里就不做讲解。在~/catkin_ws_apps/src/中建立一个叫patrol的功能包，建好后的patrol功能包文件结构，如图51。
（图51）patrol功能包文件结构
关于功能包的文件结构，大家已经很熟悉了，就不啰嗦了。这里重点讲解一下patrol_fsm.py这个文件，文件内容如图52。
（图52）patrol_fsm.py文件内容
这里采用python来编写多目标点巡航的逻辑，python开发ROS节点的优点是简洁高效。代码中waypoints数组里面存放的是要巡航的各个目标点，大家可以根据自己的需要进行相应的替换和增减；with patrol代码块里面实现状态机的构建；最后调用状态机的执行函数，状态机就开始工作了，也就是开始执行巡航了。
启动多目标点巡航分为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最后，启动多目标点巡航节点，打开终端，通过下面的命令直接启动就行了。
source ~/catkin_ws_apps/devel/setup.bash
roslaunch patrol patrol.launch4.3.复杂多任务机器人未来展望
机器人可以进行自动导航、人机对话、用机械臂抓取物体、物体识别等。将这些任务结合起来，利用机器人强大的大脑推理机制能完成更为复杂和智能化的任务。如果说基于状态机的复杂任务调度是1.0版本的智能，那么基于大脑推理机制的复杂任务调度将是2.0版本的智能。
我的设想是利用强化学习神经网络作为大脑推理机制的实现实体，如图53。自动导航、人机对话、用机械臂抓取物体、物体识别等任务组合的整体作为机器人与外界环境交互的动作空间，动作空间的的状态分为两种形态：执行结果、执行策略。执行结果作为强化学习神经网络的输入，而执行策略作为强化学习神经网络的输出。我们不断通过各种复杂的实际场景的粒子来训练机器人，让机器人能在复杂场景下能做正确的事情。比如说，当机器人收到主人“我渴了”的语音信息后，自动导航到桌子边，然后识别桌上的可乐，并用机械臂抓取，最后递给主人，并提醒主人“你的可乐来了”。
（图53）强化学习神经网络作为大脑推理机制
哈哈！这样的想法很炫酷，不过以目前的技术实现难度还比较大，所以作为未来展望分享给大家。希望和大家一起努力，在不远的将来能实现这个梦想。
后记
如果大家对博文的相关类容感兴趣，或有什么技术疑问，欢迎加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 习题