CN113687301B - 一种煤矿井下多支路口uwb联合定位方法 - Google Patents

Abstract

本发明涉及一种煤矿井下多支路口UWB联合定位方法，属于UWB定位领域，包括：S1：在路口安装测距读卡器，在支路安装监听读卡器；S2：当标识卡进入定位区域时，中心站将标识卡信息下发至测距读卡器分配测距时序；S3：UWB标识卡向测距读卡器发起TOF测距请求；S3：完成测距；S4：在测距过程中各支路的UWB监听读卡器对TOF测距信息进行监听并记录监听数据；S5：UWB测距读卡器将测距信息、UWB监听读卡器将监听信息均通过环网上传至中心站；S6：中心站计算出UWB标识卡与测距读卡器以及各个监听读卡器的距离；S7：结合所有测距数据，实时分析出UWB标识卡位置。本发明并发量高，速度快，可靠性高。

Description

一种煤矿井下多支路口UWB联合定位方法

技术领域

本发明属于UWB定位领域，涉及一种煤矿井下多支路口UWB联合定位方法。

背景技术

煤炭作为重要的能源燃料，开采量每年都在增加，然而煤矿井下道路狭小、环境恶劣极易造成安全事故，一旦发生人员安全事故，由于不确定抢险位置，导致大量人员因抢救不及时死亡。人员定位系统作为煤矿井下安全生产的重要保障，通常采用RFID、蓝牙、Zigbee、红外线超声波等技术实现人员定位功能，但煤矿井下设备安放杂乱、粉尘浮游物含量高、多径效应严重等诸多因数影响导致传统定位技术定位精度较差，越来越不能满足现阶段高并发量、高精度的现场使用需求。UWB超宽带技术相比传统人员定位系统而言，具有定位精度高、穿透性强、抗多径强、并发量高等特点，现已成为煤矿井下主流人员定位技术。

煤矿井下环境错中复杂，存在大量的丁字路口、十字路口、Y字路口等场景，煤矿井下普通大巷采用一维定位即可满足现场环境使用需求，但在路口处会出现多个UWB信号覆盖的情况，并且无法预测UWB标识卡从哪条支路出来又从哪条支路进去，在该环境下普通处理方法会出现UWB定位真空区、UWB标识卡频繁切换网络、定位位置闪烁等问题，需要一种快速稳定的定位方法来解决煤矿井下多支路口定位困难的现象。

传统煤矿井下多支路定位采用TOF联合定位的方式进行解决，如图1所示该区域安装有4个UWB读卡器，中心站会将4个读卡器分成一组联合负责该区域定位。当标识卡进入该区域后中心站会将标识卡的基本信息下发到该区域的各个读卡器，并为该区域的各个读卡器分配测距时序，各个读卡器根据测距时序依次与UWB标识卡进行测距，最后将所有测距信息上传中心站进行联合定位。图1中标识卡从A点移动到B点，当标识卡处于A点时测距读卡器2和测距读卡器4由于遮挡无法成功测距，根据测距读卡器1和测距读卡器3的测距信息可以分析出UWB标识卡定位信息，移动到B点后同理可根据测距读卡器2和测距读卡器4进行UWB标识卡联合定位，该联合定位算法需依次和每个测距读卡器进行测距，测距时序管理困难且联合定位时间较长。

发明内容

有鉴于此，本发明的目的在于提供一种煤矿井下多支路口UWB的TDOA结合TOF联合定位方法，充分结合TDOA高并发量和TOF高精度的特点。

为达到上述目的，本发明提供如下技术方案：

一种煤矿井下多支路口UWB联合定位方法，包括以下步骤：

S1：在煤矿井下多支路路口安装UWB测距读卡器，在每条支路安装UWB监听读卡器；

S2：当UWB标识卡进入定位区域时，中心站将UWB标识卡信息下发至UWB测距读卡器，UWB测距读卡器为UWB标识卡分配测距时序；

S3：当UWB标识卡收到测距时序时，向UWB测距读卡器发起TOF测距请求；

S3：UWB测距读卡器与UWB标识卡完成UWB测距；

S4：在测距过程中各支路的UWB监听读卡器对TOF测距信息进行监听并记录监听数据，包括每条UWB信息的时间戳；

S5：UWB测距读卡器将测距信息、UWB监听读卡器将监听信息均通过环网上传至中心站；

