无人机集群的去中心化、分布式状态估计和协同定位与建图

来源：深蓝AI

编辑：郑欣欣@一点人工一点智能

01 概述：从无人机单机智能到无人机集群

无人机单机智能已经发展到相当不错的水平，不仅可以执行避障，可以建图，还可以自主的执行很多任务，比如说对人的追踪、对未知环境探索。虽然无人机单机它已经发挥了很大的作用，但是单机能做的事情比较有限，会受到数量限制。

一个非常自然的想法去采用很多架无人机，极大的扩展无人机的能力，比如可以并行的去执行很多任务，成倍的提高效率。但目前在现实中是有很大的挑战，比如希望无人机集群可以是去中性化的，如果不去中心化，当中心极点被入侵了，那么所有的飞机会飞过去打自己。

因此，如果单一节点失效，则无人机群就不能自主执行任务了；第二个是无人机集群的通讯，因为如果想要无人机群有很多的协作，那么就需要互相联系，同时也希望无人机群能够摆脱地面设施，跟去中心化是一个道理。

这里的无人机群会更偏向行业应用一些，因此一个基本的技术是定位与建图，因为有这个能力以后，飞机才能维持稳定飞行，有相对定位能力以后，飞机才能够互相避让。在这个基础之上，会希望能够有一个全局一致性的定位，全局一致性是指一个飞机飞了一圈回来以后，地图还是它的那个位置，即在原地，不希望它产生一些漂移，这会影响后续任务的执行。

这个是无人机集群的一个比较大的痛点，因为可以通过前端的正态估计以后，把3D地图贴上，进行简单的融合，其实核心就是状态估计，作用在于可以躲避移动障碍物。而多个无人机去搬运一个物体，那这时候就需要飞机之间有非常好的协作和非常好的精确的相对定位能力。

对于无人机集群来说，通讯是保障飞行进行任务的必要条件。一般会有两种通讯模式，一个比较典型的是路由通讯，这实际上是一个依赖于地面设施的中心式的通讯，相当于所有的飞机跟一个中心节点或者一个基础设施交换相换信息。

当然，这也存在一个问题是限制了无人机集群的发挥。另一个更加自然的方式是无线网络，因为WiFi本身是广播的，每个WiFi包发到大气里，谁接受后会根据WiFi的SD去建立各样的检测，但是也有一定问题，没有连接的部分则无法直接通讯。

今天讲的主题主要是去中心化和分布式，这两个概念对无人机集群非常重要。去中心化是即使单一节点本身失效了，只要预先载入了高层指令，依旧能够继续执行任务。另外，分布式是希望把一些任务的执行和计算分摊开，比如做神经网络的多机训练就是这样。

02  集群的去中心化视觉-惯性-UWB相对定位

由于当时的无人机集群，但几乎没有能够真正在一个随便环境里能飞出来的集群，所以，目标比较简单，希望对于任意环境，无人机能只靠自身携带的设施，不靠外部设备，就能飞起来。

这里有一套非常好用的两步式方法，第一步是无人机集群实现自身状态估计，第二步，使用VIO结果、检测和UWB相对测量状态估计的结果进行融合，然后结合到之前相对测量的视觉进行融合，来获得一个非常精确的结果。

UWB是超宽带通讯，实际上是通过发脉冲来实现通讯，并没有一个明确的频率编码，每次发一个脉冲，那其他飞机收到这个脉冲后，然后再回发给它。

因为脉冲是非常容易在时域上统计，所以超方向冲击可以很容易计算出来飞机之间的相对距离，其精度可以达到十厘米甚至几厘米。不好的地方在于没有定向信息，精度会受到WiFi和其他东西影响。还有一个问题是会受到遮挡的影响，从而得到错误的结果。

首先使用IMU来得到一个基本的自身状态估计，作为基础来使用。由于只能提供一个粗定位，因此融合了视觉测量。视觉测量存在一个非常大的问题，无人机集群的特点是每架飞机都一样，且很接近，即使每架飞机都不一样，但飞远后依旧是一样的，因此对于无人机集群来说，相对定位对使用视觉方法来说是一个很严重的问题。

针对这个问题，这里选用了一种比较简单的策略，先使用UWB和VIO来提供比较糙的定位，精度大概是10到20厘米左右。只要有新的运动，它会打上一个20厘米的精度，跟飞机大小是差不多的量级。根据这个粗定位，随后进行匹配到这个飞机是这个飞机，即可以确定当前飞机的ID，再将视觉测量融入。

