一种基于多传感器融合的煤矿巷道机器人导航方法与流程

本技术涉及机器人导航相关，尤其涉及一种基于多传感器融合的煤矿巷道机器人导航方法。

背景技术：

1、煤矿井下巷道通常具有空间狭长、光照不足、粉尘浓度高、地面湿滑以及巷道结构重复等特点，现有煤矿机器人通常采用激光雷达、视觉传感器或惯性测量单元等单一或少量传感器进行环境感知和定位，其中，激光雷达获取的点云容易受到粉尘、水雾及偶发反射的干扰，导致点云噪声增加、有效特征减少；视觉传感器容易受到低照度、粉尘遮挡等因素影响，造成图像特征提取不稳定；惯性测量单元虽然能够提供连续的运动信息，但其定位误差会随运行时间不断累积。同时，煤矿巷道通常存在长距离、结构相似及分支较多等情况，机器人在持续运行过程中容易产生定位漂移甚至发生定位失效等问题。

2、现有导航方法在获得机器人位姿后，通常依据单一路径评价指标进行路径规划，难以兼顾路径长度、通行安全性、作业效率及路径平滑性，并且对于运行过程中出现的突发障碍物缺乏有效的动态重规划机制。

技术实现思路

1、本技术的目的在于克服上述问题，提供一种基于多传感器融合的煤矿巷道机器人导航方法。

2、本技术的技术方案提供一种基于多传感器融合的煤矿巷道机器人导航方法，包括：

3、获取煤矿巷道环境观测数据和机器人运动状态信息；

4、基于所述煤矿巷道环境观测数据提取环境结构特征，构建局部特征地图；

5、基于所述机器人运动状态信息进行机器人位姿预测，得到预测位姿，并根据所述局部特征地图对所述预测位姿进行修正，得到机器人最终位姿；

6、根据所述机器人最终位姿与预设关键拓扑节点之间的位置关系，对所述机器人最终位姿进行重定位修正；

7、基于重定位修正后的所述机器人最终位姿和局部地图进行路径规划，生成候选路径；

8、根据预设路径评价规则对各所述候选路径进行评价并寻优，将综合评价最优的候选路径作为目标路径。

9、进一步地，所述煤矿巷道环境观测数据包括激光雷达点云；

10、所述基于所述煤矿巷道环境观测数据提取环境结构特征，构建局部特征地图，包括：

11、对所述激光雷达点云进行预处理，得到预处理点云；

12、根据所述预处理点云中的各点确定对应的邻域点集，基于所述邻域点集构建协方差矩阵，并对所述协方差矩阵进行特征值分解，得到对应的特征值；

13、根据所述特征值之间的关系，将所述预处理点云中的店划分为角点、平面点和地面点，并得到所述环境结构特征；

14、基于机器人当前位姿，将所述环境结构特征转换为全局坐标系，根据机器人当前位置确定预设空间范围，将位于所述预设空间范围内的历史环境结构特征与当前环境结构特征进行关联，构建所述局部特征地图。

15、进一步地，所述对所述激光雷达点云进行预处理，得到预处理点云，具体包括：

16、将所述激光雷达点云划分为多个预设尺寸的体素网格，并在每个所述体素网格内保留一个代表性点；

17、对各所述代表性点确定对应的邻域点集，计算各所述代表性点与对应邻域点集中的邻近点之间的平均距离，得到各所述代表性点对应的邻域平均距离；

18、根据各所述代表性点对应的邻域平均距离确定所述邻域平均距离的均值和标准差；

19、当所述代表性点对应的邻域平均距离与所述均值之间的绝对差值大于预设倍数的所述标准差时，将该所述代表性点确定为离群点并删除，得到所述预处理点云。

20、进一步地，所述根据所述特征值之间的关系，将所述预处理点云中的店划分为角点、平面点和地面点，具体包括：

21、对所述特征值按照数值大小进行排序，得到第一特征值、第二特征值和第三特征值；

22、当所述第一特征值与所述第二特征值之间的差异大于预设阈值，且所述第二特征值与所述第三特征值之间的差异小于预设阈值时，将对应点确定为角点；

23、当所述第一特征值与所述第二特征值之间的差异小于预设阈值，且所述第二特征值与所述第三特征值之间的差异大于预设阈值时，将对应点确定为平面点；

24、当所述第一特征值、所述第二特征值和所述第三特征值之间的差异均小于预设阈值时，将对应点确定为地面点。

25、进一步地，所述构建所述局部特征地图，具体包括：

