基于激光雷达与双目视觉的移动机器人SLAM研究*

王消为,贺利乐,赵 涛

(西安建筑科技大学机电工程学院,西安 710055)

同步定位与地图构建SLAM(Simultaneous Localization and Mapping)是移动机器人研究领域的热点和难点,是移动机器人在未知环境中实现自主导航的前提。SLAM是指移动机器人在未知环境移动过程中通过自身携带的传感器获得环境信息增量式建立环境地图,同时利用已建立的地图来更新自身的位置与位姿。同步定位与地图构建过程中位置和地图的估算形成了一个相辅相成、不断迭代的过程,其本质是一个状态估计问题,主要通过基于滤波器或基于平滑的方法进行求解。文献[1-4]中采用Doucet等学者提出基于Rao-Blackwellized粒子滤波器RBPF(Rao-Black wellized Particle Filters)的SLAM方法,此算法能够较好的近似移动机器人位姿和环境地图的联合概率密度,但其存在计算复杂度较高、占用较大内存实时性较差等问题,因此之后很多学者针对该算法存在的问题进行了多种不同形式的改进;文献[5-6]中对激光雷达与单目视觉相融合的SLAM进行了研究,将移动机器人所处环境中的某些相关特征进行联合形成有意义的环境地标,虽然有效减少数据的不确定性,但其计算量大实时性差;文献[7]中提出了一种基于退火参数优化混合提议分布的RBPF算法,该算法不仅能够减少所需粒子数而且保持了粒子多样性,但实验验证其系统鲁棒性较低;文献[8]中采用了在计算提议分布时将里程计信息与激光采集的距离信息进行融合,验证了其可以有效减少了所需粒子数并降低了预测阶段机器人位姿的不确定性,但由于激光数据比较单一其实验效果仍然不佳,因此本文提出在计算提议分布时将移动机器人所有的观测数据与里程计信息融合。

针对上述在对未知环境中移动机器人研究中利用单一传感器,自主导航时出现不能及时定位、构建地图不精确等问题,本文提出在计算提议分布时将移动机器人所有的观测数据与里程计信息融合,并重点进行基于激光雷达与双目视觉信息融合的移动机器人同步定位与地图构建研究。

1 移动机器人的同步定位与地图构建

1.1 SLAM问题描述

SLAM问题指的是仅依赖观测信息zt和控制信息ut的情况下确定机器人的位姿并构建出地图,即在未知环境中移动递增式构建环境地图,同时利用构建的环境地图对自身的位姿进行估计。SLAM问题示意图如图1所示。

图1 SLAM问题示意图

图1中实心圆表示移动机器人真实的信息情况,空心圆表示估计信息情况,箭头表示估计信息情况;xt表示t时刻的实际位姿向量,则x1:t表示机器人的轨迹;zt,k表示t时刻对第k个路标的观测,则z1:t表示观测信息;ut表示移动机器人的控制信息,使其从xt-1到达位姿xt,则u1:t表示控制的历史信息;mk表示第k个路标的位置状态向量,则m表示地图。

因为移动机器人SLAM问题具有条件独立性,且自身定位过程满足一阶隐马尔科夫特性,所以当移动机器人的轨迹完全确定时,对各个路标的观测是相互独立的。则通过Rao-Blackwellise方法将SLAM表示为[1]:

(1)

Rao-Blackwellized SIR滤波器地图构建过程中增量式的处理获得的数据作为提议分布q,实现SLAM的步骤如下:

(2)

④重采样:当有效粒子数小于预先设置的阈值Neff时,根据粒子权值的大小,用权值大的粒子代替权值小的粒子,进行重新采样。重采样后,所有粒子具有相同的权重。

而Doucet等[1-2]人提出限制提议分布q获得一个递归公式来计算重要性权重

q(x1:t|z1:t,u1:t-1)=q(xt|x1:t-1,z1:t,u1:t-1)·
q(x1:t-1|z1:t-1,u1:t-2)

