一种基于多传感器主动融合的井下无人驾驶车辆定位方法与流程

1.本发明涉及自动驾驶环境感知技术领域，特别是涉及一种基于多传感器主动融合的井下无人驾驶车辆定位方法。


背景技术：

2.近年来，5g通信和车联网技术快速发展，无人驾驶逐渐走入大众视野。但无人驾驶由于传感器稳定性、决策可靠性以及控制精准程度等原因短时间内难以在城区内普及，而井工矿区具有人少、障碍物单一等利于无人驾驶应用开展的天然优势；除此之外，在井工矿区开展无人驾驶，一定程度上保障了复杂工况下的工作顺利推进，提高作业效率。
3.井工矿开采有一定的危险性，开采需要在井下布置大量的设备、车辆、人员，各种自然或人为因素造成的灾害、事故对井下工作人员的人身安全造成了极大的威胁。对井下人员、车辆有效的组织管理，以及事故、灾害的救援都需要掌握井下人员、车辆、设备的精确位置数据，井下设备的远程控制、智能控制更离不开井下设备的实时精确位置数据。精确定位技术已成为矿井安全生产的重要技术支撑，实时精确位置数据已成为智能矿山日常运行的重要基础。目前，井工矿区通常采用惯导定位或者通信式定位：惯导定位只需要起点的坐标，根据陀螺仪及加速度计推算当前位置，但存在较大累计误差；而通信式定位通常需要在井下安装多个基站，每个基站服务一片区域，但往往都存在定位盲区。因此，如何在井工矿区实现井下的精准定位是亟待解决的问题。
4.目前，关于井下矿区定位已经成为研究热点，但对于本场景下应用要求都难以满足：
5.在论文《关于煤矿井下人员智能化定位系统的设计研究》中，提出一种基于uwb超宽带定位技术和lora无线通信技术的矿井定位方法，井下工作人员及车辆携带对应的定位标签，无意外情况下一定时间内更新标签位置信息，井上解析数据信息获取具体位置。但该方法在井下矿区定位时需要得到信号在传播媒介中的具体速度，在井下矿区复杂工况场景下存在较大误差，无法准确获得的井下定位信息。
6.在论文《基于wifi无线网络技术的新型矿井人员定位系统的设计研究》中，提出一种基于先进的wifi无线网络技术设计的矿井定位方法，井下工作人员及车辆携带定位终端，根据一定范围内的定位标签上传数据信息，井上根据信息分析井下具体定位信息。但该方法在井下矿区受限于wifi信号强度、工作人员活动场景、硬件软件的兼容度及定位盲区等多重不利条件，缺乏完整的井下定位信息。
7.基于此，本发明提出一种基于多传感器主动融合的井下无人驾驶车辆定位方法，通过imu、轮速里程计、摄像头及激光雷达在内的多种传感器在不同工况下进行主动融合，按照实际场景输出准确定位信息，实现车体在井工矿区内的精准定位。


技术实现要素：

