A path-planning method for coal mine robot based on improved probability road map algorithm
-
摘要: 路径规划是煤矿机器人在煤矿井下非结构化狭长受限空间中应用亟待解决的关键技术之一。针对传统概率路线图（PRM）算法在空间狭长封闭巷道环境中难以保障采样的节点均匀分布于自由空间中导致路径规划失效，以及节点可能距离障碍物较近导致规划的路径可通行性差等问题，提出了一种基于改进PRM算法的煤矿机器人路径规划方法。在构造阶段引入人工势场法，将落在障碍物中的节点沿与其距离最近自由空间中的节点连线方向推至自由空间，并在障碍物边缘建立斥力场，实现节点的均匀分布且使其距离障碍物有一定距离；在查询阶段融合D* Lite算法，当遇到动态障碍物或前方无法通行时可实现路径的重规划。仿真结果表明：改进PRM算法的节点均匀分布在自由空间中，且均距离障碍物一定距离，提高了路径规划的安全性；当节点数为100个时，改进PRM算法成功率较传统PRM算法提高了25%；随着节点数增加，传统PRM算法和改进PRM算法路径规划成功次数均呈增长趋势，但改进PRM算法在效率方面优势更明显；当节点数为400个时，改进PRM算法运行效率较传统PRM算法提高了35.13%，且规划的路径更平滑，路径长度更短；当障碍物突然出现时，改进PRM算法能够实现路径的重规划。Abstract: Path planning is a key technology that urgently need to be solved in application of coal mine robots in unstructured narrow confined spaces underground. The traditional probabilistic road map (PRM) algorithms are difficult to ensure uniform distribution of sampled nodes in free space in narrow and enclosed roadway environments, resulting in path planning failure. Nodes may be close to obstacles, resulting in poor passability of the planned path. In order to solve the above problems, a path-planning method for coal mine robot based on improved PRM algorithm is proposed. In the constructive phase, the artificial potential field method is introduced to push the node falling in the obstacle to the free space along the direction of the connection line of the node in the free space nearest to it. The repulsive force field is established at the edge of the obstacle to realize uniform distribution of nodes and make them a certain distance from the obstacle. In the query phase, the D* Lite algorithm is integrated to achieve path re-planning when encountering dynamic obstacles or when the front is impassable. The simulation results show that the nodes of the improved PRM algorithm are uniformly distributed in free space and are at a certain distance from obstacles. It improves the safety of path planning. When the number of nodes is 100, the success rate of the improved PRM algorithm is 25% higher than that of the traditional PRM algorithm. As the number of nodes increases, the number of successful path-planning attempts for both traditional and improved PRM algorithms shows an increasing trend. But the improved PRM algorithm has a more significant advantage in efficiency. When the number of nodes is 400, the operational efficiency of the improved PRM algorithm is 35.13% higher than that of the traditional PRM algorithms. The planned path is smoother and the path length is shorter. When obstacles suddenly appear, the improved PRM algorithm can achieve path re-planning.
-
0. 引言
为提高矿山安全生产保障能力，国家要求全国煤矿及非煤矿山建立和完善监测监控、人员定位、供水施救、压风自救、通信联络及紧急避险等井下安全避险“六大系统”[1]。其中，井下人员定位系统在遏制井下超定员生产、防止人员进入危险区域、及时发现未按时升井人员、加强特种作业人员管理、加强干部带班管理、实现煤矿井下作业人员考勤管理等工作中发挥着重要作用[2-4]。同时，井下车辆精确定位是矿井安全高效运输的重要保障[5]。目前矿井人员和车辆精确定位主要采用超宽带（Ultra Wide Band，UWB）无线通信技术，可实现厘米级高精度定位，并具有抗多径能力强、系统复杂性低等特点[6-11]。在算法上，主要采用飞行时间（Time of Flight，TOF）定位方法，具有定位精度不受信号发送功率、接收灵敏度和信号传输衰减影响，不需定位分站与定位卡时钟同步等优点，但需双天线或双定位分站联合测距和定向，以辨识定位卡位于定位分站左侧或右侧，不便于天线安装维护[12-14]。因此，本文提出了到达相位差（Phase Difference of Arrival，PDOA）与TOF煤矿井下联合定位方法，通过定位分站的2根天线与定位卡之间的TOF平均值计算定位卡距分站的距离，通过PDOA值判断定位卡在分站的哪一侧，可缩短双天线距离，将双天线集成为一体，便于安装维护。
1. 方法原理
1.1 TOF测距方法
目前应用广泛的TOF测距方法属于双向测距技术，主要利用无线电信号在源节点和目的节点之间的飞行时间来测量二者之间的距离[15-16]。TOF测距原理如图1所示。
源节点A向目的节点B发送一个请求数据包，目的节点B收到数据包并对其进行处理，将周转时间TA封装到应答数据包中并发送给源节点A，源节点A计算出从开始发送请求数据包到接收到目的节点B返回的应答数据包的总时间TZ，用总时间TZ减去周转时间TA就是双方数据包在飞行中度过的往返时间，记为TR。假定在每个方向发生的飞行时间TF等于一半的往返时间：
$$ T_{{\rm{F}}}=T_{{\rm{R}}}/2=(T_{{\rm{Z}}}-T_{{\rm{A}}})/2 $$ (1) 则2个节点之间的距离为
$$ D=cT_{{\rm{F}}} $$ (2) 式中c为光速，c=3×108 m/s。
矿用本安型定位分站为双天线设计，分站通过1 m长的馈线与天线相连。安装时，2根天线在分站相反方向形成与巷道壁平行的直线。2根天线独立与通信覆盖范围内的定位卡测距，利用二者距离差的符号判断定位卡在分站的哪一侧，从而实现对定位卡的一维定位，如图2所示。
1.2 PDOA定位算法
PDOA定位算法具有以下优点：角度估计精度高；可减小用于定向的2根天线之间的距离；将定位分站的2根天线一体化，便于安装维护；定位精度高。基于PDOA的到达角度（Angle of Arrival，AOA）估计原理如图3所示。
从定位卡发送的无线电信号到达定位分站的2根天线，信号路径长度的差别p与天线M，N之间的距离d和AOA值θ满足下式。
$$ p=d{\rm{sin}} \; \theta $$ (3) 2根天线接收的信号中的第一路径（First-Path）的PDOA为
$$\alpha =2{\text{π}} p / \lambda $$ (4) 式中λ为波长。
$$\theta=\arcsin \; (\alpha\lambda / 2 {\text{π}} d) $$ (5) 当d＜λ/2时，θ与α在[−π/2，π/2]上有一一对应关系。
1.3 PDOA与TOF联合定位方法
基于UWB的PDOA与TOF联合定位方案：通过2根天线与定位卡之间的TOF平均值估计定位卡距分站的距离；在天线距离小于信号半波长的情况下，对于煤矿巷道一维定位场景，可以基于PDOA值判断定位卡在分站的哪一侧，而不必求出具体的AOA值。
仅采用TOF技术的定位分站安装时2根天线必须分开一定的距离，且使用支架固定，需要将2根天线的距离录入定位软件系统，而天线较易因其他矿井施工活动误碰而改变位置，使系统稳定性受到影响[20-21]。采用PDOA与TOF联合定位方法的定位分站用仅5 cm长的棒状天线取代接有1 m馈线的平面天线，便于天线角度固定，安装简单，维护方便，有利于节省人力成本，提升系统稳定性。棒状天线和平面天线对比如图4所示。
2. 应用测试
2.1 测试设计
在神东煤炭集团大柳塔煤矿东辅助运输大巷进行测试。巷道宽6 m、高5 m，断面为半圆拱形，巷道平坦、无煤尘。巷道顶部及左右侧巷帮中部有金属管道，整个巷道为水泥墙面，地面有有轨电车轨道，如图5所示。主要测试器材为矿用本安型定位分站、车辆定位卡及相应的固定支撑器材，激光测距仪等。定位分站和车辆定位卡发射的电磁波信号中心频率为4.0 GHz。
本文测试分为PDOA方向测试和TOF精度测试2个部分，布置如图6所示。测试步骤：① 将定位分站与车辆定位卡分别固定到支撑器材上。② 用RS485通信线缆将定位分站与业务化运行的矿井定位系统定位分站连接，开启分站。③ 将定位分站天线固定到巷帮，高度为2 m。④ 调节车辆定位卡到合适位置固定，高度为1.5 m。⑤ 进行PDOA方向测试，车辆定位卡在定位分站两侧位置取样。⑥ 用激光测距仪测量车辆定位卡天线与定位分站天线的距离。⑦ 车辆定位卡与定位分站每1 s通信1次，距离固定后，每个采样点车辆定位卡固定10 s以获得稳定读数。⑧ 移动车辆定位卡远离定位分站，重复测试步骤⑤和步骤⑥。⑨ 进行TOF精度测试，车辆定位卡在定位分站单侧位置取样。⑩ 用激光测距仪测量车辆定位卡天线与定位分站天线的距离。⑪ 距离固定后，每个采样点车辆定位卡固定1 min以获得足够数据。⑫ 移动车辆定位卡远离定位分站，重复测试步骤⑩和步骤⑪。⑬ 测试数据通过定位系统上传至地面服务器，测试完毕后从服务器下载数据到本地。
![]()
图 6 PDOA方向测试和TOF精度测试布置Figure 6. Layout of PDOA directional test and TOF precision test2.2 PDOA方向测试结果
PDOA方向测试结果如图7所示（图中包含41个点位的测试数据，零点是定位分站所在位置），可看出在定位分站两侧，PDOA值符号相反。因此，可以通过PDOA值的符号判断定位卡方向。
2.3 TOF精度测试结果
在距定位分站一侧200 m范围内的18个点位进行TOF精度测试，距离近时测试点间距小，距离远时测试点间距大。将测试点位测试数据的平均值用于误差计算，结果见表1。
表 1 TOF精度测试数据Table 1. TOF precision test data真实距离/m 测量均值/m 绝对误差/m 相对误差/% 1.938 1.895 0.043 2.22 2.922 2.855 0.067 2.29 4.779 4.829 0.050 1.05 8.186 8.206 0.020 0.24 10.984 10.874 0.110 1.00 15.338 15.198 0.140 0.91 20.726 20.616 0.110 0.53 25.446 25.541 0.095 0.37 30.921 30.952 0.031 0.10 40.767 40.696 0.071 0.17 61.731 61.627 0.104 0.17 82.217 82.229 0.012 0.01 100.479 100.398 0.081 0.08 122.302 122.376 0.074 0.06 139.672 139.557 0.115 0.08 159.092 158.951 0.141 0.09 177.285 177.398 0.113 0.06 198.489 198.350 0.139 0.07 将测试点位误差与真实距离绘制成曲线，如图8所示。可看出在约82 m处误差最小，为1.2 cm；在约159 m处误差最大，为14.1 cm，接近定位系统设备所采用的DW1000型UWB定位芯片的设计理论精度10 cm；在测试距离范围内，精度与距离没有明显的相关变化趋势。
由于业务化运行的矿井定位系统有实时展示需求，为保证展示效果，运动中的定位目标轨迹应尽量平滑。所以，系统的定位稳定性较为关键，即在不考虑定位精度的情况下，对同一位置的定位卡进行多次测量，其测量值的分布范围应尽量小。取距定位分站距离最远的198 m处测试点位的测试数据进行分析，在此处共有62个测量值。计算测量值与其均值的离差，将离差分为10组，通过直方图统计各组测量值的频数，结果如图9所示。可看出离差全部在10 cm内，分布近似正态分布。将离差进行正态分布拟合，得到离差分布的标准差为0.033 4。
离差经验分布与正态分布拟合结果的累计概率曲线如图10所示，可看出二者吻合度非常好，表明本文提出的联合定位方法具有良好的稳定性。
3. 结语
采用PDOA与TOF煤矿井下联合定位方法的定位分站用长度仅5 cm的棒状天线取代接有1 m馈线的平面天线，硬件实现方案更简单，维护更方便，有利于节省人力成本，提升系统稳定性。在大柳塔煤矿井下进行了PDOA方向和TOF精度测试，结果表明：利用PDOA值的符号可以正确判断定位卡在定位分站的哪一侧；定位精度在15 cm以内，可为煤矿安全生产提供精准的位置服务；在200 m测试距离范围内，定位精度不受距离远近影响；TOF测距数值稳定在相对其均值±10 cm的范围内，具有良好的稳定性。
-
表 1 不同节点数量时改进前后PRM算法路径规划结果统计
Table 1 Path planning results statistics of probabilistic road map algorithm before and after improvement with different number of sampling nodes
算法 节点
数/个成功
次数运行
时间/s路径
长度/m转折点
数/个路径数
量/条传统PRM算法 100 10 3.14 420.35 10 4 200 30 9.78 390.39 8 3 300 61 14.09 381.09 7 2 400 100 29.92 372.78 6 1 改进PRM算法 100 35 2.63 400.93 6 3 200 66 5.13 382.64 5 2 300 97 9.87 372.46 4 1 400 100 19.41 369.57 4 1 
下载: 导出CSV
-
[1] 周李兵. 煤矿井下无轨胶轮车无人驾驶系统研究[J]. 工矿自动化,2022,48(6):36-48. ZHOU Libing. Research on unmanned driving system of underground trackless rubber-tyred vehicle in coal mine[J]. Journal of Mine Automation,2022,48(6):36-48.
[2] 薛光辉,候称心,张云飞,等. 煤矿巷道修复重载作业机器人现状与发展趋势[J]. 工矿自动化,2020,46(9):8-14. XUE Guanghui,HOU Chenxin,ZHANG Yunfei,et al. Current situation and development trend of heavy-duty operation robot for coal mine roadway repair[J]. Industry and Mine Automation,2020,46(9):8-14.
[3] 袁晓明,郝明锐. 煤矿辅助运输机器人关键技术研究[J]. 工矿自动化,2020,46(8):8-14. YUAN Xiaoming,HAO Mingrui. Research on key technologies of coal mine auxiliary transportation robot[J]. Industry and Mine Automation,2020,46(8):8-14.
[4] 金祖进,程刚,郭锋,等. 煤矿搜救机器人最优路径规划算法[J]. 工矿自动化,2018,44(10):24-28. JIN Zujin,CHENG Gang,GUO Feng,et,al. Optimal path planning algorithm for coal mine search and rescue robot[J]. Industry and Mine Automation,2018,44(10):24-28.
[5] 王国法,赵国瑞,任怀伟. 智慧煤矿与智能化开采关键核心技术分析[J]. 煤炭学报,2019,44(1):34-41. WANG Guofa,ZHAO Guorui,REN Huaiwei. Analysis on key technologies of intelligent coal mine and intelligent mining[J]. Journal of China Coal Society,2019,44(1):34-41.
[6] 葛世荣,胡而已,李允旺. 煤矿机器人技术新进展及新方向[J]. 煤炭学报,2023,48(1):54-73. DOI: 10.13225/j.cnki.jccs.2022.1661 GE Shirong,HU Eryi,LI Yunwang. New progress and direction of robot technology in coal mine[J]. Journal of China Coal Society,2023,48(1):54-73. DOI: 10.13225/j.cnki.jccs.2022.1661
[7] 田子建,高学浩,张梦霞. 基于改进人工势场的矿井导航装置路径规划[J]. 煤炭学报,2016,41(增刊2):589-597. TIAN Zijian,GAO Xuehao,ZHANG Mengxia. Path planning based on the improved artificial potential field of coal mine dynamic target navigation[J]. Journal of China Coal Society,2016,41(S2):589-597.
[8] GAO Yongxin,DAI Zhonglin,YUAN Jing. A multiobjective hybrid optimization algorithm for path planning of coal mine patrol robot[J]. Computational Intelligence and Neuroscience,2022,2022:9094572. DOI: 10.1155/2022/9094572.
[9] SONG Baoye,MIAO Huimin,XU Lin. Path planning for coal mine robot via improved ant colony optimization algorithm[J]. Systems Science & Control Engineering,2021,9(1):283-289.
[10] 陶德俊,姜媛媛,刘延彬,等. 煤矿救援机器人路径平滑算法研究[J]. 工矿自动化,2019,45(10):49-54. TAO Dejun,JIANG Yuanyuan,LIU Yanbin,et al. Research on path smoothing algorithm of coal mine rescue robot[J]. Industry and Mine Automation,2019,45(10):49-54.
[11] MAO Ruiqing,MA Xiliang. Research on path planning method of coal mine robot to avoid obstacle in gas distribution area[J]. Journal of Robotics,2016,2016:120-125.
[12] 鲍久圣,张牧野,葛世荣,等. 基于改进A*和人工势场算法的无轨胶轮车井下无人驾驶路径规划[J]. 煤炭学报,2022,47(3):1347-1360. BAO Jiusheng,ZHANG Muye,GE Shirong,et al. Underground driverless path planning of trackless rubber tyred vehicle based on improved A* and artificial potential field algorithm[J]. Journal of China Coal Society,2022,47(3):1347-1360.
[13] 代嘉惠. 大功率本安驱动煤矿救援机器人定位与建图算法研究[D]. 重庆: 重庆大学, 2019. DAI Jiahui. Study on localization and mapping algorithm of high-power intrinsically safe coal mine rescue robot[D]. Chongqing: Chongqing University, 2019.
[14] 金书奎,寇子明,吴娟. 煤矿水泵房巡检机器人路径规划与跟踪算法的研究[J]. 煤炭科学技术,2022,50(5):253-262. JIN Shukui,KOU Ziming,WU Juan. Research on path planning and tracking algorithm of inspection robot in coal mine water[J]. Coal Science and Technology,2022,50(5):253-262.
[15] 田洪清,王建强,黄荷叶,等. 越野环境下基于势能场模型的智能车概率图路径规划方法[J]. 兵工学报,2021,42(7):1496-1505. DOI: 10.3969/j.issn.1000-1093.2021.07.017 TIAN Hongqing,WANG Jianqiang,HUANG Heye,et al. Probabilistic roadmap method for path planning of intelligent vehicle based on artificial potential field model in off-road environment[J]. Acta Armamentarii,2021,42(7):1496-1505. DOI: 10.3969/j.issn.1000-1093.2021.07.017
[16] SULAIMAN S,SUDHEER A P. Modeling of a wheeled humanoid robot and hybrid algorithm-based path planning of wheel base for the dynamic obstacles avoidance[J]. Industrial Robot,2022,49(6):1058-1076. DOI: 10.1108/IR-12-2021-0298
[17] KAVRAKI L E,SVESTKA P,LATOMBE J C,et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces[J]. IEEE Transactions on Robotics and Automation,1996,12(4):566-580. DOI: 10.1109/70.508439
[18] KHATIB O. Real-time obstacle avoidance for manipulators and mobile robots[J]. The International Journal of Robotics Research,1986,5(1):90-98. DOI: 10.1177/027836498600500106
[19] 杨奇峰,曲道奎,徐方. 基于障碍物运动预测的移动机器人路径规划[J]. 计算机工程与设计,2021,42(1):182-188. DOI: 10.16208/j.issn1000-7024.2021.01.027 YANG Qifeng,QU Daokui,XU Fang. Path planning of mobile robot based on obstacle motion prediction[J]. Computer Engineering and Design,2021,42(1):182-188. DOI: 10.16208/j.issn1000-7024.2021.01.027
[20] 杜轩,欧资臻. 改进D* Lite和人工势场法的移动机器人路径规划研究[J]. 制造业自动化,2022,44(2):153-158. DU Xuan,OU Zizhen. Research on the path planning of mobile robots to improve the method of D* Lite and artificial potential field[J]. Manufacturing Automation,2022,44(2):153-158.
[21] KOENIG S, LIKHACHEV M. D* lite[C]. AAAI Conference on Artificial Intelligence, Palo Alto, 2002: 476-483.
[22] 周非同. 室内移动机器人导航系统研究与设计[D]. 合肥: 中国科学技术大学, 2019. ZHOU Feitong. Research and design of indoor mobile robot's navigation system[D]. Hefei: University of Science and Technology of China, 2019.
[23] 黄鲁,周非同. 基于路径优化D* Lite算法的移动机器人路径规划[J]. 控制与决策,2020,35(4):877-884. HUANG Lu,ZHOU Feitong. Path planning of moving robot based on path optimization of D* Lite algorithm[J]. Control and Decision,2020,35(4):877-884.
-
期刊类型引用(2)
1. 季瑞翔. 一种基于视觉感知的带式输送机煤量测量方法研究. 山东煤炭科技. 2024(05): 110-114 . 
百度学术
2. 解海燕，李杰，赵国栋. 非结构化高维大数据异常流量时间点挖掘算法. 计算机仿真. 2024(07): 474-478 . 百度学术
其他类型引用(1)
计量
- 文章访问数: 1092
- HTML全文浏览量: 46
- PDF下载量: 46
- 被引次数: 3
