CN106595659A - 城市复杂环境下多无人机视觉slam的地图融合方法 - Google Patents

Abstract

本发明公开了一种城市复杂环境下多无人机视觉SLAM的地图融合方法，该方法包括以下步骤：1）通过安装在各个无人机上的RGB‑D相机采集图像，再利用无人机对图像进行预处理，再进行图像配准；2）构建视觉里程计，实现回环检测；3）优化无人机的位姿；4）构建octomap地图，实现实时在线SLAM；5）将各octomap传输到地面计算机中，将各局部octomap融合为全局octomap，再将融合后的全区域的octomap传输给各无人机；本方法的计算量减少，可实现实时在线SLAM，减少了不稳定的无线传输带来的信息丢失的隐患；同时，缩减了任务执行时间，提高了任务执行效率，降低了无人机续航能力不足带来的隐患，最终可以获得更精准的定位和建立了更精确的地图。

Description

城市复杂环境下多无人机视觉SLAM的地图融合方法

技术领域

本发明属于视觉导航系统领域，具体讲是一种城市复杂环境下多无人机视觉SLAM的地图融合方法。

背景技术

由于无人机具有小型化，易于操纵，控制等优点，广泛应用于城市救灾、地理勘测等场景。基于城市环境的复杂性，GPS信号易于丢失，而惯性测量元件的累积误差使得无人机无法获得精准的自身位姿信息，因此无人机无法实现自主导航。SLAM技术是解决未知环境下导航的关键技术，SLAM技术已经广泛应用于地面机器人、室内机器人。基于SLAM技术的传感器有激光扫描仪，单目相机，立体相机，RGB-D相机。近些年，利用较轻便，价格便宜，信息丰富的RGB-D相机实现视觉SLAM越来越引起行业内的关注。除此之外，丰富的视觉信息还可以用于无人机的目标跟踪，目标检测。

在单无人机视觉SLAM中，考虑到大规模环境下，时间紧迫，无人机的续航能力有限，并且长时间内，无人机获得的图像信息越来越多，视觉SLAM需要处理的图像越多，视觉里程计中计算无人机位姿变换矩阵所需的计算量大，加重无人机处理器的计算负担。单无人机绘制大规模环境地图可能造成占用大量无人机的内存。同时，大规模的环境下，单无人机SLAM视觉里程计的累积误差随着时间增加。然而，多无人机协同SLAM很好地解决了这些问题。多无人机协同技术可将目标区域划分为多个子区域，减轻各无人机自身处理器的计算量，避免了单个无人机执行导航任务的电池续航能力弱的缺点，快速地实现了区域的定位与地图构建，同时获得更精确的定位与地图。

在无人机的应用领域中，要求一个概率表示、对空白区域(可自由移动区域)，占有区域(障碍物)和未知区域建模，在时间和存储空间具有较高的效率的地图。其中：概率表示：无人机通过RGB-D相机感知环境，测量值受不确定性因素影响，例如：动态物体。当要求从嘈杂的环境中创建环境的精确模型时，必须概率地考虑潜在的不确定。多个不确定性的测量结果可以融合成鲁棒性地估计正确的结果。除此之外，此种概率的表示还可以融合来自于其他传感器的数据。对未知区域建模：在自主导航任务中，无人机可以由相机确定占有区域和空闲区域，规划无碰撞路径。未知区域认为是占有区域，需要避免出现在规划的路线上，因此需要对未知区域进行建模。高效率性：无人机可以路径规划和执行任务的关键是地图，同时，地图需要在时间和内存两方面具有高效率。在3D建图领域，内存消耗是一个大问题。因此，在大规模的环境中此地图必须有效地、节省内存地存储。

octomap(概率占有栅格地图)满足上述三种要求，octomap形式紧凑，占内存较小，可以用于导航，方便压缩，更新且分辨率可调。因此采用octomap地图用于无人机的自主导航是很好地选择。

综上分析：多无人机的视觉SLAM是一项军事、救灾领域的重大课题。目前，国内的多无人机视觉SLAM技术还处于初级阶段。因此，如何实现在线、实时的多无人机视觉SLAM用于导航是无人机的前沿技术，同时，面对城市复杂环境下，能够有效的缩小地图内存，减少无人机计算量，提高任务执行的效率的自主导航也是本领域技术人员厄待发展的技术。

发明内容

针对现有技术中存在的问题，本发明公开了一种城市复杂环境下多无人机视觉SLAM的地图融合方法，本发明利用多个无人机搭建的视觉SLAM系统，利用无线发送设备将构建的各octomap发送给地面PC，用迭代最近点算法进行融合,将融合后的全局octomap传送给无人机，用于无人机的自主导航。

