分布式多车协同视觉SLAM系统*

蒋朝阳，兰天然，郑晓妮，高九龙，叶学通

（北京理工大学机械与车辆学院，北京 100081）

前言

定位与导航是自动驾驶技术的关键组成部分［1］。视觉SLAM技术能在没有环境先验信息的情况下，建立环境模型，同时估计运动［2］。目前相关研究成果丰富［3-5］，已经应用于智能车辆领域［6］。但该方案的误差累计问题严重，难以长期保证准确定位。基于多车协同［7-8］的视觉SLAM技术相较于单车SLAM定位更加精准，在大型场景、长时间定位任务中具有广泛应用价值。

视觉传感器因其硬件廉价、结构简单、安装方便、体积小、质量轻、特征丰富等优点成为SLAM系统的重要传感器之一。目前已经有众多视觉SLAM方案被提出，例如ORB-SLAM3［9］、VINS-Mono［10］等。基于视觉的协同SLAM也得到了一定的研究和发展，然而大多数研究方案为了减轻带宽要求往往采用集中式框架，例如CCM-SLAM［11］、CVI-SLAM［12］、COVINS［13］等。其劣势在于数据隐私性差，对服务端的计算能力要求较高。这种框架不适用于自动驾驶车辆独立完成定位建图任务。分布式系统能够更好地解决集中式系统所存在的问题，对于单个节点的加入与退出均具有较强的鲁棒性和适应性。自2013年以来，分布式协同SLAM已经取得了部分研究成果［14-15］，但仍存在受限于带宽的问题［16］。

现有的协同SLAM方案通常利用多车间的共视区域识别（visual place recognition，VPR）实现数据关联。词袋法被广泛应用于该项任务，但视觉传感器易受光线、环境等影响，基于词袋的识别方法容易产生假阳性闭环。Cieslewski等［17］利用基于端到端训练的NetVLAD描述子［18］实现了共视区域识别，提升了系统对于光照变化的鲁棒性，降低了协同SLAM系统的数据交换量。该方案在DOOR-SLAM［19］中也得到了应用。

多车共视区域识别通过当前关键帧与多车关键帧库中图像描述子的匹配实现。面对具有大量相同特征的环境，基于图像相似性的识别方法易出现假阳性闭环。因此众多研究者通过闭环离群值剔除，保证正确数据关联。Agarwal等［20］为协方差矩阵增添尺度因子实现协方差矩阵动态缩放，改变闭环在优化过程中所占的比重。一种寻找一致性最大团的离 群 值 剔 除 算 法［21］在DOOR-SLAM［19］、DiSCo-SLAM［22］等分布式协同SLAM系统中均有体现，但是相对位姿变换估计精度有限，错误的识别结果也可能满足一致性。

多车协同SLAM利用共视区域识别结果实现多车数据关联，增加多车间相对位姿约束，实现协同定位精度增强。目前广泛应用于分布式协同SLAM位姿图优化的算法是由Choudhary等［23］所提出的两阶段高斯赛德尔算法（DGS），该方案第一阶段估计旋转分量，第二阶段利用第一阶段的估计结果恢复完整的位姿。但该方案需要求解非凸优化问题，算法收敛速度受初始值的精确性影响，同时该方案仅考虑唯一的相对位姿变换作为约束，在车辆数量增加以及位姿求解不准确时，定位精度和一致性会受到影响。Huang等［22］提出了一种两阶段全局-局部优化算法，该方案提高了整体定位估计的一致性，但是在共视闭环约束数量较少时无法进行优化。

针对自动驾驶应用场景，为了解决现有协同视觉SLAM中存在的环境适应性差、假阳性闭环多以及多车协同优化约束单一等问题，本文提出了一种分布式、数据传输轻量、鲁棒的多车协同视觉SLAM系统。该系统有如下特点：

（1）通过引入NetVLAD全局图像描述子，实现多车间共视区域识别，提升视觉系统对于光照变化的鲁棒性，减轻数据交换量，降低分布式系统对于带宽的要求；

