CN112164117A - 一种基于Kinect相机的V-SLAM位姿估算方法 - Google Patents

Abstract

本发明公开了一种基于Kinect相机的V‑SLAM位姿估算方法，包括以下步骤：1)通过Kinect采集图像信息，获取彩色图像和深度图像，并且进行彩色图像和深度图像配准，获取三维点云；2)采用改进的FAST对采集的彩色图像进行特征点检测，并且计算描述子；3)利用FLANN匹配方法，对待匹配图像和基准图像进行特征点匹配，得到特征点匹配对；4)对步骤3)中的匹配对剔除误匹配；5)对步骤4)中精准匹配对求解位姿。本发明根据图像灰度设计一种自适应阈值来改变算法的鲁棒性，同时，特征点提取后利用非极大值抑制的方法筛选掉密集的特征点，改善特征点分布不均的问题。该方法具有良好的位姿估算效果，可广泛应用于机器人、VR和无人机等方面。

Description

一种基于Kinect相机的V-SLAM位姿估算方法

技术领域

本发明涉及计算机视觉技术，尤其涉及一种基于Kinect相机的V-SLAM位姿估算方法。

背景技术

SLAM(同时定位和地图构建)技术广泛应用于移动机器人领域、无人驾驶和无人机等领域，其作用是使移动机器人等在未知环境中进行环境探索，并同时构建相应的环境地图，提高机器人的智能化水平。随着视觉传感器的不断发展，视觉SLAM也逐渐兴起，其中Kinect视觉传感器因为其包含环境信息丰富、精度高和直接测量深度等特点，被广泛应用于移动机器机器人。

V-SLAM主要分为前端视觉里程计部分和后端优化两部分。视觉里程计的作用是用来估算相邻两帧之间相机的运动，确定相机的位姿，以特征点来估算相机位姿则一直是视觉里程计的主流方法，而相机相邻两帧之间匹配效率低、精度低等情况，会影响相机位姿估算，从而会直接影响机器人的定位和地图构建的准确性和性能。本发明在基于Kinect相机的基础下针对ORB算法匹配效率低和精度低等问题，对ORB算法中的配准过程进行改进，以提高算法的效率和精度。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种基于Kinect相机的V-SLAM位姿估算方法。

本发明解决其技术问题所采用的技术方案是：一种基于Kinect相机的V-SLAM位姿估算方法，包括以下步骤：

1)通过Kinect采集图像信息，获取彩色图像和深度图像，并且进行彩色图像和深度图像配准，获取三维点云数据；

2)采用改进的FAST对采集的彩色图像进行特征点检测，并且计算描述子；

所述采用改进的FAST进行特征点检测，具体如下：

2.1)根据彩色图像的灰度值来设定自适应FAST阈值，设图像上(x₀,y₀)点为候选特征点，以(x₀,y₀)为中心取方形区域的边长为L，定义FAST的自适应阈值为s，计算如下：

其中，I_max和I_min分别代表方形区域L中最大的n个灰度值和最小的n个灰度值，I_arer为方形区域L的灰度平均值，a为比例系数。

2.2)利用阈值s进行特征点检测的判定，确定候选特征点；

2.3)对检测到的候选特征点进行非极大值抑制，筛选出来的点作为最后进行匹配的特征点；

3)利用FLANN方法对相邻两帧彩色图进行特征匹配，得到特征匹配对；

4)对步骤3)中的匹配对剔除误匹配；

5)利用步骤4)中精准匹配对求解位姿。

按上述方案，所述步骤1)中彩色图像和深度图像配准生成三维点云数据，具体如下：

步骤1.1)利用张正友标定法对相机进行标定和配准，获取相机的内参矩阵K。

步骤1.2)结合相机内参矩阵，利用针孔模型将二维像素点转化为对应的三维坐标点，设一个图像特征点的像素坐标(u，v)和相机的测量深度d，根据内参矩阵求得特征点在三维空间的位置(x，y，z)。

按上述方案，所述步骤2.2)中利用阈值s进行特征点检测的判定，确定候选特征点的具体步骤如下：

在图像中选取像素点p作为中心像素，检测中心点周围的16个像素，I_p代表像素中心强度，I_x代表周围像素的强度，首先比较第1和第9个像素，若1和9的像素强度在中心像素p的变化范围s内，则该点不是特征点，若1和9的像素强度大于变化范围s，然后检测第5和13点时，若四个像素有三个检测不在中心像素p的变化范围内，则p是特征点候选点，最后对检测出的所有候选特征点进行16点检测，若有12个连续的点不在中心P的阈值变化范围内，否者筛选掉该点；

