激光SLAM的回环检测优化

ref：基于建图优化与回环检测的激光雷达 SLAM 技术研究

1. SLAM概述

SLAM 解决的是根据观测到的环境信息，估计自身的位置和姿态并建立环境地图的问题，本质在于利用观测到的数据和先验信息来估计最大后验概率，观测到的数据之间存在着一定的约束关系。 在激光 SLAM 中，主要存在两种优化方式，即滤波器和非线性优化方法：滤波器优化算法基于贝叶斯概率模型，通过利用先验信息、测量信息和状态转移公式计算后验概率，从而得到最优估计的状态量；通过观测数据建立求解位姿的估计问题，可以转化为一个非线性最小二乘问题，例如梯度下降法和高斯牛顿法，在进行整体优化时，可以采用图优化的方法进行建模和求解，比如基于概率模型的因子图优化，常用的开源非线性优化算法库包括 Ceres-Solver、G2o、iSAM、GTSAM和 SE-Sync。

4bc326aabce057b6f3f1e02a12e7655c.png

2. 回环检测

回环检测可以通过建立当前帧和曾到达地方历史帧的约束，来降低累计误差，主要包括基于欧氏距离和基于上下文的回环检测方法：基于欧式距离的回环检测算法原理是在当前载具位置进行搜索其曾经到达过的位置印记，如果搜索到的位置印记与当前位置的欧式距离小于设定的阈值，则认为检测到了回环；基于上下文扫描回环检测算法的核心思想是将每个关键帧转化为全局描述子，并通过比较全局描述子的相似度评分来获取候选回环帧，可以将三维点云的鸟瞰图降维至二维矩阵进行实施。

对输入帧的平面点云进行地面去除，并通过聚类方法去除干扰点，然后进行二次线性拟合，以找出最能代表当前环境 结构的特征点，这些特征点经过去中心化处理，去中心化点坐标根据载具坐标进行偏移得到结构原点，以确保视场角与结构化环境吻合。接下来，构建关于结构原点与特征点的全局描述子，该描述子反映了它们之间的空间几何关系。最后，由于软编码具有旋转不变性，所以选择软编码的方式记录当前结构信息。

6e22b3026375054543b7330a9857c171.png

3. 点云优化

去除动态物体：在世界坐标系下，静态物体与动态物体之间最大的区别在于动态物体会出现相对位置的变化，分析连续帧之间的位移变化，进行区分动态和静态点云，根据实际经验和场景特点，选择适当的阈值作为判别动态点云的界限，比如采用阈值0.2m。

优化雷达误差：一般来说，激光雷达的测距误差在2-5 厘米之间，测距误差会导致建立的点云地图中平面的微观表面变得粗糙，使得地图中的物体位置、形状和表面特征存在一定的误差。基于局部地图中的微小面元相对平整并且在局部范围内保持一 致的假设，将局部地图中的所有点向其自身周围的微小面元进行平移，相当于对所有点向均值进行插值。