室内环境下结合里程计的双目视觉SLAM研究

西安理工大学学报JournalofXi.anUniversityofTechnology(2009)Vo.l25No.4

文章编号:1006-4710(2009)04-0466-06

室内环境下结合里程计的双目视觉SLAM研究

王晓华,傅卫平,苏立

1,2

1

1

(1.西安理工大学机械与精密仪器工程学院,陕西西安710048;

2.西安工程大学电信学院,陕西西安710048)

摘要:针对视觉SLAM要解决的定位精度低和鲁棒性低的问题,提出一种基于双目视觉传感器与里程计信息的扩展卡尔曼滤波SLAM方法,应用改进的SIFT算子提取双目视觉图像的环境特征获得特征点,并构建出视觉特征地图;应用扩展卡尔曼滤波算法融合视觉信息与机器人位姿信息,完

成同时定位与地图创建。这种方法既可以解决单目视觉利用特殊初始化方法获取特征点信息不准确的问题,也可以避免双目视觉里程计利用图像信息恢复运动带来的计算量极大和运动估计不鲁棒的缺点。仿真实验表明,在未知室内环境下,算法运行稳定,定位精度高。关键词:SLAM;双目视觉;里程计;SIFT;扩展卡尔曼滤波中图分类号:TP24 文献标识码:A

ResearchonBinocularVisionSLAMwithOdometerinIndoorEnvironment

WANGXiao-hua,FUWe-iping,SULi

1,2

1

1

(1.FacultyofMechanicalandPrecisionInstrumentEngineering,Xi.anUniversityofTechnology,Xi.an710048,China;

2.CollegeofElectronicandInformation,Xi.anPolytechnicUniversity,Xi.an710048,China)

Abstract:Withtheaimofsolvingthelowpositioningaccuracyandlowrobustnessproblemsofvision

SLAMalgorithm,ExtendedKalmanFilter(EKF)methodbasedonbinocularvisionandodometerispro-posedinthispaper.FeaturepointcanbeobtainedbyextractingimagefeatureswithimprovedSIFTalgo-rithm,andthevisionfeaturemapisconstituted.SLAMiscompletedbyusingtheinformationofbinocularvisionandrobotpositionwithEKF.Thismethodcaneithersolvethemonocularvisioninaccuracyprob-lemoffeaturepointinformationobtainedbyspecialinitializationmethodoravoidtheenormouscomputa-tionbroughtaboutbybinocularvisionodometerusingimageinformationtorestoremovementaswellasthein-robustdisadvantagesofmotionestimation.Theresultsfromsimulationexperimentsindicatethatintheunknownindoorenvironments,thisalgorithmoperationisstable,andthepositioningaccuracyishigh.

Keywords:SLAM;binocularvision;odometer;SIFT;ExtendedKalmanFilter(EKF) 移动机器人在未知环境中进行同时定位与地图创建(SimultaneousLocalizationandMapping,SLAM)

[1,2]

是机器人研究领域的热点问题。传统的移动机器人大多应用声纳和激光雷达等距离传感器实现SLAM,这些距离传感器的分辨率较低,在复杂的环境中由于观测数据的高度不确定性导致很难取得理

[3]

想的效果。视觉传感器采集到的图像信息包含了丰富的环境信息,基于视觉信息完成SLAM逐渐成为当前该领域新的研究重点,一般分为单目视觉

SLAM

[4]

和双目视觉SLAM

[5]

。单目视觉传感器结

合声纳和激光雷达等距离传感器实现SLAM,由于单个CCD摄像头无法直接获得单幅图像的深度信息,需要在标定摄像机内参数的基础上用特殊方法获得特征点的三维坐标

[6,7]

,存在定位精度不高的

问题。双目视觉SLAM只单纯使用双目视觉传感器

[8,9]

实现SLAM,标定摄像机的内外参数后,可直接从左右图像获得特征点的三维坐标。但此方法需要利用图像信息恢复机器人的运动信息,鲁棒的运动

