一种具备自修正的井下多传感融合低功耗无线定位系统

摘要

本申请涉及一种具备自修正的井下多传感融合低功耗无线定位系统。该系统包括：通过预设数量的UWB测距基站是根据UWB测距基站和UWB标签的通讯距离a，按小于等于通讯距离a的1/2尺寸，等间距的布置于煤矿井下巷道的同一侧；RFID基站是按照每两个UWB测距基站之间中间位置处布置一个RFID基站，且两个相邻的RFID基站之间的布置距离大于RFID基站与RFID标签的通讯距离b；每个UWB测距基站和各UWB测距基站两侧的RFID基站通过网络连接同一个嵌入式硬件平台；RFID标签和UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧；IMU模块设置于待定位的井下辅助运输车辆上；上位机与嵌入式硬件平台网络、IMU模块网络连接，通过IMU与UWB的融合定位，提高了定位精度。

权利要求

1.一种具备自修正的井下多传感融合低功耗无线定位系统，其特征在于，所述系统包括：UWB测距基站、RFID基站、UWB标签、RFID标签、IMU模块、嵌入式硬件平台和上位机；

预设数量的所述UWB测距基站是根据UWB测距基站和UWB标签的通讯距离a，按小于等于所述通讯距离a的1/2尺寸，等间距的布置于井下辅助运输车辆所通过的煤矿井下巷道没有异物遮挡的同一侧，并保持同一水平；

所述RFID基站是按照每两个所述UWB测距基站之间中间位置处布置一个RFID基站，且两个相邻的RFID基站之间的布置距离大于RFID基站与RFID标签的通讯距离b，确保RFID标签不会同时与两个RFID基站通讯；

每个所述UWB测距基站和各所述UWB测距基站两侧的RFID基站通过有线网络连接方式接入同一个嵌入式硬件平台；

所述RFID标签和所述UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧；

所述IMU模块设置于待定位的井下辅助运输车辆上；

所述上位机与所述嵌入式硬件平台网络、所述IMU模块网络连接；

其中，待定位的井下辅助运输车辆运动初始时，UWB测距基站UWB1和UWB2处于激活状态，其他UWB测距基站处于休眠状态；

在t时刻，所述待定位的井下辅助运输车辆运动至UWB测距基站UWBi负方向上的RFID基站RFIDia的信号接收范围内时，RFID标签与RFID基站RFIDia建立通讯，通过嵌入式硬件平台发出信号激活UWB测距基站UWBi+1，且此时UWB测距基站UWBi‑1已在车辆到达之前激活并保持激活状态，则此时UWBi‑1、UWBi和UWBi+1共计三个UWB测距基站同时处于激活状态，其他UWB测距基站处于节能休眠状态；

当所述待定位的井下辅助运输车辆继续行进，在t+1时刻到达RFID基站RFIDib/RFIDi+1a处时，RFID标签与该RFID基站RFIDib/RFIDi+1a建立通讯，通过嵌入式硬件平台发出信号关闭UWB测距基站UWBi‑1，激活UWB测距基站UWBi+2。

2.根据权利要求1所述的系统，其特征在于，所述嵌入式硬件平台与RFID基站之间的数字信息传输采用2进制编码，使用通用串行总线接口、以RS485协议传输，嵌入式硬件平台通过发出高电平信号的上沿激活UWB测距基站、低电平信号的下沿关闭UWB测距基站，该高电平信号和低电平信号使用通用串行总线接口、以RS422协议传输。

3.根据权利要求1所述的系统，其特征在于，所述嵌入式硬件平台通过RS485协议接收RFID基站的信号进行处理，通过COM传输实现UWB测距基站激活与休眠功能，并将接收所述UWB测距基站反馈信号传输至所述上位机。

4.根据权利要求1所述的系统，其特征在于，所述IMU模块用于获取待定位的井下辅助运输车辆的速度、加速度、角速度信息，进行位置解算，获得第一位置信息Ila。

5.根据权利要求1所述的系统，其特征在于，所述上位机用于获取所述IMU模块传输的第一位置信息Ila，以及所述嵌入式硬件平台网络传输的反馈信号，根据第一位置信息Ila和反馈信号进行位置解算，获得最终测量位置信息并输入偏移卡尔曼滤波器，输出最终的定位信息并显示。

说明书

一种具备自修正的井下多传感融合低功耗无线定位系统

技术领域

[0001] 本申请涉及井下定位技术领域，特别是涉及一种具备自修正的井下多传感融合低功耗无线定位系统。

背景技术

[0002] 要实现井下辅助运输车辆的井下无人自主工作，首要问题就是辅助运输车辆的煤矿井下精确定位问题，现有的煤矿运输机车定位大多数采用人工巡检或基于RFID、ZigBee、Wi‑Fi和UWB等无线通信技术的定位方式，人工巡检成本高、效率低、时间长，安全隐患较大，而单纯通过无线通信方式进行定位，由于煤矿井下巷道狭长，空间狭小，存在较多的大型用电设备，都将对无线信号的传播和通信带来较多的干扰，使得单纯基于无线通信的方式进行定位，定位精度无法达到满意的效果，系统稳定性也差强人意，虽然UWB技术的定位精度以及系统稳定性较好，但该技术涉及的定位方式要求工作时间所有设备都处于在线工作状态，不仅增加资源消耗，还容易造成无线通信网络拥堵等问题，容易影响定位精度。

发明内容

[0003] 基于此，有必要针对上述技术问题，提供一种能够解决容易影响定位精度的问题的具备自修正的井下多传感融合低功耗无线定位系统。

[0004] 一种具备自修正的井下多传感融合低功耗无线定位系统，所述系统包括：UWB测距基站、RFID基站、UWB标签、RFID标签、IMU模块、嵌入式硬件平台和上位机；

[0005] 预设数量的所述UWB测距基站是根据UWB测距基站和UWB标签的通讯距离a，按小于等于所述通讯距离a的1/2尺寸，等间距的布置于井下辅助运输车辆所通过的煤矿井下巷道没有异物遮挡的同一侧，并保持同一水平；

[0006] 所述RFID基站是按照每两个所述UWB测距基站之间中间位置处布置一个RFID基站，且两个相邻的RFID基站之间的布置距离大于RFID基站与RFID标签的通讯距离b，确保RFID标签不会同时与两个RFID基站通讯；

[0007] 每个所述UWB测距基站和各所述UWB测距基站两侧的RFID基站通过有线网络连接方式接入同一个嵌入式硬件平台；

[0008] 所述RFID标签和所述UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧；

[0009] 所述IMU模块设置于待定位的井下辅助运输车辆上；

[0010] 所述上位机与所述嵌入式硬件平台网络、所述IMU模块网络连接。

[0011] 在其中一个实施例中，所述嵌入式硬件平台与RFID基站之间的数字信息传输采用2进制编码，使用通用串行总线接口、以RS485协议传输，嵌入式硬件平台通过发出高电平信号的上沿激活UWB测距基站、低电平信号的下沿关闭UWB测距基站，该高电平信号和低电平信号使用通用串行总线接口、以RS422协议传输。

[0012] 在其中一个实施例中，所述嵌入式硬件平台通过RS485协议接收RFID基站的信号进行处理，通过COM传输实现UWB测距基站激活与休眠功能，并将接收所述UWB测距基站反馈信号传输至所述上位机。

[0013] 在其中一个实施例中，所述IMU模块用于获取待定位的井下辅助运输车辆的速度、加速度、角速度信息，进行位置解算，获得第一位置信息Ila。

[0014] 在其中一个实施例中，所述上位机用于获取所述IMU模块传输的第一位置信息Ila，以及所述嵌入式硬件平台网络传输的反馈信号进行位置解算，获得最终测量位置信息并输入偏移卡尔曼滤波器，利用偏移卡尔曼滤波器降低非视距干扰等测量干扰，输出最终的定位信息并显示。

[0015] 上述具备自修正的井下多传感融合低功耗无线定位系统，通过预设数量的UWB测距基站是根据UWB测距基站和UWB标签的通讯距离a，按小于等于通讯距离a的1/2尺寸，等间距的布置于井下辅助运输车辆所通过的煤矿井下巷道没有异物遮挡的同一侧，并保持同一水平；RFID基站是按照每两个UWB测距基站之间中间位置处布置一个RFID基站，且两个相邻的RFID基站之间的布置距离大于RFID基站与RFID标签的通讯距离b，确保RFID标签不会同时与两个RFID基站通讯；每个UWB测距基站和各UWB测距基站两侧的RFID基站通过网络连接同一个嵌入式硬件平台；RFID标签和UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧；IMU模块设置于待定位的井下辅助运输车辆上；上位机与嵌入式硬件平台网络、IMU模块网络连接，通过IMU与UWB的融合定位，避免了只用无线定位时易受环境影响而导致的定位精度下降，提高了定位精度。

附图说明

[0016] 图1为一个实施例中煤矿井下的UWB基站、RFID基站、UWB标签、RFID标签布置示意图；

[0017] 图2为一个实施例中嵌入式硬件平台示意图。

具体实施方式

[0018] 为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

[0019] 在一个实施例中，提供了一种具备自修正的井下多传感融合低功耗无线定位系统，包括：UWB测距基站、RFID基站、UWB标签、RFID标签、IMU模块、嵌入式硬件平台和上位机；

[0020] 预设数量的UWB测距基站是根据UWB测距基站和UWB标签的通讯距离a，按小于等于通讯距离a的1/2尺寸，等间距的布置于井下辅助运输车辆所通过的煤矿井下巷道没有异物遮挡的同一侧，并保持同一水平；RFID基站是按照每两个UWB测距基站之间中间位置处布置一个RFID基站，且两个相邻的RFID基站之间的布置距离大于RFID基站与RFID标签的通讯距离b，确保RFID标签不会同时与两个RFID基站通讯；每个UWB测距基站和各UWB测距基站两侧的RFID基站通过有线网络连接方式接入同一个嵌入式硬件平台；RFID标签和UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧；IMU模块设置于待定位的井下辅助运输车辆上；上位机与嵌入式硬件平台网络、IMU模块网络连接。

[0021] 其中，预设数量可根据煤矿井下巷道的长度确定，也可以根据实际需要确定。根据UWB测距基站和UWB标签的通讯距离a，等间距将预设数量的UWB测距基站布置于井下辅助运输车辆所通过的煤矿井下巷道，可以提高UWB技术测距精度，减小因传输过程中的信号衰减导致的测距误差。上位机与嵌入式硬件平台网络、IMU模块的网络连接，可以利用井下已具备的网络环境实现网络连接。

[0022] 以辅助运输车辆前进方向为正方向，通过对UWB测距基站和RFID基站进行“正、负”标定命名的方式进行区分，例“某UWB测距基站正方向上的RFID基站”，同时，在后文描述中可见，“某UWB测距基站正方向上的RFID基站”与“该UWB测距基站正方向上相邻的UWB测距基站的负方向上的RFID基站”两种描述指向为同一个RFID基站，即同一个RFID基站在不同的语境描述下有两种指向性描述。

[0023] 具体地：如图1所示，以井下辅助运输车辆前进方向为“正”方向，依据UWB测距基站和UWB标签的通讯距离a，将UWB测距基站以小于等于通讯距离的1/2尺寸，等间距的布置于辅助运输车辆所通过的煤矿井下巷道的同一侧，保持同一水平，并保证没有异物遮挡。并从井下辅助运输车辆出发起点开始，依次命名为UWB1、UWB2……UWBi……UWBn。

[0024] 每两个UWB测距基站之间中间位置处布置一个RFID基站，根据RFID基站与RFID标签的通讯距离b，保证两个相邻的RFID基站之间的布置距离大于通讯距离b，确保RFID标签不会同时与两个RFID基站通讯。以辅助运输车辆前进方向为正方向，对RFID基站进行区分命名，如第i个UWB测距基站UWBi负方向上的RFID基站记为RFIDia，正方向上的RFID基站记为RFIDib，第i+1个UWB测距基站UWBi负方向上的RFID基站记为RFIDi+1a，且RFIDib与RFIDi+1a所指为同一个RFID基站，后续描述中为简便起见，将该RFID基站描述为RFID基站RFIDib/RFIDi+1a。

