激光雷达3D SLAM技术

激光雷达3D SLAM技术
10.1.什么是激光雷达3D SLAM技术？
什么是激光雷达3D SLAM？在了解这个概念之前，首先需要弄懂什么是SLAM。
SLAM，英文是Simultaneous Localization and Mapping，意思是即时定位与建图。通俗理解，SLAM的主要功能就是让机器人一边计算自身位置，一边构建环境地图，这两个过程相辅相成。可以说，SLAM技术是机器人后续自主行动和实现交互的基础，SLAM也被认为是实现自动驾驶和全自主移动机器人的关键技术。
SLAM要实现定位和建图，第一步就是采集数据。而采集数据的关键是传感器。根据目前主流的应用，传感器主要是激光雷达和摄像头两大类，根据传感器选择不同，目前有两个技术流派：基于激光雷达和视觉的SLAM技术分别称为激光SLAM和视觉VSLAM。
而激光SLAM又因为传感器应用有单线（2D）和多线（3D）激光雷达两种，所以，激光SLAM又有2D SLAM和3D SLAM技术路线之分。他们采集的数据区别在于：
1）2D SLAM用单线激光雷达探测二维平面环境信息，在一个平面数据上进行定位，二维成像没有高度信息。
2）3D SLAM用多线激光雷达获取环境三维数据，通过三维数据的特征点匹配进行定位，三维动态成像可以知道物体形状大小，环境信息还原度高。
无论是激光2D SLAM、激光3D SLAM还是VSLAM，他们都是建图定位技术。
什么是激光雷达3D SLAM技术呢？激光雷达3D SLAM就是基于多线激光雷达探测的建图定位技术。图10.1表示高性能3D激光雷达打造最齐全的产品矩阵。
 
 图10.1. 高性能3D激光雷达打造最齐全的产品矩阵
可能会有人问，激光雷达3D SLAM有什么优势呢？
1.从技术成熟度看
早在2005年，激光SLAM已经被研究比较透彻，是最为成熟的定位导航方案。而激光雷达3D SLAM技术近年来迅猛发展，愈来愈成为未来的发展趋势。VSLAM目前由于受限于计算复杂度及光线对视觉干扰等问题，工程应用案例不多。
2.从适用环境看
2D SLAM无法适用动态场景变化大的室内场景，更无法应用到复杂的室外环境，VSLAM对光的依赖程度高，在暗处无法适用，激光雷达3D SLAM则室内外通用，且不受光线环境影响。
综上可知，激光雷达3D SLAM技术具备测量精度高、环境适应能力强、部署便捷等优势。
随着国产多线激光雷达的量产和性能提升，3D激光雷达逐渐走向低成本、低功耗和高可靠性的应用，基于3D激光雷达的SLAM算法得到了迅猛的发展。近几年激光雷达3D SLAM技术在无人驾驶、机器人等智能设备上大量落地应用，直接验证了激光雷达3D SLAM技术的可靠性。
基于多年来对客户在物流仓储领域的痛点研究，镭神智能历经7年匠心打磨出多线激光雷达3D SLAM自然导航定位技术产品，完美解决了反光板、二维码、2D SLAM等传统导航方案的：场外不能使用、场景应用局限大，成本投入高以及部署繁杂、易烂尾等三大痛点，成为行业内3D SLAM无人叉车技术应用最早、技术最先进、技术最成熟、作业最安全、落地应用项目最多的公司。图10.2表示高性能3D激光雷达应用场景。
 
图10.2表示高性能3D激光雷达应用场景
镭神智能作为在全球范围内率先将激光雷达3D SLAM技术应用于无人叉车上的开拓者，已在众多实际应用案例中取得实质性的成效，满足不同行业的多种搬运需求，并提供极致安全防护。目前已累计服务超过150+行业客户，覆盖新能源、汽车电子、物流、皮革、食品、纺织等领域，帮助客户解决痛点，实现智能化升级。
这些案例都在说明激光雷达3D SLAM正成为全球SLAM技术的重要发展趋势。
10.2.3D SLAM关于z轴精度的优化思路整理
10.2.1. 3D SLAM优化背景
此次的优化针对的场景是室内小场景、室内大场景和室外场景下运行3D SLAM时，z方向的误差累积、以及十万平米（≈400*400）建图时，回环检测效率低下的问题，机器上使用的主要传感器3D LiDAR有velodyne16线（或ouster16线），IMU使用的是阿路比-LPMS-IG1。那么在进行优化前，进行普通的3D SLAM能得到什么样离谱的结果呢？有如此一幅点云地图（见图10.3上）在俯瞰视角下彷佛一切岁月静好，但在正视视角下其尾巴都翘上了天（见图1下）：
 
 
图10.3.开环室内20x260米走廊场景（俯视图：上，正视图：下）
由图10.3，可以看到在SLAM过程中x、y、yaw的状态量估计还是基本准确的，在点云地图上的边界没有出现明显的结构性问题，但是其在建图开始（始于地图最右侧）后，随着roll、pitch、z的误差严重积累，在跑完这260+米的走廊后终点位置（地图最左侧）的z坐标与起始点位置的z坐标偏差了8.57米（实际偏差<2m）。再看另一个现象在一个厂区室外130x240的回环区域下进行3D SLAM，得出了如图10.4所示的点云地图：
 
 
图10.4.闭环室外130x240米厂区场景（俯视图：上，正视图：下）
由图10.4，还是可以得出SLAM过程中状态量估计基本准确的状态为x、y、yaw，而roll、pitch、z的误差积累问题严重，在图10.4中长边（240米）左、右关键帧的位姿上z坐标间的差值有9.88米（实际偏差<2m）。
针对上面两个例子中发现的问题，将简单分析其误差的产生的原因，并分享根据误差产生原因的物理依据进行优化设计和实现思路。
10.2.2. 轨迹z方向飘移原因分析
在SLAM建图过程中轨迹某个状态量的不准确、在某个状态量上的误差积累，本质上都是算法设计时的假设与实际情况不符，且在进行SLAM问题建模和解决方案设计时，没有考虑到假设与真实情况的不一致性，而导致的解算误差。而在点云配准的阶段，经典点云配准算法ICP及其变体针对了各种环境、场景上出现的问题，进行了各种各样的算法设计，以便可以高效且鲁棒的解决所有使用上的问题，但是z轴方向的飘移，其实是由于z轴、roll和pitch这三个机器人状态量，在一定程度上不可观测且难以评价和消除，虽然机器上使用了IMU和LiDAR传感器进行惯性信息与环境信息的输入，但轨迹中z方向的误差来源还是主要集中在LiDAR的数据。
10.2.3. LiDAR静态tf标定引入的误差
一般来说机器人出厂后都需要进行一次详细地标定工作，但即使再详尽的标定工作，多多少少都还是会留有一些误差存在，另外随着机器的使用机器上实际的LiDAR静态tf也会因为机器的损耗逐渐与最初标定的数值发生偏差。以图10.3中场景点云数据为例，在获取到一帧激光点云数据后直接进行静态tf的变换，将其从LiDAR坐标系转移到车体坐标系后可获得俯视图和正视图如图10.5所示：
 
 
图10.5. 室内工厂单帧点云数据（俯视图：上，正视图：下）
由于该机器是刚出厂且完成了标定，可以由图10.5看到当机器人静止水平时LiDAR返回的点云数据经过tf变换后的roll、pitch误差还是较小的。但是在另外一台使用年限较长、经过多次维修的机器人上做相同的实验，roll、pitch的误差就变得肉眼可见的大了（见图10.6）。将上述两帧点云分别与对应的以LiDAR坐标系的点云进行ICP匹配以获取静态tf的真值，可以发现图10.5中roll、pitch的误差分别是0.011和0.008弧度，而图10.6中的roll、pitch误差分别是0.035和0.017弧度。
在上述分析的基础上，若是进行纯LiDAR的SLAM，一般情况下第一帧关键帧的位姿就是（0，0，0，0，0，0），而由于静态tf的误差存在其真实情况的关键帧并不完全水平，即使在后续的SLAM轨迹构件中没有其他问题发生，在开环情况下z轴的累积误差就是：
z轴的累积误差=(实际行走距离)×sin⁡(初始俯仰角误差)
 
 
图10.6.室外单帧点云数据（俯视图：上，正视图：下）
10.2.4. LiDAR竖直方向分辨率及入射角引入的误差
首先从3D激光雷达的竖直方向分辨率开始，以常见的机械式激光雷达为例，即使是128线的3D激光雷达，其竖直方向上的分辨率，也是小于水平方向的分辨率的，更何况16线的3D激光雷达。以Velodyne-VLP-16为例。

