如何学习SLAM

由于SLAM是一个错综复杂的研究领域，涉及到非常多的关键技术。这里先讲讲学习方法论，然后对一些关键性概念（包括SLAM、ROS、SLAM移动机器人）进行分析，最后给出典型应用案例方便大家进行实战。

温馨提示

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

• 点这里购买：《机器人SLAM导航核心技术与实战》购买链接
• 点这里观看视频讲解

0.学习方法论

关于SLAM学习方法论，我觉得可以用"螺旋上升"这四个字来概括，大家可以根据自己的喜好和实际基础，选择从理论或者代码开始看。下面假设先从理论开始学习，当然从代码开始学习也是类似的。

n 第1步：

假如以理论开始看起，就是涉猎各种会议期刊论文、博客、知乎，看得过程中肯定有很多不懂的很正常，一般论文或者博客都会提及相应的一些开源代码实现。

n 第2步：

然后你就可以挑一两个开源代码框架自己安装到电脑上运行跑跑数据集;如果你能将一两个开源代码亲自安装到电脑上，并且解决安装过程中的各种bug和兼容性问题，并成功的把数据集跑通了，相信你的编程和代码功底绝对有了质的飞跃。这时候你就会想，我光跑跑别人的数据集有什么用，于是你就想自己改改这些代码中的一些功能，比如给ORB-SLAM2添加一个地图保存功能(原版ORB-SLAM2是没有这个功能的)、或者让AMCL重新上电后能记住之前的位置(这样就不需要费劲吧啦的重定位搜索过程)，当然要改代码，必须懂原理。

n 第3步：

因此就要重新回到理论学习，这次就不是泛泛看书了，而是有针对性的看你要解决的问题所涉及的具体理论知识。比如你要实现ORB-SLAM2的地图保存功能，你就需要知道ORB-SLAM2的地图数据结构是什么，这个地图数据结构中涉及到关键帧、共视关系图、生成树本征图等概念，你就需要针对性的理解。当你对开源代码做了一些改进后，你可能信心百倍，想要搞到真实机器人上玩玩，这个时候就要回到实操。

n 第4步：

这个时候的实操就与之前跑个数据集不一样了，而是要考虑真实的机器人上的传感器，以及传感器数据的标定校准，tf转换关系，以及代码在机器人主板上的实时性能，还有整个系统的稳定性，就需要更全面系统的知识，实现系统性技术突破，达到这个程度你应该就是大牛了，带领团队做实际的项目，或者站在顶层解决最核心的技术问题。

图1  SLAM学习路线图

1.关于SLAM的那些事

在了解SLAM之前，需要先对机器人有一个整体的认识。机器人是一个复杂的装置，涉及到执行机构、感知、决策等主要环节。机器人上的配备的常用执行机构有轮式运动底盘、机械手臂、音响和显示屏；机器人上的感知设备通常有激光雷达、声呐、摄像头、IMU、轮式里程计编码盘、麦克风、触摸感应；机器人的决策是机器人智能的体现，机器人通常借助感知装置持续跟外部环境进行交互，从而来获取机器人的状态和环境的状态。

图2  一个典型机器人的模样

这样一看，机器人是一个超级复杂的东西，但是机器人上现在关键性的技术就那么几个。其实，学界和工业界热门的研究和开发也是围绕这几个关键技术展开的。

• 关键技术1：机器人移动底盘
• 关键技术2：机械手臂
• 关键技术3：SLAM导航
• 关键技术4：语音识别
• 关键技术5：物体识别

SLAM导航技术，主要用来解决机器人的定位、环境建图、自主导航、动态避障等问题。SLAM这项技术其实已经有几十年的历史了，SLAM最早是出现在军事应用中，比如勇气号火星探测车，在不能实时遥控的未知环境行星上的探测车为了执行任务，需要借助SLAM技术来导航和避障。后来慢慢的SLAM技术就从军用转民用了，有了我们现在看到的小到家里的扫地机器人大到无人驾驶汽车的各种SLAM应用，还有各种AR和VR应用很多也用到了SLAM技术。

