CN111578937A - 同时优化外参数的视觉惯性里程计系统 - Google Patents

同时优化外参数的视觉惯性里程计系统 Download PDF

Info

Publication number
CN111578937A
CN111578937A CN202010471312.6A CN202010471312A CN111578937A CN 111578937 A CN111578937 A CN 111578937A CN 202010471312 A CN202010471312 A CN 202010471312A CN 111578937 A CN111578937 A CN 111578937A
Authority
CN
China
Prior art keywords
imu
rotation
gravity vector
equation
frame
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
Application number
CN202010471312.6A
Other languages
English (en)
Other versions
CN111578937B (zh
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.)
Shanghai Xintiance Digital Technology Co ltd
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 CN202010471312.6A priority Critical patent/CN111578937B/zh
Publication of CN111578937A publication Critical patent/CN111578937A/zh
Application granted granted Critical
Publication of CN111578937B publication Critical patent/CN111578937B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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

Landscapes

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

Abstract

对于机器人系统中的视觉惯性里程计，可以通过高效的初始化来提高最终定位的性能。本文提出了一种基于单目视觉惯性系统的初始化方法，该方法可以同时优化相机与惯性测量单元之间的外部参数。首先，通过视觉惯性信息估计旋转外参数、陀螺仪偏差、绝对尺度和重力矢量。然后，在视觉惯性测量信息对齐阶段优化平移外参数，并通过之前的状态估计计算得到速度。当旋转角度不够大时，由于加速度计偏差对初始化几乎没有影响，因此在初始化阶段不进行估计，而是在后端优化中进行处理。对比实验结果验证了该方法的有效性。

Description

