首页 文章详情

VSLAM:IMU预积分公式推导

3D视觉工坊 | 1384 2021-05-05 00:46 0 0 0
UniSMS (合一短信)

点击上方“3D视觉工坊”,选择“星标”

干货第一时间送达

VSLAM:IMU预积分公式推导

一、IMU预积分

 传统的递推算法是根据上一时刻的IMU状态量,利用当前时刻测量得到的加速度与角速度,进行积分得到当前时刻的状态量。但是在VIO紧耦合非线性优化当中,各个状态量都是估计值,并且会不断调整,每次调整都会重新进行积分,传递IMU测量值。预积分的目的是将相对测量量与据对位姿解耦合,避免优化时重复进行积分。四元数的表示方法有两种:一种是Hamilton(右手系)表示,另一种是JPL(左手系)表示。读者对公式推导时一定注意。

 常用性质:

 1. 四元数的连续积分:

 2. 四元数的左乘与右乘:
 我们将四元数表示为:
,则左右乘可以表示为:

 3. 四元数导数如下:
 有角速度:,则导数可表示为:

1.1 当前时刻的位置,速度,旋转变量的连续表达式

    我们将图像帧记作,body坐标系下记作,我们将位置,速度和旋转在时间内进行积分,其世界坐标系下的公式可以写为:

1.2 当前时刻的位置,速度,旋转变量的离散表达式

 我们以中值积分给出离散表示:

1.3 两帧之间的位置,速度,旋转增量的连续表达式

 基本思想就是将参考坐标系从转到第帧的body坐标系下,相当于两边同时乘,我们直接用论文中的公式来表示:
 其中:
 上述可以理解为的相对运动量,其中状态的改变并不会对其产生影响,可以将其作为非线性优化变量,避免重复计算。实际到这里,只要求解出积分,我们就完成了预积分的计算,我们的目标也就是在此。实际当中随机游走也是发生改变的,所以我们将上述变量再次进行一阶近似,我们再次使用论文中的公式进行表示:
 至此,IMU的预积分表达式我们就已经得到了。

1.4 两帧之间的位置,速度,旋转增量的离散表达式

1.5 连续表达式下的位置、速度、旋转增量误差、协方差、Jacobian

 IMU在每一个时刻积分出来的变量都是有误差的,我们针对误差进行分析,分析误差的传递方程,下面为连续时间下的误差导数方程:
 我们对进行推导,假设true表示真实测量值,含误差,nominal表示不含噪声的理论值,则有:
 其中:
 则:
 我们再对进行推导:
 根据四元数的导数性质:
 我们将上面的等式进行汇总:
 最终:
 我们将导数的定义拿来:
 则:
 对比扩展卡尔曼公式,我们发现其正好是非线性系统进行线性的表达式,据此,我们给出相似的协方差预测公式:
 其中,协方差初始值为0,噪声协方差矩阵可以表示为:
 类似,我们也可以获得误差的Jacobian迭代公式:
 Jacobian的初始值为单位矩阵。

1.6 离散形式的增量分析

 实际只需根据中值积分,将连续形式表达式进行离散化即可,推导过程省略,比较简单
 其中:
 离散误差传递方程可以简写为:
 则Jacobian的迭代公式为:
 Jacobian的初始值为:。注意,我们在此计算Jacobian,仅仅是为了后端非线性优化过程对bias的计算提供帮助。
 协方差的迭代公式:
 协方差矩阵初始值为0,噪声的协方差矩阵为:
本文仅做学术分享,如有侵权,请联系删文。
下载1
在「3D视觉工坊」公众号后台回复:3D视觉即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2
「3D视觉工坊」公众号后台回复:3D视觉github资源汇总即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3
「3D视觉工坊」公众号后台回复:相机标定即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配即可下载独家立体匹配学习课件与视频网址。

重磅!3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群,目前主要有3D视觉CV&深度学习SLAM三维重建点云后处理自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。


一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球:针对3D视觉领域的视频课程(三维重建系列三维点云系列结构光系列手眼标定相机标定orb-slam3等视频课程)、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近2000星球成员为创造更好的AI世界共同进步,知识星球入口:

学习3D视觉核心技术,扫描查看介绍,3天内无条件退款
 圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~  

good-icon 0
favorite-icon 0
收藏
回复数量: 0
    暂无评论~~
    Ctrl+Enter