CN111812978A

CN111812978A - 一种多无人机协作slam方法与系统

Info

Publication number: CN111812978A
Application number: CN202010534426.0A
Authority: CN
Inventors: 杨志华; 李子恒; 齐晓晗; 赵喜双
Original assignee: Shenzhen Graduate School Harbin Institute of Technology
Current assignee: Shenzhen Graduate School Harbin Institute of Technology
Priority date: 2020-06-12
Filing date: 2020-06-12
Publication date: 2020-10-23
Anticipated expiration: 2040-06-12
Also published as: CN111812978B

Abstract

本发明提供了一种多无人机协作SLAM方法，其特征在于，包括以下步骤：S1、检测无人机与地面站之间的通信是否正常,如果正常，则传递无人机的关键帧以及对应的无人机编号；S2、如果对比当前的某一图片与另一无人机的某时刻图片相似，则首先计算出两个关键帧对应的相对位姿,再结合单机SLAM结果计算出相对初始位姿；S3、将所有相似场景的相对初始位姿,纳入到一个集合中，再进行一个自适应数量的聚类，并从中选择中最优的聚类，得到关联数据；S4、将关联数据与单机SLAM结果结合到一起进行优化,最后返回优化后的全局地图信息与飞行轨迹。本发明还提供了一种多无人机协作SLAM系统。本发明的有益效果是：较好的解决了数据关联方案不能解决相似场景的问题。

Description

一种多无人机协作SLAM方法与系统

技术领域

本发明涉及无人机，尤其涉及一种多无人机协作SLAM方法与系统。

背景技术

在未知环境下，无人机可以对自身位置和姿态进行估计，是能够有效完成任务的关键技术。当无人机在低空，低速行驶时，使用全球定位系统(Global PositioningSystem，GPS)和惯性导航系统(Inertial Navigation System，INS)等方法可以获得飞行所需的位置、速度等信息，满足在城市中的飞行需求。但是GPS和INS存在使用的局限性，GPS的误差范围为3-10米，而且在室内环境和高山、峡谷等恶劣环境下由于信号弱导致无法正常工作。同时，INS会随着无人机的不断运动，造成误差的逐渐积累。在这种情况下，使用同步定位与建图(Simultaneous Localization and Mapping，SLAM)可以解决无人机在未知环境下，对位置和姿态的估计。对位姿的估计是实现无人机自主飞行和自主导航等智能化飞行的关键环节。

目前将无人机和SLAM相结合的研究中，主要以单个无人机为研究对象，对于多个无人机的协作SLAM(Cooperative SLAM，CSLAM)没有较为成熟的研究结果，CSLAM是指多个无人机同时在环境中飞行，通过互相协作进行地位和地图的构建，是将SLAM算法与多无人机系统相结合。通过CSLAM，可以提高建图的精度、提高构建地图的速度、扩大单位时间内无人机在环境中的覆盖范围，在大规模的未知环境的搜索应用中具有重要的意义。

在复杂的环境下，由于传感器视线被遮挡或飞行空域限制等因素，仅使用单架无人机无法维持大范围区域内对目标的持续跟踪，需要依靠多架无人机合作完成任务。相比单架无人机来说，采用多无人机协同执行任务具有以下优势：

(1)可以通过配合提高任务完成的质量；

(2)可以通过并行执行任务，缩短完成任务的时间，提高整体效率；

(3)可以通过共享资源与信息，提高执行任务的能力；

(4)可以通过任务分配的比重增加任务成功的概率，提高鲁棒性。

目前，在很多无人机领域的应用都逐步由单机独自完成任务向多机协同的方向发展。已有大量的项目针对无人机的协同侦查、未知环境地图的构建、目标跟踪、定位、组网编队控制等领域展开研究。

协作SLAM中的数据关联问题是指将多个SLAM的数据相互关联起来，使得彼此之间处于不同坐标系下的位姿信息和地图可以统一到一个共同坐标系中，从而可以进行进一步的优化，从而得到更准确的信息。

