矿井救援机器人的B 样条-APF-BRRT 路径规划方法

姜媛媛 , ，李宏伟，路子佩

（1.安徽理工大学 电气与信息工程学院，安徽淮南 232000；2.安徽理工大学 环境友好材料与职业健康研究院（芜湖），安徽芜湖 241003）

我国煤炭资源储量丰富，煤炭开采人员众多，一旦发生安全事故，煤矿井下环境就会十分危险甚至危及开采人员生命。随着煤矿智能化的应用，越来越多的矿井救援机器人能够代替救援人员迅速的进入复杂危险的灾后现场，完成灾后矿井环境探测和人员搜救任务[1-2]。灾后矿井环境较为复杂，存在无规则的甚至是动态的障碍物。救援机器人在形状狭长的矿井环境中的路径规划是其完成救援任务的关键。此外，大量移动机器人路径规划方面的算法被提出，如快速扩展随机树（Rapidly-exploring random trees，RRT）算法、A*算法、Dijkstra 算法、遗传算法、神经网络算法等[3-6]。其中RRT 算法及其变体[7]最具代表性，广泛应用于各种机器人类型、各种复杂约束下的移动机器人运动规划。

国内外基于RRT 算法研究较多。李兆强等采用优化重新父节点、采取剪枝策略、引入自适应步长，改善采样方式、改进了传统RRT 算法，减少了算法的耗时与路径长度[8]。姜媛媛等在RRT 算法的基础之上，引入了路径光滑算法，能够改善原始算法路径冗余点过多、路径转折点较多等问题[9]。Kuffner和Lavalle 提出了RRT-Connect 算法，该算法基于RRT 算法引入了双向扩展的策略，从起点和目标点分别生成双向快速拓展随机树，在搜索地图时效率更高。赵雷等优化了Kuffner 提出的RRT-Connect算法，去除冗余路径节点时使用二分法，且增加了平滑策略，减少路径中的转折点[10]。Reza 提出Informed RRT * -Connect 算法，与RRT * -Connect 算法不同，该方案在BRRT 的基础上对环境地图进行采样，迭代次数更少，极大的降低了成本[11]。辛书等提出了局部引导多Bi-RRT（LGM-BRRT）方法，可以结合改进的新颖搜索策略来提供快速解决方案。与BRRT 算法相比，该算法易于实现、内存利用率高、能够使救援机器人快速有效地避开危险区域[12]。刘奥博等针对BRRT 算法在路径规划时，存在的采样效率较为低下等问题，提出GBB-RRT 算法（目标偏置的双向快速扩展随机树），可以加快扩展的速度，优化采样点的选择[13]。上述这些算法都是对传统的RRT 算法及其变体进行改进和研究。在一些常规的环境中使用传统地算法可以比较快速地实现路径规划，但未引入环境信息，对于可能出现无规则甚至动态障碍物的矿井环境中，直接使用传统路径规划方法的可操作性较低，救援机器人在所规划路径中的运动性能较差。例如，传统的RRT 算法规划的路径会产生大量的冗余节点的树，规划出来的路径易存在接近障碍物的情况，极大可能导致发生碰撞。为此，面向复杂的矿井环境的路径规划方法，需要提高其对动态环境的适应能力，应结合环境感知信息实现无规则甚至动态避障。以实现对复杂矿井环境中的救援机器人的移动控制。

本文提出的B 样条-APF-BRRT 矿井救援机器人的路径规划方法，在传统的BRRT 算法中加入APF 算法的目标引力思想，APF 算法适用于部分环境信息已知的动态环境下的路径规划，其目标引力会对双向随机搜索树产生导向性，可以减少BRRT 算法中大量的冗余节点的树。然后再通过Douglas-Peuker 算法进行分线段处理，提取出关键节点，最后对提取出来的关键节点进行B 样条拟合，从而优化转折次数和路径长度。实验表明，基于B 样条函数优化的APF-BRRT 算法，具有更好地在复杂环境下路径规划效果。

1 矿井环境特点及对路径规划的影响