传感器名称型号

性能名称

性能数据

Velodyne-VLP-16

通道数

16

测量范围

高达100米

范围精度

高达 ±3cm (典型的)

垂直视场

+ 15.0° 至 -15.0°(30°)

角分辨率(垂直)

2.0°

水平视场

360°

角分辨率 (水平/方位角)

0.1°- 0.4°

旋转频率

5 Hz – 20 Hz

集成网页服务器

便于监控和配置
 
 由于较低的竖直方向分辨率和竖直视场，以至于每一帧激光数据能检测到地面的点云十分有限，且地图特征远远不如水平视场探测到的角点、平面特征，例子可见图10.5和图10.6中的俯视图，其中蓝色标记的点云即为激光打到地面上的点云。
其次，由于机器人上3D激光雷达普遍的安装方式（雷达水平安装在机器人正上方），导致在SLAM构建轨迹的过程中，激光打到地面上的入射角很大（远大于水平视场的垂直入射角），雷达入射角越大其产生的光斑也就越大，绝对精度和重复精度就越差。
上述的结论，除了可以通过各个雷达技术论文和技术参数手册获取，还有一个广而知之的论证途径就是LOAM中针对激光点云数据的预处理中定义了两类异常点并进行了剔除，其中一类就是平行点（定义图示见图10.7），平行点指的就是图10.7中的B点，就是激光的射线几乎和物体的平面平行了，而LOAM中剔除这类点的原因有两个：
1）激光的数据会不准，射线被拉长
2）这种点被视为特征点后会非常不稳定，下一帧可能就没了，无法进行下一步的点云配准，例如图10.7中的LiDAR原点再向左移一点，那么B点就消失了，与之形成对比的就是A点，极短时间内不会消失。
 
图10.7. LOAM中平行点定义图示
由此，若是在SLAM系统中的配准使用的是点云层面的处理，而不是基于特征的匹配，则入射角很大处的地面点云，由于绝对精度和重复精度差，就很有可能影响点云配准时roll、pitch的观测估算了。
综上，LiDAR竖直方向分辨率及入射角对SLAM过程中，轨迹z轴的影响在于两个方面，第一个就是由于竖直分辨率低导致地面点云及其特征少，特别是降采样后，可能都没剩下多少可以用来点云配准了，第二个就是激光雷达打到地面上的点入射角通常较大，其点云精度差且不稳定导致在点云配准时出现误差。
10.2.5. SLAM轨迹z方向飘移问题优化思路
以上的分析都是常规过程中可量化且较为统一的误差来源分析，而其他的原因，比如退化场景下配准算法失效引起的误差、配准算法本身性能导致的误差，还有机器运算资源高压条件下，SLAM运算阻塞引发的位姿滞后误差等，不在这里进行优化的讨论。针对SLAM过程中轨迹z轴飘移的优化思路，还是针对静态tf标定误差、LiDAR竖直方向低分辨率及大入射角带来的误差。
10.2.6. 静态tf标定引入的误差优化
激光雷达坐标系与车体中心坐标系的静态tf，首先会在SLAM开始阶段引入一个初始位姿误差，因为机器人在每一次开始SLAM时，不一定是完全水平的。需要利用IMU的数据进行重力方向估计和对每一个关键帧位姿进行重力方向校准，这部分的工作可以参考LIO-SAM（局部因子图紧耦合的LIO）和VINS-Fusion（滑窗优化下的VIO）中针对IMU的高频惯性数据的处理。图10.8表示VINS中的滑窗优化示意图。
 
 图10.8.VINS中的滑窗优化示意图
以LIO-SAM为例，可以考虑将SLAM前端模块进行基于IMU的紧耦合局部因子图的联合优化，与此同时完成针对IMU数据的初始化（外参及bias的初始估计）、IMU重力估计模块和IMU预积分功能，局部因子图的构建，主要节点还是基于点云配准后的关键帧，但此时关键帧之间会再多上一个重力方向校准的约束，以及IMU预积分计算出的惯性变换约束，以尽可能地减少静态tf和点云配准时带来的误差，特别是IMU重力估计模块，可以避免纯LiDAR SLAM在roll、pitch的完全不可观测问题，在很大程度上优化z轴的飘移问题。图10.9表示LIO-SAM的系统结构。
 
图10.9. LIO-SAM的系统结构。该系统接收来自3D激光雷达、IMU和可选的GPS的输入。引入四种类型的因子来构建因子图（a）IMU预积分因子、（b）激光雷达里程计因子、（c）GPS因子和（d）闭环因子。后面讨论了这些因子的产生。
10.2.7.竖直方向低分辨率及大入射角带来的误差优化
竖直方向低分辨率及大入射角带来的误差来源根据上文中的分析，可以归结为两个：第一，由于竖直分辨率低导致地面点云及其特征少；第二，激光雷达打到地面上的点入射角通常较大，其点云精度差且不稳定。一言以蔽之，3D激光雷达获取到的地面信息较少，且获取到的点云信息较差，导致观测校正不够理想，误差由此被引入到SLAM轨迹中，且难以在后续构建中被消除。
既然点云的信息又少又差，避免直接使用点云信息进行配准操作不就行了。还是先以LIO-SAM为例，在其系统的激光回调中完成了以下关键步骤：
1）将当前帧位姿初始化并随后提取当前帧的局部角点、平面点云集合，加入到局部map；
2）将当前激光帧角点、平面点集合降采样后进行scan-to-map匹配，以优化当前帧的位姿；
3）若当前帧为关键帧，则执行因子图优化，更新因子图中所有变量节点的位姿，由此获得优化后的SLAM轨迹。可以看到，LIO-SAM的系统中没有进行基于点云的配准，而是通过提取平面点进行基于特征的匹配优化计算，而且特征匹配时，要求当前帧特征点数量需要足够多，且参考匹配的点数也足够多才执行优化。
根据平面特征的特征匹配过程进一步扩展，平面点云特征不再只是作为前端关键帧位姿观测更新的一部分，而是将其扩展成一个地面的约束（Ground Constrain），以避免z轴的飘移问题。
而添加了地面约束后，即是尝试利用地面特征作为假设条件，在优化时考虑地面约束带来的残差以影响优化结构，为了避免添加地面约束对上下坡场景的不友好，可以对地面约束进行打分计算，只要地面特征足够明显和地面约束足够可靠，才在对应的关键帧处添加地面约束的残差块。
另一个延申就是可以针对每一帧点云数据进行地面点的分割和标记，而这个分割后的结果，可以不再单独使用在前端的配准模块中，而是进一步地被利用在回环检测模块中，以保证地面特征被充分利用。
在这里分享两篇对启发较大的有关地面点云分割的技术论文和开源代码，
1. 论文名称及链接：《Fast Segmentation of 3D Point Clouds for Ground Vehicles》，https://ieeexplore.ieee.org/document/5548059/figures#figures，开源代码：https://github.com/lorenwel/linefit_ground_segmentation；
2. 论文名称及链接：《GndNet: Fast Ground plane Estimation and Point Cloud Segmentation for Autonomous Vehicles.》，https://ieeexplore.ieee.org/abstract/document/9340979，开源代码：https://github.com/anshulpaigwar/GndNet；
10.2.8.万能误差消除术
万能误差消除术其一：添加和利用好GNSS、Landmarks等外部的绝对位置信息，唯一需要注意的就是当GNSS数据或者Landmark信息被机器人获取时，需要自动地判断出该绝对位置信息是否可靠，避免因为使用垃圾、离群点位置信息而带来更严重的误差。可参考文章链接：https://zhuanlan.zhihu.com/p/603099637。
万能误差消除术其二：回环检测。回环检测是所有SLAM问题的解决方案里必不可少关键模块，比如经典的Scan-Context，基于多分辨率地图的分支定界+相关性匹配，以及利用语义信息的各种回环检测算法，只要在SLAM过程中找到正确的回环，基本上就可以消除90%的误差了，但是怎么保证回环检测的效率、准确度又是一个研究方向了。
10.2.9.优化结果展示
使用优化后的算法模块，在相同的数据集和硬件环境下，获得了与图10.3和图10.4例子进行对比的图10.12开环室内工厂环境和图8闭环室外厂区环境结果，其中优化后的结果（见图10.10）SLAM轨迹图中长边（240米）中最左和最右关键帧的位姿上z坐标间的差值，从之前的9.88米优化成1.82m，图10的轨迹结果，则从原先260米长度下累积8.57米误差，到现在300米累积0.89米的误差，这两个对比实例都说明了优化思路是可行的。图10.10表示闭环室外厂区130x240米（俯视图：上，正视图：下）；图10.11. 闭环室内+室外厂区205x240米（俯视图：上，正视图：下）；图10.12. 室内厂区120x300米（俯视图：上，正视图：下）。
 
图10.10. 闭环室外厂区130x240米（俯视图：上，正视图：下）
 
 
 
图10.11. 闭环室内+室外厂区205x240米（俯视图：上，正视图：下）
 
 
 
图10.12. 室内厂区120x300米（俯视图：上，正视图：下）
除开大场景、长轨迹的SLAM构建，还使用优化后的SLAM算法模块在小范围的地库及室内房间场景，进行了鲁棒性测试，地库场景的有效高度区间相较于工厂与室外环境会更加小，同时存在更多的动态障碍物（移动的车辆和行人），室内房间场景由于空间更加狭窄，3D激光雷达获取到的地面特征几乎没有，由此，也就更加依赖IMU的重力估计和校准功能，同时还需要避免错误的地面约束影响SLAM轨迹发生偏移。图10.13和图10.14分别对应优化后的SLAM算法模块下地库、室内房间场景下的结果。
 
 
图10.13. 室内地库50x70米（俯视图：上，正视图：下）
 
图10.14. 室内房间22x35米（俯视图：上，正视图：下）
总结
在纯LiDAR SLAM的算法模块中添加IMU惯性里程计约束、IMU重力估计及校准，并针对点云数据，进行地面点云的分割和针对地面点云进行平面特征匹配，可在一定程度上解决LO在z、roll、pitch这几个机器人状态量轻微不可观的问题。值得说明的一点是，先是尝试在SLAM算法模块的前端使用局部因子图框架，利用LiDAR观测（地面点云在此阶段也被利用到点云配准算法中，以进一步提升z、roll、pitch的观测精度）得到的关键帧，及帧间IMU惯性里程计约束，获取一个优化后LIO关键帧并输入到后端中，这样的处理在回环检测触发之前，前端LIO中还是会积累一定程度的z轴飘移（但比没有添加地面点云优化配准和imu约束前要好一些）。而最有效的优化处理则是在后端的优化问题构造时，针对每一个关键帧，添加必要的IMU重力校准残差项和地面约束残差项，才能真正意义上的解决z轴飘移的问题。
在后端优化的改造工作中，难点在于如何保证添加的IMU重力校准残差项和地面约束残差项是精准和可靠的，在改造初期，由于经常引入较差的IMU重力校准约束和地面约束，而出现关键帧位姿出现大跳变和SLAM轨迹失败的问题，地面约束的约束质量确认，可以通过地面点云分割质量和地面特征匹配得分的高低来进行把控，而要保证IMU重力校准精度，一方面需要加强针对IMU数据的使用问题建模和解决方案设计，添加重力校准约束前，再进一步进行完整性分析，以最后把控约束质量。
10.3.基于3D激光雷达的SLAM算法研究现状与发展趋势
10.3.1. 3D激光雷达的SLAM算法概述
SLAM算法是移动机器人实现自主移动的关键环节。激光雷达(LiDAR)具有测距精度高、不易受外部干扰和地图构建直观方便等优点，广泛应用于大型复杂室内外场景地图的构建。随着3D激光器的应用与普及，国内外学者围绕基于3D激光雷达的SLAM算法的研究已取得丰硕的成果。
梳理了3D激光SLAM算法在前端数据关联、后端优化等环节的国内外研究现状，分析总结了目前各种3D激光SLAM算法，以及改进方案的原理和优缺点，阐述了深度学习和多传感器融合理论与技术在3D激光SLAM算法中的应用情况，指出多源信息融合、与深度学习结合、应用场景的鲁棒性、SLAM算法通用框架及移动传感器，和无线信号体制的技术渗透，是3D激光SLAM算法的研究热点和发展趋势。
研究成果对3D激光SLAM算法和未知环境中移动机器人即时定位和地图构建的研究具有重要的参考价值和指导意义。
SLAM算法的经典框架包括前端数据关联和后端优化两部分，如图10.15所示。前端依据激光雷达测得的点云数据，通过扫描匹配进行帧间配准，闭环检测以获得不同帧间点云数据的关系，不断更新位置估计，并存储相应的地图信息；后端通过维护和优化前端得到的机器人位姿和观测约束，得到所构建地图的最大似然估计与机器人当前位姿。
 
图10.15.激光SLAM算法框架
根据所用传感器，SLAM算法大致分为基于视觉传感器的视觉SLAM算法和基于激光雷达的激光SLAM算法两种。与视觉传感器相比，激光雷达具有测距精度高、不易受光照与视角变化等外部干扰和地图构建直观方便等优点，广泛应用于大型复杂室内外场景地图的构建。
    激光雷达按照激光线数不同，分为2D激光雷达和3D激光雷达。目前2D激光SLAM技术发展较为成熟，已在服务机器人和工业现场得到了应用。
    2D激光雷达同时发射和接受单线的激光，其结构简单、扫描速度高、角度分辨率高，功耗低且精度可达厘米级，但受限于平面环境而无法适用于全地形环境，在有起伏或者存在坡道等场景，就不能实现定位和地图构建。
    3D激光雷达通过发射多束激光测量其与未知环境中物体的几何信息，获取包含精准的距离和角度信息的点云数据，体现3维空间结构信息，而且线数越多获得的激光点个数越多，对环境结构的描述也更清晰。
    随着多线激光雷达的量产和普及，以及嵌入式处理器功耗降低和算力增强，3D激光雷达开始走向低成本、低功耗和高可靠性的应用，基于3D激光雷达的SLAM算法得到了迅猛的发展。
    LOAM(LiDAR odometry and mapping)算法是目前最具代表性的3D激光SLAM算法。其原理是将激光SLAM分成里程计算法和地图构建算法，具有速度快、精度高、鲁棒性好，以及计算消耗小等优点；缺点是没有闭环检测，大规模测试中会出现漂移，在空旷环境下会因特征缺少而出现退化问题。SHAN等优化了LOAM的特征匹配，增加了闭环检测，提出了LeGO-LOAM，运行时间比LOAM算法减少了一个数量级，在占用计算资源减少的情况下实现了相似甚至更好的精度，但有时会出现错误或识别遗漏。有学者使用Eigen线性迭代和Ceres Solver非线性优化简化了代码结构，减少了复杂的数学推导和冗余操作，提出了LOAM的高级实现A-LOAM。
    此外，提出了一个帧到模型的匹配框架IMLS-SLAM，引入隐式移动最小二乘(IMLS)表面公式，采用最小化点到隐式表面距离的方法，表示局域激光雷达的扫描点，从而提供了准确的姿态估计，但不能实时实现。作为一种典型的密集方法，提出了基于面元的地图进行配准和闭环检测的SuMa(surfel-based mapping)算法，对于缺失特征或缺失数据更具鲁棒性，仅基于激光点云数据就可以建立大规模环境下的全局一致性地图。
    现有的3D激光雷达SLAM算法研究，在公开数据集测试中表现出了良好的性能，平移或旋转的漂移较低，图像质量和轨迹的精度较高，但在实际应用中仍存在较多局限性：包括对环境的鲁棒性，例如，从室内到室外或者从静态环境到动态环境；对实时性的要求，例如，在一些无人地面车辆(UGV)等机器人平台上，计算资源有限，而计算单元需要同时进行定位和路径规划，计算成本较高等。
    已有学者总结了SLAM的相关工作，描述了SLAM的一般问题、模型及框架、难点和主要方法，回顾了相关工作进展，并且讨论了SLAM的发展趋势，其中关于算法的进展，多介绍视觉SLAM算法或SLAM算法的特定方面或其子领域，涉及3D激光雷达SLAM算法综述研究较少。概述了3D激光雷达SLAM算法框架和关键模块，但是，对于算法方案的介绍只选取了6种开源算法进行测试对比，并未详细展开。介绍了基于3D激光雷达的多传感器融合SLAM的发展过程和最新的研究工作。
    从3D激光SLAM算法的前端、后端，深度学习和多传感器融合在3D激光SLAM中的应用等方面，系统阐述现在主流的3D激光SLAM算法方案，探讨目前主流3D激光SLAM算法存在的问题和不足，总结其研究热点和发展趋势，以期为其后续研究提供借鉴和指导。