收稿日期:2009-08-24

基金项目:国家自然科学基金资助项目(10872160)。

作者简介:王晓华(1972-),女,黑龙江拜泉人,西安工程大学副教授,西安理工大学博士生,研究方向为智能机器人、机器

视觉。E-mai:lw_xiaohua@126.com。

王晓华等:室内环境下结合里程计的双目视觉SLAM研究

467

感器提取的环境特征信息和里程计信息,实现机器人的同时定位与地图创建及更新。

估计很难实现,且计算量极大。针对单目视觉和双目视觉存在的问题,提出了结合里程计的双目视觉SLAM方法。事先对双目摄像机进行离线标定,得到摄像机的内外参数;应用改进的SIFT算法匹配机器人运动过程中获取的左右图像中的特征点,根据已知的摄像机内外参数计算出此特征点在世界坐标系中的三维坐标,由机器人机载里程计获得机器人的运动信息,应用扩展卡尔曼滤波方法融合视觉传

1 结合里程计的双目视觉SLAM框架

本文所述的SLAM算法是基于视觉传感器对环境观测信息与里程计信息的融合,具体采用扩展卡尔曼滤波算法进行融合。系统框架如图1所示。

图1 系统框架Fig.1 Systemframework

获取左右图像时要求同步曝光及图像质量尽量

一致。为了校正由透镜畸变引起的畸变效应,需对采集图像进行校正。应用本文提出的改进SIFT算法分别提取左右图像特征,并完成左右图像特征点的立体匹配,匹配成功的特征点也称为自然视觉路标;经过标定的双目摄像机内外参数均已知,则左右图像成功匹配的特征点的世界坐标可求得,这些世界坐标已求得的特征点构成地图的元素。

里程计通过测量并计算出机器人相对于初始位置的距离和方向来确定机器人的当前位置,但里程计定位方法由于打滑等因素影响存在误差累积。里程计信息融合视觉特征信息,通过扩展卡尔曼滤波(EKF)方法进行定位,使得定位结果更加精确。

地图是由图像中提取出的、带有空间位置信息的SIFT视觉特征点组成。当视觉传感器观测到的环境特征与特征地图库(以前观测到的特征总和)中的某个视觉特征匹配时,则用该特征点来更新地图,同时结合此环境特征在机器人坐标系中的位置信息和世界位置信息以及里程计信息进行定位,否则就作为一个新信息加入到特征地图库中。

成特征描述符。特征描述符为128维的特征向量,

称为关键点。

SIFT关键点是128维的高维特征向量,为了两幅图像之间的匹配而获得特征点时,必须计算一幅图像中的关键点与另外一幅图像中的所有关键点的距离,而每一个距离涉及128维数据,计算的复杂度是可想而知的。原算法应用关键点全部特征向量的欧式距离作为两幅图像中关键点的相似性度量,对一幅图像中的某个关键点,找出其与另一幅图像中欧式距离最近及次近的前两个关键点,如果次近的距离除以最近的距离少于某个比例阈值,则接收这一对匹配点。降低此阈值,SIFT匹配点数目会减少,但匹配更加稳定。通过以下两点改进来提高算法的效率。

1)用街区距离与棋盘距离的线性组合代替欧氏距离。在1次计算欧氏距离时,需要128次乘法和1次开平方计算,而1次计算街区距离和棋盘距离的线性组合距离只用1次乘法计算,节省计算时间;

2)根据部分特征的计算结果逐步减少参与计算的特征点。SIFT特征点的128个特征并非是不相关的,其分别为不同种子点8个方向的向量信息。因此,可以按照角度将它们分成8组,每45b角度间隔的特征向量为一组。首先,计算两幅图像在0b方向上的距离;接着计算45b方向上的距离以及其它距离时,可以只选择特征点附近的对象进一步考察,对距离特征点较远的点不再进行考察,此处距离的2 改进的SIFT算法