本发明是这样实现的，一种城市复杂环境下多无人机视觉SLAM的地图融合方法，该方法的具体步骤如下：

步骤一，通过安装在各个无人机上的RGB-D相机采集图像，再利用无人机对图像进行预处理，再进行图像配准；

步骤二，构建视觉里程计(visual odometry)，实现回环检测；

步骤三，优化无人机的位姿，SLAM的处理方法主要包含滤波和图优化。常见的滤波方法有扩展卡尔曼滤波(EKF)，粒子滤波等。滤波方法的SLAM是递增、实时的处理数据并矫正无人机的位姿。在RGB-D SLAM中，使用滤波方法，随着时间的推移，地图扩大，内存消耗与计算量大，利用图优化方法可以保证地图的高精度与快效率，所以，选择图优化工具；利用通用的图优化工具优化无人机的位姿，尽可能满足各位姿之间的约束条件，实现优化无人机位姿的目的；

步骤四，构建octomap地图，利用迭代最近点算法实现关键帧对应的点云地图的融合，利用点云地图构建octomap地图，实现实时在线SLAM，建立了概率占有栅格地图octomap，此地图可用于无人机的自主导航及给工作人员提供环境信息；

步骤五，将各octomap传输到地面计算机中，将各局部octomap融合为全局octomap，再将融合后的全区域的octomap传输给各无人机，此全区域地图可用于路径规划，自主导航。

进一步，所述的步骤一具体为：

1.1，该系统的硬件设备包括多架无人机，RGB-D相机，无线传输模块将需要执行SLAM的全区域划分为多个子区域，在各子区域中，利用安装在多架无人机中的RGB-D相机ZED采集关于环境的视频流，获取关于环境的每帧的RGB图像和深度图；本发明将任务区域划分为多个子区域，每个无人机在该子区域进行视觉SLAM,使得单个无人机的计算量相对减少，可实现在线实时视觉SLAM，减少了不稳定的无线传输带来的信息丢失的隐患；

1.2，利用无人机的机载ARM处理芯片，对获得的RGB图像进行预处理；

1.3，将RGB-D图像转化为点云；RGB-D相机使用针孔相机模型，空间点三维坐标[x,y,z]和它在图像中的像素坐标[u,v,d]的对应关系为：

式中，f_x,f_y分别表示相机在x,y两个轴上的焦距，c_x,c_y指相机的光圈中心，S指深度

图的缩放因子，即深度图中数据与实际距离的比例；

根据上述式子求得空间点的三维坐标x,y,z如下：

1.4，图像配准；首先进行特征提取与匹配，利用最近邻算法匹配特征点，再利用随机采样一致性算法剔除误匹配，删除外点；

1.5，在获得良好的特征匹配的前提下，利用迭代最近点算法获得相邻两帧之间的变换矩阵。

进一步，所述的空间点与像素点坐标关系还可用矩阵模型表示，对应关系如下：

其中C称为相机的内参，R称为相机的外参，R和t是相机的姿态；R代表旋转矩阵，t代表位移矢量。

进一步，所述的迭代最近点算法的过程如下：

相邻两帧为F₁和F₂，将两邻两帧的SIFT特征点表示为：

两帧之间的变换矩阵由旋转矩阵R和位移变化向量t组成，因此存在下式：

由于误差的存在，使得左端不等于右端，通过优化下式求得R，t；

将求得R，t用于匹配P，Q中的特征点，获得新的对应点关系；再优化误差项求得新的R,t；依次循环直到到达设定的循环次数或者R,t不再发生改变为止。

进一步，所述的步骤二具体为：选取第一帧作为关键帧，定义无人机运动大小为位移与旋转的范数之和，当无人机运动大小在一定的范围时，选取此刻该无人机位姿对应的帧作为一个关键帧。求取所有相邻两关键帧之间的变换矩阵，将构成视觉里程计。对于增加的新的关键帧，与关键帧序列的末尾m个关键帧进行匹配，若匹配成功则在图优化中增加一条边，从而实现回环检测；利用回环检测识别之前经过的场景，构建连续一致性地图，矫正地图的误差漂移。无人机在实际应用场景中将无可避免遇到闭合场景，利用回环检测功能可将之间遇到的相同环境识别，构成全区域一致性闭合环境地图，也进一步了缩小无人机位姿误差。

进一步，所述的步骤三具体为：利用通用的图优化工具优化无人机的位姿，优化的目标

