多无人系统协同视觉SLAM 算法

魏 强 张冬梅 范勇生

(1.中国空间技术研究院, 北京 100098; 2.中国东方红卫星股份有限公司, 北京 100081;3.上海宇航系统工程研究所, 上海 201108)

1 引言

近年来,随着机器人领域技术的不断发展,以地面无人车辆和无人机为代表的智能无人系统在军事、民用等领域中的应用逐渐成为了研究热点,智能无人系统不仅能够完成浇灌、测绘、巡检等民用任务,还能有效完成侦察、打击、追踪等作战任务。 然而随着任务需求的增高,对无人系统能力的需求也愈来愈高。

导航系统是无人系统功能实现的关键组成部分,负责向无人系统提供其在参考坐标系下的位置、速度、姿态等关键信息,引导无人系统按照既定路线行进,导航系统的精确程度和稳定性直接影响了后续任务规划和控制的能力。 传统的导航系统多依赖于全球导航卫星系统(Global Navigation Satellite System,GNSS)和机载惯性导航元件之间的组合导航,然而,在室内环境或复杂室外环境中GNSS 系统会受高大障碍物遮挡以及多路径效应的影响,导致信号解析错误或信号中断。 同时定位与建图技术(Simultaneous Localization and Mapping, SLAM)[1]使智能体获得自定位的能力,以保证在GPS 拒止的情况下无人系统能够完成自主导航,同时SLAM 技术能够建立一个全局一致表征的环境地图,可用于环境探索等任务,因此,SLAM 技术是在GNSS 拒止情况下,使无人系统能够完成自主导航的关键技术之一。 按照使用的传感器不同,SLAM 技术主要可以分为激光雷达SLAM 和视觉SLAM 2 种:激光雷达误差模型简单,对于距离估计更为准确,并且不受弱光照等环境因素的影响,但是价格昂贵,重量大;相较之下,视觉传感器价格低廉,设备轻便,应用前景更加广泛。 目前小范围环境下的SLAM 问题研究较为透彻,但单一无人系统在大范围场景下进行探索时效率较低,并且由于地图数据量的不断增长,计算时间和内存占用都会对机载计算机的能力提出严苛的要求。

多机协同SLAM 可以将问题分而治之,将整个任务的计算/存储压力以及计算量分摊到参与协同的各个无人系统上,大幅提高任务效率。 因此,多机协同SLAM 逐渐成为了当前的研究重点。目前多机协同视觉SLAM 系统多数为中心式架构[2],中心式架构将智能体分为子端和中心端,子端和中心端直接进行信息交互,子端智能体执行部分SLAM 任务,完成状态估计并建立局部地图;中心端利用子端传输的数据,对地图进行融合和优化,创建全局一致性表示的全局地图。 2008年,Castle 等[3]在Klein 等[4]的基础上使用了2 个单目视觉传感器,利用2 个子端产生的关键帧共同完成地图构建。 基于相似的思路,Zou 等[5]提出了CoSLAM,将多视觉传感器协同的数量规模扩张到了12 个。 然而随着参与协同的传感器数目增多,CoSLAM[5]的算力压力较大,并且对通信系统的带宽具有较高的要求。 Riazuelo 等[6]通过引入云端服务器,使子端将关键帧上传至云端,在云端服务器上完成局部地图的构建和子端地图融合,改善了系统的计算压力和带宽压力。 Forster等[7]使用了减少数据传输量的方法来缓解通讯压力,只发送检测出的特征点和关键帧帧间的相对位姿。 通过比较子端和中心端计算的关键帧位姿之间的差异来恢复绝对位姿。

本文提出了一种大范围场景下的多无人系统协同定位与建图方法,利用多个无人系统进行探索,同时每个子端进行同时建图与定位过程,建立稀疏点云地图。 之后,根据区域重叠等公共信息进行局部地图的融合,建立全局的稀疏点云地图,完成协同定位与建图功能,在提升定位精度与效率的基础上建立一个全局一致的环境地图表示,以提升无人系统集群的智能程度。

2 多无人系统协同定位与建图系统

2.1 系统整体框架

