vins中imu融合_vins-fusion代码解读[五] imu在vins里的理解-程序员宅基地

SLAM新手，欢迎讨论。

IMU作用

vins中，IMU只读取IMU六轴的信息，3轴线加速度(加速度计)和3轴角速度(陀螺仪)。

通过对陀螺仪的一次积分，直觉上可以获取姿态的变化情况，对加速度的一次积分和二次积分结合陀螺仪的信息，可以获取位置的变换情况。

背景知识：

IMU: 通常6轴陀螺仪，包括3轴线加速度和3轴角速度。9轴陀螺仪增加3轴地磁信息。

MEMS 陀螺仪没有姿态的输出，所有的MEMS陀螺都是非旋转装置，通过获取一个振动机械元件上的哥氏加速度效应，实现角速率检测。即一个验证质量在一个平面内做正弦振动，如果此平面以角速率Ω旋转，那么哥氏力就会使该质量以垂直于该平面的方向做正弦振动，其幅值正比于Ω。对哥氏力所引起的运动进行测量，就可获得Ω的信号，这就是微机械陀螺的基本工作原理。

市面上IMU可以输出姿态四元数或者欧拉角，是通过3轴线加速度和3轴角加速度或者增加3轴地磁信息融合求解出来。

加速度a能够提供重力g在俯仰角和翻滚角平面的投影，但是由于IMU本身在不断运动变化，a不等于g，因此不能很好的反应翻滚角和俯仰角，但是长期来看，这种方法不会产生大的漂移。

角速度w能够预测三个角度的变化量，在短期来看，积分能够预测两个时间段内角度的变化，长期运用会造成角度的漂移。

因此通常的做法是融合两者计算结果(卡尔曼滤波)，能够得到精确的俯仰角和翻滚角，由于g在偏航角的平面上没有投影，因此偏航角是不准确的(因为相当于只能依靠角速度的积分获取)，会有很大的漂移。融合地磁信息之后，可以提高偏航角的准确度，但是同样存在偏移。而且地磁的信息很容易受到环境的干扰，大型设备用电，电机转动都可能产生磁场，干扰地磁信息。

IMU的模型：

以t时刻，IMU作为参考系，我们能够得到以下的IMU模型。

左边是我们得到的测量值，包括了3轴的加速度，以及3轴的角速度信息。右边第一项是陀螺仪和加速度计的真值，第二项是偏移偏移，最后一项是测量噪声项。注意一点的是，由于IMU测量的时候，侧的是除了重力加速度外，所有外力对IMU参考系力的和对系统产生的加速度，会把反向重力加速度测量在内。意思是，当IMU自由落体的时候，加速度的读数是0。IMU平放在桌子上的时候，读数是g。

这里假设噪声na,nb是服从高斯正态分布，而ba,bw偏置服从随机游走模型(倒数服从高斯正态分布)。

因此，我们通过这个IMU模型，可以获取得到任意时刻的位置，速度，以及姿态。即，我们在已知初始的P1,Q1,V1,ba1,bg1的情况下，并且知道每个时刻ba,bg的时候，通过下面的这个式子的离散化表示，我们就能够通过积分得到后面这每一个时刻的P,Q,V。

即可以通过这个式子：

正常这个公式是可以比较直观上理解的。但是有一个问题，因为这个公式中，有一个R旋转矩阵是从imu坐标系旋转到世界坐标系的绝对位姿。由于我们后端优化的过程中，会不断的修正p,q,v的值，使得一个代价函数最小，现在我们一旦修改了一个p,因为积分项里面有一项与IMU的绝对位姿有关，因此我们又要进行重复的一个积分运算。

即：

第二帧位姿发生了改变，导致最终从第二帧开始，所有的积分项都要重新进行积分，非常耗时。

为了避免这一个重复积分运算的过程，vins提出了预积分的概念，通过了坐标系的转换，将绝对的坐标姿态转化为了相对的坐标转换，把要积分的项转化了一个形式。即把R的绝对位姿转化成相对位姿，相对于每一帧的绝对位姿，即：

现在三个临时量只和当前的加速度计偏移和陀螺仪的偏移有关。而于他们在世界坐标系的绝对位姿没有任何关系。任何帧的P，V，Q的改变不影响积分过程，因此就不用一直重复积分。