救援机器人的路径规划，最重要的是要考虑到救援机器人的运动路径所经过区域的环境因素影响，这样才能有效地解决救援机器人的通行问题，使救援机器人快速高效地到达目标位置，实施救援工作。煤矿井下救援所处环境为复杂的非结构化环境，主要体现在：

1）正常开采的煤矿内充斥着各种采矿所需要的机械设备、施工材料等。当矿井发生瓦斯爆炸等灾害事故后，爆炸冲击波会对矿井环境造成巨大破坏，导致顶部的支撑物倾塌、煤块和施工材料散落、机械设备翻倒。这些会形成很多形状各异的障碍物[14]。

2）煤矿井下环境存在着大规模、多分支甚至特别狭长的几何形状的地形结构，其巷道狭窄且深，距离极长并且走向崎岖、位置复杂，由于地面是顺山脉开挖而成，陡峭的台阶、泥泞的地面、大小不一的水坑随处可见。

因此救援机器人在矿井环境中开展救援工作较为困难。矿井救援机器人需要结合环境信息，能够避开无规则的甚至是动态的障碍物，以较为平滑的运动路径在形状狭长的矿井巷道结构中顺利地完成救援任务。

2 BRRT 算法和APF 算法

2.1 BRRT 算法

BRRT 算法的全称是双向快速拓展随机树算法[15]。其搜索的方法是从初始状态点建立一棵快速随机搜索树，目标状态点也同样建立一棵快速随机搜索树，从两点同时出发，其搜索空间的效率比传统的RRT 算法更高，如图1 所示。

图1 BRRT 算法规划示意图Fig.1 Schematic diagram of BRRT algorithm planning

BRRT 算法可以用于复杂环境下矿井下救援机器人的路径规划，具体步骤如下：

步骤1 初始化，从初始状态点建立一棵随机树T1，从目标状态点建立一棵随机树T2。T1和T2从不同的方向往对方的区域延伸。

步骤2 在空间内分别选择不同的随机点Qrand1和Qrand2，若Qrand1和Qrand2都未在障碍物的区域内产生，则在随机树T1和T2分别找到随机点的最近节点Qclosest1和Qclosest2。

步骤3 分别连接随机点Qrand1和最近的节点Qclosest1，随机点Qrand2和最近的节点Qclosest2，检测其连接线是否会发生碰撞。若不会，则分别比较其与扩展步长距离，若Qrand和Qclosest的距离小于扩展步长，即在扩展树上增加随机点，若大于，则取和扩展步长长度一样的点Qnew增添到扩展树上，再取随机点增加到随机树上。若随机节点和最近节点的连线上发生碰撞，则返回步骤2，重新运行。

步骤4 在两棵随机树上分别重新布线，分别重新选择父节点。

步骤5 判断T1和T2距离阈值，若两棵扩展树距离大于阈值，则新执行步骤2～步骤4。若小于阈值，在相遇的T1和T2节点上反向取初始状态点和目标状态点，进行路径保存。

步骤6 读出BRRT 算法所规划的路径。

2.2 APF 算法

APF 算法全称是人工势场法，是由Khatib 提出一种比较常用的局部路径规划方法[16]。其基本思想是设计一种抽象的引力场和斥力场，使灾后矿井救援机器人在抽象的人造力场的合力下产生运动，目标点对救援机器人产生的作用是Fatt，障碍物对救援机器人产生的作用是Frep，由于其结构和原理简单，应用较为广泛。如图，Ftot是Fatt和Frep的合力，由Ftot控制救援机器人在地图中产生运动，如图2 所示。

图2 基于APF 的移动机器人受力示意图Fig.2 Schematic diagram of forces exerted by mobile robot based on APF

APF 算法数学描述如下：设机器人位置为Q=（x,y），目标点位置为Qgoal=（xgoal，ygoal），目标在地图中对移动机器人有着吸引作用，且其距离越远，产生吸引力越大，反之相反。

机器人与目标间的引力场函数定义为

式中：ρ0为地图中障碍物的影响距离；ρ（Q，Qobs）为机器人和地图障碍物之间的距离；Krep为正比例函数；Qobs=（xobs，yobs）为地图中障碍物的位置。

机器人在地图中受到的合力为

3 B 样条-APF-BRRT 路径规划方法

3.1 APF-BRRT 算法