（2）提出并引入一种新的基于数据相似性和结构一致性的多车间闭环离群值剔除算法，保证协同定位过程的正确数据关联；

（3）在现有的DGS优化算法［23］的基础上，提出了一种改进的DGS优化算法（multi-closure DGS，MC-DGS）。该算法基于共视区域闭环构建多车间相对位姿变换约束，在闭环数量增多时，利用历史闭环信息构建里程计约束，通过位姿图优化求解，提升了多车协同定位效果；

（4）通过自动驾驶测试平台和开源数据集KITTI进行实验，验证了系统的有效性。

1 分布式协同视觉SLAM系统概述

多车协同视觉SLAM定位增强系统原理如图1所示，该系统能够实现以下功能：（1）每辆无人车搭载双目相机，接收图像信息并运行双目视觉里程计；（2）当多车间满足通信范围时，交互图像全局描述子信息并构建多车间共视区域识别闭环约束；（3）对产生多车间闭环约束的车辆进行位姿图优化，提升系统的定位效果。

图1 多车协同定位增强系统原理图

图2展示了本文所提出的分布式多车协同视觉SLAM系统的结构。首先，每辆无人车通过车载的双目相机获取图像，采用ORB-SLAM2的双目模式作为视觉里程计进行位姿估计。与此同时，提取图像的NetVLAD描述子，发送至闭环检测模块实现单车SLAM闭环检测、多车共视区域识别闭环检测，获得满足数据相似性和结构一致性的闭环集合。然后，利用多车通信，交换多车间共视区域识别闭环关键帧的图像特征、特征描述子集合以及由建图模块获得的3D点位置集合，实现多车间相对位姿变换估计。最后，根据由视觉里程计构建的单车视觉里程计约束和由共视区域匹配所产生的多车间位姿约束，执行分布式位姿图优化模块。

图2 分布式多车协同视觉SLAM系统框图

得益于分布式的框架，车辆可以自由地加入、退出协同定位系统。引入通过端到端训练的NetVLAD图像描述子，系统能够一定程度适应光照变化，并且传输一帧图像描述子的大小仅为1 Kb，减轻了带宽负担。闭环检测模块同时检验数据相似性和结构一致性，避免了只考虑单一因素所造成的假阳性闭环，保证正确数据关联。分布式位姿图优化模块利用本文所提出的MC-DGS算法，在单车里程计约束的基础上，增加了多车间共视区域闭环约束以及两次闭环所构成的里程计约束，优化求解位姿，得到更加精准的位姿估计。

2 多车间共视区域识别模块

为了保证在有可能感知混叠的情况下实现更加精准的多车间共视区域识别，本文采用了对光照鲁棒的、轻量的全局图像描述子NetVLAD。此外，提出一种基于数据相似性和结构一致性的闭环离群值剔除方法，由此保证多车间数据关联的鲁棒性。

2.1 基于NetVLAD描述子的共视区域识别模型

图像特征对于光照的鲁棒性由图像描述子决定。NetVLAD全局图像描述子是基于卷积神经网络实现场景识别的先进性结果。通过端到端训练，能够在一定程度上应对光线变化，弥补传统图像描述子的不足，提高视觉系统对于光照变化的鲁棒性。与此同时，NetVLAD描述子十分紧凑，采用描述子前128维描述一帧图像即可良好地完成共视区域识别任务，大小仅占用1 Kb。应用于本系统中，能够降低数据交换量，适用于带宽受限的分布式协同SLAM系统。

基于文献［17］的研究结果，本文采用128维的NetVLAD描述子实现多车间的共视区域识别功能。对于i时刻来自车α的图像描述子vαi以及j时刻来自车β的图像描述子vβj，满足成为多车间共视区域闭环匹配候选集的条件是描述子间的欧式距离小于相似度阈值τNetVLAD：

相似阈值越低，本步骤所获得的闭环候选集中的匹配对数量就越少。考虑到本文引入了闭环离群值剔除模块，因此可以设置较为宽松的相似度阈值，在本文实验中，该阈值被设置为0.2。

2.2 几何验证模型

