本发明涉及一种基于改进图像匹配策略的同步定位与建图方法,属于图像匹配和视觉slam(simultaneouslocalizationandmapping)领域。
背景技术：
机器人的同步定位与建图是实现机器人自主导航的关键，机器人如何在未知环境中完成自身的定位和建图是一个极具挑战的问题。传统gps定位通常应用于室外环境，在室内环境中往往存在定位失效的问题。随着计算机视觉领域研究的不断发展，室内环境下基于视觉的slam技术逐渐成为研究热点。由于相机能够获取更加丰富的环境信息，并且随着价格相对低廉的微软kinect深度相机的推出，将其应用于视觉slam领域并在近年来取得了良好的定位与建图效果。
经典的视觉slam流程主要包括以下几个步骤：(1)信息读取，主要为图像信息的读取和预处理；(2)视觉里程计，主要估算相邻图像间的运动，以及局部地图的创建；(3)后端优化，后端用来不断接收视觉里程计测量的位姿，通过非线性优化方法对位姿进行优化，从而得到全局一致的轨迹和地图；(4)回环检测，主要判断机器人是否到达过先前的位置，若检测到则交给后端进行处理；(5)地图创建，根据后端优化后估计的轨迹，建立相应的地图。
其中视觉里程计部分主要是对相机位姿进行初步估计。该过程主要包括特征提取、特征匹配、误匹配剔除、位姿求解等几个步骤。ransac算法通常被应用在剔除图像误匹配过程中。标准ransac(randomsampleconsensus)算法由于随机选取样本数据，在内点较少的情形下会导致迭代次数增多，效率低下。原始orb(orientedfastandrotatedbrief)特征比较聚集，在跟踪的过程中鲁棒性比较差。此外，由于视觉里程计存在累计误差，导致所建地图不准确。
针对上述存在的问题，本发明提出了一种基于改进图像匹配策略的同时定位与建图的方法。此方法能够提高误匹配剔除的效率并且能够改善跟踪过程的鲁棒性得到较为准确的地图。
技术实现要素：
本发明提出了一种基于改进图像匹配策略的同时定位与建图的方法。该方法采用改进的orb特征对图像各个区域进orientedfast(orientedfeaturesfromacceleratedsegmenttest)角点检测，计算特征点的描述子，之后利用改进的ransac算法进行特征匹配及筛选。最后使用pnp(perspective-n-point)方法求解机器人的初始位姿，利用得到的初始位姿生成位姿图，然后使用列文伯格-马夸尔特方法对位姿进行更新并利用g2o(generalgraphicoptimization,g2o)优化库对位姿进行优化。最终把优化后的位姿以及相对应帧的结合在一起生成点云地图。
准备工作：
首先要在电脑上安装linux操作系统，本发明安装的是ubuntu14.04版本。然后安装必备的软件包：opencv3.1.0(用于图像处理的开源库)、pcl(pointcloudlibrary)处理点云的必备工具、eigen库(有关矩阵的线性代数运算库)、g2o(基于图优化的库)以及kinect深度相机的开源驱动libfreenect2。
本发明的主要操作步骤如下：
步骤一，图像数据的获取
采用微软公司的kinect相机获取周围环境的彩色图像和深度图像，编写程序实现获取图像功能。
步骤二，特征提取
采用改进的orb特征对图像进行特征点检测，此改进算法的步骤如下：
1)计算图像金字塔，设定金字塔层数为8，层间尺度比例为1.2。给每层分配待提取的特征点数，具体分配是通过等比数列求和的方式计算出每一层特征点的数目。
2)对图像金子塔中每一层图像进行特征点计算，然后将每一层中的图像划分成大小为14*20区域，然后进行fast角点检测。
3)根据每层划分的图像区域，将图像特征表示为四叉树形式，由每一层检测到的特征点数确定四叉树的节点数目。
4)对划分之后的节点进行判断，如果此节点的特征点数为1则不再分裂此节点(保留每个节点里面最好的特征点)。
5)当节点个数大于初始设定的总特征点数(1000)或者所有节点中只包含一个特征点时则终止检测，反之则继续。
步骤三，特征匹配及初始位姿求解
ransac算法通常用在图像误匹配剔除的过程中，其中内点比例和迭代次数的关系如式(1)所示。
其中p为置信度，γ为内点比例，η为计算模型所需的最少特征点数量，l为迭代次数。本发明提出的改进ransac算法主要步骤如下：
1)计算特征描述子，采用高斯分布模型来选取记录特征点周围的环境信息的图像点。其中采样点的坐标(xi,yi)满足高斯分布
2)计算待匹配的特征点的描述子与其他描述子之间的欧氏距离，得到最小距离和次小距离(距离代表相似度)。计算方法如式(2)所示。
其中pi和pj为两个特征点，desik和desjk分别为pi和pj特征点描述子的第k个分量。l(pi,pj)为待匹配的特征点描述子之间的欧氏距离，m为第一对描述子，n为特征描述子的总对数。
3)判断步骤2)中的最小欧氏距离和次小欧氏距离的比值是否小于0.7，比值越小代表区分度越大，发生误匹配的概率越小。把这些特征点保留下作为数据样本。若不满足此阈值条件，则在计算单应矩阵模型时不使用该数据点。
4)将原始数据通过步骤3)筛选后，计算两幅待匹配的图像间的单应矩阵，用此模型来剔除两幅图像中的错误匹配。
5)利用pnp求解机器人的初始位姿，求解方法如式(3)所示。
其中(x,y)是图像坐标点，(x,y,z)为空间坐标点，rt表示所求变量,其中t＝1,2,3,…,11,12。由于r一共有12维，因此至少需要6对匹配点来计算。
步骤四：位姿优化与建图
在slam中通常的做法是先使用pnp的方法得到机器人初始的位姿，然后构建非线性最小二乘问题对初始值进行调整。考虑w个三维空间点p及其投影p,要得到相机的位姿r,t(位姿李代数形式表示为ξ)。假设某空间点坐标为pa＝[xa,ya,za]t,相机内参矩阵为k,其中投影的像素坐标为u＝(ua,va)t，sa为缩放因子。像素位置与空间点位置的关系如式(4)所示：
具体的位姿优化与建图步骤如下：
1)确定误差项：将像素坐标点与空间坐标点按照当前估计的位姿进行投影，得到空间点重投影后在图像中的位置(和真实位置不重合)。把此误差构建成非线性最小二乘问题，关系如式(5)所示：
其中ε*表示重投影误差，b表示空间点总数目，运算符号
2)建立位姿图：本发明利用g2o建立位姿图，把待优化的变量定义成顶点，把误差项定义成边。
3)误差项对位姿求导：
其中fx,fy为g2o中对相机焦距的描述，x',y',z'为空间点的3d坐标，ξ为位姿。
4)误差项对空间点求导：
其中p表示为空间点，r表示为旋转矩阵。
5)利用g2o图模型中所建立的位姿图对位姿和空间点进行优化。
6)根据针孔相机模型以及优化后的位姿生成点云地图。设像素坐标为[u,v,1],3d点坐标为[x,y,z],缩放因子为d,相机光心为(cx,cy)根据相机成像模型二者之间的关系如式(8)所示。
同原始的方法相比，本发明的优势如下：
(1)采用改进的orb特征，增强了位姿跟踪时的鲁棒性。
(2)误匹配剔除时采用改进的ransac算法，提高了剔除效率尤其是在内点比例较低的情况下。
(3)利用图优化模型对位姿进行优化能够得到跟准确的位姿和地图。
附图说明
图1基于改进图像匹配策略的同步定位与建图算法流程图
图2改进的orb特征算法流程图
图3改进的ransac算法流程图
图4特征点分布图
图5改进ransac算法效果图
图6特征点重投影效果图
图7三维重建效果图
具体实施方式
本发明针对原始的orb特征跟踪过程中鲁棒性较差，特征匹配时，在内点比例较低的情况下ransac迭代次数较高以及定位过程中误差累计的问题，采用本发明改进orb特征以及改进的ransac算法结合g2o优化库完成定位与建图的任务。本发明的整体流程图参见说明附图1，具体实施方式分为以下几个步骤：
步骤一，安装kinect的开源驱动libfreenect2,编写相应程序实现通过kinect来获取周围环境的彩色图和深度图。
步骤二，使用改进后的orb特征对图像进行特征提取，参见说明书附图2。
步骤三，对图像进行特征匹配，初始匹配过程中会产生误匹配，采用本发明提取的改进的ransac算法对误匹配进行剔除，加快匹配效率。算法流程参见说明书附图3。
步骤四，求解初始位姿，结合对应的深度信息生成3d空间点，之后将得到的初始位姿和空间点作为顶点，将误差项定义成边，建立对应的位姿图。
步骤五，利用g2o图优化模型库对位姿和空间点进行优化，更新位姿以及空间坐标点。
步骤六，利用更新后得到的位姿结合空间点的三维信息最后建立点云地图。
下面给出本发明的一个实验实例。
(1)首先在linux操作系统下安装kinect2的驱动libfreenect2。结合驱动的相关说明文档，编程实现显示深度图像和彩色图像并保存。
(2)采集图像信息，本实验采用微软公司的kinect2深度相机,此款相机的彩色图像分辨率为1920*1080，深度图像的分辨率大小为512*424。检测的范围0.5-4.5m,观测范围：水平方向70度，垂直方向60度。
(3)对采集到的图像进行特征提取，此部分采用本发明中提出的改进orb特征进行特征点的提取。如说明书附图4中为改进的orb和原始的orb提取特征的效果对比，其中左侧为改进后特征点的分布右侧为原始分布，点表示特征点。
(4)相邻帧之间进行特征匹配，特征提取后进行特征匹配来估计相邻帧之间的运动。具体剔除效果如附图5所示，其中左图和右图分别为相机采集到的相邻时刻图像，圆圈表示提取到的特征点，线表示误匹配剔除后得到的匹配点对。通过记录标准ransac和改进ransac所需的执行时间，标准的ransac执行时间为0.003888秒，改进后的ransac执行的时间为0.000643秒时间缩短近6倍。重投影效果如图6所示，其中左图为原始方法重投影后的效果，右图为改进后算法重投影的效果。可以看出改进后重投影后的效果相比原始算法也有所提升。
(5)估算得出单应性矩阵，本例中的具体结果为：
(6)根据slam的数学模型，建立图优化模型。将位姿用g2o中的vertexse3expmap、vertexsbapointxyz表示空间点位置以及投影方程边edgeprojectxyz2uv。然后根据发明步骤中误差项对位姿的求导以及误差项对空间点的求导，将求导得到的雅克比矩阵编写程序在edgeprojectxyz2uv的linearizeoplus函数中实现。
(7)以本例中的具体两帧图像结合深度图像给出优化前和优化后的结果。
本次优化前的旋转矩阵和平移向量分别为：
t＝[-0.01038603-0.005249340.01139507]
然后根据步骤(6)中的图优化理论进行优化，此次优化花费的时间为0.004514秒，迭代11次后收敛。优化后的变换矩阵为：
(8)根据图优化中构建的图模型利用高斯牛顿方法迭代求解优化后的位姿以及空间点，根据优化后的位姿建立周围的环境地图，如附图7所示。
实验结果表明，本发明所述方法可对机器人的位姿进行估计，提高了特征匹配阶段的误匹配剔除效率，增加了位姿跟踪过程中的鲁棒性。能够建立一个全局一致的环境地图。