机器人技术的迅猛发展，促使机器人逐渐走进了人们的生活，服务型室内移动机器人更是获得了广泛的关注。但室内机器人的普及还存在许多亟待解决的问题，定位与导航就是其中的关键问题之一。在这类问题的研究中，需要把握三个重点：一是地图精确建模；二是机器人准确定位；三是路径实时规划。在近几十年的研究中，对以上三个重点提出了多种有效的解决方法。

室外定位与导航可以使用GPS，但在室内这个问题就变得比较复杂。为了实现室内的定位定 姿， 一 大 批 技 术 不 断 涌 现， 其 中， SLAM 技 术 逐 渐 脱 颖 而 出。 SLAM（Simultaneous
Localization and Mapping，即时定位与地图构建）最早由Smith、Self和Cheeseman于1988年提出。作为一种基础技术，SLAM从最早的军事用途到今天的扫地机器人，吸引了一大批研究者和爱好者，同时也使这项技术逐步走入普通消费者的视野。

使用ROS实现机器人的SLAM和自主导航等功能是非常方便的，因为有较多现成的功能包可 供 开 发 者 使 用， 如 gmapping、 hector_slam、 cartographer、 rgbdslam、 ORB_SLAM、
move_base、amcl等。本章我们将学习这些功能包的使用方法，并且使用仿真环境和真实机器人实现这些功能。

理论基础

SLAM可以描述为：机器人在未知的环境中从一个未知位置开始移动，移动过程中根据位置估计和地图进行自身定位，同时建造增量式地图，实现机器人的自主定位和导航。

想象一个盲人在一个未知的环境里，如果想感知周围的大概情况，那么他需要伸展双手作为他的“传感器”，不断探索四周是否有障碍物。当然这个“传感器”有量程范围，他还需要不断移动，同时在心中整合已经感知到的信息。当感觉新探索的环境好像是之前遇到过的某个位置，他就会校正心中整合好的地图，同时也会校正自己当前所处的位置。当然，作为一个盲人，感知能力有限，所以他探索的环境信息会存在误差，而且他会根据自己的确定程度为探索到的障碍物设置一个概率值，概率值越大，表示这里有障碍物的可能性越大。一个盲人探索未知环境的场景基本可以表示SLAM算法的主要过程。这里不详细讨论SLAM的算法实现，只对概念做一个基本理解，感兴趣的读者可以查找相关资料进行深度学习。

下图所示即为使用SLAM技术建立的室内地图。

家庭、商场、车站等场所是室内机器人的主要应用场景，在这些应用中，用户需要机器人通过移动完成某些任务，这就需要机器人具备自主移动、自主定位的功能，我们把这类应用统称为自主导航。

自主导航往往与SLAM密不可分，因为SLAM生成的地图是机器人自主移动的主要蓝图。这类问题可以总结为：在服务机器人工作空间中，根据机器人自身的定位导航系统找到一条从起始状态到目标状态、可以避开障碍物的最优路径。

要完成机器人的SLAM和自主导航，机器人首先要有感知周围环境的能力，尤其要有感知周围环境深度信息的能力，因为这是探测障碍物的关键数据。用于获取深度信息的传感器主要有以下几种类型。

（1）激光雷达
激光雷达是研究最多、使用最成熟的深度传感器，可以提供机器人本体与环境障碍物之间的距离信息，很多常见的扫地机器人就配有高性价比的激光雷达（见图9-2）。激光雷达的优点
是精度高，响应快，数据量小，可以完成实时SLAM任务；缺点是成本高，一款进口高精度的激光雷达价格在一万元以上。现在很多国内企业专注高性价比的激光雷达，也有不少优秀的产品已经推向市场

（2）摄像头
SLAM所用到的摄像头又可以分为两种：一种是单目摄像头，也就是使用一个摄像头完成SLAM。这种方案的传感器简单，适用性强，但是实现的复杂度较高，而且单目摄像头在静止状态下无法测量距离，只有在运动状态下才能根据三角测量等原理感知距离。另一种就是双目摄像头（见图9-3），相比单目摄像头，这种方案无论是在运动状态下还是在静止状态下，都可以感知距离信息，但是两个摄像头的标定较为复杂，大量的图像数据也会导致运算量较大。

3）RGB-D摄像头
RGB-D摄像头是近年来兴起的一种新型传感器，不仅可以像摄像头一样获取环境的RGB图像信息，也可以通过红外结构光、Time-of-Flight等原理获取每个像素的深度信息。丰富的数据
让RGB-D摄像头不仅可用于SLAM，还可用于图像处理、物体识别等多种应用；更重要的一点是，RGB-D摄像头成本较低，它也是目前室内服务机器人的主流传感器方案。常见的RGB-D摄
像头有Kinect v1/v2、华硕Xtion Pro等（见图9-4）。当然，RGB-D**摄像头也存在诸如测量视野窄、盲区大、噪声大等缺点。**

