CN114216455A - 同时优化时间偏差的视觉惯性里程计系统 - Google Patents

同时优化时间偏差的视觉惯性里程计系统 Download PDF

Info

Publication number
CN114216455A
CN114216455A CN202111297656.0A CN202111297656A CN114216455A CN 114216455 A CN114216455 A CN 114216455A CN 202111297656 A CN202111297656 A CN 202111297656A CN 114216455 A CN114216455 A CN 114216455A
Authority
CN
China
Prior art keywords
camera
coordinate system
time offset
imu
time
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
CN202111297656.0A
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.)
Tianjin Polytechnic University
Original Assignee
Tianjin Polytechnic University
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 Tianjin Polytechnic University filed Critical Tianjin Polytechnic University
Priority to CN202111297656.0A priority Critical patent/CN114216455A/zh
Publication of CN114216455A publication Critical patent/CN114216455A/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/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)

Abstract

一种同时优化时间偏差的视觉惯性里程计系统。对于机器人系统中的视觉惯性里程计，可以通过考虑传感器的不确定性来提高定位性能。本发明针对单目视觉惯性定位提出了一种初始化策略，该策略优化了相机和惯性测量单元之间的时间偏移。在本系统中，前端流程主要涉及持续细化，后端流程主要包括优化特定状态。优化时间偏差分为三个阶段，首先，在假设惯性测量单元的时间戳有效的前提下，分析了相机和惯性测量单元的时间戳之间时间偏移的影响，其中时间偏移来自重投影误差，要根据得到的时间偏差将相机的时间戳与惯性测量单元对齐。然后，通过考虑相机采样，计算投影三维点的平移和旋转运动，并针对相应的相机关键帧分析重投影图像特征的变化。最后，通过优化重投影误差估计时间偏移和特定状态，使视觉惯性定位消除不确定时间偏移的影响，进行比较实验以验证所提出方法的性能。

Description

同时优化时间偏差的视觉惯性里程计系统
技术领域
本发明属于计算机视觉与移动机器人的技术领域，特别是涉及需要同时优化时间偏差的视觉惯性里程计系统
背景技术
对于各种传感器套件，虽然已经进行了出厂参数的校准，但对于实际使用而言还不够。由于各种传感器有自己特定的属性，想要融合传感器的测量信息，需要将它们的外参数和时间戳对齐。对于相机和IMU套件，外参数表示这两个传感器空间上的相对物理位置，通常可以通过离线标定或在线估计的方法获得。而对于时间戳，由于相机的图片传输延迟和触发延迟，导致相机和IMU的时间戳不同步，即两个传感器产生了时间偏差。在数据融合过程中，通常是将两个传感器在相同时间戳下的信息进行融合并估计位姿，但是由于时间偏差的存在，相机和IMU在同一时刻的时间戳不相同导致两个传感器的数据融合产生偏差，从而影响视觉惯性里程计初始化的精度。当今比较流行的方式也是对时间偏差进行了在线估计，进而优化系统的初始化过程。
发明内容
本发明提出了一种方法，能够在视觉惯性里程计系统初始化时实时估计并补偿相机与IMU之间时间偏差并同时估计其他状态变量，这将会提高整个视觉惯性里程计初始化的精度。
本发明提出了一种基于单目视觉惯性系统的初始化方法，该方法可以同时优化相机和惯性测量单元之间的时间偏差。首先，在假设IMU时间戳有效的前提下，分析了相机和IMU时间戳之间时间偏移的影响，其中时间偏移来自重投影误差，要根据得到的时间偏差将相机的时间戳与IMU对齐。然后，通过考虑相机采样，计算投影3D点的平移和旋转运动，并针对相应的相机关键帧分析重投影图像特征的变化。最后，通过优化重投影误差估计时间偏移和特定状态，使视觉惯性定位消除不确定时间偏移的影响。
同时优化时间偏差的视觉惯性里程计系统，其特征包括以下步骤：
第1，系统描述
第1.1，系统坐标系的描述
将第一个相机关键帧的初始位姿定义为世界坐标系
Figure BSA00002567790400000110
并在重力矢量细化时调整其方向；
Figure BSA0000256779040000011
和
Figure BSA0000256779040000012
分别表示相对于第i个和第j个摄像机关键帧的坐标系。
Figure BSA00002567790400000111
表示
Figure BSA0000256779040000013
下的第l个坐标。随着相机的移动，第i个和第j个相机关键帧都可以观察到
Figure BSA0000256779040000014
两个相机坐标系下的
Figure BSA0000256779040000015
坐标分别为
Figure BSA0000256779040000016
和
Figure BSA0000256779040000017
投影在第i个和第j个摄像机关键帧的图像上，相关图像像素分别表示为
Figure BSA0000256779040000018
和
Figure BSA0000256779040000019
第1.2，控制方案
本发明的目的是提出一种在线估计方法来仔细处理时间偏移的影响，以提高视觉惯性里程计的准确性；附图1给出了所提供方案的框图；优化时间偏差分为三个阶段：在阶段1中，由于相机延迟不稳定，假设惯性测量单元(IMU)的时间戳是正确的，用于估计相机和IMU之间的时间偏移。由于图像特征只是在连续的相机关键帧上观察到的，因此在时间偏移期间，它们在一个相机关键帧下的坐标被确认不变，从而在相应的相机关键帧上处理重投影误差。在阶段2中，通过考虑时间偏移来计算投影3D点的平移和旋转运动，并针对匹配的相机关键帧分析重投影图像特征的变化。此外，通过考虑时间偏移的影响重新计算重投影误差的雅可比矩阵。在阶段3中，通过优化重投影误差的协方差方程来估计时间偏移和其他状态。
第2，时间偏移分析
第2.1，时间偏移影响
由于相机和IMU之间的时间偏移，通过与IMU时间戳对齐，相机关键帧的坐标系
Figure BSA0000256779040000021
实际上位于
Figure BSA0000256779040000022
如附图2所示。即
Figure BSA0000256779040000023
通过考虑时间偏移来表示相机坐标系的实际位姿。在这种情况下，坐标
Figure BSA0000256779040000024
在
Figure BSA0000256779040000025
下表示为
Figure BSA0000256779040000026
表示坐标系
Figure BSA0000256779040000027
和
Figure BSA0000256779040000028
之间的旋转矩阵。为了促进后续分析，对于第i个相机关键帧，将第l个特征点的坐标统一到坐标系
Figure BSA0000256779040000029
下，
Figure BSA00002567790400000210
重命名为
Figure BSA00002567790400000211
如附图2所示。
与引入
Figure BSA00002567790400000212
相同，坐标系
Figure BSA00002567790400000213
是
Figure BSA00002567790400000214
与IMU时间戳对齐后的实际位姿，如附图3所示。可以假设
Figure BSA00002567790400000215
在
Figure BSA00002567790400000216
下投影，计算得到的图像特征记为
Figure BSA00002567790400000217
已知
Figure BSA00002567790400000218
和捕获的图像特征
Figure BSA00002567790400000219
都在第j个相机关键帧上。因此，时间偏移td导致像点
Figure BSA00002567790400000220
移动到第j个摄像机关键帧
Figure BSA00002567790400000221
上的
Figure BSA00002567790400000222
然后在确定坐标系
Figure BSA00002567790400000223
后可以导出td。因此，可以从第j次相机重投影误差得到时间偏移量td，然后将其添加到优化过程中。
第2.2，重投影模型
令
Figure BSA00002567790400000224
表示相机坐标系
Figure BSA00002567790400000225
下的第l个特征点，那么它的视觉残差
Figure BSA00002567790400000247
投影到
Figure BSA00002567790400000226
的归一化图像上可以得到：
Figure BSA00002567790400000227
其中
Figure BSA00002567790400000228
表示图像特征
Figure BSA00002567790400000229
的归一化像素坐标。
Figure BSA00002567790400000230
其中观测信息
Figure BSA00002567790400000231
是
Figure BSA00002567790400000232
的归一化像素坐标。在公式(2)中，
Figure BSA00002567790400000233
和
Figure BSA00002567790400000234
分别是从第i个相机坐标系
Figure BSA00002567790400000235
到世界坐标系
Figure BSA00002567790400000236
的旋转矩阵和平移向量。相机投影模型π(·)用于在失真影响下将3D特征映射到归一化的图像平面上。
从第i个相机关键帧的图像，
Figure BSA00002567790400000237
可以计算如下：
Figure BSA00002567790400000238
其中
Figure BSA00002567790400000239
是
Figure BSA00002567790400000240
下的归一化像素坐标，
Figure BSA00002567790400000241
是
Figure BSA00002567790400000242
下第l个特征点的深度。
Figure BSA00002567790400000243
下的
Figure BSA00002567790400000244
与
Figure BSA00002567790400000245
下的
Figure BSA00002567790400000246
之间的关系可以通过坐标系变换原理得到：
Figure BSA0000256779040000031
其中
Figure BSA0000256779040000032
和
Figure BSA0000256779040000033
分别是从相机坐标系到IMU坐标系
Figure BSA0000256779040000034
的旋转矩阵和平移向量，即外参数
Figure BSA0000256779040000035
和
Figure BSA0000256779040000036
分别为第i个IMU坐标系
Figure BSA0000256779040000037
到世界坐标系
Figure BSA0000256779040000038
的旋转矩阵和平移向量，
Figure BSA0000256779040000039
和
Figure BSA00002567790400000310
分别是从第j个IMU坐标系
Figure BSA00002567790400000311
到
Figure BSA00002567790400000312
的旋转矩阵和平移向量。
第2.3，相机和IMU时间戳
如2.1所述，选择IMU时间戳作为参考，然后在与相机采样进行比较后可以得出未知的时间偏移。假设在连续摄像机关键帧的采样中时间偏移是恒定的，并且每个关键帧持续相对较短的时间。
相机和IMU采样的时间戳如附图4，其中实线箭头是设置为基准的IMU时间戳，实际相机采样用两者中右边虚线箭头标记，左边虚线箭头表示相对于时间偏移进行调整后的相机时间戳。因此，IMU时间戳timu和相机时间戳tcam的关系可以写成：
td＝tcam-timu (5)
其中td是将相机时间戳与IMU时间戳对齐的时间偏移。
第3，具有时间偏移的VIO
如附图3所示，第i个相机关键帧中的一个图像特征在相机移动下有位移偏差，第l个3D点
Figure BSA00002567790400000313
的坐标在坐标系
Figure BSA00002567790400000314
和
Figure BSA00002567790400000315
下受时间偏移影响而变化td。在本发明中，通过考虑图像特征漂移来分析重投影误差，然后使用时间偏移进行优化以进行VIO定位。
第3.1，坐标漂移分析
第i个和第j个相机坐标系之间的旋转矩阵记为
Figure BSA00002567790400000316
可以进一步用四元数形式表示为
Figure BSA00002567790400000317
通过坐标系变换计算得到：
Figure BSA00002567790400000318
其中
Figure BSA00002567790400000319
和
Figure BSA00002567790400000320
分别是
Figure BSA00002567790400000321
和
Figure BSA00002567790400000322
的四元数表示。旋转矩阵
Figure BSA00002567790400000323
由时间偏移td引起，并且
Figure BSA00002567790400000324
可以通过第i个和第j个相机关键帧之间的四元数
Figure BSA00002567790400000325
计算，在公式(6)中计算方式如下：
Figure BSA00002567790400000326
其中
Figure BSA00002567790400000327
是
Figure BSA00002567790400000328
的四元数表示，符号
Figure BSA00002567790400000329
是四元数
Figure BSA00002567790400000330
的向量部分，即得到
Figure BSA00002567790400000331
通过估计
Figure BSA00002567790400000332
可以针对第j个摄像机关键帧
Figure BSA00002567790400000333
优化时间偏移td。
第i个相机关键帧的线速度记为vi(t)，可以直接通过IMU测量获得。通过考虑vi(t)，坐标
Figure BSA00002567790400000334
变为
Figure BSA00002567790400000335
其偏移偏差如下：
Figure BSA00002567790400000336
根据坐标系变换原理，第l个点
Figure BSA00002567790400000337
与其偏差
Figure BSA00002567790400000338
的关系如下：
Figure BSA00002567790400000339
通过考虑时间偏移，
Figure BSA0000256779040000041
在坐标系
Figure BSA0000256779040000042
下通过以下变换旋转到
Figure BSA0000256779040000043
Figure BSA0000256779040000044
根据此式，在时间偏移的影响下获得
Figure BSA0000256779040000045
第3.2，重投影误差
在坐标系
Figure BSA0000256779040000046
下，移动的3D点
Figure BSA0000256779040000047
处的位置被重命名为
Figure BSA0000256779040000048
在第j个相机坐标系
Figure BSA0000256779040000049
下，3D点
Figure BSA00002567790400000410
转换为
Figure BSA00002567790400000411
具有以下关系：
Figure BSA00002567790400000412
与公式(8)类似，与第j个相机关键帧相关的坐标
Figure BSA00002567790400000413
随相机坐标系位移而变化：
Figure BSA00002567790400000414
其中vj是第j个相机关键帧的线速度。
根据以上分析，地标
Figure BSA00002567790400000415
可以通过以下关系计算：
Figure BSA00002567790400000416
因此，
Figure BSA00002567790400000417
的重投影误差
Figure BSA00002567790400000418
可以表示为：
Figure BSA00002567790400000419
其中
Figure BSA00002567790400000420
表示相对于图像特征
Figure BSA00002567790400000421
的归一化像素坐标。
第4，时间偏移的优化
通过考虑IMU约束，VIO框架的优化状态列出如下：
Figure BSA00002567790400000422
这些状态包括第i个和第j个IMU坐标系在
Figure BSA00002567790400000423
下的旋转矩阵
Figure BSA00002567790400000424
和平移向量
Figure BSA00002567790400000425
Figure BSA00002567790400000426
外部参数
Figure BSA00002567790400000427
和
Figure BSA00002567790400000428
尺度
Figure BSA00002567790400000429
和时间偏移td。注意，通过优化td，设计的估计器完成了时间偏移补偿。
视觉残差的雅可比矩阵是通过对
Figure BSA00002567790400000430
取偏导数导出的，如下所示：
Figure BSA00002567790400000431
其中
Figure BSA00002567790400000432
是坐标系
Figure BSA00002567790400000433
的视觉残差，由公式(14)得到。
位姿的雅可比矩阵如下：
Figure BSA0000256779040000051
Figure BSA0000256779040000052
分别与
Figure BSA0000256779040000053
下的第i个和第j个IMU坐标系相关。
外部参数的雅可比矩阵是：
Figure BSA0000256779040000054
第i个关键帧中第l个特征点深度的雅可比矩阵如下：
Figure BSA0000256779040000055
时间偏移的雅可比矩阵如下：
Figure BSA0000256779040000056
因此，在优化
Figure BSA0000256779040000057
下
Figure BSA0000256779040000058
的重投影误差的过程中估计时间偏移td。此外，在优化过程中还可以估计相机的外部参数
Figure BSA0000256779040000059
和
Figure BSA00002567790400000510
附图说明：
图1为考虑时间偏差的视觉惯性里程计初始化框架
图2为考虑时间偏差的第i个相机关键帧坐标系的变换
图3为考虑时间偏差的两个相机关键帧坐标系之间的变换
图4为连续采样下考虑时间偏差的相机与IMU的时间戳示意图
图5为实验图：运行结果的均方根误差对比
图6为实验图：所提方法、优化外参数方法与VINS-Mono在MH01简单数据集下的对比
图7为实验图：所提方法、优化外参数方法与VINS-Mono在MH05困难数据集下的对比
图8为实验图：数据集运行结果的绝对姿态误差和均方根误差对比
图9为实验图：本方法真实环境实验过程图
图10为实验图：室内实验轨迹估计路径
图11为实验图：室外实验轨迹估计路径
图12为实验图：室内实验的时间偏差估计结果
图13为实验图：室外实验的时间偏差估计结果
具体实施方式：
1、同时优化时间偏差的视觉惯性里程计系统，其特征包括如下：
第1，系统描述
第1.1，系统坐标系的描述
将第一个相机关键帧的初始位姿定义为世界坐标系
Figure BSA0000256779040000061
并在重力矢量细化时调整其方向；
Figure BSA0000256779040000062
和
Figure BSA0000256779040000063
分别表示相对于第i个和第j个摄像机关键帧的坐标系。
Figure BSA0000256779040000064
表示
Figure BSA0000256779040000065
下的第l个坐标。随着相机的移动，第i个和第j个相机关键帧都可以观察到
Figure BSA0000256779040000066
两个相机坐标系下的
Figure BSA0000256779040000067
坐标分别为
Figure BSA0000256779040000068
和
Figure BSA0000256779040000069
投影在第i个和第j个摄像机关键帧的图像上，相关图像像素分别表示为
Figure BSA00002567790400000610
和
Figure BSA00002567790400000611
第1.2，控制方案
本发明的目的是提出一种在线估计方法来仔细处理时间偏移的影响，以提高视觉惯性里程计的准确性；附图1给出了所提供方案的框图；优化时间偏差分为三个阶段：在阶段1中，由于相机延迟不稳定，假设惯性测量单元(IMU)的时间戳是正确的，用于估计相机和IMU之间的时间偏移。由于图像特征只是在连续的相机关键帧上观察到的，因此在时间偏移期间，它们在一个相机关键帧下的坐标被确认不变，从而在相应的相机关键帧上处理重投影误差。在阶段2中，通过考虑时间偏移来计算投影3D点的平移和旋转运动，并针对匹配的相机关键帧分析重投影图像特征的变化。此外，通过考虑时间偏移的影响重新计算重投影误差的雅可比矩阵。在阶段3中，通过优化重投影误差的协方差方程来估计时间偏移和其他状态。
第2，时间偏移分析
第2.1，时间偏移影响
由于相机和IMU之间的时间偏移，通过与IMU时间戳对齐，相机关键帧的坐标系
Figure BSA00002567790400000612
实际上位于
Figure BSA00002567790400000613
如附图2所示。即
Figure BSA00002567790400000614
通过考虑时间偏移来表示相机坐标系的实际位姿。在这种情况下，坐标
Figure BSA00002567790400000615
在
Figure BSA00002567790400000616
下表示为
Figure BSA00002567790400000617
表示坐标系
Figure BSA00002567790400000618
和
Figure BSA00002567790400000619
之间的旋转矩阵。为了促进后续分析，对于第i个相机关键帧，将第l个特征点的坐标统一到坐标系
Figure BSA00002567790400000620
下，
Figure BSA00002567790400000621
重命名为
Figure BSA00002567790400000622
如附图2所示。
与引入
Figure BSA00002567790400000623
相同，坐标系
Figure BSA00002567790400000624
是
Figure BSA00002567790400000625
与IMU时间戳对齐后的实际位姿，如附图3所示。可以假设
Figure BSA00002567790400000626
在
Figure BSA00002567790400000627
下投影，计算得到的图像特征记为
Figure BSA00002567790400000628
已知
Figure BSA00002567790400000629
和捕获的图像特征
Figure BSA00002567790400000630
都在第j个相机关键帧上。因此，时间偏移td导致像点
Figure BSA00002567790400000631
移动到第j个摄像机关键帧
Figure BSA00002567790400000632
上的
Figure BSA00002567790400000633
然后在确定坐标系
Figure BSA00002567790400000634
后可以导出td。因此，可以从第j次相机重投影误差得到时间偏移量td，然后将其添加到优化过程中。
第2.2，重投影模型
令
Figure BSA00002567790400000635
表示相机坐标系
Figure BSA00002567790400000636
下的第l个特征点，那么它的视觉残差
Figure BSA00002567790400000637
投影到
Figure BSA00002567790400000638
的归一化图像上可以得到：
Figure BSA00002567790400000639
其中
Figure BSA00002567790400000640
表示图像特征
Figure BSA00002567790400000641
的归一化像素坐标。
Figure BSA0000256779040000071
其中观测信息
Figure BSA0000256779040000072
是
Figure BSA0000256779040000073
的归一化像素坐标。在公式(2)中，
Figure BSA0000256779040000074
和
Figure BSA0000256779040000075
分别是从第i个相机坐标系
Figure BSA0000256779040000076
到世界坐标系
Figure BSA0000256779040000077
的旋转矩阵和平移向量。相机投影模型π(·)用于在失真影响下将3D特征映射到归一化的图像平面上。
从第i个相机关键帧的图像，
Figure BSA0000256779040000078
可以计算如下：
Figure BSA0000256779040000079
其中
Figure BSA00002567790400000710
是
Figure BSA00002567790400000711
下的归一化像素坐标，
Figure BSA00002567790400000712
是
Figure BSA00002567790400000713
下第l个特征点的深度。
Figure BSA00002567790400000714
下的
Figure BSA00002567790400000715
与
Figure BSA00002567790400000716
下的
Figure BSA00002567790400000717
之间的关系可以通过坐标系变换原理得到：
Figure BSA00002567790400000718
其中
Figure BSA00002567790400000719
和
Figure BSA00002567790400000720
分别是从相机坐标系到IMU坐标系
Figure BSA00002567790400000721
的旋转矩阵和平移向量，即外参数
Figure BSA00002567790400000722
和
Figure BSA00002567790400000723
分别为第i个IMU坐标系
Figure BSA00002567790400000724
到世界坐标系
Figure BSA00002567790400000725
的旋转矩阵和平移向量，
Figure BSA00002567790400000726
和
Figure BSA00002567790400000727
分别是从第j个IMU坐标系
Figure BSA00002567790400000728
到
Figure BSA00002567790400000729
的旋转矩阵和平移向量。
第2.3，相机和IMU时间戳
如2.1所述，选择IMU时间戳作为参考，然后在与相机采样进行比较后可以得出未知的时间偏移。假设在连续摄像机关键帧的采样中时间偏移是恒定的，并且每个关键帧持续相对较短的时间。
相机和IMU采样的时间戳如附图4，其中实线箭头是设置为基准的IMU时间戳，实际相机采样用两者中右边虚线箭头标记，左边虚线箭头表示相对于时间偏移进行调整后的相机时间戳。因此，IMU时间戳timu和相机时间戳tcam的关系可以写成：
td＝tcam-timu (5)
其中td是将相机时间戳与IMU时间戳对齐的时间偏移。
第3，具有时间偏移的VIO
如附图3所示，第i个相机关键帧中的一个图像特征在相机移动下有位移偏差，第l个3D点
Figure BSA00002567790400000730
的坐标在坐标系
Figure BSA00002567790400000731
和
Figure BSA00002567790400000732
下受时间偏移影响而变化td。在本发明中，通过考虑图像特征漂移来分析重投影误差，然后使用时间偏移进行优化以进行VIO定位。
第3.1，坐标漂移分析
第i个和第j个相机坐标系之间的旋转矩阵记为
Figure BSA00002567790400000733
可以进一步用四元数形式表示为
Figure BSA00002567790400000734
通过坐标系变换计算得到：
Figure BSA00002567790400000735
其中
Figure BSA00002567790400000736
和
Figure BSA00002567790400000737
分别是
Figure BSA00002567790400000738
和
Figure BSA00002567790400000739
的四元数表示。旋转矩阵
Figure BSA00002567790400000740
由时间偏移td引起，并且
Figure BSA00002567790400000741
可以通过第i个和第j个相机关键帧之间的四元数
Figure BSA00002567790400000742
计算，在公式(6)中计算方式如下：
Figure BSA0000256779040000081
其中
Figure BSA0000256779040000082
是
Figure BSA0000256779040000083
的四元数表示，符号
Figure BSA0000256779040000084
是四元数
Figure BSA0000256779040000085
的向量部分，即得到
Figure BSA0000256779040000086
通过估计
Figure BSA0000256779040000087
可以针对第j个摄像机关键帧
Figure BSA0000256779040000088
优化时间偏移td。
第i个相机关键帧的线速度记为vi(t)，可以直接通过IMU测量获得。通过考虑vi(t)，坐标
Figure BSA0000256779040000089
变为
Figure BSA00002567790400000810
其偏移偏差如下：
Figure BSA00002567790400000811
根据坐标系变换原理，第l个点
Figure BSA00002567790400000812
与其偏差
Figure BSA00002567790400000813
的关系如下：
Figure BSA00002567790400000814
通过考虑时间偏移，
Figure BSA00002567790400000815
在坐标系
Figure BSA00002567790400000816
下通过以下变换旋转到
Figure BSA00002567790400000817
Figure BSA00002567790400000818
根据此式，在时间偏移的影响下获得
Figure BSA00002567790400000819
第3.2，重投影误差
在坐标系
Figure BSA00002567790400000820
下，移动的3D点
Figure BSA00002567790400000821
处的位置被重命名为
Figure BSA00002567790400000822
在第j个相机坐标系
Figure BSA00002567790400000823
下，3D点
Figure BSA00002567790400000824
转换为
Figure BSA00002567790400000825
具有以下关系：
Figure BSA00002567790400000826
与公式(8)类似，与第j个相机关键帧相关的坐标
Figure BSA00002567790400000827
随相机坐标系位移而变化：
Figure BSA00002567790400000828
其中vj是第j个相机关键帧的线速度。
根据以上分析，地标
Figure BSA00002567790400000829
可以通过以下关系计算：
Figure BSA00002567790400000830
因此，
Figure BSA00002567790400000831
的重投影误差
Figure BSA00002567790400000832
可以表示为：
Figure BSA00002567790400000833
其中
Figure BSA00002567790400000834
表示相对于图像特征
Figure BSA00002567790400000835
的归一化像素坐标。
第4，时间偏移的优化
通过考虑IMU约束，VIO框架的优化状态列出如下：
Figure BSA00002567790400000836
这些状态包括第i个和第j个IMU坐标系在
Figure BSA00002567790400000837
下的旋转矩阵
Figure BSA00002567790400000838
和平移向量
Figure BSA00002567790400000839
Figure BSA00002567790400000840
外部参数
Figure BSA00002567790400000841
和
Figure BSA00002567790400000842
尺度
Figure BSA00002567790400000843
和时间偏移td。注意，通过优化td，设计的估计器完成了时间偏移补偿。
视觉残差的雅可比矩阵是通过对
Figure BSA0000256779040000091
取偏导数导出的，如下所示：
Figure BSA0000256779040000092
其中
Figure BSA0000256779040000093
是坐标系
Figure BSA0000256779040000094
的视觉残差，由公式(14)得到。
位姿的雅可比矩阵如下：
Figure BSA0000256779040000095
Figure BSA0000256779040000096
分别与
Figure BSA0000256779040000097
下的第i个和第j个IMU坐标系相关。
外部参数的雅可比矩阵是：
Figure BSA0000256779040000098
第i个关键帧中第l个特征点深度的雅可比矩阵如下：
Figure BSA0000256779040000099
时间偏移的雅可比矩阵如下：
Figure BSA00002567790400000910
因此，在优化
Figure BSA00002567790400000911
下
Figure BSA00002567790400000912
的重投影误差的过程中估计时间偏移td。此外，在优化过程中还可以估计相机的外部参数
Figure BSA00002567790400000913
和
Figure BSA00002567790400000914
第5，实验和结论
第5.1，公共数据集实验结果
通过实验结果来证明该方法的有效性。在VINS-Mono启发性工作的基础上实施提出的方法。该方法通过两个公共基准数据集进行评估：EuRoC MAV数据集和TUM视觉惯性数据集。实验在一台计算机上实施，配有4核因特尔i7处理器，2.8GHZ主频，8GB运行内存，Ubuntu16.04和ROS Kinetic。使用开源软件包Evo评估轨迹误差。
本实验与经典的VINS-Mono工程进行比较，同时与之前的工作优化外参数的视觉惯性里程计系统进行比较。附图5表示本方法、优化外参数方法以及VINS-Mono数据集实验轨迹估计的RMSE结果。粗体的数据代表三个方法中最精确的结果，通过附图5可以看出，本方法在6个实验上具有更好的性能，3个实验不如优化外参数方法，1个实验三种方法都出现漂移。
附图6显示了三种方法相对于MH01简单数据集的结果路径，附图7显示了三种方法相对于MH05困难数据集轨迹估计结果。深实线和浅实线分别代表本方法和优化外参数方法的轨迹结果，点划线表示VINS-Mono的轨迹结果，粗实线代表理想的轨迹结果。与VINS-Mono以及优化外参数的方法相比，可以看出本方法的轨迹估计结果更接近理想值。
此外，使用绝对位置误差(APE)进行评估，它可以直接比较VIO估计结果和理想结果之间的路径误差。附图8分别显示了MH01简单、MH05困难和V103困难数据集运行结果的绝对姿态误差和均方根误差对比，与附图5中的均方根误差数据对应。可以看出，该方法的估计误差比VINS-Mono的估计误差小。
第5.2，真实场景实验结果
为了进一步验证，我们在实验室的室内和室外进行了真实场景的实验。使用IntelRealSense D435i摄像机套件作为传感器获取外界环境信息。D435i摄像机内置RGB彩色摄像机(OmniVision OV2740)和IMU(BMI055)。
附图9为所提方法在真实场景实验过程中的几幅图像，前四个为室内实验；后四个为室外实验。附图10为室内实验的轨迹估计路径，附图11为室外实验的轨迹估计路径。其中实线和虚线分别代表所提出方法和VINS-Mono的轨迹估计结果。附图12和附图13分别表示的是室内和室外场景下本实验方法与VINS-Mono时间偏差的估计结果，实线为本方法的估计结果，虚线为VINS-Mono的估计结果。通过以上实验结果可以发现，本实验的轨迹估计精度和定位精度都高于VINS-Mono。
第5.3，结论
提出了一种优化相机和IMU之间时间偏移的新方法，在两个传感器对齐的过程中，考虑了受时间偏移影响的平移和旋转偏差。通过分析具有3D坐标的两个连续相机关键帧，推断出时间偏移的影响。在对重投影误差的雅可比矩阵进行重新分析后，对时间偏移与IMU和相机状态估计联合优化。对比实验结果表明该方法对视觉惯性里程计系统具有良好的精度和设计的时间偏移补偿机制。

