双目相机和激光雷达的融合SLAM研究

刘鸿勋,王 伟

(南京信息工程大学自动化学院,江苏 南京 210006)

SLAM问题可以描述为机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和地图进行自身定位,同时在自身定位的基础上建造增量式地图,实现机器人的自主定位和导航. 此课题在学术界已经有二三十年的历史,根据获取环境信息的传感器不同可以分为视觉SLAM、激光SLAM两个大类. 视觉SLAM又分为单目、双目、深度(RGBD)相机3种. 激光雷达主流的有单线激光、16、32、64、128线激光等,价格基本呈指数增长.

激光雷达SLAM算法是目前产业界落地使用最多、实际运行表现最稳定的算法,主要分为两种,一种是传统的以扩展卡尔曼、粒子滤波为代表的贝叶斯滤波方法[1],另一种是基于图优化的全局优化处理方法[2]. 由于近年来对SLAM算法的深入研究,其应用场景逐步向大规模环境发展. 图优化方法由于支持全局优化、方便消除累积误差的特点,渐渐成为主流. 但是单线激光雷达获取的点云数据只是在单一的平面内,近些年来,经过研究改进,基于图优化的激光SLAM算法中的闭环检测有ICP、PL_ICP[3]、NDT等方法,这些方法在结构化、重复度较高的场景容易失效,导致全局优化失败或得到错误结果.

本文从激光SLAM实际应用场景出发,研究了视觉双目相机提取特征及匹配算法[4],采用视觉回环检测,用双目相机的图像提取出改进的FAST特征点[5],并使用BOW方法进行特征点匹配及闭环的检测,配合基于图优化方法的激光SLAM算法,有效提高了激光SLAM的回环准确率.

1 双目—激光融合SLAM框架

1.1 运动和观测

把离散时刻t=1,2,…,k机器人的位置记为X={x1,x2,…xk}. 路标点集合Y={y1,y2,…,yk},当机器人在xk位置上观测到某个路标点yj,就产生了一个观测数据,得到运动方程和观测方程分别为:

(1)

(2)

对于观测方程,激光雷达观测一个路标点时,测量到的有两个量:路标点与机器人之间的距离r和夹角φ,记路标点为y=[px,py]T,观测数据为Z=[r,φ]T,激光雷达观测方程的形式为:

(3)

对于相机,使用小孔成像模型,记路标点坐标为y=[px,py,pz]T,观测数据为Z=[u,v]T,则观测方程的形式为:

(4)

其中z为路标点与相机成像平面的垂直距离,u、v为路标点在图像中的像素坐标,fx,fy,cx,cy是相机内参,可通过相机标定.

1.2 非线性优化求解机器人状态估计

上文建立了一个机器人SLAM系统的模型:已知从零时刻开始的机器人运动(imu、里程计)和测量传感器(激光雷达、相机)的读数,求解定位问题和建图问题,图1是系统状态的描述图.

图1 机器人SLAM系统状态Fig.1 State of robot SLAM system

使用非线性优化的方法求解这个状态估计问题,设从1到n所有时刻,有m个路标点.

x={x1,…,xn},y={y1,…,ym}.

那么这个状态估计就是已知输入数据u和观测数据Z的条件下计算状态x,y的条件概率分布:

P(x,y|Z,u).

利用贝叶斯法则,有:

(5)

式中,P(Z|u)与待估计的状态x,y无关,可忽略. 左侧为后验概率,正比于右侧似然和先验的乘积. 无需求解完整的后验分布,只求一个最优的状态,使该状态下后验概率最大化,

(6)

参考上文中的观测模型,对于某次观测Zk,j=h(yj,xk)+vk,j,假设噪声项vk～N(0,Qk,j),则观测数据的条件概率为P(Zk,j|xk,yj)=N(h(yj,xk),Qk,j),它是一个高斯分布. 取其负对数,

(7)

第一项与x无关可以省略,代入观测方程,相当于求

(8)

定义各次输入和观测数据与模型之间的误差:

eu,k=xk-f(xk-1,uk),
ez,k,j=Zk,j-h(xk,yj).

(9)

最小化所有时刻的误差等价于求最大似然估计,转化为一个最小二乘问题,

(10)

式中,eu,k是运动误差,只与xk和xk-1有关,eZ,k,j是观测误差,只与xk,yj有关.

2 双目相机前端处理

由式(4)可知,单个相机的观测方程中缺少了图像点的深度信息Z,现使用双目相机和改进的FAST角点检测匹配算法进行深度信息恢复,并选择关键帧,为后端闭环检测做准备.

2.1 改进的FAST角点检测算法

