CN110986939B - 一种基于imu预积分的视觉惯性里程计方法 - Google Patents

一种基于imu预积分的视觉惯性里程计方法 Download PDF

Info

Publication number
CN110986939B
CN110986939B CN202010007374.1A CN202010007374A CN110986939B CN 110986939 B CN110986939 B CN 110986939B CN 202010007374 A CN202010007374 A CN 202010007374A CN 110986939 B CN110986939 B CN 110986939B
Authority
CN
China
Prior art keywords
imu
representing
camera
follows
equation
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.)
Active
Application number
CN202010007374.1A
Other languages
English (en)
Other versions
CN110986939A (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 CN202010007374.1A priority Critical patent/CN110986939B/zh
Publication of CN110986939A publication Critical patent/CN110986939A/zh
Application granted granted Critical
Publication of CN110986939B publication Critical patent/CN110986939B/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
    • 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

Abstract

本发明公开了一种基于IMU预积分的视觉惯性里程计方法，包括如下步骤：获取载体在固定坐标系下的视觉和IMU传感器数据；构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；对系统状态向量增广，并对系统协方差矩阵增广；建立相机传感器的测量模型；由相机观测构建系统观测模型；执行系统滤波更新得到载体位姿。本发明可以提高滤波方式下的视觉惯性里程计定位精度。

Description

一种基于IMU预积分的视觉惯性里程计方法
一种基于IMU预积分的视觉惯性里程计方法
技术领域
本发明涉及运动载体导航的多传感器数据融合技术领域，尤其是一种基于IMU预积分的视觉惯性里程计方法。
背景技术
即时定位与同步构图(simultaneouslocalization and mapping)一直被认为是一种移动机器人领域的关键技术，在陌生的环境下，机器人自主移动成为移动机器人领域研究的热点，在过去几十年里，许许多多的研究人员投身到移动机器人领域。自主机器人与传统机器人不同的是，它能够在没有人为控制下进行自主移动，能够完成在陌生环境下救援等任务。
惯性导航是由牛顿第二运动定律理论发展而来的技术，通过惯性仪器测量移动机器人的旋转角速度和加速度，并通过积分求解位姿，速度等信息。其特点是稳定性高，不易受外部环境干扰，具有良好的自主导航特性。但是纯惯性导航存在很大的漂移，精度会逐渐发散。因此惯性导航不适合长时间，长距离的导航。
近些年来，科学技术发展迅速，计算机视觉以及图像处理技术飞速发展，视觉导航作为计算机视觉的一个分支也得到了飞速的发展。视觉导航通过摄像头拍摄图像，计算机根据图像处理提取特征信息并由此建模。通过视觉里程计模型准确输出相机位姿。由于视觉传感器类似于人眼，可以探测到非常丰富的环境信息。但是视觉导航容易受环境条件的影响，如光照、天气等自然环境。
视觉惯性融合导航作为一种新兴技术，它在很大程度上解决了纯视觉导航或者纯惯性导航存在的一些缺陷。并且基于滤波的视觉惯性导航算法可以在一些嵌入式平台上实时工作，满足现实环境中的一些需要。
发明内容
发明目的：为了解决现有的机器人位姿估计技术的不足，本发明提出了一种基于IMU预积分的视觉惯性里程计方法。
技术方案：一种基于IMU预积分的视觉惯性里程计方法，包括如下步骤：
(1)搭建视觉惯性里程计系统：定义固连在载体上的IMU参考坐标系为I系；定义固连在载体上的相机参考坐标系为C系；以初始时刻相机位置为世界坐标系原点，右前上为XYZ轴建立世界坐标系，即G系；定义载体坐标系与I系重合；载体运动过程中采集相机传感器和IMU传感器数据；
(2)构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；
(3)构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行预积分计算当前时刻IMU的位置以及速度；
(4)当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广；
(5)由相机观测构建系统观测模型；
(6)执行扩展卡尔曼滤波更新获得载体位姿。
优选的，在步骤(2)中，构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程的具体过程如下：
IMU状态向量定义为：
Figure BDA0002353278720000021
式中，
Figure BDA0002353278720000022
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
Figure BDA0002353278720000023
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
Figure BDA0002353278720000024
且
Figure BDA0002353278720000025
式中，δq表示误差四元数；
Figure BDA0002353278720000026
表示四元数估计值；
Figure BDA0002353278720000027
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
Figure BDA0002353278720000028
式中，
Figure BDA0002353278720000029
表示小角度旋转量；
Figure BDA00023532787200000210
表示陀螺仪偏置误差；
Figure BDA00023532787200000211
表示世界坐标系下载体速度误差；
Figure BDA00023532787200000212
表示加速度计偏置误差；
Figure BDA00023532787200000213
表示世界坐标系下载体的位置误差；
Figure BDA00023532787200000214
表示IMU参考系和相机参考系之间的小角度旋转；
Figure BDA00023532787200000215
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
Figure BDA00023532787200000216
式中，每个相机误差状态
Figure BDA00023532787200000217
定义为：
Figure BDA00023532787200000218
式中，
Figure BDA00023532787200000219
和
Figure BDA00023532787200000220
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
Figure BDA0002353278720000031
式中，
Figure BDA0002353278720000032
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
Figure BDA0002353278720000033
表示为陀螺仪偏置的估计值；
Figure BDA0002353278720000034
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
Figure BDA0002353278720000035
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
Figure BDA0002353278720000036
定义如下：
Figure BDA0002353278720000037
Figure BDA0002353278720000038
式中，
Figure BDA0002353278720000039
其中
Figure BDA00023532787200000310
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
Figure BDA00023532787200000311
式中，F表示为系统矩阵；G表示观测噪声矩阵；其中，n，定义如下：
nI T＝[ng Tnwg TnaTnwaT]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下：
Figure BDA00023532787200000312
Figure BDA00023532787200000313
优选的，在步骤(3)中，构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行预积分计算当前时刻IMU的位置以及速度的具体过程如下：
离散系统的状态转移矩阵如下：
Figure BDA0002353278720000041
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
Figure BDA0002353278720000042
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
Figure BDA0002353278720000043
式中，
Figure BDA0002353278720000044
表示k时刻IMU状态协方差矩阵；
Figure BDA0002353278720000045
表示k+1时刻增广的IMU状态协方差矩阵;
k时刻IMU和相机系统协方差矩阵方程如下：
Figure BDA0002353278720000046
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
Figure BDA0002353278720000047
表示k时刻IMU和相机之间的协方差关联矩阵；
Figure BDA0002353278720000048
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
Figure BDA0002353278720000049
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
Figure BDA00023532787200000410
式中，
Figure BDA00023532787200000411
表示k+1时刻旋转四元数；
Figure BDA00023532787200000412
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
Figure BDA0002353278720000051
Figure BDA0002353278720000052
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
Figure BDA0002353278720000053
表示k时刻载体坐标系到世界坐标系的旋转矩阵，由零阶四元数积分获得；
Figure BDA0002353278720000054
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
Figure BDA0002353278720000055
Figure BDA0002353278720000056
式中，kαk+1和kβk+1表示k+1时刻的预积分项，其方程如下：
Figure BDA0002353278720000057
Figure BDA0002353278720000058
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
优选的，在步骤(4)中，当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广具体过程如下：
当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
Figure BDA0002353278720000059
Figure BDA00023532787200000510
式中，
Figure BDA00023532787200000511
表示世界坐标系到此时刻相机坐标系的旋转四元数；
Figure BDA00023532787200000512
表示世界坐标系到此时刻相机坐标系的平移矢量；
Figure BDA00023532787200000513
和
Figure BDA00023532787200000514
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
Figure BDA00023532787200000515
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI 06×6N)
其中，
Figure BDA0002353278720000061
优选的，在步骤(5)中，由相机观测构建系统观测模型的具体过程如下：
假设当前时刻相机观测到空间中单个特征点fj，测量方程为：
Figure BDA0002353278720000062
式中，
Figure BDA0002353278720000063
表示第i个相机观测到第j个特征点的像素坐标；
Figure BDA0002353278720000064
和
Figure BDA0002353278720000065
分别为特征点j在第i个相机中的三维坐标分量；
特征点在第i个相机中的三维坐标方程如下：
Figure BDA0002353278720000066
式中，
Figure BDA0002353278720000067
表示第i个相机中的特征点j；Gpj表示特征点在世界坐标系下的坐标；
由测量方程构建观测残差并线性化方程如下：
Figure BDA0002353278720000068
式中，ri j表示第i个相机的观测残差；
Figure BDA0002353278720000069
表示第i个相机观测的估计值；
Figure BDA00023532787200000610
表示第i个相机测量噪声；
Figure BDA00023532787200000611
和
Figure BDA00023532787200000612
分别为
Figure BDA00023532787200000613
关于
Figure BDA00023532787200000614
和
Figure BDA00023532787200000615
的雅可比矩阵，其中
Figure BDA00023532787200000616
和
Figure BDA00023532787200000617
方程分别如下：
Figure BDA00023532787200000618
Figure BDA00023532787200000619
式中，各偏导项公式如下：
Figure BDA00023532787200000620
Figure BDA00023532787200000621
Figure BDA00023532787200000622
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
Figure BDA0002353278720000071
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
Figure BDA0002353278720000072
和
Figure BDA0002353278720000073
分别为叠加后的雅可比矩阵；
对上式方程左乘
Figure BDA0002353278720000074
的左零空间矩阵VT，得：
Figure BDA0002353278720000075
式中，
Figure BDA0002353278720000076
优选的，在步骤(6)中，执行扩展卡尔曼滤波更新获得载体位姿具体过程如下：
由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
Figure BDA0002353278720000077
Figure BDA0002353278720000078
Figure BDA0002353278720000079
式中，K表示卡尔曼增益；ΔX表示系统状态更新量；Pk+1|k+1表示k+1时刻系统协方差矩阵；Rn表示噪声协方差矩阵。
由系统状态更新量可以获得当前机器人的系统状态X，从而得到当前其机器人位姿。
本发明的有益效果为：融合视觉信息和惯导信息进行组合导航，计算出机器人在移动过程中的姿态、位置，同时避免了环境的影响。使用IMU预积分算法，避免了当新图像帧到来时，传统IMU姿态解算方法中需要多次解算的需求，简化了计算过程，并且提高了算法速度和精度。相对于现有技术，本发明可以进行长时间。长距离的位姿解算，并且提高了定位精度。
附图说明
图1为本发明的方法流程示意图；
图2为本发明在EuRoc数据集V1_01_easy序列上的估计位姿示意图。
具体实施方式
如图1所示，本发明的一种基于IMU预积分的视觉惯性里程计方法，包括以下步骤：
步骤1：搭建视觉惯性里程计系统，定义固连在载体上的IMU参考坐标系为I系；定义固连在载体上的相机参考坐标系为C系；以初始时刻相机位置为世界坐标系原点，右前上为XYZ轴建立世界坐标系，即G系；定义载体坐标系与I系重合；载体运动过程中采集相机传感器和IMU传感器数据；
步骤2：构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；
步骤3：构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；
步骤4：当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广；
步骤5：由相机观测构建系统观测模型；
步骤6：执行扩展卡尔曼滤波更新获得载体位姿；
上述技术方案中，构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程具体过程如下：
IMU状态向量定义为：
Figure BDA0002353278720000081
式中，
Figure BDA0002353278720000082
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
Figure BDA0002353278720000083
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
Figure BDA0002353278720000084
且
Figure BDA0002353278720000085
式中，δq表示误差四元数；
Figure BDA0002353278720000086
表示四元数估计值；
Figure BDA0002353278720000087
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
Figure BDA0002353278720000088
式中，
Figure BDA0002353278720000089
表示小角度旋转量；
Figure BDA00023532787200000810
表示陀螺仪偏置误差；
Figure BDA00023532787200000811
表示世界坐标系下载体速度误差；
Figure BDA00023532787200000812
表示加速度计偏置误差；
Figure BDA00023532787200000813
表示世界坐标系下载体的位置误差；
Figure BDA00023532787200000814
表示IMU参考系和相机参考系之间的小角度旋转；
Figure BDA00023532787200000815
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
Figure BDA00023532787200000816
式中，每个相机误差状态
Figure BDA00023532787200000817
定义为：
Figure BDA00023532787200000818
式中，
Figure BDA00023532787200000819
和
Figure BDA00023532787200000820
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
Figure BDA0002353278720000091
式中，
Figure BDA0002353278720000092
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
Figure BDA0002353278720000093
表示为陀螺仪偏置的估计值；
Figure BDA0002353278720000094
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
Figure BDA0002353278720000095
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
Figure BDA0002353278720000096
定义如下：
Figure BDA0002353278720000097
Figure BDA0002353278720000098
式中，
Figure BDA0002353278720000099
其中
Figure BDA00023532787200000910
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
Figure BDA00023532787200000911
式中，F表示为系统矩阵，其中，nI定义如下：
nI T＝[ng Tnwg Tna Tnwa T]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下：
Figure BDA00023532787200000912
Figure BDA00023532787200000913
上述技术方案中，步骤3构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度具体过程如下：离散系统的状态转移矩阵如下：
Figure BDA0002353278720000101
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
Figure BDA0002353278720000102
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
Figure BDA0002353278720000103
式中，
Figure BDA0002353278720000104
表示k时刻IMU状态协方差矩阵；
Figure BDA0002353278720000105
表示k+1时刻增广的IMU状态协方差矩阵；
k时刻IMU和相机系统协方差矩阵方程如下：
Figure BDA0002353278720000106
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
Figure BDA0002353278720000107
表示k时刻IMU和相机之间的协方差关联矩阵；
Figure BDA0002353278720000108
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
Figure BDA0002353278720000109
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
Figure BDA00023532787200001010
式中，
Figure BDA00023532787200001011
表示k+1时刻旋转四元数；
Figure BDA00023532787200001012
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
Figure BDA0002353278720000111
Figure BDA0002353278720000112
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
Figure BDA0002353278720000113
表示k时刻载体坐标系到世界坐标系的旋转矩阵；
Figure BDA0002353278720000114
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
Figure BDA0002353278720000115
Figure BDA0002353278720000116
式中，kak+1和kβk+1表示k+1时刻的预积分项，其方程如下：
Figure BDA0002353278720000117
Figure BDA0002353278720000118
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
上述技术方案中，步骤4对系统状态向量增广，并对系统协方差矩阵增广的具体过程如下：当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
Figure BDA0002353278720000119
Figure BDA00023532787200001110
式中，
Figure BDA00023532787200001111
表示世界坐标系到此时刻相机坐标系的旋转四元数；
Figure BDA00023532787200001112
表示世界坐标系到此时刻相机坐标系的平移矢量；
Figure BDA00023532787200001113
和
Figure BDA00023532787200001114
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
Figure BDA00023532787200001115
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI 06×6N)
其中，
Figure BDA0002353278720000121
上述技术方案中，步骤5构建系统观测模型具体过程如下：假设当前时刻相机观测到空间中单个特征点fj，测量方程为：
Figure BDA0002353278720000122
式中，
Figure BDA0002353278720000123
表示第i个相机观测到第j个特征点的像素坐标；
Figure BDA0002353278720000124
和
Figure BDA0002353278720000125
分别为特征点j在第i个相机中的三维坐标分量；
特征点在第i个相机中的三维坐标方程如下：
Figure BDA0002353278720000126
式中，
Figure BDA0002353278720000127
表示第i个相机中的特征点j；Gpj表示特征点在世界坐标系下的坐标；
由测量方程构建观测残差并线性化方程如下：
Figure BDA0002353278720000128
式中，ri j表示第i个相机的观测残差；
Figure BDA0002353278720000129
表示第i个相机观测的估计值；
Figure BDA00023532787200001210
表示第i个相机测量噪声；
Figure BDA00023532787200001211
和
Figure BDA00023532787200001212
分别为ri j关于
Figure BDA00023532787200001213
和
Figure BDA00023532787200001214
的雅可比矩阵，其中
Figure BDA00023532787200001215
和
Figure BDA00023532787200001216
方程分别如下：
Figure BDA00023532787200001217
Figure BDA00023532787200001218
式中，各偏导项公式如下：
Figure BDA00023532787200001219
Figure BDA00023532787200001220
Figure BDA00023532787200001221
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
Figure BDA00023532787200001222
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
Figure BDA0002353278720000131
和
Figure BDA0002353278720000132
分别为叠加后的雅可比矩阵；
对上式方程左乘
Figure BDA0002353278720000133
的左零空间矩阵VT，得：
Figure BDA0002353278720000134
式中，
Figure BDA0002353278720000135
上述技术方案中，步骤6执行扩展卡尔曼滤波具体过程如下：由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
Figure BDA0002353278720000136
Figure BDA0002353278720000137
Figure BDA0002353278720000138
式中，K表示卡尔曼增益；ΔX表示系统状态更新量；Pk+1|k+1表示k+1时刻系统协方差矩阵；Rn表示噪声协方差矩阵。
由更新量可以获得当前机器人的系统状态X，从而得到载体位姿，载体位姿即里程计信息。
尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。

