VIO第3讲：基于优化的IMU与视觉信息融合之IMU预积分

1 IMU预积分

1.1 IMU常规积分—连续时间—不适用

旋转矩阵表示（没有写出噪声项，0均值）

$$\begin{array}{c}{p}_{b_{k+1}}^w=p_{b_k}^w+{\nu }_{b_k}^w\Delta t_k+{\iint }_{t\in [k,k+1]}\left[R_t^w\right({\hat{a}}_t-b_{a_t})-g^w]d{t}^2\\ v_{b_{k+1}}^w=v_{b_k}^w+{\int }_{t\in [k,k+1]}\left[R_t^w\right({\hat{a}}_t-b_{a_t})-g^w]dt\\ q_{b_{k+1}}^w=q_{b_k}^w\otimes {\int }_{t\in [k,k+1]}\frac{1}{2}\Omega ({\hat{\omega }}_t-b_{{\omega }_t})q_t^{b_k}dt\end{array}$$

四元数表示—上面是k时刻至k+1时刻，下面是i-j时刻，道理相同

$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+\int {\int }_{t\in [i,j]}({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w)\delta t^2\\ & \begin{array}{r}{\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w)\delta t\end{array}\\ & {\mathbf{q}}_{w{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$

对于上面积分的离散形式-----中值法----把定积分展开！认为区间内加速度等于两个边界对应的平均值

$$\begin{array}{c}{p}_{b_{i+1}}^w=p_{b_i}^w+v_{b_i}^w\delta t+\frac{1}{2}{\bar{\hat{a}}}_i\delta t^2\\ v_{b_{i+1}}^w=v_{b_i}^w+{\bar{\hat{a}}}_i\delta t\\ q_{b_{i+1}}^w=q_{b_i}^w\otimes \left[\frac{1}{\frac{1}{2}{\overline{\omega }}_i\delta t}\right]\\ {\bar{\hat{a}}}_i=\frac{1}{2}\left[q_i\right({\hat{a}}_i-b_{a_i})-g^w+q_{i+1}({\hat{a}}_{i+1}-b_{a_i})-g^w]\\ {\overline{\hat{\omega }}}_i=\frac{1}{2}\left({\hat{\omega }}_i+{\hat{\omega }}_{i+1}\right)-b_{{\omega }_i}\end{array}$$

1.2 预积分形式

  预积分量是利用传感器数据积分得到的观测量。它和真实值即差一个噪声量！

① 定义

  预积分量：将一段时间内的 IMU 数据直接积分起来就得到了预积分量。

  VINS中定义，注意加速度和角速度都是IMU坐标系，展开后会带上噪声与偏置。
$$\begin{array}{rl}& {\mathbf{\alpha }}_{b_i{b}_j}\begin{array}{r}=\int {\int }_{t\in [i,j]}({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t})\delta t^2\end{array}\\ & {\mathbf{\beta }}_{b_i{b}_j}={\int }_{t\in [i,j]}({\mathbf{q}}_{b_i{b}_t}{\mathbf{a}}^{b_t})\delta t\\ & {\mathbf{q}}_{b_i}{b}_j={\int }_{t\in [i,j]}{\mathbf{q}}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\delta t\end{array}$$

  高博新书的定义如下（离散，已经噪声分离）
$$\Delta {\tilde{\mathbf{R}}}_{ij}=\prod _{k=i}^{j-1}\mathrm{Exp}\left(({\tilde{\mathbf{\omega }}}_k-{\mathbf{b}}_{g,i})\Delta t\right)$$

$$\begin{array}{r}\Delta {\tilde{\mathbf{v}}}_{ij}=\sum _{k=i}^{j-1}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,i})\Delta t.\end{array}$$

$$\Delta {\tilde{p}}_{ij}=\sum _{k=i}^{j-1}\left[(\Delta {\tilde{\mathbf{v}}}_{ik}\Delta t)+\frac{1}{2}\Delta {\tilde{\mathbf{R}}}_{ik}({\tilde{\mathbf{a}}}_k-b_{a,i})\Delta t^2\right].$$

② 如何推导出预积分定义

  就是把旋转进行了一个拆分，后面的项就和状态量无关了！这里也可以推出预积分残差，把非预积分量都移到等式坐标，即左边状态量，右边预积分量。

③ 用预积分表达IMU积分公式

  说白了，就是IMU只取和cam相同的帧，第i帧到第j帧之间的量会由预积分量来表示。
$$\left[\begin{array}{c}{\mathbf{p}}_{w{b}_j}\\ {\mathbf{v}}_j^w\\ {\mathbf{q}}_{w{b}_j}\\ {\mathbf{b}}_j^a\\ {\mathbf{b}}_j^g\end{array}\right]=\left[\begin{array}{c}{\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t-\frac{1}{2}{\mathbf{g}}^w\Delta t^2+{\mathbf{q}}_{w{b}_i}{\mathbf{\alpha }}_{b_i{b}_j}\\ {\mathbf{v}}_i^w-{\mathbf{g}}^w\Delta t+{\mathbf{q}}_{w{b}_i}{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{q}}_{w{b}_i}{\mathbf{q}}_{b_i{b}_j}\\ {\mathbf{b}}_i^a\\ {\mathbf{b}}_i^g\end{array}\right]$$

④ 预积分误差（残差）

  定义：一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的状态量进行约束。（预积分是通过传感器数据a和w积分得到的观测量，状态量pvq推断的是预测值）
$${\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{ba}\end{array}\right]}_{15\times 1}=\left[\begin{array}{c}{\mathbf{q}}_{b_{i}w}({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2)-{\alpha }_{b_i{b}_j}\\ 2[{\mathbf{q}}_{b_j{b}_i}\otimes ({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}){]}_{xyz}\\ {\mathbf{q}}_{b_{i}w}({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t)-{\mathbf{\beta }}_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$
  就像eskf一样，我们都是认为噪声是属于0均值的高斯分布，所以后面计算的是误差变量协方差，误差变量均值没什么用。

  高博新书写的还是很清楚的，传感器数据积分得到的观测量（预积分）和预测值（状态预测）之间相差一个噪声量。噪声量原本是存在于预积分式子中的，但通过噪声分离把它分离了出来。

  总而言之，预积分看起来很复杂，实际上只是为了避免庞大的计算找了一个相对状态量-预积分来表示状态。ESKF中因为只是算了一次直接积分，后续就不会再管了，所以不存在计算量问题。本质上都是一样的，只不过基于优化的方法会多次优化进行最小二乘迭代！

⑤ 预积分的离散形式

  预积分量：将一段时间内的 IMU 数据直接积分起来就得到了预积分量。所以要搞清楚预积分的本质，两个时间内不参杂状态量的一个积分。

  连续形式的定积分转换为离散形式，本质就是下面的一个过程。
$${\alpha }_{b_i{b}_{k+1}}-{\alpha }_{b_i{b}_k}={\alpha }_{b_k{b}_{k+1}}$$

1.3 为什么采用预积分

  通过观察公式可知，IMU 的直接积分需要依赖与第 k 帧的状态 v 和 R，当我们在后端进行非线性优化时，需要迭代更新第 k 帧的 状态v 和 R，这将导致我们需要根据每次迭代后值重新进行积分，这将非常耗时。
  那为什么预积分不会造成上面的影响？因为预积分利用了在t时刻测量的加速度、角速度，我们无法改变，也不会去优化。所以才说，预积分在状态pvq发生改变后不需要重新积分。所以不要认为a和w在变化，预积分也会变化。

1.4 预积分量的误差递推(噪声模型)

  写在前面，我们要搞清楚预积分量的误差是什么？噪声又是什么？否则就会逻辑混乱。从1.2中我们已经知道，预积分是一段时间内传感器数据的积分，是观测量；而我们同时利用计算的状态量PVQ可以得到预测值，观测和预测之间存在误差，也就是噪声量。我们这里要递推的误差就是预测-观测。（如果把预积分看作一种状态量，那么预积分误差以可以看作一种误差状态，这种误差状态可以通过递推来计算）

  上面为了方便计算，所以进行噪声分离，使得预积分量和状态量的预测值之间存在一个误差，我们这里的主要目的就是为了对这个误差量进行递推！同时，误差递推中加入了噪声项。

注意，这里类似ESKF，都是利用误差状态求的方差

  一个 IMU 数据作为测量值的噪声方差我们能够标定。现在，一段时间内多个 IMU 数据积分形成的预积分量的方差呢？

  线性高斯运算如下

  要推导预积分量的协方差，我们需要知道 imu 噪声和预积分量之间的线性递推关系

相邻时刻误差的线性传递方程

状态量误差：${\mathbf{\eta }}_{ik}=[\delta {\mathbf{\theta }}_{ik},\delta {\mathbf{v}}_{ik},\delta {\mathbf{p}}_{ik}]$

测量噪声：${\mathbf{n}}_k=[{\mathbf{n}}_k^g,{\mathbf{n}}_k^a]$-高斯白噪声

$${\mathbf{\eta }}_{ik}={\mathbf{F}}_{k-1}{\mathbf{\eta }}_{ik-1}+{\mathbf{G}}_{k-1}{\mathbf{n}}_{k-1}$$

协方差矩阵可以通过递推计算得到，其中

$${\mathbf{\Sigma }}_{ik}={\mathbf{F}}_{k-1}{\mathbf{\Sigma }}_{ik-1}{\mathbf{F}}_{k-1}^{\top }+{\mathbf{G}}_{k-1}{\mathbf{\Sigma }}_{\mathbf{n}}{\mathbf{G}}_{k-1}^{\top }$$

① 基于一阶泰勒展开的误差递推方程

​

② 基于误差随时间变化的递推方程

对比两种方法可知
$$\mathbf{F}=\mathbf{I}+\mathbf{A}\Delta t,\text{ G}=\mathbf{B}\Delta t$$
  实际上两种方法都是非常类似的，在ESKF里面，通过连续误差状态方程的系统矩阵A进行泰勒展开，将其转为离散时间下状态状态转移矩阵！
$$\begin{array}{rl}\Phi (t+\Delta t,t)& =\exp \left({\mathbf{A}}_c\Delta t\right)\\ & ={\mathbf{I}}_{6\times 6}+{\mathbf{A}}_c\Delta t+\frac{1}{2!}{\mathbf{A}}_c^2\Delta t^2+\dots \end{array}$$

③ 预积分误差传递的形式

  注意，我们这里的误差状态是指预积分的误差状态，不是PVQ。具体推导见崔华坤的论文解析

连续时间下

离散时间下

1.4 零偏更新

  理论上来讲，如果IMU零偏发生了变化，预积分就应该重新计算，因为预积分的每一步都用到了 i 时刻的 IMU 零偏。但是实际操作过程中，我们也可以选用一种取巧的做法：假定预积分观测是随零偏线性变化的

$$\begin{array}{c}\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i})=\Delta {\tilde{\mathbf{R}}}_{ij}({\mathbf{b}}_{g,i})\mathrm{Exp}\left(\frac{\partial \Delta {\tilde{\mathbf{R}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}\right),\\ \begin{array}{r}\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})\end{array}=\Delta {\tilde{\mathbf{v}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{v}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i},\\ \begin{array}{r}\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i}+\delta {\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i}+\delta {\mathbf{b}}_{a,i})\end{array}=\Delta {\tilde{\mathbf{p}}}_{ij}({\mathbf{b}}_{g,i},{\mathbf{b}}_{a,i})+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{g,i}}\delta {\mathbf{b}}_{g,i}+\frac{\partial \Delta {\tilde{\mathbf{p}}}_{ij}}{\partial {\mathbf{b}}_{a,i}}\delta {\mathbf{b}}_{a,i}.\end{array}$$
  相当于只要每一次计算了预积分对偏置的雅可比，利用泰勒这种展开进行近似即可。

