0 引言

现有煤矿井下人员定位系统大多基于无线技术实现人员定位[1-3]。基本模式是在矿井巷道铺设传输总线和搭建定位基站，井下人员携带定位终端，当经过定位基站工作范围内时与定位基站建立通信并进行电磁波测距，以获得相对准确的定位终端自身位置，将定位终端自身位置与编号等信息发送至定位基站，定位基站通过传输总线将定位终端信息发送至地面监控中心，从而完成实时定位监控。飞行时间(Time of Flight，TOF)测距定位方法具有设备布设简单、能耗低、定位精度不受信号强度及发射功率等因素影响的特点[4-6]，被广泛应用于煤矿井下人员定位系统中，然而该定位方法存在以下缺点：① 在基站无法覆盖的区域容易造成定位盲区[7]，而实现全覆盖需要铺设大量基站，导致成本高。② 基于无线通信的测距技术在视距通信时效果较好，在非视距环境效果较差，而在煤矿井下非视距环境普遍存在[8]。③ 在地形复杂区域及经过大型机电设备附近时，无线通信受噪声干扰严重，导致定位精度受限。

捷联惯性导航定位是一种既不需要与外部交互信息、也不向外部辐射能量的自主定位方法[9]。其工作原理是通过对惯性传感器元件(陀螺仪和加速度计)的测量结果进行积分运算，获得速度和位置等导航信息。捷联惯性导航定位具有在短时间内获得较高的定位精度、不容易受外界干扰及设备携带方便等优点，但独立工作时定位误差会随着工作时间的增加逐渐增大[10-12]。

结合TOF测距定位方法和捷联惯性导航定位方法的特点，本文提出了一种煤矿井下人员融合定位方法。当定位终端位于定位基站通信覆盖范围内时，采用TOF测距定位方法，当定位终端位于定位基站通信覆盖范围之外时，采用捷联惯性导航定位方法，并分别通过近距离和远距离无线通信方式向定位基站传输定位数据，实现人员轨迹和位置坐标的实时监控。

1 相关原理

1.1 TOF测距定位

TOF测距定位原理是通过测量信号在收发节点间的往返时间来计算两节点之间的距离，从而完成定位估计。为减小测距过程中因时钟偏移而导致的测距误差，采用TOF对称双向双边测距方法，其原理如图1所示。定位终端向定位基站发送一个测距请求的数据包，定位基站接收数据包后对其进行处理，处理完成后向定位终端发送一个确认响应的数据包，此时定位终端记录从发送数据包到接收数据包的时间T1，定位基站记录从接收数据包到回复数据包的时间T2。再由定位基站向定位终端发送一个测距请求的数据包，此时请求端与响应端互换，重复请求响应过程，定位基站记录从发送数据包到接收数据包的时间T3，定位终端记录从接收数据包到回复数据包的时间T4。则定位终端和定位基站之间的距离为

(1)

式中C为信号传播速度。

图1 TOF对称双向双边测距原理

Fig.1 Principle of TOF symmetrical double-sided two way ranging

定位终端将包含自身信息的数据包发送给就近3个定位基站，基于TOF对称双向双边测距原理计算定位终端分别与3个定位基站之间的距离(式(2))，并利用三边测距法确定定位终端位置，从而获得矿井人员位置信息。

(2)

式中：(x,y)为定位终端坐标；(x1,y1)，(x2,y2)，(x3,y3)分别为3个定位基站坐标；d1，d2，d3分别为定位终端到3个定位基站的距离。

1.2 捷联惯性导航定位

捷联惯性导航定位过程如图2所示(b表示载体坐标系，i表示惯性坐标系，t表示导航坐标系)。

图2 捷联惯性导航定位过程

Fig.2 Strapdown inertial navigation positioning process

陀螺仪测量出载体(移动目标)的角速度与导航坐标系相对惯性坐标系的角速度完成姿态矩阵的更新。加速度计测量出载体的比力(因地球自转影响，包含有害加速度)，通过姿态矩阵计算得到相对于导航坐标系的比力：

