一种自适应特征地图匹配的改进VSLAM算法

张峻宁 苏群星 刘鹏远 朱庆 张凯

随着机器人技术的深入发展,基于视觉的同时定位与地图构建(Visual simultaneous localization and mapping,VSLAM)的技术具有越来越多的应用[1−2].VSLAM 技术同时需要机器人的三维环境信息和对应的轨迹信息,因此VSLAM技术对机器人的定位实时性和精度要求较高.

目前,机器人精确的位姿估计主要依靠轮式里程计(Wheel odometry)[3]、全球定位系统(GPS)[4]、惯性导航系统(Inertial measurement unit,IMU)[5]以及视觉里程计 (Visual odometry,VO)[6−9]等.随着稀疏矩阵和非线性优化理论的完善,基于VO技术的SLAM 得到越来越广泛的发展[10−12],出现了一些经典的实现方案,如LSD-SLAM、ORBSLAM、RGBD-SLAM 等算法.VO主要是基于视觉原理,通过相邻两帧的图像信息计算相机的相对位姿.一般而言,SLAM 的VO计算主要分为两类.一类是基于稀疏特征点的VO,另一类是稠密的方法.基于稀疏特征的VO根据SIFT(Scale invariant feature transform)、SURF(Speeded up robust features)、ORB等特征点的匹配建立帧间联系.比如,Henry等[12]利用RANSAC求解两帧图像间的变换矩阵,然后利用迭代最近点算法(Iterative closest point,ICP)求解两帧深度图像间的最优变换;Endres等[13]等在RANSAC求解变换矩阵的基础上,通过图优化器g2o优化当前帧的位姿(RGBD-SLAM-V2),得到了全局一致的地图;Huang等[11]借鉴立体视觉里程计,设计了一套基于Kinect的视觉里程计(Fast odometry from vision,FOVIS);为了提高位姿估计的精度,Dryanovski等[14]提出了一种帧到特征地图的视觉里程计(Fast visual odometry,FVO),实现了单帧对特征地图点的位姿估计.高翔等[15]在FVO基础上,提出了一种g2o(General graph optimization)优化的帧到特征地图的视觉里程计,为后端和建图提供了更准确的位姿信息.Cao等[16]在提取的关键帧的重叠区域,通过块匹配重建重叠区域,提取了少于5%的关键帧,有效地构建了关键帧的地图匹配.Martins等[17]提出了一种基于RGB和ICP点对平面误差项调节的激活函数,有效降低了前端视觉里程计的误差影响,但是付出了更多运算时间.Wadenbck等[18]利用相邻两帧的平面特征,简化了参数,提高了效率,但是不适合复杂的场景.稠密方法最早是由Steinbrcker等[19]提出的,在图像一致性的假设下,相比于特征点法能够充分利用像素信息,但缺点是需要耗费大量计算资源,通常需要GPU加速.

综上所述,目前已有方法的视觉里程计大多还是精度不够,或者对硬件要求较高.为此,本文在帧到特征地图配准的基础上,针对特征地图数据量大、耗费计算资源的缺陷,提出一种自适应特征地图的改进SLAM算法.首先,以划分的特征地图的子区域为最小单元,根据特征角点响应强度提取少量高效的特征点,并进行各帧配准;其次,为应对小规模地图出现的配准失踪的情况,提出一种子区域特征点补充和特征地图扩建的方法,快速实现当前帧的再次匹配;最后,为了进一步提高VO的位姿估计精度,增加特征地图优化环节,且在该环节中提出一种帧到帧、帧到特征地图的局部图优化模型,并通过g2o实现了相机位姿和地图特征点的同时优化.

1 VSLAM算法框架

本文通过图优化的方式实现VSLAM算法,主要包括三部分:视觉里程计、图优化以及地图构建,其中视觉里程计是利用本文提出的自适应特征地图匹配完成,框架如图1所示.

1.1 视觉里程计

视觉里程计(VO)的主流方法是特征点法,但是该方法仍存在各帧特征点错误匹配率高的问题.为了提高VO的计算精度、减少误匹配,本文在VSLAM的框架中,加入了自适应特征地图,将各帧与自适应规模的特征地图配准,在减少计算量的同时,提高了VO的位姿估计精度.

