CN111024078B - 基于gpu加速的无人机视觉slam方法 - Google Patents

Abstract

本发明公开了一种基于GPU加速的无人机视觉SLAM方法，首先进行相机图像信息的读取和预处理；执行前端任务：采用特征点法估计位姿；包括：相机图像的特征提取和特征匹配，采用ICP算法估算位姿；进行后端优化：基于图优化的后端优化进而优化位姿；进行词袋回环检测，构建和优化位姿图；其中步骤相机图像的特征提取和特征匹配在GUP上进行，相机图像的特征提取和特征匹配之后的步骤使用OpenCV实现CUDA并行化。本发明方法在现有的嵌入式小型GPU的基础上完成视觉SLAM前端部分的特征提取和特征匹配任务，SLAM算法的其它部分集中在CPU进行并行计算，在不降低定位精度的前提下，提高算法计算速度，减少无人机的体积与重量。

Description

基于GPU加速的无人机视觉SLAM方法

技术领域

本发明涉及无人机技术领域，尤其涉及基于GPU加速的无人机视觉SLAM方法。

背景技术

SLAM(Simultaneous localization and mapping)是即时定位与地图构建技术，利用摄像机和计算机视觉算法为机器人系统提供状态估计，在小型移动机器人、自动驾驶汽车和无人机中变得越来越流行。

在室外运行的无人机需要与执行SLAM计算的系统进行实时通信，脱机处理可以减小无人机的尺寸，从而增加飞行时间，但由于无人机需要工作在SLAM处理系统的无线通信范围内，该限制会对飞行范围产生严重影响。为了消除对飞行范围的影响，SLAM处理放在无人机上进行。然而，基于视觉图像处理的前端和后端的位姿优化相对来说计算量大，资源受限，很难在嵌入式控制器中实现实时运行。但是通过增加处理硬件，由于载重量和大小会对无人机的飞行时间和机动性造成影响。因此减小机载处理系统的体积、功耗和计算时间，对移动机器人，特别是无人机具有重要意义。

发明内容

本发明为解决现有应用于无人机的视觉SLAM方法存在需减小机载处理系统的体积、功耗和计算时间等问题，提供了一种基于GPU加速的无人机视觉SLAM方法。

为实现以上发明目的，而采用的技术手段是：

基于GPU加速的无人机视觉SLAM方法，包括以下步骤：

S1.相机图像信息的读取和预处理；所述相机为双目摄像头；

S2.前端任务：采用特征点法估计位姿；包括：相机图像的特征提取和特征匹配，采用ICP算法估算位姿；

S3.后端优化：基于图优化的后端优化进而优化位姿；

S4.进行词袋回环检测，构建和优化位姿图；

其中步骤S2中相机图像的特征提取和特征匹配在GUP上进行，所述步骤S2中相机图像的特征提取和特征匹配之后的步骤使用OpenCV实现CUDA并行化。

优选的，所述预处理为对读取得到的相机图像进行灰度化处理。

优选的，所述步骤S2中相机图像的特征提取和特征匹配具体包括以下步骤：

S21.对处理后的灰度图构建图像金字塔，并为每层金字塔图像分配应提取的特征点数量；

S22.进行各层金字塔图像的特征提取：首先对每层金字塔图像进行边界处理，在金字塔图像上划分网格后对每一网格进行特征点提取：选定在所述网格中的任一像素为待检测像素，以待检测像素为圆心，r为半径作一个圆，与该圆相交的共有n个像素，若所述n个像素中有连续N个像素与待检测像素灰度值的差大于预设阈值T，则认为该待检测像素为特征点；然后循环检测每一个像素得到所有特征点；其中一般N取11或9；

S23.对步骤S22提取得到的特征点进行剔除与均匀分布的优化；

S24.对于步骤S23得到的任一特征点，定义邻域像素的矩为：

其中I(x,y)为点x,y处的灰度值；

得到金字塔图像的质心为：

连接金字塔图像块的几何中心O与质心C，得到一个方向向量

则特征点的方向定义为：

θ＝arctan(m₀₁/m₁₀)

S25.利用rBRIEF算法计算每个特征点的描述子，实现所述金字塔图像中当前帧与参考帧的特征点匹配。

优选的，所述步骤S2中采用ICP算法估算位姿的具体步骤包括：

通过ICP算法，利用非线性方法估算出帧间位姿变换(R,T)，基于特征点的深度信息构建最小二乘问题：

