以不太严谨但是有逻辑的数学理论---剖析VIO之预积分

AutoGalaxy

已于 2022-09-10 11:28:13 修改

阅读量978

点赞数 5

分类专栏： VIO 文章标签： c++

于 2022-07-13 21:57:41 首次发布

本文链接：https://blog.csdn.net/qq_43526137/article/details/125772426

版权

VIO 专栏收录该内容

24 篇文章 1 订阅

订阅专栏

一. IMU测量模型

二. IMU运动模型（先明确目标：运动模型干的事就是从k时刻的状态递推出k+1时刻的状态，很明显的马氏链）

先介绍一下传统捷联惯导基本思想：

利用牛二，位置的导数等于速度的，速度的导数等于加速度，假设参考坐标系下的载体的初始速度和初始位置已知，利用载体运动过程中的加速度信息，就可以不断进行积分运算，实时更新速度和位置。

上述只是理想情况，实际还要更新姿态，由于加速度是由与载体固联的加速度计测量得到的，每一时刻的加速度都是在当前载体系下得到的，进行速度和位置的积分，前提是把加速度统一到一个坐标系。更新姿态的作用就在此：实时更新姿态，可以得到当前载体系相对于参考系的姿态，从而将载体系的加速度投影到参考系下。

在这里引用邱博写的文档里面的一段话，介绍以下知识点：

（1）传统捷联惯性导航的递推算法（重要的问题：处理重力）

（2）基于BA的视觉融合算法（这一部分疑点还是较多，绝对姿态可观，提高优化速度有没有更加直观的解释呢？目前有一个解释但是不知道对不对，已经发在知乎上，最好用数学公式表达一下，后面再做具体分析）

IMU和视觉融合使得IMU的bias可观（这句话怎么理解呢？）

IMU和相机冗余测量抑制累积误差（这句话怎么理解？）

2.1 预备知识

2.1.1 旋量法推导旋转矩阵的导数

我们首先聊一聊刚体旋转速度的表示~~（旋量法）

一般而言，我们用向量表示物体的位置，用旋转矩阵表示物体的姿态。向量和旋转矩阵组合在一起可以用一个齐次矩阵（4*4）来表示，用来表示物体的位姿。

速度是位姿的导数，包含了位置的导数和姿态的导数。位置的导数可以用线速度来表示，可以对向量求导得到，这种线速度理解比较直观。但是物体的旋转速度（或者说姿态的导数，又或者说旋转矩阵的导数）该如何表示呢？

如果用 $\dot{R}$ 表示旋转速度，那么 $\dot{R}$ 是何方神圣呢？难道是对R中每个元素求导吗？答案肯定是否定的，那么我们该如何表示旋转速度呢？这是个很关键的问题，为了解决这个问题，请看下面的分析！

假设我们有一个物体，它在空间中进行一个旋转运动，我们不考虑它的线速度，只考虑它旋转运动的角速度；我们在空间内建立一个固定的空间坐标系S（一般是世界坐标系/忽略地球自转时也可以叫惯性系），在物体上建立一个物体坐标系B，这样我们就可以用坐标系B相对于S的运动来描述 物体在空间内的运动。

我们知道这样一个事实（在四元数（quaternion.pdf (krasjet.github.io)）一文也谈到过）：物体在空间中任意旋转运动，可以等效为绕着某一根的旋转轴以一定的速度进行的旋转运动。

假设物体的旋转角速度的方向是单位向量 $\vec {w}_{s}$ ，其旋转速度大小是 $\dot{\theta}$ ；那么向量 $\vec{w}=\dot{\theta}\vec {w}_{s}$ 的方向表示旋转角速度方向，其模长表示旋转角速度的大小。

根据台湾大学机器人学老师课程所讲的，一个旋转矩阵可以用如下式来表示

$R=\[ \begin{matrix} \vec{X}_{B} &\vec{Y}_{B}&\vec{Z}_{B} \end{matrix} \]$

上述旋转矩阵表示的是：坐标系B的姿态。可以把B坐标系下的坐标，左乘这个旋转矩阵，把B坐标系下的坐标变到S系下的坐标。

那么显示，要描述R的导数，就是要描述 $\vec{X}_{B}\ \vec{Y}_{B}\ \vec{Z}_{B}$ 的导数；三个向量，以 $\vec{X}_{B}$ 为例， $\vec{X}_{B}$ 代表B坐标系X轴在参考坐标系下的单位向量；求旋转矩阵的导数问题，就转化为求三个向量的导数的问题。或者说：这三个向量的导数，组合起来，就变成旋转矩阵的导数。向量的导数自然是很好求的。

