近日，高仙机器人slam团队的研究成果在机器人领域顶级学术会议iros 2021上发表。
iros全称 ieee/rsj international conference on intelligent robots and systems （智能机器人与系统国际会议），主要展示人工智能和机器人领域的最新研究进展（）。
本期高仙技术家向大家分享这篇入选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)
论文链接：
文章简介
在前一篇文章，笔者介绍了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的代价函数最小：
符号介绍
实验结果
为了验证算法的有效性，本文在北京某商场进行了长达一个月的有效性测试，构建了超过1万平的地图，其中包含550个submap，用于后续的地图更新测试，测试的机器人如图4所示，型号为高仙scrubber 50清洁机器人：
图4. 用于测试的高仙scrubber 50商用清洁机器人
地图更新情况
图5. 商超地图前后变化图
图6. 实际变化与地图对比
图5给出了运行前后一个月的地图变化，(a)为建图时地图，(b)为经过一个月地图更新运行后的地图，(c)为两者的差异。绿色点为环境中实际消失的环境特征，红色点为新增环境特征。可以看到本文算法在长期运行下，能够有效记录环境变化，同时地图未出现重影和错误。
定位性能效果
表1. 地图更新前后的定位性能比较
表1给出了执行了25次定位任务和地图更新，分别比较了纯里程计、无地图更新、地图更新三种情况下的定位性能，对于每次定位，蓝点表示初始化成功，蓝线的长短表示定位任务执行的里程数，中间间断表示定位丢失。表中左侧百分号指标为正确轨迹的里程比值（参考论文中mrcl，通过激光与地图匹配情况计算获得），右下侧为初始化成功率（参考论文cri，通过统计初始化成功次数获得），可以看到开启地图更新后，定位精度和定位丢失概率相对于未开启地图更新得到显著提高。
表2. 平均匹配分数比较
表2基于25次地图更新数据，与不带地图更新的文献[2]比较了平均匹配分数，可以看到由于地图更新机制的引入，使得点云和地图匹配情况更好。
图6为本文方法和[2]进行比较，可以看到本文方法的定位精度更高，未出现定位跳动等问题。
图7. 与[2]方法的轨迹比较
计算量评估
图7和图8显示了在多次地图更新后，地图数据的变化和计算量变化情况。在超过18次的更新后，submap和node数量得到收敛，同时cpu和内存的占用保证在一定收敛范围内，证明本文方法适用于长期的现场部署。
图8. 多次更新后地图数据中的submap、node数量变化
结语
为了解决长期运行时环境变化导致的定位问题，本文提出了一种完整的lifelong slam框架，基于chow-liu tree互信息最大的submap更新方法，保证地图数据与环境特征能够实时同步，在取得高精度定位效果的同时，计算量也保证在一定范围内，适用于大范围的落地应用。
参考引用
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.
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.