SLAM_相机与imu的融合基础知识

旧城等待， 2022-11-13 04:29 931阅读 0赞

**目录**

1. IMU基本原理

坐标系

状态模型

IMU模型推导

2. IMU的误差模型

IMU误差分类

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

参考1：

参考2：

陀螺仪的误差模型

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

4. 融合方式概述

4.1 基于滤波的融合方法

松耦合

紧耦合

4.2 基于优化的融合算法

松耦合

紧耦合

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

6. VI-ORBSLAM 论文阅读笔记

Tracking

Local Mapping

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

--------------------

# 1. [IMU基本原理][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坐标系。其特点是与地球固定在一起，随地球一起转动。
     *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]
 *  **Earth-Centered-Inertial (ECI) Frame**：地心惯性坐标系 ECI。以地心为坐标原点，向北为 z 轴，x-y 平面为赤道平面，x 轴指向春分点（vernal equinox point，即每年春分时日心-地心连线与赤道的交点）。ECI 不跟随地球自转，在惯性导航中视为惯性坐标系。MSCKF 二代 \[3\] 使用 ECI 为参考坐标系 \{G\}。
 *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1][]
 *  **Body Frame**：体坐标系。原点在导航体的质心，固连在导航体上，用来表示导航体的姿态。
 *  参考：[进阶课程⑭ | Apollo自动定位技术——三维几何变换和坐标系介绍][_ Apollo]
 *  **车体坐标系：**车体坐标系原点在载体质量中心与载体固连处（相对于车载，选取原点位于后轴中心位置），X轴沿载体轴向指向右，Y轴指向前，Z轴与X轴和Y轴满足右手法则指向天向。该坐标系通常称为“右-前-上（R-F-U）”坐标系。它是一个局部坐标系，它与N系或者导航坐标系、当地水平坐标系之间的旋转关系表示现在车的姿态。
    
     *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 2][]
 *  **IMU坐标系：**IMU坐标系其实和车体坐标系基本上是一样，它的原点在陀螺仪和加速度计的坐标原点，XYZ三个轴方向分别与陀螺仪和加速度计对应的轴向平行。在不考虑安装误差角的情况下，载体坐标系和IMU坐标系是一样的。
    
     *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 3][]

## 状态模型 ##

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

## [IMU模型推导][IMU 1] ##

IMU 的状态量通常表示为：

![20210326171344395.png][]

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

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

![20210326171548937.png][]

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

# 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)：  
            ![20210404130005389.png][]
         *  其中 δ() 表示狄拉克函数。
 *  **对比**：高斯白噪声误差 和 随机游走误差 不同离散采样时间对他们的影响：
    
     *  离散化后，采样时间越长，高斯白噪声的影响越小(该误差越小)，因为会相互抵消；
        
         *  ![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 4][]
     *  而对于 随机游走误差 采样时间越长，影响越大(该误差越大)
        
         *  ![20210404132303881.png][]

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

### 参考1： ###

[https://www.zhihu.com/question/352293998/answer/874374903][IMU_bias]

随机误差：

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

确定性误差 对比 ：

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

### 参考2： ###

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 5][]

## 陀螺仪的误差模型 ##

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

![2021040415390398.png][]

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

![20210404153932347.png][]

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 6][]

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

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

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

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

# 4. [融合方式概述][IMU] #

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

## 4.1 基于滤波的融合方法 ##

### 松耦合 ###

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

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

### 紧耦合 ###

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

![紧耦合框架][SouthEast 1]

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

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

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 7][]

**紧耦合**是把图像的特征加到特征向量中去，这样做优点是可以免去中间状态的累计误差，提高精度，缺点是系统状态向量的维数会非常高，需要很高的计算量；**松耦合**是把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的融合（一）][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 论文阅读笔记][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）初始化比较][ORB-SLAM3 _VINS-Mono_VI] #

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 8][]

[IMU]: https://blog.csdn.net/danmeng8068/article/details/80601660
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221022/4948a20c9d5b4fefb677749c75e05c47.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 1]: /images/20221022/252f93d35efa4ee3923f1b6cf7c938b4.png
[_ Apollo]: https://developer.baidu.com/article.html#/articleDetailPage?id=290582
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 2]: /images/20221022/158740a1cb1547b79510df410bc67f03.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 3]: /images/20221022/740d16c71aa64e199686ca3b2aa33bf8.png
[IMU 1]: http://fzheng.me/2016/11/20/imu_model_eq/
[20210326171344395.png]: /images/20221022/b05d084e2cb648acbb24e1b3f552f46b.png
[20210326171424163.png]: https://img-blog.csdnimg.cn/20210326171424163.png
[20210326171438461.png]: https://img-blog.csdnimg.cn/20210326171438461.png
[20210326171452444.png]: https://img-blog.csdnimg.cn/20210326171452444.png
[20210326171508922.png]: https://img-blog.csdnimg.cn/20210326171508922.png
[20210326171525221.png]: /images/20221022/6bf6c8e4b7bf4d6a83ed3d8371dbd5b9.png
[20210326171548937.png]: /images/20221022/cccc77b57d344ccea456030da0c92b44.png
[20210326171611511.png]: /images/20221022/388a577fce414d4a86118f9888adc09d.png
[20210404130005389.png]: /images/20221022/42aeb6778d584492815cbdadc6a86de5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 4]: /images/20221022/59cf0bd8be324a3f8ea5d5c5bb6276aa.png
[20210404132303881.png]: /images/20221022/3f3a9ebbb31148a9bb0ae97af9408724.png
[IMU_bias]: https://www.zhihu.com/question/352293998/answer/874374903
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 5]: /images/20221022/b8b9e88b6b2b478d9b8874a8e96a6101.png
[2021040415390398.png]: /images/20221022/576c99c1bbd54070a17ef49b793730c3.png
[20210404153932347.png]: /images/20221022/f013dc02712a4a40b3df6d3eccbe01d7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 6]: /images/20221022/77ae65c9a0f7404ebb98a89aa5551181.png
[6_9_IMU_VRU_AHRS]: https://my.oschina.net/u/4052474/blog/4911827
[SouthEast]: /images/20221022/a24d1aa09fce49459d2020b14d22b87d.png
[SouthEast 1]: /images/20221022/b5491b06b6df4cf29c765e6aa9d0388c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 7]: /images/20221022/536503f241ff49e0b83f64d43a38a1a7.png
[okvis]: https://github.com/ethz-asl/okvis
[VINS]: https://github.com/HKUST-Aerial-Robotics/VINS-Mono
[imu]: https://blog.csdn.net/qq_18661939/article/details/53574981
[VI-ORBSLAM]: https://blog.csdn.net/guaijiaodie2064/article/details/103299964
[ORB-SLAM3 _VINS-Mono_VI]: https://blog.csdn.net/weixin_43827285/article/details/112563167
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70 8]: /images/20221022/a315ee03730d4ae9b5931931d90d16c4.png