CN112985450A - 一种具有同步时间误差估计的双目视觉惯性里程计方法 - Google Patents

一种具有同步时间误差估计的双目视觉惯性里程计方法 Download PDF

Info

Publication number
CN112985450A
CN112985450A CN202110176634.2A CN202110176634A CN112985450A CN 112985450 A CN112985450 A CN 112985450A CN 202110176634 A CN202110176634 A CN 202110176634A CN 112985450 A CN112985450 A CN 112985450A
Authority
CN
China
Prior art keywords
imu
camera
state vector
coordinate system
error
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
CN202110176634.2A
Other languages
English (en)
Other versions
CN112985450B (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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202110176634.2A priority Critical patent/CN112985450B/zh
Publication of CN112985450A publication Critical patent/CN112985450A/zh
Application granted granted Critical
Publication of CN112985450B publication Critical patent/CN112985450B/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
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Landscapes

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

Abstract

本发明公开了一种具有同步时间误差估计的双目视觉惯性里程计方法，包括如下步骤：获取在机体坐标系下的相机图像和IMU的数据；将双目相机外参和同步时间误差加入到状态向量中，构建状态向量和误差向量；进行特征点提取、双目特征匹配跟踪以及剔除外点操作；计算两帧之间带时间偏移量的IMU预积分值；当相机采集到一次新的视觉图像时，增广该状态向量及协方差矩阵；建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。本发明能有效地提高视觉和惯导滤波融合下的里程计定位精度。

Description

一种具有同步时间误差估计的双目视觉惯性里程计方法
技术领域
本发明涉及一种具有同步时间误差估计的双目视觉惯性里程计方法，属于多传感器数据融合方法技术领域。
背景技术
状态估计在过去几十年里一直是机器人和计算机视觉领域的一个基本研究课题。机器人导航、自动驾驶、虚拟现实(VR)和增强现实(AR)等各种应用都高度依赖于精确的状态估计。为了解决众所周知的尺度模糊问题，多传感器融合方法受到越来越多的关注。借助IMU(惯性测量单元)辅助摄像机，在六自由度SLAM(同步定位与测绘)中取得了令人瞩目的性能。一方面，惯性测量使得俯仰和滚转角以及比例尺可见。另一方面，惯性测量通过弥补视觉跟踪失败时的间隙，改善了运动跟踪性能。为了融合来自不同传感器的数据，必须精确地知道测量记录的时间间隔。
实践中，每个传感器的时间戳通常会受到触发和传输延迟的影响，从而导致不同传感器流之间的时间错位(时间偏移)。因此，传感器的时间同步可能成为多传感器***的一个关键问题。对于视觉惯性***，摄像机与IMU之间的时间偏移严重影响鲁棒性和精度。由于触发延迟和传输延迟，相机和IMU之间总是存在时间失调(时间偏移)。时间偏移通常从几毫秒到几百毫秒不等。数十毫秒将导致IMU序列与图像流完全错位，从而极大地影响视觉惯性***的性能。
发明内容
为解决上述问题，本发明公开了一种具有同步时间误差估计的双目视觉惯性里程计方法，能有效地提高视觉惯性里程计在定位方面的精度。
为达到上述目的，本发明的技术方案如下：
一种具有同步时间误差估计的双目视觉惯性里程计方法，包括如下步骤：
S1：视觉惯性里程计***坐标系定义；
S2：构建状态向量和误差状态向量；
S3：相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据；
S4：当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新；
S5：计算两帧之间带时间偏移量的IMU预积分值；
S6：当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵；
S7：建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。
优选的，所述步骤S1具体包括以下过程：
视觉惯性里程计***坐标系定义：
以初始时刻相机所在位置为世界坐标系的原点，定义载体“前右下”为世界坐标系(G系)的XYZ坐标轴；定义固连在载体上的IMU坐标系为I系，是IMU加速度计、角速度计的测量坐标系；定义固连在载体上的相机坐标系为C系；定义载体坐标系为B系，载体系与IMU坐标系(I系)重合。
优选的，所述步骤S2具体包括以下过程：
构建状态向量和误差状态向量：
定义加入双目外参和同步时间误差的IMU状态向量为
Figure BDA0002940896070000021
其中上标T表示向量转置，XIMU为IMU状态向量，即为
Figure BDA0002940896070000022
其中单位四元数
Figure BDA0002940896070000023
表示从世界坐标系到IMU坐标系的相对旋转，
Figure BDA0002940896070000024
和
Figure BDA0002940896070000025
分别表示IMU在世界坐标系的速度和位置信息，bg T和
Figure BDA0002940896070000026
分别表示IMU的陀螺仪和加速度计的随机游走；
Figure BDA0002940896070000027
和
Figure BDA0002940896070000028
分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；td为时间同步误差，td＝tC-tI，其中tC和tI分别为由于曝光，数据传输需要时间等问题而导致的收到相机和IMU数据延迟的时间。增广之后的IMU误差状态向量为
Figure BDA0002940896070000029
优选的，所述步骤S3具体包括以下过程：
相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作：
相机根据相机的采样频率采集双目视觉图像；IMU的加速度计和陀螺仪以IMU的采样频率采集加速度和角速度信息。对获得的视觉图像进行特征提取和特征匹配，并且利用RANSAC算法筛选出前后帧图像匹配的外点，并且利用网格均匀化提取出的特征，得到特征点在图像平面的二维观测，记为
Figure BDA00029408960700000210
为特征点fj在相机Ci的双目观测，其中u和ν分别表示特征点在图像平面上横纵坐标。
优选的，所述步骤S4具体包括以下过程：
当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新：
S4.1列写IMU状态向量
Figure BDA0002940896070000031
的动力学方程；
S4.2为简化模型，假设时间同步误差td、
Figure BDA0002940896070000032
和
Figure BDA0002940896070000033
为非时变量，则td的状态方程为
Figure BDA0002940896070000034
S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程
Figure BDA0002940896070000035
其中F和G分别为状态转移矩阵和噪声输入矩阵，nIMU为噪声序列；
,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量
Figure BDA0002940896070000036
进行预测得到当前IMU的误差状态向量；
Figure BDA0002940896070000037
中的其他变量保持不变；
S4.5根据步骤S4.3得到的近似线性化方程
Figure BDA0002940896070000038
计算协方差矩阵P。
优选的，所述步骤S5具体包括以下过程：
计算两帧之间带时间偏移量的IMU预积分值。
相机的采样频率明显低于IMU的采样频率，采用IMU预积分的方式，利用IMU的加速度计和陀螺仪的数据，再根据前一时刻的预积分量就可以更新当前时间点处的预积分量，从而得到两帧图像之间的IMU的观测，求得IMU预测到t+td时刻的值，求解出t+td时刻左目相机的位姿状态；
优选的，所述步骤S6具体包括以下过程：
当相机采集到一次新的视觉图像时，增广该状态向量及协方差矩阵：
S6.1对于双目惯性里程计，只将左目相机在t+td时刻的状态
Figure BDA0002940896070000039
加入到状态向量中，得到增广之后的误差状态向量
Figure BDA00029408960700000310
根据双目外参可以算出相机状态对应的右目相机位姿；
S6.2计算加入左目相机状态之后的协方差矩阵
Figure BDA00029408960700000311
为
Figure BDA00029408960700000312
其中Jnew＝[JI Jt 0]，其中Jnew是
Figure BDA00029408960700000313
对状态向量的一阶偏导数形成的雅克比矩阵；JI表示
Figure BDA00029408960700000314
对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即
Figure BDA00029408960700000315
其中，
Figure BDA00029408960700000316
为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；
Figure BDA00029408960700000317
为相机在IMU坐标系的位移向量的反对称矩阵。
Jt表示
Figure BDA0002940896070000041
对td的一阶偏导数形成的雅克比矩阵，即
Figure BDA0002940896070000042
其中，
Figure BDA0002940896070000043
为IMU在时间t+td的旋转角速度向量，
Figure BDA0002940896070000044
为其反对称矩阵；
Figure BDA0002940896070000045
为在世界坐标系的速度；
Figure BDA0002940896070000046
为IMU坐标系和世界坐标系之间的旋转矩阵；
优选的，所述步骤S7具体包括以下过程：
建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。
S7.1观测模型为
Figure BDA0002940896070000047
Figure BDA0002940896070000048
Figure BDA0002940896070000049
其中，
Figure BDA00029408960700000410
表示第i个相机观测到第j个特征点的像素坐标，
Figure BDA00029408960700000411
分别为第j个特征点在第i个左目相机的三维坐标；
S7.2单个特征对单相机残差模型为：
Figure BDA00029408960700000412
其中ri (j)表示第i个相机对第j个特征点的观测残差，
Figure BDA00029408960700000413
为误差状态向量
Figure BDA00029408960700000414
的雅克比矩阵，
Figure BDA00029408960700000415
为第j个特征点的雅克比矩阵：
Figure BDA00029408960700000416
Figure BDA00029408960700000417
其中
Figure BDA00029408960700000418
为表示第i个左目相机观测到第j个特征点的像素坐标；
Figure BDA00029408960700000419
为表示第i个右目相机观测到第j个特征点的像素坐标；Gpj为第j个特征点在世界坐标系的位置信息；
其中
Figure BDA00029408960700000420
为第i个相机状态的雅克比矩阵:
Figure BDA0002940896070000051
其中，各偏导项公式如下：
Figure BDA0002940896070000052
Figure BDA0002940896070000053
Figure BDA0002940896070000054
Figure BDA0002940896070000055
Figure BDA0002940896070000056
Figure BDA0002940896070000057
S7.3将所有特征点的观测模型合并，得到
Figure BDA0002940896070000058
其中r0表示合并之后的观测残差，HX表示合并之后的雅克比矩阵，n0表示合并之后的噪声序列；
然后对HX进行QR分解来减少观测模型的规模，得：
Figure BDA0002940896070000059
其中TH为QR分解之后的对误差状态向量的雅克比矩阵，nn为QR分解之后的噪声序列；
S7.4若不满足状态更新条件，则转到步骤S.4；若满足更新条件，则直接套用扩展卡尔曼滤波更新公式：
K＝PHT(HPHT+R)T
ΔX＝Krn
PK+1/K+1＝(I+KTH)PK+1/K(I+KTH)T+KRKT
其中，K表示卡尔曼增益，ΔX表示***状态更新量，PK+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵；
本发明的有益效果是：
本发明将双目相机之间的外参和相机与IMU之间的时间同步误差加入到误差状态向量中进行估计，提高了视觉惯性里程计的精度。
附图说明
图1为本发明的具有同步时间误差估计的双目视觉惯性里程计方法原理图。
具体实施方式
下面结合附图和具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
实施例1：本发明所述的一种具有同步时间误差估计的双目视觉惯性里程计方法，实现原理如图1所示，实验流程如图1所示。其流程主要包括以下步骤：
步骤S1，视觉惯性里程计***坐标系定义。
以初始时刻相机所在位置为世界坐标系的原点，定义载体“前右下”为世界坐标系(G系)的XYZ坐标轴；定义固连在载体上的IMU坐标系为I系，是IMU加速度计、角速度计的测量坐标系；定义固连在载体上的相机坐标系为C系；定义载体坐标系为B系，载体系与IMU坐标系(I系)重合。
步骤S2，构建状态向量和误差状态向量。
定义加入双目外参和同步时间误差的IMU状态向量为
Figure BDA0002940896070000061
其中上标T表示向量转置，XIMU为IMU状态向量，即为
Figure BDA0002940896070000062
其中单位四元数
Figure BDA0002940896070000063
表示从世界坐标系到IMU坐标系的相对旋转，
Figure BDA0002940896070000064
和
Figure BDA0002940896070000065
分别表示IMU在世界坐标系的速度和位置信息，bg T和
Figure BDA0002940896070000066
分别表示IMU的陀螺仪和加速度计的随机游走；
Figure BDA0002940896070000067
和
Figure BDA0002940896070000068
分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；td为时间同步误差，td＝tC-tI，其中tC和tI分别为由于曝光，数据传输需要时间等问题而导致的收到相机和IMU数据延迟的时间。增广之后的IMU误差状态向量为
Figure BDA0002940896070000069
步骤S3，相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据。
相机和惯性测量单元(IMU)分别获得图像和加速度，角速度数据，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作。
相机根据相机的采样频率采集双目视觉图像；IMU的加速度计和陀螺仪以IMU的采样频率采集加速度和角速度信息。对获得的视觉图像进行特征提取和特征匹配，并且利用RANSAC算法筛选出前后帧图像匹配的外点，并且利用网格均匀化提取出的特征，得到特征点在图像平面的二维观测，记为
Figure BDA0002940896070000071
为特征点fj在相机Ci的双目观测，其中u和ν分别表示特征点在图像平面上横纵坐标。
步骤S4，当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新。
当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新。
S4.1列写IMU状态向量XIMU的动力学方程；
S4.2为简化模型，假设时间同步误差td、
Figure BDA0002940896070000072
和
Figure BDA0002940896070000073
为非时变量，则td的状态方程为
Figure BDA0002940896070000074
S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程
Figure BDA0002940896070000075
其中F和G分别为状态转移矩阵和噪声输入矩阵，
Figure BDA0002940896070000076
为噪声序列；
,S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量
Figure BDA0002940896070000077
进行预测得到当前IMU的误差状态向量；
Figure BDA0002940896070000078
中的其他变量保持不变；
S4.5根据步骤S4.3得到的近似线性化方程
Figure BDA0002940896070000079
计算协方差矩阵P；
步骤S5，计算两帧之间带时间偏移量的IMU预积分值。
相机的采样频率明显低于IMU的采样频率，采用IMU预积分的方式，利用IMU的加速度计和陀螺仪的数据，再根据前一时刻的预积分量就可以更新当前时间点处的预积分量，从而得到两帧图像之间的IMU的观测，求得IMU预测到t+td时刻的值，求解出t+td时刻左目相机的位姿状态；
步骤S6，当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵：
S6.1对于双目惯性里程计，只将左目相机在t+td时刻的状态
Figure BDA00029408960700000710
加入到状态向量中，得到增广之后的误差状态向量
Figure BDA00029408960700000711
根据双目外参可以算出相机状态对应的右目相机位姿；
S6.2计算加入左目相机状态之后的协方差矩阵
Figure BDA0002940896070000081
为
Figure BDA0002940896070000082
其中Jnew＝[JI Jt 0]，其中Jnew是
Figure BDA0002940896070000083
对状态向量的一阶偏导数形成的雅克比矩阵；JI表示
Figure BDA0002940896070000084
对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即
Figure BDA0002940896070000085
其中，
Figure BDA0002940896070000086
为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；
Figure BDA0002940896070000087
为相机在IMU坐标系的位移向量的反对称矩阵。
Jt表示
Figure BDA0002940896070000088
对td的一阶偏导数形成的雅克比矩阵，即
Figure BDA0002940896070000089
其中，
Figure BDA00029408960700000810
为IMU在时间t+td的旋转角速度向量，
Figure BDA00029408960700000811
为其反对称矩阵；
Figure BDA00029408960700000812
为在世界坐标系的速度；
Figure BDA00029408960700000813
为IMU坐标系和世界坐标系之间的旋转矩阵；
步骤S7，建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。
S7.1观测模型为
Figure BDA00029408960700000814
Figure BDA00029408960700000815
Figure BDA00029408960700000816
其中，
Figure BDA00029408960700000817
表示第i个相机观测到第j个特征点的像素坐标，
Figure BDA00029408960700000818
分别为第j个特征点在第i个左目相机的三维坐标；
S7.2单个特征对单相机残差模型为：
Figure BDA00029408960700000819
其中ri (j)表示第i个相机对第j个特征点的观测残差，
Figure BDA00029408960700000820
为误差状态向量
Figure BDA00029408960700000821
的雅克比矩阵，
Figure BDA00029408960700000822
为第j个特征点的雅克比矩阵：
Figure BDA0002940896070000091
Figure BDA0002940896070000092
其中
Figure BDA0002940896070000093
为表示第i个左目相机观测到第j个特征点的像素坐标；
Figure BDA0002940896070000094
为表示第i个右目相机观测到第j个特征点的像素坐标；Gpj为第j个特征点在世界坐标系的位置信息；
其中
Figure BDA0002940896070000095
为第i个相机状态的雅克比矩阵:
Figure BDA0002940896070000096
其中各偏导项公式如下：
Figure BDA0002940896070000097
Figure BDA0002940896070000098
Figure BDA0002940896070000099
Figure BDA00029408960700000910
Figure BDA00029408960700000911
Figure BDA00029408960700000912
S7.3将所有特征点的观测模型合并，得到
Figure BDA0002940896070000101
其中r0表示合并之后的观测残差，HX表示合并之后的雅克比矩阵，n0表示合并之后的噪声序列；
然后对HX进行QR分解来减少观测模型的规模，得：
Figure BDA0002940896070000102
其中TH为QR分解之后的对误差状态向量的雅克比矩阵，nn为QR分解之后的噪声序列；
S7.4若不满足状态更新条件，则转到步骤S.4；若满足更新条件，则直接套用扩展卡尔曼滤波更新公式：
K＝PHT(HPHT+R)T
ΔX＝Krn
PK+1/K+1＝(I+KTH)PK+1/K(I+KTH)T+KRKT
其中，K表示卡尔曼增益，ΔX表示***状态更新量，PK+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。

Claims (8)

1.一种具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，包括以下步骤：
S1：视觉惯性里程计***坐标系定义；
S2：构建状态向量和误差状态向量；
S3：相机和惯性测量单元即IMU分别获得图像和加速度、角速度数据；
S4：当IMU采集到加速度计和陀螺仪数据的时候，对视觉惯性里程计的误差状态向量和协方差矩阵进行更新；
S5：计算两帧之间带时间偏移量的IMU预积分值；
S6：当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵；
S7：建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。
2.根据权利要求1所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S1具体包括以下过程：
视觉惯性里程计***坐标系定义：
以初始时刻相机所在位置为世界坐标系的原点，定义载体“前右下”为世界坐标系即G系的XYZ坐标轴；定义固连在载体上的IMU坐标系为I系，是IMU加速度计、角速度计的测量坐标系；定义固连在载体上的相机坐标系为C系；定义载体坐标系为B系，载体系与IMU坐标系即I系重合。
3.根据权利要求1所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S2具体包括以下过程：
构建状态向量和误差状态向量；
定义加入双目外参和同步时间误差的IMU状态向量为
Figure FDA0002940896060000011
其中上标T表示向量转置，XIMU为IMU状态向量，即为
Figure FDA0002940896060000012
其中单位四元数
Figure FDA0002940896060000013
表示从世界坐标系到IMU坐标系的相对旋转，
Figure FDA0002940896060000014
和
Figure FDA0002940896060000015
分别表示IMU在世界坐标系的速度和位置信息，bg T和
Figure FDA0002940896060000016
分别表示IMU的陀螺仪和加速度计的随机游走；
Figure FDA0002940896060000017
和
Figure FDA0002940896060000018
分别表示左目相机坐标系和右目相机坐标系之间的相对旋转变换和相对位置变换；td为时间同步误差，td＝tC-tI，其中tC和tI分别为由于曝光，数据传输需要时间等问题而导致的收到相机和IMU数据延迟的时间，增广之后的IMU误差状态向量为
Figure FDA0002940896060000019
其中向量都为上述向量的误差向量。
4.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S3具体包括以下过程：
相机和惯性测量单元IMU分别获得图像和加速度，角速度数据，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，
相机根据相机的采样频率采集双目视觉图像；IMU的加速度计和陀螺仪以IMU的采样频率采集加速度和角速度信息，对获得的视觉图像进行特征提取和特征匹配，并且利用RANSAC算法筛选出前后帧图像匹配的外点，并且利用网格均匀化提取出的特征，得到特征点在图像平面的二维观测，记为
Figure FDA0002940896060000021
为特征点fj在相机Ci的双目观测，其中u和ν分别表示特征点在图像平面上横纵坐标。
5.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S4具体包括以下过程：
当IMU采集到加速度计和陀螺仪数据的时候，就对视觉惯性里程计的误差状态向量和协方差矩阵进行更新；
S4.1列写IMU状态向量
Figure FDA0002940896060000022
的动力学方程；
S4.2为简化模型，假设时间同步误差td、左目和右目相机之间的相对旋转
Figure FDA0002940896060000023
和左目和右目之间的相对位移
Figure FDA0002940896060000024
为非时变量，则td的状态方程为
Figure FDA0002940896060000025
S4.3根据步骤S4.1和S4.2所列写的方程，列写近似线性化方程
Figure FDA0002940896060000026
其中F和G分别为状态转移矩阵和噪声输入矩阵，
Figure FDA0002940896060000027
为噪声序列；
S4.4利用测量得到的IMU的加速度计和陀螺仪数据，对IMU误差状态向量
Figure FDA0002940896060000028
进行预测得到当前IMU的误差状态向量；
Figure FDA0002940896060000029
中的其他变量保持不变；
S4.5根据步骤S4.3得到的近似线性化方程
Figure FDA00029408960600000210
计算协方差矩阵P。
6.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S5具体包括以下过程：
计算两帧之间带时间偏移量的IMU预积分值，
相机的采样频率明显低于IMU的采样频率，采用IMU预积分的方式，利用IMU的加速度计和陀螺仪的数据，再根据前一时刻的预积分量就可以更新当前时间点处的预积分量，从而得到两帧图像之间的IMU的观测，求得IMU预测到t+td时刻的值，求解出t+td时刻左目相机的位姿状态。
7.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S6具体包括以下过程：
步骤S6当相机采集到一次新的视觉图像时，对图像进行特征点提取、双目特征匹配跟踪以及剔除外点操作，增广该状态向量及协方差矩阵：
S6.1对于双目惯性里程计，只将左目相机在t+td时刻的状态
Figure FDA0002940896060000031
加入到状态向量中，得到增广之后的误差状态向量
Figure FDA0002940896060000032
根据双目外参可以算出相机状态对应的右目相机位姿；
S6.2计算加入左目相机状态之后的协方差矩阵
Figure FDA0002940896060000033
为
Figure FDA0002940896060000034
其中Jnew＝[JI Jt 0]，其中Jnew是
Figure FDA0002940896060000035
对状态向量的一阶偏导数形成的雅克比矩阵；JI表示
Figure FDA0002940896060000036
对IMU误差状态向量的一阶偏导数形成的雅克比矩阵，即
Figure FDA0002940896060000037
其中，
Figure FDA0002940896060000038
为将相机和IMU之间相对旋转四元数转化为的旋转矩阵；
Figure FDA0002940896060000039
为相机在IMU坐标系的位移向量的反对称矩阵；
Jt表示
Figure FDA00029408960600000310
对td的一阶偏导数形成的雅克比矩阵，即
Figure FDA00029408960600000311
其中，
Figure FDA00029408960600000312
为IMU在时间t+td的旋转角速度向量，
Figure FDA00029408960600000313
为其反对称矩阵；
Figure FDA00029408960600000314
为在世界坐标系的速度；
Figure FDA00029408960600000315
为IMU坐标系和世界坐标系之间的旋转矩阵。
8.根据权利要求2所述的具有同步时间误差估计的双目视觉惯性里程计方法，其特征在于，所述步骤S7具体包括以下过程：
建立新的残差模型，在满足更新条件的时候，对状态向量及协方差矩阵进行更新。
S7.1观测模型为
Figure FDA0002940896060000041
Figure FDA0002940896060000042
Figure FDA0002940896060000043
其中，
Figure FDA0002940896060000044
表示第i个相机观测到第j个特征点的像素坐标，
Figure FDA0002940896060000045
分别为第j个特征点在第i个左目相机的三维坐标；S7.2单个特征对单相机残差模型为：
Figure FDA0002940896060000046
其中
Figure FDA0002940896060000047
表示第i个相机对第j个特征点的观测残差，
Figure FDA0002940896060000048
为误差状态向量
Figure FDA0002940896060000049
的雅克比矩阵，
Figure FDA00029408960600000410
为第j个特征点的雅克比矩阵：
Figure FDA00029408960600000411
Figure FDA00029408960600000412
其中
Figure FDA00029408960600000413
为表示第i个左目相机观测到第j个特征点的像素坐标；
Figure FDA00029408960600000414
为表示第i个右目相机观测到第j个特征点的像素坐标；Gpj为第j个特征点在世界坐标系的位置信息；其中
Figure FDA00029408960600000415
为第i个相机状态的雅克比矩阵:
Figure FDA00029408960600000416
其中各偏导项公式如下：
Figure FDA0002940896060000051
Figure FDA0002940896060000052
Figure FDA0002940896060000053
Figure FDA0002940896060000054
Figure FDA0002940896060000055
Figure FDA0002940896060000056
S7.3将所有特征点的观测模型合并，得到
Figure FDA0002940896060000057
其中r0表示合并之后的观测残差，HX表示合并之后的雅克比矩阵，n0表示合并之后的噪声序列；
然后对HX进行QR分解来减少观测模型的规模，得：
Figure FDA0002940896060000058
其中TH为QR分解之后的对误差状态向量的雅克比矩阵，nn为QR分解之后的噪声序列；
S7.4若不满足状态更新条件，则转到步骤S.4；若满足更新条件，则直接套用扩展卡尔曼滤波更新公式：
K＝PHT(HPHT+R)T
ΔX＝Krn
PK+1/K+1＝(I+KTH)PK+1/K(I+KTH)T+KRKT
其中，K表示卡尔曼增益，ΔX表示***状态更新量，PK+1/K+1表示k+1时刻协方差矩阵，R表示噪声协方差矩阵。
CN202110176634.2A 2021-02-09 2021-02-09 一种具有同步时间误差估计的双目视觉惯性里程计方法 Active CN112985450B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110176634.2A CN112985450B (zh) 2021-02-09 2021-02-09 一种具有同步时间误差估计的双目视觉惯性里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110176634.2A CN112985450B (zh) 2021-02-09 2021-02-09 一种具有同步时间误差估计的双目视觉惯性里程计方法

Publications (2)

Publication Number Publication Date
CN112985450A true CN112985450A (zh) 2021-06-18
CN112985450B CN112985450B (zh) 2024-05-14

Family

ID=76392447

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110176634.2A Active CN112985450B (zh) 2021-02-09 2021-02-09 一种具有同步时间误差估计的双目视觉惯性里程计方法

Country Status (1)

Country Link
CN (1) CN112985450B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113639743A (zh) * 2021-06-29 2021-11-12 北京航空航天大学 一种基于行人步长信息辅助的视觉惯性slam定位方法
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN115615421A (zh) * 2022-10-21 2023-01-17 湖南大学 一种基于双目视觉惯性里程计的无人机定位方法及***

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110455309A (zh) * 2019-08-27 2019-11-15 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110455309A (zh) * 2019-08-27 2019-11-15 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110986939A (zh) * 2020-01-02 2020-04-10 东南大学 一种基于imu预积分的视觉惯性里程计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
MINGYANG LI: "3-D Motion Estimation and Online Temporal Calibration for Camera-IMU Systems", 2013 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA), pages 5709 - 5716 *
施佳良: "双目视觉与惯性测量单元相对姿态在线标定", 无线互联科技, no. 24, pages 121 - 131 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113639743A (zh) * 2021-06-29 2021-11-12 北京航空航天大学 一种基于行人步长信息辅助的视觉惯性slam定位方法
CN113639743B (zh) * 2021-06-29 2023-10-17 北京航空航天大学 一种基于行人步长信息辅助的视觉惯性slam定位方法
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN115615421A (zh) * 2022-10-21 2023-01-17 湖南大学 一种基于双目视觉惯性里程计的无人机定位方法及***

