第1章 绪论
同时定位与建图（simultaneous localization and mapping，SLAM）主要包括两个主要任务：定位与建图[1]。为了能够建立出精确的地图，机器人的位置必须足够精确，而机器人的位置又依赖地图的精度，两者相互依赖，互为因果关系，因此SLAM算法是一个典型的“鸡生蛋”和“蛋生鸡”的问题。机器人在未知的环境下初始化自己的位姿，地图的建立又依赖机器人自身的位姿，根据地图和机器人的共视关系实时求解得到机器人的位姿，再对每帧位姿观测到的共视点位置进行优化得到地图。定位问题一直是热门的研究方向，在室外环境下GNSS技术已经被广泛商用，但是在遮挡环境和室内环境下无法使用。IMU传感器可以推算得到相对位置但是需要初始对准并对环境温度较为敏感。随着硬件计算能力和传感器制作工艺的提升，以视觉和激光为主的主动定位传感器在近几年得到长足的发展。
SLAM算法使用的传感器主要有视觉、激光、单目/双目和IMU，为了能够感知更多的信息和得到更加稳定的定位结果，常常将多个传感器进行组合。无论前端使用何种传感器进行环境感知，SLAM算法的核心问题仍旧是对状态的估计。文献[2]*先使用扩展卡尔曼滤波（EKF）算法作为状态估计器增量地估计得到机器人的位姿和地图点。目前基于图优化的状态估计器已经替代了原有的卡尔曼滤波算法。从单一的传感器融合逐渐过渡到了多传感器融合的更加稳定的SLAM系统。
根据SLAM系统使用传感器的不同，SLAM系统可以分为基于激光的SLAM系统、基于视觉的SLAM系统和基于多传感器融合的SLAM系统。
1.1 基于激光的SLAM系统
激光雷达定位过程中常用的两种测距方法是三角测距法和飞行时间（time of flight，TOF）法，具体原理如图1-1和图1-2所示。三角测距法是通过接收器的光斑成像位置来求解距离[3]，激光发射器发射激光，在照射物体后，反射光由接收器接收，其斑点成像位置为x ，已知激光发射器和接收器 之间的相对位置s和β，以及接收器的焦距f，由三角形公式便可推出被测物体的距离d。而TOF法是通过测量光的飞行时间来测量距离的。其工作原理为通过激光发射器发出一束激光信号，经被测物体反射后由激光探测器接收，通过测量发射激光和接收激光的相位差来计算被测物体的距离[4]。与基于视觉的SLAM系统相比，区别在于激光发射器向物体发射的光不同，基于激光的SLAM系统所用的激光雷达常使用波长在905～1550nm的电磁波，而基于视觉的SLAM系统所用的深度相机利用的是可见光波段（400～780nm）的电磁波。相比于视觉传感器，激光传感器拥有直接测距能力。它的定位精度常在厘米级甚至毫米级，且不受光照条件的影响。
图1-1 三角测距激光雷达工作原理图
图1-2 TOF激光雷达工作原理图
根据激光的测量特性可以分为二维激光雷达和三维激光雷达[5]。
二维激光雷达只有一个光束在平面上转动，其感知能力有限但是价格低廉，价格通常是三维激光雷达的1/10，主要应用在家用的扫地机器人和安全防护中。
三维激光雷达能够同时发射出多个光束，通常使用的三维激光雷达拥有16线、32线和64线，随着线束的提升其价格也会升高。近些年国内激光雷达生产厂商如雷神科技、禾泰科技的崛起使得三维激光雷达的价格显著降低，以Velodyne 32线的激光雷达为例，2016年其价格为50万元，而现在以5万元的价格即可购买到。
激光的核心感知部件更加复杂，对制作工艺的要求更高，即使近几年激光传感器的价格降低，激光雷达的硬件成本仍旧高于视觉传感器。基于机械传动的激光雷达由于依赖电机的高速运动，其使用寿命只有5年的时间。而近几年出现的固态激光雷达虽然没有了机械传送带，但是量产困难且只能够感知前方180°的信息。表1-1列举了业界常用的激光传感器的型号和具体参数，供读者参考选用。表1-2所示为视觉传感器的型号和具体参数。
表1-1 激光传感器对比表
表1-2 视觉传感器对比表
注：H表示水平；V表示垂直。
三维激光雷达成本的降低，显著促进了基于激光SLAM算法的发展和落地。但是由于三维激光雷达的测量特点，对于较近的障碍物，其测距精度会受到多径反射的影响而降低，因此激光雷达多应用于室外空旷的环境下。同时，激光雷达会受到下雨和大雾的影响导致其测量可靠度降低。
激光传感器相比于视觉传感器，其感知能力较弱，近年来涌现出了很多和激光点云识别相关的论文，其中点云的语义分割及实例分割领域取得了显著进展，在点云语义分割方面，Qi等[6]*次根据点云数据不规则、采样密度不确定的特点提出PointNet。利用多层感知机逐点学习特征，通过*大池化层整合成全局特征以预测点云语义标签，同时提出T-Net网络结构解决点云旋转不变性的问题。Jiang等[7]借鉴传统图像SIFT描述子的思想设计了一种可端到端输出的编码点云方向信息和自适应物体尺度大小的网络模块PointSIFT，该模块实现了尺度不变性，集成到基于PointNet的分割方法中提升了分割精度，但速度和效率较低。Kumawat等[8]为了降低三维卷积神经网络的计算和存储成本，提出了一种基于3D短时傅里叶变换（short term Fourier transform，STFT）的Corrected Local Phase Volume （ReLPV）块来提取3D局部邻域的相位，显著减少了参数数量。Ma等[9]为了更好地捕捉高维空间中的局部几何关系，提出了点全局上下文推理（PointGCR）模块，利用无向图表示沿通道维度捕获全局上下文信息，并且PointGCR是一个即插即用、端到端可培训的模块，它可以很容易地集成到现有的分割网络，以实现性能的提高。在点云实例分割方面，Hou等[10]提出了三维全卷积语义实例分割（3D-SIS）网络，该网络从颜色和几何特征中学习，使用三维区域提议网络（3D region proposal network，3DRPN）和三维感兴趣区域（3D-ROI）层来预测边界盒位置、对象类标签和实例掩码，实现RGB-D扫描上的语义实例分割。Zhang等[11]提出了一种用于大规模户外激光雷达点云分割的网络，该网络利用自注意力机制在点云的鸟瞰图上学习特征表示，*后根据预测的水平中心和高度限制得到实例标签。Jiang等[12]提出了一种自底向上的3D点云实例分割框架PointGroup，该框架利用两个分支网络分别学习点语义标签和点偏移向量将每个点移向各自的实例质心，通过原始点坐标和偏移位移的点坐标对点进行聚类，以优化点分组精度。Shen等[13]应用离散余弦变换（DCT）将高分辨率二值Grid Mask编码为紧凑的向量，提出了一种低复杂度的高质量掩码表示DCT-Mask，可以集成到大多数基于像素的实例分割方法中，获得了显著的效果。但是由于这些方法一般计算量大，难以部署在边缘设备中。
1.1.1 发展历程
激光SLAM算法从2D激光雷达发展到了3D激光雷达算法。在2D激光算法领域中《机器人概率学》 这本书厥功至伟 ，这本书详细介绍了2D激光雷达的测量模型、基于卡尔曼滤波的状态估计器和概率论的基本原理和推导，促进了2D激光SLAM算法的发展。目前激光雷达大多使用网格地图对空间进行建模，根据状态估计器的不同大致能够分为如下三类。
（1）基于粒子滤波的2D激光SLAM算法。
Gmapping 是*为知名的基于Rao-Blackwellisation粒子滤波的2D激光SLAM算法，Gmapping算法由FastSLAM算法改进而来。
（2）基于图优化的2D激光SLAM算法。
KartoSLAM、LagoSLAM、Cartographer算法中状态估计器使用的是图优化，其中*为知名的是谷歌于2016年在ICRA上发布的Cartographer算法，此算法将IMU集成在系统中，即使使用2D激光也可以建立3D空间地图，同时使用Branch-and-Bound算法提高了回环的稳定性和精度。
（3）3D激光SLAM算法。
EKF-SLAM是激光SLAM领域中*早期的成果之一，它由状态预测、过程更新以及状态增广三部分组成。该算法的位姿估计和环境地图构建均由高维状态向量表示，利用泰勒公式将线性方程线性化，同时对状态向量的均值与方差进行估计和优化。
3D激光相比于2D激光在一帧数据中拥有了更多线的激光数据，因此提高了激光匹配的准确度。在算法发展初期，3D激光SLAM算法用 的是Scan-to-Model 算法框架，即将当前帧扫描到的激光数据和整个地图的激光数据做匹配，其典型的算法有IMLS-SLAM和NDT算法，其中NDT算法已经集成在了PCL点云库中，可以非常方便地使用。但是由于Scan-to-Model算法需要和整个地图进行匹配，因此其执行效率较低，作者在实际项目中使用TX2板卡运行NDT算法，*高只能以3Hz速度运行，难以在高速场景中进行使用。为了弥补这种缺陷，以帧-帧匹配为代表的LOAM 类算法应运而生。这种算法的核心思想是使用鲁棒的点云帧间匹配来得到帧间的相对位姿变化。更加详细的关于3D激光SLAM算法的阐述见第6章。
近年来，随着深度学习有效性的提升，激光点云的SLAM框架中也引入了深度学习这一环节。深度学习在激光点云SLAM系统中主要能够完成如下任务：更加稳定的点云描述子生成和特征点云提取；识别和分割地面或动态物体；端到端的定位网络。激光点云数量较多，导致对于GPU计算资源有着较高的要求，很难部署在边缘设备中。
1.1.2 挑战和未来发展方向
仅依赖激光进行定位与建图在动态环境下很难达到良好的效果，主要原因是激光雷达感知相比于视觉传感器的语义感知能力要弱。为了解决激光感知能力弱的问题，近几年学术界使用激光传感器进行语义感知和理解，但是激光的语义感知深度学习模型推理时间较长，并不适合于工业界实际产品的落地。早些年有些工作尝试使用IMU来对动态环境进行滤波 ，动态环境的剔除结果过分依赖IMU的稳定性和精度。同时激光在处理几何高度一致的环境时，如走廊和非常空旷的驾校练习场，定位误差和精度退化比较严重。在较大的定位场景下，常常使用绝对定位传感器弥补激光雷达的不足，如GNSS和UWB。
如果将空间中的点云进行聚类，使用类-类的匹配关系计算相对位姿，会显著降低误匹配的发生。相比于单个点只可以使用*率作为描述子，聚类之后的点云可以提取多个维度的参数作为描述子进行匹配。为了提高激光定位的精度和稳定性，第6章提出了一种基于平面几何特征的定位方法，*先，利用三维激光点云稀疏的特点设计了一种拥有几何意义的*率，新设计的*率计算方法在不同场景下*率阈值能够保持不变，从而能够有效地提取出空间中的平面点。其次，基于检测出的平面点提出了一种三次增长的平面检测方法，将三维激光点映射到体素空间后在体素空间中进行BFS增长，提高了平面点识别的准确率。然后，使用各帧提取出的平面点设计了一种基于膨胀和腐蚀的平面多边形凹凸点检测方法并用于平面匹配。*后，根据匹配的平面提出了一种不需要地图点参与的位姿优化方法。
同时激光容易受到恶意攻击的干扰导致其测量结果完全错误。文献[14]通过光电干扰方式成功欺骗了激光雷达，使其输出的测距结果完全错误。除了测距结果容易被欺骗，在使用深度学习模型进行感知的情况下，文献[15]证明其感知结果也容易被人为篡改。如何避免激光传感器被人为欺骗，尽可能地提高测量精度和稳定性是激光雷达广泛商业应用的必要前提。
目录
前言
第1章 绪论 1
1.1 基于激光的SLAM系统 1
1.1.1 发展历程 5
1.1.2 挑战和未来发展方向 6
1.2 基于视觉的SLAM系统 7
1.2.1 发展历程 7
1.2.2 未来发展方向 10
1.3 基于多传感器融合的SLAM系统 10
1.3.1 多传感器标定 13
1.3.2 激光和视觉融合 15
1.3.3 未来发展方向 16
参考文献 18
第2章 SLAM基础算法 22
2.1 视觉与激光SLAM算法 23
2.1.1 视觉SLAM算法 23
2.1.2 激光SLAM算法 32
2.1.3 IMU算法 38
2.2 状态估计模型 46
2.2.1 卡尔曼滤波 47
2.2.2 粒子滤波 49
2.2.3 非线性优化 55
参考文献 57
第3章 SLAM相关数学知识 59
3.1 非线性优化方法 59
3.1.1 基本方法介绍 59
3.1.2 高斯-牛顿法 61
3.1.3 L-M方法 63
3.1.4 Dog-leg法 65
3.2 非线性优化中的求导方法 68
3.2.1 SLAM后端非线性优化 68
3.2.2 自动求导 71
3.2.3 解析导数 73
3.2.4 代数导数 73
3.3 参数化方法 75
3.3.1 旋转矩阵 75
3.3.2 欧拉角 76
3.3.3 四元数 77
3.4 误差的定义和位姿的三种物理含义 79
3.4.1 SLAM误差定义 79
3.4.2 位姿的三种物理含义 80
参考文献 81
第4章 传感器标定 82
4.1 视觉传感器标定方法 82
4.1.1 相机模型 82
4.1.2 单目相机标定方法 84
4.1.3 多目相机标定方法 88
4.1.4 标定开源工具 91
4.1.5 基于三维标定板的相机内参标定新方法 93
4.2 视觉-激光外参标定 101
4.2.1 现有激光-相机外参标定方法 101
4.2.2 基于三维标定板的自动外参标定方法 103
4.3 GNSS-IMU-视觉外参标定方法 104
4.3.1 视觉-IMU标定方法 104
4.3.2 GNSS/INS-激光雷达/相机外参标定方法 107
参考文献 110
第5章 视觉SLAM方法 112
5.1 基于李群李代数的ICP总体*小二乘方法 112
5.1.1 研究内容概述 112
5.1.2 基于李群李代数的总体ICP算法 113
5.1.3 基于向量的坐标解算方法 120
5.1.4 实验结果 123
5.2 基于稳定框架下的PnP方法 130
5.2.1 研究内容概述 131
5.2.2 归一化PnP算法 135
5.2.3 实验结果 138
5.3 不依赖FEJ的位姿图优化方法 149
5.3.1 研究内容概述 149
5.3.2 增量式PGO算法 150
5.3.3 实验结果 158
参考文献 168
第6章 激光SLAM方法 172
6.1 现有激光SLAM算法 172
6.2 特征点提取与匹配 175
6.3 基于体素的平面检测方法 180
6.3.1 平面匹配 180
6.3.2 多边形边界估计 183
6.3.3 平面参数更新方法 184
6.3.4 激光*率的检测结果 187
6.3.5 基于三次增长法的平面检测结果 191
参考文献 193
第7章 多传感器融合的SLAM方法 195
7.1 GNSS定位原理与方法 195
7.1.1 GNSS定位基本原理 196
7.1.2 GNSS定位性能评价 199
7.1.3 GNSS定位与SLAM的坐标系统 202
7.2 多传感器松耦合/紧耦合方法 205
7.2.1 Visual-IMU组合模型 205
7.2.2 LiDAR-IMU组合模型 209
7.3 激光-视觉的融合方法 214
7.3.1 优化方法 216
7.3.2 基于平面投影的可视化方法 224
7.4 激光-视觉-GNSS-IMU融合方法 227
7.4.1 融合框架 228
7.4.2 基于因子图优化的状态估计 229
参考文献 232
第8章 SLAM系统典型应用 234
8.1 SLAM技术在变电站巡检中的应用 235
8.1.1 背景介绍 235
8.1.2 解决方案 236
8.1.3 测试结果 236
8.2 SLAM技术在村镇土地调查中的应用 238
8.2.1 村镇用地信息获取的特点 239
8.2.2 村镇用地信息获取的SLAM技术模式 240
8.2.3 基于空地协同的用地信息获取方法 241
8.2.4 基于空地协同的点云配准与三维重建 246
8.2.5 实验设计与分析 247
8.2.6 展望 253
8.3 SLAM技术在城市绿化智能管护中的应用 254
8.3.1 背景介绍 254
8.3.2 城市绿化智能管护系统组成 255
8.3.3 系统功能及性能 257
8.3.4 系统测试结果分析 260
8.3.5 应用前景 266
8.3.6 小结 266
参考文献 266
