CN116518984B - 一种煤矿井下辅助运输机器人车路协同定位***及方法 - Google Patents
一种煤矿井下辅助运输机器人车路协同定位***及方法 Download PDFInfo
- Publication number
- CN116518984B CN116518984B CN202310814856.1A CN202310814856A CN116518984B CN 116518984 B CN116518984 B CN 116518984B CN 202310814856 A CN202310814856 A CN 202310814856A CN 116518984 B CN116518984 B CN 116518984B
- Authority
- CN
- China
- Prior art keywords
- robot
- positioning
- coordinate system
- auxiliary
- information
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 239000003245 coal Substances 0.000 title claims abstract description 39
- 238000012544 monitoring process Methods 0.000 claims abstract description 56
- 230000004927 fusion Effects 0.000 claims abstract description 54
- 238000012800 visualization Methods 0.000 claims abstract description 10
- 238000004891 communication Methods 0.000 claims description 52
- 230000000875 corresponding effect Effects 0.000 claims description 33
- 230000011218 segmentation Effects 0.000 claims description 27
- 238000004364 calculation method Methods 0.000 claims description 25
- 230000009466 transformation Effects 0.000 claims description 24
- 230000008569 process Effects 0.000 claims description 19
- 230000001133 acceleration Effects 0.000 claims description 15
- 230000005540 biological transmission Effects 0.000 claims description 15
- 238000010276 construction Methods 0.000 claims description 15
- 238000013507 mapping Methods 0.000 claims description 14
- 238000012545 processing Methods 0.000 claims description 14
- 238000005457 optimization Methods 0.000 claims description 12
- 230000006870 function Effects 0.000 claims description 11
- 239000013307 optical fiber Substances 0.000 claims description 11
- 230000010354 integration Effects 0.000 claims description 9
- 238000013519 translation Methods 0.000 claims description 9
- 238000000605 extraction Methods 0.000 claims description 8
- 230000003993 interaction Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000015556 catabolic process Effects 0.000 claims description 5
- 238000006731 degradation reaction Methods 0.000 claims description 5
- 238000005286 illumination Methods 0.000 claims description 5
- 238000005259 measurement Methods 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 4
- 238000013135 deep learning Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 2
- 230000007246 mechanism Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 239000010720 hydraulic oil Substances 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种煤矿井下辅助运输机器人车路协同定位***及方法，属于煤矿井下定位领域，包括：巷道端识别定位装置、自然语义路标辅助定位装置、机器人多源信息融合导航装置和矿井辅助运输监控平台；巷道端识别定位装置和自然语义路标辅助定位装置分别与机器人多源信息融合导航装置以及矿井辅助运输监控平台进行通讯，用于实时传输定位信息；机器人多源信息导航装置与矿井辅助运输监控平台通讯，用于完成定位导航可视化。本发明采用上述煤矿井下辅助运输机器人车路协同定位***及方法，基于车路协同机制，实现辅助运输机器人在绝对坐标系下的定位，有效提高定位精度，实现煤矿辅助运输机器人无人化、自主化。
Description
技术领域
本发明涉及煤矿井下定位技术领域，尤其涉及一种煤矿井下辅助运输机器人车路协同定位***及方法。
背景技术
煤矿辅助运输是指除煤炭之外的各种运输之总和，主要包括：矿井人员、各类设备、矸石等，其是煤炭生产的重要组成部分。
近年来我国开始大力推进煤矿辅助运输机器人的自动化、智能化建设，但由于井下GPS信号屏蔽，导致地上常见的定位方式如GPS配合惯导的方法无法使用，而现有的定位方式，如基于无线传感网的定位技术具有定位精度低、建设成本高、无法连续定位等缺点；而由于煤矿井下存在环境与传感器的退化问题，导致新兴的SLAM技术（即时定位与地图构建）无法满足辅助运输机器人长期稳定定位需求。可知辅助运输机器人井下定位已经成为制约煤矿智能化建设和无人化发展的关键问题。
发明内容
为解决上述问题，本发明提供一种煤矿井下辅助运输机器人车路协同定位***及方法，基于车路协同机制，实现辅助运输机器人与巷道端之间的实时信息交互，可有效提高辅助运输机器人的定位精度，实现煤矿辅助运输机器人无人化、自主化运输。
为实现上述目的，本发明提供了一种煤矿井下辅助运输机器人车路协同定位***，包括：
巷道端识别定位装置，搭建于巷道路端，用于采集辅助运输机器人的RGB彩色图像、深度图像和点云信息，并根据采集的辅助运输机器人的RGB彩色图像、深度图像和点云信息基于投票法的六自由度位姿估计完成对机器人绝对坐标系下的识别与定位；
自然语义路标辅助定位装置，搭载于在辅助运输机器人上，用于根据采集自然语义路标的RGB彩色图、深度图像和点云信息，基于对应点的六自由度位姿估计方法实现辅助运输机器人在绝对坐标系下的定位；
机器人多源信息融合导航装置，装载于辅助运输机器人上，用于根据多种传感器分别构建激光、视觉、编码器/IMU航迹推算里程计，并基于巷道端识别定位信息、自然语义路标辅助定位信息以及里程计实现绝对坐标系下的机器人多源信息融合实时定位、建图以及自主导航；
矿井辅助运输监控平台，用于构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿，完成对辅助运输机器人监控及调度；
巷道端识别定位装置和自然语义路标辅助定位装置分别经井下有线光纤环网和5G/WiFi6与矿井辅助运输监控平台进行通讯，巷道端识别定位装置和自然语义路标辅助定位装置还分别基于5G/WiFi6和光纤与机器人多源信息融合导航装置通讯，机器人多源信息融合导航装置与矿井辅助运输监控平台基于5G/WiFi6通讯。
优选的，巷道端识别定位装置包括用于拍摄、扫描辅助运输机器人的巷道端感知单元、与巷道端感知单元相连的巷道端边缘计算单元和与巷道端边缘计算单元相连的巷道端通信单元；
巷道端感知单元包括RGB-D相机、激光雷达传感器，用于拍摄扫描得到辅助运输机器人的RGB彩色图像、深度图像、点云信息；
巷道端边缘计算单元为包括巷道端GPU和与巷道端GPU相连的巷道端计算模块，用于处理辅助运输机器人RGB彩色图像、深度图像与点云信息，并基于投票法的六自由度位姿估计算法，实时完成辅助运输机器人在绝对坐标系下的定位。
优选的，自然语义路标辅助定位装置包括自然语义路标、用于感知自然语义路标的机器人端感知单元、机器人端计算单元和机器人端通信单元；
自然语义路标为模型的物体坐标系被标定过的井下设备，用于被机器人端感知单元识别后进行辅助定位；
机器人端感知单元包括RGB-D相机、激光雷达，用于拍摄扫描自然语义路标的RGB彩色图像、深度图像以及点云信息；
机器人端计算单元包括机器人端GPU和与机器人端GPU相连的机器人端计算模块，用于处理自然语义路标RGB彩色图像、深度图像以及点云信息，并基于对应点的六自由度位姿估计算法，实时计算辅助运输机器人在绝对坐标系下的定位信息。
优选的，机器人多源信息融合导航装置包括感知单元、运算单元、通讯单元、执行单元；
感知单元包括用于实时采集辅助运输机器人周围环境的点云数据、彩色图像、深度图像与红外深度图像的外部感受器和用于实时采集辅助运输机器人自身状态信息的内部感受器，外部感受器包括激光雷达、毫米波雷达、RGB-D相机、深度红外相机；内部感受器用于检测辅助运输机器人加速度、角速度和角位移的惯性传感器；
运算单元包括运算GPU和与运算GPU相连的定位与建图计算模块以及自主导航计算模块，其中，定位与建图计算模块，用于实时处理感知单元的传感器数据，运行基于因子图优化的多源信息融合SLAM方法，实时完成辅助运输机器人的定位、地图构建；自主导航计算模块，用于根据当前位置与目标点完成底层路径规划，并根据运动学模型、动力学模型完成轨迹跟踪与自主避障，计算出相应的运动控制指令；
执行单元，执行单元包括伺服电机和执行控制器，用于根据运算单元发送的运动控制指令，执行相应的动作，完成机器人轨迹跟踪。
优选的，机器人端通信单元、巷道端通信单元与机器人多源信息融合导航均包括内部通信模块和外部通信模块，内部通信模块采用光纤通信，外部通信模块采用5G通信；
内部通信模块用于巷道端感知单元与巷道端边缘计算单元之间的数据传输、机器人端感知单元与机器人端计算单元之间的数据传输、自然语义路标辅助定位装置与机器人多源信息融合导航模块以及巷道端识别定位装置与矿井辅助运输监控平台的数据传输；
外部通信模块用于巷道端识别定位装置与机器人多源信息融合导航模块的数据传输以及自然语义路标辅助定位装置和机器人多源信息融合导航模块与矿井辅助运输监控平台之间的定位信息的传输。
优选的，矿井辅助运输监控平台包括监控单元、辅助运输调度单元与监控端通信单元；
监控单元包括视频监控模块、定位可视化模块和场景模型数据库，视频监控模块用于根据巷道端相机采集的视频流，基于图像识别网络进行实时目标识别，对辅助运输机器人进行实时监控；定位可视化模块用于在数据流层面进行巷道整体信息的汇总，构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿；
辅助运输调度单元，用于机器人多源信息融合定位信息以及辅助运输任务需求，完成对辅助运输机器人监控及调度；
监控端通讯单元采用光纤与WiFi通信，用于接收巷道端识别定位装置、自然语义路标辅助定位装置的定位信息，完成与机器人多源信息融合导航装置的信息交互。
一种煤矿井下辅助运输机器人车路协同定位***的方法，包括以下步骤：
S1、巷道端相机部署以及自然语义路标模型标定；
S2、巷道端识别定位；
S3、自然路标辅助定位；
S4、多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
S5、自主导航。
优选的，步骤S1具体包括以下步骤：
在机器人定位与建图过程中易发生场景和传感器退化的井下区域部署巷道端相机并提取、构建井下设备模型作为自然语义路标，利用物体坐标系标定方法进行自然语义路标模型标定；
步骤S2具体包括以下步骤：
通过巷道端相机采集辅助运输机器人的RGB彩色图像深度图像与点云信息，并根据基于投票法的六自由度位姿估计的识别定位方法，实时计算获得机器人绝对坐标系下的定位信息，然后将该定位信息实时传输至定位与建图计算模块；
步骤S3具体包括以下步骤：
通过机器人端相机采集到的自然语义路标的RGB彩色图像、深度图像与点云信息，并根据基于自然语义路标的辅助定位方法获得辅助运输机器人在绝对坐标系下的位置信息，并将该定位信息实时传输至定位与建图计算模块；
步骤S4具体包括以下步骤：
基于机器人多源信息融合导航装置感知单元的雷达点云信息的帧间约束、帧间时间段内IMU信息的约束、相机图像信息的帧间约束、两帧之间回环检测约束以及巷道端识别定位装置全局定位约束和自然语义路标辅助定位装置的全局定位约束，分别构建残差项作为因子，基于因子图优化方法构建最小二乘问题的目标函数如下：
；
通过求解该最小二乘问题完成多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
其中：雷达点云信息的帧间约束为通过计算雷达点云曲率，将点云分为面特征点和线特征点，通过构建相邻帧间的面特征点残差项与线特征点残差项/>构建约束即：
；
帧间时间段内IMU信息的约束为在IMU的状态传递方程基础上，构建k-1帧时刻和k帧时间间隔内IMU构建残差项，公式如下所示
；
式中、/>、/>、/>、/>分别表示平移预积分、速度预积分、旋转预积分、加速度偏置、角加速度偏置的误差值；
在区间内，速度预积分残差项公式如下所示：
；
其中，代表重力加速度，/>代表t时刻旋转矩阵，/>代表t时刻IMU坐标系下的加速度计测量值，/>代表t时刻的加速度零偏，/>代表t时刻计算得到的加速度白噪声, />代表t时刻速度，/>代表时间间隔；
在区间内，平移预积分残差项公式如下所示：
；
其中，代表t时刻IMU坐标系在世界坐标系下位置；
在区间内，旋转预积分残差项公式如下所示：
；
其中，是IMU坐标系下得到的陀螺仪值，/>是t时刻IMU坐标系下陀螺仪的零偏值，/>是t时刻IMU坐标系下的陀螺仪白噪声；
相机图像信息的帧间约束是指利用第K帧的观测信息与根据当前位姿基于观测模型获得的观测信息构建残差项，残差项如下所示：
；
其中，为当前k时刻的观测数据，/>为k时刻基于当前位置X通过观测方程获得的观测数据；
回环约束为观测信息相似的两个位置之间建立的约束关系，设观测信息相似的两帧点云分别为和/>，通过对源点云/>中的每一个点/>，在目标点云/>中搜索和其欧式距离最近的对应点/>，找到对应关系之后构建残差函数如下：
；
其中，与/>分别为k时刻的旋转矩阵与平移向量；
巷道端识别定位装置全局定位约束为利用巷道端识别定位装置的位姿测量值作为因子，构建残差项如下所示：
；
式中，是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的旋转分量， />是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的平移分量，/>是t时刻激光里程计估计的在绝对坐标系下的坐标。
自然语义路标辅助定位装置的全局定位约束为利用自然语义路标辅助定位装置的位姿测量值作为因子，构建残差项如下所示：
；
式中，是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的旋转分量，/> 是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的平移分量，/>是t时刻SLAM里程计估计的在绝对坐标系下的坐标；
步骤S5具体包括以下步骤：
自主导航计算模块根据感知单元的传感器信息，离线生成轨迹库来模拟辅助运输机器人在未来一段时间内可能走过的轨迹，计算轨迹覆盖的空间点与轨迹发生碰撞的可能性，实时选择最优路径，并根据辅助运输机器人运动学模型与动力学模型计算出相应的运动控制指令；
在步骤S5中经矿井辅助运输监控平台实时监控：通过对巷道端相机视频流画面以及机器人六自由度位姿，实时观察机器人运输情况以及是否出现事故，通过多源信息融合SLAM 构建的地图完成巷道井下的3D可视化。
优选的，步骤S1中所述的物体坐标系标定方法是指在待标定物体的周围摆放视觉标签，通过相机环绕待标定物体进行数据采集，待完整地采集到物体信息后，利用内参变换将各帧深度信息转化为点云，并将各帧点云投影至初始帧实现自然语义路标的三维重建，获得物体CAD模型，进而通过全站仪和标定板标定初始帧相机坐标系外参，从而完成自然语义路标CAD模型标定，即物体坐标系标定；
其中通过全站仪和标定板标定初始帧相机坐标系外参具体包括以下步骤：利用全站仪读取标定板的特征点坐标，利用机器人端相机获得标定板图像与深度图像后，采用OpenCV库角点提取函数进行操作，通过对应位置深度值的内参变换获得角点在相机坐标系下的三维坐标；利用全站仪测量角点在世界坐标系下的坐标，通过匹配对应的特征点求得相机坐标系与世界坐标系位姿变换，完成相机坐标系外参标定；
步骤S2中所述的基于投票法的六自由度位姿估计的定位方法为：利用全站仪实现巷道相机在绝对坐标系下的标定后，获得机器人CAD模型，通过预先训练过的语义分割网络对辅助运输机器人RGB彩色图像以及点云信息进行语义分割，提取辅助运输机器人掩码图像与掩码点云，根据掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云数据；根据分割图像、点云信息利用六自由度位姿估计深度学习网络实现对辅助运输机器人的位姿估计，得到带有置信度的辅助运输机器人六自由度位姿，对置信度最高的位姿进行优化，最后基于机器人物体坐标系与相机坐标系位姿变换以及相机坐标系与绝对坐标系的位姿变换获得机器人在绝对坐标系下的定位信息；
步骤S3所述的基于自然语义路标的辅助定位方法为：利用全站仪实现自然语义路标物体坐标系标定后，获得自然语义路标CAD模型，通过离线处理，计算自然语义路标CAD模型点特征直方图进行特征提取，其次利用预先训练的语义分割网络在线对当前帧自然语义路标的彩色图像以及点云进行分割获得图像掩码与点云掩码；根据分割所得的掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云信息；对自然语义路标点云信息进行特征提取，通过匹配当前帧自然语义路标点云与自然语义路标CAD模型特征相似的点对，对当前帧位姿进行初步估计，再次通过ICP算法对初步估计位姿进行位姿优化，获得机器人相对于自然语义路标物体坐标系位姿变换，最后通过位姿变换得到机器人在绝对坐标系下的六自由度位姿。
优选的，在步骤S3中，当语义分割网络在煤矿井下照明达不到设定条件时，易产生噪声导致语义分割网络分割精度下降出现误识别现象，从而影响定位精度，故在此环境中利用识别网络对机器人RGB图像与自然语义路标RGB图像进行预处理，获取包围框后，对包围框内裁剪图片进行语义分割，从而降低语义分割误差，提高定位精度。
本发明具有以下有益效果：
1、在煤矿井下，基于车路协同机制（车路协同是采用先进的无线通信和新一代互联网等技术，全方位实施车车、车路动态实时信息交互，并在全时空动态交通信息采集与融合的基础上开展车辆主动安全控制和道路协同管理），通过自然语义路标辅助定位与巷道端识别定位，实现辅助运输机器人在绝对坐标系下的定位，并通过与机器人端激光、视觉、惯导等里程计之间实时信息融合，可获得机器人在绝对坐标系下的位姿，可有效提高机器人定位精度，便于将机器人定位与环境地图融入GIS***，实现煤矿辅助运输机器人无人化、自主化运输。
2、基于巷道识别定位信息、基于自然语义路标的辅助定位信息，利用基于因子图优化的多源信息融合SLAM架设成本低，可消除SLAM过程产生的累计误差，实现精确稳定的定位与煤矿井下地图构建。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
图1为本发明的一种煤矿井下辅助运输机器人车路协同定位***的结构框图；
图2为本发明的一种煤矿井下辅助运输机器人车路协同定位***的方法的流程图。
具体实施方式
以下将结合附图对本发明作进一步的描述，需要说明的是，本实施例以本技术方案为前提，给出了详细的实施方式和具体的操作过程，但本发明的保护范围并不限于本实施例。
如图1所示，一种煤矿井下辅助运输机器人车路协同定位***，包括：
巷道端识别定位装置，搭建于巷道路端，用于采集辅助运输机器人的RGB彩色图像、深度图像和点云信息，并根据采集的辅助运输机器人的RGB彩色图像、深度图像和点云信息基于投票法的六自由度位姿估计完成对机器人绝对坐标系下的识别与定位；
自然语义路标辅助定位装置，搭载于在辅助运输机器人上，用于根据采集自然语义路标的RGB彩色图、深度图像和点云信息，基于对应点的六自由度位姿估计方法实现辅助运输机器人在绝对坐标系下的定位；
机器人多源信息融合导航装置，装载于辅助运输机器人上，用于根据多种传感器分别构建激光、视觉、编码器/IMU航迹推算里程计，并基于巷道端识别定位信息、自然语义路标辅助定位信息以及里程计实现绝对坐标系下的机器人多源信息融合实时定位、建图以及自主导航；
矿井辅助运输监控平台，用于构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿，并完成对辅助运输机器人监控及调度；
巷道端识别定位装置和自然语义路标辅助定位装置分别经井下有线光纤环网和5G/WiFi6与矿井辅助运输监控平台进行通讯，巷道端识别定位装置和自然语义路标辅助定位装置还分别基于5G/WiFi6和光纤与机器人多源信息融合导航装置通讯，机器人多源信息融合导航装置与矿井辅助运输监控平台基于5G/WiFi6通讯。
具体的，巷道端识别定位装置包括用于拍摄、扫描辅助运输机器人的巷道端感知单元、与巷道端感知单元相连的巷道端边缘计算单元和与巷道端边缘计算单元相连的巷道端通信单元；巷道端感知单元包括RGB-D相机、激光雷达传感器，用于拍摄扫描得到辅助运输机器人的RGB彩色图像、深度图像、点云信息；巷道端边缘计算单元为包括巷道端GPU和与巷道端GPU相连的巷道端计算模块，用于处理辅助运输机器人RGB彩色图像、深度图像与点云信息，并基于投票法的六自由度位姿估计算法，实时完成辅助运输机器人在绝对坐标系下的定位。
优选的，自然语义路标辅助定位装置包括自然语义路标、用于感知自然语义路标的机器人端感知单元、机器人端计算单元和机器人端通信单元；自然语义路标为模型的物体坐标系被标定过的井下设备，用于被机器人端感知单元识别后进行辅助定位；机器人端感知单元包括RGB-D相机、激光雷达，用于拍摄扫描自然语义路标的RGB彩色图像、深度图像以及点云信息；机器人端计算单元包括机器人端GPU和与机器人端GPU相连的机器人端计算模块，用于处理自然语义路标RGB彩色图像、深度图像以及点云信息，并基于对应点的六自由度位姿估计算法，实时计算辅助运输机器人在绝对坐标系下的定位信息。
本实施例中的自然语义路标为井下设备（井下物体坐标系已标定的特征明显、位置固定、不易被破坏的井下自然特征或巷道机械设备，包括但不限于通风机、风门、供电设备、液压支架等，可以被机器人端计算与通讯单元识别并进行辅助定位）。
优选的，机器人多源信息融合导航装置包括感知单元、运算单元、通讯单元、执行单元；感知单元包括用于实时采集辅助运输机器人周围环境的点云数据、彩色图像、深度图像与红外深度图像的外部感受器和用于实时采集辅助运输机器人自身状态信息的内部感受器，外部感受器包括激光雷达、毫米波雷达、RGB-D相机、深度红外相机；内部感受器用于检测辅助运输机器人加速度、角速度和角位移的惯性传感器；运算单元包括运算GPU和与运算GPU相连的定位与建图计算模块以及自主导航计算模块，其中，定位与建图计算模块，用于实时处理感知单元的传感器数据，运行基于因子图优化的多源信息融合SLAM方法，实时完成辅助运输机器人的定位、地图构建；自主导航计算模块，用于根据当前位置与目标点完成底层路径规划，并根据运动学模型、动力学模型完成轨迹跟踪与自主避障，计算出相应的运动控制指令；执行单元，执行单元包括伺服电机和执行控制器，用于根据运算单元发送的运动控制指令，执行相应的动作（如控制刹车、油门、转向机构以及液压油缸、电动推杆、泵、阀等装置的动作），完成机器人轨迹跟踪。
优选的，机器人端通信单元、巷道端通信单元以及机器人多源信息融合导航装置通讯单元均包括内部通信模块和外部通信模块，内部通信模块采用光纤通信，外部通信模块采用5G通信；内部通信模块用于巷道端感知单元与巷道端边缘计算单元之间的数据传输、机器人端感知单元与机器人端计算单元之间的数据传输、自然语义路标辅助定位装置与机器人多源信息融合导航模块以及巷道端识别定位装置与矿井辅助运输监控平台的数据传输；外部通信模块用于巷道端识别定位装置与机器人多源信息融合导航模块的数据传输以及自然语义路标辅助定位装置和机器人多源信息融合导航模块与矿井辅助运输监控平台之间的定位信息的传输。
优选的，矿井辅助运输监控平台包括监控单元、辅助运输调度单元与监控端通信单元；监控单元包括视频监控模块、定位可视化模块和场景模型数据库，视频监控模块用于根据巷道端相机采集的视频流，基于图像识别网络进行实时目标识别，对辅助运输机器人进行实时监控；定位可视化模块用于根据巷道识别定位***监测信息、自然路标辅助定位***的机器人端运算模块监测信息以及机器人端的感知-定位-导航-控制信息，在数据流层面进行巷道整体信息的汇总，构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿；场景模型数据库包括由定位与建图计算模块、固定式场景更新与边缘计算装置执行多模态信息融合SLAM而产生并上传的栅格、特征、语义多尺度地图等数据，实现工作场景巷道模型、设备模型的动态重建与实时更新；辅助运输调度单元，用于根据机器人多源信息融合定位信息以及辅助运输任务需求，完成对辅助运输机器人监控及调度；监控端通讯单元采用WiFi通信，用于接收巷道端识别定位装置、自然语义路标辅助定位装置的定位信息，完成与机器人多源信息融合导航装置的信息交互。
如图2所示，一种煤矿井下辅助运输机器人车路协同定位***的方法，包括以下步骤：
S1、巷道端相机部署以及自然语义路标模型标定；
优选的，步骤S1具体包括以下步骤：
在机器人定位与建图过程中易发生场景和传感器退化的井下区域部署巷道端相机并提取、构建井下设备模型作为自然语义路标，利用物体坐标系标定方法进行自然语义路标模型标定；
在本实施例中，在机器人定位与建图过程中易发生场景和传感器退化的井下区域（部署巷道端相机并提取、构建井下固定物体为模型作为自然语义路标、构建巷道交汇处岔口、通风机、风门、供电设备等特征明显、位置固定、不易被破坏的井下自然特征或巷道机械设备模型作为自然语义路标），利用物体坐标系标定方法进行自然语义路标模型标定；
优选的，步骤S1中所述的物体坐标系标定方法是指在待标定物体的周围摆放视觉标签（如二维码，通过单帧的二维码标签位置求解相对于初始帧位姿），通过机器人端相机环绕待标定物体进行数据采集，待完整地采集到物体信息后，利用内参变换将各帧深度信息转化为点云，并将各帧点云投影至初始帧实现自然语义路标的三维重建，获得物体CAD模型，进而通过全站仪和标定板标定初始帧相机坐标系外参，从而完成自然语义路标CAD模型标定，即物体坐标系标定；
其中通过全站仪和标定板标定初始帧相机坐标系外参具体包括以下步骤：利用全站仪读取标定板的特征点坐标，利用机器人端相机获得标定板图像与深度图像后，采用OpenCV库角点提取函数进行操作,得到特征点的相机坐标系下坐标为/>，通过对应位置深度值的内参变换获得角点在相机坐标系下的三维坐标/>；利用全站仪测量角点在世界坐标系下的坐标，通过匹配对应的特征点得到：
；
其中，R为相机坐标系到世界坐标系的旋转矩阵，t为相机坐标系到世界坐标系的平移向量，利用非线性优化方法求解获得R、t，从而求得相机坐标系与世界坐标系位姿变换，完成相机坐标系外参标定；
S2、巷道端识别定位；
步骤S2具体包括以下步骤：
通过巷道端相机采集辅助运输机器人的RGB彩色图像深度图像、与点云信息，并根据基于投票法的六自由度位姿估计的识别定位方法，实时计算获得机器人绝对坐标系下的定位信息，然后将该定位信息实时传输至定位与建图计算模块；
步骤S2中所述的基于投票法的六自由度位姿估计的定位方法为：利用全站仪实现巷道相机在绝对坐标系下的标定，获得机器人CAD模型，照明良好区域通过预先训练过的DeeplabV3+网络对辅助运输机器人RGB彩色图像以及点云信息进行语义分割，提取辅助运输机器人分割图像与掩码点云；照明差的区域通过darknet_ros网络对辅助运输机器人RGB彩色图像进行预处理，提取包围框后，对包围框内图像进行语义分割处理，获得掩码图像；根据掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云数据；对分割图像的颜色信息与点云数据的几何信息进行编码（本实施例中利用卷积神经网络与PointNet对所述分割图像与上述部分点云进行编码），根据点云体素与图像像素的对应关系进行编码信息融合获得融合特征，根据融合特征利用六自由度位姿估计深度学习网络实现对辅助运输机器人的位姿估计，得到带有置信度的辅助运输机器人六自由度位姿，对置信度最高的位姿进行优化，最后基于机器人物体坐标系与相机坐标系位姿变换以及相机坐标系与绝对坐标系的位姿变换获得机器人在绝对坐标系下的定位信息；
S3、自然路标辅助定位；
步骤S3具体包括以下步骤：
通过机器人端相机采集到的自然语义路标的RGB彩色图像、深度图像与点云信息，并根据基于自然语义路标的辅助定位方法获得辅助运输机器人在绝对坐标系下的位置信息，并将该定位信息实时传输至定位与建图计算模块；
在本实施例中，照明良好区域利用预先训练过的DeeplabV3+网络对所述自然语义路标RGB图像进行语义分割，获得分割掩码；照明差的区域通过darknet_ros网络对自然语义路标RGB彩色图像进行目标识别，提取自然语义路标RGB图像包围框，对包围框内图像进行语义分割处理，获得掩码图像；通过该分割掩码可获得所述自然语义路标在RGB图像中位置，利用分割掩码对应位置的深度值通过内参变换将深度信息转化为3D点云，从而获得相机坐标系下的自然语义路标上述点云，求解当前帧点云与所述自然语义路标CAD模型点云的点特征直方图，将点特征直方图相似的点对进行匹配（本实施例中利用SAC-IA算法对匹配点对进行配准，求解当前帧点云与CAD模型的位姿变换，实现点云粗配准），当前帧特征点云与自然语义路标CAD模型利用迭代最近点算法进行点云匹配，并根据基于自然语义路标的辅助定位方法获得辅助运输机器人在绝对坐标系下的位置信息，并将该定位信息实时传输至定位与建图计算模块；
在步骤S3中，当语义分割网络在煤矿井下照明达不到设定条件时，易产生噪声导致语义分割网络分割精度下降出现误识别现象，从而影响定位精度，故在此环境中利用识别网络对机器人RGB图像与自然语义路标RGB图像进行预处理，获取包围框后，对包围框内裁剪图片进行语义分割，从而降低语义分割误差，提高定位精度。
迭代最近点算法(Iterative Closest Point ,ICP)具体为：通过求当前帧点云和所述CAD模型点云之间的对应点对，基于对应点对和点云粗配准获得位姿变换初值，构造旋转平移矩阵，并利用所求矩阵，将当前帧云变换到所述CAD模型点云的坐标系下，即所述自然语义路标物体坐标系下，估计变换后当前帧点云与所述CAD模型点云的误差函数，若误差函数值大于阈值，则迭代进行上述运算直到满足给定的误差要求。
步骤S3所述的基于自然语义路标的辅助定位方法为：首先通过离线处理，计算机器人CAD模型点特征直方图进行特征提取，其次在线对当前帧机器人点云进行特征提取，通过匹配当前帧点云与机器人CAD模型特征相似的点对，对当前帧位姿进行初步估计，再次通过ICP算法对初步估计位姿进行位姿优化，获得自然语义路标CAD模型，进而获得机器人相对于自然语义路标物体坐标系位姿变换，最后通过位姿变换得到机器人在绝对坐标系下的六自由度位姿；
S4、多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
步骤S4具体包括以下步骤：
基于机器人多源信息融合导航装置感知单元的雷达点云信息的帧间约束、帧间时间段内IMU信息的约束、相机图像信息的帧间约束、两帧之间回环检测约束以及巷道端识别定位装置全局定位约束和自然语义路标辅助定位装置的全局定位约束，分别构建残差项作为因子，基于因子图优化方法构建最小二乘问题的目标函数如下：
；
通过求解该最小二乘问题完成多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
其中：雷达点云信息的帧间约束为通过计算雷达点云曲率，将点云分为面特征点和线特征点，通过构建相邻帧间的面特征点残差项与线特征点残差项/>构建约束即：
；
帧间时间段内IMU信息的约束为在IMU的状态传递方程基础上，构建k-1帧时刻和k帧时间间隔内IMU构建残差项，公式如下所示
；
式中、/>、/>、/>、/>分别表示平移预积分、速度预积分、旋转预积分、加速度偏置、角加速度偏置的误差值；
相机图像信息的帧间约束是指利用第K帧的观测信息与根据当前位姿基于观测模型获得的观测信息构建残差项，残差项如下所示：
；
回环约束为观测信息相似的两个位置之间建立的约束关系，设观测信息相似的两帧点云分别为和/>，通过对源点云/>中的每一个点/>，在目标点云/>中搜索和其欧式距离最近的对应点/>，找到对应关系之后构建残差函数如下：
；
巷道端识别定位装置全局定位约束为利用巷道端识别定位装置的位姿测量值作为因子，构建残差项如下所示：
；
式中，是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的旋转分量，/>是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的平移分量，/>是t时刻激光里程计估计的在绝对坐标系下的坐标。
自然语义路标辅助定位装置的全局定位约束为利用自然语义路标辅助定位装置的位姿测量值作为因子，构建残差项如下所示：
；/>
式中，是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的旋转分量，/>是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的平移分量，/>是t时刻SLAM里程计估计的在绝对坐标系下的坐标。
S5、自主导航。
步骤S5具体包括以下步骤：
自主导航计算模块根据感知单元的传感器信息，离线生成轨迹库来模拟辅助运输机器人在未来一段时间内可能走过的轨迹，计算轨迹覆盖的空间点与轨迹发生碰撞的可能性，实时选择最优路径，并根据辅助运输机器人运动学模型与动力学模型计算出相应的运动控制指令；
且在辅助运输机器人运动过程中，激光雷达、IMU分别采集周围环境点云与自身运动状态信息，并利用IMU完成对激光雷达点云的运动补偿；（由于直接使用原始点云进行匹配计算成本太高，无法保证***的实时性对，故采取 特征提取方法，使用较少的边缘点和平面点便能全面的反映出环境特征，避免使用全部点云参与计算，并且，在对每个扫描划分子区域选择特征点之前，应尽可能剔除特征点的噪声点，保证所选特征点的准确性和稳定性。）通过计算曲率值筛选出最能表达环境中边、平面等几何特征的点，根据相邻时刻的帧间匹配，基于ICP算法实时更新位姿，完成激光里程计的搭建，并将激光里程计位置信息、IMU位置信息、巷道端识别定位信息、基于自然语义路标的定位信息以及回环检测信息作为因子，利用基于因子图优化的方法，对辅助运输机器人定位与建图结果进行优化，构建多源信息融合SLAM***。根据当前位置与目标点完成路径规划，路径追踪，自主避障，实现所述辅助运输机器人自主导航，完成辅助运输任务；
在步骤S5中经矿井辅助运输监控平台实时监控：通过对巷道端相机视频流画面以及机器人六自由度位姿，实时观察机器人运输情况以及是否出现事故，通过多源信息融合SLAM 构建的地图完成巷道井下的3D可视化。
因此，本发明采用上述煤矿井下辅助运输机器人车路协同定位***及方法，基于车路协同机制，实现辅助运输机器人与巷道端之间的实时信息交互，可有效提高辅助运输机器人的定位精度，实现煤矿辅助运输机器人无人化、自主化运输。
最后应说明的是：以上实施例仅用以说明本发明的技术方案而非对其进行限制，尽管参照较佳实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对本发明的技术方案进行修改或者等同替换，而这些修改或者等同替换亦不能使修改后的技术方案脱离本发明技术方案的精神和范围。
Claims (8)
1.一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：包括：
巷道端识别定位装置，搭建于巷道路端，用于采集辅助运输机器人的RGB彩色图像、深度图像和点云信息，并根据采集的辅助运输机器人的RGB彩色图像、深度图像和点云信息基于投票法的六自由度位姿估计完成对机器人绝对坐标系下的识别与定位；
自然语义路标辅助定位装置，搭载于在辅助运输机器人上，用于根据采集自然语义路标的RGB彩色图深度图像和点云信息，基于对应点的六自由度位姿估计方法实现辅助运输机器人在绝对坐标系下的定位；
机器人多源信息融合导航装置，装载于辅助运输机器人上，用于根据多种传感器分别构建激光、视觉、编码器/IMU航迹推算里程计，并基于巷道端识别定位信息、自然语义路标辅助定位信息以及里程计实现绝对坐标系下的机器人多源信息融合实时定位、建图以及自主导航；
矿井辅助运输监控平台，用于构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿，完成对辅助运输机器人监控及调度；
巷道端识别定位装置和自然语义路标辅助定位装置分别经井下有线光纤环网和5G/WiFi6与矿井辅助运输监控平台进行通讯，巷道端识别定位装置和自然语义路标辅助定位装置还分别基于5G/WiFi6和光纤与机器人多源信息融合导航装置通讯，机器人多源信息融合导航装置与矿井辅助运输监控平台基于5G/WiFi6通讯；
煤矿井下辅助运输机器人车路协同定位方法，应用上述煤矿井下辅助运输机器人车路协同定位***；
包括以下步骤：
S1、巷道端相机部署以及自然语义路标模型标定；
S2、巷道端识别定位；
S3、自然路标辅助定位；
S4、多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
S5、自主导航；
步骤S1具体包括以下步骤：
在机器人定位与建图过程中易发生场景和传感器退化的井下区域部署巷道端相机并提取、构建井下设备模型作为自然语义路标，利用物体坐标系标定方法进行自然语义路标模型标定；
步骤S2具体包括以下步骤：
通过巷道端相机采集辅助运输机器人的RGB彩色图像深度图像与点云信息，并根据基于投票法的六自由度位姿估计的识别定位方法，实时计算获得机器人绝对坐标系下的定位信息，然后将该定位信息实时传输至定位与建图计算模块；
步骤S3具体包括以下步骤：
通过机器人端相机采集到的自然语义路标的RGB彩色图像、深度图像与点云信息，并根据基于自然语义路标的辅助定位方法获得辅助运输机器人在绝对坐标系下的位置信息，并将该定位信息实时传输至定位与建图计算模块；
步骤S4具体包括以下步骤：
基于机器人多源信息融合导航装置感知单元的雷达点云信息的帧间约束、帧间时间段内IMU信息的约束、相机图像信息的帧间约束、两帧之间回环检测约束以及巷道端识别定位装置全局定位约束和自然语义路标辅助定位装置的全局定位约束，分别构建残差项作为因子，基于因子图优化方法构建最小二乘问题的目标函数如下：
；
通过求解该最小二乘问题完成多源信息融合SLAM，完成绝对坐标系下的实时定位与建图；
其中：雷达点云信息的帧间约束为通过计算雷达点云曲率，将点云分为面特征点和线特征点，通过构建相邻帧间的面特征点残差项与线特征点残差项/>构建约束即：
；
帧间时间段内IMU信息的约束为在IMU的状态传递方程基础上，构建k-1帧时刻和k帧时间间隔内IMU构建残差项，公式如下所示
；
式中、/>、/>、/>、/>分别表示平移预积分、速度预积分、旋转预积分、加速度偏置、角加速度偏置的误差值；
在区间内，速度预积分残差项公式如下所示：
；
其中，代表重力加速度，/>代表t时刻旋转矩阵，/>代表t时刻IMU坐标系下的加速度计测量值，/>代表t时刻的加速度零偏，/>代表t时刻计算得到的加速度白噪声, />代表t时刻速度，/>代表时间间隔；
在区间内，平移预积分残差项公式如下所示：
；
其中，代表t时刻IMU坐标系在世界坐标系下位置；
在区间内，旋转预积分残差项公式如下所示：
；
其中，是IMU坐标系下得到的陀螺仪值，/>是t时刻IMU坐标系下陀螺仪的零偏值，/>是t时刻IMU坐标系下的陀螺仪白噪声；
相机图像信息的帧间约束是指利用第K帧的观测信息与根据当前位姿基于观测模型获得的观测信息构建残差项，残差项如下所示：
；
其中，为当前k时刻的观测数据，/>为k时刻基于当前位置X通过观测方程获得的观测数据；
回环约束为观测信息相似的两个位置之间建立的约束关系，设观测信息相似的两帧点云分别为和/>，通过对源点云/>中的每一个点/>，在目标点云/>中搜索和其欧式距离最近的对应点/>，找到对应关系之后构建残差函数如下：
；
其中，与/>分别为k时刻的旋转矩阵与平移向量；
巷道端识别定位装置全局定位约束为利用巷道端识别定位装置的位姿测量值作为因子，构建残差项如下所示：
；
式中，是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的旋转分量， />是SLAM过程中建立的相对坐标系到巷道端识别定位装置的绝对坐标系的平移分量，/>是t时刻激光里程计估计的在绝对坐标系下的坐标；
自然语义路标辅助定位装置的全局定位约束为利用自然语义路标辅助定位装置的位姿测量值作为因子，构建残差项如下所示：
；
式中，是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的旋转分量，/>是SLAM过程中建立的相对坐标系到自然语义路标辅助定位装置的绝对坐标系的平移分量，/>是t时刻SLAM里程计估计的在绝对坐标系下的坐标；
步骤S5具体包括以下步骤：
自主导航计算模块根据感知单元的传感器信息，离线生成轨迹库来模拟辅助运输机器人在未来一段时间内可能走过的轨迹，计算轨迹覆盖的空间点与轨迹发生碰撞的可能性，实时选择最优路径，并根据辅助运输机器人运动学模型与动力学模型计算出相应的运动控制指令；
在步骤S5中经矿井辅助运输监控平台实时监控：通过对巷道端相机视频流画面以及机器人六自由度位姿，实时观察机器人运输情况以及是否出现事故，通过多源信息融合SLAM构建的地图完成巷道井下的3D可视化。
2.根据权利要求1所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：巷道端识别定位装置包括用于拍摄、扫描辅助运输机器人的巷道端感知单元、与巷道端感知单元相连的巷道端边缘计算单元和巷道端通信单元；
巷道端感知单元包括RGB-D相机、激光雷达，用于拍摄扫描得到辅助运输机器人的RGB彩色图像、深度图像、点云信息；
巷道端边缘计算单元为包括巷道端GPU和与巷道端GPU相连的巷道端计算模块，用于处理辅助运输机器人RGB彩色图像、深度图像与点云信息，并基于投票法的六自由度位姿估计算法，实时完成辅助运输机器人在绝对坐标系下的定位。
3.根据权利要求2所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：自然语义路标辅助定位装置包括自然语义路标、用于感知自然语义路标的机器人端感知单元、机器人端计算单元和机器人端通信单元；
自然语义路标为模型的物体坐标系被标定过的井下设备，用于被机器人端感知单元识别后进行辅助定位；
机器人端感知单元包括RGB-D相机、激光雷达，用于拍摄扫描自然语义路标的RGB彩色图像、深度图像以及点云信息；
机器人端计算单元包括机器人端GPU和与机器人端GPU相连的机器人端计算模块，用于处理自然语义路标RGB彩色图像、深度图像以及点云信息，并基于对应点的六自由度位姿估计算法，实时计算辅助运输机器人在绝对坐标系下的定位信息。
4.根据权利要求1所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：机器人多源信息融合导航装置包括感知单元、运算单元、通讯单元、执行单元；
感知单元包括用于实时采集辅助运输机器人周围环境的点云数据、彩色图像、深度图像与红外深度图像的外部感受器和用于实时采集辅助运输机器人自身状态信息的内部感受器，外部感受器包括激光雷达、毫米波雷达、RGB-D相机、深度红外相机；内部感受器用于检测辅助运输机器人加速度、角速度和角位移的惯性传感器；
运算单元包括运算GPU和与运算GPU相连的定位与建图计算模块以及自主导航计算模块，其中，定位与建图计算模块，用于实时处理感知单元的传感器数据，运行基于因子图优化的多源信息融合SLAM方法，实时完成辅助运输机器人的定位、地图构建；自主导航计算模块，用于根据当前位置与目标点完成底层路径规划，并根据运动学模型、动力学模型完成轨迹跟踪与自主避障，计算出相应的运动控制指令；
执行单元，执行单元包括伺服电机和执行控制器，用于根据运算单元发送的运动控制指令，执行相应的动作，完成机器人轨迹跟踪。
5.根据权利要求3所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：机器人端通信单元、巷道端通信单元与机器人多源信息融合导航装置通讯单元均包括内部通信模块和外部通信模块，内部通信模块采用光纤通信，外部通信模块采用5G通信；
内部通信模块用于巷道端感知单元与巷道端边缘计算单元之间的数据传输、机器人端感知单元与机器人端计算单元之间的数据传输、自然语义路标辅助定位装置与机器人多源信息融合导航模块以及巷道端识别定位装置与矿井辅助运输监控平台的数据传输；
外部通信模块用于巷道端识别定位装置与机器人多源信息融合导航模块的数据传输以及自然语义路标辅助定位装置和机器人多源信息融合导航模块与矿井辅助运输监控平台之间的定位信息的传输。
6.根据权利要求1所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：矿井辅助运输监控平台包括监控单元、辅助运输调度单元与监控端通信单元；
监控单元包括视频监控模块、定位可视化模块和场景模型数据库，视频监控模块用于根据巷道端相机采集的视频流，基于图像识别网络进行实时目标识别，对辅助运输机器人进行实时监控；定位可视化模块用于在数据流层面进行巷道整体信息的汇总，构建巷道3D点云模型并动态更新辅助运输机器人的六自由度位姿；
辅助运输调度单元，用于根据机器人多源信息融合定位信息以及辅助运输任务需求，完成对辅助运输机器人监控及调度；
监控端通讯单元采用光纤与WiFi通信，用于接收巷道端识别定位装置、自然语义路标辅助定位装置的定位信息，完成与机器人多源信息融合导航装置的信息交互。
7.根据权利要求1所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：
步骤S1中所述的物体坐标系标定方法是指在待标定物体的周围摆放视觉标签，通过相机环绕待标定物体进行数据采集，待完整地采集到物体信息后，利用内参变换将各帧深度信息转化为点云，并将各帧点云投影至初始帧实现自然语义路标的三维重建，获得物体CAD模型，进而通过全站仪和标定板标定初始帧相机坐标系外参，从而完成自然语义路标CAD模型标定，即物体坐标系标定；
其中通过全站仪和标定板标定初始帧相机坐标系外参具体包括以下步骤：利用全站仪读取标定板的特征点坐标，利用机器人端相机获得标定板图像与深度图像后，采用OpenCV库角点提取函数进行操作，通过对应位置深度值的内参变换获得角点在相机坐标系下的三维坐标；利用全站仪测量角点在世界坐标系下的坐标，通过匹配对应的特征点求得相机坐标系与世界坐标系位姿变换，完成相机坐标系外参标定；
步骤S2中所述的基于投票法的六自由度位姿估计的定位方法为：利用全站仪实现巷道相机在绝对坐标系下的标定后，获得机器人CAD模型，通过预先训练过的语义分割网络对辅助运输机器人RGB彩色图像以及点云信息进行语义分割，提取辅助运输机器人掩码图像与掩码点云，根据掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云数据；根据分割图像、点云信息利用六自由度位姿估计深度学习网络实现对辅助运输机器人的位姿估计，得到带有置信度的辅助运输机器人六自由度位姿，对置信度最高的位姿进行优化，最后基于机器人物体坐标系与相机坐标系位姿变换以及相机坐标系与绝对坐标系的位姿变换获得机器人在绝对坐标系下的定位信息；
步骤S3所述的基于自然语义路标的辅助定位方法为：利用全站仪实现自然语义路标物体坐标系标定后，获得自然语义路标CAD模型，通过离线处理，计算自然语义路标CAD模型点特征直方图进行特征提取，其次利用预先训练的语义分割网络在线对当前帧自然语义路标的彩色图像以及点云进行分割获得图像掩码与点云掩码；根据分割所得的掩码图像将辅助运输机器人的深度图像中的对应位置深度值，基于相机内参将深度值转化为点云信息；对自然语义路标点云信息进行特征提取，通过匹配当前帧自然语义路标点云与自然语义路标CAD模型特征相似的点对，对当前帧位姿进行初步估计，再次通过ICP算法对初步估计位姿进行位姿优化，获得机器人相对于自然语义路标物体坐标系位姿变换，最后通过位姿变换得到机器人在绝对坐标系下的六自由度位姿。
8.根据权利要求7所述的一种煤矿井下辅助运输机器人车路协同定位***，其特征在于：在步骤S3中，当语义分割网络在煤矿井下照明达不到设定条件时，易产生噪声导致语义分割网络分割精度下降出现误识别现象，从而影响定位精度，故在此环境中利用识别网络对机器人RGB图像与自然语义路标RGB图像进行预处理，获取包围框后，对包围框内裁剪图片进行语义分割，从而降低语义分割误差，提高定位精度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310814856.1A CN116518984B (zh) | 2023-07-05 | 2023-07-05 | 一种煤矿井下辅助运输机器人车路协同定位***及方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310814856.1A CN116518984B (zh) | 2023-07-05 | 2023-07-05 | 一种煤矿井下辅助运输机器人车路协同定位***及方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116518984A CN116518984A (zh) | 2023-08-01 |
| CN116518984B true CN116518984B (zh) | 2023-09-08 |
Family
ID=87390762
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310814856.1A Active CN116518984B (zh) | 2023-07-05 | 2023-07-05 | 一种煤矿井下辅助运输机器人车路协同定位***及方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116518984B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116698018B (zh) * | 2023-08-08 | 2023-10-13 | 山西戴德测控技术股份有限公司 | 一种导航定位辅助装置及煤矿巷道导航定位*** |
| CN117308900B (zh) * | 2023-11-30 | 2024-02-09 | 中国矿业大学 | 井下运输车移动测量***、运载通行状态模拟与监测方法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111735445A (zh) * | 2020-06-23 | 2020-10-02 | 煤炭科学研究总院 | 融合单目视觉与imu的煤矿巷道巡检机器人***及导航方法 |
| CN113485325A (zh) * | 2021-06-16 | 2021-10-08 | 重庆工程职业技术学院 | 煤矿井下水泵房巡检机器人slam建图、自主导航方法 |
| CN115100622A (zh) * | 2021-12-29 | 2022-09-23 | 中国矿业大学 | 深部受限空间无人运输设备可行驶域检测和自主避障方法 |
| CN115256414A (zh) * | 2022-07-21 | 2022-11-01 | 中国矿业大学 | 矿用钻孔机器人及其与地质、巷道模型的耦合作业方法 |
| CN115454057A (zh) * | 2022-08-24 | 2022-12-09 | 中国矿业大学 | 一种煤矿机器人群数字孪生智能管控建模***与方法 |
| WO2022257801A1 (zh) * | 2021-06-09 | 2022-12-15 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及*** |
| CN115540858A (zh) * | 2022-09-13 | 2022-12-30 | 天地（常州）自动化股份有限公司 | 高精度的井下车辆定位方法及其*** |
-
2023
- 2023-07-05 CN CN202310814856.1A patent/CN116518984B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111735445A (zh) * | 2020-06-23 | 2020-10-02 | 煤炭科学研究总院 | 融合单目视觉与imu的煤矿巷道巡检机器人***及导航方法 |
| WO2022257801A1 (zh) * | 2021-06-09 | 2022-12-15 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及*** |
| CN113485325A (zh) * | 2021-06-16 | 2021-10-08 | 重庆工程职业技术学院 | 煤矿井下水泵房巡检机器人slam建图、自主导航方法 |
| CN115100622A (zh) * | 2021-12-29 | 2022-09-23 | 中国矿业大学 | 深部受限空间无人运输设备可行驶域检测和自主避障方法 |
| CN115256414A (zh) * | 2022-07-21 | 2022-11-01 | 中国矿业大学 | 矿用钻孔机器人及其与地质、巷道模型的耦合作业方法 |
| CN115454057A (zh) * | 2022-08-24 | 2022-12-09 | 中国矿业大学 | 一种煤矿机器人群数字孪生智能管控建模***与方法 |
| CN115540858A (zh) * | 2022-09-13 | 2022-12-30 | 天地（常州）自动化股份有限公司 | 高精度的井下车辆定位方法及其*** |
Non-Patent Citations (1)
| Title |
|---|
| 煤矿移动机器人LiDAR/IMU 紧耦合SLAM 方法;李猛钢等;工矿自动化;第48卷(第12期);第68-78页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116518984A (zh) | 2023-08-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109945858B (zh) | 用于低速泊车驾驶场景的多传感融合定位方法 | |
| CN110097553B (zh) | 基于即时定位建图与三维语义分割的语义建图*** | |
| CN109709801B (zh) | 一种基于激光雷达的室内无人机定位***及方法 | |
| CN116518984B (zh) | 一种煤矿井下辅助运输机器人车路协同定位***及方法 | |
| CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
| JP7073315B2 (ja) | 乗物、乗物測位システム、及び乗物測位方法 | |
| CN110706248B (zh) | 一种基于slam的视觉感知建图方法及移动机器人 | |
| CN107741234B (zh) | 一种基于视觉的离线地图构建及定位方法 | |
| CN108051002B (zh) | 基于惯性测量辅助视觉的运输车空间定位方法及*** | |
| CN107967473B (zh) | 基于图文识别和语义的机器人自主定位和导航 | |
| CN113903011B (zh) | 一种适用于室内停车场的语义地图构建及定位方法 | |
| CN111060924B (zh) | 一种slam与目标跟踪方法 | |
| CN112734765B (zh) | 基于实例分割与多传感器融合的移动机器人定位方法、***及介质 | |
| CN115407357B (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN208323361U (zh) | 一种基于深度视觉的定位装置及机器人 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| WO2022062480A1 (zh) | 移动设备的定位方法和定位装置 | |
| CN116630394A (zh) | 一种三维建模约束的多模态目标物体姿态估计方法及*** | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN113188557A (zh) | 一种融合语义特征的视觉惯性组合导航方法 | |
| CN115077519A (zh) | 基于模板匹配与激光惯导松耦合的定位建图方法和装置 | |
| Aryal | Object detection, classification, and tracking for autonomous vehicle | |
| CN112729318A (zh) | 一种agv叉车自主定位移动slam导航*** | |
| CN116977628A (zh) | 一种应用于动态环境的基于多模态语义框架的slam方法及*** | |
| Ma et al. | Robust visual-inertial odometry with point and line features for blade inspection UAV |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |