基于改进FAST算法的移动机器人导航

蒲兴成，谭少峰，张毅

(1.重庆邮电大学 数理学院，重庆 400065； 2. 重庆邮电大学 自动化学院，重庆 400065)

移动机器人在完全未知的环境中利用自身传感器获得周围环境信息进行实时定位和地图创建(simultaneous localization and mapping, SLAM)是移动机器人研究领域的一个热点问题[1]。所谓SLAM，是指机器人在一个未知的环境中，从一个未知的位置开始，通过对环境的观测，递增地构建环境地图，并同时运用环境地图实现机器人定位的一个过程[2]。

近年来，基于单目视觉的SLAM问题获得了非常广泛的研究。现有的研究表明，在简单的结构化环境以及地图信息已知(具有人工路标)的环境中进行视觉导航已经有了很大进展，但在完全未知的环境中进行定位与导航还存在诸多问题。造成这一现象的根本原因在于如何在未知环境中获取自然特征。所谓自然特征，是指环境中已有的、非人工设置的、能够用以标识不同环境场景的特征对象[3]。通过提取一些具备局部不变性的角点作为环境的自然特征，该方法选取的自然路标有非常好的稳定性，因此角点检测也是机器人导航的热点问题之一[4]，也出现了一些较好的角点检测方法。FAST(features from accelerated segment test)是一种运算简单直观的角点检测方法，相关测试表明，FAST在计算速度上优于SIFT、SURF和Harris等角点检测方法[5]。为此，笔者一方面采用改进的FAST算法[6]来提取角点，剔除了大部分边缘点和局部非极大值点；另一方面，针对误匹配剔除，采用结合扩展卡尔曼滤波(extended Kalman filter, EKF)的一点RANSAC算法[7]。RANSAC算法是一种应用广泛的误匹配剔除算法，但该算法随着匹配点个数增加，算法迭代次数会迅速上升，降低了算法的计算速度。采用一点RANSAC算法可以充分利用EKF预测阶段得到的先验信息，在确保计算精度的同时，有效降低了算法迭代次数。在整个实验过程中，机器人通过自身摄像头，采用改进的FAST角点提取算法采集周围环境信息，并结合EKF的一点RANSAC算法[7]对误匹配点进一步剔除，获得鲁棒性更强的特征点，从而构建环境的地图信息，最后用得到的匹配点进行三维环境重建和机器人定位。

1 FAST角点提取算法

FAST角点检测算子是Rosten和Drummond在SUSAN角点检测方法基础上利用机器学习原理提出的，其运算速度不仅大大高于SIFT、SURF算子，更优于以速度见长的Harris算子。FAST角点检测的原理是检测待检测点与周围点的灰度是否相近，如图1(c)所示，即对待检测特征点p，最多只需检测16个候选特征点，大大减轻了计算量。图中每个方格均代表一个像素，位于中心的p点为待检测特征点，周围16个方格为检测过程中需检测的像素点。角点的判断过程为：设Ip为待检测点p的灰度值，Ip→x为圆周上像素点的灰度值，x∈{1,2,…,16}，对半径为3的圆上的像素进行分割测试， 判定条件如式(1)：

|Ip→x-Ip|≥t

(1)

如果满足判定条件的像素点数满足设定数量n，则判定p点为角点，其中，t为设定的阈值，n一般为12或9，分别对应FAST-12与FAST-9检测算法。

在实际操作中，为了获得更快的结果，还可以采用一种加速办法。该方法首先测试候选点周围每隔90°角的4个点，即图1(c)中位于水平与垂直位置的1、5、9和13四点，如果至少有3个和候选点的灰度值差足够大，则继续计算其他12个点；否则，不用再计算其他点，直接认为该候选点不是特征点。

图1 FAST角点提取流程Fig.1 Process of FAST corner extraction

经过上述步骤后，会初步提取到一些角点，但这些点中存在较多边缘点和局部非极大值点，采用文献中的方法[6]，可以剔除边缘点，如式(2)所示：

(2)

式中：q为圆周上任意一点，Iq+1与Iq-1为关于q对称的2个点，从圆周任一点开始计算，若有大于10个点满足式(2)，则认为该点属于边缘点，予以剔除。

在剔除边缘点后，还要剔除局部非极大值点。通过计算角点候选点周围很小邻域内各像素点的拉普拉斯值，进一步剔除一些局部非极大值点。此处局部极大值采用拉普拉斯极值计算公式为

(3)

