高仙IROS论文分享丨适用于变化环境下的长期建图定位系统

高仙IROS论文分享丨适用于变化环境下的长期建图定位系统

近日,高仙机器人SLAM团队的研究成果在机器人领域顶级学术会议IROS 2021上发表。

IROS全称 IEEE/RSJ International Conference on Intelligent Robots and Systems (智能机器人与系统国际会议),主要展示人工智能和机器人领域的最新研究进展(iros2021.org/)。

本期高仙技术家向大家分享这篇入选IROS的SLAM论文,该论文提出了一种应对动态环境变化的长期定位与建图方法,该方法引入一种高效的动态地图更新机制,用于解决在变化环境下(如超市、商场、车库等)的定位跳动和丢失问题。从而大大提高了机器人在动态变化场景的适用性。

论文基本信息

  • 标题:《A General Framework for Lifelong Localization and Mapping in Changing Environment》
  • 作者:Min Zhao, Xin Guo, Le Song, Baoxing Qin, Xuesong Shi, Gim Hee Lee, Guanghui Sun
  • 来源:2021 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS2021)
  • 论文链接arxiv.org/abs/2111.1094

文章简介

在前一篇文章从机器人落地角度谈谈ICRA2020对SLAM研究的启发,笔者介绍了SLAM的落地难点和相关进展,其中由于环境的变化,当前帧点云与预先建立的地图无法正确匹配建立约束(constraint),引入错误的约束信息或者无法建立有效约束,从而导致定位偏斜和定位丢失,因此极大地影响机器人的正常使用。

为了克服环境变化,我们提出了一种lifelong SLAM的方法。基于通用的图优化SLAM框架,增加了动态地图自动更新机制。更新的地图与实际场景保持同步,从而保证在定位过程中点云和地图始终建立有效的约束,从而克服定位偏斜和丢失。

基于上述问题和考虑,本文提出了一种通用的lifelong SLAM框架,相对于已有的lifelong方法,主要的贡献点包括:

  • 完整且稳定的lifelong SLAM框架,通过了实际场景的长期测试,并且得到大规模落地部署;
  • 基于Chow-Liu Tree的最大互信息submap稀疏化和优化方案,在维持常量计算复杂度的同时,达到了厘米级的定位精度;
  • 测试过程采集了包含激光雷达、IMU、轮式编码器在内的lifelong SLAM数据,并开源了该数据集,便于之后学术界和工业界的研究。

系统架构

图1. lifelong SLAM 系统框图

图1给出了系统架构图,主要包含6大模块,分别为:

  • 输入(Input):听取传感器数据,包含IMU、轮式编码计、激光雷达;
  • 预处理模块(Pre-Process):包含IMU和轮式编码器的数据融合、激光去畸变;
  • 局部激光里程计(Local LiDAR Odometry,简称LLO):SLAM前端,接收预处理模块的传感器数据,并进行连续帧匹配,同时生成submap;
  • 全局激光匹配(Global LiDAR Matching,简称GLM):SLAM后端,接收前端的激光数据并与地图数据中的submap进行匹配,构建约束信息,并送入PGR模块;
  • 位姿图优化(Pose Graph Refinement,简称PGR):SLAM后端,也是本文核心模块,负责submap的删除和添加,根据最大互信息原则进行位姿图的稀疏化,之后启动全局位姿图优化;
  • 输出(Output):实时输出计算得到的定位信息,TF转换、位姿和地图;

地图更新

地图更新过程

图2. 地图更新过程图示

本文针对lifelong SLAM问题引入session概念,具体更新过程如图2所示。第一次建图属于session 0,此时系统处于建图阶段(mapping stage),构建了当前环境的地图描述,并存入数据库。在机器人执行任务时,进入定位模式,每次定位会从数据库读取地图数据,并加载到内存,创建新session(session 1~n),定位过程中根据图1的算法,不断将新生成的submap添加到当前session,同时删除旧session的submap。在删除旧submap时,触发位姿图的稀疏化和位姿优化。最后在结束定位任务时,调用接口保存更新的地图,并传入数据库,从而完成一次地图更新。经过多次地图更新后,地图中包含多条session,每条session由一定数量的node、submap和约束信息构成。

