1. IMU基本原理

惯性测量单元(Inertial Measurement Unit，简称 IMU)一般由3轴加速度计和3轴陀螺仪组成。加速度计为力传感器，可根据各方向受力(包含重力)情况计算每个轴上的加速度。陀螺仪为角速度检测仪，可根据每个轴上的角加速度得到各个轴上的角度变化。惯导解算主要是通过加速度计测得的载体加速度和陀螺仪测得的载体相对于导航坐标系的角速度，来对载体的位置、 姿态及速度进行解算。注意仅仅使用IMU，在对加速度进行两次积分得到运动距离的过程中，容易产生累计误差。

坐标系

惯性导航中常用的坐标系有地心惯性坐标系、导航坐标系以及载体坐标系。载体坐标轴一般指向载体正前、正右及正下方。

  • Earth-Centered-Earth-Fixed (ECEF) Frame：地心地固坐标系 ECEF。以地心为坐标原点，向北为 z 轴，x-y 平面为赤道平面，x 轴指向经纬度 (0,0) 点。非惯性坐标系。MSCKF 一代 [1] 使用 ECEF 为参考坐标系 {G}。
    • ECEF地心地固坐标系也称为e系，它的原点也是地球的原点， Z轴指向北极， 它与前面的ECI的区别就在于它的XY随着地球的自转而转动，它是以地球为基准的。它的X轴指向格林威治的子午面的交线， Y轴在赤道平面内与X轴、Z轴满足右手系法则，常用的如WGS84坐标系。其特点是与地球固定在一起，随地球一起转动。
  • Earth-Centered-Inertial (ECI) Frame：地心惯性坐标系 ECI。以地心为坐标原点，向北为 z 轴，x-y 平面为赤道平面，x 轴指向春分点（vernal equinox point，即每年春分时日心-地心连线与赤道的交点）。ECI 不跟随地球自转，在惯性导航中视为惯性坐标系。MSCKF 二代 [3] 使用 ECI 为参考坐标系 {G}。
  • Body Frame：体坐标系。原点在导航体的质心，固连在导航体上，用来表示导航体的姿态。
  • 参考：进阶课程⑭ | Apollo自动定位技术——三维几何变换和坐标系介绍
  • 车体坐标系：车体坐标系原点在载体质量中心与载体固连处（相对于车载，选取原点位于后轴中心位置），X轴沿载体轴向指向右，Y轴指向前，Z轴与X轴和Y轴满足右手法则指向天向。该坐标系通常称为“右-前-上（R-F-U）”坐标系。它是一个局部坐标系，它与N系或者导航坐标系、当地水平坐标系之间的旋转关系表示现在车的姿态。

  • IMU坐标系：IMU坐标系其实和车体坐标系基本上是一样，它的原点在陀螺仪和加速度计的坐标原点，XYZ三个轴方向分别与陀螺仪和加速度计对应的轴向平行。在不考虑安装误差角的情况下，载体坐标系和IMU坐标系是一样的。


状态模型

惯导系统的状态量包括 IMU 的位置、姿态、速度以及加速度计和陀螺仪的两个零偏，其中姿态采用四元数表示，状态量可表示为 16 维的列向量。具体可参考以下链接： 

IMU模型推导

IMU 的状态量通常表示为：

这里我们使用和 MSCKF [1] 一样的 notation。用 {I} 表示 IMU 坐标系，{G} 表示参考坐标系。IMU 的姿态由旋转量 和平移量表示。更具体来说，前者为将任意向量从 {G} 坐标映射到 {I} 坐标的旋转量，用单位四元数表示；后者为 IMU 在 {G} 下的三维位置。表示 IMU 在 {G} 下的平移速度。另外两个量​ 和表示陀螺仪（角速度计）和加速度计的 bias。可以注意一下这里除了 bias 之外的状态量的时间维度：平移量表达到速度（p 和 v，对时间的一阶导），因为 IMU 只提供到加速度（对时间的二阶导）的测量；旋转量只表达姿态量（对时间的零阶导），因为 IMU 提供到角速度（对时间的一阶导）。状态量的估计可以由 IMU 测量积分得到。

对于 IMU 状态估计问题，需要提供运动模型、观测（噪声）模型、估计误差模型：

这是一个通用模型，我们用 x表示真实状态量（待估计，不可知），用 z 表示观测量，n 表示观测噪声，表示当前的状态估计量。

2. IMU的误差模型

IMU误差分类

  • 加速度计和陀螺仪的误差可以分为：确定性误差，随机误差。
  • 确定性误差：可以事先标定确定，包括： bias（如零偏误差）, scale（IMU测量值与实际值之间的比例，不同于视觉中的表示尺度大小的scale） …
    • 其他确定性误差：
    • 零偏重复性 Run-to-Run Bias/Scale Factor : 每次上电零偏可能不同
    • 零偏稳定性 In Run (Stability) Bias/Scale Factor：一次上电后，运行过程中，零偏在不同发生变化
    • 温度独立性误差 Temperature-Dependent Bias/Scale Factor： 如电子器件，测量电压容易受到温度的影响
  • 随机误差：通常假设噪声服从高斯分布，包括：高斯白噪声， bias(确定性误差也会有一定的变化)，随机游走…
    • 高斯白噪声
      • IMU 数据连续时间上受到一个均值为 0，方差为 σ，各时刻之间相互，独立的高斯过程 n(t)：
      • 其中 δ() 表示狄拉克函数。
  • 对比：高斯白噪声误差 和 随机游走误差 不同离散采样时间对他们的影响：
    • 离散化后，采样时间越长，高斯白噪声的影响越小(该误差越小)，因为会相互抵消；
    • 而对于 随机游走误差 采样时间越长，影响越大(该误差越大)

怎么才能形象的说明IMU的bias随机游走？

参考1：

https://www.zhihu.com/question/352293998/answer/874374903

随机误差：

  • 随机游走: 随机游走，其实就是高中时候学过的布朗运动。
  • 可以等效为马尔可夫过程，就是这次的bias，是在上次的bias基础上随机变化一个值造成的，而下次的bias是在这次的bias基础上又随机变化一个值得到，经过一段时间的累计，就会有一个大的偏差。
  • 在下面1）假设的均值不变（代表随机游走强度为零），实际中，每隔一段时间取均值，发现它会边，这就是随机游走决定的，原理就是马尔可夫形式的累计误差。

确定性误差 对比 ：

  • 1）零偏稳定性：即噪声，反应数据围绕均值的波动情况，静态下认为均值不变
  • 2）常值零位：可以理解为是输出减掉输入，一般指均值
  • 3）零偏重复性：每次上电得到的bias不一样大，重复性反应他们的差别程度

参考2：

深蓝VIO课程 SLAM进阶从零开始手写VIO

陀螺仪的误差模型

考虑尺度因子，高斯白噪声，以及 bias, 陀螺仪的误差模型如下：

低端传感器，考虑加速度对陀螺仪的影响，即 g-灵敏度：

陀螺仪受四种噪声的影响分别如下图所示：

3. 6轴,9轴,IMU,VRU和AHRS分别指的是什么?

常见的 IMU 为六轴传感器，配备输出三轴加速度的加速度计和输出三轴角速度的陀螺仪。九轴 IMU 还会配备输出三轴姿态角的磁力计

6轴9轴的概念很好理解：说白了就是模块上装了哪些，多少传感器

  • 6轴 : 三轴(XYZ)加速度计 + 三轴(XYZ)陀螺仪(也叫角速度传感器)
  • 9轴 : 6轴 + 三轴(XYZ)磁场传感器
  • 6轴模块可以构成 VRU(垂直参考单元)和IMU(惯性测量单元)，9轴模块可以构成AHRS(航姿参考系统)
  • IMU: 惯性测量单元，可以输出加速度和角速度。并不输出姿态角等其他信息
  • VRU: IMU的基础上内置姿态解算算法，可以输出姿态信息。

4. 融合方式概述

同SLAM发展过程类似，视觉融合IMU问题也可以分成基于滤波和基于优化两大类。 
同时按照是否把图像特征信息加入状态向量来进行分类，可以分为松耦合和紧耦合两大类。

4.1 基于滤波的融合方法

松耦合

松耦合将视觉传感器和 IMU 作为两个单独的模块，两个模块均可以计算得到位姿信息，然后一般通过 EKF 进行融合。 
松耦合框架

可参考的算法有ethz的Stephen Weiss的ssf和msf

紧耦合

紧耦合则是指将视觉和 IMU 得到的中间数据通过一个优化滤波器进行处理，紧耦合需要把图像特征加入到特征向量中，最终得到位姿信息的过程。由于这个原因，系统状态向量最终的维度也会非常高，同时计算量也很大。

