[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN118376230A - 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质 - Google Patents

一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质 Download PDF

Info

Publication number
CN118376230A
CN118376230A CN202410460082.1A CN202410460082A CN118376230A CN 118376230 A CN118376230 A CN 118376230A CN 202410460082 A CN202410460082 A CN 202410460082A CN 118376230 A CN118376230 A CN 118376230A
Authority
CN
China
Prior art keywords
imu
coordinate system
coal mine
frame
inspection robot
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.)
Pending
Application number
CN202410460082.1A
Other languages
English (en)
Inventor
蒋萍
刘涵
张安泰
谭力铭
赵丰瑞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Jinan
Original Assignee
University of Jinan
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by University of Jinan filed Critical University of Jinan
Priority to CN202410460082.1A priority Critical patent/CN118376230A/zh
Publication of CN118376230A publication Critical patent/CN118376230A/zh
Pending legal-status Critical Current

Links

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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
    • 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/3844—Data obtained from position sensors only, e.g. from inertial navigation
    • 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/3863—Structures of map data
    • G01C21/387—Organisation of map data, e.g. version management or database structures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/10—Pre-processing; Data cleansing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/213—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • 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)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Geometry (AREA)
  • Databases & Information Systems (AREA)
  • Image Processing (AREA)

Abstract

本发明提供一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质，属于移动机器人同步定位技术领域，定义煤矿巡检机器人的功能模块；构建实现煤矿巡检机器人移动定位的地图，将双目相机和IMU模块采集的原始数据传输到工控机中进行预处理，并进行融合优化；进行初始化，构建整体残差方程，通过非线性优化的方法融合滑动窗口内的数据；配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。本发明提高系统定位与建图精度，提升系统的鲁棒性。

Description