在这篇文章中，直接使用了VIO的深度测量，进行一些简单的校准，能够提供几厘米左右的精度，可以极大的改善UWB的效果。

这个问题也被很多人研究，比如维杰他们采用搜索所有可能性的解的方案，但这种方案会存在算力问题；高飞老师他们是将其转化为一个数学的优化问题，用essdp问题求解。

本文方法是是直接进行匹配，比较粗暴，坏处在于飞机如果非常密集，容易产生一些奇异性。到底哪一种好用，其实取决于具体场景，因为确实如果是全靠本文算法，而不是靠视觉测量本身去解决视觉测量奇异的问题，因为是相当复杂。

算法框架如图1所示，会有一个UWB和IMU融合，融合以后还会进行VIO、tracking，之后使用推优方法。并且使用测量进行能得一个实时的性能。

图1 Decentralized Visual-Inertial-UWB Fusion

因子图如图2所示，有相对距离，有视觉方向，还有一个必要性条件是要想它能工作，必须要有可观测性。因为UWB测距相当于一个三角测量，要进行一次三角测量去测定别人的位置，必须自己得有运动，然后对方可能没有运动。

当时每个飞机都在动，也有可能不动，就相当于是一个比较复杂的问题。但其实真正需要相对运动，才能进行UWB和VIO的运动，即需要有一定的初始化，之后，才能建立粗测量，同时相对运动越大，测量的初始化精度越好。

图2 因子图

最终，通过室内实验证明了依靠相对定位可以任意飞。如图3所示，可以看到每个轴都有大约4-5cm的精度，但如果不开视觉测量，精度会退化的非常厉害，尤其是Z轴，这是因为Z轴的相对运动比较少。

如果有视觉测量后，如图3右边所示，可以实现厘米级别精度的实时定位，这个集群的状态估计是去中心化和低通信带宽。而且每两个飞机之间互相估计的结果比较接近，存在一致性，同时在在测量或无人机暂时丢失时具有可扩展性和鲁棒性。

图3 实验结果

03 omni-Swarm具有全局一致性的去中心化全向视觉-惯性-UWB无人机集群状态估计

如果飞机互相看不到，那么视觉测量上则不起作用，此时的定位精度会很低，还有最严重的一种情况是当两个飞机并列平行往前飞，那么他们的摄像头互相看不到，因此就完全没有相对视觉测量，也完全没有位置测量，那时的精度会严重退化，因此需要全局致性。针对这些问题，有了一个新的工作，叫Omni-swarm。

针对视觉看不到的问题，一个很直接的办法是把摄像头弄成全像，所以有一个omni directional的VIO。

其实从视觉测量开始，就加入了一些比较传统视觉slam的东西，比如使用了地面特征点来进行快速的初始化和全局定位，那么它有一个传统协作Slam的特征，又有UWB融合的centralize，还有视觉系统，因此几乎是把所有无人机集群能用得到的定位手段都采用了，那么它有一个去中心化的思想，还有全局性、高精度。整体结构如图4所示。

图4 Omni-swarm

该工作有几种典型的测量，第一个是UWB的测量，第二个是视觉测量，对于地面目标landmark，来计算相对位姿。双目视觉系统如图5所示，两个摄像头都大概有230°的视角，左右会有两个区，可以覆盖所有的视野。

图5 双目视觉系统

全像视觉的工作动性比普通视觉要高许多，然后进行了改进。这次直接做了6DOF的识别，比如飞机一些比较典型的特征点，然后跟一些特征点进行反投，可以计算出来一个相对的6DOF位姿。这是一个比较典型的方法，还有是用网络辅助方法去建立位姿，还有使用UWB融合VIO进行初始化，再把视觉测量和其他测量出来的位置进行匹配。

之后引入了一个新的map-based localization，如图6所示。用全局描述子去建立VIO的keyframe，然后使用mobilenetvLAD的虚拟网络来建立全景描述，鲁棒性比较好。

图6 Map-based Localization

后端是一个比较典型的图优化，如图7所示，因为无人机集群中的两个飞机必须要有足够相对距离，或者是使用地图进行相对测量，再或者是使用视觉进行相对测量，才能进行一个集群状态的估计。其次，引入了PCM outlier rejection方法，可以有效的剔除视觉相对测量误差。

图7 图优化

