基于改进A*算法的煤矿救援机器人路径规划

姜媛媛 丰雪艳

摘要：路徑规划是煤矿救援机器人研究的重要内容之一。针对灾后煤矿环境非结构化的特点，以及传统 A*算法规划的路径长度非最短、拐弯次数多和平滑度较差等问题，提出一种基于改进 A*算法的煤矿救援机器人路径规划方法。对真实环境中的地图信息进行二值化处理，构建栅格地图；判断当前点与目标点的相对位置，利用改进 A*算法进行路径规划，得到一条从当前点到目标点的路径；利用 Douglas-Peucker（D?P）算法提取路径上的关键节点，采用三次样条插值函数对关键节点进行拟合，完成对路径的平滑处理。改进 A*算法将传统 A*算法的8邻域搜索扩展为有目的性的13邻域搜索，在进行路径搜索时，先对当前点和目标点的位置关系进行判断，从而减少路径节点，减小路径长度，提升路径平滑度。Matlab仿真结果表明：与8邻域 A*算法、24邻域 A*算法、48邻域 A*算法相比，改进 A*算法在路径长度、拐弯次数、平滑度等方面有一定优化，更适用于煤矿救援机器人路径规划；与 Fuzzy算法相比，改进 A*算法路径规划所用时间更短，规划的路径长度更短，拐弯次数更少。

关键词：煤矿救援机器人；路径规划；A*算法；邻域扩展；Douglas-Peucker算法；路径平滑；三次样条插值函数

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

Path planning of coal mine rescue robot based on improved A* algorithm

JIANG Yuanyuan1，2， FENG Xueyan1

（1. School of Electrical and Information Engineering， Anhui University of Science and Technology， Huainan 232000，China；2. Institute of Environmental Friendly Materials and Occupational Health （Wuhu），Anhui University of Science and Technology， Wuhu 241003， China）

Abstract： Path planning is one of the important contents of research on coal mine rescue robots. A path planning method for coal mine rescue robots based on improved A* algorithm is proposed to address the unstructured features of post disaster coal mine environments and the problems of non-shortest path length， multiple turns， and poor smoothness of path planned by traditional A* algorithm. The method constructs raster maps by binarizing map information in real environments， determines the relative position between the current point and the target point， and uses the improved A* algorithm for path planning. Then a path from the current point to the target point is obtained. Douglas-Pucker （D-P） algorithm is used to extract key nodes on the path， and cubic spline interpolation function is used to fit the key nodes， thereby completing the smooth processing of the path. The improved A* algorithm expands the traditional A* algorithm's 8 neighborhood search to a purposeful 13 neighborhood search. When conducting path search， the position relationship between the current point and thetarget point is first determined， thereby reducing path nodes and length， and improving path smoothness. The Matlab simulation results show that compared with the 8 neighborhood A* algorithm， 24 neighborhood A* algorithm， and 48 neighborhood A* algorithm， the improved A* algorithm has certain optimizations in pathlength， number of turns and smoothness. It is more suitable for path planning of coal mine rescue robots. Compared with the Fuzzy algorithm， the improved A* algorithm achieve shorter path planning time， shorter planned pathlength， and fewer turns.

Key words： coal mine rescue robot; path planning; A* algorithm; neighborhood extension; Douglas-Pucker algorithm; smooth path; cubic spline interpolation function

0 引言

煤矿救援机器人可在危险区域对环境进行探测，对矿工实施救援，对于提高煤矿灾后救援效率、减少二次伤亡具有重要意義[1]。路径规划是煤矿救援机器人研究的重要内容之一。从起点到终点规划一条连续且与路径上的障碍物不发生任何碰撞的路径，可使煤矿救援机器人在顶板坍塌及爆炸后散落物遍布的非结构化环境中快速到达事故现场并实施救援。