FAST特征点的原理:检测图像中任意一个像素Pix,它的亮度为LPix,以此像素为中心,选取半径为3的圆上的16个像素点,如果有连续11个点的亮度大于LPix*1.2或小于LPix*0.8,那么认为像素Pix是一个特征点,如图2所示.

图2 FAST特征点Fig.2 FAST feature points

为了比较各个特征点提取算法的性能,对SIFT、SURF、ORB、FAST特征点提取算法在intel-i5-8250U下用5张测试图片提取特征点数量和用时.

由表1可见,FAST算法可提取大量图像特征点,且检测速率比SIFT提高了两个数量级. 因此,本研究采用FAST角点作为特征点检测算法.SIFT、SURF、ORB、FAST特征点提取用时分别为5.42 s、4.35 s、0.24 s、0.06 s.

表1 4种特征点提取数量Table 1 Four kinds of feature points extraction time 个

但是机器人在运动过程中的视角变化较大,FAST角点过于简单,需要添加旋转不变性. 在Pix点的邻域S内计算灰度质心,定义邻域S的矩为:

(11)

式中,I(u,v)为像素坐标(u,v)点的灰度值,(u,v)∈S,令p,q分别为0和1,则质心位置:

(12)

定义FAST特征点的方向为:

A=(atan 2(M01,M10)).

(13)

2.2 特征描述子及双目特征匹配

使用BRIEF-128描述子,选取特征点附近的128对像素点,选取方式如图3所示.

图3 特征点周围的128个点对Fig.3 128 pairs of points near feature point

则旋转后的点集SA=RAS,特征描述子为:

(14)

使用Hamming距离判定它们的匹配程度,我们认为当前特征点与最邻近点的Hamming距离越小且与次临近点的Hamming距离越大,匹配的效果越好. 对双目相机的图像分别做特征提取和描述子计算,经过匹配可以得到一组从两个角度同时拍摄到的图像点,如图4所示.

图4 特征点匹配结果Fig.4 Feature points match

2.3 双目相机模型和特征点深度计算

经过特征匹配,得到物理世界中B点在两个相机图像中的投影点PL和PR,左眼相机和右眼相机的中心距离为基线的长度b,相机焦距为f,建立如图5所示的双目相机几何模型.

图5中,z为B点与相机的垂直距离,是我们想要恢复的深度信息,uL为PL的像素横坐标,uR为PR的像素横坐标. 由相似三角形原理可得:

图5 双目相机视差模型Fig.5 Binocular camera parallax model

(15)

整理可得:

(16)

以左目相机中心为原点,建立相机坐标系O-x-y-z,如图6、图7所示. 设B点在相机坐标系中的坐标为(xC,yC,zC),此时B点的z坐标、在左目图像中的像素坐标(uL,vL)均已知,由右图中的相似三角形关系可得:

图6 相机坐标系Fig.6 Camera coordinate system

图7 相机坐标系俯视图Fig.7 Top view of camera coordinate system

整理可得:

(17)

结合式(16)和式(17),可以计算出所有特征点在相机坐标系下的三维坐标,每一组图像算出的特征点作为一帧点云数据. 同时,为了保证运算速度和节省内存资源,根据以下几个原则选取关键帧进行保存:

(1)当前帧至少提取了50个有效的特征点.

(2)当前帧中的特征点与上一帧关键帧相同点的数量少于90%.

(3)局部建图处于空闲状态,或者超过20帧图像未保存关键帧.

3 地图构建和全局优化

3.1 激光雷达点云帧间匹配

激光雷达点云数据H={hk}.k=1,…,K,hk∈R2. 设激光雷达的位姿为Tξ,则点云中每个数据点BL的全局坐标为:

(18)

构建一个概率网格G:rZ×rZ→[pmin,pmax],如下图所示,这些分立的网格点可以设置分辨率大小,比如我们设置为5 cm. 每个网格点的数值代表它是障碍物点的概率,我们定义每个网格点包含了它所在的方形区域内所有的点,如图8所示.

图8 概率网格Fig.8 Probability grid

为了提高帧间匹配的稳定性,我们把连续的多帧点云数据累积起来,形成“局部地图”,然后用点云与这个“局部地图”进行匹配. 设置两个点集“hit”和“miss”(如图9所示),同时设置初始概率phit和pmiss,插入点云时,把点云所在的网格点记为“hit”,把点云与机器人之间连线的所有网格点记为“miss”,如果某网格点是第一次被观测到,就把它的概率Pm相应地赋值为phit或pmiss,如果以前已经观测到过,则以下列公式更新它的概率值:

图9 miss和hit的标记规则Fig.9 Rule of miss and hit

(19)