目前在协作SLAM数据关联中主要采用2种策略：

1.在进行协作SLAM之前就给定具体的关联，从而使得以后的数据关联都基于最开始的关联来进行判断；

2.与单机SLAM类似，采取直接的办法来进行识别，针对其中可能存在的错误测量，采取筛选个别点的方式。

上述无人机技术存在以下技术问题及缺陷：

(1)单无人机技术缺陷

视觉SLAM面临的问题是，随着无人机的不断运行，定位漂移会逐渐累积，造成地图的大面积失真。虽然后端单机的回环检测可以有效缓解误差的积累。但是为了增加回环的次数，就会造成无人机大量的环路运动，转弯次数增多，同时单位时间内的覆盖区域也显著减小。因此如何采用多无人机协作完成SLAM是很有必要的。

近年来，随着智能体需要更加精确的定位和构建高精度的三维地图的需要，单个的无人机的三维SLAM算法已经不能满足这样的要求，特别是大尺度的三维重建，单机完成任务需要耗费大量的时间，而且会产生很大的积累误差，导致定位和建图的准确性大大下降。所以，多无人机三维SLAM算法框架成为一种好的解决方案。多无人机SLAM系统是一种耦合式的多智能体系统，它利用各个无人机之间的协作，彼此进行关键信息的共享，通过通信的方式实现多无人机协同定位和构建整体的全局三维点云地图的过程，以这样的方式提高三维重建的效率和精度要求，更加契合复杂任务的需求。

在现有的研究中，多无人机的协作没有成熟、系统的研究成果，在机器人的研究领域主要将多机协作分成集中式和分布式两种。更多的研究停留在基于卡尔曼滤波作为后端优化的多机协作，随着无人机的运动，使用滤波作为后端处理的方案的计算量成指数型上升，不适合应用在工程中。

(2)协作SLAM的数据关联缺陷

虽然现存的数据关联方案在一定程度上满足一定场景下的协作SLAM的要求，但是仍然存在一定的问题。首先是相似场景的问题。

在实际情况中，我们很难能够得到无人机之间的准确相对初始位姿，而且在大规模复杂的环境中，可能会存在很多复杂的情况，譬如存在单纯从外观来看相似度非常高的场景，在单机SLAM中，我们可以通过记录单机SLAM的飞行轨迹信息来判断相似场景是否为同一场景，但是在多无人机我们无法直接利用上飞行轨迹信息来进行判断，因为其飞行轨迹是不相关的。如果使用直接再加个别点筛选的方案的话，所有的相似场景都会被带进协作SLAM中，从而会为协作SLAM系统带来不可逆转的破坏，造成协作SLAM的失败。

其次是基于无人机的协作SLAM问题，现在大部分的协作SLAM都是基于二维的机器人来进行的，无人机平台将其转换成三三维空间，所需要的计算量将大大增加。

综上，针对现有单无人机SLAM方法局限性和分布式集中式各自存在的弊端以及现存的数据关联方案不能解决相似场景的问题，如何设计一种基于复杂大规模环境的多无人机的协作SLAM的方法，是本领域技术人员所亟待解决的技术问题。

发明内容

为了解决现有技术中的问题，本发明提供了一种多无人机协作SLAM方法与系统。

本发明提供了一种多无人机协作SLAM方法，其特征在于，包括以下步骤：

S1、检测无人机与地面站之间的通信是否正常,如果正常，则传递无人机的关键帧以及对应的无人机编号；

S2、相似场景判断，如果对比当前的某一图片与另一无人机的某时刻图片相似，则首先计算出两个关键帧对应的相对位姿,再结合单机SLAM结果计算出相对初始位姿；

S3、将所有相似场景的相对初始位姿,纳入到一个集合中，再进行一个自适应数量的聚类，并从中选择中最优的聚类，得到关联数据；

S4、将关联数据与单机SLAM结果结合到一起进行优化,最后返回优化后的全局地图信息与飞行轨迹。