10.3.2. SLAM前端
3D激光SLAM算法的前端主要解决数据关联的问题，扫描匹配考虑局部数据关系，而闭环检测则处理全局数据关系；扫描匹配与闭环检测都是根据激光帧数据建立节点间的约束。
1. 扫描匹配算法
扫描匹配算法的精度和计算效率直接影响着SLAM算法轨迹估计和地图构建的准确性，为后端优化提供机器人状态的初始值与约束关系，大致分为基于迭代最近点(ICP)的、基于几何特征的及基于数学特征的匹配算法。
    1) 基于ICP的匹配算法经典ICP算法是1992年提出的。其原理为以距离误差最小为目标函数，通过迭代的方式寻找相邻扫描帧两扫描点间的最佳变换关系。该算法易陷入局部最优，且当扫描点数增多时，计算成本过高。
    为了解决经典ICP算法易陷入局部最优的问题，提高效率和准确性，许多学者研究了基于ICP的改进算法，优化了ICP算法的步骤，包括点的选择、匹配、加权和剔除错误关联，或者最小化成本函数的选择。其中适合3D激光雷达SLAM的算法有PP-ICP(point-to-plane ICP)、PL-ICP(point-to-line ICP)、GICP(generalized ICP)、NICP(normal ICP)等。PP-ICP算法的误差代价为点对面的距离；而PL-ICP算法利用点到线的距离作为误差代价，更符合实际情况；GICP将ICP算法和PL-ICP算法相结合，在概率框架下进行点云配准，提高了适用性和精确度；为提高精度，NICP则考虑了环境曲面的法向量和曲率。
    IMLS-ICP为每个点分配一个权重值，选择具有代表性的点匹配；利用基于目标点的局部曲面法线，采用IMLS法重建曲面。BLAM通过GICP算法，初步计算两帧点云数据的位姿变换，获取当前帧对应的最近点，并再次利用GICP算法，得到精确的位姿数据，执行闭环检测得到地图，提高了建图精度。LiTAMIN算法将对称的K-L(Kullback-Leibler)散度引入到反映两个概率分布之间差异的ICP代价函数中，减少了点云配准的点数，提高了处理速度；使用K-D tree(K-dimension tree)的观点，构建体素网格地图，保持配准精度的同时，提高了算法的准确性、鲁棒性。然而，实现收敛所需的大量迭代、对良好初始化的要求，以及对噪声的敏感性，仍限制了ICP及其变体算法；而且应用原始点云匹配的计算成本通常很高。
    2) 基于几何特征的匹配算法
基于几何特征的扫描算法，主要利用边缘和平面特征，并保持边缘和平面的匹配，如图10.16所示。LOAM基于局部平滑度提取当前帧和匹配帧的边缘和平面特征，并将当前点云中的点与地图中的边缘和平面进行匹配。还有许多研究扩展了基于几何特征的匹配算法，以实现更好的性能，例如LeGO-LOAM在特征提取之前利用分离地面优化，算法软件系统框图如图10.17所示。R-LOAM(reference LOAM)结合参考对象提取的网格特征，有效减少了长期漂移，但对3维网格和参考对象定位精度有较高要求。由于不需要点云转换关系的先验信息，利用了特定形状的几何特征迭代求解，基于几何特征的匹配算法计算量大大减少，实时性较好，匹配精度高，被广泛应用；缺点是当环境中的几何特征不明显时，结果可能会不准确、不稳定。

 图10.16. 特征扫描匹配
 
图10.17. LeGO-LOAM算法系统框图
面元(Surfel)是一种基于点的渲染方法，用点云而非多边形网格来渲染3维形状，但是需要GPU处理。DROESCHEL等利用面元特征，将连续的3D激光扫描与增量构建的多分辨率栅格地图配准；为实现测量稀疏和网格离散化情况下的准确性，在高斯混合模型中以概率方式分配面元，提高算法的准确性和效率。参照视觉SLAM中Elastic-Fusion的处理方式，包括SuMa，Elastic-LiDAR Fusion，将激光雷达点云数据的形状近似为面元，利用面元特征实现激光里程计。SuMa算法的软件系统框图如图10.18所示。
 
图10.18. SuMa算法系统框图
3) 基于数学特征的匹配算法
基于数学特征的匹配算法，是使用各种数学性质来表征扫描数据和数据帧姿态变化的扫描匹配方法，具有代表性的有基于正态分布变换(NDT)的匹配算法，NDT算法是一种快速空间建模技术，用以建立精简的目标3维点云模型。
    2003年，提出了基于正态分布变换函数的点云配准2D-NDT(2-dimension NDT)算法，并成功应用于SLAM中。其在配准的过程中利用正态分布表示关键帧，得到全局姿态的集合。对于大量的点云数据，NDT算法比ICP算法配准速度快，但配准精度不如ICP算法高。2006年，提出了3D-NDT(3-dimension NDT)算法，并不断加以改进，使之更适用于激光雷达点云数据的配准。ONDT(occupancy NDT)算法研究了多维正态分布变换的地图表示，相较于OctoMap和NDT-OM(normal distributions transform occupancy mapping)等3D地图表示，在内存消耗较低情况下，准确性较高且运行速度快。RGC-NDT(regional growth clustering normal distribution transformation)算法使用聚类算法捕获细节自然特征，相比传统NDT算法，匹配准确度提高1.18倍，且匹配耗时仅为其1／3。2021年，KOIDE等结合NDT和类ICP算法的优势，提出了VGICP(voxelized GICP)算法，通过体素化的方法扩展GICP算法，避免了高代价的最近邻搜索，能够快速准确地实现3维激光点云配准。
2.闭环检测
SLAM前端的近似计算和后端的渐近建图，都会因传感器的信息偏差而产生计算误差，并且会不断累积。闭环检测则是避免误差过多累积的关键模块，如实际闭环被正确检出，则可大大减少相应两个时刻位姿等信息的估计误差，进而校正全局相关时刻位姿和地图信息的误差；如误判实际相距较远位置所得的两组点云为闭环，则可能导致全局位姿和地图信息的近似计算出现较大偏差，甚至导致约束信息不一致而不可解。更加紧凑、准确的闭环约束，最终可以形成拓扑一致的轨迹地图，其准确性和效率对SLAM均很关键，过程如图10.19所示。由于3D激光点云数据信息单一、无序且较稀疏，随着点云匹配的时间增长，SLAM往往会出现轨迹不准确、构建地图重叠等问题。但是目前，闭环检测仍然没有广泛认可的最佳解决方案。
 