(3)

则相应的计算权重公式为:

(4)

1.2 改进的RBPF-SLAM算法

1.2.1 提议分布改进

由于里程计运动模型噪声比较大,而一般改进RBPF-SLAM一般将里程计运动模型作为提议分布,因此会使得各粒子间的权值差别很大,导致粒子出现乏匮现象。因此,提出基于改进提议分布(即在计算提议分布时将所有观测数据和里程计信息融合)的改进RBPF算法。即将观测传感器(激光雷达与双目视觉当前观测信息)提取出来的融合信息zt融入到粒子提议分布中,以有效降低粒子匮乏问题的发生。采样提议分布如下:

(5)

则计算重要性权重公式为:

(6)

1.2.2 基于改进的RBPF-SLAM算法步骤

③根据式(5)求取提议分布,在提议分布中进行粒子采样;

④根据式(6)进行计算并更新粒子权重,计算有效粒子数Neff判断是否达到阈值,如未达到则进行重新采样;

2 特征处理及数据融合

考虑到移动机器人在运动过程中容易出现轮子打滑等问题导致里程计数据与实际运动距离不一致,二维激光雷达只能获得在一定高度的某一平面上信息;视觉易受环境背景变化等条件影响。故提出应用上述1.2节中改进RBPF-SLAM算法进行采用双目视觉与激光雷达结合里程计数据信息融合,进而实现移动机器人的自主导航。

图2 特征点提取

2.1 基于ORB的特征提取与匹配

Ethan Rublee等人提出的ORB(Oriented Fast and rotated Brief)算法[9]是基于FAST特征检测和BRIEF特征描述算子的新算子,此算法用灰度质心法给FAST角点检测基础上加上方向,用旋转矩阵和机器学习的方式给BRIEF加上方向特性,并通过建立图像金字塔的方式加上尺度特性。算法的特征提取速度相对于SURF、SIFT等算法有非常大的提高,其主要步骤分为特征点检测及特征点描述。图像2为移动机器人所在实验室环境下双目视觉传感器获得的图像中的ORB特征点提取效果,匹配效果如图3所示。

图3 特征点提取匹配

将上述获得的特征级匹配点作为标记路标,随机选取标记路标点作为代表如图2所示A、B、C、D,建立起包含标记路标点坐标信息的路标数据库,从图像中提取出的三维坐标转化至水平面上,并构建出水平面上2D路标地图,选取的4个路标点的坐标如表1所示。

表1 选取的标记路标点(图2中)三维坐标

2.2 数据融合及步骤

基于多传感器融合及组合导航的方法已经在很多方面得到验证及广泛的应用[10],来自不同传感器的数据信息不仅可以提供冗余信息,而且能提高环境特征定位的精确性和特征识别的可靠性。

2.2.1 数据融合

移动机器人不同传感器所感知获得的环境特征要经过融合之后才能加入到全局地图中[11],因此要对双目视觉系统获得的数据信息ZV状态向量与激光测距仪获得的数据信息ZL状态向量进行处理,首先对这些数据信息进行对应于环境中同一特征匹配对(即数据关联),然后进行特征层次的匹配融合。其过程简述如下:

从以上可以获得其协方差矩阵:

2.2.1 数据融合步骤

①对激光测距数据进行分析,得到环境中障碍物的距离信息;

②用ORB算法从双目视觉传感器中获得的两幅不同视角的图像中寻找特征点,匹配并得到旋转角度,将得到的匹配角度与里程计角度融合,使其更加接近实际角度;

③结合激光测距与双目视觉传感器,分别融合距离信息与旋转角度信息,定位移动机器人的初始位置;

④根据激光雷达与双目视觉传感器获得的路标信息,构建特征地图;

⑤最后采用上述改进的RBPF算法进行移动机器人的自主导航。

图4 履带式移动机器人

3 实验及分析

3.1 实验平台及环境

本文实验在一台履带式移动机器人(如图4)进行,移动机器人带有HOKUYO二维激光测距仪URG-04LX、双目摄像机系统Bumblebee2、里程计及一个串口连接的控制计算机等附件,控制计算机为英特尔奔腾双核、主频2.93 GHz、内存4 G。控制计算机上的操作系统为Ubuntu14.04 LTS版本,且其系统上安装了Indigo版本的ROS,通讯采用54 M带宽的802.11 g无线通讯协议,通过与另外一台安装有Ubuntu14.04操作系统及Indigo版本的ROS的笔记本电脑(PC)建立通信,进行远程连接并控制机器人的运动,并用Rviz显示出所构建的地图。

实验环境为学校附体楼研究生实验室及外部走廊作为实验环境。

3.2 ROS及导航实现流程

ROS是一种开源的机器人操作系统或者说是次级操作系统[13],它能提供类似操作系统所提供的功能,如硬件抽象描述、底层驱动程序管理、共用功能的执行、程序间的消息传递等,它也提供一些工具程序和库用于获取、建立、编写和运行多机整合的程序,其具有点对点设计、不依赖编程语言、开源等优点。本文实验研究时正是充分利用了ROS的这些优点及其分布式的处理框架。移动机器人导航实现的流程如下:

①使远程控制计算机和移动机器人控制计算机连接到同一个WIFI网络下;

②在远程控制计算机上建立节点管理器、SLAM、路径规划等节点;

③在移动机器人控制计算机上创建各个传感器采集的数据信息及基础控制器等节点;

④把以上创建的所有节点在节点管理器中进行注册并由节点管理器统一管理,即实现了在同一网络中通过端对端的拓扑结构进行不同计算机上节点间的有效通信;

⑤最后可利用远程计算机上的3D可视化工具RVIZ实时显示移动机器人自主导航的状态信息。

3.3 实验结果及分析

分别使用上述改进的RBPF算法与文献[7]提出的一般改进RBPF算法在实验室内及实验室外部走廊环境下进行移动机器人的自主导航实验,且每个环境下分别重复进行10次实验,实验数据如表2所示。移动机器人的移动速度设置为0.5 m/s,通过远程计算机(PC)机上的ROS中可视化工具RVIZ可以实时显示地图创建的过程如图5所示。

表2 创建一致性地图时参数表

图5 RVIZ显示的实时地图

图6为在实验室内环境下采用多传感器融合的一般改进RBPF算法48个粒子构建的2D栅格环境地图,由于其算法仅使用里程计作为提议分布,因此随着时间的推移里程计累计误差越来越大,所构建出来的地图出现了不一致现象。图7为在实验室内使用上述多传感器融合改进的RBPF算法15个粒子构建出精确的2D栅格环境地图。图8、图9为在实验室外部走廊环境下采用多传感器融合的一般改进RBPF算法与上述多传感器融合改进的RBPF算法构建出的2D栅格环境地图。

图8 一般改进算法构建的地图

图6 一般改进算法构建的地图

图7 本文改进算法构建的地图

图9 本文改进算法构建的地图

从表2明显可以看出一般改进RBPF算法与本文改进RBPF算法创建程度一致性地图时,本文改进RBPF算法需要的粒子数目远小于一般改进RBPF算法,而且其构建地图的过程中所用的时间也得到了大大的缩短,有效提高了算法的运算效率。从上面一般改进RBPF算法与本文改进RBPF算法所构建的地图中可以看出一般改进算法随着时间的推移,构建出的地图会出现重复定位不一致的现象,但采用本文改进RBPF算法时图中可以看出构建的地图相对稳定、精确。

在移动机器人导航实时性可以容忍的情况下,一般改进算法与改进算法分别选取6,8,12,18,26,32,38,48,52,68;10组粒子数在实验室内环境下进行实验,每个条件下进行α=20次,创建地图成功的次数为β得出相应粒子数下地图构建的成功率s,则计算公式为:s=(β/α)×100%,构建地图粒子数与成功率的关系如图10所示。

