一种矿用井下巡检机器人路径规划方法及系统

本发明涉及机器人路径规划领域，更具体地说，涉及一种矿用井下巡检机器人路径规划方法及系统。

背景技术：

1、机器人技术作为一个综合了多个学科的技术，是多年研究的热门。其中，路径规划作为移动机器人技术研发的关键技术，其目的在于参照某一个参数的指标(如代价最低、选择路径最短、运算时间最短等)，在任务区域选择出一条可以从起点运动到终点的最优或次优的避障路径。路径规划按照机器人对工作区域信息的理解层次，可以分为给予完整区域信息理解的全局路径规划和基于部分区域信息理解的局部路径规划，两者各有优缺。现在实际工作中大多情况下都需要融合兼用全局和局部路径规划，前者旨在寻找全局优化路径，后者旨在实时避障。

2、矿用井下巡检机器人是一种用于危险环境下进行巡检任务的智能机器人系统，其具有高效、安全的特点，可以在井下环境中完成巡检任务，减少人力成本和避免人员受到井下环境的伤害。路径规划是矿用井下巡检机器人的重要技术之一，其目的是指导机器人在复杂的环境中找到最优的路径，完成巡检任务。

3、目前，传统的路径规划方法存在着计算复杂度高、路径规划效果不理想的问题，难以适应井下环境中复杂的地形和障碍。

技术实现思路

1、本发明的目的在于，提供一种矿用井下巡检机器人路径规划方法及系统，提高路径规划效率和增强机器人避障能力。

2、本发明提供一种矿用井下巡检机器人路径规划方法，包括以下步骤：s1：获取环境地图数据集、起始位置、目标位置、目标节点、障碍物节点和当前位置，根据当前位置、目标位置和障碍物节点，得到节点集合；s2：根据当前位置和目标位置，利用目标偏置策略，得到随机采样点；s3：根据节点集合和随机采样点，将节点集合中距离随机采样点最近的节点，作为最近邻节点；s4：获取初始步长，根据初始步长、最近邻节点、随机采样点，得到新节点；s5：根据新节点和障碍物节点，检查新节点与障碍物之间是否存在碰撞，如果存在碰撞，则放弃该节点，返回步骤s4；如果不存在碰撞，则将新节点插入到节点集合，得到新的节点集合；以最近邻节点为父节点，得到连接；s6：重复步骤s2-s5，进行迭代，根据连接，得到生成的树；当生成的树包含目标节点，或者迭代次数达到预设值，停止迭代；否则，继续迭代；s7：根据连接、父节点、目标节点和起始位置，从目标节点回溯到起始位置，沿着父节点连接构建全局路径，得到全局路径；s8：根据全局路径和节点集合，删除全局路线中的冗余节点，将路线转换为折线，得到折线全局路径；s9：对折线全局路径进行b样条曲线平滑处理，得到平滑全局路径；s10：根据平滑全局路径，利用动态窗口算法，得到执行路径。

3、进一步地，上述的矿用井下巡检机器人路径规划方法的步骤s1具体包括：s11：获取环境地图数据集、起始位置、目标位置、目标节点和障碍物节点；s12：获取当前位置，根据当前位置和目标位置，得到目标偏置量；根据当前位置和障碍物节点，得到补偿参数；根据补偿参数，得到搜索方向；根据目标偏置量和搜索方向，得到节点集合；s13：根据起始位置和节点集合，得到新的节点集合。

4、进一步地，上述的矿用井下巡检机器人路径规划方法的步骤s2具体包括：根据当前位置和目标位置，利用目标偏置策略，得到随机采样点，如公式：

5、

6、式中：qrand为下一个要拓展的节点，qgoal为目标节点，pg为按照均匀概率分布随机获取的概率值，ptarget为事先设定的阈值。

7、进一步地，上述的矿用井下巡检机器人路径规划方法的步骤s4具体包括：

8、s41：获取初始步长，根据初始步长，得到自适应步长，如公式：

9、s＝s0eλ(p-q) (2)

10、其中：s为自适应步长，s0为初始步长，e为自然底数，λ为系数，p为障碍物数量阈值，q为机器人障碍物数量；s42：根据自适应步长，从最近邻节点向随机采样点方向生成新节点。

11、进一步地，上述的矿用井下巡检机器人路径规划方法的步骤s9具体包括：对折线全局路径进行b样条曲线平滑处理，得到平滑全局路径，如公式：

12、

13、

14、

15、

16、其中，p(u)为平滑处理后的函数，di为第i个控制点，ni,j(u)为第i个控制点的j次的b样条插值函数的基函数，u为当前节点，ui为第i个节点，n’i,j为基函数的微分。

