CN115218906A - 面向室内slam的视觉惯性融合定位方法及系统 - Google Patents
面向室内slam的视觉惯性融合定位方法及系统 Download PDFInfo
- Publication number
- CN115218906A CN115218906A CN202210849157.6A CN202210849157A CN115218906A CN 115218906 A CN115218906 A CN 115218906A CN 202210849157 A CN202210849157 A CN 202210849157A CN 115218906 A CN115218906 A CN 115218906A
- Authority
- CN
- China
- Prior art keywords
- data
- key frame
- image
- optical flow
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 90
- 230000000007 visual effect Effects 0.000 title claims abstract description 85
- 230000004927 fusion Effects 0.000 title claims abstract description 45
- 230000003287 optical effect Effects 0.000 claims abstract description 71
- 238000005259 measurement Methods 0.000 claims abstract description 37
- 230000010354 integration Effects 0.000 claims abstract description 35
- 230000008878 coupling Effects 0.000 claims abstract description 17
- 238000010168 coupling process Methods 0.000 claims abstract description 17
- 238000005859 coupling reaction Methods 0.000 claims abstract description 17
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 238000012545 processing Methods 0.000 claims abstract description 12
- 239000013598 vector Substances 0.000 claims description 39
- 230000008569 process Effects 0.000 claims description 29
- 230000033001 locomotion Effects 0.000 claims description 19
- 230000005484 gravity Effects 0.000 claims description 15
- 230000002441 reversible effect Effects 0.000 claims description 11
- 238000006073 displacement reaction Methods 0.000 claims description 10
- 230000007613 environmental effect Effects 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000000605 extraction Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 238000013473 artificial intelligence Methods 0.000 abstract description 2
- 230000004807 localization Effects 0.000 abstract description 2
- 238000013507 mapping Methods 0.000 abstract 1
- 238000004364 calculation method Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000002452 interceptive effect Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000012216 screening Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000007670 refining Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000013519 translation 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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor 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/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/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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manufacturing & Machinery (AREA)
- Image Analysis (AREA)
Abstract
本发明涉及人工智能技术领域，提供一种面向室内SLAM的视觉惯性融合定位方法及系统，双目鱼眼摄像头获取视频图像数据，通过视频图像数据相邻帧之间的I‑LK光流法特征追踪，IMU采集位置和姿态数据，进行预积分处理；将多源数据进行松耦合后融合确定系统初始化参数值，由关键帧策略确定关键帧数据；关键帧数据通过视觉位姿初始化和IMU预积分对齐；外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿，已知当前帧相机的位置，通过IMU信息可以预测场景的空间特征点在下一帧图像的位置，从而提高特征点跟踪的稳定性，能够在实现移动机器人定位精度的前提下，提升系统鲁棒性。
Description
技术领域
本发明涉及人工智能技术领域，特别涉及一种面向室内SLAM的视觉惯性融合定位方法及系统。
背景技术
SLAM技术(SimultaneousLocalization AndMapping，SLAM)，即为即时定位与地图构建技术在室内移动机器人执行任务过程中发挥着举重若轻的作用，如移动机器人通过传感器识别障碍物的相对位置和方向，防止本体与物体接触碰撞；通过感知且估计本体所在空间位姿信息，以保证移动机器人构建的地图与场景环境的全局一致性。因此研究SLAM技术能够促进移动机器人在室内环境下的空间信息感知与定位的功能，并为移动机器人自主导航提供先验信息。
CN111429574B公开了基于三维点云和视觉融合的移动机器人定位方法和系统，通过建立环境地图、点云与视觉特征融合、匹配地图并进行定位。其中，环境地图即建立环境的特征栅格地图，特征栅格地图中每个栅格存储从点云中提取的特征点和视觉图像的特征点组成的点集，并提取了高度值、强度值、法向量投影值；点云与视觉特征融合是将图像提取的特征点投影到点云空间，与点云特征组成联合特征点；匹配地图并进行定位是把联合特征点投影到二维栅格，并提取得到特征向量，将特征栅格与地图进行匹配，采用直方图滤波器，确定每个候选位姿的后验概率，基于各后验概率确定机器人在地图中的位置。
现有技术中，依靠视觉传感器进行SLAM定位存在一定局限性，在弱光照的环境、纹理缺失的场景以及载体的快速运动中容易丢失环境特征信息；同时场景动态物体的干扰和遮挡容易造成特征数据的错误关联。
发明内容
通过长期的研究实践，在传感器空间定位原理中，视觉与IMU融合定位具有数据优势互补的特性。IMU不受外界场景物体、纹理和光照信息影响，能够在短时间内观测出较高精度的相对位移数据，视觉系统融合IMU数据后能够增强视觉传感器在动态物体、弱纹理和阴暗场景的适应性，并且提升相机快速运动时系统的稳健性；IMU能够测量出高频的定位姿态数据，视觉系统融合后可在视觉图像帧间约束相机位姿；但IMU长时间运行受噪声与偏置的影响存在累积误差，此时视觉定位信息可以抑制其轨迹漂移。
有鉴于此，本发明旨在提出一种面向室内SLAM的视觉惯性融合定位方法，该面向室内SLAM的视觉惯性融合定位方法包括，
步骤S1，通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
步骤S2，由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
步骤S3，将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
步骤S4，将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。
优选地，在步骤S3中，关键帧策略包括，
步骤S31，由双目鱼眼摄像头获得情景特征信息后进行初始化，将稳定获取的第一帧图像设置为参考关键帧；
步骤S32，根据图像的环境特征变化确定上一个关键帧和当前帧平均视差，当平均视差D小于设定阈值ε，则输出非关键帧；当平均视差D大于或等于设定阈值ε，则输出预选关键帧；
步骤S33，当预选关键帧跟踪的特征数量低于设置阈值δ，将当前预选关键帧作为关键帧。
优选地，在步骤S2中，预积分处理包括，
步骤S21，将惯性测量单元状态量的积分项拆分为第i时刻的固定积分项和第i至j时刻的预积分相对量；
步骤S22，离散时间下惯性测量单元测量数据由预积分模型转换后，分为固定积分项和预积分相对量。
优选地，步骤S1中，I-LK光流法特征追踪包括，
步骤S11，在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
步骤S12，将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
步骤S13，将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
优选地，在步骤S4中，外参在线标定包括双目鱼眼摄像头与惯性测量单元相对位置和姿态参数标定；双目鱼眼摄像头与惯性测量单元之间时间差参数标定。
优选地，视觉位姿初始化包括经过特征提取、I-LK追踪匹配和图像关键帧挑选后，采用视觉里程计通过光束平差法估计相机位姿和路标点，相机获取到轨迹位姿信息。
优选地，视觉惯性初始化包括陀螺仪偏置求解过程、重力矢量估计过程、重力方向细化过程、加速度计偏置过程和速度解算过程。
本发明还公开了一种用于实施上述的面向室内SLAM的视觉惯性融合定位方法的系统，所述系统包括，
获取单元，包括多个双目鱼眼摄像头，用于通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
预积分单元，用于由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
数据融合单元，用于将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
定位单元，用于将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建六自由度位姿。
优选地，所述系统包括，所述获取单元还包括，用于I-LK光流法特征追踪的装置，所述装置包括，
构图模块，用于在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
分量生成模块，用于将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
光流矢量模块，用于将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
相对于现有技术，本发明提供的面向室内SLAM的视觉惯性融合定位方法，通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，由惯性测量单元采集位置和姿态数据，进行预积分处理；再将多源数据进行松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐；并通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。该方法和系统在视觉惯性融合定位在移动机器人快速运动时，通过惯性测量单元(Inertial MeasurementUnit，IMU)短时间观测模型降低图像匹配的搜索范围，约束视觉的轨迹漂移；已知当前帧相机的位置，通过IMU信息可以预测场景的空间特征点在下一帧图像的位置，从而提高特征点跟踪的稳定性；在弱纹理或者特征点较少的场景中，视觉图像帧信息丢失，IMU通过积分形式递推本体的相对位姿信息，进而恢复移动机器人的位姿，增强系统的鲁棒性；而IMU长时间航迹位姿推算，造成测量数据存在累积误差，此时视觉图像帧位姿能够约束并抑制其航迹的漂移；因此视觉惯性融合定位方法能够在实现移动机器人定位精度的前提下，提升系统鲁棒性。
本发明的其它特征和优点将在随后的具体实施方式部分予以详细说明。
附图说明
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施方式及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1为本发明的面向室内SLAM的视觉惯性融合定位方法一种实施方式的流程图；
图2为本发明的面向室内SLAM的视觉惯性融合定位方法流程示意图；
图3为本发明的面向室内SLAM的视觉惯性融合定位方法关键帧计算流程图；
图4为本发明的面向室内SLAM的视觉惯性融合定位方法关键帧示意图；
图5为本发明的面向室内SLAM的视觉惯性融合定位方法逆向光流方法示意图。
具体实施方式
以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为了解决现有技术中依靠视觉传感器进行SLAM定位存在一定局限性，在弱光照的环境、纹理缺失的场景以及载体的快速运动中容易丢失环境特征信息，同时场景动态物体的干扰和遮挡容易造成特征数据的错误关联等一系列问题。本发明提供一种面向室内SLAM的视觉惯性融合定位方法，如图1-2所示，面向室内SLAM的视觉惯性融合定位方法包括，
步骤S1，通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
步骤S2，由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
步骤S3，将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
步骤S4，将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。
本发明通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，由惯性测量单元采集位置和姿态数据，进行预积分处理；再将多源数据进行松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐；并通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。该方法在视觉惯性融合定位在移动机器人快速运动时，通过IMU短时间观测模型降低图像匹配的搜索范围，约束视觉的轨迹漂移；已知当前帧相机的位置，通过IMU信息可以预测场景的空间特征点在下一帧图像的位置，从而提高特征点跟踪的稳定性；在弱纹理或者特征点较少的场景中，视觉图像帧信息丢失，IMU通过积分形式递推本体的相对位姿信息，进而恢复移动机器人的位姿，增强系统的鲁棒性；而IMU长时间航迹位姿推算，造成测量数据存在累积误差，此时视觉图像帧位姿能够约束并抑制其航迹的漂移；因此视觉惯性融合定位方法能够在实现移动机器人定位精度的前提下，提升系统鲁棒性。
移动机器人搭载相机以固定帧率拍摄场景图片，在运动的过程中系统获取大量的相似性和重复率的图像，这些冗余的图像占用较多的运算资源，但对提升系统的定位精度却十分有限。在本发明优选的情况下，在步骤S3中，关键帧策略包括，
步骤S31，由双目鱼眼摄像头获得情景特征信息后进行初始化，将稳定获取的第一帧图像设置为参考关键帧；
步骤S32，根据图像的环境特征变化确定上一个关键帧和当前帧平均视差，当平均视差D小于设定阈值ε，则输出非关键帧；当平均视差D大于或等于设定阈值ε，则输出预选关键帧；
步骤S33，当预选关键帧跟踪的特征数量低于设置阈值δ，将当前预选关键帧作为关键帧。
例如，在连续图像序列中选择具有特征内容丰富和运动代表性的场景图像成为关键帧，提升系统运算效率。为确保图像序列挑选出的关键帧符合场景信息描述内容和满足系统数据处理的实时性。如图3-4所示，采取关键帧筛选策略：双目鱼眼摄像头获得情景特征信息后进行初始化，将稳定获取的第一帧图像设置为参考关键帧，之后对当前图像帧进行判断；首先根据图像的环境特征变化确定上一个关键帧和当前帧平均视差，然后确保图像帧间的相似性进行跟踪质量判断，符合要求系统则保存关键帧的位置信息，否进入下一帧图像帧筛选。移动机器人根据当前时刻的环境信息进行特征的提取，选择情景丰富具有代表性的关键帧图像。I-LK追踪FAST特征角点环境感知时，在特征管理器中储存着相邻帧间的光流追踪特征点的数目和总视差。
在IMU的整体姿态估计中，每次估计IMU在j时刻的状态(Pj,Vj,Rj)会随着i时刻的状态(Pi,Vi,Ri)发生改变而重新计算，导致占用系统大量计算资源。IMU在位姿求解过程中以四元数描述的旋转矩阵R，在连续时间j中可以表示为如下运动学关系：
将积分模型转化为预积分模型方式，将IMU状态量的积分项拆分为两部分，分别为第i时刻的固定积分项和第i至j时刻的预积分相对量。如此IMU当前时刻的积分量仅需要固定时刻数据和两者时刻之间的预积分相对量即可更新位姿预测数据。
在离散时间下IMU测量数据由预积分模型转换后，分为固定积分项和预积分相对量，无论在第i至j时刻时间内有多段离散积分数据，此时只需积分运算一次，大幅度降低系统数据的冗余解耦。在本发明优选的情况下，在步骤S2中，预积分处理包括，
步骤S21，将惯性测量单元状态量的积分项拆分为第i时刻的固定积分项和第i至j时刻的预积分相对量；
步骤S22，离散时间下惯性测量单元测量数据由预积分模型转换后，分为固定积分项和预积分相对量。
光流追踪特征点进行匹配是建立相机图像帧间的对应关系，进而恢复相机位姿。LK光流在基于亮度不变和运动微小的普通光流的基础上，图像领域内的光流具有一致性，即图像局部领域内所有像素点具有相同的运动。
本发明增加多层图像迭代金字塔跟踪匹配特征点，并采取逆向光流剔除误匹配或大位移的特征外点，称为I-LK(Improve LK，I-LK)光流法。原始图像上采样后形成多层图像金字塔，将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解。求解的光流矢量为金字塔图像的各分量累加和，最终逆光流反向追踪剔除误匹配，提高在不同环境中图像特征提取能力和特征匹配的实时性、鲁棒性。如图5所示，在本发明优选的情况下，步骤S1中，I-LK光流法特征追踪包括，
步骤S11，在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
步骤S12，将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
步骤S13，将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
为了计算金字塔图像矢量分量，本发明更为优选的情况下，例如，金字塔光流跟踪流程：首先，从顶层开始计算图像的光流，根据上层光流的计算结果估计下层光流的初始值，初始值计算出后再优化光流得到精确值。自上而下从最顶层到原始图像层，最后计算出底层的原始图像的光流。
对于大多数VI-SLAM数据融合时，视觉与惯性的外部参数通常在执行任务前进行重复的联合标定以获取其精准的数值，但离线标定过程繁琐耗时。因此采用一种在线标定视觉与惯性的旋转外参的方法。
针对视觉与惯性之间的外部参数为
表示视觉到IMU的刚体变换矩阵，相对平移量
在初始化时影响有限，取视觉惯性校准值，而相对旋转量
是视觉与IMU空间位姿变化中的重要因素，在移动机器人运动中进行在线优化，在本发明优选的情况下，在步骤S4中，外参在线标定包括双目鱼眼摄像头与惯性测量单元相对位置和姿态参数标定；双目鱼眼摄像头与惯性测量单元之间时间差参数标定。
采用视觉里程计在经过特征提取、I-LK追踪匹配和图像关键帧挑选后，并通过光束平差法估计相机位姿和路标点，相机获取到良好轨迹位姿信息，在定位精度分析后，视觉里程计初始化后取得较精确的初始值位姿，在本发明优选的情况下，视觉位姿初始化包括经过特征提取、I-LK追踪匹配和图像关键帧挑选后，采用视觉里程计通过光束平差法估计相机位姿和路标点，相机获取到轨迹位姿信息。
系统初始化准确的参数值影响视觉惯性数据融合定位的性能，而参数值的不确定性直接造成视觉惯性系统的运动漂移以及降低轨迹定位的精度。针对初始化参数的精准求解，在滑动窗口内采用视觉与惯性数据松耦合的方式分步迭代优化求解参数值，为视觉惯性系统提供准确的初始化参数，从而推动场景移动机器人迅速进入工作状态。
视觉惯性系统初始化时移动机器人是否达到稳健的位姿状态量，可以经由滑动窗口局部优化的IMU偏置变量收敛来判断系统初始化和视觉惯性数据融合的成功与否。通过对IMU偏置的三轴数据进行解析，验证初始化的时效性和场景各异情况下系统的鲁棒性。在本发明优选的情况下，视觉惯性初始化包括陀螺仪偏置求解过程、重力矢量估计过程、重力方向细化过程、加速度计偏置过程和速度解算过程。
陀螺仪偏置求解过程中，陀螺仪的误差来源为两部分：环境测量噪声和陀螺仪的随机游走的偏置。在短时间内系统初始化时，视觉和IMU的测量噪声视为微小(置零)。根据系统旋转量的一致性，相邻帧间陀螺仪旋转量预积分值与视觉旋转量观测值相等，由于外界与IMU本身的干扰因素，定义两者的差值为陀螺仪偏置bg。对整个滑动窗口的相邻帧间的旋转量误差函数构建最小二乘优化模型校准陀螺仪偏置。
在窗口相邻帧间第k、k+1帧，ΔRk,k+1为IMU预积分旋转量，
为IMU预积分旋转量
关于bg求导的雅可比矩阵。
与
分别为第k、k+1关键帧的世界坐标系下和IMU坐标系下的旋转量。在陀螺仪偏置求解出后，代入IMU预积分更新观测模型。
重力矢量估计过程采用加速度计偏置与重力在方向上一致，在同时刻被观测时，由于加速度数值在量级上与重力数值相比过于微小，初始化时暂时采用IMU校准的加速度计偏置的初始值，由于初始化时系统工作时间极短，噪声干扰微小。
在初始化阶段中，忽略微小的噪声干扰因素，在偏置已经解算出的条件下，根据速度预积分计算出移动机器人的本体速度。计算出的速度为时间戳上IMU对应关键帧的速度，在移动机器人快速运动时，通过预积分模型降低图像在空间匹配的搜索范围。
重力方向细化过程、加速度计偏置过程和速度解算过程中，重力矢量在IMU中被大致估计出后，重力大小取当地重力值G，再对方向进一步细化。而且加速度计偏置在数值上较小，采用重力G和惯性坐标系I，避免与B系旋转量表述重合下的重力方向协助估计。
随着移动机器人在场景的不断探索和长时间的执行工作任务，视觉惯性系统获取关键帧位姿和环境路标点组成的状态估计量的规模剧增，系统的实时性与鲁棒性受到约束。针对视觉惯性融合定位的状态优化变量规模和累积误差。在衡量视觉惯性系统定位精度、计算效率和鲁棒性后，对此建立基于限定优化变量领域的滑动窗口，通过边缘化旧关键帧状态信息和插入新关键帧信息，在窗口内进行局部最优位姿更新解算。
其中，滑动窗口：在窗口内固定关键帧数量，在加入一个新的关键帧以及观测到的路标点时，对一个旧的关键帧进行“删除”，以此达到约束窗口图像关键帧的数量，从而限制位姿和路标点的状态参数变量，进而控制滑动窗口内状态量优化数据规模。
其中滑动窗口关键帧的插入条件，在滑动窗口外的最新图像帧为关键帧时，边缘化最旧关键帧观测到状态数据，但移除状态量的约束关系作为先验信息保留在信息矩阵中，以避免与窗口中关键帧的位姿和路标点信息的丢失。如果滑动窗口外最新图像帧为普通图像帧，保存IMU测量的预积分集成数据进而传递至下一视觉图像帧，普通图像帧观测数据丢弃，直至下一帧图像帧为关键帧。
随着移动机器人在场景执行任务时，系统连续对滑动窗口插入视觉和IMU的观测数据，对在窗口内所有的视觉关键帧图像和惯性预积分以紧耦合的方式非线性优化求解，避免局部相邻帧间的误差产生累积并传递至下一时刻的状态量，实现在滑动窗口内的精确定位。
为了更好地执行上述一种面向室内SLAM的视觉惯性融合定位方法的系统，所述系统包括，
获取单元，包括多个双目鱼眼摄像头，用于通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
预积分单元，用于由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
数据融合单元，用于将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
定位单元，用于将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。
本发明通过获取单元中多个双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，由惯性测量单元采集位置和姿态数据，预积分单元中进行预积分处理；数据融合单元将多源数据进行松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐；并通过定位单元中进行外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。该系统在视觉惯性融合定位在移动机器人快速运动时，通过IMU短时间观测模型降低图像匹配的搜索范围，约束视觉的轨迹漂移；已知当前帧相机的位置，通过IMU信息可以预测场景的空间特征点在下一帧图像的位置，从而提高特征点跟踪的稳定性；在弱纹理或者特征点较少的场景中，视觉图像帧信息丢失，IMU通过积分形式递推本体的相对位姿信息，进而恢复移动机器人的位姿，增强系统的鲁棒性；而IMU长时间航迹位姿推算，造成测量数据存在累积误差，此时视觉图像帧位姿能够约束并抑制其航迹的漂移；因此视觉惯性融合定位方法能够在实现移动机器人定位精度的前提下，提升系统鲁棒性。
原始图像上采样后形成多层图像金字塔，将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解。求解的光流矢量为金字塔图像的各分量累加和，最终逆光流反向追踪剔除误匹配，提高在不同环境中图像特征提取能力和特征匹配的实时性、鲁棒性，在本发明优选的情况下，所述系统包括，所述获取单元还包括，用于I-LK光流法特征追踪的装置，所述装置包括，
构图模块，用于在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
分量生成模块，用于将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
光流矢量模块，用于将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
本发明实施例还提供了一种存储介质，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行上述方法。
需要说明的是，对于前述的各方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定是本发明所必须的。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。
在本发明所提供的几个实施例中，应该理解到，所揭露的装置，可通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、移动终端、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种面向室内SLAM的视觉惯性融合定位方法，其特征在于，所述面向室内SLAM的视觉惯性融合定位方法包括，
步骤S1，通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
步骤S2，由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
步骤S3，将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
步骤S4，将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。
2.根据权利要求1所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，在步骤S3中，关键帧策略包括，
步骤S31，由双目鱼眼摄像头获得情景特征信息后进行初始化，将稳定获取的第一帧图像设置为参考关键帧；
步骤S32，根据图像的环境特征变化确定上一个关键帧和当前帧平均视差，当平均视差D小于设定阈值ε，则输出非关键帧；当平均视差D大于或等于设定阈值ε，则输出预选关键帧；
步骤S33，当预选关键帧跟踪的特征数量低于设置阈值δ，将当前预选关键帧作为关键帧。
3.根据权利要求1所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，在步骤S2中，预积分处理包括，
步骤S21，将惯性测量单元状态量的积分项拆分为第i时刻的固定积分项和第i至j时刻的预积分相对量；
步骤S22，离散时间下惯性测量单元测量数据由预积分模型转换后，分为固定积分项和预积分相对量。
4.根据权利要求1所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，步骤S1中，I-LK光流法特征追踪包括，
步骤S11，在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
步骤S12，将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
步骤S13，将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
5.根据权利要求1所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，在步骤S4中，外参在线标定包括双目鱼眼摄像头与惯性测量单元相对位置和姿态参数标定；双目鱼眼摄像头与惯性测量单元之间时间差参数标定。
6.根据权利要求1所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，视觉位姿初始化包括经过特征提取、I-LK追踪匹配和图像关键帧挑选后，采用视觉里程计通过光束平差法估计相机位姿和路标点，相机获取到轨迹位姿信息。
7.根据权利要求1-6任意一项所述的面向室内SLAM的视觉惯性融合定位方法，其特征在于，视觉惯性初始化包括陀螺仪偏置求解过程、重力矢量估计过程、重力方向细化过程、加速度计偏置过程和速度解算过程。
8.一种用于实施权利要求1-7任意一项所述的面向室内SLAM的视觉惯性融合定位方法的系统，其特征在于，所述系统包括，
获取单元，包括多个双目鱼眼摄像头，用于通过双目鱼眼摄像头获取视频图像数据，并通过视频图像数据相邻帧之间的I-LK光流法特征追踪，得到第一数据；
预积分单元，用于由惯性测量单元采集位置和姿态数据，进行预积分处理后得到第二数据；
数据融合单元，用于将所述第一数据和所述第二数据松耦合后交互融合确定系统初始化参数值，并由关键帧策略确定关键帧数据；其中，关键帧数据通过视觉位姿初始化和惯性测量单元预积分对齐后输出为第三数据；
定位单元，用于将所述第三数据通过外参在线标定、视觉惯性初始化和紧耦合非线性优化，由基于关键帧的滑动窗口构建移动机器人六自由度位姿。
9.根据权利要求8所述的系统，其特征在于，所述系统包括，所述获取单元还包括，用于I-LK光流法特征追踪的装置，所述装置包括，
构图模块，用于在获取视频图像数据的原始图像上采样后形成多层金字塔图像；
分量生成模块，用于将大位移光流矢量自上而下分解成在各层图像上符合微小运动的光流，且在该图像层领域内进行光流迭代求解得到金字塔图像矢量分量；
光流矢量模块，用于将金字塔图像各矢量分量累加得到光流矢量，并在逆光流反向追踪剔除误匹配。
10.一种存储介质，其特征在于，所述存储介质包括存储的程序，其中，在所述程序运行时控制所述存储介质所在设备执行权利要求1-7任意一项所述的面向室内SLAM的视觉惯性融合定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210849157.6A CN115218906A (zh) | 2022-07-19 | 2022-07-19 | 面向室内slam的视觉惯性融合定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210849157.6A CN115218906A (zh) | 2022-07-19 | 2022-07-19 | 面向室内slam的视觉惯性融合定位方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115218906A true CN115218906A (zh) | 2022-10-21 |
Family
ID=83612166
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210849157.6A Pending CN115218906A (zh) | 2022-07-19 | 2022-07-19 | 面向室内slam的视觉惯性融合定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115218906A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116442248A (zh) * | 2023-06-19 | 2023-07-18 | 山东工程职业技术大学 | 一种基于目标检测的机器人视觉定位模块及方法 |
| CN116758161A (zh) * | 2023-06-26 | 2023-09-15 | 北京道仪数慧科技有限公司 | 移动端空间数据生成方法及空间感知移动端 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107869989A (zh) * | 2017-11-06 | 2018-04-03 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
-
2022
- 2022-07-19 CN CN202210849157.6A patent/CN115218906A/zh active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107869989A (zh) * | 2017-11-06 | 2018-04-03 | 东北大学 | 一种基于视觉惯导信息融合的定位方法及系统 |
Non-Patent Citations (1)
| Title |
|---|
| 刘大鹏: "视觉惯性融合定位的室内SLAM研究", 南方农机, 31 March 2022 (2022-03-31), pages 22 - 25 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116442248A (zh) * | 2023-06-19 | 2023-07-18 | 山东工程职业技术大学 | 一种基于目标检测的机器人视觉定位模块及方法 |
| CN116758161A (zh) * | 2023-06-26 | 2023-09-15 | 北京道仪数慧科技有限公司 | 移动端空间数据生成方法及空间感知移动端 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110125928B (zh) | 一种基于前后帧进行特征匹配的双目惯导slam系统 | |
| CN111258313B (zh) | 多传感器融合slam系统及机器人 | |
| CN109974693B (zh) | 无人机定位方法、装置、计算机设备及存储介质 | |
| CN107255476B (zh) | 一种基于惯性数据和视觉特征的室内定位方法和装置 | |
| CN110298914B (zh) | 一种建立果园中果树冠层特征地图的方法 | |
| US9071829B2 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| CN112525202A (zh) | 一种基于多传感器融合的slam定位导航方法及系统 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及系统 | |
| CN110553648A (zh) | 一种用于室内导航的方法和系统 | |
| CN110533719B (zh) | 基于环境视觉特征点识别技术的增强现实定位方法及装置 | |
| EP3786891A1 (en) | Method and system for visual localization based on dual dome cameras | |
| CN113674412B (zh) | 基于位姿融合优化的室内地图构建方法、系统及存储介质 | |
| CN110749308B (zh) | 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法 | |
| CN114964276B (zh) | 一种融合惯导的动态视觉slam方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN114494629A (zh) | 一种三维地图的构建方法、装置、设备及存储介质 | |
| CN113551665A (zh) | 一种用于运动载体的高动态运动状态感知系统及感知方法 | |
| CN111932637B (zh) | 一种车身相机外参自适应标定方法和装置 | |
| CN115355901A (zh) | 一种融合动态目标感知的多机联合建图方法 | |
| CN112580683A (zh) | 一种基于互相关的多传感器数据时间对齐系统及其方法 | |
| CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN116045965A (zh) | 一种融合多传感器的环境地图构建方法 | |
| CN115344033B (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 | |
| CN113379911A (zh) | Slam方法、slam系统及智能机器人 |
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 |