在Intel Pentium处理器、2 G内存的硬件配置下，从视频中任取一帧640像素×480像素的图像利用FAST角点检测算子进行检测，实验证明，同一帧图像在FAST算法改进前后的提取结果有较大改变，如图1所示，改进前图1(a)一帧图像中的特征点个数为4 704，改进后图1(d)特征点个数为1 526，这是因为改进后的算法滤掉了部分伪角点，留下了角点特征更加明显的特征点。而且提取时间上也有了提高，改进前的FAST角点提取时间为33 ms，改进后为16 ms。

2 相关归一化的特征匹配算法

通过FAST角点检测算子可以对视频图像中的每一帧图像进行角点检测，为了建立环境中三维特征点地图，必须对相邻2帧图像进行特征匹配。笔者采用相关归一化匹配算法，其相关归一化函数为

ρ(f0,f1)=

(4)

3 基于扩展卡尔曼滤波的一点RANSAC算法

用相关归一化算法对相邻2帧图像进行特征匹配之后，会产生很多误匹配点，为了提高匹配精度，要对匹配点进行过滤。标准RANSAC算法是一种应用广泛的误匹配剔除算法，但该算法随着匹配点个数增加，算法迭代次数会迅速上升，降低了算法的计算速度。笔者结合扩展卡尔曼滤波的一点RANSAC算法，在确保计算精度的同时，有效降低了算法迭代次数，使机器人定位与导航的时效性得到了保证[7]。

3.1 扩展卡尔曼滤波

卡尔曼滤波是一种高效率的递归滤波器，它能从一组有限的、包含噪声的、对物体位置的观察序列预测出物体位置的坐标和速度，并能够实现对系统状态的最优估计，使系统的真实值与观测值之间的均方差最小[8]。它是对一个线性随机系统的状态进行估计，但在实际应用中，大部分系统都是非线性的，处理这些系统时，需采用EKF。其算法原理如下。

1)预测：

(5)

式中：uk表示第k步系统的输入，Fk表示在第k步fk关于状态向量xk|k-1的雅可比矩阵，Qk表示动态模型的零均值高斯噪声的协方差，Gk表示该噪声关于状态向量xk|k-1的雅可比矩阵。

2)滤波：数据更新是用传统的扩展卡尔曼滤波方程执行的。

Pk|k=(I-KkHk)Pk|k-1

(6)

3.2 RANSAC算法

RANSAC算法是通过一组包含异常数据的样本数据点集计算出数据的数学模型，最终得到有效样本数据的算法[9]。一般情况下，需要考虑的参数有构建模型所需的最少数据个数、算法迭代次数k、模型验证条件t、判断模型是否合理的数据点个数d。算法流程为：首先，用随机选择的数据拟合一个假设模型，其次，用得到的模型去测试所有其他数据，如果有足够多的点被归类为该假设模型的内点，则认为该模型是合理的，并用得到的内点重新估计模型；上述过程被重复执行一定的次数，产生的模型要么越来越接近真实模型，要么因为内点数目过少而被舍弃。

用w来表示每次迭代时从数据集中选取一个点并且该点是正确数据点的概率，那么，每次迭代选取的m个数据均为正确数据的概率为wm，则1-wm表示m个数据中至少有一点是异常数据的概率，而包含这些异常点则可能产生不合理的模型。因此，n次迭代过程中，每次选取的m个数据均包含至少一个错误数据的概率必然等于1-p,也就是

1-p=(1-wm)n

通常情况下，p事先给定，如果w也给定，n会随着m的增加不断增加，那么算法的计算复杂度就会相应地增加。m增大，RANSAC算法的迭代次数n将会迅速地上升。对于这里采用的一点RANSAC，摄像机运动的额外信息来自EKF概率分布函数。举一个简单的例子，如果w=0.5，概率p是0.99，n=1，则随机假设的数据将会从145(m=5，没有先验信息)降到7(m=1,一点RANSAC采用先验信息)。

3.3 一点RANSAC算法

一点RANSAC与普通RANSAC相比，前者采用先验信息，降低了数据集的大小，减少了RANSAC迭代次数，从而降低了计算成本。一点RANSAC充分利用了EKF预测阶段得到的地图状态预测值的先验信息，选出匹配效果最好，满足全局模型的地图特征，从性能和计算复杂度方面对传统的RANSAC算法做了改进，提高了SLAM系统的鲁棒性，其主要步骤如下：

