基于激光雷达和Kinect信息融合的导盲机器人SLAM研究

刘 志,陈 超

(江苏科技大学 机械工程学院,镇江 212003)

一般来说大多数盲人和视力障碍者使用的导盲工具基本为功能简单的手杖,当今社会发展迅速,周围的环境变得越来越复杂,简单的手杖已经不能适应变化如此之快的环境.以导盲犬为代表的生物导盲方式受高训练成本、长训练周期和寿命等影响始终无法普及.由于智能化机械越来越成熟,性能优异的导盲机器人是解决盲人生活困难问题的最优解．

在导盲机器人自定位与自动导引研究方面,文献[1]利用超声波和红外传感器结合模糊PID控制理论实现导盲机器人的寻迹导航功能,但是缺乏实用性,只能以局部环境避障作为工作主体．文献[2]采用天线射频识别(radio frequency identifcation,RFID)技术完成导盲机器人自主定位,但该方法需要在房间内序言布置好射频标签等硬件．文献[3]利用Kinect深度相机进行定位和地图构建,实现导盲机器人的蔽障和路径规划,但其构建的地图是直接将Kinect获取的三维模型进行平面投影所得,与真实环境相差较大．可见传统的导盲机器人难以实现对复杂环境精确而有效的认知,且无法完成全局定位和导航,在人机交互方面上也存在较为明显的不足．在数据融合地图构建(simultaneous localization and mapping，SLAM)方面国内的相关研究还很少,文献[4]使用Kinect和激光雷达进行数据融合,优化所得地图;文献[5]对飞行时间技术(time of flight,TOF)相机和Kinect进行融合,获得更精细的点云,构建更准确的三维地图;文献[6]使用声纳和激光雷达进行数据融合,使得机器人在低能见度下进行SLAM．

为解决上述研究中存在的不足,针对导盲机器人的功能特点,提出将激光雷达和深度传感器进行数据融合的构建导盲机器人SLAM方法．首先通过对比现阶段在机器人操作系统(robot operating system，ROS)中常用的二维SLAM方法来建立导盲机器人激光雷达的二维地图；然后通过Kinect建立环境的三维地图,结合Kinect生成的三维环境模型地图数据信息丰富但数据精度欠缺的特点和激光雷达构建的二维地图进行拼接,弥补激光雷达构建二维地图数据精确但相对来说数据信息较少的缺点,得到融合二者优点的三维空间障碍物模型地图；最后以Turtlebot2为基础搭建实验平台,在真实环境中对所提方法进行实验验证和比较．

1 激光雷达SLAM

SLAM方法可大致分为两大类：基于滤波器的方法,以及基于图优化的方法[7]．目前ROS系统中有5种常用的SLAM构建方法：HectorSLAM、Gmapping、KartoSLAM、CoreSLAM 和LagoSLAM．通过对这5种方法的优缺点和精确的定量比较,定义一个通用的准则,选取一种合适导盲机器人使用的方法．

1.1 ROS系统中2D SLAM方式对比

HectorSLAM[8]对扫描的频率有较高的要求,当扫描频率较低时无法取得良好的结果,该方法不需要里程计的数据,适用于飞行器和在地形起伏较大的地面行驶的机器人．HectorSLAM是结合已经构建的地图对激光束点阵进行优化,以此来预测激光点在实际地图中的位置以及占据栅格的概率,从而更新地图．Gmapping[9]采用基于RBPF (Rao-Blackwellized particle filter)粒子滤波的Fast SLAM 2.0算法．其利用扩展卡尔曼滤波将当前机器人的环境观测信息融入粒子滤波器的提议分布设计之中,使得粒子集中地分布于高观测似然区域,自适应重采样技术引入减少了粒子耗散问题,计算粒子分布的时候不单单仅依靠机器人的运动,同时将当前观测考虑进去,减少了机器人位置在粒子滤波步骤中的不确定性．KartoSLAM[10]和LagoSLAM[11]是基于图优化的方法,CoreSLAM[12]是为了简单和容易理解最小化性能损失而设计的一种SLAM算法．

1.2 仿真环境对比试验

在仿真环境stdr-simulator仿真器中选取一个简单的楼层地图作为实验环境,地图总大小为20 m×15 m，共有6个房间,每个房间大小为5 m×4.5 m,加载机器人半径为0.15 m,在机器人中部安装URG-04LX激光雷达,扫描半径为4 m,扫描范围为270°,模拟地图和生成地图如图1．

图1 仿真环境下机器人构建的地图Fig.1 SLAM in simulation environment

结果表明,在仿真环境下,HectorSLAM、Gmapping和KartoSLAM表现较好,其中Gmapping最为精确,CoreSLAM和LagoSLAM存在较为明显的误差．

2 三维环境模型建立

文中基于Kinect传感器的实时三维环境构建地图，先提取彩色图像中的特征点,计算其在深度点云空间中所对应的三维坐标;而后进行彩色图像配准,利用配准中得到的运动结构信息,进行深度图像的拼接;最后形成同一坐标系中的三维环境地图．

2.1 图像预处理

2.1.1 特征点提取

文中选择点特征作为视觉路标,选用FAST角点检测方法,FAST[13]角点检测利用像素点某个邻域内相邻像素点灰度值的差异值的大小来决定该点是否为角点．该方法具有旋转不变性,以及较快的速度和较多的检测数量,能支持实时系统．

2.1.2 特征点匹配

采用的特征点匹配算法是归一化互相关匹配(normalized cross correlation,NCC),原理是计算相邻帧上角点邻域内所有像素点灰度值的相关性,利用相关系数来进行图像匹配[13](图2)．在图像n中以角点A为中心取一个大小为(2l+1)×(2k+1)的邻域T,在待配准图片m中以B点在图像n中的坐标为中心选取一个大小为(2H+1)×(2W+1)的搜索域S,然后对邻域T和搜索域S进行归一化互相关计算,将邻域T叠放在搜索域S上平移．若搜索域中S中的某个角点B大于设定的阈值,则待配准图片m中的B与图像n中的A相匹配,认定是空间中的同一点．

图2 NCC算法示意Fig.2 Schematic diagram of NCC algorithm

2.2 三维点云匹配

在获取可靠的特征点对后,需要对三维点云进行匹配,使用迭代最近点(iterative closest points, ICP)算法，对于两个来自不同坐标系的三维数据点集,找出两个点集之间的空间变换， 对它们进行空间匹配．文中采用FAST角点检测和NCC角点匹配是为了减小点集元素的数目,使用k-d tree来寻找特征点在参考点云中的最近点,不用遍历三维点云中的每一个数据,减少算法的时间代价．ICP三维点云匹配流程如图3．

图3 ICP算法Fig.3 ICP algorithm

3 三维模型与二维地图融合

激光雷达构建的二维SLAM地图在精度、速度和鲁棒性方面都有很大的优势,但是二维的激光雷达只能获取某一个平面内的环境信息,无法获取立体的环境信息,将Kinect建立的三维环境模型与激光雷达建立的二维地图进行拼接融合,既保留了激光雷达的高精度等优点,又增加了三维环境的认知．

图像拼接方法的具体步骤如下:首先使用坎尼边缘检测法对地图进行边缘检测,对于三维模型地图需选取某一层来进行;然后使用扩展相位相关法直接计算出两幅地图的平移、旋转和尺度变化参数,使用这些参数来拼接两幅地图内的图像边缘;地图的融合使用渐入渐出法,即在拼接地图中的重叠区域,由第一幅地图慢慢过渡到第二幅,从而保持拼接地图在视觉上的一致性．

3.1 坎尼边缘检测法

坎尼(Canny)算子是一种最优的边缘检测算子,广泛应用在数字图像处理领域[14]．其检测边缘的标准是:低误判率;高定位精度;抑制虚假边缘，尽可能地把图像中的边缘点误认为是非边缘点,同时准确地将图像的边缘点定位在灰度差异最大的像素点上，并尽量抑制图像中虚假边缘的产生．

坎尼边缘检测流程如下:

(1) 由于大多图像数据中含有噪声,噪声点与附近像素点梯度幅值差异大,容易将其无检测视为边缘点,所以利用高斯滤波器对要处理的图像进行卷积滤波,减小噪声对梯度计算的影响．高斯滤波器如下:

(1)

(2) 使用一阶差分算子分别算出水平方向和垂直方向的梯度幅值分量,进而得到图像的梯度的幅值M和方向θ：

(2)

θ=arctan(Gy/Gx)

(3)

(3) 非极大值抑制．对梯度幅值图像M(i,j)上每个像素和梯度方向上两个相邻像素进行插值计算,获得该像素的梯度幅值,如果该像素的梯度幅值是大的,则把该像素视为可能的边缘点,认为是非边缘点,梯度幅值图像M(i,j)经过非极大值抑制得到图像NMS(i,j)．

(4) 双阈值检测和边缘连接．遍历图像NMS(i,j),分别使用高阈值Th和低阈值Tl进行阈值化获得边缘图像E1和E2,E1是强边缘点,可能出现间断,E2是弱边缘点．跟踪E1中的边缘,如果边缘遇到端点,则在图像E2相对位置的8邻域内寻找边缘点来连接E1中的间断,经过持续的边缘跟踪,把图像E1中边缘的间断连接起来．

3.2 基于边缘扩展相位相关法

将对数极坐标变换与相位相关法相结合,形成了扩展相位相关法,目的是解决同时存在相对平移、旋转和尺度变化的两幅图像的配准问题[14]．

假设两幅图像信号f1(x,y)和f2(x,y)满足:

(4)

即f2(x,y)相对f1(x,y)沿正时针方向旋转了θ0角度,尺度变化了α倍,平移了(x0,y0),要根据已知图像f1(x,y)和f2(x,y)求出参数角度θ0、尺度因子α和平移量(x0,y0)，使用扩展相位相关法:

(1) 对f1(x,y)和f2(x,y)分别取傅里叶变换得F1(μ,υ)和F2(μ,υ),应满足:

(5)

(2) 对F1(μ,υ)和F2(μ,υ)取模得M1(μ,υ)和M2(μ,υ),则有:

(6)

(3) 对M1(μ,υ)和M2(μ,υ)进行对数极坐标变换,即把这两幅图像从笛卡尔系中变换成对数极坐标系．

对M1(μ,υ)和M2(μ,υ)做极坐标变换得M1(ρ,θ)和M2(ρ,θ),由式(6)得:

M2(ρ,θ)=α2M1(αρ,θ+θ0)

(7)

式中：ρ为极径,θ为极角．对式(7)沿ρ轴取对数得:

M2(lgρ,θ)=α2M1(lgρ+lgα,θ+θ0)

(8)

令γ=lgρ,γ0=lgα,则式(8)变为:

M2(γ,θ)=α2M1(γ+γ0,θ+θ0)

(9)

式中：γ为对数极径.式(9)表示尺度因子和旋转角度在对数极坐标系中表现为M1(γ,θ)和M2(γ,θ)的相对平移量．

(4) 相位相关法求解尺度因子α和平移量(x0,y0)．

将M1(γ,θ)和M2(γ,θ)做傅里叶变换得Fm1(μ,υ)和Fm2(μ,υ),则其归一化互功率谱为:

(10)

将C(μ,υ)进行傅里叶反变换得:

c(γ,θ)=δ(γ-γ0,θ-θ0)

(11)

(5) 对图像f1(x,y)沿正时针方向旋转,并尺度变化α倍得到相对于f2(x,y)仅存在(x0,y0)平移的图像,再次使用步骤(4)就可以求解平移量(x0,y0)．

4 实验

为了验证文中所述的方法能合理有效地应用于导盲机器人,以Turtlebot2为基础搭建实验平台,主要包括:Yujin Kobuki移动底座、Kinect摄像机、HOKUYO激光雷达和笔记本电脑以及其他辅件,如图6．

图6 实验平台Fig.6 Experiment platform

4.1 实际环境中SLAM算法对比

真实场景中的实验选择在有4条走廊的办公楼,其中最长的走廊长度为45 m,宽度为2 m,用手柄控制移动机器人采集走廊环境信息,并使用不同方式构建地图,所得地图如图7．

经过实验发现其精度由激光雷达的扫描频率决定,低扫描频率下误差较大,所以HectorSLAM只适用于高性能的激光雷达．Gmapping在所有试验中表现出最好的精确性和鲁棒性,通过里程计和扫描匹配的结合减少了粒子数量,并且能长时间保持精度,错误率和CPU使用率都较低．

图7 真实环境下机器人构建的地图Fig.7 SLAM in actual environment

KartoSLAM和LagoSLAM都是基于图优化的SLAM方法,KartoSLAM提供精确的地图较低的CPU负载,而LagoSLAM生成地图误差和CPU负载较高,原因在于两者使用的节点配置和优化方式不一样．CoreSLAM在整个试验中结果最差,主要是因为它没有回环检测环节,CoreSLAM设计的初衷是在长走廊中进行SLAM,使用简单的粒子滤波环节来匹配雷达数据来构建地图,由图7,它在较细的长廊中表现差强人意,但是其他地方的位置错误估计较多．5种方法在构建地图时的CPU使用率如表1,测量时间为360 s．

表1 不同SLAM方法的CPU使用率Table 1 CPU Load of the 2D SLAM approaches %

经过在仿真环境和真实环境中的对比,Gmapping方法在各方面表现较好,速度快,鲁棒性好,因此选用Gmapping作为导盲机器人的二维地图构建方法．

4.2 三维环境模型建立

Kinect的三维SLAM地图建立在一个房间里进行,机器人在一个矩形房间里环绕一周,采集环境信息,当某一帧的特征点数量太小,则舍弃该帧,当机器人的移动小于5 cm或者转角小于2°,也舍弃这些帧的数据．Kinect建立的三维模型和渲染效果如图8(a)和(b),(c)为去除地面特征的效果．

图8 三维模型获取及渲染效果Fig.8 3D model

4.3 三维模型与二维地图拼接

在同一个房间内使用Kinect建立三维SLAM地图,再使用激光雷达建立二维SLAM地图,然后进行三维模型和二维地图的拼接,后期将整个流程集成到一个功能包中．该房间大小为5 m×7 m,当机器人环绕场地一周后,同时构建二维和三维地图,并完成拼接,第一帧的结果如图9(a);整个房间的构图和拼接结果如图9(b);去除点云信息后保留的二维地图如图9(c),可见即使是在障碍物较多、环境较为复杂的房间内,依然可以获得较为理想的结果．由于Kinect精度以及导盲机器人速度控制等不可避免的问题,匹配遗漏的现象还会存在,想取得良好的实验结果,需保持机器人均匀低速运行．

图9 地图拼接Fig.9 Map merging

5 结论

(1) 仿真环境和实际环境中对几种ROS系统下的SLAM算法进行对比,分析各自的图像质量、运行速度、鲁棒性和CUP使用率,总结得出Gmapping算法最适合室内导盲机器人使用．

(2) 利用深度传感器Kinect采集的信息,采用FAST角点检测提取稳定特征点,并使用归一化互相关匹配法进行特征点匹配,再使用ICP算法对获取的深度图像进行精确配准,得到三维环境地图,图中包含环境内的三维深度信息,但是在相应速度和精度上略有不足．

(3) 提出了一种基于激光雷达和Kinect信息融合的导盲机器人SLAM方法,通过综合激光传感器和深度图象传感器Kinect在采集数据和精度方面的优缺点,使用边缘扩展相位相关法,对Kinect生成的三维SLAM地图和激光雷达构建的二维SLAM地图进行拼接,得到融合二者优点的三维空间障碍物模型地图．在真实环境中进行实验,发现融合激光和深度信息的三维空间障碍物模型地图能足够全面和精确的认知环境,机器人因此能更好地完成导盲任务．

(4) 采用分布式ROS机器人操作系统,使得整个机器人具有良好的实时性,文中激光扫描匹配和深度特征的提取与匹配很好的解决了机器人在构建地图的过程中遇到移动物体的干扰,所得地图包含了室内二维和三维的信息,并且所得地图可以直接用于后续的机器人定位和导盲,配合相应的蔽障和路径规划算法可以让导盲机器人自主移动,进而可以引导盲人安全行走．