传统的全局路径规划算法有 Dijkstra 算法[2]、快速扩展随机树算法[3]、A*算法[4]等，局部路径规划算法有动态窗口法[5]、人工势场法[6]等。其中具有启发式思想的 A*算法被认为是进行静态全局路径规划最有效的方法，但其规划出的路径存在拐角过大、扩展节点较多等缺点[7]。文献[8]将传统 A*算法的 8邻域搜索扩展为24邻域搜索，改进后的 A*算法增加了搜索方向，规划的路径长度有所减小，但以牺牲时间为代价，扩展后的24邻域搜索方向不具有目的性。文献[9]通过基于象限判别的改进 A*算法寻找从起始点到目标点的最佳路径。文献[10]将传统的 A*算法改进为7×7的 A*算法，在扩展搜索邻域的基础上去除与搜索邻域同方向的子节点，并将改进 A*算法与动态窗口法相结合，实现了机器人实时避障。文献[11]优化了 A*算法的启发搜索函数，在保留关键点的基础上剔除所规划路径上的冗余节点和多余转折点。文献[12]采用 Douglas-Peucker（D?P）算法提取传统 A*算法所规划路径的关键节点，通过三次样条插值函数对规划的路径进行平滑，改善了传统 A*算法所规划的路径节点多、拐弯次数多的问题，但因为传统8邻域搜索单次搜索的邻域范围小，所以需要搜索的次数多，计算量也会相应增大，不适用于规模较大的地图。

针对上述问题，本文提出一种基于改进 A*算法搜索邻域的煤矿救援机器人路径规划方法，在传统 A*算法的基础上，从原来的8邻域搜索扩展为有目的性的13邻域搜索，并采用 D?P 算法提取路径中的关键节点，再用三次样条插值函数对基于关键节点的整段路径进行拟合处理，得到一条相对平滑的路径。

1 煤矿救援机器人路径规划方法

1.1 改进 A*算法

A*算法在 Dijkstra 算法的基础上引入评估函数，是一种启发式搜索算法[13-14]，评估函数的引入提高了搜索效率。 A*算法的表达式为

式中：F（n）为节点n的代价评估函数；G（n）为煤矿救援机器人从起始点行驶到当前点n所花费的实际代价； H（n）为煤矿救援机器人从当前点n行驶到目标点将要花费的预估代价。

H（n）可采用欧几里得距离、曼哈顿距离、对角线距离表示，本文采用欧几里得距离表示，其公式为

式中：（xg，yg）为目标点 g 的坐标；（xn，yn）为当前点 n的坐标。

传统的 A*算法一般采用8邻域搜索方式搜索下一个要扩展的节点，其邻域搜索的角度被限定为45o 的整数倍，传统 A*算法搜索方式如图1所示。

为解决传统 A*算法8邻域搜索存在的问题，本文将传统 A*算法的8邻域搜索扩展为有目的性的13邻域搜索，在进行路径搜索时，先对当前点和目标点的位置关系进行判断。首先定义一个向量P：

当P1≥0且P2≤0时，目标点位于当前点的第一象限；当P1≤0且P2≤0时，目标点位于当前点的第二象限；P1≤0且P2≥0时，目标点位于当前点的第三象限；P1≥0且P2≥0时，目标点位于当前点的第四象限。以目标点位于当前点的第四象限为例，位置关系如图2所示，xoy为节点所在坐标系。

当目标点位于当前点的不同象限时，改进 A*算法的搜索邻域如图3所示。改进 A*算法可以有目的性地向目标点进行扩展搜索，相比于传统的8邻域 A*算法，虽然搜索时间略有增加，但是路径节点减少，路径长度减小，平滑度有所提升，更适用于煤矿救援机器人在复杂非结构化环境中的救援工作。

1.2 路径平滑算法

D?P 算法是一种经典的直线简化算法。该算法通过设置合适的距离阈值?，有效地识别曲线特征点[15-16]。为了优化路径，本文采用 D?P 算法提取改进 A*算法搜索路径中的关键节点，再采用三次样条插值函数对 D?P算法提取的关键节点进行拟合，从而完成对路径的平滑处理。

D?P 算法提取关键节点的步骤如下[17]：

1）将路径上的起点A 和终点B 连接，形成一条线段 AB。

2）找出路径上与线段 AB距离最远的点并记为 C ，计算点 C到线段 AB 的距离，记为 L。