S6：中心站计算出UWB标识卡与UWB测距读卡器以及各个UWB监听读卡器的距离；

S7：结合所有测距数据，实时分析出UWB标识卡进入哪条支路并进行位置定位。

进一步，所述UWB测距读卡器和UWB监听读卡器的结构均包括主控MCU，还包括与主控MCU连接的监控芯片、实时时钟RTC模块、UWB模块、功放模块、网络接口、闪存模块、DC/DC模块；所述主控MCU用于读卡器功能控制，所述监控芯片用于主控MCU工作状态监控，所述实时时钟RTC模块用于提供实时时钟信息，所述UWB模块负责UWB信号的发送与接收，所述功放模块用于UWB信号放大，所述网络接口用于与中心站进行数据交互，所述闪存模块用于读卡器相关配置信息，所述DC/DC模块用于进行电源供给。

进一步，所述UWB标识卡从其它区域或一维定位状态进入TDOA结合TOF定位区域时保持和之前UWB测距读卡器进行TOF测距，中心站检测到UWB标识卡进入新区域时将新区域UWB测距读卡器的信息下发到当前UWB读卡器，当前UWB测距读卡器与UWB标识卡进行测距时在测距响应帧里附带新区域UWB测距读卡器的信息，UWB标识卡接收到重新入网UWB测距读卡器信息后，测距数据帧里附带信息表示断开与当前UWB测距读卡器测距，并立刻发起定向入网请求帧与新区域UWB测距读卡器进行入网。

进一步，将所述UWB监听读卡器依次记作MonitorAnchor-1,MonitorAnchor-2,...,MonitorAnchor-N，将所述UWB测距读卡器记作RangAnchor，将所述UWB标识卡记作Tag；当Tag进入TDOA结合TOF测距区域并与RangAnchor进行入网后，RangAnchor将分配时序与Tag进行TOF测距，区域内的所有MonitorAnchor都将处于监听状态，对TOF测距流程的各条数据帧进行监听；

Tag入网到UWB测距读卡器并分配时序后，当Tag收到测距时序之后发起Poll帧(测距请求帧)，然后记录下发送时间戳T0A，RangAnchor接收到Poll帧之后记录下时间戳T1A，此时MonitorAnchor-1也处于监听状态，同样监听到Poll帧并记录下接收时间戳T2A；RangAnchor接收数据处理完毕后回复Resp帧(测距响应帧)并记录下发送时间戳T1B，Tag接收到Resp帧后记录下时间戳T0B，MonitorAnchor-1同样记录下时间戳T2B；Tag接收到Resp帧后说明握手成功，发起Final帧(测距数据帧)，同时记录下发送时间戳T0C，RangAnchor和MonitorAnchor-1接收到Final帧的时间戳分别记作T1C和T2C；

Tag和RangAnchor之间的距离计算公式如下：

式中：d0表示Tag和RangAnchor之间距离；Tround1表示T0B和T0A时间差；

Treply1表示T1B和T1A时间差；Tround2表示T1C和T1B时间差；Treply2表示T0C和T0B时间差；c表示光速；

采用TOF测量出Tag和RangAnchor之间的距离d0之后利用TDOA技术测量出MonitorAnchor-1,MonitorAnchor-2,...,MonitorAnchor-N分别距离Tag的距离d1,d2,...,dN；

MonitorAnchor-1与Tag之间距离d1，计算公式如下：

d0-d1＝ΔT12*c (2)

(T2B-T2A)+ΔT12-(T1B-T1A)＝T12 (3)

d01＝T12*c (4)

根据上面三个公式得出：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)) (5)

将MonitorAnchor-1时钟同步到RangAnchor时钟，MonitorAnchor-1与RangAnchor的时间同步系数，公式如下：

(T1C-T1A)＝k(T2C-T2A) (6)

将公式(5)进行时间同步后公式如下：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)*k) (7)

式中：d0表示Tag和RangAnchor之间距离；d1表示Tag和MonitorAnchor1之间距离；d01表示RangAnchor和MonitorAnchor1之间距离，为已知常数；ΔT12表示Poll帧传递到RangAnchor和MonitorAnchor1时间差；T12表示Resp在RangAnchor和MonitorAnchor1之间传输的时间；k表示MonitorAnchor1与RangAnchor时钟同步系数；