我们知道，一个向量 $\vec{e}$ 绕着某个轴 $\vec{w}$ 旋转时，该向量的线速度 $\dot{\vec{e}}$ 可以用下式表示：

$\dot{\vec{e}} = \vec{w} \times \vec{e}$

考虑到有人可能不知道这个来源，现在给出一个具体证明（从纯数学的角度给出的证明）：

Proof：

符号说明： $R_{B}^{A}$ :表示坐标系B相对于坐标系A的姿态

: $P^{B}$ ：表示一个物体在坐标系B下的坐标

已知：

$RR^{T}=I$

两边对时间求导有

$\dot{R}R^{T}+R\dot{R}^{T}=0$

移项有

$\dot{R}R^{T}=-R\dot{R}^{T}$

此时令

$S=\dot{R}R^{T}$

那么S是反对称矩阵。假设B坐标系有一个物体，坐标为 $P^{B}$

$P^{A}=R_{B}^{A}P^{B}$

又假设这个物体相对于B坐标系始终不动，那么两边对时间求导有（一般VIO中，存在body坐标系，IMU坐标系，世界坐标系，物体相对B坐标系始终不动，有点类似IMU相对body始终不动）

$\dot{P}^{A}=\dot{R}_{B}^{A}P^{B}$

又因为有 $P^{B}=R_{A}^{B}P^{A}=(R_{B}^{A})^{T}P^{A}$ ，代入上式可以得到

$\dot{P}^{A}=\dot{R}_{B}^{A}(R_{B}^{A})^{T}P^{A}$

又因为 $\dot{P}^{A}$ 定义为向量 $P^{A}$ 的线速度，因此，上式可以进一步改写为

$\vec{V}^{A}=SP^{A}$

因为S是反对称矩阵，因此有 w^ = S（w是三维向量，^代表取反对称）

因此，我们就可以得到

$\vec{V}^{A}=\vec{w} \times P^{A}$

上式代表的含义就是，物体在A系下的位置的导数，等于，坐标系B的旋转角速度（这一部分貌似通过这种证明方式不能够很直观的说明，但是实际上就是这个意思，待定） 与 物体在A系的坐标系位置 的叉乘

至此，上述的式子证明完毕。

下面，我们再回到原问题：我们要求出旋转矩阵的导数形式，而目前我们能够利用的东西只有 $\dot{\vec{e}} = \vec{w} \times \vec{e}$ ，并且上述原问题可以等效为求出旋转矩阵中 $\vec{X}_{B}\ \vec{Y}_{B}\ \vec{Z}_{B}$ 的导数，那就求把，依次有下式成立：

$\dot{\vec{X}}_{B}=\vec{w} \times \vec{X}_{B}$

$\dot{\vec{Y}}_{B}=\vec{w} \times \vec{Y}_{B}$

$\dot{\vec{Z}}_{B}=\vec{w} \times \vec{Z}_{B}$

我们再写成更加紧凑的形式：

$[\dot{\vec{X}}_{B}\ \dot{\vec{Y}}_{B}\ \dot{\vec{Z}}_{B}]=[w \times \vec{X}_{B}\ w \times \vec{Y}_{B}\ w \times \vec{Z}_{B}]$

这里定义：

$\dot{R} = [\dot{\vec{X}}_{B}\ \dot{\vec{Y}}_{B}\ \dot{\vec{Z}}_{B}]$

由于叉乘相对于左乘反对称矩阵

因此，最终可以得到：

$\dot{R}=w^{\wedge} R$

你以为这就结束了？其实这只是通过旋量法推导出的表达式，跟大部分SLAM里面所需的形式不同，上面这一部分只是热身，贴出来，供各位小伙伴学习。

2.1.2 通过李代数扰动模型来求解旋转矩阵的导数

$\frac{dR}{dt} \triangleq \lim_{\delta t \rightarrow 0} \frac{Re^{(w\delta t)^{\wedge}} - R}{\delta t}$

$\lim_{\delta t \rightarrow 0} \frac{Re^{(w\delta t)^{\wedge}} - R}{\delta t} = \lim_{\delta t \rightarrow 0} \frac{R(I+(w\delta t)^{\wedge}) - R}{\delta t}= \lim_{\delta t \rightarrow 0} \frac{R(w\delta t)^{\wedge}}{\delta t} = Rw^{\wedge}$