Claims (1)

1.同时优化时间偏差的视觉惯性里程计系统，其特征在于包括以下步骤：
第1，系统描述
第1.1，系统坐标系的描述
将第一个相机关键帧的初始位姿定义为世界坐标系
Figure FSA0000256779030000011
和
Figure FSA0000256779030000012
分别表示相对于第i个和第j个摄像机关键帧的坐标系，Pl w表示
Figure FSA0000256779030000013
下的第l个坐标；两个相机坐标系下的Pl w坐标分别为
Figure FSA0000256779030000014
和
Figure FSA0000256779030000015
第1.2，控制方案
本发明的目的是提出一种在线估计方法来仔细处理时间偏移的影响，以提高视觉惯性里程计的准确性；附图1给出了所提供方案的框图；
第2，时间偏移分析
第2.1，时间偏移影响
相机关键帧的坐标系
Figure FSA0000256779030000016
实际上位于
Figure FSA0000256779030000017
坐标Pl w在
Figure FSA0000256779030000018
下表示为
Figure FSA0000256779030000019
表示坐标系
Figure FSA00002567790300000110
和
Figure FSA00002567790300000111
之间的旋转矩阵；
Figure FSA00002567790300000112
重命名为
Figure FSA00002567790300000113
如附图2所示；坐标系
Figure FSA00002567790300000114
是
Figure FSA00002567790300000115
与IMU时间戳对齐后的实际位姿，如附图3所示；
第2.2，重投影模型
令
Figure FSA00002567790300000116
表示相机坐标系
Figure FSA00002567790300000117
下的第l个特征点，那么它的视觉残差
Figure FSA00002567790300000118
投影到
Figure FSA00002567790300000119
的归一化图像上可以得到：
Figure FSA00002567790300000120
其中
Figure FSA00002567790300000121
表示图像特征
Figure FSA00002567790300000122
的归一化像素坐标；
Figure FSA00002567790300000123
其中观测信息
Figure FSA00002567790300000124
是
Figure FSA00002567790300000125
的归一化像素坐标，
Figure FSA00002567790300000126
和
Figure FSA00002567790300000127
分别是从第i个相机坐标系
Figure FSA00002567790300000128
到世界坐标系
Figure FSA00002567790300000129
的旋转矩阵和平移向量；
从第i个相机关键帧的图像，
Figure FSA00002567790300000130
可以计算如下：
Figure FSA00002567790300000131
其中
Figure FSA00002567790300000132
是
Figure FSA00002567790300000133
下的归一化像素坐标，
Figure FSA00002567790300000134
是
Figure FSA00002567790300000135
下第l个特征点的深度；
Figure FSA00002567790300000136
下的
Figure FSA00002567790300000137
与
Figure FSA00002567790300000138
下的
Figure FSA00002567790300000139
之间的关系可以通过坐标系变换原理得到：
Figure FSA00002567790300000140
其中
Figure FSA00002567790300000141
和
Figure FSA00002567790300000142
分别是从相机坐标系到IMU坐标系
Figure FSA00002567790300000143
的旋转矩阵和平移向量，
Figure FSA00002567790300000144
Figure FSA00002567790300000145
和
Figure FSA00002567790300000146
均代表相应两个坐标系间的旋转矩阵和平移向量；
第2.3，相机和IMU时间戳
相机和IMU采样的时间戳如附图4；IMU时间戳timu和相机时间戳tcam的关系为：
td＝tcam-timu (5)
其中td是将相机时间戳与IMU时间戳对齐的时间偏移；
第3，具有时间偏移的VIO
第3.1，坐标漂移分析
第i个和第j个相机坐标系之间的旋转矩阵记为
Figure FSA0000256779030000021
四元数形式为
Figure FSA0000256779030000022
Figure FSA0000256779030000023
其中
Figure FSA0000256779030000024
和
Figure FSA0000256779030000025
分别是
Figure FSA0000256779030000026
和
Figure FSA0000256779030000027
的四元数表示，在公式(6)中计算方式如下：
Figure FSA0000256779030000028
其中
Figure FSA0000256779030000029
是
Figure FSA00002567790300000210
的四元数表示，符号
Figure FSA00002567790300000211
是四元数
Figure FSA00002567790300000212
的向量部分，即得到
Figure FSA00002567790300000213
第i个相机关键帧的线速度记为vi(t)，坐标
Figure FSA00002567790300000214
变为
Figure FSA00002567790300000215
其偏移偏差如下：
Figure FSA00002567790300000216
根据坐标系变换原理，第l个点Pl w与其偏差
Figure FSA00002567790300000217
的关系如下：
Figure FSA00002567790300000218
通过考虑时间偏移，Pl w在坐标系
Figure FSA00002567790300000219
下通过以下变换旋转到
Figure FSA00002567790300000220
Figure FSA00002567790300000221
根据此式，在时间偏移的影响下获得
Figure FSA00002567790300000222
第3.2，重投影误差
在第j个相机坐标系
Figure FSA00002567790300000223
下，3D点
Figure FSA00002567790300000224
转换为
Figure FSA00002567790300000225
具有以下关系：
Figure FSA00002567790300000226
与公式(8)类似，与第j个相机关键帧相关的坐标
Figure FSA00002567790300000227
随相机坐标系位移而变化：
Figure FSA00002567790300000228
根据以上分析，地标
Figure FSA00002567790300000229
可以通过以下关系计算：
Figure FSA00002567790300000230
因此，
Figure FSA00002567790300000231
的重投影误差
Figure FSA00002567790300000232
可以表示为：
Figure FSA00002567790300000233
其中
Figure FSA00002567790300000234
表示相对于图像特征
Figure FSA00002567790300000235
的归一化像素坐标；
第4，时间偏移的优化
通过考虑IMU约束，VIO框架的优化状态列出如下：
Figure FSA00002567790300000236
这些状态包括第i个和第j个IMU坐标系在
Figure FSA0000256779030000031
下的旋转矩阵和平移向量，外部参数Tb c和
Figure FSA0000256779030000032
尺度
Figure FSA0000256779030000033
和时间偏移td；
视觉残差的雅可比矩阵是通过对
Figure FSA0000256779030000034
取偏导数导出的，如下所示：
Figure FSA0000256779030000035
其中
Figure FSA0000256779030000036
是坐标系
Figure FSA0000256779030000037
的视觉残差，由公式(14)得到；
位姿的雅可比矩阵如下：
Figure FSA0000256779030000038
Figure FSA0000256779030000039
分别与
Figure FSA00002567790300000310
下的第i个和第j个IMU坐标系相关；
外部参数的雅可比矩阵是：
Figure FSA00002567790300000311
第i个关键帧中第l个特征点深度的雅可比矩阵如下：
Figure FSA00002567790300000312
时间偏移的雅可比矩阵如下：
Figure FSA00002567790300000313
因此，在优化
Figure FSA00002567790300000314
下
Figure FSA00002567790300000315
的重投影误差的过程中估计时间偏移td，此外，在优化过程中还可以估计相机的外部参数Tb c和
Figure FSA00002567790300000316
CN202111297656.0A 2021-11-04 2021-11-04 同时优化时间偏差的视觉惯性里程计系统 Pending CN114216455A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111297656.0A CN114216455A (zh) 2021-11-04 2021-11-04 同时优化时间偏差的视觉惯性里程计系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111297656.0A CN114216455A (zh) 2021-11-04 2021-11-04 同时优化时间偏差的视觉惯性里程计系统

