煤矿巡检机器人路径规划方法

摘要：路径规划是巡检机器人自主移动的关键技术。煤矿巡检机器人采用快速扩展随机树（RRT）算法规划路径时存在收敛速度慢、搜索效率低等问题。针对该问题，提出了一种合力势场引导RRT 算法：利用合力势场中的斥力场构建动态步长，使煤矿巡检机器人在障碍物附近调整步长，提高算法收敛速度；利用目标节点和随机节点2 个方向上的引力场与最近障碍物对煤矿巡检机器人产生的斥力场形成的合力场来改善新节点的生成方向，降低树在扩展时的随机性，提高算法搜索效率。对基于合力势场引导RRT 算法规划的路径进行剪枝操作，并利用三阶贝塞尔曲线进行平滑处理。在Matlab 软件中对基于合力势场引导RRT 算法的煤矿巡检机器人路径规划方法进行仿真实验，结果表明：与RRT 算法和RRT*算法相比，简单环境下合力势场引导RRT 算法的路径规划时间平均值分别减少了33.84% 和44.27%，路径长度平均值分别减少了15.29% 和4.42%，复杂环境下路径规划时间平均值分别减少了34.93% 和47.12%，路径长度平均值分别减少了13.64% 和9.44%，模拟煤矿环境下路径规划时间平均值分别减少了28.06% 和42.67%，路径长度平均值分别减少了12.22% 和10.18%；对基于合力势场引导RRT 算法规划的路径进行剪枝和平滑操作后，路径转折点减少，路径角度变化减小，路径更加平滑。

关键词：煤矿巡检机器人；路径规划；RRT 算法；合力势场；动态步长；路径平滑

中图分类号：TD67 文献标志码：A

0 引言

煤矿井下为高风险工作环境，温湿度变化较大，空气质量差，噪声大，易发生瓦斯爆炸、煤尘爆炸、水害、火灾等安全事故，需通过定期巡检来排除安全隐患。人工巡检方式对矿工身体健康及生命安全造成一定威胁。煤矿巡检机器人是一种可替代人工的移动机器人系统，可在井下恶劣环境中执行巡检任务[1]。路径规划是巡检机器人自主移动的关键技术，旨在预定条件下找到从起点到终点的可行路径[2]。路径规划包括局部路径规划和全局路径规划。局部路径规划是指已知局部环境的静态与动态信息，规划出一条可行的局部最优路径。局部路径规划算法主要有动态窗口法[3]、神经网络法[4-5]、人工势场法[6-7]等，其中人工势场法因算法简单有效、规划的路径平滑等优点而被广泛关注。全局路径规划是指已知全局环境的静态信息，规划出一条可行的从起点到终点的全局最优路径。全局路径规划算法主要有Dijkstra 算法[8-9]、蚁群算法[10-11]、快速扩展随机树（Rapidly-expanding Random Tree， RRT） 算法[12-13]等，其中RRT 算法因能够进行空间预处理且具有概率完备性[14-15]而被广泛应用，但在搜索过程中具有盲目性和随机性，因此产生大量无效节点，导致搜索效率较低。

许多学者针对RRT 算法的不足提出了解决方案。文献[16]提出了将随机采样点向目标点方向偏移的RRT*算法，偏移量与采样点和障碍物之间的最小距离相关，隐性地引入了人工势场中的斥力值，但没有引入斥力方向，无法使树在扩展时远离障碍物。文献[17]在目标偏置RRT 算法基础上，加入了人工势场法的引力部分，提高了算法搜索效率，但没有考虑机器人与障碍物之间人工斥力场的作用。文献[18]在动态环境下引入人工势场引导随机树扩展，将自适应概率选择目标点作为采样点，采用全局规划结合局部重新规划的方法，提高了动态规划的成功率，但在高度动态的环境中无法实时适应环境变化，导致规划出的路径非最优。文献[19]引入目标动态概率采样和人工势场引导随机树扩展，减少了冗余节点，保证了路径的安全性，但采用固定步长，导致收敛速度降低。文献[20]引入人工势场法来引导随机采样点的生成，降低了搜索范围，并采用遗传算法优化路径，但算法复杂度高，导致路径规划时间变长。

本文提出一种合力势场引导RRT 算法，结合路径平滑策略，可使煤矿巡检机器人在短时间内规划出一条光滑的路径，且路径长度较短。通过仿真实验验证了基于合力势场引导RRT 算法的煤矿巡检机器人路径规划方法的有效性。