图10.19.闭环检测（绿色为当前帧点云，红色为历史回环帧点云）
OLSON通过旋转和平移，判断两帧激光数据的相似性，采用相关性扫描匹配的方法达到闭环检测的效果。LeGO-LOAM则利用ICP算法，结合欧氏距离进行闭环检测。然而，这类简单的利用几何估计的策略，由于前端里程计的累积漂移误差较大，可能导致在大规模环境中无法有效地进行闭环检测。
    除两组点云点对点的直接匹配，另一种方法是用几何信息、强度信息或者其他来构造描述符，再通过比较描述符衡量相似度，有基于手工设计和基于深度学习两种方法。前者可以直接处理3维点云，包括基于局部的描述符、基于全局的描述符和基于混合的描述符；后者需要将3维点云表示成深度学习可处理的格式，如预处理为结构化的、固定大小的表示形式。有效提取具有鉴别能力和鲁棒性的特征描述符，也是学者关注和研究的问题之一。
    局部描述符是从每个关键点位置提取，并基于词袋模型进行场景匹配，提高了匹配能力和鲁棒性。许多关键点检测的方法被提出，如3DSift(3-dimensional sift)、3DHarris(3-dimensional Harris)、3D-SURF、 ISSs(intrinsic shape signatures)及局部描述符，如SHOT(signature of histogram of orientation)、B-SHOT (binary signature of histogram of orientation)、ISHOT(intensity signature of histogram of orientation)和FPFH(fast point feature histogram)。
    局部描述符对激光点云的稀疏和不连续特征鲁棒性不足，忽略了局部特征之间的关系；通常需要大量的局部几何计算，而全局描述符更擅长检测具有高重复性的关键点，在全局点云中进行匹配。M2DP(multiview 2D projection)将点云投影到多个2维平面中，并为每个平面的点生成一个密度签名，这些签名的左右奇异向量被用作全局描述符。基于几何关系的平移和旋转不变的3D描述符，GLAROT-3D(geometric landmark relations rotation-invariant 3D)将关键点对的相对几何位置编码成直方图。Scan Context方法则利用存储高度值的2维矩阵格式的数据作为描述符；还提出了融合相似度评分和最近邻搜索，以实现快速的搜索，以及采用两阶搜索算法检测回环。
    通过集成几何和强度特征，提出了一种两阶段的分层位置重识别策略ISC(intensity scan context)以提高效率，包括基于二元运算的快速几何检索和强度结构匹配。受启发于人类虹膜，LiDAR Iris将环境的高度信息编码为像素强度，并提取一个用以判别的二值特征图像，使用对应图像的汉明距离计算相似度。将点云分割成不同的对象，Seed将分割对象的拓扑信息编码到全局描述符中。这些方法将几何结构编码为描述符并取得了良好的效果，但是目前激光的全局或者局部描述子的描述能力和旋转不变性能仍然有所欠缺，集成更高级的特征可以进一步增强描述符的鉴别能力。
    基于混合的描述符如SegMatch(segment matc-hing)和SegMap(segment mapping)，将一个点云聚为多个段，并提取片段特征，使用KNN(K-nearest neighbour)算法来识别对应物，结合局部描述和全局描述的优势，解决了点云数据稀疏、形式单一的问题。SegMap描述符解决了实时单一和多机器人系统的数据压缩要求，也可以用于密集环境的三维地图重建和语义信息提取，用于导航任务和面向机器人操作员等最终用户的视觉反馈。SegGraph (segment graphing)在分割点云后，以边匹配为主要依据，基于K-公共子图判定方法实现闭环检测。
10.3.3. SLAM后端
3D激光SLAM算法中，后端优化的目的是，将各帧雷达的位姿和帧间运动约束综合起来达到整体优化，以消除局部累计误差，并根据前端提供的数据生成环境地图估计，主要有滤波和非线性优化两种思路：滤波主要采用扩展卡尔曼滤波获得最优估计，存在适应性差、缺少回环检测、更新效率慢等问题，难以在回环多、距离长的大规模环境中应用；非线性优化方案以图优化为代表，进行全局优化，把所有数据都考虑进来，虽然会增大计算量，但融合精度较高。后端优化前后对比如图10.20所示，如何高效地通过后端优化来修正运动位姿和提高地图精度是SLAM算法的研究热点。
 
图10.20. 后端优化前后对比
1.基于滤波估计的SLAM算法
基于滤波理论的SLAM算法主要是利用了贝叶斯估计的原理，具体可分为基于卡尔曼滤波的SLAM算法和基于粒子滤波的SLAM算法。
    基于扩展卡尔曼滤波的SLAM算法，通过将非线性的运动模型和观测模型线性化处理进而实现SLAM，此过程会引入线性化误差，鲁棒性较差且计算效率低。研究了无迹卡尔曼滤波方法，对非线性系统进行无迹变换，其精度比扩展卡尔曼滤波方法高。
    粒子滤波方法使用一定数量的、带有权重的粒子，来代表状态的后验概率分布，可用于非线性非高斯的系统。提出了RBPF(Rao-Blackwellized particle filter)算法，分别使用扩展卡尔曼滤波算法和粒子滤波算法，用以解决机器人位置校正和位置估计问题，极大地降低了算法的计算复杂度。Fast-SLAM就是基于RBPF的SLAM算法，它提高了地图精度。GRISETTI等考虑了传感器信息模型，并且采用自适应重采样技术来保持合理的粒子种类，并提出了gmapping来改善粒子退化问题，其使用了较以前方法低一个数量级的粒子数量，就生成了高精度地图，是激光SLAM的一个里程碑。
2.基于非线性优化的SLAM算法
图优化算法框架是目前应用最为广泛的SLAM框架之一。图优化算法框架通过维护关键节点，以达到优化的目的，在保证精度的同时大大降低了计算量，如图10.21所示。基于图优化的算法可以分为基于最小二乘法的优化、基于松驰技术的优化、基于随机梯度下降法的优化和流形优化四种。提出了用于2维姿态图的Karto SLAM开源方案，一定程度上取代了基于滤波理论的SLAM方案，但实时性较差。  
Cartographer融合多传感器数据创建局部子图，采用子地图匹配算法进行全局优化，能够构建2维和3维点云地图，效率高，鲁棒性好，缺点是计算资源消耗大，在环境较大、计算资源不足时，可能出现无法正常回环的现象。
 
图10.21.图优化算法框架
针对非线性优化问题已经存在成熟C++开源库，如：Goole开源库Ceres Solver，基于图优化的开源优化库有iSAM(incremental smoothing and mapping)、GTSAM（Georgia tech smoothing and mapping）、G20（general graph optimization)、BA（bundle adjustment）等，借助这些优化库可节省后端迭代求解优化值的时间。
10.3.4.基于深度学习的3D激光SLAM算法
基于深度学习的方法通过数据驱动的方式学习，能够得到比手工设计更加精确的模型。SLAM与深度学习的结合，主要是将深度学习应用到传统SLAM框架的一个或几个环节，如帧间估计、闭环检测等，以提高SLAM算法的效率、精度和鲁棒性，表10.1列出了一些实现帧间估计、闭环检测的特征学习算法；或是语义SLAM中利用深度学习提取语义信息、构建语义地图，以提高机器人的自主理解能力。后端算法从线性滤波逐步向非线性优化发展，其中因子图优化与滑动窗口结合的方法以其良好的实时性和鲁棒性，被广泛应用在各类融合定位和建图系统中。
表10.1.实现帧间估计、闭环检测的特征学习算法
 
