一种煤矿井下辅助运输机器人车路协同定位系统及方法

本发明涉及煤矿井下定位，尤其涉及一种煤矿井下辅助运输机器人车路协同定位系统及方法。

背景技术：

1、煤矿辅助运输是指除煤炭之外的各种运输之总和，主要包括：矿井人员、各类设备、矸石等，其是煤炭生产的重要组成部分。

2、近年来我国开始大力推进煤矿辅助运输机器人的自动化、智能化建设，但由于井下gps信号屏蔽，导致地上常见的定位方式如gps配合惯导的方法无法使用，而现有的定位方式，如基于无线传感网的定位技术具有定位精度低、建设成本高、无法连续定位等缺点；而由于煤矿井下存在环境与传感器的退化问题，导致新兴的slam技术（即时定位与地图构建）无法满足辅助运输机器人长期稳定定位需求。可知辅助运输机器人井下定位已经成为制约煤矿智能化建设和无人化发展的关键问题。

技术实现思路

1、为解决上述问题，本发明提供一种煤矿井下辅助运输机器人车路协同定位系统及方法，基于车路协同机制，实现辅助运输机器人与巷道端之间的实时信息交互，可有效提高辅助运输机器人的定位精度，实现煤矿辅助运输机器人无人化、自主化运输。

2、为实现上述目的，本发明提供了一种煤矿井下辅助运输机器人车路协同定位系统，包括：

3、巷道端识别定位装置，搭建于巷道路端，用于采集辅助运输机器人的rgb彩色图像、深度图像和点云信息，并根据采集的辅助运输机器人的rgb彩色图像、深度图像和点云信息基于投票法的六自由度位姿估计完成对机器人绝对坐标系下的识别与定位；

4、自然语义路标辅助定位装置，搭载于在辅助运输机器人上，用于根据采集自然语义路标的rgb彩色图、深度图像和点云信息，基于对应点的六自由度位姿估计方法实现辅助运输机器人在绝对坐标系下的定位；

5、机器人多源信息融合导航装置，装载于辅助运输机器人上，用于根据多种传感器分别构建激光、视觉、编码器/imu航迹推算里程计，并基于巷道端识别定位信息、自然语义路标辅助定位信息以及里程计实现绝对坐标系下的机器人多源信息融合实时定位、建图以及自主导航；

6、矿井辅助运输监控平台，用于构建巷道3d点云模型并动态更新辅助运输机器人的六自由度位姿，完成对辅助运输机器人监控及调度；

7、巷道端识别定位装置和自然语义路标辅助定位装置分别经井下有线光纤环网和5g/wifi6与矿井辅助运输监控平台进行通讯，巷道端识别定位装置和自然语义路标辅助定位装置还分别基于5g/wifi6和光纤与机器人多源信息融合导航装置通讯，机器人多源信息融合导航装置与矿井辅助运输监控平台基于5g/wifi6通讯。

8、优选的，巷道端识别定位装置包括用于拍摄、扫描辅助运输机器人的巷道端感知单元、与巷道端感知单元相连的巷道端边缘计算单元和与巷道端边缘计算单元相连的巷道端通信单元；

9、巷道端感知单元包括rgb-d相机、激光雷达传感器，用于拍摄扫描得到辅助运输机器人的rgb彩色图像、深度图像、点云信息；

10、巷道端边缘计算单元为包括巷道端gpu和与巷道端gpu相连的巷道端计算模块，用于处理辅助运输机器人rgb彩色图像、深度图像与点云信息，并基于投票法的六自由度位姿估计算法，实时完成辅助运输机器人在绝对坐标系下的定位。

11、优选的，自然语义路标辅助定位装置包括自然语义路标、用于感知自然语义路标的机器人端感知单元、机器人端计算单元和机器人端通信单元；

12、自然语义路标为模型的物体坐标系被标定过的井下设备，用于被机器人端感知单元识别后进行辅助定位；

