A fusion positioning method for underground personnel based on UWB and PDR
-
摘要: 现有超宽带（UWB）与行人航位推算（PDR）融合定位方法大多忽略了非视距（NLOS）环境下的定位误差校正，以简单的阈值划分作为NLOS环境判断依据，而阈值划分在很大程度上与定位场景及场地大小相关。针对上述问题，提出一种考虑NLOS环境的基于UWB与PDR的井下人员融合定位方法。首先，利用UWB技术进行井下人员位置解算，通过三边定位算法得到人员初步位置后，使用最小二乘法对位置进行优化，通过多项式拟合实现NLOS环境下基站和标签之间实际值和测量值之间的拟合，减小NLOS环境下的测距误差，提高定位精度。其次，采用PDR算法对步态进行识别和分析，PDR算法使用惯性导航传感器采集的步态数据，通过步态识别、步长估计和方向估计，实现目标位置的更新；然后，通过卷积神经网络（CNN）−长短期记忆（LSTM）网络分析信道脉冲响应（CIR）特征，实现视距（LOS）/NLOS识别，解决NLOS环境判断存在场景限制的问题；最后，根据LOS/NLOS识别结果确定融合系数，实现UWB和PDR定位结果融合。测试结果表明：多项式拟合后UWB平均测距误差降低0.59 m；LOS/NLOS识别的平均准确率为95.3%，召回率和F1分数均在90%以上，验证了CNN−LSTM具有较好的识别效果；融合定位方法的平均误差为0.31 m，较UWB降低1.57 m，较PDR降低1.41 m。Abstract: Most existing fusion positioning methods for ultra-wideband (UWB) and pedestrian dead reckoning (PDR) ignore the correction of positioning errors in non line of sight (NLOS) environments. The methods use simple threshold division as the basis for NLOS environment judgment, which is largely related to the positioning scene and site size. In order to solve the above problems, a fusion positioning method for underground personnel considering NLOS environment is proposed. Firstly, UWB technology is used to calculate the position of underground personnel. After obtaining the preliminary position of personnel through the trilateral positioning algorithm, the least squares method is used to optimize the position. Polynomial fitting is used to achieve the fitting between the actual value and the measured value between the base station and the tag in the NLOS environment, reducing the ranging error in the NLOS environment and improving the positioning precision. Secondly, the PDR algorithm is used for gait recognition and analysis. The PDR algorithm uses gait data collected by inertial navigation sensors to update the target position through gait recognition, step size estimation, and direction estimation. Thirdly, the convolutional neural network (CNN) - long short term memory (LSTM) network is used to analyze the features of channel impulse response (CIR) and achieve line of sight (LOS)/NLOS recognition. It solves the problem of scene limitations in NLOS environment judgment. Finally, the fusion coefficient is determined based on the LOS/NLOS recognition results to achieve the fusion of UWB and PDR positioning results. The experimental results show that after polynomial fitting, the average ranging error of UWB is reduced by 0.59 m. The average accuracy of LOS/NLOS recognition is 95.3%, and the recall rate and F1 score are both above 90%, verifying that CNN-LSTM has good recognition performance. The average error of the fusion positioning method is 0.31 m, which is 1.57 m lower than UWB and 1.41 m lower than PDR.
-
0. 引言
我国煤炭资源丰富，是世界上最大的煤炭生产和消费国。由于作业设备和工艺相对落后，煤矿井下事故发生率较高，煤矿安全形势严峻[1]。《全国安全生产专项整治三年行动计划》中明确指出，要全面推进煤矿智能化建设[2]，实现煤矿高危险现场少人化、无人化。这给煤矿机器人发展带来新的机遇和挑战[3]。煤矿井下作业环境恶劣且存在高危气体，消防灭火机器人不仅能够帮助救援人员率先探明灾情、预测火势、熟悉环境，还能代替救援者进入火灾现场援救受困人员，更能发挥灭火功效，高效完成抢险救灾工作，是煤矿井下防灭火治理及灾后救援的重要装备。
目前煤矿机器人研究在底盘设计及防爆处理等方面取得进展，机器人底盘环境适应性及越障能力显著提高，但针对煤矿井下复杂地形的机器人定位、路径规划[4]、火灾险情识别及精准灭火等方面的研究较少，相关技术仍需重点攻克。为减少煤炭生产环节消防安全隐患，提升煤矿防灭火及救援时效性，本文设计了煤矿消防灭火机器人系统，结合超宽带（Ultra Wide Band，UWB）和激光雷达技术，采用自适应蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL）算法实现机器人定位，采用优化A*算法实现机器人路径规划，采用模板匹配方法识别险情目标，并通过云台控制将消防弹投射至险情目标，为煤矿精准灭火提供技术支持。
1. 系统总体设计
煤矿消防灭火机器人系统包括机器人硬件子系统、定位与路径规划子系统、目标识别与投掷子系统3个部分，如图1所示。机器人硬件子系统为机器人基础运动和信息采集提供保障。定位与路径规划子系统读取UWB定位模块数据、激光雷达数据和里程计信息，实现机器人初始化定位、实时避障运动和路径规划等功能。目标识别与投掷子系统通过调用摄像头的视频流信息，采用模板匹配方法进行特定目标识别，并控制二轴云台运动和消防弹投掷。
2. 机器人硬件子系统
机器人底盘搭载基于STM32F427芯片的主控板，采用4个由M3508减速电动机驱动的麦克纳姆轮作为动力组件，进行前行、横移、斜移、旋转及其组合方式的多种运动，实现机器人全向移动。
激光雷达是一种通过发射激光束来探测目标对象位置及速度等特征量的传感器[5-6]，本文选用RPLIDAR A3M1型激光雷达。
机载计算设备选用Intel NUC8i7BEH型工业计算机，运行机器人操作系统（Robot Operating System，ROS），实现图像处理和机器人运动控制等相关功能。
UWB是一种无线载波通信技术，具有功耗低、抗干扰能力和穿透能力强、定位准确度较高等优点[7-8]。本文选用DJI−RM−V1 UWB系统，其最高水平定位精度为5.5 cm，最高垂直定位精度为6.5 cm，位置更新频率为50 Hz。
3. 定位与路径规划子系统
3.1 实时定位
3.1.1 定位原理
机器人开机后，首先通过UWB定位模块确定机器人当前位置，在获取到的位置附近生成一系列正态分布的坐标点，再对这些坐标点进行筛选。针对每个坐标点，调用激光雷达扫描周边环境，得到二维点云数据；采用迭代最近点（Iterative Closest Point, ICP）算法对二维点云数据和预先存储的地图数据进行比较，根据误差最小原则确定机器人在该坐标点的姿态角及其置信度。对所有坐标点姿态角的置信度进行比较，将置信度最高的坐标点作为机器人的真实坐标，并将机器人在该坐标点的姿态角作为初始化时的真实姿态信息。
采用AMCL算法[9]实现机器人实时定位。AMCL算法是适用于二维平面内机器人运动位姿估计的算法，其流程如图2所示。
AMCL算法实现步骤：
（1） 随机生成一批粒子并进行地图载入工作，根据惯性测量装置和里程计数据更新粒子的位姿信息。
（2） 通过UWB数据判断机器人位置，通过激光雷达数据判断机器人与周边障碍物的距离（根据灰度值判断是否为障碍物），对初始生成的粒子进行置信度更新。距离机器人越远的粒子，其置信度越低；与激光雷达所测得的周边障碍物姿态信息越不匹配的粒子，其置信度越低。
（3） 根据粒子置信度高低赋予权值，置信度越高，权值越大。舍弃置信度低于阈值的粒子。
（4） 重采样阶段，根据粒子权值对粒子进行复制，复制份数与权值大小成正比。采用KL散度（Kullback−Leibler Divergense，KLD）算法确定粒子数量。KLD算法是一种自适应的粒子数量确定算法，可使粒子数量估计值与后验值之间的偏差不超过设定界限，即新生成粒子的数量随迭代次数增加而逐渐减少。粒子之间的距离不超过规定距离0.05 m时，认为定位成功，输出机器人当前位置。
在实际应用中，可能出现机器人运动过程中定位丢失的情况。为解决该问题，采用ROS中的SnapMapICP包进行机器人重定位[10-11]。
3.1.2 定位测试
首先获取UWB信号，确定机器人初始位置信息，使用激光雷达实时扫描得到点云数据，并与该位置地图数据进行匹配。初始化阶段，机器人姿态角的预测精度达3～5°，耗时1～2 s。在运动过程中，通过UWB模块判断机器人大致位置，根据激光雷达数据进行精确定位修正，测试结果如图3所示。图3（b）中十字坐标位置为机器人中心位置，与障碍物（黄色）之间的距离以二维云图方式体现，实测定位精度达5～10 cm。
3.2 路径规划
3.2.1 A*算法原理
A*算法作为一种求解静态网络最短路径最有效的直接搜索算法，综合了深度优先算法和广度优先算法的特性，被广泛应用于静态场景导航中[12-13]。该算法在深度优先策略的基础上增加了一个启发函数，使得搜索方向具有目标性。但是当选取的启发因子不适合当前规划任务时，会导致路径规划失败。针对该问题，对A*算法进行优化，加入寻找新目标点的容忍距离参数。加入该参数后，若路径规划失败，则在目标位置附近寻找另一个可以到达的位置替换当前目标位置，重新规划一条全局路径[14]。
3.2.2 路径规划测试
机器人路径规划测试结果如图4所示，图4（a）中右上角红色为机器人位置，绿色曲线为通过优化A*算法规划的路径，绿色曲线终点为目标灭火点。
在试验场地内设置不同着火点进行测试，实测数据见表1。可看出优化A*算法用时短，规划的路径合理。
表 1 路径规划指标Table 1. Path planning indexes序号 起止点间
欧氏距离/m算法
用时/s路径
长度/m与障碍物
最小距离/m1 5.650 1.05 10.980 0.750 2 7.095 1.25 13.813 0.900 3 6.450 1.34 13.682 0.750 4 5.732 0.78 7.433 0.640 5 8.137 1.09 11.803 0.590 4. 目标识别与投掷子系统
4.1 目标识别
通过先验知识生成着火点特征图像库，将特征图像作为特征匹配环节的模板图像。读取摄像头采集的视频流，并对模板图像和视频帧进行归一化处理。采用滑动窗口方式对模板图像（分辨率为128×128）和视频帧（分辨率为1920×1080）进行比较，计算相关系数R。相关系数R反映模板图像与待检验图像的相关程度，当模板图像与视频帧中某区域完全一致时，R=1。设置相关系数阈值，将相关系数大于阈值且相关系数值最大的区域确定为着火点目标。
获取着火点中心位置，将云台当前位置与着火点中心位置的偏差值作为给定信息传输到云台控制模块。
在分辨率为640×480的较复杂场景下，在距离机器人3 m范围内，着火点目标识别测试结果见表2。可看出机器人对动态多着火点的虚警率在10%以内，识别率达90%，满足应用要求。
表 2 着火点目标识别测试结果Table 2. Test results of ignition points detection测试场景 帧率/（帧·s−1） 虚警率/% 最大值 最小值 平均值 静态单一着火点 15.1 8.2 10.5 7.9 动态单一着火点 14.7 5.0 9.3 8.2 静态多着火点 11.0 4.2 8.5 9.0 动态多着火点 9.5 3.2 5.2 9.7 4.2 云台控制
云台控制的目的是准确、快速地跟踪着火点位置，使消防弹能够投射至目标区域。采用速度、位置双闭环的串级控制器实现云台俯仰轴和偏航轴角度控制。以偏航轴角度控制为例，串级控制流程如图5所示。
通过速度、位置双闭环PID调节[15]，使得云台响应曲线较平滑且准确地跟随给定曲线。在实际测试中，即使目标识别模块给出的控制量波动较大，云台的实际响应曲线也较平滑，且具有一定的超前预测效果[16]。通过整定串级控制器的PID参数，可弥补目标识别频率略小于云台控制频率的不足，抵消零阶保持效果[17]，实现对险情目标的精确处理。
5. 结论
（1） 结合UWB、激光雷达技术，采用ICP算法实现机器人定位初始化；在实时定位阶段，结合惯性测量装置和里程计数据，采用AMCL算法实现机器人定位，实测定位精度达5～10 cm。
（2） 在A*算法中，当选取的启发因子不适合当前规划任务时，会导致路径规划失败。针对该问题，对A*算法进行优化，加入寻找新目标点的容忍距离参数。加入该参数后，若路径规划失败，则在目标位置附近寻找另一个可以到达的位置替换当前目标位置，重新规划一条全局路径。测试结果表明，采用优化A*算法进行路径规划用时短，规划的路径合理。
（3） 通过先验知识生成着火点特征图像库，将特征图像作为特征匹配环节的模板图像，采用模板匹配方法识别险情目标。测试结果表明，机器人对动态多着火点的虚警率在10%以内，识别率达90%，满足应用要求。
（4） 采用速度、位置双闭环的串级控制器实现云台俯仰轴和偏航轴角度控制，能够准确、快速地跟踪着火点位置，使消防弹投射至目标区域。
-
![]()
图 1 基于UWB与PDR的井下人员融合定位方法原理
Figure 1. Principle of underground personnel fusion positioning method based on UWB and PDR
![]()
图 5 PDR与UWB融合定位流程
Figure 5. The fusion positioning process of pdestrian dead reckoning(PDR) and UWB
表 1 环境识别结果
Table 1 Environmental recognition result
% 环境 准确率 召回率 F1分数 环境1 95.0 92.1 91.9 环境2 95.2 91.3 93.0 环境3 95.8 91.6 91.7 环境4 95.1 92.6 91.5 平均值 95.3 91.9 92.0 
下载: 导出CSV
表 2 多项式拟合前后误差对比
Table 2 Comparison of errors before and after polynomial fitting
测试序号 UWB测距误差/m 拟合前 拟合后 1 1.29 0.86 2 1.23 0.88 3 1.32 0.91 4 1.49 0.97 5 1.38 0.65 6 1.44 0.71 7 1.19 0.59 8 1.63 0.84 9 1.51 0.78 10 1.20 0.63 平均值 1.37 0.78
下载: 导出CSV
表 3 3种定位方法误差统计结果
Table 3 Statistical results of errors of three positioning methods
m 定位方法 最大误差 最小误差 平均误差 UWB 3.71 0.21 1.88 PDR 3.75 0.05 1.72 UWB+PDR 1.19 0.06 0.31
下载: 导出CSV
表 4 融合方法误差对比
Table 4 Comparison of errors of fusion methods
融合方法 平均定位误差/m 抗差卡尔曼滤波 1.420 扩展卡尔曼滤波 0.475 自适应扩展卡尔曼滤波 0.330 视距分析融合 0.310
下载: 导出CSV
-
[1] 王显政. “以煤为基,多元发展”是我国能源安全战略的必然选择[J]. 煤炭经济研究,2014,34(4):1. WANG Xianzheng. "Coal-based,diversified development" is the inevitable choice of China's energy security strategy[J]. Coal Economy Research,2014,34(4):1.
[2] LI Menggang,ZHU Hua,YOU Shaoze,et al. UWB-based localization system aided with inertial sensor for underground coal mine applications[J]. IEEE Sensors Journal,2020,20(12):6652-6669. DOI: 10.1109/JSEN.2020.2976097
[3] ZHAO Yuxuan,WANG Manyi. The LOS/NLOS classification method based on deep learning for the UWB localization system in coal mines[J]. Applied Sciences,2022,12(13). DOI: 10.3390/app12136484.
[4] 陈炜翰,李世银. 基于超宽带和微惯导组合的室内精确定位[J]. 电子元器件与信息技术,2020,4(1):24-26. CHEN Weihan,LI Shiyin. Indoor precise positioning based on ultra-wideband and micro-inertial navigation combination[J]. Electronic Component and Information Technology,2020,4(1):24-26.
[5] 汪义庭. 基于UWB的无线室内定位系统设计与实现[D]. 淮南:安徽理工大学,2019. WANG Yiting. Design and implementation of wireless indoor positioning system based on UWB[D]. Huainan:Anhui University of Science & Technology,2019.
[6] BRIGADNOV I,LUTONIN A,BOGDANOVA K. Error state extended Kalman filter localization for underground mining environments[J]. Symmetry,2023,15(2). DOI: 10.3390/sym15020344.
[7] YU Da,LI Changgeng,XIAO Jiaxun. Neural networks-based Wi-Fi/PDR indoor navigation fusion methods[J]. IEEE Transactions on Instrumentation and Measurement,2023,72:1-14.
[8] 李金奎. 基于超声波时差法定位系统设计[J]. 自动化应用,2022(10):11-14. LI Jinkui. Design of positioning system based on ultrasonic time difference method[J]. Automation Application,2022(10):11-14.
[9] 陶靖. 煤矿测量红外定位放线装置的研制[J]. 煤矿机电,2020,41(3):111-112,115. TAO Jing. Development of infrared positioning and actuating device for coal mine measurement[J]. Colliery Mechanical & Electrical Technology,2020,41(3):111-112,115.
[10] 王轩. 基于模式感知和地图匹配的PDR/BLE室内融合定位方法研究[D]. 徐州:中国矿业大学,2021. WANG Xuan. Research on PDR/BLE indoor fusion positioning based on mode awareness and map matching[D]. Xuzhou:China University of Mining and Technology,2021.
[11] KONG Xiaotong,WU Chang,YOU Yuan,et al. Hybrid indoor positioning method of BLE and PDR based on adaptive feedback EKF with low BLE deployment density[J]. IEEE Transactions on Instrumentation and Measurement,2023,72:1-12.
[12] 李宗伟,王翀,王刚,等. 煤矿井下人员融合定位方法[J]. 工矿自动化,2020,46(1):59-64. LI Zongwei,WANG Chong,WANG Gang,et al. Fusion positioning method of underground mine personnel[J]. Industry and Mine Automation,2020,46(1):59-64.
[13] 吴静然,崔冉,赵志凯,等. 矿井人员融合定位系统[J]. 工矿自动化,2018,44(4):74-79. WU Jingran,CUI Ran,ZHAO Zhikai,et al. Mine personnel fusion location system[J]. Industry and Mine Automation,2018,44(4):74-79.
[14] 赵子凡,张艺蒙,彭霄,等. 基于UWB/INS组合的室内高精度移动定位算法研究[J]. 导航定位学报,2022,10(5):74-80. DOI: 10.3969/j.issn.2095-4999.2022.05.011 ZHAO Zifan,ZHANG Yimeng,PENG Xiao,et al. Indoor high-precision mobile positioning algorithm design based on the Integration of UWB/INS[J]. Journal of Navigation and Positioning,2022,10(5):74-80. DOI: 10.3969/j.issn.2095-4999.2022.05.011
[15] YUAN Debao,ZHANG Jian,WANG Jian,et al. Robustly adaptive EKF PDR/UWB integrated navigation based on additional heading constraint[J]. Sensors,2021,21(13). DOI: 10.3390/s21134390.
[16] ANGRISANO A,VULTAGGIO M,GAGLIONE S,et al. Pedestrian localization with PDR supplemented by GNSS[C]. European Navigation Conference,Warsaw,2019:1-6.
[17] 刘送永,崔玉明. 煤矿井下定位导航技术研究进展[J]. 矿业研究与开发,2019,39(7):114-120. LIU Songyong,CUI Yuming. Research progress of positioning and navigation technology in underground coal mine[J]. Mining Research and Development,2019,39(7):114-120.
[18] 吴益凡,刘帅,刘婷月. 基于UWB的室内运动定位算法研究[J]. 电脑编程技巧与维护,2024(4):151-153. DOI: 10.3969/j.issn.1006-4052.2024.04.046 WU Yifan,LIU Shuai,LIU Tingyue. Research on indoor motion location algorithm based on UWB[J]. Computer Programming Skills & Maintenance,2024(4):151-153. DOI: 10.3969/j.issn.1006-4052.2024.04.046
[19] 黎昱杰,黎蕾蕾,卜继军,等. 基于UWB/PDR的无基础设施行人协同导航研究[J]. 压电与声光,2023,45(5):736-739. LI Yujie,LI Leilei,BU Jijun,et al. Research on non-infrastructure pedestrian cooperative navigation based on UWB/PDR combination[J]. Piezoelectrics & Acoustooptics,2023,45(5):736-739.
[20] 李景文,韦晶闪,周俊芬,等. 融合UWB+PDR的室内定位方法改进[J]. 测绘通报,2022(3):36-40. DOI: 10.3969/j.issn.0494-0911.2022.3.chtb202203008 LI Jingwen,WEI Jingshan,ZHOU Junfen,et al. Improvement of indoor positioning method combining UWB and PDR[J]. Bulletin of Surveying and Mapping,2022(3):36-40. DOI: 10.3969/j.issn.0494-0911.2022.3.chtb202203008
[21] 杨国华. 基于无线传感网络与UWB技术的煤矿井下采掘人员定位方法[J]. 煤矿现代化,2024,33(2):41-44,50. DOI: 10.3969/j.issn.1009-0797.2024.02.010 YANG Guohua. A method for locating mining personnel in coal mines based on wireless sensor networks and UWB technology[J]. Coal Mine Modernization,2024,33(2):41-44,50. DOI: 10.3969/j.issn.1009-0797.2024.02.010
[22] 方贤宝,林勇,苏羿安,等. 基于TOF和自适应抗差卡尔曼滤波的UWB室内定位算法[J]. 传感器与微系统,2024,43(3):134-138. FANG Xianbao,LIN Yong,SU Yi'an,et al. UWB indoor positioning algorithm based on TOF and adaptive robust KF[J]. Transducer and Microsystem Technologies,2024,43(3):134-138.
[23] SI Lei,WANG Zhongbin,WEI Dong,et al. Fusion positioning of mobile equipment in underground coal mine based on redundant IMUs and UWB[J]. IEEE Transactions on Industrial Informatics,2024,20(4):5946-5958. DOI: 10.1109/TII.2023.3342427
[24] 李金昆,修春娣,杨东凯. 基于UWB/PDR/地磁的地下空间多源融合定位方法[C]. 第十六届全国信号和智能信息处理与应用学术会议,2022:182-187. LI Jinkun,XIU Chundi,YANG Dongkai. Multi-Source fusion location method for underground space based on UWB/ PDR/Geomagnetic[C]. The 14th National Academic Conference on Signal and Intelligent Information Processing and Application,2022:182-187.
[25] 刘宇,谢宇,彭慧,等. 基于UWB/PDR的航向发散自适应修正算法研究[J]. 电子测量技术,2022,45(3):98-103. LIU Yu,XIE Yu,PENG Hui,et al. Research on adaptive correction algorithm of heading divergence based on UWB/PDR[J]. Electronic Measurement Technology,2022,45(3):98-103.
-
期刊类型引用(6)
1. 伊海洋，张才. 矿用胶囊式救生舱设计与仿真分析. 工业安全与环保. 2024(04): 34-38 . 
百度学术
2. 吴力，孙国亮. 机器人自动化技术在建筑消防系统施工中的应用研究. 模具制造. 2024(12): 194-196+199 . 百度学术
3. 陈杰. 探究消防灭火机器人及配套装备技术. 中国设备工程. 2023(14): 26-28 . 百度学术
4. 郭小龙. 消防救援队伍灭火救援实战能力提升对策研究. 今日消防. 2023(11): 57-59 . 百度学术
5. 李鹏程，郝文静，闫红玲. 狭窄空间智能巡检机器人设计及运动分析. 运城学院学报. 2023(06): 42-46 . 百度学术
6. 毕丽敏，罗延歆. 煤矿生活区室外装配式箱泵一体化消防给水泵站设计. 煤炭工程. 2022(06): 32-34 . 百度学术
其他类型引用(1)
计量
- 文章访问数: 150
- HTML全文浏览量: 45
- PDF下载量: 22
- 被引次数: 7