把当前位姿作为优化变量，采用Bundle Adjustment为核心的图优化方法进行优化，通过不断迭代的方法估算当前位姿。

优选的，所述步骤S3的具体步骤包括：

对于SLAM过程的观测方程，其整体的代价函数为：

其中z_ij为在位姿ξ_i处观察路标pj产生的观测数据，ei_j为观测值与测量值的误差；

对上式的最小二乘进行求解，其中以位姿和路标是优化变量，采用BundleAdjustment非线性优化方法，不断寻找下降方向Δx以求得上式的最优解，即给优化变量一个增量，并对上式进行一次泰勒展开后简化为：

其中F_ij表示整体代价函数在当前状态下对相机姿态的偏导，E_ij表示整体代价函数对路标点位置的偏导；

将位姿变量放到一起：

将空间点的变量放到一起：

简化为：

其中Δx_c和Δx_p分别为所有位姿变量整合到一起的整体增量、空间点的变量整合到一起的整体增量；雅克比矩阵F和E是整体代价函数对所述整体增量的导数；

展开得到增量线性方程：

HΔx＝g

使用LM方法求解则：H＝J^TJ+λI，而J＝[FE]，g＝-J(x)^Tf(x)；

其中I为单位阵，λ为正实数；当参数λ较小时，H占主导地位，LM方法更接近高斯牛顿法，当λ较大时，λI占主导，LM方法更接近于最速下降法。

优选的，所述步骤S4具体包括以下步骤：

S41.利用词袋模型计算图像间的相似度加速回环发生的识别，通过动态计算回环帧筛选阈值，确定回环关键帧；对当前关键帧与回环关键帧计算Sim3变换得到回环检测结果；

S42.根据步骤S41的回环检测结果进行运行轨迹的纠正，构建得到轨迹符合回环检测结果的位姿图。

优选的，所述步骤S41中图像间相似度的计算步骤为：

基于所述词袋模型并使用TF-IDF算法进行计算：

设训练词典时所有特征数量为n，图像中单词w_i数量为n_i，则单词的IDF为：

设图像中单词w_i出现了n_i次，一共出现的单词数为n，则TF为：

得到单词w_i的权重为：

n_i＝TF_i×IDF_i

对于每一幅图像具有多个特征点，每个特征点分别对应上词典中的某个单词并赋予权重，组成属于该图像的Bag-of-words，其中N为所述词典中的单词数，表示如下：

设另一幅图像的Bag-of-words为υ_B，则两幅图像的相似度为：

其中N为所述词典中的单词数，υ_A为一幅图像的Bag-of-words，υ_B为另一幅图像的Bag-of-words。

优选的，所述步骤S41中确定回环关键帧的具体步骤包括：

将与当前帧相连的局部关键帧剔除后遍历所有关键帧；

找出与当前关键帧具有相同单词的关键帧，并统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80％设置为第一阈值；

找出所有单词数超过所述第一阈值，且相似度检测大于相邻关键帧最低分数的关键帧；将其与自己相邻最紧密的前10个关键帧设定为一组；即所述组以每一个关键帧为中心，加上与中心的关键帧相邻的关键帧所形成，一个关键帧出现在一个或多个组内；

计算分组后每组的总得分以及每组得分最高的关键帧，以组得分最高的0.75作为第二阈值，找出高于所述第二阈值的所有组里面得分最高的帧，作为回环关键帧。

与现有技术相比，本发明技术方案的有益效果是：

本发明中采用了基于OBR特征提取以及结合词袋模型进行特征匹配判断的方法，减少了视觉SLAM前端处理的运算量，以及计算所需的硬件资源；另外将视觉SLAM前端图像处理的特征提取和特征匹配部分进行了CUDA并行化并调用GPU进行加速，减轻CPU的计算负担，提升处理速度，减小机载处理系统的体积、功耗和计算时间，实现无人机的实时视觉SLAM。

附图说明

图1为本发明方法的整体流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

在本实施例中，为减少无人机的体积和载重，选择Nvidia Jetson TX2作为视觉SLAM算法执行机载处理器进行运算和图像处理；Nvidia Jetson TX2核心模块加上载板重量不到300g，大大降低了无人机的载重负荷；同时Nvidia Jetson TX2内含ARM Cortex-A57(四核)和Nvidia Denver2(双核)处理核心、256个CUDA核心Pascal TM架构设计，可以满足本发明的GPU加速的要求。