准备工作

ROS中SLAM和自主导航的相关功能包可以通用于各种移动机器人平台，但是为了达到最佳效果，对机器人的硬件仍然有以下三个要求。

1）导航功能包对差分、轮式机器人的效果好，并且假设机器人可直接使用速度指令进行控制，速度指令的定义如图9-5所示。

• linear：机器人在xyz三轴方向上的线速度，单位是m/s。
• angular：机器人在xyz三轴方向上的角速度，单位是rad/s。

2）导航功能包要求机器人必须安装激光雷达等测距设备，可以获取环境深度信息。

3）导航功能包以正方形和圆形的机器人为模板进行开发，对于其他外形的机器人，虽然可以正常使用，但是效果可能不佳。

传感器信息

1.环境深度信息
无论是SLAM还是自主导航，获取周围环境的深度信息都是至关重要的。要获取深度信息，首先要清楚ROS中的深度信息是如何表示的。针对激光雷达，ROS在sensor_msgs包中定义了专用数据结构——LaserScan，用于存储激光消息。LaserScan消息的具体定义如图9-6所示。

rosmsg show sensor_msgs/LaserScan

·angle_min：可检测范围的起始角度。
·angle_max：可检测范围的终止角度，与angle_min组成激光雷达的可检测范围。
·angle_increment：采集到相邻数据帧之间的角度步长。
·time_increment：采集到相邻数据帧之间的时间步长，当传感器处于相对运动状态时进行
补偿使用。
·scan_time：采集一帧数据所需要的时间。
·range_min：最近可检测深度的阈值。
·range_max：最远可检测深度的阈值。
·ranges：一帧深度数据的存储数组。

如果使用的机器人没有激光雷达，但配备有Kinect等RGB-D摄像头，也可以通过红外摄像头获取周围环境的深度信息。但是RGB-D摄像头获取的原始深度信息是三维点云数据，而ROS的很多功能包所需要的输入是激光二维数据，是否可以将三维数据转换成二维数据呢？如果你已不记得RGB-D摄像头所发布的点云三维数据的类型，可以回顾7.1节的相关内容。

如果你已了解点云三维数据类型，那么将三维数据降维到二维数据的方法也很简单，即把大量数据拦腰斩断，只抽取其中的一行数据，重新封装为LaserScan消息，就可以获取到需要的二维激光雷达信息。这么做虽然损失了大量有效数据，但是刚好可以满足2D SLAM的需求。

原理就是这么简单，ROS中也提供了相应的功能包——depthimage_to_laserscan，可以在launch文件中使用如下方法调用：

<!-- 运行depthimage_to_laserscan节点，将点云深度数据转换成激光数据 -->
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
<remap from="image" to="/camera/depth/image_raw" />
<remap from="camera_info" to="/camera/depth/camera_info" />
<remap from="scan" to="/scan" />
<param name="output_frame_id" value="/camera_link" />
</node>

2.里程计信息

里程计根据传感器获取的数据来估计机器人随时间发生的位置变化。在机器人平台中，较为常见的里程计是编码器，例如，机器人驱动轮配备的旋转编码器。当机器人移动时，借助旋转编码器可以测量出轮子旋转的圈数，如果知道轮子的周长，便可以计算出机器人单位时间内的速度以及一段时间内的移动距离。里程计根据速度对时间的积分求得位置这种方法对误差十分敏感，所以采取如精确的数据采集、设备标定、数据滤波等措施是十分必要的。

导航功能包要求机器人能够发布里程计nav_msgs/Odometry消息。如图9-7所示，nav_msgs/Odometry消息包含机器人在自由空间中的位置和速度估算值。

·pose：机器人当前位置坐标，包括机器人的x、y、z三轴位置与方向参数，以及用于校正误差的协方差矩阵。

·twist：机器人当前的运动状态，包括x、y、z三轴的线速度与角速度，以及用于校正误差的协方差矩阵。

上述数据结构中，除速度与位置的关键信息外，还包含用于滤波算法的协方差矩阵。在精度要求不高的机器人系统中，可以使用默认的协方差矩阵；而在精度要求较高的系统中，需要先对机器人精确建模后，再通过仿真、实验等方法确定该矩阵的具体数值。

rosmsg show nav_msgs/Odometry