一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位 方法、终端机及可读存储介质
技术领域
本发明属于移动机器人同步定位技术领域，尤其涉及一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质。
背景技术
矿山存在环境复杂、空间狭小和绝大部分是易燃、易爆、有毒、有腐蚀的危险品等环境因素。目前矿山的巡视和检修工作大部分都由人工完成，而矿山巡检机器人的出现，它可以用于替代人工进行安全检查、监测以及采集矿山中的信息等操作，使得矿山巡检更加先进、安全地进行矿山巡检。
为使得煤矿巡检机器人能够实现无人化、自动化运行，现有文件中公开了一种煤矿机器人用智能识别系统，该文件主要包括巡检端、云端及审核端；本发明，通过视觉及声音融合的方式实时检测变电所、水泵房内设备的运行状态，自动识别和记录设备显示屏上仪表显示的数值、开关状态、指示灯状态，搭载升降云台可对不同位置的目标进行识别，同时具备温湿度数据、烟雾、多种气体浓度参数采集等功能，将数字化的图像、声音和准确的数据实时分析并预警，实现巡检机器人的功能多样化。
但是，现有技术中，面对煤矿井下信号弱、光线差、GPS定位无法使用等复杂多变的工作环境，目前大部分煤矿巡检机器人采用激光雷达来实现自动运行，由于煤矿井下光线不够均匀，使得视觉传感器采集的数据鲁棒性较差，故只有单一的视觉传感器无法满足煤矿井下精确的同步定位与地图构建，造成煤矿巡检机器人在执行巡检过程中，无法有效的进行作业，无法满足巡检要求。
发明内容
本发明提供一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，方法实现对煤矿巡检机器人的定位，使得煤矿巡检机器人同步定位与地图更加精准、可靠。
方法包括：
S101：定义煤矿巡检机器人的功能模块；
S102：构建实现煤矿巡检机器人移动定位的地图，并基于地图分别建立世界坐标系、双目相机坐标系、IMU坐标系、成像坐标系以及像素坐标系；构建IMU坐标系与世界坐标系之间的转换矩阵；
还构建世界坐标系、双目相机坐标系与成像坐标系之间的转换矩阵以及构建IMU坐标系与双目相机坐标系之间的转换矩阵；
S103：将双目相机和IMU模块采集的原始数据传输到工控机中进行预处理，使IMU模块采集的数据同双目视觉数据进行融合优化；
S104：对双目相机和IMU模块进行初始化，并将双目视觉数据与IMU数据对齐；
S105：构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据；
S106：配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。
进一步需要说明的是，所述S102还包括：
以煤矿巡检机器人启动时的位置为原点，建立世界坐标系Ow-Xw-Yw-Zw；
以双目相机的左目相机中心为基准，建立双目相机坐标系Oc-Xc-Yc-Zc；
将煤矿巡检机器人车头的朝向为X轴正方向，垂直于地面向上为Z轴正方向，垂直于X轴向右为Y轴正方向，建立IMU坐标系Ob-Xb-Yb-Zb；
基于IMU坐标系与双目相机坐标系之间的外参标定，使得IMU数据通过平移旋转到双目相机坐标系，最终使IMU坐标系与双目相机坐标系的数据构成关联关系。
进一步需要说明的是，所述S103中的预处理方式包括对IMU数据进行预积分处理，具体步骤为：
S1031：建立IMU测量模型与运动学模型；
建立加速度计和陀螺仪得到的测量值与真值之间的误差模型：
其中，和分别表示加速度计和陀螺仪的测量值，a(t)和ω(t)分别表示两者的真值，ba(t)和bg(t)分别表示加速度计和陀螺仪的偏置，σa(t)和σg(t)分别表示二者的噪声项，σa(t)和σg(t)服从零均值的高斯分布；
用t表示采样时刻，对IMU模块的运动学模型离散化后进行欧拉积分，得到如下方式：
Δt为IMU模块两次相邻测量之间的时间间隔，exp()为李代数so(3)到李群SO(3)的指数映射，σgd(t)和σad(t)分别代表σg(t)和σa(t)的离散形式；
S1032：对IMU模块预积分处理；
在第i帧和第j帧图像之间有多个IMU数据时，则将IMU数据的第一帧与第i帧图像关键帧对齐，将IMU数据的最后一帧与第j帧图像关键帧对齐，对齐之后，对这两个连续时间间隔为Δt的图像关键帧之间的IMU数据进行积分处理，预积分使得每项与起始状态无关，将参考坐标系从世界坐标系调整为第k个关键帧bk时刻，有：
ΔRij的含义为从i帧时刻到j帧时刻旋转的预积分增量；
的含义为i帧时刻世界坐标系到IMU坐标系的旋转矩阵的转置；
的含义为j帧时刻世界坐标系到IMU坐标系的旋转矩阵；
的含义为k帧时刻陀螺仪的偏置；
的含义为k帧时刻陀螺仪噪声项的离散形式；
ΔVij的含义为从i帧时刻到j帧时刻线速度的预积分增量；
Vj的含义为j帧时刻的线速度；
Vi的含义为i帧时刻的线速度；
gW的含义为世界坐标下的重力加速度；
ΔRik的含义为i帧时刻到k帧时刻旋转的增量；
的含义为k帧时刻IMU坐标系下的加速度；
的含义为k帧时刻加速度计的偏置；
的含义为k帧时刻加速度计噪声项的离散形式；
ΔPij的含义为i帧时刻到j帧时刻平移的预积分增量；
Pj的含义为j帧时刻的平移量；
Pi的含义为i帧时刻的平移量；
Δvik的含义为i帧时刻到k帧时刻线速度的增量；
的含义为k帧时刻世界坐标系下的加速度；
的含义为k帧时刻陀螺仪的偏置；
i的含义为双目相机采集图像的第i帧时刻；
j的含义为双目相机采集图像的第j帧时刻；
k的含义为双目相机采集图像的第k帧时刻；
进一步需要说明的是，所述S103中，还对双目相机采集的图像数据进行特征提取与位姿解算处理，具体步骤为：
提取Oriented FAST关键点和Steered BRIEF描述子，并采用图像金字塔来确保特征点的尺度不变性，最后使用灰度质心法来计算关键点的主方向，统一将像素旋转到所述主方向，其中，灰度质心法求关键点旋转角度的计算方式如下：定义区域图像的矩为：
其中，p，q取0或1，I(x，y)为像素坐标(x，y)处图像的灰度值，mpq表示图像的矩；
从图像圆形区域形心O指向质心C的方向向量即可为关键点的主方向，故关键点的旋转角度记为：θ＝arctan2(cy，cx)＝arctan2(m01，m10)；
方法中还对位姿进行解算，具体为：
由三维空间点的匹配关系，根据位姿变换关系得：pw＝Rp′w+t；
其中pw和p′w分别表示前后两个图像中观测到的三维空间点。
首先计算两组三维空间点的质心位置，用p和p′表示，得：
再计算两组三维空间点的去质心坐标，用qw和q′w表示，得：
qw＝pw-p
q′w＝p′w-p′
然后将旋转参数作为优化变量计算旋转矩阵R，构建最小二乘问题R*得：
最后由计算出的旋转矩阵R，计算平移矩阵t得：
t＝p-Rp′。
进一步需要说明的是，所述S104还包括：通过双目相机和IMU之间的旋转约束初始化陀螺仪偏置bias；通过双目相机和IMU模块之间的平移约束来估计速度和重力方向；最后将双目相机坐标系的变量转化到世界坐标系下，将轨迹对齐到世界坐标系下；对齐之后在滑动窗口内采用非线性优化的方法进行数据融合。
进一步需要说明的是，所述S105还包括：
S1051：将待优化的状态量定义为：
λi＝[x，y，z]T，i∈[0，m]
滑动窗口内存在n个关键帧，X为状态向量，其中，λi为观测到的地图点，xi为关键帧对应的系统状态量，Rbc为双目相机到IMU的旋转外参；xk为滑动窗口内第k帧的状态量，IMU预积分得到的旋转项为速度项为位移项为陀螺仪偏置为ba、加速度计偏置为ba；为双目相机到IMU模块的外参。
S1052：构建IMU约束，将IMU两帧之间的位置、速度、旋转、加速度偏置bias以及陀螺仪偏置bias的变化量之差定义为IMU残差；
S1053：构建双目视觉约束，像素重投影造成视觉残差，将首次投影在第i个相机坐标系中的第m个路标点的像素坐标值，变换至第j个坐标系中，变换后的像素坐标值与直接观测的像素坐标值构成残差具体如下：
xcj，ycj，zcj的含义为第j个相机坐标系三维空间点的坐标(xcj，ycj，zcj)；
ucj，vcj的含义为第j个相机坐标系直接观测到的二维像素坐标(ucj，vcj)。
S1054：将先验信息、双目视觉约束信息和IMU约束信息构成整体的目标函数，通过非线性优化的方法优化特征点在图像中的重投影误差和IMU的测量误差。
进一步需要说明的是，测量误差的目标函数为：
rp-JpX的含义为滑动窗口中边缘化的先验信息误差；
的含义为滑动窗口中IMU的测量误差；
的含义为IMU预积分产生的噪声协方差；
的含义为滑动窗口中的视觉观测误差；
的含义为视觉观测产生的噪声协方差；
C的含义为当前滑动窗口中一对匹配良好的至少观察到两次的特征点对。
进一步需要说明的是，所述S106还包括：使用Oriented FAST关键点进行特征检测，使用Steered BRIEF描述子来构建地图的单词，并利用DBoW3词袋模型来进行存储；
具体包括：
设地图中所有的特征数量为α1，特征为xi的数量为αi1，那么单词的IDF为：
设地图N中特征xi有αi2，则单词的TF为：
则权重为：ηi＝TFi×IDFi；
地图N中每个特征的权重计算出来后，地图N中所有单词构成词袋模型VN，VN为：VN＝{(x1，η1)，(x2，η2)，...，(xN，ηN)}；
地图M和地图N的相似程度可用L1范数求解，如下式所示：
xN的含义为地图N中的某一个特征；
ηN的含义为地图N中某个特征的权重；
VNi的含义为地图N中i个特征构成的词袋模型；
VMi的含义为地图M中i个特征构成的的词袋模型。
本发明还提供一种终端机，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序时实现所述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。
本发明还提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。
从以上技术方案可以看出，本发明具有以下优点：
本发明提供的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法可以实现多信息融合及同步定位。通过非线性优化的方法融合两种传感器的数据，解决了IMU长时间运行产生漂移和双目视觉实时性差、不稳定的问题。本发明还在煤矿巡检机器人中配置了闭环检测模块，并构建关键帧数据库，使得煤矿巡检机器人移动到曾经到过的位置后，可以对当前帧数据与词袋模型中的关键帧数据进行匹配，进行全局优化，从而消除累计轨迹误差和地图误差，提高系统定位与建图精度，提升系统的鲁棒性。
附图说明
为了更清楚地说明本发明的技术方案，下面将对描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为煤矿巡检机器人建图和定位方法的SLAM系统框图；
图2为本发明涉及的煤矿巡检机器人建图和定位方法流程图；
图3为煤矿巡检机器人建图和定位方法实施例流程图；
图4为Oriented FAST关键点提取流程图；
图5为基于词袋模型的闭环检测算法流程图；
图6为终端机示意图。
具体实施方式
如图1是本发明提供基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法所对应的煤矿巡检机器人SLAM系统框图，图中示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的模块而非按照实际实施时的模块数目及功能，其实际实施时各模块的功能、数量及作用可为一种随意的改变，且其模块的功能和用途也可能更为复杂。
本实施例涉及的SLAM系统是SLAM是Simultaneous Localization and Mapping的缩写，将SLAM系统应用到煤矿巡检机器人建图和定位中，可以对煤矿巡检机器人在建图和定位过程中，对关键帧进行追踪，以提高系统的实时性和在特征缺失环境下的鲁棒性。本发明通过检测闭环关键帧和执行位姿图优化，从而提高基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法在执行过程的全局一致性。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图2和图3所示，本实施例提供的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法包括如下步骤：
S101：定义煤矿巡检机器人的功能模块。
在一个示例性实施例中，煤矿巡检机器人的硬件功能模块包括但不限于双目视觉相机、无线通信模块、用户输入模块、感测模块、输出模块、存储器、接口模块、控制器、行走模块、伺服电机和电源模块等等。但是应理解的是，并不要求实施所有示出的组件。可以替代地实施更多或更少的组件。
煤矿巡检机器人的软件功能模块可以包括但不限于数据获取模块、IMU模块、联合初始化模块、前端视觉里程计模块、后端非线性优化模块、闭环检测模块。
S102：构建实现煤矿巡检机器人移动定位的地图，并基于地图分别建立世界坐标系、双目相机坐标系、IMU坐标系、成像坐标系以及像素坐标系；构建IMU坐标系与世界坐标系之间的转换矩阵；还构建世界坐标系、双目相机坐标系与成像坐标系之间的转换矩阵以及构建IMU坐标系与双目相机坐标系之间的转换矩阵；
在一个示例性实施例中，定义世界坐标系为w系，双目相机坐标系为c系，IMU坐标系即载体坐标系为b系，成像坐标系为i系，像素坐标系为p系，并构建载体坐标系即IMU坐标系(b系)与世界坐标系(w系)之间的转换矩阵；并构建世界坐标系(w系)、相机坐标系(c系)与成像坐标系(i系)之间的转换矩阵；最后构建载体坐标系(b系)与双目相机坐标系(c系)之间的转换矩阵。
根据本申请的实施例，以煤矿巡检机器人启动时的位置为原点，建立世界坐标系Ow-Xw-Yw-Zw。
以双目相机的左目相机中心为基准，建立双目相机坐标系Oc-Xc-Yc-Zc。
可选地，IMU模块安装在载体的质心位置处，也就是煤矿巡检机器人的质心位置处。此时IMU坐标系与载体坐标系为重合状态，在实际应用中，也可将这两个坐标系的差距忽略不计，故可将煤矿巡检机器人车头的朝向为X轴正方向，垂直于地面向上为Z轴正方向，垂直于X轴向右为Y轴正方向，建立IMU坐标系即载体坐标系Ob-Xb-Yb-Zb。
需要说明的是，IMU坐标系与双目相机坐标系可通过两者之间的外参标定，使得IMU数据可通过平移旋转到双目相机坐标系，最终使两者传感器的数据联系起来。
S103：将双目相机和IMU模块采集的原始数据传输到工控机中进行预处理，使IMU模块采集的数据同双目视觉数据进行融合优化；
在一个示例性实施例中，首先对IMU数据进行预积分处理，具体步骤为：
S1031：建立IMU测量模型与运动学模型。
由于IMU测量值中存在噪声，对IMU的加速度计和陀螺仪测量值与真值之间建立误差模型，有：
其中和分别表示加速度计和陀螺仪的测量值，a(t)和ω(t)分别表示两者的真值，ba(t)和bg(t)分别表示加速度计和陀螺仪的偏置，σa(t)和σg(t)分别表示二者的噪声项，σa(t)和σg(t)服从零均值的高斯分布。
分别用下标W(w系)和下标B(b系)代表世界坐标系和IMU坐标系，由上述IMU误差模型分别得到t时刻加速度和角速度建立IMU的测量模型为：
和为IMU坐标系下的加速度和角速度的测量值，aw(t)和ωB(t)分别为加速度和角速度的真实值，gw为世界坐标系下的重力，为世界坐标系到IMU坐标系的旋转矩阵。
将测量到的和求解微分方程得到IMU的运动学模型：
其中分别表示IMU坐标系到世界坐标系的旋转矩阵、线速度以及平移向量。
用t表示采样时刻，带入IMU的测量模型，并对IMU的运动学模型离散化后进行欧拉积分，可得：
上式中，Δt为IMU两次相邻测量之间的时间间隔，exp()为李代数so(3)到李群SO(3)的指数映射，σgd(t)和σad(t)分别代表σg(t)和σa(t)的离散形式。
S1032：IMU预积分处理。
具体方式为：在第i帧和第j帧图像之间有多个IMU数据时，则将IMU数据的第一帧与第i帧图像关键帧对齐，将IMU数据的最后一帧与第j帧图像关键帧对齐，对齐之后，对这两个连续时间间隔为Δf的图像关键帧之间的IMU数据进行积分处理，预积分使得每项与起始状态无关，将参考坐标系从世界坐标系调整为第k个关键帧bk时刻，有：
公式中的积分项不再是相对于世界坐标系的状态量，而是相对于第k时刻的状态量。在后面的优化迭代中，不需再重复计算积分量。双目视觉与IMU数据融合时，可以选取任意两个时刻之间的预积分量进行。
本实施例中，还对双目相机采集的图像数据进行特征提取与位姿解算处理，具体步骤为：
(1)提取Oriented FAST关键点和Steered BRIEF描述子，并采用图像金字塔来确保特征点的尺度不变性，最后使用灰度质心法来计算关键点的主方向，统一将像素旋转到这个主方向，解决特征点的旋转不变性。
其中，灰度质心法求关键点旋转角度的计算方式如下：
定义区域图像的矩为：
其中，p，q取0或1，I(x，y)为像素坐标(x，y)处图像的灰度值，mpq表示图像的矩。
在图像半径为R的圆形区域，沿两个坐标轴x，y方向的图像矩分别为：
则圆形区域内全部像素灰度值的总和为：
图像的质心为：
从图像圆形区域形心O指向质心C的方向向量即可为关键点的主方向，故关键点的旋转角度记为：θ＝arctan2(cy，cx)＝arctan2(m01，m10)。
(2)位姿解算的方式为：通过双目相机可直接计算得到深度，采用迭代最近点的方法来求解估计相机位姿的变化。假定一组匹配好的3D点为p＝{p1，p2，...，pn}，p′＝{p′1，p′2，...，p′n}，先定义第i对点的误差项ei为：
ei＝pi-(Rp′i+t)
构建最小二乘问题，求使得误差平方和达到极小的旋转矩阵R和平移矩阵t。
求解过程为：由三维空间点的匹配关系，根据位姿变换关系得：pw＝Rp′w+t
其中pw和p′w分别表示前后两个图像中观测到的三维空间点。
首先计算这两组三维空间点的质心位置，用p和p′表示，得：
再计算两组三维空间点的去质心坐标，用qw和q′w表示，得：
qw＝pw-p
q′w＝p′w-p′
然后将旋转参数作为优化变量计算旋转矩阵R，构建最小二乘问题R*得：
最后由计算出的旋转矩阵R，计算平移矩阵t得：
t＝p-Rp′。
S104：对双目相机和IMU模块进行初始化，并将双目视觉数据与IMU数据对齐。
具体包括：通过双目相机和IMU之间的旋转约束初始化陀螺仪偏置bias；通过双目相机和IMU之间的平移约束来估计速度和重力方向，改进重力向量g；最后将双目相机坐标系(c系)的变量转化到世界坐标系(w系)下，将轨迹对齐到世界坐标系下。对齐之后在滑动窗口内采用非线性优化的方法进行数据融合。
根据本申请的实施例，陀螺仪偏置bias初始化的方式是在一个连续n帧图像的数据窗口，定义i时刻的IMU和双目相机存在以下约束：
下标w、c、b分别表示世界坐标系(w系)、相机坐标系(c系)和IMU坐标系(b系)，Rbc和tbc分别为c系相对于b系的旋转和平移外参。
由IMU预积分可求得i时刻相邻图像间的预积分旋转ΔRi，i+1，则陀螺仪偏置bg为：
本实施例还对重力向量g和速度进行初始化。
其中，设重力为G，重力向量参数化得：
式中为重力方向，b1和b2为与垂直的两个相互垂直的单位向量，ω1和ω2分别为b1和b2方向上的扰动，b1和b2为：
窗口内图像帧对应的速度为vi，由IMU预积分的速度项和平移项可得：
合并n+1个图像帧的n个上式为：
将g代入上式进行迭代，使g收敛，得到最终的重力向量g和速度v。
S105：构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据。
可以看出，构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据，可以提高位姿估计的准确度，优化定位与建图的精度。具体的方式为：
S1051：对存在于滑动窗口内的n+1个关键帧和m+1个地图点，将待优化的状态量定义为：
λi＝[x，y，z]T，i∈[0，m]
滑动窗口内存在n个关键帧，X为状态向量，其中λi为观测到的地图点，xi为关键帧对应的系统状态量，Rbc为双目相机到IMU的旋转外参；xk为滑动窗口内第k帧的状态量，其中包括IMU预积分得到的旋转项为速度项为位移项为陀螺仪偏置为ba、加速度计偏置为ba；为双目相机到IMU的外参，通过双目相机与IMU联合标定得到。
S1052：构建IMU约束，将IMU两帧之间的位置、速度、旋转、加速度偏置bias以及陀螺仪偏置bias的变化量之差定义为IMU残差，有：
上式中，为四元素的乘法，优化变量为第k帧和第k+1帧的状态量，共有10个变量。
S1053：构建双目视觉约束，像素重投影造成视觉残差，将首次投影在第i个相机坐标系中的第m个路标点的像素坐标值，变换至第j个坐标系中，变换后的像素坐标值与直接观测的像素坐标值构成残差，具体如下：
其中，特征点在第i帧投影到第j帧相机坐标系中的值为：
上式中，λ为逆深度。优化变量为两时刻间的状态量、外参和逆深度。
S1054：将先验信息、双目视觉约束信息和IMU约束信息构成整体的目标函数，通过非线性优化的方法优化特征点在图像中的重投影误差和IMU的测量误差，进一步提高定位精度，目标函数为：
由视觉观测产生的噪声协方差为由IMU预积分产生的噪声协方差为最后通过列文伯格-马夸尔特迭代方法求解，使得误差最小。
S106：配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。
本实施例中，如图4和图5所示，在煤矿巡检机器人中配置了闭环检测模块，并构建关键帧数据库，使得煤矿巡检机器人移动到曾经到过的位置后，可以对当前帧数据与词袋模型中的关键帧数据进行匹配，进行全局优化，从而消除累计轨迹误差和地图误差，提高系统定位与建图精度，提升系统的鲁棒性。
作为本发明的一种实施例，使用Oriented FAST关键点进行特征检测，使用Steered BRIEF描述子来构建地图的单词，并利用DBoW3词袋模型来进行存储。
设地图中所有的特征数量为α1，特征为xi的数量为αi1，那么单词的IDF(InverseDocument Frequency)为：
设地图N中特征xi有αi2，则此单词的TF(Term Frency)为：
则权重为：ηi＝TFi×IDFi
地图N中每个特征的权重计算出来后，地图N中所有单词构成词袋模型VN，VN为：VN＝{(x1，η1)，(x2，η2)，...，(xN，ηN)}
地图M和地图N的相似程度可用L1范数求解，如下式所示：
基于上述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法可以实现多信息融合及同步定位。通过非线性优化的方法融合两种传感器的数据，解决了IMU长时间运行产生漂移和双目视觉实时性差、不稳定的问题。本发明还在煤矿巡检机器人中配置了闭环检测模块，并构建关键帧数据库，使得煤矿巡检机器人移动到曾经到过的位置后，可以对当前帧数据与词袋模型中的关键帧数据进行匹配，进行全局优化，从而消除累计轨迹误差和地图误差，提高系统定位与建图精度，提升系统的鲁棒性。
本申请的实施例还提供了终端机，如图6所示，包括存储器102、处理器101及存储在所述存储器上并可在所述处理器上运行的计算机程序。处理器101可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置。所述处理器101执行所述程序时实现基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。上述终端机还可以包括用于通信功能的无线通信模块103以及输入输出设备104。输入输出设备104可以包括但不限于液晶显示器(LCD)、触摸屏显示器、键盘、鼠标、轨迹球、语音识别器、或其他已知的输入输出设备104。
本领域普通技术人员可以理解，图6所的结构仅为示意，其并不对上述终端机的结构造成限定。例如，终端机还可包括比图6中所示更多或者更少的组件，或者具有与图6所示不同的配置。
作为本发明的一种实施例，终端机可以基于用户输入控制指令，定义煤矿巡检机器人的功能模块；根据煤矿巡检机器人的使用环境及生产工艺的需要构建实现煤矿巡检机器人移动定位的地图，并基于地图分别建立世界坐标系、双目相机坐标系、IMU坐标系、成像坐标系以及像素坐标系；构建IMU坐标系与世界坐标系之间的转换矩阵。
本实施例对煤矿巡检机器人的定位可以基于建立的坐标系来进行定位，对于煤矿巡检机器人的位置上传给终端机的方式可以通过在煤矿巡检机器人上安装定位模块，将煤矿巡检机器人的位置信息实时上传给终端机。终端机基于具有坐标系的地图来显示煤矿巡检机器人的位置信息。
本实施例中，终端机还构建世界坐标系、双目相机坐标系与成像坐标系之间的转换矩阵以及构建IMU坐标系与双目相机坐标系之间的转换矩阵；将双目相机和IMU模块采集的原始数据传输到工控机中进行预处理，使IMU模块采集的数据同双目视觉数据进行融合优化；对双目相机和IMU模块进行初始化，并将双目视觉数据与IMU数据对齐。
本实施例中，终端机构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据；配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。
本申请的实施例还提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。
通过以上的实施方式的描述，本领域的技术人员易于理解，这里描述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法可以通过软件实现，也可以通过软件结合必要的硬件的方式来实现。因此，根据基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法公开实施方式的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个可读存储介质(可以是CD-ROM，U盘，移动硬盘等)中或网络上，包括若干指令以使得一台计算设备(可以是个人计算机、服务器、移动终端、或者网络设备等)执行根据本公开实施方式的索引方法。
本发明涉及的可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以为但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，方法包括：
S101：定义煤矿巡检机器人的功能模块；
S102：构建实现煤矿巡检机器人移动定位的地图，并基于地图分别建立世界坐标系、双目相机坐标系、IMU坐标系、成像坐标系以及像素坐标系；构建IMU坐标系与世界坐标系之间的转换矩阵；
还构建世界坐标系、双目相机坐标系与成像坐标系之间的转换矩阵以及构建IMU坐标系与双目相机坐标系之间的转换矩阵；
S103：将双目相机和IMU模块采集的原始数据传输到工控机中进行预处理，使IMU模块采集的数据同双目视觉数据进行融合优化；
S104：对双目相机和IMU模块进行初始化，并将双目视觉数据与IMU数据对齐；
S105：构建整体残差方程，建立优化问题，通过非线性优化的方法融合滑动窗口内的数据；
S106：配置闭环检测模块，利用词袋模型构建关键帧数据库，并将煤矿巡检机器人移动至历史的位置后，再将煤矿巡检机器人当前帧数据与词袋模型中的关键帧数据进行匹配，实现构建的地图形成一个闭环。
2.根据权利要求1所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S102还包括：
以煤矿巡检机器人启动时的位置为原点，建立世界坐标系Ow-Xw-Yw-Zw；
以双目相机的左目相机中心为基准，建立双目相机坐标系Oc-Xc-Yc-Zc；
将煤矿巡检机器人车头的朝向为X轴正方向，垂直于地面向上为Z轴正方向，垂直于X轴向右为Y轴正方向，建立IMU坐标系Ob-Xb-Yb-Zb；
基于IMU坐标系与双目相机坐标系之间的外参标定，使得IMU数据通过平移旋转到双目相机坐标系，最终使IMU坐标系与双目相机坐标系的数据构成关联关系。
3.根据权利要求1所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S103中的预处理方式包括对IMU数据进行预积分处理，具体步骤为：
S1031：建立IMU测量模型与运动学模型；
建立加速度计和陀螺仪得到的测量值与真值之间的误差模型：
其中，和分别表示加速度计和陀螺仪的测量值，a(t)和ω(t)分别表示两者的真值，ba(t)和bg(t)分别表示加速度计和陀螺仪的偏置，σa(t)和σg(t)分别表示二者的噪声项，σa(t)和σg(t)服从零均值的高斯分布；
用t表示采样时刻，对IMU模块的运动学模型离散化后进行欧拉积分，得到如下方式：
Δt为IMU模块两次相邻测量之间的时间间隔，exp()为李代数so(3)到李群SO(3)的指数映射，σgd(t)和σad(t)分别代表σg(t)和σa(t)的离散形式；
S1032：对IMU模块预积分处理；
在第i帧和第j帧图像之间有多个IMU数据时，则将IMU数据的第一帧与第i帧图像关键帧对齐，将IMU数据的最后一帧与第j帧图像关键帧对齐，对齐之后，对这两个连续时间间隔为Δf的图像关键帧之间的IMU数据进行积分处理，预积分使得每项与起始状态无关，将参考坐标系从世界坐标系调整为第k个关键帧bk时刻，有：
其中，ΔRij为从i帧时刻到j帧时刻旋转的预积分增量；为i帧时刻世界坐标系到IMU坐标系的旋转矩阵的转置；为j帧时刻世界坐标系到IMU坐标系的旋转矩阵；为k帧时刻陀螺仪的偏置；为k帧时刻陀螺仪噪声项的离散形式；ΔVij为从i帧时刻到j帧时刻线速度的预积分增量；Vj为j帧时刻的线速度；Vi为i帧时刻的线速度；gW为世界坐标下的重力加速度；ΔRik为i帧时刻到k帧时刻旋转的增量；为k帧时刻IMU坐标系下的加速度；为k帧时刻加速度计的偏置；为k帧时刻加速度计噪声项的离散形式；ΔPij为i帧时刻到j帧时刻平移的预积分增量；Pj的含义为j帧时刻的平移量；Pi的含义为i帧时刻的平移量；Δvik的含义为i帧时刻到k帧时刻线速度的增量；的含义为k帧时刻世界坐标系下的加速度；的含义为k帧时刻陀螺仪的偏置；i的含义为双目相机采集图像的第i帧时刻；j的含义为双目相机采集图像的第j帧时刻；k的含义为双目相机采集图像的第k帧时刻。
4.根据权利要求3所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S103中，还对双目相机采集的图像数据进行特征提取与位姿解算处理，具体步骤为：
提取Oriented FAST关键点和Steered BRIEF描述子，并采用图像金字塔来确保特征点的尺度不变性，最后使用灰度质心法来计算关键点的主方向，统一将像素旋转到所述主方向，其中，灰度质心法求关键点旋转角度的计算方式如下：定义区域图像的矩为：
其中，p，q取0或1，I(x，y)为像素坐标(x，y)处图像的灰度值，mpq表示图像的矩；
从图像圆形区域形心O指向质心C的方向向量即可为关键点的主方向，故关键点的旋转角度记为：θ＝arctan2(cy，cx)＝arctan2(m01，m10)；
方法中还对位姿进行解算，具体为：
由三维空间点的匹配关系，根据位姿变换关系得：pw＝Rp′w+t；
其中，pw和p′w分别表示前后两个图像中观测到的三维空间点；
首先计算两组三维空间点的质心位置，用p和p′表示，得：
再计算两组三维空间点的去质心坐标，用qw和q′w表示，得：
qw＝pw-p
q′w＝p′w-p′
然后将旋转参数作为优化变量计算旋转矩阵R，构建最小二乘问题R*得：
最后由计算出的旋转矩阵R，计算平移矩阵t得：
t＝p-Rp′。
5.根据权利要求1或2所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S104还包括：通过双目相机和IMU之间的旋转约束初始化陀螺仪偏置bias；通过双目相机和IMU模块之间的平移约束来估计速度和重力方向；最后将双目相机坐标系的变量转化到世界坐标系下，将轨迹对齐到世界坐标系下；对齐之后在滑动窗口内采用非线性优化的方法进行数据融合。
6.根据权利要求1或2所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S105还包括：
S1051：将待优化的状态量定义为：
λi＝[x，y，z]T，i∈[0，m]
滑动窗口内存在n个关键帧，X为状态向量，其中，λi为观测到的地图点，xi为关键帧对应的系统状态量，Rbc为双目相机到IMU的旋转外参；xk为滑动窗口内第k帧的状态量，IMU预积分得到的旋转项为速度项为位移项为陀螺仪偏置为ba、加速度计偏置为ba；为双目相机到IMU模块的外参；
S1052：构建IMU约束，将IMU两帧之间的位置、速度、旋转、加速度偏置bias以及陀螺仪偏置bias的变化量之差定义为IMU残差；
S1053：构建双目视觉约束，像素重投影造成视觉残差，将首次投影在第i个相机坐标系中的第m个路标点的像素坐标值，变换至第J个坐标系中，变换后的像素坐标值与直接观测的像素坐标值构成残差
具体如下：
xcj，ycj，zcj的含义为第j个相机坐标系三维空间点的坐标(xcj，ycj，zcj)；
ucj，vcj的含义为第j个相机坐标系直接观测到的二维像素坐标(ucj，vcj)；
S1054：将先验信息、双目视觉约束信息和IMU约束信息构成整体的目标函数，通过非线性优化的方法优化特征点在图像中的重投影误差和IMU的测量误差。
7.根据权利要求6所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，测量误差的目标函数为：
rp-JpX的含义为滑动窗口中边缘化的先验信息误差；
的含义为滑动窗口中IMU的测量误差；
的含义为IMU预积分产生的噪声协方差；
的含义为：滑动窗口中的视觉观测误差；
的含义为视觉观测产生的噪声协方差；
C的含义为当前滑动窗口中一对匹配良好的至少观察到两次的特征点对。
8.根据权利要求1或2所述的基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法，其特征在于，所述S106还包括：使用Oriented FAST关键点进行特征检测，使用SteeredBRIEF描述子来构建地图的单词，并利用DBoW3词袋模型来进行存储；
具体包括：
设地图中所有的特征数量为α1，特征为xi的数量为αi1，那么单词的IDF为：
设地图N中特征xi有αi2，则单词的TF为：
则权重为：ηi＝TFi×IDFi；
地图N中每个特征的权重计算出来后，地图N中所有单词构成词袋模型VN，VN为：VN＝{(x1，η1)，(x2，η2)，...，(xN，ηN)}；
地图M和地图N的相似程度可用L1范数求解，如下式所示：
xN的含义为地图N中的某一个特征；
ηN的含义为地图N中某个特征的权重；
VN的含义为地图N中i个特征构成的词袋模型；
VMi的含义为地图M中i个特征构成的的词袋模型。
9.一种终端机，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至8任一项所述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。
10.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8任一项所述基于双目视觉与IMU融合的煤矿巡检机器人建图和定位方法的步骤。
CN202410460082.1A 2024-04-16 2024-04-16 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质 Pending CN118376230A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410460082.1A CN118376230A (zh) 2024-04-16 2024-04-16 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410460082.1A CN118376230A (zh) 2024-04-16 2024-04-16 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质

Publications (1)

Publication Number Publication Date
CN118376230A true CN118376230A (zh) 2024-07-23

Family

ID=91911990

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410460082.1A Pending CN118376230A (zh) 2024-04-16 2024-04-16 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质

Country Status (1)

Country Link
CN (1) CN118376230A (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN111735445A (zh) * 2020-06-23 2020-10-02 煤炭科学研究总院 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN112033400A (zh) * 2020-09-10 2020-12-04 西安科技大学 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
WO2022257801A1 (zh) * 2021-06-09 2022-12-15 山东大学 基于slam的移动机器人矿山场景重建方法及系统
WO2024045632A1 (zh) * 2022-08-31 2024-03-07 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN111735445A (zh) * 2020-06-23 2020-10-02 煤炭科学研究总院 融合单目视觉与imu的煤矿巷道巡检机器人系统及导航方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN112033400A (zh) * 2020-09-10 2020-12-04 西安科技大学 一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
WO2022257801A1 (zh) * 2021-06-09 2022-12-15 山东大学 基于slam的移动机器人矿山场景重建方法及系统
WO2024045632A1 (zh) * 2022-08-31 2024-03-07 华南理工大学 基于双目视觉和imu的水下场景三维重建方法及设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
张玉龙;张国山;: "基于关键帧的视觉惯性SLAM闭环检测算法", 计算机科学与探索, no. 11, 30 April 2018 (2018-04-30) *
李同;张奇志;: "基于ORB词袋模型的SLAM回环检测研究", 信息通信, no. 10, 31 October 2017 (2017-10-31) *

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN109991636B (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
WO2021180128A1 (zh) 一种基于rgbd传感器和imu传感器的定位方法
CN113052908B (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN112484725A (zh) 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN108051002A (zh) 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
CN105843223A (zh) 一种基于空间词袋模型的移动机器人三维建图与避障方法
CN111536967A (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
CN106030430A (zh) 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN114088087B (zh) 无人机gps-denied下高可靠高精度导航定位方法和系统
CN106052584A (zh) 一种基于视觉及惯性信息融合的轨道空间线形测量方法
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN108981693A (zh) 基于单目相机的vio快速联合初始化方法
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN103940442A (zh) 一种采用加速收敛算法的定位方法及装置
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN114627253A (zh) 地图构建方法、装置与设备
CN118376230A (zh) 一种基于双目视觉与imu融合的煤矿巡检机器人建图和定位方法、终端机及可读存储介质
Zhang et al. UWB/IMU integrated inspection robot positioning in underground substation
Shmatko et al. Estimation of rotation measurement error of objects using computer simulation
US11992961B2 (en) Pose determination method, robot using the same, and computer readable storage medium
CN113375665B (zh) 基于多传感器松紧耦合的无人机位姿估计方法
Duan et al. A low-cost and portable indoor 3D mapping approach using biaxial line laser scanners and a one-dimension laser range finder integrated with microelectromechanical systems

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