前言

论文全程为C2VIR-SLAM: Centralized Collaborative Visual-Inertial-Range Simultaneous Localization and Mapping，是发表在MDPI drones（二区，IF=4.8）上的一篇论文。这篇文章使用单目相机、惯性测量单元( IMU )和UWB设备作为每个智能体上的机载传感器，以构建准确高效的集中式协同SLAM系统，并设计了一个全局优化算法，利用视觉位置技术检测到的跨智能体地图匹配信息，以及智能体到智能体的距离信息来优化所有参与者的运动参数，并将局部地图合并成一个全局地图。

一、问题背景

未知静态UWB锚点虽然去除了位置校准过程的要求，但仍然需要进行分布式处理；在大规模应用中，由于UWB设备的有效作用范围有限，也需要不同区域的锚点。UWB信号也会受到干扰。这些因素限制了UWB辅助协同SLAM系统的应用。

与基于UWB的系统相比，基于视觉的协同SLAM需要更少的基础设施建设，并且不受测距测量块的限制。然而，智能体之间需要有共同的视图区域并具有条件不变的位置识别能力才能进行协作，这限制了任务执行的效率。共享的视觉地图也给多智能体系统带来了巨大的通信负担。

虽然基于UWB和基于视觉的协同SLAM各有优劣，但研究者们已经提出了几种同时使用UWB和视觉来相互增强[ 4、5]的系统。然而，这些系统主要侧重于利用机载视觉系统来估计未标定的静态UWB锚点的运动和位置，严重依赖锚点进行协作。

二、主要假设

1、多无人机集中式协同架构的基本假设

2、VIO、UWB的应用假设条件

三、理论流程

本文提出了一种新颖的集中式协同视觉/惯性/测距SLAM系统( C2VIR-SLAM )，仅依靠机载视觉、惯性和UWB测距设备即可实现精确高效的集中式协同SLAM。对于每个智能体，采用视觉-惯性里程计( VIO )估计运动参数并重建局部地图。同时，VIO估计的参数、本地地图和代理到代理的UWB距离信息被发布到中央服务器。然后服务器对运动参数进行优化，将局部地图合并为全局地图。

提出的C2VIR - SLAM包括四个主要部分：( i )单智能体视觉惯性里程计，它估计智能体的运动并重建被探索局部区域的地图；( ii )智能体之间的距离测量，使用机载UWB设备；( iii )位置识别，它检测所有智能体共享的地图中的回环，并估计参与者之间的相对运动；( iv )协作定位与地图构建，它优化所有智能体的运动参数并构建全局地图。

1、单机器人视觉惯性里程计

（使用VINS-mono）

2、位姿图协同定位

在中心服务器中执行协同定位，并将其建模为位姿图优化问题（结合板载UWB和视觉）：

第i个智能体的t个关键帧数据，包含位置向量和四元数。

上述问题包含三个待优化残差。序列残差是指每个智能体内部VIO的相对运动约束。地图匹配残差描述了来自视觉位置识别组件的重定位约束。智能体间测距约束的UWB测距残差。

（1）来自里程计的序列约束

序列残差代表一个智能体的两个连续关键帧之间的变换：

（2）视觉位置识别的地图匹配约束

具体来说，对于一个新的查询关键帧，我们使用DBOW2外观相似度[ 20 ]将其与所有现有的关键帧进行比较。一旦相似度超过一定阈值，则尝试检测两个相似帧之间的循环。查询关键帧上的3D特征点与候选关键帧上的2D特征进行匹配，其中的特征被描述为BRIEF描述符[ 21 ]。然后，使用RANSAC算法通过求解透视N点( Perspective-N-Points，PNP )问题来评估3D - 2D连接[ 22 ]。一旦内点数量超过一定阈值，则被认为是有效的地图匹配约束。通过求解PNP问题，可以得到两个匹配关键帧之间对应的相对位姿，地图匹配残差为：

通过比较( 5 )和( 7 )，我们可以注意到，当顺序约束使用智能体内部的VIO状态时，地图匹配残差可以使用来自不同智能体的运动信息。现有的一些研究表明，将地图匹配残差引入到协同SLAM系统中，可以极大地提高[ 6、7 ]的精度；然而，这要求协作参与者具有共同的浏览地点和识别重叠的视图，这降低了效率，并给视觉识别组件带来了挑战。

（3）板载UWB的相对范围约束

当地图匹配残差需要共同访问的地点时，UWB可以直接测量两个设备之间的距离信息。在这里我们提出将UWB设备刚性地安装在协作代理上，这去除了锚点分布过程，更适合在未探索环境中的应用。

UWB测距模型：

在我们的系统中，UWB设备与代理一起安装在载体上。因此，得到的测距为两个对应智能体之间的距离；该测距信息可以作为观测量，通过定义残差项为，对里程计测量的运动参数进行修正

需要注意的是，位姿图优化可能是耗时的，只有在( 4 )式中加入地图匹配残差或UWB测距残差项时才有效。为了提高效率，我们在一个恒定的时间间隔内检查是否存在有效的地图匹配和UWB测量值，只有当在该时间间隔内存在地图匹配残差或UWB测距残差时才进行优化。

3、结合全局光束法平差的地图精化

计算协作代理共享的所有关键帧中所有特征的重投影，并优化状态χ和特征的3D位置，以最小化整体重投影误差。global bundle adjustment (GBA)的目标函数可以表示为：

重投影因子遍历所有关键帧和所有地图点。为了提高效率，GBA仅在有效的位姿图优化后执行。

四、实验方案

1、使用公开的EuRoC数据集（UWB数据模拟得到）

我们使用两个和三个智能体设置不同的序列组合对所提出的系统进行了测试，并将结果与CVI - SLAM [ 6 ]进行了比较，CVI - SLAM是一种最先进的使用视觉和惯性传感器的集中式协作SLAM系统。

2、使用自建的多无人机数据集Testing Zone数据集

Test Zone数据集包含两个实验，每个实验包含一个双智能体飞行。在国家智能网联汽车(中国长沙)试验区，利用两架DJI铸字用的铜字模600Pro无人机在300 m × 200 m的室外区域同时飞行采集数据。

五、创新总结

这篇文章从相比于VIR-SLAM，深挖了协同估计中单独使用UWB的不足（信号直线传播），将无人机群估计的知识与UWB-VIO相结合，总体属于组合创新，但也解决了实际问题。

• 我们提出在所提出的C2VIR - SLAM系统中使用机载UWB设备，而不是校准或未校准的静态UWB锚点，这就消除了先验锚点分布过程的要求，并随着设备随智能体移动而扩大了UWB的有效范围；
• 我们设计了一个能够单独或组合使用视觉或机载UWB的系统来进行协作定位和建图；
• 我们在不同的数据集和不同的系统设置中进行了系统的实验，并全面分析了单独使用视觉、单独使用UWB以及它们的组合在协作中带来的性能改进。