CN109615698A - 基于互回环检测的多无人机slam地图融合算法 - Google Patents

Abstract

本发明提供了一种基于互回环检测的多无人机SLAM地图融合算法，对来自不同无人机的关键帧信息，使用BOW算法快速比较其图像特征，检测出两个UAV或者更多UAV到过相同的位置，通过检测建立2个或者更多关键帧之间的关联信息。在特征的选择上，使用ORB特征代替BOW算法中使用的SIFT特征来优化词袋模型。当检测出相似场景时，对特征点进行匹配，并且去除错误匹配，然后执行点云地图的融合，使用增强版迭代最邻近点算法，其中包括曲面法线和切线信息的匹配，改善对运动方程的估算结果。本发明通过利用来自不同无人机的地图信息，融合地图，增加回环的个数，提高全了局地图精度和提高整个系统鲁棒性。

Description

基于互回环检测的多无人机SLAM地图融合算法

技术领域

本发明涉及多无人机即时定位与地图构建技术领域，尤其涉及一种多无人机SLAM地图融合算法。

背景技术

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

按照搭载使用传感器的不同，可以分成激光SLAM和视觉SLAM，使用SLAM对位姿进行估计，需要对环境信息进行采集，在研究的过程中，人们使用了很多种传感器，例如将声呐，激光，红外和视觉等应用在SLAM算法当中。由于激光传感器具有能够精确测量出与被观测对象之间的距离和角度信息的优点，在很长的研究过程中，激光传感器都是SLAM算法研究的首选研究对象。因此，有关激光SLAM算法相关研究已经较为成熟，其中一些优秀的解决方案已经投入商用。视觉传感器，具有重量轻便、体积较小、能耗低、价格低廉等优势，同时在地图的构建中可能提取更多的信息量。相对比于机器人来说，无人机的运动场景更加灵活，具有质量轻便，按照功能可以设计的较小，价格较为廉价等优点。可以适用于户外、野外等环境恶劣的条件下进行勘测。同样的由于无人机的负载能力有限，也造成了不能搭载过多的传感器，因此，在传感器有限的环境下，无人机可以较好的实现对自身位姿进行估计显得格外重要。视觉SLAM逐渐成为SLAM研究的主流方向。

搭载激光传感器的SLAM长期作为SLAM算法研究的主流方向，但如果将激光搭载在无人机上，无人机负载能力有限，电池的续航也不如地面机器人，因此，激光传感器也同样存在体积大、能耗高、成本过高等缺点，在小型飞行器和低成本的设计方案中难以使用。因此，近几年有关视觉SLAM的研究引起了广泛的关注。视觉SLAM面临的问题是，随着无人机的不断运行，定位漂移会逐渐累积，造成地图的大面积失真。虽然后端单机的回环检测可以有效缓解误差的积累。但是为了增加回环的次数，就会造成无人机大量的环路运动，转弯次数增多，同时单位时间内的覆盖区域也显著减小。因此如何采用多无人机协作完成SLAM是很有必要的。

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

集中式系统是指有一个主无人机承担中央处理器的任务，其他无人机会将自身信息传递给住无人机，这种协作方式会造成一旦中央处理器出现故障，整个系统将陷入瘫痪，随着代理机的增多，中央模块的计算量增大，通信也需要很大的带宽，可靠性也会降低，整个多无人机系统的鲁棒性很差。

分布式系统是指每个无人机可以从不同的位置出发，无人机需要向周边其他无人机共享自身信息，这种方案每个无人机都能得到一个全局地图，分布式的灵活性和鲁棒性更好。但是和集中式的只需要无人机都和中央处理器保持通信相比较而言，分布式需要保证所有无人机之间通信正常，不适合环境较为复杂的场景，信号遮挡较为严重。同时分布式系统，来自其他无人机的信息全部转化为自身坐标系下的地图，这样就需要每个无人机系统都搭载高速度，计算能力很强的处理器，这本身也会造成资源的浪费。

发明内容

针对多无人机未知环境下的即时定位和地图的构建，本发明提出一种基于互回环检测的协作方式，在有重合地点被检测的同时，融合地图并对全局地图进行优化。

本发明具体通过如下技术方案实现：

一种基于互回环检测的多无人机SLAM地图融合算法，无人机i与无人机j分别独立运动，在进行地图构建的同时进行位姿估计，当中央处理器检测出两个相关联关键帧图片时，在中央处理器执行所述融合算法，得到全局地图；所述算法包括：

S1、对来自不同无人机的关键帧信息，使用BOW算法快速比较其图像特征，检测出两个UAV或者更多UAV到过相同的位置，通过检测建立2个或者更多关键帧之间的关联信息；

S2、当检测出相似场景时，对特征点进行匹配，并且去除错误匹配，然后执行点云地图的融合；此时，来自不同无人机的地图将被合并为一个全局地图；

S3、采用光束平差优化算法对全局地图进行优化，关键帧图像的更新姿势被反馈到每个UAV，以依次改进其局部估计和局部地图。

作为本发明的进一步改进，使用ORB特征代替BOW算法中使用的SIFT特征来优化词袋模型。。

作为本发明的进一步改进，所述步骤S2中，使用增强版迭代最邻近点算法，其中包括曲面法线和切线信息的匹配，改善对运动方程的估算结果；所述增强版迭代最邻近点算法具体包括：当通过相对运动对两组二维或三维点进行配准和对准时，采用迭代连续求解最优值，通过找到下式中所示的最小二乘问题的最优解来建模上述问题：

其中P＝{p_i|i＝1，...，n}和Q＝{q_i|i＝1，...，n}分别是来自不同无人机的3D特征点集，其3D坐标为通过连续迭代旋转矩阵R和平移矩阵t，可以得到运动变换矩阵T＝[R，t]；通过运动变换矩阵T，可以得到从源点集Q到目标点集P的合并映射。

本发明的有益效果是：为了解决现有技术中的单无人机SLAM算法局限性和分布式集中式各自存在的弊端，本发明采用多无人机系统和视觉SLAM相结合，以集中式算法设计出发，设计一种基于多无人机互回环检测来融合地图的算法，通过利用来自不同无人机的地图信息，融合地图，增加回环的个数，提高全了局地图精度和提高整个系统鲁棒性。

附图说明

图1是地图融合示意图；

图2是基于MLC的地图融合总体流程示意图；

图3是单独的UAV的位姿图和点云图；

图4是点云融合地图；

图5是位姿融合地图；

图6是点云数目对比结果；

图7是关键帧数目对比结果。

具体实施方式

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

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

KeyFrame：关键帧图片，与相邻图片具有一定差异性，避免所有采集照片都进行处理；

Link-node：相关节点，相同位置建立关联性的节点；

uavLabel：无人机的编号；

Pose graph：无人机的位姿图，位姿矩阵在图像中的变化。

图1所示的是地图融合的示例，无人机1与无人机2分别单独运动，在进行地图构建的同时进行位姿估计，当中央处理器检测出两个相关联关键帧图片时，在中央处理器执行融合优化算法，得到全局地图；并将全局地图共享到单独的每一个无人机，对每个无人机的定位信息作出调整。

中央处理器可由云服务器承担，无人机和云端保持通信，可以减小因为某个无人机破坏对整个任务的影响。在云端完成了关键帧的检测和融合后，将全局地图反馈到每个无人机，提高整个系统的可靠性和鲁棒性。

本发明提出的基于互回环检测(Mutually Loop Closing，MLC)的地图融合算法，对来自不同无人机的关键帧信息，本发明使用Bag of Word(BOW)技术快速比较其图像特征，检测出两个UAV或者更多UAV到过相同的位置，通过检测建立2个或者更多关键帧之间的关联信息。在特征的选择上，本发明可以使用ORB特征代替BOW算法中使用的SIFT特征来优化词袋模型。当检测出相似场景时，对特征点进行匹配，并且去除错误匹配，然后执行点云地图的融合，使用增强版迭代最邻近点(Iterative Closest Point，ICP)，其中包括曲面法线和切线信息的匹配，改善对运动方程的估算结果。

此时，来自不同无人机的地图将被合并为一个全局地图。在算法的最后，对全局地图进行优化，运行光束平差优化(Bundle Adjustment，BA，又称为捆集优化)算法。关键帧图像的更新姿势被反馈到每个UAV，以依次改进其局部估计和局部地图。

算法的伪代码描述如表1：

表1基于MLC的地图融合算法

当用于通过相对运动对两组二维或三维点进行配准和对准时，ICP可以通过迭代连续求解最优值。可以通过找到式(1)中所示的最小二乘问题的最优解来建模该问题：

其中P＝{p_i|i＝1，...，n}和Q＝{q_i|i＝1，...，n}分别是来自不同无人机的3D特征点集，其3D坐标为通过连续迭代旋转矩阵R和平移矩阵t，可以得到运动变换矩阵T＝[R，t]。通过运动变换矩阵T，可以得到从源点集Q到目标点集P的合并映射。

在ICP程序之后，需要建立不同无人机地图之间的关系。然后，使用这些图像对几何特征点进行三角剖分。构造的三角形可以确定三维点的位置。最后，计算三维点的坐标以重新投影相机矩阵。这个问题可以在式(2)中显示：

其中，R∈SO(3)表示相机方向优化，是相机位置。和分别表示世界坐标和关键点中匹配的3D点之间的最小化重投影误差。单目相机或立体相机i∈χ表示具有所有匹配的集合。基于MLC的地图融合的整体执行流程框图如图2所示。

每个单独无人机的位姿图和点云图如图3所示，根据MLC地图融合算法，两个平台的关键帧数据，基于相互回环检测，地图的融合结果如图4所示，可以清楚地看到通过多机融合获得的地图，多机融合优于独立，并且可以使用地图，观察到的环境信息。

它显示了图4中的合并点云图。根据协同可视SLAM框架合并算法，来自不同代理的点云图被合并为相对完整的全局点云图。在图5中可以看到，通过信息的融合，路径图增加了循环次数，并提高了定位精度。

图6和图7分别描述了单机系统和多机系统中的地图点的数量和生成的关键帧的数量。在相同的环境中，多UAV系统的地图点数量多于独立系统的数量。相反，关键帧的数量少于独立系统的数量。可以得出结论，多机系统可以减少提取的关键帧的数量和算法的复杂性。然而，多机系统具有更多的地图点，更多的融合地图信息和更高的地图精度。

表2描述了闭环的次数，其中，在多机系统中通过多机信息融合可以增加往复循环的数量，从而提高整个系统的定位精度和鲁棒性。

表2闭环次数对比

	单个无人机	多无人机系统
			回环个数	6	13
互回环个数	-	7

综上所述，本发明设计了一种基于互回环检测的地图融合算法，对于关键点检测和地图融合给出了详细的方案和算法描述。算法通过利用来自不同无人机的地图信息，融合地图，增加回环的个数，提高全了局地图精度和提高整个系统鲁棒性。通过对算法的实验和仿真，得出本文算法适用于多无人机系统的SLAM。计算功能更强大的中央服务器会聚合他们各自的位置信息和地图，检测回环和合并地图。最后将合并和优化的信息传递回代理，代理能够对更新的信息进行操作，从而实现更好的一致性估计。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims (3)

1.一种基于互回环检测的多无人机SLAM地图融合算法，无人机i与无人机j分别独立运动，在进行地图构建的同时进行位姿估计，当中央处理器检测出两个相关联关键帧图片时，在中央处理器执行所述融合算法，得到全局地图；其特征在于：所述算法包括：

S1、对来自不同无人机的关键帧信息，使用BOW算法快速比较其图像特征，检测出两个UAV或者更多UAV到过相同的位置，通过检测建立2个或者更多关键帧之间的关联信息；

S2、当检测出相似场景时，对特征点进行匹配，并且去除错误匹配，然后执行点云地图的融合；此时，来自不同无人机的地图将被合并为一个全局地图；

S3、采用光束平差优化算法对全局地图进行优化，关键帧图像的更新姿势被反馈到每个UAV，以依次改进其局部估计和局部地图。

2.根据权利要求1所述的地图融合算法，其特征在于：使用ORB特征代替BOW算法中使用的SIFT特征来优化词袋模型。

3.根据权利要求1所述的地图融合算法，其特征在于：所述步骤S2中，使用增强版迭代最邻近点算法，其中包括曲面法线和切线信息的匹配，改善对运动方程的估算结果；所述增强版迭代最邻近点算法具体包括：当通过相对运动对两组二维或三维点进行配准和对准时，采用迭代连续求解最优值，通过找到下式中所示的最小二乘问题的最优解来建模上述问题：

其中P＝{p_i|i＝1，...，n}和Q＝{q_i|i＝1，...，n}分别是来自不同无人机的3D特征点集，其3D坐标为通过连续迭代旋转矩阵R和平移矩阵t，可以得到运动变换矩阵T＝[R，t]；通过运动变换矩阵T，可以得到从源点集Q到目标点集P的合并映射。