基于一点ransac和fast算法的移动机器人导航方法

基于一点ransac和fast算法的移动机器人导航方法
【技术领域】
[0001] 本发明属于移动机器人自主定位和导航领域，具体设及基于图像匹配的移动机器 人定位与导航。
【背景技术】
[0002] 随着计算机技术的飞速发展和社会的不断进步，移动机器人的应用得到了不断扩 展，人工智能水平也得到了不断提高。和传统的工业机器人相比较而言，移动机器人集成了 更多的先进科技，正逐步成为一个集任务规划、环境创建与自主定位、路径规划、实时导航、 目标识别等多项功能于一体的综合性系统。其应用领域也从工业逐渐扩展到了军事、医疗、 家用服务、紧急救援、海洋开发、太空探索等各个方面，应用领域不断扩展的同时也对机器 人的自主定位与导航能力提出了更高的要求。
[0003] 近20年来，基于各种传感器的导航技术研究获得了巨大的发展，而基于视觉传感 器的同时定位与地图创建技术（SimultaneousLocalizationAndMapping,SLAM问题）受 到了最为广泛的重视。移动机器人在完全未知的环境中利用自身传感器获得周围环境信息 进行实时定位和地图创建SLAM是移动机器人需要解决的关键问题。SLAM是指机器人在一 个未知的环境中，从一个未知的位置开始，通过对环境的观测，递增地构建环境地图，并同 时运用环境地图实现机器人定位的一个过程。基于视觉传感器的导航技术根据视觉摄像机 数目的不同，可W分为全向视觉定位与导航、双目视觉定位与导航、单目视觉定位与导航。 其中基于单目视觉的导航方法操作方便、模型简单，其实时性也较高，而且单目视觉成像设 备如数码相机、监控摄像头、拍照手机等，其数量多，体积小，结构简单，更重要的是该些设 备更加符合人们的使用习惯。因此基于单目视觉SLAM的移动机器人导航技术最有发展前 景的一种机器人自主导航技术。
[0004] 另外，尽管在简单的结构化环境W及地图信息已知（具有人工路标）的环境中 进行视觉导航已经有了很大进展，但在完全未知的环境中进行定位与导航还存在诸多问 题。造成该一现象的根本原因在于如何在未知环境中获取自然特征。所谓自然特征，是 指环境中已有的、非人工设置的、能够用W标识不同环境场景的特征对象。通过提取一 些具备局部不变性的角点作为环境的自然特征，该方法选取的自然路标有非常好的稳定 性，因此角点检测也是机器人导航的热点问题之一，也出现了一些较好的角点检测方法。 FAST(feagresfromacceleratedsegmenttest)是一种运算简单直观的角点检测方 法，相关测试表明，FAST在计算速度上优于SIFT、SURF和Harries等角点检测方法。文献 (基于FAST改进的快速角点探测算法，红外与激光工程，2009 (6) : 1104-1108)采用改进 的FAST算法来提取角点，剔除了大部分边缘点和局部非极大值点；另一方面，针对误匹配 剔除，文献（STRASDAH，M0NTIELJMM,DAVISONAJ.Real-timemonocularSLAM:why filter[C],ProceedingsoftheIEEEInternationalConferenceonroboticsand automation. 2010:2657-2664. )RANSAC算法是一种应用广泛的误匹配剔除算法，但该算法 随着匹配点个数增加，算法迭代次数会迅速上升，降低了算法的计算速度。

【发明内容】