判断公式如下:

||I_p-I_x||＞s。

按上述方案，所述步骤4)中利用PROSAC算法对特征点匹配后的结果进行筛选，得到精准的特征点匹配对，具体如下；

步骤4.1)设置最大迭代次数、内点阈值和初始迭代次数(初始迭代次数为0)。

步骤4.2)计算特征点的最小欧式距离d_min和次小欧氏距离d_min2，求解欧式距离比值β；

步骤4.3)计算质量因子γ，并采用质量因子衡量匹配点对质量，比值越小，则说明距离越小，特征点匹配的质量也就越好，计算公式如下；

步骤4.4)判断迭代次数是否大于最大迭代次数，若是，则为未找到合适的数学模型，给出错误提示，反之，进行下一步。

步骤4.5)根据质量因子γ将匹配对匹配集合进行降序排序，选取质量最高的m个数据点。

步骤4.6)从m中随机抽取K个数据计算模型参数，并且计算内点数量。判断内点数量是否小于内点阈值，若是，则返回模型参数以及内点数目，反之，迭代次数加1，返回到步骤4.4)。

通过计算返回高质量的匹配对P＝{p₁,p₂,…,p_i}和P′＝{p₁′,…,p′_n}，同时得到单应矩阵H和内点数目。

按上述方案，所述步骤5)中求解位姿，利用ICP求解方法进行相机位姿求解。

按上述方案，所述步骤5)中利用ICP求解方法进行相机位姿求解，具体如下：

步骤5.1)根据两组匹配对P＝{p₁,p₂,…,p_i}和P'＝{p₁'，…,p_n'}，计算两组点的质心位置p，p'；

步骤5.2)计算各组中每个特征点的去质心坐标：

q_i＝p_i-p

q_i'＝p_i'-p'

步骤5.3)根据优化方程来计算旋转矩阵R；

步骤5.4)根据旋转矩阵R求取平移矩阵t；

t^*＝p-Rp'

根据相机的旋转矩阵R和平移矩阵t，求解相机的位姿。

本发明产生的有益效果是：

本发明针对FAST特征点提取方法存在计算量较大的问题，对其计算方法进行改进，减小计算量，同时对于对比度和噪声情况有差异的图像，选取相同的阈值会导致算法鲁棒性变差，本发明根据图像灰度设计一种自适应阈值来改变算法的鲁棒性。同时，特征点提取后利用非极大值抑制的方法筛选掉密集的特征点，改善特征点分布不均的问题。最后针对匹配结果存在误匹配等问题，利用PROSAC(渐进一致性采样方法)把匹配结果按照质量的好坏进行排序，剔除错误匹配对以提高匹配精度，最后通过ICP对匹配的结果进行解算求解相机位姿。该方法具有良好的位姿估算效果，可广泛应用于机器人、VR和无人机等方面。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的原FAST特征点提取示意图；

图3是本发明实施例的原始ORB图像匹配图；

图4是本发明实施例的改进的ORB图像匹配图；

图5是本发明实施例的改进的算法和另外两种算法轨迹对比图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1是本发明实例方法的流程图，一种基于Kinect相机的V-SLAM位姿估算方法，主要包括如下：

步骤1：采集Kinect相机图片信息，即获取彩色信息和深度信息。为了配合ICP求解位姿，需要将二维像素点转化为三维空间点，生成三维点云。利用Kinect相机的内参矩阵进行计算，将二维像素点转化为三维点。具体的步骤：

步骤1.1：利用张正友标定法对相机进行标定和配准，获取相机的内参矩阵K，如式(1)。

其中f_x，f_y分别是X，Y轴上的焦距；c_x，c_y为像素坐标系和成像平面的平移量。

步骤1.2：结合相机内参矩阵K，利用针孔模型将二维像素点转化为对应的三维坐标点，设一个图像特征点的像素坐标(u，v)和相机的测量深度d，可以求得特征点在三维空间的位置坐标(x，y，z)，计算公式如式(2)-(4)：

x＝(u-c_x)·z/f_x (2)

y＝(v-c_x)·z/f_y (3)

z＝d/s (4)

步骤2：对步骤1的相邻两帧彩色图像分别进行FAST特征点提取，具体包括以下步骤：