(3)

姿态矩阵通过欧拉角法计算，即载体的俯仰角θ、横滚角γ、航向角Ψ分别绕三轴旋转得到用于坐标系之间转换的姿态矩阵[13-14]：

(4)

经过有害加速度补偿后，得到载体相对导航坐标系的加速度a(t)，由各参数进行定位计算：对加速度进行一次积分运算可得t时刻载体移动速度v(t)，再对移动速度进行一次积分运算可得t时刻载体位置s(t)，同时通过姿态矩阵获取载体姿态角。

v(t)=v(t0)+a(t)dt

(5)

s(t)=s(t0)+v(t)dt

(6)

式中：v(t0)为t0时刻载体初始速度；s(t0)为t0时刻载体初始位置。

捷联惯性导航定位方法在短时间内有较高定位精度，但定位误差随时间不断累计[15-17]，因此本文采用卡尔曼滤波算法得到最优误差估计进行反馈校正，实现精确定位。

选取状态误差参数：

X=[Δve,Δvn,Δvu,Δse,Δsn,Δsu,Δφe,Δφn,Δφu,

Δεe,Δεn,Δεu,ΔNe,ΔNn,ΔNu]T

(7)

式中：Δve，Δvn，Δvu分别为东、北、天方向的速度误差；Δse，Δsn，Δsu分别为东、北、天方向的位置误差；Δφe，Δφn，Δφu分别为东、北、天方向的姿态角误差；Δεe,Δεn,Δεu分别为东、北、天方向的陀螺仪零偏误差；ΔNe，ΔNn，ΔNu分别为东、北、天方向的加速度计零偏误差。

状态方程和观测方程[18-20]为

(8)

式中：Φ为状态转移矩阵；W为系统噪声矩阵；Z为观测矩阵；H为量测矩阵；V为观测噪声矩阵。

卡尔曼滤波的最优误差估计包括以下步骤。

① 状态一步预测：

(9)

② 均方误差一步预测：

P(t|t-1)=Φ(t|t-1)P(t-1)ΦT(t|t-1)+

W(t-1)Q(t-1)WT(t-1)

(10)

③ 滤波增益：

K(t)=P(t|t-1)HT(t)(H(t)P(t|t-1)HT(t)+

P(t))-1

(11)

④ 状态估计：

(12)

⑤ 均方误差估计：

P(t)=(I-K(t)H(t))P(t|t-1)

(13)

式中：为状态估计矩阵；P为均方误差矩阵；Q为系统噪声方差矩阵；K为滤波增益矩阵；I为单位矩阵。

经过最优误差修正后的最优速度估计和最优位置估计为

(14)

(15)

式中：ve,vn,vu为速度估计;为最优速度误差；se,sn,su为位置估计;为最优位置误差。

2 融合定位

TOF测距定位方法具有能耗低、设备部署方便、定位精度不受信号强度和发射功率影响等优点，但受非视距影响较大，所以在定位精度要求不高且空间较宽阔的巷道(非视距误差小)应用效果好。捷联惯性导航定位方法具有抗电磁干扰能力强、不依赖外界信息及定位精度高等优点，适用于定位基站无线定位范围无法覆盖的巷道。

基于TOF测距定位方法和捷联惯性导航定位方法的特点，提出了一种煤矿井下人员融合定位方法，如图3所示。在主要巷道布设支持近距离和远距离2种无线通信方式的多通信融合定位基站。在近距离无线通信覆盖范围的边缘位置布设蓝牙信标，广播其位置信息；在近距离无线通信覆盖范围之外，每隔1 km布设1个蓝牙信标，为惯性导航单元提供校准信息(包括初始位置信息和方向信息)。定位终端由通过蓝牙无线连接的设备1和设备2组成，设备1由矿井人员随身携带，设备2固定在矿井人员的足部。当定位终端处于定位基站近距离无线通信覆盖范围内，采用TOF测距定位方式；当定位终端处于定位基站近距离无线通信覆盖范围之外，定位终端自动切换工作模式，由TOF测距定位转变为捷联惯性导航定位。

