近日,高仙机器人SLAM团队的研究成果在机器人领域顶级学术会议IROS 2021上发表。
IROS全称 IEEE/RSJ International Conference on Intelligent Robots and Systems (智能机器人与系统国际会议),主要展示人工智能和机器人领域的最新研究进展(https://www.iros2021.org/)。
本期高仙技术家向大家分享这篇入选IROS的SLAM论文,该论文提出了一种应对动态环境变化的长期定位与建图方法,该方法引入一种高效的动态地图更新机制,用于解决在变化环境下(如超市、商场、车库等)的定位跳动和丢失问题。从而大大提高了机器人在动态变化场景的适用性。
论文基本信息
在前一篇文章从机器人落地角度谈谈ICRA2020对SLAM研究的启发,笔者介绍了SLAM的落地难点和相关进展,其中由于环境的变化,当前帧点云与预先建立的地图无法正确匹配建立约束(constraint),引入错误的约束信息或者无法建立有效约束,从而导致定位偏斜和定位丢失,因此极大地影响机器人的正常使用。
为了克服环境变化,我们提出了一种lifelong SLAM的方法。基于通用的图优化SLAM框架,增加了动态地图自动更新机制。更新的地图与实际场景保持同步,从而保证在定位过程中点云和地图始终建立有效的约束,从而克服定位偏斜和丢失。
基于上述问题和考虑,本文提出了一种通用的lifelong SLAM框架,相对于已有的lifelong方法,主要的贡献点包括:
图1给出了系统架构图,主要包含6大模块,分别为:
本文针对lifelong SLAM问题引入session概念,具体更新过程如图2所示。第一次建图属于session 0,此时系统处于建图阶段(mapping stage),构建了当前环境的地图描述,并存入数据库。在机器人执行任务时,进入定位模式,每次定位会从数据库读取地图数据,并加载到内存,创建新session(session 1~n),定位过程中根据图1的算法,不断将新生成的submap添加到当前session,同时删除旧session的submap。在删除旧submap时,触发位姿图的稀疏化和位姿优化。最后在结束定位任务时,调用接口保存更新的地图,并传入数据库,从而完成一次地图更新。经过多次地图更新后,地图中包含多条session,每条session由一定数量的node、submap和约束信息构成。
由于地图更新需要考虑删除旧submap,简单粗暴的方法是直接在位姿图中,将submap和附属的node、约束信息删除,其带来的问题是整个图将会丢失该submap的所有信息,因此在删除数据时,需要尽可能的在删除该submap时,将信息转移并保存到位姿图中,并用于后面的优化。
为了达到稀疏化的目的,考虑将submap和附属的node视为因子图(factor graph)的一个节点(node),constraint视为因子图的因子(factor),在删除submap时,对被删的submap及其附属数据进行一次边缘化(marginalization),同时为了消除边缘化带来的dense fill in问题,我们根据Chow-Liu Tree最大互信息原则保留了信息量最多的变量对,同时剔除其他的变量。图3给出了整个稀疏化的流程(具体符号含义可以参考“符号介绍“小节):
在完成一次地图稀疏化后,需要进行一次全局优化,即PGR的Pose Graph Optimization的过程。该部分方法与其他SLAM后端差异不大,本质是创建一个无约束优化问题,优化的目标是调整submap和node的位姿,使得公式1的代价函数最小:
为了验证算法的有效性,本文在北京某商场进行了长达一个月的有效性测试,构建了超过1万平的地图,其中包含550个submap,用于后续的地图更新测试,测试的机器人如图4所示,型号为高仙Scrubber 50清洁机器人:
图5给出了运行前后一个月的地图变化,(a)为建图时地图,(b)为经过一个月地图更新运行后的地图,(c)为两者的差异。绿色点为环境中实际消失的环境特征,红色点为新增环境特征。可以看到本文算法在长期运行下,能够有效记录环境变化,同时地图未出现重影和错误。
表1给出了执行了25次定位任务和地图更新,分别比较了纯里程计、无地图更新、地图更新三种情况下的定位性能,对于每次定位,蓝点表示初始化成功,蓝线的长短表示定位任务执行的里程数,中间间断表示定位丢失。表中左侧百分号指标为正确轨迹的里程比值(参考论文中MRCL,通过激光与地图匹配情况计算获得),右下侧为初始化成功率(参考论文CRI,通过统计初始化成功次数获得),可以看到开启地图更新后,定位精度和定位丢失概率相对于未开启地图更新得到显著提高。
表2基于25次地图更新数据,与不带地图更新的文献[2]比较了平均匹配分数,可以看到由于地图更新机制的引入,使得点云和地图匹配情况更好。
图6为本文方法和[2]进行比较,可以看到本文方法的定位精度更高,未出现定位跳动等问题。
图7和图8显示了在多次地图更新后,地图数据的变化和计算量变化情况。在超过18次的更新后,submap和node数量得到收敛,同时CPU和内存的占用保证在一定收敛范围内,证明本文方法适用于长期的现场部署。
为了解决长期运行时环境变化导致的定位问题,本文提出了一种完整的lifelong SLAM框架,基于Chow-Liu Tree互信息最大的submap更新方法,保证地图数据与环境特征能够实时同步,在取得高精度定位效果的同时,计算量也保证在一定范围内,适用于大范围的落地应用。