之后进行了一些实验，准确率结果如表1所示。在三个数据集上进行了测试，所提出的方法的准确性在绝对轨迹误差和相对误差（用于相对状态估计）中表现最好。

表1 实验准确率结果对比

04  分布式协同SLAM(Distributed Collaborative SLAM)基础和BDPGO(平衡分布式位姿图优化)

希望无人机飞行不只是一个机器人，还是一个电脑，这样可以把他们的算力更好的用起来，变成一个分布式的系统。这是一个自然而然的想法，所以首先会讲一个问题是比较典型的分布式Slam系统里常用的一个方法叫做distributed pose graph optimization，即每个飞机有自己的VIO测量，飞机之间有相对位姿，可以将其变成一个比较大的因子图。

目前为止，绝大多数的分布式Slam测量都集中在使用post graph方法，就是飞机和飞机之间相对的位置测量，就使用激光也好，然后建立pose graph，再进行分布式的求解，当求解的时候，它可能会有两种位置，一个是两个飞机之间的位置，另一个是可以提供相对测量。

目前，对于pose graph常常采用非线性方法进行求解，然后可以写成一致性问题，即把问题拆成每个飞机上的各个子问题，这个子问题可能涉及到本机的状态，比如飞机可能观察的关键帧，但可能也会有K飞机上的关键帧，因此希望飞机观测出来K飞机的关键帧和K飞机观测出来关键帧应该状态一致。

多飞机进行探索时，实际上每个飞机飞的距离有可能是不定的，开始实验时，飞的越远的飞机计算量会更大点，但是从逻辑上来看，应该是越近的飞机可以承担更多的计算量，跑得越远的飞机应该承担更少的计算量。

因此，这涉及到算力也是无人机集群里面的一个资源，应该能够进行调度，这里提出了一个方法来比较好的去均衡飞机本身keyframe的数量和算力调配问题，算法如图8所示。

图8 BDPGO算法

在进行分布式优化的时候，每个飞机会解决一个子问题，且子问题之间存在联系。这些联系相当于图优化的一个边，边越多就表示需要交换的信息越多，因为需要把边上边缘的信息必须要拿出去分享，然后内部信息没有必要拿出去分享。那么希望边数量非常少，因为边的数量越多，优化越慢，所以能把分项的所有问题规模进行重新分配。

这里一个DEMO如图9所示，在数据集里弄到十个飞机，形成非常不规则的轨迹，每个飞机走的距离有长有短，但是如果进行重新分割，颜色非常混乱(每个颜色代表一个飞机),但经过重新分配以后，颜色会变得非常规整，比如a上的一些点也会变少，所以优化性能也会变好，速度也会变快。

图9 图优化对比

把计算问题重新动态重新分配后，会保证这个问题的完整性，能够达到更高的计算效率、更小的带宽利用，当然他的缺点是没有做实验，还有一个问题是因为需要共享所有的信息，如果截获一架飞机，则能够知道其他飞机的信息。

05 D^2SLAM：去中心化和分布式无人机协同定位与建图( CSLAM)系统

进行全局定位的时候，需要是全局一致性；在距离比较近的时候，需要进行互相避障。这些都是需要去处理的问题，并且保证这个方法能够被无人机算力所计算出来。所以，要把问题分成两个处理，一个是平滑状态估计，另外一个是飞机之间相对状态的高精度估计，来保证飞行本身的稳定性。

在这之前，需要有个基础的东西，称为Near-field state estimation(D2VINS)，因为飞机之间要紧密的交换信息，才能够得到一个比较高度的相对定位。本方法里没有加入相对状态的信息，因为当更换其中一个飞机时，则需要重新计算相对状态信息。

但是希望有一个更加通用的方法，就需要靠地面目标来进行相对协作。需要比较稠密的进行信息交换，还需要有fov视野的重合，且最好使用全像相机。另外，这个方法有一个比较高通信带宽的要求，因为视觉本身是一个可观的自信问题，必须要通过大量信息进行交换来获得比较好的可观自信，然后另外一个问题是需要集群内的一致性以及全局一致性。最后，整体结构如图10所示。

图10 D2SLAM结构

这个网络的前端是比较统一的，直接使用了特征点进行追踪，由于全向相机价格偏贵，因此兼容了双目相机，因此有一个混合的前端。安装方式采用了新的构型，因为它的双目视野区比较好，比较好像素点也多，做深度生成也不错。