步骤2.1：计算FAST阈值。由于不同图像的对比度和噪声，固定的FAST阈值会降低算法的鲁棒性，本发明采用自适应阈值方法，通过改变FAST的阈值来提高算法的鲁棒性。设图像上(x₀,y₀)点为候选特征点，以(x₀,y₀)为中心取方形区域的边长为L，定义FAST自适应阈值为s，计算公式5如下:

其中s为自适应系数，式中I_max和I_min分别代表方形区域L中最大的n个灰度值和最小的n个灰度值，I_arer为方形区域L的灰度平均值，a为比例系数，一般取2～4。

步骤2.2：利用改进的FAST方法进行特征点检测，原始的FAST需要检测周围的16个像素，如图2，在图像中选取像素点p，假设灰度值为I_p，设定阈值s，若其中有n个连续的点都比中心像素p的灰度值都大s，或都小s，这样的中心点就是特征点。本方法利用改进的FAST特征点检测进行检测，其中I_p代表像素中心强度，I_x代表周围像素的强度，判断公式如6：

||I_p-I_x||＞s (6)

首先比较第1和第9个像素，若1和9的像素强度在中心像素p的变化范围s内，则该点不是特征点。若1和9的像素强度大于变化范围s，然后检测第5和13点时，若四个像素有三个检测不在中心像素p的变化范围内，则p是候选点，最后对检测出的所有候选特征点进行16点检测，若有12个连续的点不在中心P的阈值变化范围内，否者筛选掉该点。

步骤2.3：非极大值抑制。首先计算Harris响应值，在每个节点中选出Harris响应值最大的特征点作为最终的特征点，保留每个节点中响应值R最大的特征点,计算公式(7)和(8)如下:

R＝det(M)-k(trace(M))² (7)

其中，R为Harris响应值，M为一个大小为2×2的矩阵，k的取值范围为0.04～0.06，w(x,y)为图像窗口函数，I_x为特点在水平方向上的变化量，I_y为特征点在竖直方向上的变化量。具体的步骤为：

(1)首先建立高斯金字塔，再对金字塔的每层进行网格划分，以图像建立的尺度空间金字塔顶层为基准，选取在每层金字塔上都出现的特征点p_i，p_i∈P，P＝{p₁，p₁，…，p_i}为已有的特征点，对候选特征点q_i，q_i∈Q，Q＝{q₁，q₁，…，q_i}进行非极大值抑制剔除，防止特征点的重叠输出。

(2)计算图片中现有特征点q_i所在的网格区域与已有特征点p_i坐标距离，同时设定距离的临界阈值l，求解候选特征点q_j＝{x_j,y_j,ε_j}与特征点P＝{p₁,p₂,…,p_i}之间的坐标距离，筛选出不大于l的候选特征点q_j'＝{x_j',y_j',ε_j'}。

(3)计算(2)中筛选出候选点q_j'＝{x_j',y_j',ε_j'}的Harris角点响应阈值ε_j'，并且将这些值从小到大排序，将Harris角点响应阈值ε_j'极大值所在的候选特征点q_j'＝{x_j',y_j',ε_j'}保留下来，得到特征点所处最佳位置的临近特征点，同时剔除低阈值的邻近特征点。

通过上面步骤，本发明首先根据图像灰度值设定FAST自适应阈值，然后通过改进的FAST进行特征点检测，并且为解决特征点分布不均匀的问题，剔除非极大值点，最终获取我们所需要的特征点。

步骤2.4：计算描述子。本发明使用BRIEF特征作为描述子，BRIEF是一种二进制描述子，通过0和1编码了附近两个像素的大小关系，p(x)和p(y)代表任意一对像素点，准则公式(9)如下：

通常利用一个向量来表示BRIEF特征描述符，如公式(10)：

为了解决特征点的旋转不变性，通常采用灰度质心法进行计算。灰度质心法的原理是假设特征点的灰度与该领域质心之间存在偏差移，最后通过此特征点到质心的向量来计算该特征点的主方向。在以p点为圆心，半径为3像素的圆形图像块内，定义(x，y)为图像块内的点相对于圆心的相对坐标，质心矩的公式(11)和质心公式(12)分别是：

最后求得的特征方向θ的公式(13)如下：

上述步骤2中，计算出两幅图像(相邻关键帧)的特征点和描述子，即获取两幅图片的特征点。

