高仙IROS论文分享丨适用于变化环境下的长期建图定位系统（SLAM团队持续招人）

2021年11月23日

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

IROS全称 IEEE/RSJ International Conference on Intelligent Robots and Systems （智能机器人与系统国际会议），主要展示人工智能和机器人领域的最新研究进展（https://www.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)

论文链接：https://arxiv.org/abs/2111.10946

文章简介

在前一篇文章从机器人落地角度谈谈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的代价函数最小：

公式1

其中，，由下式得到：

公式2

符号介绍

使用，，分别表示节点(node)的位姿、submap位姿和约束信息。是的集合，为node的ID，表示session ID。类似的，submap构成。对于，本文出现三种约束：

1.：node到submap的约束，即node到submap的相对位姿观测，由点云到submap匹配计算得到，为约束的协方差矩阵，表示观测的不确定性；

2.：node到node的约束，即node到node的相对位姿观测，由位姿图稀疏化时获得，为约束的协方差矩阵；

3.：submap到submap的约束，即submap到submap的相对位姿观测，由位姿图稀疏化时得到，为约束的协方差矩阵；

实验结果

为了验证算法的有效性，本文在北京某商场进行了长达一个月的有效性测试，构建了超过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数量变化

图9. 多次更新后CPU和内存的占用变化

结语

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

参考引用

1. 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. 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.

高仙SLAM团队招人啦！

SLAM团队是高仙机器人核心算法研发部门之一，团队多年来自主研发的多模态SLAM系统，，可运行于室内外全场景，已支持高仙机器人6大系列产品在全球43个国家和地区实现商业落地，累计运行超过1.5亿公里。

团队目前依托大量落地场景和数据，致力于攻克现实世界中最具挑战的SLAM问题，并通过智能地图服务持续提升用户体验。团队成员全部来自国内985高校或海外知名高校，其中多人拥有博士学位，研究方向覆盖激光SLAM、视觉SLAM和深度学习。

团队与国内外多所知名高校展开学术合作，包括新加坡国立大学、清华大学、上海交通大学、哈尔滨工业大学、西安交通大学等。其中，与上海交通大学合作项目“低速无人系统定位导航技术与应用”荣获2020年度上海市科学技术奖技术发明一等奖。

团队长期招募对SLAM技术充满热情的全职或实习算法工程师、科学家、产品经理和项目经理，以创新拓展技术边界，引领行业发展。