采用中心式架构,将系统分为两部分:子端和中心端。 每个子端搭载一台单目相机、通信单元和机载计算机;中心端和子端进行数据交互,接收子端传输的关键帧,地图点等数据,完成回环检测,地图融合,全局优化等非实时操作;并将优化后的数据发送回子端,指导子端的导航系统。 整体的系统结构如图1 所示。

图1 算法框架图Fig.1 Diagram of the algorithm framework

1)所有智能体子端都运行一个独立的前端视觉里程计,维持一个规模较小的局部地图并将关键帧和地图点发送给中心端,中心端接收各子端信息,形成各子端的历史地图。

2)中心端通过从所有子端处获得的关键帧增量式构建了一个关键帧数据库。 用于存储所有接收到的关键帧的信息,中心端还建立了一个包含了所有子端局部地图的地图堆栈,不同子端地图的构建独立进行,互不干扰。 重叠区域检测过程不断进行,新接收的关键帧通过基于字典机制的相似度检测算法不断检测子端是否位于过去到达过的区域,重叠区域包括该子端过去到达过的区域和其他子端到达过的区域。 子端内部重叠,则进行回环检测过程;子端之间重叠,则通过匹配的关键帧建立两子端局部地图间的数据关联,进一步进行地图融合,地图融合后这2 个地图被合并,原有的局部地图从地图堆栈中删除,而由它们的融合产生的一个新的全局地图将被添加到地图堆栈中,并与2 个子端直接关联。 最后根据得到的全局地图进行全局优化。

3)在全局优化结束后,中心端会将优化后的相机位姿发送回对应的子端,子端会更新位姿并将其进行局部优化,从而更精确的完成协同SLAM 过程,并获得更准确的导航结果。

2.2 子端算法

子端的主要功能是运行视觉里程计,并将跟踪的路标点进行存储,组成局部地图,按照设定好的数据结构将信息发送给中心端。 这部分基于ORB-SLAM2[8]进行开发,如图 2 所示。 由于子端局部地图的大小会随着运行时间而增长,大范围协同SLAM 一般都需要长时间运行,这就对子端的存储能力提出了挑战。 由于中心端的地图堆栈中包含了每个子端的完整地图,因此在通信系统畅通的情况下,子端无需保留过多地图信息。 本方法对各子端建立的局部地图中关键帧的数量进行限制,以降低在大范围探索任务中子端机载处理器的存储压力和计算压力。

图2 跟踪线程框架图Fig.2 Block diagram of the tracking process

设定子端局部地图中关键帧数目的上限为N,一旦关键帧数目超过N,就去除旧的关键帧,同时将新创建的关键帧添加进局部地图,为了防止出现中心端没有接收到被删除的关键帧而造成数据丢失的现象,在删除关键帧之前,子端会和中心端进行一次数据交互,确认中心端是否接收到该关键帧,确认后再进行删除,流程如图3 所示。

图3 子端局部建图流程Fig.3 Process of the local mapping

2.3 中心端算法

不同子端局部地图之间的参考坐标系关系如图4 所示,假设有子端a,子端b,子端a 的参考坐标系为worlda,子端b 的参考坐标系为worldb,ki为子端a 创建的一个关键帧,它在子端a 的局部地图中的位姿为Ta,它在子端b 的局部地图中的位姿Tb=TabTa。 因此要完成子端a,b 之间的地图融合,就需要得知从worlda到worldb的位姿转换关系Tab。 如果子端a 和子端b 在观测过一处公共区域,那么2 个局部地图之间就存在着重叠区域,可以利用其完成地图融合与全局优化,然而,若2 个子端没有观测过公共区域,那么在没有其他信息的情况下无法完成两地图之间的数据融合。

图4 参考坐标系转换关系Fig.4 Conversion relation of reference coordinate system

由于局部地图之间重叠部分的地图点所对应的特征描述子之间相似度较高。 而系统创建了一个关键帧数据库,其中存储了所有子端发送的关键帧,因此,可以为关键帧数据库建立一个字典树,在整个地图堆栈中检索有无关键帧匹配,检测到匹配后利用匹配关系解算地图间的相对位姿关系,实现全局地图融合与优化。 下面介绍基于重叠区域检测的地图融合优化算法。