图10 构建地图成功率与粒子数的关系曲线

从图10中对比本文改进算法与一般改进算法构建地图粒子数与成功率的关系可以观察到,本文改进算法构建出一个高精度的栅格地图仅需12个粒子数,而一般改进算法需要的粒子数则多达32个;当移动机器人在同一个环境下,使用同样多的粒子数创建环境地图时,本文改进算法的成功率明显高于一般改进算法,有效提高了移动机器人SLAM的鲁棒性。

为了进一步地分析基于改进RBPF算法组合导航的优势,通过对改进算法与一般改进算法在MATLAB中仿真位姿状态估计精确性分析得到的对比曲线如图11所示,明显可以观察到本文改进算法的曲线趋势相对平滑,即位姿估计的均方根误差小于一般改进算法。因此,本文改进算法的状态估计精度优于一般改进算法。这正是因为文章在提议分布中加入了激光雷达与双目视觉的融合信息,使得粒子的分布更加的接近其真实状态。

图11 状态估计误差曲线

4 结论

①通过实验验证表明采用文章提出的改进RBPF算法,在计算提议分布时将移动机器人的所有观测数据与里程计信息融合,有效减少了所需粒子的数量,且降低了粒子滤波器预测阶段移动机器人的不确定性;

②通过本文提出一种双目视觉与激光雷达信息融合的组合导航方法,建立包含世界坐标系的路标数据库,并获得全局环境下的特征地图进而实现自主导航。验证了多传感器融合提供的冗余信息能够构建可靠性更高、更精确的2D栅格环境地图,能够有效提高移动机器人SLAM的鲁棒性。

[1] Montemerlo M,Thrun S,Koller D,et al. FastSLAM 2.0:An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping That Provably Converges[C]//Menlo Park CA,USA,AAAI Press 2003,1151-1156.

[2] Doucet A,Freitas N,GORDAN. Sequential Monte Carlo Methods in Practice[J]. New York:Springer Verlag,2001:496-497.

[3] 周绍磊,吴修振,刘刚,等. 一种单目视觉ORB-SLAM/INS组合导航方法[J]. 中国惯性技术学报,2016,24(5):633-637.

[4] 朱磊,樊继壮,赵杰,等. 改进粒子滤波器的移动机器人同步定位与地图构建方法[J]. 重庆大学学报,2014,37(4):39-45.

[5] Castellanos J A,Neira J,Tardos J D. Multisensor Fusion for Simultaneous Localization and Mapping[J]. IEEE Trans Robot Automat,2001,17(6):908-914.

[6] 庄严,王伟,王珂. 移动机器人基于激光测距和单目视觉的室内同步定位与地图构建[J]. 自动化学报,2005,31(6):925-933.

[7] 罗元,苏琴,张毅,等. 基于优化RBPF的同步定位与地图构建[J]. 华中科技大学学报,2016,44(5):30-34.

[8] 罗元,傅有力,程铁凤. 基于改进Rao-Blackwellized粒子滤波器的同步定位与地图构建[J]. 控制理论与应用,2015,32(2):267-272.

[9] Rublee E,Rabaud V,Konolige K,et al. ORB:An Efficient Alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision(ICCV). Barcelona,Spain,2011:2564-2571.

[10] 张毅,杜凡宇,罗元,等. 一种融合激光和深度视觉传感器的SLAM地图创建方法[J]. 计算机应用研究,2016,33(10):2970-2972.

[11] 陈晓飞,凌有铸,陈孟元. WSNs环境下基于高斯混合容积卡尔曼滤波的移动机器人定位算法[J]. 传感技术学报,2017,30(1):133-138.

[12] 许曈,凌有铸,陈孟元,等. 基于姿态测量模块和闭环检测算法的仿生SLAM研究[J]. 传感技术学报,2017,30(6):916-921.

[13] 张锦涛,张瑞雷,译. ROS机器人程序设计[M]. 北京:机械工业出版社,2016.