CN116380070A - 一种基于时间戳优化的视觉惯性定位方法 - Google Patents

一种基于时间戳优化的视觉惯性定位方法 Download PDF

Info

Publication number
CN116380070A
CN116380070A CN202310250012.9A CN202310250012A CN116380070A CN 116380070 A CN116380070 A CN 116380070A CN 202310250012 A CN202310250012 A CN 202310250012A CN 116380070 A CN116380070 A CN 116380070A
Authority
CN
China
Prior art keywords
frame
loop
frames
optimization
imu
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
CN202310250012.9A
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.)
Shenzhen Institute of Advanced Technology of CAS
Original Assignee
Shenzhen Institute of Advanced Technology of CAS
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 Shenzhen Institute of Advanced Technology of CAS filed Critical Shenzhen Institute of Advanced Technology of CAS
Priority to CN202310250012.9A priority Critical patent/CN116380070A/zh
Publication of CN116380070A publication Critical patent/CN116380070A/zh
Pending legal-status Critical Current

Links

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/18—Stabilised platforms, e.g. by gyroscope
    • 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/383—Indoor data
    • 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/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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

Abstract

本发明公开一种基于时间戳优化的视觉惯性定位方法。该方法包括：针对待测场景，利用相机获取图像数据，利用惯性测量单元获取IMU数据，并对图像数据进行点特征提取与线特征提取，进而恢复特征点的逆深度以及图像帧的初始位姿信息；利用图像数据和IMU数据，并采用滑动窗口非线性策略，以对多个状态量进行估计，其中，所述多个状态量包含IMU数据与图像数据的时间偏移量，进而利用所述时间偏移量补偿IMU数据与图像数据；在每一次优化后，判断关键帧，并基于关键帧与关键帧数据库的比对寻找候选回环帧，进而对候选回环帧进行特征点匹配，识别出正确的回环帧，以利用回环帧建立后端优化过程中的回环约束。本发明提高了全局地图位姿估计精度和鲁棒性。

Description