其实单独的定位技术我们日常生活中能见到很多，比如最常用的GPS定位技术、wifi定位技术、磁条导轨定位技术。单独的环境建图技术也有很多成熟的应用，比如医学中的CT对人体的全方位扫描技术、电影制作中对某个三维物体的扫描建模、隧道勘探测绘等。但是面对机器人这样一个复杂的应用，单独的定位技术和单独的环境建图技术都不能很好的解决问题，于是结合了定位与建图的SLAM技术就出现了。接下来，简单的梳理一下定位、建图、SLAM同时定位于建图、SLAM导航技术的理论发展过程。

（1）机器人中的不确定性

机器人所处的环境存在大量不可预测性，机器人中传感器的测量存在噪声和干扰，电机等执行机构也存在一定程度的不可靠性，还有一些不确定性是由机器人的软件导致的。要研究机器人的感知和行为就必须对这些不确定性进行建模，利用概率理论可以明确的表示这种不确定性，并且可以利用概率算法来对概率分布进行推理计算，以数学的方式合理的表示机器人的模糊性和置信度。简单点说，就是通过对机器人的概率建模，可以对机器人的不确定性进行明确的可计算性的表示。

（2）机器人中的状态估计

处理机器人中的这种不确定性的概率技术我们称之为概率机器人技术，其核心是用传感器数据来估计状态的思路。

 

图3  机器人与环境交互

机器人通过传感器对机器人自身状态和环境状态进行测量，这里的机器人状态包括机器人位姿、运动里程计信息、线速度和角速度信息等，而环境状态包括环境中物体的位置、物体的特征等。机器人同时利用控制动作来调用执行机构和环境进行交互，这里的控制动作包括机器人运动、物体操纵等。这样，机器人在通过传感器测量和控制动作与环境进行交互的过程中，对环境模型和机器人位姿进行估计。对环境模型的迭代估计就是我们常说的环境建图，对机器人位姿的迭代估计就是我们常说的机器人定位。

利用传感器测量和控制动作对环境模型和机器人位姿进行估计由概率法则迭代计算来完成。控制动作的概率模型我们称之为运动概率模型，传感器测量的概率模型我们称之为测量概率模型，这里的概率法则迭代计算过程就是大名鼎鼎的贝叶斯迭代网络。

 

图4  贝叶斯迭代网络

（3）贝叶斯滤波及其各种实现算法

通过对机器人中状态估计的了解，我们知道了贝叶斯概率法则起着重要的作用，而且大部分计算置信度的通用算法都是由贝叶斯算法给出的。这里说的贝叶斯算法也称作贝叶斯滤波，该算法根据测量和控制数据计算置信度分布bel()。贝叶斯滤波是一种递归算法，也就是说t时刻的置信度由t-1时刻的置信度来计算。

贝叶斯滤波算法具有两个基本步骤，即预测和更新。根据运动概率模型，可以对初步的预测置信度分布；然后利用测量概率模型，对上一步预测得到的置信度分布做进一步的更新计算。贝叶斯滤波的具体实现算法有多种，根据置信度表示方式的不同，贝叶斯滤波的具体实现算法可以分为参数化实现算法和非参数化实现算法两种。参数化实现算法，就是将置信度分布用多元高斯分布的参数进行表示。根据采用不同高斯分布的参数，又可以分为卡尔曼滤波和信息滤波。非参数化实现算法，就是将置信度分布用有限数量的具体值来近似表示。根据采用不同具体值近似的方法，又可以分为直方图滤波和粒子滤波。

 

图5  贝叶斯滤波及各种具体实现算法

由于贝叶斯滤波理论及各种具体实现算法涉及到大量深奥的数学知识，由于篇幅和个人能力受限，就不展开了，有兴趣的朋友可以参阅《概率机器人》这本书，这里面有细致的讲解和推导。为了加深大家在阅读和编写代码时的理解，这里贴出了贝叶斯滤波的伪代码实现，和机器人中广泛应用的卡尔曼滤波和粒子滤波的展示实例。

 

图6  基础贝叶斯滤波的伪代码实现

 

图7  卡尔曼滤波的伪代码实现

 

图8  卡尔曼滤波对置信度分布的更新过程

 

图9  粒子滤波的伪代码实现

 

图10  粒子滤波对置信度分布的更新过程

（4）机器人中的运动与测量概率模型

了解了机器人中的状态估计及各种滤波概率算法后，我们发现机器人的运动和测量概率模型对算法中的预测和更新步骤起着至关重要的作用。

运动模型我们都知道，就是机器人一个地方运动到另一个地方，位姿变化与运动控制量之间的关系。那什么叫运动概率模型呢？由于控制量作用在机器人上使机器人的位姿变化的过程是一个存在误差的不确定过程，所以控制量作用后产生的结果需要用一个概率分布来描述，这就是运动概率模型。根据运动控制量的不同，可以分成速度运动模型和里程计运动模型。速度运动模型中，控制量是线速度和角速度；里程计运动模型中，控制量是机器人的前后时刻的相对位姿。速度运动模型中，线速度和角速度会受到机器人底盘构造的约束。实验表明，里程计运动模型虽然仍存在误差，但比速度运动模型要更精确。

 

图11  机器人的运动概率模型

同样的道理，机器人上的传感器在测量过程中存在误差，所以测量结果需要用一个概率分布来描述。主要讨论当下最流行的机器人测距仪，即激光扫描测距仪。由测距仪的近似物理模型，可以沿着一个波束测量到附近物体的距离，也就是波束模型。另一种测距仪模型不依赖任何传感器的物理模型，而是将传感器扫描到的终点值映射到地图的全局坐标空间，这就是似然域模型。估计一个测量和地图之间的相关性中，测距仪用到了地图匹配模型。还有一种是从传感器原始测量中提取特征，提取出来的特征我们可以称之为路标，这就是路标模型。

 

图12  机器人的测量概率模型

（5）移动机器人定位与建图

在了解了机器人中的状态估计、状态估计的各种概率实现算法、机器人运动与测量概率模型，我们再来重新理解上面这样一个经典的贝叶斯迭代网络，就很好理解了，这个里面包含了机器人定位和建图两大问题。机器人的定位其实就是位姿估计问题，位姿估计的概率实现算法可以有上面提到的EKF、AMCL等算法来实现，由于这两种算法在机器人位姿估计中都很流行，所以就不多展开了。机器人的建图其实就是环境特征估计问题，同样也可以用概率的方法就行估计。

 

图13  移动机器人定位与建图

（6）SLAM同时定位与建图

上面已经单独的提出了机器人定位与机器人建图的方法，但是独立的定位问题是建立在地图已知的情况下的，单独的建图问题也是建立在定位已知的情况下的。当机器人不能得到环境地图，也不知道自身位姿的时候，SLAM问题就出现了。也就是说SLAM要同时的进行机器人定位和建图，这个问题比单独的定位和单独的建图都要难得多。

在讨论具体的SLAM算法之前，我们先要了解SLAM的两种主要形式：在线SLAM问题和全SLAM问题。

 

图14  在线SLAM问题和全SLAM问题

在线SLAM算法的代表是EKF SLAM，历史上最早并可能是最有影响力的SLAM算法，可以说是SLAM研究的元老级算法。EKF SLAM中的地图是基于特征的，地图由点地标组成。除了估计机器人当前的位姿，EKF SLAM算法还估计路径上遇到的所有地标的坐标，也就是机器人位姿和地图地标点包含进联合状态矢量里，算法对该联合状态矢量进行估计。

（7）现今主流的SLAM算法

现今在机器人上使用最广泛的应该算激光SLAM了，在扫地机器人、服务机器人、AGV智能车上普遍搭载了单线激光雷达SLAM算法，像无人驾驶汽车、户外机器人则普遍搭载了多线激光雷达SLAM。另一种热门的研究是视觉SLAM，视觉SLAM有配备单目、双目、深度相机的多种形态，并且根据采用视觉特征点的区别还有直接法、半直接法、稀疏法之分。然后还有就是各种复合式的SLAM算法，比如激光与视觉融合的SLAM、融合了IMU的视觉SLAM。最后，就是一些最新颖的SLAM算法，比如用深度学习来做的端到端的SLAM、基于物体识别的语义SLAM。由于本文的重点不是SLAM综述，所以具体的算法性能比较就不展开了，有兴趣的朋友可以参阅相关SLAM综述文章。

 

图17  现今主流的SLAM算法

（8）机器人自主导航与动态避障