同理计算出d2,d3,...,dN，通过一次TOF测距计算出Tag距离各个UWB监听读卡器之间的距离，最后中心站结合所有距离分析出Tag定位位置。

本发明的有益效果在于：本发明通过将UWB定位中的TDOA和TOF结合，在煤矿井下多支路情况下实现UWB多读卡器联合定位，测距并发量高，测距速度快，测距可靠性高。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为传统的TOF联合定位图；

图2为本发明所述TDOA结合TOF联合定位图；

图3为读卡器硬件框图；

图4为定位区域切换流程图；

图5为TDOA结合TOF测距时序图；

图6为TDOA结合TOF联合定位图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

请参阅图2～图6。TDOA结合TOF联合定位可以充分结合TDOA高并发量和TOF高精度的特点，如图2所示在路口安装测距读卡器，在其它支路安装监听读卡器，中心站将5个读卡器分成一组。当标识卡进入该区域时中心站只需将标识卡信息下发测距读卡器，测距读卡器为UWB标识卡分配测距时序，当UWB标识卡收到测距时序时，通知测距读卡器进行TOF测距，其它监听读卡器对整个TOF测距过程进行监听，记录下每条UWB信息的时间戳，最后将测距信息以及所有时间戳上传中心站，中心站根据算法求出UWB标识卡距离每个读卡器的距离，最后将所有距离进行联合定位。下图中标识卡从A点移动到B点，当标识卡处于A点时UWB标识卡同测距读卡器进行测距，监听读卡器1和监听读卡器3可监听UWB信息，但监听读卡器2和监听读卡器4由于遮挡无法监听UWB信息，中心站无法计算出距离，可根据其它距离进行联合定位，移动到B点后同理可根据测量距离进行UWB标识卡联合定位，TDOA联合TOF进行煤矿井下多支路口定位只需进行一次TOF测距就可计算出UWB标识卡与各个读卡器距离，无需复查的时序管理同时提高定位并发量。

读卡器由主控MCU、看门狗、RTC模块、UWB模块、PA模块、网络接口、FLASH、DC/DC模块等部件组成。主控MCU实现读卡器功能控制，看门狗负责主控MCU工作状态监控，RTC模块负责提供实时时钟信息，UWB模块负责UWB信号的发送与接收，PA模块负责UWB信号放大，网络接口负责与中心站进行数据交互，FLASH负责读卡器相关配置信息，DC/DC负责进行电源供给，硬件框图如图3所示。

UWB标识卡从其它区域或一维定位状态进入TDOA结合TOF定位区域时会保持和之前读卡器进行TOF测距，中心站检测到UWB进入该区域时会将该区域测距读卡器的信息下发到当前读卡器，当前读卡器与UWB标识卡进行测距时会在测距响应帧里面附带该区域测距读卡器的信息，UWB标识卡接收到重新入网读卡器信息后，测距数据帧里面会附带信息表示断开与当前读卡器测距，并立刻发起定向入网请求帧与区域内测距读卡器进行入网，流程如图4所示。

如图2所示将监听读卡器依次记作MonitorAnchor1、MonitorAnchor2、MonitorAnchor3、MonitorAnchor4，测距读卡器记作RangAnchor，UWB标识卡记作Tag。当Tag进入TDOA结合TOF测距区域并与RangAnchor进行入网后，RangAnchor将分配时序与Tag进行TOF测距，区域内的所有MonitorAnchor都将处于监听状态，对TOF测距流程的各条数据帧进行监听，测距时序如图5所示。

Tag入网到测距读卡器并分配时序后，当Tag测距时序到之后Tag发起Poll帧(测距请求帧)，Tag发送完毕Poll帧后记录下发送时间戳T0A，RangAnchor接收到Poll帧之后记录下时间戳T1A，此时MonitorAnchor1也处于监听状态，同样可以监听到Poll帧并记录下接收时间戳T2A。RangAnchor将接收数据处理完毕后回复Resp帧(测距响应帧)并记录下发送时间戳T1B，Tag接收到Resp帧之后记录下时间戳T0B，MonitorAnchor1同样记录下时间戳T2B，Tag接收到Resp帧之后说明握手成功将发起Final帧(测距数据帧)同时记录下发送时间戳T0C，RangAnchor和MonitorAnchor1接收到Final帧的时间戳分别记作T1C和T2C。

Tag和RangAnchor之间的距离可由UWB信号在两者的飞行时间乘以光速求得，计算公式如下所示。

式中：d0---Tag和RangAnchor之间距离；

Tround1---T0B和T0A时间差；

Treply1---T1B和T1A时间差；

Tround2---T1C和T1B时间差；

Treply2---T0C和T0B时间差；

c---光速。

采用TOF测量出Tag和RangAnchor之间的距离d0之后可以利用TDOA技术测量出MonitorAnchor1、MonitorAnchor2、MonitorAnchor3、MonitorAnchor4分别距离Tag的距离d1、d2、d3、d4，下面以上图所画时序为例，计算MonitorAnchor1与Tag之间距离d1，计算公式如下所示。

d0-d1＝ΔT12*c (2)

(T2B-T2A)+ΔT12-(T1B-T1A)＝T12 (3)

d01＝T12*c (4)

根据上面三个公式可以得出：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)) (5)

上述公式建立的前提是RangAnchor和MonitorAnchor1两个读卡器UWB信号部分时钟是完全同步的，现实应用中时钟不可能同步，需要进行读卡器间时钟同步，这里将MonitorAnchor1时钟同步到RangAnchor时钟，从图5可知RangAnchor接收Poll帧到Final帧中间的时间段是一定的，MonitorAnchor1接收Poll帧到Final帧中间的时间段应该与RangAnchor相等，因此可以得到MonitorAnchor1与RangAnchor的时间同步系数，公式如下。

(T1C-T1A)＝k(T2C-T2A) (6)

将公式(5)进行时间同步后公式如下：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)*k) (7)

式中：d0---Tag和RangAnchor之间距离；

d1---Tag和MonitorAnchor1之间距离；

d01---RangAnchor和MonitorAnchor1之间距离；

ΔT12---Poll帧传递到RangAnchor和MonitorAnchor1时间差；

T12---Resp在RangAnchor和MonitorAnchor1之间传输的时间；

c---光速；

k---MonitorAnchor1与RangAnchor时钟同步系数。

因为读卡器在现场安装完毕后会测量出各个读卡器的坐标，那么读卡器与读卡器之间的距离就可事先计算出来，即d01表示RangAnchor和MonitorAnchor1之间距离为常数，通过上述方法也可以计算出d2、d3、d4，那么只需一次TOF测距就可以计算出Tag距离各个读卡器之间的距离，最后中心站结合所有距离分析出Tag定位位置。

如图2所示标识卡从A点移动到B点，当标识卡处于A点时，采用TDOA结合TOF测距可以计算出d0、d1、d3，监听读卡器2和监听读卡器4由于遮挡无法监听到Tag发出的信息，从而无法计算出d2、d4，如图6所示结合d0、d1、d3就可以计算出标识卡在支路1的具体位置，当标识卡走到十字路口中间时d0、d1、d2、d3、d4都可以采用TDOA结合TOF计算出来，定位出标示卡处于十字路口中心位置，标识卡最后走往B处后同理可求出d0、d2、d4，然后联合定位出标识卡在支路4具体位置。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (2)

1.一种煤矿井下多支路口UWB联合定位方法，其特征在于：包括以下步骤：

S1：在煤矿井下多支路路口安装UWB测距读卡器，在每条支路安装UWB监听读卡器；

S2：当UWB标识卡进入定位区域时，中心站将UWB标识卡信息下发至UWB测距读卡器，UWB测距读卡器为UWB标识卡分配测距时序；

S3：当UWB标识卡收到测距时序时，向UWB测距读卡器发起TOF测距请求；

S3：UWB测距读卡器与UWB标识卡完成UWB测距；

S4：在测距过程中各支路的UWB监听读卡器对TOF测距信息进行监听并记录监听数据，包括每条UWB信息的时间戳；

S5：UWB测距读卡器将测距信息、UWB监听读卡器将监听信息均通过环网上传至中心站；

S6：中心站计算出UWB标识卡与UWB测距读卡器以及各个UWB监听读卡器的距离；

S7：结合所有测距数据，实时分析出UWB标识卡进入哪条支路并进行位置定位；

