基于二维激光的机器人室内构图

宫婉婷，孙作雷，潘 盼

(上海海事大学 信息工程学院, 上海 201306)

基于二维激光的机器人室内构图

宫婉婷，孙作雷，潘 盼

(上海海事大学 信息工程学院, 上海 201306)

建立了2D激光扫描匹配数据的构图方案，使用GMapping(一种高效的Rao-Blackwellized粒子滤波器，从激光数据学习网格地图)通过改变内部扫描配准算法，实现录制数据(原始2D扫描数据)经过激光配准算法后生成相对运动文件作为tf，用以提高地图生成和激光扫描配准的精度。实验基于机器人操作系统(ROS)，开源的GMapping软件包进行同时定位和地图构建(SLAM)，通过RVIZ(用于ROS的3D可视化工具)控制机器人运动。该方案可应用于移动机器人的导航、构图与2D重建、同步定位与地图构建(SLAM)等领域。

ROS；SLAM；激光匹配；SLAM-GMapping；tf

Abstract: This paper establishes the composition scheme of 2D laser scanning matching data. Using GMapping (an efficient Rao-Blackwellized particle filter, from the laser data to learn the grid map) by changing the internal scan registration algorithm, it achieves the recording data (raw 2D scan data) through the laser registration algorithm to generate a relative motion file as tf, to improve the accuracy of map generation and laser scanning registration. The experiment is based on the robot operating system (ROS), the open source GMapping package isused for simultaneous localization and mapping (SLAM), and robot movement is controlled through RVIZ (3D visualization tool for ROS). This scheme can be applied to the research of mobile robot navigation, composition and 2D reconstruction, simultaneous localization and mapping (SLAM).

Key words:ROS; SLAM; laser matching; SLAM-GMapping; tf

0 引言

机器人的应用方向主要是自身定位、地图构建以及自主导航等,建立地图是移动机器人的重要任务。学术范围中，移动机器人构图是同时定位与地图构建(SLAM)的重要组成部分。随着工业、商业、医疗以及军事和民用领域对人工智能与自动化的日益增长的技术需求，机器人导航相关的理论与应用已成为研究重点。获取机器人导航中里程数据对构图是至关重要的，可由编码器获取，或基于CMOS的图像处理方法和使用多传感数据融合来获得更精确的里程数据方法，还可以用单个激光扫描测距仪使用GMapping包来进行构图和定位。本文中，使用激光扫描仪与光学增量编码器进行预映射。由于激光扫描仪可以提供详尽的数据，因此可以进行直接数据处理。 此外激光数据的精度高于超声波和光学相机[1]。市场上已经有许多广泛应用于科研领域的2D激光仪，在论著中被广泛提及。

目前导航技术中，激光导航应用得最为广泛。激光导航有以下几点优势：(1)激光导航应用广泛，不易受环境干扰，具有很强的稳定性；(2)激光测距仪测量距离远、测量范围广、测量精度高、测量准确性强；(3)激光导航成本相对较低，且扫描频率高，可以获得大量有效点集。因此本文使用激光测距仪Hokuyo进行数据采集。本文对传感器数据应用匹配算法，进而优化GMapping构图，实现SLAM。

1 SLAM算法

SLAM是同步定位与地图构建。 在构图过程中，机器人位置是已知的，因此相对于机器人位置检测标记位置。在定位中，相对于标记位置检测机器人的位置。 因此，很明显，在环境构图中，构图和定位彼此耦合[2]。

在构图过程中机器人位姿Xv(k)已知，因此需要知道标定a的位置，测量z(k)来构建地图p。SLAM算法是将定位与构图问题[3]同时考虑的方法。如图1所示已知机器人位姿为xv(k)和标记位置为p(k)，需要知道测量值z(k)。

图1 SLAM算法的参数

2 GMapping算法

2.1 GMapping算法

