[发明专利]融合UWB和单目视觉SLAM的煤矿井下车辆定位方法

权利要求书

1.一种融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于，具体步骤如下：

第1步骤、获取车辆前视摄像头图像；

第2步骤、加载井下目标检测模型进行图像目标检测：利用针对煤矿井下特定目标制作目标检测数据集训练目标检测模型，对图像进行目标检测，获取目标的boundingbox，该boundingbox包含图像中目标类别及位置信息；

第3步骤、加载ORB-SLAM制作的全局地图；

第4步骤、对地图关键帧和当前前视摄像头图像中同一目标ROI区域进行特征匹配：利用井下目标检测模型对当前前视摄像头获取的图像帧和地图中关键帧图像进行目标检测，取相同类别的目标boundingbox区域作为图像匹配的ROI区域，对图像ROI区域进行ORB特征匹配，得到匹配的图像的Oriented关键点和BRIEF描述子所构成的ORB特征；

第5步骤、通过匹配成功的帧确定车辆位姿，读取车辆在地图中位置；

第6步骤、将ORB-SLAM确定的车辆位姿和UWB确定的车辆井下坐标两者位置信息统一到UWB坐标系，获取车辆在井下x，y方向的坐标位置和速度更新；

第7步骤、通过EKF扩展卡尔曼滤波进行车辆运动量和观测量更新，最终得到UWB和视觉SLAM融合后的车辆井下定位。

2.根据权利要求1所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：在第6步骤中，所述UWB坐标系的获取步骤如下：

第6.1步骤、采用TDOA无线网络保持UWB基站的时间同步；

第6.2步骤、通过Chan算法解算UWB定位信息；

第6.3步骤、对Chan解算结果采用中值平均滤波进行优化，最终获得车辆在井下的UWB定位。

3.根据权利要求2所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：在第6.2步骤中，所述UWB定位信息的获取是利用车载UWB定位标签卡获取原始UWB定位数据，并通过解算和优化获取UWB定位信息，具体步骤如下：

第6.2.1步骤、保持时间同步：TDOA需要定位基站保持时钟同步，或者获知定位基站间的定位时间差，采用基于无线网络的同步方式，由任意一个定位基站广播发送一个测距报文，其他定位基站接收到测距报文，统计出测试时间差T，依据基站的相对位置和电磁波传播速度，计算出实际的时间差T＇，得到各个基站的定位时间差ΔT，其中，ΔT=T-T＇，由于存在时钟的漂移，因此，上述时间同步需要在定位过程中持续进行；

第6.2.2步骤、车载UWB定位标签卡对外发送一次UWB信号，在标签定位距离内的所有基站都会收到无线信号，通过TDOA方法获取车辆标签卡与各个基站的报文飞行时间差，计算标签卡与基站的距离差。

4.根据权利要求1所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：利用单目视觉SLAM获取位姿估计值，利用车辆前视摄像头采集图像后利用ORB-SLAM算法求解相机位姿估计，获得车辆位置信息。

5.根据权利要求1所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：根据暴力匹配算法对前一帧图片中的每个ORB特征的描述子与后一帧图像的每个ORB特征点描述子计算描述子汉明距离，取距离在设定阈值范围内的点作为两帧图像的最优匹配点，得到两帧图像的匹配特征点个数Q。

6.根据权利要求5所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：设定匹配特征点数量阈值U，将获得匹配到的特征点数Q与该阈值U进行比较，若Q≥U，则匹配成功，追踪车辆运动过程中的相邻帧ORB特征，确定当前相机位姿定位，根据确定的目标车辆位姿读取目标车辆在视觉地图中的位置，完成车辆定位。

7.根据权利要求3所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：在第6.2.2步骤中，当定位基站超过3个时，采用Chan算法通过两次加权最小二乘法。

8.根据权利要求1所述的融合UWB和单目视觉SLAM的煤矿井下车辆定位方法，其特征在于：进行数据融合，在单目视觉ORB-SLAM和UWB组合方法中，以UWB定位过程中所使用的独立坐标系作为全局坐标系，将单目视觉ORB-SLAM解算出的位置信息经过空间变换，转换到UWB坐标系下，将UWB和单目视觉ORB-SLAM的数据进行融合，获得融合后的车辆位置和速度更新。