Research on path planning for coal mine rescue robots
-
摘要:
针对煤矿救援机器人采用双向A*算法存在搜索效率低、路径安全性和平滑性差，及动态窗口法（DWA）融合全局路径规划算法存在实时寻路效率低等问题，提出了一种基于分层平滑优化双向A*引导DWA（HSTA*−G−DWA）算法的煤矿救援机器人路径规划方法。首先，将碰撞约束函数的调整机制引入双向A*算法中，以提高路径规划的安全性。其次，在双向A*算法的代价函数中增加归正因子函数，防止正反向搜索路径不相交的情况，同时为预估代价函数增加动态加权因子函数以剔除路径搜索过程中无关扩展节点的搜索，从而提升路径搜索效率。然后，利用分层平滑优化策略消除路径中的冗余点和转折角，以减少节点数量和路径长度，并提高路径平滑性。最后，若煤矿救援机器人按照初始全局路径行驶过程中探测到未知障碍物，则利用全局路径引导DWA实现局部动态避障。仿真实验结果表明：① 静态环境下HSTA*−G−DWA算法路径搜索时间较传统A*算法和双向A*算法分别平均减少了81.82%和64.63%，路径的安全性和平滑性更好。② 未知环境下HSTA*−G−DWA算法可实时避开环境中出现的未知障碍物，路径长度较快速扩展随机树（RRT）算法、改进A*算法和现有融合算法分别减少了10.34%，14.28%和2.45%，路径搜索时间较现有融合算法平均减少了70.48%。实验室环境下实验结果表明：① 静态环境下，HSTA*−G−DWA算法路径搜索时间较传统A*算法平均减少了58.75%，机器人边缘距障碍物的最小距离平均增加了0.71 m。② 未知环境下，相比于传统A*算法，HSTA*−G−DWA算法可实时避开环境中出现的未知障碍物且路径的平滑性更好。
Abstract:This paper proposes a path planning method for coal mine rescue robots based on a Hierarchical Smooth Optimization Bidirectional A* guided dynamic window approach (HSTA*-G-DWA) algorithm. The method addresses several limitations of the traditional bidirectional A* algorithm, including low search efficiency, poor path safety, and inadequate smoothness, as well as low real-time pathfinding efficiency when integrating the DWA with global path planning algorithms. Firstly, an adjustment mechanism of collision constraint function is incorporated into the bidirectional A* algorithm to improve path safety. Next, a correction factor is incorporated into the cost function of the Bidirectional A* algorithm to ensure that the forward and backward search paths intersect, preventing them from diverging. Additionally, a dynamic weighting factor is added to the estimated cost function to eliminate irrelevant expanded nodes during pathfinding, thus improving search efficiency. A hierarchical smoothing optimization strategy is employed to remove redundant points and sharp turns, reducing both the number of waypoints and the overall path length, while enhancing smoothness. Finally, if the robot detects unknown obstacles while traveling along the global path, the DWA, guided by the global path, enables dynamic local obstacle avoidance. Simulation results show that: ① In static environments, the path search time using the HSTA*-G-DWA algorithm is reduced by 81.82% and 64.63% on average compared to the traditional A* and bidirectional A* algorithms, respectively, with improved path safety and smoothness. ② In unknown environments, the HSTA*-G-DWA algorithm can avoid unknown obstacles in real time, reducing the path length by 10.34%, 14.28%, and 2.45% compared to the rapidly-exploring random tree (RRT) algorithm, the improved A* algorithm, and existing integrated algorithms, respectively. The average path search time is reduced by 70.48% compared to existing integrated algorithms. In laboratory environments, experimental results show: ① In static environments, the HSTA*-G-DWA algorithm reduces the path search time by 58.75% on average compared to the traditional A* algorithm, and the minimum distance between the robot's edge and obstacles increases by 0.71 m on average. ② In unknown environments, compared to the traditional A* algorithm, the HSTA*-G-DWA algorithm can avoid unknown obstacles in real time, resulting in smoother paths.
-
0. 引言
矿难发生后，灾区环境复杂，仓促救援可能导致人员伤亡。 煤矿救援机器人具有在危险区域内感知周围环境的能力，从而显著提高煤矿救灾行动的效率，最大限度地减少人员伤亡[1]。
在矿井灾后复杂环境中如何为煤矿救援机器人快速规划出一条从指定起点至终点的无碰撞最优路径是众多学者考虑的重点，也是实现煤矿救援机器人自主完成各项任务的前置基础。根据煤矿机器人对环境信息的掌握程度可将煤矿救援机器人路径规划分为动态局部路径规划和静态全局路径规划[2]。局部路径规划算法适用于动态环境，注重煤矿救援机器人的动态避障能力。常用的局部路径规划算法主要包括动态窗口法（Dynamic Window Approach，DWA）[3-4]、人工势场法（Artificial Potential Field，APF）[5-6] 等。全局路径规划常用于静态环境，实时性较差。目前常用的全局路径规划算法主要包括A*算法[7]、蚁群算法（Ant Colony Optimization, ACO）[8]、遗传算法（Genetic Algorithm, GA）[9-10]、迪杰斯特拉算法（Dijkstra's Algorithm, Dijkstra）[11-12]、快速扩展随机树算法（Rapidly−exploring Random Tree, RRT）[13-14]、跳点搜索算法（Jump Point Search, JPS）[15-16]等。DWA考虑了速度和加速度的限制，是一种基于预测控制理论的方法，其优点在于能够实现未知环境中的实时动态避障，且规划出的路径较为光滑。A*算法引入了启发式信息作为决策辅助，在路径规划过程中不需要遍历整个地图，从而降低了计算复杂度并提升了路径搜索效率。因此，这2种算法在煤矿井下路径规划中得到广泛应用。但DWA在大尺度复杂环境中缺乏中间引导点而无法得到最优路径，甚至无法到达目标点；传统A*算法在煤矿救援机器人路径规划的应用中存在拐点多、路径不平滑等问题，不利于机器人的运行。针对以上算法在煤矿井下救援环境中的不足，许多学者对其进行了优化与改进。文献[17]通过提取若干路径节点作为关键点改进了A*算法，有效解决了路径冗余点和转折次数多的问题，但并未考虑路径搜索效率和安全性。文献[18]通过改进A*算法的领域搜索条件，并利用三次样条曲线拟合路径，在一定程度上提高了路径搜索效率和平滑性，但路径安全性差且无法进行实时避障。文献[19]通过引入曲率相似度评价因子改进了DWA，有效提升了路径的安全性，但仍存在大量冗余路段。文献[20-21]通过提取全局路径的关键点作为DWA的临时目标点，有效解决了全局路径规划算法实时性差、局部路径规划算法易陷入陷阱和局部最优的问题，但算法实时寻路效率低且存在冗余路段。
煤矿救援机器人在采用双向A*算法进行路径规划时，因缺乏方向引导导致正反向搜索不相交，从而降低搜索效率。另外，在煤矿未知环境中，现有全局路径规划算法融合DWA在完成全局路径规划后，需依赖全局路径的关键点引导DWA再次从起点到终点进行二次路径规划，导致实时寻路效率低，并引入冗余路段。针对上述问题，本文提出一种基于分层平滑优化双向A*（ Hierarchical Smoothing Optimization A*，HSTA*）引导DWA（HSTA*−G−DWA）的煤矿救援机器人路径规划方法。首先将动态加权因子、归正因子和碰撞约束函数的调整机制引入双向A*算法中，以提升路径搜索的方向性、路径搜索效率及路径的安全性；然后采用分层平滑优化方式去除冗余节点和冗余路段，并通过三次B样条曲线对路径进行进一步平滑处理，以生成安全光滑的初始全局路径；最后引导DWA优化生成局部修正的实时动态避障路径，从而实现安全、平滑、高效的煤矿井下路径规划。
1. 分层平滑优化双向A*算法
1.1 环境建模
在平面坐标轴XOY创建尺寸为42$ \times $42的环境地图，如图1所示， 其中黑色方格表示不可到达的障碍物区域，白色方格为煤矿救援机器人可自由行走的无障碍物区域。
1.2 动态加权和碰撞约束的双向A*路径规划方法
针对双向A*算法的代价函数会导致正反向搜索路径不相交及路径搜索效率和安全性低等问题，提出了一种动态加权因子和归正因子修正的代价函数与碰撞约束的双向A*路径规划方法，对正反向代价函数进行改进。以正向代价函数为例，改进公式为
$$ \min {\text{ }}f({{{x}}_{n1}}) = g({{{x}}_{n1}}) + {\kappa _1} h({{{x}}_{n1}}) + c({{{x}}_{n1}}) $$ (1) $$ {\mathrm{s.t.}}{\text{ }}{\| {{{{x}}_{n1}} - {{{o}}_J}} \|_1} \geqslant L \;\; J= 1,2, \cdots ,O $$ (2) $$ {\kappa _1} = 1 + \frac{{{{({{\| {{{{x}}_{n1}} - {{{x}}_{{\mathrm{first}}1}}} \|}_1})}^2}}}{{4(l + m)}} $$ (3) $$ h({{{x}}_{n1}}) = {\left\| {{{{x}}_{n1}} - {{{x}}_{n2}}} \right\|_2} $$ (4) $$ c({{{x}}_{n1}}) = {\| {{{{x}}_{n1}} - {{{x}}_{{\mathrm{goal}}1}}} \|_2} $$ (5) 式中：$ f\left( {{{{x}}_{n1}}} \right) $为当前节点总的移动代价，n为当前节点坐标的索引；$ {{{x}}_{n1}} $为正向搜索当前节点；$ g({{{x}}_{n1}}) $为起始节点到当前节点的实际移动代价；$ h({{{x}}_{n1}}) $为正向当前节点和反向当前节点预估移动代价；$ {\kappa _1} $为作用于$ h({{{x}}_{n1}}) $的动态加权因子；$ c({{{x}}_{n1}}) $为归正因子；$ o_J $为障碍物列表中第$ J $个障碍物节点；$ L $为碰撞约束阈值，考虑到煤矿救援机器人尺寸，不能将其视为质点，因此设$ L = 2 $；$ O $为障碍物个数；$ {{{x}}_{{\mathrm{first1}}}} $为正向起始节点； $ l $和$ m $分别为地图的长度和宽度； $ {{{x}}_{n2}} $为反向搜索当前节点；$ {{{x}}_{{\mathrm{goal}}1}} $为正向目标节点。
$ {\kappa _1} $可以动态调节$ h({{{x}}_{n1}}) $的权重，搜索开始时，代价函数的权重相同，均为1，通过$ c({{{x}}_{n1}}) $选择靠近终点的节点作为扩展节点，以避免搜索双方出现不相交的情况。随着正反向搜索当前节点的距离不断减小，$ {\kappa _1} $逐渐增大，预估代价函数成为代价函数中代价的主要贡献者，此时搜索的双方会快速靠近。
同理可得反向代价函数。
1.3 分层平滑优化策略
为了消除非光滑转角、冗余节点及冗余路段，设计并引入了分层平滑优化策略，其可分为内层冗余点、转折角优化和外层路径平滑生成优化，如图2所示。
内层优化的目的是消除路径中的冗余节点和不必要的转弯，以减少路径的拐角和长度。
1） 冗余点精简。首先对直线路段上的节点进行精简，只保留使路径方向改变的节点，并将精简后的节点存入节点列表中，记节点列表的集合为$ x= x[{x_1},{x_2}, \cdots \cdots {x_n}] $。
2） 转折角精简。若节点$ {x_0} $和$ {x_4} $能直接连线且连线路径与最近障碍物距离大于安全距离$ d $，则$ {x_0} $和$ {x_4} $之间的$ {x_1} $，$ {x_2} $和$ {x_3} $被当作冗余转折角去除，从起点到终点依次遍历整个集合，剔除所有的冗余转折角（图2（a））。
由于内层优化后的路径是分段的，不够平滑，煤矿救援机器人在拐点处可能会因为方向的变化无法正常救援，影响煤矿救援机器人的稳定性，所以需通过外层优化（图2（b））对路径进行进一步平滑处理，使路径平滑且安全。
1） 控制节点生成。首先将$ {{{x}}_{{\mathrm{current}}}} $（当前转折节点）记为控制节点，然后以$ {{{x}}_{{\mathrm{current}}}} $为圆心、R为半径作圆，交相邻两边于点A，B，以得到控制节点$ {{{x}}_A} $，$ {{{x}}_B} $，同理可得其他转角处控制节点的位置坐标。
$$ {{{x}}_A} = {{{x}}_{{\mathrm{current}}}} + R{{\boldsymbol{e}}_{{\mathrm{sc}}}} $$ (6) $$ {{{x}}_B} = {{{x}}_{{\mathrm{current}}}} + R{{\boldsymbol{e}}_{{\mathrm{nc}}}} $$ (7) 式中：$ {{\boldsymbol{e}}_{{\mathrm{sc}}}} $为当前转折节点$ {{{x}}_{{\mathrm{current}}}} $到前一个节点$ {{{x}}_{{\mathrm{start}}}} $方向的单位向量；$ {{\boldsymbol{e}}_{{\mathrm{nc}}}} $为当前转折节点$ {{{x}}_{{\mathrm{current}}}} $到下一个节点$ {{{x}}_{{\mathrm{next}}}} $方向的单位向量。
2） 三次B样条曲线平滑路径。B 样条曲线通常被用于路径的平滑处理，$ k $次B样条曲线的表达式为
$$ {P}_{j,k}\left(a\right)={\displaystyle \sum _{i=0}^{k}{x}_{j+i}} \frac{1}{k!}{\displaystyle \sum _{m=0}^{k-i}{(-1)}^{m}\frac{(k+1)!}{m!(k+1-m)!}{(a+k-i-m)}^{k}} $$ (8) 式中：$ {P_{j,k}}\left( a \right) $为第$ j $段k次B样条曲线， $ a $为基函数变量，$ k $为B样条曲线的总次数；$ {x_{j + i}} $为第$ j $段i次B样条曲线中的控制节点$ ,{\text{ }}i \in [0,{\text{ }}k] $；$ m $为B样条曲线的次数，m∈[0,k-i]。
高阶样条曲线的导数较高，存在许多零点，会出现许多极值，使得曲线有很多峰谷，因此，次数越低，B 样条曲线逼近控制点的效果越好。三次 B 样条曲线可以实现二阶导数的连续性，满足煤矿救援机器人速度和加速度连续性的需求。因此，在综合考虑下采用三次 B 样条曲线对路径进行平滑优化。当$ k = 3 $时，三次B样条曲线方程为
$$ {P_{0,3}}(a) = \frac{1}{6}[1{\text{ }}a{\text{ }}{a^2}{\text{ }}{a^3}]\left[ {\begin{array}{*{20}{c}} 1&4&1&0 \\ { - 3}&0&3&0 \\ 3&{ - 6}&3&0 \\ { - 1}&3&{ - 3}&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_0}} \\ {{x_1}} \\ {{x_2}} \\ {{x_3}} \end{array}} \right] $$ (9) 设控制节点数量为p，由于相邻的4个控制节点能够定义一段三次B样条曲线，所以采用相邻的4个控制节点xp−3，xp−2，xp−1， xp绘制第（p−3）段三次B样条曲线，按此规律可求出每段三次B样条曲线与控制节点的关系：
$$ P_{{n}, 3}(a)=\frac{1}{6}\left[ \begin{array}{llll} 1 & a & a^{2} & a^{3} \end{array} \right]\left[ \begin{array}{cccc} 1 & 4 & 1 & 0 \\ -3 & 0 & 3 & 0 \\ 3 & -6 & 3 & 0 \\ -1 & 3 & -3 & 1 \end{array} \right]\left[ \begin{array}{c} x_{p-3} \\ x_{p-2} \\ x_{p-1} \\ x_{p} \end{array} \right] $$ (10) 由式（10）绘制出控制节点所有的曲线段，然后将绘制的各段曲线平滑连接，可得到一条连续的光滑曲线路径。
2. 分层平滑优化双向A*引导DWA
2.1 DWA路径规划算法
2.1.1 煤矿救援机器人运动模型
煤矿救援机器人运动模型如图3所示，（Xrobot，Yrobot）为机器人坐标，$\theta $为$ \Delta t $时间段内机器人沿Xrobot运动的路径与X轴的夹角，煤矿救援机器人在一定时间分隔内的位置和姿态变换公式为
$$ {\mu _{t2}} = {\mu _{t1}} + v\Delta t\cos\; {\theta _{\Delta t}} $$ (11) $$ {\varepsilon _{t2}} = {\varepsilon _{t1}} + v\Delta t\sin \;{\theta _{\Delta t}} $$ (12) $$ {\theta _{t2}} = {\theta _{t1}} + w\Delta t $$ (13) 式中：$ {\mu _{t2}} $，$ \mu_{t1} $分别为t2和t1时刻煤矿救援机器人所在节点的X轴坐标值；$ \varepsilon_{t2} $，$ {\varepsilon _{t1}} $分别为$ {t_2} $和$ {t_1} $时刻煤矿救援机器人所在节点的Y轴坐标值；$ v $，$ w $分别为煤矿救援机器人线速度和角速度；$ \Delta t $为$ {t_1} $—$ {t_2} $的时间间隔；$ {\theta _{t2}} $，$ {\theta _{t1}} $分别为$ {t_2} $，$ {t_1} $时刻煤矿救援机器人所在位置节点的航向角。
2.1.2 速度采样
根据煤矿救援机器人自身的速度限制, 其线速度与角速度都存在最值，因此满足自身速度限制条件下的允许速度Vs为
$$ {V}_{{\mathrm{s}}}=\{(v,w)|{v}_{\mathrm{min}}\leqslant v\leqslant {v}_{\mathrm{max}},\;{w}_{\mathrm{min}}\leqslant w\leqslant {w}_{\mathrm{max}}\} $$ (14) 式中$ {v_{\min }} $,$ {v_{\max }} $,$ {w_{\min }} $，$ {w_{\max }} $分别为煤矿救援机器人的最小线速度、最大线速度、最小角速度和最大角速度。
由于受机器人动力学限制,在当前线速度$ {v_{\mathrm{c}}} $、角速度$ {w_{\mathrm{c}}} $已知条件下，下一时刻的可达速度$ {V_{\mathrm{d}}} $满足：
$$ {V_{\mathrm{d}}} = \{ v \in [{v_{\mathrm{c}}} { - {\dot v_{\mathrm{b}}}} \Delta t,{v_{\mathrm{c}}} { + {\dot v_{\mathrm{a}}}} \Delta t],w \in [{w_{\mathrm{c}}} { - {\dot w_{\mathrm{b}}}} \Delta t,{w_{\mathrm{c}}} { + {\dot w_{\mathrm{a}}}} \Delta t]\} $$ (15) 式中：$ {{\dot v_{\mathrm{b}}}} $,$ {{\dot w_{\mathrm{b}}}} $为线速度中最大减速度、最大加速度；$ {{\dot v_{\mathrm{a}}}} $，$ {{\dot w_{\mathrm{a}}}} $分别为角速度中最大减速度、最大加速度。
从安全性考虑，需要保证移动机器人运行至障碍物时速度减为零，因此不发生碰撞的允许速度$ {V_{\mathrm{a}}} $满足：
$$ {V_{\mathrm{a}}} = \{ v \leqslant \sqrt {2d(v,w) {{\dot v_{\mathrm{b}}}} } ,w \leqslant \sqrt {2d(v,w) {{\dot w_{\mathrm{b}}}} } \} $$ (16) 式中$ d(v,w) $为轨迹上与障碍物最近的距离。
最终的速度$V_{\mathrm{r}} $为上述3个速度的合集：
$$ {V_{\mathrm{r}}} = {V_{\mathrm{s}}} \cap {V_{\mathrm{d}}} \cap {V_{\mathrm{a}}} $$ (17) 2.1.3 DWA评价函数
DWA 评价函数综合考虑了与终点的方位角、距离、与最近障碍物距离3个偏差量。通过对以上 3 个偏差量进行归一化处理，并赋予相应权重组合得到DWA轨迹评价函数：
$$ E(v,w) = \psi (\alpha {\mathrm{heading}}(v,w) + \beta {\mathrm{target}}(v,w) + \gamma {\mathrm{od}}(v,w)) $$ (18) 式中：$ \psi $ 为归一化参数；$ \alpha $，$ \beta $，$ \gamma $为各项权重；$ {\mathrm{heading}} (v,w) $为煤矿救援机器人方位角评价函数，用来评价煤矿救援机器人轨迹末端的朝向与终点方向的角度差值，差值越小，评价越高；$ {\mathrm{target}}(v,w) $为目标距离评价函数，用于评价煤矿救援机器人轨迹末端与终点之间的距离，距离越小，评价越高；$ {\mathrm{od}}(v,w) $为障碍物距离评价函数，用于评价煤矿救援机器人轨迹末端与最近障碍物之间的距离，距离越大，轨迹越安全，评价越高。
2.2 局部避障路径引导优化生成机制
局部路径引导点选取规则如图4所示。煤矿救援机器人按照全局路径行驶，在探测到未知障碍物时将煤矿救援机器人当前时刻位置和下一时刻位置分别记为$ {{{x}}_{j - 1}} $和$ {{{x}}_j} $，通过DWA进行局部避障。首先确定局部路径引导节点$ {{{x}}_{{\mathrm{ga}}}} $，然后确定路径节点$ {{{x}}_{{\mathrm{st}}}} $，最后确定局部路径引导节点$ {{{x}}_{{\mathrm{it}}}} $。
$$ {{{x}}_{{\mathrm{ga}}}} = 2{{{x}}_{{\mathrm{ob}}}} - {{{x}}_j} $$ (19) $$ {{{x}}_{{\mathrm{st}}}} = \mathop {\arg \min }\limits_{{{{x}}_{{\text{Route}}}} \in \left\{ {{{{x}}_{j}},{{{x}}_{j+1}}, \cdots, {{{x}}_{{\mathrm{target}}}}} \right\}} \left\| {{{{x}}_{{\mathrm{ga}}}} - {{{x}}_{{\mathrm{Route}}}}} \right\| $$ (20) $$ {{{x}}_{it}} = \mathop {\arg \max }\limits_{{{{x}}_{_{{\text{Route}}}}} \in \left\{ {{{{x}}_{j}},{{{x}}_{j+1}}, \cdots, {{{x}}_{{\mathrm{target}}}}} \right\}} \frac{{{{\boldsymbol{e}}_{{\mathrm{gi}}}}^{\rm T}({{{x}}_{{{\mathrm{Route}}}}} - {{{x}}_{{\mathrm{ga}}}})}}{{\left\| {{{{x}}_{{\mathrm{Route}}}} - {{{x}}_{{\mathrm{ga}}}}} \right\|}} $$ (21) 式中：$ {{{x}}_{{\mathrm{ob}}}} $为未知障碍物节点；$ {{{x}}_{{\mathrm{target}}}} $为目标节点；$ {{{x}}_{{\mathrm{Route}}}} $为全局路径上所有路径节点的集合；$ {{\boldsymbol{e}}_{{\mathrm{gi}}}} $为$ {{\boldsymbol{e}}_{{\mathrm{gt}}}} $与$ {{\boldsymbol{e}}_{{\mathrm{gs}}}} $的合向量，$ {{\boldsymbol{e}}_{{\mathrm{gt}}}} $为煤矿救援机器人进入引导节点$ {{{x}}_{{\mathrm{ga}}}} $无效区域（图4蓝色虚线圆圈）时机器人线速度方向的单位向量，$ {{\boldsymbol{e}}_{{\mathrm{gs}}}} $为引导节点$ {{{x}}_{{\mathrm{ga}}}} $到路径节点$ {{{x}}_{{\mathrm{st}}}} $方向的单位向量。
2.3 HSTA*−G−DWA算法流程
HSTA*−G−DWA算法流程如图5所示。
![]()
图 5 HSTA*−G−DWA算法流程Figure 5. Hierarchical smoothing optimization A* guiding dynamic window approach(HSTA*-G-DWA) algorithm flow1） 在空间M中定义起始节点与目标节点，并采用动态加权因子和归正因子对双向A*算法的代价函数进行改进。
2） 将起始节点和目标节点分别添加至open1列表和open2列表中，作为正向搜索和反向搜索的初始节点。
3） 对正向搜索和反向搜索的初始节点进行邻域扩展，此时起始节点的坐标分别被加入到close1列表和close2列表中。
4） 通过计算正向和反向的代价函数值，将子节点分别按顺序存入2个open列表中，并分别更新当前节点。
5） 判断正向和反向扩展中选出的最优节点是否为同一节点，若不是，则继续在正反2个方向上进行邻域扩展，重复上述过程，直到找到最终目标节点。若是，则表明双向搜索已相遇，路径搜索成功。
6） 利用分层平滑优化策略消除路径中的冗余路径节点和转折节点，采用三次B样条曲线对路径进行平滑处理，得到初始全局路径。
7） 若煤矿救援机器人按照初始全局路径行驶过程中未探测到未知障碍物，则煤矿救援机器人按照初始全局路径行驶。
8） 若煤矿救援机器人按照初始全局路径行驶过程中探测到未知障碍物时，则利用全局路径引导DWA实现局部动态避障。
9） 如果煤矿救援机器人处于静态场景，则引导煤矿救援机器人回到全局路径继续行驶，并跟随初始全局路径运动；若不处于静态场景，则继续利用全局路径引导DWA完成局部动态避障。
10） 如果机器人没有到达目标节点，则继续重复步骤7）—步骤9）；若到达目标节点，则路径规划完成。
3. 实验分析
3.1 HSTA*−G−DWA算法仿真实验
为验证HSTA*−G−DWA算法的正确性与有效性，在 Matlab 中进行算法的仿真实验验证。在静态场景下，将HSTA*−G−DWA算法和双向A*算法、传统A*算法进行仿真对比分析，所得实验结果如图6所示，数据见表1。
表 1 静态场景仿真实验数据对比Table 1. Data comparison of simulation experiment in static scenes算法 路径转折
角数/个非光滑转折
角数/个与障碍物
最小距离/m搜索时间/s 传统A*算法 18 18 0 31.57 双向A*算法 16 16 0 16.23 HSTA*−G−DWA算法 4 0 0.83 5.74 由图6和表1可知，在静态环境下，HSTA*−G−DWA算法路径转折角数较传统A*算法、双向A*算法分别降低了77.78%，75%，搜索时间分别降低了81.82%，64.63%。说明HSTA*−G−DWA算法规划路径更加平滑且效率更高。
在煤矿未知场景下将HSTA*−G−DWA算法与RRT算法、改进4领域A*算法进行仿真对比分析，所得实验结果如图7所示，数据见表2。
![]()
图 7 煤矿未知场景下仿真结果对比Figure 7. Comparison of simulation results in unknown coal mine scenes表 2 煤矿未知场景下仿真实验数据对比Table 2. Data comparison of simulation experiments in unknown coal mine scenes测试场景 非光滑转折角数/个 路径长度/m 与障碍物最小距离/m RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法转弯巷道 38 51 0 66.78 77 60.09 0 0 1.11 连转巷道 46 5 0 77.99 80 69.72 0 0 1.17 转巷巷道 61 7 0 90.39 88 80.94 0 0 1.09 由图7和表2可知，在存在未知静态和动态障碍物的转弯、连转、转巷3种特殊复杂煤矿环境中，HSTA*−G−DWA算法在路径长度、路径平滑性及路径安全性方面均优于RRT算法和改进4领域A*算法，且HSTA*−G−DWA算法可以实现实时避障。
在煤矿环境下将HSTA*−G−DWA算法与传统A*算法、Dijkstra算法和文献[20]融合算法进行对比分析，所得实验结果如图8所示，数据见表3。
![]()
图 8 煤矿环境下HSTA*−G−DWA与其他路径规划算法性能对比Figure 8. Performance comparison of HSTA*-G-DWA and other path planning algorithms in coal mine environment表 3 煤矿场景下不同算法仿真实验对比数据Table 3. Data comparison of simulation experiments of different algorithms in coal mine scenes算法 路径长度/m 搜索时间/s 与障碍物最小距离/m 传统A*算法 86.31 36.72 0 Dijkstra算法 86.31 58.69 0 文献[20]融合算法 75.53 65.42 0.81 HSTA*-G-DWA算法 73.68 19.31 0.82 由图8和表3可知，在相同煤矿环境下，传统A*算法和Dijkstra算法路径平滑性和安全性较差且不能实现实时避障，当煤矿环境中出现未知障碍物时可能会导致路径规划失败。HSTA*−G−DWA算法规划路径长度较文献[20]融合算法降低了2.45%，搜索时间降低了70.48%，这是因为HSTA*−G−DWA算法无需在完成全局路径规划后使煤矿救援机器人依赖全局路径的关键点作为中间引导点，而是再次利用DWA从起点到终点进行二次路径规划并行驶至终点，当煤矿救援机器人按照全局路径行驶过程中探测到未知障碍物时，按照局部避障路径引导优化生成机制生成避障与返回全局路径的局部动态修正路径行驶，安全避障后回到全局路径继续行驶至终点。
3.2 实验室环境实验
为进一步验证HSTA*−G−DWA算法的有效性和可行性，利用煤矿救援机器人进行实验室环境实验，实验室环境如图9所示，其中图9（a）为不存在未知障碍物的静态环境，图9（b）为存在未知障碍物的未知环境（临时增加静态障碍物替代未知障碍物）。在机器人操作系统（Robot Operating System，ROS）中完成煤矿救援机器人的自身定位和地图构建，并完成路径轨迹规划，所得静态环境路径如图10所示，其中绿色实线为煤矿救援机器人从起点到终点的路径。
![]()
图 10 实验室静态环境实验结果Figure 10. Experimental results in a static environment in the laboratory由图10可看出，在静态环境下，HSTA*−G−DWA算法可以有效地规划出一条安全平滑的路径，煤矿救援机器人能够按照所规划的路径行驶，并安全避开障碍物后到达终点。
为验证HSTA*−G−DWA算法的实时避障能力，在实验室环境中分别临时增加3个未知障碍物，所得实验结果如图11所示。
![]()
图 11 实验室未知环境下实验结果Figure 11. Experimental results in an unknown environment in the laboratory由图11可看出，随着煤矿救援机器人向终点不断运动，在煤矿救援机器人探测到第1个未知障碍物时，利用DWA对局部路径进行调整，安全避开第1个障碍物后回到全局路径继续运动。当煤矿救援机器人再次探测到第2个和第3个未知障碍物时，通过相同的方式进行避障调整直至到达终点。
为了进一步验证本文所提算法的优越性，将HSTA*−G−DWA算法与传统A*算法分别在静态环境与未知环境中进行对比分析，其中传统A*算法实验结果如图12所示。
由图10—图12可看出，在静态环境下，煤矿救援机器人采用传统A*算法进行路径规划时，机器人行驶的路径比较弯折，非光滑转折角数为10个，机器人边缘距障碍物的最小距离为0.11 m，机器人到达终点所用时间为27.54 s。当采用HSTA*−G−DWA算法进行路径规划时，煤矿救援机器人行驶的路径较为光滑，机器人边缘距障碍物的最小距离为0.82 m，路径安全性提高，机器人到达终点所用时间为11.36 s，较传统A*算法缩短了58.75%。在未知环境中，煤矿救援机器人采用传统A*算法进行路径规划时，无法成功避开环境中存在的未知障碍物，机器人边缘距障碍物的最小距离为0，且路径较为弯折。当采用HSTA*−G−DWA算法进行路径规划时，煤矿救援机器人行驶路径较为光滑，且能够成功避开环境中出现的未知障碍物并到达终点，机器人边缘距障碍物的最小距离为0.86 m。
4. 结论
1） 在双向A*算法中引入动态加权因子、碰撞约束函数和归正因子的调整机制，提高了路径的搜索效率和安全性。仿真结果表明，经过上述优化后，双向A*算法规划的路径能够确保煤矿救援机器人与巷道壁、障碍物之间保持安全距离，且路径搜索时间较传统A*算法和双向A*算法分别减少了81.82%和64.63%。
2） 采用分层平滑优化策略消除路径中的冗余点和转折角，减少节点数量和路径长度，提高路径平滑性。仿真结果表明，经路径平滑处理后，路径转折角数较双向A*算法和传统A*算法分别减少了75%和77.8%，路径平滑性得到改善。
3） 构建局部避障路径引导优化生成机制，提升了实时寻路效率。仿真结果表明，在相同煤矿环境下相比于现有融合算法，HSTA*−G−DWA算法路径搜索时间和路径长度分别减少了70.48%和2.45%。
4） 对煤矿救援机器人在存在未知静态和动态障碍物的转弯、连转、转巷3种巷道中进行寻路搜索仿真实验。结果表明，在不同巷道环境下，HSTA*−G−DWA算法规划路径长度较RRT算法和改进A*算法分别平均减少了10.34%和14.28%，并且具有更好的路径平滑性，可以实时避开环境中出现的未知障碍物。
5） 在实验室环境下验证了HSTA*−G−DWA算法的可行性，然而在真实复杂的煤矿救援场景中，需要考虑到煤矿救援机器人的防爆和通信性能等方面。未来的研究将综合考虑上述因素对算法进行改进范畴，以提高煤矿救援机器人在复杂煤矿救援环境中的应用价值。
-
![]()
图 5 HSTA*−G−DWA算法流程
Figure 5. Hierarchical smoothing optimization A* guiding dynamic window approach(HSTA*-G-DWA) algorithm flow
![]()
图 7 煤矿未知场景下仿真结果对比
Figure 7. Comparison of simulation results in unknown coal mine scenes
![]()
图 8 煤矿环境下HSTA*−G−DWA与其他路径规划算法性能对比
Figure 8. Performance comparison of HSTA*-G-DWA and other path planning algorithms in coal mine environment
![]()
图 10 实验室静态环境实验结果
Figure 10. Experimental results in a static environment in the laboratory
![]()
图 11 实验室未知环境下实验结果
Figure 11. Experimental results in an unknown environment in the laboratory
表 1 静态场景仿真实验数据对比
Table 1 Data comparison of simulation experiment in static scenes
算法 路径转折
角数/个非光滑转折
角数/个与障碍物
最小距离/m搜索时间/s 传统A*算法 18 18 0 31.57 双向A*算法 16 16 0 16.23 HSTA*−G−DWA算法 4 0 0.83 5.74 
下载: 导出CSV
表 2 煤矿未知场景下仿真实验数据对比
Table 2 Data comparison of simulation experiments in unknown coal mine scenes
测试场景 非光滑转折角数/个 路径长度/m 与障碍物最小距离/m RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法RRT算法 改进4领域
A*算法HSTA*−G−
DWA算法转弯巷道 38 51 0 66.78 77 60.09 0 0 1.11 连转巷道 46 5 0 77.99 80 69.72 0 0 1.17 转巷巷道 61 7 0 90.39 88 80.94 0 0 1.09
下载: 导出CSV
-
[1] ZHENG Li,YU Wenjie,LI Guangxu,et al. Particle swarm algorithm path-planning method for mobile robots based on artificial potential fields[J]. Sensors,2023,23(13). DOI: 10.3390/s23136082.
[2] 陈艺文,江文松,杨力,等. 基于运动约束的移动机器人路径规划[J]. 计算机集成制造系统,2023,29(4):1186-1193. CHEN Yiwen,JIANG Wensong,YANG Li,et al. Path planning based on motion constraints for mobile robot[J]. Computer Integrated Manufacturing Systems,2023,29(4):1186-1193.
[3] 朱洪波,殷宏亮. 分层平滑优化A*引导DWA用于机器人路径规划[J]. 电子测量与仪器学报,2024,38(9):155-168. ZHU Hongbo,YIN Hongliang. Hierarchical smoothing optimization A*-guided DWA for robot path planning[J]. Journal of Electronic Measurement and Instrumentation,2024,38(9):155-168.
[4] 彭育强,黄泽龙,李少伟. 基于动态窗口法的移动机器人自动避障导航研究[J]. 自动化仪表,2020,41(10):26-29,33. PENG Yuqiang,HUANG Zelong,LI Shaowei. Research on automatic obstacle avoidance navigation of mobile robot based on dynamic window approach[J]. Process Automation Instrumentation,2020,41(10):26-29,33.
[5] 郑维,王昊,王洪斌. 动态环境下基于自适应步长Informed−RRT*和人工势场法的机器人混合路径规划[J]. 计量学报,2023,44(1):26-34. DOI: 10.3969/j.issn.1000-1158.2023.01.05 ZHENG Wei,WANG Hao,WANG Hongbin. Adaptive step size Informed-RRT* and artificial potential field algorithm for hybrid path planning of robot[J]. Acta Metrologica Sinica,2023,44(1):26-34. DOI: 10.3969/j.issn.1000-1158.2023.01.05
[6] 陈尔奎,吴梅花,张英杰. 复杂环境下煤矿救灾机器人路径规划[J]. 煤炭技术,2018,37(10):301-304. CHEN Erkui,WU Meihua,ZHANG Yingjie. Path planning for coal mine rescue robot in complex environment[J]. Coal Technology,2018,37(10):301-304.
[7] XU Huimin,YU Gaohong,WANG Yimiao,et al. Path planning of mecanum wheel chassis based on improved A* algorithm[J]. Electronics,2023,12(8):1754. DOI: 10.3390/ELECTRONICS12081754
[8] LUO Qiang,WANG Haibao,ZHENG Yan,et al. Research on path planning of mobile robot based on improved ant colony algorithm[J]. Neural Computing and Applications,2020,32(6):1555-1566. DOI: 10.1007/s00521-019-04172-2
[9] 程晓明,刘银华,赵文政. 面向大型三维结构检测的多机器人覆盖路径规划方法[J]. 计算机集成制造系统,2023,29(1):246-253. CHENG Xiaoming,LIU Yinhua,ZHAO Wenzheng. Multi-robot coverage path planning for large 3D structure inspection[J]. Computer Integrated Manufacturing Systems,2023,29(1):246-253.
[10] 魏彤,龙琛. 基于改进遗传算法的移动机器人路径规划[J]. 北京航空航天大学学报,2020,46(4):703-711. WEI Tong,LONG Chen. Path planning for mobile robot based on improved genetic algorithm[J]. Journal of Beijing University of Aeronautics and Astronautics,2020,46(4):703-711.
[11] 周伟,胡毅,刘进江,等. 基于Dijkstra算法的AMR集群协同调度研究[J]. 制造技术与机床,2023(6):175-179. ZHOU Wei,HU Yi,LIU Jinjiang,et al. Research on AMR cluster cooperative scheduling based on Dijkstra algorithm[J]. Manufacturing Technology & Machine Tool,2023(6):175-179.
[12] 张家旭,王志伟,郭崇,等. 面向动态障碍物场景的自主代客泊车路径规划[J]. 东南大学学报(自然科学版),2022,52(2):369-376. ZHANG Jiaxu,WANG Zhiwei,GUO Chong,et al. Autonomous valet parking path planning for dynamic obstacle scene[J]. Journal of Southeast University(Natural Science Edition),2022,52(2):369-376.
[13] 王梓强,胡晓光,李晓筱,等. 移动机器人全局路径规划算法综述[J]. 计算机科学,2021,48(10):19-29. DOI: 10.11896/jsjkx.200700114 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. DOI: 10.11896/jsjkx.200700114
[14] 朱子祺,李创业,代伟. 基于G−RRT*算法的煤矸石分拣机器人路径规划[J]. 工矿自动化,2022,48(3):55-62. ZHU Ziqi,LI Chuangye,DAI Wei. Path planning of coal gangue sorting robot based on G-RRT* algorithm[J]. Journal of Mine Automation,2022,48(3):55-62.
[15] 黄健萌,吴宇雄,林谢昭. 移动机器人平滑JPS路径规划与轨迹优化方法[J]. 农业机械学报,2021,52(2):21-29,121. DOI: 10.6041/j.issn.1000-1298.2021.02.002 HUANG Jianmeng,WU Yuxiong,LIN Xiezhao. Smooth JPS path planning and trajectory optimization method of mobile robot[J]. Transactions of the Chinese Society for Agricultural Machinery,2021,52(2):21-29,121. DOI: 10.6041/j.issn.1000-1298.2021.02.002
[16] 黄智榜,胡立坤,张宇,等. 基于改进跳点搜索策略的安全路径研究[J]. 计算机工程与应用,2021,57(1):56-61. DOI: 10.3778/j.issn.1002-8331.2003-0064 HUANG Zhibang,HU Likun,ZHANG Yu,et al. Research on security path based on improved hop search strategy[J]. Computer Engineering and Applications,2021,57(1):56-61. DOI: 10.3778/j.issn.1002-8331.2003-0064
[17] 陶德俊,姜媛媛,刘延彬,等. 煤矿救援机器人路径平滑算法研究[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.
[18] 张伟民,张月,张辉. 基于改进A*算法的煤矿救援机器人路径规划[J]. 煤田地质与勘探,2022,50(12):185-193. DOI: 10.12363/issn.1001-1986.22.04.0317 ZHANG Weimin,ZHANG Yue,ZHANG Hui. Path planning of coal mine rescue robot based on improved A* algorithm[J]. Coal Geology & Exploration,2022,50(12):185-193. DOI: 10.12363/issn.1001-1986.22.04.0317
[19] 徐保来,管贻生,苏泽荣,等. 改进动态窗口法的阿克曼移动机器人局部路径规划器[J]. 机电工程技术,2016,45(9):21-26. DOI: 10.3969/j.issn.1009-9492.2016.09.005 XU Baolai,GUAN Yisheng,SU Zerong,et al. A modified dynamic window approach to local path planning for the ackermann mobile robot[J]. Mechanical & Electrical Engineering Technology,2016,45(9):21-26. DOI: 10.3969/j.issn.1009-9492.2016.09.005
[20] 毛清华, 姚丽杰, 薛旭升. 煤矿履带式定向钻机路径规划算法[J]. 工矿自动化,2024,50(2):18-27. MAO Qinghua, YAO Lijie, XUE Xusheng. Path planning algorithm for tracked directional drilling rigs in coal mines[J]. Journal of Mine Automation,2024,50(2):18-27.
[21] 王倩,李俊丽,杨立炜,等. 改进蚁群融合DWA算法的移动机器人路径规划[J]. 兵工自动化,2023,42(4):79-84. WANG Qian,LI Junli,YANG Liwei,et al. Mobile robot path planning based on improved ant colony and DWA algorithm[J]. Ordnance Industry Automation,2023,42(4):79-84.
计量
- 文章访问数: 48
- HTML全文浏览量: 17
- PDF下载量: 6