几何验证模型用于初步实现多车间闭环离群值剔除以及多车间的相对位姿变换计算。对于候选闭环关键帧匹配对，提取视觉特征、视觉特征描述子以及相应的3D点位置估计信息，利用PNP算法以及RANSAC算法计算产生闭环匹配的两车间的相对位姿变换矩阵，同时返回一组内点。如果内点的数量足够大，认为本次闭环是比较成功的。通过设定内点数量阈值可以初步筛选部分闭环，在本文实验中，该项阈值被设定为20。由几何验证步骤可以计算出在多车间的相对位姿变换估计值，因此本文所提出的多车协同定位系统无需事先测量多车间的初始位置关系。

2.3 闭环离群值剔除模型

最大成对一致性（PCM）［21］在协同SLAM闭环离群值剔除领域得到了广泛的使用和认可，但是基于两帧图像所计算的相对位姿变换并不完全准确，错误的闭环也可能具有成对一致性。因此本文提出了一种基于数据相似性和结构一致性的多车间共视区域闭环离群值剔除方法。利用ORB-SLAM2在视觉里程计部分所构成的共视帧关系，验证闭环帧的数据相似性，利用PCM算法，验证闭环帧的结构一致性，筛选同时具备两项性质的闭环关键帧，避免产生由于感知混叠和位姿估计不准所导致的假阳性闭环，保证多车间实现稳健的数据关联。这里，可以直观地认为相似度高的图像成为闭环的可能性更大。

由PCM算法［21］可知，对于两车之间所构成的共视区域闭环帧应满足以下关系：

式中：‖‖⋅Σ代表马氏距离；zαiαk表示无人车α由k帧到i帧的相对位姿变换矩阵；zαk βl代表由无人车β坐标系下的l帧到无人车α坐标系下的k帧的相对位姿变换矩阵，其他符号具有类似的含义；γ代表PCM成对一致性检测阈值。阈值越低表明对于闭环的要求越严格，反之越宽松。

记c(zi，zk)表示为一对多车共视区域闭环的结构一致性评分，评分越高，代表闭环的结构一致性越强。假设当前车辆为α，其中zi表示当前车辆在i帧所产生的多车间闭环<αi，βj>，即车辆α在i帧与车辆β在j帧经过了同一地点，构成了多车间闭环。同理，zk表示当前车辆在k帧所产生的多车闭环<αk，βl>。

ORB-SLAM2前端将关键帧以簇的形式进行管理。当两个关键帧所观测到的相同地图点满足一定数量要求时，构成共视关键帧的关系。其中，共视程度最高的共视关键帧被称为该关键帧的父关键帧。同时，单车内闭环关键帧与当前帧的相似度应高于当前帧与其父关键帧间的相似度。基于上述关系，文中假设正确的多车间闭环具有以下特征：与当前帧具有较高的相似度，与图像的父关键帧（或者单车内闭环关键帧）也具有较高的相似度，其关系如图3所示。

图3 基于数据相似性计算相似度评分原理

假设当前车辆为α，对于多车间闭环约束zi=<αi，βj>，计算关键帧βj与当前车辆闭环关键帧αi的父关键帧（单车内闭环关键帧）之间的相似度评分s(zi)：

同理，对于多车间闭环约束zk=<αk，βl>，可以计算得到s(zk)。相似度评分越高，代表图像越相似，正确闭环概率就越高。

综合考虑数据相似性和结构一致性，定义多车间共视区域闭环质量评分，由一对无人车间闭环候选可计算出闭环质量评分m(zi，zk)：

图4（a）表示无人车α与无人车β间的闭环候选集合，图4（b）表示闭环两两间质量评分所构成的评分矩阵，该邻接矩阵是一个对称矩阵，图4（c）表示由闭环关系和质量评分所产生的无向加权图，处于最大团中的闭环将被最终确定为用于位姿图优化的正确多车间闭环。

图4 闭环质量评分矩阵与最大团原理