26、响应于机器人启动信号，获取初始位姿，将当前获取的环境结构特征基于所述初始位姿转换为全局坐标系，作为所述局部特征地图的初始特征；

27、将环境结构特征基于机器人当前位姿转换至全局坐标系，并加入所述局部特征地图，所述环境结构特征包括角点、平面点和地面点；

28、以所述机器人当前位姿对应的位置为中心，确定预设空间范围，将位于所述预设空间范围内的环境结构特征作为当前局部特征地图；

29、随着机器人移动，对所述局部特征地图进行更新，将超出所述预设空间范围的历史环境结构特征移除，将新获取的环境结构特征加入所述局部特征地图。

30、进一步地，所述基于所述机器人运动状态信息进行机器人位姿预测，得到预测位姿，具体包括：

31、获取轮速信息和惯性测量信息；

32、根据所述轮速信息确定线速度和角速度；

33、根据所述线速度和所述角速度以及所述惯性测量信息，并基于上一时刻的机器人位姿确定当前时刻的位姿变化量；

34、根据上一时刻的机器人位姿和所述位姿变化量，确定当前时刻的预测位姿。

35、进一步地，所述根据所述局部特征地图对所述预测位姿进行修正，得到机器人最终位姿，具体包括：

36、将当前时刻的所述预处理点云与所述局部特征地图进行配准，确定当前时刻的所述预处理点云与所述局部特征地图之间的匹配误差；

37、根据所述匹配误差对所述预测位姿进行优化，并基于修正后的位姿与所述预测位姿进行融合，得到所述机器人最终位姿。

38、进一步地，所述将当前时刻的所述预处理点云与所述局部特征地图进行配准，具体包括：

39、确定所述预处理点云与所述局部特征地图之间的可匹配环境结构特征；

40、当所述可匹配环境结构特征的数量大于或等于预设特征数量阈值时，将当前时刻的所述预处理点云中的角点与所述局部特征地图中的角点进行点到线匹配，将当前时刻的所述预处理点云中的平面点与所述局部特征地图中的平面点进行点到面匹配，并根据点到线匹配结果和点到面匹配结果确定匹配误差；

41、当所述可匹配环境结构特征的数量小于预设特征数量阈值时，对所述预处理点云和所述局部特征地图进行整体点云配准，并根据整体点云配准结果确定匹配误差。

42、进一步地，所述根据所述匹配误差对所述预测位姿进行优化，得到所述机器人最终位姿，具体包括：

43、将所述预测位姿作为位姿优化初始值，根据所述匹配误差对所述预测位姿进行迭代优化，得到修正后的位姿；

44、将所述预测位姿作为状态预测结果，将修正后的位姿作为观测结果，基于所述状态预测结果和所述观测结果进行状态融合，得到融合定位结果；

45、将所述融合定位结果作为所述机器人最终位姿。

46、进一步地，所述根据所述机器人最终位姿与预设关键拓扑节点之间的位置关系，对所述机器人最终位姿进行重定位修正，具体包括：

47、在煤矿巷道设置若干预设关键拓扑节点，建立各预设关键拓扑节点对应的全局地图特征；

48、根据所述机器人最终位姿确定机器人与各所述预设关键拓扑节点的距离；

49、当机器人与所述预设关键拓扑节点之间的距离小于预设距离阈值时，将当前环境结构特征与所述预设关键拓扑节点对应的全局地图特征进行匹配；

50、当匹配结果满足预设匹配条件时，根据所述匹配结果确定机器人当前位姿与所述预设关键拓扑节点对应位姿之间的位姿偏差；

51、根据所述位姿偏差对所述机器人最终位姿进行重定位修正。

52、采用上述技术方案后，具有如下有益效果：

53、本技术的一种基于多传感器融合的煤矿巷道机器人导航方法，通过获取煤矿巷道环境观测数据和机器人运动状态信息，提取环境结构特征并构建局部特征地图，结合机器人运动状态进行位姿预测，并利用局部特征地图对预测位姿进行修正融合，从而提高复杂巷道环境下机器人定位的连续性和准确性；进一步通过预设关键拓扑节点对机器人位姿进行重定位修正，降低机器人长距离运行过程中产生的累计定位误差；后续根据重定位后的位姿和局部地图生成候选路径，并基于预设路径评价规则对候选路径进行综合评价和寻优，确定目标路径，兼顾路径安全性、作业效率和行驶性能，提高煤矿巷道机器人的自主导航能力和环境适应性。