动态环境下的地图自主更新和定位方法

文｜秦金龙 董敏杰 陈放 王玮琪

一、引言

静态场景下的机器人导航定位技术目前已经比较成熟。为了解决动态场景下机器人定位问题，本文介绍了一种动态环境下机器人自主更新地图和定位方法。

本文提出的方法是在cartographer框架基础上改进优化的。本文主要贡献有：①通过不同轨迹内的nodes和submaps建立的约束个数作为机器人定位状态是否正常的依据。②通过机器人定位状态来判断是否保留新增的submaps来更新当前区域的地图数据。③删除旧的地图数据，保留新的地图数据，防止动态环境下地图数据持续增大。

二、建图算法原理

Cartographer是目前主流的基于图优化的激光SLAM算法。激光雷达扫描得到的点云数据代表障碍物与雷达的距离，其出现的位置表示该位置有障碍物，通过栅格的概率值表示该位置点存在障碍物的概率。

（一） 前端匹配

Cartographer前端是激光雷达数据与栅格地图匹配进行位姿估计。主要方法是通过暴力匹配，得到最高得分的解即为最优解，获取匹配程度最高的位姿就是激光雷达与栅格地图的相对位姿。

经过扫描匹配会得到激光雷达与submap相对位姿的估计值；如果在障碍物特征相似的环境如长走廊，扫描匹配效果较差；为了解决该问题，在每帧激光雷达数据插入到submap之前，cartographer引入Ceres扫描匹配器进一步优化node与submap之间的局部位姿，该优化器不仅考虑激光雷达数据与submap之前的概率匹配，还加入其他传感器预测权重，如里程计预测位置和角度，如果在特征相似的环境中可以把里程计预测的位姿权重增大，这样就可以避免陷入局部最优的情况。

（二）后端优化

全局优化需要进行回环检测建立闭环约束；回环检测是指识别曾到过的环境，使得地图符合闭环的条件。闭环约束由同一条轨迹的node和submap形成的位姿约束；回环检测实时检测激光雷达数据与已知的submap的匹配结果，如果找到高于阈值的匹配就把其添加到闭环约束中；全局优化通过node、submap、闭环约束等信息形成位姿图，通过图优化的方式使整体约束之间的残差最小，残差即约束与真实值之差。

全局约束建立的匹配方法与前端扫描匹配的方法原理相同；回环检测是判断机器人是否与历史submap匹配，由于误差累计的影响会导致位置不确定性大，搜索范围也扩大，所以后端采用了分支定界的方法加快匹配速度。

Cartographer在动态环境下的建图和定位效果较差，随着运行次数增多，其submap数量会持续增加影响机器人运行效率。为了解决上述问题，本文提出了一种动态环境下的地图自主更新和定位方法。

三、动态环境下地图自主更新和定位方法

为了保证在动态环境下机器人定位不丢失，本文通过机器人当前闭环约束的情况判断当前环境是否发生变换，同时判断是否保留当前node和submap更新当前地图信息。

（一）定位轨迹的建立

在定位模式下，会创建一条新的轨迹，把建图生成的轨迹记作trajectory0，定位生成的轨迹记作trajectory1；定位模式下，trajectory1的node会与trajectory0的submap进行特征匹配，如果匹配评分大于阈值则认定形成了闭环，两者会形成约束（constraint）；同时如果trajectory1新生成一个submap，则该submap会与trajectory0的node进行特征匹配，如果匹配评分大于一定值也会建立constraint；当前trajectory1新生成一个submap后，会对之前建立的所有constraint进行最小二乘法图优化。

随着机器人不间断运行，会生成较多的node和submap，会降低计算量和保证定位效果；因此定位轨迹只保留最新的4个submap以及对应的node，会把其他多余的submap及其node裁剪掉；当新增的constraint的数量大于阈值时则认定机器人定位正确；该定位方法不仅考虑机器人当前位姿的约束状态，还考虑之前一定时间内的约束状态，而且当前的定位状态依赖于之前所有约束的优化效果，所以该定位方法具有较好的鲁棒性和稳定性。

（二）地图更新方法

为了解决变化环境下地图实时更新的问题，本文会对定位轨迹trajectory1的submap及其node进行选择性保留，当任务执行完成后，会把trajectory0和trajectory1的相关信息一起保存到数据库中得到最新的地图文件。

机器人执行任务中，当新增的constraint数量小于阈值时，可以认定定位效果差，为了保留当前更新后的实际环境地图数据，不再对trajectory1的submap及其node进行裁剪，直到新增的constraint总数大于一定的值，这时说明trajectory1有足够的constraint进行闭环；后续对trajectory1的submap及其node进行裁剪时保留n个之前记录的submap及其node和最新的4个submap及其node，把中间多余的submap及其node进行裁剪；当机器人执行任务结束后，把所有轨迹的submap及其node一同保留到地图数据库，这样就保证变化环境信息可以实时保存到地图数据中，从而保证机器人长时间运行下的定位准确性。

（三）裁剪重复子图及约束重建

当某些区域环境变化时，本文方法会保存当前submap直到机器人建立足够的闭环约束。既然变化的区域已经保存为最新的submap，就可以把之前的submap及其node删除掉。

因为所删除的submap及其node会与其他存在的node或submap存在约束关系，如果直接忽略这些约束关系则会导致地图错乱；为了解决上述问题，本文利用稠密图稀疏化的方式重建这些约束。

本文把与需要裁剪的submap有约束关系的submap和node分别放在不同的vector中，通过整数0-99和100-200分别映射与被删除submap有约束关系的submap和node，把这些整数当做图的顶点，两两结合当做图的边，通过边的距离设置图的weight，这样就形成了以submap及node为顶点，两者直接的相对位姿为边的稠密图；然后通过图稀疏化的方式减少边的个数，最后根据边的编号提取处对应的submap和node，然后根据两者的相对位姿关系形成约束，保证剪裁重复的submap时不会导致其他submap的闭环约束丢失。

四、实验结果对比

通过机器人在实际动态环境中运行数据来对比本文提出的动态环境下的地图自主更新和定位方法的有效性。首先设置两组对比实验，第一组为正常未添加障碍物的地图，第二组为添加了障碍物的地图，且障碍物添加在机器人运行路线中。实验中发现，机器人可实时检测路径上的障碍物，并根据障碍物的变化实时更新submap。

根据第一组与第二组的对比，在走廊区域增加了某些动态障碍物导致地图数据出现变化。本文通过不同位置处激光雷达实时检测的环境数据分别于上述两组进行定位匹配，得到的概率匹配评分如表1。

表1 机器人定位匹配评分对比

由表1中的匹配评分数据可知，动态更新后的匹配评分更高，可以有效减少机器人定位丢失问题。

实验数据表明，对重复子图进行裁剪时，未对裁剪的submap及其node进行约束重建处理的地图会出现重影或错乱问题，进行约束重建处理后则不会出现地图重影、错乱问题（见图1）。

图1 结果展示 左-约束前地图 右-约束构建后地图

五、结论

为了解决变化环境下机器人定位问题，本文提出了一种动态环境下的地图自主更新和定位方法；机器人在执行任务过程中可以自主判断哪些区域需要动态更新并保证后续执行任务时有准确的定位信息。本文在主流的激光雷达SLAM cartographer框架中增加地图动态更新和定位功能，增加重复子图删除和约束重建功能，保证机器人长时间运行效率。通过实际测试数据验证了该方法在动态环境中有较稳定的运行效果，不需要人为更新变化区域的地图数据。