作为本发明的进一步改进，在步骤S2中，相似场景判断包括：在单机SLAM中，无人机将实时拍到的图进行ORB特征提取和进行特征点的描述，将所对应的描述通过预先训练好的词袋模型转换成对应的单词并记录下来；在后续的飞行过程中，任意一无人机将每一帧新拍摄的图片所对应的单词与另一无人机已经存储的单词进行对比，并与所设定的阈值进行对比，如果超过该阈值，则认为其是相似场景。

作为本发明的进一步改进，在步骤S2中，通过ICP算法计算出关键帧对应的相对位姿，再结合单机SLAM中存储的无人机的飞行轨迹信息，求得该相似场景所对应的相对初始位姿。

作为本发明的进一步改进，相似场景所对应的相对初始位姿通过下式求得：

运算符号代表的是4x4的位姿所进行的运算，X代表单机SLAM的结果，包括地图信息以及飞行轨迹信息，U代表两架无人机识别的相似场景的相对位姿，T代表两架无人机的相对初始位姿。

作为本发明的进一步改进，在步骤S3中，针对所有相似场景的相对初始位姿,首先进行AP聚类,然后进行聚类选择，得到关联数据。

作为本发明的进一步改进，在步骤S3中，首先将相对初始位姿的数据转换成AP算法能够利用的度量单位：

上式表示为两个不同时刻的相对初始位姿的距离；其中T1和T2代表不同时刻的相对初始位姿，∑代表了4x4维变换矩阵的协方差矩阵，这里的n相对应的取值为4；

将每个聚类的中心视为每个聚类中最具有代表性的相对初始位姿，相对应的就有对应数量的相对初始位姿Ti[i＝1,2,3,...n]，针对这生成的n个假

设，接下来的目的就是从中选择最优的那个聚类：

上式表示在已知单机SLAM结果和所有相似场景对应的相对位姿的情况下，从生成的多个聚类h中选择最优聚类的方法；其中h_i代表不同相似场景生成的不同聚类，

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合；

上式后面的式子表示成：

上式代表了应用贝叶斯公式来对

进行转换求解的过程；其中h代表不同相似场景生成的不同聚类，

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿；h代表每一个聚类所对应的选择，X代表单机SLAM的结果，包括地图信息以及飞行轨迹信息，U代表两架无人机识别的相似场景的相对位姿；其中，p[Z|X]与假设无关，故不需要计算；先验概率p[h|X]我用CRP过程来模拟，单独计算即可；剩余部分则如下：

该部分代表着在某个具体聚类h以及所有的单机SLAM结果的条件下，每个相对初始位姿的概率；其中h代表不同相似场景生成的不同聚类，

代表单机SLAM的结果，T代表相对初始位姿，T₀代表该聚类中心所对应的相对初始位姿，Σ₀代表该聚类所对应的协方差矩阵；

其中T₀代表AP聚类中心所对应的相对初始位姿，以及

上式代表了在某个具体聚类h中，在已知单无人机SLAM结果以及此时的相对初始位姿的条件下，求出来相似场景所对应的相对位姿的概率；其中h代表不同相似场景生成的不同聚类，

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿，Σ_in代表某个相似场景在该聚类下属于内点时所对应的协方差矩阵，Σ_out代表某个相似场景在该聚类下属于外点时所对应的协方差矩阵；最后根据经验设置一个阈值C，用来判断最后取得的最优聚类是否为数据关联，如果认定为据关联，则将两架无人机的数据关联起来。

作为本发明的进一步改进，在步骤S4中，将关联数据与单机SLAM结果结合到一起代入到一个协作SLAM的优化公式中：

上式代表了在已知单无人机的观测信息以及多个无人机之间所有“相遇”所对应的相对位姿以后，求得协作SLAM最后优化结果的过程；其中X代表了各个无人机的位姿信息，Z代表了无人机在飞行过程中的观测信息，U代表无人机之间经过验证的“相遇”时刻的相对位姿，X^*代表经过优化过后的各个无人机的位姿信息，r代表不同无人机，k代表在某个时刻的观测值的数量序号，m代表观测值的数量，i代表在飞行过程中“相遇”的序号，相对应的N代表“相遇”的次数；

