基于 SIFT 算法的移动机器人同时定位与地图创建

第 21 卷 第 1 期 2008 年 3 月

宁 波 大 学 学 报( 理 工 版 ) JOURNAL OF NINGBO UNIVERSITY ( NSEE )

Vol.21 No.1 Mar. 2008

文章编号:1001-5132(2008)01-0068-04

基于 SIFT 算法的移动机器人同时定位与地图创建
王彭林,石守东,洪小伟
(宁波大学 信息科学与工程学院,浙江 宁波 315211)

摘要: 研究了基于尺度不变特征变换(SIFT)算法的移动机器人同时定位与地图创建(SLAM)方法, 即在视角改变情况下,用 SIFT 算法对不同图像进行特征匹配,根据极线几何原理得到摄像头的 旋转角度,将之与里程计的角度信息融合,从而实现较准确的自我定位与地图创建. 实验表明: 本方法利用电荷耦合器件(CCD)摄像头和里程计间内在的几何关系来实现 SLAM, 提高了低成本 移动机器人的定位精度. 关键词:SIFT 算法;同时定位与地图创建;极线几何;融合 中图分类号:TP24 文献标识码:A

移动机器人的定位与地图创建是机器人领域 的热点研究问题,是实现机器人导航的前提. 目 前, 对已知环境地图的机器人自主定位和已知机器 人姿态的地图创建, 已经有了许多不同的解决方法. 根据先验地图, 在结构化室内环境中进行的移动机 器人定位与地图创建的研究已经取得了很大的成 功,并且已有很多的应用实例. 然而在很多情况 下,事先较难获得机器人的工作环境地图,而且在 很多环境中机器人不能利用全局定位系统进行定 位. 所以,机器人如需在完全未知的环境中创建地 图,需同时利用地图进行自主定位和导航,这方面 的研究已经取得了很大的进展,并应用于室内[1]、 水下[2]和室外[3]等各种不同的环境. 移动机器人大多采用机载传感器, 如激光测距 仪、声纳及摄像头等. 近年来,随着图像处理技术 的进步(如SIFT算法[4,5]), 以及CCD摄像头具有代价 低、重量小、能耗少等优点,基于视觉的SLAM越

来越受到国内外学者的重视. 目前,对于基于视觉 的移动机器人同时定位与地图创建而言, 一般可分 为单目视觉[6,7]和双目视觉[8]. 本文提出了一种基于单目视觉的移动机器人 SLAM 方法,将 CCD 摄像头和里程计组合,来实 现 SLAM. 为提高定位的精确性和避免误定位的发 生,在基于里程计定位的基础上,将不同视角的视 觉图像中提取的特征进行匹配, 根据极线几何计算 摄像头旋转的角度, 得到摄像头与里程计的角度冗 余信息,采用扩展卡尔曼滤波(EKF)对信息进行融 合,从而提高 SLAM 的鲁棒性.

1

SLAM 的数学模型

1.1 移动机器人模型

yθ 在二维环境中, 移动机器人的位姿使用 x,, 表示,其中 x, 表示移动机器人相对世界坐标位 y

收稿日期:2007-01-12. 宁波大学学报(理工版)网址:http://3xb.nbu.edu.cn 基金项目:浙江省教育厅科研基金(2005473). 第一作者:王彭林(1982-) ,女,浙江瑞安人,在读硕士研究生,主要研究方向:移动机器人导航. E-mail: wangpenglin_09@163.com

第1期

王彭林,等:基于 SIFT 算法的移动机器人同时定位与地图创建

69

置, θ 表示机器人的朝向. 在位姿跟踪问题中,移 动机器人的初始位姿是已知的 . 地图的创建过程 是指移动机器人在自身位姿已知情况下的环境地 机器人利用自身携带的 图创建过程. 在 SLAM 中, 传感器识别未知环境中的特征标志, 然后根据机器 人与特征标志之间的相对位置和里程计的读数估 计机器人和特征标志的世界坐标 . 这种在线的定 位与地图创建需要保持机器人与特征标志之间的 详细信息. 在室内特定环境下,基于环境特征SLAM方法 的基本思想是将移动机器人的位姿和环境特征坐 在机器人的走行过程中 标表达在 1 个状态向量中, 通过对环境特征的观测做最准确的估计 . 假设移 动机器人在世界坐标系(用 x, 表示)中为 1 个点, y 机器人的起始位置为世界坐标系原点, 前进的方向
(机器人的朝向 θ )为机器人坐标系中的横坐标,即
x' 轴, 逆时针旋转 90o为纵坐标 y' 轴, 如图 1 所示.

i yi 系中的位置 . 机器人获得的目标位置 ( xRk, Rk ) 是

指第 i 个目标在机器人坐标系中的位置,因此还需 将其转换成世界坐标,可表示为:
i i ? xGk ? ? xRk ? ? xk ? ? i ? = R ? i ? + ? ?, ? yGk ? ? yRk ? ? yk ? ? ? ? ? ? cos ? k ? sin ? k ? R=? ?. ? sin ? k cos ? k ?

