首页 > 其他分享> > VINS梳理：（二）IMU预积分推导及代码实现

VINS梳理：（二）IMU预积分推导及代码实现

2022-03-01 21:03:33 作者：互联网

## 转载请注明出处，欢迎转载 ##

1、算法推导

我相信大家在很多不同的地方都听说过IMU预积分这个名词，尤其是基于图优化的框架下，几乎都会用到IMU预积分，那为什需要IMU预积分呢？一方面是因为IMU数据频率往往高于图像的频率，一般都能达到100~500Hz，而图像往往只有30~60Hz，所以为了获得每个图像帧对应的IMU数据，就需要对两个图像帧之间的IMU数据进行积分，才能实现图像帧和IMU数据的意义配对；另一方面，在图优的框架下，经常需要对历史状态进行更新，如果不使用预积分的话，每当一个状态发生变化时，就需要从头往后，运用每一帧IMU数据进行计算，直至更新完所有的状态量为止，这样显然就过于费时费力。但当我们使用IMU预积分时，当图中某个状态量发生了变化，可以直接通过预积分的值直接更新之后的每个关键帧的状态量，这就高效很多。因此可以形象地把IMU预积分理解为，将连续不断的IMU数据根据关键帧的时间戳进行打断，然后每次只记录前后帧的相对位姿关系。

这里对VINS中的IMU预积分进行推导：

对于连续时间的IMU预积分，已知k时刻的状态量，k+1时刻可以表示为

$p^w_{k+1} = p^w_{k} +v^w_{k}\triangle t+\iint_{t}^{}a^w dt^2$ (1)

$v^w_{k+1} = v^w_{k}+\int_{t} a^wdt$ (2)

$q^w_{k+1}=q^w_{k}\otimes \frac{1}{2}\int _{t}\Omega (w^w)dt$ (3)

其中 $p^w_{k+1}$ 代表k+1时刻world坐标系下的位置， $a^w$ 和 $w^w$ 为世界坐标系下的线加速度和角速度，根据IMU模型，有

$a^w=R_{wb}(a^b-b_a-\eta _a)+g_w$ (4)

$w^w=w^b-b_{g}-\eta_{g}$ (5)

其中 $a^b$ 和 $w^b$ 分别代表IMU坐标系下的线加速度和角速度， $\eta_a,\eta_g$ 分别为零均值随机噪声， $b_{a},b_{g}$ 分别为加速度及角速度的bias， $g_{w}$ 为重力加速度。将上式代入式(1)-(3)，则有

$p^w_{k+1} = p^w_{k} +v^w_{k}\Delta t+\iint_{t}^{}R_{wb}(a^b-b_a-\eta _a)+g_w dt^2$ (6)

$v^w_{k+1} = v^w_{k}+\int_{t} R_{wb}(a^b-b_a-\eta _a)+g_wdt$ (7)

$q^w_{k+1}=q^w_{k}\otimes \frac{1}{2}\int _{t}\Omega (w^b-b_{g}-\eta_{g})dt$ (8)

再对式(6)-(8)左边同时乘以 $R^T_{wb}$ 后移项，则有

$R^T_{wb}(p^w_{k+1} -p^w_{k} -v^w_{k}\triangle t-g^w\triangle t^2)= \iint_{t}^{}(a^b-b_a-\eta _a) dt^2$ (9)

$R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)= \int_{t} (a^b-b_a-\eta _a)dt$ (10)

$q^w_{k}\otimes q^w_{k+1}=\frac{1}{2}\int _{t}\Omega (w^b-b_{g}-\eta_{g})dt$ (11)

等式(9)-(11)的右侧则为IMU的预积分值，可以看到，IMU预积分值与IMU的位姿无关，仅仅代表第k帧至第k+1帧的PVQ变化值。也可以从另一个方面进行理解，等式左侧均为系统的优化变量，而等式的右侧则为两帧间的观测值，用 $\hat{\alpha} ^k_{k+1}$ ， $\hat{\beta}^k_{k+1}$ 和 $\hat{\gamma }^k_{k+1}$ 表示PVQ的预积分值（与论文保持一致），则图优化中的IMU残差可以表示为

$\delta \hat{\alpha}^k_{k+1} = R^T_{wb}(p^w_{k+1} -p^w_{k} -v^w_{k}\triangle t-g^w\triangle t^2)-\hat{\alpha}^k_{k+1}$ (12)

$\delta \hat{\beta}^k_{k+1}=R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)-\hat{\beta}^k_{k+1}$ (13)

$\delta \hat{\gamma}^k_{k+1} = 2((\hat{\gamma}^k_{k+1} )^{-1}\otimes(q^w_{k})^{-1}\otimes q^w_{k+1} )_{xyz}$ (14)