位姿图稀疏化和优化

submap稀疏化

由于地图更新需要考虑删除旧submap,简单粗暴的方法是直接在位姿图中,将submap和附属的node、约束信息删除,其带来的问题是整个图将会丢失该submap的所有信息,因此在删除数据时,需要尽可能的在删除该submap时,将信息转移并保存到位姿图中,并用于后面的优化。

为了达到稀疏化的目的,考虑将submap和附属的node视为因子图(factor graph)的一个节点(node),constraint视为因子图的因子(factor),在删除submap时,对被删的submap及其附属数据进行一次边缘化(marginalization),同时为了消除边缘化带来的dense fill in问题,我们根据Chow-Liu Tree最大互信息原则保留了信息量最多的变量对,同时剔除其他的变量。图3给出了整个稀疏化的流程(具体符号含义可以参考“符号介绍“小节):

图3. 基于Chow-Liu Tree最大互信息的地图稀疏化流程

位姿图优化

在完成一次地图稀疏化后,需要进行一次全局优化,即PGR的Pose Graph Optimization的过程。该部分方法与其他SLAM后端差异不大,本质是创建一个无约束优化问题,优化的目标是调整submap和node的位姿,使得公式1的代价函数最小:

公式2

实验结果

为了验证算法的有效性,本文在北京某商场进行了长达一个月的有效性测试,构建了超过1万平的地图,其中包含550个submap,用于后续的地图更新测试,测试的机器人如图4所示,型号为高仙Scrubber 50清洁机器人:

图4. 用于测试的高仙Scrubber 50商用清洁机器人

地图更新情况

图5. 商超地图前后变化图
图6. 实际变化与地图对比

图5给出了运行前后一个月的地图变化,(a)为建图时地图,(b)为经过一个月地图更新运行后的地图,(c)为两者的差异。绿色点为环境中实际消失的环境特征,红色点为新增环境特征。可以看到本文算法在长期运行下,能够有效记录环境变化,同时地图未出现重影和错误。

高仙https://www.zhihu.com/video/1446786696499503104

定位性能效果

表1. 地图更新前后的定位性能比较

表1给出了执行了25次定位任务和地图更新,分别比较了纯里程计、无地图更新、地图更新三种情况下的定位性能,对于每次定位,蓝点表示初始化成功,蓝线的长短表示定位任务执行的里程数,中间间断表示定位丢失。表中左侧百分号指标为正确轨迹的里程比值(参考论文中MRCL,通过激光与地图匹配情况计算获得),右下侧为初始化成功率(参考论文CRI,通过统计初始化成功次数获得),可以看到开启地图更新后,定位精度和定位丢失概率相对于未开启地图更新得到显著提高。

表2. 平均匹配分数比较

表2基于25次地图更新数据,与不带地图更新的文献[2]比较了平均匹配分数,可以看到由于地图更新机制的引入,使得点云和地图匹配情况更好。

图6为本文方法和[2]进行比较,可以看到本文方法的定位精度更高,未出现定位跳动等问题。

图7. 与[2]方法的轨迹比较

计算量评估

图7和图8显示了在多次地图更新后,地图数据的变化和计算量变化情况。在超过18次的更新后,submap和node数量得到收敛,同时CPU和内存的占用保证在一定收敛范围内,证明本文方法适用于长期的现场部署。

图8. 多次更新后地图数据中的submap、node数量变化
图9. 多次更新后CPU和内存的占用变化

结语

为了解决长期运行时环境变化导致的定位问题,本文提出了一种完整的lifelong SLAM框架,基于Chow-Liu Tree互信息最大的submap更新方法,保证地图数据与环境特征能够实时同步,在取得高精度定位效果的同时,计算量也保证在一定范围内,适用于大范围的落地应用。

参考引用

  1. H. Kretzschmar and C. Stachniss, “Information-theoretic compression of pose graphs for laser-based slam,” The International Journal of Robotics Research, vol. 31, no. 11, pp. 1219–1230, 2012.
  2. W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2d lidar slam,” in 2016 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2016.
编辑于 2021-11-24 10:27