一种低数据传输的多机器人实时视觉SLAM

2021-09-28段胜青崔雨晨周帅琳

导航定位与授时 2021年5期

段胜青，熊智,2，赵耀，崔雨晨，周帅琳

(1.南京航空航天大学自动化学院，南京 211106；2.先进飞行器导航、控制与健康管理工业和信息化部重点实验室，南京 211106)

0 引言

多机器人系统相比于单个机器人系统在执行任务时具备更高的效率和鲁棒性，可应用于森林/城市救火、自然灾害搜救、水下/空间探测和安全巡检等任务[1]。多机器人同步定位与构图(Multi-Robot Simultaneous Localization and Mapping，MR SLAM)技术作为多机器人系统的重要组成部分，承担着在全球定位系统(Global Positioning System，GPS)卫星拒止环境下的定位和导航的关键作用。由于相机具有成本低、质量小等特点，以相机为核心的视觉SLAM技术近年来得到了广泛的研究和应用。目前，单个机器人SLAM技术已经相对成熟，多机器人SLAM的主要瓶颈在于通信带宽限制和计算负载，而多机器人SLAM的数据交换和计算主要集中在回环检测和后端优化阶段。

回环检测是多机器人SLAM的重要基础。在SLAM研究的古典时期(classical age)[2]，通常采用激光角点作为路标，并采用最大似然估计的方式对路标进行数据关联[3-4]，此类方法需要持续共享信息矩阵，传输数据量与状态量个数呈平方增长，数据传输量大；此外，由于路标特征简单，容易发生错误关联，因此此类方法适用于人工环境，对于复杂的自然环境鲁棒性较差。A.Howard[5]和L.Carlone[6]等采用机器人携带人工标签的方式实现多机器人的检测识别和相对位置量测，特殊的人工标签能有效解决回环检测中错误关联的问题，且仅在检测到回环时交换数据，但要求机器人在执行任务过程中必须相遇，具有较大使用限制。随着图像处理技术的进步，基于特征点的回环检测被应用于单机器人SLAM和多机器人SLAM。P.M.Newman在2006年提出了一种基于尺度不变特征变换(Scale-Invariant Feature Transform，SIFT)特征点的回环检测方法[7]，通过聚类的方法，采用视觉单词对场景进行描述，从而实现实时地回环检测。D.Gálvez-López提出了一种基于FAST(Features from Accelerated Segment Test)特征点和BRIEF(Binary Robust Independent Elementary Features)描述子的词袋模型[8]，实现了更高的实时性，被应用于一些主流的SLAM系统[9-10]。基于词袋模型的回环检测虽然能够减少数据的传输，但准确率有待提高。近年来，神经网络凭借其更好的准确性和鲁棒性开始被应用于SLAM回环检测。S.Choudhary等通过YOLO(You Look Only Once)神经网络生成视觉单词进行回环检测[11],但以YOLO为代表的用于分类识别的模型主要关注图片的局部特征，信息利用率较低。此外，刘强等指出图片中的环境建筑等背景对于现有网络模型通常属于非感兴趣区域[12]，不利于回环检测。R.Arandjelovic等根据传统方法中的局部聚合描述子向量(Vector of Local Aggregated Descriptors，VLAD)设计了一种 NetVLAD神经网络结构[13]，并以GPS信息为标签进行弱监督训练，所生成的描述子包含了图片的全局信息，相比于传统方法能以较短的描述子实现更高的准确率，可有效降低数据传输，比其他网络更加适用于多机器人SLAM系统。因此,本文将NetVLAD应用于多机器人回环检测，但NetVLAD对硬件计算能力有较高要求，难以适用于低成本图形处理器(Graphics Processing Unit，GPU)，硬件资源占用和实时性有待进一步优化。