原始的BRRT 算法在路径规划中出现路径延伸的随机性强、扩展随机树难以平滑的连接、规划路径避开障碍物的影响区域困难等问题。该算法由于随机的选取参考节点，然后在参考节点上进行新节点的延伸，导致规划的路径曲折较多，甚至产生较大的转角。并且Qrand和Qnew的选取是没有导向性的，随机选择，从而导致搜索效率较为低下，路径规划的收敛时间较长[17-18]。而APF 算法作为一种结构简单的启发式算法，在部分环境信息已经知道的动态规划中具有优良的效果，规划的路径既平滑又安全。具有很好的运动轨迹实时控制的性能。但其也易陷入局部最优问题，因此APF 算法只适合解决局部避障，对于全局地图的某些地方，其受到合力容易导致震荡或者停滞不前。

本文基于APF 算法引导的BRRT 路径规划算法，将APF 算法中的人工势场引入到BRRT 中，可以提高原始BRRT 路径规划算法的运行效率。根据目标点和周围障碍物信息在原始BRRT 中构建人工势场，在原始BRRT 算法中加入APF 算法的目标引力思想，然后针对其中第t个扩展出来的节点，构建基于BRRT 算法的目标引力函数，引导BRRT 算法进行路径规划搜索，使扩展随机树能够通过导向性以较为平滑的运动趋势朝规划的目标点生长，可以更加高效的躲避障碍物，降低搜索无效性，从而快速搜索到到达目标的安全路径。

构建的BRRT 目标引力函数Ht为

其具体流程如下：

1）对RRT 算法进行初始化。设置起始和目标点位置，把随机树在地图中初始化；给出相关参数：如机器人半径、引力场系数、斥力场系数、最大的迭代次数、扩展步长等。

2）根据目标点和周围障碍物信息，引入APF 算法，对环境进行预处理。

3）构建目标引力函数Ht，根据各点的合力Ftot的大小,从而选择新的采样点Qrand。

4）对随机树进行连续的延伸，将两棵随机树生成的节点记录到数组RRT1 和RRT2 中。当随机树生成的新节点Qnew到Qrand的距离小于给定的阈值，认为随机树找到路径，直到两棵树都停止扩展。

5）通过回溯找到完整的路径。从Qnew回溯到起点和目标点，找到随机扩展树的父节点，并将路径存储在数组中。

3.2 B 样条函数优化的APF-BRRT 算法

B 样条函数是一种有效的路径优化方法，在路径规划中得到了广泛的应用，使用B 样条函数拟合路径可以重建局部路径，不需要改变整个路径就可以平滑整体路径，并且缓解路径振荡问题。作为一种最常用的样条曲线，可以看作是贝塞尔曲线的推广。B 样条函数具有许多特性，例如几何不变性、凹凸性等[19]。加入B 样条函数优化后的井下救援机器人路径方法能够避免其运动路径转折角度大，能够以更加平滑的运动路径在矿井地下结构中顺利地完成救援任务。B 样条函数的基本公式为

式中：di为控制顶点；Ni,m为高阶数是m的一种B 样条的基函数；i=0，1，···，m。基函数是由节点矢量的非 递 减 参 数u的 序 列U:u0,u1,u2, ···,ui+n+1(u0≤u1≤···≤ui+n+1)所决定的n次分段多项式。

递推公式如下：

根据式（8）定义，在确定完其控制顶点之后，便可以确定B 样条函数。本文引入Douglas-Peuker 算法（DP 算法）进行关键节点提取来确定B 样条函数的控制顶点。DP 算法是一种应用较为广泛的曲线简化算法，属于单参数简化，在层次结构、多尺度过程、图像处理和计算几何等许多领域，其都被称为最优算法[20]。利用DP 算法提取关键节点，重新计算合适的路径，本质其实是一种二次重采样的过程，能够将APF-BRRT 算法的可通路径调整为最短路径。样条函数对其提取最短路径进行光滑拟合，曲线次数k=2，节点两端重复度为k+ 1，此时u0=u1=···=uk，un+ 1=un+ 3=···=un+k+ 1，因为内节点分布较为均匀，所以生成曲率连续的光滑路径，作为最优路径输出。B 样条函数拟合曲线如图3 所示。