紧耦合框架

可参考的算法有MSCKF(Multi-State Constraint Kalman Filter, 多状态约束下的Kalman滤波器)，ROVIO

对比：参考深蓝学院的公开课里面介绍

紧耦合是把图像的特征加到特征向量中去，这样做优点是可以免去中间状态的累计误差，提高精度，缺点是系统状态向量的维数会非常高，需要很高的计算量；松耦合是把VO处理后获得的变换矩阵和IMU进行融合，这样做优点是计算量小但是会带来累计误差。

4.2 基于优化的融合算法

松耦合

基于松耦合优化的工作不多，这里只给出一篇文章作为参考： 
Inertial Aided Dense & Semi-Dense Methods for Robust Direct Visual Odometry

紧耦合

提到基于优化的紧耦合，就不得不提okvis了，主体思想是建立一个统一的损失函数同时优化视觉与IMU的位姿，用到了sliding window的思想。（这方面博主也需要再深入研究一下） 
可参考okvis的github主页： 
okvis 
以及港科大今年上半年刚刚开源的VINS： 
VINS

5. 单目/双目与imu的融合（一）

视觉 SLAM 存在输出频率低、旋转运动时、或运动速率加快时定位易失败等问题，而 IMU 有输出频率高、能输出6DoF测量信息等优点。因此现阶段的一个研究热点是将视觉 SLAM 与 IMU 得到的位姿估计结果进行融合，得到更加鲁棒的输出结果。通过二者的融合，可以解决视觉位姿估计输出频率低的问题，同时位姿估计精度有一定的提高，整个系统也更加鲁棒。这也是一个 VIO (Visual Inertial Odometry) 问题。

目前单目slam存在初始化的尺度问题和追踪的尺度漂移问题，而双目也存在精度不高和鲁棒性不好的问题。针对这些问题，人们提出了融合imu的想法。

视觉与IMU融合之后会弥补各自的劣势，可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差；IMU可以为视觉提供快速运动时的定位，IMU可以提供尺度信息，避免单目无法测尺度 。

那么imu的作用是什么呢？

单目

  • （1)解决初始化尺度问题
  • （2)追踪中提供较好的初始位姿。
  • （3)提供重力方向
  • （4)提供一个时间误差项以供优化

双目

  • （1)追踪中提供较好的初始位姿。
  • （2)提供重力方向
  • （3)提供一个时间误差项以供优化

6. VI-ORBSLAM 论文阅读笔记

Tracking

紧耦合的VI-SLAM策略

通过新帧与旧帧的共视匹配点，构建重投影误差方程，进行优化解算当前帧状态，需要注意的是：
（1）初始pose由IMU递推得到，将地图点通过该pose，投影到新帧上，与新帧上相邻的特征点进行特征匹配；
（2）优化时的残差，由IMU残差和重投影误差两部分构成，都存在使用信息矩阵进行加权的情况：

  • 对IMU的速度、位置、角度，使用IMU预积分的信息矩阵
  • 对IMU的陀螺仪和角速度偏差，使用随机游走
  • 对重投影误差，使用特征点的观测信息矩阵（与特征点追踪次数相关？）

（3）有没有预加载地图的区别在于，构建残差方程时IMU的残差计算使用先验还是后验。

Local Mapping

LocalMap实现了一个固定窗口内的状态优化，其策略如下：
（1）固定窗口大小N；
（2）新帧一定会被加入窗口中（因其包含IMU状态）；
（3）不在窗口中却与窗口中的帧有共视点的帧，将参与残差方程的构建，但其位姿将被设为fixed（窗口外帧的pose非优化项）；
（4）VIO的优化相较于VO更加复杂，相当于每帧中添加了9个待优化项（速度，陀螺仪、加速度零偏），在需要跑实时的情况下需要设定合理的N的大小；
（5）VIO中的关键帧策略与VO中也不太一样，在VO中我们可以直接丢弃冗余帧以控制mapsize，但使用VIO时需要考虑IMU的积分时间随帧间间隔增长可信度下降迅速的问题，所以在系统设计中需要注意不能让两帧之间间隔时间过长（在本文中设置阈值为0.5s）

7. ORB-SLAM3 与VINS-Mono的视觉惯导（VI）初始化比较

转载自：https://visionary.blog.csdn.net/article/details/115215724