17、本发明还提供一种系统，包括：初始化模块、随机采用模块、最近邻节点获取模块、新节点获取模块、连接模块、迭代模块、路径获取模块、折线路径获取模块、平滑路径获取模块和执行路径获取模块；初始化模块，配置为：获取环境地图数据集、起始位置、目标位置、目标节点、障碍物节点和当前位置，根据当前位置、目标位置和障碍物节点，得到节点集合；随机采用模块，配置为：根据当前位置和目标位置，利用目标偏置策略，得到随机采样点；最近邻节点获取模块，配置为：根据节点集合和随机采样点，将节点集合中距离随机采样点最近的节点，作为最近邻节点；新节点获取模块，配置为：获取初始步长，根据初始步长、最近邻节点、随机采样点，得到新节点；连接模块，配置为：根据新节点和障碍物节点，检查新节点与障碍物之间是否存在碰撞，如果存在碰撞，则放弃该节点，返回步骤s4；如果不存在碰撞，则将新节点插入到节点集合，得到新的节点集合；以最近邻节点为父节点，得到连接；迭代模块，配置为：重复依次执行随机采用模块、最近邻节点获取模块、新节点获取模块和连接模块的配置功能，进行迭代，根据连接，得到生成的树；当生成的树包含目标节点，或者迭代次数达到预设值，停止迭代；否则，继续迭代；路径获取模块，配置为：根据连接、父节点、目标节点和起始位置，从目标节点回溯到起始位置，沿着父节点连接构建全局路径，得到全局路径；折线路径获取模块，配置为：根据全局路径和节点集合，删除全局路线中的冗余节点，将路线转换为折线，得到折线全局路径；平滑路径获取模块，配置为：对折线全局路径进行b样条曲线平滑处理，得到平滑全局路径；执行路径获取模块，配置为：根据平滑全局路径，利用动态窗口算法，得到执行路径。

18、进一步地，上述系统的初始化模块具体配置为：获取环境地图数据集、起始位置、目标位置、目标节点和障碍物节点；获取当前位置，根据当前位置和目标位置，得到目标偏置量；根据当前位置和障碍物节点，得到补偿参数；根据补偿参数，得到搜索方向；根据目标偏置量和搜索方向，得到节点集合；根据起始位置和节点集合，得到新的节点集合。

19、进一步地，上述系统的随机采用模块具体配置为：根据当前位置和目标位置，利用目标偏置策略，得到随机采样点，如公式：

20、

21、式中：qrand为下一个要拓展的节点，qgoal为目标节点，pg为按照均匀概率分布随机获取的概率值，ptarget为事先设定的阈值。

22、进一步地，上述系统的新节点获取模块具体配置为：获取初始步长，根据初始步长，得到自适应步长，如公式：

23、s＝s0eλ(p-q) (2)

24、其中：s为自适应步长，s0为初始步长，e为自然底数，λ为系数，p为障碍物数量阈值，q为机器人障碍物数量；根据自适应步长，从最近邻节点向随机采样点方向生成新节点。

25、进一步地，上述系统的平滑路径获取模块具体配置为：对折线全局路径进行b样条曲线平滑处理，得到平滑全局路径，如公式：

26、

27、

28、

29、

30、其中，p(u)为平滑处理后的函数，di为第i个控制点，ni,j(u)为第i个控制点的j次的b样条插值函数的基函数，u为当前节点，ui为第i个节点，n’i,j为基函数的微分。

31、实施本发明提供的矿用井下巡检机器人路径规划方法及系统，具有以下有益效果：

32、通过改进的rrt算法，可以快速生成有效的全局路径，减少了计算复杂度，同时利用b样条曲线平滑处理，提高了路径的可行性和规划效率；

33、融合dwa算法，使得矿用井下巡检机器人能够实时响应周围环境变化，快速做出避障决策，有效避免矿井下的突发障碍物；

34、进而，在矿井下复杂环境中，实时避障和路径优化能够确保机器人避免与地下结构或作业人员发生碰撞，减少安全事故的发生；通过平滑的路径规划，机器人能够更加稳定地移动，保证巡检设备的准确性和稳定性，提高巡检数据的质量和可靠性；自适应权重调整和目标点差值优化，使得机器人能够更好地适应矿井下不断变化的环境条件，保证巡检任务在不同环境下的顺利进行；改进的路径规划方法使得机器人能够在无需人工干预的情况下，自主完成巡检任务，提高了机器人的自主性和智能化水平；由于机器人能够高效、安全地完成巡检任务，减少了因事故造成的设备损坏和人员伤亡，从而降低了长期的维护成本；实时路径规划和避障能力，使得机器人在矿井下的巡检工作能够持续进行，减少了因障碍物导致的工作中断时间，提升了整体的工作效率。