高动态环境变化下的SLAM技术落地 | 高仙机器人发表IROS论文

　　近日，高仙机器人SLAM团队的研究成果在机器人领域顶级学术会议IROS （IEEE/RSJ International Conference on Intelligent Robots and Systems 智能机器人与系统国际会议）2021上发表。该论文提出了一种应对动态环境变化的长期定位与建图方法，引入了一种高效的动态地图更新机制，用于解决在变化环境下（如超市、商场、车库等）的定位跳动和丢失问题，从而大大提高了机器人在动态变化场景的适用性。

　　（高仙机器人SLAM团队持续招聘中）

　　文章简介

　　近些年刊登了不少有关于SLAM研究的最新成果，ICRA(IEEE International Conference on Robotics and Automation：机器人与自动化国际会议)是目前机器人和人工智能领域最重要的国际会议之一，高仙机器人SLAM团队对其中相关的文献进行追踪，并发表文章《从机器人落地角度谈谈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给出了整个稀疏化的流程（具体符号含义可以参考“符号介绍“小节）：

　　位姿图优化

　　在完成一次地图稀疏化后，需要进行一次全局优化，即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数量变化

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

　　高仙SLAM团队招人啦！

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

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

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

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

　　工作地点：

　　北京中关村/上海张江/杭州/新加坡/其他

　　应聘条件（满足其一即可）：

　　·有SLAM或相关岗位任职经历

　　·有SLAM或机器人、计算机视觉相关研究成果

　　·有丰富的C++项目开发经验，且对SLAM有兴趣

　　我们重视：

　　·严谨的逻辑思维和沟通能力

　　·在特定领域有深入研究和独立见解

　　·代码品味和质量追求