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

Abstract

本发明请求保护一种基于一点RANSAC和FAST算法的移动机器人导航方法，该方法主要包括：机器人通过自身摄像头采用FAST角点提取算法采集周围的环境信息；在匹配过程中采用结合扩展卡尔曼滤波的一点RANSAC算法进行误匹配剔除；用得到的匹配点进行三维环境重建和地图创建。针对现有技术采用RANSAC算法迭代次数过高的问题，充分利用了滤波阶段得到的先验信息，在保证算法鲁棒性的同时有效降低了算法迭代次数，完成了特征匹配。可以解决传统移动机器人在单目视觉导航方面实时性与鲁棒性较差，以及RANSAC算法迭代次数过高的问题。

Description

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

技术领域

本发明属于移动机器人自主定位和导航领域，具体涉及基于图像匹配的移动机器人定位与导航。

背景技术

随着计算机技术的飞速发展和社会的不断进步，移动机器人的应用得到了不断扩展，人工智能水平也得到了不断提高。和传统的工业机器人相比较而言，移动机器人集成了更多的先进科技，正逐步成为一个集任务规划、环境创建与自主定位、路径规划、实时导航、目标识别等多项功能于一体的综合性系统。其应用领域也从工业逐渐扩展到了军事、医疗、家用服务、紧急救援、海洋开发、太空探索等各个方面，应用领域不断扩展的同时也对机器人的自主定位与导航能力提出了更高的要求。

近20年来，基于各种传感器的导航技术研究获得了巨大的发展，而基于视觉传感器的同时定位与地图创建技术(Simultaneous Localization AndMapping，SLAM问题)受到了最为广泛的重视。移动机器人在完全未知的环境中利用自身传感器获得周围环境信息进行实时定位和地图创建SLAM是移动机器人需要解决的关键问题。SLAM是指机器人在一个未知的环境中，从一个未知的位置开始，通过对环境的观测，递增地构建环境地图，并同时运用环境地图实现机器人定位的一个过程。基于视觉传感器的导航技术根据视觉摄像机数目的不同，可以分为全向视觉定位与导航、双目视觉定位与导航、单目视觉定位与导航。其中基于单目视觉的导航方法操作方便、模型简单，其实时性也较高，而且单目视觉成像设备如数码相机、监控摄像头、拍照手机等，其数量多，体积小，结构简单，更重要的是这些设备更加符合人们的使用习惯。因此基于单目视觉SLAM的移动机器人导航技术最有发展前景的一种机器人自主导航技术。

另外，尽管在简单的结构化环境以及地图信息已知(具有人工路标)的环境中进行视觉导航已经有了很大进展，但在完全未知的环境中进行定位与导航还存在诸多问题。造成这一现象的根本原因在于如何在未知环境中获取自然特征。所谓自然特征，是指环境中已有的、非人工设置的、能够用以标识不同环境场景的特征对象。通过提取一些具备局部不变性的角点作为环境的自然特征，该方法选取的自然路标有非常好的稳定性，因此角点检测也是机器人导航的热点问题之一，也出现了一些较好的角点检测方法。FAST(features fromaccelerated segment test)是一种运算简单直观的角点检测方法，相关测试表明，FAST在计算速度上优于SIFT、SURF和Harries等角点检测方法。文献(基于FAST改进的快速角点探测算法，红外与激光工程,2009(6):1104-1108)采用改进的FAST算法来提取角点，剔除了大部分边缘点和局部非极大值点；另一方面，针对误匹配剔除，文献(STR ASDA H,MONTIEL J M M,DAVISON A J.Real-time monocular SLAM:why filter[C],Proceedings of the IEEE InternationalConference on robotics and automation.2010:2657-2664.)RANSAC算法是一种应用广泛的误匹配剔除算法，但该算法随着匹配点个数增加，算法迭代次数会迅速上升，降低了算法的计算速度。

发明内容

本发明针对现有技术中存在的上述技术问题，采用一点RANSAC(RandomSample Consensus，随机抽样一致性)算法可以充分利用EKF预测阶段得到的先验信息，在确保计算精度的同时，有效降低了算法迭代次数。机器人通过自身摄像头，采用改进的FAST(Features from Accelerated Segment Test,加速段检测特征点)角点提取算法采集周围环境信息，把一点RANSAC算法和FAST算法结合起来，共同用于移动机器人自主导航中的地图构建和定位。并结合EKF的一点RANSAC算法对误匹配点进一步剔除，获得鲁棒性更强的特征点，从而构建环境的地图信息，最后用得到的匹配点进行三维环境重建和机器人定位。