函数为：

式中，x_i，x_j分别代表无人机在i，j时刻对应的无人机位姿，T_i,j代表位姿变化矩阵，表示无人机从位姿x_j转化为x_i。e_ij为x_i与x_j经过变换后的差值，表示两者之间的误差。Ω_ij为权重矩阵，又称为信息矩阵，代表传感器测量过程中对各测量值的信任程度。

进一步，所述的步骤四具体为：

4.1，无人机采集的RGB-D图像转化为点云地图，利用获得的各帧之间的变换矩阵，获

得连续一致的融合后的局部点云地图；

4.2，将点云地图构建为octomap地图。

进一步，所述的步骤五具体为：

5.1，将各无人机的局部octomap地图通过无线传输模块传送给地面PC；

5.2，将各局部octomap利用迭代最近点算法融合为全局octomap；

5.3，将全局octomap通过无线传输模块传递给各无人机，可用于路径规划，导航。

本发明相较于现有技术的有益效果在于：

(1)本发明将任务区域划分为多个子区域，每个无人机在一个子区域进行视觉SLAM,单个无人机的计算量相对减少，可实现在线实时视觉SLAM，减少了不稳定的无线传输带来的信息丢失的隐患。同时，缩减了任务执行时间，提高了任务执行效率，降低了无人机续航能力不强带来的隐患。

(2)相比于单无人机视觉SLAM，视觉里程计中的无人机位姿数量减少，求取的位姿变换矩阵减少，位姿变换矩阵的累积误差减少，因此获得了更精准的定位和建立了更精确的地图。

(3)同时，相比于单无人机SLAM，多无人机SLAM将定位与构图的任务分散到各子无人机，分散了无人机自身处理器的计算量和存储地图所需的空间，使得效率提高。

(4)通过本发明的方法可以获得更精准的定位和建立了更精确的octomap，并且该octomap地图形式紧凑，占内存较小，可以用于导航，方便压缩，更新且分辨率可调。此地图可用于无人机的视觉导航及提供环境信息给工作人员。

附图说明

图1是本发明的视觉SLAM各模块框图；

图2是本发明的实施例中RGB-D图像转化为点云时的透视投影图；

图3是单无人机视觉SLAM原理框图；

图4是本发明的多无人机协同视觉SLAM原理框图。

具体实施方式

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

本设计发明了一种城市复杂环境下多无人机视觉SLAM的地图融合方法，包括如下步骤：

如图1所示，利用安装在无人机中的RGB-D相机ZED采集关于环境的视频流，获取关于环境的每帧的RGB图像和深度图；利用无人机的机载ARM处理芯片，对获得的RGB图像进行预处理，利用高斯滤波滤除噪声。

RGB-D图像转化为点云；RGB-D相机使用针孔相机模型，透视投影关系如图2所示。空间点[x,y,z]和它在图像中的像素坐标[u,v,d]的对应关系为：

式中，f_x,f_y分别表示相机在x,y两个轴上的焦距，c_x,c_y指相机的光圈中心，S指深度图的缩放因子，即深度图中数据与实际距离的比例。

根据上述式子求得x,y,z如下：

根据上式求得空间点的三维坐标。

空间点与像素点坐标关系还可用矩阵模型表示如下：

其中C称为相机的内参，R和t是相机的姿态；R代表旋转矩阵，t代表位移矢量，R称为相机的外参。

图像配准；首先进行特征提取与匹配；如图3所示，提取SIFT(尺度不变特征变换)特征。提取SIFT特征分为计算关键点，根据关键点周围的像素计算128维描述子。当两个关键点的描述子越相似，说明两个关键点越匹配。利用最近邻算法匹配特征点。由于基于最近邻的匹配存在误差，因此利用随机采样一致性算法剔除误匹配，删除外点。

如图3所示，在获得良好的特征匹配的前提下，利用迭代最近点算法获得相邻两帧之间的变换矩阵。

迭代最近点算法的过程如下：假设有相邻两帧：F₁和F₂，由上述提取的两邻两帧的SIFT特征点表示为：

假设两帧之间的变换矩阵由旋转矩阵R和位移变化向量t组成。因此存在下式：

由于误差的存在，使得左端不等于右端；通过优化下式求得R，t。

将求得R，t用于匹配P，Q中的特征点，获得新的对应点关系；再优化误差项求得新的R,t，依次循环直到到达设定的循环次数或者R,t不再发生改变为止。

