用于机器人定位和建图的增强型LiDAR-惯性SLAM系统

粒子滤波也是一个十分经典的算法，它与卡尔曼滤波的不同之处在于卡尔曼滤波假设概率分布是高斯分布，然后在计算后验概率（pdf）时，利用正态分布的性质，可以计算出来；而粒子滤波的后验概率分布是通过蒙特卡洛方法采样得到的。蒙特卡洛方法很清楚的一点是采样的粒子越多，概率分布越准确，但是计算速度会下降。也就是说如何分布你的有限个数的采样粒子来得到更为准确的后验概率分布是粒子滤波一直在做的事情。在本文中粒子滤波的改善一个是局部采样，另一个是采样时更好的概率分布来得到更精确的后验概率。在闭环检测这里则是应用了深度学习的方法。具体实现可以随笔者一起看下面的文章。

An Enhanced LiDAR-Inertial SLAM System for Robotics Localization and Mapping摘要——基于激光雷达和惯性传感器的定位和建图对于无人地面车辆相关应用具有重要意义。在这项工作中，我们为无人地面车辆开发了改进的 LiDAR 惯性定位和映射系统，适用于多功能搜索和救援应用。与现有的基于 LiDAR 的定位和建图系统（如 LOAM）相比，我们有两个主要贡献：第一个是提高了基于粒子群滤波器的 LiDAR SLAM 的鲁棒性，第二个是为全局开发的闭环方法优化以提高整个系统的定位精度。我们通过实验证明，LiDAR SLAM 系统的准确性和鲁棒性都得到了提高。最后，我们在香港科学园以及其他室内或室外真实复杂的测试环境中进行了系统的实验测试，证明了我们的方法的有效性和效率。结果表明，我们的系统具有高精度、鲁棒性和效率。我们的系统对于无人驾驶地面车辆在未知环境中的定位和建图具有重要意义。一、引言及相关工作激光雷达(Light Detection And Ranging, LiDAR) 传感器是一种典型的传感器，可以获取具有局部范围的所有周围物体的距离[1]。基于激光雷达的同步定位和建图(SLAM) 对各种应用具有重要意义，例如自动驾驶、3D 机器人抓取、搜索和救援机器人以及机器人监视和检查。在过去的几年中，除了各种先进的控制技术 [2] 之外，基于深度学习的方法也引起了极大的关注，因为它们重新创建了视觉世界。最近，由于开发了基于大规模 LiDAR 的定位基准，例如用于自动驾驶的 KITTI [3]-[7]，已经提出了各种 LiDAR-SLAM 系统。基于 gmapping 的方法已经提出[8]，这是一个典型的基于粒子滤波器的LiDAR SLAM。谷歌提出的Cartographer [9]，[10]利用Ceres求解器求解非线性最小二乘来实现扫描中的匹配。结构通过子图和子图到扫描的匹配，实现闭环检测和全局优化。这种方法包括Fast-LIO [11]，它在里程计的前端利用增量KD-Tree .后端使用迭代扩展卡尔曼滤波器。它具有持续时间短和计算速度快的优点。基于LOAM的LiDAR-SLAM利用点云的边缘和平面特征作为前端特征。这些特征是存储在地图中，匹配时利用点到线和点到面的距离信息。高频视觉信息和低频LiDAR里程计信息都被用来实现机器人运动估计[ 12].[13]中提出了用于细粒度特征提取的LEGO-LOAM。并且该系统可以在简单的环境中以可接受的精度实现实时定位和映射性能。最近，也提出了一些方法战术解决了固态LiDAR视场受限的问题，例如Loam-Livox。最近提出了基于深度学习的特征提取方法。但是，主要问题是以前的方法都没有包含循环因此，LiDAR SLAM系统存在漂移和精度低的问题。此外，上述所有方法都利用卡尔曼滤波器在LiDAR SLAM系统的前端计算里程计。但是，粒子滤波器-基于[8]、[14]的方法计算里程计尚未得到充分探索。总而言之，目前基于LiDAR的SLAM的各种研究大致可以概括为两个方面：第一部分着重于提高精度第二部分侧重于提高系统的鲁棒性。已经提出了各种方法来提高各种室内或室外场景中的定位精度和鲁棒性。但是，如何取得很好的效果这两个因素之间的平衡需要更多的探索。图 1. 复杂环境下的大尺度测绘结果图 2. 推荐的 LiDAR SLAM 建图结果的最终演示。可以看出，在 Mulran [15] 基准上，我们提出的基于学习的回环检测方法可以达到令人满意的精度，并将较大的误差（子图（a）是提出回环检测方法之前的全局图）纠正为全局一致地图（子图（b）是我们做闭环后得到的地图。）子图（c）和子图（d）分别展示了我们在室外道路案例和室外港口案例上的测试结果。可以看出，使用机械和固态 LiDAR [16]-[18] 可以在室内和室外环境中实现全局一致映射。证明了我们提出的方法的稳健性。为了应对上述挑战，在这项工作中，我们提出了一种基于粒子滤波的方法来解决未知环境下的激光雷达惯性定位和映射问题。首先，我们提出了一种适用于室内和室外定位和映射的自适应粒子群滤波器。我们在自适应粒子群滤波器的最优化中提出了较小的时间步长，以提高定位的成功率和稳健性。此外，我们还提出了使用重采样策略来做出正确的抽样分布。最后，我们利用基于扩展卡尔曼滤波器的粒子滤波器进行状态估计。此外，我们还提出了一种基于轻量级学习的方法来寻找激光雷达SLAM中的闭环。我们利用带有分类头的深度神经网络判断两个局部激光雷达扫描是否重叠。此外，我们为深度神经网络设计了一个回归头来预测两个激光雷达扫描之间的相对角度。总之，我们有以下突出贡献：1) 我们针对定位问题提出了基于粒子群滤波器的自适应框架。适应的方法利用重采样策略和适当的建议密度基于构建的局部地图进行优化。2）我们设计了一种有效的闭环方法来提高定位精度和鲁棒性。各种实验证明了我们提出的闭环算法的有效性。3）对于效率问题，我们还利用各种最近邻搜索算法来快速计算邻居。在原有实现的基础上，我们将最近邻搜索的速度提高了 5.8 倍。4）我们已经将所提出的方法集成到我们的系统中，无论是Livox-Avia的固态激光雷达还是VLP-16等机械激光雷达。可以证明，我们的方法在有效性和效率上有明显的优点。它在包括香港科技园在内的各种测试环境中取得了令人满意的性能。此外，我们的方法可以与基于机器人学安全走廊的运动规划无缝集成。我们方法的稳健性和准确率在公共数据集上以及真实复杂的室内和室外环境中都得到了证明图 3. 固态激光雷达的扫描模式图 4. 校园场景的机械 LiDAR 建图结果图 5. 推荐的 LiDAR-惯性 SLAM 系统的最终软件框架在接下来的工作中，第二部分介绍了我们的整体系统框架和我们的两个主要贡献。接下来，我们提出我们的两个主要贡献，自适应粒子群滤波器和基于学习的闭环方法。由于页数限制，我们在每个小节中都包含了我们的实验结果。最后，我们简要介绍了我们在香港进行本地建筑检查的综合系统。二、推荐的方法A. 整体系统框架1）我们提出的激光雷达惯性定位和建图系统的框架：提出的定位和建图框架主要基于典型的激光雷达 SLAM 或激光雷达惯性系统，如 LEGO-LOAM [13]和 LIO-SAM [19]。与他们的方法相比，我们有两个主要区别：首先是我们提出了第一个基于粒子滤波器的方法，与以前的方法如 LEGO-LOAM [13] 和 LIO-SAM [19] 相比，它获得了更好的效率。第二个是我们提出了一种有效的闭环检测网络，称为 LoopNet。结果表明，我们提出的方法可以非常有效地执行闭环，并且整个 LiDAR SLAM 系统的整体精度大大提高。图 6. 基于 FG-Conv 的 FG-Net 网络架构的总体框架2) 推荐的 LiDAR-惯性 SLAM 系统的最终软件框架：我们推荐的 LiDAR-惯性 SLAM 的编码框架如图 6 所示。我们软件的运行过程总结如下：1.激光运动畸变校正。预积分是通过使用当前帧开始和结束之间的 IMU 数据和 IMU 里程计数据来计算的。获取各时刻激光点的位姿，将其转化为激光在初始时刻的坐标系，实现标定。2.提取特征。针对运动畸变校正后的当前帧激光点云，计算每个点的曲率，然后提取角点和平面点特征。3.扫描到地图匹配。提取局部关键帧图的特征点，与当前帧的特征点进行scan-to-map匹配，更新当前帧的位姿。4. 因子图优化或基于扩展卡尔曼滤波器的优化。在图中添加激光里程计因子、GPS因子、闭环因子并进行因子图优化，更新所有关键帧位姿。5. 闭环检测。在历史关键帧中寻找候选闭环匹配帧，进行扫描到地图匹配以获得位姿变换。然后我们可以构建闭环因子，并将它们添加到因子图中进行优化。应该提到的是，为了保持我们传感器套装的最低要求，我们在我们的软件中包含了 GPS 融合模块，但我们没有在所有实验中使用 GPS 信息。B.自适应粒子群滤波器1）粒子群滤波器的原理：基于粒子滤波器的SLAM的基本思想是使用m个随机样本 表示机器人在第k个时间步的置信度值 . Xr(k)表示机器人在第k步的位姿值。wk 表示 Xr(k) 对应的权值。d0,1,2,..,k表示从时间步长Zk = {Z(0), Z(1), .., Z(k)开始的数据，估计状态为Xr(k)。Xr(k) 是移动机器人的位姿。Xr(k) 是移动机器人的位姿。Xr(k) 代表移动机器人的位姿。Xr(k) = (xr(k), yr(k), θr(k))。系统说明如下：其中 u(k) 是运动输入，例如来自里程计的数据。w(k) 是过程噪声。测量函数可以表述如下：Xn(k)代表路由信息，而v(k)代表测量噪声。然后我们就可以获得机器人的可信度。根据贝叶斯定理，我们可以得到递归计算，可以表述为：根据马尔可夫假设，测量 Z(k) 与先前的测量条件独立。将所有先前的测量表示为 Zk, Zk−1 = {Z(0), ..., Z(K − 1)}，这仅取决于先前的测量。并且它只与姿势 Xn(k) 有关。给定 Xn(k − 1) 和 u(k − 1)，状态 Xn(k) 独立于之前的状态 Xn(k − 1) 和控制 u(k − 1)，即：这个方程是粒子群滤波器的基础。其中  , p(Z(k)|Xr(k) 是传感器模型对应的机器人。我们可以定义作为重要性函数。q的确定对机器人的影响很大基于粒子滤波的SLAM性能图 7. 推荐的 LiDAR SLAM 建图结果的最终演示。可以看出，在 Mulran [15] 基准上，我们提出的基于学习的回环检测方法可以达到令人满意的精度并将大误差（子图（a）是提出回环检测方法之前的全局图）纠正为全局一致图（子图（b）是我们做闭环后得到的地图。）子图（c）和子图（d）分别展示了我们在室外道路案例和室外港口案例上的测试结果。可以看出，无论是在室内还是室外环境下，机械和固态激光雷达都可以实现全局一致的建图。证明了我们提出的方法的稳健性2）先前粒子群滤波器的缺点：我们提出了一种用于定位和建图的自适应粒子群滤波器。传统上，SLAM 中典型的粒子群滤波器 [8] 寻求达到全局收敛。在理想情况下，基于标准粒子群滤波器的 SLAM 系统将在无限次迭代优化后达到全局收敛。最后，我们可以获得机器人系统的确定性姿态和环境的确定性映射。然而，在现实中，由不确定的机器人姿势构建的映射并不是很准确。同时，从不确定的环境地图中估计的机器人位姿不准确。随着机器人的运动，机器人姿态的准确性将对建图精度产生很大影响，反之亦然。这将导致机器人映射和定位的准确性和效率低下。有时，可能不满足一些弱约束，最终定位不会收敛。同时，基于标准粒子滤波器的 SLAM 在整个探索环境中采用均匀分布的粒子。并且收敛速度也会降低。此外，粒子数量也会增加，这将增加计算需求并影响整个LiDAR-Inertial SLAM系统的实时性能[20]，[21]3）自适应粒子群滤波器优化的更小时间步长：我们提出了自适应粒子群滤波器。自适应粒子群滤波器针对机器人姿态的不确定性和建图结果的不确定性这一事实。我们提出的方法在局部范围内寻找机器人的最佳姿态 [11]、[22]、[23]。然后，利用机器人在局部范围内的最佳位姿来构建局部范围内的环境地图。我们最终提出的具体方法可以总结如下：1）找出以计算出的机器人姿势为中心的小方块。对于室内案例，我们将矩形的边长设置为 1cm，对于室外案例，我们将矩形的边长设置为 10cm。2) 我们让一定数量的粒子均匀分布在矩形中。机器人移动十个时间步后，定位完成。3）然后我们使用激光雷达重新扫描环境并执行地图更新。这样，利用更小时间步的简单技巧，并在局部范围内进行优化，我们可以提高最终 LiDAR SLAM 系统定位的成功率和鲁棒性。总之，我们在短时间内基于构建的局部地图执行基于粒子群的定位。另外，我们让粒子相对于机器人在一个小范围内，这可以大大减少所需的粒子4）解决退化问题的建议方法：基本粒子滤波器中存在的问题是退化问题。粒子权值的变化会增加迭代次数，退化不可避免。通过迭代，其他粒子的权重将达到一个可以忽略不计的小值。那么退化意味着大量的时间和计算资源将浪费在重量很小的粒子上。不仅会造成资源的浪费，还会影响最终的估算结果。在我们的工作中，我们提出了解决权重退化问题的方法：1）利用重采样策略。重采样策略的本质是增加粒子的多样性。通过引入重采样，多样性会变大。在我们的工作中，我们利用 Gordon 等人提出的多项重采样 [24]，它解决了粒子滤波器中的退化问题。2) 我们还利用了类似于 [25] 的适当的建议密度分布。基本粒子滤波器的假设是：基于重要性的重采样可以从适当的后验建议密度分布中采样一组点。建议的密度分布函数指导重采样以做出正确的样本分布。因此，如果我们能够找到一个最优的建议密度分布函数，那么最终的滤波质量就可以得到保证三、基于学习的闭环此外，我们还提出了一种基于深度学习的 SLAM 系统闭环算法。通过我们提出的回环检测技术，可以有效地比较相似地方的特征，并且可以有效地检测回环。此外，我们提出的网络架构非常轻巧，适合实时机器人应用。我们还提出了基于学习的闭环检测网络 Loop-Net。该网络严重依赖于我们之前的工作 FG-Net [26]-[28]。我们使用分类损失来判断两次 LiDAR 扫描之间是否存在重叠。我们使用回归损失来预测两次 LiDAR 扫描之间的相对角度四、香港本地验楼综合系统最后，我们成功利用上述LiDAR SLAM系统在香港本地进行建筑物检查并获得监控。该系统如图 8 所示。实验表明，我们提出的 LiDAR-Inertial 同步定位和建图系统具有强大的生成高质量地图的能力，这对于进行自主运动规划以生成可行路径。LiDAR SLAM 系统构建的 3D 地图对于 3D 建筑物重建、3D 模型分析、建筑物目标或缺陷定位与检测等进一步应用也具有重要意义五、结论在这项工作中，我们提出了一种用于无人驾驶地面车辆定位的集成改进型 LiDAR-惯性同步定位和建图系统 [29]。我们提出了一种基于粒子滤波器的里程计的系统设计。通过进行局部优化，我们提高了以前的 LiDAR-Inertial SLAM 的鲁棒性和效率。并且通过提出的基于学习的闭环检测方法，我们可以提高最终全局地图的准确性。可以证明，我们提出的 LiDAR-Inertial SLAM 系统在室内和室外环境中都显示出出色的性能和鲁棒性，机械 LiDAR 或固态 LiDAR 具有良好的准确性。在未来，我们希望探索使用语义 SLAM [30] 进行机器人场景解析。图 8. 基于我们提出的 LiDAR SLAM 系统的 UGV 检查部署系统的详细说明

编辑：黄飞