[0025] 在一个实施例中，嵌入式硬件平台与RFID基站之间的数字信息传输采用2进制编码，使用通用串行总线接口、以RS485协议传输，嵌入式硬件平台通过发出高电平信号的上沿激活UWB测距基站、低电平信号的下沿关闭UWB测距基站，该高电平信号和低电平信号使用通用串行总线接口、以RS422协议传输。

[0026] 在一个实施例中，所述嵌入式硬件平台通过RS485协议接收RFID基站的信号进行处理，通过COM传输实现UWB测距基站激活与休眠功能，并将接收所述UWB测距基站反馈信号传输至所述上位机。

[0027] 其中，RFID基站检测到待定位的井下辅助运输车辆上的RFID标签信号时，将该信号反馈给对应的嵌入式硬件平台，嵌入式硬件平台将信号转换为激活UWB测距基站的信号，传输给对应的UWB测距基站，实现对UWB测距基站激活与休眠功能。如图2所示，嵌入式硬件平台是基于STM32平台构建的，包括STM32嵌入式单片机、RFID信号接收模块1、RFID信号接收模块2和UWB测距基站激活/休眠模块。

[0028] 在对煤矿井下巷道的待定位的井下辅助运输车辆进行定位时，首先初始化UWB标签，使UWB标签以固定的间隔发射广播请求帧R0。井下辅助运输车辆运动初始时，出发点为起点的第一个和第二个UWB测距基站处于激活状态，其他UWB测距基站处于休眠状态。被激活的UWB测距基站采集UWB标签发出的信号，并以各自Re帧返回UWB标签，采集UWB标签与UWB基站之间的信号强度、信号飞行时间等信息作为反馈信号返回给嵌入式硬件平台，嵌入式硬件平台将反馈信号反馈给上位机。

[0029] 待定位的井下辅助运输车辆前进方向为“正”方向进行标识定位，当待定位的井下辅助运输车辆运动至某一UWB测距基站(记为UWBi)的负方向上的RFID基站(记为RFIDia)信号接收范围内，待定位的井下辅助运输车辆上的RFID标签与RFID基站RFIDia建立通讯，激活该UWB测距基站UWBi正方向上的相邻UWB测距基站(记为UWBi+1)，此刻该UWB测距基站UWBi的负方向上的相邻UWB测距基站(记为UWBi‑1)已处于激活状态，则此时共有UWBi‑1、UWBi和UWBi+1共计三个UWB测距基站同时处于激活状态，其他UWB测距基站处于节能休眠状态，[0030] 在一个实施例中，所述IMU模块用于获取待定位的井下辅助运输车辆的速度、加速度、角速度信息，进行位置解算，获得第一位置信息Ila。

[0031] 运动开始时，待定位的井下辅助运输车辆上的IMU模块被激活，开始同步实时记录待定位的井下辅助运输车辆的速度、加速度、角速度信息，并基于这些信息实时解算位置信息，记为位置信息Ila。

[0032] 在一个实施例中，上位机用于获取所述IMU模块传输的第一位置信息Ila，以及所述嵌入式硬件平台网络传输的反馈信号进行位置解算，获得最终测量位置信息并输入偏移卡尔曼滤波器，输出最终的定位信息并显示。

[0033] 其中，输入偏移卡尔曼滤波器进一步处理，可以降低非视距干扰等测量干扰。

[0034] 上位机通过三个UWB测距基站与UWB标签分别建立的通讯，传输的反馈信号，分别计算三个UWB测距基站与UWB标签的相对距离。