1 合力势场引导RRT 算法

1.1 合力势场

合力势场由引力场和斥力场构成，即

Utotal （p） = Uatt1 （p）+Uatt2 （p）+Urep （p） （1）

式中： 为当前节点位置；Utotal （p）为当前节点所受合力势场；Uatt1 （p）为目标节点对当前节点产生的引力场；Uatt2 （p）为随机节点对当前节点产生的引力场；Urep （p）为最近障碍物对当前节点产生的斥力场。

式中： KP为引力场常数； pgoal为目标节点位置；ρ（p， pgoal）为当前节点到目标节点的欧氏距离； prand随机节点位置；ρ（p， prand）为当前节点到随机节点的欧氏距离；pobs为最近障碍物位置； ρ（p， pobs）为当前节点到最近障碍物的欧氏距离；ρ0为障碍物影响距离；Kr为斥力场常数。

若ρ（p; pobs）≤ 0，则煤矿巡检机器人受到斥力作用，反之不受斥力作用。

对式（2）−式（4）求负梯度，得到目标节点对当前节点的引力Fatt1 （p）、随机节点对当前节点的引力Fatt2 （p）和最近障碍物对当前节点的斥力 Frep （p）。

则合力为

Ftotal （p） = Fatt1 （p）+ Fatt2 （p）+ Frep （p） （10）

1.2 动态步长

在煤矿复杂环境中，若选择的固定步长过大，RRT 算法在扩展时会在障碍物附近过度生长，增加与障碍物碰撞的可能性；若选择的固定步长过小，RRT 算法需要更多的迭代次数来覆盖相同的空间，导致收敛速度慢。对此，在RRT 算法中引入与障碍物有关的斥力场Urep （p）来对步长进行动态调整[21]，使巡检机器人在开阔空间使用长步长来快速接近目标节点，在狭小空间使用短步长以避免碰撞障碍物。具体方案：设置障碍物影响距离ρ0。当煤矿巡检机器人离最近障碍物的距离≤ρ0，则步长受到障碍物对煤矿巡检机器人的斥力作用而变小，离障碍物越近则步长越小，从而避开障碍物。当煤矿巡检机器人离最近障碍物的距离＞ρ0，则步长不受影响，保持长步长快速接近目标节点。动态步长为

式中e为初始扩展步长。

1.3 合力势场引导新节点生成策略

为了改善RRT 算法中新节点的生成方向，减少树在扩展时的随机性，在目标节点和随机节点2 个方向上引入引力Fatt1 （p） ，Fatt2 （p）。用2 个方向的引力与离巡检机器人最近障碍物对其产生的斥力Frep （p）的合力来引导树的扩展。引入合力势场后新节点的生成如图1 所示。新节点生成方向由2 个方向的引力Fatt1 （p）， Fatt2 （p）和斥力Frep （p）叠加而成，即图1 中红色箭头方向。新节点位置为

若pnew与p连线上没有障碍物，则将pnew加入扩展树；pnew与p连线上有障碍物，则将pnew舍去，并根据随机产生的点重新生成pnew。

2 路径平滑策略

合力势场引导RRT 算法生成路径的折角过多，导致煤矿巡检机器人进行巡检任务时不够顺畅，需对路径进行剪枝及平滑处理。

假设生成的全局路径中所有路径点的集合为X={X1，X2，…，Xn}（n 为路径点总数） ， 如图2 所示。若路径点X1和X4能直接连线且中间没有障碍物，则X1和X4之间的X2和X3作为冗余节点被去除。从起点到终点依次遍历整个集合，剔除所有的冗余节点，完成路径剪枝。

剪枝后的路径是分段的，不够平滑，煤矿巡检机器人在拐点处可能会因方向变化而无法正常行走，因此需对路径进行平滑处理。贝塞尔曲线是一种连续的平滑曲线，具有曲率连续、控制简单等特点，因此采用三阶贝塞尔曲线来平滑路径。n 阶贝塞尔曲线可由n 次Bernstein 基多项式表示。

式中：t为归一化时间变量，t ∈[0;1]；Bi，n （t）为Bernstein基多项式。

三阶贝塞尔曲线表达式为

X （t） = （1-t）3X1 +3t（1-t）2X2 +3t2 （1-t）X3 +t2X4（15）