机器人用SLAM构建出了环境的地图，在已知了环境地图的情况下，可以用SLAM的重定位功能或者单独的基于已知地图的定位算法比如AMCL来进行机器人的定位。环境地图和机器人位姿都有了，就可以开始来做自主导航和避障了。机器人自主导航可以分成两个实现部分，第一个部分就是路径规划，第二个部分就是控制策略。路径规划利用地图信息寻找一条能到达目标的全局路径，全局路径在机器人导航过程中起到全局战略性的指导。理想情况是，机器人完全按照全局路径移动到目标，但是实际环境往往是多变和复杂的，而且机器人实际控制也会存在偏差，所以机器人的实际运动控制需要有一套控制策略来最终实现。控制策略需要尽量逼近全局路径、尽量远离障碍物、最快时间到达目标等因素，这些因素可以用一个回报函数来评价，寻找最佳控制策略的过程中递归的计算每一次行动的回报函数值。这样控制策略在回报函数的指引下，就可以给出最佳的控制策略，控制策略控制机器人完成实际的移动。

 

图18  主流路径规划算法

2.关于ROS的那些事

ROS是一个适用于机器人的开源的元操作系统。其实它并不是一个真正的操作系统，其底层的任务调度、编译、寻址等任务还是由Linux操作系统完成，也就是说ROS实际上是运行在Linux上的次级操作系统。但是ROS提供了操作系统应用的各种服务（如：硬件抽象、底层设备控制、常用函数实现、进程间消息传递、软件包管理等），也提供了用于获取、编译、跨平台运行代码的工具和函数。ROS主要采用松耦合点对点进程网络通信，目前主要还是支持Ubuntu系统，windows和Mac OS目前支持的还不好，所以推荐在Ubuntu系统上安装使用ROS。

总结起来就是，使用ROS能够方便迅速的搭建机器人原型。ROS使用了BSD许可证，这是一个很宽松的开放许可证，允许在商业和闭源产品中使用，这一点对开发产品的创业公司很重要。ROS当前的代码统计量，总行数超过1400万，作者超过2477名。代码语言以C++为主，63.98%的代码是用C++编写的，排名第二的是python，占13.57%，可以说ROS基本上都是使用这两种语言，来实现大部分的功能。

大家一听到ROS机器人操作系统，就被操作系统几个字给吓到了。其实，ROS就是一个分布式的通信机制，帮助程序进程之间更方便的通信。由于机器人是一个多部件的设备，四肢、视觉、听觉等部位都有配套的程序进行控制，那么要协调这些部位怎么办呢？就需要让这些分散的部位能够互相的通信，这正是ROS被设计的最初目的。随着越来越多的人参与到ROS的开发完善，ROS才发展成今天很完善很系统的样子来，涵盖了大量的第三方工具和大量的实用的开源算法软件包。搞懂了ROS的通信机制后，机器人的各种算法的开发还是基于我们常见的C++和Python的。

节点是主要的计算执行进程，功能包中创建的每个可执行程序在被启动加载到系统进程中后，该进程就是一个ROS节点，node1、node2、node3等都是节点（node）。节点都是各自独立的可执行文件，能够通过主题（topic）、服务（server）或参数服务器（parameter server）与其他节点通信。ROS通过使用节点将代码和功能解耦，提高了系统的容错力和可维护性。所以你最好让每一个节点都具有特定的单一的功能，而不是创建一个包罗万象的大节点。节点如果用c++进行编写，需要用到ROS提供的库roscpp；节点如果用python进行编写，需要用到ROS提供的库rospy。

 

图19  ROS网络通信的架构

如果你是刚刚接手ROS方面的开发或项目，你肯定会觉得ROS中的各种概念非常奇怪，但是当你对ROS的使用熟练之后，你就觉得这些概念很好理解了。与其他操作系统相似，一个ROS程序的不同组件要被放在不同的文件夹下，这些文件夹是根据不同的功能来对文件进行组织的。

 

图20  ROS程序的组织形式

3.SLAM与ROS到底是什么关系

SLAM最核心的地方在算法，侧重点在于如何构建出效果好的地图，并为机器人导航提供更好的数据保障。ROS帮忙解决传感器驱动、显示、各种核心算法间的沟通协调问题。如果做商用产品就另当别论了，商业的产品一般会专门开发自己的一套驱动、调度、显示的系统，或者拿ROS系统来裁剪以保障稳定性和效率。

4.了解SLAM移动机器人的整体构造