下面对本实施例提供的基于GPU加速的无人机视觉SLAM方法进行说明，如图1所示，包括以下步骤：

S1.相机图像信息的读取和预处理；所述相机为双目摄像头；其中的预处理为对读取得到的相机图像进行灰度化处理。本实施例选用的双目摄像头的分辨率为752*480，并可以达到60fps的采集频率，基线长度为120mm；

S2.前端任务：采用特征点法估计位姿；包括：相机图像的特征提取和特征匹配，采用ICP算法估算位姿；其中步骤S2中相机图像的特征提取和特征匹配在GUP上进行，所述步骤S2中相机图像的特征提取和特征匹配之后的步骤使用OpenCV实现CUDA并行化；

其中相机图像的特征提取和特征匹配具体包括以下步骤：

S21.对处理后的灰度图构建图像金字塔，并为每层金字塔图像分配应提取的特征点数量；CUDA并行化后，该处使用OpenCV CUDAGpuMat方法对尺度金字塔进行预计算；

S22.进行各层金字塔图像的特征提取：首先对每层金字塔图像进行边界处理，在金字塔图像上划分网格后对每一网格进行特征点提取：选定在所述网格中的任一像素为待检测像素，以待检测像素为圆心，r为半径作一个圆，与该圆相交的共有16个像素，若所述16个像素中有连续12个像素与待检测像素灰度值的差大于预设阈值T，则认为该待检测像素为特征点；其中T设为原点像素灰度值的20％；然后循环检测每一个像素得到所有特征点；CUDA并行化后，对于每层金字塔，都会启动一个CUDA异步内核，以便在块上快速运行；

S23.对步骤S22提取得到的特征点进行剔除与均匀分布的优化；

S24.对于步骤S23得到的任一特征点，定义邻域像素的矩为：

其中I(x,y)为点x,y处的灰度值；

得到金字塔图像的质心为：

连接金字塔图像块的几何中心O与质心C，得到一个方向向量

则特征点的方向定义为：

θ＝arctan(m₀₁/m₁₀)

S25.利用rBRIEF算法计算每个特征点的描述子，实现所述金字塔图像中当前帧与参考帧的特征点匹配；CUDA并行化后，对于每层金字塔，都会启动一个CUDA异步内核来计算关键点的描述子。

其中采用ICP算法估算位姿的具体步骤包括：

通过ICP算法，利用非线性方法估算出帧间位姿变换(R,T)，基于特征点的深度信息构建最小二乘问题：

把当前位姿作为优化变量，采用Bundle Adjustment为核心的图优化方法进行优化，通过不断迭代的方法估算当前位姿。

S3.后端优化：本实施例采用了基于图优化的后端优化，由于视觉SLAM的前端视觉里程计仅给出一个短时间内的轨迹和地图，不可避免的会存在着误差，这样随着时间的延长会导致地图不够精确，因此在后端优化中考虑一段更长时间内的状态估计问题，在SLAM中的运动方程和观测方程都是非线性函数，因此在本实施例中采用非线性方法表述此类问题，具体步骤包括：

对于SLAM过程的观测方程，其整体的代价函数为：

其中z_ij为在位姿ξ_i处观察路标p_j产生的观测数据，e_ij为观测值与测量值的误差；

对上式的最小二乘进行求解，其中以位姿和路标是优化变量，即通过迭代的方法不断的对位姿和路标进行调整，采用Bundle Adjustment非线性优化方法，不断寻找下降方向Δx以求得上式的最优解，即给优化变量一个增量，并对上式进行一次泰勒展开后简化为：

其中F_ij表示整体代价函数在当前状态下对相机姿态的偏导，E_ij表示整体代价函数对路标点位置的偏导；

将位姿变量放到一起：

将空间点的变量放到一起：

简化为：

其中Δx_c和Δx_p分别为所有位姿变量整合到一起的整体增量、空间点的变量整合到一起的整体增量；雅克比矩阵F和E是整体代价函数对所述整体增量的导数；因为后端优化考虑一个更长时间的状态估计，这是一个非常大的矩阵，其中中由每个误差项的导数拼凑起来的；

展开得到增量线性方程：

HΔx＝g

使用LM方法求解则：H＝J^TJ+λI，而J＝[FE]，g＝-J(x)^Tf(x)；其中I为单位阵，λ为正实数；

此处的H矩阵是一个很大的矩阵，直接求逆非常消耗计算资源，但是该矩阵稀疏矩阵有一定的特殊结构，利用这个矩阵的稀疏性进行边缘化可以加速求解过程。

