煤矿井下人员融合定位方法

李宗伟，王翀，王刚，徐志明，崔朋志，姜孟冯

(应急管理部信息研究院， 北京 100029)

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对称双向双边测距原理

定位终端将包含自身信息的数据包发送给就近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 捷联惯性导航定位过程

(3)

(4)

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

(5)

(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)

(14)

(15)

2 融合定位

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

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

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

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

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

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

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

(a) 设备1

(b) 设备2

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

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 定位实验路线

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

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

图8 定位实验结果

4 结语

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