[0035] 上位机根据UWB标签与三个UWB测距基站的相对距离，计算出此时UWB标签所代表的待定位的井下辅助运输车辆的实时位置信息，记为第二位置信息Ula。当待定位的井下辅助运输车辆继续行进，到达UWB测距基站UWBi的正方向上的RFID基站处，即UWB测距基站UWBi+1的负方向上的RFID基站(记为RFIDib/RFIDi+1a)，此时RFID标签与该RFID基站RFIDib/RFIDi+1a建立通讯，关闭此UWB测距基站UWBi‑1，同样的，激活UWB测距基站UWBi的正方向上间隔相邻的UWB测距基站UWBi+2，保证UWB标签同时与三个UWB测距基站(UWBi、UWBi+1、UWBi+2)建立通讯，获得此刻的位置信息。为叙述方便，该位置信息仍命名为第二位置信息Ula。

[0036] 计算三个UWB测距基站与UWB标签的相对距离，可以选用TOF、SS‑TWR、DS‑TWR、ADS‑TWR等技术计算，均为该领域内的常见技术，在此不再赘述。

[0037] 根据UWB标签与三个UWB测距基站的相对距离，计算出此时UWB标签所代表的待定位的井下辅助运输车辆的实时位置信息，可以采用TOA、TDOA、三边定位等定位技术，该技术为领域内的常见技术，在此不再赘述。

[0038] 在t时刻，计算得到t时刻的第二位置信息为Ula(t)，IMU模块计算得到t时刻的第一位置信息为Ila(t)，当|Ula(t)‑Ila(t)|≥Er时，重新测量并计算Ula(t)和Ila(t)，当|Ula(t)‑Ila(t)|＜Er时，得到融合后的最终测量位置信息Tl＝α1×Ula(t)+α2×Ila(t)，其中，Er、α1与α2为权重融合参数，结合实际环境测算得出。

[0039] 将实测的位置信息Tl作为观测值输入偏移卡尔曼滤波器，解算得出最终的定位信息回传上位机显示，完成定位。

[0040] 其中，偏移卡尔曼滤波器是对基本的卡尔曼滤波器进行卡尔曼增益矩阵的重构，卡尔曼滤波器为该领域内的常见技术，在此不再赘述。增益矩阵的重构，是指通过对卡尔曼滤波器中的测量残差向量元素yres(i)进行比较，获得系数矩阵Ξ，

[0041]

[0042]

[0043] 其中，ξ(i)为系数矩阵中第i个元素，i∈1,2,3……n，δ与ε为卡尔曼增益修正系数，其取值与现场环境相关，需要结合实际环境测算得出。并以此矩阵代替原来的卡尔曼增益，获得的新卡尔曼滤波器称为偏移卡尔曼滤波器。

[0044] 本申请的一种基于射频识别技术(RFID)，捷联惯导技术(IMU)，超宽带(Ultra Wide Band，UWB)技术融合的低功耗井下辅助运输车辆的高精度定位技术方法，以解决复杂井下环境干扰下煤矿井下辅助运输车辆定位精度差及传统的UWB技术定位过程中大量基站同时工作引发功耗高及通讯网络拥堵等问题。主要分为两个功能部分，第一功能部分的目的为利用射频识别技术(RFID)及超宽带(Ultra Wide Band，UWB)技术实现井下辅助运输车辆的低功耗粗定位，得到UWB粗定位信息，记为Ula，第二功能部分的目的是利用第一部分得到的UWB粗定位信息Ula，结合IMU设备得到的粗定位信息，记为Ila，基于扩展卡尔曼滤波技术及权重抉择重构实现井下辅助运输车辆在复杂环境干扰下的高精度动态定位[0045] 上述具备自修正的井下多传感融合低功耗无线定位系统，用于井下辅助运输车辆的高精度定位，一方面可以通过休眠及分段唤醒机制，减小功耗，同时减少UWB系统同时在线通讯设备数量，减少通讯拥堵，提高通讯及定位效率，另一方面IMU与UWB的融合定位，避免了只用无线定位时易受环境影响而导致的定位精度下降，同时还通过引入偏移卡尔曼滤波器，解决了因复杂井下环境干扰下而导致的非视距定位误差(NLOS)或因车辆震动而导致的IMU设备测量误差，提高了煤矿井下辅助运输车辆定位精度和系统可靠性，为煤矿井下辅助运输车辆后续的自主导航无人驾驶等技术的发展提供了可靠保证，降低安全风险。