2 VIO残差函数的构建

2.1 VIO系统的状态量

  一般来讲，优化的是与相机保持同步的IMU数据，区间内的IMU数据会被积分累计。

  为了节约计算量采用滑动窗口形式的 Bundle Adjustment, 在 i 时刻，滑动窗口内待优化的系统状态量定义如下:

  下面N表示了滑动窗口中的关键帧数量，对应位姿；M表示被滑动窗口所有关键帧观测到的路标点数量，对应地图点。${\mathbf{x}}_i$表示了IMU在惯性坐标系（世界坐标系）中的位置、速度、姿态、以及IMU坐标系中的加速度和角速度偏置。
$$\begin{array}{rl}\mathcal{X}& =[{\mathbf{x}}_n,{\mathbf{x}}_{n+1},...,{\mathbf{x}}_{n+N},{\lambda }_m,{\lambda }_{m+1},...,{\lambda }_{m+M}]\\ {\mathbf{x}}_i& =[{\mathbf{p}}_{w{b}_i},{\mathbf{q}}_{w{b}_i},{\mathbf{v}}_i^w,{\mathbf{b}}_a^{b_i},{\mathbf{b}}_g^{b_i}{]}^{\top },i\in [n,n+N]\end{array}$$

2.2 目标函数

  三个残差项即误差项分别为边缘化的先验信息、IMU 测量残差、视觉的重投影残差。三种残差都是用马氏距离表示。

$$\begin{array}{r}\underset{\mathcal{X}}{\min }\underset{\text{prior}}{\underbrace{\rho \left(\Vert {\mathbf{r}}_p-{\mathbf{J}}_p\mathcal{X}{\Vert }_{{\Sigma }_p}^2\right)}}+\underset{\text{IMU error}}{\underbrace{\sum _{i\in B}\rho \left(\Vert {\mathbf{r}}_b({\mathbf{z}}_{b_i{b}_{i+1}},\mathcal{X}){\Vert }_{{\Sigma }_{b_i{b}_i+1}}^2\right)}}\\ +\underset{\text{image error}}{\underbrace{\sum _{(i,j)\in F}\rho \left({\Vert {\mathbf{r}}_f({\mathbf{z}}_{{\mathbf{f}}_j}^{c_i},\mathcal{X})\Vert }_{{\Sigma }_{{\mathbf{f}}_j}^{c_i}}^2\right)}}\end{array}$$

套入上面最小二乘，以IMU残差为例，X即优化变量，这里就是那5个状态

$$\underset{\delta X}{\min }{\Vert r_B\left({\hat{z}}_{b_{k+1}}^{b_k},X+\Delta X\right)\Vert }_{P_{b_{k+1}}^{b_k}}^2=\underset{\delta X}{\min }{\Vert r_B\left({\hat{z}}_{b_{k+1}}^{b_k},X\right)+J_{b_{k+1}}^{b_k}\Delta X\Vert }_{P_{b_{k+1}}^{b_k}}^2$$

  其中，$J_{bk+1}^{bk}$是误差项$r_B$关于所有状态向量（即优化变量）X 的 Jacobian，将上式展开并令关于∆X的导数为 0，可以得到增量∆X的计算公式：
$$\begin{array}{c}{J_{b_{k+1}}^{b_k}}^T{P_{b_{k+1}}^{b_k}}^{-1}{J}_{b_{k+1}}^{b_k}\Delta X=-\begin{array}{c}{J_{b_{k+1}}^{b_k}}^T{P_{b_{k+1}}^{b_k}}^{-1}{r}_B\end{array}\end{array}$$
  整体目标函数的整体增量方程可写成