多机器人SLAM按照后端优化算法可分为基于滤波的多机器人SLAM和基于非线性优化的多机器人SLAM。基于滤波的多机器人SLAM常用的滤波方法有卡尔曼滤波[4,14]、信息滤波[3]和粒子滤波[5-6]，但对于以相机为主要传感器的视觉SLAM系统，大量量测数据造成协方差矩阵和粒子数目增大，数据传输增大，降低了算法的实时性。H.Strasdat等指出，在视觉SLAM领域非线性优化方法相比于滤波方法具有更好的精度和实时性[15]。对于多机器人系统，理论上可以使每个机器人收集其他所有机器人的量测信息，然后通过非线性优化估计出所有状态量，但实际应用中通信带宽和计算资源都是有限的，因此该方法并不实用。A.Cunningham等提出了一种具备分布式数据融合(Decentralized Data Fusion，DDF)功能的平滑与构图方法DDF-SAM[16]，该方法通过高斯消元降低了通信和计算负载。T.Cieslewski等提出了一种基于Gauss-Seidel算法的分布式非线性优化方法[17]，以解决多机器人SLAM通信带宽受限的情况。但以上两种方法本质上是对非线性优化过程中求解正规方程的改进，而SLAM系统仍需要多次传输数据和迭代才能逼近最优估计值，考虑到网络传输的延迟，将会降低系统的实时性和鲁棒性。目前，位姿图优化方法因其数据少和计算快的优点，在多机器人SLAM领域得到了较多的研究和应用[7,18]。

综上，在多机器人SLAM领域的回环检测和后端优化方面，尽管已有很多方法被提出，但无法完全满足实际场景对系统实时性、鲁棒性以及带宽限制的要求。为此，本文将引入NetVLAD神经网络模型，在NetVLAD的基础上提出了一种低数据传输的实时多机器人回环检测方法，并根据现有的非线性优化理论对多机器人后端位姿图优化方法进行分析和改进，以减少通信和计算资源占用，并以ORB-SLAM2为前端实现了一个实时多机器人SLAM系统。

1 系统概述

本文提出系统如图1所示，分为前端、后端和NetVLAD三部分。前端部分机器人本地图片采用ORB SLAM2[9]前端处理，对于外部图片消息采用4.2节算法初始匹配后再采用ORB SLAM2前端处理。后端本地位姿图优化采用ORB SLAM2方法，多机器人位姿图优化和共享采用第5节所述方法。NetVLAD模块用于计算关键帧的局部特征聚合描述符(Vector of Local Aggregated Descriptors，VLAD)向量，根据VLAD的相似度(4.1节)进行多机器人回环帧匹配。

图1 本文系统流程框图

2 背景知识

通常SLAM问题通过概率模型来求解。设状态量X，量测量Z，则有

P(X|Z)=P(Z|X)P(X)/P(Z)

(1)

先验分布P(X)未知时，可看作实数域上的均匀分布，故可忽略。X的最优估计即为

(2)

为了方便描述和分析问题，通常将式(1)和式(2)用因子图的形式描述，节点(Vertex)表示待估计状态量，边(Edge)表示量测约束P(zij|xi,xj)。当式(2)中的概率分布P(zij|xi,xj)服从正态分布时，可通过非线性最小二乘方法进行求解。SLAM系统中的BA优化和位姿图优化均是基于上述原理。

3 多机器人SLAM模型描述

记α、β、γ为机器人的序号，并且α、β、γ可指代任意同一个机器人或不同机器人，状态量xγ0αi定义为关键帧Fαi在机器人γ初始关键帧Fγ0对应相机坐标系下的位姿，记作

(3)

量测量zαiβj定义为当前机器人β的关键帧Fβj相对于机器人α历史关键帧Fαi的位姿，记作

(4)

当α=β时，zαiβj采用ORB SLAM2前端进行求解；当α≠β时，求解方法见本文4.3节。

用ρ(·)表示李群到李代数的运算，量测约束概率模型定义如下

(5)

其中，Σ表示量测模型的方差；eαiβj对应因子图中的一条边。此时

(6)

4 多机器人回环检测

4.1 NetVLAD神经网络模型

NetVLAD是一种专用于位置识别的神经网络模型，相比其他神经网络模型具有更高的准确率，但对GPU资源要求较高。为了减少该神经网络模型对GPU资源占用、提高回环检测速度，本文直接对文献[13]训练完成的NetVLAD模型做出参数调整而无需重新训练。

4.1.1 conv1_1卷积层合并

NetVLAD网络conv1_1层卷积核的维度为3×3×3×64，conv1_1层输出可表示如下

(7)

其中，Oi表示卷积层第i个输出通道；Ic对应输入图像的3个通道；ac∈R为预处理时减去的一个常数；Kic为卷积核；bi为偏置项；⊗表示卷积运算。本文以灰度图作为输入，I1=I2=I3，因此

(8)

可以对conv1_1层进行合并

(9)

最终conv1_1卷积核维度变为3×3×1×64。

4.1.2 WPCA层裁剪

