《复杂环境下基于因子图的SLAM理论与方法》系统介绍以激光雷达(LiDAR)、惯性测量单元（IMU）和全球导航卫星系统为主要传感器的多源融合同时定位与地图构建（SLAM）技术。《复杂环境下基于因子图的SLAM理论与方法》以因子图优化为多源数据融合框架，围绕复杂环境下所面临的几种典型重难点问题，从原理阐述、公式推导和应用实践等多个维度，对基于因子图的SLAM理论与方法进行全面、深入解读。《复杂环境下基于因子图的SLAM理论与方法》主要内容包括基础数学原理、组合导航原理、性能评估方法、强度和时间信息增强的激光雷达里程计、结构化环境下基于连续时间轨迹估计的LiDAR/IMU时空标定、基于连续时间轨迹估计的LiDAR/IMU紧耦合导航系统、大尺度复杂环境下基于分层优化的三维地图构建等，采用循序渐进的方式，从基础理论和数学模型入手，由单一LiDAR系统逐步扩展到与IMU、GNSS融合的系统，对完整的SLAM体系进行抽丝剥茧的分析和介绍。

目录
“导航与时频技术丛书”序
前言
第1章 绪论 1
1.1 现代化地理空间信息的发展 1
1.2 SLAM技术的发展 4
1.2.1 SLAM问题的提出和定义 4
1.2.2 SLAM的发展和瓶颈 6
1.3 本书重点和主要内容安排 8
1.3.1 本书重点讨论的难点问题 8
1.3.2 本书重点关注的技术问题 9
1.3.3 本书主要内容安排 9
参考文献 11
第2章 基础数学原理 13
2.1 坐标系 13
2.1.1 坐标系的定义 13
2.1.2 坐标系的表达及变换 15
2.2 李群和李代数 17
2.2.1 两种重要的李群和李代数 18
2.2.2 指数映射与对数映射 22
2.2.3 两种非线性误差模型的李代数求导 24
2.3 基于因子图的非线性优化 27
2.3.1 非线性优化 27
2.3.2 因子图模型 28
2.3.3 稀疏性 29
2.3.4 增量式平滑算法 31
参考文献 32
第3章 组合导航原理 33
3.1 运动学 33
3.1.1 刚体运动学 33
3.1.2 特殊正交群和向量空间上的动力学 36
3.1.3 特殊欧氏群上的动力学 37
3.2 三维视觉导航 37
3.2.1 LiDAR测量 37
3.2.2 使用LiDAR进行航迹推算 39
3.3 惯性与组合导航 42
3.3.1 惯性导航 42
3.3.2 惯性/卫星组合导航 48
参考文献 54
第4章 性能评估方法 55
4.1 实验环境搭建 55
4.1.1 时间同步 55
4.1.2 基准真值 57
4.2 数据集 60
4.2.1 仿真平台 61
4.2.2 开源数据集 62
4.2.3 地面移动平台 62
4.2.4 车载平台 63
4.2.5 可穿戴平台 65
参考文献 66
第5章 强度和时间信息增强的激光雷达里程计 67
5.1 概述 67
5.2 相关技术及典型方法 69
5.2.1 基于LiDAR的SLAM技术 69
5.2.2 基于点云的动态目标分割技术 71
5.2.3 典型方法 72
5.3 系统概述 77
5.4 方法介绍 78
5.4.1 特征提取滤波器 78
5.4.2 强度配准 83
5.4.3 动态目标剔除 85
5.4.4 LiDAR里程计 87
5.4.5 LiDAR地图构建 90
5.5 实验分析 91
5.5.1 模块功能测试 91
5.5.2 系统性能测试 95
5.5.3 总结与分析 101
参考文献 101
第6章 结构化环境下基于连续时间轨迹估计的LiDAR/IMU时空标定 107
6.1 相关技术及典型方法 109
6.1.1 LiDAR与IMU的标定及IMU初始化技术 109
6.1.2 典型方法 111
6.2 方法概述 114
6.2.1 问题描述 114
6.2.2 系统框架 115
6.3 方法介绍 116
6.3.1 结构化地图构建 116
6.3.2 数据预处理 118
6.3.3 动态初始化 119
6.3.4 基于连续轨迹的标定 123
6.4 实验验证 128
6.4.1 实验介绍 128
6.4.2 仿真测试 128
6.4.3 实测数据测试 132
6.4.4 总结与分析 136
参考文献 136
第7章 基于连续时间轨迹估计的LiDAR/IMU紧耦合导航系统 139
7.1 相关技术及典型方法 141
7.1.1 LiDAR/IMU组合系统技术 141
7.1.2 典型方法 144
7.2 方法概述 152
7.2.1 问题描述 152
7.2.2 系统框架 153
7.3 方法介绍 154
7.3.1 因子图的状态估计 154
7.3.2 数据预处理 155
7.3.3 自适应初始化 156
7.3.4 LiDAR/IMU紧耦合估计 158
7.3.5 点云地图管理 164
7.4 实验验证 164
7.4.1 鲁棒性测试 164
7.4.2 系统性能测试 171
7.4.3 总结与分析 177
参考文献 178
第8章 大尺度复杂环境下基于分层优化的三维地图构建 182
8.1 相关技术及典型方法 184
8.1.1 大尺度三维地图构建和优化技术 184
8.1.2 大尺度回环检测技术 185
8.1.3 典型方法 186
8.2 方法概述 188
8.2.1 问题描述 188
8.2.2 系统框架 189
8.3 移动测量的概率模型 190
8.3.1 传统MMS概率模型 191
8.3.2 融合SLAM技术的MMS概率模型 191
8.4 方法介绍 192
8.4.1 局部地图优化 194
8.4.2 全局地图优化 196
8.5 实验验证 201
8.5.1 定位定姿精度测试 201
8.5.2 地图质量测试 206
8.5.3 复杂环境下的鲁棒性测试 209
8.5.4 总结与分析 215
参考文献 215

