一种基于SURF算子的闭环检测优化方法与流程

本发明涉及一种slam算法，尤其是涉及一种基于surf算子的闭环检测优化方法。

背景技术：

同时定位与地图构建(simultaneouslocationandmapping,简slam)是利用机器人自身携带的传感器获取所在环境的信息估计机器人位姿并构建环境地图，是实现机器人完全自主移动的关键技术。同时，视觉salm还是目前vr技术的关键所在，在未来的虚拟现实产业应用前景广阔。

特征点检测、特征描述和特征匹配是基于局部特征点的图像匹配技术的三个主要组成部分，被广泛地应用在医疗、图像检索、目标识别和增强现实技术上。移动机器人长时间运行会带来的累计误差，这会造成定位与地图构建的不一致问题，而闭环检测的引入可以减小累积误差。但传统的闭环检测算法检测闭环效率不高、且准确率低。

技术实现要素：

本发明的目的在于一种基于surf算子的闭环检测优化方法，本发明为了克服已有机器人定位与环境地图构建中定位一致性差，环境地图构建质量不高以及优化效率低的不足的问题，提供一种可提高机器人位姿一致性、环境地图构建质量较高以及优化效率高的闭环检测slam方法。

本发明解决上述技术问题采用的技术方案是：一种基于surf算子的闭环检测优化方法，包括以下步骤：

步骤一：使用张定友标定法对kinect进行标定，获得rgb图像；

步骤二：对获取的rgb图像运用surf算法进行特征提取，获得点集；

步骤三：采用最大相异系数对获得的点集进行筛选，获得筛选后的匹配点集；

步骤四：对筛选后的匹配点集采用ransac算法进行优化，删除误匹配。

张定友标定法是基于使用针孔模型，具备良好的鲁棒性并且不需要高昂的精制标定块，极大推进了机器视觉实际应用化。张定友标定方法通过假定标定用平面图板在世界坐标系中z芦0，并经过分析线性模型得出摄像机参数的优化解，而后用基于最大似然法进行非线性求精。在这个过程中标定出镜头畸变的目标函数，最后求出所需的摄像机内、外部参数。

随机抽样一致性ransac(randomsampleconsensus)，是一种鲁棒的参数估计方法，该算法能简单、有效的去除噪声影响，其采用迭代的方式从1组包含离群的被观测数据中估算出数学模型的参数。与普通的去噪算法不同，ransac算法是使用尽可能少的点来估计模型参数，然后尽可能的扩大得到的模型参数的影响范围。ransac算法的基本假设是样本中包含正确数据(inliers，可以被模型描述的数据)，也包含异常数据(outliers，偏离正常范围很远、无法适应数学模型的数据)，即数据集中含有噪声。在经过上述基于深度信息筛选后的特征点匹配对中存在正确匹配，也存在少量的异常匹配，这些异常匹配可能是由于错误的测量、环境因素、错误的计算等产生的。此算法最确定的模型中局内点(即正确匹配)与所有点(即总匹配对数)的百分比即为匹配准确率。若未进行基于深度信息的特征点筛选这一步骤，得到的异常匹配较多，经过ransac算法后得到的数学模型准确率低，单应性矩阵h存在误差。

通过最大相异系数同优化ransac算法相结合多次对点集进行筛选优化，提高闭环检测的效率和准确率、提高机器人位姿一致性、环境地图构建质量。

作为优选，surf算法进行特征提取的方法，包括以下步骤：

步骤a：以特征点为圆心、6s为半径的邻域内构建扇形，计算特征点附近区域内的像素在x与y方向的小波响应，并对这些响应赋予高斯权重系数，使越靠近特征点的响应贡献越大，越远离特征点的响应贡献越小；

步骤b：以60°为基准进行统计扇形内所有点水平方向的小波特征和垂直方向的小波特征的总和mw，具体公式为：

mw＝∑dx+∑dy；

步骤c：将60°扇形以5°的间隔进行旋转，得出各方向扇形的合成向量模长最大值，具体公式为：

其中向量模长最大值对应的角度即特征点主方向；

步骤d：在特征点周围取一个边长为20s、方向为特征点主方向的正方形框，将正方形框分为16个子区域，计算每个子区域内水平方向的小波特征dx和每个子区域内垂直方向的小波特征dy，并对这些响应通过高斯窗口函数赋予高斯权重系数；

步骤e：将每个子区域内的dx和dy分别求和，同时也对dx和dy的绝对值分别求和，得到一个四维的矢量v，具体公式为：

v＝(∑dx,∑dy,∑|dx|,∑|dy|)；

步骤f：对16个子区域符合描述的结合形成此特征点的特征描述符，最终的特征描述符是一个64维的向量；

步骤h：对rgb图像重复执行步骤a-步骤f，获得特征描述符的集合，特征描述符的集合即为点集。

其中，s为特征点的尺度。

作为优选，最大相异系数点集筛选方法包括以下步骤：

步骤i：用邻近算法处理点集后采用欧氏距离比筛选，获取最初筛选匹配点集；在surf全局空间采用欧氏距离ds作为度量，计算公式为：