$\delta\hat{b}_a=b_{ak+1}-b_{ak}$ (15)

$\delta\hat{b}_g=b_{gk+1}-b_{gk}$ (16)

推导完残差以后，很自然的就需要推导残差项关于各状态量的雅克比矩阵，其中状态量为

$p_{k},p_{k+1},q_{k},q_{k+1},v_{k}, v_{k+1}, b_{ak}, b_{gk}$

雅克比的推导主要用到摄动法，即对状态量增加一个小的摄动量，观察结果对摄动量的变化情况，为了书写方便，以下以 $\alpha ,\beta ,\gamma$ 分别代表 $\delta \hat{\alpha}^k_{k+1},\delta \hat{\beta}^k_{k+1},\delta \hat{\gamma}^k_{k+1}$ ：

$\delta \hat{\alpha}^k_{k+1}$ 项雅克比

$\frac{\partial \alpha}{\partial p^w_{k}}=-R^T_{wb}$ (17)

$\frac{\partial \alpha}{\partial p^w_{k+1}}=R^T_{wb}$ (18)

$\frac{\partial \alpha}{\partial v^w_{k}}=-R^T_{wb}\Delta t$ (19)

$\frac{\partial \alpha}{\partial v^w_{k+1}}=\frac{\partial \alpha}{\partial q^w_{k+1}}=0$ (20)

$\frac{\partial \alpha}{\partial q^w_{k}}=\lim_{\delta \theta_{k} \rightarrow 0}=\frac{EXP(\delta \theta _{k})R^T_{wb}(p^w_{k+1}-p^w_{k}-g\Delta t^2-v^w_{k}\Delta t)-R^T_{wb}(p^w_{k+1}-p^w_{k}-g\Delta t^2-v^w_{k}\Delta t)}{\delta \theta_{k} }$

$=\lim_{\delta \theta _{k}\rightarrow 0}\frac{[\delta \theta _{k}]^\wedge R^T_{wb}(p^w_{k+1}-p^w_{k}-g\Delta t^2-v^w_{k}\Delta t)}{\delta \theta _{k}}$

$=-[R^T_{wb}(p^w_{k+1}-p^w_{k}-g\Delta t^2-v^w_{k}\Delta t)]^\wedge$ (21)

$\frac{\partial \alpha }{\partial b_{ak}} = -\frac{\partial \hat{\alpha}^k_{k+1} }{\partial b_{ak}} =-J^\alpha _{ba}$ (22)

$\frac{\partial \alpha }{\partial b_{gk}} =-J^\alpha_{bg}$ (23)

$\delta \hat{\beta}^k_{k+1}$ 项雅克比

$\frac{\partial \beta }{\partial p^w_{k}}=\frac{\partial \beta }{\partial p^w_{k+1}}=0$ (24)

$\frac{\partial \beta }{\partial v^w_{k}}=-R^T_{wb}$ (25)

$\frac{\partial \beta }{\partial v^w_{k+1}}=R^T_{wb}$ (26)

$\frac{\partial \beta }{\partial q^w_{k}}=\lim_{\delta \theta _{k}\rightarrow 0}\frac{EXP(\delta \theta _{k})R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)-R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)}{\delta \theta _{k}}$

$=\lim_{\delta \theta _{k}\rightarrow 0}\frac{[\delta \theta _{k}]^\wedge R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)}{\delta \theta _{k}}=-[R^T_{wb}(v^w_{k+1}-v^w_{k} -g_w\Delta t)]^\wedge$ (27)

$\frac{\partial \beta }{\partial q^w_{k+1}}=0$ (28)

$\frac{\partial \beta }{\partial b_{gk}}=-J^\beta_{bg}$ (29)

$\frac{\partial \beta }{\partial b_{ak}}=-J^\beta_{ba}$ (30)

$\delta \hat{\gamma}^k_{k+1}$ 项雅克比

$\frac{\partial \gamma }{\partial p_{k}}=\frac{\partial \gamma }{\partial p_{k+1}}=0$ (31)

$\frac{\partial \delta \hat{\gamma}^k_{k+1}}{\partial q_{k}}=2\lim_{\delta \theta \rightarrow 0}\frac{(\hat{\gamma}^k_{k+1} )^{-1}(q^w_{k}\cdot\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta \end{bmatrix})^{-1} q^w_{k+1}-(\hat{\gamma}^k_{k+1} )^{-1}(q^w_{k}\cdot\begin{bmatrix} 1\\0 \end{bmatrix})^{-1}q^w_{k+1}}{\delta \theta }$