最后应用光束平差优化算法来对上式进行优化求解，即得到优化后的地图和位姿信息。

本发明还提供了一种多无人机协作SLAM系统，包括可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现如上述中任一项所述的方法

本发明的有益效果是：通过上述方案，针对在大规模复杂的环境中下的同时定位和建图，提出了一种基于多无人机的协作和数据关联方式，在得到单机SLAM的结果后，识别相似场景，对所有相似场景进行聚类并从中选择最优聚类从而进行数据关联，得到协作SLAM的全局地图，从而解决感知混叠问题，较好的解决了数据关联方案不能解决相似场景的问题。

附图说明

图1是本发明一种多无人机协作SLAM方法中相对初始位姿关系示意图。

图2是感知混叠示意图。

图3是协作SLAM的环境地图。

图4是单机SLAM的环境地图。

图5是不经过筛选的数据关联地图。

具体实施方式

下面结合附图说明及具体实施方式对本发明作进一步说明。

一种多无人机协作SLAM方法，其过程描述如下：

首先检测无人机与地面站之间的通信是否正常,如果正常的话正常传递无人机的关键帧以及对应的无人机编号；如果对比此时的某一图片与另一无人机的某时刻图片相似,则首先计算出两个关键帧对应的相对位姿,再结合单机SLAM结果计算出相对初始位姿；针对所有相似场景的相对初始位姿,我们首先进行AP(Affinity Propagation)聚类,然后进行聚类选择；最后将经过上述过程的数据关联数据与单机SLAM结果结合到一起带入优化式子中,最后返回优化后的全局地图信息与飞行轨迹。

具体过程如下：

1.数学建模

首先给出本文涉及的若干概念定义：

假设我们现在已经通过词袋模型得到两个相似的场景，对应两幅图，分别拥有彩色图片和深度图片，在通过RANSAC等匹配算法匹配后，可以计算出相对位姿：

两个不同坐标系下的点P和Q进行坐标变换时，可以通过下式实现刚体变换：

q_i'(x',y',z')＝Rq_i(x,y,z)+t (1)

其中，R为旋转矩阵，t为平移矩阵，可以分别表示为：

t_3×1＝[t_x t_y t_z]^T (3)

其中α、β、γ分别表示点沿x、y、z轴的旋转角度，t_x、t_y、t_z分别表示点沿x、y、z轴的平移量。

该刚体变换中涉及到六个未知数α、β、γ、t_x、t_y、t_z，因此至少需要确定六个线性方程，即至少需要在待匹配点云的重叠区域内找到3组对应点对，且不能共线，才能完成刚体矩阵的参数估计。

一般在表示多次的三维空间旋转与平移时，上式的形式会过于复杂，所以一般采取变换矩阵T的方式来表达旋转矩阵和平移矩阵：

将之前坐标转换的式子可重写成：

假设现在已经得知在某一时刻两架无人机识别到了相似场景，并根据独自的SLAM结果我们可以得到分别的轨迹信息，从中我们可计算出无人机当前位姿相对于初始位姿的相对位姿X,X’，并结合相似场景所估计的相对位姿Z，可以求出相对初始位姿T，如图1所示：

这里的符号代表4x4矩阵的运算。

2.方案描述：

下面结合附图对本发明的技术方案做进一步的说明：

如图2所示，在一个大规模复杂的环境中，两个小型四旋翼无人机对处于环境的哪里毫不知情，并对彼此位置毫不知情。两架无人机均搭载了深度相机，我们称其分别为UAV1和UAV2。两架无人机均配备通信装置，将所拍摄到的信息返回到地面站，从而进行进一步处理。

首先通过日常的图片集训练我们的词袋模型，得到我们的词袋，以便将无人机实际拍到的图片与其进行对比。

