同步定位和建图(SLAM)是实现煤矿井下机器人自主导航的基础[1],而机器人利用各种传感器,在三维空间中进行 6DOF 状态估计,是实现高性能同步定位和建图(SLAM)的关键[2]。虽然基于激光雷达的方法可实时获得三维空间的精确场景测距信息,且对环境照明条件的不变性。但是,由于激光雷达测量存在运动失真[3]、低频更新和点云稀疏[4]的问题,这些特性决定了纯激光雷达不利于机器人处理激进的移动或重复的结构,如煤矿巷道或狭窄走廊[5]。值得注意的是,激光雷达的缺陷可以通过融合 IMU 来弥补。与激光雷达不同,IMU 不受结构特征和环境的剧烈变化的影响,并可以提供高频率和短时间的高精度位姿估计,但由于 IMU 传感器的噪声和偏置,以及误差累积会随着时间的积累而漂移。为了克服独立传感器的这些缺点,可采用多传感器融合的方法来提高状态估计[6][7]的可靠性。
多传感器融合的相关工作,根据不同的融合框架被分为:松耦合和紧耦合。基于松耦合的融合方法通常分别处理各传感器输入的测量,并对其测量结果进行加权融合,以推断机器人的运动状态。LOAM[8]和 LeGO-LOAM[9]是松耦合系统,IMU 测量通常用于减轻无特征环境中激光退化的问题,只以 IMU 计算出的姿态作为激光雷达扫描配准的初始估计值和运动失真校正的先验,而不作为全局优化的约束条件。为了提高自主机器人的定位精度,[10]在 LIO 松耦合框架中融入轮速计,利用轮速计测量 的 速 度 和 距 离 建 立 了 EKF 的 观 测 方 程 。Cartographer[11]是基于图优化实现了激光雷达、IMU、里程计和 GNSS 的融合,灵活处理扫描匹配和 IMU 联合约束以及其他观测约束。一般来说,松耦合的方法计算效率高[12],实现也相对灵活。然而,由于忽略了 IMU 内部状态的修正,它们很容易受到噪声的干扰[13]。与松耦合方法不同,紧耦合的方法通常会将激光雷达的原始特征点与 IMU 数据相融合,表现出较高的鲁棒性和准确性,并可分为基于优化和滤波的方法。对于基于优化的方法,LIO-mapping[14]引入了 IMU 预积分,最小化 IMU和激光雷达测量的残差,并且可以通过回环检测来纠正长时间的漂移,取得了较好的精度,而该算法的实时应用则较耗时。作为目前最先进的算法之一,LIO-SAM[15]通过引入激光雷达关键帧的滑动窗口来限制计算复杂度,利用因子图进行联合 IMU和激光雷达约束优化,以获得更高的精度。它是一种基于因子图的融合框架,适用于多传感器融合。而基于滤波的方法,经常使用扩展卡尔曼滤波(EKF)。LINS[16]提出了一种以机器人为中心的激光雷达惯性估计器,通过使用误差状态迭代卡尔曼滤波器。该方法虽然具有较高的精度,计算效率高,但在长期导航过程中也存在漂移问题。为了减少线性 化 导 致 的 估 计 误 差 对 系 统 带 来 的 影 响 ,R-LINS[17]、FAST-LIO[18]和 FAST-LIO2[19]都采用了迭代后的卡尔曼滤波器。与 R-LINS 和 FAST-LIO相比, FAST-LIO2 提 出 了 一 种 新 的 数 据 结 构ikd-Tree 来保存地图,它可以在不提取特征的情况下直接将原始点注册到地图上,使算法的工作速度更快、更准确。
尽管三维激光雷达性能优越,但结合煤矿井下的特殊环境存在空间非结构化、环境复杂、光照不均匀[20]、空间狭窄等实际情况,此类场景给激光SLAM 带来了新的挑战:1)激光雷达在环境中识别到的几何特征,通常是用数学方法人工定义的特征(例如点、线、面),当机器人在无几何特征、环境相似、对称结构的环境中运行时,基于激光雷达的方案很容易特征退化。2)激光雷达测量到的数据会在某一维度上呈现高度的相似,主要原因是巷道环境中的侧壁,只能提供横向定位参考,而不能提供纵向定位参考。在这种情况下,侧壁的特征匹配很容易导致错误的姿态约束估计。3)重复的激光特征会导致纵向上的误导性约束和姿态漂移,以致点云配准收敛至错误结果,定位性能会快速劣化或完全失效。严重地影响了 SLAM 整体的性能,也限制了机器人在此类特殊环境下自主导航的能力。
综上所述,机器人在煤矿井下如何保证 SLAM系统的估计精度、实时性、累计误差和鲁棒性之间的权衡,仍然是 LIO[21]面临的一大难题。因此,本文提出了一种以 LiDAR 和 IMU 两种传感器作为输入信息源基于紧耦合的激光惯性融合 SLAM 算法,并围绕其展开研究工作具有重要意义。
1 系统框架
考虑到我们的系统接收来自 3D LiDAR、IMU传感器的数据。我们的目标是机器人进行实时6DOF 的状态估计,并建立全局一致的地图。我们所提出的算法系统框架主要由前端迭代卡尔曼滤波和后端位姿图优化两部分组成,如图所示 1。在前端将传感器数据预处理,分别建立 IMU 预测模型和激光雷达观测模型,构建迭代卡尔曼滤波器,将机器人先验位姿经过预测和观测的状态传播过程,使其状态更新后的姿态更加准确,进而循环迭代得到基于紧耦合的激光惯性里程计。在后端使用关键帧判断当前位姿是否被加入到全局优化,使用回环检测判断当前位置是否被重新访问,这可以表述成一个最大后验(MAP)的问题。此外,再优化框架中添加了回环检测和地面约束,优化相邻关键帧之间的相对位姿,减少累积漂移,保持全局一致,提高定位精度。最后,在全局优化后输出运动轨迹、全局地图、状态估计等信息。
暂无评论内容