本文中，使用基于GMapping算法和扫描匹配算法的网格地图来进行地图构建。为了从激光扫描数据中学习网格地图，GMapping算法使用Rao-Blackwellized粒子滤波器[4](RBPF), 在RBPF中，地图数据间的联合后验pt和使用激光扫描测量数据Z1:t=z1…zt估测机器人的状态x1:t=x1…xt[5]。

p(x1:t,pz1:t,u1:t-1)

(1)

RBPF的基本原理是首先设置假设状态，每个粒子保持通过激光扫描器获得的测量状态，并与相应的粒子相关联标记。最强的假设持续给定权重，并且重采样[5]后将弱假设丢弃。粒子状态可以被定义为机器人的潜在位姿，并且每个状态表示后验。

p(x1:t,pz1:t,u1:t-1)=p(x1:tz1:t,u0:t-1)×p(pz1:t,u1:t)

(2)

使用式(2)中的概率比例方程选择粒子，将观测值与其自己的地图进行比较，并选择最可能的粒子，其中每个粒子滤波器携带地图特征。这种方法为SLAM的主要定位问题提供了无成本的解决方案。GMapping算法避免从里程表或其他控制数据定义姿态进行验证控制数据。

图2中，xt指车辆和地标的状态，zt代表测量值，pt代表获得的地图。可以看出在这个流程图中没有使用控制数据， 因此，表明GMapping算法没有控制数据。

图2 无控制数据的GMapping

算法可以表示为以下几步：

(1)初始状态假设。假设机器人的姿势从先前的姿势获得，即激光扫描器获得的测量值zt-1。

(2)扫描匹配算法从初始状态假设获取地图pt-1。

(3)更新粒子点。基于测量值zt更新粒子点，地图pt的粒子基于状态量xt与测量值zt更新。

使用扫描匹配将当前扫描的地图与构造的地图配准。扫描匹配用于对准当前扫描zt与给定的初始状态xt0[6]和地图pt-1[7]。 从初始状态xt开始，地图pt-1应用扫描匹配算法。仅通过算法对xt状态的有限区域进行对准。

(3)

扫描匹配选择具有高假设的粒子状态xt,根据式(3)的结果计算下一个状态。

p(ztpt-1,xj)→p(xjxt-1,ut-1)

(4)

在这个阶段，使用归一化器来计算权重:

(5)

最后根据状态值xt与测量值zt更新地图pt的粒子t[6]。

2.2使用SLAM- GMapping构建地图

机器人操作系统(ROS)中SLAM-GMapping包是对SLAM问题开源实现的一个封装，ROS包包含名为SLAM-GMapping的可执行节点，是SLAM 算法的实现，可以通过激光扫描数据和移动机器人姿势创建2D占用网格图[4]。

SLAM-GMapping节点订阅激光数据scan(sensor_msgs/LaserScan)和传输数据( tf/tfMessage)，发布占用网格地图作为输出。可以通过运行map_sever保存构建的地图，地图被存储在YAML文件和图像文件中。YAML文件描述地图元数据，并命名图像文件对占用数据进行编码。

3 点集配准算法

3.1 ICP算法

目前有开放许多配准解决方案，例如，迭代最近点算法(ICP)被认为是许多基于激光定位和构图系统的主干。ICP算法流程如下[8]：

(1)判断E←0，重复步骤(2)～(6)直到满足终止条件：n=0或E=0。

(2)将Lnew投影到世界坐标系w下。

(6)

(7)

(3)外层循环i从1执行到N。

(8)

(5)求得参考激光束与新激光束之间的激光点关联配置：

(9)

将Cnn，Lref，Lnew代入机器人相对闭合解估计算法，求出相对运动估计RTnn。

输出：参考激光束与新激光束所有激光点间的关联配置CICP[9]，以及对相对运动的估计RTICP。

3.2 Libpointmatcher

由于ICP算法的本质是局部收敛近似算法，结果取决于初始姿态，需要提供初始假设，初始假设通常由惯性测量、里程计或启发式运动模型提供，这些都存在精度有限和观测中随时间增加的不确定度问题。另外激光扫描匹配的一个难点是大量的离群点，有大量的不能反映扫描特征的点。而且扫描的内容也将对配准产生很大的影响，室内环境中通常是一些平面(例如：地面、墙体、天花板等)。

为了弱化这些因素对ICP算法性能的影响，本文中使用ETH的Libpointmatcher[10]进行实验点集的匹配。Libpointmatcher是模块化ICP链，其中libnabo1基于kd-tree优化实现最近邻搜索，它是ICP最快的kd-tree之一。由于模块化和快速性，Libpointmatcher库为配准算法提供了理想的解决方案[11]。

4 实验部分

4.1 ROS实验平台

该系统在通过WiFi(分布式系统)连接的两个Linux机器上实现。具体地，运行Ubuntu的PC(Core-i5,2.53 GHz CPU，6.00 GB RAM)，基于ROS在RVIZ上的机器人远程操作，进行地图可视化。

然后，配备Hokuyo URG-04LX-UG01激光测距仪并由Raspberry Pi 2 B型控制的Hercules 4WD移动平台构成低成本移动机器人。应用了激光传感器Hokuyo，在ROS环境下获取多个环境点集，使用rosbag来录制数据。实际场景图主要环境为办公区域、走廊、工位隔断之间的空隙以及花盆等静态标志物。

4.2机器人控制过程

当以预定路线手动控制运行机器人时获取实验数据是有效的，例如识别传感器、轮子或者其他拓展等误差。手动导航是使用基于Android的应用程序，通过蓝牙与Arduino控制板通信。 蓝牙模块(HC-06)用于实现Android应用和控制板之间的通信。 Android手机是用来进行控制的。用于构图过程的控制方法不同于基于蓝牙模块的手动控制方法。 构图过程中在ROS中使用rqt控制台[12]并给出了一个速度命令。 这个速度命令与机器人在导航时获取的自主构建环境中的命令相同。 为此获得了电机速度与电机转速的关系输入PWM到电机：

v=0.003 2p-0.404 5

(10)

式中v是机器人速度，p是PWM。上述关系假设是线性关系，虽然不是完全线性，由于电机的PWM控制，PWM中的一点变化可以忽略。

4.3 ROS节点话题订阅

启动hukuyo_node节点保持机器人与计算机的连接，通过base_laser与base_link之间静态tf变换来安装和确定激光测距仪的位置[13]；发布并广播odom变换,使用编写的odom_tf，将里程计信息发布并广播，发布/pose_topic获得的里程计位姿。控制小车运动，启动GMapping;运行/map与/odom 之间的坐标变换，然后使用RVIZ进行地图显示，设置LaserScan的topic为/laser， Map的topic为/map用以显示构造环境图。

4.4优化方法

为了优化GMapping的性能，在建筑走廊构图环境进行试验，机器人远程操作以记录和探测它的几个ROS bag，然后启动GMapping以从不同参数的记录bag来分析它们对性能的影响，可以找到有关在记录bag时使用的步骤快速参考[14]。

ROS bag数据包经过转换后成为过程数据。过程数据都是.csv格式，第一行为标题，这种格式可被许多语言和软件(包括Python、MATLAB)轻松解析。点云在局部坐标和全局坐标中可用。所提供的真实地面姿态直接给出原点到给定扫描基础的变换[10]。场景环境为地面实况信息，过程数据使用Libpointmatcher进行ICP匹配，求出read cloud 与reference cloud相对变换关系，最终输出为.vtk格式的点云匹配关系。将过匹配的对应关系作为GMapping的订阅tf话题进行广播，实现扫描数据的相对变换。将tf话题与原始scan信息进行节点订阅与广播，进行构图。

4.5实验结果

运用GMapping录制bag包[9]进行环境地图构建，直接录制的地图实况如图3所示。将ROS平台上根据遥感操作杆方式录制的bag转换点集经过本文的方法进行GMapping，建立的图形如图4所示。

