SLAM导航全栈书的正确打开姿势
SLAM导航全栈书的正确打开姿势
随着人工智能、机器人、无人驾驶等技术的蓬勃发展，作为底层技术基石的SLAM也逐渐被大家所熟知。人工智能技术如果仅仅停留在虚拟的网络和数据之中的话，那么它挖掘并利用知识的能力将非常有限。也就是说人工智能的最终归宿是与机器人实体相结合，实现机器人的完全自主化。而实现自主移动是其中的首要目标，目前SLAM导航技术是实现这一目标的热门研究方向。
与以往的技术浪潮不同，SLAM是一个软硬件相结合、理论加实战的浩大工程性问题。企业的目的是要将SLAM技术真正落地到产品，而不是等你慢慢研究数学理论或简单调几个参数。每个从业者都应该建立起大局观，以避免重复造轮子，换句话说我们缺乏SLAM全栈人才。
图1 SLAM从业人员现状
温馨提示
1.学习SLAM究竟在学些什么
对于大学里的学生，学习SLAM大多从看公式推公式开始入手。对于企业界的工程师们，则多以热门SLAM框架的代码解析或移植开始入手。由于SLAM导航技术在机器人、自动驾驶等方面的广泛应用，所以有大量的从业者是跨学科过来的，比如嵌入式、机械设计、自动化、电子工程、软件工程等。因此，对于不懂SLAM数学理论的从业者，通常会从学习ROS机器人系统编程、传感器、底盘、驱动等开始入手。
（1）深入数学原理
在高校的学生，往往跟着教授从SLAM的数学公式以及推导学起。高校教授讲课用的书籍通常具备很强的理论性，但大多数高深的数学理论在实际中使用极少，另外实际情况也不见得与数学理论模型完全吻合。这打消了许多人的学习积极性，感觉学习过程费时费力，到头来又感觉什么也没学会。正所谓天下SLAM学习者苦数学久矣，图2非常生动形象地反映了他们的心路历程（嘿嘿）。
图2 学习SLAM数学理论的心路历程
（2）解析热门框架
在企业中，工程师往往忙着在Github上收罗各种SLAM开源代码，找到一些与应用需求相符的代码慢慢逐句研读。
图3 代码中的各种矩阵向量的运算公式都是什么鬼哦
高高兴兴上Github收罗出一大堆SLAM开源代码，然后各种选择恐惧症都来了。只怪自己专业水平不够，只能根据排名胡乱选择一个靠前的开源代码先练手。一顿操作猛如虎，貌似每句代码都看得懂（不就是一些C++语法嘛，嘿嘿），貌似每句代码又都看不懂（各种矩阵向量运算，到底在搞什么鬼）。
（3）移植代码
对于坚持实用主义的学习者，管它什么数学原理还是代码逻辑，通通不重要。先搞个别人写好的项目代码安装到自己的电脑跑起来再说，然后顺利的话再移植到自己的实际机器人上搞出几个炫酷的演示效果先唬唬人。
图4 移植代码过程中的心酸只有自己知道
在移植代码前兴高采烈，不就是敲几个命令嘛。在移植代码时，被各种坑（“对不起您运行的节点不存在或者找不到路径，请确保已经正确安装了”、“系统终止了运行，并抛出了一个错误”、“由于缺少依赖xxx程序已终止”）折磨的脾气全无。大部分人是没有搞懂整个代码的组织逻辑、加之一些专业的第三方库的原理作用不了解、以及SLAM项目的开发与运行流程不熟。因此从移植代码中暴露的问题看，也提醒了广大实用主义者们要恶补一些必要的数学理论和代码解析知识。
（4）学完ROS很迷茫
对于嵌入式、机械设计、自动化、电子工程、软件工程等跨学科过来的学习者，通常最开始就是学习ROS机器人系统编程。跟着网上的一些教程，让小海龟在电脑屏幕上动起来十分令人激动。
图5 令ROS学习者激动的小海龟动起来喽
恐怕没有企业会招聘一个只会使用ROS的工程师吧，毕竟只会使用ROS的话可能什么也做不了。可能很多初学者听到ROS机器人操作系统，就被操作系统几个字吓住了。其实简单点说，ROS就是一个分布式的通信机制，帮助程序进程之间更方便地实现通信。解决这种分布式的通信问题正是ROS被设计出来的初衷，随着越来越多的人参与ROS开发及源码贡献，涌现了大量的第三方工具和实用开源软件包，ROS才变成现在的样子。
一个经常让初学者困惑的地方是，学会了ROS就算是学会机器人开发了吗？当然不是，严格意义上讲ROS只是一套通信机制而已，机器人中的各种算法和应用程序依然是用C++/Python等常见编程语言进行开发。换句话说，学习完ROS的代码组织方式以及通信方式之后，还要再进一步学习C++或Python程序开发，以及掌握必要的底层硬件的驱动亦或是学习上层的SLAM、导航、识别感知等算法。
（5）苦战各种传感器融合
对于搞机器人SLAM导航的人来说，万物皆可融合。有些剑走偏锋的学习者，一上来就各种传感器、各种卡尔曼融合、各种拼凑，想一想就很辣眼睛。
图6 各种辣眼睛的传感器融合方法
当然越多的传感器提供观测数据在理论上是能提升效果的，但毫无原理根据的胡乱融合往往会起到副作用。在融合各种传感器的数据之前，你最好先搞清楚背后的数学原理，不然各种数据反而在互相干扰。
2.目前SLAM学习的现状
很多初学者在学习完ROS之后，就不知道下一步该干什么了。而对于搞嵌入式及传感器的底层开发者来说，对ROS上层及算法层面软件的具体工作原理又非常困惑，常常有了解的好奇心但却无从下手。对于专门研究SLAM算法或导航算法的研究人员，他们往往专注于算法层面的某个很细分的领域，一般比较缺乏全局性的工程思维，至于将某项研究成果部署到实际的机器人上落地运行难度就更大了。这就导致各个领域的研究开发人员都在自己熟悉的领域内闭门造车，而缺乏领域之外的必要交流与实践。软件层面的开发者由于缺乏对机器人传感器、机器人主机和机器人底盘的系统性认识，往往在软件性能优化过程中涉及到软硬件深度优化方面的问题时就束手无策了。而硬件层面的开发者由于缺乏软件方面的必备基础，经常在理解软件层需求时出现偏差。由于缺乏相关的数学理论体系，ROS及硬件相关领域的开发人员大多只能充当调参侠，对SLAM导航方面的算法很难有实质性的改善。由于缺乏工程思维和实践经验，SLAM算法或导航算法方面的研究人员则很难将研究成果落地到实际机器人，甚至SLAM算法研究人员与导航算法研人员也存在不小的交流障碍。
图7 SLAM学习者的现状
总之在学习SLAM的道理上，“重复造轮子”、“调参侠”、“调包党”、“沉迷于高深理论研究而不能自拔”、“头疼医头脚疼医脚”、“不是在安装代码就是在去安装代码的路上”、“写bug改bug然后循环往复”、“以为在干算法（真实是疯狂的调包党）”、“一直在入门，从未被精通”这些标签总有一款适合你。这其实也不能怪大家，因为SLAM是一个软硬件相结合、理论加实战的浩大工程性问题。遗憾是，目前国内系统讲解SLAM的书籍非常之少，SLAM全栈技术则更是空白，大量涌入的从业者苦于找不到一本趁手的学习宝典。这正是我写作的初衷，用一本书将SLAM导航中的软件、硬件、数学理论、工程落地等一系列问题一网打尽。提升行业的整体认知水平，为SLAM导航技术的普及与落地贡献微薄力量。
图8 你可能需要一本SLAM导航全栈书
3.每个从业者都需要一本SLAM导航的全栈书籍
首先SLAM算法和导航算法是两大核心技术，都涉及到理论和实战两部分。而这两大核心技术的实战都需要落地到机器人，构造一台真实的机器人既需要硬件基础知识也需要编程基础知识。整本书在逻辑上分成四篇：
第一篇讲解ROS、C++、OpenCV，让大家掌握机器人开发中的必备编程知识。
第二篇讲解机器人的传感器、主机、底盘，让大家熟悉整个机器人的硬件构造以及工作原理。第三篇讲解本书的主角SLAM，首先从概率论、状态估计、滤波和最优理论逐步揭示出SLAM的理论本质，然后结合目前的各种热门SLAM框架进行代码实操，让理论结合实战。
第四篇讲解自主导航，首先从感知、路径规划、运动控制逐步揭示出自主导航的理论本质，然后结合导航开源代码以及在真实机器人上的实操让大家理解SLAM与导航之间的联系及应用。
图9 整本书的内容组织逻辑
（1）学习SLAM数学原理
在1986年，Smith和Cheeseman将机器人定位问题和机器人建图问题放在基于概率论理论框架之下进行统一研究。其中有两个开创性的点，第一是采用了基于概率论理论框架对机器人的不确定性进行讨论，第二是将定位和建图中的机器人位姿量与地图路标点作为统一的估计量进行整体状态估计，这算得上是同时定位与建图问题研究的起源。
图10 SLAM定位与建图中的概率学
在没有误差的理想状态，运动轨迹上的机器人位姿可以由运动位移量准确计算，由于机器人每个位姿都是准确的并且观测也是理想情况，观测路标特征的坐标也可以准确计算出来。实际情况是存在误差的，这就是为什么要用概率模型来表示机器人的运动模型、观测模型和待求状态的原因。
在不确定性条件下，求解SLAM问题中的机器人位姿（即定位）和环境路标（即建图）其实就是一个状态估计过程。由于机器人位姿的偏差，加上观测误差的影响，观测到的路标特征坐标也自然会偏离于真实路标特征坐标。也就是说机器人位姿和路标特征坐标的真实值是无法直接通过观测信息得到的，因为误差渗透进了各个地方。在统计意义上，如果能通过不断调整机器人位姿和路标的取值大小，使机器人位姿和路标的当前取值基本逼近于真实情况，那么这个不断调整机器人位姿和路标的取值的迭代过程就是所谓的状态估计过程。
图11 SLAM就是一个状态估计问题
用大白话说就是，如果图11中虚线表示的七角星及三角形与实线表示的七角星及三角形完全重合的话估计就准确无误了。其中三角形表示运动过程中机器人位姿，七角星表示运动过程中机器人观测到的环境路标特征，实线表示真实值，虚线表示估计值。状态估计过程之所以有效，是因为机器人位姿与环境路标之间存在某种强有力的关联（也叫约束）。目前大都从两个思路去构建约束，一个思路是在贝叶斯网络上构建这些约束，另一个思路是在因子图上构建这些约束。
图12 在贝叶斯网络上构建SLAM的约束
图13 在因子图上构建SLAM的约束
不管是贝叶斯网络还是因子图，只是把SLAM的约束构建起来了，可以理解是在构建方程组。那么接下来就要想办法求解SLAM约束中的机器人位姿和环境路标，可以理解是在解方程组。由于基于贝叶斯网络的SLAM约束一般采用卡尔曼滤波、粒子滤波等方法求解机器人位姿和环境路标，也被俗称为“滤波方法”。而基于因子图的SLAM约束一般采用梯度下降以及各种梯度下降变种等优化迭代的方法求解机器人位姿和环境路标，也被俗称为“优化方法”。
按照机器人上搭载的传感器，SLAM算法可以大致分为激光SLAM（对应书中的第8章）、视觉SLAM（对应书中的第9章）和融合SLAM（对应书中的第10章）。而按照SLAM约束求解方法，SLAM算法可以大致分为基于滤波方法的SLAM、基于优化方法的SLAM、基于人工智能方法的端到端SLAM等。
（2）通过主流框架进行SLAM实战
SLAM的发展日新月异，有大量的开源算法框架不断涌现，并且同一个算法框架也有大量不同版本的更新迭代。要将市面上出现的所有SLAM算法框架逐一讨论一遍既不现实，也没有必要。其实目前的SLAM具体技术路线就那么几个，无外乎激光SLAM、视觉SLAM、多传感器融合SLAM、与机器学习相结合的SLAM等等。其实目前市面上五花八门的SLAM框架基本上都是在这几个主要技术路线的经典框架下的微创新，要么对代码结构做一些重构、要么多加几种传感器、要么对一些功能模块进行升级替换，能划时代做出系统性技术突破和创新的少之又少。这并不是说咱们这个领域的研究人员不够努力不上进，恰恰反映了SLAM问题的特性，即需要大量的开发人员共同参与，进行小步快跑式的递进式创新，这些微小创新积少成多就能促成一次系统性技术突破，靠某个单独的技术改进就想实现SLAM技术跨越式的发展是不太可能的。
图14 挑选几个非常有代表性的SLAM实现框架进行讨论
因此本书中挑选了几个非常有代表性的SLAM实现框架进行讨论，激光SLAM技术路线的实现框架包括Gmapping、Cartographer、LOAM，视觉SLAM技术路线的实现框架包括ORB-SLAM2、LSD-SLAM、SVO，多传感器融合SLAM技术路线的实现框架包括RTABMAP、VINS，与机器学习相结合的SLAM技术路线的实现框架包括CNN-SLAM、DeepVO。到这里肯定有人会质疑，区区几百页书能将这些代码完全讲明白吗？我的答案是“能”。
分析一个开源代码，既不是长篇大论逐字逐句地分析每段代码的作用，也不是像记流水账式的介绍代码中每个参数的配置过程。研究一个开源代码，应该是要从中获得某些程序设计思想或者对其进行快速的移植裁剪。因为很多开源代码本来就存在一大堆问题，其代码的实现细节可能并没有多大学习价值，代码中的众多参数大部分可能实际也用不到。想要看代码细节含有的完全可以去Github上看代码注释或者一些Issue的讨论，而有光代码参数的配置详情也在代码所提供的官网文档中进行了逐一介绍，所以我在书中采用了三个角度（原理分析、源码解读、安装与运行）来分析各个开源代码。原理分析，其目的是将前面泛泛而谈的SLAM理论落实到具体的算法的具体数学公式中来，并给出这些数学公式适当的伪代码实现，这样做有利于在接着的源代码解读中理解数学公式的实现细节。源码解读，其目的是授人以渔，通过对源代码的输入输出数据关系图、代码文件组织结构、代码的总体调用流程让大家快速建立起全局性认识，学会这些方法论后修改代码或者进行移植裁剪就是小菜一碟了。安装与运行，其实就是让大家快速对代码建立感性认识，对于新手小白来说，将代码安装到电脑上并运行起来能极大地建立起学习的兴趣。
就拿书中关于Cartographer算法的例子来说，首先从局部建图、闭环检测和全局建图进行Cartographer原理分析。其中，局部建图介绍了激光雷达扫描点的帧间匹配、激光雷达扫描点的地图栅格化过程和地图数据结构。不管是什么SLAM算法，在工程实现上都表现为某个具体的地图数据结构，算法的运算过程基本都围绕着地图数据结构中的各个数据成员以及成员关系而展开，因此解读一个SLAM算法，首要任务就是搞清楚它的地图数据结构是怎样的。
图15 Cartographer算法的地图数据结构
然后从cartographer_ros功能包、cartographer核心库和ceres-solver非线性优化库进行Cartographer源码解读。在解读具体代码时，除了要搞清楚整个项目文件夹下各个具体文件都是什么用途外，最好还要能从全局上搞懂数据在程序之间是如何流动的，同时跟随数据的输入输出关系搞懂程序中各个类（Class）和函数（function）的调用关系，搞清楚了这些基本就掌握了代码的一大半了，剩下不过是一些细枝末节的编程语法细节罢了。书中通过框图来生动形象地展示Cartographer程序顶层的数据输入输出关系，如图16。通过流程图展示Cartographer程序类和函数的调用关系，如图17。
图16 Cartographer程序顶层数据输入输出关系
图17 Cartographer程序类和函数的调用流程
最后从Cartographer安装和运行进行具体实战，在讲解Cartographer安装时分享了我实际遇到的很多坑以及闭坑方法（比如要将src/.rosinstall配置文件中有关ceres-solver的下载地址更换为Github源，以解决ceres-solver下载慢的问题），对初学者来说应该很有帮助。在讲解Cartographer运行时，既有从运行官方的配置启动文件和测试数据集让大家快速将算法跑起来，又介绍了如何编写自己的配置启动文件和通入实际传感器数据来在真实机器人运行算法。
启动传感器命令
#启动激光雷达
roslaunch ydlidar my_x4.launch
#启动底盘，并发布轮式里程计
roslaunch xiihoo_bringup minimal.launch
#启动IMU
roslaunch xiihoo_imu imu.launch
#启动urdf模型
roslaunch xiihoo_description xiihoo_description.launch
算法配置启动文件
1 <launch>
2 <node name="cartographer_node" pkg="cartographer_ros"
3 type="cartographer_node" args="
4 -configuration_directory $(find cartographer_ros)/configuration_files
5 -configuration_basename xiihoo_mapbuild.lua"
6 output="screen">
7 <remap from="scan" to="/scan" />
8 <remap from="imu" to="/imu" />
9 <remap from="odom" to="/odom" />
10 </node>
11
12 <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
13 type="cartographer_occupancy_grid_node" args="
14 -resolution 0.05
15 -publish_period_sec 1.0" />
16 </launch>
启动建图命令
#启动建图
source ~/catkin_ws_carto/install_isolated/setup.bash
roslaunch cartographer_ros xiihoo_mapbuild.launch
#启动键盘遥控
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
#查看地图
rosrun rviz rviz
图18 Cartographer在实际机器人上的建图效果
在具体SLAM算法实战的过程中，补充讲解了很多在第7章SLAM中的数学基础不便于展开的理论知识。比如Gmapping算法实战中补充讲解了RBPF粒子滤波的具体细节，Cartographer算法实战中补充讲解了闭环检测的一种高效方法（分支界定搜索），ORB-SLAM2算法实战中补充讲解了欧拉角、四元数、旋转矩阵、李群李代数、多视图几何、视觉词袋模型等重要内容，VINS算法实战中补充讲解了多传感器联合标定原理、松耦合、紧耦合等，CNN-SLAM和DeepVO算法实战中补充讲解了机器学习、神经网络、深度学习等方面的内容。
（3）补习机器人开发中的编程和硬件基础知识
在上面的SLAM实战过程中，涉及到许多编程和硬件相关知识。比如ROS，几乎所有的SLAM实战项目都基于ROS。另外，要用实际的传感器数据来运行SLAM，必须搞清楚IMU、激光雷达、相机、编码器等传感器的工作原理以及使用方法，这样才能标定以及正确的驱动这些传感器。这时候读者就需要借助书中的第1~3章来补习ROS、C++、OpenCV等编程基础知识，借助书中的第4~6章来补习机器人传感器、机器人主机、机器人底盘等硬件基础知识。
ROS虽然全称是机器人操作系统，但简单点理解就是提供了一套分布式的通信机制而已。ROS中可运行程序的基本单位叫节点，节点之间通过消息机制进行通信，消息机制有话题（topic）、服务（service）和动作（action）三种。机器人要实现某个功能（比如SLAM），需要有众多不同功能的程序之间相互配合完成，ROS的分布式通信机制正是解决这些程序之间相互配合的利器。
图19 ROS的本质就是一套分布式通信机制
话题通信方式是单向异步的，发布者只负责将消息发布到话题，订阅者只从话题订阅消息，发布者与订阅者之间并不需要事先确定对方的身份，话题充当消息存储容器的角色，这种机制很好地实现了发布者与订阅者程序之间的解耦。
图20 话题通信
服务通信方式是双向同步的，服务客户端向服务提供端发送请求，服务提供端在收到请求后立马进行处理并返回响应信息。
图21 服务通信
动作通信方式是双向异步的，动作客户端向动作提供端发送目标后，动作的执行需要一个过程（比如处在导航节点的机器人），动作在被执行的过程中会实时反馈状态（比如导航过程中机器人的实时位置信息），动作被执行完成后会返回结果（比如已到达导航目标点或者导航失败）。
图22 动作通信
不难发现，只要掌握了话题通信，服务通信和动作通信也很容易上手。我觉得学习ROS最大的困难是从0到1的过程，在看了大量学习资料后，最重要的是自己亲自动手写一个ROS的程序出来并且编译调试全部通过，将程序真正在电脑上跑起来，哪怕只是一个hello_world程序。跟着书中1.5.1节的例程很容易编写出一个发布和订阅话题消息的ROS程序。
话题发布程序
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 #include <sstream>
5
6 int main(int argc, char **argv)
7 {
8 ros::init(argc, argv, "publish_node");
9 ros::NodeHandle nh;
10
11 ros::Publisher chatter_pub = nh.advertise<std_msgs::String>("chatter", 1000);
12 ros::Rate loop_rate(10);
13 int count = 0;
14
15 while (ros::ok())
16 {
17 std_msgs::String msg;
18
19 std::stringstream ss;
20 ss << "hello " << count;
21 msg.data = ss.str();
22 ROS_INFO("%s", msg.data.c_str());
23
24 chatter_pub.publish(msg);
25
26 ros::spinOnce();
27 loop_rate.sleep();
28 ++count;
29 }
30
31 return 0;
32 }
话题订阅程序
1 #include "ros/ros.h"
2 #include "std_msgs/String.h"
3
4 void chatterCallback(const std_msgs::String::ConstPtr& msg)
5 {
6 ROS_INFO("I heard: [%s]",msg->data.c_str());
7 }
8
9 int main(int argc, char **argv)
10 {
11 ros::init(argc, argv, "subscribe_node");
12 ros::NodeHandle nh;
13
14 ros::Subscriber chatter_sub = nh.subscribe("chatter", 1000,chatterCallback);
15
16 ros::spin();
17
18 return 0;
19 }
学会了ROS的话题、服务和动作通信基本就掌握了ROS的一大半了，剩下在学习一些ROS的基本调试方法和调试工具就够了，ROS的调试工具大致分为命令行工具和可视化工具两种。ROS命令行工具能在shell终端直接输入使用，类似于Linux命令，比如roscore、rosrun、roslaunch、rostopic、rosbag、rospack等。ROS可视化工具主要是rviz和rqt，rviz是ROS自带的三维可视化工具，可以让用户通过图形界面非常方便地开发调试ROS。比如可视化显示激光雷达、深度相机、超声波等传感器的数据、显示机器人的三维几何模型、显示路径规划实时轨迹、发送导航目标点等。rqt是基于Qt来开发的，因此rqt用户可以自由添加和编写插件来实现自己的功能。
如果说计算机程序是机器人的灵魂，那么硬件本体就是机器人的躯干。熟悉机器人上各个硬件的工作原理，能让你深入理解机器人中的计算机程序运行以及软硬件深度优化的过程。书中第4~6章将从“机器人传感器”、“机器人主机”和“机器人底盘”来展开讲解，以帮你熟悉机器人开发过程中必备的硬件基础知识。
图23 激光测距
图24 激光雷达扫描原理
从激光雷达的工作原理的介绍中，很容易理解软件算法中有关激光雷达性能参数的含义（激光线数、测距频率、扫描频率、量程、角度分辨率、测距精度）。同时利用书中介绍的雷达运动畸变校正方法，很容易对激光雷达的运动畸变进行校正。
图25 激光雷达的运动畸变
同样通过书中对相机成像原理的介绍，就很容易理解相机内参模型以及视觉SLAM中所谓的重投影误差是个什么东西了。
图26 单目和双目相机的成像原理
（4）让SLAM与导航相结合
通过上面的一系列学习，相信大家都了解了机器人的构造并将SLAM算法成功跑起来了，但是光有SLAM如何让机器人实现自主导航呢？接下你就可以跟着书中的第11~13章将SLAM与自主导航结合起来，在一台真实的机器人（xiihoo）上实现自主导航避障了。
图27 全局路径规划、局部路径规划、轨迹跟踪
#启动底盘
roslaunch xiihoo_bringup minimal.launch
#启动激光雷达
roslaunch ydlidar my_x4.launch
#启动IMU
roslaunch xiihoo_imu imu.launch
#启动相机
roslaunch usb_cam usb_cam.launch
#启动urdf模型
roslaunch xiihoo_description xiihoo_description.launch
#激光建图
roslaunch cartographer_ros xiihoo_mapbuild.launch
#视觉建图
rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt mono.yaml
#启动move_base自主导航
roslaunch xiihoo_nav move_base.launch
图28 实际建图效果
参考文献
[1] 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.
