本专栏目录
前言
第一章：Linux基础
第二章：ROS入门
(资料图)
第三章：感知与大脑
第四章：差分底盘设计
第五章：树莓派3开发环境搭建
第六章：SLAM建图与自主避障导航
第七章：语音交互与自然语言处理
附录A：用于ROS机器人交互的Android手机APP开发
附录B：用于ROS机器人管理调度的后台服务器搭建
附录C：如何选择ROS机器人平台进行SLAM导航入门
视频教程
https://www.bilibili.com/video/BV1jS4y1a7Lz
通过前面的基础学习，本章进入最为激动的机器人自主导航的学习。在前面的学习铺垫后，终于迎来了最大乐趣的时刻，就是赋予我们的miiboo机器人能自由行走的生命。本章将围绕机器人SLAM建图、导航避障、巡航、监控等内容展开。本章内容：
1.在机器人上使用传感器
2.google-cartographer机器人SLAM建图
3.ros-navigation机器人自主避障导航
4.多目标点导航及任务调度
5.机器人巡航与现场监控
下面这本书是本篇文章的参考文献，大家有需要可以入手一本：
1.在机器人上使用传感器
SLAM建图需要用到底盘、激光雷达和IMU，所以这里详细介绍如何在机器人上使用这些传感器。要使用这些传感器也很简单，就是在机器人上开启相应传感器的ROS驱动节点，在设置合适的可配参数就行了。
需要注意的是，底盘、激光雷达、IMU这三个传感器都使用串口与树莓派通信，为了防止每次开机这三个设备的串口号发生变动，需要将串口号进行绑定与重映射，操作方法在前面已经介绍过了，如果还没有绑定直接前往前面相关内容参考。
这里将建立一个叫catkin_ws的ROS工作空间，专门用于存放机器人传感器相关的ROS驱动功能包。关于创建ROS工作空间的操作，请参考前面相应部分内容，这里就不做讲解。
1.1.使用底盘
在机器人上只需要使用miiboo这个驱动包就可以驱动底盘了。将miiboo这个驱动包拷贝到~/catkin_ws/src/中，编译后就可以使用了。miiboo驱动包文件结构，如图1。miiboo驱动包中含有两个ROS功能包miiboo_bringup和miiboo_description，驱动miiboo底盘、底盘PID整定、里程计标定这些功能包含在miiboo_bringup中，miiboo底盘urdf模型包含在miiboo_description中。
底盘控制可配参数：
关于底盘控制可配参数都放在miiboo_bringup/launch/minimal.launch中，如图2。
参数com_port是底盘控制的串口号，由于前面已经做了绑定，所以直接填入绑定好的名称/dev/miiboo就行了；
参数speed_ratio是里程计走直线标定值，这个值通过标定得到。
参数wheel_distance是里程计转角标定值，这个值通过标定得到。
其余参数一般不需要修改，如有需要可以结合阅读源码来了解参数含有和做相应修改。
驱动miiboo底盘：
其实很简单，一条命令启动miiboo底盘控制。
底盘PID整定：
我们的miiboo机器人底盘的stm32控制板中已经内置了整定好的PID参数，如果选用我们提供的控制板和电机，一般情况下是不需要整定PID的。
对于想体验一下PID参数整定过程或将我们的miiboo机器人底盘的stm32控制板应用到其他地方的朋友，这里给出了整定PID的整个操作过程和思路，方便大家学习和更深层次的研究。这里主要讲解PID整定的操作，关于原理性的东西可以参考前面相关内容进行了解。
由于底盘PID整定是非必须的功能，所以没有对底盘PID整定的串口（DEBUG-uart1）做绑定，需要先手动插入该串口到树莓派3，然后手动查看该串口的设备号，并修改该设备号的可读写权限。然后将该设备号填入miiboo_bringup/launch/pid_set.launch中的com_port参数中。然后，需要启动底盘控制节点、底盘调试节点、键盘控制节点。键盘控制节点teleop_twist_keyboard需要通过apt-get命令来安装，rqt_plot是ROS提供的绘图工具。
ROS提供的绘图工具rqt_plot用法很简单，在rqt_plot界面中，在Topic栏输入曲线数据来源，我们这里为左、右轮速度，然后点击旁边的“+”将曲线加入绘制界面，人如图3。
然后，通过在启动teleop_twist_keyboard节点的终端通过I/</J/L四个按键来控制底盘前进/后退/左转/右转控制，并观察速度曲线的变化，根据PID整定规则对PID参数进行整定，在启动 pid_set.launch的终端下按相应提示输入PID参数实现对整定参数的编辑。直到得到一个比较好的速度曲线，就可以结束整定过程了。实时速度曲线显示，如图4。
里程计标定：
机器人底盘运行的精度是衡量底盘的重要指标。底盘精度受里程计的走直线误差和转角误差影响。因此，需要对里程计的走直线和转角进行标定，尽量减小误差。miiboo机器人底盘的ROS驱动中已经写好了相应的标定程序，跟里程计标定有关的文件主要有：
.../miiboo_bringup/launch/check_linear.launch为里程计走直线标定启动文件
.../miiboo_bringup/launch/check_angular.launch为里程计转角标定启动文件
.../miiboo_bringup/launch/minimal.launch为设置标定参数及底盘控制启动文件
下面是标定步骤过程。
由于标定过程在前面已经讲解过了，请直接前往相应内容参考。
miiboo底盘urdf模型：
urdf模型描述了机器人底盘的形状、传感器之间的安装关系、各个传感器在tf tree中的关系。其实，miiboo底盘urdf模型的主要作用是提供各个传感器在tf tree中的关系，这些关系将在SLAM和导航算法中被使用。
图5是miiboo机器人底盘中各个传感器tf关系，base_footprint是底盘的运动中心，base_laser_link是激光雷达的中心，imu_link为IMU模块的中心。以base_footprint为原点，建立机器人底盘的坐标系，坐标系为标准右手系，即底盘正前方为x轴、正左方为y轴、正上方为z轴、以x轴起始逆时针方向为theta轴。以base_footprint为父坐标系，建立base_footprint->base_laser_link关系，建立base_footprint->imu_link关系，就实现了各个传感器tf关系的构建，构建的具体实现在miiboo_description/urdf/miiboo.urdf中完成。如图6，为miiboo.urdf的具体内容。
要使用这个urdf模型就很简单了，直接一句命令启动。
1.2.使用激光雷达
在机器人上只需要使用ydlidar这个驱动包就可以驱动ydlidar-x4雷达了。将ydlidar这个驱动包拷贝到~/catkin_ws/src/中，编译后就可以使用了。ydlidar驱动包文件结构，如图7。ydlidar驱动包中的其他文件我们不需要关心，这些都是由雷达厂商提供的标准驱动，只需要设置我们自己建立的ydlidar/launch/my_x4.launch文件，这个用于启动雷达。
激光雷达数据可配参数：
关于激光雷达数据可配参数都放在ydlidar/launch/my_x4.launch中，如图8。
参数port是激光雷达的串口号，由于前面已经做了绑定，所以直接填入绑定好的名称/dev/lidar就行了；
参数range_min和range_max是设置激光雷达数据的有效值区间。
其余参数一般不需要修改，如有需要可以结合阅读源码来了解参数含有和做相应修改。
驱动ydlidar-x4激光雷达：
其实很简单，一条命令启动ydlidar-x4激光雷达。
激光雷达数据格式：
激光雷达采用右手坐标系，雷达正前方为x轴、正左方为y轴、正上方为z轴、以x轴起始逆时针方向为theta轴。激光雷达的扫描数据以极坐标的形式表示，雷达正前方是极坐标0度方向、雷达正左方是极坐标90度方向，红色点为扫描到的数据点，如图9所示。
激光雷达的数据在ROS中是以sensor_msgs/LaserScan消息类型进行表示，如图10，angle_increment表示激光数据点的极坐标递增角度，ranges数组存放实际的极坐标点距离值。
1.3.使用IMU
在机器人上只需要使用miiboo_imu这个驱动包就可以驱动mpu9250模块了。将miiboo_imu这个驱动包拷贝到~/catkin_ws/src/中，编译后就可以使用了。miiboo_imu驱动包文件结构，如图11。ydlidar驱动包中的其他文件我们不需要关心，只需要设置ydlidar/launch/my_x4.launch文件，这个用于启动IMU。
IMU数据可配参数：
关于IMU数据可配参数都放在miiboo_imu/launch/imu.launch中，如图12。
参数come_port是IMU的串口号，由于前面已经做了绑定，所以直接填入绑定好的名称/dev/imu就行了；
其余参数一般不需要修改，如有需要可以结合阅读源码来了解参数含有和做相应修改。
驱动IMU模块：
其实很简单，一条命令启动IMU模块。
IMU数据格式：
IMU模块采用右手坐标系，IMU模块正前方为x轴、正左方为y轴、正上方为z轴。IMU模块提供3轴加速度、3轴角速度、3轴磁力计、经数据融合后用欧拉角表示的姿态。
IMU数据在ROS中是以sensor_msgs/Imu消息类型进行表示，如图13。
1.4.使用摄像头
miiboo机器人上使用的是USB摄像头，用ROS驱动USB摄像头可以采用以下3中方法。
方法1：
使用usb_cam这个ROS包直接驱动
方法2：
使用gscam这个ROS包直接驱动
方法3：
自制OpenCV，cv_bridge，image_transport驱动ROS包
为了方便起见，我采用的是方法1，直接安装usb_cam这个ROS包直接驱动。
usb_cam摄像头驱动安装：
将usb_cam下载到~/catkin_ws/src/中，直接编译就行了。
摄像头数据可配参数：
关于摄像头数据可配参数都放在usb_cam/launch/usb_cam.launch中，如图14。
参数video_device是摄像头的设备号，由于直插了一个USB摄像头，所以直接填入名称/dev/video0就行了；
其余参数一般不需要修改，如有需要可以结合阅读源码来了解参数含有和做相应修改。
驱动USB摄像头：
其实很简单，一条命令启动USB摄像头。
摄像头远程显示：
摄像头数据远程显示的方法有两种，方法一是在PC端rviz中订阅摄像头发布的图像topic，方法二是用Android手机上miiboo机器人APP直接显示。
先说方法一，在PC端打开rviz，在rviz中添加需要显示的Topic，这样就可以看到图像了。如图15和16。
方法二，就很简单了，只要Android手机上miiboo机器人APP连接到机器人端成功后，就能自动显示图像了。如图17。
1.5.局域网内广播机器人自己的IP
这个很简单，由broadcast_ip这个功能包实现，我已经写好放入~/catkin_ws/src/并编译了。只需要一句命令启动就行了。
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策略的实现。
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工具进行编译。
（2）创建存放cartographer_ros的专门工作空间
特别说明，在执行wstool update -t src之前，需要将src/.rosinstall文件修改成以下内容,以解决ceres-solver下载不了的问题，如图19。
（3）安装依赖项
安装cartographer_ros的依赖项proto3、deb包等。如果执行sudo rosdep init
报错，可以直接忽略。
（4）编译和安装
上面的配置和依赖都完成后，就可以开始编译和安装cartographer_ros整个项目工程了。
特别提醒，以后对cartographer_ros中的配置文件或源码有改动时，都需要执行这个编译命令使修改生效。
2.3.cartographer_ros使用
cartographer_ros整体代码结构分析：
最顶层的是cartographer_ros，作为rosj接口调用层，通过调用cartographer核心算法，订阅多传感器数据（/scan、/imu、/odom等），并发布地图、机器人位置信息（/map、/tf等）；其次是cartographer，作为SLAM算法的核心实现，特征提取、子图构建、闭环检测、全局优化都在这里实现，其中优化过程需要调用ceres-solver非线性优化库；最后是ceres-solver，是非线性优化库，用于求解SLAM中的优化问题。
在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
然后需要配置*.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地图发布的频率。
配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。
启动cartographer_ros建图：
要在miiboo机器人上，启动cartographer_ros建图，分为这几个步骤：启动机器人上的各个传感器、启动cartographer_ros、在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。
首先，启动机器人上的各个传感器，为了操作方便，我已经将要启动的传感器都写入miiboo_bringup/launch/miiboo_all_sensor.launch这个启动文件了，文件内容如图23。这个启动文件包含机器人urdf启动项、miiboo底盘启动项、激光雷达启动项、IMU启动项、摄像头启动项、广播IP启动项。
打开终端，通过下面的命令直接启动就行了。
然后，启动cartographer_ros，由于前面已经做好了相应的配置，所以直接使用命令启动就行了。
这里给个小提示，为了查看cartographer_ros建图算法有没有正常开始工作，我们可以用rosrun rqt_tf_tree rqt_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模型提供，这个静态关系主要用于多传感器数据融合。
最后，在PC端启动键盘控制机器人运动并启动rviz观察地图（或者在Android手机端用miiboo机器人APP控制机器人运动和观察地图）。如果用PC端控制和观察，启动命令如下。
在PC端打开一个新终端，运行rviz启动命令。
在rviz窗口中添加订阅/map，就可以看到建图效果了，如图25。
在PC端再打开一个新终端，运行键盘控制启动命令。
在该终端下，用键盘就可以控制机器人前进、后退、左转、右转了。
如果是在Android手机端用miiboo机器人APP控制机器人运动和观察地图，直接就能使用，如图26。
保存cartographer_ros建图结果：
当我们在房间里面扫描一圈，地图建立的差不多了，就可以将建图结果保存下来了，cartographer_ros提供了将建图结果保存为*.pbstream专门的方法，其实就是一条命令。
其实就是调用cartographer_ros提供的叫/write_state这个名字的服务，服务传入参数/home/ubuntu/map/carto_map.pbstream为地图的保存路径。保存成功后，会返回相应的状态信息，如图27。
地图格式转换：
由于用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为转换后存放结果的文件路径。
配置参数修改好后，不要忘了再编译一次整个catkin_ws_carto工作空间，切换到catkin_ws_carto目录，执行下面的编译命令。
最后，就可以打开终端，使用启动这个启动文件，对地图格式进行转换了，命令如下。
保存结束后，节点会自动退出，这时我们可以得到转换后的地图，转换后的GridMap地图由*.pgm和*.yaml两部分构成，这时标准的ROS格式地图，可以被ROS导航框架中的map_server节点直接调用，转换后的地图结果如图29。
3.ros-navigation机器人自主避障导航
前面的学习教程打好了必须的基础，现在就正式开始探讨ROS系统最强大的特性之一，让我们的机器人能自主导航和避障。这都得益于开源社区和共享代码，使ROS拥有大量可用的导航算法。将这些导航算法集大成者，便是ros-navigation导航功能包集。了解更多ros-navigation的信息，请参考官方wiki教程：http://wiki.ros.org/navigation/。
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。
机器人全局定位amcl粒子滤波算法：
amcl的全称是自适应蒙特卡洛粒子滤波，这里通过讲解粒子滤波、重要性采样、机器人绑架、自适应蒙特卡洛这几个概念来说明机器人全局定位的原理。由于amcl的数学理论比较复杂，限于篇幅这里不展开讲解，感兴趣的朋友可以参考《Probabilistic Robotics》这本书，里面有详细的推导过程，如图31。
粒子滤波，是一种思想，比如要计算一个矩形里面一个不规则形状的面积，这个问题不好直接计算，但是可以拿一把豆子均匀撒到矩形中，统计落在不规则形状中豆子的占比就能算出其面积了。在机器人定位问题中，我们在地图的任意位置撒上许多粒子点，然后通过传感器观测数据按照一定的评价方法对每个粒子点进行打分，评分高的粒子点表示机器人有更大的可能在此位置；在下一轮撒点时，就在评分高的粒子点附近多撒一些点，这样通过不断的迭代，粒子点就会聚拢到一个地方。这个粒子点聚集的地方，就是机器人位置的最优估计点。如图32，红色的粒子点慢慢聚拢到一团。
重要性采样，在粒子滤波的迭代过程中，评分高的粒子点会被下一轮迭代时更加看重，这样不断迭代真实估计值附近的粒子点会越来越多。
机器人绑架，当机器人被突然从一个地方抱走到另一个地方，这个时候前一轮迭代得到的粒子点完全不能在新的位置上试用，这样继续迭代下去就会发生位置估计的错误。
自适应蒙特卡洛，自适应主要体现在两个方面。通过判断粒子点的平均分突变来识别机器人绑架问题，并在全局重新撒点来解决机器人绑架问题；通过判断粒子点的聚集程度来确定位置估计是否准确，在估计比较准确的时候降低需要维护的粒子点数目，这样来降低算法的计算开销。
代价地图costmap：
代价地图用于描述环境中的障碍物信息，代价地图利用激光雷达、声呐、红外测距等探测传感器的数据来生成，大致的原理是通过建立不同的图层Layer然后叠在一起，被填充的栅格点表示有障碍物，如图33，想要了解代价地图的更多细节可以阅读《Layered Costmaps for Context-Sensitive Navigation》这篇论文。机器人导航中用到了两个代价地图，全局代价地图global_costmap和局部代价地图local_costmap。
路径规划：
机器人导航中核心的就是路径规划，路径规划是通过利用环境障碍物信息找到一条到达目标并且开销小的路劲。导航中会用到两种路径规划，即全局路径规划global_planner和局部路径规划local_planner。全局路径规划更像是一种战略性策略，需要考虑全局，规划处一条尽量短并且易于执行的路径。在全局路径的指导下，机器人在实际行走时还需要考虑周围实时的障碍物并制定避让策略，这就是局部路径规划要完成的事，可以说机器人的自主导航最终是由局部路径规划一步步完成的。ROS中推荐的全局路径规划器是global_planner，基于A*算法，如图34。
由于ROS中的局部路径规划器比较老了，所以我推荐大家使用比较新的局部路径规划器teb_local_planner。teb_local_planner是基于弹性时间带碰撞约束的算法，算法将动态障碍物、运行时效、路径平滑性等约束做综合考虑，在复杂环境下有更优秀的表现，如图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/后，就可编译安装了。
然后需要安装teb_local_planner，前往https://github.com/rst-tu-dortmund/teb_local_planner，将分支切换到kinetic-devel然后下载源码到本地，并解压源码包到~/catkin_ws_nav/src/后，就可以编译安装了。
值得一提的是，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中存放启动导航所需的各种节点的启动文件。下面依次介绍这些文件的作用。
机器人全局定位AMCL算法配置amcl.launch.xml：
导航中用到的全局定位AMCL算法功能包包含很多可以配置的参数，这里建立配置文件amcl.launch.xml对这些参数进行配置，内容如图37。配置参数分为3类：粒子滤波参数、雷达模型参数、里程计模型参数。由于参数比较多，关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：http://wiki.ros.org/amcl。
代价地图公有参数配置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官方教程：http://wiki.ros.org/costmap_2d。
全局代价地图参数配置global_costmap_params.yaml：
全局代价地图是以插件的形式组建的，在costmap_common_params.yaml中定义的各Layer图层都可以通过插件的形式放入全局代价地图中，各Layer图层的组合可以根据需求自由选择，如图39。我们的miiboo机器人的全局代价地图只用了static_layer和global_inflation_layer两个图层。
局部代价地图参数配置global_costmap_params.yaml：
局部代价地图和全局代价地图类似，就不展开讲解了，如图40。我们的miiboo机器人的局部代价地图只用了obstacle_layer和local_inflation_layer两个图层。
全局路径规划器参数配置global_planner_params.yaml：
全局路径规划器也是以插件的形式被使用，这里主要对全局路径规划器GlobalPlanner插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图41。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：
http://wiki.ros.org/global_planner。
局部路径规划器参数配置teb_local_planner_params.yaml：
局部路径规划器也是以插件的形式被使用，这里主要对局部路径规划器TebLocalPlannerROS插件及参数进行申明，在后面的move_base_params.yaml中将会调用这个插件。配置文件内容如图42。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：
http://wiki.ros.org/teb_local_planner。
导航核心控制参数配置move_base_params.yaml：
导航核心控制由move_base节点最终实现，move_base在整个导航框架中处于核心地位，全局代价地图、局部代价地图、全局路径规划、局部路径规划、恢复策略等等都将在这里得到具体实现的调用。配置内容如图43。不难发现，最开始两行便对调用的全局路径规划器和局部路径规划器进行了申明，然后就是一些运动控制方面的参数。关于参数配置的具体讲解就不展开，请直接参考wiki官方教程：http://wiki.ros.org/move_base。
启动导航所需的各种节点的启动文件miiboo_nav.launch：
在config目录中将各个导航模块的参数配置好后，就可以在一个启动文件中将导航所需的各种节点进行启动，并载入相关配置。启动文件miiboo_nav.launch就是来完成这个任务的，如图44。不难发现，第一个启动项是map_server，将SLAM建图中得到的地图进行载入后发布到ROS中供导航中需要的模块使用；第二个启动项是amcl，amcl开始运行后将为导航中的模块提供机器人在地图中的全局位置；第三个启动项是move_base，在这里将载入代价地图公有参数、全局代价地图参数、局部代价地图参数、move_base参数、全局路径规划参数、局部路径规划参数。
到这里，我们已经准备好了导航的各个配置文件和启动文件，现在就正式来进行导航。这里需要特别提醒，我们假设已经在SLAM建图过程得到了一张比较好的环境地图，并保存到了正确的路径。启动导航分为3步：启动机器人上所有传感器、启动导航所需各个节点、发送导航目标点。
首先，启动机器人上所有传感器，打开终端，通过下面的命令直接启动就行了。
然后，启动导航所需各个节点，打开终端，通过下面的命令直接启动就行了。
最后，发送导航目标点。这里有两种方式来下发导航目标点，方法一是通过rviz交互界面，方法二是通过手机APP交互界面。下面对两种方法分别介绍。
方法一：
在PC端打开rviz，打开一个新终端，运行rviz启动命令。
订阅/map、/scan、/tf等信息，并观察机器人的初始位置是否正确，如果机器人的初始位置不正确，需要用[2D Pose Estimate]按钮手动给定一个正确的初始位置，如图45。操作方法很简单，先点击[2D Pose Estimate]按钮，然后将鼠标放置到机器人在地图中实际应该的位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。
初始位置设置正确后，就可以用[2D Nav Goal]按钮手动指定导航目标点了，如图46。操作方法很简单，先点击[2D Nav Goal]按钮，然后将鼠标放置到地图中任意的想让机器人到达的空白位置，最后按住鼠标并拖动鼠标来完成机器人朝向的设置。这样机器人就会开始规划路径并自动导航到该指定目标点。
方法二：
直接打开手机上的miiboo机器人APP，就可以看到地图和机器人在地图中的位置了。和方法一中的一样，需要观察机器人的初始位置是否正确，如果机器人的初始位置不正确，用[Set Pose]按钮手动给定一个正确的初始位置，如图47。操作过程和方法一类似，就不展开了。
初始位置设置正确后，就可以用[Set Goal]按钮手动指定导航目标点了，如图48。操作过程和方法一类似，就不展开了。
4.多目标点导航及任务调度
通过前面的学习，我们已经可以通过点击地图的方式来命令机器人运动到目标点。其实，ros-navigation导航框架就是为我们提供了一个最基本的机器人自动导航接口，即单点导航。然而，在实际的机器人应用中，机器人往往要完成复杂的任务，这些复杂的任务都是由一个个基本的任务组合而成的。一般的，机器人通过状态机的形式将一个个基本任务组合在一起来进行复杂任务的调度实现。
4.1.状态机
这里我们只讨论有限状态机,也称为FSM(Finite State Machine)，其在任意时刻都处于有限状态集合中的某一状态。当其获得一个输入条件时，将从当前状态转换到另一个状态，或者仍然保持在当前状态。任何一个FSM都可以用状态转换图来描述，图中的节点表示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。
这里将建立一个叫catkin_ws_apps的ROS工作空间，专门用于放置日后开发的各种应用层功能包。关于创建ROS工作空间的操作，请参考前面相应部分内容，这里就不做讲解。在~/catkin_ws_apps/src/中建立一个叫patrol的功能包，建好后的patrol功能包文件结构，如图51。
关于功能包的文件结构，大家已经很熟悉了，就不啰嗦了。这里重点讲解一下patrol_fsm.py这个文件，文件内容如图52。
这里采用python来编写多目标点巡航的逻辑，python开发ROS节点的优点是简洁高效。代码中waypoints数组里面存放的是要巡航的各个目标点，大家可以根据自己的需要进行相应的替换和增减；with patrol代码块里面实现状态机的构建；最后调用状态机的执行函数，状态机就开始工作了，也就是开始执行巡航了。
启动多目标点巡航分为3步：启动机器人上所有传感器、启动导航所需各个节点、启动多目标点巡航节点。
首先，启动机器人上所有传感器，打开终端，通过下面的命令直接启动就行了。
然后，启动导航所需各个节点，打开终端，通过下面的命令直接启动就行了。
最后，启动多目标点巡航节点，打开终端，通过下面的命令直接启动就行了。
4.3.复杂多任务机器人未来展望
机器人可以进行自动导航、人机对话、用机械臂抓取物体、物体识别等。将这些任务结合起来，利用机器人强大的大脑推理机制能完成更为复杂和智能化的任务。如果说基于状态机的复杂任务调度是1.0版本的智能，那么基于大脑推理机制的复杂任务调度将是2.0版本的智能。
我的设想是利用强化学习神经网络作为大脑推理机制的实现实体，如图53。自动导航、人机对话、用机械臂抓取物体、物体识别等任务组合的整体作为机器人与外界环境交互的动作空间，动作空间的的状态分为两种形态：执行结果、执行策略。执行结果作为强化学习神经网络的输入，而执行策略作为强化学习神经网络的输出。我们不断通过各种复杂的实际场景的粒子来训练机器人，让机器人能在复杂场景下能做正确的事情。比如说，当机器人收到主人“我渴了”的语音信息后，自动导航到桌子边，然后识别桌上的可乐，并用机械臂抓取，最后递给主人，并提醒主人“你的可乐来了”。
哈哈！这样的想法很炫酷，不过以目前的技术实现难度还比较大，所以作为未来展望分享给大家。希望和大家一起努力，在不远的将来能实现这个梦想。
5.机器人巡航与现场监控
机器人在实际生产生活中能给人类带来很大的帮助，比如一款能巡视和现场监控的机器人。商场、机场、家里等需要监控和安保的场景，拍一个机器人去是再合适不过的了。结合前面所学的知识，我们可以让miiboo机器人执行巡视与现场监控的任务。这里介绍两种工作方式：手动遥控式现场监控、自动巡航式现场监控。
5.1.手动遥控式现场监控
首先，启动机器人上所有传感器，这样机器人上的摄像头就可以将图像发布到ROS中，供远程订阅和显示。打开终端，通过下面的命令直接启动就行了。
然后，启动导航所需各个节点，这样就可以实时查看机器人所在的具体位置。打开终端，通过下面的命令直接启动就行了。
最后，在Android手机端用miiboo机器人APP控制机器人运动和视频监控周围环境，如图54。
5.2.自动巡航式现场监控
与手动遥控式现场监控的区别是，自动巡航式现场监控不需要人员值守和遥控机器人运动。机器人能根据设定的巡逻路线巡航并实时回传视频监控，结合人脸识别、事件识别等人工智能及视频分析算法，可以对异常事件进行自动的监控。
首先，启动机器人上所有传感器，这样机器人上的摄像头就可以将图像发布到ROS中，供远程订阅和显示。打开终端，通过下面的命令直接启动就行了。
然后，启动导航所需各个节点，这样就可以实时查看机器人所在的具体位置。打开终端，通过下面的命令直接启动就行了。
接下来，设定好巡逻路线后，启动多目标点巡航节点，打开终端，通过下面的命令直接启动就行了。
最后，在Android手机端用miiboo机器人APP视频监控周围环境，或者将订阅的视频放入人工智能及视频分析算法，如图55。
后记
为了防止后续大家找不到本篇文章，我同步制作了一份文章的pdf和本专栏涉及的例程代码放在github和gitee方便大家下载，如果下面给出的github下载链接打不开，可以尝试gitee下载链接：
github下载链接：https://github.com/xiihoo/DIY_A_SLAM_Navigation_Robot
gitee下载链接：https://gitee.com/xiihoo-robot/DIY_A_SLAM_Navigation_Robot
技术交流
QQ技术交流群：117698356
参考文献
[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.