图3 煤矿井下人员融合定位

Fig.3 Fusion positioning of underground mine personnel

多通信融合定位基站电路组成如图4所示。近距离无线通信单元与定位终端进行TOF测距定位并接收定位数据；远距离无线通信单元在定位终端切换到捷联惯性导航定位模式时接收定位数据；定位基站通过总线传输单元向地面监控中心传输定位数据。

图4 多通信融合定位基站电路组成

Fig.4 Composition of multi-communication fusion positioning base station

定位终端电路组成如图5所示。设备1的近距离无线通信单元用于与多通信融合定位基站进行TOF测距定位和传输定位数据，远距离无线通信单元用于传输捷联惯性导航定位数据至定位基站；设备2的捷联惯性导航单元由MEMS惯性器件组成，蓝牙通信单元用于与设备1通信和接收巷道内蓝牙信标的位置校准信息。

煤矿井下人员融合定位流程如图6所示。定位终端首先发射近距离通信广播，定位基站接收到广播包，返回近距离通信应答。若定位终端接收到应答，说明定位终端在定位基站近距离无线通信覆盖范围内，则采用TOF测距定位方法，得到定位终端与定位基站之间的距离，进而获得定位终端相对定位基站的相对坐标，通过近距离无线通信方式将定位数据(相对坐标和时间戳)发送给附近的定位基站；若定位终端不能接收到应答，说明定位终端处于定位基站近距离通信覆盖范围之外，则采用捷联惯性导航定位方法，通过蓝牙通信启动捷联惯性导航单元，同时接收周围蓝牙信标提供的校准信息，得到定位终端在切换成捷联惯性导航定位模式后的位置和方向，进而获得定位终端相对定位基站的相对坐标，通过远距离无线通信方式将定位数据(相对坐标和时间戳)发送给附近的定位基站。如果定位终端处于定位基站远距离通信覆盖范围之外，则对惯性导航定位数据进行本地存储，在恢复通信后传输至定位基站。定位基站将定位数据传输至地面监控中心，地面监控中心的定位服务器根据定位终端的相对坐标、时间戳及定位基站的绝对坐标，获得定位终端在井下的绝对坐标，从而得到人员轨迹和位置。

(a) 设备1

(b) 设备2

图5 定位终端电路组成

Fig.5 Composition of positioning terminal

图6 煤矿井下人员融合定位流程

Fig.6 Flow of fusion positioning of underground mine personnel

3 实验验证

为验证本文方法的有效性，开发实验系统在井下进行实验。采用ZigBee技术进行近距离无线传输，使用CC2530芯片和Z-Stack协议栈设计定位终端组成设备1的近距离无线通信单元；采用LoRa技术进行远距离无线通信，使用远距离、低功耗的LoRa无线收发模块SX1278设计定位终端组成设备1的远距离无线通信单元；采用加速度传感器ADXL345、陀螺仪ITG-3205和数字罗盘HMC5883L设计定位终端组成设备2的捷联惯性导航单元；采用NRF51822设计蓝牙通信单元。

实验地点在北京京煤集团有限责任公司大台煤矿水平运输巷道，该巷道长1 200 m、宽5 m、高3.5 m，如图7所示。在A点布设定位基站，定位基站通过网口与计算机连接，通过计算机测试，设置AB段在定位基站ZigBee无线通信覆盖范围内，BC段在定位基站ZigBee无线通信覆盖范围之外、LoRa通信覆盖范围内，CD段在LoRa通信覆盖范围之外。在B点布设蓝牙信标，参与实验人员携带定位终端由A点出发行走至D点，再原路返回。

图7 定位实验路线

Fig.7 Positioning experimental route