多车间闭环产生于经过同一地点，因此闭环关键帧需具备数据相似性。同时，由于视觉里程计约束，闭环帧需要具备结构一致性。因此，正确的多车间闭环兼备两种性质，考虑满足两种性质的可能性，能够提高筛选闭环关键帧的正确率，优于考虑单一因素。

设置m(zi，zk)阈值可以筛选同时具备数据相似性和结构一致性条件的闭环，阈值越大，闭环要求越严格，数量越少，正确率越高。该方案容易计算，并且在不需要额外通信负担的情况下，综合利用了视觉里程计前端提供的数据相似性结果以及多车间闭环所构成的结构一致性结果。

3 分布式位姿图优化模块

这里提出一种改进的DGS算法MC-DGS实现多车协同定位优化。在构建多车间相对位姿变换约束的基础上，考虑历史多车间闭环信息，构建基于两次闭环过程的里程计约束，降低不精准的相对位姿变换估计对优化过程的影响，进一步提高位姿优化精度和估计一致性。

3.1 单车SLAM优化建模

首先，考虑单车视觉SLAM问题的表现形式，定义时间0到t范围内6自由度姿态为X={x0，x1，⋅⋅⋅，xt}，X⊂SE(3)。定义观测相对位姿变换矩阵zij和期望相对位姿变换矩阵̂之间的误差eij为

式中：I4×4表示4阶单位矩阵；Rij表示旋转分量；tij表示平移分量；‖‖⋅F表示Frobenius范数。则单车SLAM问题可以简化为一个最小二乘问题：

式中：Fij表示位姿xi与xj之间的约束；Ωij表示协方差矩阵。通过求解式（9），得到一组最优的位姿轨迹，可以实现无人车辆的自主定位。

3.2 多车协同SLAM优化建模

假设多车协同SLAM中有N辆无人车参与，N={1，2，…，n}。对于无人车α⊂N，其位姿可以表示为Xa，所有车辆的位姿集合表示为Xall={Xa∣α∈N}。对于多车协同SLAM系统，希望解决以下问题：

式中：Fintra(Xall)表示所有单车SLAM约束所构成的代价；Finter(Xall)表示所有多车间约束所构成的代价，通过最小化两种代价，获得多车间两两一致的位姿估计结果，提高定位精度。

式中：Cα表示无人车α内部约束的集合；Cαβ表示无人车α和β之间的多车间约束集合；表示产生约束的两个关键帧。因此对于每一辆参与协同定位的无人车，如无人车α，可以构建包含内部约束以及外部约束的优化问题，如式（13）所示：

区别于单车SLAM，本系统考虑多车间约束，建立以下模型：

式中：R∈和t∈分别代表旋转和平移噪声，并假设平移噪声符合信息矩阵为零均值的高斯分布，旋转噪声遵循参数为的冯-米塞斯分布；‖‖⋅F表 示Frobenius范数［24］。

3.3 MC-DGS优化算法

区别于DGS算法［23］，本文考虑历史闭环信息对于提高优化精度的作用，提出了一种改进的DGS算法，即MC-DGS。当一对无人车间存在不止一对闭环时，该方案能够利用历史闭环信息，提高位姿估计的一致性和精准性。

由多车间闭环约束<αi，βj>、<αk，βl>可以建立以下模型：

每确定一个多车间闭环可以计算出两帧间的相对 位 姿 变 换 矩 阵，如Rαi βj、Rβlαk。对 于 闭 环 约 束<αk，βl>利用已优化的Rαi、Rβj构造视觉里程计约束<αi，αk>。对于无人车α和无人车β，分别解决以下线性最小二乘问题，完成算法第一阶段旋转估计：

算法第二阶段代入旋转估计并求解式（16），获得完整位姿估计。当不存在两次多车间闭环时，Rαi、Rβj、Rαi βj置位为单位矩阵，第一阶段变为求解式（21），与DGS算法的第一阶段相同，不影响定位优化过程。

良好的初始值以及优化顺序将会加快求解的过程。本文采用一种选择性排序的方式加快求解。系统根据此次通信过程中多车间闭环的数量进行排序，首先解决闭环约束少的无人车定位优化问题，再解决闭环约束多的无人车定位优化问题，为优化过程提供良好的初始值。