(3) (4)

1.2 扩展卡尔曼滤波(EKF)

卡尔曼滤波器假设系统是线性系统, 但实际的 机器人运动模型与观测模型是非线性的. 因此,通 常采用扩展卡尔曼滤波器, 基于 EKF 的建图与定位
EKF 可以归纳为 1 个循环迭代的估计-校正过程,

算法在处理不确定信息方面有独到之处, 因此 EKF 成为应用最广泛的 SLAM 方法. 根据以上的公式和假设, 可以设计卡尔曼滤波 器公式为: X k = F ( X k ?1 ) + vk ?1,
Yk = H ( X k ) + wk,

(5)

其中, vk 和 wk 为互不相关且均值为零的正态白噪 声序列,方差分别为 Qk 和 Rk . 而 k 时刻的状态向 量和量测向量可表示为如下形式: ? xk ? ?y ? ? k ? ? x1 ? Rk ? θk ? ? 1 ? yRk ? ? 1 ? ? ? xGk ? ? ? 1 ? yGk ? ? i ? ? ? x X k = ? ? , Yk = ? Rk ? . ? yi ? ? xi ? ? Rk ? ? Gk ? ? ? i ? yGk ? ? xn ? ? ? ? Rk ? n ? ? ? yRk ? n ? ? ? xGk ? ? n ? ? yGk ?

图1

机器人移动过程

因此,世界坐标系中的方程为: xk = xk ?1 + Δlk cos(θ k ?1 + ? k ),
yk = yk ?1 + Δlk sin(θ k ?1 + ? k ),

(6)

(1)

θ k = θ k ?1 + a tan(

x

2 k ?1

Δlk sin ? k ), 2 + yk ?1 + Δlk cos ? k

其中, xk 和 yk 表示 k 时刻机器人在世界坐标系中 的位置; θ k 表示 k 时刻机器人的朝向; Δlk 是 k 时

? 刻移动机器人的位移; k 表示 k 时刻机器人的偏航
角. 由于目标是静态物体, 所以在世界坐标系中的 位置可以表示为:
i i i xGk = xGk ?1, Gk = yGk ?1. yi i Gk i Gk

量测值 xk 是从状态预测向量中计算得到的目 标位置, 它是指目标点在机器人坐标系中的具体位 置,可以表示为:
i ? xRk ? H i ( X k ) = ? i ? = RT ? yRk ? ? ? i ? xGk ? xk ? i ? i ? + wk, yGk ? yk ? ? ? ?

(2)

x 和 y (i = 1 ???, ) 是第 i 个目标在世界坐标 , n

i = 1 ? ? ?, . , n

(7)

70

宁波大学学报(理工版)

2008

所以,系统状态方程和量测方程为: X k = Fk ?1 X k ?1 + uk ?1 + vk ?1,
Yk = H k X k + wk,

技术的特征检测方法, 并正式提出了一种基于尺度
(8)

空间的、对图像缩放、旋转甚至仿射变换保持不便 性的图像局部特征描述算子—–SIFT 算子,即尺度 不变特征变换. SIFT 算法首先在尺度空间进行特

其中,

? ?H 1 ?H i ?F ?H n ? , k =? ??? ??? Fk = H ? ?X k ?X ?X ? ? ?X 1.3 估计与更新方程

' k

.