一种基于时间戳优化的视觉惯性定位方法
技术领域
本发明涉及机器人技术领域，更具体地，涉及一种基于时间戳优化的视觉惯性定位方法。
背景技术
在室内移动机器人的自主定位作业中，需要通过传感器获取周围环境的信息，利用获取到的信息进行定位。然而在实际场景中，利用一种单一的传感器很难得到较为精确的位置信息，因此基于多传感器的移动机器人定位系统，可以提高其自身定位的鲁棒性，进而获取更加精准的位置信息。如何在位姿估计中充分利用不同传感器的数据越来越得到重视，在多源信息融合方面有着多样的研究，近年来有一类方法通过增添三维立体环境中的场景约束来提高物体在运动过程中位姿估计的精度，然而该类方案还存在传感器数据之间存在时间戳偏移的问题。
基于多传感器的SLAM(即时定位与地图构建)已发展多年，按照传感器数据处理方式可分为松耦合与紧耦合。在位姿估计中将不同传感器数据作为独立模块的结构称为松耦合，通常使用扩展卡尔曼滤波器(EKF)完成数据处理；而基于紧耦合的解决方案不仅有使用基于滤波的方法，还有基于优化的方法，这两类方法都是将多个传感器数据作为一个模块同时处理。
视觉SLAM在机器人的自主导航、无人车自动驾驶和虚拟现实等应用中对于位姿估计起着重要作用。实现位姿估计方案中常以单目相机和IMU(Inertial Measurement Unit，惯性测量单元)结合的方式组成一个最小的位姿估计平台，且这类解决方案已经有了较好的效果，通常这种方案主要利用图像中场景的特征信息对位姿进行估计，同时利用IMU获取物体在运动状态中的加速度与角速度等信息，进而增加相机运动过程中位姿估计的约束信息。
近年来，PL-VIO、PL-VINS等方法通过增添三维立体环境中的场景约束来提高物体在运动过程中位姿估计的精度，但是该类方法都没有有效地解决传感器之间时间同步的问题。对于校正传感器数据之间测量时间戳与实际采样时间戳之间差异的问题，Mair等人提出了一种时空校准的初始化方法，它将时间校准与相对位姿校准分离，单独校准时间但没有考虑空间关系。Kelly等人通过对齐相机和IMU的轨迹的方式校准时间偏移，通过类似于ICP的方式逐渐匹配两条轨迹，求得时间偏移。
经分析，在多传感器数据融合的方案中，由于传感器硬件本身的原因，数据从传感器获取的过程中会因为触发延迟和传输延迟产生时间延迟，不同传感器的时间延迟往往不同，因而系统获取到的多个数据时间戳与实际采样数据的时间戳存在误差，由于这类误差往往很小，所以很多系统默认地选择忽略了该误差。但是在移动机器人的姿态估计中，尤其是在一些由低成本传感器组成的平台中，非同类数据的时间戳信息可能存在较大的偏差，如果对于这种误差处理不当，位姿估计的精度将受到影响。而目前在增添线特征约束的位姿估计方案中，仍存在传感器数据之间的时间戳偏移的问题。
发明内容
本发明的目的是克服上述现有技术的缺陷，提供一种基于时间戳优化的视觉惯性定位方法。该方法包括：
状态预处理步骤：针对待测场景，利用相机获取图像数据，利用惯性测量单元获取IMU数据，并对图像数据进行点特征提取与线特征提取，进而恢复特征点的逆深度以及图像帧的初始位姿信息；
后端优化步骤：利用图像数据和IMU数据，并采用滑动窗口非线性策略，以对多个状态量进行估计，其中，所述多个状态量包括惯性测量单元的状态信息、特征点的逆深度、特征线的表示以及IMU数据与图像数据的时间偏移量，进而利用所述时间偏移量补偿IMU数据与图像数据；
回环检测步骤：在每一次后端优化结束后，判断关键帧，并基于关键帧与关键帧数据库的比对寻找候选回环帧，进而对候选回环帧进行特征点匹配，识别出正确的回环帧，以利用该回环帧建立后端优化过程中的回环约束。
与现有技术相比，本发明的优点在于，通过对IMU和图像数据之间的时间偏移量进行动态估计，并将估计的时间差补偿到前端，使得IMU与图像数据时间戳对齐，并结合线特征信息提高结构化场景中的约束，进而提升位姿估计的精度。本发明提出的基于时间戳优化的视觉惯性定位方法，在特征提取阶段通过改进的视觉重投影模型有效的估计IMU与相机的时间偏移量，提升了多模态数据融合性能，同时在后端非线性优化中引入线特征约束将有效地提升全局位姿的精度。
通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。
附图说明
被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。
图1是根据本发明一个实施例的基于时间戳优化的视觉惯性定位方法的流程图；
图2是根据本发明一个实施例的基于时间戳优化的视觉惯性定位方法的过程示意图。
具体实施方式
现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。
以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。
在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。
应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。
为了提高IMU与图像之间的数据融合的性能，同时提高位姿估计的精度，本发明以IMU和单目相机为信息接收的传感器，通过提取周围环境的特征信息，同时结合IMU数据，通过预积分的方式更新位姿，并通过回环检测的方式进行重定位，从而对全局的位姿进行优化。
IMU是测量物体三轴姿态角(或角速率)以及加速度的装置。通常IMU包含三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
结合图1和图2所示，所提供的基于时间戳优化的视觉惯性定位方法包括如下步骤：
步骤S1，采集图像数据和IMU数据，并进行状态预处理，获得各传感器的初始状态数据。
例如，首先针对待测场景，利用相机获取图像数据，利用惯性测量单元获取IMU数据。
步骤S1用于对系统状态进行预处理，该步骤为后续非线性优化提供一个较好的初始值，总体上包括：对图像进行均衡化处理，然后提取图像中的点特征和线特征；对提取到的点特征和线特征进行特征追踪并判断关键帧；由于IMU数据和图像帧可以各自求得一个变换的位姿关系，通过假设一个空间变换外参，多组数据可以组成超定方程，基于此计算出IMU与相机之间的空间外参；通过单目SfM恢复特征点逆深度，和图像帧的初始位姿及尺度信息。
在一个实施例中，状态预处理阶段依次包含IMU预处理模块、特征点检测与跟踪模块、线特征提取与跟踪模块。
在IMU预处理模块中，通过IMU预积分模型只对相邻两相机帧之间的IMU数据进行积分求得当前相机帧与上一帧的相对位姿。
在特征点检测与跟踪模块中，通过Shi-Tomasi算子从图像中提取特征点，采用KLT光流法对这些特征点在下一帧上寻找匹配点进行跟踪，然后对前后匹配的点进行校正，在校正后每对匹配点用本征矩阵和RANSAC算法进行筛选。
在线特征提取与跟踪模块中，通过使用LSD(最小显著性差异法)算法提取图像中的线段，检测分别采用LBD描述子和KNN(最近邻)算法进行线特征的描述和匹配。
在获取特征信息跟踪成果后，对初始环境进行三角化，例如通过稀疏SfM(Structure from motion，运动重建)方法恢复特征点的逆深度，并恢复环境中的图像帧的初始位姿信息。
步骤S2，采用非线性优化对所获得的初始状态数据进行后端优化，并将图像数据与IMU数据的时间偏移量估计结果补偿到步骤S1的传感器状态值，以持续优化。
步骤S2是后端优化阶段，可采用非线性滑动窗口策略进行优化。滑动窗口策略在保证优化多帧数据的同时也保证了实时性，同时采用紧耦合的方式对多个残差信息的状态量进行估计，以获取高精度的位姿估计信息。例如分别从先验信息、IMU残差、带有时间估计的点特征重投影误差、线特征残差和回环约束信息等五个方面对状态量进行优化，待优化的变量信息包括IMU的状态信息、特征点的逆深度、特征线的表示以及IMU与图像的时间偏移量，同时将获取到的IMU与图像数据之间的时间偏移量补偿到传感器的外参，进而提高传感器数据融合的性能。
对于步骤S2，获取系统初始状态数据后，进入滑窗优化阶段，基于非线性优化策略通过对多个残差进行约束，使得整个系统的残差量最小，参见公式(4)，待优化的变量信息参见公式(3)，包括IMU的状态信息、特征点的逆深度、特征线的表示以及IMU与图像的时间偏移量td。在获取到时间偏移量td后，将其补偿到IMU与相机之间，进而提高传感器数据融合的性能。
在一个实施例中，步骤S2具体包括：
步骤S2.1，通过引入特征点速度来建模时间偏差(或称时间偏移量)，由于在两个连续图像帧Ik与Ik+1之间的运动时间极小，将相机运动视为匀速运动，假设在tk至tk+1时间段内相机从Ck移动到Ck+1，对应的归一化平面中同一特征点的坐标由(uk，vk)移动至(uk+1，vk +1)，则特征点l速度Vk的计算公式如下：
Figure BDA0004127566230000061
其中，在tk至tk+1时间段内相机从Ck移动到Ck+1，对应的归一化平面中特征点l的坐标由
Figure BDA0004127566230000062
移动至/>
Figure BDA0004127566230000063
步骤S2.2，构建含有td视觉重投影误差，通过特征点速度V与时间差td的乘积补偿位置，更新后的残差计算模型如下：
Figure BDA0004127566230000064
特征点l从第i帧运动至第j帧，
Figure BDA0004127566230000065
为分别为两帧上特征点l的归一化坐标，λi表示该特征点在第i帧的逆深度，由R和p组成的矩阵称为变换矩阵(R,p)，/>
Figure BDA0004127566230000066
表示从第i帧的相机坐标系通过旋转与平移变换至世界坐标系，再利用/>
Figure BDA0004127566230000067
从世界坐标系变换至第j帧，π()为特征点重投影模型，/>
Figure BDA0004127566230000068
为特征点重投影的残差量，在位姿优化的过程中，通过最小化重投影误差来调整变换矩阵(R,p)。
步骤S2.3，在后端优化模型中，将td作为新添加的状态量进行估计，更新后待估计的状态量如下：
Figure BDA0004127566230000069
其中，xk包含第k个IMU的位置
Figure BDA00041275662300000610
速度/>
Figure BDA00041275662300000611
和旋转/>
Figure BDA00041275662300000612
ba和bg分别是加速度和陀螺仪的偏置，λi表示第i个特征点的逆深度，/>
Figure BDA00041275662300000613
表示三维线的表示，td为待优化的时间偏差。
此外，在后端优化中，可使用ceres工具进行优化，目标函数设置为：
Figure BDA0004127566230000071
其中，Iprior表示在滑动窗口中边缘化一个旧帧时获得的先验信息，eimu表示IMU的残差，e(td)point表示带有时间偏差优化的特征点重投影误差，eline表示线特征残差，eloop表示回环约束信息。
步骤S3，对后端优化的结果进行闭环检测，以确定正确的回环帧，并利用该回环帧建立后端优化过程中的回环约束。
步骤S3是回环检测阶段，在每一次滑窗优化结束后，将当前帧与最近关键帧做视差比对，当两帧视差大于某个阈值时，判断当前帧为关键帧。确定关键帧后，将其与关键帧数据库进行比对寻找候选回环帧。如果寻找到候选回环帧，将进行特征点匹配，然后对匹配的特征点进行几何异常值剔除，经过剔除后内点依然满足阈值，则判定为正确的回环帧。当找到回环帧后，便可建立回环约束，进行全局优化，以降低系统的累计误差。
在一个实施例中，步骤S3包括：
步骤S3.1，对当前的最新帧重新进行特征点提取，将提取到的点通过词袋模型的方法进行回环检测。
步骤S3.2，当检测到系统中出现回环时，建立一个回环约束，通过调整当前关键帧与回环帧之间的关键帧位姿，使得当前帧估计的位姿与实际位姿误差减小。
回环检测是指机器人识别曾到达某场景，使得地图闭环的能力。通过回环检测，可以显著地减小累积误差，帮助机器人更精准、快速的进行避障导航工作。词袋模型可用于完成闭环检测，简言之，是将帧与帧之间进行特征匹配。从每幅图像中提取特征点和特征描述，特征描述通常是一个多维向量，因此可以计算两个特征描述之间的距离。将这些特征描述进行聚类，类别的个数就是词典的单词数。将这些词典组织成树的形式，方便搜索。
综上所述，本发明提供一种基于时间戳优化的视觉惯性定位方法，通过动态估计时间戳的策略来计算IMU和相机的数据时间偏差，然后补偿到前端数据读取，从而改善了数据融合的性能，提升了IMU信息和图像信息之间的约束关系，同时引入线特征约束，提高弱光环境下和结构化场景中的位姿估计精度，最终提高了全局地图位姿估计精度。经过实验验证，本发明在性能和鲁棒性方面均优于其他现有方法。
本发明可以是系统、方法和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于使处理器实现本发明的各个方面的计算机可读程序指令。
计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、静态随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸起结构、以及上述的任意合适的组合。这里所使用的计算机可读存储介质不被解释为瞬时信号本身，诸如无线电波或者其他自由传播的电磁波、通过波导或其他传输媒介传播的电磁波(例如，通过光纤电缆的光脉冲)、或者通过电线传输的电信号。
这里所描述的计算机可读程序指令可以从计算机可读存储介质下载到各个计算/处理设备，或者通过网络、例如因特网、局域网、广域网和/或无线网下载到外部计算机或外部存储设备。网络可以包括铜传输电缆、光纤传输、无线传输、路由器、防火墙、交换机、网关计算机和/或边缘服务器。每个计算/处理设备中的网络适配卡或者网络接口从网络接收计算机可读程序指令，并转发该计算机可读程序指令，以供存储在各个计算/处理设备中的计算机可读存储介质中。
用于执行本发明操作的计算机程序指令可以是汇编指令、指令集架构(ISA)指令、机器指令、机器相关指令、微代码、固件指令、状态设置数据、或者以一种或多种编程语言的任意组合编写的源代码或目标代码，所述编程语言包括面向对象的编程语言—诸如Smalltalk、C++、Python等，以及常规的过程式编程语言—诸如“C”语言或类似的编程语言。计算机可读程序指令可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络—包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。在一些实施例中，通过利用计算机可读程序指令的状态信息来个性化定制电子电路，例如可编程逻辑电路、现场可编程门阵列(FPGA)或可编程逻辑阵列(PLA)，该电子电路可以执行计算机可读程序指令，从而实现本发明的各个方面。
这里参照根据本发明实施例的方法、装置(系统)和计算机程序产品的流程图和/或框图描述了本发明的各个方面。应当理解，流程图和/或框图的每个方框以及流程图和/或框图中各方框的组合，都可以由计算机可读程序指令实现。
这些计算机可读程序指令可以提供给通用计算机、专用计算机或其它可编程数据处理装置的处理器，从而生产出一种机器，使得这些指令在通过计算机或其它可编程数据处理装置的处理器执行时，产生了实现流程图和/或框图中的一个或多个方框中规定的功能/动作的装置。也可以把这些计算机可读程序指令存储在计算机可读存储介质中，这些指令使得计算机、可编程数据处理装置和/或其他设备以特定方式工作，从而，存储有指令的计算机可读介质则包括一个制造品，其包括实现流程图和/或框图中的一个或多个方框中规定的功能/动作的各个方面的指令。
也可以把计算机可读程序指令加载到计算机、其它可编程数据处理装置、或其它设备上，使得在计算机、其它可编程数据处理装置或其它设备上执行一系列操作步骤，以产生计算机实现的过程，从而使得在计算机、其它可编程数据处理装置、或其它设备上执行的指令实现流程图和/或框图中的一个或多个方框中规定的功能/动作。
附图中的流程图和框图显示了根据本发明的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。对于本领域技术人员来说公知的是，通过硬件方式实现、通过软件方式实现以及通过软件和硬件结合的方式实现都是等价的。
以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。本发明的范围由所附权利要求来限定。

