CN113485363A - 基于膜计算和rrt的煤矿井下机器人多步长路径规划方法 - Google Patents

Abstract

本发明公开了一种基于膜计算和RRT的煤矿井下机器人多步长路径规划方法，其包括：快速连通阶段和路径寻优阶段：快速连通阶段包括连通初始化、构建多步长膜结构和快速寻路连通，主要功能为快速寻找出从起始点到目标点的一条可行路径；路径寻优阶段包括寻优初始化、构建多采样点膜结构和最短路径优化，主要功能是基于快速连通阶段已经找到的一条可行路径，进行最短路径优化，寻找出从起始点到目标点之间最短可行路径；在煤矿井下狭窄空间中，通过变步长并行膜计算，可以快速的搜索到起始点和目标点之间的可行性路径，利用多采样点并行膜计算，可以迅速得到一条最短可行路径，极大的提高了机器人路径规划的成功率，加快了路径规划时间，提升了路径规划效率，为机器人在煤矿井下自主安全运行提供了基础保证。

Description

基于膜计算和RRT的煤矿井下机器人多步长路径规划方法

技术领域

本发明涉及一种用于煤矿井下的基于膜计算和RRT的机器人多步长路径规划的方法。

背景技术

煤炭资源在我国能源结构中占据主导地位，对国民经济发展起至关重要的作用。随着科技的发展与交叉应用，各类煤机装备自动化和智能化程度越来越高，大幅改善了煤炭行业人员的工作环境和劳动强度，煤炭开采的安全性得到极大提升，但同其他行业相比其安全系数仍然较低。2019年国家煤矿安监局颁布了煤矿机器人重点研发目录，从国家层面指明了煤矿机器人的发展方向，使用机器人代替人工采煤是践行“无人则安”理念的重要途径，煤炭开采“少人化、无人化”已成为大势所趋。我国煤矿运输装备研究起步晚、发展快，计算机、物联网、人工智能等技术在煤矿中的应用，使得煤矿运输装备的机械化和自动化程度逐步提高，对提升煤矿安全、经济效益等起到了良好作用，但距“煤矿运输机器人”目标尚有差距。

移动机器人导航中的路径规划技术从开始研究发展到现在，路径规划问题已经从如何从起点到终点规划出无碰撞的路径变成如何在复杂多变环境中用更高效的方法规划出更合理的路径。为了提高在高维度或复杂状态空间规划的能力，基于采样的路径规划方法被提出。快速探索随机树(RRT)是目前最受青睐的基于采样的规划算法。RRT使用随机采样从而避免在整个状态空间构建图，达到快速规划且是概率完备的。

由于煤矿井下空间狭小，再加上安装摆放了许多生产运输相关设备，使得机器人可以通行的路径更加不规则，有些区域可行空间较大，而有些地方只有很狭窄的通道，传统的RRT路径规划算法需要较多的搜索时间才能寻找到一条可行路径，甚至有些时候由于空间狭小无法找到可行路径造成路径规划失败，这些势必严重影响井下生产的工作效率；由于RRT算法的只是寻找一条从起始点到目标点的可行路径，而这个寻找到的可行路径并不是最优最短路径，在机器人按照这条路径行驶时，也会影响机器人行驶效率，具有一定的安全隐患。

发明内容

针对上述问题，本发明的目的是为了提供一种用于煤矿井下的基于膜计算和RRT的机器人多步长路径规划的方法。在煤矿井下狭窄空间中，通过多步长并行膜计算，快速搜索到起始点和目标点之间的可行性路径，利用多采样点并行膜计算，迅速得到一条最短可行路径，极大的提高了机器人路径规划的成功率，加快了路径规划时间，提升了路径规划效率，为机器人在煤矿井下自主安全运行提供了基础保证。

为了达到上述目的，本发明所采用的技术方案为：

基于膜计算和RRT的煤矿井下机器人多步长路径规划方法，其特征在于，所述方法包括快速连通阶段和路径寻优阶段：所述快速连通阶段包括连通初始化、构建多步长膜结构和快速寻路连通，主要功能为快速寻找出从起始点到目标点的一条可行路径；所述路径寻优阶段包括寻优初始化、构建多采样点膜结构和最短路径优化，主要功能是基于快速连通阶段已经找到的一条可行路径，进行最短路径优化，寻找出从起始点到目标点之间最短可行路径。

进一步的，所述的连通初始化，其特征在于获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为K；设置当前路径点为P_now，设置路径点集合为V，设置到达目标点距离阈值L_target。

进一步的，所述构建多步长膜结构，其特征为，利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将连通初始化过程中获得的区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数K和路径点集合V作为表层膜0内的基本粒子；将区域地图信息Map、目标点P_T、步长Step1、随机采样概率阈值为P_th1、采样点P_smp1和新路径点P_new1作为基本膜1内的基本粒子；将区域地图信息Map、目标点P_T、步长Step2、随机采样概率阈值为P_th2、采样点P_smp2和新路径点P_new2作为基本膜2内的基本粒子；将区域地图信息Map、目标点P_T、步长Step3、随机采样概率阈值为P_th3、采样点P_smp3和新路径点P_new3作为基本膜3内的基本粒子；。

进一步的，所述快速寻路连通，其步骤如下：

(1)设置当前路径点P_now＝P₀，当前迭代次数i＝1，路径点集合V＝{P₀}；

(2)复制当前路径点P_now到3个基本膜内，令P_now1＝P_now、P_now2＝P_now和P_now3＝P_now；分别同时进行基本膜计算，计算过程为(2-1)、(2-2)和(2-3)，其中(2-1)、(2-2)和(2-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：

(2-1)基本膜1在地图Map中随机采样一个点P_rand1，若P_rand1≥P_th1,则基本膜1的采样点P_smp1＝P_rand1；若P_rand1＜P_th1,则P_smp1＝P_T；在当前路径点P_now到采样点P_smp1连接的直线方向上P_now→P_smp1，获取步长为Step1的新路径点P_new1，计算当前路径点P_now和新路径点P_new1之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new1＝P_now；如果没有交点，说明是可行路径，将新路径点P_new1从基本膜1中输出到表层膜0中；

(2-2)基本膜2在地图Map中随机采样一个点P_rand2，若P_rand2≥P_th2,则基本膜2的采样点P_smp2＝P_rand2；若P_rand2＜P_th2,则P_smp2＝P_T；在当前路径点P_now到采样点P_smp2连接的直线方向上P_now→P_smp2，获取步长为Step2的新路径点P_new2，计算当前路径点P_now和新路径点P_new2之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new2＝P_now；如果没有交点，说明是可行路径，将新路径点P_new2从基本膜2中输出到表层膜0中；

(2-3)基本膜3在地图Map中随机采样一个点P_rand3，若P_rand3≥P_th3,则基本膜3的采样点P_smp3＝P_rand3；若P_rand3＜P_th3,则P_smp3＝P_T；在当前路径点P_now到采样点P_smp3连接的直线方向上P_now→P_smp3，获取步长为Step3的新路径点P_new3，计算当前路径点P_now和新路径点P_new3之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P_new3＝P_now；如果没有交点，说明是可行路径，将新路径点P_new3从基本膜3中输出到表层膜0中；

(3)表层膜0在获取到基本膜1、基本膜2和基本膜3分别输入的新路径点P_new1、P_new2和P_new3后，分别计算每个新路径点到目标点之间的距离L₁＝||P_new1-P_T||、L₂＝||P_new2-P_T||和L₃＝||P_new3-P_T||，再比较L1、L2和L3的值，获取最小值L_min＝min(L₁,L₂,L₃)，其对应的新路径点设为P_new-min，将其作为当前迭代次数寻找到的路径点P_i＝P_new-min，并将该路径点放入到路径点集合中V←P_i；

(4)将P_i作为当前路径点P_now＝P_i，计算当前路径点与目标点的距离L_now＝||P_T-P_now||，若L_now≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(6)；若L_now＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(5)；

(5)判断迭代次数是否完成，即i是否等于K，是，则认为迭代完成，跳转到步骤(6)；否则，令i＝i+1，跳转到步骤(2)；

(6)快速寻路连通结束，输出路径点集合V，按顺序连接路径点集合中所有路径点，作为寻找到的起始点到目标点的一条可行路径。

进一步的，所述寻优初始化，其特征在于获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为N；设置寻优路径点集合为V′，设置到达目标点距离阈值L_target。

进一步的，所述构建多采样点膜结构，其特征为，利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数N和路径点集合V′作为表层膜0内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M1、当前迭代次数j1、步长Sp1、采样点P′_smp1、路径点集合V′1、当前路径点P′_now1、新路径点P′_new1和到达目标点距离阈值L_target作为基本膜1内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M2、当前迭代次数j2、步长Sp2、采样点P′_smp2、路径点集合V′2、当前路径点P′_now2、新路径点P′_new2和到达目标点距离阈值L_target作为基本膜2内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M3、当前迭代次数j3、步长Sp3、采样点P′_smp3、路径点集合V′3、当前路径点P′_now3、新路径点P′_new3和到达目标点距离阈值L_target作为基本膜3内的基本粒子。

进一步的，所述最短路径优化，其步骤如下：

(1)设置寻优路径点集合V′为快速连通阶段找到的路径点集合V，即V′＝V，当前迭代次数j＝1；

(2)表层膜0根据区域地图信息Map、起始点P₀、目标点P_T和最短路径点集合V′形成一个椭圆形区域作为随机采样区M_oval，其中，椭圆的两个焦点分别为起始点P₀和目标点P_T，两个焦点的距离记为c_focus，c_focus＝||P_T-P₀||，将V′中所有路径点之间距离之和作为椭圆的长轴为c_long，c_long＝∑||P_i-P_i-1||,P_i∈V′，将c_short作为椭圆的短轴，

并将M_oval输入到三个基本膜中；

(3)3个基本膜分别同时进行基本膜计算，计算过程为(3-1)、(3-2)和(3-3)，其中(3-1)、(3-2)和(3-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：：

(3-1)基本膜1在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′1到表层膜0；

(3-2)基本膜2在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′2到表层膜0；

(3-3)基本膜3在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′3到表层膜0；

(4)表层膜0分别计算路径点集合V′、V′1、V′2和V′3内所有路径点距离之和：L′、L′1、L′2和L′3，选取最小值对应的路径点集合作为新的最短路径点集合V′＝min{V′,V′1,V′2,V′3}；

(5)判断迭代是否完成，即j是否为N，是，则认为迭代完成，跳转到步骤(6)；否则，令j＝j+1，跳转到步骤(2)

(6)最短路径优化结束，输出V′即为起始点P₀到目标点P_T的最优可行路径。

进一步的，(3-1)所述基本膜1在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜1设置迭代次数为M1,当前迭代次数为j1＝1，路径点集合V′1＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp1，将当前路径点设置为上一轮迭代的路径点P′_now1＝P_j1-1，在从当前路径点P′_now1到采样点P′_smp1连线的方向上P′_now1→P′_smp1，获取步长为Sp1的新路径点P′_new1，计算当前路径点P′_now1和新路径点P′_new1之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new1＝P′_now1；如果没有交点，说明是可行路径，将新路径点P′_new1作为当前迭代次数的路径点P_j1，并加入到基本膜1的路径点集合V′1中，V′1←P_j1；

(3)计算当前路径点与目标点的距离L′_j11＝||P_T-P_j||₁，若L′_j11≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j11＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j1是否为M1，是，则认为迭代完成，跳转到步骤(5)；否则，令j1＝j1+1，跳转到步骤(2)；

(5)基本膜1寻找路径结束，输出路径点集合V′1到表层膜0中。

进一步的，(3-2)所述基本膜2在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜2设置迭代次数为M2,当前迭代次数为j2＝1，路径点集合V′2＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp2，将当前路径点设置为上一轮迭代的路径点P′_now2＝P_j2-1，在从当前路径点P′_now2到采样点P′_smp2连线的方向上P′_now2→P′_smp2，获取步长为Sp2的新路径点P′_new2，计算当前路径点P′_now2和新路径点P′_new2之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new2＝P′_now2；如果没有交点，说明是可行路径，将新路径点P′_new2作为当前迭代次数的路径点P_j2，并加入到基本膜2的路径点集合V′2中，V′2←P_j2；

(3)计算当前路径点与目标点的距离L′_j22＝||P_T-P_j2||，若L′_j22≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j22＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j2是否为M2，是，则认为迭代完成，跳转到步骤(5)；否则，令j2＝j2+1，跳转到步骤(2)；

(5)基本膜2寻找路径结束，输出路径点集合V′2到表层膜0中。

进一步的，(3-3)所述基本膜3在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜3设置迭代次数为M3,当前迭代次数为j3＝1，路径点集合V′3＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp3，将当前路径点设置为上一轮迭代的路径点P′_now3＝P_j3-1，在从当前路径点P′_now3到采样点P′_smp3连线的方向上P′_now3→P′_smp3，获取步长为Sp3的新路径点P′_new3，计算当前路径点P′_now3和新路径点P′_new3之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new3＝P′_now3；如果没有交点，说明是可行路径，将新路径点P′_new3作为当前迭代次数的路径点P_j3，并加入到基本膜3的路径点集合V′3中，V′3←P_j3；

(3)计算当前路径点与目标点的距离L′_j33＝||P_T-P_j3||，若L′_j33≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j33＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j3是否为M3，是，则认为迭代完成，跳转到步骤(5)；否则，令j3＝j3+1，跳转到步骤(2)；

(5)基本膜3寻找路径结束，输出路径点集合V′3到表层膜0中。

本发明有益效果体现在：

本发明可以使机器人在煤矿井下空间不规则，可行空间狭小的条件下，根据起始点位置、目标点位置和地图环境信息，将膜计算的并行性和高效性与RRT算法相结合，以最短的时间和最高的效率寻找到一条可行的最短路径。在路径快速连通阶段，通过构建多步长膜结构，在可行空间较大的区域采用大步长搜索，可以使搜索速度加快，而在狭小的空间使用小步长搜索，使搜索空间更加精细，提高狭小空间路径搜索成功率，从而提高使路径搜索的从功率，加快搜索时间。在快速的找到起始点到目标点的一条可行路径后，构建多采样点膜结构，通过同时在多个椭圆区域内并行搜索最短可行路径，提高了路径优化效率，可以在最短的时间内找到一条起始点到目标点的最优路径，大大增强了机器人路径规划实时性，提高了机器人自动行驶时的行驶效率，为机器人在煤矿井下自主安全行驶提供了基础保证。

附图说明

图1为本发明方法的整体框图。

图2为本发明方法构建的多步长膜结构的示意图。

图3为本发明方法构建的多采样点膜结构的示意图。

图4为本发明方法的快速寻路连通过程流程图。

图5为本发明方法的最短路径优化过程流程图。

图6为本发明方法的最短路径优化过程中基本膜1在椭圆区域内寻找可行路径的流程图。

图7为本发明方法的最短路径优化过程中基本膜2在椭圆区域内寻找可行路径的流程图。

图8为本发明方法的最短路径优化过程中基本膜3在椭圆区域内寻找可行路径的流程图。

图9为本发明方法的快速连通阶段实验结果图。

图10为本发明方法的路径寻优阶段实验结果图。

具体实施方式

如图1所示，基于膜计算和RRT的煤矿井下机器人多步长路径规划方法总体过程为：

本发明方法包括快速连通阶段和路径寻优阶段：所述快速连通阶段包括连通初始化、构建多步长膜结构和快速寻路连通，主要功能为快速寻找出从起始点到目标点的一条可行路径；所述路径寻优阶段包括寻优初始化、构建多采样点膜结构和最短路径优化，主要功能是基于快速连通阶段已经找到的一条可行路径，进行最短路径优化，寻找出从起始点到目标点之间最短可行路径。

本发明方法的连通初始化为获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为K；设置当前路径点为P_now，设置路径点集合为V，设置到达目标点距离阈值L_target。

如图2所示，本发明方法构建的多步长膜结构为：

利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将连通初始化过程中获得的区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数K和路径点集合V作为表层膜0内的基本粒子；将区域地图信息Map、目标点P_T、步长Step1、随机采样概率阈值为P_th1、采样点P_smp1和新路径点P_new1作为基本膜1内的基本粒子；将区域地图信息Map、目标点P_T、步长Step2、随机采样概率阈值为P_th2、采样点P_smp2和新路径点P_new2作为基本膜2内的基本粒子；将区域地图信息Map、目标点P_T、步长Step3、随机采样概率阈值为P_th3、采样点P_smp3和新路径点P_new3作为基本膜3内的基本粒子。

如图4所示，本发明方法的快速寻路连通过程为：

(1)设置当前路径点P_now＝P₀，当前迭代次数i＝1，路径点集合V＝{P₀}；

(2)复制当前路径点P_now到3个基本膜内，令P_now1＝P_now、P_now2＝P_now和P_now3＝P_now；分别同时进行基本膜计算，计算过程为(2-1)、(2-2)和(2-3)，其中(2-1)、(2-2)和(2-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：

(2-1)基本膜1在地图Map中随机采样一个点P_rand1，若P_rand1≥P_th1,则基本膜1的采样点P_smp1＝P_rand1；若P_rand1＜P_th1,则P_smp1＝P_T；在当前路径点P_now到采样点P_smp1连接的直线方向上P_now→P_smp1，获取步长为Step1的新路径点P_new1，计算当前路径点P_now和新路径点P_new1之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new1＝P_now；如果没有交点，说明是可行路径，将新路径点P_new1从基本膜1中输出到表层膜0中；

(2-2)基本膜2在地图Map中随机采样一个点P_rand2，若P_rand2≥P_th2,则基本膜2的采样点P_smp2＝P_rand2；若P_rand2＜P_th2,则P_smp2＝P_T；在当前路径点P_now到采样点P_smp2连接的直线方向上P_now→P_smp2，获取步长为Step2的新路径点P_new2，计算当前路径点P_now和新路径点P_new2之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new2＝P_now；如果没有交点，说明是可行路径，将新路径点P_new2从基本膜2中输出到表层膜0中；

(2-3)基本膜3在地图Map中随机采样一个点P_rand3，若P_rand3≥P_th3,则基本膜3的采样点P_smp3＝P_rand3；若P_rand3＜P_th3,则P_smp3＝P_T；在当前路径点P_now到采样点P_smp3连接的直线方向上P_now→P_smp3，获取步长为Step3的新路径点P_new3，计算当前路径点P_now和新路径点P_new3之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P_new3＝P_now；如果没有交点，说明是可行路径，将新路径点P_new3从基本膜3中输出到表层膜0中；

(3)表层膜0在获取到基本膜1、基本膜2和基本膜3分别输入的新路径点P_new1、P_new2和P_new3后，分别计算每个新路径点到目标点之间的距离L₁＝||P_new1-P_T||、L₂＝||P_new2-P_T||和L₃＝||P_new3-P_T||，再比较L1、L2和L3的值，获取最小值L_min＝min(L₁,L₂,L₃)，其对应的新路径点设为P_new-min，将其作为当前迭代次数寻找到的路径点P_i＝P_new-min，并将该路径点放入到路径点集合中V←P_i；

(4)将P_i作为当前路径点P_now＝P_i，计算当前路径点与目标点的距离L_now＝||P_T-P_now||，若L_now≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(6)；若L_now＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(5)；

(5)判断迭代次数是否完成，即i是否等于K，是，则认为迭代完成，跳转到步骤(6)；否则，令i＝i+1，跳转到步骤(2)；

(6)快速寻路连通结束，输出路径点集合V，按顺序连接路径点集合中所有路径点，作为寻找到的起始点到目标点的一条可行路径。

本发明方法的寻优初始化为获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为N；设置寻优路径点集合为V′，设置到达目标点距离阈值L_target。

如图3所示，本发明方法构建的多采样点膜结构为：利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数N和路径点集合V′作为表层膜0内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M1、当前迭代次数j1、步长Sp1、采样点P′_smp1、路径点集合V′1、当前路径点P′_now1、新路径点P′_new1和到达目标点距离阈值L_target作为基本膜1内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M2、当前迭代次数j2、步长Sp2、采样点P′_smp2、路径点集合V′2、当前路径点P′_now2、新路径点P′_new2和到达目标点距离阈值L_target作为基本膜2内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M3、当前迭代次数j3、步长Sp3、采样点P′_smp3、路径点集合V′3、当前路径点P′_now3、新路径点P′_new3和到达目标点距离阈值L_target作为基本膜3内的基本粒子。

如图5所示，本发明方法的最短路径优化过程为：

(1)设置寻优路径点集合V′为快速连通阶段找到的路径点集合V，即V′＝V，当前迭代次数j＝1；

(2)表层膜0根据区域地图信息Map、起始点P₀、目标点P_T和最短路径点集合V′形成一个椭圆形区域作为随机采样区M_oval，其中，椭圆的两个焦点分别为起始点P₀和目标点P_T，两个焦点的距离记为c_focus，c_focus＝||P_T-P₀||，将V′中所有路径点之间距离之和作为椭圆的长轴为c_long，c_long＝∑||P_i-P_i-1||,P_i∈V′，将c_short作为椭圆的短轴，

并将M_oval输入到三个基本膜中；

(3)3个基本膜分别同时进行基本膜计算，计算过程为(3-1)、(3-2)和(3-3)，其中(3-1)、(3-2)和(3-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：：

(3-1)基本膜1在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′1到表层膜0；

(3-2)基本膜2在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′2到表层膜0；

(3-3)基本膜3在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′3到表层膜0；

(4)表层膜0分别计算路径点集合V′、V′1、V′2和V′3内所有路径点距离之和：L′、L′1、L′2和L′3，选取最小值对应的路径点集合作为新的最短路径点集合V′＝min{V′,V′1,V′2,V′3}；

(5)判断迭代是否完成，即j是否为N，是，则认为迭代完成，跳转到步骤(6)；否则，令j＝j+1，跳转到步骤(2)

(6)最短路径优化结束，输出V′即为起始点P₀到目标点P_T的最优可行路径。

如图6所示，本发明方法的最短路径优化过程中基本膜1在椭圆区域内寻找可行路径为：

(1)基本膜1设置迭代次数为M1,当前迭代次数为j1＝1，路径点集合V′1＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp1，将当前路径点设置为上一轮迭代的路径点P′_now1＝P_j1-1，在从当前路径点P′_now1到采样点P′_smp1连线的方向上P′_now1→P′_smp1，获取步长为Sp1的新路径点P′_new1，计算当前路径点P′_now1和新路径点P′_new1之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new1＝P′_now1；如果没有交点，说明是可行路径，将新路径点P′_new1作为当前迭代次数的路径点P_j1，并加入到基本膜1的路径点集合V′1中，V′1←P_j1；

(3)计算当前路径点与目标点的距离L′_j11＝||P_T-P_j1||，若L′_j11≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j11＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j1是否为M1，是，则认为迭代完成，跳转到步骤(5)；否则，令j1＝j1+1，跳转到步骤(2)；

(5)基本膜1寻找路径结束，输出路径点集合V′1到表层膜0中。

如图7所示，本发明方法的最短路径优化过程中基本膜2在椭圆区域内寻找可行路径为：

(1)基本膜2设置迭代次数为M2,当前迭代次数为j2＝1，路径点集合V′2＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp2，将当前路径点设置为上一轮迭代的路径点P′_now2＝P_j2-1，在从当前路径点P′_now2到采样点P′_smp2连线的方向上P′_now2→P′_smp2，获取步长为Sp2的新路径点P′_new2，计算当前路径点P′_now2和新路径点P′_new2之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new2＝P′_now2；如果没有交点，说明是可行路径，将新路径点P′_new2作为当前迭代次数的路径点P_j2，并加入到基本膜2的路径点集合V′2中，V′2←P_j2；

(3)计算当前路径点与目标点的距离L′_j22＝||P_T-P_j2||，若L′_j22≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j22＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j2是否为M2，是，则认为迭代完成，跳转到步骤(5)；否则，令j2＝j2+1，跳转到步骤(2)；

(5)基本膜2寻找路径结束，输出路径点集合V′2到表层膜0中。

如图8所示，本发明方法的最短路径优化过程中基本膜3在椭圆区域内寻找可行路径为：：

(1)基本膜3设置迭代次数为M3,当前迭代次数为j3＝1，路径点集合V′3＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp3，将当前路径点设置为上一轮迭代的路径点P′_now3＝P_j3-1，在从当前路径点P′_now3到采样点P′_smp3连线的方向上P′_now3→P′_smp3，获取步长为Sp3的新路径点P′_new3，计算当前路径点P′_now3和新路径点P′_new3之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new3＝P′_now3；如果没有交点，说明是可行路径，将新路径点P′_new3作为当前迭代次数的路径点P_j3，并加入到基本膜3的路径点集合V′3中，V′3←P_j3；

(3)计算当前路径点与目标点的距离L′_j33＝||P_T-P_j3||，若L′_j33≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j33＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j3是否为M3，是，则认为迭代完成，跳转到步骤(5)；否则，令j3＝j3+1，跳转到步骤(2)；

(5)基本膜3寻找路径结束，输出路径点集合V′3到表层膜0中。

如图9所示，本发明方法的快速连通阶段实验结果图，起始点在图中左下角位置，坐标为(0,0)，目标点在图中右上角位置，坐标为(15,12)，图中黑色区域为障碍物，不能通行，图中白色区域为可行驶区域，最大迭代次数设为200次。其中，(a)为传统RRT算法，步长为0.5时的运行结果，运行迭代170次后，路径规划结束，可以成功搜索到一条起始点到目标点的路径，但由于步长很小，需要较多的迭代次数才能最终找到可行路径。(b)为传统RRT算法，步长为2.5时的运行结果，可以看出，由于步长较大，很难搜索到较为狭小的可行空间，最大迭代次数200次运行结束后，也很难找到一条可行路径。(c)为本发明方法，三个基本膜中分别设置步长为0.5、2.5和5，在迭代40次时即可寻找到一条从起始点到目标点的可行路径，从图中可以看出在狭小区域，选择较小步长，可以顺利搜素到可行路径，在较大可行空间，采用大步长，可以加快搜索速度，从而提高了路径规划效率。

如图10所示，本发明方法的路径寻优阶段实验结果图，是在快速连通阶段寻找到一条可行路径后进行实验，起始点(0,0)，目标点(15,12)，图中黑色区域为障碍物，不能通行，图中白色区域为可行驶区域；(a)中为本发明方法优化迭代120次的结果，(b)中为传统方法迭代120次的结果，(c)中为传统方法迭代200次的结果，通过对比可以看出，由于本发明方法利用膜计算的并行性，同时3个基本膜进行采样搜索，再与表层膜进行信息粒子交换，在相同迭代次数，即120次迭代的情况下，寻找到的路径最短，如(a)比(b)的路径更短，优化效果更好；在寻找到相同优化路径条件下，需要的迭代次数更少，如(a)仅需要迭代120次，而(c)需要迭代200次。

Claims (10)

1.基于膜计算和RRT的煤矿井下机器人多步长路径规划方法，其特征在于，所述方法包括快速连通阶段和路径寻优阶段：所述快速连通阶段包括连通初始化、构建多步长膜结构和快速寻路连通，主要功能为快速寻找出从起始点到目标点的一条可行路径；所述路径寻优阶段包括寻优初始化、构建多采样点膜结构和最短路径优化，主要功能是基于快速连通阶段已经找到的一条可行路径，进行最短路径优化，寻找出从起始点到目标点之间最短可行路径。

2.权利要求1中所述连通初始化，其特征在于获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为K；设置当前路径点为P_now，设置路径点集合为V，设置到达目标点距离阈值L_target。

3.权利要求1中所述构建多步长膜结构，其特征为，利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将连通初始化过程中获得的区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数K和路径点集合V作为表层膜0内的基本粒子；将区域地图信息Map、目标点P_T、步长Step1、随机采样概率阈值为P_th1、采样点P_smp1和新路径点P_new1作为基本膜1内的基本粒子；将区域地图信息Map、目标点P_T、步长Step2、随机采样概率阈值为P_th2、采样点P_smp2和新路径点P_new2作为基本膜2内的基本粒子；将区域地图信息Map、目标点P_T、步长Step3、随机采样概率阈值为P_th3、采样点P_smp3和新路径点P_new3作为基本膜3内的基本粒子。

4.权利要求1中所述快速寻路连通，其步骤如下：

(1)设置当前路径点P_now＝P₀，当前迭代次数i＝1，路径点集合V＝{P₀}；

(2)复制当前路径点P_now到3个基本膜内，令P_now1＝P_now、P_now2＝P_now和P_now3＝P_now；分别同时进行基本膜计算，计算过程为(2-1)、(2-2)和(2-3)，其中(2-1)、(2-2)和(2-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：

(2-1)基本膜1在地图Map中随机采样一个点P_rand1，若P_rand1≥P_th1,则基本膜1的采样点P_smp1＝P_rand1；若P_rand1＜P_th1,则P_smp1＝P_T；在当前路径点P_now到采样点P_smp1连接的直线方向上P_now→P_smp1，获取步长为Step1的新路径点P_new1，计算当前路径点P_now和新路径点P_new1之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new1＝P_now；如果没有交点，说明是可行路径，将新路径点P_new1从基本膜1中输出到表层膜0中；

(2-2)基本膜2在地图Map中随机采样一个点P_rand2，若P_rand2≥P_th2,则基本膜2的采样点P_smp2＝P_rand2；若P_rand2＜P_th2,则P_smp2＝P_T；在当前路径点P_now到采样点P_smp2连接的直线方向上P_now→P_smp2，获取步长为Step2的新路径点P_new2，计算当前路径点P_now和新路径点P_new2之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，P_new2＝P_now；如果没有交点，说明是可行路径，将新路径点P_new2从基本膜2中输出到表层膜0中；

(2-3)基本膜3在地图Map中随机采样一个点P_rand3，若P_rand3≥P_th3,则基本膜3的采样点P_smp3＝P_rand3；若P_rand3＜P_th3,则P_smp3＝P_T；在当前路径点P_now到采样点P_smp3连接的直线方向上P_now→P_smp3，获取步长为Step3的新路径点P_new3，计算当前路径点P_now和新路径点P_new3之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P_new3＝P_now；如果没有交点，说明是可行路径，将新路径点P_new3从基本膜3中输出到表层膜0中；

(3)表层膜0在获取到基本膜1、基本膜2和基本膜3分别输入的新路径点P_new1、P_new2和P_new3后，分别计算每个新路径点到目标点之间的距离L₁＝||P_new1-P_T||、L₂＝||P_new2-P_T||和L₃＝||P_new3-P_T||，再比较L1、L2和L3的值，获取最小值L_min＝min(L₁,L₂,L₃)，其对应的新路径点设为P_new-min，将其作为当前迭代次数寻找到的路径点P_i＝P_new-min，并将该路径点放入到路径点集合中V←P_i；

(4)将P_i作为当前路径点P_now＝P_i，计算当前路径点与目标点的距离L_now＝||P_T-P_now||，若L_now≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(6)；若L_now＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(5)；

(5)判断迭代次数是否完成，即i是否等于K，是，则认为迭代完成，跳转到步骤(6)；否则，令i＝i+1，跳转到步骤(2)；

(6)快速寻路连通结束，输出路径点集合V，按顺序连接路径点集合中所有路径点，作为寻找到的起始点到目标点的一条可行路径。

5.权利要求1中所述寻优初始化，其特征在于获取区域地图信息Map，包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P₀(x₀,y₀),目标点位置坐标P_T(x_T,y_T)；设置迭代次数为N；设置寻优路径点集合为V′，设置到达目标点距离阈值L_target。

6.权利要求1中所述构建多采样点膜结构，其特征为，利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将区域地图信息Map、起始点位置坐标P₀、目标点位置坐标P_T，迭代次数N和路径点集合V′作为表层膜0内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M1、当前迭代次数j1、步长Sp1、采样点P′_smp1、路径点集合V′1、当前路径点P′_now1、新路径点P′_new1和到达目标点距离阈值L_target作为基本膜1内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M2、当前迭代次数j2、步长Sp2、采样点P′_smp2、路径点集合V′2、当前路径点P′_now2、新路径点P′_new2和到达目标点距离阈值L_target作为基本膜2内的基本粒子；将区域地图信息Map、起始点P₀、目标点P_T、迭代次数M3、当前迭代次数j3、步长Sp3、采样点P′_smp3、路径点集合V′3、当前路径点P′_now3、新路径点P′_new3和到达目标点距离阈值L_target作为基本膜3内的基本粒子。

7.权利要求1中所述最短路径优化，其步骤如下：

(1)设置寻优路径点集合V′为快速连通阶段找到的路径点集合V，即V′＝V，当前迭代次数j＝1；

(2)表层膜0根据区域地图信息Map、起始点P₀、目标点P_T和最短路径点集合V′形成一个椭圆形区域作为随机采样区M_oval，其中，椭圆的两个焦点分别为起始点P₀和目标点P_T，两个焦点的距离记为c_focus，c_focus＝||P_T-P₀||，将V′中所有路径点之间距离之和作为椭圆的长轴为c_long，c_long＝∑||P_i-P_i-1||,P_i∈V′，将c_short作为椭圆的短轴，

并将M_oval输入到三个基本膜中；

(3)3个基本膜分别同时进行基本膜计算，计算过程为(3-1)、(3-2)和(3-3)，其中(3-1)、(3-2)和(3-3)分别为基本膜1、基本膜2和基本膜3同时运算过程：：

(3-1)基本膜1在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′1到表层膜0；

(3-2)基本膜2在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′2到表层膜0；

(3-3)基本膜3在椭圆区域M_oval内寻找到一条从起始点P₀到目标点P_T的可行路径，输出路径点集合V′3到表层膜0；

(4)表层膜0分别计算路径点集合V′、V′1、V′2和V′3内所有路径点距离之和：L′、L′1、L′2和L′3，选取最小值对应的路径点集合作为新的最短路径点集合V′＝min{V′,V′1,V′2,V′3}；

(5)判断迭代是否完成，即j是否为N，是，则认为迭代完成，跳转到步骤(6)；否则，令j＝j+1，跳转到步骤(2)

(6)最短路径优化结束，输出V′即为起始点P₀到目标点P_T的最优可行路径。

8.权利要求7中(3-1)所述基本膜1在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜1设置迭代次数为M1,当前迭代次数为j1＝1，路径点集合V′1＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp1，将当前路径点设置为上一轮迭代的路径点P′_now1＝P_j1-1，在从当前路径点P′_now1到采样点P′_smp1连线的方向上P′_now1→P′_smp1，获取步长为Sp1的新路径点P′_new1，计算当前路径点P′_now1和新路径点P′_new1之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new1＝P′_now1；如果没有交点，说明是可行路径，将新路径点P′_new1作为当前迭代次数的路径点P_j1，并加入到基本膜1的路径点集合V′1中，V′1←P_j1；

(3)计算当前路径点与目标点的距离L′_j11＝||P_T-P_j1||，若L′_j11≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j11＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j1是否为M1，是，则认为迭代完成，跳转到步骤(5)；否则，令j1＝j1+1，跳转到步骤(2)；

(5)基本膜1寻找路径结束，输出路径点集合V′1到表层膜0中。

9.权利要求7中(3-2)所述基本膜2在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜2设置迭代次数为M2,当前迭代次数为j2＝1，路径点集合V′2＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp2，将当前路径点设置为上一轮迭代的路径点P′_now2＝P_j2-1，在从当前路径点P′_now2到采样点P′_smp2连线的方向上P′_now2→P′_smp2，获取步长为Sp2的新路径点P′_new2，计算当前路径点P′_now2和新路径点P′_new2之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new2＝P′_now2；如果没有交点，说明是可行路径，将新路径点P′_new2作为当前迭代次数的路径点P_j2，并加入到基本膜2的路径点集合V′2中，V′2←P_j2；

(3)计算当前路径点与目标点的距离L′_j22＝||P_T-P_j2||，若L′_j22≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j22＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j2是否为M2，是，则认为迭代完成，跳转到步骤(5)；否则，令j2＝j2+1，跳转到步骤(2)；

(5)基本膜2寻找路径结束，输出路径点集合V′2到表层膜0中。

10.权利要求7中(3-3)所述基本膜3在椭圆区域内寻找到一条从起始点P₀到目标点P_T的可行路径，其过程为：

(1)基本膜3设置迭代次数为M3,当前迭代次数为j3＝1，路径点集合V′3＝{P₀}；

(2)在椭圆区域M_oval内随机采样一个点P′_smp3，将当前路径点设置为上一轮迭代的路径点P′_now3＝P_j3-1，在从当前路径点P′_now3到采样点P′_smp3连线的方向上P′_now3→P′_smp3，获取步长为Sp3的新路径点P′_new3，计算当前路径点P′_now3和新路径点P′_new3之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′_new3＝P′_now3；如果没有交点，说明是可行路径，将新路径点P′_new3作为当前迭代次数的路径点P_j3，并加入到基本膜3的路径点集合V′3中，V′3←P_j3；

(3)计算当前路径点与目标点的距离L′_j33＝||P_T-P_j3||，若L′_j33≤L_target，则认为寻找的路径已经到达目标点，跳转到步骤(5)；若L′_j33＞L_target，则认为寻找的路径还没到达目标点，跳转到步骤(4)；

(4)判断迭代次数是否完成，即j3是否为M3，是，则认为迭代完成，跳转到步骤(5)；否则，令j3＝j3+1，跳转到步骤(2)；

(5)基本膜3寻找路径结束，输出路径点集合V′3到表层膜0中。

Images