3）将预先设置的距离阈值ε与距离 L 进行比较，如果ε大于 L，则将线段 AB 作为该段路径的近似。

4）如果ε小于 L，则连接点A 和点 C，连接点 B 和点 C，点 C 即为要提取的关键节点，分别对线段 AC 和 BC 重复步骤1）?步骤3）的处理。

5）处理完每段路径后，依次连接所提取的关键节点，形成一条拐弯次数较少的新路径。

样条插值常用于工业设计，三次样条插值是其中应用较广泛的一种，其在保留分段低次插值多项式优点的基础上，提高了插值函数的光滑性。三次样条插值函数的定义如下：给定区间[a，b]上的 m 个节点 X（X1，X2，···， Xm）和这些点上的函数值 f（Xi ）= Yi （i =1，2，··· ， m），若 S（X）满足以下条件，则称 S（X）为函数f（X）关于节点X1，X2，…，Xm的三次样条插值函数[18-19]：① S（Xi ）= Yi；②在每个小区间[Xi，Xi+1]上都是三次多项式；③ S （X），S ′（X），S ′′（X）在[a，b]内连续。

若S ′′（X）在[a，b]內连续，则还需满足以下条件：

1.3 路径规划方法流程

在进行路径规划前，首先，对真实环境中的地图信息进行二值化处理[20-21]，构建栅格地图；其次，判断当前点与目标点的相对位置，利用改进 A*算法进行路径规划，得到一条从当前点到目标点的路径；再次，利用 D?P算法提取路径上的关键节点；最后，利用三次样条插值函数对路径进行平滑处理。具体流程如图4所示。

2 仿真结果与分析

当矿井发生瓦斯爆炸、粉尘爆炸等灾害事故后，煤矿环境会被爆炸所产生的冲击波破坏，导致巷道内散落很多大小不一、形状各异的煤块和施工器材。为了验证改进 A*算法及路径平滑算法在该环境下的有效性，利用Matlab R2022a进行了2组仿真实验。在简单环境下将改进 A*算法与8邻域 A*算法、24邻域 A*算法、48邻域 A*算法进行对比，用不同形状的黑色图形代表灾后煤矿环境中形状各异的障碍物。由于矿井内存在一些不规则的复杂障碍物，再次在复杂环境下进行仿真实验。环境地图大小均为500 m×500 m。

简单环境下起点为（70 m，40 m），终点为（450 m，480 m），改进 A*算法和不同邻域 A*算法所生成的路径及平滑后的路径如图5所示，各算法性能对比见表1。由表1可知，在起点和终点相同的情况下，改进 A*算法较传统的8邻域 A*算法搜索所用的时间略长，但路径长度减小了2.45%，拐弯次数减少了41.18%，平滑后路径长度减小了1.27%，路径节点数减少了22.92%。24邻域 A*算法搜索的路径与本文算法搜索的路径长度、路径节点数相同，路径平滑前后的拐弯次数也相同，但搜索时间有所增加。与本文算法相比，48邻域 A*算法搜索所用的时间略长，但搜索的路径长度略减小，拐弯次数略减少，平滑后的路径比本文算法长。

复杂环境下起点为（30 m，40 m），终点为（450 m，480 m），改进 A*算法和不同邻域 A*算法所生成的路径及平滑后的路径如图6所示，各算法性能对比见表2。由表2可知，在起点和终点相同的情况下，改进 A*算法较传统的8邻域 A*算法搜索所用的时间略长，但路径长度减小了4.28%，拐弯次数减少了26.67%，平滑后路径长度减小了3.82%，路径节点数减少了28.1%。24邻域 A*算法搜索的路径与本文算法搜索的路径长度、路径节点数相同，路径平滑前后的拐弯次数也相同，但搜索时间有所增加。与本文算法相比，48邻域 A*算法搜索所用的时间略长，拐弯次数相同，但路径长度略有减小，而平滑后的路径长度、拐弯次数与本文算法一致。综合表1、表2可知，本文算法更适用于煤矿救援机器人路径规划。