常用的特征点有:SIFT、SURF、ORB等[15,20],考虑到实时性要求,本文选择ORB算子进行特征提取与匹配.ORB算子于2011年由Rublee等[21]提出,其保持了特征具有的旋转和尺度不变性的同时,速度效率提升明显.在ORB的基础上结合汉明距离,利用BruteForceMatcher匹配特征地图与各帧的特征点,并针对误匹配、匹配点数不足的情形,分别利用区域特征点补充和地图扩建的方式再次匹配各帧特征点;最后,利用匹配的特征点计算内点和变换矩阵,并计算帧间相对运动距离.

图1 VSLAM算法框架Fig.1 Framework of VSLAM algorithm

1.2 图优化

VO完成后,可通过位姿图优化相机位姿,常用的方法有:基于非线性的最小二乘优化、基于松弛的优化方法、随机梯度下降法以及流行优化方法.前三类方法都在欧氏空间下计算但实际的相机位姿表达是非欧氏空间的,利用此类方法容易产生欧氏空间的奇异值,因此,可用四元数表示相机位姿并通过流行空间优化位姿.g2o是Kmmerle等[22]提出的一种用于优化流行空间的开源工具包,具有更高的效率和开发精度.本文在VSLAM 算法中,为了进一步提高各帧位姿的精度,不仅利用g2o进行后端图优化[15],而且在前端也利用g2o优化特征地图的内外点以及前端位姿估计.

1.3 地图构建

三维空间表示有很多方法,主要为3D点云图和3D octomap[23]地图.3D点云图在表示地图环境时清楚、直观,但是冗余信息较多且占据大量内存,不适合复杂的环境和终生SLAM(Life-long SLAM)算法.相反3D octomap地图则避免了上述缺点,通过较小的内存空间直观的表示地图中占据、空闲和未知区域,更有利于机器人的导航和路径规划.

2 自适应特征地图构建和帧到帧、帧到模型的g2o的特征地图优化

视觉里程计是通过帧间的匹配信息估计相机的位姿变化,为了提高VO的位姿估计精度,一般将当前帧与历史帧集做特征点匹配,但是需要多次匹配且计算量大,耗费了大量计算资源.本文在Dryanovski提出的模型[14]基础上,以较小规模维护特征地图.首先,针对小规模特征地图配准失踪的情况,补充子区域特征点和特征地图扩建,在保证位姿精度的同时缩短了运算时间;其次,提出一种帧到帧、帧到特征地图的图优化模型更新当前帧的位姿和地图特征点位置.

2.1 自适应特征地图构建

在不考虑光照、遮挡的特殊情况下,特征地图的特征点是丰富的,除了特征地图与各帧匹配的特征点,大部分特征点是冗余的,例如地图中不在当前帧视野内或者视野角度过大的特征点等.为此,本文从VSLAM 的实时性出发,研究如何维护一个低规模的特征地图,使得满足各帧的正确匹配点集,以及提出一种未满足配准时当前帧的快速再次配准的方法.

2.1.1 特征地图构建

假设Kinect在第k时刻采集的数据集为Ik,设利用特征提取和相机投影模型得到的特征点集为Lk(li,i=1,···,n),其中每个特征点为为第i个特征点在相机坐标系下的位置向量,具体计算见式(1);是从OpenCV的SIFT库检测到的该特征点的角点响应强度,表征该特征点的角点强度大小.

式中,[u,v]为第i个特征点的像素位置,fx,fy,cx,cy为相机内参,d为Kinect采集的对应像素深度值.

同理,构造特征地图M=[mi],特征点mi=.不仅包含第i个特征点的位置向量和特征角点响应强度,还有该特征点的ORB描述子,以及历史匹配次数.另外,需要强调的是,特征地图初始化是由最初的前k−1个时刻的特征点组成,并且全部都是以世界坐标系为参考(本文以第一帧为世界坐标系).随着相机运动,需要往地图里实时添加新的特征点.为了能够让VO做得更快、节约计算资源,利用VSLAM 中已有的关键帧线程,直接将距离当前帧近的关键帧的特征点添加到特征地图中,当特征地图达到一定规模之后,再把距离当前帧远的特征点筛除,具体过程如下:

步骤1.设帧间的旋转向量r和平移向量t,可求解出各帧间的相对运动距离:.利用D添加特征地图:

1)若Dmin≤D≤Dmax,则Framecurr=Framekey;

2)若D

3)若D>Dmax,则Framecurrkey.其中,Framecurr是当前帧,Framekey是关键帧,Dmin、Dmax分别是两帧之间的最小和最大的运动距离,只有满足条件(1)的当前帧才被选作关键帧,该关键帧的特征点才能被添加至特征地图中.