1)产生稳定的内点集zli_inliers，并用zli_inliers进行EKF部分更新。

假设经过匹配所得的数据点集为zi，用zli_inliers(low-innovation inliers)来表示匹配性最好的数据点集，用这些点集来拟合一个数据模型。其余的点用zhi_inliers(high-innovation inliers)来表示。

在这举一个简单的例子来说明“补救”不稳定点的重要性。因为，远距离点可用来估计相机角度变换，而近距离点用来估计相机尺度变换。在RANSAC假设中，一个远距离点可以产生一个对角度变换较敏感的点，而对尺度变换不是很精确。在这种情况下，其他远距离点也会支持这个拟合模型。但是由于尺度变化的精度不高，附近的点有可能表现出较高的不确定性，即使他们是内点。所以，在确定了拟合的模型之后，还是要从不稳定的点集中“补救”一些内点。在仅采用可靠内点进行局部状态和协方差更新后，这些不稳定的内点会被挽救。下面公式是用zli_inliers进行EKF部分更新的过程。

Pk|k=(I-KkHk)Pk|k-1

2)对不稳定内点进行补救，并用zhi_inliers进行EKF部分更新。

用zli_inliers进行EKF部分更新后，在EKF预测阶段的大部分相关误差得到了纠正，协方差也大大降低，这些都可以用来补救不稳定的内点，这是由于数据关联性有所减弱，没有必要计算点集的一致性。对“补救点”的部分EKF更新过程为

4 定位与地图构建

4.1 地图三维重建

机器人基于FAST角点重建运行环境的三维地图，根据摄像机内外参模型与标定理论[10]，设焦距参数为fi，内参矩阵表示如下：

式中：(cx,cy)是视频序列中第i帧图像的中心。每个特征点代表了一个三维点Xj=[X1X2X31]，在每帧图像上的投影表达式为：uij=PiXj=Ki(Ri|ti)Xj，这里uij代表了Xj在第i帧图像中的齐次位置，Pi为射影矩阵。根据文献中的方法[11]，即可重建出欧式空间下的所有特征点的三维坐标。

4.2 定位与导航实现

移动机器人要实现定位，需要计算当前视图中特征点与三维地图中特征点的几何关系。用u、v来表示像素点在图像中的像素坐标，dx、dy表示每个像素点在世界坐标系上的物理尺寸，图像像素坐标与世界坐标系之间的变换关系如下：

(9)

由式9可知，只需知道自然特征点坐标、当前视图中同名点的坐标和对应的摄像机内参，便可求解当前视图的摄像机外参，也即机器人的位置。又因同名关系需要匹配才可以确定，所以需要寻找最近邻自然特征才能保证定位精度。

在建立了环境信息的三维地图信息之后，移动机器人就可通过特征点匹配得到自身在环境中的位置信息设定路径实现机器人的导航。

5 实验结果与分析

机器人在室内实验过程中，一边控制机器人移动一边利用摄像头采集视频，把视频中的每一帧图像采集出来，共3 788帧(30帧/s,共2 min 15 s)。从所有帧中选取一些关键帧，可以作为匹配模板。在Intel Pentium处理器、2 GB内存的硬件配置下，计算机对每个关键帧进行角点提取、误匹配剔除并完成地图更新的处理时间为300 ms，把机器人移动速度v设定为10 cm/s、转弯角速度w设定为0.52 rad/s(30°/s)可保证较高的鲁棒性。

图2(a)为图像的角点匹配情况，其中粗线圆圈表示匹配性最好的点，细线圆圈表示匹配失败的点；图2(b)所示为重建的SLAM地图，曲线表示机器人运动轨迹。从图2中可清晰地看出机器人在行进过程中的90°转角；图3所示为画面中存在大范围移动目标时，会对特征点匹配造成一定影响，从而对地图构建造成一定误差。表1为不同时刻前后各50帧轨迹与里程计轨迹的误差对比。

图2 第1 472帧图像的角点匹配与地图构建Fig.2 The corner matching and map building of 1 472th frame

图3 第2 093帧图像的角点匹配与地图构建Fig.3 The corner matching and map building of 2 093th frame

图4 机器人室内轨迹图Fig.4 The indoor path of robot

帧数范围平均误差/cm最大误差/cm平均误差/%475-5741.13.50.51 422-1 5211.75.10.92 043-2 1428.216.44.13 689-3 7882.05.71.0

