一种井下多传感器的融合定位系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113701750A(43)申请公布日2021.11.26(21)申请号202110969537.9(22)申请日2021.08.23(71)申请人长安大学地址710064陕西省西安市南二环路中段(72)发明人高扬王兴奔陈士伟(74)专利代理机构西安通大专利代理有限责任公司61200代理人崔方方(51)Int.Cl.G01C21/16(2006.01)G01C21/18(2006.01)G01C21/20(2006.01)权利要求书2页说明书5页附图2页(54)发明名称一种井下多传感器的融合定位系统(57)摘要本发明公开了一种井下多传感器的融合定位系统，属于定位领域。本发明的井下多传感器的融合定位系统，照明装置和图像预处理模块的存在克服了低照度条件下图像亮度值低，相机难以采集有效信息的困难，使得视觉惯性里程计可在低照度的环境中仍可正常工作；从惯性测量单元读取相邻两帧图像相对位姿作为相机运动跟踪模块初始值，可提高特征匹配速度和准确率，提高定位精度与实时性；基于可定位性估计，合理设置融合权值，有效地融合双目相机，惯性测量单元的信息，充分发挥每个传感器的特点，提供精度高，鲁棒性好的定位。CN113701750ACN113701750A权利要求书1/2页1.一种井下多传感器的融合定位系统，其特征在于，包括双目相机(1)、惯性测量单元(2)、照明装置(5)和工控机(6)，所述双目相机(1)、惯性测量单元(2)、照明装置(5)用于安装在待定位的设备上；双目相机(1)、惯性测量单元(2)和照明装置(5)分别与工控机(6)相通讯；双目相机(1)用于场景的拍摄与传输；惯性测量单元(2)用于获取待定位的设备运动时的加速度和角速度数据；所述工控机由照明控制模块、图像预处理模块、相机运动跟踪模块、视觉惯性里程计模块、地图栅格化模块、可定位性评估定位模块、无线通信模块和中央处理模块组成；所述照明控制模块用于对双目相机拍摄的图像进行亮度检测，若检测到连续多帧图像亮度值低于预设阈值，且提取到的特征点数目低于阈值，则控制照明装置开启；所述图像预处理模块用于基于图像增强算法对实时拍摄的图像进行图像增强处理；所述相机运动跟踪模型用于获取两帧图像之间的相对位姿，将所述相对位姿作为特征跟踪的初始位姿，结合所述初始位姿与已经标定完成的相机内参矩阵将上一帧特征点投影到当前帧，以当前帧投影点为中心，在投影点附近进行特征点匹配，对成功匹配的特征点进行随机采样并基于一致算法进行计算，得到去除外点后的匹配点；所述地图栅格化模块用于获取三维点云地图，将3D点映射到OXZ平面，在OXZ平面划分栅格，若栅格内存在地图点投影，则所述栅格状态设为占据，否则设为空闲，并对非障碍物的地图点进行剔除；所述视觉惯性里程计模块用于融合图像、加速度和角速度数据，通过紧耦合的方式，得到优化后的位姿；并基于已有地图，进行地图匹配定位；可定位性评估定位模块，用于获取基于视觉惯性里程计模块的地图匹配的当前位姿和基于惯性测量单元的航迹推算的当前位姿，对地图匹配和惯性测量单元航迹推算过程中的协方差矩阵进行计算，求得当前时刻的可定位性估计，基于可定位性估计的结果确定定位融合的权重；中央处理模块用于获取惯性测量单元航迹和视觉惯性里程计模块的地图匹配结果，基于粒子滤波器采用惯性测量单元航迹推算进行状态预测，将视觉惯性里程计模块的地图匹配的结果作为测量更新，基于定位融合的权重进行状态融合定位。2.根据权利要求1所述的井下多传感器的融合定位系统，其特征在于，图像预处理模块的工作流程为：对获取的图像进行中值滤波以去除噪声，将去噪后的图像从RGB颜色空间转换到HSV颜色空间，对V分量进行多尺度Retinex处理，之后同时进行伽马变换和对比度受限的直方图均衡化处理，最后将处理之后的各分量转换为RGB颜色空间。3.根据权利要求1所述的井下多传感器的融合定位系统，其特征在于，所述视觉惯性里程计模块的具体工程过程为：将双目相机拍摄的图像帧上的特征点的深度信息，加速度计和陀螺仪的偏置量以及惯性测量单元测量的旋转、平移与速度量加入到一个状态向量中，进行后端优化分别构造视觉重投影残差、惯性测量单元预积分残差和边缘化先验信息残差，对三个残差进行联合优化，以寻找三种残差和最小时的目标状态向量。4.根据权利要求3所述的井下多传感器的融合定位系统，其特征在于，采用列文伯格—2CN113701750A权利要求书2/2页马夸尔特方法进行联合优化。5.根据权利要求1所述的井下多传感器的融合定位系统，其特征在于，中央处理模块还用于计算两帧图像之间的相对位姿。6.根据权利要求5所述的井下多传感器的融合定位系统，其特征在于，计算两帧图像之间的相对位姿的具体过程为：当更新一帧图像时，