本发明解决上述技术问题的技术方案是，提出一种基于一点RANSAC(Random Sample Consensus，随机抽样一致性)和FAST(Features fromAccelerated Segment Test,加速段检测特征点)算法的移动机器人导航方法，该方法包括步骤：机器人运动过程中摄像头提取FAST特征点，根据特征点进行相邻帧的匹配，如果匹配点个数大于阈值，进行一点RANSAC误匹配剔除，建立拟合模型获取移动机器人路径，重建三维环境，根据提取的FAST特征点进行KLT特征跟踪，完成机器人导航。

具体包括，机器人运动过程中摄像头采集图像信息，从采集的图像信息视频帧中提取特征点，根据特征点对相邻两个视频帧的同名特征点进行粗匹配，对同名特征点进行精确匹配获得图像的匹配特征点，根据匹配特征点进行环境模型拟合，规划机器人的运动路径，驱动移动机器人进行逐步探索，获得运动路径的环境信息建立环境三维模型，根据三维模型进行特征跟踪控制机器人自主导航。

同名特征点进行粗匹配具体包括：从提取的特征点中截取特征片元，从中选出稳定点集，稳定点集中的每个稳定点片元变换得到训练片元；随机生成随机蕨丛和蕨判断节点属性参数，将训练片元投入随机蕨丛，得到二值串划分，统计各个二值串划分的类型分布，输出一个完整的随机蕨丛结构和训练样本的分类统计；检测当前帧的自然特征并截取形成待匹配特征片元，将所有待匹配特征片元投入完整的随机蕨丛得到二值串划分，分类器集成各二值串划分的训练统计结果，作为同名特征点的粗匹配结果。

提取特征点进一步包括：产生稳定的内点集z^li_inliers，根据卡尔曼滤波增益矩阵K_k，预测误差均方矩阵P_k|k-1，预测模型评估的雅可比矩阵H_k，调用公式：

$\begin{array}{c}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z^{\mathrm{li}\_\mathrm{inliers}}-h^{\′}\left({\hat{x}}_{k|k-1}\right))\\ K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array},$ 获得稳定的内点集z^li_inliers的特征向量

对其余的不稳定内点z^hi_inliers进行补救，根据设定的补救条件，重新得到卡尔曼滤波增益矩阵K'_k和预测模型评估的雅可比矩阵H'_k，并调用公式：

$\begin{array}{c}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+{K^{\′}}_k(z^{\mathrm{hi}\_\mathrm{inliers}}-h^{\′}\left({\hat{x}}_{k|k-1}\right))\\ {K^{\′}}_k=P_{k|k-1}{H}_k^{\′T}{({H^{\′}}_k{P^{\′}}_{k|k-1}{H}_k^{\′T}+R_k)}^{-1}\\ P_{k|k}=(I-{K^{\′}}_k{H^{\′}}_k)P_{k|k-1}\end{array},$ 获得不稳定内点z^hi_inliers的特征向量

以任一像素点X为圆心，作一个半径为3个像素点的圆，圆周上所有像素点按顺时针依次编号，根据公式将每个像素点分别与中心像素X求灰度差,将灰度差值作为特征判断条件，如果圆周上像素至少有连续n个像素灰度差在预定阈值范围内，则中心像素X被视为图像的匹配特征点FAST-n。

通常远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在RANSAC假设中，一个远距离点可以产生一个对角度变换较敏感的点，而对尺度变换不是很精确。在这种情况下，远距离点支持拟合模型，但是由于尺度变化的精度不高，附近的点即使是内点也可能表现出较高的不确定性。所以在确定了拟合模型之后，需要从不稳定的点集中“补救”一些内点，以提高系统的稳定性。在采用可靠内点进行局部状态和协方差更新后，一些不稳定的内点会被挽救。

使用改进的FAST角点提取算法相对于经典FAST算法，既可以保持环境模型的各向同性，又具有便于计算机高效处理的超高速度。采用FAST-9角点检测具有最高的可再现性，依次检测圆周像素P₁,P₅,P₉,P₁₃，当其中连续三个点经过灰度差值判断，满足灰度差值条件，就进一步检测这三个点所覆盖圆弧的圆周像素点，若该圆周像素点也满足灰度差值条件，就可以判定该点为特征点，否则该点就不为特征点。也可采用本领域技术人员熟知的方法其他方法实现，以下以FAST-9角点检测为例对其实现方法进一步具体说明。以像素点X为圆心，作一个半径为3个像素点的圆，对圆周上所有像素点按顺时针依次编号，将每个像素P_i,i∈{1,...,16}分别与中心像素X求灰度差，灰度差值作为特征判断条件： $\mathrm{\Δ}{I}_{P_i\→X}=I_{P_i}-I_X,I_{P_i}=I_{P_{i\mathrm{mod}16}\·}.$