Claims (9)

1.一种基于时间戳优化的视觉惯性定位方法，包括：
状态预处理步骤：针对待测场景，利用相机获取图像数据，利用惯性测量单元获取IMU数据，并对图像数据进行点特征提取与线特征提取，进而恢复特征点的逆深度以及图像帧的初始位姿信息；
后端优化步骤：利用图像数据和IMU数据，并采用滑动窗口非线性策略，以对多个状态量进行估计，其中，所述多个状态量包括惯性测量单元的状态信息、特征点的逆深度、特征线的表示以及IMU数据与图像数据的时间偏移量，进而利用所述时间偏移量补偿IMU数据与图像数据；
回环检测步骤：在每一次后端优化结束后，判断关键帧，并基于关键帧与关键帧数据库的比对寻找候选回环帧，进而对候选回环帧进行特征点匹配，识别出正确的回环帧，以利用该回环帧建立后端优化过程中的回环约束。
2.根据权利要求2所述的方法，其特征在于，所述状态预处理步骤包括：
通过IMU预积分模型对相邻两图像帧之间的IMU数据进行积分，求得当前图像帧与上一帧的相对位姿；
利用Shi-Tomasi算子从图像中提取特征点，并采用KLT光流法对所提取的特征点在下一帧上寻找匹配点进行跟踪，进而对前后匹配的特征点进行校正，并对每对匹配点采用本征矩阵和RANSAC算法进行筛选；
使用LSD算法提取图像中的线段，分别采用LBD描述子和K近邻算法进行线特征的描述和匹配；
通过稀疏SfM方法恢复特征点的逆深度，并恢复图像帧的初始位姿信息。
3.根据权利要求1所述的方法，其特征在于，所述后端优化步骤包括：
通过引入特征点速度来建模时间偏移量，特征点l的速度表示为：
Figure FDA0004127566180000011
其中，在tk至tk+1时间段内相机从Ck移动到Ck+1，对应的归一化平面中特征点l的坐标由
Figure FDA0004127566180000021
移动至/>
Figure FDA0004127566180000022
构建含有时间偏移量td的视觉重投影误差，通过特征点速度与时间偏移量td的乘积补偿位置，更新后的残差计算表示为：
Figure FDA0004127566180000023
其中，特征点l从第i帧运动至第j帧，
Figure FDA0004127566180000024
分别为两帧上特征点l的归一化坐标，λi表示该特征点l在第i帧的逆深度，/>
Figure FDA0004127566180000025
表示从第i帧的相机坐标系通过旋转与平移变换至世界坐标系，再利用/>
Figure FDA0004127566180000026
从世界坐标系变换至第j帧，π()为特征点重投影模型，/>
Figure FDA0004127566180000027
为特征点重投影的残差量；
将时间偏移量td作为新添加的状态量进行估计。
4.根据权利要求3所述的方法，其特征在于，所述多个状态量表示为：
Figure FDA0004127566180000028
Figure FDA0004127566180000029
其中，xk包含第k个惯性测量单元的位置
Figure FDA00041275661800000210
速度/>
Figure FDA00041275661800000211
和旋转/>
Figure FDA00041275661800000212
ba和bg分别是角速度计和陀螺仪的偏置，onl是三维线的表示。
5.根据权利要求3所述的方法，其特征在于，在所述后端优化步骤中，将目标函数设置为：
Figure FDA00041275661800000213
其中，Iprior表示在滑动窗口中边缘化一个旧帧时获得的先验信息，eimu表示惯性测量单元的残差，e(td)point表示带有时间偏移量td优化的特征点重投影误差，eline表示线特征残差，eloop表示回环约束信息。
6.根据权利要求1所述的方法，其特征在于，所述回环检测步骤包括：
在每一次后端优化结束后，将当前帧与最近关键帧作视差比对，当两帧视差大于设定阈值时，判断当前帧为关键帧；
将所确定的关键帧与关键帧数据库进行比对，以寻找候选回环帧；
在寻找到候选回环帧后，进行特征点匹配，并对匹配的特征点进行几何异常值剔除，在经过剔除后的特征点依然满足阈值的情况下，判定为正确的回环帧，进而利用该回环帧建立回环约束。
7.根据权利要求6所述的方法，其特征在于，利用词袋模型检测所述回环帧。
8.一种计算机可读存储介质，其上存储有计算机程序，其中，该计算机程序被处理器执行时实现根据权利要求1至7中任一项所述的方法的步骤。
9.一种计算机设备，包括存储器和处理器，在所述存储器上存储有能够在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
CN202310250012.9A 2023-03-06 2023-03-06 一种基于时间戳优化的视觉惯性定位方法 Pending CN116380070A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310250012.9A CN116380070A (zh) 2023-03-06 2023-03-06 一种基于时间戳优化的视觉惯性定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310250012.9A CN116380070A (zh) 2023-03-06 2023-03-06 一种基于时间戳优化的视觉惯性定位方法