图3 B 样条拟合曲线示意图Fig.3 Schematic of APF-based mobile robot forces

B 样条-APF-BRRT 算法流程图如图4 所示。

图4 B 样条-APF-BRRT 算法流程图Fig.4 Flowchart of B spline-APF-BRRT algorithm

4 实验结果与实验分析

为了测试本文提出的矿井救援机器人的B 样条-APF-BRRT 路径规划方法的有效性，本实验利用MATLAB R2017a 分别进行了3 组仿真对比实验。

4.1 3 种环境地图仿真模拟（实验一 ）

为验证本文提出的B 样条-APF-BRRT 算法的适用性，考虑到矿井存在无规则的复杂障碍物及狭窄巷道的特点，用大小不同的矩形和圆形块代表无规则的障碍物，并根据矿井环境中障碍物的分布情况，分别建立了简单环境地图、复杂环境地图以及狭窄巷道环境地图进行路径规划仿真实验，3 种环境地图如图5 ～ 图7 所示。地图大小均为500 m×500 m，地图中黑色块代表障碍物。

图5 简单环境下B 样条-APF-BRRT 运动路径Fig.5 B-spline-APF-BRRT motion path in a simple environment

本实验所提算法扩展步长为15，最大迭代次数为5 000，距离阈值设为30，引力系数为1，斥力系数为20。同时，对地图进行膨胀处理，其膨胀尺寸为机器人尺寸大小。利用本文所提算法进行20 次仿真实验，取其平均值作为算法性能指标，如表1 所示。从20 次实验所得路径中任意选择一条，最终如图5 中蓝色实线所示。图5 中绿色和黄色线条表示BRRT-APF 算法中从起点和目标点向对方延伸的路径，红色带星线条表示BRRT-APF 算法起点和目标点连接起来的路径，黑色带圈虚线表示DP 算法提取的关键节点的路径，蓝色实线表示B 样条函数拟合的最终输出路径。

表1 3 种算法在不同环境下的性能测试对比结果Tab.1 Comparison results of three algorithms′ performance tests in different environments

实验一中图5 是在简单矿井环境下实现救援机器人的路径规划，救援机器人起点为（30，30），目标点为（430，430）。可以看出，BRRT 算法在简单环境中，生成的路径弯弯曲曲，有较多的转折点，路径不够简洁。本文算法生成的路径如表1 中平均测试结果，其提取的关键节点为9 个，转折次数为7 次，路径长度减少了4.07%，可减少救援机器人在矿井环境中的耗能。能够体现B 样条-APF-BRRT 算法在简单环境运行效果较好。

图6 和图7 是针对在矿井中无规则的复杂障碍物和矿井中存在着特别狭长的巷道环境的建筑物所搭建的。复杂环境地图中，救援机器人起点为（30，30），目标点为（430，430）。BRRT 算法产生的路径如图4 所示，产生了较多冗余节点的树，而加入APF 算法之后，其路径明显减少了冗余节点。在表1 通过分析仿真实验的实验数据，可以得出B 样条-APF-BRRT 算法比原始的BRRT 算法节点数减少了67.64%，路径长度优化了6.08%。在狭窄巷道中，对于矿下灾后救援机器人路径规划是一个挑战，救援机器人起点为（30，30），目标点为（430，430）由图7 所示，本文提出的算法路径较原始算法明显平滑了很多，且由表1 中数据可以看出其转折次数减少到9 次，路径长度减少了7.73%。图6 和图7 实验结果表明本文提出的B 样条-APF-BRRT 路径规划方法能够避开无规则的障碍物，以更加平滑的运动路径在形状狭长的巷道环境顺利地完成救援任务。证明本文提出的B-APF-BRRT 路径规划方法对这两种环境较为适用。

图6 复杂环境下B 样条-APF-BRRT 运动路径Fig.6 B-spline-APF-BRRT motion paths in complex environments

图7 狭窄巷道环境下B 样条-APF-BRRT 运动路径Fig.7 B-spline-APF-BRRT motion path in narrow alley environment

4.2 相同环境下算法改进对比（实验二 ）

