贺利乐, 王消为, 赵 涛

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

0 引 言

同步定位与地图构建(simultaneous localization and mapping，SLAM)是指在未知环境中，移动机器人从某一未知起点开始移动，在移动过程中通过自身携带的传感器进行识别特征路标，在增量式建立环境地图的同时利用已建立的地图来更新自身的位置，进而实现移动机器人的自主定位与导航。Montemerlo M,Doucet A等学者[1～4]提出基于Rao-Blackwellized粒子滤波器(Rao-Blackwellized particle filters,RBPF)的SLAM算法，至今这种算法仍然得到广泛应用。文献[5～7]主要基于RBPF算法应用组合导航的方式进行移动机器人SLAM的研究，采用多传感器融合有效提高了定位与地图构建的准确性及鲁棒性，但仍存在地图构建不精确，实时性差等问题；文献[8]基于图优化的同步定位与地图创建进行了阐述，但图优化的方法复杂度高、计算量较大，对SLAM的实时性无法满足；文献[9]提出一种单目视觉ORB-SLAM(oriented-FAST and rotated BRIEF-SLAM)/惯性导航系统(inertial navigation system,INS)组合导航的方法并设计了试验平台进行验证，组合导航系统具有较高的定位精度，但鲁棒性不强。

本文基于改进的RBPF算法研究激光雷达与双目视觉融合的移动机器人同步定位与地图构建；采用改进的RBPF算法，在计算提议分布时将移动机器人的观测数据与里程计信息融合；对于常规视觉信息处理速度较慢，采用基于快速鲁棒性局部特征检测器—ORB对视觉图像进行处理，从不同视觉角度的图像中提取特征点，匹配并计算得到旋转角度;通过在安装有开源机器人操作系统(robot operating system,ROS)的履带式移动机器人上进行多组实验，验证其有效性及可行性。

1 改进RBPF实现SLAM

由于传统RBPF-SLAM一般将里程计运动模型作为提议分布，但里程计运动模型噪声比较大，会使得各粒子间的权值差别很大，导致粒子出现乏匮现象，因此提出采用改进的RBPF算法。

1.1 改进提议分布

将观测传感器(激光雷达与双目视觉当前观测信息)提取出来的融合信息zt融入到粒子提议分布中，能够有效降低粒子匮乏问题的发生。采样分布提议如下

(1)

式中xt为t时刻的实际位姿向量,ut为移动机器人的控制信息,m为地图。则相应的计算重要性权重公式为

(2)

式中z1:t为观测信息，u1:t为控制的历史信息。

1.2 改进的RBPF的SLAM算法步骤

2 特征提取及数据关联与融合

由于移动机器人移动过程中可能出现轮子打滑、漂移等问题，导致里程计数据出错;二维激光雷达只能在固定某一平面上扫描;视觉易受环境背景、光照等条件影响。故提出上述应用改进RBPF的SLAM算法信息融合及同步定位与地图构建建模的分析，采用双目视觉与激光雷达结合里程计，实现移动机器人的自主导航。

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

ORB算法[10]主要步骤分为特征点检测及特征点描述。机器人所在环境下双目视觉传感器获得的图像中的ORB特征点提取和匹配效果如图1所示。

图1 图像特征点处理

将上述匹配的特征点作为可视化路标，随机选取路标点代表如图1(a)中A(0.93,1.32,-0.01),B(0.84,1.94,-0.01),C(-0.91,2.63,0.01),D(-0.76,5.68,1.98),E(0.67,6.63,2.53)，建立包含可视化路标点坐标信息的路标数据库，从图像中提取出的三维坐标转化至水平面上，并构建出水平面上2D路标地图。

2.2 数据关联与融合

2.2.1 数据关联

基于多传感器融合及组合导航的方法将不同传感器的观测信息进行关联时，存在局部数据之间及局部与全局数据的关联问题，为了更好的获得全局环境地图和实现定位，本文采用近邻算法[11]进行数据关联，其过程为：

2.2.2 数据融合

首先对激光雷达测距数据进行处理，得到环境中障碍物的距离信息；然后用ORB算法从双目视觉传感器中获得的两幅不同视角的图像中寻找特征点，匹配并得到旋转角度，将得到的匹配角度与里程计角度融合，使其更加接近实际角度；再结合激光测距与双目视觉传感器，分别融合距离信息与旋转角度信息，定位移动机器人的初始位置；根据激光雷达与双目视觉传感器获得的路标信息，构建特征地图；最后采用上述改进的RBPF算法进行移动机器人的自主导航。其流程如图2。

图2 数据融合流程

3 实 验

3.1 实验平台及环境

本文利用一台双轮差分驱动的履带式移动机器人进行实验，如图3。机器人本体上配有HOKUYO二维激光雷达URG-04LX、双目视觉系统Bumblebee2、里程计等传感器及一个串口连接的控制计算机。且控制计算机上安装有Ubuntu14.04操作系统及Indigo版本的ROS，通过与另外一台安装ROS的DELL笔记本电脑建立通信，进行远程连接并控制机器人移动。

图3 履带式移动机器人

实验环境选取光线较明亮的学校工科大楼十九层走廊作为实验环境。

3.2 自主导航实现

1)使远程控制计算机和移动机器人控制计算机连接到同一个WiFi网络下;2)在远程控制PC上建立节点管理器和创建键盘调试、SLAM与路径规划等节点，同时在移动机器人控制计算机上创建双目视觉、激光雷达测距、里程和基础控制器等节点;3)在节点管理器中进行所有的节点注册并由节点管理器统一管理，通过端对端的拓扑结构在同一网络中进行TCP/IP通信；4)利用3D可视化工具RVIZ实时显示移动机器人自主导航的状态信息。

3.3 实验结果与分析

在实验环境下进行，分别用上述改进的RBPF算法与传统RBPF算法进行对比实验。移动机器人运动速度为0.5 m/s，通过ROS中的可视化工具RVIZ可以实时显示地图创建的过程,如图4、图5(a)为采用多传感器融合的传统RBPF算法30个粒子构建的2D栅格环境地图，由于其算法仅使用里程计作为提议分布，因此随着时间的推移里程计累积误差越来越大，所构建的地图出现了不一致现象。图5(b)为使用上述多传感器融合改进的RBPF算法10个粒子构建精确的2D栅格环境地图。

图4 为RVIZ显示的实时地图

图5 构建的栅格地图

实验中传统RBPF算法与改进RBPF算法创建程度一致性地图时所平均消耗时间分别为280 s和130 s，可以看出:改进RBPF算法需要的粒子数目远小于传统RBPF算法，同时在构建地图的过程中所用的时间也很大程度地缩短，有效提高了算法的运算效率。

分别仅使用激光雷达传感器与仅使用双目视觉传感器进行移动机器人的自主导航实验，效果如图6所示。

图6 单一传感器与改进算法构建的地图

对比传统算法与改进算法，可以看出传统算法随着时间的推移，构建出的地图出现扭曲、不一致现象，但采用改进RBPF算法时,构建的地图相对稳定、精确。

在满足移动机器人SLAM实时性的情况下，传统算法与改进算法各选取6，10，15，20，28，35，40，46，50，60共10组粒子数在实验环境下进行实验，每个条件下进行a=10次，创建地图成功的次数为b，得出相应粒子数下地图构建的成功率h=(b/a)×100 %，将所创建地图的连续来作为判断创建地图成功的标准，地图连续性可通过将所建地图放大30倍像素观察判断。构建地图粒子数与成功率的关系如图7所示。

图7 构建地图成功率与粒子数的关系

可以看出，当粒子数达到10个时改进算法就可以构建出一个高精度的栅格地图，而传统算法则需要30个粒子才能构建出同样完整的地图；当使用相同粒子数创建地图时，改进算法的成功率明显高于传统算法，有效提高了移动机器人SLAM的鲁棒性。

为了更加直观地对比、分析基于改进RBPF算法的多传感器融合组合导航优势，本文进行改进算法与传统算法在MATLAB中仿真位姿状态估计精确性对比。分析结果如图8，可以看出改进算法对位姿估计的均方根误差远小于传统算法，曲线趋势相对平滑。由于提议分布中加入了激光雷达与双目视觉的融合信息，使粒子分布在真实状态附近。因此，改进算法的状态估计精度优于传统算法。

图8 状态估计误差

4 结 论

本文提出一种双目视觉与激光雷达信息融合实现移动机器人的自主导航。实验表明通过基于改进RBPF算法的履带式移动机器人组合导航方法，进行多传感器融合提供的冗余信息能够构建可靠性更高、更精确的2D栅格环境地图，有效提高了移动机器人SLAM的鲁棒性。