Gnew(x)=clamp(odds-1(odds(Gold(x))·odds(phit))).

(20)

式(10)是计算最优插入位置ξ的最小二乘问题:

(21)

式中,Tξhk表示把点云坐标转换到局部地图坐标系的过程,Msmooth函数是更新概率值函数的平滑版本,使用ceres和双三次插值方法. 实验结果显示,概率值计算结果有可能超过1,但是由于求最小二乘的过程不受全概率理论约束,故不会影响优化结果. 求解这个最小二乘问题的方法可以参考文献[6-7].

3.2 闭环检测

图像闭环检测使用视觉词袋[8]度量两幅图像的相似程度. 经离线采集大量图像,提取出N个特征点,使用K-means方法将它们归成k个单词的词典:

(1)随机选取k个中心点:c1,…ck.

(2)计算每个样本与每个中心点的距离,取最小的作为它的归类.

(3)重新计算每个类的中心点.

(4)如果每个中心点都变化很小,则算法收敛结束,否则返回第二步.

使用TF-IDF模型[9-10]计算相似度,在建立字典时计算IDF,统计某个节点wi中特征点数量占所有特征点数量的比例. 设一共有n个特征,ni个wi,该单词的I为:

(22)

设图像A中单词wi出现了li次,而单词一共出现了l次,F即为:

Fi=li/l.

(23)

则wi的权重等于F与IDF的乘积:

ηi=Fi×Ii.

(24)

用一个向量vA描述图像A:

(25)

参考文献[11]中的L1范数形式,描述两个向量vA,vB的差异程度:

(26)

3.3 全局优化

经过闭环检测后,判断检测结果,比如分数大于70(总分100),则认为找到了足够相似的图像匹配,然后把这个匹配结果作为一个全局“约束”加入到优化问题中,根据第2章节中的问题模型,将其中的观测误差具体化为:

(27)

式中,为了保证残差最小化时的收敛速度和稳定性,使用Huber Loss函数ρ对观测误差进行封装,Im是关键帧A在全局坐标系下的位姿,Is是与A建立闭环的关键帧B在全局坐标系下的位姿,它们之间的约束通过相对位姿ξij和协方差Σij来描述,对于一组关键帧i和j,相对位姿ξij描述的是分别采集到该帧的时刻机器人所处的位置,这个位置由3.1中激光雷达的连续的帧间匹配给出,协方差Σij的由闭环检测算法给出.E函数是残差函数,在这个约束问题中具体形式为:

(28)

(29)

4 实验

实验部分,使用双目相机和一台peppfuches扫描范围为30m的单线激光雷达,将相机和激光雷达检测到的闭环约束及优化效果对比,然后给出约束的数量和准确率,并验证最终的建图效果.

4.1 激光雷达的局部定位和建图

首先是点云与局部地图的匹配,这个过程在SLAM问题中被称为“前端”,衡量它的精度方法与里程计的评估方法相同,表2是采用5cm分辨率局部地图,通过扫描匹配得到的局部运动误差. 可见,前端匹配精度在0.7%左右,也就是机器人运动100m约产生0.7m的误差,图10是两张实验场地内构建的局部地图.

表2 局部误差Table 2 Local error

图10 局部地图Fig.10 Submap

4.2 视觉与激光闭环检测对比

目前学术界流行的使用激光雷达进行闭环检测的方法是Real-TimeCorrelativeScanMatching[12],图11是在实验场景中的闭环检测效果,蓝色线连接的是使用激光雷达scan-match匹配的相似度较高的位置,它们明显不是相同位置,而是出现了误检测,从而导致地图出现偏差,如图12所示,使用图像进行闭环检测的建图结果如图13所示.

图11 错误的闭环约束Fig.11 Wrong loop closure

图13 视觉闭环的地图Fig.13 Map by visual-loop closure

图12 激光闭环的地图Fig.12 Map by laser-loop closure

表3是在不同场景中分别使用激光和视觉闭环检测测试出的约束数量及准确率. 可见,使用视觉进行检测闭环约束的方法明显提高了准确率,在大部分场景下可保证99%以上,即使出现了一两个错误的闭环约束,由于损失函数的加入,最小化误差算法仍然能给出正确的优化结果,图14是使用视觉闭环构建的完整的实验场地地图.

表3 全局约束Table 3 Global constrains

图14 完整地图Fig.14 Full map

5 结论

激光雷达传感器的优势是工作稳定,对环境、光照影响不敏感,但是数据量小,感知能力有限;而相机的特点则是数据量非常大,双目相机又可以解决图像深度问题. 使用双目相机替代激光的回环检测很好地解决了平面内相似度较高的场景下的误匹配问题.