蒋磊， 杨柳铭， 武方达， 韩会杰， 周雪
(中国矿业大学(北京) 机电与信息工程学院， 北京 100083)
摘要：针对现有井下定位算法定位精度差和依赖信号源坐标的问题，提出一种基于GMapping算法与指纹地图构建的井下定位方法。将待定位的位置信号特征与信号分布图进行匹配，选择最优定位坐标，从而提高井下定位精度；通过与GMapping算法相结合，避免了指纹地图构建过程维护成本高的问题，优化了算法的搜索与匹配效率。实际测试结果表明，该方法平均定位误差为57.7 cm，可以满足井下定位要求。
关键词：井下定位； 即时定位与地图构建； GMapping算法； 指纹地图; 信号分布图
无线传感器网络在煤矿的应用较为广泛，可用于井下环境气体采集、结构力学监测、定位及视频监控等[1-2]，为煤矿安全生产提供保障。对于无线定位，目前大多数研究主要关注如何精确计算传感器节点到待定位目标的距离和角度，通过准确测算待定位目标与节点间的相对位置来实现定位功能[3-4]。井下无线定位方法分为测距相关与测距无关2种。测距相关的定位方法最典型的是三角定位法，其定位精度为5～10 m，比较依赖于视距环境，定位误差大；测距无关的定位方法如质心算法[5-7]，其定位精度为2～10 m，应用条件相对苛刻，依赖于信号源密集部署，且定位精度不高[8-9]。传统的井下定位技术认为信号源节点在井下环境中的位置是绝对精确的，然而，实际应用场景中信号源往往是人工部署，所以其位置标定会存在误差。这样，即使三角定位法或质心算法的定位精度再高，也要面临信号源误差的干扰问题。
目前，即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术是机器人领域和计算机科学领域的一个热点研究方向[10]。机器人在煤矿井下的应用依然停留在人工控制或重复性作业工作中，未实现智能化和通用化。在煤矿安全问题日益突出的背景下，煤矿无人化生产的需求已经被提上日程。智能化机器人在井下的应用必将不可或缺[11]。GMapping算法是SLAM算法中比较成熟稳定的一种[12-13]，已被广泛应用于普通室内机器人。
本文对现有的井下定位算法进行充分研究之后，针对其定位精度差和依赖信号源坐标的问题，提出了一种基于GMapping算法与指纹地图构建的井下定位方法。部署有GMapping算法的移动机器人在井下完成独立建图的同时，记录信号源信号特征，并绘制信号分布图。在定位阶段，待定位的人员或设备通过一致的信号采集设备采集信号特征，并与信号分布图做匹配，选择最优定位坐标。本文重点优化了信号分布图的构建算法和搜索匹配算法，提高了算法工作效率。
基于GMapping算法与指纹地图构建的井下定位方法由2个阶段组成：测绘阶段与恢复阶段。在测绘阶段，测量井下环境中信号强度分布情况，并与环境物理结构相映射后形成信号分布图，机器人移动过程中结合GMapping算法和路径规划算法完成自主导航功能。测绘阶段的工作分为2个部分：移动机器人建立物理结构地图与自身定位，信息采集与建立信号分布图。2个部分工作并行完成，完成后将数据存储并上传到数据库。物理结构地图仅包含井下二维平面结构，在SLAM工作中实时构建，用于移动机器人自主导航与自身定位，也可作为井下巷道结构的参考地图。信号分布图用于描绘信号分布情况，并与物理结构地图的坐标通过一一映射形成数据表。在恢复阶段，将当前位置采集到的指纹数据与信号分布图做匹配，得出最优定位坐标。本文重点考虑了信号分布图的查找策略，提出了一种数据存储结构和最小生成树搜索算法，提高了恢复过程的搜索效率。
WiFi网络在煤矿井下的应用已经比较广泛，在普通巷道中平均每50～100 m均会部署WiFi热点，因此，本文采用WiFi通信技术，通过接收周围WiFi信号节点的接收信号强度指示(Received Signal Strength Indicator，RSSI)值来实现定位。下文将指纹数据称为信号坐标。
2.1 信号分布图数据结构
要实现基于GMapping算法与指纹地图构建的井下定位方法，需要解决的一个问题是信号分布图的存储和查找。如果采用连续化恢复建图，每个信号节点的信号坐标都将存储为一幅地图，以信号节点作为基本单位，用向量表示为
G={{ξn,ξxn,ξyn|n∈N*}(k)|k∈(1,K)}
(1)
式中：n为采样点编号；ξn为对应采样点n处的RSSI值的匹配结果；ξxn,ξyn为对应采样点n处的坐标(xn,yn)的匹配结果；k为信号源节点编号；K为信号节点总个数。
这样的存储方式会浪费存储空间，但对算法进行优化后匹配会比较迅速。另外，采用叠加建图，以采样点坐标作为基本单位，将每个采样点所有信号节点的RSSI值累积后存储，用向量表示为
k∈(1,K)}
(2)
式中
为第k个信号源标签，即WiFi发射节点的MAC地址。
这样存储地图的空间开销小，虽然匹配复杂一些，但避免了连续化恢复建图中多节点求解唯一坐标的运算；虽然图像表达信息不如连续化恢复建图方法全面，但表示单一、简洁，反而有利于监控。由于叠加建图不适用于搜索匹配，本文选用连续化恢复建图方式来处理信号分布情况。采用连续化恢复建图是为了能在有限的计算空间内尽可能快地完成定位过程。每个信号节点生成一幅单节点信号分布图，表示为
SL(k)={ξn,ξxn,ξyn|n∈N*}
(3)
整个井下的信号分布图称为全信号分布图，是由所有单节点信号分布图组成的集合。所有单节点信号分布图采用无向图的方式组织在一起，如图1所示。每个单节点信号分布图作为一个顶点，用Tk来表示，存在信号叠加的节点间采用边来连接，边的权值ρmn为信号节点的关联程度，m,n∈(1,K)。
图1 信号分布图存储结构
Fig.1 Radiomap storage structure
对同时采集到1个以上节点的采样点进行计数，如某次采样同时采集到{T1,T2}的RSSI值，则2个节点的关联程度为
ω12=αω12_old
(4)
式中：ω12为节点1和节点2对应的关联程度；α为累积系数，默认取2；ω12_old为上次更新时节点1和节点2的关联程度，第1次更新时ω12取值为1。
边的权值用于在定位阶段搜索信号分布图时作为参考值代入算法，从而提高算法效率。边的权值ρmn取决于信号节点的关联程度ωmn：
ρmn=∑STkωmn
(5)
在算法实现上，采用索引表优化全信号分布图的搜索算法。全信号分布图使用二维邻接矩阵作为索引表A：
A={(1,r,ρ1r)1,(2,r,ρ2r)2,…，(k,r,ρk,r)(k)|r,
k∈(1,K)}
(6)
每一组索引元素对应一个指向的单节点信号分布图。在定位过程中，通过索引表查询全信号分布图中的单节点信号分布图，可在很大程度上提高算法效率。
2.2 信号分布图构建
2.2.1 单节点信号分布图构建算法
单节点信号分布图与物理结构地图的构建同步进行，采用栅格建图的思路。在正交化的栅格SL(k)中，每格栅格存储值表示最小分辨率R下的信号坐标中的RSSI值，R取值与结构子地图最小分辨率相同。设信号采样频次为P，在采样周期内，记录当前采样值ξP，并结合当前单节点信号分布图位置估计值(ξx,ξy) 形成信号坐标。在局部匹配过程中，每格位置坐标下会记录一系列可能的 {ξs|s∈(1,P)}。为了运算便捷并提高准确度，将RSSI值换算为功率的负倒数形式：
(7)
在结构子地图融合到单节点信号分布图中时，每个位置坐标的RSSI确定值就转换为求取最小二乘的问题：
(8)
求取
最优值
将对应的RSSI值写入单节点信号分布图栅格位置，作为当前位置最优分布值。在采样周期结束后，单节点信号分布图创建完成。创建完成后，不会再有新的信号坐标加入该信号分布图中，该信号分布图将参与到全信号分布图关联构建中。
2.2.2 全信号分布图关联算法
全信号分布图采用无向图的存储结构，将每个单节点信号分布图作为图中的一个顶点。每次添加时，检查全信号分布图中是否已经保存有该节点的节点序号，匹配该单节点信号分布图与全信号分布图的相似性。出于计算复杂度考虑，信号分布图的每一个像素位置通过双线性插值后得出，然后更新全信号分布图。同时，如果某个节点的序号不在当前正在构建的单节点信号分布图中，该节点序号将首先遍历全信号分布图的索引表A，查询匹配的节点序号关系，从而快速查找该节点是否已经存在于全信号分布图中。若存在，则调取对应的信号分布图到内存重新匹配，若不存在，则创建新的单节点信号分布图。
全信号分布图匹配构建算法流程如图2所示。
2.3 定位方法实现
在定位阶段，需要配合之前完成的物理结构地图和信号分布图，采集实际场景中定位坐标未知的信号坐标，并搜索信号分布图中最适合的匹配项，然后映射到物理结构地图中寻找到坐标，最终实现人员或设备的定位。
全信号分布图的搜索过程即求取无向图中最小生成树的过程，在最小生成树的搜索路径下可以找到最优路径。采用Kruskal算法[14]求取最小生成树，流程如图3所示。
求取最小生成树之后，利用最小生成树来优化全信号分布图的搜索效率。首先通过观测到的信号量中的L(k)值来选定某个节点Tk，值得一提的是，必须要保证至少有3个信号源的信号被观测到，才可以通过3点或多点匹配法计算坐标值。然后通过ρmn值依次匹配所有与该选定节点相邻的节点，选取出最小匹配节点对{m,n,…,l}⊂(1,K)，其对应的信号节点坐标集合为{Tm,Tn,…,Tl}。通过该观测信号量的信号值索引出最小匹配节点对中每个节点的单节点信号分布图，并求取对应的坐标对Cs={(xm,ym),(xn,yn),…,(xl,yl)}，Cs又称为定位坐标簇，定位坐标之间相互独立，属于稀疏数据集。
最终位置坐标通过最小圆覆盖算法得出，用式(9)对定位坐标簇进行处理：
图2 全信号分布图匹配构建算法流程
Fig.2 Flow diagram of the global radiomap matching and construction algorithm
图3 Kruskal算法生成最小生成树流程
Fig.3 Flow diagram of the Kruskal algorithm to create minimum spanning tree
(9)
式中
为最小圆圆心坐标，即最优坐标；f()为最小圆算法的基本要求，用欧拉距离表示；(xk,yk)为选定的节点k处对应的物理坐标。
采用递归算法来求取最优坐标，流程如图4所示。
图4 最优坐标求解算法流程
Fig.4 Flow diagram of optimal coordinate solution algorithm
定位过程如图5所示。
图5 定位过程
Fig.5 Processing of the localization
3.1 硬件平台
移动机器人采用双轮差速移动底盘，最大速度为0.7 m/s，最大爬坡角度为15°。安装有超声波传感器与激光扫描测距仪UTM-30LX LIDAR，用于避障与路径规划。移动机器人运行在32位Linux系统上，搭配LIDAR、超声波传感器、旋转编码器及无线路由器等传感器和直流电动机、驱动、电池等硬件，完成底盘驱动、SLAM、路径导航、避障、信号采集驱动及服务器通信等功能。
3.2 试验环境
选择与巷道结构类似的长廊模拟井下巷道，长廊总长约100 m，回环结构，廊壁垂直。在夜间试验，以模拟井下光照度差、明暗度不均匀的条件。长廊中布置6个WiFi信号节点，均运行在2.4 GHz频段。实际试验环境结构如图6所示，图中黑色标注点为WiFi信号节点位置。
图6 实际试验环境结构
Fig.6 Structure of actual test environment
3.3 试验结果
3.3.1 建图算法验证
在实际试验环境中反复多次测试算法，并通过调整移动机器人的运行参数来寻找最优的建图效果。图7(a)是通过SLAM构建的物理结构地图，图7(b)是2号WiFi热点的单节点信号分布图。
(a) 物理结构地图
(b) 单节点信号分布图
图7 物理结构地图与单节点信号分布图
Fig.7 Structuremap and single radiomap
实际地图测绘品质见表1。
表1 实际地图测绘品质
Table 1 Measuring quality of actual map
单节点信号分布图展示了一个WiFi信号源产生的信号分布情况，在实际定位中，坐标匹配阶段依赖于所有节点信号分布图在三维空间中的展开。从地图测绘品质数据可以得出，机器人移动速度过快会影响建图精度，而激光雷达的采样频率对物理结构地图也有一定影响。因此，精确的定位结果需要依赖于物理结构地图的准确构建。
3.3.2 定位方法验证
采用所提算法对匹配信号分布图进行定位，测试定位精度，其中一次测量的数据如图8所示。其中，A,B分别为起点和终点；坐标x,y为试验环境的二维坐标值。经分析，定位轨迹与真实轨迹的最优相关度达到0.854，平均定位误差为57.7 cm，最小定位误差为11 cm。
(a) 坐标估计结果
(b) 位移估计结果
图8 坐标估计结果与位移估计结果
Fig.8 Results of coordinate estimation and displacement estimation
(1) 提出一种基于GMapping算法与指纹地图构建的井下定位方法，通过将定位方法与煤矿机器人SLAM技术相结合，巧妙地避免了传统指纹定位方法人工构建信号分布图时维护困难的问题。同时，通过改进信号分布图的存储结构，有效提高了在定位阶段搜索与匹配坐标的效率，使定位方法更易于应用。
(2) 经过实际环境验证，所提定位方法定位精度较高，在100 m×30 m的长廊环境下，依赖于有限的WiFi热点，平均定位误差为57.7 cm。定位精度依赖于定位设备在地图构建阶段对井下结构的准确绘制。
(3) 所提定位方法也适用于ZigBee定位、RSSI定位[15]、射频识别(Radio Frequency Identification, RFID)定位[16]等。另外，算法效率依然是一个值得研究的问题，如果移动机器人在定位时移动速度过快，定位精度会受到影响，所以必须提高定位过程的运算速率。
参考文献(References)：
[1] LIU H,DARABI H,BANERJEE P,et al.Survey of wireless indoor positioning techniques and systems[J].IEEE Transactions on Systems,Man and Cybernetics,Part C(Applications and Reviews),2007,37(6):1067-1080.
[2] DING E J,WANG M Y,WEN J C,et al.Performance evaluation of 2.4 GHz wireless sensor nodes transmission in coal mine[C]//2009 WRI World Congress on Computer Science and Information Engineering,Los Angeles,2009.
[3] SCHOLL P M,KOHLBRECHER S,SACHIDANANDA V,et al.Fast indoor radio-map building for RSSI-based localization systems[C]//9th International Conference on Networked Sensing Systems,Antwerp,2012.
[4] MELAMED R.Indoor localization:challenges and opportunities[C]//Proceedings of the International Workshop on Mobile Software Engineering and Systems,Austin,2016.
[5] ZHOU S,WANG B,MO Y,et al.Indoor multi-resolution subarea localization based on received signal strength fingerprint[C]//International Conference on Wireless Communications and Signal Processing,Huangshan,2012.
[6] XIAO Y.IEEE 802.11n:enhancements for higher throughput in wireless LANs[J].IEEE Wireless Communications,2005,12(6):82-91.
[7] BULUSU N,HEIDEMANN J,ESTRIN D.GPS-less low-cost outdoor localization for very small devices[J].IEEE Personal Communications,2000,7(5):28-34.
[8] MADIGANL D,EINAHRAWY E,MARTIN R P,et al.Bayesian indoor positioning systems[C]//24th Annual Joint Conference of the IEEE Computer and Communications Societies,Miami,2005.
[9] AL ALAWI R.RSSI based location estimation in wireless sensors networks[C]//17th IEEE International Conference on Networks,Singapore,2011.
[10] THRUN S,LEONARD J J.Simultaneous localization and mapping[M]//SICILIANO B,KHATIB O.Springer handbook of robotics.Berlin:Springer Berlin Heidelberg,2008:871-889.
[11] SANTOS J M,PORTUGAL D,ROCHA R P.An evaluation of 2D SLAM techniques available in robot operating system[C]//IEEE International Symposium on Safety,Security,and Rescue Robotics,Linkoping,2013.
[12] 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.
[13] GRISETTIYZ G,STACHNISS C,BURGARD W.Improving grid-based SLAM with rao-blackwellized particle filters by adaptive proposals and selective resampling[C]//Proceedings of the 2005 IEEE International Conference on Robotics and Automation,Barcelona,2005.
[14] WEISS M A.Data structures and algorithm analysis in C[M/OL].[2017-04-06].http://svslibrary.pbworks.com/f/Data%2BStructures%2Band%2BAlgorithm%2BAnalysis%2Bin%2BC%2B-%2BMark%2BAllen%2BWeiss.pdf.
[15] MORRAL G,DIENG N A.Cooperative RSSI-based indoor localization: B-MLE and distributed stochastic approximation[C]//IEEE 80th Vehicular Technology Conference,Vancouver,2014.
[16] NI L,ZHANG D,SOURYAL M.RFID-based localization and tracking technologies[J].IEEE Wireless Communications,2011,18(2):45-51.
Underground positioning method based on GMapping algorithm and fingerprint map construction
JIANG Lei, YANG Liuming, WU Fangda, HAN Huijie, ZHOU Xue
(School of Mechanical Electronic and Information Engineering, China University of Mining and Technology(Beijing), Beijing 100083, China)
Abstract:In view of problems of poor accuracy and dependence on source coordinates of existing underground positioning algorithm, an underground positioning method based on GMapping algorithm and fingerprint map construction was proposed. The signal characteristics of unknown position is matched with signal distribution map, and the optimal positioning coordinates is selected, thereby improving accuracy of the underground positioning; by combining with GMapping algorithm, problem of high maintenance cost of fingerprint map construction process is avoided, and searching and matching efficiency of the algorithm is optimized. Actual test results show that the average positioning error of the proposed method is 57.7 cm, which can meet requirements of underground positioning.
Key words:underground positioning; simultaneous localization and mapping construction; GMapping algorithm; fingerprint map; signal distribution map
收稿日期：2017-04-27；
修回日期：2017-07-17；责任编辑：胡娴。
作者简介：蒋磊(1982-)，男，安徽淮北人，讲师，主要研究方向为控制理论与控制工程，E-mail：xxlei@vip.sina.com。通信作者：杨柳铭(1994-)，男，内蒙古鄂尔多斯人，硕士研究生，主要研究方向为煤矿通信与定位感知，E-mail:dickyangliuming@163.com。
引用格式：蒋磊，杨柳铭，武方达，等.基于GMapping算法与指纹地图构建的井下定位方法[J].工矿自动化，2017,43(9)：96-101. JIANG Lei, YANG Liuming, WU Fangda, et al. Underground positioning method based on GMapping algorithm and fingerprint map construction[J].Industry and Mine Automation，2017,43(9)：96-101.
文章编号：1671-251X(2017)09-0096-06
DOI：10.13272/j.issn.1671-251x.2017.09.017
中图分类号：TD655.3
文献标志码：A 网络出版时间：2017-08-28 11:48
网络出版地址：http://kns.cnki.net/kcms/detail/32.1627.TP.20170828.1148.017.html