VINS_MONO系列:(二)IMU预积分详细推导

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

目录

1、算法推导

2、反思与探讨

3、参考文献


相关链接:

VINS_MONO系列:(一)总体框架_Derrr...的博客-CSDN博客_vins框架

VINS_MONO系列:(二)IMU预积分详细推导_Derrr...的博客-CSDN博客

VINS_MONO系列:(三)VIO初始化_Derrr...的博客-CSDN博客

VINS_MONO系列:(四)紧耦合VIO实现_Derrr...的博客-CSDN博客

 VINS_MONO系列:(五)前端特征提取_Derrr...的博客-CSDN博客

1、算法推导

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

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

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

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

v^w_{k+1} = v^w_{k}+\int_{t} a^wdt

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

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

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

w^w=w^b-b_{g}-\eta_{g}

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

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

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

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

再对左边同时乘以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

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

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

上式右侧则为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}

\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}

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

\delta\hat{b}_a=b_{ak+1}-b_{ak}

\delta\hat{b}_g=b_{gk+1}-b_{gk}

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

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}

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

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

\frac{\partial \alpha}{\partial v^w_{k+1}}=\frac{\partial \alpha}{\partial q^w_{k+1}}=0

\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 

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

\frac{\partial \alpha }{\partial b_{gk}} =-J^\alpha_{bg}


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

\frac{\partial \beta }{\partial p^w_{k}}=\frac{\partial \beta }{\partial p^w_{k+1}}=0 

\frac{\partial \beta }{\partial v^w_{k}}=-R^T_{wb}

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

\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

\frac{\partial \beta }{\partial q^w_{k+1}}=0

\frac{\partial \beta }{\partial b_{gk}}=-J^\beta_{bg}

\frac{\partial \beta }{\partial b_{ak}}=-J^\beta_{ba}


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

\frac{\partial \gamma }{\partial p_{k}}=\frac{\partial \gamma }{\partial p_{k+1}}=0

\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}]

\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}]

\frac{\partial \gamma }{\partial v_{k}}=\frac{\partial \gamma }{\partial v_{k+1}}=0

\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^\gamma _{bg}\delta b_{gk}\end{bmatrix}}{\delta b_{gk}}                                             

=-R[\gamma^{-1} \otimes [q^w_{k}]^{-1}\otimes q^w_{k+1}]J^\gamma _{bg}=L[ [q^w_{k+1}]^{-1}\otimes q^w_{k}\otimes \gamma]J^\gamma _{bg}

上式中的J^\alpha _{b_a},J^\alpha _{b_g},J^\beta _{b_a},J^\beta _{b_g},J^\gamma _{b_g}分别为预积分项相对于偏置误差的雅克比,满足:

\alpha ^{k}_{k+1}=\hat{\alpha}^k_{k+1}+J^\alpha_{b_a}\delta b_a+J^\alpha_{b_g}\delta b_g

\beta ^{k}_{k+1}=\hat{\beta}^k_{k+1}+J^\beta_{b_a}\delta b_a+J^\beta_{b_g}\delta b_g

\gamma ^{k}_{k+1}=\hat{\gamma}^k_{k+1}+J^\gamma_{b_g}\delta b_g

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

P=F_{x}PF^T_{x}+F_{i}Q_{i}F^T_{i}

其中

VINS_MONO系列:(二)IMU预积分详细推导_第1张图片

 至此,VINS中整个关于IMU预积分的部分已经推导完毕,最后再整体的梳理一下,预积分部分主要要把握三个部分,分别是状态值的更新,协方差的更新以及IMU残差的构建,将其写成离散的形式并运用中值法进行积分,有

p^w_{k+1}=p^w_{k}+v^w_{k}\Delta t+\frac{1}{2}R_{wb}(\frac{\hat{a_{k}}+\hat{a_{k+1}}}{2}-b_{ak})\Delta t^2-g^w\Delta t^2

v^w_{k+1}=v^w_{k}+R_{wb}(\frac{\hat{a_{k}}+\hat{a_{k+1}}}{2}-b_{ak})\Delta t-g^w\Delta t

q^w_{k+1}=q^w_{k}\otimes \begin{bmatrix} 1\\ \frac{1}{2}[\frac{w_{k}+w_{k+1}}{2}-b_{gk}]\Delta t) \end{bmatrix}

2、反思与探讨

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

  • 计算角度残差的不同

VINS中的方向残差没有取对数,而[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.

你可能感兴趣的:(SLAM,slam,c++,计算机视觉,opencv)