SIFT是一种特征点提取方法。这种特征对图像的尺度变化、图像缩放、旋转甚至仿射变换是不变量,而且对光照的变化和图像变形具有较强的适应性。SIFT特征向量的生成包括四个步骤

[10]

:检测尺

度空间极值、精炼特征点位置、设置特征点方向和生468

判断所采用的比例阈值与原算法中的阈值相同,这样需要考察的对象逐步减少,但保留下来继续计算的点的特征向量逐渐增加到128维,保证算法的稳健性,且算法时间可有效地缩减。

图2是机器人行走过程中拍摄的一对图像的SIFT匹配结果。在比例阈值均取0.4的情况下,两

西安理工大学学报(2009)第25卷第4期

种方法获得的关键点数目是一致的,但改进算法的98个匹配点数目明显少于原算法236个匹配点,图2b中左右两幅1280@1024图像的匹配时间较原算法(图2a)节省了50%左右,更符合实时应用的要求。

图2 SIFT匹配结果

Fig.2 SIFTmatchingresults

3 结合里程计的双目视觉SLAM

假设双目摄像机事先标定,且摄像机在机器人运动过程中内、外参数都不变化。

3.1 机器人运动模型

由于里程计信息是移动机器人双轮运动的一个表示,因此可以利用里程计的信息来建立移动机器人的运动模型,机器人运动模型见图3。

Xxr(k)Xyr(k)XHr(k)其中,xr(k)、yr(k)和Hr(k)为k时刻机器人的位置和方位角,$Dk是里程计信息获得的相对k时刻的位移增量,$Hk是机器人相对k时刻的偏移航向角度。

3.2 传感器观测模型

观测模型描述了传感器观测量与移动机器人位姿之间的相互关系,见图4。

(2)

图3 机器人运动模型

Fig.3 Robotmotionmodel

移动机器人运动模型通常是非线性的:Xr(k+1)=f(Xr(k),u(k))+Xr(k)

n

(1)

图4 传感器观测模型

Fig.4 Sensorobservationmodel

其中,Xr(k)IR表示机器人在第k步的状态,u(k)为机器人的运动控制值,Xr(k)为高斯噪声,其