首先,进行重叠区域检测。 遍历新关键帧k的所有共视关键帧,计算k与每个共视关键帧的词袋向量相似度,并得到当前帧与其共视关键帧相似度的最低分minScore,在关键帧数据库中寻找与k相似度超过minScore的关键帧,称为候选匹配关键帧,接下来在候选匹配关键帧中检测具有连续性的候选帧,连续次数大于3 则认为成功匹配,将所有符合条件的关键帧组合起来,就得到了候选关键帧组。

然后,利用求解出的匹配关系进行地图之间相对位姿的计算。 ①遍历匹配候选关键帧集合,筛选出与当前帧的匹配特征点大于20 的候选帧集合;②每一个候选帧都构造一个位姿求解器进行迭代求解;③本文的位姿求解算法使用三维相似变换算法(Similarity,Sim3)[9],常用于求取2 组匹配点之间的位姿变换关系。

利用Sim3 转换通过匹配的地图点之间的关系求解出两关键帧间的相似变换关系,假设当前帧到匹配帧的坐标变换矩阵为Tmc,逆变换矩阵为Tcm,当前关键帧坐标系下的地图点Pc对应的图像像素点为pc,同理,有候选帧坐标系下与地图点Pc匹配的地图点Pl及其对应的图像像素点pl,则重投影误差为:

设置两个重投影误差阈值分别为E1和E2,若elc ＜E1且ecl ＜E2,则该组匹配构建的三维点被称为内点。 当内点数量满足要求时,视为该候选帧匹配成功。 当有一个候选帧匹配成功,成功解算出相对位姿关系后,为了优化这个初步解算出的位姿转换关系,通过将匹配关键帧以及其相连关键帧的地图点都投影到当前关键帧进行匹配,以构造更多的匹配关系来进一步优化Sim3 变换,直到优化后的内点数目符合要求后,则视为相似变换解算成功。

最后,进行地图融合与优化。 由于上一步中计算Sim3 转换改变了当前关键帧的地图点,因此需要根据共视关系更新当前帧与其他关键帧之间的连接以及其他关键帧的地图点,对冲突的地图点进行替换或填补。 然后将匹配关键帧组的地图点投影到局部关键帧组上进行地图点检查与替换,更新关键帧之间的共视相连关系,得到因匹配时地图点融合而新得到的连接关系。 接下来进行本质图优化,优化因匹配而更改的连接关系,最后添加当前帧与闭环匹配帧之间的边,进行全局优化,基于地图点到关键帧的投影关系,优化所有的地图点和关键帧的位姿。 算法流程图如图5所示。

图5 基于重叠区域检测的地图融合优化算法流程Fig.5 Algorithm flowchart of Map Fusion and optimization based on overlapping region Detection

3 实验结果

3.1 数据集和测试指标

由于目前少有针对多机SLAM 的测试数据集,因此常应用EuRoC、TUM 等单系统于单一环境以不同轨迹收集的多个数据集应用于多机协同SLAM 的性能测试。 EuRoC[10]是SLAM 领域常用的测试数据集,常用于SLAM、VIO 等系统的性能测试。 EuRoC 中包含一个在小型工业厂房中收集的数据集,该厂房如图 6 所示,厂房结构较复杂,特征丰富,适合用来对SLAM 算法进行性能测试。 该数据集中MH01、MH02 和MH03 是由无人机在光照条件较为良好的情况下进行低速飞行采集到的3 组数据集,算法测试难度适中。 由于厂房面积较小,因此这3 组数据集存在大量的重叠关系,为多机协同SLAM 的地图融合创造了条件。本文使用MH01,MH02 和MH03 数据集作为3 个无人系统子端对系统进行测试,用Rviz[11]来进行数据可视化。

图6 厂房图Fig.6 Machine hall overview

SLAM 系统性能测试中常用的精度指标为绝对轨迹误差(Absolute Trajectory Error,ATE),本文选取绝对轨迹误差ATE 作为评测指标。

3.2 实验结果

