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

466　西安理工大学学报Journal of Xi ’an University of Technol ogy (2009) Vol . 25No . 4　

　　文章编号:100624710(2009) 0420466206

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

王晓华

1, 2

, 傅卫平, 苏立

11

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

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

摘要:针对视觉S LAM 要解决的定位精度低和鲁棒性低的问题, 提出一种基于双目视觉传感器与

里程计信息的扩展卡尔曼滤波S LAM 方法, 应用改进的SI FT 算子提取双目视觉图像的环境特征获得特征点, 并构建出视觉特征地图; , 完成同时定位与地图创建。确的问题, 棒的缺点。仿真实验表明, , 。关键词:S LAM; ; 中图分类号::A

Research on B i n ocul ar V isi on S LAM w ith O dom eter i n I ndoor Env i ronm en t

WANG Xiao 2hua

1, 2

, F U W ei 2p ing , S U L i

11

(1. Faculty of Mechanical and Precisi on I nstru ment Engineering, Xi ’an University of Technol ogy, Xi ’an 710048, China;

2. College of Electr onic and I nf or mati on, Xi ’an Polytechnic University, Xi ’an 710048, China )

Abstract:W ith the ai m of s olving the l ow positi oning accuracy and l ow r obustness p r oble m s of visi on

S LAM alg orith m , Extended Kal m an Filter (EKF ) method based on binocular visi on and odometer is p r o 2posed in this paper . Feature point can be obtained by extracting i m age features with i m p r oved SI FT algo 2rithm , and the visi on feature map is constituted . S LAM is comp leted by using the inf or mati on of binocular visi on and r obot positi on with EKF . This method can either s olve the monocular visi on inaccuracy p r ob 2le m of feature point infor mati on obtained by s pecial initializati on method or avoid the enor mous computa 2ti on br ought about by binocular visi on odometer using i m age inf or mati on t o rest ore move ment as well as the in 2r obust disadvantages of moti on esti m ati on . The results fr om si m ulati on experi m ents indicate that in the unkno wn indoor envir on ments, this alg orithm operati on is stable, and the positi oning accuracy is high .

Key words:S LAM; binocular visi on; odometer; SI FT; Extended Kal m an Filter (EKF )

　　移动机器人在未知环境中进行同时定位与地图创建(Si m ultaneous Localizati on and Mapp ing, S LAM )

[1, 2]

是机器人研究领域的热点问题。传统的移动机器人大多应用声纳和激光雷达等距离传感器实现S LAM , 这些距离传感器的分辨率较低, 在复杂的环境中由于观测数据的高度不确定性导致很难取得理想的效果。视觉传感器采集到的图像信息包含了丰富的环境信息, 基于视觉信息完成S LAM 逐渐成为当前该领域新的研究重点, 一般分为单目视觉

[3]

。单目视觉传感器结

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

[6, 7]

获得特征点的三维坐标, 存在定位精度不高的问题。双目视觉S LAM 只单纯使用双目视觉传感器实现S LAM , 标定摄像机的内外参数后, 可直接从左右图像获得特征点的三维坐标。但此方法需要利用图像信息恢复机器人的运动信息, 鲁棒的运动

[8, 9]

S LAM 和双目视觉S LAM

[4][5]

收稿日期:2009208224

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

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

视觉。E 2mail:w_xiaohua@126. com 。

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

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

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

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

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

。

图1　系统框架Fig . 1　Syste m fra me work

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

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

(EKF ) 方法进行定位, 使得定位结果更加精确。

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

成特征描述符。特征描述符为128维的特征向量, 称为关键点。

SI FT 关键点是128维的高维特征向量, 为了两

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

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

2) 根据部分特征的计算结果逐步减少参与计

2　改进的SI FT 算法

SI FT 是一种特征点提取方法。这种特征对图

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

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

[10]

:检测尺

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

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

判断所采用的比例阈值与原算法中的阈值相同, 这