8.本发明的目的在于克服现有技术的缺点与不足，提供一种基于多传感器主动融合
的井下无人驾驶车辆定位方法，目的在于增强在井下场景下矿车的环境感知能力，根据精准定位达到作业要求，不仅保障工人的作业安全，还一定程度上提高矿区的生产工作效率。本发明采用如下技术方案：
9.一种基于多传感器主动融合的井下无人驾驶车辆定位方法，包括以下步骤：
10.步骤1、根据无人驾驶车辆各个传感器的初始参数以及位置关系，对各个传感器进行参数标定，得到各个传感器的外参，并将其统一在同一车体坐标系；其中，所述传感器包含轮速里程计、惯导、摄像头及激光雷达；
11.步骤2、开启各个传感器及照度计，无人驾驶车辆按照规划路径在井下行驶一周，得到先验信息，包括惯导先验数据、激光雷达点云生成的先验地图及井下照度情况；
12.步骤3、在无人驾驶车辆执行任务过程中，根据先验信息确定定位主传感器：根据惯导先验数据进行判断，如果场景坡度小于给定值，采用轮速里程计作为主传感器对车体进行定位；否则对井下照度进行判断，如果照度高于给定值，采用摄像头作为主传感器，根据视觉里程计信息对车体进行定位；否则采用激光雷达作为主传感器，根据激光雷达的当前场景点云与先验地图匹配信息对车体进行定位；其中，主传感器确定后，依据主传感器信息进行定位，其他传感器保持关机状态；
13.步骤4、将主传感器及惯导的定位信息加权融合，得到无人驾驶车辆最终定位。
14.进一步，所述步骤1，所述摄像头与激光雷达标定利用点对应方法匹配，得到精确的旋转平移矩阵并记录更新该矩阵；惯导通过测算静止状态下的零偏误差进行标定。
15.进一步，所述步骤3，当采用轮速里程计作为主传感器时，根据轮速及运动模型推算车体位移，得到行驶里程进行定位；
16.当采用摄像头作为主传感器时，先采用暗通道优先的图像增强方法减小井下扬尘对视觉里程计的影响，而后采用vins-mono方法进行定位；
17.当采用激光雷达作为主传感器时，采用迭代临近点方法对当前场景点云信息与先验地图信息进行实时匹配，得到定位结果。
18.进一步，所述采用摄像头作为主传感器进行定位，包括以下步骤：
19.(1)采用暗通道优先的图像增强方法，减小井下扬尘对视觉里程计的影响，其中图像的暗通道定义为两次取最小值的操作：先选取原图中rgb三通道内中的最小值，再对窗口内作最小值滤波：
[0020][0021]
其中，j
dark
(x)为像素点x的暗通道的值，jc(y)为彩色图像的每个通道，ω(x)为以x为中心的一块方形区域，c为颜色通道在无雾或灰尘场景下，j
dark
在三通道中至少有一个通道值很，最终计算出的暗通道通常为0，而在有雾或灰尘场景下，则会大于0；
[0022]
而后结合路侧导引井下特征，对井下特征点进行提取与跟踪；
[0023]
(2)利用imu预积分方法恢复出尺度信息，并给予短时姿态估计；
[0024]
(3)前端处理完成后经过滑动窗口图优化后端处理输出最终状态估计结果，即定位信息输出。
[0025]
进一步，所述采用激光雷达作为主传感器进行定位，包括以下步骤：
[0026]
(1)获取当前帧点云并进行点云预处理操作，并导入先验地图，将当前帧点云与先
验地图点云进行迭代临近点匹配：
[0027][0028]
其中pi是当前帧点云中任意一点，qi是地图点云中距离pi最近的一点，r为旋转矩阵，t为平移矩阵；
[0029]
(2)调整对应点权重，剔除误匹配点对，不断迭代此过程直到求解出最小值的loss，即求解出最优变换；
[0030]
(3)得到最优匹配后，输出此时的定位结果。
[0031]
本发明与现有技术相比所具有的有益效果：
[0032]
(1)本专利提出一种基于井工矿环境差异的动态融合定位方法。由于井工矿环境具有差异性，结合需求场景下的先验环境信息，动态确定主传感器适应不同感知环境，自动切换车体融合定位方法，相较于传统井下定位方式适应性更强，实时性更好，覆盖范围更广。
[0033]
(2)本专利提出一种井下场景下优化的视觉里程计定位方法。由于井下场景无明显可提取特征且存在扬灰现象，本专利采用暗通道优先的图像增强算法，去除场景中灰尘噪点得到更为清晰地图像，再结合路侧导引丰富井下特征，提高算法定位精确度。
[0034]
(3)本专利提出一种视觉与激光里程计协同定位的高实时性井工矿定位方法。该系统应用于井工矿无人驾驶车辆上，具有低内存消耗、高实时性、高精度的特性，突破井工矿传统定位方案的定位高时延问题，视觉与激光雷达里程计实时定位技术协同解决井下无gps情况下的无人驾驶车辆定位问题，形成一套井下场景实时定位方法库。
附图说明
[0035]
图1为基于多传感器主动融合的井下无人驾驶车辆定位方法框架；
[0036]
图2为井下场景下优化的视觉里程计定位方法框架；
[0037]
图3为激光雷达里程计定位方法框架。
具体实施方式
[0038]
下面结合附图和实施例对本发明进行进一步的详细介绍。
[0039]
一种基于多传感器主动融合的井下无人驾驶车辆定位方法，主要包括以下四个部分：多传感器标定，先验信息采集，主传感器确定及切换，融合定位。
[0040]
步骤1、用户根据无人驾驶车辆各个传感器的初始参数以及位置关系，对该系统传感器间进行参数标定；其中，多传感器标定需要得到各个传感器的外参，包含轮速里程计、惯导、摄像头及激光雷达，通过测量计算后进行参数标定，将其统一在同一车体坐标系下。
[0041]
具体地，本发明采用的装置为井工矿区作业无人驾驶车辆，是一种特定场景下的工作车辆，传感器轮速里程计安装在后轮上，摄像头安装于车内挡风玻璃后，imu惯导设备及激光雷达置于保险杠上方。
[0042]
用户启动无人驾驶车辆后，各传感器开始工作。用户利用标尺测量各传感器的位置姿态关系参数。计算机得到各传感器外参后，其中摄像头与激光雷达标定利用点对应方法匹配，得到精确的旋转平移矩阵并记录，更新系统该矩阵；imu惯导设备需要测算静止状
态下的零偏误差，至此多传感器标定工作结束。
[0043]
步骤2、打开传感器设备及照度计，无人驾驶车辆在井下场景内行驶一周，得到先验信息，包括imu惯导先验数据、激光雷达点云生成的先验地图及井下照度情况。
[0044]
具体地，标定后，用户选取实验场景地图，即在遥控平台框选地图区域，计算机根据选择的范围地图查找可行驶路径，并从中根据最短路算法选择最优路径作为规划路径，采用模拟退火算法执行规划路径，根据当前位置与起点位置找到可行使路段并行驶前往，实现井工矿无人驾驶车辆的自动驾驶控制。
[0045]
抵达起点后，计算机开启各个传感器及照度计元件，按照规划路径行驶后返回，得到imu惯导先验数据、激光雷达点云生成的先验地图及井下照度情况。
[0046]
优选地，先验信息采集首先需要路径规划，车辆按照用户感兴趣区域中可行驶路径进行择优，选取根据迪杰斯特拉算法中的最短路径作为最优路径，并将该路径根据模拟退火算法规划车辆行驶状态。迪杰斯特拉算法是一种单源最短路径算法，在矿区环境下较为适用。而模拟退火算法是一种适用于大规模组合优化问题的有效近似算法。它模仿固体物质的退火过程，通过设定初温、初态和降温率控制温度的不断下降，结合概率突跳特性，利用解空间的邻域结构进行随机搜索，最终得到较为平滑的规划路径。路径规划后，打开采集设备，包含惯导采集俯仰角信息，照度计采集井下光照强度，激光雷达采集点云信息，并进行点云匹配生成先验地图。
[0047]
步骤3、在无人驾驶车辆执行任务过程中，根据先验信息确定定位主传感器：根据惯导先验数据进行判断，如果场景坡度较小，采用轮速里程计作为主传感器对车体进行定位；否则对井下照度进行判断，如果照度较高，采用摄像头作为主传感器，根据视觉里程计信息对车体进行定位；否则采用激光雷达作为主传感器，根据激光雷达的当前场景点云与先验地图匹配信息对车体进行定位。主传感器确定后，依据主传感器信息进行定位，其他传感器保持关机状态，降低系统复杂度。
[0048]
具体地，计算机收到先验信息后，在无人驾驶车辆执行任务过程中，根据先验信息状态控制无人驾驶车辆切换定位模式，将原本的imu定位转变为主传感器与imu融合定位方法，主传感器确定方法及其定位方法如图1所示。
[0049]
主传感器确定是根据实验环境切换定位主传感器，根据先验惯导俯仰角信息进行判断，如果场景坡度较小，在一定时间区间内俯仰角变化值小于60
°
，采用轮速里程计作为主传感器对车体进行定位；如果变化值大于60
°
，判断井下环境照度是否满足摄像头定位条件，如果照度大于20勒克斯，采用摄像头作为主传感器；如果照度小于20勒克斯，采用激光雷达作为主传感器。
[0050]
优选地，如果采用轮速里程计作为主传感器定位，定位方法为根据轮速及运动模型推算车体位移，得到行驶里程进行定位，本发明采用机器人模型对车体进行定位。
[0051]
优选地，如果采用摄像头作为主传感器，采用在井下场景下的优化的视觉里程计对车体进行定位。本发明采用vins-mono方法，vins-mono是香港科技大学开源的一个vio算法，是用紧耦合方法实现的，通过单目+imu恢复出尺度。如图2所示，具体包括以下步骤：
[0052]
(1)本发明先采用暗通道优先(dcp)的图像增强方法，减小井下扬尘对视觉里程计的影响。其中图像的暗通道定义为：两次取最小值的操作：先选取原图中rgb三通道内中的最小值，再对窗口内作最小值滤波。公式如下：
[0053][0054]
其中，j
dark
(x)为像素点x的暗通道的值，jc为彩色图像的每个通道，ω(x)为以x为中心的一块方形区域，c为颜色通道在无雾或灰尘场景下，j
dark
在三通道中至少有一个通道值很，最终计算出的暗通道通常为0，而在有雾或灰尘场景下，则会大于0。
[0055]
而后结合路侧导引丰富井下特征，而后对井下特征点进行提取与跟踪。
[0056]
(2)同时，利用imu预积分方法恢复出尺度信息，并给予短时姿态估计。
[0057]
(3)前端处理完成后经过滑动窗口图优化后端处理输出最终状态估计结果，即定位信息输出。
[0058]
优选地，如果采用激光雷达作为主传感器定位，激光雷达采用迭代临近点方法(icp)对当前场景点云信息与先验地图信息进行实时匹配，得到定位结果。如图3所示，具体包括以下步骤：
[0059]
(1)首先，获取当前帧点云并进行点云预处理操作，并导入先验地图，将当前帧点云与先验地图点云进行迭代临近点匹配。其方法公式如下：
[0060][0061]
其中pi是当前帧点云中任意一点，qi是地图点云中距离pi最近的一点，r为旋转矩阵，t为平移矩阵。
[0062]
(2)调整对应点权重，剔除误匹配点对，不断迭代此过程直到求解出最小值的loss，即求解出最优变换。
[0063]
(3)得到最优匹配后，输出此时的定位结果。
[0064]
步骤4、将主传感器及惯导的定位信息加权融合，得到车体最终定位。
[0065]
具体地，根据主传感器定位与imu惯导数据进行定位融合，具体融合方式与vins-mono方法的融合方式相同。本专利采用的是加权融合方式，具体权重系数依据主传感器的定位准确度确定，其中定位准确度由在有rtk真值的路段下应用该传感器方法和gps真值的差值与gps真值的比值确定。最后生成融合定位信息，计算机将定位信息上传云端，矿上工作人员收集定位信息，此系统执行结束，工作无人驾驶车辆返回车间。
[0066]
以上所述仅为本发明的具体实施方式，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。