经过多次重复实验发现，人员由A点行走至B点期间，定位终端处于TOF测距定位模式，定位精度为3～5 m；在B点处，定位终端切换至捷联惯性导航定位模式，能接收蓝牙信标的信号并通过LoRa通信将定位数据发送至定位基站，定位精度在1 m以内；当人员由C点行走至D点期间，定位终端自动将惯性导航定位数据进行本地存储，当人员返回由C点向B点行走时，定位终端能将存储的定位数据通过LoRa通信发送至定位基站。

计算机将惯性导航定位数据通过卡尔曼滤波处理后，与未经过卡尔曼滤波处理的定位数据进行对比，如图8所示(以B点位置为坐标原点,(x,y)为人员位置)。从图8可看出，行走距离达500 m后，无卡尔曼滤波的捷联惯性导航定位轨迹方向和位置偏移较大，这是由于陀螺仪和加速度计的累计误差逐渐增大；加入卡尔曼滤波后，方向和位置偏差得到有效纠正。

图8 定位实验结果

Fig.8 Positioning experimental results

4 结语

煤矿井下人员融合定位方法结合TOF测距定位和捷联惯性导航定位的优点，分区域定位。在定位基站近距离无线通信覆盖区域，采用TOF测距定位，仅需定位基站和定位终端，硬件设备少、结构简单，能有效快速定位，节约了通信成本和时间；在定位基站近距离无线通信无法覆盖区域，及时切换工作模式，转换为捷联惯性导航定位方式，并通过卡尔曼滤波算法进一步提高定位精度，具有很强的抗干扰能力；在定位基站远距离无线通信无法覆盖区域，将惯性导航定位数据进行本地存储，减少了定位盲区。

参考文献（References）：

[1] 孙哲星,孙继平.异步测时矿井人员精确定位方法[J].煤炭学报,2018,43(5):1464-1470.

SUN Zhexing,SUN Jiping.Underground coal mine accurate personnel positioning method based on asynchronous time-measuring[J].Journal of China Coal Society,2018,43(5):1464-1470.

[2] 赵彦如,苗炜丽.一种基于RFID技术的矿井人员定位系统[J].自动化与仪器仪表,2018(1):191-194.

ZHAO Yanru,MIAO Weili.A system of mine personnel positioning system based on RFID[J].Automation & Instrumentation,2018(1):191-194.

[3] 方水平,王怀群.浅析矿井人员定位系统中的定位技术[J].煤炭工程,2010,42(7):77-80.

FANG Shuiping,WANG Huaiqun.Analysis of mine personnel positioning system positioning technology[J].Coal Engineering,2010,42(7):77-80.

[4] 徐丽.基于RFID的矿井人员定位系统的设计[D].太原:太原理工大学,2013.

XU Li.A position system of mine personnel based on RFID[D].Taiyuan:Taiyuan University of Technology,2013.

[5] 田子建,朱元忠,张向阳,等.非视距误差抑制方法在矿井目标定位中的应用[J].工矿自动化,2015,41(6):78-82.

TIAN Zijian,ZHU Yuanzhong,ZHANG Xiangyang,et al.Application of non line-of-sight error suppression method in mine target location[J].Industry and Mine Automation,2015,41(6):78-82.

[6] 吴忠.基于TOF测距的无线传感器网络定位算法研究[D].合肥:合肥工业大学,2016.

WU Zhong.Research on the TOF-based localization algorithm in WSN[D].Hefei:Hefei University of Technology,2016.

[7] 陈康,王军,包建军,等.基于消息复用的TOF井下精确定位技术[J].工矿自动化,2019,45(2):1-5.

CHEN Kang,WANG Jun,BAO Jianjun,et al.TOF underground accurate positioning technology based on message multiplexing[J].Industry and Mine Automation,2019,45(2):1-5.

[8] 车志平.基于TOF测距的无线传感器网络定位技术研究[D].大连:大连理工大学,2016.

CHE Zhiping.The research of localization technology based on TOF ranging in wireless sensor network[D].Dalian:Dalian University of Technology,2016.