样需要考察的对象逐步减少, 但保留下来继续计算的点的特征向量逐渐增加到128维, 保证算法的稳健性, 且算法时间可有效地缩减。

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

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

。

图2　SI FT 匹配结果Fig . 2　SI FT matching results

3　结合里程计的双目视觉S LAM

假设双目摄像机事先标定, 且摄像机在机器人

运动过程中内、外参数都不变化。3. 1　机器人运动模型

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

。

ωx r (k ωy r (k ) ωθr (k 其中, x r (k ) 、y r (k ) 和θr (k ) 为k 时刻机器人的位置

和方位角, ΔD k 是里程计信息获得的相对k 时刻的

θ位移增量, Δk 是机器人相对k 时刻的偏移航向角度。

3. 2　传感器观测模型

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

(2)

图3　机器人运动模型Fig . 3　Robot moti on model

移动机器人运动模型通常是非线性的:

X r (k +1) =f (X r (k ) , u (k ) ) +ωr (k )

n

(1)

图4　传感器观测模型Fig . 4　Sens or observati on model

其中, X r (k ) ∈R 表示机器人在第k 步的状态,

u (k ) 为机器人的运动控制值, ωr (k ) 为高斯噪声, 其

θ协方差矩阵为Q (k ) 。令u (k ) =[ΔD k , Δk ],则在

全局坐标系中机器人运动模型可以表示为:

ΔD k

(r (k ) +Δθθx r (k ) [c os θos k ) -c r (k ) Δθx r (k +1k

y r (k +1=

当前观测Z (k ) 被定义为k 时刻观测到的一组

环境特征点。则移动机器人系统的观测模型为:

(3) Z (k ) =h (X (k ) ) +ε(k )

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

r r

θx r +x L co s r -y L sin r

() Z k =+ε(k ) r r

θy r +x L sin +y cos r L r

θr (k +y r (k ) +ΔD k

(r (k ) +Δθ) θ() [sin θ-sin k k r

Δθk

(4)

θΔθ　　　　　　

　r (k ) +k

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

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

(11)

其中, (x L , y L ) 是环境特征点在机器人坐标系下的位置, θr 是机器人的方位角。3. 3　基于扩展卡尔曼滤波器(EKF ) 的状态估计

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

X (k ) =

X r (k X L (k T

r r

4　仿真实验及其结果分析

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

每隔5s 获取一次环境图像, 运动速度保持在0. 1m /s 。机器人在世界坐标系下的初始位置为

X r (0) =[-30,

(5)

-30, SI FT 算法提取双

T

其中, X r (k ) =[x r , y r , θr ]表示机器人的位姿,

X L (k ) =[X 1(k ) , X 2(k ) , …, X n (k ) ]表示n 境特征位置。

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

(6) 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) (8)

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

T -11) P (k +1) H (k +1) +R (k +1) )

T

, 获得初始位

(0) ; 通过运动模型和控制命令进行机器人下一步的状态预测X (1) , 运动到下一步实际位置时, 用改进SI FT 算法获得新的环境特征, 也称为实际观测Y (1) , 并通过观测模型估计观测的环境特征, 即观测预测Z (1) , 计算出卡尔曼滤波增益K (1) 和状态误差协方差(以上步骤也称为预测过程) ; 接下来称为更新过程, 用实际观测Y (1) 和观测预测Z (1) 对X (1) 进行更新, 获得更接近实际值的X ^(1) , 并更新状态误差协方差。在定位的同时将观测到的环境特征加入地图。在后面的运动中上述过程以此类推, 因而基于EKF 的建图与定位是一个循环迭代的估计2校正过程。仿真实验结果如图5所示。图中机器人运动过程中获得的SI FT 特征

(9)

　　步骤5:状态更新。

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

(10)

点用“3”表示, 检测到的特征点用“・”表示, 图中两条轨迹曲线分别表示给定的机器人运动轨迹(大部分处于内圈的曲线) 和应用EKF 估计出的运动轨迹(大部分处于外圈的曲线)

, 估计出的运动轨迹上点的坐标即为机器人的位置信息。由于两条曲线吻合良好, 不便区分, 故图中不予以区分。

图5　S LAM 实验结果Fig . 5　S LAM experi m ental results

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

　　由图5可知, 实际SI FT 特征点并没有被机器人

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

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

[7]单目视觉的S LAM 方法中0. 1m 的定位误差,

表明本方法定位精度较单目视觉S LAM 定位精度高。

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

。

图6　误差分析Fig . 6　Err or

analysis

图7　不同条件下S LAM 实验结果

Fig . 7　S LAM experi m ental results in different conditi ons

　　图中两条轨迹曲线分别表示给定的机器人运动

轨迹(大部分处于内圈的曲线) 和应用EKF 估计出的运动轨迹(大部分处于外圈的曲线) 。由于两条曲线吻合良好, 不便区分, 故图中不予以区分。由图

7可知, 在环境中特征点密集度大致相同时, 噪声越

大, 定位精度越低; 在噪声相同情况下, 特征点越密集, 定位精度越高; 但噪声对定位的影响更大一些。当噪声大而特征点少情况下, 本算法不能获得满意

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

[5]Ki m G H, Ki m J S, Hong K S . V isi on 2Based Si m ultaneous

Localizati on and Mapp ing with T wo Cameras:2005I EEE /RSJ I nternati onal Conference on I ntelligent Robots and Sys 2te m s[C ].T okyo:I EEE Press, 2005:340123405. [6]O rtega J S, Le maire T, DevyM , et al . A Monin Delayed vs

Undelayed Land mark I nitializati on f or Bearing only S LAM:Pr oceeding of the I EEE I nternati onal Onference on Robotics and Aut omati on Workshop on S LAM [C ].Japan:I EEE Press, 2005:123.

[7]王彭林, 石守东, 洪小伟(W ang Peng 2lin, Shi Shou 2dong,

Hong Xiao 2wei ) . 基于单目视觉和里程计的S LAM 算法

的定位效果。

5　结　语

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

[1]S m ith R, SelfM , Chesse man P . On the rep on esti m ati on of s patial I Robotics Resarch, 4) :[2]Durrant 2W hyte H, Si m l ocalizati on and

mapp ing . Part I[J ].I EEE Robotics and Aut omati on Maga 2zine, 2006, 13(3) :992108.

[3]王耀南, 余洪山(W ang Yao 2nan, Yu Hong 2shan ) . 未知

研究(A S LAM algorith m on monocular visi on and o 2

) [J Computer Si m ulati on ) , 2008, 101722.

], 顾伟康(W u Gong 2wei, Zhou W en 2hui,

Gu W ei 2kang ) . 基于视差空间的双目视觉里程计(D is 2parity s pace based binocular visual odometry ) [J ].传感技

术学报(Chinese Journal of Sens ors and Actuat or ) , 2007,

20(6) :143221436.

[9]Davis on A J, Nobuyuki K . 3D Si m ultaneous Localizati on

and Map Building U sing Active V isi on for a Robot Moving on Undulating Terrain:Pr oceedings of the I EEE I nterna 2ti onal Conference on Computer V isi on and Recognizati on [C ]. Ha wail:I EEE Press, 2001:3842391.

[10]Lowe D . D istinctive i m age features fr om scale 2invariant

keypoints[J ].

I nternati onal Journal of Computer V isi on,

(责任编辑　王卫勋)

2004, 60(2) :912110.

环境下移动机器人同步地图创建与定位研究进展(A

revie w of si m ultaneous l ocalizati on and map building algo 2rith m s for mobile r obots in unknown envir on ment ) [J ].控

制理论与应用(Contr ol Theory &App licati ons ) , 2008, 25

(1) :57264.

[4]Davis on A J. Real 2ti m e si m ultaneous l ocalicati on and map 2

p ing with a single ca mera [J ].880.

I EEE Transacti on on Pat 2

tern Analysis and M achine I ntelligence, 2002, 24(7) :8652