基于粒子滤波的单目视觉SLAM算法.docx
《基于粒子滤波的单目视觉SLAM算法.docx》由会员分享，可在线阅读，更多相关《基于粒子滤波的单目视觉SLAM算法.docx（22页珍藏版）》请在冰豆网上搜索。
基于粒子滤波的单目视觉SLAM算法
第37卷第3期 2008年5月
文章编号:
100220446（2008）0320242206
机器人 ROBOT
Vol.30,No.3
May,2008
基于粒子滤波的单目视觉SLAM算法
陈伟,吴涛,李政,贺汉根
（国防科学技术大学机电工程与自动化学院,湖南长沙 410073）
摘 要:
针对携带有单目摄像机和码盘的微小机器人的定位与建图问题,提出了基于粒子滤波的SLAM（同时定位与建图）算法.从摄像机中提取图像特征点,并在图像序列中加以匹配,根据相应时刻的摄像机位姿计算得到对应的环境标志点坐标;机器人的大致位姿估计由码盘运动模型获得.在机器人移动过程中,息和码盘信息通过粒子滤波相融合,从而提高了机器人定位的精度,.仿真实验结果表明本算法有效、可靠.
关键词:
SLAM;微小机器人;码盘;单目视觉中图分类号:
TP24 :
VAlgorithmBasedonParticleFilter
CHENWei,WUTao,LIZheng,HEHan2gen
（CollegeofMechatronicsandAutomation,NationalUniversityofDefenseTechnology,Changsha410073,China）
Abstract:
Todealwiththelocalizationandmappingproblemofminiaturerobotequippedwithmonocularcameraanden2
coders,anSLAM（simultaneouslocalizationandmapping）approachbasedonparticlefilteringispresented.Theimagefea2turesaredetectedfromcameraandmatchedamongsuccessiveframes,andthencoordinatesofthecorrespondingfeaturesinenvironmentsarecomputedaccordingtothecameraposes.Theroughestimatesofrobotposesareobtainedfromtheencodermotionmodel.Duringrobotmotion,theinformationfromfeatureobservationsisfusedwiththatfromtheencodersbyparticlefilter,solocationprecisionoftherobotisimprovedandalsomoreexactcoordinatesofthefeaturesaregotten.Thereliabilityandefficiencyofthemethodareprovedbysimulationexperiments.
Keywords:
simultaneouslocalizationandmapping（SLAM）;miniaturerobot;encoder;monocularvision
1 引言（Introduction）
机器人的定位与建图是自主移动机器人的热点研究领域.以往是将定位和建图作为两个独立的领域分别进行研究———或者在精确定位的前提下进行环境建模,或者在已有环境地图的条件下实现定位.但是随着机器人技术的发展,其应用的范围越来越广,从室内拓展到了室外,甚至某些未知环境.此时,机器人既不能提前获取环境地图,也无法借助外界手段提供精确的定位.因而在未知环境中,机器人如何创建地图并同时进行自主定位和导航成为当今机器人研究领域的热点问题之一———移动机器人的同时定位
[1]
（simultaneouslocalizationandmap2与建图SLAM
ping）.SLAM也称为CML
[2]
（concurrentmappingand
localization）,最先是由Smith和Cheeseman在相关的
收稿日期:
2007-08-13
论文中提出的,它具有重要的理论价值与应用前
景,被认为是实现移动机器人完全自主化的关键技术
[5,6]
之一.SLAM方法可以简单地描述为:
在未知环境中,机器人从某一位置开始移动,根据控制信息和传感器观测数据同时进行位置估计与地图构建.定位与地图构建融为一体,而不再是独立的两个阶段.
视觉传感器包含了丰富的环境信息,可以用于目标识别跟踪、环境地图构建、障碍检测等.对于微小型机器人而言,装备体积较大而又昂贵的传感器是不现实的,因而利用尽可能少而且廉价的传感器获得尽可能多的信息是对微小机器人的一个基本要求.机器人装备的传感器要尽可能兼顾多种任务,而摄像机无疑是一种理想的选择.摄像机以其获取信息丰富、价格低廉的特点,在机器人定位与建图中引起了越来越多的关注.有很多学者提出了不同的定位方法,这些定
[3,4]
第30卷第3期陈伟等:
基于粒子滤波的单目视觉SLAM算法 243
位方法大体可分为以下三类:
第一类是基于立体视觉
[7]
的方法,这类方法的突出优点是能获取周围环境的深度信息,从而能够实现较为准确的定位,但存在需要对摄像机进行标定等问题;第二类是基于全方位视觉传感器的定位方法
[8]
初步估计,然后,根据摄像机观测到的不同时刻标志点的位置变化,对机器人控制中的位置估计结果进行进一步修正,从而提高定位和环境模型的精度.2.1 码盘差速定位模型
它无需转动就能获得360°如果轮式机器人所运动的环境可被假定为二维平面,那么它的位姿可以用两侧车轮的轮速推算得
n到.车在n时刻的位置可用xn=xn表示,βn为机器
yR
的环境信息,其缺点是感知到的环境信息会产生很大
的畸变;第三类是基于单目视觉的机器人定位算法
[9]
这类方法具有简单易用、适用范围广等特点,它
还可以与里程仪等传感器相结合实现运动立体视觉定位,实现对环境特征的三维测量,完成环境建图,因而单目视觉使用较为灵活,也不会像全方位视觉传感器那样产生很大的畸变.
鉴于微小机器人的客观条件所限,单目视觉与码盘里程仪相结合的S.靠码盘,姿态,.在滑移现象,,如,最终必将导致定位失败.视觉图像包含了丰富的信息,利用它进行误差修正是一种合理的选择.此方法既达到了提高定位精度的目的,又不会增加移动机器人的硬件负担,这对于微小型机器人是非常重要的.另外,对于未知环境中的建图问题,机器人无法通过某一时刻的单目图像获取环境深度信息,但依靠不同时刻两帧图像及码盘提供的机器人位姿变化信息,模仿立体视觉就可以实现对环境特征的三维测量,我们称之为运动立体视觉,这就是本文建图方法的基本思想.
本文所提出的方法在摄像机图像序列中提取特征点,并进行特征匹配和三维计算.将初始状态的车体坐标系作为世界坐标系,最初两个时刻利用码盘实现较为准确的定位,由摄像机得到从机器人到标志点的向量,结合机器人两个位置坐标以直线求交点的方式获得标志点的世界坐标.机器人在移动中通过码盘不断计算出新的位姿信息,摄像机也不断地获取旧标志点的新观测信息,也可能得到新的标志点.利用这些标志点观测信息的变化,不断修正机器人的位姿,并反过来对环境标志点坐标加以修正.这个过程中会有旧的标志点消失、新的标志点被获取,这是一个不断更新的过程.
人的方向角,xn、yn.非全轮动（,）,设两轮轮距为L,码盘的线数）,轮径为D.通过左右码盘的脉冲频率fL和fR可以算得轮子的线速度为:
ss
R
=
fP
R
D+γ
L
f
L=D+γP
（1）
其中,由于地面轮胎变形以及打滑等原因,两轮的实际速度与通过码盘计算得到的数据存在误差.这个误差显然是随机的,和路面情况、行驶速度、驱动力大小等都有关系.所以,这里必须存在一个噪声干扰γ,统计显示这个噪声大致服从一个非零均值的高斯
R2L2
γ分布,γ～N（μ～N（μR,σR）、L,σL）相互独立.
机器人的角速度为:
ω=L
运动学方程为:
βn+1=βn+
xn+1
=xn+yn+1=yn-s-sL
RRR
LR
L
（2）
t
L
L（sn+sn）
2（sn-sn）
L（sn+sn）
R
L
L
（sinββn+1-sinn）（cosββn+1-cosn）
（3）
2（sn-sn）
RL
2.2 摄像机成像模型
摄像机模型延用针孔模型,忽略摄像机畸变,则有如下关系式:
cyczc
R
xwywxw
2 算法使用模型的描述（Descriptionofmod2
elsusedinthealgorithm）
本文所用数据来自码盘和单目摄像机.首先,码
盘差速定位模型对机器人各个时刻的位置坐标进行
T
（4）
M
244 机 器 人2008年5月
即为由世界坐标系到摄像机坐标系的变换方程.
fu-
fucoθtu0
f0c
θ0f/sinvv0=0fyc（5）
00 K
式（5）是摄像机坐标系中特征点坐标到像素坐标系的投影方程.其中（u0,v0）为O在像素坐标系（u,v）
Δy、fu=中的坐标,像素在轴上的尺寸分别为
Δx、
、fv=.K即为摄像机的内参数矩阵,M为外参ΔxΔy数矩阵.内参数矩阵K由摄像机标定得到,
外参数矩阵M按照坐标系之间的相互转换关系得到.
成像点的像素坐标为un=
i
unvn
i
这是一个经过投影
变换和离散化得到的坐标,可以认为真实的投影点是在这个像素附近范围内的一个均匀分布,即
x
iun
=
xunyvn
i
Δx =
Δ0
unv
i
+
ΔΔ（6）
ΔΔΔΔΔ,u～-.,,
22通过式（5）得到的像素坐标可以转化为摄像机坐,归一化其中Δu～-pn
cici
n
ci
nn
（,其
pn
wi
向量为Vn
wi
=
qnrn
如果在下一时刻n+1还能观测
wi
wi
到该标志点,同样可以得到对应的向量Vn+1=
pn+1qn+1rn+1
图1 摄像机坐标系、车体坐标系和世界坐标系关系图
Fig.1 Therelationshipamongcamera,vehicle
andworldreferenceframes
wxn
.这两个时刻摄像机的相应位置分别为yn、
wi
xn+1
yn+1,因此,在世界坐标系中就得到两条直线方程,即h
qn
wi
（x-xn）=pn（x-xn）=pn
wi
ii
iwi
（y-yn）（z-h）
wii
i
i
摄像机的安装如图1所示,摄像机轴线Zc与车体坐标系的Xr轴在同一平面内,两轴有夹角χ,也就
是相当于摄像机坐标系是在车辆坐标系基础上向上平移了h、并且绕Yr轴顺时针旋转了角χ（夹角为正值）.我们假设机器人初始时刻的车体坐标系为世界坐标系,这样我们就可以得到外参数矩阵M.
βsin
R
=
rn
wiwi
qn+1rn+1
（x-xn+1）=pn+1（x-xn+1）=pn+1
i
（y-yn+1）（z-h）
i
wiwi
β-cosβ-sinχsinβcosχsin
-sinxn
这两条直线的交点即为标志点坐标.但是在三维坐标
系中,两直线不一定相交,所以这里我们用最小二乘解得到标志点的坐标估计.
β-sinχcosβcosχcos
-cos,t=-yn
3 粒子滤波的SLAM算法
按上述方法计算得到环境标志点的坐标估计,此
标志点在机器人的后继运动过程中一般可以保持在一段时间内被观测到,当标志点离开观测范围或者没法准确匹配时就退出计算,与此同时机器人又可以不断获得新的标志点加入计算.这样就可以利用这些观测信息不断地修正机器人的位置估计和环境标志点坐标,这就是本文同时定位与建图的核心思想.鉴于实际系统误差的非高斯性,具体SLAM算法采用粒子滤波方法来实现.
2.3 环境标志点的计算
在计算标志点之前必须先确定世界坐标系,我们将机器人初始时刻的车体坐标系定义为世界坐标系,如果在不同时刻的图像上分别得到同一个标志点的对应特征,并且成功匹配,那么结合机器人在相应时刻的位姿信息就能够计算出标志点坐标.首先,当机
x器人在n时刻通过摄像机观测到标志点yi时,得到
i
第30卷第3期陈伟等:
基于粒子滤波的单目视觉SLAM算法
245
其中,λ∝
（i）
π（x
q（x
⌒（i）
⌒（i）
（i=1,2,…,N）,是）
niλ∑i=1
（）
第i个粒子的正则权值,满足
⌒（i）
N
=1.设
{x0:
n}i=1={x0,x1,…,xn},i=1,2,…,N{λ
（i）
n
def
（i）（i）（i）
}i=1={λ
N
def
（1）n,λ
（2）n,…,λ（N）n
表
}
示n时刻对所有状态采样且容量为N的样本,以及相
⌒（i）N
应的权值.如果样本{x0:
n}i=1是由重要性密度函数
q（Xn|Zn）抽取得到的,:
图2 环境特征计算示意图
Fig.2 Sketchoffeaturecoordinatescalculation
λ（i）
n
p（x|）0:
|n⌒（i）
⌒（i）
（9）
法来实现贝叶斯滤波,用样本形式———式———采样（iS）,表示后验概率密度函数,根据这些样本和权值来计算估计值.
设概率密度函数p（x）∝π（x）,而π（x）也是一个概率密度函数,难以对其进行采样,但能够对其进
）,行计算.假设有一个容易采样的概率密度函数q（・
）进行采样而得到的样本根据q（・
（7）～q（x）, i=1,2,…,N
称为采样粒子（samplingparticle）,而q（・）称为重要性密度函数（importancedensityfunction）.那么概率密度函数p（・）的加权近似就可以表示为:
x
N
p（Xn-1|Zn-1）,zn之后,需要用一组p（Xn|Zn）进行近似重构.如果这个重要性函数为一阶马尔可夫过程,且可表示为递推形式
q（Xn|Zn）=q（xn|Xn-1,Zn）q（Xn-1|Zn-1）,那么就
能利用已有的样本x0:
n-1～q（Xn-1|Zn-1）以及新的状态采样xn～q（xn|Xn-1,Zn）得到样本x0:
n～q（Xn|Zn）.进而,如果q（xn|Xn-1,Zn）=q（xn|xn-1,zn）,那么重要性密度函数仅仅依赖于xn-1和zn,这对在每一步只要求得到滤波估计p（xn|Zn）的一般情况非常有用.在此情况下只需要存储xn.利用贝叶斯公式得到
p（Xn|Zn）=
=
p（zn|Xn,Zn-1）p（Xn|Zn-1）
p（zn|Zn-1）p（zn|xn）p（xn|xn-1）
p（Xn-1|Zn-1）
p（zn|Zn-1）
⌒（i）
⌒（i）
⌒（i）
⌒（i）
[10]
⌒（i）
p（x）≈
λ∑
i=1
（i）
δ（x-x
⌒（i）
）（8）
⌒（i）
∝p（zn|xn）p（xn|xn-1）p（Xn-1|Zn-1）
由上式可得
⌒（i）
λ∝
（i）n
p（x0:
n|Zn）q（x0:
n|Zn）
⌒（i）
⌒（i）
=
p（zn|xn）p（xn
q（xn
⌒（i）
⌒（i）
|xn-1）p（xn-1|Zn-1）
⌒（i）
⌒（i）⌒（i）
|xn-1,zn）q（xn-1|Zn-1）
修正的权值就是
λni∝λn-i1
归一化后的权值为
λni
（）
（）
（）
以要消除退化现象是不可能的.这种退化意味着大
p（zn|xn）p（xn
q（xn
⌒（i）
⌒（i）
⌒（i）
⌒（i）
|xn-1）
⌒（i）
|xn-1,zn）
（i）n-1
（）
（10）
量的计算都用来更新粒子,但这些粒子对于逼近概率密度函数的贡献几乎为零.对样本进行重新采样是限制重要性权值退化现象的一个有效办法.其主要思想是在重要性采样的基础上加入重采样,淘汰权值低的粒子,而集中于权值高的粒子,从而限制退化现象.重采样方法对每个粒子x=n按其权值生成Ni个副样本,并使得子被淘汰.
粒子滤波中的一个主要问题就是高维数、大计
⌒（i）
=
λ
Ni=1
（11）
i
λn-∑1
以上（7）～（11）式组成了SIS粒子滤波的基本算法.
SIS粒子滤波的一个普遍问题就是退化现象,即经过几次迭代之后,差不多所有的粒子都有极小的权值.重要性权值的方差随着时间的递增而增加,所
∑N
i
=N,若Ni=0则该粒
246 机 器 人2008年5月
i（t）
算量,在本文算法中如果按照传统的方法,每增加一个环境标志点就要将状态向量增加三维,在保持相同采样精度的条件下,采样粒子数随着特征点的增加成几何级数增长,在标志点较多的情况下会因为计算量太大使算法无法实现.针对本文实际情况,状态向量定义为xn=[（xn） （x）…（x）…（x）],其中x=[x y z],m为标志点数目,1m.如果每一个标志点采样次数为N,则增加一个标
i
i
i
i
的情况下λj∝λj-1p（zj|xj
i（t）i（t）
）.
iii.对所有观测到的标志点完成滤波之后将得
RT
到的（xn）取均值作为这一步的定位结果,最后一
次观测到的标志点滤波后得到的坐标值当成其实际坐标.
③计算Neff=
^
RT1TiTmTT
N
T
如果Neffi（t）j
^
λ∑
t=1
3
则使用重
志点计算量就为原来的N倍.而为了尽可能逼近状态的真实分布,采样数量又不能太小,标志点过少相当于减少了观测次数,也不利于定位精度的提高.所以对滤波方法作如下改进:
每次只用一个标志点和
iRTiTT
机器人位姿坐标xn=[（xn） （x）],
采样法防止权值退化.
4 实验结果（Experim,:
以均值为1,m滤波计算.①当n,其位姿为xn
=0,观测到a个标志点,并得到对应向量.
R
320×240、观测角度的摄像机得到观测量,观测噪声由标志点在像素坐标上的一个均匀分布确定.实验结果如表1和图3所示.图3（a）中显示的三条轨迹分别为真实的轨迹、码盘得到的轨迹和使用该SLAM算法得到的轨迹,结合图3（d）可以看出使用SLAM方法的定位精度明显得到提高,而且随着时间的推移提高的程度越发明显,这也显现出了码盘误差的累积效应,SLAM算法能有效地降低累积误差.图3（b）为机器人实际移动过程中所构建的环境标志点模型,因为视觉测量的距离越远误差就越大,所以只选取观测到的较近范围内的标志点,而且为了使用高精度匹配的标志点,其数量较少.结合图3（c）,该SLAM算法同样提高了环境标志点的精度,但是由图3（c）也可以看到该SLAM算法并没有提高个别标志点的精度,这是因为标志点计算中用到了两个时刻机器人的方向角和位移,这两个误差在计算过程中相互抵消,反而使标志点计算较为准确,而SLAM算法的观测量并非完全独立于码盘定位,也就是说它也要受到累积误差的影响,标志点计算还是会存在误差.表1的统计数据显示,相对于码盘差速模型,使用SLAM算法,定位误差由11.6%减小到3.6%,环境标
②当n=j时,观测到b个标志点,同样得到对应向量,其中有m个标志点（m并非固定值）与j-1时刻匹配成功.
i.如果标志点为第二次观测到的坐标点,也就
是没有计算过坐标,则由2.3节所述的方法计算其坐标.由式（6）可知,这个坐标值存在误差.假设误差服从均匀分布,根据重要密度函数q（xj
p（xj
i（t）i（t）j
i（t）
i
i（t）
|xj-1,u,zj）=
i（t）i
|xj-1,u）作N次随机采样并计算得到状态粒
子x.p函数的值由式
（1）和摄像机观测范围的标志点的均匀分布计算得到.具体来说,这个粒子由机器人的位姿和一个环境特征坐标构成,机器人的位姿可以由左右实际轮速唯一确定,两者相互独立,其分布如式
（1）所述;另外,标志点的图像坐标在像素上服从均匀分布.因此,我们可以得到:
p（x
x
i（t）j-1
i
i（t）j
i
|
u）=p（vj-1）p（vj-1）p（xuj-1）p（yvj-1）p（xuj）p（yvj）.
i（t）j
RLiii
归一化得到对应x的权值λ.
i（t）j
i（t）j
ii.如果标志点已经计算过坐标并且得到采样粒
志点估计误差也从0.365m减小到0.106m,定位和环境建模误差都降低到前者的1/3左右,定位和环境模型精度明显提高.
表1 误差统计
Table1 Errorstatistics
子,则同样根据重要密度函数q（x
p（xj
i（t）
|x
i（t）
j-1
u,zj）=
i
|xj-1,u）抽取粒子xj
i（t）ii（t）
按下式更新权值,并
归一化.
p（zj|xj
i（t）i（t）
λ
i（t）j
∝λ
i（t）j-1
）p（xj
i（t）
|xj-1,u）
i
i（t）i
q（xj
i（t）
|xj-1,zj,u）
i
i（t）
i（t）
i（t）
（12）
码盘定位算法
平均值误差距离（m）
0.3320.09
百分比（%）
11.63.6
环境标志点平均误差
0.3650.106
在q（xj
i（t）
|xj-1,zj,u）=p（xj|xj-1,u）
i
SLAM算法
第30卷第3期陈伟等:
基于粒子滤波的单目视觉SLAM算法
247
图3 实验结果图
Fig.3 Sketchofexperimentresults
5 结论（Conclusions）
本文针对微小移动机器人自身特点提出了一种
结合码盘和单目视觉的粒子滤波SLAM方法.该SLAM方法无需大量环境标志点,但是为了提高环境模型的精细程度,可以对一般的标志点单纯地进行环境建模而不参与定位计算,这样既减少了计算量,又不会影响地图对实际环境的表达.此算法依赖码盘定位差速模型,因此仍然摆脱不了误差累积的影响,机器人长距离的移动导致的误差累积会严重影响定位和环境建模的精度.实验表明:
该SLAM方法虽然未能彻底消除误差的累积,但能较好地对其进行抑制,有效地提高了系统定位精度.
参考文献 （References）
[1]Durrant2WhyteH,MajumderS,ThrunS,etal.ABayesianalgo2
rithmforsimultaneouslocalizationandmapbuilding[A].Proceed2ingsofthe10thInternationalSymposiumofRoboticsResearch[C].Berlin,Germany:
Springer,2003.49-66.
[2]ThrunS,BurgardW,FoxD.Aprobabilisticapproachtoconcurrent
mappingandlocalizationformobilerobots[J].MachineLearning,1998,31（123）:
29-53.
[3]SmithR,SelfM,ChessemanP.Estimatinguncertainspatialrela2
tionshipsinrobotics[A].ProceedingsoftheIEEEInternationalConferenceonRoboticsandAutomation[C].USA:
IEEE,1987.850.
[4]SmithR
升级会员 