因此咱们就i可以得到： $\dot{R}=Rw^{\wedge}$

2.2 IMU的运动模型（我们要明确目标是什么：根据k时刻的加速度值和角速度值以及k时刻的速度、位置、姿态，要递推出k+1时刻的速度、位置、姿态）

2.2.1 运动模型的微分方程

结合2.1.2的证明，以及传统捷联惯导的基本思想，可以引出IMU运动模型的微分方程：

$\left\{ \begin{aligned} \dot{R}_{wb_{t}} &= R_{wb_{t}} (w^{b_{t}})^{\wedge}\\ \dot{v}_{t}^{w} &= a_{t}^{w} \\ \dot{p}_{wb_{t}} &= v_{t}^{w} \end{aligned} \right.$

2.2.2 使用欧拉法离散化的IMU模型递推方程

对上式两端在 $[t,t+\Delta t]$ 积分，有下式成立（注意，姿态更新部分你仅仅只是直观理解，更具体的推导证明估计得看惯性导航那本书，目前本人看不太懂）

此时将IMU测量模型代入离散运动方程：

注意，上式中间的 $n_{t}^{a}$ 和 $n_{t}^{b}$ 是离散化后的服从高斯分布的随机变量，方差与原连续时相差了一个 $\Delta t$ 因子，这里的离散化暂时不做推导，后面有时间再补充，可以肯定的是，这一步是可以严谨推导出来的。

设 $\Delta t$ 固定，即IMU采样频率固定，那么可以将上式写成更加一般的离散形式：

三. 正式开始引入，基于离散化欧拉积分IMU运动模型的IMU预积分

3.1 不使用预积分的计算量分析

以IMU获取数据的时间点为时间戳，有... i，i+1，i+2，....，j-1，j，j+1...等等，但是IMU获取数据的频率太快了，相机仅仅只在i时刻和j时刻出现，依次类推，间隔 (j-i)个时间单位出现一次。

根据欧拉积分，可以由（输入） i时刻到j-1时刻的所有IMU测量值，以及i时刻的姿态 $R_{wb_{i}}$ ，位置 $p_{i}^{w}$ ，速度 $v_{i}^{w}$ ，得到（输出）j时刻的姿态，位置和速度。注意抓输入和输出。

从上述式子，可以看出，每次我们经过优化算法，都会更新 $R_{wb_{i}}$ ， $p_{i}^{w}$ 和 $v_{i}^{w}$ 。

对于姿态更新式，我们要求出j时刻的姿态，得重新计算 j-i次矩阵乘法才能得到j时刻的姿态，而且还要记录中间时刻的姿态，因为下面的速度更新式需要中间的姿态（总共记录j-i个姿态）；

对于速度更新式，利用姿态更新式保存的姿态，我们得计算j-i 次矩阵向量乘法，以及j-i次加法，才能得到j时刻的速度，同时还要记录中间的速度（总共记录j-i个速度）；

对于位置更新式，要计算 2*(j-i）次加法，j-i次矩阵向量乘法，才能得到最终j时刻的位置。

综上所述，每次优化更新一下姿态、速度和位置，那么为了得到下一时刻的姿态、速度和位置，就得计算至少 3(j-i)次加法，3(j-i)次矩阵向量乘法以上，才能完成一次优化更新之后的递推。

上述这一大段话，可以用更加精炼的一句话表示：不使用预积分，会导致每次更新一次 $R_{wb_{i}}$ ， $p_{i}^{w}$ 和 $v_{i}^{w}$ ，就得重新从头积分求 $R_{wb_{j}}$ ， $p_{j}^{w}$ 和 $v_{j}^{w}$

（小提示：其实上面存储的姿态和速度暗含了一个信息，那就是做了坐标变换，每次我们都妄图在世界坐标系下直观的计算，因此导致了计算量的增长，换个思路，先相对计算，最后再变换到世界坐标系下，因为中间的那些量不是我们所特别关心的，我们只关心i和j时刻的状态在世界坐标系下是怎么样的，至于其他时刻，其实我们不是特别关心，预积分量就是利用这点，（个人不太严谨地理解））

关于上面图片的公式，我这里再给出一些说明吧，可能这里看不太懂：

首先我们要明确目标：需要由 i,i+1,i+2,...,j-1 时刻的IMU数据，以及i时刻的状态，递推出j时刻的状态。

我们目前已知的条件是2.2.2节中一般的离散化形式：

即我们利用欧拉法可以从 i 时刻递推出 i+1时刻，那依次类推，要递推出 j 时刻的，这里给出不太严谨的数学归纳法证明来说明：

Proof：

由2.2.2中最后一张图的递推形式

我们先推导 $R_{wb_{j}}$ 是怎么由以前时刻的IMU测量值和状态表示

已知：

$R_{wb_{i+1}}=R_{wb_{i}} e^{((\widetilde{w}_{i}^{b}-n_{k}^{g}-b_{k}^{g})\Delta t)^{\wedge}}$

$R_{wb_{i+2}}=R_{wb_{i+1}} e^{((\widetilde{w}_{i+1}^{b}-n_{k}^{g}-b_{k}^{g})\Delta t)^{\wedge}}$

......

依次类推，可以得到

我们再来推导 $v_{j}^{w}$ 是怎么由以前的IMU测量值和状态表示的：

已知

依次类推，最终可以得到：

其中的 $\Delta t_{ij} = (j-i)\Delta t$ 。

最后再推导 $p_{j}^{w}$ 是怎么由以前的IMU测量值和状态表示：

已知：

$p_{i+1}^{w}=p_{i}^{w}+v_{i}^{w}\Delta t + \frac{1}{2}(R_{wb{i}}(a_{i}^{b}-b_{i}^{a}-n_{i}^{a})-g^{w})\Delta t^{2}$

$p_{i+2}^{w}=p_{i+1}^{w}+v_{i+1}^{w}\Delta t + \frac{1}{2}(R_{wb{i+1}}(a_{i+1}^{b}-b_{i+1}^{a}-n_{i+1}^{a})-g^{w})\Delta t^{2}$

......

依次类推，最终可以得到：

至此，一切得证。

3.2 为了解决上述计算量的问题，引入了预积分量(定义在i帧与j帧之间的)

在开始详细叙述之前，我还是想强调一点：引入预积分量之后，我们的最终目标是希望根据以下输入：

输入：（1）预积分量

（2）i 时刻的姿态、速度、位置

仅仅根据以上现有的数据，得出以下输出

输出：（1）j 时刻的姿态、速度、位置

可以发现，这时候的输入多了一个预积分量的家伙。接下来我们就需要构造这些输入了

3.2.1 构造预积分量

为了使得引入预积分量之后，能够减少计算量，那么相对关系这个时候就非常重要了。其实直观想一想也确实是这样的，假设IMU 在 i时刻有一个坐标系，在j时刻有一个坐标系，那么j坐标系相对于i坐标系其实是一直不变的。

我们先来看一下离散化后的基于欧拉积分的IMU运动模型：（3.1中的模型）

（1）我们要引入的第一个预积分量是上面第一个表达式的蓝框框

定义为：

依据 i,...,j-1 时刻的IMU陀螺仪数据就可以求出这个预积分量。

（2）引入的第二个预积分量是上面第二个表达式的黄框框的一部分

先由黄框变形：

此时令速度预积分量为

上式中，第一个等式，需要 $R_{b_{i}b_{k}}$ ，以及i到j-1时刻的加速度计数值，才能求出速度预积分量；

那么 $R_{b_{i}b_{k}}$ 怎么求呢？

我们不妨展开变一下形有 $R_{b_{i}b_{i+1}},R_{b_{i}b_{i+2}},...$ ：

由3.1中姿态的递推式可以得到：

$R_{wb_{k}}=R_{wb_{i}}R_{b_{i}b_{k}}$

因此我们可以得到：

因此，我个人猜测在计算姿态预积分量的时候，是不是得先开辟一块内存用来记录： $R_{b_{i}b_{i+1}},R_{b_{i}b_{i+2}},...$

其实还有这样一个关系： $R_{b_{i}b_{k}}=(R_{wb_{i}})^{T}R_{wb_{k}}=\Delta R_{ik}$

（3）第三个预积分量就不是那么直观了，位置预积分量定义如下红框，

要计算这个预积分得需要加速度、 $R_{b_{i}b_{k}}$ ， $\Delta v_{ik}$

加速度可以读出来， $R_{b_{i}b_{k}}$ 在（2）中已经说明是在（1）中求的，那现在 $\Delta v_{ik}$ 怎么求呢？我估计也是利用（2）中的预积分公式提前计算出来

至此，预积分量已经引出，如何获取输入值计算也大致说明了，带入预积分量得到的IMU运动模型递推公式如下所示：

很显然，预积分量是固定的值，变的只有状态量，因此，计算量大大减少。

四. 预积分测量值以及测量噪声（本节的唯一目标：给出预积分测量噪声协方差的计算式）

递推的精度，也要求预积分量的测量得是准确的；这里把预积分看作一个随机变量，有理想值 ”加“白噪声的形式。

在推导之前，作了如下的一些假设：预积分计算的区间内bias是相等的（预积分计算的区间通常是相机的两帧之间）

4.1 旋转量预积分

这里直接给出定义，邱博的文档已经写的比较清楚了，我下面再做进一步的细化，可以得到最终结果

最终得到下面这样的式子（我们所关心的，预积分量可以由测量值”加“测量噪声）

4.2 速度预积分量

具体证明，邱博文档有很清楚的证明，我这里不再说明

4.3 位置预积分量

同理，邱博文档说明很清楚，这里不再说明

4.4 测量值 = 理想值 + 噪声

通过以上步骤，最终得到了IMU预积分量形如测量值 = 理想值 + 噪声的形式

4.5 分析预积分的噪声（即服从的分布形式）

你的4.5和4.6节有一些正负号搞错了。

4.6. 预积分量测量噪声的递推形式（预积分测量噪声的协方差矩阵）

注意上面最后一个离散高斯分布是陀螺仪的而不是加速度计的。

由上面这三个式子，最终可以推出下式：

预积分的测量噪声的协方差递推公式。

注意！注意！注意！以上的推导都是在i到j时刻之间的bias不变的条件下进行推导的。 bias若变化，参考邱博p12页第五节的推导。

开始推导具体A和B的表达式（参考深蓝学院文档），邱博文档也给出了A和B的具体形式，但是为了与程序对应，还是以深蓝学院为准。

（一）预积分基于误差随时间变化的递推方程

上面这个式子成立的条件依据源于邱博文档

五. bias变化时（暂时不看邱博的文档，写的推导过于复杂繁琐）

六. 残差及雅可比推导

残差包括：视觉重投影误差，逆深度重投影误差，

残差定义：预积分计算值（由非IMU的其他方式猜测/估计的预积分值）与测量值的差别。

在optimization时，将所有待优化的导航状态进行lifting（更新），从而影响预积分计算值以及预积分测量值，进而改变残差，optimization的最终目的是使残差（加权范数）最小化

残差的雅可比矩阵是对两个时刻的状态量，外参以及逆深度求导。

6.1. 视觉重投影误差

定义在归一化相机坐标系下的估计值和观测值之差。

估计值是特征点的三维空间坐标（相机坐标系下），观测值是归一化相机平面的坐标（从像素坐标投影过去的）。

6.2. 逆深度重投影误差

第i帧的相机坐标投影到第j帧下

把上式拆为三维坐标形式：（傻眼了吧，不知道怎么拆解了吧）

这个拆解确实有一些复杂

非常容易搞混的一个点，就是总以为变换矩阵是正交矩阵，只有旋转矩阵才是正交矩阵。

经历以上的符号分析，下面开始正式进入残差雅可比的推导

6.3. 残差雅可比的推导（链式求导）

首先残差长下面这个样子：是rc，这个2行1列的vector

一个2维向量对3维向量求导，根据向量求导法则，很自然得可以得出导数结果是2x3维。

很奇怪，本身就有一个视觉重投影残差雅可比了，更加奇怪的是这里是对微小量求导，这个没有问题，因为求导的法则有如下规则

另一个奇怪的点是，本身就有残差雅可比了，用来更新i和j状态的位姿，外参，以及逆深度，这个预积分量的残差雅可比是什么鬼？

七. IMU误差相对两帧PVQ的雅可比（是深蓝学院的文档解释得不太清楚，还是邱博解释得更加清楚，再加上苏工程师的说明就更加清楚了）

就是对

其实第一次听这句话，我反正挺懵的，先弄懂这个公式是啥样，再来深究。

其实这句话的意思是 预积分的误差。

所以，首先你要写出 PVQ 的通用表达式：参考以下链接。

VIO----相关公式解释说明_AutoGalaxy的博客-CSDN博客

最后可以得到：

上面的三个蓝色项的具体表达式如下所示，

八. 邱博定义的预积分残差