Also Published As

Publication number Publication date
CN112985450B (zh) 2024-05-14

Similar Documents

Publication Publication Date Title
CN109376785B (zh) 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112985450B (zh) 一种具有同步时间误差估计的双目视觉惯性里程计方法
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN108627153B (zh) 一种基于惯性传感器的刚体运动追踪***及其工作方法
CN110986939B (zh) 一种基于imu预积分的视觉惯性里程计方法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN107255476A (zh) 一种基于惯性数据和视觉特征的室内定位方法和装置
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN111723624B (zh) 一种头部运动跟踪方法和***
CN112747750B (zh) 一种基于单目视觉里程计和imu融合的定位方法
CN114719843B (zh) 复杂环境下的高精度定位方法
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN114419109B (zh) 一种基于视觉和气压信息融合的飞行器定位方法
CN112284381B (zh) 视觉惯性实时初始化对准方法及***
CN112284379B (zh) 一种基于非线性积分补偿的组合运动测量***的惯性预积分方法
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
CN114993306B (zh) 一种尺度自恢复的视觉惯性组合导航方法和装置
CN116167919A (zh) 一种基于核岭回归的激光点云数据去畸变方法
CN115239758A (zh) 时间戳校正方法、装置、设备、介质及计算机程序产品
CN115290076A (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
GR01 Patent grant
GR01 Patent grant