[0005] 本发明针对现有技术中存在的上述技术问题，采用一点RANSAC(RandomSample Consensus,随机抽样一致性）算法可W充分利用EKF预测阶段得到的先验信息，在确 保计算精度的同时，有效降低了算法迭代次数。机器人通过自身摄像头，采用改进的 FAST(FeaUiresfromAcceleratedSegmentTest,加速段检测特征点）角点提取算法采集 周围环境信息，把一点RANSAC算法和FAST算法结合起来，共同用于移动机器人自主导航中 的地图构建和定位。并结合EKF的一点RANSAC算法对误匹配点进一步剔除，获得鲁椿性更 强的特征点，从而构建环境的地图信息，最后用得到的匹配点进行=维环境重建和机器人 定位。
[0006] 本发明解决上述技术问题的技术方案是，提出一种基于一点RANSAC(Random SampleConsensus,随机抽样一致性）和FAST(Fea1:uresfromAcceleratedSegment Test,加速段检测特征点）算法的移动机器人导航方法，该方法包括步骤；机器人运动过程 中摄像头提取FAST特征点，根据特征点进行相邻帖的匹配，如果匹配点个数大于阔值，进 行一点RANSAC误匹配剔除，建立拟合模型获取移动机器人路径，重建=维环境，根据提取 的FAST特征点进行KLT特征跟踪，完成机器人导航。
[0007] 具体包括，机器人运动过程中摄像头采集图像信息，从采集的图像信息视频帖中 提取特征点，根据特征点对相邻两个视频帖的同名特征点进行粗匹配，对同名特征点进行 精确匹配获得图像的匹配特征点，根据匹配特征点进行环境模型拟合，规划机器人的运动 路径，驱动移动机器人进行逐步探索，获得运动路径的环境信息建立环境=维模型，根据= 维模型进行特征跟踪控制机器人自主导航。
[0008] 同名特征点进行粗匹配具体包括；从提取的特征点中截取特征片元，从中选出稳 定点集，稳定点集中的每个稳定点片元变换得到训练片元；随机生成随机藤丛和藤判断节 点属性参数，将训练片元投入随机藤丛，得到二值串划分，统计各个二值串划分的类型分 布，输出一个完整的随机藤丛结构和训练样本的分类统计；检测当前帖的自然特征并截取 形成待匹配特征片元，将所有待匹配特征片元投入完整的随机藤丛得到二值串划分，分类 器集成各二值串划分的训练统计结果，作为同名特征点的粗匹配结果。
[0009] 提取特征点进一步包括；产生稳定的内点集ziuniisu，根据卡尔曼滤波增益矩阵 Kk，预测误差均方矩阵Pk|k_i，预测模型评估的雅可比矩阵Hk，调用公式：
[0010]
获得稳定的内点集ziunlis"的特征向量 V;
[0011] 对其余的不稳定内点zhunlku进行补救，根据设定的补救条件，重新得到卡尔曼滤 波增益矩阵K'k和预测模型评估的雅可比矩阵H'k，并调用公式：
[001引 ，获得不稳定内点zhUnliKs的特征向量是D
[0013]W任一像素点X为圆屯、，作一个半径为3个像素点的圆，圆周上所有像素点按顺时 针依次编号，根据公式二二将每个像素点分别与中屯、像素X 求灰度差，将灰度差值作为特征判断条件，如果圆周上像素至少有连续n个像素灰度 差在预定阔值范围内，则中屯、像素X被视为图像的匹配特征点FAST-n。
[0014] 通常远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在 RANSAC假设中，一个远距离点可W产生一个对角度变换较敏感的点，而对尺度变换不是很 精确。在该种情况下，远距离点支持拟合模型，但是由于尺度变化的精度不高，附近的点即 使是内点也可能表现出较高的不确定性。所W在确定了拟合模型之后，需要从不稳定的点 集中"补救"一些内点，W提高系统的稳定性。在采用可靠内点进行局部状态和协方差更新 后，一些不稳定的内点会被挽救。
[0015] 使用改进的FAST角点提取算法相对于经典FAST算法，既可W保持环境模型的各 向同性，又具有便于计算机高效处理的超高速度。采用FAST-9角点检测具有最高的可再现 性，依次检测圆周像素Pi，Pe，Pg，Pi3,当其中连续S个点经过灰度差值判断，满足灰度差值条 件，就进一步检测该=个点所覆盖圆弧的圆周像素点，若该圆周像素点也满足灰度差值条 件，就可W判定该点为特征点，否则该点就不为特征点。也可采用本领域技术人员熟知的方 法其他方法实现，W下WFAST-9角点检测为例对其实现方法进一步具体说明。W像素点 X为圆屯、，作一个半径为3个像素点的圆，对圆周上所有像素点按顺时针依次编号，将每个 像素Pi，iG 分别与中屯、像素X求灰度差，灰度差值作为特征判断条件： Mr气x=IprIx，Ip,=Ip,，,md、b.〇
[001引圆周像素Pi经灰度差值判断后的状态为为像素点X的状态。如果 圆周上1-16号像素中至少有连续n个像素点灰度差都大于阔值t或都小于阔值-t，则中屯、 像素点X被视为匹配特征点FAST-n。
[0017] 如公式所示：
[001引使用W上算法遍历检测所有像素，可得到当前帖的FAST角点。通过改变阔值t可 W控制图像帖的FAST角点个数。
[0020] 本发明采用的改进FAST(加速段检测特征点）算法，可W剔除经典FAST算法在 提取角点时存在的大部分边缘点和局部非极大值点；同时采用一点RANSAC(随机抽样一致 性）算法来充分利用EKF(扩展卡尔曼滤波）预测阶段W得到先验信息，在确保计算精度的 同时有效降低了算法迭代次数。本发明提供的移动机器人导航方法能有效地提高特征匹配 的正确率，将匹配过程中出现的错误匹配剔除，并能有效提高算法实时性。
【附图说明】
[0021] 图1为移动机器人导航方法流程图。
【具体实施方式】
[0022] 图1所示为基于一点RANSAC和FAST算法的移动机器人导航方法流程图，其具体 内容如下：
[0023] (1)启动移动机器人控制系统，进行摄像头参数标定。
[0024] (2)移动机器人在伺服驱动器和伺服电机驱动下在环境中探索行进，同时摄像头 不停地采集环境图像信息。
[0025] (3)从采集的视频帖中提取特征点。如采用如下方法提取：
[0026] 对相邻两个视频帖的同名特征点进行粗匹配。提取关键帖的自然特征点截取特征 片元，经仿射变换选出稳定点集，稳定点集中的每个稳定点片元经仿射变换得到训练片 元； 随机生成随机藤丛及其判断节点属性参数；将训练片元投入随机藤丛，得到二值串划分，统 计各个二值串划分的类型分布；输出一个完整的随机藤丛和训练样本的分类统计结果；检 测当前帖的自然特征并截取形成待匹配特征片元，将每个特征片元投入随机藤丛得到一系 列二值串划分，分类器基于半朴素贝叶斯模型集成各二值串划分的训练统计结果，实现相 邻帖的同名点粗匹配。具体为：
[0027] 1)提取帖中的自然特征点，并截取特征片元，从中选出稳定点集，从稳定点集中的 每个稳定点片元变换得到大量训练片元，可通过仿射变换得到大量训练片元，仿射变换可 采用公式（4)所示获得。
[0028]
(4)
[0029] 其中，X和X'为相邻两帖中的同名点，两者的关系为仿射变换。
[0030] 2)随机生成一定规模的随机藤丛和藤判断节点属性参数；将训练片元投入随机 藤丛，得到二值串形式的划分，统计各个划分的类型分布；输出一个完整的随机藤丛结构和 训练样本的分类统计结果。
[0031] 3)通过快速检测当前帖的自然特征并截取形成待匹配片元，将每个特征片元投入 随机藤丛得到二值串划分，分类器将基于半朴素贝叶斯模型集成各划分的训练统计结果， 实现同名点粗匹配。
[003引（4)同名点粗匹配W后，再基于一点RANSAC剔除误匹配算法，得到精确匹配的同 名点对。
[0033] (5)根据图像中匹配的特征点进行环境模型的拟合。
[0034] 做根据已知环境信息，规划移动机器人的运动路径，驱动移动机器人对未知的环 境进行逐步探索。
[0035] (7)移动机器人对环境完成全部探索，获得充分的环境信息，并W此建立环境=维 模型。
[0036] (8)在环境S维模型的基础上，通过KLT(Kanade-Lucas-Tomasi)算法进行特征跟 踪，控制移动机器人按照预定要求进行自主导航。
[0037] -点RANSAC算法得到精确匹配的同名点对。
[003引产生稳定的内点集zlUnlisu，并用zlUnliets进行邸F(扩展卡尔曼滤波）部分更新。
[0039]对移动机器人摄像头视频相邻的帖进行特征匹配，所得的数据点集为Zi， 匹配性最好的数据点集为zii-iniiets(low-innovationiniiers)，其余的点为zhi-inue"(hi曲-innovationinliers)，用匹配性最好的数据点集拟合一个数据模型。通常，远 距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在RANSAC中，一个 远距离点可W产生一个对角度变换较敏感的点，而对尺度变换不是很精确，在该种情况下， 其他远距离点也会支持该个拟合模型。但是由于尺度变化的精度不高，附近的点有可能表 现出较高的不确定性，即使他们是内点。所W，在确定了拟合的模型之后，还要从不稳定的 点集中"补救"一些内点。在仅采用可靠内点进行局部状态和协方差更新后，该些不稳定的 内点会被挽救。本发明可采用ziuniku进行EKF部分更新。建立测量方程/?'(兩1;_^的雅可比 行列式Hk=化…H"〇t，摄像头噪声的协方差Rk，调用公式（1)进行EKF(扩展卡尔曼 滤波）部分更新。
[0040]
…
[0041] 其中，而是特征向量，Kk是卡尔曼滤波增益矩阵，PkM是预测误差均方矩阵，Hk预 测模型评估的雅可比矩阵。
[004引对其余的点不稳定内点zhUnlk"进行补救。
[0043] 调用公式（2)对不稳定点进行EKF(扩展卡尔曼滤波）部分更新。
[0044]
(2)
[004引其中，是I*是特征向量，K'k是卡尔曼滤波增益矩阵，Pkik-1是预测误差均方矩阵，吐k预测模型评估的雅可比矩阵。
[0046] 通常远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在 RANSAC假设中，一个远距离点可W产生一个对角度变换较敏感的点，而对尺度变换不是很 精确。在该种情况下，远距离点支持拟合模型，但是由于尺度变化的精度不高，附近的点即 使是内点也可能表现出较高的不确定性。所W在确定了拟合模型之后，需要从不稳定的点 集中"补救"一些内点，w提高系统的稳定性。在采用可靠内点进行局部状态和协方差更新 后，一些不稳定的内点会被挽救。
[0047] 改进的FAST算法
[0048] 使用改进的FAST角点提取算法相对于经典FAST算法，既可W保持环境模型的各 向同性，又具有便于计算机高效处理的超高速度。采用FAST-9角点检测具有最高的可再现 性，依次检测圆周像素Pi，Pe，Pg，Pi3,当其中连续S个点经过灰度差值判断，满足灰度差值条 件，就进一步检测该=个点所覆盖圆弧的圆周像素点，若该圆周像素点也满足灰度差值条 件，就可W判定该点为特征点，否则该点就不为特征点。也可采用本领域技术人员熟知的方 法其他方法实现，W下WFAST-9角点检测为例对其实现方法进一步具体说明。W像素点X 为圆屯、，做一个半径为3个像素点的圆，圆周上的16个像素点按顺时针依次编号，将每个 像素Pi，iG 分别与中屯、像素X求灰度差，灰度差值作为特征判断条件： A//:一义二/,;-/,'，//;=/巧
[004引圆周像素P备灰度差值判断后的状态为乂巧像素点X的状态。如果圆 周上1-16号像素中至少有连续n个像素点灰度差都大于阔值t或都小于阔值-t，则中屯、像 素点X被视为FAST-n特征点。如公式所示：
[005引使用W上算法遍历检测所有像素，可得到当前帖的FAST角点。通过改变阔值t可W控制图像帖的FAST角点个数。
[0053] W上实施例仅用W说明本发明的技术方案而非限制，尽管参照较佳实施例对本发 明进行了详细说明，本领域的普通技术人员应当理解，可W对本发明的技术方案进行修改 或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围 当中。
【主权项】
1. 基于一点RANSAC和FAST算法的移动机器人导航方法，其特征在于，机器人运动过程 中摄像头采集图像信息，从采集的图像信息视频帧中提取特征点，根据特征点对相邻两个 视频帧的同名特征点进行粗匹配，在粗匹配基础上对同名特征点进行精确匹配获得图像的 匹配特征点，根据匹配特征点进行环境模型拟合，规划机器人的运动路径，驱动移动机器人 进行逐步探索，获得运动路径的环境信息建立环境三维模型，根据三维模型进行特征跟踪 控制机器人自主导航。2. 根据权利要求1所述的方法，其特征在于，同名特征点进行粗匹配具体包括：从提取 的特征点中截取特征片元，从中选出稳定点集，稳定点集中的每个稳定点片元变换得到训 练片元；随机生成随机蕨丛和蕨判断节点属性参数，将训练片元投入随机蕨丛，得到二值串 划分，统计各个二值串划分的类型分布，输出一个完整的随机蕨丛结构和训练样本的分类 统计；检测当前帧的自然特征并截取形成待匹配特征片元，将所有待匹配特征片元投入完 整的随机蕨丛得到二值串划分，分类器集成各二值串划分的训练统计结果，作为同名特征 点的粗匹配结果。3. 根据权利要求1所述的方法，其特征在于，提取特征点进一步包括：产生稳定的内点 集Zli-inlia^根据卡尔曼滤波增益矩阵K k，预测误差均方矩阵Pkllrl，预测模型评估的雅可比 矩阵Hk，调用公式：t稳定的内点集Z1Unli…的特征向量 Pklk - (I-K kHk) PkIk-! 其中，/?'Uu d是测量方程Hk= (H1-Hi…Ηη，）τ的雅可比行列式，R k是摄像头噪声的协 方差矩阵，是对当前状态的估计，是根据上个状态对当前状态的估计。4. 根据权利要求1所述的方法，其特征在于，以任一像素点X为圆心，作一 个半径为3个像素点的圆，圆周上所有的像素点按顺时针依次编号，根据公式 =/,卩_/ζ，& =&_#·将每个像素点分别与中心像素点X求灰度差，将灰度差 值作为特征判断条件，如果圆周上像素点至少有连续η个像素点灰度差在预定阈值 范围内，则中心像素X被视为图像的匹配特征点FAST-n。5. 根据权利要求2所述的方法，其特征在于，根据公式通过仿射变换得到大量训练片元，其中，X和X'为相邻两帧中的同名点，Ha是仿射变换矩 阵，该矩阵中的6个参数为3对同名控制点坐标，根据实际应用场景可以确定3对同名控制 点的坐标。6. 根据权利要求3所述的方法，其特征在于，提取特征点进一步包括：对其余的不稳定 内点Zhi- inliOTS进行补救，根据设定的补救条件，重新得到卡尔曼滤波增益矩阵K' k和预测模 型评估的雅可比矩阵H'k，并调用公式： xm=xk\k-i+K Λζ - -h (?-!)) K',=PuJi'[ui', Z51ii-, //'[+^r1 ，获得不稳定内点 zhi-inliers 的特征向量 7 1 ? Pklk= α-K' kH'JPklk-P
【专利摘要】本发明请求保护一种基于一点RANSAC和FAST算法的移动机器人导航方法，该方法主要包括：机器人通过自身摄像头采用FAST角点提取算法采集周围的环境信息；在匹配过程中采用结合扩展卡尔曼滤波的一点RANSAC算法进行误匹配剔除；用得到的匹配点进行三维环境重建和地图创建。针对现有技术采用RANSAC算法迭代次数过高的问题，充分利用了滤波阶段得到的先验信息，在保证算法鲁棒性的同时有效降低了算法迭代次数，完成了特征匹配。可以解决传统移动机器人在单目视觉导航方面实时性与鲁棒性较差，以及RANSAC算法迭代次数过高的问题。
【IPC分类】G05D1/02
【公开号】CN104881029
【申请号】CN201510248666
【发明人】刘想德, 张毅, 唐贤伦, 罗元, 李彦
【申请人】重庆邮电大学
【公开日】2015年9月2日
【申请日】2015年5月15日