圆周像素P_i经灰度差值判断后的状态为为像素点X的状态。如果圆周上1-16号像素中至少有连续n个像素点灰度差都大于阈值t或都小于阈值-t，则中心像素点X被视为匹配特征点FAST-n。

如公式所示：

$S_{P_i\&RightArrow;X}=\left\{\begin{array}{cc}-1,I_{P_i}<I_X-t& \left(\mathrm{darker}\right)\\ 0,I_X-t<I_{P_i}<I_X+t& \left(\mathrm{simillar}\right)\\ 1,I_X+t\&le;I_{P_i}& \left(\mathrm{brighter}\right)\end{array}\right.---\left(3\right)$ $S\left(X\right)=\left\{\begin{array}{cc}\mathrm{ture},\mathrm{if}& \&Exists;\underset{i=k}{\overset{k+n}{\mathrm{\&Sigma;}}}{S}_{P_i\&RightArrow;X}=\&PlusMinus;n\\ \mathrm{false},& \mathrm{else}\end{array}\right..$

使用以上算法遍历检测所有像素，可得到当前帧的FAST角点。通过改变阈值t可以控制图像帧的FAST角点个数。

本发明采用的改进FAST(加速段检测特征点)算法，可以剔除经典FAST算法在提取角点时存在的大部分边缘点和局部非极大值点；同时采用一点RANSAC(随机抽样一致性)算法来充分利用EKF(扩展卡尔曼滤波)预测阶段以得到先验信息，在确保计算精度的同时有效降低了算法迭代次数。本发明提供的移动机器人导航方法能有效地提高特征匹配的正确率，将匹配过程中出现的错误匹配剔除，并能有效提高算法实时性。

附图说明

图1为移动机器人导航方法流程图。

具体实施方式

图1所示为基于一点RANSAC和FAST算法的移动机器人导航方法流程图，其具体内容如下：

(1)启动移动机器人控制系统，进行摄像头参数标定。

(2)移动机器人在伺服驱动器和伺服电机驱动下在环境中探索行进，同时摄像头不停地采集环境图像信息。

(3)从采集的视频帧中提取特征点。如采用如下方法提取：

对相邻两个视频帧的同名特征点进行粗匹配。提取关键帧的自然特征点截取特征片元，经仿射变换选出稳定点集，稳定点集中的每个稳定点片元经仿射变换得到训练片元；随机生成随机蕨丛及其判断节点属性参数；将训练片元投入随机蕨丛，得到二值串划分，统计各个二值串划分的类型分布；输出一个完整的随机蕨丛和训练样本的分类统计结果；检测当前帧的自然特征并截取形成待匹配特征片元，将每个特征片元投入随机蕨丛得到一系列二值串划分，分类器基于半朴素贝叶斯模型集成各二值串划分的训练统计结果，实现相邻帧的同名点粗匹配。具体为：

1)提取帧中的自然特征点，并截取特征片元，从中选出稳定点集，从稳定点集中的每个稳定点片元变换得到大量训练片元，可通过仿射变换得到大量训练片元，仿射变换可采用公式(4)所示获得。

$x^{\&prime;}=H_{A}x=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]x=\left[\begin{array}{ccc}{a}_{11}& a_{12}& t_x\\ a_{21}& a_{22}& t_y\\ 0& 0& 1\end{array}\right]x---\left(4\right)$

其中，x和x'为相邻两帧中的同名点，两者的关系为仿射变换。

2)随机生成一定规模的随机蕨丛和蕨判断节点属性参数；将训练片元投入随机蕨丛，得到二值串形式的划分，统计各个划分的类型分布；输出一个完整的随机蕨丛结构和训练样本的分类统计结果。

3)通过快速检测当前帧的自然特征并截取形成待匹配片元，将每个特征片元投入随机蕨丛得到二值串划分，分类器将基于半朴素贝叶斯模型集成各划分的训练统计结果，实现同名点粗匹配。

(4)同名点粗匹配以后，再基于一点RANSAC剔除误匹配算法，得到精确匹配的同名点对。

(5)根据图像中匹配的特征点进行环境模型的拟合。

(6)根据已知环境信息，规划移动机器人的运动路径，驱动移动机器人对未知的环境进行逐步探索。

(7)移动机器人对环境完成全部探索，获得充分的环境信息，并以此建立环境三维模型。

(8)在环境三维模型的基础上，通过KLT(Kanade-Lucas-Tomasi)算法进行特征跟踪，控制移动机器人按照预定要求进行自主导航。