$=2\lim_{\delta \theta \rightarrow 0}\frac{L[(\hat{\gamma}^k_{k+1} )^{-1}]R[(q^w_{k})^{-1}\otimes q^w_{k+1}](\begin{bmatrix} 1\\- \frac{1}{2}\delta \theta \end{bmatrix}-\begin{bmatrix} 1\\ 0\end{bmatrix})}{\delta \theta }$

$=-L[(q^w_{k})^{-1}\otimes q^w_{k+1}]R[(\hat{\gamma}^k_{k+1} )^{-1}]$ (32)

$\frac{\partial \delta \hat{\gamma}^k_{k+1}}{\partial q_{k+1}}=2\lim_{\delta \theta \rightarrow 0}\frac{(\hat{\gamma}^k_{k+1} )^{-1}(q^w_{k}\cdot)^{-1} q^w_{k+1}\begin{bmatrix} 1\\ \frac{1}{2}\delta \theta \end{bmatrix}-(\hat{\gamma}^k_{k+1} )^{-1}(q^w_{k}\cdot)^{-1}q^w_{k+1}\begin{bmatrix} 1\\0 \end{bmatrix}}{\delta \theta }$

$=L[(\hat{\gamma}^k_{k+1} )^{-1}(q^w_{k}\cdot)^{-1}q^w_{k+1}]$ (33)

$\frac{\partial \gamma }{\partial v_{k}}=\frac{\partial \gamma }{\partial v_{k+1}}=0$ (34)

$\frac{\partial \gamma }{\partial b_{gk}}=2\lim_{\delta b_{gk}\rightarrow 0}\frac{[\gamma \otimes \begin{bmatrix} 1\\ \frac{1}{2}J_{bg}\delta b_{gk} \end{bmatrix}]^{-1}\otimes [q^w_{k}]^{-1}\otimes q^w_{k+1}-[\gamma \otimes \begin{bmatrix} 1\\0 \end{bmatrix}]^{-1}\otimes [q^w_{k}]^{-1}\otimes q^w_{k+1}}{\delta b_{gk}}$

$=2\lim_{b_{gk}\rightarrow 0}\frac{R[\gamma^{-1} \otimes [q^w_{k}]^{-1}\otimes q^w_{k+1}]\otimes \begin{bmatrix} 0\\ -\frac{1}{2}J_{bg}\delta b_{gk}\end{bmatrix}}{\delta b_{gk}}$

$=-R[\gamma^{-1} \otimes [q^w_{k}]^{-1}\otimes q^w_{k+1}]J_{bg}=L[ [q^w_{k+1}]^{-1}\otimes q^w_{k}\otimes \gamma]J_{bg}$ (35)

$\frac{\partial \gamma }{\partial b_{ak}}=0$ (36)

至此，IMU预积分残差项已经基本推导完成了。但是在优化问题中，还有一项非常重要的参数，就是IMU残差对应的信息矩阵没有给出。由于VINS中IMU协方差的更新采用ESKF的方式进行递推，这里不加证明的直接给出其协方差矩阵的递推方式，具体的证明可以查看[2]和[3]：

$P=F_{x}PF^T_{x}+F_{i}Q_{i}F^T_{i}$ (37)

其中

至此，VINS中整个关于IMU预积分的部分已经推导完毕，最后再整体的梳理一下，预积分部分主要要把握三个部分，分别是状态值的更新，协方差的更新以及IMU残差的构建。

2、反思与探讨

整体推到完VINS的预积分过程之后，我发现它和经典的IMU预积分[3]，还有ORBSLAM3[5]里的预积分都有些许的差别，总结起来主要有以下几点：

计算角度残差的不同

VINS中的方向残差是公式(14)，而[3]和[5]中的方向残差为

$r_{\Delta R_{ij}}=Log(\Delta \hat{R}^T_{ij}R^T_{i}R_{j})$

差别主要是多了一个LOG函数，将李群映射到李代数上，而VINS则是只取四元数的虚部进行比较，精度上可能有一定差距。

协方差更新的方式不同

VINS中使用的是ESKF里协方差的更新方式，而[3][5]则是通过具体的推导给出的协方差更新方式，具体请查看文献[3][5]的内容

角度的表示不同（VINS用的是四元数，而[3][5]用的是SO3）

3、参考文献

[1] Tong Q , Li P , Shen S . VINS-Mono[J]. IEEE Transactions on Robotics, 2018.

[2] VINS 论文推导及代码解析. 崔华坤

[3] J Solà. Quaternion kinematics for the error-state Kalman filter[J]. 2017.

[4] Forster C , Carlone L , Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.

[5] Campos C , Elvira R , JJG Rodríguez, et al. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM[J]. 2020.

标签：推导,积分,残差,协方差,IMU,VINS
来源： https://blog.csdn.net/weixin_44413400/article/details/123031662