[0046] 上述具备自修正的井下多传感融合低功耗无线定位系统的具体工作步骤为：

[0047] 步骤1：如图1所示，将UWB标签配置于待定位的井下辅助运输车辆上靠近UWB测距基站布置方位的一侧，运动开始时，初始化UWB标签，使UWB标签以固定的间隔发射广播请求帧R0，同时IMU模块被激活，开始同步实时记录车辆的速度、加速度、角速度信息，并基于这些信息实时解算位置信息，记为第一位置信息Ila。

[0048] 步骤2：待定位的井下辅助运输车辆运动初始时，UWB测距基站UWB1和UWB2处于激活状态，其他UWB测距基站处于休眠状态。

[0049] 步骤3：在t时刻，待定位的井下辅助运输车辆运动至UWB测距基站UWBi负方向上的RFID基站RFIDia的信号接收范围内时，RFID标签与RFID基站RFIDia建立通讯，通过嵌入式硬件平台发出信号激活UWB测距基站UWBi+1，且此时UWB测距基站基站UWBi‑1已在车辆到达之前激活并保持激活状态，则此时UWBi‑1、UWBi和UWBi+1共计三个UWB测距基站同时处于激活状态，其他UWB测距基站处于节能休眠状态，三个UWB测距基站与UWB标签建立通讯，采集UWB标签发出的信号，并以各自Re帧返回UWB标签，采集UWB标签与UWB基站之间的信号强度、信号飞行时间等信息作为反馈信号返回给嵌入式硬件平台，嵌入式硬件平台将反馈信号反馈给上位机，上位机分别计算UWB测距基站与UWB标签的相对距离。

[0050] 步骤4：上位机根据UWB标签与三个UWB测距基站的相对距离，计算出此时UWB标签所代表的待定位的井下辅助运输车辆的实时位置信息，记为第二位置信息Ula(t)，IMU模块计算得到的第一位置信息为Ila(t)，当|Ula(t)‑Ila(t)|≥Er时，重新测量并计算Ula(t)和Ila(t)，当|Ula(t)‑Ila(t)|＜Er时，得到融合后的最终测量位置信息Tl(t)＝α1×Ula(t)+α2×Ila(t)，其中，Er、α1与α2为权重融合参数，结合实际环境测算得出。将最终测量位置信息Tl(t)作为观测值输入偏移卡尔曼滤波器，解算此刻最终的定位信息Location(t)并在上位机显示。

[0051] 步骤5：当待定位的井下辅助运输车辆继续行进，在t+1时刻到达RFID基站RFIDib/RFIDi+1a处时，RFID标签与该RFID基站RFIDib/RFIDi+1a建立通讯，通过嵌入式硬件平台发出信号关闭UWB测距基站UWBi‑1，激活UWB测距基站UWBi+2，保证UWB标签同时与UWBi、UWBi+1、UWBi+2建立通讯，嵌入式硬件平台将反馈信号反馈给上位机获得此刻的第二位置信息。为叙述方便，该位置信息仍命名为位置信息Ula(t+1)，IMU模块计算得到的第一位置信息为与Ila(t+1)，当|Ula(t+1)‑Ila(t+1)|≥Er时，重新测量并计算Ula(t+1)和Ila(t+1)，当|Ula(t+1)‑Ila(t+1)|＜Er时，得到融合后的最终测量位置信息Tl(t+1)＝α1×Ula(t+1)+α2×Ila(t+1)。将最终测量位置信息Tl(t+1)作为观测值输入偏移卡尔曼滤波器，解算此刻最终的定位信息Location(t)并在上位机显示。

[0052] 进一步的，每一次调用偏移卡尔曼滤波器，都需进行一次增益矩阵重构并以此矩阵代替原来的卡尔曼增益，过程为对卡尔曼滤波器中的测量残差向量元素yres(i)进行比较，获得系数矩阵Ξ,

[0053]

[0054]

[0055] 以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

[0056] 以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。