Claims (1)

1.一种基于IMU预积分的视觉惯性里程计方法，其特征在于，包括如下步骤；
(1)搭建视觉惯性里程计系统：定义固连在载体上的IMU参考坐标系为I系；定义固连在载体上的相机参考坐标系为C系；以初始时刻相机位置为世界坐标系原点，右前上为XYZ轴建立世界坐标系，即G系；定义载体坐标系与I系重合；载体运动过程中采集相机传感器和IMU传感器数据；
(2)构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；
(3)构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；
(4)当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广；
(5)由相机观测构建系统观测模型；
(6)执行扩展卡尔曼滤波更新获得载体位姿；
步骤(2)中，构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程的具体步骤如下：
IMU状态向量定义为：
Figure FDA0003636932320000011
式中，
Figure FDA0003636932320000012
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
Figure FDA0003636932320000013
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
Figure FDA0003636932320000014
且
Figure FDA0003636932320000015
式中，δq表示误差四元数；
Figure FDA0003636932320000016
表示四元数估计值；
Figure FDA0003636932320000017
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
Figure FDA0003636932320000018
式中，
Figure FDA0003636932320000019
表示小角度旋转量；
Figure FDA00036369323200000110
表示陀螺仪偏置误差；
Figure FDA00036369323200000111
表示世界坐标系下载体速度误差；
Figure FDA00036369323200000112
表示加速度计偏置误差；
Figure FDA00036369323200000113
表示世界坐标系下载体的位置误差；
Figure FDA00036369323200000114
表示IMU参考系和相机参考系之间的小角度旋转；
Figure FDA00036369323200000115
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
Figure FDA0003636932320000021
式中，每个相机误差状态
Figure FDA0003636932320000022
定义为：
Figure FDA0003636932320000023
式中，
Figure FDA0003636932320000024
和
Figure FDA0003636932320000025
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
Figure FDA0003636932320000026
式中，
Figure FDA0003636932320000027
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
Figure FDA0003636932320000028
表示为陀螺仪偏置的估计值；
Figure FDA0003636932320000029
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
Figure FDA00036369323200000210
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
Figure FDA00036369323200000211
定义如下：
Figure FDA00036369323200000212
Figure FDA00036369323200000213
式中，
Figure FDA00036369323200000214
其中
Figure FDA00036369323200000215
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
Figure FDA00036369323200000216
式中，F表示为系统矩阵；G表示系统观测噪声矩阵；其中，nI定义如下：
nI T＝[ng Tnwg Tna Tnwa T]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下:
Figure FDA0003636932320000031
Figure FDA0003636932320000032
步骤(3)中，构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度的具体步骤如下：
离散系统的状态转移矩阵如下：
Figure FDA0003636932320000033
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
Figure FDA0003636932320000034
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
Figure FDA0003636932320000035
式中，
Figure FDA0003636932320000036
表示k时刻IMU状态协方差矩阵；
Figure FDA0003636932320000037
表示k+1时刻增广的IMU状态协方差矩阵；
k时刻IMU和相机系统协方差矩阵方程如下：
Figure FDA0003636932320000038
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
Figure FDA0003636932320000039
表示k时刻IMU和相机之间的协方差关联矩阵；
Figure FDA00036369323200000310
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
Figure FDA0003636932320000041
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
Figure FDA0003636932320000042
式中，
Figure FDA0003636932320000043
表示k+1时刻旋转四元数；
Figure FDA0003636932320000044
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
Figure FDA0003636932320000045
Figure FDA0003636932320000046
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
Figure FDA0003636932320000047
表示k时刻载体坐标系到世界坐标系的旋转矩阵；
Figure FDA0003636932320000048
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
Figure FDA0003636932320000049
Figure FDA00036369323200000410
式中，kαk+1和kβk+1表示k+1时刻的预积分项，其方程如下：
Figure FDA00036369323200000411
Figure FDA00036369323200000412
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
步骤(4)中，当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广具体步骤如下：
当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
Figure FDA0003636932320000051
Figure FDA0003636932320000052
式中，
Figure FDA0003636932320000053
表示世界坐标系到此时刻相机坐标系的旋转四元数；
Figure FDA0003636932320000054
表示世界坐标系到此时刻相机坐标系的平移矢量；
Figure FDA0003636932320000055
和
Figure FDA0003636932320000056
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
Figure FDA0003636932320000057
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI 06×6N)
其中，
Figure FDA0003636932320000058
步骤(5)中，由相机观测构建系统观测模型具体步骤如下：
假设当前时刻相机观测到空间中单个特征点fj,测量方程为：
Figure FDA0003636932320000059
式中，
Figure FDA00036369323200000510
表示第i个相机观测到第j个特征点的像素坐标；
Figure FDA00036369323200000520
和
Figure FDA00036369323200000521
分别为特征点j在第i个相机中的三维坐标分量；
由测量方程构建观测残差并线性化方程如下：
Figure FDA00036369323200000511
式中，
Figure FDA00036369323200000512
表示第i个相机的观测残差；
Figure FDA00036369323200000513
表示第i个相机观测的估计值；
Figure FDA00036369323200000514
表示第i个相机测量噪声；
Figure FDA00036369323200000515
和
Figure FDA00036369323200000516
分别为
Figure FDA00036369323200000517
关于
Figure FDA00036369323200000518
和
Figure FDA00036369323200000519
的雅可比矩阵；
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
Figure FDA0003636932320000061
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
Figure FDA0003636932320000062
和
Figure FDA0003636932320000063
分别为叠加后的雅可比矩阵；
对上式方程左乘
Figure FDA0003636932320000064
的左零空间矩阵VT,得：
Figure FDA0003636932320000065
式中，
Figure FDA0003636932320000066
步骤(6)中，执行扩展卡尔曼滤波更新获得当前载体位姿具体步骤如下：
由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
Figure FDA0003636932320000067
Figure FDA0003636932320000068
Figure FDA0003636932320000069
式中，K表示卡尔曼增益；ΔX表示系统状态更新量；Pk+1|k+1表示k+1时刻系统协方差矩阵；Rn表示噪声协方差矩阵，由系统状态更新量可以获得当前机器人的系统状态X，从而得到载体位姿。
CN202010007374.1A 2020-01-02 2020-01-02 一种基于imu预积分的视觉惯性里程计方法 Active CN110986939B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010007374.1A CN110986939B (zh) 2020-01-02 2020-01-02 一种基于imu预积分的视觉惯性里程计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010007374.1A CN110986939B (zh) 2020-01-02 2020-01-02 一种基于imu预积分的视觉惯性里程计方法