1.帧间估计
与基于几何信息接近策略的扫描匹配算法相比，基于深度学习的帧间估计方法性能卓越。但是由于数据稀疏性和计算成本高，传统的卷积模型无法直接应用到点云中。总的来说，利用深度学习实现帧间估计的方式有两种：
    1) 基于特征学习的点云配准方法
如GMM(Gaussian mixed model)、3DFeat-Net、FCGF(fully convolutional geometric features)、Siamese-PointNet(Siamese point network)、UGMMReg(unified Gaussian mixture model registration)；
2) 基于端到端的深度网络模型的点云配准方法如DeepGMR(Deep Gaussian mixture registration)、3DRegNet(3D registration network)、MLP_GCN(multilayer perceptron-graph convolutional networks)、IPCR(iterative point cloud registration)。
    现有的许多实验表明，直接应用配准的数学理论会花费巨大的计算时间，而直接应用深度学习无法保证准确性。直接结合深度学习和ICP仍然需要很高的计算时间。将传统的数学理论和深度神经网络融合到端到端的框架中，以获得高精度和高效率，是未来的研究方向。
2. 闭环检测
传统的闭环检测大多使用人工选取特征，精确度低、计算量大。相比于传统人工设计的特征，深度学习可以通过大量的学习训练以及合理的网络构建，高效提取对分类无关因素不敏感且区分度高的特征。
    近年来，基于深度学习的闭环检测算法逐渐发展。PointNetVLAD(PointNet and NetVLAD)利用PointNet提取局部特征，并利用NetVLAD聚合全局特征，从3维点云端到端提取全局描述符。DH3D(deep hierarchical 3D)从原始的3维点云中学习局部特征，然后使用注意力机制将这些特征聚合为全局特征描述符。KOMOROWSKI提出了一种基于替代点云表示和网络架构的全局点云描述符提取方法—MinkLoc3D。提出了一种金字塔点云变换器网络PPT-Net(pyramid point cloud transformer network)，利用金字塔点Transformer模块来增强对局部特征的区分并生成全局描述符。FastLCD(fast and compact loop closure detection method)直接从原始点云扫描提取多模态特征，编码成全局综合描述符，具有旋转不变性。NDT-Transformer针对大规模点云，通过学习轻量级处理后的NDT单元的全局描述符进行闭环检测。提出了一种基于局部3D深度描述符的闭环检测方法，通过一种新颖的重叠度计算方法检测闭环，实现了更好的精度。Overlap-Transformer是一个轻量级的Transformer神经网络，基于深度图像构造全局描述符进行闭环检测。
    由于图像特征描述符(如SIFT(scale invariant feature transform))的惊人性能和高成熟度，基于图像的闭环检测发展迅速，然而对于3维点云数据并没有相同成熟的类似方法提出，难以从点云中提取局部特征描述符，然后将其编码为检索任务的全局描述符。尽管基于深度学习的描述符也有突破结果，但是其严重局限性是需要大量的训练数据，当训练和应用于不同地形或在不同条件下获得的数据时，这些方法并不适用；而且它们通常需要GPU，在保持精度的同时，对轻量级网络结构的探索，以及对实时性能的追求是当下的研究热点。
    几何特征在恶劣环境下难以保证准确的闭环检测，而语义信息不易受环境因素的影响，是一种长期稳定的特征。基于语义信息的方法能够构建长期稳定的定位系统，实现闭环检测。GOSMatch(graph-of-semantics matching)利用场景中的语义对象来检测闭环，通过编码语义对象之间的空间关系，构建全局和局部描述符，进行闭环检测。提出了一种基于3D激光数据的合并语义信息和Scan Context的全局描述符SSC(semantic scan context)，并利用3阶段检索方法实现闭环检测。提出一种基于点云片段匹配约束的方法提升回环检测的效率。RINet(rotation invariant neural network)设计了结合语义和几何特征的旋转等变全局描述符，提高了描述能力，并且通过一个旋转不变的孪生神经网络来预测描述符对的相似性。
3.语义分割及语义地图
不同于传统SLAM方法以静态环境假设为前提，语义SLAM可以预知物体的可移动属性；共享相似物体表示，通过维护共享知识库提高了SLAM系统的可扩展性和存储效率；可以应对复杂场景，实现智能路径规划。语义SLAM的关键在于对目标物体的精准识别，而深度学习技术恰好是当前最具潜力和优势的物体识别方法，因此深度学习和语义SLAM的结合受到广泛关注。语义SLAM研究主要是利用语义信息辅助定位及构建语义地图，通过语义特征提取、动态物体剔除以提高SLAM算法的鲁棒性和定位精度；通过高级语义信息为机器人的人机交互提供高层次的语义地图，提高机器人的自主理解能力。
    3维点云的语义场景理解在机器人和无人驾驶领域中有着广泛的应用需求，也是目前亟待解决的问题，许多基于3维点云的目标检测和语义分割方法被提出。对于3维点云的几何分类、目标检测和跟踪以及点云分割包括语义分割、实例分割等，进行了较为全面的综述。
    由于3维点云的无序性、稀疏性和非结构化特点，为了处理此类非结构化数据，通常将点云转换为3D体素网格或2D投影的结构化形式，或者直接作用于输入点云定义一种新操作。开创性工作PointNet利用尺度变换统一3维点云输入，再通过Max Pooling操作解决点云无序性问题，可直接利用3维卷积神经网络，对3维点云进行特征提取和语义分割。基于此，PointNet＋＋和PointCNN被提出以分层学习深度特征，增强局部模型结构的能力。提出了图注意力卷积网络GACNet (graph attention convolution network)，能够在点云端到端进行结构化特征学习。针对大规模三维点云，提出了高效且轻量级的神经网络架构RandLA-Net(random sampling local feature aggregation network)；提出空间上下文特征学习模块，以实现点云语义分割网络架构SCF-Net(spatial contextual feature network)。LAF-Net(local attention fusion network)则是能够自适应融合低维特征和高维语义信息的局部注意力融合模型。在PointNet＋＋基础上， DSPNet＋＋(deep scale purifier network＋＋)是基于自注意力机制的特征提取模块，直接处理原始点云，进行端到端的点云分割。
    尽管这些基于点的方法直接应用于原始点云，不会丢失任何信息，然而大多数方法需要采用邻近搜索机制，固有地限制了这些方法的效率，且计算量大，无法部署在资源受限的设备上。
    基于距离图像，RangeNet＋＋通过卷积神经网络(convolutional neural network，CNN)实现了语义分割；RangeSeg(range-aware real time segmentation)获得了准确的实例级分割结果。为了更好地处理3维点云，并克服点密度不均匀和体素化步骤中信息丢失等限制，(AF)2-S3Net设计了一个端到端的编码器-解码器3D稀疏CNN。FIDNet(fully interpolation decoding network)设计了一种新的基于投影的激光雷达点云语义分割的神经网络。为了利用不同视图的优势，基于多视图融合方法，RPVNet(range-point-voxel fusion network)设计了一种新颖的距离-点-体素融合网络；Hybrid CNN-LSTM(hybrid convolutional neural network and long short-term memory)结合了空间上下文和时间序列信息，解决CNN网络池化层丢失细节的缺点，由高效的点云特征处理方法和新颖的神经网络结构组成。提出了解决弱监督多任务3D点云理解的统一框架WeakLabel3D-Net；基于点云局部几何特征无监督生成聚类，且提出了一种基于学习的聚类级相似性预测网络。
    尽管这些方法实现了先进的性能，然而由于将点云转换为结构化形式的量化存在不可避免的几何信息丢失，且处理大规模点云时将导致消耗大量资源。如何设计一个模型能够可靠、快速地实现激光雷达点云的语义分割仍然是一个有待解决的问题。
    在构建语义地图方面，Recurrent-OctoMap算法是融合语义特征的语义映射方法，使用循环神经网络及单帧语义检测，可以灵活地维护长期的语义地图。SuMa＋＋是SuMa算法的扩展方法，利用全卷积神经网络有效提取语义信息，并将语义信息集成到基于面元的地图表示上。SA-LOAM(semantic-aided LOAM)是基于LOAM的语义辅助激光SLAM算法，利用里程计和回环检测中的语义信息，提高了定位精度，能有效地进行闭环检测，在大规模场景中可构造全局一致语义地图，其在KITTI序列13的语义分割结果如图10.22所示。目前完备的语义辅助激光SLAM系统值得进一步的探索和尝试。
 
