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

2022-03-01 21:03:33 阅读：490 来源： 互联网

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

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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

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

1、算法推导

2、反思与探讨

3、参考文献