，，，

(国防科技大学，长沙 410073)

0 引言

移动机器人发展研究的挑战在于如何让机器人处在复杂变动的环境情形下高效率地完成预定的任务[1]。随着现代化社会的发展，人们有了减轻家务负担的需求，这使得室内服务机器人有很大的市场空间与前景。室内服务机器人作为移动机器人的典型代表，要求能够实时进行自定位，并且有效地避开各种静态和动态障碍物。自定位问题是移动机器人系统研究中的重难点之一，高精度可靠的定位是保证移动机器人自主地完成预定任务的关键[2]，根据环境模型是否已知可分为基于环境模型的定位、未知环境模型的定位以及同时定位与建图，其中基于环境模型的定位方法可分为相对定位、绝对定位与组合定位三类。绝对定位技术通常借助于外部传感器获得机器人的绝对位置，常用的方法有陆标定位、视觉定位以及卫星导航定位等。相对定位又称航迹推算技术，该方法仅根据编码器便能在短距离内获得较高的定位精度，但是长时间工作时存在漂移。

本文针对室内环境的特点提出了一种基于RGB-D相机的移动机器人自定位方法，该方法基于环境模型利用匹配优化定位算法可以高效率地实现精度较高自定位又能够确保系统的鲁棒性，本文最后通过开展实验研究和实验结果分析，验证所提方法的有效性。

1 研究现状

精准可靠的自定位是保证移动机器人完成各项自主性任务的关键，不同的定位算法各有优缺点，一些学者将各种理论应用于移动机器人的定位研究，其中Monte Carlo定位[3-5]是一种比较常用的方法，也称为粒子滤波方法，该算法的主要思想是使用带有权重的粒子点的集合来表示机器人的定位值在状态空间中的分布。初始化时，通常假设粒子点在场地中均匀分布，定位过程中，基于权重高低的重采样、基于机器人的运动模型更新粒子点的位姿和基于机器人的感知模型更新粒子点的权重等三个步骤循环进行，最后将所有的粒子点加权求和即可得到机器人自定位的结果，但是传统的粒子滤波定位算法将过多的时间用于处理权重不高的粒子，因此有一定的盲目性。文献[6----7]提出了一种基于超宽带的室内定位系统，该系统使用传输速度较高、穿透能力较强以及发射功率低的无线技术，基于超宽带的室内移动机器人自定位系统响应速度较快，可达到定位精度的要求，但是其成本较高，不适合应用于低成本的服务机器人。文献[8]设计了一种基于路标和三角测距法的移动机器人自定位系统并通过实物验证定位系统的有效性，但该系统严重依赖预先设定的环境模型，并且机器人上需要安置特定的传感器用于观测环境中的信标，其安装与维护信标的费用较高。文献[9]采用一种利用Hough变换的移动机器人定位算法，该方法利用建立全局地图的Hough能量谱函数估计机器人在全局地图中的位姿分布。针对RoboCup中型组足球机器人的结构化场地环境的特殊性，文献[10]提出了基于匹配优化定位的中型组足球机器人自定位算法，其核心思想是将机器人所观测到的特征点与环境信息相匹配，预先设定误差函数，并利用优化算法求出机器人自定位的最优解从而最小化误差函数。理论上而言，匹配优化定位方法的定位精度只由优化算法本身的计算精度和视觉测量精度所决定，并且优化算法可以在较短的时间内对一帧图像完成定位解算，所以匹配优化定位方法具有高效率与高精度的特点，但该定位算法需要使用机器人的初始定位值进行优化，也就是需要获得已知的机器定位初值。

2 基于RGB-D相机的匹配优化自定位算法

机器人自定位问题的核心就是求出机器人在世界坐标系中的位置与姿态(x,y,θ)，本文针对室内环境的特点提出一种基于RGB-D相机的匹配优化自定位方法，该方法首先建立机器人工作环境的地图模型，接着基于建立的地图模型生成环境误差查找表，最后通过匹配优化自定位算法完成机器人的自定位，利用匹配优化定位算法，既能高效率地实现高精度自定位又可以保证系统的鲁棒性。本文所提出的自定位算法的执行流程如下：

1)基于gmapping建立室内环境地图模型；

2)基于Hough变换提取线特征生成环境模型误差查找表并储存；

3)将RGB-D相机获得的深度图像转化为等价的激光扫描信息并发布；

4)机器人订阅实时的激光扫描信息；

5)机器人基于环境模型误差表通过匹配优化定位算法解算出机器人的位姿，算法结束。

2.1 基于gmapping建立未知环境的二维占据栅格地图

gmapping是一种高效率的Rao-Blackwellized粒子滤波算法，在建议分布与自适应重采样两个方面具有很大的改进，本文基于gmapping将RGB-D相机得到的深度图像转化为等价的激光扫描信息，以建立未知室内环境的二维占据栅格地图，其建图过程大致可分为三步，首先利用RGB-D相机每秒所产生的30帧深度图生成局部二维占据栅格地图同时估计出粒子(机器人)的位置与姿态，接着读取新的激光扫描信息并将其加入到粒子群再利用RBPF算法[11]优化粒子数目，最后选取权重最大的粒子作为机器人的最佳位姿并将生成的局部二维占据栅格地图加入到全局地图中，这三步骤不断地循环直至环境建图结束。

本文以实验室为研究环境，首先让机器人遍历室内环境，获得RGB-D相机输出的等价的激光扫描信息和里程计输出的机器人位姿数据，接着利用gmapping对机器人所采集的数据进行处理，生成二维占据栅格地图并存储，图1为机器人遍历室内长走廊与会议室后所建立的二维占据栅格地图，地图分辨率为每个像素点5 cm，白色，黑色与灰色像素分别代表未被占据、占据以及未知3种状态，墙壁障碍物已经用黑色粗实线标出。

图1 机器人建立的环境地图

2.2 基于霍夫变换线特征提取建立环境模型误差查找表

霍夫变换(Hough Transform)方法能够对二值图像进行直线特征的提取，标准参数化方式下，式(1)为直线l的表达(定义与性质，参考文献[12])。

r=x1cosθ+y1sinθ

(1)

记N×N二值图像(xi,yi)像素的灰度值为I(xi,yi)，参数空间中，[0,π]区间上，θ均匀地取M个离散值，r的采样个数为Q，标准霍夫变换(standrad hough transform , SHT)在直线检测中由式(2)表示：

(2)

Hough变换可以有效地提取线特征，本文基于Hough变换线特征提取来构建环境模型误差查找表。环境模型误差查找表表示世界坐标系下所有环境内的点到离它最近的线特征点之间的实际距离。

环境模型误差表的建立描述如下。首先对保存的环境地图模型进行边缘检测并二值化处理，接着利用累计概率霍夫变换对二值化后的环境地图进行特征提取，即对障碍物点进行线特征提取，如图2(a)所示，将非结构化的室内环境转化为结构化的室内环境，检测出的障碍物线特征用红色标记，非结构化环境向结构化环境转化的目的是便于使用匹配优化算法进行最优解的解算。接着遍历环境地图，计算出环境中每一个点与离该点最近障碍物直线段的实际距离(该距离用于匹配优化定位中近似观测的特征点与实际位置的偏差)，最后利用环境中每一个点与最近障碍物直线段的实际距离来生成环境模型的误差查找表并存储，图2(b)为环境模型误差查找表示意图，图中每一个像素点的灰度值代表该点与最近障碍物直线段之间的距离，灰度值越大意味着距离越大，即误差越大。

2.3 基于匹配优化的自定位

匹配优化定位的核心思想是把机器人所观测到的特征点与环境信息相匹配，预先定义误差函数，利用优化算法寻找使误差函数最小化的最优解[13]。本文机器人所观测到的特征点主要有室内环境中的墙壁与桌椅等障碍物。

基于匹配优化自定位的实现过程描述如下。机器人首先通过RGB-D相机获得障碍物特征点在机器人体坐标系下的坐标

图2 Hough变换提取的结果和误差查找表

值，并经过坐标转换将机器人体坐标系下的坐标值转换到世界坐标系中。定义

为机器人检测到的障碍物特征点在机器人体坐标系下的坐标值，机器人在世界坐标系下的定位值为(

x

,

y

,

θ

)，坐标转换关系如图3(a)所示，公式(3)将障碍物特征点从机器人体坐标系转换到世界坐标系中，世界坐标系下障碍物特征点的坐标值与环境中该特征点的真实位置的偏差可定义为

d

(

o

i

)，理想情况下，该偏差值应该等于0，因为所观测的障碍物特征点理论上应该在环境模型中障碍物直线段上。该偏差可用观测的特征点与离该特征点最近障碍物直线段实际距离来近似，因此机器人的自定位问题可转化为求解式(4)的寻优问题，其中

n

为障碍物特征点的数目，本文将误差函数

e

(

t

)定义为

如图3(b)所示，

c

为常量，取值为250。本文使用了RPROP算法

[14]

用于求解误差函数寻优的过程中无约束最优化问题，RPROP算法最早用于训练神经网络，优化权值，与常见的BP算法不同，RPROP算法只利用梯度的方向以及方向变化的信息来进行权值调整而不是根据梯度大小调整优化步长。本文中机器人自定位问题需要优化的是(

x

,

y

,

θ

)三个参数，求出误差函数的一阶导数，作为梯度，再利用梯度的方向和梯度方向变化的特点来寻找函数的最优解从而完成机器人的自定位，见公式(5)～(7)。

图3 坐标系转换与误差函数

(3)

(4)

(5)

(6)

(7)

3 实验结果与分析

本文在室内非结构化环境下进行了定位实验，测试了所提出算法的性能。总共设计了两组实验，第一组实验中，移动机器人在建有地图的室内环境中进行定位，记录定位得到的机器人位姿和定位算法处理时间，对定位算法的准确性与实时性进行分析；第二组实验在第一组实验的基础上，人为地在环境中添加噪声干扰，测试定位算法的鲁棒性。

实验环境如图4(a)所示，实验软件平台基于机器人操作系统(robot operating System，ROS)[15]，该操作系统能够提供类似传统操作系统的相关功能，如硬件抽象、底层设备控制、常用功能实现、进程间消息的传递以及软件包管理等。实验硬件平台选用Willow Garage公司的TurtleBot2机器人开源硬件平台，如图4(b)所示，TurtleBot2由Kobuki移动底盘和Kinect组成。Kobuki接口控制板32个内置传感器，两个驱动轮，单轴陀螺仪，一个可以扩展的输入输出端口和一个后挡板，开放式接口可以直接实现对机器人的移动与输入传感器的操作。Kinect是一款具有高精度的深度传感器，被广泛应用于室内移动机器人的研究[16-17]。Kinect的RGB 相机每秒能够获取30帧的彩色图像，左右两个深度传感器，基上安装有电机，可以使Kinect在竖直平面内上下倾斜正负27°，深度传感器利用主动射出的红外光往返的相位差来测量深度，与传统的结构光测量技术不同，Kinect的光编码所使用的是连续的照明(非脉冲)，无需特制的感光芯片，普通的CMOS感光芯片即可满足要求。

3.1 机器人自定位准确性和实时性分析

本实验通过遥控TurtleBot2遍历室内大厅并记录定位过程中自定位算法得到的机器人位置和姿态，得到的定位结果如图5所示，误差统计结果见表1。本文将TurtleBot2提供的定位算法解算的位姿设为真值(浅色圆圈点为本文算法的估计值，深色星号点为真值)。

图4 实验环境与硬件平台

图5 沿室内大厅移动时自定位结果

MAERMSEX/cm13．315．3Y/cm17．520．2θ/rad0．120．18

从图5和表1中能够得出，机器人位置定位的平均绝对误差都不超过17.5 cm，朝向定位的平均绝对误差小于7°，表明本文提出的机器人自定位算法能够满足室内移动机器人的定位要求。

实验环境改为长1 400 cm，宽132 cm的长条形走廊，沿走廊A、B两点往返做对比实验，将本文提出的自定位算法与粒子滤波定位算法在定位时间上进行对比，结果如图6所示，其中上细实线为粒子滤波定位计算所需时间曲线，下粗实线为匹配优化定位计算所需时间曲线，经过测试，机器人采用本文所提出的算法平均处理一次定位计算需要的时间为13.7 ms，粒子滤波定位算法完成一次定位计算平均所需的时间为21.8 ms。实验中机器人所携带处理器主频为1.7 GHz，内存为4.0 G，故本文所提出的算法在实时性上要优于粒子滤波定位算法。

图6 自定位算法时间对比

3.2 机器人自定位算法的鲁棒性

本文通过在机器人工作的室内环境里人为地加入噪声来模拟环境的变化情况，进而测试本文所提出的自定位算法的鲁棒性，噪声的设置有门的开关、增加桌椅数目以及人员的随意走动。实验通过遥控TurtleBot2遍历长走廊与会议室记录定位过程中自定位算法得到的机器人位置和姿态，得到的定位结果如图7所示，图7(a)为未加入噪声的结果，图7(b)为加入噪声的结果，定位误差统计结果见表2。从图7和表2中可以看出，加入噪声后的机器人位置定位的平均误差增幅小于5 cm，朝向定位的平均误差增幅小于6°，环境的变动并没有导致定位误差的明显增大，所以本文所提出的自定位算法对环境的变化具有一定的鲁棒性。

图7 室内环境机器人定位结果轨迹图

未加入噪声加入噪声MAERMSEMAERMSEX/cm17．321．320．725．7Y/cm17．521．121．626．6θ/rad0．180．260．240．31

4 结论

本文针对室内环境下移动机器人基于RGB-D传感器的视觉精确自定位问题开展研究，通过离线建立二维占据栅格地图，并建立环境模型误差查找表，将非结构化环境中的自定位问题转化为结构化环境中的自定位问题，进而采用匹配优化定位方法实现了移动机器人在室内环境中的自定位，所提出的自定位算法在室内环境下满足实时性、精确性和鲁棒性要求，并

且基于ROS框架的自定位算法具有很好的可移植性与扩展性等优点，为下一步研究路径规划与自主导航等问题奠定了良好的基础。

[1]Wang Y, Wang D, Mihankhah E. Navigation of multiple mobile robots in unknown environments using a new decentralized navigation function[A].Control, Automation, Robotics and Vision (ICARCV), 2016 14th International Conference on[C]. IEEE, 2016: 1-6.

[2]Patompak P, Jeong S, Chong N Y, et al. Mobile robot navigation for human-robot social interaction[A].Control, Automation and Systems (ICCAS), 2016 16th International Conference on[C]. IEEE, 2016: 1298-1303.

[3]刘开创，施家栋, 王建中, 等. 移动机器人自主返航控制系统设计与实验[J]. 计算机测量与控制, 2016, 24(3): 71-75.

[4]Handschin J E. Monte Carlo techniques for prediction and filtering of non-linear stochastic processes[J]. Automatica, 1970, 6(4): 555-563.

[5]Bukhori I, Ismail Z H. Detection strategy for kidnapped robot problem in Monte Carlo Localization based on similarity measure of environment[A].Underwater System Technology: Theory and Applications (USYS), IEEE International Conference on[C]. IEEE, 2016: 55-60.

[6]Kok M, Hol J D, Schon T B. Indoor positioning using ultrawideband and inertial measurements[J]. IEEE Transactions on Vehicular Technology, 2015, 64(4): 1293-1303.

[7]Jeábek J, Zaplatílek L, Pola M. A proposal of radio ultra wide band systems for precision indoor localization[A].Radioelektronika, 2015 25th International Conference[C]. IEEE, 2015: 355-358.

[8]Loevsky I, Shimshoni I. Reliable and efficient landmark-based localization for mobile robots[J]. Robotics and Autonomous Systems, 2010, 58(5): 520-528.

[9]Iocchi L, Nardi D. Hough localization for mobile robots in polygonal environments[J]. Robotics and Autonomous Systems, 2002, 40(1): 43-58.

[10]Xiong D, Xiao J, Lu H, et al. The design of an intelligent soccer-playing robot[J]. Industrial Robot: An International Journal, 2016, 43(1): 91-102.

[11]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.

[12]Zhang W, Li H, Yan X, et al. A Method of Recognizing Curve Direction Based on Hough Transform[A].Computational Intelligence and Design (ISCID), 2016 9th International Symposium on[C]. IEEE, 2016, 2: 3-6.

[13]Lauer M, Lange S, Riedmiller M. Calculating the perfect match: an efficient and accurate approach for robot self-localization, RoboCup 2005[A]. robot Soccer World Cup IX[C]. 2006.

[14]Riedmiller M, Braun H. A direct adaptive method for faster backpropagation learning: The RPROP algorithm[A].Neural Networks, 1993, IEEE International Conference on[C]. IEEE, 1993: 586-591.

[15]Martinez A, Fernández E. Learning ROS for robotics programming[M]. Packt Publishing Ltd, 2013.

[16]Hamzeh O, Elnagar A. A Kinect-based indoor mobile robot localization[A].Mechatronics and its Applications (ISMA), 2015 10th International Symposium on[C]. IEEE, 2015: 1-6.

[17]Kamarudin K, Mamduh S M, Shakaff A Y M, et al. Performance analysis of the microsoft kinect sensor for 2D simultaneous localization and mapping (SLAM) techniques[J]. Sensors, 2014, 14(12): 23365-23387.