一点RANSAC算法得到精确匹配的同名点对。

产生稳定的内点集z^li_inliers，并用z^li_inliers进行EKF(扩展卡尔曼滤波)部分更新。

对移动机器人摄像头视频相邻的帧进行特征匹配，所得的数据点集为z_i，匹配性最好的数据点集为z^li_inliers(low-innovation inliers)，其余的点为z^hi_inliers(high-innovation inliers)，用匹配性最好的数据点集拟合一个数据模型。通常，远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在RANSAC中，一个远距离点可以产生一个对角度变换较敏感的点，而对尺度变换不是很精确，在这种情况下，其他远距离点也会支持这个拟合模型。但是由于尺度变化的精度不高，附近的点有可能表现出较高的不确定性，即使他们是内点。所以，在确定了拟合的模型之后，还要从不稳定的点集中“补救”一些内点。在仅采用可靠内点进行局部状态和协方差更新后，这些不稳定的内点会被挽救。本发明可采用z^li_inliers进行EKF部分更新。建立测量方程的雅可比行列式H_k＝(H₁…H_i…H_n')^T，摄像头噪声的协方差R_k，调用公式(1)进行EKF(扩展卡尔曼滤波)部分更新。

$\begin{array}{c}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z^{\mathrm{li}\_\mathrm{inliers}}-h^{\&prime;}\left({\hat{x}}_{k|k-1}\right))\\ K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}---\left(1\right)$

其中，是特征向量，K_k是卡尔曼滤波增益矩阵，P_k|k-1是预测误差均方矩阵，H_k预测模型评估的雅可比矩阵。

对其余的点不稳定内点z^hi_inliers进行补救。

调用公式(2)对不稳定点进行EKF(扩展卡尔曼滤波)部分更新。