第1章绪论
　　1.1现代化地理空间信息的发展
　　地理空间信息是指用于描述地球表面或附近物体时空属性的数字化信息，包含但不限于地理坐标等位置信息、北偏角等朝向信息、目标纹理色彩等属性信息及时间信息等众多要素。传统测绘技术一直是获取地理空间信息的主要技术手段，如获取目标位置信息的卫星大地测量技术、获取环境三维信息的航空摄影测量与移动测绘系统（mobile mapping system，MMS）。在传感器技术和计算机技术的飞速发展和大力推动下，人类对环境的观测手段越来越丰富，对数据的处理和表达方式越来越多样，伴随大数据、云计算、人工智能、机器人等新技术浪潮的席卷，全球新一轮的科技革命悄然而至。新技术的快速发展为测绘科技的进步不断蓄能，使现代测绘正成为大众创业、万众创新的重要领域[1]。打破对传统模式的认知，探索测绘技术与高新科技的融合路径，研究更加泛化、高效的地理空间信息数据获取新方法，实现地理空间信息数据对现实环境的支持和增强，是现代测绘发展的重要方向之一[2]。数字化地理空间信息是自动驾驶、数字地球等高新应用产业的数据基础，它能够将现实世界中的物理对象与虚拟世界的数据相联通，从而由虚拟世界的数据挖掘驱动现实世界的生产力增长。空间位置服务和实景三维地图是地理空间信息数字化的两个重要环节，作为真实、立体、时序化反映人类生产、生活和生态空间的时空信息，已然成为国家重要的新型基础设施。就目前的移动测量、全源导航定位和三维地图构建等技术的发展情况来看，对于***规模的实景三维数据的采集和处理任务，仍有大量的技术瓶颈亟待突破[3]。
　　传统测绘技术重点解决了高精度的问题，但通常要求具备良好的观测条件，需要大量人工参与，且观测数据规模相对较小。以高精度地图为*的地理空间信息产品对数据的采集和质量都提出了更高的要求，主要体现在以下四个方面：
　　（1）无缝化。以全球导航卫星系统（global navigation satellite system，GNSS）为定位基础的移动测量技术能够满足室外开阔环境下三维地理空间信息的快速获取需求，但在长时间GNSS失锁状态下难以正常作业。现实环境极具复杂性，存在大量诸如地下停车场、地下管廊和隧道、森林、城市峡谷、井工矿等庞大且复杂的室内环境，因此要求数据采集系统必须具备室内外无缝的一体化定位与地图构建能力。
　　（2）动态化。传统测绘的高精度通常以静态测量和对稳定目标长时间的重复观测为代价。然而，在实际应用中“动态化”无处不在。其中既包括了测量设备的动态运动，如移动测量、GNSS动态定位等，又包括了观测环境或观测目标的动态情况，如道路环境中的行人和车辆在点云中形成的“鬼影”。因此，系统需要能够适应动态环境，同时具备动态测量能力。
　　（3）高效化。传统测量工作需要投入大量人力，严重制约了数据采集效率。实际的高精度地图数据采集任务覆盖区域广，依靠传统测量方式进行数据采集显然是不现实的，因此需要采集系统具备更高效的数据采集能力。此外，数据处理的复杂度不宜过高，以确保高效的数据后处理效率。
　　（4）多样化。传统的测绘信息主要包括距离、角度和坐标等几何相关量，这种地理信息表达方式不够直观，难以真实反映观测环境的现实情况。另外，低维的几何信息缺乏对所处环境的深层次表达，如道路中的路标信息、道路指示灯信息和车辆行人的运动信息等。因此，要求地理空间信息产品包含更深层次的语义化信息，以满足无人系统的智能化需求。
　　目前，常用的定位与地图构建传感器包括GNSS设备、惯性导航系统（inertial navigation system，INS）、激光雷达（light detection and ranging，LiDAR）和相机等。这些传感器均存在各自的局限性，例如，INS存在严重的误差累积，且因高精度的惯性器件价格高而应用成本较大；基于相机的视觉里程计（visual odometry，VO）无法获得环境的真实尺度，且难以适应复杂的光照变化；激光雷达里程计（LiDAR odometry，LO）在几何结构较弱的环境下难以配准点云；GNSS的定位性能受环境的影响巨大，当信号受遮挡或存在严重的多路径误差时，将出现严重的定位偏差。因此，依靠多传感器信息深度融合的方式解决高精度、高鲁棒的定位与地图构建问题已成为工业界和学术界的共识。多源信息融合的优势主要表现在以下几个方面[4]：
　　（1）弥补数据源自身缺陷。多源数据融合可以获得对同一观测目标的不同类型观测，可以有效弥补某一种数据本身的缺陷。例如，通过与LiDAR组合来弥补相机无法获取观测点景深的不足，同时为三维点云附着颜色和纹理信息；利用惯性测量单元（inertial measurement unit，IMU）的高频积分输出补充VO或LO低频位姿估计的输出频率。
　　（2）有效利用算法原理的互补性。从算法层面来看，多传感器组合为解决定位与地图构建问题提供了更多可能。例如，根据IMU的动力学模型对其观测量积分得到相对位姿变换，它可以作为LO/VO邻帧匹配的可靠初值，并辅助机械式旋转测量的LiDAR消除点云运动畸变。
　　（3）提供更多可靠的观测约束。在保证一定观测精度的前提下，更多的观测信息有助于建立更加完备的约束模型，从而保证估计量的无偏和*优，例如，或INS等递推系统不可避免地存在误差累积，加入GNSS观测可以有效限制轨迹的漂移。
　　（4）实现更加全面的功能。利用多源融合信息构建的室内外一体化三维地图能够更加真实、全面地复现现实场景，为载体感知外部环境和导航控制提供重要的数据资料。例如，地图构建任务离不开GNSS的支持，一方面是因为GNSS是目前获取定位信息*可靠的手段；另一方面是因为GNSS能够获得大地坐标系下的三维坐标，更便于用户使用，但同时需要相机和LiDAR等视觉观测设备的加入，从而利用点云和图像建立环境的更真实表达。
　　综上所述，深入研究复杂环境下多传感器融合的自主定位及探索式三维地图构建相关理论与方法具有重要意义。该领域的研究无论是在民用方面还是在军事方面都具有巨大的发展潜力和应用价值。
　　（1）民用方面，定位与地图构建技术是矿山无人驾驶、无人机勘测、移动测量与高精度地图、移动机器人无人配送等应用的核心技术（图1-1），同时也是智慧城市和地理信息平台数据资源获取的重要技术手段。
　　（2）军事方面，对实现无人机的自主侦察和巡航、战场环境的快速构建等军事任务提供技术支撑，为提升军队信息化作战能力提供技术保证。
　　本书将系统介绍该领域有关的基本原理和技术方法，旨在为无人系统技术的研究与发展、传统测绘技术的转型和升级起到一定的助推作用。
　　1.2 SLAM技术的发展
　　1.2.1 SLAM问题的提出和定义
　　1.SLAM问题的提出
　　同时定位与地图构建（simultaneous localization and mapping，SLAM）问题于1986年在电气电子工程师学会国际机器人及自动化大会（International Conference on Robotics and Automation，ICRA）被*次提出[5，6]，其*初的研究目的是解决移动机器人在未知环境下的自主定位和导航问题[7]，历经多年的发展，其已成为多学科、多技术紧密交织的领域。自动化、控制、测绘、计算机等多学科背景的研究者从各自学科角度出发，为这一技术开辟了广阔的应用领域并建立了完整的体系架构。
　　SLAM、移动测量和导航制导等研究领域与定位、地图构建、控制规划等学科方向的关系如图1-2[8]所示。在移动测量的工程实践中，利用高精度组合导航设备在可控条件下引入高精度轨迹作为基准，并将对周围环境的扫描引入该基准框架下，以构成点云地图数据产品。在这一过程中，地图仅是扫描测量的一种表达形式，是测绘任务的*终产物。导航的目的是精确控制某一物体从一个位置移动到另一个位置，它需要以定位数据为基础，控制和规划是实现导航的具体方式。在SLAM问题中，定位和地图并不存在逻辑上的先后关系，而是相互依赖、互为条件，二者紧密地耦合在一起。载体的位姿信息是地图构建的基础，而地图又是实现定位的先决条件。
　　2.问题的定义*先需要明确的是SLAM的定义是对某一类问题的定义，解决的问题是一个
　　机器人被放置在未知环境中，利用机载传感器获得的观测信息来构建环境地图，同时确定自身所处地图中的位置。从数学角度可将SLAM问题抽象描述为一个条件概率分布问题，引用Thrun等[9]在《概率机器人》一书中的例子进行说明：假设载体在未知任何先验信息的环境中自由运动，并每间隔时间t对环境进行一次观测，将t时刻载体的位姿状态记为，控制量记为，观测量记为，则控制量序列可记为观测量序列可记为，题可描述为：在给定初始位姿、观测量序列和控制量序列的条件下，求解当前位姿和地图估计的*大后验概率（maximum a posteriori probability，MAP），即
　　（1-1）
　　观测量zt的条件概率密度可描述为：在给定载体姿态估计和先验地图的条件下，当前观测量的概率密度，即
　　（1-2）
　　位姿状态xt的条件概率密度可描述为：在给定上一时刻载体位姿状态xt.1和控制量ut的条件下，当前时刻载体位姿状态的概率密度，即
　　（1-3）
　　在上述条件概率分布问题中，从t.1时刻到t时刻的位姿变换过程可以根据贝叶斯法则分解为运动更新和测量更新两步。
　　（1）运动更新：
　　（2）测量更新：
　　所构建的地图可由随机变量表1示；载体的位姿估计可由随机变量变量，的估t值可以通过式（1-5）由极大似PxZUM表示。上述随机，然估计（maximum lik，elih，ood estimate，MLE）算法求解[8]。
　　从广义角度理解，上述概率模型适用于一切包含环境观测传感器的组合导航系统，因此绝不能将SLAM片面地视作某一种特定的技术，SLAM从来都不局限于某一种传感器或某一种MLE优化算法，而是涵盖了一切包含环境测量传感器的组合导航系统，是多传感器融合领域的一个重要方向。
　　需要说明的是，学术界通常习惯将不包含闭合回环（loop closure）和全局优化的系统称为里程计方法，而将包含二者的系统称为SLAM系统或SLAM解决方案。VO和LO是*常用的两种里程计方法[10，11]，根据前面SLAM的定义，其同样能够在确定自身位姿的同时实现环境地图的构建，应属于SLAM定义的范畴。为避免这类歧义，本书将SLAM方法和SLAM系统进行区别，将所有能够解决SLAM问题的方法统称为SLAM方法或SLAM技术，而将包含闭合回环的SLAM方法称为SLAM系统或SLAM解决方案。以直接稀疏里程计（direct sparse odometry，DSO）[12]为例进行说明：DSO能够解决SLAM问题，可以称为一种SLAM方法；但只有包含闭合回环和全局优化的L-DSO（loop-DSO）[13]才能称为完整的SLAM系统。
　　1.2.2 SLAM的发展和瓶颈
　　Cadena等[14]详细梳理了SLAM的发展脉络，并对未来进行了展望，将SLAM的发展划分为三个阶段：1986～2004年为**时代，Durrant-Whyte等[5，6]对这一阶段的相关研究进行了系统性梳理。在这一阶段，由于传感器和计算机性能的限制，主要采用二维LiDAR并以扩展卡尔曼滤波和粒子滤波等滤波方法来解决SLAM的MLE问题。2005～2014年为算法分析时代，随着计算机、自动化等相关技术的发展成熟以及新型传感器的面世，大批优秀的SLAM解决方案纷纷涌现[15]，SLAM问题的收敛性、一致性和可观性等基础性质也得到了深入研究。在这一阶段，逐步形成了SLAM方法前端运动估计和后端位姿优化的标准数据处理架构，如图1-3所示。