Publications (2)

Publication Number Publication Date
CN110986939A CN110986939A (zh) 2020-04-10
CN110986939B true CN110986939B (zh) 2022-06-28

Family

ID=70080805

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010007374.1A Active CN110986939B (zh) 2020-01-02 2020-01-02 一种基于imu预积分的视觉惯性里程计方法

Country Status (1)

Country Link
CN (1) CN110986939B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111678515A (zh) * 2020-04-28 2020-09-18 北京三快在线科技有限公司 设备状态估计方法、装置、电子设备及存储介质
CN111578937B (zh) * 2020-05-29 2024-01-09 上海新天策数字科技有限公司 同时优化外参数的视觉惯性里程计系统
CN113932820A (zh) * 2020-06-29 2022-01-14 杭州海康威视数字技术股份有限公司 对象检测的方法和装置
CN111949929B (zh) * 2020-08-12 2022-06-21 智能移动机器人(中山)研究院 一种多传感器融合的四足机器人运动里程计设计方法
CN112697142B (zh) * 2020-12-21 2023-03-10 南京航空航天大学 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法
CN113358117B (zh) * 2021-03-09 2023-05-26 北京工业大学 一种利用地图的视觉惯性室内定位方法
CN113358134A (zh) * 2021-04-20 2021-09-07 重庆知至科技有限公司 一种视觉惯性里程计系统
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114485574B (zh) * 2021-12-21 2023-03-21 武汉大学 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法
CN115060260A (zh) * 2022-02-22 2022-09-16 上海大学 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法
CN114509071B (zh) * 2022-04-20 2022-07-08 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108731700B (zh) * 2018-03-22 2020-07-31 东南大学 一种视觉惯性里程计中的加权欧拉预积分方法
CN108731670B (zh) * 2018-05-18 2021-06-22 南京航空航天大学 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109540126B (zh) * 2018-12-03 2020-06-30 哈尔滨工业大学 一种基于光流法的惯性视觉组合导航方法
CN110160522A (zh) * 2019-04-16 2019-08-23 浙江大学 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法
CN110455309B (zh) * 2019-08-27 2021-03-16 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计

Also Published As

Publication number Publication date
CN110986939A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110986939B (zh) 一种基于imu预积分的视觉惯性里程计方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
WO2020253854A1 (zh) 移动机器人姿态角解算方法
CN109540126B (zh) 一种基于光流法的惯性视觉组合导航方法
Fleps et al. Optimization based IMU camera calibration
Weiss et al. Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments
CN107314778B (zh) 一种相对姿态的标定方法、装置及系统
CN111462231B (zh) 一种基于rgbd传感器和imu传感器的定位方法
CN107014371A (zh) 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN106708066A (zh) 基于视觉/惯导的无人机自主着陆方法
CN106056664A (zh) 一种基于惯性和深度视觉的实时三维场景重构系统及方法
CN108534772B (zh) 姿态角获取方法及装置
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN111340851A (zh) 基于双目视觉与imu融合的slam方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
CN114046800B (zh) 一种基于双层滤波框架的高精度里程估计方法
CN112556681A (zh) 一种基于视觉的果园机械导航定位方法
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN114764830A (zh) 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法
CN113237485A (zh) 一种基于多传感器融合的slam方法与系统
CN111637894A (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