步骤2.设特征地图中特征点个数为,特征地图规模阈值为γ,当时,将特征地图中最早添加的帧丢掉.

2.1.2 自适应的特征地图维护

特征地图中冗余特征点的匹配的计算量是巨大的,一般CPU无法满足SLAM的实时性要求.为此,本文提出一种自适应规模的特征地图用于帧间匹配,该特征地图以较小的规模快速地计算各帧配准,即使未能正常匹配,该特征地图也能通过子区域特征点补充和轨迹预测的地图扩建方法快速再次匹配,具体地图构维护如下:

1)剔除特征地图中不在视野内的点

计算地图特征点pM在位姿Ti+1下的像素位置:中特征信息冗余、匹配时间增加,不能满足实时性的要求,若取值过小则不能充分利用特征地图信息,造成当前帧与特征地图配准个数偏少,计算的位姿偏差,甚至视觉里程计追踪丢失的情况.因此θ的取值应适宜,依据经验常选取为[0.5～0.7](具体特征地图的有效因子实验见第3.2节),但是实际工况复杂,很有可能相邻两帧之间的旋转、平移距离较远或者一些相机旋转抖动的特殊情况,造成当前帧与减小规模后的特征地图匹配点数偏少,甚至VO追踪丢失.为此,本文对此类情形进一步讨论,研究各帧配准点较少时的特征地图的扩充方法.

上述算法中,length rgb,widthrgb分别为各帧图像的长、宽.需要注意的是,由于当前的位姿估计不准确,一般将视野比例扩大.因此,通过上述算法能够初步从特征地图中提取出在第i+1帧视野内的特征点.

2)基于视野区域分块的特征地图构建

由于特征地图是由各参考帧为地图贡献的一些信息组成,那么在相同的视野区域内,不同参考帧贡献的信息就存在冗余.为了剔除冗余信息,本文对视野区域内的特征地图分块,依据角点响应强度(越大,该特征点被匹配的概率越高)对地图子区域中的特征点排序,然后将部分角点强度大的特征点集作为特征地图,具体步骤为:

步骤1.对每帧图像区域分块,即均匀地将图像分成m×n块,并对各子区域编号见图2所示;再根据特征点的像素位置,将特征点标记到对应的子区域中.

步骤2.利用对子区域中的特征点排序,并从各子区域中提取θDj(0<θ<1)个特征点组成第i+1帧的特征地图,其中Dj是j区域的特征点个数,θ为区域特征有效因子.

按照上述算法构建的特征地图,有效地利用了各参考帧中易被匹配的特征点,在保证位姿估计精度的同时缩减了运算时间,但是在该算法中θ的取值受到大小两端的限制,若θ取值过大将造成地图

图2 子区域划分Fig.2 Division of sub regions

3)配准点较少时的特征地图扩充

设定各帧与特征地图的配准个数为q,配准阈值为λ,则造成当前帧与特征地图配准个数q小于λ的原因可能是:a)取值偏小,造成特征地图中对当前帧的有效特征数量偏少;b)当前帧的帧间运动Ti,i+1与前一帧的帧间运动Ti−1,i相等的假设不成立,即帧间运动相差较大,造成特征地图中保留的是假视野内的特征点.

对于a)情形,虽然特征地图与当前帧的配准点数较少,但是匹配得到的配准点是正确的,根据当前帧与特征地图的配准点分布,从对应的子区域和相邻子区域中补充历史匹配次数多的特征点(越大,该特征点被匹配的概率越高).由于各子区域的位置分布不同,其补充特征点的方式也不相同,如图3所示.其中图3(a)是当配准点分布在对应的视野边界角区域(对应图1中1,n,m,mn子区域)时的地图特征点补充方式,基于子区域特征点补充的最大原则,考虑到边界角区域相邻边较少,在该区域的特征点补充从边相邻1,3区域以及角相邻的2区域补充;图3(b)是配准点分布在边界区域时的特征点补充方式,直接从边相邻区域1,2,3补充;同理,图3(c)是配准点在中间区域时的特征点补充方式,直接从边相邻区域1,2,3,4补充.

针对a)情形,按照图3的补充方式能够有效地补充地图特征点,再将补充的特征点与当前帧做匹配,加上首次的配准点,就能够快速完成当前帧的特征点匹配.