同时优化外参数的视觉惯性里程计系统
技术领域
本发明属于计算机视觉与移动机器人的技术领域，特别是涉及需要同时优化外参数的视觉惯性里程计系统
背景技术
VO通过安装在机器人上的相机采集图像数据，根据图像特征和运动约束进行位姿估计，不会产生积累误差。但当视觉传感器数据采样率和机器人运动速度比较快时，会造成部分图像缺失而引起估计精度降低，同时单目相机也存在尺度不确定的问题。与视觉传感器不同，惯性传感器具有成本低、尺寸小等特点，能够以较高的频率采集物体的运动信息，非常适合应用在物体做高速运动的场景，对于短期运动预测精度较好，不会受到运动突然变化的影响，但其误差随时间累积严重。当今比较流行的解决方式是将视觉与IMU结合，通过IMU恢复出视觉的绝对尺度，同时利用视觉信息对IMU的偏差进行矫正。
发明内容
本文提供了一种方法，能够在视觉惯性里程计系统初始化时同时优化相机和IMU的外参数，这将会大大提高机器人定位与建图的精度。
本文提出了一种基于单目视觉惯性系统的初始化方法，该方法可以同时优化相机与惯性测量单元之间的外部参数。首先，通过视觉惯性信息估计旋转外参数、陀螺仪偏差、绝对尺度和重力矢量。然后，在视觉惯性测量信息对齐阶段优化平移外参数，并通过之前的状态估计计算得到速度。当旋转角度不够大时，由于加速度计偏差对初始化几乎没有影响，因此在初始化阶段不进行估计，而是在后端优化中进行处理。对比实验结果验证了该方法的有效性。
本发明提出的同时优化外参数的视觉惯性里程计系统包括：
同时优化外参数的视觉惯性里程计系统，其特征包括以下步骤：
第1，系统描述
第1.1，系统框架
所提出方法的系统框架，特别是视觉惯性对齐模块，如附图1所示。首先，通过运动恢复结构算法(SFM)获得两个连续相机帧之间的旋转和平移，该算法仅用于获得相机关键帧之间关系的。然后，采用IMU预积分算法来获得IMU两帧之间的差量，并在之前获得估计变量的基础上估算当前IMU关键帧的测量值。通过运动恢复结构和IMU预积分算法获得的相关关键帧的旋转
Figure BSA0000210024080000011
和
Figure BSA0000210024080000012
利用经典算法，使用收敛准则获得外部参数的旋转部分。其次，估计陀螺仪偏差、平移外参数、绝对尺度和重力向量，并重新纠正重力的方向。最后，每一帧的速度可以通过先前的状态估计计算得到。加速度计偏差通过后端VIO紧耦合优化进行补偿。
第1.2，IMU预积分
IMU的测量数据包括三轴角速度
Figure BSA0000210024080000013
和三轴线性加速度
Figure BSA0000210024080000014
偏差b和白噪声n影响加速度和角速度的精度；测量值和理想值之间的关系如下：
Figure BSA0000210024080000015
wb和ab分别为
Figure BSA0000210024080000021
和
Figure BSA0000210024080000022
的实际值，
Figure BSA0000210024080000023
和
Figure BSA0000210024080000024
分别为线性加速度和角速度的偏差。nw和na分别为wb和ab的高斯白噪声，服从高斯分布。
Figure BSA0000210024080000025
是世界坐标系
Figure BSA0000210024080000026
到IMU坐标系
Figure BSA0000210024080000027
的旋转矩阵，gw是世界坐标系
Figure BSA0000210024080000028
下的重力向量，所以
Figure BSA0000210024080000029
表示的是重力向量在IMU坐标系
Figure BSA00002100240800000210
下的值。
忽略噪声，IMU连续两帧
Figure BSA00002100240800000211
和
Figure BSA00002100240800000212
在时间间隔[tk，tk+1]内的位移、速度和旋转表示如下：
Figure BSA00002100240800000213
把公式(1)代入公式(2)，并从
Figure BSA00002100240800000214
变换到
Figure BSA00002100240800000215
得到IMU预积分公式如下：
Figure BSA00002100240800000216
公式(3)中，
Figure BSA00002100240800000217
和
Figure BSA00002100240800000218
分别为两帧之间的位移、速度旋转差量。由公式可以看出惯性测量单元的当前关键帧的测量值与之前帧无关，仅由
Figure BSA00002100240800000219
和
Figure BSA00002100240800000220
决定。
第2，估计旋转外参数
第2.1，运动恢复结构
视觉初始化使用运动恢复结构(SFM)方法估计滑动窗口内的所有关键帧的位姿。首先，将第一帧相机帧
Figure BSA00002100240800000221
作为第一个参考关键帧，然后选择一个与当前帧具有更多公共点和足够视差的帧作为第二个参考关键帧。通过五点法或八点法估算当前帧和参考关键帧之间的旋转和平移。然后，利用三角化和PnP算法以及捆集调整(BA)获得滑动窗口内所有关键帧的位姿。在此基础上，得到第k个和第k+1个摄像机关键帧之间的旋转
Figure BSA00002100240800000222
和缩放平移
Figure BSA00002100240800000223
第2.2，旋转外参数
根据坐标系之间的变换，可得到以下关系：
Figure BSA00002100240800000224
其中
Figure BSA00002100240800000225
由IMU陀螺仪在IMU坐标系
Figure BSA00002100240800000226
下积分得到，
Figure BSA00002100240800000227
通过视觉初始化得到，
Figure BSA00002100240800000228
是需要估计的旋转外参数矩阵。
把以上旋转矩阵用四元数表示得到以下公式：
Figure BSA00002100240800000229
将Q1和Q2定义为四元数的左乘和右乘，得到以下公式：
Figure BSA0000210024080000031
变换后得到：
Figure BSA0000210024080000032
随着关键帧数量的增加，得到以下超线性方程组：
Figure BSA0000210024080000033
其中N为关键帧数量的最大值，
Figure BSA0000210024080000034
为剔除重投影误差的权重，利用终止准则可以得到旋转外参数。
第3，视觉惯性初始化
第3.1，陀螺仪偏差校正
在相机坐标系
Figure BSA0000210024080000035
下，设置第一个参考关键帧后，通过SFM算法得到第k帧和参考关键帧之间的旋转
Figure BSA0000210024080000036
然后
Figure BSA0000210024080000037
可以由以下公式得到：
Figure BSA0000210024080000038
对于滑动窗口内的两个IMU关键帧
Figure BSA0000210024080000039
和
Figure BSA00002100240800000310
和
Figure BSA00002100240800000311
可由公式(9)得到，相对旋转
Figure BSA00002100240800000312
由公式(3)获得。因此陀螺仪偏差的目标方程表示如下：
Figure BSA00002100240800000313
其中：
Figure BSA00002100240800000314
因此，最小化公式(10)可以得到陀螺仪偏差bw。相对位移
Figure BSA00002100240800000315
和相对速度
Figure BSA00002100240800000316
根据即时计算得出的bw进行更新。
第3.2，平移外参数、绝对尺度和重力向量
通过以上分析，所有的参数都可以从IMU坐标系
Figure BSA00002100240800000317
转换到相机坐标系
Figure BSA00002100240800000318
下：
Figure BSA00002100240800000319
其中s是尺度因子，
Figure BSA00002100240800000320
是缩放的位移。绝对尺度和缩放尺度之间的关系如下：
Figure BSA00002100240800000321
所以(3)式可写成如下形式：
Figure BSA0000210024080000041
通过公式(11)(12)(13)，
Figure BSA0000210024080000042
和
Figure BSA0000210024080000043
可以被表示为：
Figure BSA0000210024080000044
假设关键帧的下标i，i+1，i+2分别为1，2，3，并把(15)式的两个式子放在一起得到：
Figure BSA0000210024080000045
其中：
Figure BSA0000210024080000046
通过公式(16)，利用最小二乘算法可得到平移外参数
Figure BSA0000210024080000047
重力向量
Figure BSA0000210024080000048
和绝对尺度s。
第3.3，重力方向矫正
估计出的重力向量受加速度计偏差的影响，假设重力向量的幅值是一个常量，重力向量的方向由于偏差而发生改变。因此，此时矫正重力向量可以设为两个自由度：
Figure BSA0000210024080000049
其中
Figure BSA00002100240800000410
为重力向量的幅值，w1b1和w2b2是分别来自两个正交方向的扰动。b为扰动的方向，w为对应的b方向的扰动量。
把公式(18)代入(16)得到：
Figure BSA00002100240800000411
其中：
Figure BSA0000210024080000051
通过求解公式(20)，重力向量被重新矫正。在此基础上，
Figure BSA0000210024080000052
和
Figure BSA0000210024080000053
坐标系下的所有状态变量都可以变换到世界坐标系
Figure BSA0000210024080000054
下。此外，平移和地图点的坐标可以根据得到的绝对尺度s进行缩放。
第3.4，重力方向矫正
重力和绝对尺度得到后，每一关键帧的速度可以由公式(2)和(15)计算得到。在视觉惯性初始化时没有估计加速度计偏差ba，因为它对初始化的影响并不是很大，并且会在后端的VIO紧耦合优化中被估计出来。
附图说明：
图1为视觉惯性系统框架图
图2为所提出方法运行MH05困难数据集捕获的图像
图3为实验图：运行结果的均方根误差对比
图4为实验图：所提方法与VINS-Mono在MH05困难数据集下的对比
图5为实验图：所提方法与VINS-Mono在V103困难数据集下的对比
图6为实验图：所提方法与VINS-Mono在room3-512-16数据集下的对比
图7为实验图：数据集运行结果的绝对姿态误差和均方根误差对比
具体实施方式：
1、同时优化外参数的视觉惯性里程计系统，其特征包括以下步骤：
第1，系统描述
第1.1，系统框架
所提出方法的系统框架，特别是视觉惯性对齐模块，如附图1所示。首先，通过运动恢复结构算法(SFM)获得两个连续相机帧之间的旋转和平移，该算法仅用于获得相机关键帧之间关系的。然后，采用IMU预积分算法来获得IMU两帧之间的差量，并在之前获得估计变量的基础上估算当前IMU关键帧的测量值。通过运动恢复结构和IMU预积分算法获得的相关关键帧的旋转
Figure BSA0000210024080000055
和
Figure BSA0000210024080000056
利用经典算法，使用收敛准则获得外部参数的旋转部分。其次，估计陀螺仪偏差、平移外参数、绝对尺度和重力向量，并重新纠正重力的方向。最后，每一帧的速度可以通过先前的状态估计计算得到。加速度计偏差通过后端VIO紧耦合优化进行补偿。
第1.2，IMU预积分
IMU的测量数据包括三轴角速度
Figure BSA0000210024080000057
和三轴线性加速度
Figure BSA0000210024080000058
偏差b和白噪声n影响加速度和角速度的精度；测量值和理想值之间的关系如下：
Figure BSA0000210024080000061
wb和ab分别为
Figure BSA0000210024080000062
和
Figure BSA0000210024080000063
的实际值，
Figure BSA0000210024080000064
和
Figure BSA0000210024080000065
分别为线性加速度和角速度的偏差。nw和na分别为wb和ab的高斯白噪声，服从高斯分布。
Figure BSA0000210024080000066
是世界坐标系
Figure BSA0000210024080000067
到IMU坐标系
Figure BSA0000210024080000068
的旋转矩阵，gw是世界坐标系
Figure BSA0000210024080000069
下的重力向量，所以
Figure BSA00002100240800000610
表示的是重力向量在IMU坐标系
Figure BSA00002100240800000611
下的值。
忽略噪声，IMU连续两帧
Figure BSA00002100240800000612
和
Figure BSA00002100240800000613
在时间间隔[tk，tk+1]内的位移、速度和旋转表示如下：
Figure BSA00002100240800000614
把公式(21)代入公式(22)，并从
Figure BSA00002100240800000615
变换到
Figure BSA00002100240800000616
得到IMU预积分公式如下：
Figure BSA00002100240800000617
公式(23)中，
Figure BSA00002100240800000618
和
Figure BSA00002100240800000619
分别为两帧之间的位移、速度旋转差量。由公式可以看出惯性测量单元的当前关键帧的测量值与之前帧无关，仅由
Figure BSA00002100240800000620
和
Figure BSA00002100240800000621
决定。
第2，估计旋转外参数
第2.1，运动恢复结构
视觉初始化使用运动恢复结构(SFM)方法估计滑动窗口内的所有关键帧的位姿。首先，将第一帧相机帧
Figure BSA00002100240800000622
作为第一个参考关键帧，然后选择一个与当前帧具有更多公共点和足够视差的帧作为第二个参考关键帧。通过五点法或八点法估算当前帧和参考关键帧之间的旋转和平移。然后，利用三角化和PnP算法以及捆集调整(BA)获得滑动窗口内所有关键帧的位姿。在此基础上，得到第k个和第k+1个摄像机关键帧之间的旋转
Figure BSA00002100240800000623
和缩放平移
Figure BSA00002100240800000624
第2.2，旋转外参数
根据坐标系之间的变换，可得到以下关系：
Figure BSA00002100240800000625
其中
Figure BSA00002100240800000626
由IMU陀螺仪在IMU坐标系
Figure BSA00002100240800000627
下积分得到，
Figure BSA00002100240800000628
通过视觉初始化得到，
Figure BSA00002100240800000629
是需要估计的旋转外参数矩阵。
把以上旋转矩阵用四元数表示得到以下公式：
Figure BSA0000210024080000071
将Q1和Q2定义为四元数的左乘和右乘，得到以下公式：
Figure BSA0000210024080000072
变换后得到：
Figure BSA0000210024080000073
随着关键帧数量的增加，得到以下超线性方程组：
Figure BSA0000210024080000074
其中N为关键帧数量的最大值，
Figure BSA0000210024080000075
为剔除重投影误差的权重，利用终止准则可以得到旋转外参数。
第3，视觉惯性初始化
第3.1，陀螺仪偏差校正
在相机坐标系
Figure BSA0000210024080000076
下，设置第一个参考关键帧后，通过SFM算法得到第k帧和参考关键帧之间的旋转
Figure BSA0000210024080000077
然后
Figure BSA0000210024080000078
可以由以下公式得到：
Figure BSA0000210024080000079
对于滑动窗口内的两个IMU关键帧
Figure BSA00002100240800000710
和
Figure BSA00002100240800000711
和
Figure BSA00002100240800000712
可由公式(29)得到，相对旋转
Figure BSA00002100240800000713
由公式(23)获得。因此陀螺仪偏差的目标方程表示如下：
Figure BSA00002100240800000714
其中：
Figure BSA00002100240800000715
因此，最小化公式(30)可以得到陀螺仪偏差bw。相对位移
Figure BSA00002100240800000716
和相对速度
Figure BSA00002100240800000717
根据即时计算得出的bw进行更新。
第3.2，平移外参数、绝对尺度和重力向量
通过以上分析，所有的参数都可以从IMU坐标系
Figure BSA00002100240800000718
转换到相机坐标系
Figure BSA00002100240800000719
下：
Figure BSA00002100240800000720
其中s是尺度因子，
Figure BSA00002100240800000721
是缩放的位移。绝对尺度和缩放尺度之间的关系如下：
Figure BSA0000210024080000081
所以(23)式可写成如下形式：
Figure BSA0000210024080000082
通过公式(31)(32)(33)，
Figure BSA0000210024080000083
和
Figure BSA0000210024080000084
可以被表示为：
Figure BSA0000210024080000085
假设关键帧的下标i，i+1，i+2分别为1，2，3，并把(35)式的两个式子放在一起得到：
Figure BSA0000210024080000086
其中：
Figure BSA0000210024080000087
通过公式(36)，利用最小二乘算法可得到平移外参数
Figure BSA0000210024080000088
重力向量
Figure BSA0000210024080000089
和绝对尺度s。
第3.3，重力方向矫正
估计出的重力向量受加速度计偏差的影响，假设重力向量的幅值是一个常量，重力向量的方向由于偏差而发生改变。因此，此时矫正重力向量可以设为两个自由度：
Figure BSA00002100240800000810
其中
Figure BSA00002100240800000811
为重力向量的幅值，w1b1和w2b2是分别来自两个正交方向的扰动。b为扰动的方向，w为对应的b方向的扰动量。
把公式(38)代入(36)得到：
Figure BSA00002100240800000812
其中：
Figure BSA0000210024080000091
通过求解公式(40)，重力向量被重新矫正。在此基础上，
Figure BSA0000210024080000092
和
Figure BSA0000210024080000093
坐标系下的所有状态变量都可以变换到世界坐标系
Figure BSA0000210024080000094
下。此外，平移和地图点的坐标可以根据得到的绝对尺度s进行缩放。
第3.4，重力方向矫正
重力和绝对尺度得到后，每一关键帧的速度可以由公式(22)和(35)计算得到。在视觉惯性初始化时没有估计加速度计偏差ba，因为它对初始化的影响并不是很大，并且会在后端的VIO紧耦合优化中被估计出来。
第4，实验和结论
第4.1，实验结果
通过实验结果来证明该方法的有效性。在VINS-Mono的启发性工作的基础上实施提出的方法。该方法通过两个公共基准数据集进行评估：EuRoc MAV数据集和TUM视觉惯性数据集。实验在一台计算机上实施，配有4核因特尔i7处理器，2.8GHZ主频，8GB运行内存，Ubuntu14.04和ROS Indigo。使用开源软件包Evo评估轨迹误差。
本实验与经典的VINS-Mono工程进行比较。图2显示了MH05困难数据集的实现过程，由于无人机运动快速且环境复杂，因此对VIO算法提出了很高的要求。运行结果的均方根误差(RMSE)如图3所示，数字为粗体的代表运行结果的性能较好。通过图2可以看出，与VINS-Mono相比，本方法在10个实验上具有更好的性能，其中3个等于VINS-Mono，1个不如VINS-Mono。
图4-6展示了本方法与VINS-Mono分别在MH05困难，V103困难和room3-512-16数据集下的对比结果。在这3个图中，粗虚线表示轨迹的理想值，点划线表示VINS-Mono的估计结果，实线表示所本方法的估计结果。可以看出本方法的估计结果相比VINS-Mono更接近于理想值。
此外，使用绝对姿态误差(APE)以获得更好的评估，它可以直接比较VIO估计结果与理想结果之间的轨迹误差。图7显示了以上三个数据集运行结果的绝对姿态误差和均方根误差对比。可以看出，该方法的估计误差比VINS-Mono的估计误差小。
需要注意的是，在VIO初始化阶段，我们将平移外参数添加到优化变量中，并通过先前的状态估计直接计算每个关键帧的速度。通过这些改进，相对于VINS-Mono，该方法降低了VIO初始化阶段的迭代和计算复杂度，因此可以获得更准确的结果。
第4.2，结论
提出了一种新颖的视觉惯性初始化方法，该方法可以在视觉惯性状态估计的同时优化相机和IMU外参数。获得旋转外在参数后，联合估算陀螺仪偏差，绝对尺度，重力向量和平移外参数，并通过先前的估计变量计算得出速度。加速度计偏差通过后端VIO紧耦合优化得到。对比实验结果表明，该方法具有良好的性能。

