Path planning of wheeled coal mine rescue robot based on improved A* and potential field algorithm
-
摘要:
煤矿救援机器人在非结构化井下巷道环境中执行搜救任务，传统的路径规划算法用于搜索空间较大或者复杂度较高的非结构化环境时，可能会面临效率低、非最优路径和平滑度较差等问题；此外，巷道存在复杂的交叉口等环境特征，机器人在该区域容易偏离预设路线或与路口壁面发生剐蹭。为了克服这些问题并提高机器人的导航精度，提出了针对轮式煤矿救援机器人的路径规划算法改进方案：①改进启发式全局路径规划A*算法，通过分层邻域搜索与剪枝的方法改进邻域搜索方式，优化代价函数以更好地平衡实际代价和启发式代价的影响，更准确地评估每个节点的代价，使其适应实际情况且缩减了计算量，并利用B样条方法对路径进行平滑处理；②利用随机样本一致算法（RANSAC）构建煤矿巷道壁面的几何模型，以便提取交叉路口特征点坐标纳入规划系统，利用B样条曲线基函数的局部支撑性优化路径，当后续添加路径优化点时，仅影响相应区间的曲线形状，对路径的其余部分不产生影响；③根据构建的环境几何模型和提取的特征点建立综合局部势场，引入调整系数优化势场分布，并使用粒子群算法（PSO）优化的PID算法实现运动控制，增强机器人针对巷道交叉口等复杂环境通行的适应能力。通过MATLAB和机器人操作系统（ROS）仿真验证了算法原理与应用的可行性，结果表明该方法可以实现在复杂交叉环境中路径规划、自主行驶等功能。
Abstract:Coal mine rescue robots perform search and rescue tasks in unstructured underground tunnel environments. Traditional path planning algorithms may encounter issues such as low efficiency, non-optimal paths, and poor smoothness when applied to search spaces that are large or complex. Additionally, tunnels feature complex environmental characteristics such as intersections, where robots are prone to deviating from preset routes or scraping against tunnel walls. To address these challenges and enhance the navigation accuracy of robots, improvements to the path planning algorithm for wheeled coal mine rescue robots are proposed: ① The heuristic global path planning A* algorithm is enhanced by employing layered neighborhood search and pruning techniques to optimize the search process. The cost function is refined to better balance the influence of actual cost and heuristic cost, thus more accurately assessing the cost of each node, adapting to real situations, reducing computational complexity, and smoothing the path using B-spline methods. ② The Random Sample Consensus (RANSAC) fitting algorithm is utilized to construct a geometric model of coal mine tunnel walls, facilitating the extraction of feature point coordinates of intersections for inclusion in the planning system. The path is optimized using the local support property of B-spline basis functions. When additional path optimization points are added subsequently, only the shape of the curve in the corresponding interval is affected, leaving the rest of the path unaffected. ③ A comprehensive local force field is established based on the constructed environmental geometric model and extracted feature points. Adjustment coefficients are introduced to optimize the distribution of the force field, and motion control is achieved using the Particle Swarm Optimization (PSO) optimized PID (Proportion Integral Differential) algorithm, enhancing the robot's adaptability to complex environments such as tunnel intersections. The feasibility of the algorithm principles and applications is validated through MATLAB and ROS (Robot Operating System) simulations. Experimental results demonstrate that the proposed method can realize functions such as path planning and autonomous driving in complex intersection environments.
-
0. 引 言
在煤矿地下开采生产过程中，由于煤层和地质条件的差异，巷道环境条件极其复杂，主要受到煤与瓦斯突出、矿井涌水、岩爆等灾害的威胁[1-3]。为保障矿工安全，灾害后的救援与探察是一项重要的措施[4]。因此，研发煤矿救援机器人已成为当前煤炭行业发展的重要趋势[5-7]，在工业自动化、煤矿智能装备等领域也引起广泛关注[8]。可靠的导航技术是救援机器人安全行驶的基础[9]，而路径规划作为导航过程中的关键环节，具有重要意义[10-12]。尽管现如今的路径规划技术已逐渐发展成熟，并在机器人等领域得到了广泛应用[13]，但在煤矿井下，路径规划技术仍面临许多挑战。发生煤与瓦斯突出、煤尘瓦斯爆炸、冲击地压、水害火灾等灾害后[14-15]，煤矿巷道通常存在狭窄、封闭、黑暗、高温、缺氧、粉尘和有害气体浓度高、障碍物多、可通行区域受限等恶劣条件，呈现出复杂的非结构化特征，导致现有的路径规划算法在井下难以取得良好效果[16]。针对煤矿救援机器人路径规划技术的进一步研究与改进亟待开展。
为有效应对上述挑战，诸多学者进行了改进研究。马小陆等[17]融合了蚁群算法和跳点搜索算法的搜索策略,使规划出的路径更加平滑；引入了势场合力递减系数，减少了局部最优问题；提高了算法前期的搜索效率。鲍久圣等[18]选择采用最优性启发式全局路径规划算法（A*算法）和基于势场的局部路径规划算法（人工势场算法），使用指数函数加权和三次样条插值的方法改进A*算法，并引入斥力势场修正因子和出逃力优化人工势场法，更适用于巷道环境。GAO等[19]改进了人工鱼群算法（Artificial Fish Swarm Algorithm，AFSA）和动态窗口算法（Dynamic Window Approach，DWA）之间的混合算法，用于煤矿机器人的全局路径规划，并引入改进的遗传算法（Genetic Algorithm，GA）来提高路径规划的精度。但这些方法在搜索空间较大或者环境复杂度增加时，有可能会面临效率低与路径非最优解的问题。煤矿救援机器人需迅速通行巷道，以便尽快赶至灾变发生地点，传统规控算法在通行巷道交叉口时，难以完全遵循理论路径进行运动控制。
为解决上述问题，提出了一种面向煤矿救援机器人路径规划算法的改进方法，改进A*算法的邻域搜索方式、代价函数和平滑度，构建综合势场，提高机器人面对交叉口等复杂环境的应变能力。在实际应用中，以二维激光雷达作为数据采集传感器，处理局部信息并快速识别当前巷道交叉口的关键点信息，进一步提高机器人的规划和运动控制精度。
1. 全局路径规划A*算法改进
1.1 全局路径规划算法
常见的路径规划算法可以分为2类：基于采样的规划和基于搜索的规划[20]。
快速扩展随机树（Rapidly-exploring Random Trees，RRT）系列算法是基于采样规划的典型代表，该方法适用于复杂高维的配置空间，且通常具有较低的时间复杂度。但该系列算法路径生成是基于随机采样的，其结果会受到随机性的影响，导致生成的路径质量不稳定，RRT算法只能保证找到可行解，但无法保证找到最优解[21]。
图1展示了在相同栅格地图中进行2次RRT算法求解路径时的结果。如图1所示，RRT算法随机性较大，2次运行RRT算法规划的路径并不相同，且可明显看出生成路径非最优路径。
另一类路径规划算法是基于搜索的规划，该方式适用于对路径质量要求较高、需要全局最优解的任务，常见的算法包括Dijkstra算法、A*算法等。图2为这2种算法的路径规划仿真效果对比，应用地图均为30×30的栅格地图，障碍物占总栅格数比例为30%，已搜索节点根据计算代价值由小到大依次设置颜色为浅黄、深黄、橙色、绿色。2种算法路径规划性能对比见表1。由于地图较为简单，2种算法得到的路径点数量和路径长度相同，说明它们都能够找到一条连续的路径。但A*算法搜索节点数量和所搜时间明显少于Dijkstra算法，这是因为A*算法在搜索时考虑了启发式信息，可以更快地找到最优解。
![]()
图 2 Dijkstra与A*算法路径规划效果对比Figure 2. Path planning comparison of Dijkstra algorithm and A* algorithm表 1 算法性能对比Table 1. Algorithm performance comparison算法 Dijkstra算法 A*算法 搜索节点数量 632 363 路径点数量 34 34 路径长度 41.698 5 41.698 5 搜索时间/s 0.835 1 0.610 9 综合来看，A*算法相对于Dijkstra算法具有更高的搜索效率，可以更快速地找到最优解。在实际应用中，如果能够定义一个合理的代价函数，A*算法通常会比Dijkstra算法更适合用于路径规划。因此选取A*算法用于机器人全局路径规划，并展开深入研究。
1.2 邻域搜索
在A*算法中，邻域搜索方式主要涉及确定节点的邻域以及如何扩展节点。节点的邻域通常指与当前节点相邻、可以直接从当前节点到达的节点集合。扩展节点是指将当前节点扩展到其邻域节点并计算其代价，然后选择合适的邻域节点作为下一步的搜索节点。具体的邻域搜索方式包括：四向邻域搜索、八向邻域搜索等。为解决传统邻域搜索方式存在的问题，采用分层邻域搜索方法，并采用剪枝策略进行改进。
1.2.1 分层邻域搜索
分层邻域搜索是指将邻域划分为多个层次以改进搜索过程，通常以当前节点为中心，分别定义不同半径的圆形或方形邻域，优先考虑靠近当前节点的邻域层次，逐步扩展到更远的邻域层次。先尝试靠近当前节点的邻域，如果找到方向较为贴合的目标，就不必继续向更远的邻域搜索，从而降低搜索的复杂度。这种改进方法在实际应用中能够显著提高A*算法的搜索效率，特别是在复杂环境下的路径规划问题中，可以有效缩短搜索时间。
如图3所示，黑色区域为当前节点，将邻域划分为3个层次，内层邻域的搜索优先级最高，最外层邻域的搜索优先级最低。首先搜索内层邻域，然后逐步向外扩展，依次搜索中层邻域以及外层邻域，直到找到目标节点或完成搜索。
1.2.2 剪枝处理
前文中提及的分层邻域搜索的方法，随着向外层邻域的扩展，计算量也随之增大。为避免不必要的计算，通过判断目标节点所在位置，可以确定某些节点很难产生更优的解，从而可以剪枝掉这些节点，不再扩展它们，即最外层邻域进行有方向的扩展。
如图4所示，剪枝仅针对最外层蓝色节点。图4a为目标点在当前节点正方向时的剪枝处理，图4b为目标点在当前节点斜方向时的剪枝处理。
1.3 代价函数优化
A*算法[22]的代价函数$ f\left(x\right) $为
$$ f(x) = g(x) + h(x) $$ (1) 式中：$ {g}\left({x}\right) $为从起始节点到当前节点$ {x} $的实际代价；$ {h}\left({x}\right) $为当前节点至目标点的估计消耗，也称启发函数。
启发函数$ {h}\left({x}\right) $估计的指标包括距离、方向等，常用的距离计算方式包括：
1）曼哈顿距离：
$$ h(x) = \left| {a - {a_n}} \right| + \left| {b - {b_n}} \right| $$ (2) 2）欧几里得距离：
$$ h(x) = \sqrt {{{(a - {a_n})}^2} + {{(b - {b_n})}^2}} $$ (3) 3）对角线距离：
$$ h(x)=\max (\left| a-{{a}_{n}} \right|,\left| b-{{b}_{n}} \right|) $$ (4) 式中：$ (a,b) $为当前节点的坐标；$ ({a_n},{b_n}) $为目标节点的坐标。
根据仿真验证，曼哈顿距离规划的曲线更为平滑。因此，选取曼哈顿距离参与启发函数。
针对传统A*算法存在的搜索速度慢、搜索节点多的缺点，对其代价函数进行优化，引入加权函数式。在实际搜索过程中，当前节点离目标节点较远时，估计代价$ h\left(x\right) $的误差较大，此时应以实际代价为准，故增大实际代价对代价函数的影响权重。随着当前节点逐渐接近目标点，估计代价$ h\left(x\right) $会逐渐接近真实消耗，此时权值应适当增大。使用固定的权重系数显然无法在接近目标点使预估消耗接近真实消耗，优化代价函数为
$$ f(x)=\left[\dfrac{h(x)}{g(x)+h(x)}+C_1\right]g(x)+\left[\dfrac{g(x)}{g(x)+h(x)}+C_2\right]h(x) $$ (5) 式中，C1、C2为引入的常数项。
优化代价函数的$ g\left(x\right) $与$ h\left(x\right) $项都受到权衡，随着搜索节点不断靠近目标节点，实际代价所占权重越来越小，而启发函数的估计代价越发重要。通过引入常数项C1、C2，使得$ g\left(x\right) $与$ h\left(x\right) $始终占有一定权重，这样可以更好地平衡实际代价和估计代价的影响，使搜索更符合实际情况。
传统 A*算法与优化后算法的路径规划仿真效果对比如图5所示，应用地图均为 40×40的栅格地图，障碍物占总栅格数比例为30%，已搜索节点根据计算代价值由小到大依次设置颜色为浅黄、深黄、橙色、绿色、紫色。2种算法规划路径结果性能对比见表2，优化 A*算法规划的路径长度稍长，但与传统A*算法差距较小，可以认为是相似的路径长度，但在其他性能方面优势显著。优化 A*算法的搜索节点数量和搜索时间明显少于传统 A*算法，且路径点数量比传统 A*算法少，分别为 38 和 44，说明优化后的算法找到了更为直接和连续的路径，通过优化算法，减少不必要的节点搜索，降低了计算量，减轻了系统工作压力，从而提高了搜索效率。
![]()
图 5 传统A*与优化A*算法规划路径对比Figure 5. Path planning comparison of A* algorithm and improved A* algorithm表 2 A*算法性能对比Table 2. A* algorithm performance comparison算法 传统A*算法 优化A*算法 搜索节点数量 555 266 路径点数量 44 38 路径长度 55.012 2 55.066 2 搜索时间/s 0.932 1 0.601 6 综合来看，优化A*算法相较于传统A*算法有效减少搜索空间，大大提高了搜索效率。在实际的应用中，特别是面对复杂的环境和大规模的地图，这个特性将起到很大的作用，也有效避免机器人行进到不可达的区域。
1.4 B样条平滑
代价函数加权改进后的A*路径算法能够快速搜索出一条从起点至目标点的最优路径，但路径上存在拐点的情况依然没有解决，在实际应用中会出现较大且不平稳的转向，因此需对这样的路径进行平滑处理。常用的路径平滑方式有贝塞尔曲线、三次样条插值法、B样条曲线等。
B样条曲线[23]具有良好的光滑性，能够生成连续且平滑的路径。相比之下，贝塞尔曲线一旦确定了顶点数，就决定了曲线的阶数，当数据点的数量太多时，计算量急剧增加，且无法做局部修改。而三次样条插值法可能会出现振荡或过拟合问题，导致路径的不连续或不平滑，且不支持路径点动态更新。
B样条曲线节点上的权重可以调整，从而实现不同节点间曲率的平滑过渡，进而控制路径的变化自由度。B样条基函数的局部支撑性决定了B样条的局部性，当后续添加路径优化点时，仅影响到定义在此点对应区间上的曲线形状，对曲线的其余部分不产生影响，故选用B样条曲线来平滑A*算法的路径。用$ {B}_{i,k}\text{(}u\text{)} $表示第i个k阶B样条基函数，其定义为
$$ {B}_{i,0}\text{(}u\text{)=}\left\{\begin{array}{l} 1,{u}_{i}\leqslant u\leqslant {u}_{i+1}\\ 0\text{，}其他\end{array}\right.$$ (6) $$ {B}_{i,k}\text{(}u\text{)=}\dfrac{u-{u}_{i}}{{u}_{i+k}-{u}_{i}}{B}_{i,k-1}\text{(}u\text{)+}\dfrac{{u}_{i+k+1}-u}{{u}_{i+k+1}-{u}_{i+1}}{B}_{i+1,k-1}(u),\;\;k\geqslant 1 $$ (7) 式中：k为基函数阶数；u为自变量；$ {u}_{i} $为一组被称为节点矢量的非递减序列的连续变化值。
当生成曲线的控制点个数为n+1时，$ {u}_{i} $所有的取值组成的序列如下：
$$ {u_i} \in \left\{ {{u_0},{u_1},\cdots,{u_k},{u_{k + 1}},\cdots,{u_n},{u_{n + 1}},\cdots,{u_{n + k}}} \right\} $$ (8) 给予10个路径点作为曲线的控制点，进行B样条平滑，优化效果如图6a所示；在原有控制点的基础上加入新的控制点（绿色六边形）时，B样条对曲线平滑的处理效果如图6b所示。
由图6可知，将控制点添加在第5、6点之间，仅影响该路段的曲线形状，且能保证曲线的平滑度，对其他路段并未产生显著影响，该性质大大减小了计算量，为后续通行交叉口路径规划的研究提供了重要的改进方案。
2. 交叉口点云数据处理
2.1 RANSAC拟合算法
使用随机样本一致算法（Random Sample Consensus，RANSAC）[24]构建巷道交叉口处的几何模型，以便收集巷道交叉口关键点信息，用于优化针对巷道交叉口的路径规划。
在RANSAC算法中，首先随机选择一定数量的采样点来进行模型参数估计和假设验证。之后，通过计算每个点与当前模型的拟合误差，选取与当前模型最近的一些点作为新的候选采样点，并再次进行模型参数估计和假设验证。RANSAC算法会反复执行上述步骤，直到达到预设的循环次数或满足停止条件。
RANSAC算法的基本步骤如下：
1）从数据集中随机选择一组样本点，计算对应的模型参数；
2）遍历所有未被选中的数据点，计算其与模型的误差。对于拟合误差小于给定阈值的点，标记为内点，否则标记为外点；
3）如果当前模型满足拟合标准，则将当前模型作为最终模型；
4）否则，将重新构建模型，并更新模型参数；
5）重复步骤2）至4），直到达到预设的迭代次数或满足评价拟合要求；
6）选定当前最优模型；
7）输出最优模型，以及相关的内点和外点。
RANSAC算法拟合效果如图7所示，该模型可以基本描述巷道壁面的几何特征。部分巷道拐角处为弧线，所以使用传感器（激光雷达）收集巷道壁面的数据点时，需确保数据点足够覆盖需要拟合的直线区域。RANSAC算法有很多参数需要调整，例如迭代次数、内点阈值等。通过调整参数，可以使算法更适应巷道点云数据和问题获得更好的拟合效果。
2.2 局部关键点信息提取
在检测到路口时，需要提取路口角点的坐标。只对可能与直角相关的点云数据簇进行多次RANSAC计算。首先对直角区域的第1条直线模型进行RANSAC拟合，获取模型的参数；接着对第2条直线模型进行RANSAC计算，并通过计算2条直线模型的交点来判断路口角点的相对坐标；最后，利用路口角点信息估计路口中心点信息。
在求取路口角点的过程中，由于部分巷道拐角处为弧线，直线模型不能完全拟合巷道拐角，导致求解的路口角点并不符合实际环境，将该类点称为伪角点。伪角点虽与巷道转弯处壁面存在一定偏差，但由于其位于两相连巷道壁面的延长线的交点处，不影响路口中心点的定位，而与巷道壁面具有一定距离更能保证机器人的安全行驶。同时，不太大的偏差也限制了机器人对于巷道交叉口位置的错误估计，防止后续优化规划路径时，机器人太过靠近巷道中心干扰其他设备。
针对巷道不同交叉路口下关键点坐标信息提取效果如图8所示。当检测到各交叉口的关键点后，先消除部分与路口角点距离较近的路径点的影响，防止机器人转弯时与巷道壁面发生剐蹭。之后，将路口中心点坐标与待驶入路口的角点坐标中间的一个点作为路径优化点纳入路径规划算法，再使用B样条平滑曲线。此外，降低路口中心点对机器人的影响，防止过于靠近路口中心，干扰其他移动设备正常运行。
几何模型的建立和路口关键点坐标的提取均依托于机器人自身坐标系，通过这些信息来准确描述机器人与巷道结构特征在空间中的相对位置关系，从而通过几何模型和特征点匹配进行定位优化。
3. 势场融合控制
3.1 势场法
煤矿救援机器人通常用于发生事故后的煤矿环境，条件比较恶劣，仅依靠全局路径规划算法无法在复杂度较高的非结构化环境中安全通行。因此，还需使用局部路径规划算法实时处理局部信息，增强机器人的适应能力，防止复杂工况干扰。
势场法[25]是一种常用的机器人路径规划技术，通过构建反映环境物理特征的势场，实现自主移动和避障等功能。
以机器人实时位置为原点，规划器提供的目标点设为引力点，巷道壁和障碍物的点云数据则设为斥力点，构建一个实时更新的局部势场进行路径规划。通过计算机器人周围环境的力场情况，来确定机器人的运动方向和速度。以最小化机器人运动轨迹上的势能之和作为评价指标，对势场函数中各个势场的权重进行实时调节，从而实现机器人的自主导航和控制，这种动态调整实时局部势场方法为机器人的自主导航和控制提供了一种有效的解决方案。势场函数$ \boldsymbol{U}\left(\boldsymbol{X}\right) $定义为
$$ {\boldsymbol{U}}({\boldsymbol{X}}) = {{\boldsymbol{U}}_{{\mathrm{att}}}}({\boldsymbol{X}}) + {{\boldsymbol{U}}_{{\mathrm{rep}}}}({\boldsymbol{X}}) $$ (9) 式中：$ {{\boldsymbol{U}}_{{\mathrm{att}}}}({\boldsymbol{X}}) $为目标点产生的引力势场；$ {{\boldsymbol{U}}_{{\mathrm{rep}}}}({\boldsymbol{X}}) $为巷道壁和障碍物产生的斥力势场；$ {\boldsymbol{X}} $为移动机器人的当前位置。
人工势场中机器人所受的合力$ \boldsymbol{F}\left(\boldsymbol{X}\right) $为
$$ {\boldsymbol{F}}({\boldsymbol{X}}) = {{\boldsymbol{F}}_{{\mathrm{att}}}}({\boldsymbol{X}}) + {{\boldsymbol{F}}_{{\mathrm{rep}}}}({\boldsymbol{X}}) $$ (10) 式中：$ {{\boldsymbol{F}}_{{\mathrm{att}}}}({\boldsymbol{X}}) $为目标点对移动机器人的引力；$ {{\boldsymbol{F}}_{{\mathrm{rep}}}}({\boldsymbol{X}}) $为巷道壁和障碍物对机器人的斥力。
机器人所受的合力为引力与斥力的矢量和，合力的方向即为机器人运动的方向，势场中机器人受力分析如图9所示。图9中，$ {\boldsymbol{F}}_{{{\mathrm{rep}}}1} $和$ {{\boldsymbol{F}}_{{\mathrm{rep}}2}} $为巷道壁面给予机器人的斥力。
为解决势场法易陷入局部最小值的问题，加入调整系数优化势场分布。两侧巷道壁始终给予机器人以斥力（如图9中$ {{\boldsymbol{F}}_{{\mathrm{rep1}}}} $与$ {{\boldsymbol{F}}_{{\mathrm{rep2}}}} $），根据机器人与巷道壁面的距离数值，设置对应的势场衰减系数，即当机器人距离墙壁较近时，应该设置较大的系数，此时机器人受到巷道壁面给予的斥力也较大。同时，将机器人与巷道壁之间的一定距离内部分区域设置为死区，以防止机器人撞到墙壁或者太过靠近墙面发生剐蹭。根据区域特点和机器人的运动轨迹设置合适的势场参数，不同障碍物的影响范围不同，不同宽度的巷道对机器人影响也不同，保证机器人能够安全、高效地执行搜救任务。
当机器人驶入巷道交叉口时，交叉路口的各关键点会构建新的叠加势场，路口拐点与路口中心点均构建斥力势场，将所有的势场进行求和，得到一个综合势场。这个综合势场反映了所有目标和障碍物对机器人运动的综合影响，以防止机器人与巷道壁面发生剐蹭，也防止机器人通过路口时太过靠近路口中心导致干扰其他设备正常行驶。
仿真验证势场对机器人运动轨迹的影响，初始化参数设置见表3。图10为MATLAB仿真模拟势场对机器人轨迹的影响。考虑到发生事故后环境和关键交叉点的存在，基于势场的导航方法引导机器人从起始位置向预期目的地移动，确保机器人在特定环境中安全高效地运动。
表 3 势场初始化参数Table 3. Initialization parameters of potential field参数 数值 引力增益系数 5 斥力增益系数 1 边界斥力增益系数 100 障碍物影响范围/m 5 壁面附近死区范围/m 0.5 初始线速度/(m·s−1) 1 初始角速度/(rad·s−1) 0 最大循环迭代次数 200 3.2 粒子群优化PID控制
机器人通过PID（Proportion Integral Differential）控制器实现轨迹控制。PID控制器是整个导航系统轨迹控制的基础，由比例项、积分项和微分项组成，通过测量系统输出与期望值之间的误差来调节控制器的控制信号，从而达到稳定控制的目的。
使用粒子群算法（Particle Swarm Optimization，PSO）[26]对PID控制器进行优化，可以帮助轮式机器人更好地跟踪和控制路径。相比于传统PID算法，基于PSO的PID控制算法具有更高的优化精度。传统PID控制算法中的参数需通过试错方法反复调节，而基于PSO算法的PID控制器能够自适应地搜索最优的参数组合，从而获得更高的优化精度。并且传统PID控制算法在面对系统噪声或干扰时容易出现不稳定的情况，但基于PSO算法的PID控制器能够通过粒子群群体的协作优化，获取更鲁棒的控制能力。PSO算法的快速全局搜索特性使得基于PSO的PID控制器能够更快地收敛到最优解，从而让系统响应更加迅速。并且PSO算法能够对不同类型的控制问题进行全面优化，尤其对于非线性、时变、多变量控制问题，基于PSO算法的PID控制器表现得更为出色。
图11为传统PID与基于PSO的PID算法控制对比，可以看出，传统PID算法变化更快，收敛时振动相对较大，并不适用于对安全稳定性要求较高的煤矿机器人，而基于PSO的PID算法能够更稳定地收敛到最优解，使得机器人运动控制更具鲁棒性。
4. 仿真分析与试验验证
4.1 ROS仿真验证
4.1.1 搭建仿真环境
为验证算法的可行性，利用ROS操作系统的Gazebo物理仿真平台进行仿真验证。路口仿真环境搭建如图12所示，差速移动机器人仿真模型如图12d所示。操作系统平台为Ubuntu18.04 LTS 64-bit，搭载了Melodic版ROS进行机器人控制和数据处理。
仿真使用轮式行走的差速移动机器人，采用ROS内置电机驱动，机器人长宽均为70 cm。为模拟真实巷道的路口环境，设置巷道宽度为5 m，高度4 m，调整Gazebo环境光照亮度，模拟黑暗等恶劣条件。为便于观察，将巷道设置为透明，并将激光雷达射线可视化。
4.1.2 仿真结果及分析
首先对改进A*算法进行全局路径规划试验，之后检测算法对于巷道交叉口信息的提取情况，最后在灾变后的2种场景，对改进A*算法规划路径进行人工势场的干预仿真。图13为针对各种路口的特征点信息的提取；图14为传统A*算法针对井下各种路口规划的路径；图15为改进A*算法针对井下各路口规划的路径，利用rviz可视化界面进行效果对比。
煤矿发生事故后，可能导致环境发生变化，环境复杂度增大，这时需通过局部路径规划算法进行调整机器人的行进路线。局部路径规划算法在2种场景下对机器人行进路线的影响如图16所示。图16中，巷道环境已因灾变发生变化，例如因冲击地压导致侧壁塌陷、冒顶片帮等灾害，由此产生的障碍物（场景1）或巷道变形（场景2）等变化，形成不可通行区域。红线为在环境变化后，局部路径规划算法驱使机器人的规划路线。
改进的路径规划算法在搜寻最优路径和提取关键交叉点坐标方面表现出较高的准确性和稳定性。此外，该算法对环境具有很强的适应性，可实现实时控制，使机器人安全通过巷道交叉口，有效提高了导航系统的性能和可靠性。
4.2 试验验证
4.2.1 搭建试验环境
模拟巷道路口试验环境搭建如图17 所示。验证算法的实体机器人（图17b） 内置酷睿i5工控机电脑和 ARM 控制主板，软件环境为 Ubuntu16.04 LTS 64-bit 操作系统平台，使用 C++代码编程实现算法的各项功能。机器人左右两侧的驱动轮分别由2个电机实现差速驱动，由可充电电池提供能源，无需连接复杂的电源线或管道，具有更高的灵活性和便携性；设置4个从动万向轮，以增强机器人稳定性；激光雷达为二维激光雷达，型号为 LSO−50，置于机器人底盘和平台之间；棱镜放置在伸缩杆上，以便全站仪的测量。
4.2.2 试验结果及分析
图18为针对各交叉路口的关键点信息提取，在针对不同交叉路口时，算法可以准确提取相应的路口关键点坐标，为后续路径优化以及势场控制提供关键信息。
针对各交叉路口的路径规划方法，将提取到的交叉路口特征点纳入路径规划系统，再使用B样条平滑路径曲线，既防止机器人转弯时与巷道拐角发生剐蹭，又防止太过靠近路口中心，提高了运动安全性。改进后A*算法针对不同路口的路径规划效果如图19所示。
改进的A*算法在环境建模、路径规划和控制策略等多方面进行了优化，并与巷道搜救机器人的特定需求和环境进行了深度结合，可以适应复杂交叉口环境。
5. 结 论
1）通过使用分层邻域搜索与剪枝策略改进邻域搜索方法，减少了搜索空间，提高了搜索效率；通过优化代价函数以更好地平衡实际代价和启发式代价的影响，让A*算法更准确地评估每个节点的代价；利用B样条方法对路径进行平滑处理，降低路径跟踪的难度。
2）使用RANSAC算法拟合巷道交叉口壁面轮廓，建立二维几何模型，不完全依赖于已知地图，以便实时收集巷道交叉口的特征点信息，针对规划的路径点进行优化。而B样条的局部性可以更好的处理动态更新的路径点，仅影响相应区间的曲线形状，对路径的其余部分不产生影响，极大地提升了路径生成效率。
3）根据构建的环境几何模型和提取的特征点建立综合局部势场，引入调整系数优化势场分布，配合控制器采用基于PSO的PID控制算法实现自主行驶功能，增强机器人针对巷道交叉口通行的适应能力，更好地实现搜救功能。
-
![]()
图 2 Dijkstra与A*算法路径规划效果对比
Figure 2. Path planning comparison of Dijkstra algorithm and A* algorithm
![]()
图 5 传统A*与优化A*算法规划路径对比
Figure 5. Path planning comparison of A* algorithm and improved A* algorithm
表 1 算法性能对比
Table 1 Algorithm performance comparison
算法 Dijkstra算法 A*算法 搜索节点数量 632 363 路径点数量 34 34 路径长度 41.698 5 41.698 5 搜索时间/s 0.835 1 0.610 9 
下载: 导出CSV
表 2 A*算法性能对比
Table 2 A* algorithm performance comparison
算法 传统A*算法 优化A*算法 搜索节点数量 555 266 路径点数量 44 38 路径长度 55.012 2 55.066 2 搜索时间/s 0.932 1 0.601 6
下载: 导出CSV
表 3 势场初始化参数
Table 3 Initialization parameters of potential field
参数 数值 引力增益系数 5 斥力增益系数 1 边界斥力增益系数 100 障碍物影响范围/m 5 壁面附近死区范围/m 0.5 初始线速度/(m·s−1) 1 初始角速度/(rad·s−1) 0 最大循环迭代次数 200
下载: 导出CSV
-
[1] 谢和平. 深部岩体力学与开采理论研究进展[J]. 煤炭学报,2019,44(5):1283−1305. XIE Heping. Research review of the state key research development program of China:deep rock mechanics and mining theory[J]. Journal of China Coal Society,2019,44(5):1283−1305.
[2] 张吉雄,鞠杨,张强,等. 矿山生态环境低损害开采体系与方法[J]. 采矿与岩层控制工程学报,2019,1(1):013515. ZHANG Jixiong,JU Yang,ZHANG Qiang,et al. Low ecological environment damage technology and method in coal mines[J]. Journal of Mining and Strata Control Engineering,2019,1( 1):013515.
[3] 李猛,张吉雄,邓雪杰,等. 含水层下固体充填保水开采方法与应用[J]. 煤炭学报,2017,42(1):127−133. LI Meng,ZHANG Jixiong,DENG Xuejie,et al. Method of water protection based on solid backfill mining under water bearing strata and its application[J]. Journal of China Coal Society,2017,42(1):127−133.
[4] 李达. 厚煤层残煤复采区掘进大巷安全影响因素分析及评价[J]. 煤炭工程,2023,55(7):115−119. LI Da. Analysis and evaluation of safety influencing factors for excavation of main roadway in thick coal seam remining area[J]. Coal Engineering,2023,55(7):115−119.
[5] 石宏民. 煤矿巷道履带式巡检机器人系统设计[J]. 机电工程技术,2022,51(6):112−115. doi: 10.3969/j.issn.1009-9492.2022.06.026 SHI Hongmin. Design of track inspection robot system in coal mine roadway[J]. Mechanical & Electrical Engineering Technology,2022,51(6):112−115. doi: 10.3969/j.issn.1009-9492.2022.06.026
[6] 李森,王峰,刘帅,等. 综采工作面巡检机器人关键技术研究[J]. 煤炭科学技术,2020,48(7):218−225. LI Sen,WANG Feng,LIU Shuai,et al. Study on key technology of patrol robots for fully-mechanized mining face[J]. Coal Science and Technology,2020,48(7):218−225.
[7] 聂珍,马宏伟. 煤矿巡检机器人巷道气体环境智能检测系统设计[J]. 工矿自动化,2020,46(6):17−22. NIE Zhen,MA Hongwei. Design of intelligent detection system of gas environment in roadway of coal mine inspection robot[J]. Industry and Mine Automation,2020,46(6):17−22.
[8] 武俊. 煤矿机器人关键共性技术与发展探讨[J]. 工矿自动化,2023,49(S1):1−3. WU Jun. Discussion on key common technologies and development of coal mine robots[J]. Industry and Mine Automation,2023,49(S1):1−3.
[9] 刘庆运,杨华阳,刘涛,等. 基于激光雷达与深度相机融合的SLAM算法[J]. 农业机械学报,2023,54(11):29−38. doi: 10.6041/j.issn.1000-1298.2023.11.003 LIU Qingyun,YANG Huayang,LIU Tao,et al. SLAM algorithm based on fusion of Lidar and depth camera[J]. Transactions of the Chinese Society for Agricultural Machinery,2023,54(11):29−38. doi: 10.6041/j.issn.1000-1298.2023.11.003
[10] 杨春雨,张鑫. 煤矿机器人环境感知与路径规划关键技术[J]. 煤炭学报,2022,47(7):2844−2872. YANG Chunyu,ZHANG Xin. Key technologies of coal mine robots for environment perception and path planning[J]. Journal of China Coal Society,2022,47(7):2844−2872.
[11] WANG H,LI C,LIANG W,et al. Adaptive autonomous navigation system for coal mine inspection robots:overcoming intersection challenges[J/OL]. Industrial Robot,2024(2024−06−24)[2024−07−19]. https://doi.org/10.1108/IR-11-2023-0295.
[12] 张捍东,郑睿,岑豫皖. 移动机器人路径规划技术的现状与展望[J]. 系统仿真学报,2005,17(2):439−443. doi: 10.3969/j.issn.1004-731X.2005.02.050 ZHANG Handong,ZHENG Rui,CEN Yuwan. Present situation and future development of mobile robot path planning technology[J]. Journal of System Simulation,2005,17(2):439−443. doi: 10.3969/j.issn.1004-731X.2005.02.050
[13] PATLE B K,PANDEY A,PARHI D R K,et al. A review:on path planning strategies for navigation of mobile robot[J]. Defence Technology,2019,15(4):582−606. doi: 10.1016/j.dt.2019.04.011
[14] 丁百川. 我国煤矿主要灾害事故特点及防治对策[J]. 煤炭科学技术,2017,45(5):109−114. DING Baichuan. Features and prevention countermeasures of major disasters occurred in China coal mine[J]. Coal Science and Technology,2017,45(5):109−114.
[15] 文志杰,景所林,宋振骐,等. 采场空间结构模型及相关动力灾害控制研究[J]. 煤炭科学技术,2019,47(1):52−61. WEN Zhijie,JING Suolin,SONG Zhenqi,et al. Study on coal face spatial structure model and control related dynamic disasters[J]. Coal Science and Technology,2019,47(1):52−61.
[16] 马旭伟,徐华龙. 基于策略的矿井水灾避灾路径动态规划[J]. 矿冶,2023,32(4):12−18. doi: 10.3969/j.issn.1005-7854.2023.04.003 MA Xuwei,XU Hualong. Dynamic planning of mine water avoidance path based on strategy[J]. Mining and Metallurgy,2023,32(4):12−18. doi: 10.3969/j.issn.1005-7854.2023.04.003
[17] 马小陆,梅宏. 基于改进势场蚁群算法的移动机器人全局路径规划[J]. 机械工程学报,2021,57(1):19−27. doi: 10.3901/JME.2021.01.019 MA Xiaolu,MEI Hong. Mobile robot global path planning based on improved ant colony system algorithm with potential field[J]. Journal of Mechanical Engineering,2021,57(1):19−27. doi: 10.3901/JME.2021.01.019
[18] 鲍久圣,张牧野,葛世荣,等. 基于改进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.
[19] GAO Y,DAI Z,YUAN J. A multiobjective hybrid optimization algorithm for path planning of coal mine patrol robot[J/OL]. Computational Intelligence and Neuroscience,2022:9094572(2022−6−23)[2023−11−15]. https://doi.org/10.1155/2022/9094572.
[20] 林韩熙,向丹,欧阳剑,等. 移动机器人路径规划算法的研究综述[J]. 计算机工程与应用,2021,57(18):38−48. doi: 10.3778/j.issn.1002-8331.2103-0519 LIN Hanxi,XIANG Dan,OUYANG Jian,et al. Review of path planning algorithms for mobile robots[J]. Computer Engineering and Applications,2021,57(18):38−48. doi: 10.3778/j.issn.1002-8331.2103-0519
[21] 栾添添,王皓,孙明晓,等. 基于动态变采样区域RRT的无人车路径规划[J]. 控制与决策,2023,38(6):1721−1729. LUAN Tiantian,WANG Hao,SUN Mingxiao,et al. Path planning of unmanned vehicle based on dynamic variable sampling area RRT[J]. Control and Decision,2023,38(6):1721−1729.
[22] 王洪斌,尹鹏衡,郑维,等. 基于改进的A*算法与动态窗口法的移动机器人路径规划[J]. 机器人,2020,42(3):346−353. WANG Hongbin,YIN Pengheng,ZHENG Wei,et al. Mobile robot path planning based on improved A* algorithm and dynamic window method[J]. Robot,2020,42(3):346−353.
[23] 刘成志,韩旭里,李军成. 三次均匀B样条扩展曲线的渐进迭代逼近法[J]. 计算机辅助设计与图形学学报,2019,31(6):899−910. LIU Chengzhi,HAN Xuli,LI Juncheng. Progressive-Iterative approximation by extension of cubic uniform B-spline curves[J]. Journal of Computer-Aided Design & Computer Graphics,2019,31(6):899−910.
[24] RAGURAM R,CHUM O,POLLEFEYS M,et al. USAC:A universal framework for random sample consensus[J]. IEEE transactions on pattern analysis and machine intelligence,2012,35(8):2022−2038.
[25] MACT T,COPOT C,TRAN D T,et al. Heuristic approaches in robot path planning:a survey[J]. Robotics and Autonomous Systems,2016,86:13−28. doi: 10.1016/j.robot.2016.08.001
[26] ZHANG Y,WANG S,JI G. A comprehensive survey on particle swarm optimization algorithm and its applications[J/OL]. Mathematical problems in engineering,2015:931256(2015−10−07)[2023−11−15]. https://doi.org/10.1155/2015/931256.
-
期刊类型引用(1)
1. 赵克顿，赵洪华，陈志良，赵建，赵丰瑞，韩青，张荣成，周涛，李钰茹. 矿用多功能机器人设计与应用研究. 煤炭工程. 2025(01): 212-217 . 
百度学术
其他类型引用(1)
计量
- 文章访问数: 127
- HTML全文浏览量: 39
- PDF下载量: 53
- 被引次数: 2