NetVLAD模型将vgg16网络的输出经过NetVLAD core处理后，通过加权主成分分析(Weighted Principal Component Analysis，WPCA)将输出降维至4096维[13]。考虑到PCA矩阵中越靠后的列包含信息越少，本文将WPCA层32768×4096的矩阵裁剪为32768×128维，只保留矩阵的前128列，并将偏置项裁剪为128×1维，从而极大地减少参数数目，降低计算量。

4.1.3 相似度计算

图片U和V根据上面所述改进NetVLAD进行计算，得到归一化128维向量u和v，相似度定义如下

(10)

S(u,v)越大,图片越可能构成回环。

4.2 特征匹配

为减少数据传输，本文所述方法仅发送左目相机中图片特征点像素坐标和对应单词Id，以量测相对于其他机器人历史关键帧的位姿。

机器人在接收到上述图片信息后，由于缺失特征点的描述子信息，采用ORB SLAM2前端会产生大量误匹配(见第6节图4)，导致相对位姿解算失败。为此，本文提出了以下方法用于解决描述子缺失情况下的特征匹配。

设图片i宽和高分别为W和H，词袋为Bi，特征点集合为Pi，对于特征点p∈Pi，对应单词为wp，像素坐标为cp=[xp,yp]T，记τd=(W2+H2)/4，匹配算法描述如下：

通过上述方法可以得到误匹配较少的匹配结果，此后通过RANSAC PnP(Random Sample Consensus Perspective-n-Point)方法求解初始位姿，再由初始位姿通过反投影方式增加匹配点对。

4.3 回环检测

为减少数据传输，回环检测分为五步：

1)计算机器人α新增关键帧图片U的NetVLAD特征向量u，并广播给其他机器人；

2)任意其他机器人β收到u后，与本地NetVLAD特征库进行对比，如果存在特征向量为v的关键帧图片V满足S(u,v)>τmatch(τmatch为相似度阈值)，则将图片V的特征点信息(坐标、单词)发送给机器人α，否则结束回环检测；

3)采用算法1进行特征点匹配；

4)机器人α用RANSAC PnP方法求解U与V的相对位姿zαUβV，如果求解成功，则认为U和V对应同一地点，并广播机器人α和β的关键帧匹配消息；

5)每个机器人通过接收关键帧匹配消息判断自身是否与其他机器人轨迹构成回环或间接回环(例如A↔B↔C↔A)。

5 增量式多机器人位姿图优化

先验分布P(X)未知时，因子图只存在双边约束，可将因子图按照下面的规则分割为多个子图：

1)构成一个环路的节点和边属于同一子图；

2)有至少一条公共边的2个子图应合并为一个子图；

3)不经过任何环路的边和其连接2个节点构成一个子图。

约定依照上述规则所得的子图称为回环子图。记完整因子图为G，回环子图Si∈G，经过上述分割，子图间无法再形成环路，本文参考iSAM2[19]用贝叶斯树表示各个子图的邻接关系(如图2所示)。相比于iSAM2的贝叶斯树，本文中树的节点为非完全子图，无需三角化，每次检测到新的回环时只需通过并查集操作合并子图，从而简化计算。

图2 位姿图(左)和贝叶斯树(右)

观察式(5)发现

=ρ(zβjαi·(Txγ0αi)-1·(Txγ0βj))

(11)

可知式(6)的最优解位姿变换之后仍为它的一组最优解。因此可通过3个步骤求解Xopt：

1)根据上述规则将因子图划分为回环子图；

阿奇霉素与红霉素有交叉耐药性。对阿奇霉素耐药最常见的机制是通过甲基化修饰23S rRNA靶点。核糖体修饰可能决定对其他大环内酯类、林可酰胺类和链阳菌素B（MLS B表型）的交叉耐药性。

2)根据式(6)求解各个子图的最优估计；

3)根据式(11)特性，按贝叶斯树从根节点向下的顺序依此将各个子图的解做整体位姿变换，以确保相连子图的共享变量数值一致性，即可求得整个因子图G的最优解。

以图2为例，左图为原始因子图，右图为所得贝叶斯树，右图每个节点表示一个回环子图，每条边上的字母表示2个相邻子图的共有节点。分别求出子图ab、bcdef、ci、fgh、ij的最优解，再依次对bcdef、ci、ij、fgh进行位姿变换，以保证状态变量b、c、f、i的一致性，即可求得因子图最优解。

根据上述特性，在多机器人位姿图优化时，只需要发送与其他机器人构成回环子图的部分边和节点即可，从而实现了节省带宽的目的。