协方差矩阵为Q(k)。令u(k)=[$Dk,$Hk],则在全局坐标系中机器人运动模型可以表示为:xr(k+1)yr(k+1)=H)r(k+1xr(k)+yr(k)+

$Dk

[cos(HosHr(k)+$Hk)-cr(k)]$Hk+$Dk

[sin(HnHr(k)+$Hk)-sir(k)]$Hk

当前观测Z(k)被定义为k时刻观测到的一组环境特征点。则移动机器人系统的观测模型为:

Z(k)=h(X(k))+E(k)(3)h是特征点的观测函数,其雅克比矩阵为H,E(k)是高斯白噪声序列,其协方差矩阵为R(k)。在全局坐标系下双目视觉传感器观测数据可表示为:

rr

xr+xLcosHnHr-yLsir

Z(k)=+E(k)rr

yr+xLsinHr+yLcosHr

(4)

HHr(k)+$k 王晓华等:室内环境下结合里程计的双目视觉SLAM研究

469

P(k+1)=(I-K(k+1)H(k+1))P(k)

(11)

其中,(xL,yL)是环境特征点在机器人坐标系下的位置,Hr是机器人的方位角。3.3 基于扩展卡尔曼滤波器(EKF)的状态估计

通过Taylor展开完成对运动模型和观测模型的线性化处理,得到EKF的系统状态预测和更新的估计过程。系统状态包括机器人状态和环境特征状态,机器人状态与环境特征状态之间的关系及状态之间的不确定性用协方差矩阵描述,移动机器人的系统状态方程为:

X(k)=

Xr(k)XL(k)T

T

rr

4 仿真实验及其结果分析

应用上述机器人运动模型和传感器观测模型进行仿真实验,为验证本文方法,实验过程中手动控制机器人的实际运行轨迹,机器人在行走过程中用EKF估计出机器人运行轨迹,两条轨迹的吻合情况表明了算法的性能。

每隔5s获取一次环境图像,运动速度保持在0.1m/s。机器人在世界坐标系下的初始位置为Xr(0)=[-30,-30,0],并根据SIFT算法提取双目视觉传感器获得的环境图像特征点,获得初始位置的环境特征,机器人位姿与初始环境特征点即为初始状态X󰀁(0);通过运动模型和控制命令进行机器人下一步的状态预测X󰀁(1),运动到下一步实际位置时,用改进SIFT算法获得新的环境特征,也称为实际观测Y(1),并通过观测模型估计观测的环境特征,即观测预测Z(1),计算出卡尔曼滤波增益

(6)

K(1)和状态误差协方差(以上步骤也称为预测过程);接下来称为更新过程,用实际观测Y(1)和观测预测Z(1)对X󰀁(1)进行更新,获得更接近实际值的X^(1),并更新状态误差协方差。在定位的同时将观测到的环境特征加入地图。在后面的运动中上述过程以此类推,因而基于EKF的建图与定位是一个循环迭代的估计-校正过程。仿真实验结果如图5所示。图中机器人运动过程中获得的SIFT特征点用/*0表示,检测到的特征点用/#0表示,图中两条轨迹曲线分别表示给定的机器人运动轨迹(大部分处于内圈的曲线)和应用EKF估计出的运动轨迹(大部分处于外圈的曲线),估计出的运动轨迹上点的坐标即为机器人的位置信息。由于两条曲线吻合良好,不便区分,故图中不予以区分。

(5)

其中,Xr(k)=[xr,yr,H表示机器人的位姿,r]XL(k)=[X1(k),X2(k),,,Xn(k)]表示n个环境特征位置。

具体定位过程分为以下5个步骤完成。

步骤1:机器人位置预测。由第k步机器人状态和控制命令u(k)预测k+1步的状态X󰀁(k+1)及误差协方差P󰀂(k+1):

X󰀁(k+1)=f(X^(k)+u(k))

T

P󰀂(k+1)=¨F(k)P(k)¨F(k)+Q(k)(7)其中,¨F(k)为f(k)对于X(k)的偏导数。步骤2:实际观测。应用传感器获取环境特征

的实际观测值Y(k+1)。

步骤3:观测预测。利用机器人位置预测与实际观测得到观测预测Z(k+1)。

步骤4:对实际观测值和预测观测值进行匹配,并计算新息+(k)和增益K(k+1)。

+(k)=Y(k+1)-Z(k+1)

T

(8)

K(k+1)=P󰀂(k+1)H(k+1)(H(k+

T-1

1)P󰀂(k+1)H(k+1)+R(k+1))(9) 步骤5:状态更新。

X^(k+1)=X󰀂(k+1)+K(k+1)[Y(k+1)-Z(k+1)]

(10)

图5 SLAM实验结果Fig.5 SLAMexperimentalresults

470

由图5可知,实际SIFT特征点并没有被机器人全部估计到,距离机器人近的实际特征点与机器人估计到的特征点之间的误差值小;机器人绕行一周后,实际运动轨迹和估计的运动轨迹吻合情况较好,表明定位误差小。

误差分析见图6。随机取50个点,X和Y方向的最大误差均小于0.07m,最大误差值小于文献

西安理工大学学报(2009)第25卷第4期

[7]单目视觉的SLAM方法中0.1m的定位误差,表明本方法定位精度较单目视觉SLAM定位精

度高。

为进一步验证本文方法的性能,在不同场景下获得不同数目的环境特征点以及人为改变环境噪声大小的情况下进行了多次实验,部分实验结果见图7。

图6 误差分析

Fig.6 Erroranalysis

图7 不同条件下SLAM实验结果

Fig.7 SLAMexperimentalresultsindifferentconditions

图中两条轨迹曲线分别表示给定的机器人运动轨迹(大部分处于内圈的曲线)和应用EKF估计出

的运动轨迹(大部分处于外圈的曲线)。由于两条曲线吻合良好,不便区分,故图中不予以区分。由图7可知,在环境中特征点密集度大致相同时,噪声越大,定位精度越低;在噪声相同情况下,特征点越密集,定位精度越高;但噪声对定位的影响更大一些。当噪声大而特征点少情况下,本算法不能获得满意 王晓华等:室内环境下结合里程计的双目视觉SLAM研究

471

[5]KimGH,KimJS,HongKS.Vision-BasedSimultaneous

LocalizationandMappingwithTwoCameras:2005IEEE/RSJInternationalConferenceonIntelligentRobotsandSys-tems[C].Tokyo:IEEEPress,2005:3401-3405.[6]OrtegaJS,LemaireT,DevyM,eta.lAMoninDelayedvs

UndelayedLandmarkInitializationforBearingonlySLAM:ProceedingoftheIEEEInternationalOnferenceonRoboticsandAutomationWorkshoponSLAM[C].Press,2005:1-3.

[7]王彭林,石守东,洪小伟(WangPeng-lin,ShiShou-dong,

HongXiao-wei).基于单目视觉和里程计的SLAM算法研究(ASLAMalgorithmbasedonmonocularvisionando-dometer)[J].计算机仿真(ComputerSimulation),2008,25(10):172-175.

[8]吴功伟,周文辉,顾伟康(WuGong-we,iZhouWen-hu,i

GuWe-ikang).基于视差空间的双目视觉里程计(Dis-parityspacebasedbinocularvisualodometry)[J].传感技术学报(ChineseJournalofSensorsandActuator),2007,20(6):1432-1436.

[9]DavisonAJ,NobuyukiK.3DSimultaneousLocalization

andMapBuildingUsingActiveVisionforaRobotMovingonUndulatingTerrain:ProceedingsoftheIEEEInterna-tionalConferenceonComputerVisionandRecognization[C].Hawai:lIEEEPress,2001:384-391.

[10]LoweD.Distinctiveimagefeaturesfromscale-invariant

keypoints[J].

InternationalJournalofComputerVision,

(责任编辑 王卫勋)

2004,60(2):91-110.

Japan:

IEEE

的定位效果。

5 结 语

结合里程计的双目视觉SLAM方法解决了单目视觉利用特殊初始化方法获取特征点信息不准确从

而导致定位精度低的问题,也避免了双目视觉利用图像信息恢复运动带来的极大计算量的缺点。改进的SIFT算法能缩短图像处理时间,提高计算效率。如何从环境图像中提取合适的特征信息,以及如何在动态环境下实现SLAM是需要进一步研究的内容。参考文献:

[1]SmithR,SelfM,ChessemanP.Ontherepresentationand

estimationofspatialuncertainty[J].InternationalJournalofRoboticsResarch,1986,5(4):56-58.

[2]Durrant-WhyteH,BaileyT.Simultaneouslocalizationand

mapping.PartI[J].IEEERoboticsandAutomationMaga-zine,2006,13(3):99-108.

[3]王耀南,余洪山(WangYao-nan,YuHong-shan).未知

环境下移动机器人同步地图创建与定位研究进展(Areviewofsimultaneouslocalizationandmapbuildingalgo-rithmsformobilerobotsinunknownenvironment)[J].控制理论与应用(ControlTheory&Applications),2008,25(1):57-64.

[4]DavisonAJ.Rea-ltimesimultaneouslocalicationandmap-pingwithasinglecamera[J].880.

IEEETransactiononPa-t

ternAnalysisandMachineIntelligence,2002,24(7):865-