步骤3：两幅图像(关键帧)特征点匹配。利用最近FLANN创建匹配对象，即相邻关键帧的特征匹配。FLANN匹配算法首先调用匹配函数，会提前训练一个匹配器，通过训练好的匹配器来提高算法的性能。特征匹配的结果会得到两个特征集合的对应关系，两个集合分别是训练集和查询集，训练集会建立特征集的索引树，查询集的作用就是根据匹配集来进行匹配。所以在匹配时，是通过查询图片上的每一个特征点(查询集)与训练匹配器(训练集)进行匹配，找出最佳结果。而相邻关键帧的匹配也就是查询集和匹配集的匹配。其核心在于使用欧式距离找到实例点的最近邻点，具体的步骤：

步骤3.1：首先对当前帧进行处理，找出当前帧中的感兴趣对象；接下来，通过帧与帧之间的关系找到另一个最近邻关系集合，也就是训练集。

步骤3.2：在数据集中找到方差最高维度，根据这个最高维度数值将数据集划分为2个部分，对符合特征的每个子集进行上述操作，若当前子集不能再划分时，则该子集中数的据点将保存在叶子节点。

步骤3.3：建立多棵随机K-d树，从具有最高方差N_d维中随机选取若干维度，用来做划分，对随机K-d树进行遍历搜索。在对随机K-d森林进行搜索时候，所有的随机K-d树将共享一个优先队列增加树的数量能加快搜索速度。

需要说明的是，步骤3中随机K-d树的数量不能太多也不能太少，应该控制在可控范围内，考虑内存负载的问题，将数的数量控制在20左右比较合适。

步骤4：由于初始的匹配仍然存在大量的误匹配，因此利用PROSAC算法对匹配后的结果进行筛选，按照匹配的质量高低可以更快剔除误匹配，得到精准的匹配对。具体的实施步骤如下；

步骤4.1：设置最大迭代次数、内点阈值和初始迭代次数(初始迭代次数为0)。

步骤4.2：计算特征点的最小欧式距离d_min和次小欧氏距离d_min2，求解欧式距离比值β；

步骤4.3：计算质量因子γ，并采用质量因子衡量匹配点对质量，比值越小，则说明距离越小，特征点匹配的质量也就越好，计算公式如下；

步骤4.4：判断迭代次数是否大于最大迭代次数，若是，则为未找到合适的数学模型，给出错误提示，反之，进行下一步。

步骤4.5：根据质量因子γ将匹配集合进行降序排序，选取质量最高的m个数据点。

步骤4.6：从m中随机抽取K个数据计算模型参数，并且计算内点数量。判断内点数量是否小于内点阈值，若是，则返回模型参数以及内点数目，反之，迭代次数加1，返回到步骤4.4。

通过计算返回高质量的匹配对P＝{p₁,p₂,…,p_i}和P'＝{p₁',…,p_n'}，同时得到单应矩阵H和内点数目。

步骤5：求解位姿，利用ICP求解方法进行相机位姿求解，由于ICP方法是一种3D-3D的位姿求解方法，因此根据步骤1得到二维像素点对应的三维点，并计算两组点的质心位置p，p'，具体的步骤如下：

步骤5.1：计算各组中每个点的去质心坐标如式(16)和(17)：

q_i＝p_i-p (16)

q_i'＝p_i'-p' (17)

步骤5.2：根据优化方程来计算旋转矩阵R，如式(18):

步骤5.3：根据R求取t，如式(19):

t^*＝p-Rp' (19)

上述方法我们就可以求取相机的旋转矩阵R和平移矩阵t，也就可以求解相机的位姿。

本发明对原始ORB图像匹配和改进后的图像匹配进行比较，如图3和图4，从图3中可以看出匹配存在大量的误匹配(交叉匹配)，而改进后的图像匹配算法并未出现大量的相差匹配，证明改进后的特征匹配算法精度得到提高。进一步的，利用改进后的算法求解位姿并进行定位，最后将轨迹图分别与ORB-SLAM2和RGB-D SLAMv2的轨迹图进行比较，如图5，其中改进后的RMSE与另外两种算法相比分别降低了26.6％和15.2％。

上述技术方案中，上述一种基于Kinect相机的V-SLAM位姿估算方法，基于Windows10的64位操作系统下的Visual Studio 2017，以及版本为3.4.1的开源OpenCV库，FLANN函数为OpenCV库函数。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (7)

1.一种基于Kinect相机的V-SLAM位姿估算方法，其特征在于，包括以下步骤：

1)通过Kinect采集图像信息，获取彩色图像和深度图像，并且进行彩色图像和深度图像配准，获取三维点云数据；