当然a)的地图特征补充方式是不适合b)情形的.当第i+1帧的帧间运动Ti,i+1与前一帧的帧间运动Ti−1,i相差较大,则之前假设Ti,i+1=Ti−1,i而构建的特征地图是不合适的.为此,本文提出一种基于位姿预测的特征地图扩建方法.首先提出一种简单有效的位姿预测,利用预测的位姿寻找对应地图中的视野区域,再将该区域的特征点与当前帧匹配,具体过程如下:

图3 各子区域的特征点补充Fig.3 Complement each region of feature points

i)增大地图视野.设地图视野缓冲长度β,将地图视野分别从长宽两端增加为length rgb+2β,width_rgb+2β,然后按照1)中步骤剔除特征地图中不在视野内的特征点,则增大的地图视野见图4所示,其中p,e,b,w分别作为特征地图的增大区域的标记,其编号按照顺时针方向编排.

图4 扩建特征地图Fig.4 Extension feature map

图5 各区域的特征地图扩建方式Fig.5 Characteristic map expansion methods in different regions

图5是配准点分布在不同区域的地图扩建方式,其中图5(a)是配准点分布在视野边界角区域(对应图1中编号为1,n,mn−m+1,mn的子区域)时的地图扩建方式.基于地图扩建的最大原则,由于该配准点在视野的左边界角区域,只是基于该区域的配准点估计该帧的最大位姿运动方向如图5(a)中箭头方向,那么包含该配准点的最大视野应包含边相邻和角相邻1,2,3区域,因此该区域的扩建地图应包围相邻的1,2,3区域.图5(b)是配准点在边界区域时的特征地图扩建方式,当配准点位于边界区域时,那么该配准点的最大视野应包含图中1,2,3区域,因此从边相邻区域1,2,3扩建地图.图5(c)是配准点对应第二层边界角区域时的特征地图扩建方式,从最大位姿运动考虑,该地图特征点补充从1,2,3,4,5区域补充,但是因为该配准点是在第二层边界角区域,相对于图5(a)、图5(b)的地图扩建的面积应该线性减少,面积计算见式(2).同理,图5(d)是配准点在中间区域时的地图扩建方式,直接从边相邻区域1,2,3,4补充,地图扩建面积的计算见式(2).

其中,β是地图视野缓冲长度,n是特征地图子区域的边长,inlier(i)表示第i配准点所在的子区域编号,layer(i)为第i配准点最靠近视野边界所在的层数,layers是视野中心到视野边界的层数,S1,S2,S3,S4为边界区域编号的集合,分别为:S1={2,3,···,n−1},S3={mn−m+1,mn−m+2,···,mn−1},S2={2n,3n,···,mn−n},S4={n+1,2n+1,···,(m−2)n+1},S5是第二层边界角区域编号的集合,即:S5={n+2,2n−1,mn−n−m+2,mn−n−1}.

针对b)情况,按照图5的特征地图扩建方法能够有效地补充正确视野下的特征点,然后再将这部分特征点与当前帧做匹配,加上之前匹配的配准点,就能够完成当前帧的特征点匹配.图6给出了各帧的视觉里程计的配准流程,其中pnp表示匹配的特征点数,λ是各帧配准点个数的阈值,curr代表当前帧,update是更新配准点所在的子区域编号.

2.2 基于帧到帧、帧到模型的g2o的特征地图的更新

本文构建的自适应特征地图与当前帧做视觉里程计,极大地缩短了匹配时间,但付出了匹配精度下降的代价,为了提高位姿估计精度,减少累积漂移的问题,本文引入Kummerle提出的g2o图优化算法,提出一种帧到帧、帧到模型的g2o优化特征地图和相机位姿的优化算法.

图6 视觉里程计过程Fig.6 The process of visual odometry

首先,通过改进的特征地图与当前帧的视觉里程计,计算出转换矩阵,见式(3)所示.

根据该转换矩阵,将当前帧与特征地图建立起对应关系,并定义当前帧与特征地图匹配的特征点为内点,未建立对应关系的特征点称为外点,因此特征地图点的更新分为内点更新和外点更新.

外点的更新则是将各帧中未建立关系的特征点利用式(4)和式(5)转换为世界坐标系下的三维空间点,然后直接导入特征地图中.

其中,[u,v]是当前帧中外点的像素坐标,d是Kinect测定的深度,K为相机内参,pc,pw是该外点对应相机坐标系和世界坐标系的坐标.