S4.进行词袋回环检测，构建和优化位姿图；具体包括：

S41.所述步骤S41中图像间相似度的计算步骤为：

基于所述词袋模型并使用TF-IDF算法进行计算：

设训练词典时所有特征数量为n，图像中单词w_i数量为n_i，则单词的IDF为：

设图像中单词w_i出现了n_i次，一共出现的单词数为n，则TF为：

得到单词w_i的权重为：

n_i＝TF_i×IDF_i

对于每一幅图像具有多个特征点，每个特征点分别对应上词典中的某个单词并赋予权重，组成属于该图像的Bag-of-words，其中N为所述词典中的单词数，表示如下：

设另一幅图像的Bag-of-words为υ_B，则两幅图像的相似度为：

其中N为所述词典中的单词数，υ_A为一幅图像的Bag-of-words，υ_B为另一幅图像的Bag-of-words。

其中确定回环关键帧的具体步骤包括：

将与当前帧相连的局部关键帧剔除后遍历所有关键帧；

找出与当前关键帧具有相同单词的关键帧，并统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80％设置为第一阈值；

找出所有单词数超过所述第一阈值，且相似度检测大于相邻关键帧最低分数的关键帧；将其与自己相邻最紧密的前10个关键帧设定为一组；即所述组以每一个关键帧为中心，加上与中心的关键帧相邻的关键帧所形成，一个关键帧出现在一个或多个组内；

计算分组后每组的总得分以及每组得分最高的关键帧，以组得分最高的0.75作为第二阈值，找出高于所述第二阈值的所有组里面得分最高的帧，作为回环关键帧；

S42.根据步骤S41的回环检测结果进行运行轨迹的纠正，构建得到轨迹符合回环检测结果的位姿图。由于位姿的估计往往是一个递推的过程，即由上一帧位姿解算当前帧位姿，因此其中的误差便在这个递推过程中一帧一帧地传递下去，从而造成累计误差。为了消除或减少这里的累积误差，进行运行轨迹的纠正，构建位姿图对位姿进行优化。

其中在本实施例中使用OpenCV实现CUDA并行化，并行化须为CUDA追踪块ID和线程ID。CUDA在同一个代码块中有许多线程，与同一块中的其他线程相比每个线程都有它ID，它们具有唯一标识符，每个代码块都有它拥有唯一的ID，以便每个代码块中的线程不会被意外地视为线程另一个代码块。对于每个块的大小，它可以显示每个块中有多少线程。线程被放置到块中以存储线程跟踪信息，而不需要使用大量内存。

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (6)

1.基于GPU加速的无人机视觉SLAM方法，其特征在于，包括以下步骤：

S1.相机图像信息的读取和预处理；所述相机为双目摄像头；

S2.前端任务：采用特征点法估计位姿；包括：相机图像的特征提取和特征匹配，采用ICP算法估算位姿；

S3.后端优化：基于图优化的后端优化进而优化位姿；

S4.进行词袋回环检测，构建和优化位姿图；

其中步骤S2中相机图像的特征提取和特征匹配在GPU上进行，所述步骤S2中相机图像的特征提取和特征匹配之后的步骤使用OpenCV实现CUDA并行化；

所述预处理为对读取得到的相机图像进行灰度化处理；

所述步骤S2中相机图像的特征提取和特征匹配具体包括以下步骤：

S21.对处理后的灰度图构建图像金字塔，并为每层金字塔图像分配应提取的特征点数量；CUDA并行化后，该处使用OpenCV CUDAGpuMat方法对尺度金字塔进行预计算；

S22.进行各层金字塔图像的特征提取：首先对每层金字塔图像进行边界处理，在金字塔图像上划分网格后对每一网格进行特征点提取：选定在所述网格中的任一像素为待检测像素，以待检测像素为圆心，r为半径作一个圆，与该圆相交的共有n个像素，若所述n个像素中有连续N个像素与待检测像素灰度值的差大于预设阈值T，则认为该待检测像素为特征点；然后循环检测每一个像素得到所有特征点；CUDA并行化后，对于每层金字塔，都会启动一个CUDA异步内核，以便在块上快速运行；

S23.对步骤S22提取得到的特征点进行剔除与均匀分布的优化；

S24.对于步骤S23得到的任一特征点，定义邻域像素的矩为：

m_pq＝∑_x,yx^py^qI(x,y)，p,q＝{0,1}

其中I(x,y)为点x,y处的灰度值；