图3 录制数据包构图

可以看出相同环境下，相较于图3，图4的边界线条更加清晰，且通过多个数据包验证，说明优化后的GMapping对于障碍物轮廓匹配正确率更高。因为Libpointmatcher封装的ICP算法使用多重滤波方法，使匹配对应关系更加准确，因此构图效果更佳。

图4 优化gmapping后的构图

5 结论

ROS有许多开源库和软件包，支持传感器融合和其他必要的框架。通过使用ROS，可以消除复杂算法(SLAM，

AMCL)的硬编码，ROS的RVIZ接口可以用来观测构图与导航过程。

GMapping算法处理包括固定和非固定地标。 在构图过程中，检测到的非静态地标将从所创建的地图中移除，只有静止地标将被添加到状态矢量，本文中使用的都是静止地标。原始GMapping构图将录制数据包数据进行匹配优化，或者优化GMapping参数达到优化构图的目的。将2D激光扫描数据准换成GMapping订阅的tf话题，实现2D激光扫描数据在ROS平台上的地图显示以及环境构建。该算法为低成本小型机器人导航构图提供了方法，可以用于机器人同步定位与地图构建的算法验证工作。

[1] DISSANAYAKE M, NEWMAN P, CLARK S, et al. A solution to the simultaneous localization and map building(slam) problem[J]. IEEE Transactions on Robotics and Automation, 2001,17(4): 229-241.

[2] 赵康.基准点线激光路面车辙检测技术研究[D].长安大学,2013.

[3] 吕强,林辉灿,张洋,等.基于视觉的多机器人协作 SLAM 问题[J]. 科技导报, 2015, 33(23): 110-115.

[4] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with rao-blackwellized particle filters[J].IEEE Transactions on Robotics, 2007, 23(1): 34-46.

[5] THRUN S, BURGARD W, FOX D. Probabilistic robotics(1st ed)[M]. The MIT Press, 2005.

[6] CENSI A, IOCCHI L, GRISETTI G. Scan matching in the hough domain[C]. Proceediags of the 2005 IEEE International Conference on Robotics and Automation,ICRA 2005, 2005,3(1): 2739-2744.

[7] LU F, MILIOS E. Globally consistent range scans alignment for environment mapping[J]. Journal of Autonomous Robots, 1997,4(4):333-349.

[8] 孙作雷.大规模不规则环境中的移动机器人定位与地图构图[D].上海:上海海事大学，2010.

[9] How to build a map using logged data[EB/OL]. [2017-02-27]http://wiki.ros.org.slam_gmapping/Tutorials/MappingFromLoggedData.

[10] RUSINKIEWICZ S, LEVOY M. Efficient variants of the ICP algorithm[C]. Proceedings of Third International Conference on 3-D Digital Imaging and Modeling, IEEE, 2001: 145-152.

[12] ROS TUTORIALS[EB/OL].[2017-02-27]http://wiki.ros.org/ROS/Tutorials.

[13] COLAS F, SIEGWART R. A review of point Cloud registration algorithms for mobile robotics[M]. Now Publisher Inc., 2005.

Indoor mapping of robot based on 2D laser

Gong Wanting, Sun Zuolei, Pan Pan

(College of Information Engineering, Shanghai Maritime University, Shanghai 201306, China)

TP242.3

A

10.19358/j.issn.1674- 7720.2017.18.017

宫婉婷，孙作雷，潘盼.基于二维激光的机器人室内构图[J].微型机与应用，2017,36(18)：55-58,62.

2017-02-27)

宫婉婷(1993-)，女，硕士研究生，主要研究方向：机器人自主导航。

孙作雷(1982-)，男，博士，副教授，主要研究方向：移动机器人导航，机器学习算法等。

潘盼(1989-)，男，硕士研究生，主要研究方向：移动机器人导航。