2)采用改进的FAST对采集的相邻的两帧彩色图像进行特征点检测，并且计算描述子；

所述采用改进的FAST进行特征点检测，具体如下：

2.1)根据彩色图像的灰度值来设定自适应FAST阈值，设图像上(x₀,y₀)点为候选特征点，以(x₀,y₀)为中心取方形区域的边长为L，定义自适应阈值为s,计算如下：

其中，I_max和I_min分别代表方形区域L中最大的n个灰度值和最小的n个灰度值，I_arer为方形区域L的灰度平均值，a为比例系数；

2.2)利用阈值s进行特征点检测的判定，确定特征点候选点；

2.3)对检测到的特征点候选点进行非极大值抑制，筛选出来的点作为最后进行匹配的特征点；

3)利用FLANN方法对相邻的两帧彩色图进行特征匹配，得到特征匹配对；

4)对步骤3)中的匹配对剔除误匹配；

5)利用步骤4)中精准匹配对求解位姿。

2.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤1)中彩色图像和深度图像配准获取三维点云数据，具体如下：

利用张正友标定法对相机进行标定和配准，获取相机的内参矩阵；

结合相机内参矩阵，利用针孔模型将二维像素点转化为对应的三维坐标点，设一个图像特征点的像素坐标(u，v)和相机的测量深度d，求得特征点在三维空间的位置坐标(x，y，z)。

3.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤1)中彩色图像和深度图像配准，具体如下：

步骤1.1)先测量彩色图像和深度图像上同一边长的比例；

步骤1.2)利用OpenCV中的函数按照步骤1.1)中的比例将两幅图调整为同样大小的尺寸；

步骤1.3)去掉边缘区域，保留中心位置中配准好的矩形区域，即完成深度图和彩色图的配准。

4.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤2.2)中利用阈值s进行特征点检测的判定，确定特征点候选点，具体如下：

在图像中选取像素点p作为中心像素，I_p代表像素中心强度，检测周围的16个像素，I_x代表周围像素的强度，首先比较第1和第9个像素，若1和9的像素强度在中心像素p的变化范围s内，则该点不是特征点，若1和9的像素强度大于变化范围s，然后检测第5和13点时，若四个像素有三个检测不在中心像素p的变化范围内，则p是特征点候选点，最后对检测出的所有候选特征点进行16点检测，若有12个连续的点不在中心P的阈值变化范围内，否者筛选掉该点；

判断公式如下:

||I_p-I_x||＞s。

5.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤4)中利用PROSAC算法对特征点匹配后的结果进行筛选，得到精准的特征匹配对，具体如下；

步骤4.1)设置最大迭代次数、内点阈值和初始迭代次数；

步骤4.2)计算特征点的最小欧式距离d_min和次小欧氏距离d_min2，求解欧式距离比值β；

步骤4.3)计算质量因子γ，并采用质量因子衡量匹配点对质量，比值越小，则说明距离越小，特征点匹配的质量越好，计算公式如下；

步骤4.4)判断迭代次数是否大于最大迭代次数，若是，则给出错误提示，反之，进行下一步；

步骤4.5)根据质量因子γ将匹配对匹配集合进行降序排序，选取质量最高的m个数据点；

步骤4.6)从m中随机抽取K个数据计算模型参数，并且计算内点数量，判断内点数量是否小于内点阈值，若是，则返回模型参数以及内点数目，反之，迭代次数加1，返回到步骤4.4)；

通过计算返回高质量的匹配对P＝{p₁,p₂,…,p_i}和P′＝{p′₁,…,p′_n}，同时得到单应矩阵H和内点数目。

6.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤5中求解位姿，利用ICP求解方法进行相机位姿求解。

7.根据权利要求1所述的基于Kinect相机的V-SLAM位姿估算方法，其特征在于，所述步骤5)中利用ICP求解方法进行相机位姿求解，具体如下：

步骤5.1)根据匹配对P＝{p₁,p₂,…,p_i}和P'＝{p₁',…,p_n'}，计算两组点的质心位置p，p'；

步骤5.2)计算各组中每个特征点的去质心坐标：

q_i＝p_i-p

q_i'＝p_i'-p'

步骤5.3)根据优化方程来计算旋转矩阵R；

步骤5.4)根据旋转矩阵R求取平移矩阵t；

t^*＝p-Rp'

根据相机的旋转矩阵R和平移矩阵t，求解相机的位姿。

Images