13、机器人端感知单元包括rgb-d相机、激光雷达，用于拍摄扫描自然语义路标的rgb彩色图像、深度图像以及点云信息；

14、机器人端计算单元包括机器人端gpu和与机器人端cpu相连的机器人端计算模块，用于处理自然语义路标rgb彩色图像、深度图像以及点云信息，并基于对应点的六自由度位姿估计算法，实时计算辅助运输机器人在绝对坐标系下的定位信息。

15、优选的，机器人多源信息融合导航装置包括感知单元、运算单元、通讯单元、执行单元；

16、感知单元包括用于实时采集辅助运输机器人周围环境的点云数据、彩色图像、深度图像与红外深度图像的外部感受器和用于实时采集辅助运输机器人自身状态信息的内部感受器，外部感受器包括激光雷达、毫米波雷达、rgb-d相机、深度红外相机；内部感受器用于检测辅助运输机器人加速度、角速度和角位移的惯性传感器；

17、运算单元包括运算gpu和与运算gpu相连的定位与建图计算模块以及自主导航计算模块，其中，定位与建图计算模块，用于实时处理感知单元的传感器数据，运行基于因子图优化的多源信息融合slam方法，实时完成辅助运输机器人的定位、地图构建；自主导航计算模块，用于根据当前位置与目标点完成底层路径规划，并根据运动学模型、动力学模型完成轨迹跟踪与自主避障，计算出相应的运动控制指令；

18、执行单元，执行单元包括伺服电机和执行控制器，用于根据运算单元发送的运动控制指令，执行相应的动作，完成机器人轨迹跟踪。

19、优选的，机器人端通信单元、巷道端通信单元与机器人多源信息融合导航均包括内部通信模块和外部通信模块，内部通信模块采用光纤通信，外部通信模块采用5g通信；

20、内部通信模块用于巷道端感知单元与巷道端边缘计算单元之间的数据传输、机器人端感知单元与机器人端计算单元之间的数据传输、自然语义路标辅助定位装置与机器人多源信息融合导航模块以及巷道端识别定位装置与矿井辅助运输监控平台的数据传输；

21、外部通信模块用于巷道端识别定位装置与机器人多源信息融合导航模块的数据传输以及自然语义路标辅助定位装置和机器人多源信息融合导航模块与矿井辅助运输监控平台之间的定位信息的传输。

22、优选的，矿井辅助运输监控平台包括监控单元、辅助运输调度单元与监控端通信单元；

23、监控单元包括视频监控模块、定位可视化模块和场景模型数据库，视频监控模块用于根据巷道端相机采集的视频流，基于图像识别网络进行实时目标识别，对辅助运输机器人进行实时监控；定位可视化模块用于在数据流层面进行巷道整体信息的汇总，构建巷道3d点云模型并动态更新辅助运输机器人的六自由度位姿；

24、辅助运输调度单元，用于机器人多源信息融合定位信息以及辅助运输任务需求，完成对辅助运输机器人监控及调度；

25、监控端通讯单元采用光纤与wifi通信，用于接收巷道端识别定位装置、自然语义路标辅助定位装置的定位信息，完成与机器人多源信息融合导航装置的信息交互。

26、一种煤矿井下辅助运输机器人车路协同定位系统的方法，包括以下步骤：

27、s1、巷道端相机部署以及自然语义路标模型标定；

28、s2、巷道端识别定位；

29、s3、自然路标辅助定位；

30、s4、多源信息融合slam，完成绝对坐标系下的实时定位与建图；

31、s5、自主导航。

32、优选的，步骤s1具体包括以下步骤：

33、在机器人定位与建图过程中易发生场景和传感器退化的井下区域部署巷道端相机并提取、构建井下设备模型作为自然语义路标，利用物体坐标系标定方法进行自然语义路标模型标定；

34、步骤s2具体包括以下步骤：

35、通过巷道端相机采集辅助运输机器人的rgb彩色图像深度图像与点云信息，并根据基于投票法的六自由度位姿估计的识别定位方法，实时计算获得机器人绝对坐标系下的定位信息，然后将该定位信息实时传输至定位与建图计算模块；

