0 引言
目前煤矿井下人员和车辆定位方案主要基于RFID(Radio Frequency Identification，无线射频识别)、WiFi、ZigBee、UWB(Ultra Wideband，超宽带)等技术[1-3]，由于井下工作环境恶劣，这些方案存在各自缺点：RFID射频模块是被动式的定位装置，无法实现实时定位，一般只能用于人员考勤；基于WiFi的定位方案功耗较高，而且只能通过检测WiFi网络的覆盖来实现区域定位；基于ZigBee、UWB的定位方案需要相对复杂的硬件设备及较高的成本。基于蓝牙协议的井下车辆及人员定位技术采用无基站信标定位方式，定位精度为5～10 m，具有成本低、易部署和维护、功耗低、实时性高等优点，但是也存在在信号区域内距离不可分辨、两站中间无信号时不能进行定位的问题[4-6]。
井下定位是一种特殊的室内定位，所受限制较多，如室内在二维平面上可以使用RSSI算法交汇得到蓝牙定位信息，但在井下巷道条件下只能获得蓝牙定位的区域信息[7-8]。井下纯惯性导航要想得到较高精度，成本较高，而低成本MEMS(Micro-Electro-Mechanical System，微机电系统)惯性传感器漂移较大，惯性导航短时间内就会发散，导致无法使用。针对该问题，提出井下多传感器组合导航系统，采用低成本MEMS惯性传感器、蓝牙传感器、里程计组合导航方法，利用卡尔曼滤波技术融合多传感器数据，得到高精度井下定位结果，配合车载终端，为井下车辆和地面监控室提供实时、准确的导航监控信息，方便车辆调度、安全管理等。
1 系统结构
多传感器组合导航系统主要由导航模块和本安车载终端组成，如图1所示。导航模块包括蓝牙探测器、MEMS惯性传感器、里程计接口及处理器等，本安车载终端包括电源模块、显示屏、处理器、存储器、WiFi模块等。
图1 多传感器组合导航系统结构
Fig.1 Structure of multi-sensor integrated navigation system
系统通过蓝牙测距信息、MEMS惯性传感器及车载里程计信息进行组合导航。蓝牙探测器用于探测蓝牙标签的信号强度信息，经处理器处理后转换为导航模块与蓝牙标签的距离信息(噪声波动较大)[9-10]。MEMS惯性传感器包括三轴MEMS陀螺、三轴MEMS加速度计、磁强计，可获得导航模块的三轴角速度、加速度、磁航向信息，传感器信息经过RS232接口送入处理器，经导航解算可得到载体位置、速度、姿态信息。通过里程计接口可以接入车载里程计信息。解算后的导航数据通过RS232接口送入本安车载终端，本安车载终端内部存储有矿井地图，可配合导航数据显示车辆位置。本安车载终端通过自带的WiFi将车辆行驶信息发送到地面监控室，并可接收地面监控室发送的指令和数据。
组合导航系统整机效果如图2所示。导航模块作为独立模块，通过电缆与本安车载终端配合进行数据交互。导航模块预留外部通信接口，根据实际情况可接入更多传感器。
图2 多传感器组合导航系统整机效果
Fig.2 Whole machine effect of multi-sensor integrated navigation system
2 系统工作流程
多传感器组合导航系统工作流程如图3所示。
图3 多传感器组合导航系统工作流程
Fig.3 Work process of multi-sensor integrated navigation system
(1) 检查导航模块与本安车载终端的CAN接口连接是否正常，若未连接则进行手动连接。
(2) 车辆启动，电源开始供电，本安车载终端自动开机，导航模块开机。
(3) 导航模块搜索附近蓝牙标签，若没有发现标签，持续搜索；若发现标签，则结合本安车载终端中的地图信息确定起始标签和目的标签，开始导航。
(4) 组合导航。导航模块在标签数据有效且具有参考价值的区域实现蓝牙/MEMS惯性传感器数据融合，修正位置信息，同时向本安车载终端输出起始标签ID、坐标、航向信息、速度及总里程信息。
(5) 在蓝牙标签数据无效区域，由MEMS惯性传感器/里程计组合维持位置递推，并通过里程计较为准确的速度信息修正MEMS惯性传感器产生的漂移。
(6) 再次进入标签数据有效且具有参考价值的区域，发现新标签，若满足切换条件，则进行标签切换。
重复步骤(4)、步骤(6)，直至导航结束。
由于MEMS惯性传感器在短时间内可以维持一定精度，所以布站间隔可以设置为几十米甚至上百米。
3 数据融合原理
在低成本MEMS惯性传感器系统中，由于陀螺精度较低，不能反映地球自转信息，所以，可以对捷联惯导误差传播方程进行简化，简化后的方程如下[11-13]：
(1)
(2)
(3)
(4)
(5)
(6)
(7)
式中：φi为姿态失准角,i=E,N,U(E表示东向，N表示北向，U表示天向)；Vi为速度；R为地球半径；L、
、h分别为当地纬度、经度、高度；
分别为地理坐标系内陀螺的等效漂移和加速度计的等效偏置；εWi为陀螺角增量输出的随机游走；fi为比力加速度；g为重力加速度。
状态变量为
X=[ΔLΔΔhΔVEΔVNΔVU φE φN
(8)
观测变量包含蓝牙标签位置和惯导位置的3个位置误差信息，以及里程计瞬时速度和惯导速度的3个速度误差信息。
观测方程为
(9)
式中：Z(k)(k=1,2,…,6)为实际3个位置、3个速度的观测误差；vk为观测噪声。
由式(1)—式(8)构建卡尔曼滤波器的状态方程，由式(9)构建卡尔曼滤波观测方程[14-15]，采用离散型卡尔曼滤波方程对MEMS惯性传感器误差作最优估计[16-17]。根据巷道特点加入坐标约束和零速校正。在实际应用中，利用蓝牙探测器所观测到的2个蓝牙标签的距离与惯性传感器所测量得到的2个蓝牙标签距离进行比较，作为位置观测量输入。采用卡尔曼滤波估计MEMS惯性传感器加速度计零偏和陀螺漂移并进行修正。这样车辆连续的位置、速度、姿态信息由MEMS惯性传感器输出，而蓝牙标签观测量的作用是抑制MEMS传感器漂移，保证MEMS惯性传感器能够长时间有效工作。
低精度惯导系统在导航初始化时，静止状态下可以由三轴加速度关系获得粗略的水平姿态角，但是，由于陀螺精度太低，不能完成方位自主对准，而只能采用地磁测量或者依靠矿井地图的巷道方向信息来辅助进行方位对准。
4 仿真
蓝牙标签按照10 m等间隔布站，模拟小车初始位置为(5 m，10 m)，速度为(3 m/s，0 m/s)，若不采用MEMS惯性传感器，蓝牙标签每10 m段的观测噪声幅值为3 m，设定惯导速度误差为0.3 m/s，进行仿真分析，结果如图4所示。数据融合后蓝牙观测噪声被平滑掉，前段为卡尔曼滤波收敛的过程，初始误差较大，滤波收敛后误差在1 m以内。
滤波前后的误差情况如图5所示，滤波后蓝牙观测噪声降低，综合位置估计精度得到提高。MEMS惯性传感器速度误差估计如图6所示，估计值正确收敛到设定的误差值。
在速度观测中加入里程计测速作为共同的速度观测量时，需要先在线标定里程计，消除胎压引起的测速误差，量化噪声是观测噪声的一部分。由以上分析可以看出，利用蓝牙标签已标定位置信息就可以对MEMS惯性传感器参数变化进行估计修正，而数据融合后，蓝牙噪声也得到了减小和平滑。仿真定位精度为1.5 m，满足使用要求。
图4 蓝牙/MEMS组合测量仿真结果
Fig.4 Simulation result of Bluetooth/MEMS combination measurement
图5 滤波前后的误差对比
Fig.5 Comparison of error before and after filtering
图6 MEMS惯性传感器速度误差估计
Fig.6 Speed error estimation of MEMS inertial sensor
5 测试验证
5.1 地面测试
在某企业园区内按照如图7所示位置将蓝牙标签布置于灯杆上，标签距地面2 m左右。地面标签安装方式及柴油车测试现场如图8所示，将标签ID及位置信息提前存储在本安车载终端的导航地图中。
图7 标签布置
Fig.7 Label placement
图8 标签安装方式及柴油车测试现场
Fig.8 Label installation method and diesel vehicle test site
将导航模块安装于车辆顶部，车辆沿蓝牙标签所在道路行驶，将导航模块导航坐标转换为距离信息，并与实际距离比较，以验证导航模块精度。车辆运行过程中导航模块探测到的与蓝牙标签的距离如图9所示，可以看出，导航模块在各个标签处发生切换。
图9 导航模块测距结果
Fig.9 Navigation module ranging result
导航模块输出的速度如图10所示，可以看出，惯导的测量速度在蓝牙测量数据的影响下发生了修正。
柴油车在园区长时间运动情况下测得的相对距离、速度及运动轨迹如图11—图13所示。测试结果稳定可靠，惯导航向漂移由观测到的蓝牙标签信息进行估计校正。
图10 惯导测速及观测修正结果
Fig.10 Inertial navigation velocity measurement and observation correction
图11 长时间运动情况下测得的相对距离
Fig.11 Relative distance measured under
long-term motion
图12 长时间运动情况下测得的速度
Fig.12 Speed measured under long-term motion
图13 导航模块测得的车辆运动轨迹
Fig.13 Vehicle trajectory measured by integrated navigation module
在园区内测试组合定位精度，结果见表1。可见，每10 m误差在3 m以内。在无标签情况下，导航模块也可以维持几十秒的纯惯性导航，并保持一定精度。
表1 组合定位精度地面测试数据
Table 1 Combined positioning accuracy ground test data
5.2 井下测试
在地面测试良好的状态下，对导航模块进行井下测试。在某煤矿井下主要巷道内，按10 m间隔将蓝牙标签布置于右侧巷道壁，标签距地面高度约为2 m，测试路线如图14所示。井下标签实际安装位置及车辆测试现场如图15所示。
图14 井下测试路线
Fig.14 Underground test route
图15 井下标签安装位置及柴油车测试现场
Fig.15 Underground label installation location and diesel vehicle test site
将导航模块安装于柴油车驾驶室仪表盘上方塑料盖板上，车辆沿标签所在道路行驶，平均速度为6 m/s左右，用导航模块测量柴油车的位置、速度、航向信息，计算蓝牙标签之间的距离，并与实际距离比较，以验证导航模块定位精度，测试数据见表2。可见，井下测试每10 m误差在3.2 m以内，在无标签情况下，导航模块也可维持约5 min的纯惯性导航，并保持一定精度。
表2 组合定位精度井下测试数据
Table 2 Underground combined positioning accuracy test data
采用导航模块测得的柴油车在井下全段运动轨迹如图16所示。对照图14可以看出，测得的轨迹与实际线路轮廓基本吻合，总里程数也对应，且图14左边部分建模为竖直方向，而多次实测该巷道是稍有倾斜的，导航模块测得的车辆运动轨迹符合实际情况，验证了系统的实用性。
图16 导航模块测得的井下车辆运动轨迹
Fig.16 Underground vehicle trajectory measured by navigation module
虽然测试时巷道环境较为复杂，蓝牙信号存在折反射情况，但是导航模块具有一定的容错机制，通过轨迹预测可以滤除掉大部分蓝牙干扰标签。
6 结语
井下多传感器组合导航系统将蓝牙测距信息、MEMS惯性传感器及车载里程计信息进行组合，采用卡尔曼滤波技术融合多传感器数据，能够获得较单一传感器更好的导航精度和稳定性。该系统通过MEMS惯性传感器信息进行轨迹预测，降低蓝牙位置探测噪声；结合巷道地图信息来约束航向姿态的发散，从而减小航向估计误差。仿真结果表明，利用蓝牙标签已标定位置信息就可以对MEMS惯性传感器参数变化进行估计修正，而数据融合后，蓝牙噪声也得到了降低和平滑；仿真定位精度为1.5 m，满足使用要求。地面测试结果表明，每10 m定位误差在3 m以内；井下测试结果表明，每10 m误差在3.2 m以内。
参考文献( References)：
[1] 赵锐,钟榜,朱祖礼,等.室内定位技术及应用综述[J].电子科技,2014,27(3):154-157.
ZHAO Rui,ZHONG Bang,ZHU Zuli,et al.Overview of indoor localization techniques and applications[J].Electronic Science and Technology,2014,27(3):154-157.
[2] 张浩,赵千川.蓝牙手机室内定位系统[J].计算机应用,2011,31(11):3152-3156.
ZHANG Hao,ZHAO Qianchuan.Indoor positioning system for Bluetooth cell phone[J].Journal of Computer Applications,2011,31(11):3152-3156.
[3] 杨萍,李智.一种基于RSSI的权重值选择及加权定位算法[J].信息与电子工程,2012,10(2):148-151.
YANG Ping,LI Zhi.Weight-select and weighted location algorithm based on RSSI[J].Information and Electronic Engineering,2012,10(2):148-151.
[4] 卞合善.基于蓝牙4.0低功耗室内定位研究[D].北京:北京邮电大学,2015.
[5] 陆杨洁.基于蓝牙4.0的定位算法研究及系统实现[D].绵阳:西南科技大学,2016.
[6] 刘沛丰.基于WiFi/蓝牙融合的室内定位算法研究[D].徐州:中国矿业大学,2017.
[7] WU Chenshu, YANG Zheng, LIU Yunhao, et al. WILL:Wireless indoor localization without site survey[J]. IEEE Transactions on Parallel and Distributed Systems, 2012,24(4):64-72.
[8] 华海亮.基于WiFi和蓝牙的室内定位技术研究[D].锦州:辽宁工业大学,2016.
[9] CHEN L,PEI L,KUUSNIEMI H,et al.Bayesian fusion for indoor positioning using Bluetooth fingerprints[J].Wireless Personal Communications, 2013,70(4):1735-1745．
[10] 韩旭海,夏文龙,周渊平.基于线性加权的蓝牙室内定位算法[J].计算机系统应用,2015,24(1):119-122.
HAN Xuhai,XIA Wenlong,ZHOU Yuanping.Bluetooth indoor positioning based on linear weighted[J].Computer Systems & Applications,2015,24(1):119-122.
[11] 王缓缓,宫娜娜.基于距离区间概率加权的RSSI测距方法[J].电子科技大学学报,2013,42(6):862-868.
WANG Huanhuan,GONG Nana.RSSI ranging method based on the weighted distance interval probability[J].Journal of University of Electronic Science and Technology of China,2013,42(6):862-868.
[12] 宋宇,喻文举,程超,等.基于WiFi、PDR、地磁相融合的室内定位研究[J].微电子学与计算机,2018,35(6):60-64.
SONG Yu,YU Wenju,CHENG Chao,et al.Research on indoor positioning based on fusion of WiFi,PDR and geomagnetism[J].Microelectronics & Computer,2018,35(6):60-64.
[13] 李金凤,王庆辉,刘晓梅,等.基于MEMS惯性器件的行人室内定位系统[J].计算机测量与控制,2014,22(11):3761-3763.
LI Jinfeng,WANG Qinghui,LIU Xiaomei,et al.An indoor pedestrian positioning system based on MEMS inertial devices[J].Computer Measurement & Control,2014,22(11):3761-3763.
[14] 秦永元.惯性导航[M].2版.北京:科学出版社，2014.
[15] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合定位原理[M].3版.西安：西北工业大学出版社,2015.
[16] 代汝勇.基于MEMS与智能手机电子罗盘的室内定位与导航算法研究[D].北京:北京工业大学,2016.
[17] 李桂娥.知识引导下室内蓝牙定位精度优化[D].武汉:华中师范大学,2017.