选取X1−X4 作为控制点来构建三阶贝塞尔曲线，以达到路径平滑的目的，平滑效果如图3 所示。

3 煤矿巡检机器人路径规划流程

基于合力势场引导RRT 算法的煤矿巡检机器人路径规划流程如图4 所示。

1） 在目标空间中， 设定起点pstart、目标节点pgoal 、初始扩展步长e 、障碍物影响距离ρ0， 根据式（1）−式（5）计算合力势场。

2） 随机生成采样点prand，遍历整个随机树产生的节点，选取距采样点prand最近的节点作为当前节点p。

3） 根据式（11） 计算动态步长， 其受到斥力Frep （p）的影响而自适应动态调整。当前节点离障碍物越近，则步长越小，反之保持初始步长。

4） 根据式（12）计算得到新节点pnew。连接新节点pnew和当前节点p，检测pnew和p之间有无障碍物，若无则将pnew加入扩展树，否则取消本次扩展，并返回步骤2）。

5） 若扩展树中包含目标节点pgoal，则从pgoal回溯到起点pstart规划出一条路径，否则返回步骤2）。

6） 对规划路径进行剪枝和平滑处理。

4 实验与结果分析

4.1 合力势场引导RRT 算法实验验证

为了验证合力势场引导RRT 算法的有效性，采用Matlab 软件对其在二维空间中进行仿真实验。仿真环境分为简单环境、复杂环境、模拟煤矿环境，大小均为800 mm×800 mm（长×宽）。简单环境中障碍物少，可通行区域大，环境较为单一；复杂环境中存在大量形状各异的障碍物，可通行区域小；模拟煤矿环境是模拟由辅运巷到达回风巷工况（转巷工况）。在3 种环境下对合力势场引导RRT 算法、RRT算法和RRT*算法重复进行100 次仿真实验。

4.1.1 简单环境仿真实验

设置简单环境中起点位置为（50 mm， 50 mm） ，目标节点位置为（750 mm，750 mm），初始扩展步长为30 mm，障碍物影响距离为30 mm。3 种算法在简单环境中规划的路径如图5 所示，其中黑色区域代表障碍物，白色区域代表可通行区域，x，y 分别为长度、宽度方向。可看出RRT 算法和RRT*算法规划路径中的扩展树会随机分布在可通行区域，合力势场引导RRT 算法规划路径中的扩展树向目标节点靠近，降低了扩展树的随机性，路径长度更短、路径规划效率更高。

100 次仿真实验后，3 种算法的路径规划时间和路径长度平均值见表1。可看出合力势场引导RRT算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了33.84% 和44.27%，规划的路径长度平均值分别减少了15.29%和4.42%。

4.1.2 复杂环境仿真实验

考虑煤矿巷道中存在一些不规则的障碍物，为了进一步检验合力势场引导RRT 算法的有效性，在复杂环境中放置了大量大小不一、形状各异的障碍物，其他参数设置与简单环境中一致。3 种算法规划的路径如图6 所示。可看出RRT 算法和RRT*算法规划路径较曲折，而合力势场引导RRT 算法规划的路径更加平整。

3 种算法的路径规划时间和路径长度平均值见表2。可看出合力势场引导RRT 算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了34.93% 和47.12%，规划的路径长度平均值分别减少了13.64% 和9.44%。

4.1.3 模拟煤矿环境仿真实验

煤矿巡检机器人主要在井下巷道巡检。转巷工况通常发生在巷道交汇或转弯处，这些位置往往狭窄且曲折，且距离较长，坡度较大，限制了大型设备和工具使用。相比简单环境和复杂环境，转巷工况更贴合煤矿实际环境。设置模拟煤矿环境中起点位置为（50 mm， 50 mm） ， 目标节点位置为（750 mm，750 mm），初始扩展步长为40 mm，障碍物影响距离为50 mm。3 种算法的路径规划结果如图7 所示。可看出RRT 算法和RRT*算法生成的扩展树杂乱无章，规划路径质量很差，而合力势场引导RRT 算法利用障碍物的斥力作用，可在巷道中规划出可行的路径。

3 种算法的路径规划时间和路径长度平均值见表3。可看出合力势场引导RRT 算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了28.06% 和42.67%，规划的路径长度平均值分别减少了12.22% 和10.18%。

可见，合力势场引导RRT 算法能够在生成较短路径的同时，提高路径规划效率。

4.2 路径平滑策略实验验证