其中l1和l2是两个输入的surf全局特征，下标s代表surf全局空间.lji表示第j个特征lj的第i个元素，即64维特征向量中的第i维。其中欧氏距离比为与样本图像最近邻特征点欧氏距离与次近邻特征点欧氏距离的比值。

步骤ii：计算最大相异系数zy；在欧氏距离比小于0.8的基础上，进一步计算两描述符向量的最大相异系数zy，最大相异系数zy取值范围[0，∞)，zy取值越小表示两向量越接近，zy＝0则表示两向量相同。其中，最大相异系数zy的计算过程为：

步骤a1:计算两个向量的相对误差向量λ：

步骤a2:将各分量按照从大到小的顺序排列为:λ＝{λi＞λj，i≤j}；

步骤a3:取出λ的前k个值得到ξ＝{λ1，λ2，λ3，…，λk}；

步骤a4:对ξ向量进行加权平均得到最大相异系数zy：

步骤iii：在最大相异系数zy∈[0.2，1.0]的范围内自适应估计最优最大相异系数zb，从最初筛选匹配点集si中选取满足特征点间最大相异系数值zy＜zb的特征点，获得优化后的匹配点集。

通过用邻近算法处理点集后采用欧氏距离比筛选，再将最初筛选匹配点集用最大相异系数处理，双重处理后，提高闭环检测的效率和准确率。

作为优选，最初筛选匹配点集si的再次筛选方法包括以下步骤：

s1：取zy∈[0.2，1.0]；

s2：以0.02个单位为步长，用每个zy来筛选最初筛选匹配点集si中的所有匹配点；

s3：获取筛选后符合条件的匹配点集sp，其中匹配点集sp满足每组特征描述符向量的最大相异系数均小于zy。

作为优选，ransac算法优化方法包括以下步骤：

步骤①：将步骤s2中的匹配点集sp所有子点集进行排序，得到子点集sp1，sp2，sp3······spn；

步骤②：从步骤①中得到的子点集中选择排序靠前的m个子集合并为新的数据集d；

步骤③：从数据集d中随机选取7个数据与第n个数据组合，利用八点法计算基础矩阵h，同时计算基础矩阵h到各自极线距离的平方和，记为d；

步骤④：将得到的距离的平方和d与设定值s比较，将满足d<s条件的数据点视为内点，统计内点个数；

步骤⑤：重复上述过程，直到符合终止条件，算法终止；

所述终止条件为最大内点超过点集数目的1/2或者抽样得到的内点数目未增加或者在点数目相同的情况下误差没有变小，所述误差为d与s的差值。

对筛选后的匹配点集通过排列顺序、比较进行优化，且设置三种终止方式，更高效的得到优化后的匹配点集，提高闭环检测的效率和准确率。

作为优选，子点集的排列方式为按照最大相异系数从小到大的方式排序。从小到大排列更易获得所需的优化后的匹配点集，提高闭环检测的效率和准确率。

本发明具有的有益效果是：

1、克服了已有机器人定位与环境地图构建中定位一致性差，环境地图构建质量不高以及优化效率低的不足的问题；

2、通过最大相异系数同优化ransac算法相结合来删除误匹配，可提高机器人位姿一致性、环境地图构建质量以及闭环检测的效率和准确率；

3、通过用邻近算法处理点集后采用欧氏距离比筛选，再将最初筛选匹配点集用最大相异系数处理，双重处理后，提高闭环检测的效率和准确率。

附图说明

图1是本发明特征点的主方向示意图。

图2是本发明特征描述符的构成示意图。

图中：1、扇形，2、正方形框，3、子区域

具体实施方式

以下结合附图和实施方式对本发明作进一步的说明。

如图1-2所示，本发明一种基于surf算子的闭环检测优化方法，包括以下步骤：

步骤一：使用张定友标定法对kinect进行标定，获得rgb图像；

步骤二：对获取的rgb图像运用surf算法进行特征提取，获得点集；

步骤三：采用最大相异系数对获得的点集进行筛选，获得筛选后的匹配点集；

步骤四：对筛选后的匹配点集采用ransac算法进行优化，删除误匹配。

张定友标定法是基于使用针孔模型，具备良好的鲁棒性并且不需要高昂的精制标定块，极大推进了机器视觉实际应用化。张定友标定方法通过假定标定用平面图板在世界坐标系中z芦0，并经过分析线性模型得出摄像机参数的优化解，而后用基于最大似然法进行非线性求精。在这个过程中标定出镜头畸变的目标函数，最后求出所需的摄像机内、外部参数。