多机器人后端优化采用如下策略：

1)当检测到自身与其他机器人构成回环时，广播位姿图给其他机器人，且仅广播与其他机器人构成回环子图的边和节点；

2)广播时，如果在上次广播到本次广播期间机器人没有发生内部闭环和BA优化，仅广播新增位姿图节点和边，已广播的节点不需要再传输；

3)当机器人发生内部闭环和BA优化时，对内部闭环优化影响到回环子图上的边和节点进行重新广播；

6 实验与分析

本文系统前端和后端部分由C++编程开发，NetVLAD采用Python3和Tensorflow实现，每个机器人都运行有NetVLAD回环检测和SLAM前后端2个进程，进程间通过机器人操作系统(Robot Operating System, ROS)收发消息，机器人间也通过ROS通信，并通过广播的方式进行信息发送，从而实现一对多信息传递。

6.1 NetVLAD回环检测

采用混淆矩阵的方式检验回环检测效果，并将混淆矩阵用灰度图的形式显示，矩阵的行列对应KITTI00数据集中图片编号(0～4540)，图片相似度越高，对应像素的颜色越黑。图3(a)中矩阵计算规则如下

(12)

ti和tj表示图片对应的真实位置，即二者距离大于4m时aij为0(白色)，否则为1(黑色)。图3(b)表示原始NetVLAD网络计算所得特征向量根据式(10)计算所得的混淆矩阵，图3(c)表示根据第5节修改后NetVLAD网络计算所得特征向量根据式(10)计算所得的混淆矩阵，图3(d)为对图3(c)的结果设置0.85阈值的二值化处理结果，此时图3(d)的准确率为99.44%，召回率为39.16%，准确度能够满足SLAM需求，召回率虽然偏低，但SLAM系统在一段连续图片序列中只需要找到一个回环帧即可，因此仍能满足回环检测的需求。

(a)

分别用不同显卡设备搭载NetVLAD网络处理KITTI00数据集(4541张图片)，对比结果如表1和表2所示。

表1 Nvidia GTX1650Ti 4G测试结果

表2 GTX965M 2G 测试结果

通过上述结果可知，改进后的NetVLAD拥有更快的计算速度和更少的资源占用，可应用于低成本GPU，同时能够满足回环检测准确率和实时性需求。

6.2 描述子缺失情况下的特征匹配

图4和图5所示分别为ORB SLAM2和本文算法1对KITTI00数据集图片000391和002442的匹配结果，可知ORB SLAM2前端匹配算法在无描述子情况下，当2幅待匹配图片存在视角偏移时会产生较多误匹配，影响机器人的定位结果(参见图6)，而算法1仅存在少量误匹配，不影响系统后续运行。

图4 ORB SLAM2 特征匹配结果

图5 本文方法特征匹配结果

6.3 多机器人协同定位导航轨迹

将KITTI00数据集按照图片序号平均划分为4段，分别配置在4台i7-6700HQ/Nvidia GTX965M 2G笔记本电脑上,配置4个多机器人SLAM系统节点，并且机器人初始位置未知，最终运行结果如图6、图7和表3、表4所示。

图6 匹配失败情况下的MR SLAM轨迹

图7 本文方案轨迹

表3 本文方案与ORB SLAM2精度对比

表4 本文方案与ORB SLAM2实时性对比

通过以上数据可以看出，本文提出的多机器人SLAM系统能够实现与ORB SLAM2系统同样数量级的定位精度和实时性，多机器人SLAM的定位精度要略高于ORB SLAM2，其原因来自于多机器人SLAM仅进行了位姿图优化，而未进行BA优化，由于ORB SLAM2前端特征点匹配存在误差，BA优化带来了过拟合效应。表4中图片处理时间均值小于表2中的均值，原因是本文方案中NetVLAD仅对关键帧进行处理。

图8所示为部署的4个机器人节点在运行期间的数据传输统计图，可见在未检测到回环帧时，带宽保持在10kb/s以下，在发现回环时发送图片数据会占用25kb/s的带宽，位姿图占用带宽与机器人所经过路径长度有关，145s时刻由于多个机器同时构成回环，造成了位姿图流量高峰，但不超过40kb/s。整个过程传输数据总量为1.18MB，位姿图消息105.16kB，图片消息265.22kB(11张图片)，其他消息(主要为NetVLAD)814.13kB，证明了本文提出方法可有效降低带宽负载。