分析式(4)和式(5),在不考虑Kinect外在干扰的情况下,K、d可认为是常量,则该外点的世界坐标值只与当前帧的转换矩阵相关,即更新的外点的精度直接取决于当前帧的位姿估计.因此,本文通过图优化器g2o构建最小二乘问题,然后寻找最优的相机位姿,使它最小化,如式(6)所示.

其中,K为相机内参,ξ是相机位姿(R,t)对应的李代数,也称为g2o中的节点,ui是第i个外点的像素坐标,pi是第i个外点的世界坐标,表示反对称矩阵,g2o中每个3D点在相机中的投影,以观测方程来描述:zj=h(ξ,pj),具体的g2o优化示意图如图7所示.

图7 外点的g2o图优化Fig.7 g2o graph optimization of exterior points

对于内点的更新,通常的做法是与外点的更新一样,但是这样构建的g2o约束条件较少,优化的精度不明显.为此,本文将地图点作为优化变量考虑进来,为了提高g2o的优化精度,建立了当前帧与相邻的关键帧(为了方便叙述,称其为历史关键帧)以及特征地图点的约束关系,提出一种帧到帧、帧到模型的g2o优化模型,其基本思想将当前帧与历史关键帧建立匹配关系,因为历史关键帧中除了特征点还有各自的位姿信息,这样能够与当前帧建立更丰富的约束关系,有利于提高g2o的优化精度.

考虑到VSLAM的实时性要求,为了建立当前帧与历史关键帧的特征点对应关系,本文在特征地图点更新的过程中标记特征点的来源帧信息,这样在VO中,若当前帧与特征地图匹配成功,由于配准点具有标记的历史关键帧信息,即可建立当前帧与历史关键帧的特征点匹配关系,再构建当前帧与历史关键帧的优化问题,并通过g2o求解,具体过程为:

步骤1.建立投影关系(假设参考帧的相机位姿为单位矩阵);

其中,L表示当前帧,ref表示历史关键帧,即:ref=L−1,L−1,···,L−c,c为历史关键帧的数目,ξi,ξj分别表示第i和第j个相机位姿,∆ξi,j表示ξi和ξj之间的运动,dref,dL是对应像素的深度,pi是世界坐标,uref,uL是两个相机坐标下的像素坐标,K是相机内参.

步骤2.在图优化中,构建一个优化问题并利用图去求解.这里优化问题可以表示成对所有特征点的误差求和,构建最小二乘优化如下:

步骤3.构建内点g2o图优化,再加上外点g2o的位姿优化,本文的特征地图g2o优化如图8所示.

在本文构造的内外点更新优化模型中,由于考虑了当前帧与历史关键帧的匹配关系,增加了更多的约束,提高了g2o的优化精度,但是特征点在坐标转换中也会存在误差,因此当前帧与历史关键帧的匹配不宜过多,根据经验选取历史关键帧的数目c=1,2,3,优化效果较好.

图8 内外点的g2o图优化Fig.8 g2o graph optimization of internal and external points

3 实验与分析

所有实验均是在ubuntu14.04系统下完成,电脑配置如下:CPU(2.0GHZ Intel i7),运行内存8GB.实验数据分别来自于TUM RGB-D Benchmark数据集[24],实验主要做了三方面验证:1)不同算法的特征地图规模对比;2)验证了配准不满足时的特征地图补充方法的有效性;3)比较了RGBDSLAM2、FVO、文献[15]的算法、本文改进FVO算法(1)(未使用帧到帧、帧到模型的g2o优化)以及本文改进FVO算法(2)(包含帧到帧、帧到模型的g2o优化)对VSLAM 的实时性、特征地图累计规模、轨迹精度、建图的效果.另外,由于本文是对视觉里程计的改进,为了弱化后端对前端的优化,在全部的VSLAM 算法中保留较少的关键帧,设定关键帧的参数为:min_translation=0.25,min_rotation=0.25,以对比前端视觉里程计的改进效果.

3.1 改进的特征地图规模对比

特征地图的难点是维护它的规模,一旦地图规模太大,匹配会消耗大量时间.以fr1-room数据为例,图9对比了RGBD-SLAM2、FVO、文献[15]算法以及本文算法(区域特征有效因子θ=0.3)每帧匹配的特征地图规模以及累计地图规模.

由图9可知,相比于RGBD-SLAM-V2、文献[15]算法的地图数据量大、信息冗余以及部分帧特征规模波动较大的缺点,本文算法利用区域特征提取、子区域特征补充、特征地图扩建的方法,保证了地图规模稳定在(200～400)的较低区间,有效地实现了低规模地图的位姿估计.此外,从累计地图规模分析,本文算法的地图规模约占RGBD-SLAM-V2算法的1/5(文献[15]算法的1/3),减少了内存、缩短了匹配时间,满足了实时性要求.

3.2 配准不满足时特征地图补充方法的有效性验证

本文以小规模的特征地图配准,为了应对过程中可能出现配准点数较少的情形,本节验证此情形下特征地图补充方法的有效性.区域特征有效因子是算法中控制地图规模的参数,本节通过设定不同的来控制特征地图的规模,以fr1-room数据为例,图10给出了一组不同值的VSLAM的轨迹误差对比曲线.

分析图10可知,随着区域特征有效因子的降低、特征地图规模逐渐减小,造成位姿轨迹误差越大.分析改进前的轨迹误差曲线,可以发现θ从0.72开始,轨迹误差逐渐增大,至θ为0.32时,轨迹误差大于1m,而通过改进后的特征地图在θ为[1−0.23]范围内,轨迹误差都小于0.2m,避免了因θ取值较小造成的轨迹误差,有效地降低了特征地图的规模,在保证VO位姿精度的前提下减小了匹配时间.此外,不同复杂程度的场景中,最佳的θ值是不相等的,如果每次都通过大量实验寻找最佳θ值,不仅费时费力,而且也不满足实际的需要,而通过本文的特征地图补充方法,延长了θ值的有效区间,弱化了θ的取值.

3.3 VSLAM的实时性分析

实时性是VSLAM 中十分重要的性能指标,本文利用TUM RGB-D Benchmark[19]中的数据集fr1-xyz,fr1-360,fr1-room,fr2-large-noloop( flnp),fr1-desk,fr1-desk2,fr1-long-officehousehold验证本文算法的实时性优势,表1给出了RGBD-SLAM2、FVO、文献[15]算法、本文改进算法(1)(未使用特征地图优化)以及本文改进算法(2)(使用特征地图优化)的实时性和特征地图规模比较.其中,k表示特征地图的累计特征点个数.

图9 特征地图规模比较Fig.9 Cumulative scale of feature map points

表1 不同算法的实时性、特征地图累计规模比较Table 1 Comparison of real time and feature map cumulative size of each algorithm

图10 地图改进前后的轨迹误差曲线Fig.10 Trajectory error curve before and after improvement

经分析,RGBD-SLAM2是帧与多个参考帧的视觉里程计模型,由于需要多次匹配,计算复杂、耗费资源;FVO是帧与模型的位姿估计,只需要一次匹配,缩短了时间,但是易累积漂移;文献[15]算法是以FVO为基础,通过g2o优化得到了与实际一致的位姿估计,但是构建的特征地图仍然存在冗余特征点,而本文算法自适应维护特征地图规模,可以发现,无论是否对特征地图g2o优化,本文算法在帧到特征地图的配准中,相比于其他算法都是运算时间最快、需要的地图特征点的规模是最小的,尤其在数据量大的 flnp场景中,本文算法的实时性更加突出.

3.4 VSLAM的建图和定位精度分析

以fr1-room数据为例,图11是上述算法构建的3D点云地图,对比发现:利用RGBD-SLAM2、文献[15]算法构建的3D点云地图存在冗余点和漂移点,图中桌子、电脑、椅子以及门轮廓都不是很清楚,特别是在图右边部分,由于关键帧的参数设置较高,此处的关键帧数量较少,图11(a)、图11(b)中右边白色边框的电脑无法构建,相比于图11(a)、图11(b),本文算法在关键帧参数设置相同条件下,构建的点云图的冗余点和漂移点则明显要少一些,且图中电脑、桌子、椅子等轮廓更加清楚,右边部分白色边框的电脑也能正常显现.图11(d)是由帧到帧、帧到模型的g2o优化后的本文改进算法,相比于图11(c),图11(d)中漂移点和冗余点更少,特别是在墙壁窗台处的围栏能够更好地重合,没有重影.因此,由本文改进的算法相比于其他算法,在3D点云地图的构建具有更好的精度.