在初始位置时，两架无人机均进行单机的SLAM，记录下自己所飞行过的环境周围信息以及自己的飞行轨迹信息。

在单机SLAM中UAV1，UAV2将实时拍到的图进行ORB特征提取和进行特征点描述，将所对应的描述子通过训练的词袋模型转换成对应的单词并记录下来；UAV1在后续的飞行过程中，将每一帧新的图片所对应的单词与UAV2之前所存储的单词进行对比，并于我们自己所设定的阈值进行对比(在比较常见的场景中我们设定的阈值是0.015)，如果超过该阈值，则认为其是相似场景。

一旦认为其是相似场景，下一步就通过ICP(Iterative Closest Point，迭代就近点算法)算法计算出相对位姿U；

结合单机SLAM中存储的无人机的飞行轨迹信息，可以求得该相似场景所对应的相对初始位姿：

这里的运算符号代表的是4x4的位姿所进行的运算，X分别代表两个无人机从初始位置到该时刻的飞行轨迹信息，U代表两架无人机在识别的相似场景的相互位姿，T代表两架无人机的相对初始位姿。

将刚识别的相似场景对应的相对初始位姿纳入到之前的集合中，再进行一个自适应数量的聚类，这里我们使用AP(Affinity Propagation)聚类算法，称为仿射传播聚类算法、近邻传播聚类算法、亲和传播聚类算法。首先将相对初始位姿的数据转换成AP算法能够利用的度量单位：

上式表示为两个不同时刻的相对初始位姿的距离；其中T1和T2代表不同时刻的相对初始位姿，∑代表了4x4维变换矩阵的协方差矩阵，n相对应的取值为4；

选择AP算法作为聚类算法，首先是因为其不需要对聚类的数量做出要求；其次我们后续的聚类选择算法需要利用到每个聚类的中心，选择其他聚类算法的话，这又需要占用一部分计算资源。

我们将每个聚类的中心视为每个聚类中最具有代表性的相对初始位姿，相对应的就有对应数量的相对初始位姿Ti[i＝1,2,3,...n]。针对这生成的n个假设，我们接下来的目的就是从中选择最优的那个聚类：

其后面的式子可以表示成：

上式代表了应用贝叶斯公式来对

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿；

其中，p[Z|X]与假设无关，故不需要计算；先验概率p[h|X]我们可以用CRP过程来模拟，单独计算即可；剩余部分则如下：

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿，Σ_in代表某个相似场景在该聚类下属于内点时所对应的协方差矩阵，Σ_out代表某个相似场景在该聚类下属于外点时所对应的协方差矩阵；

最后根据经验设置了一个阈值C，用来判断最后取得的最优聚类是否为数据关联。

通过为该算法设置阈值，可以避免识别的聚类所对应相似场景都不是相同场景的情况，大大提高了数据关联的鲁棒性，保证系统的稳定性。

如果认定为通过了数据关联，就可以将UAV1和UAV2的数据关联起来，将其两部分的数据带入到一个协作SLAM的优化公式中：

最后应用光束平差优化算法(BA，Bundle Adjustment)来对上式进行优化求解即可得到优化后的地图和位姿信息。

3实验结果与性能分析

在实验中，我们使用本发明所构建的协作SLAM系统去获得一个最终的全局地图，实验设备：Ubuntu16.04LTS,Inter core i5-7500CPU。

图3是双机协作SLAM的环境地图，图4是完全单机SLAM的环境地图。通过对比，可以看出两者都将实际场景完美还原出来。

图5是双机协作SLAM中未对相似场景进行筛选所得到的环境地图，通过与图2对比，可以看出未经过该发明筛选的相似场景中存在大量的错误匹配，使得协作系统无法正常运行,而该发明因为提前考虑了数据关联中的可能带来错误的特殊情况，并对其做出了筛选，从而能准确的得到真正发生了数据关联的数据信息。

表1单机SLAM和使用该发明的协作SLAM完成任务所需的时间对比