4 实验

4.1 实车测试

利用自主搭建的自动驾驶测试平台实现校园内道路环境数据采集。通过双目相机采集图像，利用一个高精度、实时的GNSS/INS组合导航系统（Novatel PwrPak7D-E1）采集车辆运动轨迹作为定位结果参考值，构建多车协同SLAM数据集，对本文所提出的系统进行验证。图5所示为本文所采集的校园道路环境场景示例。

图5 校园道路环境真实数据

在实验环节，本文利用Oxford RobotCar数据集中的部分序列进行NetVLAD特征提取，用于完成NetVLAD描述子聚类，实现单车闭环检测以及多车间的共视区域识别。实验过程采用128维的NetVLAD描述子描述图像，其表现形式如图6所示。由于本文重点验证所提出系统的定位效果提升性能，因此实验环节采用离线的方式获得定位优化结果，不考虑多车通信时延对定位效果的影响。

图6 图像的NetVLAD描述子表现形式

图7展示了不同方法下的3车协同定位结果。虚线代表轨迹真值，实线代表估计轨迹。不同颜色的线段分别代表不同车辆的运行轨迹。本文系统的估计轨迹闭环状态良好，估计误差小，与真实轨迹基本重合，体现出更好的定位精度。

图7 校园道路环境下不同协同定位方法比较

针对不同长度的校园真实数据测试序列进行测试，图8所示为两段校园真实数据在卫星地图上的投影以及基于3车协同的定位结果。从图中可以看出，估计轨迹与真实轨迹基本重合，定位精度高，验证了系统的有效性。

图8 校园道路数据集卫星投影及协同定位结果

由于真实数据采集场景限制以及实验硬件平台数量有限，本文所提出的协同视觉SLAM系统相较于单车SLAM以及其他协同SLAM系统所体现的定位精度优势并不突出，因此，利用开源自动驾驶数据集KITTI进一步实验，证明本文系统的精度效果、轻量性以及环境适应性。

4.2 KITTI数据集测试

首先针对不同算法步骤下的协同定位进行实验，证明本文系统每个模块的有效性。其次，基于多个序列测试，对比现有的单车视觉SLAM方法以及协同视觉SLAM方法，证明本文所提出的系统具有更好的精度效果和环境适应性。然后，测试协同定位过程中的数据交换量，证明系统的轻量性。最后，提升测试协同车辆数量，证明系统的通用性和可扩展性。

为了模拟测试多车协同视觉SLAM过程，对KITTI数据集的原始序列进行了一定的处理，以适应本文所述的系统。根据需要模拟的无人车协同数量，将原始数据进行分割和修改，时间戳调整为部分重叠以形成共视区域。例如，构建3车协同测试数据，将原始序列分割成长度大致相同的3段，每段数据在时间上保证部分重叠。上述方法在DOORSLAM［19］和DiSCo-SLAM［22］中均有体现，被广泛应用于协同SLAM测试。

图9所示为不同算法步骤下的3车协同定位结果。图9（a）为仅进行DGS优化的轨迹估计结果，由于未进行多车间闭环离群值剔除，定位轨迹合并错误，误差较高。图9（b）为PCM闭环离群值剔除后再进行DGS优化的轨迹估计结果，轨迹合并正确，但部分轨迹出现明显误差。图9（c）为PCM闭环离群值剔除后再进行本文所提出的MC-DGS位姿图优化算法所得到的估计轨迹，进一步减小了定位误差。图9（d）为本文系统的完整实现，轨迹真值和估计轨迹基本重合，轨迹合并效果较好，定位精度高。表1为4种不同算法步骤处理下的轨迹绝对误差（APE）最大值、平均值以及RMSE误差结果。通过比较可以得出，在KITTI 00序列上本文所提出的协同视觉SLAM系统具有优越的定位精度结果。

图9 不同算法步骤处理下的协同定位结果

表1 不同算法步骤下的APE误差对比