所述UWB标识卡从其它区域或一维定位状态进入TDOA结合TOF定位区域时保持和之前UWB测距读卡器进行TOF测距，中心站检测到UWB标识卡进入新区域时将新区域UWB测距读卡器的信息下发到当前UWB测距读卡器，当前UWB测距读卡器与UWB标识卡进行测距时在测距响应帧里附带新区域UWB测距读卡器的信息，UWB标识卡接收到重新入网UWB测距读卡器信息后，测距数据帧里附带信息表示断开与当前UWB测距读卡器测距，并立刻发起定向入网请求帧与新区域UWB测距读卡器进行入网；

将所述UWB监听读卡器依次记作MonitorAnchor-1,MonitorAnchor-2,...,MonitorAnchor-N，将所述UWB测距读卡器记作RangAnchor，将所述UWB标识卡记作Tag；当Tag进入TDOA结合TOF测距区域并与RangAnchor进行入网后，RangAnchor将分配时序与Tag进行TOF测距，区域内的所有MonitorAnchor都将处于监听状态，对TOF测距流程的各条数据帧进行监听；

Tag入网到UWB测距读卡器并分配时序后，当Tag收到测距时序之后发起Poll帧，即测距请求帧，然后记录下发送时间戳T0A，RangAnchor接收到Poll帧之后记录下时间戳T1A，此时MonitorAnchor-1也处于监听状态，同样监听到Poll帧并记录下接收时间戳T2A；RangAnchor接收数据处理完毕后回复Resp帧，即测距响应帧，并记录下发送时间戳T1B，Tag接收到Resp帧后记录下时间戳T0B，MonitorAnchor-1同样记录下时间戳T2B；Tag接收到Resp帧后说明握手成功，发起Final帧，即测距数据帧，同时记录下发送时间戳T0C，RangAnchor和MonitorAnchor-1接收到Final帧的时间戳分别记作T1C和T2C；

Tag和RangAnchor之间的距离计算公式如下：

式中：d0表示Tag和RangAnchor之间距离；Tround1表示T0B和T0A时间差；Treply1表示T1B和T1A时间差；Tround2表示T1C和T1B时间差；Treply2表示T0C和T0B时间差；c表示光速；

采用TOF测量出Tag和RangAnchor之间的距离d0之后利用TDOA技术测量出MonitorAnchor-1,MonitorAnchor-2,...,MonitorAnchor-N分别距离Tag的距离d1,d2,...,dN；

MonitorAnchor-1与Tag之间距离d1，计算公式如下：

d0-d1＝ΔT12*c (2)

(T2B-T2A)+ΔT12-(T1B-T1A)＝T12 (3)

d01＝T12*c (4)

根据上面三个公式得出：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)) (5)

将MonitorAnchor-1时钟同步到RangAnchor时钟，MonitorAnchor-1与RangAnchor的时间同步系数，公式如下：

(T1C-T1A)＝k(T2C-T2A) (6)

将公式(5)进行时间同步后公式如下：

d1＝d0-c*(d01/c+(T1B-T1A)-(T2B-T2A)*k) (7)

式中：d0表示Tag和RangAnchor之间距离；d1表示Tag和MonitorAnchor1之间距离；d01表示RangAnchor和MonitorAnchor1之间距离，为已知常数；ΔT12表示Poll帧传递到RangAnchor和MonitorAnchor1时间差；T12表示Resp在RangAnchor和MonitorAnchor1之间传输的时间；k表示MonitorAnchor1与RangAnchor时钟同步系数；

同理计算出d2,d3,...,dN，通过一次TOF测距计算出Tag距离各个UWB监听读卡器之间的距离，最后中心站结合所有距离分析出Tag定位位置。

2.根据权利要求1所述的煤矿井下多支路口UWB联合定位方法，其特征在于：所述UWB测距读卡器和UWB监听读卡器的结构均包括主控MCU，还包括与主控MCU连接的监控芯片、实时时钟RTC模块、UWB模块、功放模块、网络接口、闪存模块、DC/DC模块；所述主控MCU用于读卡器功能控制，所述监控芯片用于主控MCU工作状态监控，所述实时时钟RTC模块用于提供实时时钟信息，所述UWB模块负责UWB信号的发送与接收，所述功放模块用于UWB信号放大，所述网络接口用于与中心站进行数据交互，所述闪存模块用于读卡器相关配置信息，所述DC/DC模块用于进行电源供给。