图10.22. SA-LOAM在KITTI序列13的语义分割结果，每种颜色代表不同的语义类别
10.3.5.基于多传感器融合的3D激光SLAM算法
不同传感器获取环境观测信息的处理方式不同，单一传感器都存在着局限性问题，表10.2对比了单一传感器优缺点。
单一传感器都无法独立完成所有场景下的全局地图构建，而多传感器的融合能够解决单一传感器的局限性，获得更精确、高效、适应性强的SLAM效果。由于不同模态传感器获取到的数据，格式不同。多模态数据的处理和增强，依然是当前基于多传感器融合的3D激光任务的关键。
表10.2单一传感器优缺点对比
1.激光雷达与惯性测量单元融合
激光雷达和惯性测量单元(inertial measurement unit，IMU)的融合可以克服激光SLAM过程中激光雷达垂直分辨率低、更新速率低以及由运动引起的失真等问题。激光雷达与IMU的融合有紧耦合和松耦合两种方式。紧耦合是通过联合优化所有测量数据，以在复杂变化的环境中获得准确的位姿估计，但存在计算量大、实时性差等问题。松耦合是分别处理每个传感器的测量数据，再进行融合的方式，计算量小、易于实现，但在定位精度上存在局限性。
    HDL-Graph-SLAM算法融合了多种输入，包括IMU、LiDAR传感器以及GPS(global positioning system)信息，资源消耗小且精度较高。提出一种LiDAR／IMU紧耦合的实时定位方法Inertial-LOAM，显著降低连续配准误差造成的误差累计，提升了定位精度和实时性。研究了LiDAR-IMU紧耦合的LIO-mapping算法，建图精度提高了，但是紧耦合算法复杂度较高，后期融合其他传感器较困难。研究了紧耦合激光惯性的LIOM (laser-inertial odometry and mapping)算法，实现了低漂移、强鲁棒性的位姿估计。在LeGO-LOAM的基础上，LIO-SAM(LiDAR inertial odometry via smoothing and mapping)应用因子图框架融合了IMU预积分因子、激光里程计因子、GPS因子、闭环检测因子，可以得到更好的精度和实时性、低漂移。研究了固态激光雷达与IMU的耦合方案，提出了用于实时激光惯性里程计和映射的新型传感器融合方法LiLi-OM(livox LiDAR odometry and mapping)。
    FAST-LIO(fast LiDAR-inertial odometry)紧耦合激光雷达和IMU测量，利用快速卡尔曼增益公式，将计算复杂度从测量维度降低到状态维度，计算效率显著提高，但是系统只能在小型环境中工作。FAST-LIO2继承了FAST-LIO紧耦合的融合框架，通过开发的ikd-tree(incremental k-d tree)数据结构，计算量显著减少，提高了里程计和地图绘制的准确性和鲁棒性，算法框图如图10.23所示。在FAST-LIO2的基础上，Faster-LIO使用增量体素作为点云空间数据结构，实现了轻量级紧耦合激光雷达惯性里程计，可用于固态激光雷达。LINS(LiDAR-inertial-SLAM)算法不同于采用以世界为中心的固定全局帧的状态估计量的紧耦合方法，而使用机器人中心公式表示移动局部帧的状态估计量，设计了迭代误差状态卡尔曼滤波器实现激光雷达和IMU的紧耦合。该方法在各种具有挑战性的场景中，如无特征场景，也能够实现鲁棒和高效的算法性能，算法框图如图10.24所示。图10.25为激光惯性耦合SLAM代表性结果。
    综上可知，激光惯性紧耦合研究主要通过激光雷达和IMU的紧耦合来提高建图的精度、效率和鲁棒性。但目前针对激光雷达与IMU的联合标定方法还不成熟。
 
图10.23.FAST-LIO2算法系统框图
 
图10.24. LINS算法系统框图
 
图10.25. 激光惯性耦合SLAM代表性结果
2.激光雷达和视觉融合
激光SLAM局部定位精度高但全局定位能力差且对环境特征不敏感，而视觉SLAM全局定位能力好，但局部定位相对激光雷达较差，两者融合可以提高SLAM系统的精度和鲁棒性。
    研究了视觉和激光雷达里程计结合的通用框架V-LOAM(visual LOAM)，如图10.26所示，利用视觉里程计处理快速运动，而激光雷达里程计保证在不良照明条件下的低漂移和稳健性，算法结果低漂移、鲁棒、快速，长期在KITTI数据集的平均平移和旋转误差基准测试上占据榜首，但没有考虑闭环检测和室外长距离环境中位姿轨迹与建图的整体一致性。LVI-SAM(LiDAR-visual-inertialodometry via smoothing and mapping)是紧耦合激光雷达－视觉－惯性里程计的SLAM框架，由视觉惯性系统和激光雷达惯性系统组成，能够构建完整且精度高的地图，实时性和精度高，鲁棒性强。SuperOdometry是高精度多模态传感器融合框架，采用以IMU为中心的数据处理管道，结合了松耦合和紧耦合方法的优点，在感知退化的环境中也能实现稳健的状态估计。R2 LIVE(robust，real-Time，LiDAR-inertial-visual tightly-coupled state estimator and mapping)、R3 LIVE(robust，real-time，RGB-colored，LiDAR-inertial-visual tightly-coupled state estimation and mapping)融合激光雷达、IMU和视觉传感器的测量来实现稳健和准确的状态估计，在各种具有挑战性的情况下，甚至在具有大量运动物体和小激光雷达视场的狭窄隧道状环境中，该方法也具有足够的鲁棒性。表10.3列出了激光雷达与惯性测量单元、视觉传感器融合的一些算法。
 
图10.26 V-LOAM算法系统框图
表10.3.多传感器紧耦合的一些算法
3.激光雷达和其他雷达融合
FRITSCHE等将毫米波雷达与激光雷达相结合以应对烟雾、雾或灰尘导致的低能见度恶劣环境，研究了毫米波雷达与激光雷达数据二值决策融合方法和加权融合方法，还探索了两者扫描级和地图级数据融合的可行性。研究了基于相机、毫米波雷达和激光雷达的多传感器融合方案，首先利用激光雷达和相机图像信息检测人和车辆，再融合雷达检测信息区分静止目标和移动目标，实现目标的检测与跟踪，减小了误检概率。提出了一种基于相机、毫米波雷达、激光雷达和GPS的融合方案，首先利用相机和激光雷达构建地图，然后利用GPS确定车辆位置，同时利用雷达检测的目标运动数据更新地图中的环境信息。选取了Google的Cartographer算法框架作为算法的主要框架，综合考虑毫米波雷达与激光雷达的数据优缺点，融合二者的数据实现了环境适应性更高的SLAM算法。
10.3.6.3D激光SLAM算法的发展趋势
1.多源信息融合
单一传感器均存在着一定局限性，无法满足全场景下的SLAM算法。国内外学者为弥补单一传感器缺陷，研究了多种多传感器融合方案获取高精度和高鲁棒性的地图，有与IMU融合的，有与视觉融合的，也有与其他类型如毫米波雷达融合的；还有多个激光雷达的融合，如M-LOAM(multi-LiDAR LOAM)，许多基于各种融合方案的SLAM算法被提出，成果颇多。但相关研究仍是3D激光SLAM算法的研究热点和发展趋势之一，包括对融合层次的探索，鲁棒适应性强且实时在线的多传感器综合校准、时间同步方法的开发，以及通用鲁棒的几何特征提取、数据关联、耦合方法等。同时，基于点云的深度学习方法和基于非线性联合优化的算法，将加速多源信息融合技术的发展。
2.与深度学习的结合
3D激光SLAM算法与深度学习的结合能够提高运行效率和鲁棒性，其研究方向包括特征提取与匹配、语义SLAM、深度估计以及重定位等等仍是研究热点。虽然越来越多的深度学习技术与成果应用于SLAM领域，已经有了许多相关研究工作，但现阶段基于深度学习的SLAM有着明显的局限性，通过深度学习构建的模型缺少直观的物理意义；其次，深度学习技术往往需要大量的训练数据，前期训练周期长；结合深度学习的SLAM通常需要GPU，在计算资源有限的条件下无法应用。
3.应用场景的鲁棒性
随着机器人技术的迅速发展和应用领域的不断深化，相应的应用场景亦不断丰富和拓展，如空旷、复杂多变的或者动态的场景，也包括地面／空中机器人、汽车／AGV自动驾驶等领域，故越来越多的应用环境细分也有待挖掘。不同应用场景SLAM算法表现不近相同，如针对室内环境，LOAM算法缺乏联合优化，实时姿态估计误差大，π-LSAM(LiDAR smoothing and mapping with planes)改进了LOAM，在后端引入联合优化并显著降低优化算法计算复杂度。面对一些具有挑战性的复杂多变的应用场景，如变化的光照、剧烈运动、开阔场地或缺乏纹理的场景等，需要开发长期稳健安全的算法，提高算法鲁棒性，许多方面如多机器人系统中全局定位与目标检测、优化轨迹，并创建全局一致性地图的方法，及动态场景或大场景的环境地图构建方法，都需要完善。同时立体匹配、3维场景重建、虚拟现实等计算机前沿应用中，3D激光SLAM算法也可起到关键作用。
4.探索通用框架和地图表现形式
探索3D激光SLAM框架的通用性也是研究热点，现有3D激光SLAM算法很多沿用了LOAM的框架；IMLS-SLAM算法描述了帧到模型的匹配框架；F-LOAM(fast LiDAR odometry and mapping)提出了一种结合特征提取、失真补偿、姿态优化和映射的通用框架。更好的地图表现形式如语义地图，可以大幅提高机器人的自主理解力，亦可提高3D激光SLAM算法的鲁棒性和精度。
5. 传感器和无线信号体制技术的影响
固态激光雷达性能高且成本极低，具有促进或彻底改变机器人行业的潜力，越来越受到关注。如适用于小视场和不规则采样的固态激光雷达的SLAM算法框架Loam Livox，鲁棒性强且实时运行。随着硬件性能和通信功能不断提高，SLAM技术嵌入式实现、移动设备传感器和无线信号体制技术的参与必然，也会影响3D激光SLAM算法的发展。如专用处理器(如HoloLens HPU)和一体化功能模组(如Tango模组)等，将会大大降低现有硬件平台的计算能力瓶颈和算法调试门槛。
10.3.7.小结
SLAM算法是移动机器人构建未知环境地图，并获取其位姿信息，进而实现自主移动的有效手段，可以辅助机器人执行路径规划、自主探索、导航等任务。随着3D激光雷达向低成本、低功耗和高可靠性的应用的发展，3D激光雷达SLAM算法研究取得了丰富的成果和应用案例。从SLAM算法经典框架出发，总结分析了其前端、后端以及深度学习与多传感器融合在激光SLAM中的研究现状，系统阐述现在主流的3D激光SLAM算法原理及其存在的问题和不足，探讨了3D激光SLAM算法的研究热点和发展趋势。
SLAM是多学科组合的算法和系统工程，包括硬件模型构建到系统集成，以及各个环节的多线程执行中的资源分配、数据优化以及建图和定位的准确性，都需要平衡和整合；关键模块和硬件匹配的升级、场景或应用的丰富和拓展，都会影响3D激光SLAM算法的发展。研究多源信息融合、与深度学习结合、动态以及其他场景的应用与拓展、SLAM算法通用框架、以及SLAM算法的嵌入式实现、移动传感器和无线信号体制的技术参与，是3D激光雷达SLAM算法的研究热点和发展趋势。
10.4. 什么是3D SLAM无人叉车？
过去，常见的叉车都需要一个操作员才能完成搬运任务。然而，随着企业面临着用工难题和昂贵的人力成本，无人叉车正在成为搬运行业的新宠。
10.4.1.什么是无人叉车？
无人叉车就是没有司机的自动驾驶叉车。为了实现自动化作业，无人叉车需要具备多项功能，如导航、避障、货物识别和自动叉取等。其中，导航是无人叉车实现自主移动的关键。简而言之，无人叉车必须先熟悉外部环境，绘制出完整的地图，以便自己了解环境。
10.4.2.如何实现环境认知？
图10.27表示二维码，反光板，2D SLAM三种导航示例图。
 
图10.27. 二维码，反光板，2D SLAM三种导航示例图。
目前市面上常见的无人叉车导航方式有：二维码导航、反光板导航和2D SLAM导航。前两种方法是通过扫描和反射原理进行识别，而2D SLAM导航则利用单线激光雷达进行环境识别。
10.4.2.为什么无人叉车尚未大规模普及？
图10.28表示二维码，反光板，2D SLAM三种导航难点示例图。
 
图10.28. 二维码，反光板，2D SLAM三种导航难点示例图。
主要原因是在复杂的室外环境中，2D SLAM导航只能探测二维平面的环境信息，环境特征信息有限，定位不够稳定，无法适应复杂的室内外环境，尤其是室外环境。而且贴二维码和布设反光板的方法部署复杂、周期长，容易出现问题，并且维护成本高。如果需要大规模部署，这两种技术方案的成本将变得很高。
如何解决这些问题呢？激光雷达3D SLAM定位导航技术的出现，则解决了上面的问题。
图10.29表示激光雷达3D SLAM定位导航技术示例图。
 
 图10.29. 激光雷达3D SLAM定位导航技术示例图。
3D激光雷达作为主要传感器，通过将激光雷达、惯性测量单元（IMU）、里程计和全球定位系统（GPS）等多种传感器的数据融合处理，在高性能处理器的支持下，实现环境建图和精准定位。无需在环境中布置任何辅助设备，一台多线激光雷达就能完成超过百万平方米的高精度三维环境建图，拥有丰富的环境特征信息和稳定的定位能力。3D激光雷达具有出色的抗光干扰性，适用于室内外各种场景，无论是白天还是黑夜，都能稳定运行。这一技术完美解决了无法在室外使用、部署复杂且成本高的问题，成为无人叉车行业的理想选择。
通过引入激光雷达3D SLAM定位导航技术，无人叉车在实现自动化作业方面迈出了重要的一步。这项技术解决了传统导航方式所面临的挑战，为无人叉车的大规模应用提供了可行的解决方案。图10.30表示无人叉车技术示例图。
 
图10.30. 无人叉车技术示例图
随着3D SLAM无人叉车的普及，企业可以享受到诸多好处。首先，3D SLAM无人叉车能够提高工作效率，减少人力成本，实现自动化的搬运和物流操作。其次，3D SLAM无人叉车具备高精度的定位和导航能力，能够在复杂的环境中安全、准确地操作，降低事故风险。此外，3D SLAM无人叉车还可以实现24小时连续作业，不受时间和疲劳的限制，进一步提高生产效率。
随着技术的不断进步和应用场景的扩大，3D SLAM无人叉车将成为工业自动化和物流领域的重要助力。它们将在仓储、制造、物流等领域发挥作用，带来更高效、更安全的作业环境，为企业创造更大的价值。
posted @ 2024-02-16 04:30  吴建明wujianming  阅读(1093)  评论(0)  编辑  收藏  举报