图12是不同算法计算的轨迹对比图,其中Ground truth是TUM计算机视觉组利用路径捕捉系统得到的fr1-room场景的摄像机运动轨迹,可以认为是真实路径,Estimated是利用RGBDSLAM-V2、文献[15]算法、本文改进算法(未使用特征地图优化)以及本文改进算法(使用特征地图优化)计算得到的运动轨迹(设定关键帧的参数为:min translation=0.05,minrotation=0.05,Difference是Ground_truth与Estimate轨迹的误差.对比可知:相对于图12(a)、图12(b)点云图,由本文算法构建的图12(c)和图12(d)的轨迹误差Difference区域更小,而相对于图12(c),图12(d)使用了帧到帧、帧到模型的g2o特征地图优化,该模型在求解较大旋转和平移的位姿中更加准确,如图中黑色标记处.此外,表2给出了不同算法在不同数据集下的位姿估计精度,其中E表示与真实轨迹的绝对误差(Absolute trajectory error,ATE).

表2 不同算法的轨迹误差对比Table 2 Comparison of trajectory errors of different algorithms

图11 各算法构建的3D点云地图对比Fig.11 Comparison of 3D point cloud maps based on different algorithms

图12 不同算法的轨迹对比Fig.12 Trajectory comparison of different algorithms

由表2知,RGBD-SLAM-V2的帧到帧模型配准,位姿精度较高,但消耗了大量的计算资源;FVO的帧与地图的配准虽然提高了位姿估计效率,但是位姿估计精度较低、容易长期累积漂移;文献[15]在FVO的基础上,通过g2o优化得到了精度更高的位姿,但是构建的特征地图仍然存在特征点冗余的问题,本文算法一方面通过子区域分块、特征点补充与地图扩建的方式自适应维护地图规模,使得帧到特征地图的位姿估计兼顾了实时性和精度,另一方面提出了帧到帧、帧到模型的g2o特征地图更新方式,进一步提高了位姿估计的精度,消除了累计误差.经表2对比可知,本文算法在有无特征地图优化的情况下,都具有较好的精度和稳定性,而且在配准速度上,本文算法要明显好于RGBD-SLAM2、文献[15]的算法,更有利于用户的实时体验.

3.5 失效分析

虽然所提算法在上述测试集中表现较好,但也存在测试集失效的情况.例如比较困难的TUM中freiburg2-slam数据集,该视频总长40.07m.图13是利用本文算法测试该数据集的轨迹误差图和3D点云地图.

可以发现,测试结果与实际轨迹相比误差较大,平均误差值为0.4m,特别是在场景中急转弯的部分,特征点提取、匹配十分困难,造成VO中出现多数误匹配.本文虽然针对此类场景出现特征匹配不足的情形,会依据仅有的匹配成功的特征点,通过地图子区域以及地图扩建的方式补偿特征点匹配,但是在freiburg2-slam数据集中补偿效果不明显.分析原因是初次匹配的特征点存在误匹配,由于地图子区域特征点补充和特征地图扩建都是依赖于初次匹配的特征点对,如果初次匹配点存在误差,则本文改进方法亦效果不佳.由此可见,首次的区域地图特征点提取尤为重要.在算法中,首次地图构建提取的是ORB特征中角点响应强度大的部分,这些特征点用于VO匹配效果较好,但是对于特征缺失的环境或有快速运动和转弯出现的情形应用性不强,TUM中freiburg2-slam的数据集就是这样的环境.针对此类场景,为了进一步提升所提算法的鲁棒性,下一步将尝试利用多种评估特征方法融合更高效的特征点.

4 结论

VSLAM算法要求实时精确估计机器人的位姿,本文针对现有视觉里程计方法位姿估计精度不够,或者对硬件要求较高的缺陷,提出了一种自适应特征地图配准的VSLAM算法.首先对特征地图划分子区域,利用角点响应强度提取不同子区域中高效的特征点,解决特征地图计算量大,耗费计算资源的问题.其次,对于出现的配准失败的情形,提出一种区域特征点补充和特征地图扩建的算法,实现该情形下对应帧的快速再次匹配.最后,在VO中引入g2o图优化以更新特征地图点,并提出一种帧到帧、帧到模型的g2o特征地图优化模型,实现了对内点和外点的同时更新,提高了位姿估计的精度.通用数据集实验表明,本文提出的VSLAM 算法,在保证实时性前提下,具有较好的定位精度和建图能力.

图13 freiburg2-slam数据集的轨迹误差图和3D点云地图Fig.13 Trajectory error map and 3D point cloud map for freiburg2-slam data sets