[9] 焦斌,范文胜.基于惯性导航的井下电机车精确定位系统的研究[J].现代矿业,2012,27(2):42-44.

JIAO Bin,FAN Wensheng.Research on precise positioning system of underground electric locomotive based on inertial navigation [J].Modern Mining,2012,27(2):42-44.

[10] 樊启高,李威,王禹桥,等.一种采用捷联惯导的采煤机动态定位方法[J].煤炭学报,2011,36(10):1758-1761.

FAN Qigao,LI Wei,WANG Yuqiao,et al.A shearer dynamic positioning method using strap down inertial navigation[J].Journal of China Coal Society,2011,36(10):1758-1761.

[11] 王玮,吕攀,王琪,等.惯性导航下煤矿移动车辆动态定位研究[J].煤炭工程,2016,48(10):138-141.

WANG Wei,LYU PAN,WANG Qi,et al.Research on dynamic positioning of mobile vehicle in underground coal mine based on inertial navigation[J].Coal Engineering,2016,48(10):138-141.

[12] 吕振,刘丹,李春光.基于捷联惯性导航的井下人员精确定位系统[J].煤炭学报,2009,34(8):1149-1152.

LYU ZHEN,LIU Dan,LI Chunguang.The precise positioning system of the mine personnel in coal pit based on strapdown inertial navigation[J].Journal of China Coal Society,2009,34(8):1149-1152.

[13] 朱新宇.基于行人航位推算和MEMS惯性传感器的室内定位算法研究[D].合肥:合肥工业大学,2017.

ZHU Xinyu.Research on indoor location algorithm based on pedestrian dead reckoning and MEMS inertial sensor[D].Hefei:Hefei University of Technology,2017.

[14] 任春华,陈灿,王满喜,等.一种惯性测量与距离组合的定位修正方法研究[J].仪器仪表学报,2013,34(11):2513-2519.

REN Chunhua,CHEN Can,WANG Manxi,et al.Research on a positioning correction method with inertial measurement in combination with distance[J].Chinese Journal of Scientific Instrument,2013,34(11):2513-2519.

[15] 高伟伟,朱文杰,方丹,等.惯导滤波参数优化算法[J].传感器与微系统,2019,38(6):152-154.

GAO Weiwei,ZHU Wenjie,FANG Dan,et al.Optimization algorithm of inertial navigation filtering parameters[J].Transducer and Microsystem Technologies,2019,38(6):152-154.

[16] XU Dingjie,WU Zhemin,HUANG Yulong.A new adaptive Kalman filter with inaccurate noise statistics[J].Circuits,Systems,and Signal Processing,2019,38(9):4380-4404.

[17] LI Zengke,YAO Yifei,WANG Jian,et al.Application of improved robust Kalman filter in data fusion for PPP/INS tightly coupled positioning system[J].Metrology and Measurement Systems,2017,24(2):289-301.

[18] 周琪,秦永元,张金红,等.基于四元数卡尔曼滤波的捷联惯导初始对准算法[J].中国惯性技术学报,2012,20(2):162-167.

ZHOU Qi,QIN Yongyuan,ZHANG Jinhong,et al.Initial alignment algorithm for SINS based on quaternion Kalman filter[J].Journal of Chinese Inertial Technology,2012,20(2):162-167.

[19] 王守华,李云柯,纪元法,等.一种基于位置修正和卡尔曼滤波的姿态角推算算法[J].科学技术与工程,2019,19(7):115-119.

WANG Shouhua,LI Yunke,JI Yuanfa,et al.Alternative algorithm for determing the attitude based on position correction and Kalman filtering[J].Science Technology and Engineering,2019,19(7):115-119.

[20] 刘健,孔雨晨.使用线性卡尔曼滤波进行WiFi-惯导的融合定位[J].电子测量技术,2017,40(4):1-4.

LIU Jian,KONG Yuchen.WiFi/inertial fusion positioning using linear Kalman filter[J].Electronic Measurement Technology,2017,40(4):1-4.