征检测,并确定关键点的位置和关键点所处的尺 度, 然后使用关键点领域梯度的主方向作为该点的 方向特征,以实现算子对尺度和方向的无关性. 梯 度和方向的计算方程为: m( x, ) = (( L( x + 1, y ) ? L( x ? 1, y )) 2 + y
( L( x, y + 1) ? L( x, y ? 1))2 )1/ 2, (15) θ ( x, ) = a tan 2(( L( x, + 1) ? L( x, ? 1)) / y y y ( L( x + 1 y ) ? L( x ? 1 y )), , ,

采用 EKF 实现系统状态的估计与更新,当机 器人在运动过程中发现新的特征 n + 1 时,需要根 据新特征的观测矢量 z (k + 1) 和机器人的当前状态 计算新特征标志的初始状态, 并更新状态矢量和协 方差矩阵 P . 其中, ? prr ?p P = ? 1r ? ? ? pnr
pr1 p11 pn1 prn ? p1n ? ?. ? ? pnn ?

y (15)式为 ( x, ) 处梯度的模值和方向公式,其 中 L 所用的尺度为每个关键点各自所在的尺度 . 在实际计算时, 可在以关键点为中心的邻域窗口内 采样,并用直方图统计邻域像素的梯度方向. 直方 图的峰值代表了该关键点处邻域梯度的主方向, 即 作为该关键点的方向.

在 SLAM 中,系统的状态包括机器人和环境 特征在机器人坐标系中的位置的估计, 而协方差矩 阵 P 表示估计的误差 . 扩展卡尔曼滤波方法对信 息的处理一般分为预测和更新, 并且此方法对信息 的估计是无偏估计. 卡尔曼滤波预测方程为:

2

实验结果分析和结论
在实验室内对移动机器人进行实验, 让机器人

X k? = Fk ?1 X k ?1 + uk , Yk = H k X k? , Pk? = Fk ?1 Pk ?1 FkT?1 + Qk ?1 .
更新方程为:
T T K k = Pk? H k ( H k Pk? H k + Rk )?1 ,

(9) (10) (11)

沿着边长为 3 m 的正方形行走, 机器人行走路线与 地图创建的结果如图 2 所示.

(12) (13) (14)

X k = X + K k (Yk ? Z k ), Pk = ( I ? K k H k ) P ,
P 是误差协方差, K 是增益矩阵.
? k

? k

Q 其中, 是状态噪声协方差, 是测量噪声协方差, R 预测方程用来预测当前状态, 误差协方差矩阵 为下一时刻获得先验估计 . 新方程将先验估计和 测量值结合,得到一个较精确的后验估计.
1.4 SIFT 算法 Lowe D G 在 2004 年总结了现有的基于不变量
图2 SLAM 实验结果

对于图 2 而言,地图的创建主要分 3 个步骤:
(1)用 SIFT 算法在 2 副不同视角的图像中寻找匹配

第1期

王彭林,等:基于 SIFT 算法的移动机器人同时定位与地图创建

71

点,根据极线几何原理,计算得到旋转角度;(2) 将这个角度与里程计的角度信息进行融合,得到 1 个更接近实际的角度值;(3)把它与里程计的位移 结合进行自我定位,根据 CCD 摄像头获得的特征 点的信息,以及结合上述 2 个步骤计算得到的结 果,来实现 SLAM. 由于里程计本身存在的缺陷, 比如机器人行走 过程中轮子打滑, 就会产生实际移动距离和机器人 内部里程表数据不一致. 因此,随着时间的推移, 里程计的累积误差逐渐增大, 从而导致自我定位与 实际相差较大. 本文将 CCD 摄像头和里程计的信息进行融 合,尽可能减少里程计产生的累积误差,在此基础 上用扩展卡尔曼滤波进行地图的创建及更新, 实现 了移动机器人的自主定位与地图创建的算法建立 与仿真.
参考文献:
[1] Kwon S J, Yang K W, Park Sangdeok, et al. Robust mobile robot localization with combined Kalman filterperturbation estimator[C]//Proceedings of the IEEE International Conference on Intelligent Robots and Systems, 2005:190-195.

[2] Leonard J J, Durrant W F. Simultaneous map building and localization for an autonomous mobile robot[C]// Proceeding of the IEEE International Workshop on Intelligent Robots and Systems. London: Springer Verlag, 1999:316-321. [3] Dissanayake Gamini, Newman Paul, Clark Steven, et al. A solution to the simultaneous localization and map building SLAM problem[J]. IEEE Transactions on Probotics and Automation, 2001, 17(3):229-241. [4] David G L. Distinctive image features from scaleinvariant keypoints[J]. International Journal of Computer Vision, 2004, 60(2):91-110. [5] David G L. Object recognition from local scale-invariant Features[EB/OL]. [2006-06-08]. http://www.cs.ubc.ca/lower/papers/iccv99.pdf. [6] Davison A J. Real-time simultaneous localization and mapping with a single camera[EB/OL]. [2006-08-12]. http://www.robots.ox.ac.uk/.ajd/. [7] Jeong Wooyeon, Lee Kyoungmu. CV-SLAM: a new ceiling vision-based SLAM technique[C]//2005 IEEE/ RSJ International Conference on Intelligent Robots and Systems, 2005:3 070-3 075. [8] Kim Gabhoe, Kim Jongsung, Hong Kisang. Vision-based simultaneous localization and mapping with two cameras [C]//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2005:3 041-3 046.

Mobile Robot Simultaneous Positioning and Mapping Based on SIFT Algorithm
WANG Peng-lin, SHI Shou-dong, HONG Xiao-wei
( Faculty of Information Science and Technology, Ningbo University, Ningbo 325211, China )

Abstract: The simultaneous localization and mapping (SLAM) method is investigated based on the Scale

Invariant Feature Transform (SIFT) algorithm. In the algorithm the features from different images is matched with the variant view points. By combining the angle data from odometer and the rotate angle derived from epipolar geometry, the SLAM is thus implemented with higher precision. The algorithm mainly takes into consideration the geometry correlation between Charge Coupled Device(CCD) camera and odometer, and as a result the superior SLAM positioning accuracy is achieved.
Key words: SIFT algorithm; simultaneous localization and mapping; epipolar geometry; fusion CLC number: TP24 Document code: A (责任编辑 章践立)


相关文档

基于SIFT算法的移动机器人同时定位与地图创建
移动机器人同时定位与地图创建算法研究
移动机器人的同时定位和地图创建方法
移动机器人的同时定位与地图创建的算法研究
移动机器人同时定位与地图创建自适应算法研究
仪的移动机器人同时定位和地图创建
基于局部子地图方法的多机器人主动同时定位与地图创建
基于全景视觉的移动机器人同步定位与地图创建研究
移动机器人同时定位与地图创建研究进展
电脑版