论文部分内容阅读
摘 要： 自然路标提取与匹配是 vSLAM 的基础。文中提出了一种基于特征点三维信息的自然路标提取、局部特征描述与快速匹配方法。采用双目视觉获取环境图像， 提取左右目图像的特征点， 并进行匹配。建立左摄像机坐标系下的每个匹配点的三维信息， 提出视场约束规则对特征点进行过滤。在此基础上基于改进的MeanShift聚类算法进行自然路标提取。提出一种路标描述符， 可以快速进行两个聚类的匹配。该方法可以有效提取非结构化环境中的自然路标， 对机器人位姿估计精度要求较低。
关键词： 移动机器人；自然路标提取；路标描述符；MeanShift
中图分类号：TP242.6 文献标识号：A 文章编号：2095-2163（2015）01-
Abstract： Landmark extraction and matching is basis of vSLAM. A method of landmark extraction， local feature description and fast matching based on 3D information of feature points is proposed. Robot obtains images of environment via binocular vision， extracting feature points from left and right eye images， matching feature points of the two images. Three-dimensional information of each matched points under left camera coordinate system is built. Field of view constraint rule is proposed to filtering points. Then， the method of natural landmark extraction based on improved Mean Shift algorithm is discussed. The paper proposes a landmark descriptor， which can achieve fast matching of the two clustering. This method can extract natural landmarks in unstructured environment， tolerating relatively low accuracy of pose estimation.
Key words： Mobile Robot； Natural Landmark Extraction； Landmark Descriptor； MeanShift
0引 言
同时定位和地图创建（Simultaneous Localization and Mapping， SLAM）是实现机器人自主导航的一个关键技术[1]。SLAM技术主要采用视觉[2]、激光[3]等传感器以实现环境感知，尤其是基于视觉的SLAM技术（vSLAM）具有的对环境无侵性、获取环境信息的丰富性、以及成本低廉等显著优势，使其受到研究学界的广泛关注和高度重视。
vSLAM主要采用路标方法， 路标的自动提取与快速匹配即是其中的基础性问题。基于特征点的路标的表示方法主要分为两类。在此，可做如下具体分析：
一类以提取出的特征点直接作为路标，这类方法构建的地图路标的特征点数量较多， 进行场景匹配的运算量也随之较大。重点成果则有：文献[2]基于SIFT算法对环境图象进行特征提取，形成路标并构建分层地图。 文献[4]也提出一种基于显著场景Bayesian Surprise的自然路标检测方法。并且，文献[5]又使用关键帧Harris角点作为自然路标， 实现了机器人的实时全局定位与导航。相应地，文献[6]亦同样使用环境图像的SIFT特征点作为自然路标，利用特征点信息建立自然路标数据库，实现了环境地图创建。针对特征点的提取也进行了大量的研究，文献[7]即提出了基于图像二阶多项扩展式局部方向张量的兴趣点检测 PLOT算子，并利用双目视觉实现了机器人的SLAM过程。继而，文献[8]提出一种利用双目视觉有效提取特征点的无监督算法， 该算法使用SOM对SIFT特征点进行快速匹配以获得稳定的特征点。还有文献[9]通过应用Harris算法来提取角点。而文献[10]则针对单目视觉提取的特征点给出了统一逆深度参数方法来准确表达其不确定性。
另一类路标表示法是通过对提取的特征点进一步聚类， 再将获得的聚类整体作为一个路标。相应成果有，文献[11]提出了一种基于角点聚类的自然路标局部特征提取其匹配算法。本文则基于对特征点聚类的思路，进一步提出了一种基于三维信息对特征点进行聚类分析形成路标、对路标进行局部特征描述与快速匹配的方法。
1特征点及其三维信息的获取与预处理
机器人使用参数相同的两个摄像机获取环境图像，对图像进行特征点选取与过滤， 又采用双目视觉原理获得特征点对应空间点的三维信息。
1.1特征点的选取
本文采用SURF算法对左右目摄像机获得的图像分别进行特征点提取，基本可以达到实时处理的要求[12]。令由左目获取的图像标记为， 由的特征点组成集合， 同理右目图像的特征点组成集合， 因为两摄像机是对同一场景进行拍摄， 这就使得提取的特征点大致相同。为了计算特征点对应空间点的三维信息， 需要找到左目图象的特征点在右目图像中的对应点以获得该点在双目中的视差， 即某一点在两幅图像中相应点的位置差。 以中的点为基准与中的点进行匹配，使之一一对应，未获匹配的表示该点在其中一目中看不到。匹配上的特征点、组成特征点对并入集合中， 记 为主特征点。
1.2特征点对应空间点的三维信息的获取
特征点对对应的空间点记为， 由所有空间点组成的集合记为。摄像机坐标系的原点在摄像机镜头的光心处。为简化计算， 令左右镜头的参数相同， 两摄像机光心在同一个平面上， 在左摄像机坐标系下的三维坐标信息可以利用视差原理获得：
其中， 是空间点P在左摄像机坐标系下的三维坐标， b为左右摄像机的基线距，d为视差，f为焦距，u1、v1为特征点在左右视觉中的图像坐标。点的三维坐标在聚类过程中将会用到。而提取路标后、进行存储时， 应将所有空间点在摄像机坐标系下的坐标转换为世界坐标系下的坐标。
2自然路标的提取与描述
2.1基于改进MeanShift算法的特征点聚类提取路标
已建立在摄像机坐标系下的坐标， 根据这些点的相对空间位置进行聚类分析以提取路标。 MeanShift算法因为运算量小， 计算速度快，且有一定的鲁棒性，更为适合进行特征点聚类。但却需要对该算法加以改进， 就使得算法可以根据不同特征点的聚合情况，相应产生不同数量的聚类。
MeanShift 可以理解为一个核密度估计的过程， 即将空间点看作采样得到的数据点， 并将单位体积内的空间点数看作概率密度， 由此求取概率密度最大的点集合就完成了聚类。每个点X的概率密度可以表示为：
其中，为核密度函数，k可取高斯函数，是以点为圆心，即为半径的球型区域内的点，为聚类半径， 可以看作 Parzen 窗口尺寸，则为半径内的空间点的个数。
欲求概率密度最大的点，可先对求导，使则可求得极值点，令，有
当， 即可近似求得点概率密度的极值点， 而以其为中心点， 半径的区域内的点将形成一个聚类。其中，为阈值。
考虑到自然路标有大有小，应保证聚类内最少包含个特征点数， 否则形成的聚类特征点太少， 将导致路标不稳定且不利于路标之间的区分，而且更不利于定位。此后，在聚类过程中将自动调整半径。如果以点为圆心， 半径内特征点数， 则以步长增加半径， 直至， 可称为增长半径。其中，为聚类中最少特征点数，为聚类最大半径。
一般情况下，聚类半径长度未增长到最大半径时， 特征点数与已经满足要求了， 此时聚类已经形成， 但存在半径再适当扩大仍然可能有特征点的情况， 为此提出聚类进行小幅度扩张的应对策略。如果有距离聚类较近的点与聚类的距离小于， 可将聚类半径增加以包括该点，并重复该过程。扩张幅度， 即可取。 如果在下一步扩张了的聚类范围内没有更多的点或者超过了最大聚类半径限制， 则随即停止该扩张过程。
组成路标的特征点分布范围半径也应该控制在一定范围内，这与自然环境中作为物体多会有一定尺寸限制现象相吻合。本文取
其中，为聚类初始半径。
确定一个聚类后，从未访问过的特征点中随机选择一点作为中心，继续寻找下一聚类，直至未访问过的点的数量。
2.2聚类描述符
考虑使用一个快速索引匹配路标的方法， 问题可以描述为已知一组形成聚类的点的三维坐标， 设计一个具有唯一性和旋转不变性的描述符， 并且该描述符应在两个聚类匹配过程中有充分的区分度。
本文基于空间点的分布来表征描述符。由聚类中心指向组成聚类的各个特征点形成了一组向量，向量数学原理如式（6）所示，三维空间实现则如图1所示。
将聚类的区域平分若干份， 依次统计各个区域内向量， 得到一个特征向量。 其对应数学表示则如式（6）所示。
在图2中， 以步长0.5滑动窗口， 取绝对值最大的窗口方向为主方向。以聚类的主方向为基准方向， 围绕聚类中心的x轴， 再取逆时针为正方向， 而将整个聚类按为跨度划分作24个区域，并且分别重新计算各个区域的向量的模与高斯核函数的卷积， 将其组成一个24维的向量，该向量即为该聚类的描述符。
3路标匹配过程
机器人在环境中移动时， 对获取的图像先进行预处理， 估计自己的位姿， 并根据当前位姿到地图数据库查询之前机器人以该位姿观察到的路标。令当前位姿为， 为保证找到路标， 可查询符合如下条件的路标，具体是：， ， 。
对当前提取的环境的特征点进行聚类， 计算该聚类描述符，并与数据库中对应路标的描述符进行匹配， 在误差允许的范围内如果实现了匹配， 说明找到了路标， 则取出每个点的信息， 对已然匹配的两个聚类中的各个点进行二次匹配， 对获得匹配的点的世界坐标即可认定为一致， 并且是以数据库中的坐标为基准。根据机器人与每个点的距离、每个点的世界坐标。
4实验结果及分析
本文使配有双目视觉的移动机器人在实验室环境中运行而实现自然路标的提取。图3为左右目特征点提取与匹配结果，可以看到在初步的匹配后，有许多误匹配的点，需要对其进行高质量过滤。
应用视差约束， 视场约束等规则过滤后的结果， 剔除了大多数明显错误的匹配点，如图4所示。
对过滤后的特征点进行聚类?设定初始半径为25cm，半径增长步长为10cm，聚类结果如图5所示。为了直观起见，将聚类得到的特征点对应的图像中的点标出，如图6所示。
需要注意的是，由于机器人位姿不同， 导致对同一个物体的观察角度出现差别， 当观察角度差别较大时， 会出现聚类结果不同的现象，因为物体的不同侧面的特征点不可能完全相同。如果在匹配过程中，使用原有路标的靠近聚类中心的特征点进行引导聚类，无法聚类成功或者无法匹配成功，说明需要建立新的路标，并更新路标库。
5结束语
利用图像特征点的三维坐标信息进行聚类， 可以有效地将空间距离相近的点归为一个路标， 在机器人由于位姿变化导致视角不同时， 仍然可以较大概率匹配到原先观察到的路标。 经过改进的Mean Shift算法根据为不同的场景产生合适的数量不等的路标， 可以很好地适应复杂环境。 本文提出的路标描述符可以有效提高路标匹配速度， 避免重复存储相同路标。 参考文献：
[1] Kortenkamp D， Bonasso R P， Murphy R. AI-based mobile robots： case studies of successful robot systems [M]. Cambridge： MIT Press， 1998.
[2]黄庆成， 洪炳镕， 厉茂海， 等. 基于主动环形闭合约束的移动机器人分层同时定位和地图创建[J]. 计算机研究与发展， 2007， 44（4）： 636-642.
[3] 冯肖维，方明伦，何永义，等. 移动机器人自然路标特征提取方法[J]. 机器人， 2010， 32（4）： 540-546.
[4] 钱堃，马旭东，戴先中，等. 基于显著场景Bayesian Surprise的移动机器人自然路标检测[J]. 模式识别与人工智能， 2013， 26（6）：571-576.
[5] XU X D， LUO Y， HAO H G. Vision-based mobile robot localization using natural landmarks[C]// Systems and Informatics （ICSAI）， 2012 International Conference on， Chicago， USA： IEEE， 2012： 2012 – 2015.
[6] SE S， LOWE D G， et al. Vision-based global localization and mapping for mobile robots [J]. Robotics， IEEE Transactions on. 2005， 21（3）： 364-375.
[7]林睿.基于图像特征点的移动机器人立体视觉SLAM研究[D]. 哈尔滨： 哈尔滨工业大学， 2012.
[8] SHARMA K， MOON I， et al. Extraction of visual landmarks using improved feature matching technique for stereo vision applications [J]. IETE Technical Review. 2012， 29（6）： 473-481.
[9] MOZOS ? M， GIL A， et al. Interest Point Detectors for Visual SLAM[C]// 12th Conference of the Spanish Association for Artificial Intelligence， Springer Berlin Heidelberg. 2007： 170-179.
[10] MONTIEL J M M， CIVERA J， DAVISON A J. Unified inverse depth parametrization for monocular SLAM[C]// Proceedings of Robotics：Science and Systems. Cambridge， Massachusetts ：MIT Press， 2006.
[11] 蔡自兴，王勇，王璐.基于角点聚类的移动机器人自然路标检测与识别[J].智能系统学报，2006，1（1）： 52-56.
[12] BAY H， ESS A， et al. Speeded-up robust features （SURF） [J]. Computer Vision and Image Understanding. 2008， 110（3）： 346-359.
关键词： 移动机器人；自然路标提取；路标描述符；MeanShift
中图分类号：TP242.6 文献标识号：A 文章编号：2095-2163（2015）01-
Abstract： Landmark extraction and matching is basis of vSLAM. A method of landmark extraction， local feature description and fast matching based on 3D information of feature points is proposed. Robot obtains images of environment via binocular vision， extracting feature points from left and right eye images， matching feature points of the two images. Three-dimensional information of each matched points under left camera coordinate system is built. Field of view constraint rule is proposed to filtering points. Then， the method of natural landmark extraction based on improved Mean Shift algorithm is discussed. The paper proposes a landmark descriptor， which can achieve fast matching of the two clustering. This method can extract natural landmarks in unstructured environment， tolerating relatively low accuracy of pose estimation.
Key words： Mobile Robot； Natural Landmark Extraction； Landmark Descriptor； MeanShift
0引 言
同时定位和地图创建（Simultaneous Localization and Mapping， SLAM）是实现机器人自主导航的一个关键技术[1]。SLAM技术主要采用视觉[2]、激光[3]等传感器以实现环境感知，尤其是基于视觉的SLAM技术（vSLAM）具有的对环境无侵性、获取环境信息的丰富性、以及成本低廉等显著优势，使其受到研究学界的广泛关注和高度重视。
vSLAM主要采用路标方法， 路标的自动提取与快速匹配即是其中的基础性问题。基于特征点的路标的表示方法主要分为两类。在此，可做如下具体分析：
一类以提取出的特征点直接作为路标，这类方法构建的地图路标的特征点数量较多， 进行场景匹配的运算量也随之较大。重点成果则有：文献[2]基于SIFT算法对环境图象进行特征提取，形成路标并构建分层地图。 文献[4]也提出一种基于显著场景Bayesian Surprise的自然路标检测方法。并且，文献[5]又使用关键帧Harris角点作为自然路标， 实现了机器人的实时全局定位与导航。相应地，文献[6]亦同样使用环境图像的SIFT特征点作为自然路标，利用特征点信息建立自然路标数据库，实现了环境地图创建。针对特征点的提取也进行了大量的研究，文献[7]即提出了基于图像二阶多项扩展式局部方向张量的兴趣点检测 PLOT算子，并利用双目视觉实现了机器人的SLAM过程。继而，文献[8]提出一种利用双目视觉有效提取特征点的无监督算法， 该算法使用SOM对SIFT特征点进行快速匹配以获得稳定的特征点。还有文献[9]通过应用Harris算法来提取角点。而文献[10]则针对单目视觉提取的特征点给出了统一逆深度参数方法来准确表达其不确定性。
另一类路标表示法是通过对提取的特征点进一步聚类， 再将获得的聚类整体作为一个路标。相应成果有，文献[11]提出了一种基于角点聚类的自然路标局部特征提取其匹配算法。本文则基于对特征点聚类的思路，进一步提出了一种基于三维信息对特征点进行聚类分析形成路标、对路标进行局部特征描述与快速匹配的方法。
1特征点及其三维信息的获取与预处理
机器人使用参数相同的两个摄像机获取环境图像，对图像进行特征点选取与过滤， 又采用双目视觉原理获得特征点对应空间点的三维信息。
1.1特征点的选取
本文采用SURF算法对左右目摄像机获得的图像分别进行特征点提取，基本可以达到实时处理的要求[12]。令由左目获取的图像标记为， 由的特征点组成集合， 同理右目图像的特征点组成集合， 因为两摄像机是对同一场景进行拍摄， 这就使得提取的特征点大致相同。为了计算特征点对应空间点的三维信息， 需要找到左目图象的特征点在右目图像中的对应点以获得该点在双目中的视差， 即某一点在两幅图像中相应点的位置差。 以中的点为基准与中的点进行匹配，使之一一对应，未获匹配的表示该点在其中一目中看不到。匹配上的特征点、组成特征点对并入集合中， 记 为主特征点。
1.2特征点对应空间点的三维信息的获取
特征点对对应的空间点记为， 由所有空间点组成的集合记为。摄像机坐标系的原点在摄像机镜头的光心处。为简化计算， 令左右镜头的参数相同， 两摄像机光心在同一个平面上， 在左摄像机坐标系下的三维坐标信息可以利用视差原理获得：
其中， 是空间点P在左摄像机坐标系下的三维坐标， b为左右摄像机的基线距，d为视差，f为焦距，u1、v1为特征点在左右视觉中的图像坐标。点的三维坐标在聚类过程中将会用到。而提取路标后、进行存储时， 应将所有空间点在摄像机坐标系下的坐标转换为世界坐标系下的坐标。
2自然路标的提取与描述
2.1基于改进MeanShift算法的特征点聚类提取路标
已建立在摄像机坐标系下的坐标， 根据这些点的相对空间位置进行聚类分析以提取路标。 MeanShift算法因为运算量小， 计算速度快，且有一定的鲁棒性，更为适合进行特征点聚类。但却需要对该算法加以改进， 就使得算法可以根据不同特征点的聚合情况，相应产生不同数量的聚类。
MeanShift 可以理解为一个核密度估计的过程， 即将空间点看作采样得到的数据点， 并将单位体积内的空间点数看作概率密度， 由此求取概率密度最大的点集合就完成了聚类。每个点X的概率密度可以表示为：
其中，为核密度函数，k可取高斯函数，是以点为圆心，即为半径的球型区域内的点，为聚类半径， 可以看作 Parzen 窗口尺寸，则为半径内的空间点的个数。
欲求概率密度最大的点，可先对求导，使则可求得极值点，令，有
当， 即可近似求得点概率密度的极值点， 而以其为中心点， 半径的区域内的点将形成一个聚类。其中，为阈值。
考虑到自然路标有大有小，应保证聚类内最少包含个特征点数， 否则形成的聚类特征点太少， 将导致路标不稳定且不利于路标之间的区分，而且更不利于定位。此后，在聚类过程中将自动调整半径。如果以点为圆心， 半径内特征点数， 则以步长增加半径， 直至， 可称为增长半径。其中，为聚类中最少特征点数，为聚类最大半径。
一般情况下，聚类半径长度未增长到最大半径时， 特征点数与已经满足要求了， 此时聚类已经形成， 但存在半径再适当扩大仍然可能有特征点的情况， 为此提出聚类进行小幅度扩张的应对策略。如果有距离聚类较近的点与聚类的距离小于， 可将聚类半径增加以包括该点，并重复该过程。扩张幅度， 即可取。 如果在下一步扩张了的聚类范围内没有更多的点或者超过了最大聚类半径限制， 则随即停止该扩张过程。
组成路标的特征点分布范围半径也应该控制在一定范围内，这与自然环境中作为物体多会有一定尺寸限制现象相吻合。本文取
其中，为聚类初始半径。
确定一个聚类后，从未访问过的特征点中随机选择一点作为中心，继续寻找下一聚类，直至未访问过的点的数量。
2.2聚类描述符
考虑使用一个快速索引匹配路标的方法， 问题可以描述为已知一组形成聚类的点的三维坐标， 设计一个具有唯一性和旋转不变性的描述符， 并且该描述符应在两个聚类匹配过程中有充分的区分度。
本文基于空间点的分布来表征描述符。由聚类中心指向组成聚类的各个特征点形成了一组向量，向量数学原理如式（6）所示，三维空间实现则如图1所示。
将聚类的区域平分若干份， 依次统计各个区域内向量， 得到一个特征向量。 其对应数学表示则如式（6）所示。
在图2中， 以步长0.5滑动窗口， 取绝对值最大的窗口方向为主方向。以聚类的主方向为基准方向， 围绕聚类中心的x轴， 再取逆时针为正方向， 而将整个聚类按为跨度划分作24个区域，并且分别重新计算各个区域的向量的模与高斯核函数的卷积， 将其组成一个24维的向量，该向量即为该聚类的描述符。
3路标匹配过程
机器人在环境中移动时， 对获取的图像先进行预处理， 估计自己的位姿， 并根据当前位姿到地图数据库查询之前机器人以该位姿观察到的路标。令当前位姿为， 为保证找到路标， 可查询符合如下条件的路标，具体是：， ， 。
对当前提取的环境的特征点进行聚类， 计算该聚类描述符，并与数据库中对应路标的描述符进行匹配， 在误差允许的范围内如果实现了匹配， 说明找到了路标， 则取出每个点的信息， 对已然匹配的两个聚类中的各个点进行二次匹配， 对获得匹配的点的世界坐标即可认定为一致， 并且是以数据库中的坐标为基准。根据机器人与每个点的距离、每个点的世界坐标。
4实验结果及分析
本文使配有双目视觉的移动机器人在实验室环境中运行而实现自然路标的提取。图3为左右目特征点提取与匹配结果，可以看到在初步的匹配后，有许多误匹配的点，需要对其进行高质量过滤。
应用视差约束， 视场约束等规则过滤后的结果， 剔除了大多数明显错误的匹配点，如图4所示。
对过滤后的特征点进行聚类?设定初始半径为25cm，半径增长步长为10cm，聚类结果如图5所示。为了直观起见，将聚类得到的特征点对应的图像中的点标出，如图6所示。
需要注意的是，由于机器人位姿不同， 导致对同一个物体的观察角度出现差别， 当观察角度差别较大时， 会出现聚类结果不同的现象，因为物体的不同侧面的特征点不可能完全相同。如果在匹配过程中，使用原有路标的靠近聚类中心的特征点进行引导聚类，无法聚类成功或者无法匹配成功，说明需要建立新的路标，并更新路标库。
5结束语
利用图像特征点的三维坐标信息进行聚类， 可以有效地将空间距离相近的点归为一个路标， 在机器人由于位姿变化导致视角不同时， 仍然可以较大概率匹配到原先观察到的路标。 经过改进的Mean Shift算法根据为不同的场景产生合适的数量不等的路标， 可以很好地适应复杂环境。 本文提出的路标描述符可以有效提高路标匹配速度， 避免重复存储相同路标。 参考文献：
[1] Kortenkamp D， Bonasso R P， Murphy R. AI-based mobile robots： case studies of successful robot systems [M]. Cambridge： MIT Press， 1998.
[2]黄庆成， 洪炳镕， 厉茂海， 等. 基于主动环形闭合约束的移动机器人分层同时定位和地图创建[J]. 计算机研究与发展， 2007， 44（4）： 636-642.
[3] 冯肖维，方明伦，何永义，等. 移动机器人自然路标特征提取方法[J]. 机器人， 2010， 32（4）： 540-546.
[4] 钱堃，马旭东，戴先中，等. 基于显著场景Bayesian Surprise的移动机器人自然路标检测[J]. 模式识别与人工智能， 2013， 26（6）：571-576.
[5] XU X D， LUO Y， HAO H G. Vision-based mobile robot localization using natural landmarks[C]// Systems and Informatics （ICSAI）， 2012 International Conference on， Chicago， USA： IEEE， 2012： 2012 – 2015.
[6] SE S， LOWE D G， et al. Vision-based global localization and mapping for mobile robots [J]. Robotics， IEEE Transactions on. 2005， 21（3）： 364-375.
[7]林睿.基于图像特征点的移动机器人立体视觉SLAM研究[D]. 哈尔滨： 哈尔滨工业大学， 2012.
[8] SHARMA K， MOON I， et al. Extraction of visual landmarks using improved feature matching technique for stereo vision applications [J]. IETE Technical Review. 2012， 29（6）： 473-481.
[9] MOZOS ? M， GIL A， et al. Interest Point Detectors for Visual SLAM[C]// 12th Conference of the Spanish Association for Artificial Intelligence， Springer Berlin Heidelberg. 2007： 170-179.
[10] MONTIEL J M M， CIVERA J， DAVISON A J. Unified inverse depth parametrization for monocular SLAM[C]// Proceedings of Robotics：Science and Systems. Cambridge， Massachusetts ：MIT Press， 2006.
[11] 蔡自兴，王勇，王璐.基于角点聚类的移动机器人自然路标检测与识别[J].智能系统学报，2006，1（1）： 52-56.
[12] BAY H， ESS A， et al. Speeded-up robust features （SURF） [J]. Computer Vision and Image Understanding. 2008， 110（3）： 346-359.