本文首先建立系统中心端和3 个无人系统子端,分别接收MH01,MH02,MH03 等3 组数据集发送的图像数据,SLAM 如图7 所示。 其中绿色的轨迹和点云是MH01 中相机的轨迹估计结果和构建的点云,白色的轨迹和点云是MH02 中相机的轨迹估计结果和构建的点云,蓝色的轨迹和点云是MH03 中相机的轨迹估计结果和构建的点云。 可以发现,3 组独立的飞行过程通过本文的协同SLAM 系统完成了数据关联,得到了各自轨迹的相对位姿关系,并共同构建了一个全局一致性表征的稀疏点云地图。 最终的轨迹估计结果如图8 所示。

图7 SLAM 结果图Fig.7 Diagram of SLAM result

图8 SLAM 轨迹图Fig.8 Diagram of SLAM trajectory

为了对比本文提出的协同SLAM 方法的性能,以ORB-SLAM2 作为基准方法,对比协同情况下轨迹估计值的误差和单独进行SLAM 情况下轨迹估计值的误差。 使用SLAM 测评工具EVO 测试了轨迹跟踪情况,使用ORB-SLAM2 单独进行SLAM 过程的轨迹跟踪结果和应用本文方法进行协同SLAM 过程的轨迹跟踪结果如图9 所示,其中图9(a),图(b)是在MH01 数据集上的轨迹跟踪结果,图9(c),图9(d)是在MH02 数据集上的轨迹跟踪结果,图9(e),图9(f)是在MH03 数据集上的轨迹跟踪结果,轨迹跟踪情况较好。

图9 本方法和ORB-SLAM2 对各数据集的轨迹结果Fig.9 Trajectory results of ORB-SLAM2 and our methods for various datasets

ORB-SLAM2 算法和本方法在3 组数据集上的绝对轨迹误差对比结果如表1～表3 所示。 由表可知,2 种算法在数据集中的误差都较低,在MH01 数据集中ORB-SLAM2 的误差均值为0.07 m,本方法的误差均值为0.071 m,与ORBSLAM2 持平,在MH02 数据集中ORB-SLAM2 的误差均值为0.042 m,本方法的误差均值为0.035 m,比ORB-SLAM2 小0.007 m,在MH03 数据集中ORB-SLAM2 的误差均值为0.088 m,本方法的误差均值为0.076 m,比ORB-SLAM2 小0.008 m,但本方法在MH01 中出现了离群点现象,最大误差较高。 最大误差,最小误差,误差中值,均方误差等其他各项指标均与 ORB-SLAM2持平。 因此可以得出结论,利用本方法进行协同SLAM 的效果与ORB-SLAM2 算法单独进行SLAM 的效果基本持平。

表1 MH01 实验结果Table 1 Results of MH01

表3 MH03 实验结果Table 3 Results of MH03

为验证本文对各子端局部地图中关键帧的数量进行限制的方法的有效性,本文通过设定不同的子端局部地图中关键帧数目的上限值,进行了实时性测试。 测试在处理相同数据时,单帧跟踪时间、建图单帧迭代时间、单次数据通信时间在不同N值情况下的数值,仿真结果如表4 所示。

表4 不同N 值下的实时性测试实验结果Table 4 Real time test results for different N values

由实验结果可以看出,单帧跟踪时间、建图单帧迭代时间、单次数据通信时间都随N值增加而增加,其中建图时间增加幅度较大,这是因为N值增加直接使得局部地图规模增加,线性增加建图成本。 跟踪和通信时间由于子端建图部分占用了更多的算力,产生了更多的数据交互而速度降低,因此验证了本文通过限制子端局部地图中关键帧的数目可以有效提升算法的速度,减少子端的内存压力和计算压力。

4 结论

1) 提出了一种协同视觉SLAM 系统。 采用中心式的架构进行设计,对子端局部地图的大小进行限制,并设计了基于公共区域识别的地图融合算法,最终得到误差较小的位姿估计和全局一致性表示的全局地图。

2) 完成了数据集实验测试。 证明了本方法的精度略优于ORB-SLAM2 单机SLAM 的精度,证明了本方法可以在大范围环境下完成无人系统协同视觉SLAM。