在同一环境下将本文算法和 Fuzzy 算法进行对比，设煤矿救援机器人的起始点为（100 m，40 m），目标点为（400 m，460 m），对比结果如图7所示，算法性能测试对比见表3。由图7可知，本文算法所规划的路径拐弯次数只有3，路径较为平滑。 Fuzzy 算法所规划的路径虽然也相对平滑，但拐弯角度过大，不利于煤矿救援机器人实施灾后救援工作。由表3可知，本文算法所规划的路径长度明显小于 Fuzzy 算法所规划的路径，所用时间更短，路径拐弯次数更少。减少拐弯次数可降低煤矿救援机器人工作时进行加减速以调整运动方向的频率，而路径长度的减小则可节省救援所用时间，提高救援效率。

3 结论

1）改进 A*算法在传统 A*算法的基础上，将可搜索邻域个数从8个增加到13个，减少了无用搜索，能够有效解决传统 A*算法规划的路径节点数多、长度非最短的问题。

2）采用 D?P 算法提取改进 A*算法搜索路径中的关键节点，再采用三次样条插值函数对 D?P 算法提取的关键节点进行拟合，从而完成对路径的平滑处理，解决了传统 A*算法规划的路径平滑度较差、拐弯次数较多的问题。

3）Matlab仿真结果表明：与8邻域 A*算法、24邻域 A*算法、48邻域 A*算法相比，改进 A*算法在路径长度、拐弯次数、平滑度等方面有一定优化，更适用于煤矿救援机器人路径规划；与 Fuzzy 算法相比，改进 A*算法路径规划所用时间更短，规划的路径长度更短，拐弯次数更少。

参考文献（References）：

[1] 朱华，由韶泽.新型煤矿救援机器人研发与试验[J].煤炭学报，2020，45（6）：2170-2181.

ZHU Hua，YOU Shaoze. Research and experiment of a new type of coal mine rescue robot[J]. Journal of China Coal Society，2020，45（6）：2170-2181.

[2] 徐兴，俞旭阳，赵芸，等.基于改进遗传算法的移动机器人全局路径规划[J].计算机集成制造系统，2022，28（6）：1659-1672.

XU Xing，YU Xuyang，ZHAO Yun，et al. Global path planning of mobile robot based on improved genetic algorithm[J]. Computer  Integrated Manufacturing Systems，2022，28（6）：1659-1672.

[3] 王梓强，胡晓光，李晓筱，等.移动机器人全局路径规划算法综述[J].计算机科学，2021，48（10）：19-29.

WANG Ziqiang，HU Xiaoguang，LI Xiaoxiao，et al. Overview of global path planning algorithms for mobile robots[J]. Computer Science，2021，48（10）：19-29.

[4] SHANG E，DAI Bin，NIE Yiming，et al. An improved A-star based path planning algorithm for autonomous land vehicles[J]. International Journal of Advanced Robotic Systems，2020，17（5）：1-13.

[5] 张瑜，宋荆洲，张琪祁.基于改进动态窗口法的户外清扫机器人局部路径规划[J].机器人，2020，42（5）：617-625.

ZHANG Yu，SONG Jingzhou，ZHANG Qiqi. Local path planning of outdoor cleaning robot based on an improved DWA[J]. Robot，2020，42（5）：617-625.

[6] MIN Huasong，LIN Yunhan，WANG Sijing，et al. Path planning of mobile robot by mixing experience with modified artificial potential field method[J]. Advances in Mechanical Engineering，2015，7（12）：1-17.

[7] 郭晓静，杨卓橙.基于邻域拓展的静态路径规划A*算法研究[J].计算机工程与应用，2022，58（8）：168-174.

GUO Xiaojing， YANG Zhuocheng. Improved A* algorithm based on neighbor extension in static environment[J].  Computer  Engineering  and Applications，2022，58（8）：168-174.

[8] 崔宝侠，王淼弛，段勇.基于可搜索24邻域的A*算法路径规划[J].沈阳工业大学学报，2018，40（2）：180-184.

CUI Baoxia，WANG Miaochi， DUAN Yong. Path planning for A* algorithm based on searching 24 neighborhoods[J]. Journal of Shenyang University of Technology，2018，40（2）：180-184.