随机抽样一致性ransac(randomsampleconsensus)，是一种鲁棒的参数估计方法，该算法能简单、有效的去除噪声影响，其采用迭代的方式从1组包含离群的被观测数据中估算出数学模型的参数。与普通的去噪算法不同，ransac算法是使用尽可能少的点来估计模型参数，然后尽可能的扩大得到的模型参数的影响范围。ransac算法的基本假设是样本中包含正确数据(inliers，可以被模型描述的数据)，也包含异常数据(outliers，偏离正常范围很远、无法适应数学模型的数据)，即数据集中含有噪声。在经过上述基于深度信息筛选后的特征点匹配对中存在正确匹配，也存在少量的异常匹配，这些异常匹配可能是由于错误的测量、环境因素、错误的计算等产生的。此算法最确定的模型中局内点(即正确匹配)与所有点(即总匹配对数)的百分比即为匹配准确率。若未进行基于深度信息的特征点筛选这一步骤，得到的异常匹配较多，经过ransac算法后得到的数学模型准确率低，单应性矩阵h存在误差。

通过最优最大相异系数自适应估计方法和ransac算法多次多点集进行优化，提高闭环检测的效率和准确率。

surf算法进行特征提取的方法，包括以下步骤：

步骤a：以特征点为圆心、6s为半径的邻域内构建扇形，计算特征点附近区域内的像素在x与y方向的小波响应，并对这些响应赋予高斯权重系数，使越靠近特征点的响应贡献越大，越远离特征点的响应贡献越小；

步骤b：以60°为基准进行统计扇形内所有点水平方向的小波特征和垂直方向的小波特征的总和mw，具体公式为：

mw＝∑dx+∑dy；

步骤c：将60°扇形以5°的间隔进行旋转，得出各方向扇形的合成向量模长最大值，具体公式为：

其中向量模长最大值对应的角度即特征点主方向；

步骤d：在特征点周围取一个边长为20s、方向为特征点主方向的正方形框，将正方形框分为16个子区域，计算每个子区域内水平方向的小波特征dx和每个子区域内垂直方向的小波特征dy，并对这些响应通过高斯窗口函数赋予高斯权重系数；

步骤e：将每个子区域内的dx和dy分别求和，同时也对dx和dy的绝对值

v＝(∑dx,∑dy,∑|dx|,∑|dy|)；

分别求和，得到一个四维的矢量v，具体公式为：

步骤f：对16个子区域符合描述的结合形成此特征点的特征描述符，最终的特征描述符是一个64维的向量；

步骤h：对rgb图像重复执行步骤a-步骤f，获得特征描述符的集合，特征描述符的集合即为点集。

其中，s为特征点的尺度。

最大相异系数点集筛选方法包括以下步骤：

步骤i：用邻近算法处理点集后采用欧氏距离比筛选，获取最初筛选匹配点集；在surf全局空间采用欧氏距离ds作为度量，计算公式为：，

其中l1和l2是两个输入的surf全局特征，下标s代表surf全局空间.lji表示第j个特征lj的第i个元素，即64维特征向量中的第i维。其中欧氏距离比为与样本图像最近邻特征点欧氏距离与次近邻特征点欧氏距离的比值。

步骤ii：计算最大相异系数zy；在欧氏距离比小于0.8的基础上，进一步计算两描述符向量的最大相异系数zy，最大相异系数zy取值范围[0，∞)，zy取值越小表示两向量越接近，zy＝0则表示两向量相同。其中，最大相异系数zy的计算过程为：

步骤a1:计算两个向量的相对误差向量λ：

步骤a2:将各分量按照从大到小的顺序排列为:λ＝{λi＞λj，i≤j}；

步骤a3:取出λ的前k个值得到ξ＝{λ1，λ2，λ3，…，λk}；

步骤a4:对ξ向量进行加权平均得到最大相异系数zy：

步骤iii：在最大相异系数zy∈[0.2，1.0]的范围内自适应估计最优最大相异系数zb，从最初筛选匹配点集si中选取满足特征点间最大相异系数值zy＜zb的特征点，获得优化后的匹配点集。

通过用邻近算法处理点集后采用欧氏距离比筛选，再将最初筛选匹配点集用最大相异系数处理，双重处理后，提高闭环检测的效率和准确率。

最初筛选匹配点集si的再次筛选方法包括以下步骤：

s1：取zy∈[0.2，1.0]；

s2：以0.02个单位为步长，用每个zy来筛选最初筛选匹配点集si中的所有匹配点；

s3：获取筛选后符合条件的匹配点集sp，其中匹配点集sp满足每组特征描述符向量的最大相异系数均小于zy。

ransac算法优化方法包括以下步骤：

步骤①：将步骤s2中的匹配点集sp所有子点集进行排序，得到子点集sp1，sp2，sp3······spn；

步骤②：从步骤①中得到的子点集中选择排序靠前的m个子集合并为新的数据集d；

步骤③：从数据集d中随机选取7个数据与第n个数据组合，利用八点法计算基础矩阵h，同时计算基础矩阵h到各自极线距离的平方和，记为d；

步骤④：将得到的距离的平方和d与设定值s比较，将满足d<s条件的数据点视为内点，统计内点个数；

步骤⑤：重复上述过程，直到符合终止条件，算法终止。

所述终止条件为最大内点超过点集数目的1/2或者抽样得到的内点数目未增加或者在点数目相同的情况下误差没有变小，所述误差为d与s的差值。

子点集的排列方式为按照最大相异系数从小到大的方式排序。