SLAM+语音机器人DIY系列：（六）SLAM建图与自主避障导航——2google
温馨提示 本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》，感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习，购买链接如下：点这里购买：《机器人SLAM导航核心技术与实战》购买链接点这里观看视频讲解：《机器人SLAM导航核心技术与实战》书籍配套教学视频摘要通过前面的基础学习，本章进入最为激动的机器人自主导航的学习。在前面的学习铺垫后，终于迎来了最大乐趣的时刻，就是赋予我们的miiboo机器人能自由行走的生命。本章将围绕机器人SLAM建图、导航避障、巡航、监控等内容展开。本章内容：
1.在机器人上使用传感器
2.google-cartographer机器人SLAM建图
3.ros-navigation机器人自主避障导航
4.多目标点导航及任务调度
5.机器人巡航与现场监控
2.google-cartographer机器人SLAM建图主流的激光SLAM算法有hector、gmapping、karto、cartographer。
hector是一种结合了鲁棒性较好的扫描匹方法2D_SLAM方法和使用惯性传感系统的导航技术。传感器的要求较高，高更新频率小测量噪声的激光扫描仪，不需要里程计。使空中无人机与地面小车在不平坦区域运行存在运用的可能性。作者利用现代激光雷达的高更新率和低距离测量噪声，通过扫描匹配实时地对机器人运动进行估计。所以当只有低更新率的激光传感器时，即便测距估计很精确，对该系统都会出现一定的问题。
gmapping是一种基于粒子滤波的激光SLAM算法，它已经集成在ROS中，是移动机器人中使用最多的SLAM算法。基于粒子滤波的算法用许多加权粒子表示路径的后验概率，每个粒子都给出一个重要性因子。但是，它们通常需要大量的粒子才能获得比较好的的结果，从而增加该算法的的计算复杂性。此外，与PF重采样过程相关的粒子退化耗尽问题也降低了算法的准确性。
karto是基于图优化的SLAM算法，用高度优化和非迭代cholesky矩阵进行稀疏系统解耦作为解。图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新。路标landmark越多,内存需求越大,然而图优化方式相比其他方法在大环境下制图优势更大。
cartographer是google开发的实时室内SLAM项目，cartographer采用基于google自家开发的ceres非线性优化的方法，cartographer的量点在于代码规范与工程化，非常适合于商业应用和再开发。并且cartographer基于submap子图构建全局地图的思想，能有效的避免建图过程中环境中移动物体的干扰。并且cartographer支持多传感器数据（odometry、IMU、LaserScan等）建图，支持2D_SLAM和3D_SLAM建图。所以，我果断采用cartographer来建图，我的树莓派3主板跑cartographer实时建图是十分的流畅，这一点很欣慰^_^
2.1.google-cartographer建图算法原理分析cartographer采用的是主流的SLAM框架，也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图，一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大，但是用submap构建全局地图的长时间过程就会存在很大的累计误差，所以需要利用闭环检测来修正这些submap的位置，闭环检测的基本单元是submap，闭环检测采用scan_match策略。cartographer的重点内容就是融合多传感器数据（odometry、IMU、LaserScan等）的submap子图创建以及用于闭环检测的scan_match策略的实现。
（图18）cartographer算法系统框图
2.2.cartographer_ros安装我们直接参考google-cartographer官方教程安装就行，官方教程分为cartographer和cartographer_ros，其实cartographer就是核心算法层、cartographer_ros是核心算法层的ros调用层。官方教程如下：
https://google-cartographer.readthedocs.io/en/latest/index.html#
https://google-cartographer-ros.readthedocs.io/en/latest/index.html#
直接按照第二个链接cartographer_ros的安装教程，就可将cartographer_ros、cartographer、以及各种依赖都安装了。不过特别说明一点，为了解决从官网下载ceres-solver速度慢的问题，我将ceres-solver的下载地址换到了github源；我需要将官方教程中生成的src/.rosinstall替换成了自己的内容，如图19。其余安装过程和官方教程一模一样。
（1）安装编译工具
我来编译cartographer_ros，我们需要用到wsool和rosdep。为了加快编译，我们使用ninja工具进行编译。
sudoapt-getupdatesudoapt-getinstall-ypython-wstoolpython-rosdepninja-build（2）创建存放cartographer_ros的专门工作空间
mkdircatkin_ws_cartocdcatkin_ws_cartowstoolinitsrcwstoolmerge-tsrchttps://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstallwstoolupdate-tsrc特别说明，在执行wstoolupdate-tsrc之前，需要将src/.rosinstall文件修改成以下内容,以解决ceres-solver下载不了的问题，如图19。
（图19）我修改后的src/.rosinstall文件内容
（3）安装依赖项
安装cartographer_ros的依赖项proto3、deb包等。如果执行sudorosdepinit报错，可以直接忽略。
src/cartographer/scripts/install_proto3.shsudorosdepinitrosdepupdaterosdepinstall--from-pathssrc--ignore-src--rosdistro=${ROS_DISTRO}-y（4）编译和安装
上面的配置和依赖都完成后，就可以开始编译和安装cartographer_ros整个项目工程了。
catkin_make_isolated--install--use-ninja特别提醒，以后对cartographer_ros中的配置文件或源码有改动时，都需要执行这个编译命令使修改生效。
2.3.cartographer_ros使用cartographer_ros整体代码结构分析：
最顶层的是cartographer_ros，作为rosj接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，是非线性优化库，用于求解SLAM中的优化问题。
（图20）cartographer_ros整体代码结构
在miiboo机器人上用cartographer_ros多传感器建图进行配置：
经过前面对cartographer_ros进行安装后，我们肯定迫不及待想在实际的miiboo机器人上使用cartographer_ros进行SLAM建图了。为了最大限度的提高SLAM建图的性能，我们的miiboo机器人提供了激光雷达、IMU、轮式里程计（/scan、/imu、/odom）这三种传感器的数据，所以我们需要先将cartographer_ros配置成对应的工作模式。
cartographer算法是一个非常通用和适应不同平台的开放框架算法，所以支持多种配置与工作模式。我们就来看看cartographer_ros如何进行配置。配置文件由*.lua书写被放在路径cartographer_ros/configuration_files/，我们需要建立一个我们自己的配置文件，取名就叫miiboo_mapbuild.lua吧，具体内容如图21。由于我们的miiboo机器人采用激光雷达、IMU、轮式里程计三种传感器融合建图，所以以下参数一定要设置正确：
参数tracking_frame设置为imu_link，因为我们使用/imu的数据；
参数published_frame设置为odom，因为我们使用/odom的数据；
参数provide_odom_frame设置为false，因为我们使用外部/odom，所以这里不需要内部提供；
参数use_odometry设置为true，因为我们使用外部/odom的数据；
参数use_imu_data设置为true，因为我们使用/imu的数据；
参数imu_gravity_time_constant设置为10，这个是IMU的重力加速度常数。
其余参数根据需要自行调整，由于cartographer是发展很迅速的算法，所以很多代码和文档一直在更新，所以参考官方文档来解读这些参数的含义是最好的选择，官方文档连接地址我贴在下面了。
https://google-cartographer-ros.readthedocs.io/en/latest/index.html
（图21）我们miiboo机器人的建图配置文件miiboo_mapbuild.lua
然后需要配置*.launch文件，我们给miiboo机器人建立启动文件取名叫miiboo_mapbuild.launch，存放路径在cartographer_ros/launch/里面，具体内容如图22。
不难发现launch文件中包含三个node启动项，即urdf模型启动项、cartographer_node启动项、cartographer_occupancy_grid_node启动项。
第一个启动项是启动urdf模型，这个接口是提供给那些只使用cartographer单独建图的应用场景，由于我们miiboo机器人建立完地图后还需要继续进行自动导航任务，所以我们使用miiboo底盘提供的urdf模型，而不使用这里的urdf模型，所以这个启动项被注释掉了，这样建图和导航就更容易管理。
第二个启动项是启动cartographer_node建图节点，这个是SLAM建图主节点，我们建立的配置miiboo_mapbuild.lua将在这里被载入，同时这里可以对建图输入数据scan、imu、odom的topic名称做重映射。
第三个启动项是启动cartographer_occupancy_grid_node地图格式转换节点，由于cartographer_node建图节点提供的地图是submapList格式的，需要转换成GridMap格式才能在ROS中显示和使用。这里面有两个可配参数，resolution用来设置GridMap地图的分辨率，publish_period_sec用来设置GridMap地图发布的频率。
（图22）我们miiboo机器人的建图启动文件miiboo_mapbuild.launch
配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。
catkin_make_isolated--install--use-ninja启动cartographer_ros建图：
要在miiboo机器人上，启动cartographer_ros建图，分为这几个步骤：启动机器人上的各个传感器、启动cartographer_ros、在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。
首先，启动机器人上的各个传感器，为了操作方便，我已经将要启动的传感器都写入miiboo_bringup/launch/miiboo_all_sensor.launch这个启动文件了，文件内容如图23。这个启动文件包含机器人urdf启动项、miiboo底盘启动项、激光雷达启动项、IMU启动项、摄像头启动项、广播IP启动项。
（图23）各个传感器启动文件miiboo_all_sensor.launch
打开终端，通过下面的命令直接启动就行了。
source~/catkin_ws/devel/setup.bashroslaunchmiiboo_bringupmiiboo_all_sensor.launch然后，启动cartographer_ros，由于前面已经做好了相应的配置，所以直接使用命令启动就行了。
source~/catkin_ws_carto/install_isolated/setup.bashroslaunchcartographer_rosmiiboo_mapbuild.launch这里给个小提示，为了查看cartographer_ros建图算法有没有正常开始工作，我们可以用rosrunrqt_tf_treerqt_tf_tree查看整个tf树的结构，正常的tf树如图24。map->odom之间的关系由cartographer建图节点提供，odom->base_footprint之间的关系由miiboo底盘的轮式里程计提供，base_footprint->imu_link和base_link和base_laser_link之间的关系由miiboo机器人的urdf模型提供。从tf树不难看出整个建图过程中机器人定位的实现原理，cartographer建图节点通过维护map->odom之间的关系最终实现全局定位，miiboo底盘的轮式里程计通过维护odom->base_footprint之间的关系来实现局部定位，传感器之间的安装关系由urdf模型提供，这个静态关系主要用于多传感器数据融合。
（图24）cartographer运行时正常的tf树
最后，在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。如果用PC端控制和观察，启动命令如下。
在PC端打开一个新终端，运行rviz启动命令。
rosrunrvizrviz在rviz窗口中添加订阅/map，就可以看到建图效果了，如图25。
（图25）在PC端用rviz观察地图
在PC端再打开一个新终端，运行键盘控制启动命令。
rosrunteleop_twist_keyboardteleop_twist_keyboard.py在该终端下，用键盘就可以控制机器人前进、后退、左转、右转了。
如果是在Android手机端用miiboo机器人APP控制机器人运动和观察地图，直接就能使用，如图26。
（图26）在Android手机端用miiboo机器人APP控制机器人运动和观察地图
保存cartographer_ros建图结果：
当我们在房间里面扫描一圈，地图建立的差不多了，就可以将建图结果保存下来了，cartographer_ros提供了将建图结果保存为*.pbstream专门的方法，其实就是一条命令。
source~/catkin_ws_carto/install_isolated/setup.bashrosservicecall/write_state/home/ubuntu/map/carto_map.pbstream其实就是调用cartographer_ros提供的叫/write_state这个名字的服务，服务传入参数/home/ubuntu/map/carto_map.pbstream为地图的保存路径。保存成功后，会返回相应的状态信息，如图27。
（图27）调用/write_state服务保存建图结果
地图格式转换：
由于用cartographer_ros提供的/write_state方法保存的地图是*.pbstream的格式，而要在后续的自主导航中使用这个地图，我们需要将其转换为ROS中通用的GridMap格式。其实很简单，cartographer_ros已经跟我们提供了cartographer_pbstream_to_ros_map这个节点用于转换的实现。所以，我们只需要写一个启动文件启动这个节点就行了，我给这个启动文件取名miiboo_pbstream2rosmap.launch，存放路径是cartographer_ros/launch/，启动文件的内容如图28。在使用这个启动文件进行启动时，需要从外部传入两个参数，参数pbstream_filename为待转换的*.pbstream文件路径，参数map_filestem为转换后存放结果的文件路径。
（图28）pbstream转GridMap启动文件
配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。
catkin_make_isolated--install--use-ninja最后，就可以打开终端，使用启动这个启动文件，对地图格式进行转换了，命令如下。
roslaunchcartographer_rosmiiboo_pbstream2rosmap.launchpbstream_filename:=/home/ubuntu/map/carto_map.pbstreammap_filestem:=/home/ubuntu/map/carto_map保存结束后，节点会自动退出，这时我们可以得到转换后的地图，转换后的GridMap地图由*.pgm和*.yaml两部分构成，这时标准的ROS格式地图，可以被ROS导航框架中的map_server节点直接调用，转换后的地图结果如图29。
（图29）地图格式转换后的结果
后记------SLAM+语音机器人DIY系列【目录】快速导览------
第1章：Linux基础
1.Linux简介
2.安装Linux发行版ubuntu系统
3.Linux命令行基础操作
第2章：ROS入门
1.ROS是什么
2.ROS系统整体架构
3.在ubuntu16.04中安装ROSkinetic
4.如何编写ROS的第一个程序hello_world
5.编写简单的消息发布器和订阅器
6.编写简单的service和client
7.理解tf的原理
8.理解roslaunch在大型项目中的作用
9.熟练使用rviz
10.在实际机器人上运行ROS高级功能预览
第3章：感知与大脑
1.ydlidar-x4激光雷达
2.带自校准九轴数据融合IMU惯性传感器
3.轮式里程计与运动控制
4.音响麦克风与摄像头
5.机器人大脑嵌入式主板性能对比
6.做一个能走路和对话的机器人
第4章：差分底盘设计
1.stm32主控硬件设计
2.stm32主控软件设计
3.底盘通信协议
4.底盘ROS驱动开发
5.底盘PID控制参数整定
6.底盘里程计标
第5章：树莓派3开发环境搭建
1.安装系统ubuntu_mate_16.04
2.安装ros-kinetic
3.装机后一些实用软件安装和系统设置
4.PC端与robot端ROS网络通信
5.Android手机端与robot端ROS网络通信
6.树莓派USB与tty串口号绑定
7.开机自启动ROS节点
第6章：SLAM建图与自主避障导航
1.在机器人上使用传感器
2.google-cartographer机器人SLAM建图
3.ros-navigation机器人自主避障导航
4.多目标点导航及任务调度
5.机器人巡航与现场监控
第7章：语音交互与自然语言处理
1.语音交互相关技术
2.机器人语音交互实现
3.自然语言处理云计算引擎
第8章：高阶拓展
1.miiboo机器人安卓手机APP开发
2.centos7下部署Django(nginx+uwsgi+django+python3)
参考文献
[1]张虎，机器人SLAM导航核心技术与实战[M].机械工业出版社，2022.
序前言编程基础篇第1章ROS入门必备知识1.1ROS简介21.1.1ROS的性能特色21.1.2ROS的发行版本31.1.3ROS的学习方法31.2ROS开发环境的搭建31.2.1ROS的安装41.2.2ROS文件的组织方式41.2.3ROS网络通信配置51.2.4集成开发工具51.3ROS系统架构51.3.1从计算图视角理解ROS架构61.3.2从文件系统视角理解ROS架构71.3.3从开源社区视角理解ROS架构81.4ROS调试工具81.4.1命令行工具91.4.2可视化工具91.5ROS节点通信101.5.1话题通信方式121.5.2服务通信方式151.5.3动作通信方式191.6ROS的其他重要概念251.7ROS2.0展望281.8本章小结28第2章C++编程范式2.1C++工程的组织结构292.1.1C++工程的一般组织结构292.1.2C++工程在机器人中的组织结构292.2C++代码的编译方法302.2.1使用g++编译代码312.2.2使用make编译代码322.2.3使用CMake编译代码322.3C++编程风格指南332.4本章小结34第3章OpenCV图像处理3.1认识图像数据353.1.1获取图像数据353.1.2访问图像数据363.2图像滤波373.2.1线性滤波373.2.2非线性滤波383.2.3形态学滤波393.3图像变换403.3.1射影变换403.3.2霍夫变换423.3.3边缘检测423.3.4直方图均衡433.4图像特征点提取443.4.1SIFT特征点443.4.2SURF特征点503.4.3ORB特征点523.5本章小结54硬件基础篇第4章机器人传感器4.1惯性测量单元564.1.1工作原理564.1.2原始数据采集604.1.3参数标定654.1.4数据滤波734.1.5姿态融合754.2激光雷达914.2.1工作原理924.2.2性能参数944.2.3数据处理964.3相机1004.3.1单目相机1014.3.2双目相机1074.3.3RGB-D相机1094.4带编码器的减速电机1114.4.1电机1114.4.2电机驱动电路1124.4.3电机控制主板1134.4.4轮式里程计1174.5本章小结118第5章机器人主机5.1X86与ARM主机对比1195.2ARM主机树莓派3B+1205.2.1安装UbuntuMATE18.041205.2.2安装ROSmelodic1225.2.3装机软件与系统设置1225.3ARM主机RK33991275.4ARM主机Jetson-tx21285.5分布式架构主机1295.5.1ROS网络通信1305.5.2机器人程序的远程开发1305.6本章小结131第6章机器人底盘6.1底盘运动学模型1326.1.1两轮差速模型1326.1.2四轮差速模型1366.1.3阿克曼模型1406.1.4全向模型1446.1.5其他模型1486.2底盘性能指标1486.2.1载重能力1486.2.2动力性能1486.2.3控制精度1506.2.4里程计精度1506.3典型机器人底盘搭建1516.3.1底盘运动学模型选择1526.3.2传感器选择1526.3.3主机选择1536.4本章小结155SLAM篇第7章SLAM中的数学基础7.1SLAM发展简史1587.1.1数据关联、收敛和一致性1607.1.2SLAM的基本理论1617.2SLAM中的概率理论1637.2.1状态估计问题1647.2.2概率运动模型1667.2.3概率观测模型1717.2.4概率图模型1737.3估计理论1827.3.1估计量的性质1827.3.2估计量的构建1837.3.3各估计量对比1907.4基于贝叶斯网络的状态估计1937.4.1贝叶斯估计1947.4.2参数化实现1967.4.3非参数化实现2027.5基于因子图的状态估计2067.5.1非线性最小二乘估计2067.5.2直接求解方法2067.5.3优化方法2087.5.4各优化方法对比2187.5.5常用优化工具2197.6典型SLAM算法2217.7本章小结221第8章激光SLAM系统8.1Gmapping算法2238.1.1原理分析2238.1.2源码解读2288.1.3安装与运行2338.2Cartographer算法2408.2.1原理分析2408.2.2源码解读2478.2.3安装与运行2588.3LOAM算法2668.3.1原理分析2668.3.2源码解读2678.3.3安装与运行2708.4本章小结270第9章视觉SLAM系统9.1ORB-SLAM2算法2749.1.1原理分析2749.1.2源码解读3109.1.3安装与运行3199.1.4拓展3279.2LSD-SLAM算法3299.2.1原理分析3299.2.2源码解读3349.2.3安装与运行3379.3SVO算法3389.3.1原理分析3389.3.2源码解读3419.4本章小结341第10章其他SLAM系统10.1RTABMAP算法34410.1.1原理分析34410.1.2源码解读35110.1.3安装与运行35710.2VINS算法36210.2.1原理分析36410.2.2源码解读37310.2.3安装与运行37610.3机器学习与SLAM37910.3.1机器学习37910.3.2CNN-SLAM算法41110.3.3DeepVO算法41310.4本章小结414自主导航篇第11章自主导航中的数学基础11.1自主导航41811.2环境感知42011.2.1实时定位42011.2.2环境建模42111.2.3语义理解42211.3路径规划42211.3.1常见的路径规划算法42311.3.2带约束的路径规划算法43011.3.3覆盖的路径规划算法43411.4运动控制43511.4.1基于PID的运动控制43711.4.2基于MPC的运动控制43811.4.3基于强化学习的运动控制44111.5强化学习与自主导航44211.5.1强化学习44311.5.2基于强化学习的自主导航46511.6本章小结467第12章典型自主导航系统12.1ros-navigation导航系统47012.1.1原理分析47012.1.2源码解读47512.1.3安装与运行47912.1.4路径规划改进49212.1.5环境探索49612.2riskrrt导航系统49812.3autoware导航系统49912.4导航系统面临的一些挑战50012.5本章小结500第13章机器人SLAM导航综合实战13.1运行机器人上的传感器50213.1.1运行底盘的ROS驱动50313.1.2运行激光雷达的ROS驱动50313.1.3运行IMU的ROS驱动50413.1.4运行相机的ROS驱动50413.1.5运行底盘的urdf模型50513.1.6传感器一键启动50613.2运行SLAM建图功能50613.2.1运行激光SLAM建图功能50713.2.2运行视觉SLAM建图功能50813.2.3运行激光与视觉联合建图功能50813.3运行自主导航50913.4基于自主导航的应用51013.5本章小结511附录ALinux与SLAM性能优化的探讨附录B习题