如果说计算机程序是机器人的灵魂，那么硬件本体就是机器人的躯干。熟悉机器人上各个硬件的工作原理，能让你深入理解机器人中的计算机程序运行以及软硬件深度优化的过程。从“机器人传感器”、“机器人主机”和“机器人底盘”来展开讲解，以帮你熟悉机器人开发过程中必备的硬件基础知识。

（1）硬件构造解析

一个典型的ROS移动机器人的硬件构造包括：带编码器的减速电机、电机驱动板、IMU模块、激光雷达、ROS主机、麦克风、摄像头、喇叭。带编码器的减速电机、电机驱动板、IMU模块、激光雷达应该是SLAM导航避障的标配硬件，ROS主机是机器人的计算中心（运行SLAM导航算法等），麦克风、摄像头、喇叭是完成语音交互和图像感知时需要的。

图21  一个典型的ROS移动机器人的硬件构造

（2）电机解析

按照电机的电源类型分为直流电机、交流电机，按照电机的换相方式分为有刷电机、无刷电机，按照电机转子的构造可以分为内转子电机、外转子电机。

图22  电机的种类

电机在实际的使用中，往往配合减速箱一起使用，来提高输出力矩和载重性能。减速箱有普通的减速箱和行星减速箱。行星减速箱的优势是产生同等减速比条件下减速箱体积可以更小，一般一些对体积敏感的地方用的多吧。

编码器起着对电机转速测量反馈的重要部件，有霍尔编码器、光电编码器、碳刷编码器等，编码器通常采用正交编码去形式进行信号输出，也就是AB两相信号正交输出，有些还会有Z相信号对转过一圈进行脉冲输出。

电调产生电机需要的相位信号，并且通过PWM等方式对电机转速进行调节。像一些小功率的直流有刷电机，电调其实就是一个H全桥电路，PWM信号控制H桥的四个臂起到控制电机转速和转向的目的。像一些大功率的直流无刷电机，电调的构造就比较复杂了，电调需要实现多相控制信号产生、电子换相、保护等。

图23  电调

电机是移动机器人的动力核心，所以选择合适的电机对一个移动机器人还是很重要的。电机的选型不仅要考虑参数性能，比如转速、力矩、载重、体积，还要考虑成本、安全、可实现性。如果是10KG载重级别的小型移动机器人，可以选择DC 12V带编码直流有刷减速电机就可以了。如果载重在30KG及以上的移动机器人的话，就需要选择功率和效率更高的无刷电机了，无刷电机的功率和减速箱参数可以根据实际机器人的载重和运动速度来选择。

 

图24  小型移动机器人电机选型参考

（3）电机驱动板解析

在了解过移动机器人的电机及选型，很自然就会想到接下来用什么来控制电机动起来呢？电机控制需要有PID控制，所以电机驱动板主要是用来实现PID控制的，同时电机驱动板也负责和上位机通信实时解析上位机发过来的速度指令和反馈电机的速度信息给上位机。电机驱动板一般选用的是STM32或者Arduino单片机这样的嵌入式MCU板子，有朋友可能会问既然都有上位机了（这里的上位机通常是电脑或搭载了操作系统的ARM主机）为什么不直接拿上位机来控制电机呢？首先，电机控制PID是实时性算法，也就是严格的实时时钟驱动的逻辑，一般的上位机系统不能满足这种高实时性的需求；另外就是出于系统解耦合和模块化设计的考虑，单独用一个价格便宜的MCU来实现电机控制。

 

图25  一个典型STM32电机控制板的程序框图

电机驱动板的另一个重要功能是跟上位机通信，借助上位机上的ROS驱动程序实现用ROS来操控电机并获取电机编码器反馈的里程计信息。也就是说，电机驱动板+电调+电机组成的一个整体的硬件可以当做是ROS机器人的一个传感器来使用，只需要在上位机上启动相应的ROS驱动程序，就可以在ROS中直接操控电机和获取里程计了。这样的一个效果就是前面提到的系统解耦合与模块化的设计思想，这样的设计对很多不懂硬件的ROS开发工程师来说是非常友好的，毕竟不用考虑繁琐的硬件问题了。

图26  在ROS中驱动电机控制板

（4）底盘驱动方式的解析