基于城镇道路环境的KITTI 00序列，本文系统与两种主流视觉SLAM系统以及分布式视觉协同SLAM系统进行了绝对位姿误差的RMSE误差对比，测试均采用双目视觉模式。由于多数协同SLAM算法并不开源，因此仅从论文中获得了算法在KITTI 00数据集上的定位精度，误差比较结果如表2所示，本文所提出的系统具有更高的定位精度。

表2 现有单车、协同视觉SLAM算法误差对比

带宽受限是分布式协同SLAM系统中的关键问题。为了减轻带宽要求，除了采用轻量的NetVLAD描述子外，也限制了两次多车间相对位姿变换计算的间隔帧数，这意味着部分闭环因为带宽受限而被舍弃。经过实验表明，在降低了数据传输的情况下，该系统仍然能够保证较高的定位精度。图10所示为本系统3个主要功能模块数据传输量随运行时间的变化。

图10 系统各个模块的数据传输量变化过程

基于KITTI 00序列测试，由多车分布式位姿图优化所产生的数据传输量为0.96 MB，由共视区域识别所产生的数据传输量为0.61 MB，由多车间相对位姿估计所产生的数据传输量为1.19 MB，总计2.76 MB，大大降低了分布式系统的带宽限制。

为了证明本文所提系统的有效性和环境适应性，本文针对KITTI数据集不同环境、长度的序列进行了无协同模式以及3车协同模式定位精度对比测试，结果如表3所示。通过图11，可以更直观地看出本文所提出的协同SLAM系统相较于无协同SLAM系统的优越性。

图11 无协同模式与3车协同模式定位效果对比

表3 不同环境、长度测试序列RMSE定位误差对比

为了测试面向更多数量车辆的协同SLAM系统定位表现，模拟了6辆无人车进行基于KITTI 00序列的协同定位，并绘制了每辆无人车的APE变化曲线。如图12所示，该曲线变化与文献［17］所述系统有类似的趋势和结论。初始预测轨迹包含着轨迹的漂移误差，随着时间推移，预测轨迹误差逐渐增大，当多车间产生共视区域识别关联时，定位轨迹合并、优化，误差逐渐降低，最终稳定在2 m左右。由文献［17］的结果可知，其APE变化峰值达到26 m左右，最后稳定在4 m左右，平均值达到4.8 m。本文系统在相同序列上定位表现更加优越，APE的变化峰值不超过8 m，最后稳定在2 m左右，平均值为1.91 m。从精度评估上，本文所提出的系统显然具有更好的定位精度。

图12 每辆无人车定位误差随时间的变化

此后，在长序列KITTI 00、KITTI 08上分别采用了6、10辆无人车进行协同SLAM测试，其估计轨迹与真值轨迹的对比如图13所示。本文所提出的协同视觉SLAM系统在长序列、协同车辆增多的情况下，估计轨迹与真实轨迹基本重合，定位精度较高，证明系统具有良好的通用性和可扩展性。

图13 不同数量无人车协同轨迹估计结果

5 结论

本文提出了一种轻量化、鲁棒、定位精度更高的分布式多车协同视觉SLAM系统。通过实车采集数据测试以及公开数据集测试，可以得出以下结论：

（1）本文利用NetVLAD描述子实现共视区域识别，能够提升环境适应性，降低数据传输量。系统在城镇、高速、校园道路等多个环境测试中体现出良好的适应性和定位精度。对于长度为3.7 km的轨迹降低APE误差至2.25 m，总数据传输量仅为2.76 MB。

（2）本系统设计添加了基于数据相似性和结构一致性的闭环离群值剔除模块，相较于其他协同定位系统，保证了多车数据关联的可靠性，能够实现正确的轨迹合并与优化。

（3）本文所提出的结合历史闭环信息的MCDGS算法，在多车间闭环数量增多时，能够明显提高协同系统的定位精度，优于现有的DGS优化算法。

（4）经过实验验证，本系统在实车实验以及KITTI数据集测试过程中，均体现了优于现有单车SLAM以及协同SLAM系统的精度效果。