Claims (1)

1.同时优化外参数的视觉惯性里程计系统，其特征包括以下步骤：
第1，系统描述
第1.1，系统框架
所提出方法的系统框架，特别是视觉惯性对齐模块，如附图1所示；首先，通过运动恢复结构算法(SFM)获得两个连续相机帧之间的旋转和平移，该算法仅用于获得相机关键帧之间关系的；然后，采用IMU预积分算法来获得IMU两帧之间的差量，并在之前获得估计变量的基础上估算当前IMU关键帧的测量值；通过运动恢复结构和IMU预积分算法获得的相关关键帧的旋转
Figure FSA0000210024070000011
和
Figure FSA0000210024070000012
利用经典算法，使用收敛准则获得外部参数的旋转部分；其次，估计陀螺仪偏差、平移外参数、绝对尺度和重力向量，并重新纠正重力的方向；最后，每一帧的速度可以通过先前的状态估计计算得到；加速度计偏差通过后端VIO紧耦合优化进行补偿；
第1.2，IMU预积分
IMU的测量数据包括三轴角速度
Figure FSA0000210024070000013
和三轴线性加速度
Figure FSA0000210024070000014
偏差b和白噪声n影响加速度和角速度的精度；测量值和理想值之间的关系如下：
Figure FSA0000210024070000015
wb和ab分别为
Figure FSA0000210024070000016
和
Figure FSA0000210024070000017
的实际值，
Figure FSA0000210024070000018
和
Figure FSA0000210024070000019
分别为线性加速度和角速度的偏差；nw和na分别为wb和ab的高斯白噪声，服从高斯分布；
Figure FSA00002100240700000110
是世界坐标系
Figure FSA00002100240700000111
到IMU坐标系
Figure FSA00002100240700000112
的旋转矩阵，gw是世界坐标系
Figure FSA00002100240700000113
下的重力向量，所以
Figure FSA00002100240700000114
表示的是重力向量在IMU坐标系
Figure FSA00002100240700000115
下的值；
忽略噪声，IMU连续两帧
Figure FSA00002100240700000116
和
Figure FSA00002100240700000117
在时间间隔[tk，tk+1]内的位移、速度和旋转表示如下：
Figure FSA00002100240700000118
把公式(1)代入公式(2)，并从
Figure FSA00002100240700000119
变换到
Figure FSA00002100240700000120
得到IMU预积分公式如下：
Figure FSA00002100240700000121
公式(3)中，
Figure FSA00002100240700000122
和
Figure FSA00002100240700000123
分别为两帧之间的位移、速度旋转差量；由公式可以看出惯性测量单元的当前关键帧的测量值与之前帧无关，仅由
Figure FSA00002100240700000124
和
Figure FSA00002100240700000125
决定；
第2，估计旋转外参数
第2.1，运动恢复结构
视觉初始化使用运动恢复结构(SFM)方法估计滑动窗口内的所有关键帧的位姿；首先，将第一帧相机帧
Figure FSA0000210024070000021
作为第一个参考关键帧，然后选择一个与当前帧具有更多公共点和足够视差的帧作为第二个参考关键帧；通过五点法或八点法估算当前帧和参考关键帧之间的旋转和平移；然后，利用三角化和PnP算法以及捆集调整(BA)获得滑动窗口内所有关键帧的位姿；在此基础上，得到第k个和第k+1个摄像机关键帧之间的旋转
Figure FSA0000210024070000022
和缩放平移
Figure FSA0000210024070000023
第2.2，旋转外参数
根据坐标系之间的变换，可得到以下关系：
Figure FSA0000210024070000024
其中
Figure FSA0000210024070000025
由IMU陀螺仪在IMU坐标系
Figure FSA0000210024070000026
下积分得到，
Figure FSA0000210024070000027
通过视觉初始化得到，
Figure FSA0000210024070000028
是需要估计的旋转外参数矩阵；
把以上旋转矩阵用四元数表示得到以下公式：
Figure FSA0000210024070000029
将Q1和Q2定义为四元数的左乘和右乘，得到以下公式：
Figure FSA00002100240700000210
变换后得到：
Figure FSA00002100240700000211
随着关键帧数量的增加，得到以下超线性方程组：
Figure FSA00002100240700000212
其中N为关键帧数量的最大值，
Figure FSA00002100240700000213
为剔除重投影误差的权重，利用终止准则可以得到旋转外参数；
第3，视觉惯性初始化
第3.1，陀螺仪偏差校正
在相机坐标系
Figure FSA00002100240700000214
下，设置第一个参考关键帧后，通过SFM算法得到第k帧和参考关键帧之间的旋转
Figure FSA00002100240700000215
然后
Figure FSA00002100240700000216
可以由以下公式得到：
Figure FSA00002100240700000217
对于滑动窗口内的两个IMU关键帧
Figure FSA00002100240700000218
和
Figure FSA00002100240700000219
和
Figure FSA00002100240700000220
可由公式(9)得到，相对旋转
Figure FSA00002100240700000221
由公式(3)获得；因此陀螺仪偏差的目标方程表示如下：
Figure FSA00002100240700000222
其中：
Figure FSA0000210024070000031
因此，最小化公式(10)可以得到陀螺仪偏差bw；相对位移
Figure FSA0000210024070000032
和相对速度
Figure FSA0000210024070000033
根据即时计算得出的bw进行更新；
第3.2，平移外参数、绝对尺度和重力向量
通过以上分析，所有的参数都可以从IMU坐标系
Figure FSA0000210024070000034
转换到相机坐标系
Figure FSA0000210024070000035
下：
Figure FSA0000210024070000036
其中s是尺度因子，
Figure FSA0000210024070000037
是缩放的位移；绝对尺度和缩放尺度之间的关系如下：
Figure FSA0000210024070000038
所以(3)式可写成如下形式：
Figure FSA0000210024070000039
通过公式(11)(12)(13)，
Figure FSA00002100240700000310
和
Figure FSA00002100240700000311
可以被表示为：
Figure FSA00002100240700000312
假设关键帧的下标i，i+1，i+2分别为1，2，3，并把(15)式的两个式子放在一起得到：
Figure FSA00002100240700000313
其中：
Figure FSA00002100240700000314
通过公式(16)，利用最小二乘算法可得到平移外参数
Figure FSA00002100240700000315
重力向量
Figure FSA00002100240700000316
和绝对尺度s；
第3.3，重力方向矫正
估计出的重力向量受加速度计偏差的影响，假设重力向量的幅值是一个常量，重力向量的方向由于偏差而发生改变；因此，此时矫正重力向量可以设为两个自由度：
Figure FSA0000210024070000041
其中
Figure FSA0000210024070000042
为重力向量的幅值，w1b1和w2b2是分别来自两个正交方向的扰动；b为扰动的方向，w为对应的b方向的扰动量；
把公式(18)代入(16)得到：
Figure FSA0000210024070000043
其中：
Figure FSA0000210024070000044
通过求解公式(20)，重力向量被重新矫正；在此基础上，
Figure FSA0000210024070000045
和
Figure FSA0000210024070000046
坐标系下的所有状态变量都可以变换到世界坐标系
Figure FSA0000210024070000047
下；此外，平移和地图点的坐标可以根据得到的绝对尺度s进行缩放；
第3.4，重力方向矫正
重力和绝对尺度得到后，每一关键帧的速度可以由公式(2)和(15)计算得到；在视觉惯性初始化时没有估计加速度计偏差ba，因为它对初始化的影响并不是很大，并且会在后端的VIO紧耦合优化中被估计出来。
CN202010471312.6A 2020-05-29 2020-05-29 同时优化外参数的视觉惯性里程计系统 Active CN111578937B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010471312.6A CN111578937B (zh) 2020-05-29 2020-05-29 同时优化外参数的视觉惯性里程计系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010471312.6A CN111578937B (zh) 2020-05-29 2020-05-29 同时优化外参数的视觉惯性里程计系统

Publications (2)

Publication Number Publication Date
CN111578937A true CN111578937A (zh) 2020-08-25
CN111578937B CN111578937B (zh) 2024-01-09

Family

ID=72115841

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010471312.6A Active CN111578937B (zh) 2020-05-29 2020-05-29 同时优化外参数的视觉惯性里程计系统

Country Status (1)

Country Link
CN (1) CN111578937B (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112284381A (zh) * 2020-10-19 2021-01-29 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统
CN112556692A (zh) * 2020-11-27 2021-03-26 绍兴市北大信息技术科创中心 一种基于注意力机制的视觉和惯性里程计方法和系统
CN112649016A (zh) * 2020-12-09 2021-04-13 南昌大学 一种基于点线初始化的视觉惯性里程计方法
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113158459A (zh) * 2021-04-20 2021-07-23 浙江工业大学 一种基于视觉和惯性信息融合的人体姿态估计方法
CN113432595A (zh) * 2021-07-07 2021-09-24 北京三快在线科技有限公司 设备状态获取方法、装置、计算机设备及存储介质
CN114216455A (zh) * 2021-11-04 2022-03-22 天津工业大学 同时优化时间偏差的视觉惯性里程计系统
US11521332B1 (en) 2021-06-29 2022-12-06 Midea Group Co., Ltd. Method and apparatus for optimization of a monocular visual-inertial localization system
WO2023273311A1 (en) * 2021-06-29 2023-01-05 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014130854A1 (en) * 2013-02-21 2014-08-28 Regents Of The Univesity Of Minnesota Extrinsic parameter calibration of a vision-aided inertial navigation system
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110751123A (zh) * 2019-06-25 2020-02-04 北京机械设备研究所 一种单目视觉惯性里程计系统及方法
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2014130854A1 (en) * 2013-02-21 2014-08-28 Regents Of The Univesity Of Minnesota Extrinsic parameter calibration of a vision-aided inertial navigation system
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN110751123A (zh) * 2019-06-25 2020-02-04 北京机械设备研究所 一种单目视觉惯性里程计系统及方法
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
王帅;潘树国;黄砺枭;曾攀;: "单目视觉惯性定位的IMU辅助跟踪模型" *
程传奇;郝向阳;李建胜;刘智伟;胡鹏;: "基于非线性优化的单目视觉/惯性组合导航算法" *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112284381A (zh) * 2020-10-19 2021-01-29 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统
CN112284381B (zh) * 2020-10-19 2022-09-13 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统
CN112556692A (zh) * 2020-11-27 2021-03-26 绍兴市北大信息技术科创中心 一种基于注意力机制的视觉和惯性里程计方法和系统
CN112649016A (zh) * 2020-12-09 2021-04-13 南昌大学 一种基于点线初始化的视觉惯性里程计方法
CN112649016B (zh) * 2020-12-09 2023-10-03 南昌大学 一种基于点线初始化的视觉惯性里程计方法
CN112862768B (zh) * 2021-01-28 2022-08-02 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN112862768A (zh) * 2021-01-28 2021-05-28 重庆邮电大学 一种基于点线特征的自适应单目vio初始化方法
CN113158459A (zh) * 2021-04-20 2021-07-23 浙江工业大学 一种基于视觉和惯性信息融合的人体姿态估计方法
CN113077515B (zh) * 2021-06-07 2021-09-21 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
CN113077515A (zh) * 2021-06-07 2021-07-06 之江实验室 一种水下视觉惯导压力定位的紧耦合初始化方法
US11521332B1 (en) 2021-06-29 2022-12-06 Midea Group Co., Ltd. Method and apparatus for optimization of a monocular visual-inertial localization system
WO2023273311A1 (en) * 2021-06-29 2023-01-05 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system
US11756231B2 (en) 2021-06-29 2023-09-12 Midea Group Co., Ltd. Method and apparatus for scale calibration and optimization of a monocular visual-inertial localization system
CN113432595A (zh) * 2021-07-07 2021-09-24 北京三快在线科技有限公司 设备状态获取方法、装置、计算机设备及存储介质
CN114216455A (zh) * 2021-11-04 2022-03-22 天津工业大学 同时优化时间偏差的视觉惯性里程计系统

Also Published As

Publication number Publication date
CN111578937B (zh) 2024-01-09

Similar Documents

Publication Publication Date Title
CN111578937A (zh) 同时优化外参数的视觉惯性里程计系统
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
Weiss et al. Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN105931275A (zh) 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN112240768A (zh) 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN110595466B (zh) 轻量级的基于深度学习的惯性辅助视觉里程计实现方法
Liu et al. Towards robust visual odometry with a multi-camera system
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
US12073630B2 (en) Moving object tracking method and apparatus
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
US20190301871A1 (en) Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization
CN110520694A (zh) 一种视觉里程计及其实现方法
CN110533719B (zh) 基于环境视觉特征点识别技术的增强现实定位方法及装置
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN108958232A (zh) 一种基于深度视觉的移动扫地机器人slam装置及算法
Schauwecker et al. On-board dual-stereo-vision for autonomous quadrotor navigation
CN107145167B (zh) 一种基于数字图像处理技术的视频目标跟踪方法
CN115046545A (zh) 一种深度网络与滤波结合的定位方法
CN113503873A (zh) 一种多传感器融合的视觉定位方法
CN114440877B (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
TA01 Transfer of patent application right

Effective date of registration: 20231213

Address after: 200000 No. 2, Lane 585, Longhao Road, Shanyang Town, Jinshan District, Shanghai (Jinshan Capital Group South Economic Park)

Applicant after: Shanghai Xintiance Digital Technology Co.,Ltd.

Address before: 518000 1104, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Applicant before: Shenzhen Hongyue Information Technology Co.,Ltd.

Effective date of registration: 20231213

Address after: 518000 1104, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Applicant after: Shenzhen Hongyue Information Technology Co.,Ltd.

Address before: No. 399 Bingshui Road, Xiqing District, Tianjin, Tianjin

Applicant before: TIANJIN POLYTECHNIC University

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address

Address after: Room 209, Building 2, No. 1428 Haijie Road, Lingang New Area, China (Shanghai) Pilot Free Trade Zone, Pudong New Area, Shanghai, June 2013

Patentee after: Shanghai Xintiance Digital Technology Co.,Ltd.

Country or region after: China

Address before: 200000 No. 2, Lane 585, Longhao Road, Shanyang Town, Jinshan District, Shanghai (Jinshan Capital Group South Economic Park)

Patentee before: Shanghai Xintiance Digital Technology Co.,Ltd.

Country or region before: China

CP03 Change of name, title or address
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: Simultaneously optimizing the external parameters of the visual inertial odometry system

Granted publication date: 20240109

Pledgee: Bank of Nanjing Limited by Share Ltd. Shanghai branch

Pledgor: Shanghai Xintiance Digital Technology Co.,Ltd.

Registration number: Y2024310000781

PE01 Entry into force of the registration of the contract for pledge of patent right