$\begin{array}{c}{\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+{K^{\&prime;}}_k(z^{\mathrm{hi}\_\mathrm{inliers}}-h^{\&prime;}\left({\hat{x}}_{k|k-1}\right))\\ {K^{\&prime;}}_k=P_{k|k-1}{H}_k^{\&prime;T}{({H^{\&prime;}}_k{P^{\&prime;}}_{k|k-1}{H}_k^{\&prime;T}+R_k)}^{-1}\\ P_{k|k}=(I-{K^{\&prime;}}_k{H^{\&prime;}}_k)P_{k|k-1}\end{array}---\left(2\right)$

其中，是特征向量，K'_k是卡尔曼滤波增益矩阵，P_k|k-1是预测误差均方矩阵，H'_k预测模型评估的雅可比矩阵。

通常远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在RANSAC假设中，一个远距离点可以产生一个对角度变换较敏感的点，而对尺度变换不是很精确。在这种情况下，远距离点支持拟合模型，但是由于尺度变化的精度不高，附近的点即使是内点也可能表现出较高的不确定性。所以在确定了拟合模型之后，需要从不稳定的点集中“补救”一些内点，以提高系统的稳定性。在采用可靠内点进行局部状态和协方差更新后，一些不稳定的内点会被挽救。

改进的FAST算法

使用改进的FAST角点提取算法相对于经典FAST算法，既可以保持环境模型的各向同性，又具有便于计算机高效处理的超高速度。采用FAST-9角点检测具有最高的可再现性，依次检测圆周像素P₁,P₅,P₉,P₁₃，当其中连续三个点经过灰度差值判断，满足灰度差值条件，就进一步检测这三个点所覆盖圆弧的圆周像素点，若该圆周像素点也满足灰度差值条件，就可以判定该点为特征点，否则该点就不为特征点。也可采用本领域技术人员熟知的方法其他方法实现，以下以FAST-9角点检测为例对其实现方法进一步具体说明。以像素点X为圆心，做一个半径为3个像素点的圆，圆周上的16个像素点按顺时针依次编号，将每个像素P_i,i∈{1,...,16}分别与中心像素X求灰度差，灰度差值作为特征判断条件： $\mathrm{\&Delta;}{I}_{P_i\&RightArrow;X}=I_{P_i}-I_X,I_{P_i}=I_{P_{i\mathrm{mod}16}\&CenterDot;}.$

圆周像素P_i经灰度差值判断后的状态为为像素点X的状态。如果圆周上1-16号像素中至少有连续n个像素点灰度差都大于阈值t或都小于阈值-t，则中心像素点X被视为FAST-n特征点。如公式所示：

$S_{P_i\&RightArrow;X}=\left\{\begin{array}{cc}-1,I_{P_i}<I_X-t& \left(\mathrm{darker}\right)\\ 0,I_X-t<I_{P_i}<I_X+t& \left(\mathrm{simillar}\right)\\ 1,I_X+t\&le;I_{P_i}& \left(\mathrm{brighter}\right)\end{array}\right.---\left(3\right)$

$S\left(X\right)=\left\{\begin{array}{cc}\mathrm{ture},\mathrm{if}& \&Exists;\underset{i=k}{\overset{k+n}{\mathrm{\&Sigma;}}}{S}_{P_i\&RightArrow;X}=\&PlusMinus;n\\ \mathrm{false},& \mathrm{else}\end{array}\right..$

使用以上算法遍历检测所有像素，可得到当前帧的FAST角点。通过改变阈值t可以控制图像帧的FAST角点个数。

以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.基于一点RANSAC和FAST算法的移动机器人导航方法，其特征在于，机器人运动过程中摄像头采集图像信息，从采集的图像信息视频帧中提取特征点，根据特征点对相邻两个视频帧的同名特征点进行粗匹配，在粗匹配基础上对同名特征点进行精确匹配获得图像的匹配特征点，根据匹配特征点进行环境模型拟合，规划机器人的运动路径，驱动移动机器人进行逐步探索，获得运动路径的环境信息建立环境三维模型，根据三维模型进行特征跟踪控制机器人自主导航。

2.根据权利要求1所述的方法，其特征在于，同名特征点进行粗匹配具体包括：从提取的特征点中截取特征片元，从中选出稳定点集，稳定点集中的每个稳定点片元变换得到训练片元；随机生成随机蕨丛和蕨判断节点属性参数，将训练片元投入随机蕨丛，得到二值串划分，统计各个二值串划分的类型分布，输出一个完整的随机蕨丛结构和训练样本的分类统计；检测当前帧的自然特征并截取形成待匹配特征片元，将所有待匹配特征片元投入完整的随机蕨丛得到二值串划分，分类器集成各二值串划分的训练统计结果，作为同名特征点的粗匹配结果。

3.根据权利要求1所述的方法，其特征在于，提取特征点进一步包括：产生稳定的内点集z^li_inliers，根据卡尔曼滤波增益矩阵K_k，预测误差均方矩阵P_k|k-1，预测模型评估的雅可比矩阵H_k，调用公式：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z^{\mathrm{li}\_\mathrm{inliers}}-h^{\&prime;}\left({\hat{x}}_{k|k-1}\right))$

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\text{,}$ 获得稳定的内点集z^li_inliers的特征向量

P_k|k＝(I-K_kH_k)P_k|k-1

其中，是测量方程H_k＝(H₁…H_i…H_n')^T的雅可比行列式，R_k是摄像头噪声的协方差矩阵，是对当前状态的估计，是根据上个状态对当前状态的估计。

4.根据权利要求1所述的方法，其特征在于，以任一像素点X为圆心，作一个半径为3个像素点的圆，圆周上所有的像素点按顺时针依次编号，根据公式将每个像素点分别与中心像素点X求灰度差,将灰度差值作为特征判断条件，如果圆周上像素点至少有连续n个像素点灰度差在预定阈值范围内，则中心像素X被视为图像的匹配特征点FAST-n。

5.根据权利要求2所述的方法，其特征在于，根据公式： $x^{\&prime;}=H_{A}x=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]x=\left[\begin{array}{ccc}{a}_{11}& a_{12}& t_x\\ a_{21}& a_{22}& t_y\\ 0& 0& 1\end{array}\right]x,$ 通过仿射变换得到大量训练片元，其中，x和x'为相邻两帧中的同名点，H_A是仿射变换矩阵，该矩阵中的6个参数为3对同名控制点坐标，根据实际应用场景可以确定3对同名控制点的坐标。

6.根据权利要求3所述的方法，其特征在于，提取特征点进一步包括：对其余的不稳定内点z^hi_inliers进行补救，根据设定的补救条件，重新得到卡尔曼滤波增益矩阵K'_k和预测模型评估的雅可比矩阵H'_k，并调用公式：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+{K^{\&prime;}}_k(z^{\mathrm{hi}\_\mathrm{inliers}}-h^{\&prime;}\left({\hat{x}}_{k|k-1}\right))$

${K^{\&prime;}}_k=P_{k|l-1}{H}_k^{\&prime;T}{({H^{\&prime;}}_k{P^{\&prime;}}_{k|k-1}{H}_k^{\&prime;T}+R_k)}^{-1},$ 获得不稳定内点z^hi_inliers的特征向量

P_k|k＝(I-K'_kH'_k)P_k|k-1。