得到金字塔图像的质心为：

连接金字塔图像块的几何中心O与质心C，得到一个方向向量

则特征点的方向定义为：

θ＝arctan(m₀₁/m₁₀)

S25.利用rBRIEF算法计算每个特征点的描述子，实现所述金字塔图像中当前帧与参考帧的特征点匹配；CUDA并行化后，对于每层金字塔，都会启动一个CUDA异步内核来计算关键点的描述子。

2.根据权利要求1所述的基于GPU加速的无人机视觉SLAM方法，其特征在于，所述步骤S2中采用ICP算法估算位姿的具体步骤包括：

通过ICP算法，利用非线性方法估算出帧间位姿变换(R,T)，基于特征点的深度信息构建最小二乘问题：

把当前位姿作为优化变量，采用Bundle Adjustment为核心的图优化方法进行优化，通过不断迭代的方法估算当前位姿。

3.根据权利要求2所述的基于GPU加速的无人机视觉SLAM方法，其特征在于，所述步骤S3的具体步骤包括：

对于SLAM过程的观测方程，其整体的代价函数为：

其中z_ij为在位姿ξ_i处观察路标p_j产生的观测数据，e_ij为观测值与测量值的误差；

对上式的最小二乘进行求解，其中以位姿和路标是优化变量，采用Bundle Adjustment非线性优化方法，不断寻找下降方向Δx以求得上式的最优解，即给优化变量一个增量，并对上式进行一次泰勒展开后简化为：

其中F_ij表示整体代价函数在当前状态下对相机姿态的偏导，E_ij表示整体代价函数对路标点位置的偏导；

将位姿变量放到一起：

将空间点的变量放到一起：

简化为：

其中△x_c和△x_p分别为所有位姿变量整合到一起的整体增量、空间点的变量整合到一起的整体增量；雅克比矩阵F和E是整体代价函数对所述整体增量的导数；

展开得到增量线性方程：

HΔx＝g

使用LM方法求解则：H＝J^TJ+λI，而J＝[F E]，g＝-J(x)^Tf(x)；

其中I为单位阵，λ为正实数。

4.根据权利要求3所述的基于GPU加速的无人机视觉SLAM方法，其特征在于，所述步骤S4具体包括以下步骤：

S41.利用词袋模型计算图像间的相似度加速回环发生的识别，通过动态计算回环帧筛选阈值，确定回环关键帧；对当前关键帧与回环关键帧计算Sim3变换得到回环检测结果；

S42.根据步骤S41的回环检测结果进行运行轨迹的纠正，构建得到轨迹符合回环检测结果的位姿图。

5.根据权利要求4所述的基于GPU加速的无人机视觉SLAM方法，其特征在于，所述步骤S41中图像间相似度的计算步骤为：

基于所述词袋模型并使用TF-IDF算法进行计算：

设训练词典时所有特征数量为n，图像中单词w_i数量为n_i，则单词的IDF为：

设图像中单词w_i出现了n_i次，一共出现的单词数为n，则TF为：

得到单词w_i的权重为：

n_i＝TF_i×IDF_i

对于每一幅图像具有多个特征点，每个特征点分别对应上词典中的某个单词并赋予权重，组成属于该图像的Bag-of-words，其中N为所述词典中的单词数，表示如下：

设另一幅图像的Bag-of-words为υ_B，则两幅图像的相似度为：

其中N为所述词典中的单词数，υ_A为一幅图像的Bag-of-words，υ_B为另一幅图像的Bag-of-words。

6.根据权利要求5所述的基于GPU加速的无人机视觉SLAM方法，其特征在于，所述步骤S41中确定回环关键帧的具体步骤包括：

将与当前帧相连的局部关键帧剔除后遍历所有关键帧；

找出与当前关键帧具有相同单词的关键帧，并统计所有闭环候选帧中与当前关键帧具有共同单词最多的单词数，将最多单词的80％设置为第一阈值；

找出所有单词数超过所述第一阈值，且相似度检测大于相邻关键帧最低分数的关键帧；将其与自己相邻最紧密的前10个关键帧设定为一组；即所述组以每一个关键帧为中心，加上与中心的关键帧相邻的关键帧所形成，一个关键帧出现在一个或多个组内；

计算分组后每组的总得分以及每组得分最高的关键帧，以组得分最高的0.75作为第二阈值，找出高于所述第二阈值的所有组里面得分最高的帧，作为回环关键帧。

Images