Publications (1)

Publication Number Publication Date
CN114216455A true CN114216455A (zh) 2022-03-22

Family

ID=80695641

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111297656.0A Pending CN114216455A (zh) 2021-11-04 2021-11-04 同时优化时间偏差的视觉惯性里程计系统

Country Status (1)

Country Link
CN (1) CN114216455A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023226156A1 (zh) * 2022-05-24 2023-11-30 广东人工智能与先进计算研究院 时间戳校正方法、装置、设备、介质及计算机程序产品

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计系统

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计系统

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
李宝全;徐壮;冀东;: "基于单应矩阵分解的轮式移动机器人视觉伺服轨迹跟踪", 天津工业大学学报, no. 03, 25 June 2020 (2020-06-25) *
马英新;师五喜;: "基于模糊逻辑的机器人非标定视觉路径规划", 仪器仪表用户, no. 06, 8 December 2012 (2012-12-08) *
高喜天: "基于初始阶段优化的视觉 惯性里程计研究", 万方知识服务平台, 26 October 2021 (2021-10-26), pages 23 - 29 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023226156A1 (zh) * 2022-05-24 2023-11-30 广东人工智能与先进计算研究院 时间戳校正方法、装置、设备、介质及计算机程序产品

Similar Documents

Publication Publication Date Title
Ye et al. Tightly coupled 3d lidar inertial odometry and mapping
Kneip et al. Robust real-time visual odometry with a single camera and an IMU
CN109102525B (zh) 一种基于自适应位姿估计的移动机器人跟随控制方法
Furrer et al. Evaluation of combined time-offset estimation and hand-eye calibration on robotic datasets
Hwangbo et al. Gyro-aided feature tracking for a moving camera: fusion, auto-calibration and GPU implementation
CN110125928A (zh) 一种基于前后帧进行特征匹配的双目惯导slam系统
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN110455309B (zh) 具备在线时间校准的基于msckf的视觉惯性里程计
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN104704384A (zh) 具体用于装置的基于视觉的定位的图像处理方法
Seok et al. ROVINS: Robust omnidirectional visual inertial navigation system
CN110139031B (zh) 一种基于惯性感知的视频防抖系统及其工作方法
Alzugaray et al. Asynchronous multi-hypothesis tracking of features with event cameras
WO2022000713A1 (zh) 一种基于航空装配的增强现实自定位方法
Lin et al. A sparse visual odometry technique based on pose adjustment with keyframe matching
Chen et al. Stereo visual inertial pose estimation based on feedforward-feedback loops
Zheng et al. SE (2)-constrained visual inertial fusion for ground vehicles
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN114754768A (zh) 一种点线融合的视觉惯性导航方法
CN118135526A (zh) 基于双目相机的四旋翼无人机视觉目标识别与定位方法
Ok et al. Simultaneous tracking and rendering: Real-time monocular localization for MAVs
CN114216455A (zh) 同时优化时间偏差的视觉惯性里程计系统
Fernandez et al. Visual odometry for an outdoor mobile robot
Hug et al. Continuous-time stereo-inertial odometry
Ling et al. RGB-D inertial odometry for indoor robot via keyframe-based nonlinear optimization

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