36、步骤s3具体包括以下步骤：

37、通过机器人端相机采集到的自然语义路标的rgb彩色图像、深度图像与点云信息，并根据基于自然语义路标的辅助定位方法获得辅助运输机器人在绝对坐标系下的位置信息，并将该定位信息实时传输至定位与建图计算模块；

38、步骤s4具体包括以下步骤：

39、基于机器人多源信息融合导航装置感知单元的雷达点云信息的帧间约束、帧间时间段内imu信息的约束、相机图像信息的帧间约束、两帧之间回环检测约束以及巷道端识别定位装置全局定位约束和自然语义路标辅助定位装置的全局定位约束，分别构建残差项作为因子，基于因子图优化方法构建最小二乘问题的目标函数如下：

40、;

41、通过求解该最小二乘问题完成多源信息融合slam，完成绝对坐标系下的实时定位与建图；

42、其中：雷达点云信息的帧间约束为通过计算雷达点云曲率，将点云分为面特征点和线特征点，通过构建相邻帧间的面特征点残差项与线特征点残差项构建约束即：

43、;

44、帧间时间段内imu信息的约束为在imu的状态传递方程基础上，构建k-1帧时刻和k帧时间间隔内imu构建残差项，公式如下所示

45、;

46、式中分别表示平移预积分、速度预积分、旋转预积分、加速度偏置、角加速度偏置的误差值；

47、在区间内，速度预积分残差项公式如下所示：

48、;

49、其中，代表重力加速度，代表t时刻旋转矩阵，代表t时刻imu坐标系下的加速度计测量值，代表t时刻的加速度零偏，代表t时刻计算得到的加速度白噪声, 代表t时刻速度，代表时间间隔；

50、在区间内，平移预积分残差项公式如下所示：

51、;

52、其中，代表t时刻imu坐标系在世界坐标系下位置；

53、在区间内，旋转预积分残差项公式如下所示：

54、;

55、其中，是imu坐标系下得到的陀螺仪值，是t时刻imu坐标系下陀螺仪的零偏值，是t时刻imu坐标系下的陀螺仪白噪声；

56、相机图像信息的帧间约束是指利用第k帧的观测信息与根据当前位姿基于观测模型获得的观测信息构建残差项，残差项如下所示：

57、;

58、其中，为当前k时刻的观测数据，为k时刻基于当前位置x通过观测方程获得的观测数据；

59、回环约束为观测信息相似的两个位置之间建立的约束关系，设观测信息相似的两帧点云分别为和，通过对源点云中的每一个点，在目标点云中搜索和其欧式距离最近的对应点，找到对应关系之后构建残差函数如下：

60、;

61、其中，与分别为k时刻的旋转矩阵与平移向量；

62、巷道端识别定位装置全局定位约束为利用巷道端识别定位装置的位姿测量值作为因子，构建残差项如下所示：

63、;

64、式中是slam过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的旋转分量， 是slam过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的平移分量，是t时刻激光里程计估计的在绝对坐标系下的坐标。

65、自然语义路标辅助定位装置的全局定位约束为利用自然语义路标辅助定位装置的位姿测量值作为因子，构建残差项如下所示：

66、;

67、式中，是slam过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的旋转分量， 是slam过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的平移分量，是t时刻slam里程计估计的在绝对坐标系下的坐标；

68、步骤s5具体包括以下步骤：

69、自主导航计算模块根据感知单元的传感器信息，离线生成轨迹库来模拟辅助运输机器人在未来一段时间内可能走过的轨迹，计算轨迹覆盖的空间点与轨迹发生碰撞的可能性，实时选择最优路径，并根据辅助运输机器人运动学模型与动力学模型计算出相应的运动控制指令；

70、在步骤s5中经矿井辅助运输监控平台实时监控：通过对巷道端相机视频流画面以及机器人六自由度位姿，实时观察机器人运输情况以及是否出现事故，通过多源信息融合slam 构建的地图完成巷道井下的3d可视化。