Publications (1)

Publication Number Publication Date
CN116380070A true CN116380070A (zh) 2023-07-04

Family

ID=86960702

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310250012.9A Pending CN116380070A (zh) 2023-03-06 2023-03-06 一种基于时间戳优化的视觉惯性定位方法

Country Status (1)

Country Link
CN (1) CN116380070A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116958267A (zh) * 2023-09-21 2023-10-27 腾讯科技（深圳）有限公司 位姿处理方法、装置、电子设备及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116958267A (zh) * 2023-09-21 2023-10-27 腾讯科技（深圳）有限公司 位姿处理方法、装置、电子设备及存储介质
CN116958267B (zh) * 2023-09-21 2024-01-12 腾讯科技（深圳）有限公司 位姿处理方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
US20230194266A1 (en) Vision-aided inertial navigation
CN112484725B (zh) 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
WO2021232470A1 (zh) 基于多传感器融合的slam制图方法、系统
Liu et al. Stereo visual-inertial odometry with multiple Kalman filters ensemble
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN112197770B (zh) 一种机器人的定位方法及其定位装置
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN108981687A (zh) 一种视觉与惯性融合的室内定位方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN116380070A (zh) 一种基于时间戳优化的视觉惯性定位方法
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
CN113155152B (zh) 基于李群滤波的相机与惯性传感器空间关系自标定方法
Pöppl et al. Integrated trajectory estimation for 3D kinematic mapping with GNSS, INS and imaging sensors: A framework and review
Yu et al. Tightly-coupled fusion of VINS and motion constraint for autonomous vehicle
Pan et al. Tightly-coupled multi-sensor fusion for localization with LiDAR feature maps
CN112731503B (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN115930948A (zh) 一种果园机器人融合定位方法
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN115183767A (zh) 一种基于arkf的单目vio/uwb室内组合定位方法
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN114646993A (zh) 一种基于gnss、视觉以及imu进行精确定位的数据融合算法

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