根据上述步骤，求取所有相邻两帧之间的变换矩阵，将构成视觉里程计。在机器人学与计算机视觉领域，视觉里程计是一个通过分析相关图像序列，来确定机器人位置和姿态的过程。视觉里程计利用连续的图像序列来估计机器人位姿。

回环检测；回环检测有以下两种方法：在先前获取的关键帧中随机选择n个帧的近距离回环和在先前获取的关键帧中选取前n个帧的随机回环。

至此，SLAM的基本流程包括以下两个主要方面：1)提取关键帧，2)回环检测。在视觉里程计中，帧与帧之间的距离很近，因此不必耗费时间去计算每帧时无人机的位姿和拼接点云地图，且有利于实现实时在线点云地图显示。在SLAM流程中，当无人机的运动超过一定间隔，运动一段距离时，增加一个关键帧。所有关键帧对应的无人机位姿构成所有位姿信息，所有关键帧对应的点云图融合构成全局点云地图。

利用通用的图优化(general graph optimization，简称g2o)优化无人机的位姿。图优化可分为前端(front-end)和后端(back-end)。前端构建图为视觉里程计的过程，而后端优化无人机的位姿使得尽量符合边的约束条件。

一个图由节点与边构成：

G＝{V,E} (7)

V代表节点，E代表连接两节点的边。在无人机SLAM中，V代表相机的各个位姿表示为：

E代表两邻两位姿之间的位姿变换矩阵，可表示为：

由于通过特征提取匹配，利用迭代最近点算法求取的关键帧之间的变换矩阵T_i,j存在误差，所以通过变换矩阵求得的无人机位姿与初始设定的无人机的初始值之间存在误差，因此可以优化一个不一致性误差：

其中为x_i的估计值。考虑到传感器的测量受到高斯白噪声的影响，所有测量值的分布可以看做为高斯分布。倘若测量值为多变量的，则为多元高斯分布。测量值中的各变量的可信度不一致，构成了各测量值的权重，所有权重值构成权重矩阵，也称为信息矩阵。信息矩阵为多测量变量的协方差矩阵的逆。协方差越小，表示此次测量越准确，越值得相信，信息权重越大。因此优化的目标函数变为：

由以上可知上述优化目标函数利用最小二乘法求解，由于无人机位姿之间的变化函数不是线性的，所以上述优化问题是一个非线性最小二乘问题。利用Guass-Newton迭代法求解。

局部的点云地图；将此无人机采集的RGB-D图像转化为点云地图，利用获得的各帧之间的变换矩阵，获得连续一致的融合后的局部点云地图。

将点云地图构建为octomap地图。在无人机的应用领域中，要求一个概率表示、对空白区域(可自由移动区域)，占有区域(障碍物)和未知区域建模，在时间和存储空间具有较高的效率。构建的octomap地图满足上述三种要求，形式紧凑，占内存较小，可以用于导航，方便压缩，更新且分辨率可调。

如图4所示，将各无人机的局部octomap地图通过无线传输模块传送给地面PC，将各局部octomap利用迭代最近点算法融合为全局octomap。两张octomap地图利用迭代最近点算法进行融合步骤如下：首先，提取octomap中的三维点分别构成两个元素为三维空间点的集合P＝{p₁,p₂，…,p_n}，Q＝{q₁,q₂,…,q_n}；假设两张octomap地图的变换矩阵由旋转矩阵R和位移变化向量t组成。因此存在下式：

由于误差的存在，通过最小二乘法求得R，t，具体表示为优化下式：

因此利用R，t，将Q中对应的三维信息转化为P中。最终将P中的点云地图用octomap形式表示出来。

因此，借鉴两张octomap地图的融合方式，选取第一张octomap地图作为参考，通过上述方法获得其余octomap地图转化到第一张octomap地图的变化矩阵，从而获得一张大的三维点云地图，再表示为octomap地图形式。

获得的全局octomap通过无线传输模块传递给各无人机，可用于路径规划，导航。

以上所述，仅为本发明的较佳实施例，并非用来限定本发明的范围，凡依本发明所做的均等变化与修饰，皆为本发明专利范围所涵盖。

Claims (8)

1.一种城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，该方法包括以下步骤：

步骤一，通过安装在各个无人机上的RGB-D相机采集图像，再利用无人机对图像进行预处理，再进行图像配准；

步骤二，构建视觉里程计，实现回环检测；

步骤三，优化无人机的位姿；

步骤四，构建octomap地图，实现实时在线SLAM；

步骤五，将各octomap传输到地面计算机中，将各局部octomap融合为全局octomap，

再将融合后的全区域的octomap传输给各无人机。

2.根据权利要求1所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的步骤一具体为：

1.1，将需要执行SLAM的全区域划分为多个子区域，在各子区域中，利用安装在无人机中的RGB-D相机ZED采集关于环境的视频流，获取关于环境的每帧的RGB图像和深度图；

1.2，利用无人机的机载ARM处理芯片，对获得的RGB图像进行预处理；

1.3，将RGB-D图像转化为点云；RGB-D相机使用针孔相机模型，空间点三维坐标[x,y,z]和它在图像中的像素坐标[u,v,d]的对应关系为：

$\begin{array}{c}u=\frac{x\·f_x}{z}+cx\\ v=\frac{y\·f_y}{z}+c_y\\ d=z\·s\end{array}---\left(1\right)$

式中，f_x,f_y分别表示相机在x,y两个轴上的焦距，c_x,c_y指相机的光圈中心，S指深度图的缩放因子，即深度图中数据与实际距离的比例；

根据上述式子求得空间点的三维坐标x,y,z如下：

z＝d/s

x＝(u-c_x)·z/f_x

y＝(v-c_y)·z/f_y (2)

1.4，图像配准；首先进行特征提取与匹配，利用最近邻算法匹配特征点，再利用随机采样一致性算法剔除误匹配，删除外点；

1.5，在获得良好的特征匹配的前提下，利用迭代最近点算法获得相邻两帧之间的变换矩阵。

3.根据权利要求2所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的空间点与像素点坐标关系还可用矩阵模型表示，对应关系如下：

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=C\·\left(R\·\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+t\right)---\left(3\right)$

其中C称为相机的内参，R称为相机的外参，R和t是相机的姿态；R代表旋转矩阵，t代表位移矢量。

4.根据权利要求2所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的迭代最近点算法的过程如下：

其中一组相邻两帧为F₁和F₂，将两邻两帧的SIFT特征点表示为：

$\begin{array}{c}P=\{p_1,p_2,\mathrm{...},p_n\}\∈F_1\\ Q=\{q_1,q_2,\mathrm{...},q_n\}\∈F_2\end{array}--\left(4\right)$

两帧之间的变换矩阵由旋转矩阵R和位移变化向量t组成，因此存在下式：

$p_i={\mathrm{Rq}}_i+t,\∀i\∈\{1,2,...,n\}---\left(5\right)$

通过优化下式求得R，t；

$\underset{R,t}{min}\underset{i=1}{\overset{N}{\Σ}}\left|\right|p_i-({\mathrm{Rq}}_i+t)|{|}_2---\left(6\right)$

将求得R，t用于匹配P，Q中的特征点，获得新的对应点关系；再优化误差项求得新的R,t；依次循环直到到达设定的循环次数或者R,t不再发生改变为止。

5.根据权利要求4所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的步骤二具体为：求取所有相邻两帧之间的变换矩阵，将构成视觉里程计，实现回环检测。

6.根据权利要求5所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的步骤三具体为：利用通用的图优化工具优化无人机的位姿，优化的目标函数为F，求得最小目标函数对应的无人机位姿x^*：

$\begin{array}{c}{e}_{ij}(x_i,x_j)=x_i-T_{i,j}{x}_j\\ F=\underset{(i,j)\∈C}{\Σ}{e}_{ij}{(x_i,x_j)}^T{\mathrm{\Ω}}_{ij}{e}_{ij}(x_i,x_j)\\ x^{*}=\arg \underset{x}{\min }F\left(x\right)\end{array}---\left(7\right)$

式中，x_i，x_j分别代表无人机在i，j时刻对应的无人机位姿，T_i,j代表位姿变化矩阵，表示无人机从位姿x_j转化为x_i，e_ij为x_i与x_j经过变换后的差值，表示两者之间的误差，Ω_ij为权重矩阵，又称为信息矩阵，代表传感器测量过程中对各测量值的信任程度。

7.根据权利要求6所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的步骤四具体为：

4.1，无人机采集的RGB-D图像转化为点云地图，利用获得的各帧之间的变换矩阵，获得连续一致的融合后的局部点云地图；

4.2，将点云地图构建为octomap地图。

8.根据权利要求7所述的城市复杂环境下多无人机视觉SLAM的地图融合方法，其特征在于，所述的步骤五具体为：

5.1，将各无人机的局部octomap地图通过无线传输模块传送给地面PC；

5.2，将各局部octomap利用迭代最近点算法方法融合为全局octomap；

5.3，将全局octomap通过无线传输模块传递给各无人机。