参考文献(References)

[ 1 ] 张志美, 程立英, 赵以恒, 等. 基于模糊PID控制算法的导盲机器人研究[J]. 沈阳师范大学学报(自然科学版), 2015, 33(1): 81-85

ZHANG Zhimei, CHEN Liying, ZHAO Yiheng, et al. Fuzzy-PID arithmetic for blind guiding robots [J].Journal of Shenyang Normal University(Natural Science Edition),2015, 33(1):81-85.(in Chinese)

[ 2 ] 陈超，唐坚，靳祖光.基于RFID技术导盲机器人室内路径规划的研究[J].江苏科技大学学报(自然科学版)，2013，27(1)：60-63.DOI：10.3969/j.issn.1673-4807.2013.01.014.

CHEN Chao, TANG Jian, JIN Zuguang. Indoor path planning for seeing eyes robot based on RFID[J].Journal of Jiangsu University of Science and Technology(Natural Science Edition),2013，27(1)：60-63.DOI：10.3969/j.issn.1673-4807.2013.01.014.(in Chinese)

[ 3 ] 张探,陈超. 突破环境限制的导盲方法[J]. 中国图象图形学报,2015, 20(11):1545-1551.

ZHENG Tan,CHEN Chao.Guiding the blind to overcome environment limitation [J].Journal of Image and Graphics,2015, 20(11):1545-1551. (in Chinese)

[ 4 ] YEON A S A, KAMARUDIN K, VISVANATHAN R, et al. Feasibility analysis of 2D-SLAM using combination of kinect and laser scanner[J]. Jurnal Teknologi, 2015, 76(12).

[ 5 ] VIEJO D, GARCIA-RODRIGUEZ J, CAZORLA M. Combining visual features and growing neural gas networks for robotic 3D SLAM[J]. Information Sciences, 2014, 276: 174-185. DOI:10.1016/j.ins.2014.02.053.

[ 6 ] SANTOS J M, COUCEIRO M S, PORTUGAL D, et al. A sensor fusion layer to cope with reduced visibility in SLAM[J]. Journal of Intelligent & Robotic Systems, 2015, 80(3): 401-422. DOI:10.1007/s10846-015-0180-8.

[ 7 ] 梁明杰,闵华清,罗荣华. 基于图优化的同时定位与地图创建综述[J]. 机器人,2013, 35(4):500-512.

LIANG Mjingjie,MIN Huaqing,LUO Ronghua.Graph-based SLAM: A Survey [J]. Robots, 2013, 35(4):500-512. (in Chinese)

[ 8 ] KOHLBRECHER S, STRYK O V, MEYER J, et al. A flexible and scalable SLAM system with full 3D motion estimation[C]//IEEE International Symposium on Safety, Security, and Rescue Robotics. IEEE, 2011:155-160.

[ 9 ] SANTOS J M, PORTUGAL D, ROCHA R P. An evaluation of 2D SLAM techniques available in robot operating system[J]. 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2013. DOI:10.1109/ssrr.2013.6719348.

[10] DHIMAN N K, DEODHARE D, KHEMANI D. Where am I? Creating spatial awareness in unmanned ground robots using SLAM: A survey[J]. Sadhana, 2015, 40(5): 1385-1433. DOI:10.1007/s12046-015-0402-6.

[11] SANTOS J M, PORTUGAL D, ROCHA R P. An evaluation of 2D SLAM techniques available in robot operating system[J]. 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR), 2013. DOI:10.1109/ssrr.2013.6719348.

[12] VERKEENKO M S. Development of an algorithm for fast corner points detection[J]. Journal of Computer and Systems Sciences International, 2014, 53(3): 392-401. DOI:10.1134/s1064230714030162.

[13] SHEN X, BAO W. The remote sensing image matching algorithm based on the normalized cross-correlation and SIFT[J]. Journal of the Indian Society of Remote Sensing, 2013, 42(2): 417-422. DOI:10.1007/s12524-013-0323-y.

[14] 曾俊. 图像边缘检测技术及其应用研究[D].华中科技大学,2011:17-38.