由表1可见，机器人在直线运动中精度较高，平均误差在1%以内。在2 043～2 142帧之间，摄像头画面中存在大范围的移动目标，对特征点匹配造成了一定的影响，但误差也只有4%左右，证明系统的鲁棒性较高。该方法同样适合室外环境的定位与导问航，在室外采集了1 min 24 s的视频，共2 529帧图像。下图5分别为第1 700帧和第2 500帧时的角点匹配与地图构建。

(a) 机器人运动过程中特征匹配情况

(b) 机器人运行轨迹

(c) 机器人运动过程中特征匹配情况

(d) 机器人运行轨迹图5 机器人室外运动的角点匹配与地图构建Fig.5 The outdoor corner matching and map building of robot

图6为SLAM重建图与机器人里程计轨迹的对比，两者的平均误差为11.3 cm，平均误差百分比为0.7%。

图6 机器人室外轨迹图Fig.6 The outdoor path of robot

6 结束语

针对移动机器人在单目视觉导航方面实时性与鲁棒性较差的问题，提出一种基于改进的FAST角点提取算法与一点RANSAC剔除误匹配算法相结合来实现移动机器定位与导航的新方法。在角点提取方面，剔除了部分边缘点和局部非极大值点，提高了角点提取的速度与质量；在角点匹配与误匹配剔除方面，充分利用了滤波阶段得到的先验信息，在保证算法鲁棒性的同时有效降低了算法迭代次数，提高了算法的实时性。实验结果表明，该方法能够实时高效地重建出机器人的运行轨迹，实现机器人的定位与导航；在周围环境发生变化时，能够保持较高的匹配精度，鲁棒性较高；该方法同样可以适用于室外环境。后续工作将在如何进一步提高其实时性做深入研究。

参考文献:

[1]王耀南, 余洪山. 未知环境下移动机器人同步地图创建与定位研究进展[J].控制理论与应用, 2008(1): 57-65.

WANG Yaonan, YU Hongshan. Mobile robots simultaneous localization and mapping under unknown environments[J]. Control Theory and Applications, 2008(1): 57-65..

[2]陈卫东, 张飞. 移动机器人的同步自定位与地图创建研究[J]. 控制理论与应用, 2005(3): 455-460.

CHEN Weidong, ZHANG Fei. Study on mobile robots simultaneous localization and mapping[J]. Control Theory and Applications, 2005(3): 455-460.

[3]ALCANTARILLA P F, SANG M O, MARIOTTINI G L, et al. Learning visibility of landmarks for vision-based localization[OL/EB]. [2013-10-21]. https://smartech.gatech.edu/jspui/bitstream/1853/38323/1/Alcantarilla10icra2.pdf.

[4]梁艳菊, 李庆, 陈大鹏, 等. 一种快速鲁棒的LOG-FAST角点算法[J]. 计算机科学， 2012(6): 251-254.

LIANG Yanju, LI Qing, CHEN Dapeng, et al. A fast robust LOG-FAST algorithm[J]. Computer Science, 2012(6): 251-254.

[5]ROSTEN E, DRUMMOND T. Machine learning for high-speed corner detection[C]//Computer Vision-ECCV 2006. [S.l.]， 2006: 430-443.

[6]燕鹏, 安如. 基于 FAST 改进的快速角点探测算法[J]． 红外与激光工程， 2009( 6): 1104-1108.

YAN Peng, AN Ru. An improved FAST corner detector algorithm[J]. Infrared and Laser Engineering, 2009( 6): 1104-1108.

[7]STRASDA H, MONTIEL J M M, DAVISON A J. Real-time monocular SLAM: why filter[C]//Proceedings of the IEEE International Conference on Robotics and Automation. [S.l.]， 2010: 2657-2664.

[8]CIVERA J. Real-time EKF-based structure from motion[D]. System Engineering and Computer Science of University Press, 2003： 21-23.

[9]CIVERA J, GRASA O G, DAVISON A J, et al. 1-point RANSAC for EKF-based structure from motion[C]//IEEE Intelligent Robots and Systems, [S.l.]， 2009： 3498-3504.

[10]ZHANG Z, DERICHE R, FAUGERAS O, et al. A robust technique for matching two uncalibrated images through the recovery of the unknown epipolar geometry[J]. Artificial Intelligence, 1995, 78(1/2): 87-119.

[11]POLLEFEYS M, Van GOOL L, VERGAUWEN M, et al. Visual modeling with a hand-held camera[J]. International Journal of Computer Vision, 2004, 59(3): 207-232.