3D激光SLAM导航算法

REF：3D 激光雷达 SLAM 算法综述

1. 激光SLAM算法

激光雷达建立地图的激光 SLAM 方案按求解方式可以分为基于滤波器和基于图优化两类，基于滤波器的方法源于贝叶斯估计理论，在室内或小范围场景应用中具有不错的效果，但只考虑移动载体的当前位姿状态和当前环境观测信息，且不具有回环检测能力，存在线性化以及更新效率低等问题；基于图优化的 SLAM 方案考虑了移动载体历程中全部的位姿状态和环境观测信息，用节点和边形成的图来表示一系列的移动机器人位姿和约束，建立和维护关联数据，可独立出前端实现并行计算，是一种更为高效和普适的优化方法。

39da04e152575a8261dfc91404441ade.png

2. 基于图优化的 3D激光 SLAM 方案

图优化 SLAM 的模型表示形式也可以从弹簧能量模型的视角来解释，在 SLAM 中是对位姿的最大似然估计，弹簧模型中则是对应系统的最小能量状态，而二者的本质问题都可以转换为非线性最小二乘问题，分为扫描匹配、闭环检测、后端优化、点云地图存储表示 4 个部分

2d81666434392e4a552812b2a5df0ef0.png

1. 扫描匹配： 前端扫描匹配代表性的三维点云匹配算法可分为 基于匹配的方法和基于特征的方法两类
   • 基于匹配的方法：根据目标评价函数可以分为基于距离判断（ICP算法及其变种算法PP-ICP、NICP、IMLS-ICP、GICP等）和基于概率模型判断（正态分布变换( NDT) 算法）两种
   • 基于特征的方法：通过从扫描点云中提取特征点来提高计算效率，包括使用角点和平面点特征的 LOAM以及使用面元特征的三维栅格匹配算法，通常包括关键点检测、特征描述符提取、真实匹配、异常值剔除和转换估计几个步骤
2. 闭环检测： 基于全局数据关联，是实现鲁棒 SLAM 的核心步骤，通过识别是否到达历史场景促使地图闭环的能力，能够校正累积误差，从而产生全局一致性的映射地图，包括基于蒙特卡洛的节点搜索法、基于描述子的回环检测法（局部描述子代表算法 FPFH、全局描述子代表算法GLAROT及Scan Context）
   • SegMatch：基于分割的 3D 点云场景识别，折中局部特征和全局特征场景识别的方法
   • Segmap：与 Segmatch 相同的基于分割的思想，基于深度学习的 3D 点云分割描述子
   • SUMA ：使用 Surfel 地图生成投影数据关联并实现闭环检测
   • DELIGHT：采用雷达获取的反射强度信息而非常规的距离信息进行场景识别，包含基于密度描述子的先验估计和基于几何的验证两个阶段
   • ISC：利用点云的几何信息和密度信息构建新型全局描述子 ISC
3. 后端优化： 将各帧雷达的位姿和帧间运动约束综合起来达到整体优化，可以消除局部累计误差，在大尺度的建图中，一般需要具备一个“监管者”来时刻协调之前的轨迹，可概括分为 4 类: 基于最小二乘法的优化方法、基于松弛迭代的优化方法、基于随机梯度下降的优化方法以及基于流形迭代方法，开源优化库有 iSAM、GTSAM、G2O、Ceres、BA
   • C-T SLAM：采用高效的局部地图和分层的优化后端，允许实时建图期间优化修正
   • BALM：将视觉 SLAM 领域中BA 框架引入激光建图模块，降低建图过程中的累计误差
   • LIO-SAM：采用因子图优化，融合激光里程计因子、IMU 预积分因子、GPS 因子和回环因子
4. 地图存储：

6cee759c607c4a140d076aac5eedcdc2.png

2bf9f76fe54099736080a9fc235f01f9.png

bcd5ab726637400e15d22d80cd1601f1.png

0118c9972bc1519d3f0e47f90c569efa.png

2. 当前算法评估

一般基于以下 3 个标准进行性能评估:

• 误差指标计算数据集中相对于真实轨迹不同长度的平移误差和旋转误差，并计算平均值
• 精度指标: 相对位姿误差用均方根误差 RMSE 统计各段时间的相对位姿误差从、绝对轨迹误差(ATE)描述的是估计位姿和真实位姿之间的直接差值

a179a8a60f7974e1541d65b215df7f93.png

• 算法耗时和处理帧率: 作为计算效率的指标，能够直观反映计算负载和实时性能。

a26a8e166aa0133e924a12780d8bae37.png

f10576dbd7dfdf1824c6a7d3aa9c58e7.png

e7f01ab68c87cfbfbd09fecd153514da.png