0 引言
为了减少煤矿灾难造成的人员伤亡，进一步提升救援效率，煤矿救援机器人已成为当今的重点研究课题。矿井灾后环境复杂，救援人员进入困难，这时煤矿救援机器人就可以代替救援人员进入矿井实施环境探测、搜救任务[1]。为了使煤矿救援机器人可以顺利地到达灾难现场并实施救援，寻找出一条可行、安全和最优的移动路径非常重要[2]。
煤矿救援机器人路径规划是指机器人在已知起始点和目标点的情况下，按照一定的性能指标，找到一条连续的、无碰撞的可行路径，快速到达灾难现场进行环境探测并实施救援[3]。传统的全局路径规划算法有栅格法[4-5]、可视图法[6-7]和A*算法[8]等，局部路径规划算法有D*算法[9-10]、遗传算法[11-12]等，都能较好地解决路径规划问题。
A*算法作为一种较为成功的全局路径规划算法，已成功运用于机器人的路径规划，并取得了良好效果[13]，但由于A*算法产生的路径存在转折次数多、路径不够平滑等问题，研究者们提出了若干改进的平滑算法。单伟等[14]提出了一种基于改进A*算法的平滑路径方法，首先将全局路径简化为若干关键点衔接，然后引入极多项式曲线生成平滑路径，该路径符合机器人运动学特性，有利于实现机器人的路径跟踪。郭江等[15]提出了一种将Bezier 曲线与 A*算法融合的路径平滑方法，该方法大大改善了机器人的运动性能，有效提升了工作效率，但是用Bezier 曲线对路径进行平滑处理时，如果改变一个路径节点的位置，对整条平滑路径都有影响。赵金龙等[16]提出了将三次样条函数与A*算法结合的机械臂平滑算法，使避障路径的位移、速度和加速度的平滑性得到了显著提高，但在路径冗余节点较多的情况下，路径平滑效果稍差。
针对A*算法在路径平滑方面存在的问题，本文对A*算法进行了改进，提出了一种基于改进A*算法的煤矿救援机器人路径平滑算法。该算法首先利用D-P(Douglas-Peucker)算法消除冗余节点，再用三次样条函数对剩余的关键节点进行平滑处理，能有效解决基于A*算法规划出来的机器人路径转折次数多、路径不够平滑等问题，极大提升了路径质量，规划出来的路径更平滑，更利于煤矿救援机器人在搜救过程中的行进，同时也可以有效提升救援效率。
1 A*算法
A*算法被认为是Dijkstra算法的扩展[17]。由于借助启发函数的引导，A*算法在路径查找方面拥有较好的性能，常用来寻找地图上两点之间的最短路径。
A*算法的核心是代价评估函数f(n)的设计，在机器人进行避障搜索路径的过程中，先对每一个将要访问的节点使用代价评估函数f(n)进行评估计算，然后选择具有最小代价评估值的节点作为下一步要到达的路径节点，再不断迭代更新搜索，直至寻找到最优避障路径，到达目标节点[18]。A*算法的搜索方向是根据代价评估函数f(n)来确定的，在搜索过程中，不用遍历地图中的所有节点。A*算法在避障路径搜索过程中，需要使用OPEN列表和CLOSE列表来存放节点。OPEN列表中存放未扩展的节点，也就是用于下一步搜索的节点；CLOSE列表中存放符合机器人避障要求的路径节点。A*算法的表达式为
f(n)=g(n)+h(n)
(1)
式中：f(n)为节点n的代价评估函数；g(n)为机器人从起始节点S出发，到达节点n所花费的实际代价；h(n)为机器人从当前节点n到目标节点T将要花费的预算代价。
本文选择的预算代价h(n)为机器人从当前节点到目标节点的距离，即
h(n)=|xt-xn|+|yt-yn|
(2)
式中：xt，yt分别为目标节点T的横坐标、纵坐标；xn，yn分别为当前节点n的横坐标、横坐标。
A*算法流程如图1所示。
图1 A*算法流程
Fig.1 Flow of A*algorithm
2 改进的A*路径平滑算法
2.1 环境建模
由于A*算法仅适用于离散空间，所以,必须创建离散空间的地图。在Matlab平台中，将导入的环境地图进行二值化处理，形成离散像素阵列的地图。离散像素地图中的每个像素都被看成是一个栅格，所以转换后形成的是一个栅格地图。
机器人在仿真实验时采用的是最常用的移动方式，仅允许机器人直线移动(上和下，左和右)。连接矩阵如图2所示，在连接矩阵中，机器人的假定位置标记为2。所有可能的移动用1表示，所有不可能的移动则用0表示。
图2 连接矩阵
Fig.2 Connection Matrix
2.2 关键节点的提取
D-P算法是公认的线状要素简化的经典算法，该算法效率高且不会产生多余的点，并且可以保留路径的主要特征[19]。本文采用D-P算法对A*算法产生的全段路径进行处理，剔除路径中的冗余节点，提取出若干路径节点作为关键节点，然后基于关键节点对路径进行平滑处理。
D-P算法提取关键节点的步骤如图3所示[20]。
图3 D-P算法提取关键节点的步骤
Fig.3 Steps of extraction key nodes by D-P algorithm
(1)连接路径首尾两点A，B,形成一条线段AB。
(2)找出路径上距离线段AB最远的点C，计算点C与AB的距离d。
(3)如果距离d小于预先给定的阈值距离ε，则线段AB作为该段路径的近似。
(4)如果距离d大于预先给定的距离阈值ε，则用C将线段AB分为AC和BC，C即为提取出的关键节点，并分别对线段AC和BC重复步骤(1)—步骤(3)的处理。
(5)当所有路径都处理完毕时，依次连接各个关键节点形成的折线，即可以作为路径的近似。
2.3 路径平滑处理
为了得到平滑的路径曲线，目前常采用的方法有样条函数[21]和Bezier函数[22]。由于在Bezier函数中移动一个控制顶点将会影响整个路径曲线的形状，所以，本文选用三次样条函数来对D-P算法提取的关键节点进行拟合，从而完成对全段路径的平滑处理。
若函数S(x)∈C2[a,b]，给定节点a=x0<x1<…<xn=b，且在每个小区间[xj,xj+1]上都是三次多项式，则称S(x)是节点x0,x1,…xn上的三次样条函数[23]。若在节点xj上给定函数值yj=f(xj)(j=0,1,…,n),并且满足式(3)，则称S(x)为三次样条插值函数[24]。
S(xj)=yj
(3)
由于S(x)在[a,b]区间内具有连续的二阶导数，在节点xj(j=1,2,…,n-1)处应满足以下3个连续性条件：
S(xj-0)=S(xj+0)
(4)
S′(xj-0)=S′(xj+0)
(5)
S″(xj-0)=S″(xj+0)
(6)
考虑到路径首尾两点x0和xn与各段路径连接的连续性和平滑性，在首尾分段点处各加一个条件，使分段首尾两端点满足三次样条第一类边界条件f′(xj)=0，则加入的条件为
S′(x0)=f′(x0)=0
(7)
S′(xn)=f′(xn)=0
(8)
联合式(3)—式(8)求出三次样条函数S(x)。
路径的平滑处理过程：首先对基于D-P算法得到的关键节点进行三次样条函数的构造，然后根据求出的三次样条函数S(x)对整段路径进行拟合处理，进而得到一条平滑的路径。
2.4 改进的A*路径平滑算法实施步骤
改进的A*路径平滑算法流程如图4所示。
图4 改进的A*路径平滑算法流程
Fig.4 Flow of improved A*path smoothing algorithm
(1)在Matlab平台中，对给定环境地图进行二值化处理，将其转换为栅格地图，确定机器人在环境模型中的起始节点与目标节点，判断是否与障碍物重合。
(2)初始化算法参数，进行仿真实验。使用A*算法找出起始节点与目标节点之间的可行路径。
(3)利用D-P算法对A*算法产生的路径节点进行多次实验，找出最优距离阈值ε，提取出若干关键节点，确保优化路径不与障碍物发生碰撞。
(4)对提取出的关键节点进行三次样条函数构造，根据求出的三次样条函数对路径进行平滑处理，得到一条平滑路径。
3 仿真实验与结果分析
为了方便对煤矿救援机器人路径平滑的研究，在仿真实验中，将井下环境简化为二维空间，假设机器人的运行速度恒定，机器人在仿真过程中被视为一个质点，考虑到实际机器人的尺寸大小，对周围障碍物进行了适当的膨胀处理，防止机器人在实际运行中与障碍物发生碰撞。
由于D-P算法的距离阈值ε会影响关键节点的提取，关键节点过多将导致拟合的路径不够平滑，而关键节点过少则会导致平滑路径与障碍物发生碰撞，所以，实验中D-P算法中的距离阈值ε为经过多次实验后选取的最优值。
首先，对改进的A*路径平滑算法分别在不同的环境地图中进行仿真实验，地图的大小为500 m×500 m。由于煤矿井下环境复杂，为了测试地图的复杂程度对本文算法的影响，选用了3种不同的环境地图,如图5所示。图5(a)为一些规则障碍物构成的地图，虽然存在狭窄通道，但是相对于井下来说，找出路径还是要简单得多。图5(b)和图5(c)由一些不规则的障碍物随意组合而成，更加符合煤矿井下的情况，其中图5(c)在给定的起始节点与目标节点之间只有一条可行路径。各地图的实验参数见表1。
(a)规则障碍物
(b)不规则障碍物
(c)复杂障碍物
图5 改进的A*路径平滑算法在不同环境地图中的实验结果
Fig.5 Experimental results of improved A*path smoothing algorithm in different environmental maps
表1 不同环境地图中的实验参数
Table 1 Experimental parameters in different environmental maps
从图5可看出，随着环境地图复杂程度的增加，A*算法产生的路径(原始路径)曲曲折折，冗余节点较多，路径不够平滑，而经过改进的A*算法优化的路径(平滑路径)，由于提取了关键节点以及应用三次样条函数,路径质量有了很大程度的提升，路径转折次数明显减少，产生的平滑路径更有助于救援机器人在井下行进。
表2为改进的A*路径平滑算法与A*算法在不同环境地图中实验数据的对比情况。改进的A*路径平滑算法相比A*算法，在运行时间增加了4%～7.2%的情况下，提取的关键节点数量约为原本路径节点的5.5%～8.42%，路径长度明显减少了2.94%～4.72%，路径转折次数减少了47.61%～75%。实验数据表明，改进的A*路径平滑算法对A*算法的路径有着很好的优化效果，在简单与复杂环境中都能有效地改善路径质量，产生平滑的路径。
遗传(Genetic Algorithm,GA)平滑算法是近些年兴起的一个新的研究方向，该算法与本文所提出的改进的A*路径平滑算法的方向类似，也是先利用遗传算法提取出若干路径节点，再用B样条曲线对路径进行平滑处理，并取得了较好的平滑效果。所以，本文将改进的A*路径平滑算法与 GA平滑算法[25]进行了对比实验，以测试改进的A*路径平滑算法的优越性。
地图大小为500 m×500 m，机器人的起始节点为(10 m,10 m)，目标节点为(490 m,490 m)，改进的A*路径平滑算法的距离阈值ε为20 m。2种算法的对比实验结果如图6所示，性能测试结果见表3。从图6和表3可看出，改进的A*路径平滑算法相比较于GA平滑算法，路径规划花费的时间明显更短，且产生的路径长度也更短。虽然路径转折次数相同，但从图6中的平滑路径对比来看，用改进的A*路径平滑算法产生的路径更适用于机器人的运动。
表2 不同环境地图下A*算法与改进的A*路径平滑算法的性能测试对比
Table 2 Comparison of performance test between A*algorithm and improved A*path smoothing algorithm in different environmental maps
(a)改进的A*路径平滑算法
(b)GA平滑算法
图6 改进的A*路径平滑算法与GA平滑算法的路径对比
Fig.6 Path comparison between improved A*path smoothing algorithm and GA smoothing algorithm
表3 改进的A*路径平滑算法与GA平滑算法的性能测试对比
Table 3 Comparison of performance test between improved A*path smoothing algorithm and GA smoothing algorithm
4 结论
(1)为了使煤矿救援机器人利用A*算法规划出来的路径为一条相对平滑的路径，提出了一种基于改进A*算法的路径平滑算法。对A*算法产生的路径节点，首先利用D-P算法提取出若干关键节点，消除了路径上的大量冗余节点，路径转折次数明显减少；然后利用三次样条函数对关键节点进行拟合处理;最后产生一条平滑的路径。路径长度明显缩短，路径平滑性显著提升。
(2)在不同实验环境中的仿真实验结果表明，该算法通用性很强，虽然规划时间较A*算法略有增加，但规划出来的路径具有转折次数少、路径长度短等优点，且路径质量明显优于遗传平滑算法。
参考文献（References）：
[1] 柳玉龙.煤矿搜救机器人的研究现状及关键技术分析[J].矿山机械,2013,41(3):7-12.
LIU Yulong.Analysis on current research status and key technologies of mine search and rescue robots[J].Mining &Processing Equipment,2013,41(3):7-12.
[2] 孙国栋，李雨潭，朱华.一种新型煤矿救援机器人履带行走机构设计[J].工矿自动化,2015,41(6):21-25.
SUN Guodong,LI Yutan,ZHU Hua.Design of a new type of crawler travelling mechanism of coal mine rescue robot[J].Industry and Mine Automation,2015,41(6):21-25.
[3] TSARDOULIAS E G,ILIAKOPOULOU A,KARGAKOS A,et al.A review of global path planning methods for occupancy grid maps regardless of obstacle density[J].Journal of Intelligent &Robotic Systems,2016,84(1-4):829-858.
[4] AMMAR A,BENNACEUR H,CH
ARI I,et al.Relaxed Dijkstra and A*with linear complexity for robot path planning problems in large-scale grid environments[J].Soft Computing,2016,20(10):4149-4171.
[5] CHONG Y U,QIU Q W.Hierarchical robot path planning algorithm based on grid map[J].Journal of University of Chinese Academy of Sciences,2013,30(4):528-235.
[6] 黎萍,朱军燕,彭芳,等.基于可视图与A*算法的路径规划[J].计算机工程,2014,40(3):193-195.
LI Ping,ZHU Junyan,PENG Fang,et al.Path planning based on visibility graph and A*algorithm[J].Computer Engineering,2014,40(3):193-195.
[7] KALUDER H,BREZAK
I.A visibility graph based method for path planning in dynamic environments[C]//2011 Proceedings of the 34th International Convention MIPRO,2011:717-721.
[8] LI X M,WANG J P,NING X.A*algorithm based robot path planning method[J].Applied Mechanics &Materials,2011,63-64:686-689.
[9] FERGUSON D,STENTZ A.Using interpolation to improve path planning:the field D*algorithm[J].Journal of Field Robotics,2010,23(2):79-101.
[10] SILVEIRA L,MAFFEI R Q,BOTELHO S S C,et al.Space D*:a path-planning algorithm for multiple robots in unknown environments[J].Journal of the Brazilian Computer Society,2012,18(4):363-373.
[11] WANG X,SHI Y,DING D,et al.Double global optimum genetic algorithm-particle swarm optimization-based welding robot path planning[J].Engineering Optimization,2016,48(2):299-316.
[12] YAKOUBI M A,LASKRI M T.The path planning of cleaner robot for coverage region using genetic algorithms[J].Journal of Innovation in Digital Eco-systems,2016,3(1):37-43.
[13] CHENG C,HAO X,LI J S,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.
[14] 单伟,孟正大.基于改进A*算法的平滑路径设计[J].东南大学学报(自然科学版),2010,40(增刊1):155-161.
SHAN Wei,MENG Zhengda.Smooth path design for mobile service robots based improve A*algorithm[J].Journal of Southeast University (Natural Science Edtion),2010,40(S1):155-161.
[15] 郭江,肖宇峰,刘欣雨,等.Bezier曲线与A*算法融合的移动机器人路径规划[J].微型机与应用,2017,36(2):52-55.
GUO Jiang,XIAO Yufeng,LIU Xinyu,et al.Mobile robot path planning based on fusion of A*algorithm and Bezier curve[J].Microcomputer and Application,2017,36(2):52-55.
[16] 赵金龙,晁永生,袁逸萍.基于A*算法和三次样条的工业机械臂路径平滑性研究[J].机械设计与研究,2019,35(1):61-64.
ZHAO Jinlong,CHAO Yongsheng,YUAN Yiping.The research of industrial manipulator path smoothness based on A*algorithm and cubic spline function[J].Machine Design and Research,2019,35(1):61-64.
[17] LI X Y,ZHOU D Y,FENG Q.Multiple routes planning for A*algorithm based on hierarchical planning[J].Systems Engineering &Electronics,2015,37(2):318- 322.
[18] 梁昭阳,蓝茂俊,陈正铭.A*算法在Shortest-Path方面的优化研究[J].计算机系统应用,2018,27(7):257-261.
LIANG Zhaoyang,LAN Maojun,CHEN Zhengming.Optimization of A*algorithm in finding shortest-path[J].Computer Systems &Applications,2018,27(7):255-259.
[19] 赵真,沈敬伟,谭诗腾.基于Douglas-Peucker的面状矢量数据压缩算法[J].测绘,2017,40(3):99-102.
ZHAO Zhen,SHEN Jingwei,TAN Shiteng.Face vector data compression algorithm based on Douglas-Peucker[J].Surveying and Mapping,2017,40(3):99-102.
[20] YANG M,CHEN Y,JIN C,et al.A method of speed-preserving trajectory simplification[J].Acta Geodaetica Et Cartographica Sinica,2017,46(12):2016-2023.
[21] 吕太之,周武,赵春霞.采用粒子群优化和B样条曲线的改进可视图路径规划算法[J].华侨大学学报(自然科学版),2018,39(1):103-108.
LYU Taizhi,ZHOU Wu,ZHAO Chunxia.Improved visibility graph method using particle swarm optimization and B-spline curve for path planning[J].Journal of Huaqiao University (Natural Science),2018,39(1):103-108.
[22] 卜新苹,苏虎,邹伟,等.基于非均匀环境建模与三阶Bezier曲线的平滑路径规划[J].自动化学报,2017,43(5):710-724.
BU Xinping,SU Hu,ZOU Wei,et al.Smooth path planning based on non-uniformly modeling and cubic Bezier curves[J].Acta Automatica Sinica,2017,43(5):710-724.
[23] 强宁,高洁,康凤举.基于PSO和三次样条插值的多机器人全局路径规划[J].系统仿真学报,2017,29(7):1397-1404.
QIANG Ning,GAO Jie,KANG Fengju.Multi-robots global path planning based on PSO algorithm and cubic spline[J].Journal of System Simulation,2017,29(7):1397-1404.
[24] 聂磊,陶明,翟中生.基于三次样条曲线拟合的激光拉曼光谱基线校正研究[J].湖北工业大学学报,2017,32(1):63-67.
NIE Lei,TAO Ming,ZHAI Zhongsheng.Correction of Raman spectroscopy baseline based on cubic spline curve fitting[J].Journal of Hubei University of Technology,2017,32(1):63-67.
[25] 王宪,盛巍,宋书林,等.基于遗传算法和B样条曲线的平滑避障路径规划[J].计算机系统应用,2012,21(2):65-71.
WANG Xian,SHENG Wei,SONG Shulin,et al.Smoothing obstacle avoidance path planning based on genetic algorithms and B-spline curve[J].Computer Systems &Applications,2012,21(2):65-71.