单机SLAM	协作SLAM
		161.55s	85.63s

由上表可以看出，针对同一环境完成同一任务来说，协作SLAM所需要的时间仅仅为单机的一半左右，且达到的精度相差无几。

综上所述，本发明设计了一种在复杂大规模环境中多无人机协作SLAM的方法，在协作SLAM的基础上充分考虑了现实中存在的相似场景的误匹配情况。相对于单机SLAM来说，大大提高了任务的完成速度；并使得协作SLAM系统具有了更高的鲁棒性和稳定性。通过对其的实验和仿真，得出本文的算法和系统适应于复杂大规模环境中的多无人机协作的SLAM。

针对传统的大规模复杂环境中的场景所带来的相似场景的挑战，本专利提出了一种基于视觉传感器的自定义数量的聚类算法以及聚类选择算法，根据视觉传感器在三维空间中识别出来自不同无人机的相似场景。最后根据通过数据关联的来自不同无人机的数据带入到整体优化方程中，得到一个整体全局的地图。结果表明，对于大规模复杂环境中，本方案能够解决对空间中相似场景的筛选，提高了系统的鲁棒性，并提高了SLAM的速度。

本发明提供的一种多无人机协作SLAM方法与系统，主要用于大规模复杂环境中的多无人机进行协作地图构建。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims

1.一种多无人机协作SLAM方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的多无人机协作SLAM方法，其特征在于：在步骤S2中，相似场景判断包括：在单机SLAM中，无人机将实时拍到的图进行ORB特征提取和进行特征点的描述，将所对应的描述通过预先训练好的词袋模型转换成对应的单词并记录下来；在后续的飞行过程中，任意一无人机将每一帧新拍摄的图片所对应的单词与另一无人机已经存储的单词进行对比，并与所设定的阈值进行对比，如果超过该阈值，则认为其是相似场景。

3.根据权利要求2所述的多无人机协作SLAM方法，其特征在于：在步骤S2中，通过ICP算法计算出关键帧对应的相对位姿，再结合单机SLAM中存储的无人机的飞行轨迹信息，求得该相似场景所对应的相对初始位姿。

4.根据权利要求3所述的多无人机协作SLAM方法，其特征在于：相似场景所对应的相对初始位姿通过下式求得：

上式中，运算符号代表的是4x4的位姿所进行的运算，X代表单机SLAM的结果，包括地图信息以及飞行轨迹信息，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表两架无人机的相对初始位姿。

5.根据权利要求1所述的多无人机协作SLAM方法，其特征在于：在步骤S3中，针对所有相似场景的相对初始位姿,首先进行AP聚类,然后进行聚类选择，得到关联数据。

6.根据权利要求5所述的多无人机协作SLAM方法，其特征在于：在步骤S3中，首先将相对初始位姿的数据转换成AP算法能够利用的度量单位：

将每个聚类的中心视为每个聚类中最具有代表性的相对初始位姿，相对应的就有对应数量的相对初始位姿Ti[i＝1,2,3,...n]，针对这生成的n个假设，接下来的目的就是从中选择最优的聚类：

上式后面的式子表示成：

上式代表了应用贝叶斯公式来对

其中，p[Z|X]与假设无关，故不需要计算；先验概率p[h|X]用CRP过程来模拟，单独计算即可；剩余部分则如下：

代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿，Σ_in代表某个相似场景在该聚类下属于内点时所对应的协方差矩阵，Σ_out代表某个相似场景在该聚类下属于外点时所对应的协方差矩阵；最后设置一个阈值C，用来判断最后取得的最优聚类是否为数据关联，如果认定为据关联，则将两架无人机的数据关联起来。

7.根据权利要求1所述的多无人机协作SLAM方法，其特征在于：在步骤S4中，将关联数据与单机SLAM结果结合到一起代入到一个协作SLAM的优化公式中：

8.一种多无人机协作SLAM系统，其特征在于：包括可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现如权利要求1至7中任一项所述的方法。