[9] 劉小佳，狄梦然，梁利东，等.基于象限判别下的改进 A*算法路径规划[J].常州工学院学报，2020，33（2）：26-30，35.

LIU Xiaojia，DI Mengran，LIANG Lidong，et al. On the improved path planning of A* algorithm based on quadrant discrimination[J]. Journal of Changzhou Institute of Technology，2020，33（2）：26-30，35.

[10] 槐创锋，郭龙，贾雪艳，等.改进A*算法与动态窗口法的机器人动态路径规划[J].计算机工程与应用，2021，57（8）：244-248.

HUAI Chuangfeng，GUO Long，JIA Xueyan，et al. Improved A* algorithm and dynamic window method for robot dynamic path planning[J]. Computer Engineering and Applications，2021，57（8）：244-248.

[11] 程传奇，郝向阳，李建胜，等.融合改进A*算法和动态窗口法的全局动态路径规划[J].西安交通大学学报，2017，51（11）：137-143.

CHENG Chuanqi，HAO Xiangyang，LI Jiansheng，et al. Global dynamic path planning based on fusion of improved A* algorithm and dynamic window approach [J]. Journal of Xi'an Jiaotong University，2017，51（11）：137-143.

[12] 陶德俊，姜媛媛，刘延彬，等.煤矿救援机器人路径平滑算法研究[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.

[13] 李枭扬，周德云，冯琦.基于分级规划策略的A*算法多航迹规划[J].系统工程与电子技术，2015，37（2）：318-322.

LI Xiaoyang，ZHOU Deyun，FENG Qi. Multiple routes planning for A* algorithm based on hierarchical planning[J]. Systems Engineering and Electronics，2015，37（2）：318-322.

[14] FU Bing， CHEN Lin， ZHOU Yuntao， et al. An improved A* algorithm for the industrial robot path planning with high success rate and short length[J].Robotics & Autonomous Systems，2018，106：26-37.

[15] LI Chengming， GUO Peipei， WU Pengda， et al.Extraction of terrain feature lines from elevation contours using a directed adjacent relation tree [J]. International Journal of Geo-Information，2018，7（5）：163-177.

[16] ZHAO Liangbin， SHI Guoyou. A method  for simplifying ship trajectory based on improved Douglas- Peucker algorithm[J]. Ocean Engineering，2018，166（15）：37-46.

[17] 杨敏，陈媛媛，金澄，等.保持移动速度特征的轨迹线化简方法[J].测绘学报，2017，46（12）：2016-2023.

YANG Min，CHEN Yuanyuan，JIN Cheng，et al. Amethod of speed-preserving trajectory simplification[J]. Acta Geodaetica et Cartographica Sinica，2017，46（12）：2016-2023.

[18] 胡崢楠，佘锋.一种基于样条插值的局部路径规划模型与实现[J].微型电脑应用，2020，36（11）：106-110.

HU Zhengnan，SHE Feng. A local path planning model and implementation based on spline interpolation[J]. Microcomputer Applications，2020，36（11）：106-110.

[19] 高晓，杨志强，库新勃，等.基于三次样条插值实现无人机高动态运动轨迹插值[J].全球定位系统，2020，45（1）：37-42.

GAO Xiao，YANG Zhiqiang，KU Xinbo，et al.3D- coordinate interpolation for UAV high dynamic positioning based on cubic spline interpolation[J]. GNSS World of China，2020，45（1）：37-42.

[20] 张金泽.水面无人艇路径规划及避障策略的研究[D].大连：大连海事大学，2022.

ZHANG Jinze. Research on path planning and obstacle avoidance strategy of unmanned surface vehicle[D]. Dalian：Dalian Maritime University，2022.

[21] 刘胜，张豪，晏齐忠，等.基于ACO?SA算法的变电站巡检机器人路径规划[J].南方电网技术，2022，16（9）：75-82.

LIU Sheng，ZHANG Hao，YAN Qizhong，et al. Path planning of inspection robot in substation based on ACO-SA algorithm[J]. Southern  Power System Technology，2022，16（9）：75-82.