在3 种仿真环境下，对基于合力势场引导RRT算法规划的路径进行剪枝和平滑处理，并与处理前的路径进行对比，结果如图8 所示。可看出采用路径平滑策略后，规划路径的转折点较未采用路径平滑策略时少，路径转折点处更加平滑。

计算剪枝和平滑处理前后规划路径的角度变化平均值，结果见表4。可看出在3 种环境下，剪枝和平滑后路径角度变化平均值较之前小，说明剪枝和平滑处理后规划路径的平滑性更好。

5 结论

1） 合力势场引导RRT 算法利用合力势场中的斥力场来构建动态步长，使煤矿巡检机器人在障碍物附近调整步长，提高算法的收敛速度；利用目标节点和随机节点2 个方向上的引力场与最近障碍物对煤矿巡检机器人产生的斥力场形成的合力场来改善新节点的生成方向，降低树在扩展时的随机性，提高算法的搜索效率。

2） 路径平滑策略通过剪枝和三阶贝塞尔曲线平滑处理，使得合力势场引导RRT 算法规划的路径更加平滑。

3） 仿真实验结果表明：在简单环境下，合力势场引导RRT 算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了33.84% 和44.27%，路径长度平均值分别减少了15.29% 和4.42%；在复杂环境下，合力势场引导RRT 算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了34.93% 和47.12%，路径长度平均值分别减少了13.64% 和9.44%；在模拟煤矿环境下，合力势场引导RRT 算法的路径规划时间平均值较RRT 算法和RRT*算法分别减少了28.06% 和42.67%，路径长度平均值分别减少了12.22% 和10.18%；采用路径平滑策略后，规划路径角度变化减小，路径更加平滑。

参考文献（References）：

[ 1 ]蔡治华，周东旭，赵明辉. 煤矿巡检机器人控制系统设计[J]. 工矿自动化，2022，48（5）：112-117.

CAI Zhihua， ZHOU Dongxu， ZHAO Minghui. Designof coal mine inspection robot control system[J]. Journalof Mine Automation，2022，48（5）：112-117.

[ 2 ]姜媛媛，丰雪艳. 基于改进A*算法的煤矿救援机器人路径规划[J]. 工矿自动化，2023，49（8）：53-59.

JIANG Yuanyuan，FENG Xueyan. Path planning of coalmine rescue robot based on improved A* algorithm[J].Journal of Mine Automation，2023，49（8）：53-59.

[ 3 ]李薪颖，单梁，常路，等. 复杂环境下基于多目标粒子群的DWA 路径规划算法[J]. 国防科技大学学报，2022，44（4）：52-59.

LI Xinying， SHAN Liang， CHANG Lu， et al. DWApath planning algorithm based on multi-objectiveparticle swarm optimization in complex environment[J].Journal of National University of Defense Technology，2022，44（4）：52-59.

[ 4 ]ZHANG Yinyan， LI Shuai，GUO Hongliang. A type ofbiased consen sus-based distributed neural network forpath planning[J]. Nonlinear Dynamics， 2017， 89：1803-1815.

[ 5 ]李波，杨志鹏，贾卓然，等. 一种无监督学习型神经网络的无人机全区域侦察路径规划[J]. 西北工业大学学报，2021，39（1）：77-84.

LI Bo， YANG Zhipeng， JIA Zhuoran， et al. Anunsupervised learning neural network for planning UAVfull-area reconnaissance path[J]. Journal ofNorthwestern Polytechnical University， 2021， 39（1） ：77-84.

[ 6 ]OROZCO-ROSAS U， OSCAR M， SEPULVEDA R.Mobile robot path planning using membraneevolutionary artificial potential field[J]. Applied SoftComputing，2019，77：236-251.

[ 7 ]翟丽，张雪莹，张闲，等. 基于势场法的无人车局部动态避障路径规划算法[J]. 北京理工大学学报，2022，42（7）：696-705.

ZHAI Li，ZHANG Xueying，ZHANG Xian，et al. Localdynamic obstacle avoidance path planning algorithm forunmanned vehicles based on potential field method[J].Transactions of Beijing Institute of Technology， 2022，42（7）：696-705.

[ 8 ]叶颖诗， 魏福义， 蔡贤资. 基于并行计算的快速Dijkstra 算法研究[J]. 计算机工程与应用， 2020，56（6）：58-65.

