CN110986939A - 一种基于imu预积分的视觉惯性里程计方法 - Google Patents
一种基于imu预积分的视觉惯性里程计方法 Download PDFInfo
- Publication number
- CN110986939A CN110986939A CN202010007374.1A CN202010007374A CN110986939A CN 110986939 A CN110986939 A CN 110986939A CN 202010007374 A CN202010007374 A CN 202010007374A CN 110986939 A CN110986939 A CN 110986939A
- 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.)
- Granted
Links
- 230000010354 integration Effects 0.000 title claims abstract description 31
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000000007 visual effect Effects 0.000 title claims abstract description 25
- 239000011159 matrix material Substances 0.000 claims abstract description 94
- 239000013598 vector Substances 0.000 claims abstract description 60
- 238000005259 measurement Methods 0.000 claims abstract description 34
- 230000003190 augmentative effect Effects 0.000 claims abstract description 17
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 8
- 238000001914 filtration Methods 0.000 claims abstract description 6
- 230000001133 acceleration Effects 0.000 claims description 7
- 230000007704 transition Effects 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000005295 random walk Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000059 patterning Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于IMU预积分的视觉惯性里程计方法，包括如下步骤：获取载体在固定坐标系下的视觉和IMU传感器数据；构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；对系统状态向量增广，并对系统协方差矩阵增广；建立相机传感器的测量模型；由相机观测构建系统观测模型；执行系统滤波更新得到载体位姿。本发明可以提高滤波方式下的视觉惯性里程计定位精度。
Description
一种基于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状态向量定义为：
式中，
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
式中，δq表示误差四元数；
表示四元数估计值；
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
式中，
表示小角度旋转量；
表示陀螺仪偏置误差；
表示世界坐标系下载体速度误差；
表示加速度计偏置误差；
表示世界坐标系下载体的位置误差；
表示IMU参考系和相机参考系之间的小角度旋转；
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
式中，每个相机误差状态
定义为：
式中，
和
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
式中，
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
表示为陀螺仪偏置的估计值；
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
定义如下：
式中，
其中
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
式中，F表示为系统矩阵；G表示观测噪声矩阵；其中，n，定义如下：
nI T＝[ng Tnwg TnaTnwaT]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下：
优选的，在步骤(3)中，构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行预积分计算当前时刻IMU的位置以及速度的具体过程如下：
离散系统的状态转移矩阵如下：
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
式中，
表示k时刻IMU状态协方差矩阵；
表示k+1时刻增广的IMU状态协方差矩阵;
k时刻IMU和相机系统协方差矩阵方程如下：
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
表示k时刻IMU和相机之间的协方差关联矩阵；
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
式中，
表示k+1时刻旋转四元数；
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
表示k时刻载体坐标系到世界坐标系的旋转矩阵，由零阶四元数积分获得；
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
式中，kαk+1和kβk+1表示k+1时刻的预积分项，其方程如下：
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
优选的，在步骤(4)中，当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广具体过程如下：
当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
式中，
表示世界坐标系到此时刻相机坐标系的旋转四元数；
表示世界坐标系到此时刻相机坐标系的平移矢量；
和
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI 06×6N)
其中，
优选的，在步骤(5)中，由相机观测构建系统观测模型的具体过程如下：
假设当前时刻相机观测到空间中单个特征点fj，测量方程为：
式中，
表示第i个相机观测到第j个特征点的像素坐标；
和
分别为特征点j在第i个相机中的三维坐标分量；
特征点在第i个相机中的三维坐标方程如下：
式中，
表示第i个相机中的特征点j；Gpj表示特征点在世界坐标系下的坐标；
由测量方程构建观测残差并线性化方程如下：
式中，ri j表示第i个相机的观测残差；
表示第i个相机观测的估计值；
表示第i个相机测量噪声；
和
分别为
关于
和
的雅可比矩阵，其中
和
方程分别如下：
式中，各偏导项公式如下：
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
和
分别为叠加后的雅可比矩阵；
对上式方程左乘
的左零空间矩阵VT，得：
式中，
优选的，在步骤(6)中，执行扩展卡尔曼滤波更新获得载体位姿具体过程如下：
由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
式中，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状态向量定义为：
式中，
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
式中，δq表示误差四元数；
表示四元数估计值；
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
式中，
表示小角度旋转量；
表示陀螺仪偏置误差；
表示世界坐标系下载体速度误差；
表示加速度计偏置误差；
表示世界坐标系下载体的位置误差；
表示IMU参考系和相机参考系之间的小角度旋转；
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
式中，每个相机误差状态
定义为：
式中，
和
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
式中，
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
表示为陀螺仪偏置的估计值；
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
定义如下：
式中，
其中
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
式中，F表示为系统矩阵，其中，nI定义如下：
nI T＝[ng Tnwg Tna Tnwa T]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下：
上述技术方案中，步骤3构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度具体过程如下：离散系统的状态转移矩阵如下：
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
式中，
表示k时刻IMU状态协方差矩阵；
表示k+1时刻增广的IMU状态协方差矩阵；
k时刻IMU和相机系统协方差矩阵方程如下：
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
表示k时刻IMU和相机之间的协方差关联矩阵；
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
式中，
表示k+1时刻旋转四元数；
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
表示k时刻载体坐标系到世界坐标系的旋转矩阵；
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
式中，kak+1和kβk+1表示k+1时刻的预积分项，其方程如下：
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
上述技术方案中，步骤4对系统状态向量增广，并对系统协方差矩阵增广的具体过程如下：当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
式中，
表示世界坐标系到此时刻相机坐标系的旋转四元数；
表示世界坐标系到此时刻相机坐标系的平移矢量；
和
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI 06×6N)
其中，
上述技术方案中，步骤5构建系统观测模型具体过程如下：假设当前时刻相机观测到空间中单个特征点fj，测量方程为：
式中，
表示第i个相机观测到第j个特征点的像素坐标；
和
分别为特征点j在第i个相机中的三维坐标分量；
特征点在第i个相机中的三维坐标方程如下：
式中，
表示第i个相机中的特征点j；Gpj表示特征点在世界坐标系下的坐标；
由测量方程构建观测残差并线性化方程如下：
式中，ri j表示第i个相机的观测残差；
表示第i个相机观测的估计值；
表示第i个相机测量噪声；
和
分别为ri j关于
和
的雅可比矩阵，其中
和
方程分别如下：
式中，各偏导项公式如下：
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
和
分别为叠加后的雅可比矩阵；
对上式方程左乘
的左零空间矩阵VT，得：
式中，
上述技术方案中，步骤6执行扩展卡尔曼滤波具体过程如下：由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
式中，K表示卡尔曼增益；ΔX表示系统状态更新量；Pk+1|k+1表示k+1时刻系统协方差矩阵；Rn表示噪声协方差矩阵。
由更新量可以获得当前机器人的系统状态X，从而得到载体位姿，载体位姿即里程计信息。
尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。
Claims (6)
1.一种基于IMU预积分的视觉惯性里程计方法，其特征在于，包括如下步骤；
(1)搭建视觉惯性里程计系统：定义固连在载体上的IMU参考坐标系为I系；定义固连在载体上的相机参考坐标系为C系；以初始时刻相机位置为世界坐标系原点，右前上为XYZ轴建立世界坐标系，即G系；定义载体坐标系与I系重合；载体运动过程中采集相机传感器和IMU传感器数据；
(2)构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；
(3)构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；
(4)当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广；
(5)由相机观测构建系统观测模型；
(6)执行扩展卡尔曼滤波更新获得载体位姿。
2.如权利要求1所述的基于IMU预积分的视觉惯性里程计方法，其特征在于步骤(2)中，构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程的具体步骤如下：
IMU状态向量定义为：
式中，
表示从G系到I系的旋转单位四元数向量；GvI和GpI表示载体在世界坐标系中的速度和位置的三维向量；bg和ba表示IMU中陀螺仪和加速度计的三维偏置向量；
和IpC表示相机参考系和IMU参考系之间的旋转四元数和平移向量；
误差四元数定义为：
式中，δq表示误差四元数；
表示四元数估计值；
表示小角度旋转的三维向量；
IMU误差状态向量定义为：
式中，
表示小角度旋转量；
表示陀螺仪偏置误差；
表示世界坐标系下载体速度误差；
表示加速度计偏置误差；
表示世界坐标系下载体的位置误差；
表示IMU参考系和相机参考系之间的小角度旋转；
表示IMU参考系和相机参考系之间的位置误差；
系统误差向量定义为：
式中，每个相机误差状态
定义为：
式中，
和
分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；
连续时间IMU观测状态传递方程如下：
式中，
表示为IMU角速度测量估计值，wm表示为IMU角速度测量值，
表示为陀螺仪偏置的估计值；
表示为IMU加速度测量估计值，am表示为IMU加速度测量值，
表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中
定义如下：
式中，
其中
分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；
基于IMU运动方程，IMU误差状态方程如下：
式中，F表示为系统矩阵；G表示系统观测噪声矩阵；其中，nI定义如下：
nI T＝[ng Tnwg Tna Tnwa T]T
式中，ng和na表示陀螺仪和加速度计测量的高斯白噪声矩阵，nwg和nwa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下：
3.如权利要求1所述的基于IMU预积分的视觉惯性里程计方法，其特征在于步骤(3)中，构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度的具体步骤如下：
离散系统的状态转移矩阵如下：
式中，Φk表示k时刻状态转移矩阵；
离散系统的噪声协方差矩阵如下：
式中，Qk表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：
Q＝E[nInI T]
建立IMU状态协方差矩阵方程如下：
式中，
表示k时刻IMU状态协方差矩阵；
表示k+1时刻增广的IMU状态协方差矩阵；
k时刻IMU和相机系统协方差矩阵方程如下：
式中，Pk|k表示k时刻IMU和相机系统协方差矩阵；
表示k时刻IMU和相机之间的协方差关联矩阵；
表示k时刻相机状态协方差矩阵；
利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：
根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：
式中，
表示k+1时刻旋转四元数；
表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：
Δt＝tk+1-tk
根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：
式中，Gvk+1表示k+1时刻载体在世界坐标系下的速度；Gvk表示k时刻载体在世界坐标系下的速度；
表示k时刻载体坐标系到世界坐标系的旋转矩阵；
表示τ时刻载体系到k时刻载体系的旋转矩阵；na表示IMU测量噪声；Gpk+1表示k+1时刻载体在世界坐标系下的位置；Gpk表示k时刻载体在世界坐标系下的位置；
根据上式方程，记：
式中，kαk+1和kβk+1表示k+1时刻的预积分项，其方程如下：
式中，δt表示IMU测量时间间隔，方程如下：
Δt＝tτ+1-tτ
4.如权利要求1所述的基于IMU预积分的视觉惯性里程计方法，其特征在于步骤(4)中，当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广具体步骤如下：
当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：
式中，
表示世界坐标系到此时刻相机坐标系的旋转四元数；
表示世界坐标系到此时刻相机坐标系的平移矢量；
和
表示相机和IMU之间的外参数；
将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：
式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：
J＝(JI O6×6N)
其中，
5.如权利要求1所述的基于IMU预积分的视觉惯性里程计方法，其特征在于步骤(5)中，由相机观测构建系统观测模型具体步骤如下：
假设当前时刻相机观测到空间中单个特征点fj，测量方程为：
式中，
表示第i个相机观测到第j个特征点的像素坐标；
和
分别为特征点j在第i个相机中的三维坐标分量；
由测量方程构建观测残差并线性化方程如下：
式中，
表示第i个相机的观测残差；
表示第i个相机观测的估计值；
表示第i个相机测量噪声；
和
分别为
关于
和
的雅可比矩阵；
对特征点fj的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：
式中，rj表示叠加后的观测残差；nj表示叠加后的系统观测噪声，
和
分别为叠加后的雅可比矩阵；
对上式方程左乘
的左零空间矩阵VT，得：
式中，
。
6.如权利要求1所述的基于IMU预积分的视觉惯性里程计方法，其特征在于步骤(6)中，执行扩展卡尔曼滤波更新获得当前载体位姿具体步骤如下：
由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：
式中，K表示卡尔曼增益；ΔX表示系统状态更新量；Pk+1|k+1表示k+1时刻系统协方差矩阵；Rn表示噪声协方差矩阵，由系统状态更新量可以获得当前机器人的系统状态X，从而得到载体位姿。
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 true CN110986939A (zh) | 2020-04-10 |
| CN110986939B 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) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111578937A (zh) * | 2020-05-29 | 2020-08-25 | 天津工业大学 | 同时优化外参数的视觉惯性里程计系统 |
| CN111678515A (zh) * | 2020-04-28 | 2020-09-18 | 北京三快在线科技有限公司 | 设备状态估计方法、装置、电子设备及存储介质 |
| CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
| CN112697142A (zh) * | 2020-12-21 | 2021-04-23 | 南京航空航天大学 | 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 |
| CN112985450A (zh) * | 2021-02-09 | 2021-06-18 | 东南大学 | 一种具有同步时间误差估计的双目视觉惯性里程计方法 |
| CN113358134A (zh) * | 2021-04-20 | 2021-09-07 | 重庆知至科技有限公司 | 一种视觉惯性里程计系统 |
| CN113358117A (zh) * | 2021-03-09 | 2021-09-07 | 北京工业大学 | 一种利用地图的视觉惯性室内定位方法 |
| CN113932820A (zh) * | 2020-06-29 | 2022-01-14 | 杭州海康威视数字技术股份有限公司 | 对象检测的方法和装置 |
| CN114018284A (zh) * | 2021-10-13 | 2022-02-08 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114111818A (zh) * | 2021-12-08 | 2022-03-01 | 太原供水设计研究院有限公司 | 一种通用视觉slam方法 |
| CN114485574A (zh) * | 2021-12-21 | 2022-05-13 | 武汉大学 | 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法 |
| CN114509071A (zh) * | 2022-04-20 | 2022-05-17 | 中国空气动力研究与发展中心低速空气动力研究所 | 一种风洞试验模型姿态测量方法 |
| CN115060260A (zh) * | 2022-02-22 | 2022-09-16 | 上海大学 | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
| CN108731700A (zh) * | 2018-03-22 | 2018-11-02 | 东南大学 | 一种视觉惯性里程计中的加权欧拉预积分方法 |
| CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
| CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
| CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
-
2020
- 2020-01-02 CN CN202010007374.1A patent/CN110986939B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731700A (zh) * | 2018-03-22 | 2018-11-02 | 东南大学 | 一种视觉惯性里程计中的加权欧拉预积分方法 |
| CN108731670A (zh) * | 2018-05-18 | 2018-11-02 | 南京航空航天大学 | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 |
| CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
| CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
| CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
Non-Patent Citations (1)
| Title |
|---|
| 徐晓苏等: "《基于SALDE-UKF-SVM 算法的WLAN 室内定位方法》", 《中国惯性技术学报》 * |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111678515A (zh) * | 2020-04-28 | 2020-09-18 | 北京三快在线科技有限公司 | 设备状态估计方法、装置、电子设备及存储介质 |
| CN111578937A (zh) * | 2020-05-29 | 2020-08-25 | 天津工业大学 | 同时优化外参数的视觉惯性里程计系统 |
| CN111578937B (zh) * | 2020-05-29 | 2024-01-09 | 上海新天策数字科技有限公司 | 同时优化外参数的视觉惯性里程计系统 |
| CN113932820A (zh) * | 2020-06-29 | 2022-01-14 | 杭州海康威视数字技术股份有限公司 | 对象检测的方法和装置 |
| CN111949929B (zh) * | 2020-08-12 | 2022-06-21 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
| CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
| CN112697142A (zh) * | 2020-12-21 | 2021-04-23 | 南京航空航天大学 | 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 |
| CN112697142B (zh) * | 2020-12-21 | 2023-03-10 | 南京航空航天大学 | 一种基于预积分理论的惯性/轮速里程计融合定位与参数优化方法 |
| CN112985450A (zh) * | 2021-02-09 | 2021-06-18 | 东南大学 | 一种具有同步时间误差估计的双目视觉惯性里程计方法 |
| CN112985450B (zh) * | 2021-02-09 | 2024-05-14 | 东南大学 | 一种具有同步时间误差估计的双目视觉惯性里程计方法 |
| CN113358117A (zh) * | 2021-03-09 | 2021-09-07 | 北京工业大学 | 一种利用地图的视觉惯性室内定位方法 |
| CN113358134A (zh) * | 2021-04-20 | 2021-09-07 | 重庆知至科技有限公司 | 一种视觉惯性里程计系统 |
| CN114018284A (zh) * | 2021-10-13 | 2022-02-08 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114018284B (zh) * | 2021-10-13 | 2024-01-23 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
| CN114111818A (zh) * | 2021-12-08 | 2022-03-01 | 太原供水设计研究院有限公司 | 一种通用视觉slam方法 |
| CN114485574A (zh) * | 2021-12-21 | 2022-05-13 | 武汉大学 | 基于卡尔曼滤波模型的三线阵影像pos辅助对地定位方法 |
| CN115060260A (zh) * | 2022-02-22 | 2022-09-16 | 上海大学 | 一种基于anfis-eskf的地面移动机器人多传感器融合定位方法 |
| CN114509071A (zh) * | 2022-04-20 | 2022-05-17 | 中国空气动力研究与发展中心低速空气动力研究所 | 一种风洞试验模型姿态测量方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110986939B (zh) | 2022-06-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110986939B (zh) | 一种基于imu预积分的视觉惯性里程计方法 | |
| CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
| WO2020253854A1 (zh) | 移动机器人姿态角解算方法 | |
| CN109991636B (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN109376785B (zh) | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 | |
| Fleps et al. | Optimization based IMU camera calibration | |
| Bloesch et al. | State estimation for legged robots on unstable and slippery terrain | |
| Weiss et al. | Real-time onboard visual-inertial state estimation and self-calibration of MAVs in unknown environments | |
| CN105931275A (zh) | 基于移动端单目和imu融合的稳定运动跟踪方法和装置 | |
| CN107941217B (zh) | 一种机器人定位方法、电子设备、存储介质、装置 | |
| CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
| CN106708066A (zh) | 基于视觉/惯导的无人机自主着陆方法 | |
| CN110018691B (zh) | 小型多旋翼无人机飞行状态估计系统和方法 | |
| CN107014371A (zh) | 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置 | |
| CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
| CN107516326A (zh) | 融合单目视觉和编码器信息的机器人定位方法和系统 | |
| CN106030430A (zh) | 用于使用旋翼微型航空载具（mav）在室内和室外环境中的稳健的自主飞行的多传感器融合 | |
| CN108534772B (zh) | 姿态角获取方法及装置 | |
| CN112985450B (zh) | 一种具有同步时间误差估计的双目视觉惯性里程计方法 | |
| CN114046800B (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
| CN111156997A (zh) | 一种基于相机内参在线标定的视觉/惯性组合导航方法 | |
| CN115540860A (zh) | 一种多传感器融合位姿估计算法 | |
| CN110929402A (zh) | 一种基于不确定分析的概率地形估计方法 | |
| CN114764830A (zh) | 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法 | |
| CN117710476A (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 |