一种井下多传感器的融合定位系统的制作方法

1.本发明属于定位领域，尤其是一种井下多传感器的融合定位系统。


背景技术：

2.精度高、鲁棒性好的定位能力是机器人开展导航与路径规划等一系列任务的基础。定位与地图构建技术(simultaneouslocalizationandmapping,slam)可以在获取机器人位置信息的同时创建环境地图，是当前机器人定位与导航领域的研究热点。良好光照条件下的视觉slam通过与惯性测量单元(inertialmeasurementunit,imu)进行融合可以给出较为准确、鲁棒的定位信息，但是在低照度的环境下，如矿井，城市地下管路等，传统的视觉slam技术难以给出满意的定位效果。
3.视觉slam技术以相机为数据采集装置，相比于激光传感器价格更为低廉，获取的信息更丰富。双目相机通过立体视觉获取图像中各像素距相机的真实距离，通过双目slam技术，系统可以获取相机各关键帧的空间位置和姿态,再结合深度图像,系统就可以获得各关键帧图像中各像素的真实空间位置，但是在特征单一，低照度的条件下，双目slam技术很难提取到足够的特征，导致定位失败。imu能够测量传感器自身的角加速度和线加速度，通过对其积分可以得到自身的旋转和位移，利用imu位姿估计的快速性和连续性可以有效补充双目视觉定位在跟踪失败时候的自主定位。基于视觉slam与imu的紧耦合相比视觉slam定位精度和鲁棒性都有了提高，同时本发明基于可定位性估计结果，将视觉信息，imu信息进行加权融合，给出精确，鲁棒的定位结果。


技术实现要素：

4.本发明的目的在于克服上述现有技术的缺点，提供一种井下多传感器的融合定位系统。
5.为达到上述目的，本发明采用以下技术方案予以实现：
6.一种井下多传感器的融合定位系统，包括双目相机、惯性测量单元、照明装置和工控机，所述双目相机、惯性测量单元、照明装置用于安装在待定位的设备上；
7.双目相机、惯性测量单元和照明装置分别与工控机相通讯；
8.双目相机用于场景的拍摄与传输；
9.惯性测量单元用于获取待定位的设备运动时的加速度和角速度数据；
10.所述工控机由照明控制模块、图像预处理模块、相机运动跟踪模块、视觉惯性里程计模块、地图栅格化模块、可定位性评估定位模块、无线通信模块和中央处理模块组成；
11.所述照明控制模块用于对双目相机拍摄的图像进行亮度检测，若检测到连续多帧图像亮度值低于预设阈值，且提取到的特征点数目低于阈值，则控制照明装置开启；
12.所述图像预处理模块用于基于图像增强算法对实时拍摄的图像进行图像增强处理；
13.所述相机运动跟踪模型用于获取两帧图像之间的相对位姿，将所述相对位姿作为
特征跟踪的初始位姿，结合所述初始位姿与已经标定完成的相机内参矩阵将上一帧特征点投影到当前帧，以当前帧投影点为中心，在投影点附近进行特征点匹配，对成功匹配的特征点进行随机采样并基于一致算法进行计算，得到去除外点后的匹配点；
14.所述视觉惯性里程计模块用于融合图像、加速度和角速度数据，通过紧耦合的方式，得到优化后的位姿；
15.所述地图栅格化模块用于获取三维点云地图，将3d点映射到oxz平面，在 oxz平面划分栅格，若栅格内存在地图点投影，则所述栅格状态设为占据，否则设为空闲，并对非障碍物的地图点进行剔除；
16.可定位性评估定位模块，用于获取基于视觉惯性里程计模块的地图匹配的当前位姿和基于惯性测量单元的航迹推算的当前位姿，对地图匹配和惯性测量单元航迹推算过程中的协方差矩阵进行计算，求得当前时刻的可定位性估计，基于可定位性估计的结果确定定位融合的权重；
17.中央处理模块用于获取惯性测量单元航迹和视觉惯性里程计模块的地图匹配结果，基于粒子滤波器采用惯性测量单元航迹推算进行状态预测，将视觉惯性里程计模块的地图匹配的结果作为测量更新，基于定位融合的权重进行状态融合定位。
18.进一步的，图像预处理模块的工作流程为：
19.对获取的图像进行中值滤波以去除噪声，将去噪后的图像从rgb颜色空间转换到hsv颜色空间，对v分量进行多尺度retinex处理，之后同时进行伽马变换和对比度受限的直方图均衡化处理，最后将处理之后的各分量转换为rgb 颜色空间。
20.进一步的，所述视觉惯性里程计模块的具体工程过程为：
21.将双目相机拍摄的图像帧上的特征点的深度信息，加速度计和陀螺仪的偏置量以及惯性测量单元测量的旋转、平移与速度量加入到一个状态向量中，进行后端优化分别构造视觉重投影残差、惯性测量单元预积分残差和边缘化先验信息残差，对三个残差进行联合优化，以寻找三种残差和最小时的目标状态向量。
22.进一步的，采用列文伯格—马夸尔特方法进行联合优化。
23.进一步的，中央处理模块还用于计算两帧图像之间的相对位姿。
24.进一步的，计算两帧图像之间的相对位姿的具体过程为：
25.当更新一帧图像时，获取距离该帧时间上最近的惯性测量单元的数据，同时读取与上一帧图像对齐的惯性测量单元的数据，计算两帧之间的相对位姿。
26.进一步的，还包括用于集成双目相机、惯性测量单元和照明装置的支撑板和连接架。
27.进一步的，支撑板包括上下支撑板，连接架和照明装置设在上下支撑板之间，双目相机和惯性测量单元设在上支撑板上。
28.与现有技术相比，本发明具有以下有益效果：
29.本发明的井下多传感器的融合定位系统，照明装置和图像预处理模块的存在克服了低照度条件下图像亮度值低，相机难以采集有效信息的困难，使得视觉惯性里程计可在低照度的环境中仍可正常工作；从惯性测量单元读取相邻两帧图像相对位姿作为相机运动跟踪模块初始值，可提高特征匹配速度和准确率，提高定位精度与实时性；基于可定位性估计，合理设置融合权值，有效地融合双目相机，惯性测量单元的信息，充分发挥每个传感器
的特点，提供精度高，鲁棒性好的定位。
附图说明
30.图1为本发明的结构示意图；
31.图2为本发明的中央处理模块系统框图；
32.图3为本发明的流程示意图。
33.其中：1
‑
双目相机；2
‑
惯性测量单元；3
‑
支撑板；4
‑
连接架；5
‑
照明装置；6
‑ꢀ
工控机。
具体实施方式
34.为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
35.需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
36.下面结合附图对本发明做进一步详细描述：
37.参见图1，图1为基于可定位性的井下多传感器的融合定位系统的结构示意图，一种基于可定位性的井下多传感器的融合定位系统，包括双目相机1、惯性测量单元2、支撑板3、连接架4、照明装置5和工控机6；其中，双目相机1和惯性测量单元2分别与工控机6相连，连接架4连接上下两侧的支撑板3，连接架的材料为铝合金；照明装置5由照明控制模块控制，与工控机6相连；支撑板 3为铝合金材料，用于支撑其上安装的双目相机1和照明装置5。
38.参见图2，图2为工控机的原理结构图，工控机包含照明控制模块、图像预处理模块、相机运动跟踪模块、视觉惯性里程计模块、地图栅格化模块、可定位性评估定位模块、无线通信模块和中央处理模块；其中，照明控制模块包括控制模块和亮度检测模块；亮度检测模块工作用于对双目相机拍摄的图像进行亮度检测，若检测到连续多帧图像亮度值低于预设阈值，且提取到的特征点数目低于阈值，则判定此时工作环境难以满足双目相机工作要求；中央处理模块用于向控制模块发送信号，当控制模块接收到工作环境难以满足双目相机工作要求的信号时，控制照明装置开启。
39.图像预处理模块用于基于图像增强算法对实时拍摄的图像进行图像增强处理；图像增强算法首先对获取的图像进行中值滤波以去除噪声，针对去噪后的图像，从rgb颜色空间转换到hsv颜色空间，对v分量首先进行多尺度retinex 处理，然后同时进行伽马变换和对比度受限的直方图均衡化处理，最后将处理之后的各分量转换为rgb颜色空间。通过图像
增强处理，可增强图像整体对比度，有效提高图像亮度，获得更丰富的图像特征。
40.相机运动跟踪模块用于跟踪相机运动，具体的，相机运动跟踪模型获取惯性测量单元测量的两帧之间相对位姿作为初始位姿估计，结合此位姿与已经标定完成的相机内参矩阵将上一帧特征点投影到当前帧，以当前帧投影点为中心，在投影点附近进行特征匹配，对成功匹配的特征点进行随机采样一致算法进行计算，去除误匹配和误差较大的外点，得到去除外点后的匹配点。去除外点后的匹配点对用于视觉惯性里程计模块的多传感器融合定位。当系统开始处理新的一帧图像时，首先取出距离该帧时间上最近的惯性测量单元的数据，同时读取与上一帧图像对齐的惯性测量单元数据，计算两帧之间的相对位姿，将相对位姿输入到系统。传统的基于视觉的同步定位与地图构建技术框架中，前端运动跟踪模块都是假设匀速运动模型，即假设相邻两帧之间为匀速运动。
41.视觉惯性里程计模块用于融合双目相机、加速度计和陀螺仪的数据进行定位。双目相机与加速度计和陀螺仪的融合属于紧耦合，将双目相机拍摄的图像帧上的特征点的深度信息，加速度计和陀螺仪的偏置量以及惯性测量单元测量的旋转、平移与速度量加入到一个状态向量中，在进行后端优化时，分别构造视觉重投影残差、惯性测量单元预积分残差和边缘化先验信息残差，对三个残差进行联合优化，以寻找一个状态向量，满足三种残差和最小，采用列文伯格—马夸尔特方法进行优化。
42.地图栅格化模块用于基于已有三维点云地图移除y轴坐标，将3d点映射到 oxz平面，首先在oxz平面划分栅格尺寸为w
×
w，若栅格内存在地图点投影，则该栅格状态设为“占据”，否则设为“空闲”，同时对非障碍物的地图点进行剔除。所述非障碍物的地图点为地图点阈值范围内无其他地图点。
43.可定位性评估定位模块，用于获取基于视觉惯性里程计模块的地图匹配的当前位姿，同时获取基于惯性测量单元的航迹推算的当前位姿，对地图匹配和惯性测量单元航迹推算过程中的协方差矩阵进行计算，求得当前时刻的可定位性估计，基于可定位性估计的结果确定定位融合的权重。针对可定位性估计结果，若协方差值大，则可定位性差，融合权值小，反之融合权值大。融合算法基于粒子滤波器，采用惯性测量单元航迹推算进行状态预测，将视觉惯性里程计模块的地图匹配结果作为测量更新，并基于可定位性估计进行状态融合定位。
44.本发明系统的工作流程如图3所示。系统的输入为双目相机拍摄的图像，惯性测量单元测得的角速度和加速度。系统工作时，照明控制模块对双目相机拍摄的图像进行亮度检测，若连续多帧图像亮度值低于预设阈值，提取的特征点数目低于阈值，则此时系统所处环境为低可见度，照明控制模块控制照明灯开启，双目相机得以正常工作，图像输入到图像预处理模块进行图像增强。而由于整体环境亮度值较低，双目相机采集的图像不能直接送入视觉惯性里程计模块进行位姿求解，需先在图像预处理模块进行图像增强操作，首先对获取的图像进行中值滤波以去除噪声，针对去噪后的图像，从rgb颜色空间转换到hsv颜色空间，对 v分量首先进行多尺度retinex处理，然后同时进行伽马变换和对比度受限的直方图均衡化处理，最后将处理之后的各分量转换为rgb颜色空间。经图像预处理模块处理后的图像输入到视觉惯性里程计模块中。
45.惯性测量单元通过加速度计和陀螺仪测量设备运动时的加速度和角速度，并将测量值输入到视觉惯性里程计模块中，视觉惯性里程计模块采用紧耦合的方式对图像信息、
加速度和加速度信息进行融合，得到设备当前位姿。可定位性评估定位模块，依据可定位性评估结果决定定位融合的权重。二维栅格地图建立后，系统可在已有地图上进行基于可定位性的融合定位。可定位性评估定位模块可对地图匹配和双目视觉惯性里程计航迹推算过程中的协方差矩阵进行计算，并求得当前时刻的可定位性估计。根据可定位性估计结果设置融合权值。采用惯性测量单元航迹推算进行状态预测，将地图匹配结果作为测量更新，并基于可定位性估计进行状态融合定位。
46.以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。