在了解了电机选型和电机控制板的作用后，底盘驱动方式是移动机器人另一个重要的考虑问题。底盘驱动方式，简单点说就是轮子如何布局和速度合成来让机器人灵活的行走。按照每个轮子的速度输出是否受约束，也就是一个轮子是否可以独立的输出速度而不考虑别的轮子输出速度的约束。约束型的底盘驱动方式有：差分驱动、阿克曼驱动、同步驱动。非完整约束型的底盘驱动方式有：全向驱动、腿足驱动。

图27  底盘驱动方式

差分驱动的底盘，是最常见的形式，像有名的turtlebot机器人、pioneer先锋机器人、家庭扫地机器人都是用的这种驱动方式。差分驱动，其实就是左右两个主动轮输出动力，机器人依靠左右轮的速度差来转弯，这也正是差分驱动名称的由来吧，通过控制左右轮产生不同的差速，来让机器人以不同的线速度和角速度移动。通常，差分驱动底盘还会安装万向从动轮来起到支撑作用。

阿克曼驱动的底盘，在汽车上是常见的形式，绝大部分的汽车底盘就属于阿克曼驱动型。阿克曼车型的两个后轮提供动力输出，两个后轮通过一个差速器来进行速度分配以产生不同转弯半径的差速，前方轮由方向盘控制负责转弯。阿克曼车型的关键就在于后轮的差速器，在汽车上，差速器是一个特殊的齿轮组合结构，能够根据车体实时的转弯半径将发动机的输出速度自动的分配到两个轮子上。而在机器人上，由于驱动轮一般直接连接电动机，所以这里的差速器由电子调速器来替代了。

同步驱动的底盘，最常见的就是履带车了，之所以叫同步驱动，就是同一边的前轮和后轮速度是一模一样的，实在上和差分驱动形式是一样的。有一些没有履带的四轮底盘，也是同步驱动方式。同步驱动的底盘，由于四个轮子，所以载重可以更大也跟平稳。不过相对于差分驱动底盘来说，轮式里程计精度就没那么高了。因为同步驱动的底盘在平坦的路面转弯时，车体会不同程度的颤动。所以同步驱动底盘是不擅长转弯了，转起弯来比较费劲。

全向驱动的底盘，底盘的运动是不受约束的，简单点说就是既可以横着走，又可以直着走，不需要转动车身方向，就可以朝任意方向前进。全向轮为机器人的移动效率起到了很大的提升作用。尤其是像大型货运仓库，AGV自动物流搬运小车采用全向方式移动能大大节省时间。全向轮是关键部件，全向轮又叫麦克纳姆轮，构造很精巧，不过就是价格昂贵，在非平坦的路面运行起来不那么顺畅了。

腿足机器人，结构复杂，就不在讨论范围了，有兴趣的朋友可以自己上网了解。

（5）IMU模块解析

在移动机器人中，IMU模块也是常用的传感器，IMU通常被用来和轮式里程计做融合，或者直接参与SLAM建图。IMU是惯性测量单元的简称，用于测量物体的三轴姿态角（roll、pitch、yaw）、三轴加速度（acc_x、acc_y、acc_z）、三轴角速度（w_x、w_y、w_z）等。有六轴IMU、九轴IMU、十轴IMU等，其实就是IMU输出的数据的种类来定义的。

 

图28  六轴、九轴、十轴IMU模块

IMU惯性测量单元在制造过程中，由于物理因素，导致IMU惯性测量单元实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。这就要求在使用IMU模块的原始测量数据时需要对数据进行误差校准，来消除轴偏差、固定零点偏差、尺度偏差等的影响。

 

图29  IMU误差校准的数学模型

校准过程，其实就是求解误差校准数学模型等式右边的各个校准系数。求解校准系数一般采用最小二次法或矩阵求逆来实现，具体的过程可以参考IMU校准方面的论文。

IMU数据校准完成后，还需要拿IMU各个轴的数据进行数据融合来得到三轴姿态角（roll、pitch、yaw）。姿态融合算法有很多种，其中比较流行的有EKF姿态融合。

可想而知，要在移动机器人中使用IMU模块的数据实现里程计融合或SLAM建图，我们需要首先完成更底层的IMU误差校准和姿态角融合两个过程。遗憾的是，很多做上层算法开发的小伙伴，对IMU模块底层MCU级别的程序开发并不擅长，市面上售卖的IMU模块大部分都不带误差校准和姿态角融合，有些内置了误差校准和姿态角融合的IMU模块价格又太过昂贵。期待不就的将来，能有带误差校准和姿态角融合的IMU模块出现，而且价格又能被广大机器人爱好者接受的好产品。

（6）激光雷达解析

在移动机器人中，获取机器人周围障碍物和环境的轮廓形状是非常重要的。使用激光雷达正是为了实现这个目的。利用扫描得到的障碍物信息，机器人就可以利用SLAM建立地图、并进行避障和自主导航。激光雷达之所以流行，得益于它能够精确的测距。主流的激光雷达基于两种原理：一种是三角测距法，另一种是飞行时间（TOF）测距法。其实很好理解，就是利用了最基本的数学与物理知识。

 

图30  三角测距法与飞行时间测距法

激光雷达有单线激光雷达和多线激光雷达，应用场合不一样，价格也差距悬殊。室内移动机器人一般用的是单线激光雷达，价格从几百块到几千块的都有。国内比较早做单线激光雷达的有上海思岚的rplidar雷达，后起之秀有深圳镭神智能的LS系列雷达、大族激光的杉川系列雷达、深圳玩智科技的EAI雷达等等，国外的老牌子雷达就有日本的HOKUYO雷达、德国的SICK雷达都是很有名的。

室外的无人驾驶汽车一般都配备有多线激光雷达，多线激光雷达一般有16线、32线、64线、128线等等。当然线数越多扫描到的信息也就越多，价格自然也就越贵。Velodyne公司是美国知名的多线激光雷达制造商，其VL系列的多线激光雷达也是很有名，价格也是从几万块到几十万不等。国内做多线激光雷达的有速腾、镭神都有在做。

（7）摄像头解析

机器人上搭载摄像头，可以用来做视频远程监控、人脸识别和追踪、视觉SLAM等等。可以选择的摄像头有单目（mono）、双目（stereo）和深度（RGBD）摄像头。如果是做视觉SLAM的朋友，需要对不同摄像头的优缺点有个了解。单目摄像头的优点是结构简单、成本低，缺点是在单张图片里，无法确定一个物体的真实大小。它可能是一个很大但很远的物体，也可能是一个很近很小的物体。通过相机的运动形成视差，可以测量物体相对深度。但是单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是尺度（scale），单凭图像无法确定这个真实尺度，所以称尺度不确定性。双目摄像头的优点是基线距离越大，能够测量的距离就越远；并且可以运用到室内和室外。缺点是配置与标定较为复杂，深度量程和精度受到双目基线与分辨率限制，视差计算非常消耗计算资源，需要GPU/FPGA设备加速。深度摄像头的优点是通过结构光或ToF(time of fly)的物理方法测量物体深度信息，典型代表Kinect/xtion pro/RealSense。缺点是测量范围窄，噪声大，视野小，易受日光干扰，无法测量透射材质等问题，主要用在室内，室外很难应用。

 

图31  摄像头

（8）ROS主机解析

机器人的处理核心通常是一个运行ROS系统的计算机主机，有的复杂一点的机器人还会有好几个计算机主机，比如一个用来搭载ROS系统运行各种机器人算法，另一个用来搭载Android系统运行用户交互界面。不过这里仅讨论搭载ROS系统的主机，有些朋友为了省钱直接用笔记本电脑作为ROS主机直接塞进机器人，这种情况除外。搭载在机器人上的ROS计算机主机，需要考虑编写安装问题和功耗问题，这里就列举一些常用的ROS主机供大家参考吧。

首先是中低端机器人中使用最广泛的树莓派，尤其是树莓派3B这个最为流行的型号，当然最近也推出的3B+升级版本型号。树莓派是极致性价比的板子了，200块钱的板子，配置了cortex-A53的处理器和1GB内存，简直就是最便宜的价格最顶配的性能了，并且官方有支持的ubuntu-mate系统也是超级给力，所以初学者选用这个板子是非常适合的。

然后中高端机器人可以选择瑞星微RK3399这款国产芯片出的板子，萤火虫和友善之臂都有出RK3399对应的板子。RK3399板子价格在1000块钱左右，配置了双核cortex-A72+四核cortex-A53的处理器，处理器绝对是给力的，配置的内存2GB/4GB可选。搭载裁剪版的Lubuntu或Xubuntu也是很不错的。

如果是高端机器人可以选择Nvidia家的jetson-tx2这个板子，这个板子号称是可以搭载在核弹上的板子，可想而知性能是非常牛的。板子售价4000块钱左右，有点小贵。配置了四核cortex-A57的处理器，256个CUDA核心的英伟达自家的帕斯卡架构GPU，内存8GB。板子的亮点是GPU，非常适合用来做深度学习和计算机视觉方面的应用。

 

图32  ROS主机

选择好一款适合的ROS主机后，就可以在主机上安装ubuntu系统了，然后再在ubuntu系统上安装ROS系统，最后就可以下载各种开源SLAM算法开始运行测试了，当然前提是你得有一个能移动的机器人并且各个传感器都是配备好了的。

5.结合典型应用案例进行实战

商业应用的机器人形态各异，功能丰富多彩，让人们眼花缭乱，误认为机器人的时代已经到来了。但是大部分市面上能看到的机器人功能相似，智能水平还很有限。一类机器人是主打自主移动导航类型的，比如扫地机器人、送餐机器人、送快递机器人、喷农药机器人等。一类机器人是偏向娱乐性的语音交互机器人，比如桌面型语音助理机器人、智能音响之类的。还有一类机器人是偏内容生产的，就是机器人本体和导航技术语音交互技术都不是自己做的，主要负责生产机器人的内容，比如菜谱机器人、英语早教机器人、会唱歌会跳舞的机器人。下面就结合一些实际的案例，进行详细一点的分析，帮助大家更好的理解ROS机器人这个行业的现状。

（1）智慧农业自动喷药机器人

这是一个规模化的果园里面，能够自动喷药和配合人工运输采摘下来的果实的机器人。在这样的应用场景，用机器人取代重复繁重的工作是很有意义的。机器人前端安装了一台3D激光雷达，能够感知果园道路两旁的果树并进行避障。感兴趣的朋友，可以读一下这篇论文。

图33  智慧农业自动喷药机器人

（2）智慧餐厅送餐机器人

这是一个典型的餐厅场景下自动送餐的机器人，机器人的主要功能就是自主的导航避障，将托盘上的饭菜送达客人的餐桌，配合语音提示和显示屏的交互来满足客人的点餐需求。其实这样的应用场景，机器人采用固定的轨道或磁条导航就已经够用了。毕竟机器人是用来为人类提供服务的，只要能提供稳定高效的服务，不必刻意最求技术有多先进。但是磁条导航也是有缺陷的，就是磁条的铺设比较麻烦，不同的餐厅需要进行不同的铺设，并且磁条时间久了也会损坏需要维修。所以也有很多送餐机器人开始尝试SLAM导航了。同样，餐厅服务员的工作也相对繁琐和管理成本高。如果用机器人来替代，不仅可以提高效率，还可以方便用计算机进行统一的管理和调度，非常适合标准化。

图34  智慧餐厅送餐机器人

（3）楼宇送快递机器人

说起送快递的机器人，最有需求的莫过于大型电商平台了。以前是消费者在平台上下单，还需要快递员层层转运才能最终送到买家手中。随着物流自动分拣仓库的越来越普及，物流问题最终就集中在最后一公里配送了。如果能用机器人运输快递来解决最后一公里的问题，买家从下单到收到快递完全自动化的了，这个可以说是行业的一种颠覆。公众和企业都在期待呀，作为自建物流配送的京东来说，当然要大力发展快递机器人喽。所以说，快递机器人一旦能成熟的商用，给广大消费者会带来非常大的便利。

 

图35  楼宇送快递机器人

（4）大型机房智能巡检机器人  

一个大型的机房，有成百上千台服务主机。要实时监测每台主机的各项参数，比如主机的温度、网络连接状态、电源指示灯等是否正常。在这样一种高噪音高辐射的恶劣环境里，用人来完成日常巡检工作异常困难。所以，利用自主移动机器人配合机器视觉等机会就能解决不少问题。类似的场景还有图书馆、变电站、甚至核电站。

 

图3.1.5 大型机房智能巡检机器人

参考文献

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

序 前言 编程基础篇 第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 习题