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

第2 卷 第 1 1 期
20 0 8年 3月

宁 波 大 学 学 报 (理 工 版 )
J OUR NAL OF NI NGB U I RSI O N VE TY(NSE ) E

V 1 No. o. 21 I
Ma. 2 0 r 08

文章 编号 :0 15 3 20 o .0 80 10 .12( 08) 1 6.4 0

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

摘 要 : 究 了基 于尺度 不变特征 变换(IT算 法的 移动机 器人 同时定 位与地 图创建(L 研 SF ) S AM) 法 , 方
即在视 角改 变情 况下 ,用 SF IT算 法对 不 同图像进 行 特征 匹配 ,根 据极 线几 何原 理得 到摄 像 头的

旋转 角度 ,将之 与里程 计 的 角度 信 息融合 ,从 而 实现较 准确 的 自我 定位 与地 图创 建.实验表 明 : 本 方 法利 用 电荷耦 合 器件( C ) C D 摄像 头和里程 计 问 内在 的几何 关 系来 实现 S A , 高 了低 成 本 L M 提
移 动机 器人 的定 位精度 .

关键 词 :SF IT算法 ;同时定 位 与地 图创建 ;极 线几何 ;融合
中图分 类号 :T 2 P4 文献标 识 码 :A

移动机器人的定位 与地图创建是机 器人领域 的热点研究 问题 ,是实现机器人导航的前提.目 前, 对已知环境地图的机器人 自主定位和 已知机器 人姿态的地 图创建 , 已经有了许 多不同的解决方法 , 根据先验地 图, 在结构化室内环境 中进行 的移动机 器人定位 与地图创建 的研究 已经取得 了很大的成 功 ,并且 已有很多的应用实例. 然而在很多情况 下. 事先较难获得机器人的工作环境地图 , 而且在 很多环境中机器人不能利用全局定位系统进行定
位 .所 以 ,机 器人 如需 在 完全未 知 的环境 中创建地

来越受到国内外学者的重视.目 ,对于基于视觉 前
的移动机器人同时定位与地图创建而言 , 一般可分
为单 目视觉 【 1 6 和双 目视 觉 【. . 8 1

本文提 出了一种基于单 目视觉的移动机器人 S A 方法 ,将 C D摄像头和里程计组合 ,来实 LM C

现 S A 为提高定位的精确性和避免误定位的发 L M.
生, 在基于里程计定位的基础上 , 将不同视角的视 觉图像中提取的特征进行匹配 . 根据极线几何计算 摄像头旋转的角度 , 得到摄像头与里程计的角度冗 余信息 ,采用扩展卡尔曼滤波(K ) E F对信息进行融

图, 需同时利用地图进行 自主定位和导航 , 这方面
的研究 已经 取得 了很 大 的进展 ,并应 用于 室 内l, l J

合 ,从而提高 S A 的鲁棒性. LM