这三个变量只和偏移ba,bw有关。更新偏移，由于偏移变化是随时间缓慢变化的，因此可以用泰勒一阶展开。

而通过离散积分的方式，可以获取这三个积分项的离散迭代表达：

到这里，我们就可以带入IMU 6个轴的观测值，进行迭代求解，最终解算出这三个积分项。

到这里，我们还不知道这三个积分项的含义是什么，直观上理解，可以认为，bk帧做自由落体运动，以这个帧作为参考系，imu在bk到bk+1这段时间的位移，速度，姿态的变化。从数学上去描述就是：

因此imu的残差vins就定义为：

残差是一个向量，最终的残差是一个标量，因此我们就要把向量转化为一个标量。即要乘以他的协方差矩阵。协方差矩阵可以理解为，因为向量中每个元素变化的情况值不一样，有些元素变化得比较大(方差大)，有些元素变化比较小(方差小)，为了使得把这种变化统一起来，因此需要有一个标准来量化它，就认为是这个协方差矩阵。为了获取协方差矩阵，需要建立一个线性高斯误差状态传播方程，由线性高斯系统的协方差，就可以推导出方程协方差矩阵了，也就是测量状态的协方差矩阵了。

噪声对角线协方差矩阵就是我们需要标定imu的四个参数。两个测量噪声的方差，两个偏置倒数的方差。

总结预积分：

为了使得非线性优化中，不用每次改变位姿后，积分需要重新计算的问题。

现在改变位姿(速度，位移，旋转四元数关于世界坐标系的)，预积分内的项保持不变。

改变ba,bw后，预积分的项可以通过泰勒一阶展开来进行更新。

本文链接：https://blog.csdn.net/weixin_39832643/article/details/111782709

原作者删帖不实内容删帖广告或垃圾文章投诉

智能推荐

数据的两种归一化方法_数据量级相差较大归一化-程序员宅基地

文章浏览阅读1.3k次。数据标准化（归一化）处理是数据挖掘的一项基础工作，不同评价指标往往具有不同的量纲和量纲单位，这样的情况会影响到数据分析的结果，为了消除指标之间的量纲影响，需要进行数据标准化处理，以解决数据指标之间的可比性。原始数据经过数据标准化处理后，各指标处于同一数量级，适合进行综合对比评价。一般而言，数据的标准化（normalization）是将数据按比例缩放，使之落入一个小的特定区间。在某些比较和评价的..._数据量级相差较大归一化

立志进大厂的Owen-程序员宅基地

文章浏览阅读335次。为什么写博客？目录为什么写博客？

图表示学习Graph Embedding：DeepWalk python实现_graph embedding python-程序员宅基地

文章浏览阅读1.1w次，点赞12次，收藏45次。https://github.com/AI-luyuan/graph-embedding_graph embedding python

【JAVA开发小技巧】使用enum枚举类规范化代码_枚举代码规范-程序员宅基地

文章浏览阅读433次，点赞6次，收藏11次。阿里巴巴Java开发手册中推荐，如果常量类中变量值仅在一个范围内变化，且带有名称之外的延伸属性，建议定义为枚举类。使用枚举类可以使我们的代码更加规范且美观。_枚举代码规范

Python基础教程：strip 函数踩坑_python的rstrip为什么没用-程序员宅基地

文章浏览阅读430次。S.strip(chars=None)strip 函数用于去除字符串首尾的空格，当 chars 不为 None 时，则删除字符串首尾的 chars 中的字符。当 chars=None 时，去除首尾空格，没啥好说的，我们来看 chars 不为 None 时的情况。str = 'abc123abc'print(str.strip('a')) # bc123abcprint(str.strip('abc')) # 123结果跟预期的一样，我们再看下面的例子：'''Pyth_python的rstrip为什么没用

Kotlin 解压缩_kotlin 对上传的压缩包进行分析-程序员宅基地

文章浏览阅读638次。fun unZip(zipFile: String, context: Context) { var outputStream: OutputStream? = null var inputStream: InputStream? = null try { val zf = ZipFile(zipFile) val entries = zf.entries() while (en..._kotlin 对上传的压缩包进行分析