ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分

2.基于流型的IMU预积分
这一部分主要参考论文(2)中的内容,ORB_SLAM2融合惯性数据的算法中预积分处理是参照论文(2)中实现的。前面关于VINS的详解说过预积分计算过程,那篇使用的是四元数旋转求解,和这里的过程是一样的,主要计算三个量,分别是IMU预积分值,预积分测量值的协方差矩阵以及残差的雅各比矩阵,各自用途就不多说,有不懂的可以参照VINS的详解
https://blog.csdn.net/wangshuailpp/article/details/78461171
(1)IMU预积分计算
世界坐标系W、IMU坐标系B以及相机坐标系C的关系和转换如下图2所示,所有的位姿和特征点坐标最终都是在世界坐标系下表示,我们会将相机坐标系下的的状态量首先转换到IMU坐标系下然后再转换到世界坐标系下。TBC代表在IMU坐标系B下相机到IMU之间的外参转换矩阵,TWB=(RWB,wp)代表世界坐标系W下IMU到世界坐标的转换矩阵。关于坐标之间的关系比较重要,刚接触会感觉比较复杂,需要多思考。下面所有的公式和变量前下标代表参考坐标系,例如wp就是世界坐标系下的位移,后下标第一个下标是参考坐标系,例如RWB是世界坐标系下。
这里写图片描述
由IMU的加速度计和陀螺仪可以获得IMU坐标系下的加速度Ba和角速度BwWB,可以表示成下面的公式(27)(28),公式左边是IMU直接输出的数据。这里写图片描述
旋转、平移和速度的导数可表示为公式(29)
这里写图片描述
世界坐标系下的旋转、平移和速度可由一般的积分公式求得
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第1张图片
离散时间下采用欧拉积分可以将上面连续时间积分改写成下面式(30)
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第2张图片
联立公式(27)(28)可以得到公式(31)
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第3张图片
这时候状态量是世界坐标系下的。设现在有i和j两个相邻关键帧,需要已知i时刻对应的IMU状态量,求解离散时间下j时刻关键帧对应的IMU预积分值,
这里写图片描述
这里写图片描述
为避免重复积分采用IMU预积分方法计算i、j时刻的状态量的相对变化。也就是将参考坐标系由世界坐标系转换到IMU的i时刻坐标系下,计算得到IMU状态量在i和j时刻之间的相对量,这个相对量是只取积分算式内的值,而不是旋转、速度和位移的变化量,和VINS的处理方式相同。
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第4张图片
从上面的公式中可以看出,整个预积分公式和偏置,噪声都有关系,相当于一个二元方程,因此需要分别分析偏置和噪声对系统的影响,噪声服从高斯正态分布,相对来说比较简单,而偏置服从随机游走模型,关系很复杂,因此两者的处理方式不同。对于噪声分量采用噪声高斯传播方程来求解,而偏置采用一阶泰勒展开方式。总体思路是首先将偏置在i时刻的bi处进行一阶展开,得到公式(44)
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第5张图片
这里的数值就是IMU最终的预积分数值,但值得注意的是在初始化使用的IMU预积分是没有偏置的一阶项,也就只是等式右边前面一项而已,等式后面的雅各比矩阵是在初始化完成后进行优化之前才计算而来,得到IMU准确的预计分值(真值),和估计值从而得到IMU的残差。所以我们首先要手推一下上面公式右边的前面一项,注意这里所有的指数映射中都省去了写反对称符号。
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第6张图片
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第7张图片
至此IMU预积分的数值就计算出来,也就是等式右边的第一项。
(2)预积分雅可比和协方差矩阵
下面需要计算出噪声传播项的协方差矩阵和对应偏置的雅各比矩阵。这部分推导在附录部分A和B,和VINS的过程出奇的一致,只不过这里是将状态量分开讨论的,分别是(deta_Rj,deta_vj,deta_pj)和(deta_Rj-1,deta_vj-1,deta_pj-1)、(nba,nbg)以及(deta_Rj,deta_vj,deta_pj)和(deta_ba,deta_bg)的关系,把两者合并就和VINS相同。,这部分推导相对简单就直接贴公式了。
下面是求协方差矩阵:
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第8张图片
这里写图片描述
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第9张图片
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第10张图片
系统是服从高斯分布的,协方差矩阵是按照高斯分布计算得到,是个9*9的矩阵。
下面是求预积分对偏置的雅各比矩阵:
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第11张图片
这里写图片描述
(3)残差雅可比
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第12张图片
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第13张图片
ORB_SLAM2视觉惯性紧耦合定位技术路线与代码详解1——IMU流型预积分_第14张图片
位移残差对应的雅可比如上图所示,J时刻相应的雅可比可按照图片方式进行推导,注意对pi和pj时刻的雅可比公式原文有错误,推导过程已经给出。对于速度残差和旋转的雅可比公式与平移相似,就不进行推导直接给出,至此IMU预计分就完成了。
这里写图片描述
这里写图片描述

你可能感兴趣的:(SLAM,视觉惯性SLAM)