最后生成的图像是一个地图卷在桶里面，好处在于方向的一致性比较好，随着视野左右移动，不会有太大的偏差。使用的混合前端，即有superpoint的特征点，又有LK光流的特征点，这是因为集群必须需要处理飞机视差比较大的问题，且能够进行相对定位，所以混合使用，用于解决各自的缺陷。

另外，在前端还会有一个3Dwindow，用来查两个飞机现在的视野是否有偏离，有的话会进行视觉的匹配。

在做视觉Slam的时候，实际上需要发两个东西，首先是飞机有一个全图描述，还需要发每个特征点的描述，怎么样把特征点进行匹配。而当两飞机离开很远的时候，在互相匹配的时候会发现描述子使用不上，且空中带宽非常宝贵，乱发信息会导致资源浪费。

有一种方法叫做分布式的loop架构，通过一个飞机向外广播全部描述子，另外一个飞机如果存在对应的描述子，则发出描述子，从而进行比较正确的匹配，获取相对位姿，同时使用superpoint描述子可以节省带宽。如果飞机离得很近，只需要模式的切换。

对于分布优化，存在如何分割的问题，飞机之间要保证观测量相等，那有关键帧、相机位姿外参、特征点参数，一个大体思路是把特征点摆出来，一号机观测到了特征点a，二号机也观测到这个特征点a，这时希望两个特征点是在同一个位置上。需要把这些相机分成互不交叉的集合，然后使得特征点有可能交叉，之后需要去共享特征点信息来确认是不是一样的位置，这需要大的通讯量。

因此需要缩小问题，很多时候特征点不是用世界坐标性XYZ三维坐标表示，而是使用关键帧第一帧的位姿向外投影，投影以后有一个位置长度的逆来表示一个特征点。长度的逆的数学性质比较好，这时特征点和位置是耦合在一起。港科大全老师组对这个方案有个改进，还是分布式出一个问题，把相机的特征点分成很多互不相同的集合，把相机的位置共享就行了，通信量也会省很多，优化性能也会更好。

但是要处理联合的VIO问题，如果错误发送了关键帧信息，则系统容易崩溃。因此，改进了这个问题，把关键帧分成不同的集合，进行估计关键帧，如果两个飞机之间观测到同一关键帧，则希望一致。

ground-truth特征点只能在本机上建立，所以可以保证本机的一致性。其次IMU也是天然在本机上，也能保证本机上的本地的观测。然后使用的一个方法是用特征点的时间戳，时间戳谁在前面，这个特征点归谁。其实还有一个比较复杂的方法去挑选特征点，使得每个飞机上最多的关键帧被覆盖。其次，特征点尽可能有长的长度，能覆盖的尽可能多的关键帧。

Marginalization跟VINS没有本质区别，只不过多机的，如图11所示。

图11 多机Marginalization

在双目上进行测试，虽然无人机集群更适合全向相机，但是双目用的较多。在双目上的效果如表2所示，效果很不错。可以达到2cm的精度，同时FOV相对精度更高，但是存在一个问题是没有使用任何ground-truth进行初始化。

表2 双目视觉测试性能

为了提供全局性，设计了一个区域化分布式的算法，拥有异步性能，因为飞机距离相对较远，通讯可能很差，所以希望在比较大的通信延迟情况下，依然能够去计算。但是对于far-field state estimation，实时性能并不怎么重要，更关注的是预知性。目标优化是一个标准的分布式优化问题，希望是异步求解，进行旋转初始化，保证了多机上游的一致性。

最后，总结一下，D^2SLAM具有比较好的实时性，能够支持多款相机，高精度测量，同时全局一致性可以让它适合很多种不同的任务。

当然也有一些缺点的，首先通讯绝对是限制集群数量继续增长的一个极大因素。如果希望把它变成一个非常大的集群，那需要解决通讯问题，比如可以定向发信息，或甚至可以用激光去定向发信息；再一个是没有加相对测量，因为主要是希望这个方法能作为一个主干方法，保证基础的工作都有。

06  Future Works

未来工作主要有以下方面：

第一个是大规模的CSLAM，不仅仅指距离远，也指飞机数量能够更多，更多的需要去改进通讯、路由和方法。

第二个是分布式的视觉系统是否可以载一个单钢或多钢机型进行工作，比如现在有单一机器人，上面装了不同的计算节点和不同的视觉节点，各个节点间能够互相独立工作，还有是能帮助多刚体机器人进行观测，进一步的去进行多机体重构机器人组合。