71、优选的，步骤s1中所述的物体坐标系标定方法是指在待标定物体的周围摆放视觉标签，通过相机环绕待标定物体进行数据采集，待完整地采集到物体信息后，利用内参变换将各帧深度信息转化为点云，并将各帧点云投影至初始帧实现自然语义路标的三维重建，获得物体cad模型，进而通过全站仪和标定板标定初始帧相机坐标系外参，从而完成自然语义路标cad模型标定，即物体坐标系标定；

72、其中通过全站仪和标定板标定初始帧相机坐标系外参具体包括以下步骤：利用全站仪读取标定板的特征点坐标，利用机器人端相机获得标定板图像与深度图像后，采用opencv库角点提取函数进行操作，通过对应位置深度值的内参变换获得角点在相机坐标系下的三维坐标；利用全站仪测量角点在世界坐标系下的坐标，通过匹配对应的特征点求得相机坐标系与世界坐标系位姿变换，完成相机坐标系外参标定；

73、步骤s2中所述的基于投票法的六自由度位姿估计的定位方法为：利用全站仪实现巷道相机在绝对坐标系下的标定后，获得机器人cad模型，通过预先训练过的语义分割网络对辅助运输机器人rgb彩色图像以及点云信息进行语义分割，提取辅助运输机器人掩码图像与掩码点云，根据掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云数据；根据分割图像、点云信息利用六自由度位姿估计深度学习网络实现对辅助运输机器人的位姿估计，得到带有置信度的辅助运输机器人六自由度位姿，对置信度最高的位姿进行优化，最后基于机器人物体坐标系与相机坐标系位姿变换以及相机坐标系与绝对坐标系的位姿变换获得机器人在绝对坐标系下的定位信息；

74、步骤s3所述的基于自然语义路标的辅助定位方法为：利用全站仪实现自然语义路标物体坐标系标定后，获得自然语义路标cad模型，通过离线处理，计算自然语义路标cad模型点特征直方图进行特征提取，其次利用预先训练的语义分割网络在线对当前帧自然语义路标的彩色图像以及点云进行分割获得图像掩码与点云掩码；根据分割所得的掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云信息；对自然语义路标点云信息进行特征提取，通过匹配当前帧自然语义路标点云与自然语义路标cad模型特征相似的点对，对当前帧位姿进行初步估计，再次通过icp算法对初步估计位姿进行位姿优化，获得机器人相对于自然语义路标物体坐标系位姿变换，最后通过位姿变换得到机器人在绝对坐标系下的六自由度位姿。

75、优选的，在步骤s3中，当语义分割网络在煤矿井下照明达不到设定条件时，易产生噪声导致语义分割网络分割精度下降出现误识别现象，从而影响定位精度，故在此环境中利用识别网络对机器人rgb图像与自然语义路标rgb图像进行预处理，获取包围框后，对包围框内裁剪图片进行语义分割，从而降低语义分割误差，提高定位精度。

76、本发明具有以下有益效果：

77、1、在煤矿井下，基于车路协同机制（车路协同是采用先进的无线通信和新一代互联网等技术，全方位实施车车、车路动态实时信息交互，并在全时空动态交通信息采集与融合的基础上开展车辆主动安全控制和道路协同管理），通过自然语义路标辅助定位与巷道端识别定位，实现辅助运输机器人在绝对坐标系下的定位，并通过与机器人端激光、视觉、惯导等里程计之间实时信息融合，可获得机器人在绝对坐标系下的位姿，可有效提高机器人定位精度，便于将机器人定位与环境地图融入gis系统，实现煤矿辅助运输机器人无人化、自主化运输。

78、2、基于巷道识别定位信息、基于自然语义路标的辅助定位信息，利用基于因子图优化的多源信息融合slam架设成本低，可消除slam过程产生的累计误差，实现精确稳定的定位与煤矿井下地图构建。

79、下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。