为验证本文提出的B 样条-APF-BRRT 算法的有效性，实验二在相同矿井环境中把未加入APF 的算法的与B 样条-APF-BRRT 算法进行对比，其他描述与实验一相同。

为了对比在矿井环境中救援机器人加入APF后算法性能的影响，针对相同的矿井环境，搭建地图。救援机器人起点为（30，30），目标点为（430，430），其结果如图8 和图9 所示，经过多次仿真实验，得到表2 中的实验数据，使用B-BRRT 算法运行出来的路径长度为730.54 m，其节点总数为11 个，转折次数为9 次，而B-APF-BRRT 算法的运动路径较未加入APF 前优化了7.68%，其节点总数较少为9 个，转折次数减少到7 次。可以看出在矿井环境下救援机器人加入APF 算法后运动轨迹控制的性能更加良好。证明了引入目标引力思想后本文算法的有效性。

表2 有无APF 算法性能测试结果对比Tab.2 Comparison of performance test results with and without APF algorithm

图9 简单环境下B 样条-BRRT 运动路径Fig.9 B-spline-BRRT motion paths in simple environments

4.3 相同环境下传统算法对比（实验三）

为验证本文提出的B 样条-APF-BRRT 算法的优越性，实验三把本文所提出的B 样条-APFBRRT 算法与A*算法、GA 算法、RRT 算法在一个中等障碍物的地图环境中进行对比实验。救援机器人路径规划起点为（20，10），目标点为（430，350），路径如图10 所示。由表3中实验数据可知性能对比较为明显，从路径转折次数来看，RRT 算法的转折次数远远超过B 样条-APF-BRRT 算法、A*算法和GA 算法，GA 算法转折次数最少；从路径规划时间来看，B 样条-APF-BRRT 算法的规划时间最为短暂， RRT 次之，A*和GA 算法进行路径规划所花费时间较长；从路径规划的长度来看，本文提出的B 样条-APF-BRRT 算法运行出来的路径长度与A*算法相对来说较为优越，在600 m 左右，GA 算法、RRT 算法这两者运行的路径长度在相同的地图中都超过了700 m，和前两者相比略长。由图10d）可知：RRT 算法路径节点冗余较多，路径质量较差，综合以上路径长度、运行时间、转折次数等可以看出来本文所提出的B 样条-APF-BRRT 算法性能具有优越性。

表3 本文算法与传统算法性能测试对比结果Tab.3 Comparison of the performance test results of this paper′s algorithm and the traditional algorithm

图10 4 种算法对比实验结果Fig.10 Comparing experimental results on four algorithms

由表3中实验数据可知性能对比较为明显，从路径转折次数来看，RRT 算法的转折次数远远超过B 样条-APF-BRRT 算法、A*算法和GA 算法，GA算法转折次数最少；从路径规划时间来看，B 样条-APF-BRRT 算法的规划时间最为短暂， RRT 次之，A*和GA 算法进行路径规划所花费时间较长；从路径规划的长度来看，本文提出的B 样条-APF-BRRT算法运行出来的路径长度与A*算法相对来说较为优越，在600 m 左右，GA 算法、RRT 算法这两者运行的路径长度在相同的地图中都超过了700 m，和前两者相比略长，且由图10d）可知，RRT 算法路径节点冗余较多，路径质量较差，综合以上路径长度、运行时间、转折次数等可以看出来本文所提出的B 样条-APF-BRRT 算法性能具有优越性。

5 结论

结合矿井环境中存在无规则的复杂障碍物及狭窄巷道的路径规划问题，改进了路径规划中BRRT 算法，以达到在矿井环境中减少冗余节点、优化路径长度、平滑路径的效果。通过3 种实验对比，结果表明本文提出的B 样条-APF-BRRT 算法稳定可靠，具有优越的性能。灾后的矿井环境错综复杂，本文所提出的B 样条-APF-BRRT 路径规划方法在MATLAB 搭建的矿井环境中能明显提高救援机器人的路径质量，减少了路径转折次数，优化了路径长度，对于矿井救援机器人实现救援工作具有重要意义。下一步的研究内容是怎样减少矿井环境中救援机器人的运动时间及如何实现矿井环境下救援机器人的动态避障。