水下[ 2 1 和室外【 3 】 等各种不同的环境.

1 S AM 的数学模 型 L
11 移动 机器 人模 型 .

移动机器人大多采用机载传感器 , 如激光测距
仪, 声纳及摄像头等. 近年来,随着图像处理技术 的进步(I IT ~ S 算法f 以及C D IF 4) j: C 摄像头具有代价
低 ,重量小 ,能耗 少 等优点 ,基 于视 觉 的S AM越 L

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

收稿 日期 :20 —11. 0 70—2 宁 波 大学 学 报 ( 工版 ) 理 网址 :hp/ x b d . t :3bnueu n t / c 基 金项 目 :浙 江 省教育 厅科 研基 金 ( 0 5 7 20 43) 第 一作者 :j彭林 ( 92一 ) f 三 18 ,女 ,浙江 瑞 安 人 .在 读硕 士研 究 生 ,主要研 究 方 r :移 动机 器 人导航 - i w n pn l _9 13 o u 】 Emal ag egi 0 @ 6 . m : n c

第 l 期

彭林 ,等 :基 f IT算法 的移动 机器人同时 定位与地 图刨建 SF

6 9

置,0 表示机器人的朝向. 在位姿跟踪问题 中,移
动 机 器 人 的初 始 位 姿是 已知 的 .地 图 的 创 建 过 程

系 中 的 位 置 .机 器 人 获 得 的 目标 位 置 ( , ) 是 指第 i 目标在 机器 人坐 标 系 中的位置 ,因此 还需 个 将 其转 换 成世 界坐 标 ,可 表 示 为 :
R

是指移动机器人在 自身位姿 已知情况下 的环境地
图 创建过 程 . S A 中 , 器人 利 用 自身携带 的 在 L M 机 传 感器 识别 未知环 境 中的特 征标 志 , 后根 据机 器 然 人 与 特 征标 志 之 间 的 相对 位 置 和 里 程 计 的 渎数 估 计 机 器 人 和 特 征 标 志 的 世 界 坐 标 .这 种 在 线 的 定 位 与地 图 创建 需 要 保 持 机器 人 与特 征 标 志之 问 的 详细 信息 . 在室 内特 定环 境下 ,基于 环境 特征 S A L M方 法



( 3 )
( 4 1

月:

L0 . ls 5 1 k- c o
s c cs ,J i3 n , o~ _

1 扩 展卡 尔曼 滤波 (KF . 2 E 1

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

的基本思想是将移动机器人的位姿和环境特征坐 标表达在 1 个状态向量 中, 在机器人的走行过程 中 通过对环境特征的观测做最准确的估计. 假设移
动机 器人在 世界坐标 系( X Y表 示) 用 , 中为 1个 点 ,

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

机器 人的起始位置为世 界坐标系原点 , 前进的方向 ( 机器人的朝向0) 为机器人坐标系中的横坐标 ,即 X轴 , 逆时针旋转 9 . 0为纵坐标 '轴 , 如图 1 所示.

如 5 ()



其中,v和 为互不相关且均值为零的正态 白噪 声序列 ,方差分别为 和 R.而 后 时刻的状态向
胜.肌 ,肌 肌 肌

量和量测向量可表示为如下形式 :

图 1

L 人 移 动 过 程 器

因此 ,世界坐标系中的方程为:
= Xk


l Ic s ~1 , +A^ o ( _+a ) Of () 1 ) ,

X =

,

=

() 6

Y =Y +A i(~ +a ) l Is O一 , n l o =O j t ' k k +a a n
x


I

+ y 一 AI O dk + kC S

其中, 和 表示 k 时刻机器 人在世界坐标系中 的位置 ; 表示 k 时刻机器人的朝向;Ak k l 是 时 刻移动机器人的位移; a 表示 k 时刻机器人的偏航
角.

量测值 是从状态预测向量 中计算得到的 目
标位置 , 它是指 目 标点在机器人坐标系中的具体位
置 ,可 以表 示为 :
c =

由于 目标 是静 态物体 , 以在世 界坐 标 系 中的 所 位置 可 以表示 为 :
t



:




I ,

=


1 .

() 2

[=x + ;尺 , ]E '1
() 7

矗 和 ( 1 ) , …, 是第i 目标在世界坐标 个

i , , =1… .

维普资讯 http://www.cqvip.com

7 0

宁波大学学报 ( 理工版 )

2 0 08

所以 ,系统状态方程和量测方程为 :
=

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


Xk】 b I+ + / k 】
_

a d k
_

l ,

空问的 , 对图像缩放 , 旋转甚至仿射变换保持不便 性的图像局部特征描述算子~ sF IT算子, 即尺度 不变特 征变换. IT 算法首先在尺度空问进行特 SF 征检测 ,并确定关键点的位置和关键点所处的尺 度, 然后使用关键点领域梯度的主方向作为该点的
;

:

H X + wk .

其中,
= - ,

.
H k=

[… …

l 估 计 与更新 方 程 - 3

采用 E F实现 系统状 态 的估 计与 更新 ,当机 K

方向特征 ,以实现算子对尺度和方向的无关性. 梯
度和 方 向的 计算方 程 为 :
m(. ) ( +1 ) ( l ) + x Y =( ( , 一L x— , Y ) ( (, L xY+1一£ Y—1 1, ) (, ) )2 ) r 1 ,

器人在运 动过程 中发现新的特征 i 时 ,需要根 v / +l
据新特 征 的观 测矢 量 zk+1和 机器 人的 当前 状 态 ( ) 计算 新特 征标 志 的初始 状 态 , 更新状 态 矢量 和协 并 方 差矩阵 P.
其中,
p
P=

Ox Y = a 2( (, (, ) t (£ Y+1一 (, 1 / n ) L x Y— ) ) (( £ +lY ~L x , ) , ) ( —l , )

' ( ) 为 ( Y 处梯度的模值和方向公式 ,其 1式 5 x ) ,
p, p】


中 £所 用 的 尺 度 为 每 个 关 键 点 各 自所 在 的 尺 度 , 在实 际 汁算时 , 可在 以关键点 为 中心 的邻域 窗 L内 ]

pI ,


:

:

p

pn

采样,并用直方图统计邻域像素的梯度方向. 直方 图的峰值代表了该关键点处邻域梯度的主方向, 即
作 为该 关键点 的方 向 .

在 S AM 中 ,系统 的状 态 包括 机器 人和 环境 L 特 征在 机器 人坐标 系 中的位置 的估 计 , 协方 差矩 而

阵 P表示估 计的误差. 扩展卡尔曼滤波方 法对信
息的 处理 一般 分为 预测 和更 新 , 并且 此方 法对 信息 的估 汁是 无偏估 计 .

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

卡尔曼滤波预测方程为 :
=


X H + ,

() 9

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

Yk=H k X ,
=


(0 1)
(1 1)

F, + ¨ T

更新方程为 :
K : H H H + ) k 7 ( 7 ~, (2 1)

X + (^ , = y~z )
=

(3 1)
(4 1)

(~ ,

) ,

其 中, Q是状态噪声协方差 , 只是测量噪声协方差 ,
J是 误差协 方 差 , 是 增益矩 阵. p

预测方程用来预测当前状态 , 误差协方差矩阵 为下一时刻获得先验估计. 新方程将先验估计和 测量值结合 ,得到一个较精确的后验估计.
1 SF . IT算 法 4
图 2 S AM 实 验 结 果 L

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

L we 2 0 年总 结 了现有 的基 于 不变 量 o G在 0 4 D

维普资讯 http://www.cqvip.com

第1 期

王彭林 ,等 :基于 SF IT算法的移动 机器 人同时定位与地 图创建
L o ad e nr .Du r n .Sm u tn o s ma u li g ra tW i l eu p b i n a d

点 ,根据极线几何原理 ,计算得到旋转 角度 ;(1 2 将这个角度与里程计的角度信息进行融合 , 得到 1 个更接近实际的角度值 ;() 3把它与里程计的位移 结合进行 自我定位 ,根据 C D摄像头获得的特征 C 点的信息 ,以及结合上述 2个步骤计算得到的结
果 ,来实现 S A L M.
.

a d l ai t n fra uo o u bl rb t ] n o lai o n atn mo smo i o o[ / c z o e C/
Pr c e ig o e I E I t r a in l o k h p o n e o e d n ft EE n e n t a r s o n I t — h o W l g n b t n S se .L n o :S rn e r g l e t Ro o s i a d y t ms o d n p ig r Ve l , a
l 9: 6— 21 99 31 3 .

Dis n y k mi i Ne s a a a e Ga n , wma a l Clr t v n e 1 n P u , a k S e e , ta. A o u in t h i l n o sl c l a i n a d ma u l s l t O t e smu t e u o a i t n p b i o a z o —

由于里程计本身存在的缺陷 , 比如机器人行走

dn L igS AM rbe J.E ET asc o s nPo ois po l ]I E rnat n rb t m[ i o c
a dAuo t n 2 0 , 3 :2 —4 . n tmai , 0 1( ) 9 2 o 1 7 2 1
Da i L.Dit ci e i g e t r s fo s a en a v d G si t ma e f au e r m c li v — n v

过程 中轮子打滑 , 就会产生实际移动距离和机器人
内部里程表数据不一致. 因此 ,随着时间的推移 ,
里程计 的 累积误 差逐渐 增 大 , 从而 导致 自我定位 与 实际 相差较 大 .

r n ky onsJ.ne ai a Ju a o o u r — i t ep it ] It t n l o r l f mp t a [ n r o n C e Vi
s n 2 0 , 0 2 : 11 0 i , 0 4 6 () . o 9-l

D vdG L O jc cg io o l a sa — v r n a i . beteont nf m cl cl i ai t r i r o en a F a rsE / ] 【0 60 —8. t :ww C. c al et e[BOL .20 —60 ]ht / w. u . / — u p/ Sb c o
werpa r /c V . . / pe si c 99pdf

本文将 C D 摄像头和里程计的信息进行 融 C
合, 尽可能减少里程计产生的累积误差, 在此基础 上用扩展卡尔曼滤波进行地图的创建及更新, 实现 了移动机器人的 自主定位与地 图创建 的算法建立
与仿 真 .
参 考文献 :
【】 K n S J a g K W,P r a ge k e 1 o ut 1 wo ,Y n ak S n d o , ta.R b s
mo i o o o a ia i n wi o i e l n fl r b l r b t lc l t t c mb n d Kama t — e z o h i e

Da io A .Re l i s vsn J a — me i l n o s o ai ai n n t mu t e u l c l t a d a z o

ma pn i ige cmeaE / ] 2 0 —8 1] p ig w t a s l a r[ BOL.【0 60 —2. h n ht:w w. b tO . . /j/ t / w r os X cu . d. p/ o . a ka
Jo g e n W o y o , L e o en e Ky u g n . CV— LAM :a e on ru S n w

cin io —ae L el g vs nb sd S AM eh iu []2 0 IE / i i tcnq eC / 0 5 E E /
RS n e a i n lCo fr n e o n el e t R b t a d J I t r t a n e e c n I t l g n o o s n n o i S se , 0 5 3 0 0 3 0 5 y t ms 2 0 : 7 — 7 .

Ki Ga h e Ki J n s n , n s n . so — a e m b o , m o g u g Ho g Kia g Vii n b s d
s i l n o sl c l ai n a d ma p n t wo c me a mu t e u o ai t n p i g wi t a r s a z o h

pr ra o s ma r ]Poedn s fh E t — et b t net t [ / rce ig eI E I e u i i oC/ ot E nr
n t n l n e e c n I tl g n b t a d S s m s a i a o Co fr n e o n e l e tRo o s n y t i e ,
2 5: 9 1 . 00 l 0. 95

[ ]2 0 E ER JIt ao a C nee c nItl— C/ 0 5I E / S ne t n l o frn eo el / n r i n i
g n b t a d S se , 0 5 3 0 . 6 et Ro o s n y t ms 2 0 : 4I3 0 . 4

M o i b t i l n o sP st n n n a p n s d o I T g rtm b l Ro o mu t e u o i o ig a d M p ig Ba e n S F Alo i e S a i h
W ANG e g l , HIS o — o g HONG a — i P n —i S h u d n , n Xio we ( a ut o f r a o c n e n e h oo y Nig oU ies y Ni b 2 2 C ia F c l f no r t nS i c d c n lg , n b nv r t, n o3 5 1 hn ) y I i i a e a T i g 1,

Ab ta t h i l n o slcl a o n p ig (L s c:T es r mut e u o a zt n ad ma pn S AM)meh d i iv s gtd b sd o h cl a i i to s n et ae ae n te S ae i Iv r n etr rn fr (I T ag r h I ea o tm efa rs r m i ee tma e t e n ai t aueT a s m SF ) lo tm.nt l rh t et e o df rn i g si mac d a F o i h gi h u f s h
wi h a a tv e p i t . o i i g t e a g e d t r m d me e n h o t n l e i e o t t e v r n iw o n s By c mb n n h n l aa fo o o t ra d t e r t e a g e d rv d f m h i a r
e i olr ge p p a omer ty,t e LAM i tus i l me t d wih h gh r p e ii n.The a g rt h S s h mp e n e t i e r c so l o hm i y ake n o i manl t s i t

c n iea o eg o t orl inb t e h reCo pe vc ( C cmeaa do o tra da o s rt nt e mer c r a o e d i h y et wenC ag u l De i C D) a r n d me , n s d e e a
r s t h up rorSLAM sto i g a c a y i c e e e ul t e s e i po ii n n c ur c sa hiv d. Ke y wor :S F a g it m ; i la e u o a ia i n a d ma pi g pi a e me r ; uso ds I T l or h smu t n o sl c l to n p n ;e polrg o t f i n z y CIC anmbe :TP2 r 4 DOC Um e tc n 0de A :

( 责任 编辑

章践立 )


相关文档

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