YE Yingshi， WEI Fuyi， CAI Xianzi. Research on fastDijkstra algorithm based on parallel computing[J].Computer Engineering and Applications， 2020， 56（6） ：58-65.

[ 9 ]巩慧，倪翠，王朋，等. 基于Dijkstra 算法的平滑路径规划方法[J]. 北京航空航天大学学报，2024，50（2）：535-541。.GONG Hui，NI Cui，WANG Peng，et al. A smooth pathplanning method based on Dijkstra algorithm[J]. Journalof Beijing University of Aeronautics and Astronautics，2024，50（2）：535-541.

[10]刘新宇，谭力铭，杨春曦，等. 未知环境下的蚁群−聚类自适应动态路径规划[J]. 计算机科学与探索，2019，13（5）：846-857.

LIU Xinyu， TAN Liming， YANG Chunxi， et al. Selfadjustabledynamic path planning of unknownenvironment based on ant colony-clusteringalgorithm[J]. Journal of Frontiers of Computer Scienceand Technology，2019，13（5）：846-857.

[11]敖邦乾，杨莎，叶振环. 改进蚁群算法水面无人艇平滑路径规划[J]. 控制理论与应用， 2021， 38（7） ：1006-1014.

AO Bangqian，YANG Sha，YE Zhenhuan. Improved antcolony algorithm for unmanned surface vehicle smoothpath planning[J]. Control Theory & Applications，2021，38（7）：1006-1014.

[12]ZHANG Haojian，WANG Yunkuan，ZHENG Jun，et al.Path planning of industrial robot based on improvedRRT algorithm in complex environments[J]. IEEEAccess，2018，6. DOI：10.1109/access.2018.2871222.

[13]LI Binghui， CHEN Badong. An adaptive rapidlyexploringrandom tree[J]. IEEE/CAA Journal ofAutomatica Sinica，2021，9（2）：283-294.

[14]NGUYEN M K， JAILLET L， REDON S. ART-RRT：As-rigid-as-possible exploration of ligand unbindingpathways[J]. Journal of Computational ChemistryOrganic Inorganic Physical Biological， 2018， 39（11） ：665-678.

[15]BRY A，ROY N. Rapidly-exploring random belief treesfor motion planning under uncertainty[C]. IEEEInternational Conference on Robotics and Automation，Shanghai，2011：723-730.

[16]QURESHI A H， MUMTAZ S， FAHAD L K， et al.Adaptive potential guided directional-RRT[C]. IEEEInternational Conference on Robotics & Biomimetics，Shenzhen，2014：1887-1892.

[17]刘成菊， 韩俊强， 安康. 基于改进RRT 算法的RoboCup 机器人动态路径规划[J]. 机器人， 2017，39（1）：8-15.

LIU Chengju，HAN Junqiang，AN Kang. Dynamic pathplanning based on an improved RRT algorithm forRoboCup robot[J]. Robot，2017，39（1）：8-15.

[18]司徒华杰，雷海波，庄春刚. 动态环境下基于人工势场引导的RRT 路径规划算法[J]. 计算机应用研究，2021，38（3）：714-717，724.

SITU Huajie， LEI Haibo， ZHUANG Chungang.Artificial potential field based RRT algorithm for pathplanning in dynamic environment[J]. ApplicationResearch of Computers，2021，38（3）：714-717，724.

[19]李伟东，李乐. 基于改进RRT 算法的无人车路径规划[J]. 计算机测量与控制，2023，31（1）：160-166.

LI Weidong，LI Le. Path planning of unmanned vehiclebased on improved RRT algorithm[J]. ComputerMeasurement & Control，2023，31（1）：160-166.

[20]陈侠，刘奎武，毛海亮. 基于APF−RRT 算法的无人机航迹规划[J]. 电光与控制，2022，29（5）：17-22.

CHEN Xia， LIU Kuiwu， MAO Hailiang. UAV pathplanning based on APF-RRT algorithm[J]. ElectronicsOptics & Control，2022，29（5）：17-22.

[21]王道威，朱明富，刘慧. 动态步长的RRT 路径规划算法[J]. 计算机技术与发展，2016，26（3）：105-107，112.

WANG Daowei， ZHU Mingfu， LIU Hui. Rapidlyexploringrandom tree algorithm based on dynamicstep[J]. Computer Technology and Development，2016，26（3）：105-107，112.

基金项目：国家自然科学基金资助项目（62003001）；安徽高校自然科学研究重大项目（2023AH040157）。