申请/专利权人：安徽理工大学

申请日：2021-08-02

公开（公告）日：2024-02-20

公开（公告）号：CN113485363B

主分类号：G01C21/34

分类号：G01C21/34

优先权：

专利状态码：有效-授权

法律状态：2024.02.20#授权;2021.10.26#实质审查的生效;2021.10.08#公开

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

主权项：1.基于膜计算和RRT的煤矿井下机器人多步长路径规划方法，其特征在于，所述方法包括快速连通阶段和路径寻优阶段：所述快速连通阶段包括连通初始化、构建多步长膜结构和快速寻路连通，主要功能为快速寻找出从起始点到目标点的一条可行路径；所述路径寻优阶段包括寻优初始化、构建多采样点膜结构和最短路径优化，主要功能是基于快速连通阶段已经找到的一条可行路径，进行最短路径优化，寻找出从起始点到目标点之间最短可行路径；所述连通初始化包括获取区域地图信息Map，Map包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P0x0,y0,目标点位置坐标PTxT,yT；设置迭代次数为K；设置当前路径点为Pnow，设置路径点集合为V，设置到达目标点距离阈值Ltarget；所述构建多步长膜结构包括：利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将连通初始化过程中获得的区域地图信息Map、起始点位置坐标P0、目标点位置坐标PT，迭代次数K和路径点集合V作为表层膜0内的基本粒子；将区域地图信息Map、目标点PT、步长Step1、随机采样概率阈值为Pth1、采样点Psmp1和新路径点Pnew1作为基本膜1内的基本粒子；将区域地图信息Map、目标点PT、步长Step2、随机采样概率阈值为Pth2、采样点Psmp2和新路径点Pnew2作为基本膜2内的基本粒子；将区域地图信息Map、目标点PT、步长Step3、随机采样概率阈值为Pth3、采样点Psmp3和新路径点Pnew3作为基本膜3内的基本粒子；所述快速寻路连通，其步骤如下：A1：设置当前路径点Pnow＝P0，当前迭代次数i＝1，路径点集合V＝{P0}；A2：复制当前路径点Pnow到3个基本膜内，令Pnow1＝Pnow、Pnow2＝Pnow和Pnow3＝Pnow；分别同时进行基本膜计算，计算过程为A2-1、A2-2和A2-3，其中A2-1、A2-2和A2-3分别为基本膜1、基本膜2和基本膜3同时运算过程：A2-1：基本膜1在地图Map中随机采样一个点Prand1，若Prand1≥Pth1,则基本膜1的采样点Psmp1＝Prand1；若Prand1＜Pth1,则Psmp1＝PT；在当前路径点Pnow到采样点Psmp1连接的直线方向上Pnow→Psmp1，获取步长为Step1的新路径点Pnew1，计算当前路径点Pnow和新路径点Pnew1之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，Pnew1＝Pnow；如果没有交点，说明是可行路径，将新路径点Pnew1从基本膜1中输出到表层膜0中；A2-2：基本膜2在地图Map中随机采样一个点Prand2，若Prand2≥Pth2,则基本膜2的采样点Psmp2＝Prand2；若Prand2＜Pth2,则Psmp2＝PT；在当前路径点Pnow到采样点Psmp2连接的直线方向上Pnow→Psmp2，获取步长为Step2的新路径点Pnew2，计算当前路径点Pnow和新路径点Pnew2之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，新路径点丢弃，Pnew2＝Pnow；如果没有交点，说明是可行路径，将新路径点Pnew2从基本膜2中输出到表层膜0中；A2-3：基本膜3在地图Map中随机采样一个点Prand3，若Prand3≥Pth3,则基本膜3的采样点Psmp3＝Prand3；若Prand3＜Pth3,则Psmp3＝PT；在当前路径点Pnow到采样点Psmp3连接的直线方向上Pnow→Psmp3，获取步长为Step3的新路径点Pnew3，计算当前路径点Pnow和新路径点Pnew3之间的新的路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令Pnew3＝Pnow；如果没有交点，说明是可行路径，将新路径点Pnew3从基本膜3中输出到表层膜0中；A3：表层膜0在获取到基本膜1、基本膜2和基本膜3分别输入的新路径点Pnew1、Pnew2和Pnew3后，分别计算每个新路径点到目标点之间的距离L1＝||Pnew1-PT||、L2＝||Pnew2-PT||和L3＝||Pnew3-PT||，再比较L1、L2和L3的值，获取最小值Lmin＝minL1,L2,L3，其对应的新路径点设为Pnew-min，将其作为当前迭代次数寻找到的路径点Pi＝Pnew-min，并将该路径点放入到路径点集合中V←Pi；A4：将Pi作为当前路径点Pnow＝Pi，计算当前路径点与目标点的距离Lnow＝||PT-Pnow||，若Lnow≤Ltarget，则认为寻找的路径已经到达目标点，跳转到步骤A6；若Lnow＞Ltarget，则认为寻找的路径还没到达目标点，跳转到步骤A5；A5：判断迭代次数是否完成，即i是否等于K，是，则认为迭代完成，跳转到步骤A6；否则，令i＝i+1，跳转到步骤A2；A6：快速寻路连通结束，输出路径点集合V，按顺序连接路径点集合中所有路径点，作为寻找到的起始点到目标点的一条可行路径；所述寻优初始化包括获取区域地图信息Map，Map包括可通过区域、障碍物区域和地图边界区域；获取机器人起始点位置坐标P0x0,y0,目标点位置坐标PTxT,yT；设置迭代次数为N；设置寻优路径点集合为V′，设置到达目标点距离阈值Ltarget；所述构建多采样点膜结构包括：利用4核CPU构建细胞膜结构，设置CPU的1个内核为表层膜0，设置CPU的其他3个内核为3个基本膜，分别为基本膜1，基本膜2和基本膜3；将区域地图信息Map、起始点位置坐标P0、目标点位置坐标PT，迭代次数N和路径点集合V′作为表层膜0内的基本粒子；将区域地图信息Map、起始点P0、目标点PT、迭代次数M1、当前迭代次数j1、步长Sp1、采样点P′smp1、路径点集合V′1、当前路径点P′now1、新路径点P′new1和到达目标点距离阈值Ltarget作为基本膜1内的基本粒子；将区域地图信息Map、起始点P0、目标点PT、迭代次数M2、当前迭代次数j2、步长Sp2、采样点P′smp2、路径点集合V′2、当前路径点P′now2、新路径点P′new2和到达目标点距离阈值Ltarget作为基本膜2内的基本粒子；将区域地图信息Map、起始点P0、目标点PT、迭代次数M3、当前迭代次数j3、步长Sp3、采样点P′smp3、路径点集合V′3、当前路径点P′now3、新路径点P′new3和到达目标点距离阈值Ltarget作为基本膜3内的基本粒子；所述最短路径优化，其步骤如下：B1：设置寻优路径点集合V′为快速连通阶段找到的路径点集合V，即V′＝V，当前迭代次数j＝1；B2：表层膜0根据区域地图信息Map、起始点P0、目标点PT和最短路径点集合V′形成一个椭圆形区域作为随机采样区Moval，其中，椭圆的两个焦点分别为起始点P0和目标点PT，两个焦点的距离记为cfocus，cfocus＝||PT-P0||，将V′中所有路径点之间距离之和作为椭圆的长轴为clong，clong＝∑||Pi-Pi-1||,Pi∈V′，将cshort作为椭圆的短轴，并将Moval输入到三个基本膜中；B3：3个基本膜分别同时进行基本膜计算，计算过程为B3-1、B3-2和B3-3，其中B3-1、B3-2和B3-3分别为基本膜1、基本膜2和基本膜3同时运算过程：B3-1：基本膜1在椭圆区域Moval内寻找到一条从起始点P0到目标点PT的可行路径，输出路径点集合V′1到表层膜0；B3-2：基本膜2在椭圆区域Moval内寻找到一条从起始点P0到目标点PT的可行路径，输出路径点集合V′2到表层膜0；B3-3：基本膜3在椭圆区域Moval内寻找到一条从起始点P0到目标点PT的可行路径，输出路径点集合V′3到表层膜0；B4：表层膜0分别计算路径点集合V′、V′1、V′2和V′3内所有路径点距离之和：L′、L′1、L′2和L′3，选取最小值对应的路径点集合作为新的最短路径点集合V′＝min{V′,V′1,V′2,V′3}；B5：判断迭代是否完成，即j是否为N，是，则认为迭代完成，跳转到步骤B6；否则，令j＝j+1，跳转到步骤B2；B6：最短路径优化结束，输出V′即为起始点P0到目标点PT的最优可行路径；其中所述B3-1基本膜1在椭圆区域内寻找到一条从起始点P0到目标点PT的可行路径，其过程为：C1：基本膜1设置迭代次数为M1,当前迭代次数为j1＝1，路径点集合V′1＝{P0}；C2：在椭圆区域Moval内随机采样一个点P′smp1，将当前路径点设置为上一轮迭代的路径点P′now1＝Pj1-1，在从当前路径点P′now1到采样点P′smp1连线的方向上P′now1→P′smp1，获取步长为Sp1的新路径点P′new1，计算当前路径点P′now1和新路径点P′new1之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′new1＝P′now1；如果没有交点，说明是可行路径，将新路径点P′new1作为当前迭代次数的路径点Pj1，并加入到基本膜1的路径点集合V′1中，V′1←Pj1；C3：计算当前路径点与目标点的距离L′j11＝||PT-Pj1||，若L′j11≤Ltarget，则认为寻找的路径已经到达目标点，跳转到步骤C5；若L′j11＞Ltarget，则认为寻找的路径还没到达目标点，跳转到步骤C4；C4：判断迭代次数是否完成，即j1是否为M1，是，则认为迭代完成，跳转到步骤C5；否则，令j1＝j1+1，跳转到步骤C2；C5：基本膜1寻找路径结束，输出路径点集合V′1到表层膜0中；其中所述B3-2基本膜2在椭圆区域内寻找到一条从起始点P0到目标点PT的可行路径，其过程为：D1：基本膜2设置迭代次数为M2,当前迭代次数为j2＝1，路径点集合V′2＝{P0}；D2：在椭圆区域Moval内随机采样一个点P′smp2，将当前路径点设置为上一轮迭代的路径点P′now2＝Pj2-1，在从当前路径点P′now2到采样点P′smp2连线的方向上P′now2→P′smp2，获取步长为Sp2的新路径点P′new2，计算当前路径点P′now2和新路径点P′new2之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′new2＝P′now2；如果没有交点，说明是可行路径，将新路径点P′new2作为当前迭代次数的路径点Pj2，并加入到基本膜2的路径点集合V′2中，V′2←Pj2；D3：计算当前路径点与目标点的距离L′j22＝||PT-Pj2||，若L′j22≤Ltarget，则认为寻找的路径已经到达目标点，跳转到步骤D5；若L′j22＞Ltarget，则认为寻找的路径还没到达目标点，跳转到步骤D4；D4：判断迭代次数是否完成，即j2是否为M2，是，则认为迭代完成，跳转到步骤D5；否则，令j2＝j2+1，跳转到步骤D2；D5：基本膜2寻找路径结束，输出路径点集合V′2到表层膜0中；其中所述B3-3基本膜3在椭圆区域内寻找到一条从起始点P0到目标点PT的可行路径，其过程为：E1：基本膜3设置迭代次数为M3,当前迭代次数为j3＝1，路径点集合V′3＝{P0}；E2：在椭圆区域Moval内随机采样一个点P′smp3，将当前路径点设置为上一轮迭代的路径点P′now3＝Pj3-1，在从当前路径点P′now3到采样点P′smp3连线的方向上P′now3→P′smp3，获取步长为Sp3的新路径点P′new3，计算当前路径点P′now3和新路径点P′new3之间的新路径是否与地图中障碍物有交点，如果有交点，说明新路径会和障碍物发生碰撞，丢弃新路径点，令P′new3＝P′now3；如果没有交点，说明是可行路径，将新路径点P′new3作为当前迭代次数的路径点Pj3，并加入到基本膜3的路径点集合V′3中，V′3←Pj3；E3：计算当前路径点与目标点的距离L′j33＝||PT-Pj3||，若L′j33≤Ltarget，则认为寻找的路径已经到达目标点，跳转到步骤E5；若L′j33＞Ltarget，则认为寻找的路径还没到达目标点，跳转到步骤E4；E4：判断迭代次数是否完成，即j3是否为M3，是，则认为迭代完成，跳转到步骤E5；否则，令j3＝j3+1，跳转到步骤E2；E5：基本膜3寻找路径结束，输出路径点集合V′3到表层膜0中。

全文数据：

权利要求：

百度查询： 安徽理工大学 基于膜计算和RRT的煤矿井下机器人多步长路径规划方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD