[SLAM] IMU预积分推导

1. 前言

本文内容主要来自于对邱笑晨博士IMU.pdf和高博对预积分讲解视频的个人理解。

2. IMU应用原理

原理：IMU可以测得载体运动过程中的加速度，在初始位姿+初始速度已知的情况下，通过积分可以得到实时速度，再积分即可得到实时位置；
需要解决的事情：该加速度是在载体坐标系下的，即由于载体在运动使得每次测量所对应的坐标系是不同的，因此需要有个换算过程将其统一到同一个坐标系下。此时就需要不断更新每次测量的载体姿态，用于将IMU测量换算到世界坐标系下；
问题：
①IMU测量的加速度其实是包含了反向重力加速度的比力，可以理解为静止时IMU输出为垂直向上的重力加速度，自由落体时IMU输出为0；
②IMU的测量存在噪声，导致积分过程噪声不断累加；
应用：基于BA的VIO算法中，IMU预积分提供了两帧之间位姿变化的测量，该测量结果可以作为两帧之间的位姿边参与图优化中，就像是为卡尔曼滤波中增加了运动方程（个人理解）；同时这种结合还带来了几个优点：
①IMU与绝对位姿解耦了，即IMU积分需要的这个位姿不再完全依赖自己提供，SLAM会维护这个位姿而它本身只需在两帧之间提供积分运算，因此优化速度得到了提高；
②重力存在使得SLAM系统中俯仰角和横滚角可观（x，z轴旋转客观，不包含y轴航向角），这让系统有了绝对姿态；
③IMU之外的传感器提供的观测使得IMU的bias可观从而被估计出来，有效地抑制了IMU的累计误差；
④IMU提供的距离信息解决了单目视觉的尺度问题；

3.IMU模型

3.1 坐标关系

在VIO系统中有3个常用坐标系：世界坐标系W，IMU坐标系B和相机坐标系C；
我们将IMU坐标系在世界坐标系下的表示定义为：$T_{WB}=(R_{WB},p_W)$，IMU系到相机系的坐标变化定义为：$T_{BC}$

3.2 测量模型

此时我们有IMU的测量模型为：
$$\{\begin{array}{l}{\hat{w}}_{WB}(t)=w_{WB}(t)+b_g(t)+{\eta }_g(t)\\ {\hat{a}}_B(t)=R_{WB}^T(a_W(t)-g_W)+b_a(t)+{\eta }_a(t)\end{array}$$上述两式分别对应陀螺仪测量模型+加速度计测量模型，即：
测量角速度/加速度 = 真实角速度/加速度 + 时变的bias + 白噪声
两个测量结果都是IMU坐标系下的表示，另外这里的两个bias其实是两个随机游走，导数具有如下形式：
$${\dot{b}}_g(t)={\eta }^{bg},{\dot{b}}_a(t)={\eta }^{ba}$$知道了测量模型，我们可以得到VIO中的需要估计的状态量，与VO相比需要估计的东西多了一个三维的速度和两个三位的bias，状态量如下：
$$x_i=[R_i,p_i,v_i,b_{g_i},b_{a_i}]$$

3.3 运动模型

运动学为微分模型：
$$\{\begin{array}{l}{\dot{v}}_W=a_W\\ {\dot{p}}_W=v_W\\ {\dot{R}}_{WB}=R_{WB}\ast (w_{WB})\stackrel{ˆ}{}\end{array}$$通过积分可以得到$\Delta t$时间后的运动状态为，连续域下的积分形式：
$$\{\begin{array}{l}{v}_W(t+\Delta t)=v_W(t)+{\int }_t^{t+\Delta t}{a}_W(\tau )d\tau \\ p_W(t+\Delta t)=p_W(t)+{\int }_t^{t+\Delta t}{v}_W(\tau )d\tau +\int {\int }_t^{t+\Delta t}{a}_W(\tau )(d\tau {)}^2\\ R_{WB}(t+\Delta t)=R_{WB}\ast Exp({\int }_t^{t+\Delta t}{w}_{WB}(\tau )d\tau )\end{array}$$由于IMU的观测时$\Delta t$时刻一次离散的形式，只能假设在这段时间内是不变的，因此将连续表示转换成离散形式：
$$\{\begin{array}{l}{v}_W(t+\Delta t)=v_W(t)+a_W(t)\Delta t\\ p_W(t+\Delta t)=p_W(t)+v_W(t)\Delta t+\frac{1}{2}{a}_W(t)(\Delta t{)}^2\\ R_{WB}(t+\Delta t)=R_{WB}\ast Exp(w_{WB}(t)\Delta t)\end{array}$$

3.4 观测+运动结合

观测方程带入运动模型即可得到IMU观测下的运动方程：
$$\{\begin{array}{l}{v}_W(t+\Delta t)=v_W(t)+g_W\Delta t+({\hat{a}}_B(t)-b_a(t)-{\eta }_a(t))\Delta t\\ p_W(t+\Delta t)=p_W(t)+v_W(t)\Delta t+\frac{1}{2}{g}_W\Delta t+\frac{1}{2}({\hat{a}}_B(t)-b_a(t)-{\eta }_a(t))(\Delta t{)}^2\\ R_{WB}(t+\Delta t)=R_{WB}\ast Exp(({\hat{w}}_{WB}(t)-b_g(t)-{\eta }_g(t))\Delta t)\end{array}$$这个式子就是预积分的基础了，这里还存在一个问题，即IMU中的两个bias是不知道的，而预积分的处理方式就是通过两个照片帧得到的观测集合IMU的积分结果处理这个bias；

4. IMU预积分

在两个关键帧i和j之间，将IMU数据积分得到两帧之间的运动关系有：
$$\{\begin{array}{l}{v}_j=v_i+g\Delta t_{ij}+{\sum }_{k=i}^{j-1}(R_k({\hat{a}}_k-b_{ka}-{\eta }_{ka})\Delta t)\\ p_j=p_i+{\sum }_{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}g\Delta t+\frac{1}{2}{R}_k({\hat{a}}_k-b_{ka}-{\eta }_{ka})\Delta t^2)\\ R_j=R_i\ast {\prod }_{k=i}^{j-1}Exp(({\hat{w}}_k-b_{kg}-{\eta }_{kg})\Delta t)\end{array}$$将两帧之间的速度、位置、旋转变化量对齐到上一帧位置处，得到两帧之间的IMU观测模型：
$$\{\begin{array}{l}\Delta v_{ij}=R_i^T(v_j-v_i-g\Delta t_{ij})={\sum }_{k=i}^j\Delta R_{ik}({\hat{a}}_k-b_{ka}-{\eta }_{ka})\Delta t\\ \Delta p_{ij}=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\frac{1}{2}{\sum }_{k=i}^{j-1}g(\Delta t{)}^2)={\sum }_{k=i}^j(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}({\hat{a}}_k-b_{ka}-{\eta }_{ka})\Delta t^2)\\ \Delta R_{ij}=R_i^{T}Rj={\prod }_{k=i}^{j-1}Exp(({\hat{w}}_k-b_{kg}-{\eta }_{kg})\Delta t)\end{array}$$注意注意注意，这里是将IMU的bias部分对齐到了上一帧i表示的坐标系，因此等号右边都是$\Delta {\ast }_{ik}$的形式，至此IMU预积分的残差公式就推导完毕了，后续还涉及bias和噪声的处理以及一些细微的推导等以后有功夫再写吧；