CN113091738A - 基于视觉惯导融合的移动机器人地图构建方法及相关设备 - Google Patents
基于视觉惯导融合的移动机器人地图构建方法及相关设备 Download PDFInfo
- Publication number
- CN113091738A CN113091738A CN202110382654.5A CN202110382654A CN113091738A CN 113091738 A CN113091738 A CN 113091738A CN 202110382654 A CN202110382654 A CN 202110382654A CN 113091738 A CN113091738 A CN 113091738A
- Authority
- CN
- China
- Prior art keywords
- state
- imu
- robot
- map
- visual
- 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.)
- Granted
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 62
- 230000004927 fusion Effects 0.000 title claims abstract description 53
- 238000010276 construction Methods 0.000 title claims abstract description 11
- 238000000034 method Methods 0.000 claims abstract description 33
- 238000011156 evaluation Methods 0.000 claims abstract description 30
- 230000006870 function Effects 0.000 claims description 56
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000004364 calculation method Methods 0.000 claims description 19
- 238000005259 measurement Methods 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 12
- 230000005484 gravity Effects 0.000 claims description 11
- 238000004590 computer program Methods 0.000 claims description 10
- 230000004438 eyesight Effects 0.000 claims description 10
- 230000010354 integration Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 4
- 230000002301 combined effect Effects 0.000 claims description 3
- 238000005381 potential energy Methods 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 230000000717 retained effect Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 239000000243 solution Substances 0.000 description 4
- 230000009977 dual effect Effects 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 230000003750 conditioning effect Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000016776 visual perception Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 239000003637 basic solution Substances 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000011423 initialization method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000007670 refining Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
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/20—Instruments for performing navigational calculations
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/15—Correlation function computation including computation of convolution operations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computational Mathematics (AREA)
- Automation & Control Theory (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了基于视觉惯导融合的移动机器人地图构建方法及相关设备，该方法包括：S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；S2，构建情形评估函数实时分析姿态趋势；S3，确定融合状态局部期望值并输出该状态；S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。本发明保证了相机和IMU数据之间能准确融合，***读取当前地图点位姿信息效率高、误差小。
Description
技术领域
本发明属于同步定位与地图创建(Simultaneous Location And Mapping，SLAM)技术领域，涉及基于视觉惯导融合的移动机器人地图构建方法及相关设备。
背景技术
同步定位与地图创建(Simultaneous Location And Mapping，SLAM)的核心问题是，要求机器人在一个陌生的环境中，首先要探索环境从而了解环境(构建地图)，同步运用地图追踪机器人在该环境中的位置(定位)。对SLAM问题的传统解决方法主要是基于数学概率方法，其中，卡尔曼滤波器、粒子滤波器和极大期望算法等是机器人SLAM问题的基本解决方法。虽然这些传统的SLAM算法中仍使用激光测距和声呐测距采集信息，但是这些传感器采集信息，往往得到的数据不精确且使用有一定的局限性，更多情况下激光测距传感器已被视觉传感器所替代。
然而传统视觉SLAM算法仅依靠单一传感器已无法满足科研的需求，也无法在复杂快速运动的环境中及时捕捉机器人的运动信息并反馈给主控机进行运动补偿。科研工作者开始融合两种不同传感器的信息意图互补两者的不足，期望惯性测量单元(InertialMeasurement Unit，IMU)可以在快速运动时替代相机捕捉相机运动的物理信息提高视觉SLAM定位精度，而相机也可以弥补IMU在慢速运动下因使用时间过长使得累计误差漂移的问题。
但目前的融合方法在构建地图时，并未实现实时对运动的姿态趋势进行分析，不能确定惯导融合的局部实时最优状态，因此相机和IMU数据无法准确融合，导致读取当前地图点位姿信息效率低、误差大。同时具有后端计算量较大，运行时间长，处理误差实时性差等问题。
发明内容
本发明的目的在于提供基于视觉惯导融合的移动机器人地图构建方法，以解决现有技术中相机和IMU数据无法准确融合导致读取当前地图点位姿信息效率低、误差大以及后端处理误差实时性差的技术问题。
所述的基于视觉惯导融合的移动机器人地图构建方法，所述方法包括：
步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；
步骤S2，构建情形评估函数实时分析姿态趋势；
步骤S3，确定融合状态局部期望值并输出该状态：通过评估函数值与阈值比较判断此时机器人的运动状态观测数据是否具有高确定性，同时增加时间阈值设定双重约束评估状态以确定是否终止视觉惯性联合初始化并输出当前状态；
步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；
步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
优选的，所述步骤S1中，IMU测量值由真实值、随机游走偏差和白噪声三个部分组成，切加速度计仍受到重力加速度的影响，将IMU运动状态变量利用中值法离散化以方便相机跟IMU进行状态融合，离散化的运动状态变量算式如下：
ppre、vpre和qpre分别代表IMU位置、速度和姿态四元数的初始值；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的加速度；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的角速度；
为内积的运算法则；
和
表示在第i和i+1个时刻的时间段里IMU加速度偏差和角速度偏差的变化量；
是从IMU坐标系转移到世界坐标系下的旋转矩阵，Δt为相邻时刻的间隔时间；
上述运动状态变量算式能计算出在i+1个时刻下的计算结果为zi+1，z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用IMU上一时刻的信息计算得到当前时刻的观测数据。
优选的，所述步骤S1中，用ZF表示机器人的运动状态观测数据，则存在ZF＝{z1,z2,...,zn}，zi为ZF中具体的第i个时刻下的观测数据；若机器人的运动状态观测数据ZF＝{z1,z2,...,zn}服从概率分布f(ZF；Y)，其中Y是需要求解的机器人状态变量目标函数，得到的状态估计函数如下式：
构建机器人状态变量目标函数Y来度量机器人观测状态数据并估计观测的位姿随时间变化的趋向的置信度，机器人运动状态置信度表达如下式：
其中Ω(ZF；Y)为机器人运动状态观测数据ZF的置信度，E(·)为计算机器人运动状态观测数据ZF的期望，S(·)为计算机器人运动状态观测数据ZF的标准差，Var(·)为计算机器人运动状态观测数据ZF的方差，通过机器人运动状态置信度确定融合达成度。
优选的，所述步骤S2中，定义视觉惯性观测样本情形评估函数如下式：
其中，fλ(ZF)为机器人运动状态观测数据情形评估函数，Ω(ZF；Y)是从关于评估机器人运动状态观测数据ZF的位置、速度、旋转四元数(尺度、重力矢量)和偏差等相关状态获得的信息矩阵置信度，
为机器人运动状态中某个关键帧F观测数据ZF对应的协方差矩阵，F为运行过程中某个图像帧，Δτ是与图像帧相关的图像信息矩阵。
优选的，所述步骤S2中，构建机器人运动状态观测数据ZF情形评估函数的性能度量如下式
其中fdet(ZF)为移动机器人在运动时拍摄的某个包含惯性测量单元状态信息和视觉状态信息的图像帧F的观测数据情形性能度量，Ω(ZF；Y)为机器人运动状态中某个图像帧F状态观测数据置信度，det(·)是指求解矩阵的行列式，
为机器人运动状态中某个图像帧F观测数据ZF对应的协方差矩阵，Δτ为与图像帧有关的图像信息矩阵，log(·)为对数函数。
优选的，所述步骤S3中，定义情形评估函数值小于阈值Sth来衡量初始化状态具有较高的置信度，认为此时的融合状态具有高确定性，终止初始化，将相应的预积分和初始化数据融合，进入下一步的视觉惯性实时优化；同时在初始化过程中额外增加一个时间阈值设定，若时间阈值内情形评估函数未达到较高的置信度，在到达时间阈值时直接终止初始化，将当前初始化结果送入下一阶段实时优化。
优选的，所述步骤S4中，构建基于共视约束的关键帧局部优化概率图模型，按共视强弱对关键帧进行分级处理，将帧间观测信息及关键帧上地图点等信息作为约束项优化强共视关键帧；共视约束概率图模型可以看成一个无向加权图，其联合概率分布即可分解为多个团Q势能函数的乘积，如下式：
其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，
为包含共同观测地图点的关键帧的稀疏权重函数，ψ为正则化因子，通过正则化满足联合概率的有效性；其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，
为包含共同观测地图点的关键帧的稀疏权重函数。
优选的，所述步骤S5中，滑动窗口优化时，仅优化强共视关键帧，同时保留弱共视关键帧的位姿信息作为约束项；视觉位姿估计误差项、IMU预积分误差项分量构建一个目标函数中，最小化重投影误差和IMU误差优化状态量，目标函数如下式所示：
其中，Ereproj为两相邻关键帧之间的重投影误差；Eimu为IMU在时间段Δt的误差，
由视觉和IMU共同影响下的偏差测量组成。eobserve为相机观测误差，
为相机关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ep、ev和eq分别为IMU在运行时的位置、速度和姿态四元数的误差，
为IMU关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ρ(·)为鲁棒核函数，eb为IMU某个时间点的随机偏差集合，
为相机和IMU共同影响下关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵。
本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
本发明还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
本发明的技术效果：1、本发明使用基于费雪信息矩阵为主导的双重视觉惯导初始化方法，针对状态观测值构建观测样本情形评估函数，评估状态误差信息矩阵，判断视觉惯导融合的局部实时最优状态作为终止条件，有效保证了相机和IMU数据之间能准确融合，***读取当前地图点位姿信息效率高、误差小。为防止评估时间过长，增加时间阈值设定双重约束评估状态观测变化趋势，缩短初始化时间，提高***鲁棒性。
2、使用基于包含共同观测地图点的稀疏程度分级关键帧，优化强共视关键帧，保留弱共视关键帧的地图点信息进入滑动窗口作为约束项，与重投影误差和IMU误差一同约束强共视关键帧上的位姿观测状态。这样在保证对误差处理可靠性的前提下，减少了计算量，大大缩短耗时，解决了后端处理误差实时性差的问题。
附图说明
图1为基于双重阈值影响的视觉惯导移动机器人融合地图构建方法的流程示意图。
图2为构建视觉惯导初始化阶段中IMU初始化的流程示意图。
图3为IMU初始化情形评估的流程示意图。
图4为本发明在视觉惯性联合初始化阶段的偏差变化。
图5为本发明滑动优化阶段采用共视分级方法的效果图。
图6为本发明最终的定位效果图。
具体实施方式
下面对照附图，通过对实施例的描述，对本发明具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
根据相机传感器和惯性测量单元两种传感器对于外界和自在的环境信息读取，并构建两种信息相互交换的实现。利用该原理对现有的SLAM技术进行改进得到本发明方案。
实施例一：
本发明提供了基于视觉惯导融合的移动机器人地图构建方法，应用该方法的***同时包括有模拟视觉***和模拟惯性***。
模拟视觉***包括：两个视觉感知标定器、信号调理电路、模数(AD)转换器。其中，两个视觉标定器对称安置，主控芯片优选为STM32主控芯片。
模拟惯性***包括：陀螺仪、加速度计、信号调理电路、模数模数(AD)转换器。其中，惯性测量单元选择集成性较强的MPU6050芯片，内含三轴陀螺仪、三轴加速度计以及三轴磁力计。
如图1-3所示，本方法具体包括；
步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
所述步骤S1中，首先定义IMU测量值(包括陀螺仪和加速度计)由真实值、随机游走偏差和白噪声三个部分组成，除此之外，加速度计仍受到重力加速度的影响。为方便相机跟IMU的状态融合，将IMU运动状态变量利用中值法离散化，得到状态变量表示如下式：
ppre、vpre和qpre分别代表IMU位置、速度和姿态四元数的初始值；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的加速度；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的角速度；
为内积的运算法则；
和
表示在第i和i+1个时刻的时间段里IMU加速度偏差和角速度偏差的变化量；
是从IMU坐标系转移到世界坐标系下的旋转矩阵，Δt为相邻时刻的间隔时间。
上述运动状态变量算式能计算出在i+1个时刻下的计算结果为Zi+1，z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用IMU上一时刻的信息计算得到当前时刻的观测数据。
用ZF表示机器人的运动状态观测数据，则存在ZF＝{z1,z2,...,zn}，zi为ZF中具体的第i个时刻下的观测数据。依据步骤S1中状态变量的算式能得到在i+1个时刻下的计算结果为zi+1，而z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用IMU上一时刻的信息计算得到当前时刻的观测数据。若机器人的运动状态观测数据ZF＝{z1,z2,...,zn}服从概率分布f(ZF；Y)，Y是步骤S1得到的需要求解的机器人状态变量目标函数，得到的状态估计函数如下式
构建机器人状态变量目标函数Y来度量机器人观测状态数据并估计观测的位姿(位置、速度、旋转四元数以及偏差)随时间变化的趋向的置信度，机器人运动状态置信度表达如下式
其中Ω(ZF；Y)为机器人运动状态观测数据ZF的置信度，E(·)为计算机器人运动状态观测数据ZF的期望，S(·)为计算机器人运动状态观测数据ZF的标准差，Var(·)为计算机器人运动状态观测数据ZF的方差，通过机器人运动状态置信度确定融合达成度。
步骤S1完成视觉初始化、视觉惯性校准对齐以及惯性初始化，具体包括：
S1.1视觉初始化。相机读取视觉信息并计算重投影误差；
S1.2视觉惯导对齐配准。同步相机与IMU采集频率，保证采集图像帧的同时能采集到该图像帧的物理位姿信息；
S1.3陀螺仪偏置求解。最小化预积分值与视觉旋转差值求解陀螺仪偏置；
S1.4尺度、速度与重力加速度初始化。构建尺度、加速度与重力加速度线性方程进行求解；
S1.5重力加速度提炼。通过在重力向量切面上不断迭代微调重力向量；
S1.6设置世界坐标系。根据初始化求得的重力矢量设置世界坐标系。
步骤S2，根据融合情况实时分析姿态趋势。
定义视觉惯性观测样本情形评估函数如下式：
其中，fλ(ZF)为机器人运动状态观测数据情形评估函数，Ω(ZF；Y)是从关于评估机器人运动状态观测数据ZF的位置、速度、旋转四元数(尺度、重力矢量)和偏差等相关状态获得的信息矩阵置信度。
为机器人运动状态中某个关键帧F观测数据ZF对应的协方差矩阵，F为运行过程中某个图像帧，Δτ是与图像帧相关的图像信息矩阵。
为了最小化机器人运动状态观测数据信息矩阵置信度的局域半径，需要最小化信息矩阵协方差的行列式，由于得到的最小化置信度局域半径与最大化信息矩阵的对数行列式相同，从而得到机器人运动状态观测数据情形评估函数的性能度量：
其中fdet(ZF)为移动机器人在运动时拍摄的某个包含惯性测量单元状态信息和视觉状态信息的关键帧F的观测数据情形性能度量，det(·)是指求解运动状态观测数据信息矩阵的行列式，
为机器人运动状态中某个关键帧F观测数据ZF对应的协方差矩阵，Δτ为与图像帧有关的图像信息矩阵，log(·)为对数函数。
步骤S3，确定融合状态期望值并输出该状态。
定义情形评估函数值小于阈值Sth来衡量初始化状态具有较高的置信度，认为此时的具有高确定性，终止初始化，将相应的预积分和初始化数据融合，进入下一步的视觉惯性实时优化。为避免因情形评估函数计算时间过长使得IMU误差累积，故在初始化过程中额外增加一个15s的时间阈值设定，若在15s内未达到较高的置信度，在第15s时直接终止初始化，将当前初始化结果送入下一阶段实时优化。
步骤S3具体包括：
S3.1开始视觉惯性联合初始化；
S3.2判断初始化时间是否达到设定的时间阈值。若未达到，执行步骤S3.3；若达到，执行步骤S3.5；
S3.3构建最差情形评估函数实时计算状态观测值的状态期望值；
S3.4判断状态期望值Ω(z)是否达到状态期望阈值Sth。若未达到，执行步骤S3.1；若达到，执行步骤S3.5；
S3.5结束视觉惯性联合初始化。
步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
构建基于共视约束的关键帧局部优化概率图模型，按共视强弱对关键帧进行分级处理，保留强共视关键帧，忽略弱共视关键帧，将帧间观测信息及关键帧上地图点等信息作为约束项优化强共视关键帧。此时共视约束概率图模型可以看成一个无向加权图，其联合概率分布即可分解为多个团Q势能函数(因子)的乘积，如下式：
其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，ψ为正则化因子，通过正则化满足联合概率的有效性。其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，ψ为正则化因子，通过正则化满足联合概率的有效性，
为包含共同观测地图点的关键帧的稀疏权重函数。
步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键。
滑动窗口优化时，仅优化强共视关键帧，同时保留弱共视关键帧的位姿信息作为约束项。视觉位姿估计误差项、IMU预积分误差项分量构建一个目标函数中，最小化重投影误差和IMU误差优化状态量，目标函数如下式所示：
其中，Ereproj为两相邻关键帧之间的重投影误差；Eimu为IMU在时间段Δt的误差，
由视觉和IMU共同影响下的偏差测量组成。eobserve为相机观测误差，
为相机关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ep、ev和eq分别为IMU在运行时的位置、速度和姿态四元数的误差，
为IMU关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ρ(·)为鲁棒核函数，eb为IMU某个时间点的随机偏差集合，
为相机和IMU共同影响下关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵。
IMU测量误差包括传统的位姿误差ep、ev和eq及偏差eb，各误差分量如下式所示：
eb＝Δ(bg,ba)(i,j)
其中，
为IMU状态在时刻i从自身坐标系(Body)转移到世界坐标系(World)下的旋转矩阵，
和
是在IMU预积分阶段计算陀螺仪残差得出的位置、速度和旋转四元数雅克比矩阵，
是在IMU预积分阶段计算加速度残差得出的速度雅克比矩阵，Δtij为从时刻i到时刻j之间的时间段，
表示时刻j下的角速度偏差，
为IMU在时刻i从世界坐标系转到时刻j自身坐标系的位置变化值，
为IMU在时刻i从世界坐标系转到时刻j自身坐标系的速度变化值，
为IMU状态在时刻j从世界坐标系(World)转移到自身坐标系(Body)下的旋转矩阵。ΔR(i,j)为时刻j与时刻i之间的旋转四元数差值，Δv(i,j)为时刻j与时刻i之间的速度变化值，Δp(i,j)为时刻j与时刻i之间的位置变化值，Δ(bg,ba)(i,j)为时刻j与时刻i之间的偏差变化值。
最后，利用优化好的强共视关键帧储存层、位姿信息储存层、视觉感知储存层进行信息交融，对自感知地图进行绘制或对之前的自感知地图进行修正与删减。
下面以结合具体实验对上述构建双重阈值变化视觉惯性里程计的过程进行说明。
图4给出了本发明在视觉惯性联合初始化阶段的偏差变化。本发明初始化算法能实现偏差的快速收敛，构建了情形评估函数实时评估当前偏差的状态，因此精度和收敛性高于其他方法。加速度收敛过程整体上与陀螺仪偏置收敛过程类似，最终趋向定值，仅个别估计值存在误差。设定情形评估函数可以满足待估计参数收敛的要求，能够使得IMU达到快速收敛的目的，在一定程度上有效的抑制了IMU偏差的增大有效地节约了初始化时间，减少了IMU测量值误差，提高了后续阶段的优化精度。
图5给出了本发明滑动窗口优化阶段采用共视分级方法的效果图(采用公开数据集EuRoC数据集)，对于MH_01_easy数据集和MH_03_medium数据集来说，经过滑动窗口优化以后，位置漂移最大值在±100mm左右，对于复杂数据集(如MH_05_difficult)位置误差在±200mm以内。
表1
EuRoC测试中运行时间的比较如表1所示，在测试中，从表中可以得出，在不同的环境下，本发明运行时间跟随效率达到了90％，能够及时计算相机的图像信息并传送给主控机进行实时运动控制，取得较好的跟随精度和效率。
表2
EuRoC数据集下轨迹精度的比较如表2所示。从表中数据分析可得，针对环境的不同，轨迹误差RMSE也会不同，总体误差不高于0.14m。其中，在数据集V1_01_easy序列上的测试轨迹精度误差可达0.027m。
本发明最终的定位效果图如图6所示。
当存在复杂未知环境时，使用基于双重阈值影响的移动机器人视觉惯导融合地图构建方法具有明显的实时性优势，提高定位精度，同时利用共视分级关系减少了运行时间，增强了跟随效率。
实施例二：
与本发明实施例一对应，本发明实施例二提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时依照实施例一的方法实现以下步骤：
步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
步骤S2，构建情形评估函数实时分析姿态趋势。
步骤S3，确定融合状态局部期望值并输出该状态。
步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
上述存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、光盘等各种可以存储程序代码的介质。
上述关于计算机可读存储介质中程序执行后实现步骤的具体限定可以参见实施例一，在此不再做详细说明。
实施例三：
与本发明实施例一对应，本发明实施例三提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化。
步骤S2，构建情形评估函数实时分析姿态趋势。
步骤S3，确定融合状态局部期望值并输出该状态。
步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧。
步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
上述关于计算机设备实现步骤的具体限定可以参见实施例一，在此不再做详细说明。
需要说明的是，本发明的说明书附图中的框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与获得机指令的组合来实现。
上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的发明构思和技术方案进行的各种非实质性的改进，或未经改进将本发明构思和技术方案直接应用于其它场合的，均在本发明保护范围之内。
Claims (10)
1.基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述方法包括：
步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；
步骤S2，构建情形评估函数实时分析姿态趋势；
步骤S3，确定融合状态局部期望值并输出该状态：通过评估函数值与阈值比较判断此时机器人的运动状态观测数据是否具有高确定性，同时增加时间阈值设定双重约束评估状态以确定是否终止视觉惯性联合初始化并输出当前状态；
步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；
步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。
2.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S1中，IMU测量值由真实值、随机游走偏差和白噪声三个部分组成，切加速度计仍受到重力加速度的影响，将IMU运动状态变量利用中值法离散化以方便相机跟IMU进行状态融合，离散化的运动状态变量算式如下：
ppre、vpre和qpre分别代表IMU位置、速度和姿态四元数的初始值；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的加速度；
和
分别代表第i和i+1个时刻下IMU在世界坐标系下的角速度；
为内积的运算法则；
和
表示在第i和i+1个时刻的时间段里IMU加速度偏差和角速度偏差的变化量；
是从IMU坐标系转移到世界坐标系下的旋转矩阵，Δt为相邻时刻的间隔时间；
上述运动状态变量算式能计算出在i+1个时刻下的计算结果为zi+1，z1为初始值，在第一次计算时直接利用相机采集的信息确定，之后的计算利用IMU上一时刻的信息计算得到当前时刻的观测数据。
3.根据权利要求2所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S2中，用ZF表示机器人的运动状态观测数据，则存在ZF＝{z1,z2,...,zn}，zi为ZF中具体的第i个时刻下的观测数据；若机器人的运动状态观测数据ZF＝{z1,z2,...,zn}服从概率分布f(ZF；Y)，其中Y是需要求解的机器人状态变量目标函数，得到的状态估计函数如下式：
构建机器人状态变量目标函数Y来度量机器人观测状态数据并估计观测的位姿随时间变化的趋向的置信度，机器人运动状态置信度表达如下式：
其中Ω(ZF；Y)为机器人运动状态观测数据ZF的置信度，E(·)为计算机器人运动状态观测数据ZF的期望，S(·)为计算机器人运动状态观测数据ZF的标准差，Var(·)为计算机器人运动状态观测数据ZF的方差，通过机器人运动状态置信度确定融合达成度。
4.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S2中，定义视觉惯性观测样本情形评估函数如下式：
其中，fλ(ZF)为机器人运动状态观测数据情形评估函数，Ω(ZF；Y)是从关于评估机器人运动状态观测数据ZF的位置、速度、旋转四元数(尺度、重力矢量)和偏差等相关状态获得的信息矩阵置信度，
为机器人运动状态中某个关键帧F观测数据ZF对应的协方差矩阵，F为运行过程中某个图像帧，Δτ是与图像帧相关的图像信息矩阵。
5.根据权利要求4所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S2中，构建机器人运动状态观测数据ZF情形评估函数的性能度量如下式
其中fdet(ZF)为移动机器人在运动时拍摄的某个包含惯性测量单元状态信息和视觉状态信息的图像帧F的观测数据情形性能度量，Ω(ZF；Y)为机器人运动状态中某个图像帧F状态观测数据置信度，det(·)是指求解矩阵的行列式，
为机器人运动状态中某个图像帧F观测数据ZF对应的协方差矩阵，Δτ为与图像帧有关的图像信息矩阵，log(·)为对数函数。
6.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S3中，定义情形评估函数值小于阈值Sth来衡量初始化状态具有较高的置信度，认为此时的融合状态具有高确定性，终止初始化，将相应的预积分和初始化数据融合，进入下一步的视觉惯性实时优化；同时在初始化过程中额外增加一个时间阈值设定，若时间阈值内情形评估函数未达到较高的置信度，在到达时间阈值时直接终止初始化，将当前初始化结果送入下一阶段实时优化。
7.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S4中，构建基于共视约束的关键帧局部优化概率图模型，按共视强弱对关键帧进行分级处理，将帧间观测信息及关键帧上地图点等信息作为约束项优化强共视关键帧；共视约束概率图模型可以看成一个无向加权图，其联合概率分布即可分解为多个团Q势能函数的乘积，如下式：
其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，
为包含共同观测地图点的关键帧的稀疏权重函数，ψ为正则化因子，通过正则化满足联合概率的有效性；其中Fk＝{fk1,fk2,...,fkn}为关键帧，P(Fk)为联合概率，
为包含共同观测地图点的关键帧的稀疏权重函数。
8.根据权利要求1所述的基于视觉惯导融合的移动机器人地图构建方法，其特征在于，所述步骤S5中，滑动窗口优化时，仅优化强共视关键帧，同时保留弱共视关键帧的位姿信息作为约束项；视觉位姿估计误差项、IMU预积分误差项分量构建一个目标函数中，最小化重投影误差和IMU误差优化状态量，目标函数如下式所示：
其中，Ereproj为两相邻关键帧之间的重投影误差；Eimu为IMU在时间段Δt的误差，
由视觉和IMU共同影响下的偏差测量组成。eobserve为相机观测误差，
为相机关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ep、ev和eq分别为IMU在运行时的位置、速度和姿态四元数的误差，
为IMU关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵，ρ(·)为鲁棒核函数，eb为IMU某个时间点的随机偏差集合，
为相机和IMU共同影响下关于运行状态ZF＝{z1,z2,...,zn}的信息矩阵。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1-8中任一所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：所述处理器执行所述计算机程序时实现如权利要求1-8中任一所述的基于视觉惯导融合的移动机器人地图构建方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110382654.5A CN113091738B (zh) | 2021-04-09 | 2021-04-09 | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110382654.5A CN113091738B (zh) | 2021-04-09 | 2021-04-09 | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113091738A true CN113091738A (zh) | 2021-07-09 |
| CN113091738B CN113091738B (zh) | 2022-02-08 |
Family
ID=76675895
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110382654.5A Active CN113091738B (zh) | 2021-04-09 | 2021-04-09 | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113091738B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN114543786A (zh) * | 2022-03-31 | 2022-05-27 | 华中科技大学 | 一种基于视觉惯性里程计的爬壁机器人定位方法 |
| CN115235454A (zh) * | 2022-09-15 | 2022-10-25 | 中国人民解放军国防科技大学 | 行人运动约束的视觉惯性融合定位与建图方法和装置 |
| CN115574816A (zh) * | 2022-11-24 | 2023-01-06 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
| CN116202516A (zh) * | 2022-12-23 | 2023-06-02 | 中国铁路设计集团有限公司 | 一种轨道bim多维参数辅助imu的轨道三维重建方法 |
| CN116399326A (zh) * | 2023-04-06 | 2023-07-07 | 安徽工程大学 | 一种基于自适应关键帧选取的机器人地图构建方法、存储介质及设备 |
| WO2023165093A1 (zh) * | 2022-03-01 | 2023-09-07 | 上海商汤智能科技有限公司 | 视觉惯性里程计模型的训练方法、位姿估计方法、装置、电子设备、计算机可读存储介质及程序产品 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103026395A (zh) * | 2010-11-15 | 2013-04-03 | 图像传感***有限公司 | 混合交通传感器***和相关的方法 |
| CN105953796A (zh) * | 2016-05-23 | 2016-09-21 | 北京暴风魔镜科技有限公司 | 智能手机单目和imu融合的稳定运动跟踪方法和装置 |
| CN109029448A (zh) * | 2018-06-28 | 2018-12-18 | 东南大学 | 单目视觉惯性定位的imu辅助跟踪模型 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN111862148A (zh) * | 2020-06-05 | 2020-10-30 | 中国人民解放军军事科学院国防科技创新研究院 | 实现视觉跟踪的方法、装置、电子设备及介质 |
| CN111882607A (zh) * | 2020-07-14 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
| CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
-
2021
- 2021-04-09 CN CN202110382654.5A patent/CN113091738B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103026395A (zh) * | 2010-11-15 | 2013-04-03 | 图像传感***有限公司 | 混合交通传感器***和相关的方法 |
| US20130151135A1 (en) * | 2010-11-15 | 2013-06-13 | Image Sensing Systems, Inc. | Hybrid traffic system and associated method |
| CN105953796A (zh) * | 2016-05-23 | 2016-09-21 | 北京暴风魔镜科技有限公司 | 智能手机单目和imu融合的稳定运动跟踪方法和装置 |
| CN109029448A (zh) * | 2018-06-28 | 2018-12-18 | 东南大学 | 单目视觉惯性定位的imu辅助跟踪模型 |
| CN110345944A (zh) * | 2019-05-27 | 2019-10-18 | 浙江工业大学 | 融合视觉特征和imu信息的机器人定位方法 |
| CN111862148A (zh) * | 2020-06-05 | 2020-10-30 | 中国人民解放军军事科学院国防科技创新研究院 | 实现视觉跟踪的方法、装置、电子设备及介质 |
| CN111882607A (zh) * | 2020-07-14 | 2020-11-03 | 中国人民解放军军事科学院国防科技创新研究院 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
| CN112240768A (zh) * | 2020-09-10 | 2021-01-19 | 西安电子科技大学 | 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法 |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN114252073B (zh) * | 2021-11-25 | 2023-09-15 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| WO2023165093A1 (zh) * | 2022-03-01 | 2023-09-07 | 上海商汤智能科技有限公司 | 视觉惯性里程计模型的训练方法、位姿估计方法、装置、电子设备、计算机可读存储介质及程序产品 |
| CN114543786A (zh) * | 2022-03-31 | 2022-05-27 | 华中科技大学 | 一种基于视觉惯性里程计的爬壁机器人定位方法 |
| CN114543786B (zh) * | 2022-03-31 | 2024-02-02 | 华中科技大学 | 一种基于视觉惯性里程计的爬壁机器人定位方法 |
| CN115235454B (zh) * | 2022-09-15 | 2022-12-30 | 中国人民解放军国防科技大学 | 行人运动约束的视觉惯性融合定位与建图方法和装置 |
| CN115235454A (zh) * | 2022-09-15 | 2022-10-25 | 中国人民解放军国防科技大学 | 行人运动约束的视觉惯性融合定位与建图方法和装置 |
| CN115574816B (zh) * | 2022-11-24 | 2023-03-14 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
| CN115574816A (zh) * | 2022-11-24 | 2023-01-06 | 东南大学 | 仿生视觉多源信息智能感知无人平台 |
| CN116202516A (zh) * | 2022-12-23 | 2023-06-02 | 中国铁路设计集团有限公司 | 一种轨道bim多维参数辅助imu的轨道三维重建方法 |
| CN116202516B (zh) * | 2022-12-23 | 2024-02-20 | 中国铁路设计集团有限公司 | 一种轨道bim多维参数辅助imu的轨道三维重建方法 |
| CN116399326A (zh) * | 2023-04-06 | 2023-07-07 | 安徽工程大学 | 一种基于自适应关键帧选取的机器人地图构建方法、存储介质及设备 |
| CN116399326B (zh) * | 2023-04-06 | 2023-10-13 | 安徽工程大学 | 一种基于自适应关键帧选取的机器人地图构建方法、存储介质及设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113091738B (zh) | 2022-02-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113091738B (zh) | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
| Yin et al. | Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments | |
| Campos et al. | Fast and robust initialization for visual-inertial SLAM | |
| CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
| CN110553643B (zh) | 一种基于神经网络的行人自适应零速更新点选取方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN116205947B (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
| CN112004183B (zh) | 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法 | |
| Zhang et al. | Vision-aided localization for ground robots | |
| Wang et al. | Arbitrary spatial trajectory reconstruction based on a single inertial sensor | |
| CN113076988B (zh) | 基于神经网络的移动机器人视觉slam关键帧自适应筛选方法 | |
| CN102830391B (zh) | 一种红外搜索与跟踪***准确性指标计算方法 | |
| US20230304802A1 (en) | Passive combined indoor positioning system and method based on intelligent terminal sensor | |
| CN109682372A (zh) | 一种结合建筑物结构信息与rfid标定的改进型pdr方法 | |
| CN111735478B (zh) | 基于lstm的行人实时导航零速检测方法 | |
| CN114690230A (zh) | 一种基于视觉惯性slam的自动驾驶车辆导航方法 | |
| CN115050095A (zh) | 一种基于高斯过程回归和渐进滤波的人体姿态预测方法 | |
| CN114705223A (zh) | 多移动智能体在目标跟踪中的惯导误差补偿方法及*** | |
| CN113566828A (zh) | 一种基于多传感器决策融合的抗冲击扫描匹配方法及*** | |
| CN110849392A (zh) | 一种机器人的里程计数据校正方法及机器人 | |
| Deng et al. | Data-Driven Based Cascading Orientation and Translation Estimation for Inertial Navigation | |
| CN117576218B (zh) | 一种自适应的视觉惯导里程计输出方法 | |
| Zhao | The Application of Autoencoder In IMU Data Process | |
| CN113899362B (zh) | 一种基于残差网络的带不确定性评估的行人航迹推算方法 |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Guo Junyang Inventor after: Chen Mengyuan Inventor after: Chen Hebao Inventor before: Chen Mengyuan Inventor before: Guo Junyang Inventor before: Chen Hebao |
|
| CB03 | Change of inventor or designer information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |