vins中imu融合_VINS-Mono代码分析与总结(最终版)

VINS-Mono代码分析总结

参考文献

前言

??视觉与IMU融合的分类：

松耦合和紧耦合：按照是否把图像的Feature加入到状态向量区分，换句话说就是松耦合是在视觉和IMU各自求出的位姿的基础上做的耦合。

滤波法和优化法：

参考

??Vins-Mono是视觉与IMU的融合中的经典之作，其定位精度可以媲美OKVIS，而且具有比OKVIS更加完善和鲁棒的初始化以及闭环检测过程。同时VINS-Mono也为该邻域树立了一个信标吧，视觉SLAM的研究和应用会更新偏向于 单目+IMU。因为在机器人的导航中，尤其是无人机的自主导航中，单目不具有RGBD相机(易受光照影响、获取的深度信息有限)以及双目相机(占用较大的空间。。。)的限制，能够适应室内、室外及不同光照的环境，具有较好的适应性。而且在增强和虚拟现实中，更多的移动设备仅具有单个相机，所以单目+IMU也更符合实际情况。

??那么为什么要进行视觉与IMU的融合呢，自己总结的主要有以下几点：

视觉与IMU的融合可以借助IMU较高的采样频率，进而提高系统的输出频率。

视觉与IMU的融合可以提高视觉的鲁棒性，如视觉SLAM因为某些运动或场景出现的错误结果。

视觉与IMU的融合可以有效的消除IMU的积分漂移。

视觉与IMU的融合能够校正IMU的Bias。

单目与IMU的融合可以有效解决单目尺度不可观测的问题。

??上面总结了视觉与IMU融合的几个优点，以及与单目融合可以解决单目相机尺度不可测的问题。但是单目相机的尺度不可测也是具有优点的，参考[4]。单目尺度不确定的优点主要有两方面：单目尺度的不确定性，可以对不同的规模大小的环境空间之间进行游走切换，起到无缝连接的作用，比如从室内桌面上的环境和大规模的户外场景；诸如深度或立体视觉相机，这些具备深度信息的传感器，它们所提供的可靠深度信息范围，是有限制的，故而不像单目相机那样具有尺度灵活性的特点。

1 预积分的推导

1.1 离散状态下预积分方程

??关于这部分的论文和代码中的推导，可以参考文献[2]中Appendx部分“A Runge-Kutta numerical integration methods”中的欧拉法和中值法。

\[

w_{k}^{ {}'}=\frac{w_{k+1}+w_{k}}{2}-b_{w}

\tag{1.1}

\]

\[

q _{i+1}=q _{i}\otimes \begin{bmatrix}

1

\0.5w_{k}^{ {}'}

\end{bmatrix}

\tag{1.2}

\]

\[

a_{k}^{ {}'}=\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}+n_{a1}-b_{a_{k}})}{2}

\tag{1.3}

\]

\[

\alpha _{i+1}=\delta\alpha _{i}+\beta _{i}t+0.5a_{k}^{ {}'}\delta t^{2}

\tag{1.4}

\]

\[

\beta _{i+1}=\delta\beta _{i}+a_{k}^{ {}'}\delta t

\tag{1.5}

\]

1.2 离散状态下误差状态方程

??论文中Ⅱ.B部分的误差状态方程是连续时间域内，在实际代码中需要的是离散时间下的方程式，而且在前面的预积分方程中使用了中值法积分方式。所以在实际代码中和论文是不一致的。在推导误差状态方程式的最重要的部分是对 \(\delta \theta _{k+1}\) 部分的推导。

??由泰勒公式可得：

\[

\delta \theta _{k+1} = \delta \theta _{k}+\dot{\delta \theta _{k}}\delta t

\tag{1.6}

\]

依据参考文献[2]中 "5.3.3 The error-state kinematics"中公式(222c)及其推导过程有：

\[

\dot{\delta \theta _{k}}=-[w_{m}-w_{b}]_{\times }\delta \theta _{k}-\delta w_{b}-w_{n}

\]

对于中值法积分下的误差状态方程为：

\[

\dot{\delta \theta _{k}}=-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta \theta _{k}-\delta b_{g_{k}}+\frac{n_{w0}+n_{w1}}{2}

\tag{1.7}

\]

将式(1.7)带入式(1.6)可得：

\[

\delta \theta _{k+1} =(I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t

\tag{1.8}

\]

这部分也可以参考，文献[2]中“7.2 System kinematics in discrete time”小节。

??接下来先推导 \(\delta \beta _{k+1}\) 部分，再推导 \(\delta \alpha _{k+1}\) 部分。\(\delta \beta _{k+1}\) 部分的推导也可以参考文献[2]中“5.3.3 The error-state kinematics”公式(222b)的推导。将式(1.5)展开得到：

\[

\delta\beta _{i+1}=\delta\beta _{i}+\frac{q_{k}(a_{k}+n_{a0}-b_{a_{k}})+q_{k+1}(a_{k+1}++n_{a1}-b_{a_{k}})}{2}\delta t

\]

即，

\[

\delta\beta _{i+1}=\delta\beta _{i}+\dot{\delta\beta_{i}}\delta t

\tag{1.9}

\]

文献[2]中，公式(222b)

\[

\dot{\delta v}=-R[a_{m}-a_{b}]_{\times}\delta \theta-R\delta a_{b}+\delta g-Ra_{n}

\]

对于中值法积分下的误差状态方程为：

\[

\begin{align}\nonumber

\dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}\delta \theta _{k+1} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta \theta \\\nonumber

&-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1}

\end{align}

\tag{1.10}

\]

将式(1.8)带入式(1.10)可得

\[

\begin{align}\nonumber

\dot{\delta\beta_{i}} =&-\frac{1}{2}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta-\frac{1}{2}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t) \\\nonumber

&-\frac{1}{2}q_{k}\delta b_{a_{k}}-\frac{1}{2}q_{k+1}\delta b_{a_{k}}-\frac{1}{2}q_{k}n_{a0}-\frac{1}{2}q_{k}n_{a1}

\end{align}

\tag{1.11}

\]

同理，可以计算出 \(\delta \alpha _{k+1}\) ，可以写为：

\[

\delta\alpha _{i+1}=\delta\alpha _{i}+\dot{\delta\alpha_{i}}\delta t

\tag{1.12}

\]

\[

\begin{align}\nonumber

\dot{\delta\alpha_{i}} =&-\frac{1}{4}q_{k}[a_{k}-b_{a_{k}}]_{\times}\delta \theta\delta t-\frac{1}{4}q_{k+1}[a_{k+1}-b_{a_{k}}]_{\times}((I-[\frac{w_{k+1}+w_{k}}{2}-b_{g_{k}}]_{\times }\delta t) \delta \theta _{k} -\delta b_{g_{k}}\delta t+\frac{n_{w0}+n_{w1}}{2}\delta t)\delta t \\\nonumber

&-\frac{1}{4}q_{k}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k+1}\delta b_{a_{k}}\delta t-\frac{1}{4}q_{k}n_{a0}\delta t-\frac{1}{4}q_{k}n_{a1}\delta t

\end{align}

\tag{1.13}

\]

最后是加速度计和陀螺仪bias的误差状态方程，

\[

\delta b_{a_{k+1}}=\delta b_{a_{k}}+n_{ba}\delta t

\tag{1.14}

\]

\[

\delta b_{w_{k+1}}=\delta b_{w_{k}}+n_{bg}\delta t

\tag{1.15}

\]

??综合式(1.8)等误差状态方程，将其写为矩阵形式，

\[

\begin{align}\nonumber

\begin{bmatrix}

\delta \alpha_{k+1}\\delta \theta _{k+1}\\delta \beta _{k+1} \\delta b _{a{}{k+1}} \\delta b _{g{}{k+1}}

\end{bmatrix}&=\begin{bmatrix}

I & f_{01} &\delta t & -\frac{1}{4}(q_{k}+q_{k+1})\delta t^{2} & f_{04}\0 & I-[\frac{w_{k+1}+w_{k}}{2}-b_{wk}]_{\times }\delta t & 0 & 0&-\delta t \0 & f_{21}&I & -\frac{1}{2}(q_{k}+q_{k+1})\delta t & f_{24}\0 & 0& 0&I &0 \ 0& 0 & 0 & 0 & I

\end{bmatrix}

\begin{bmatrix}

\delta \alpha_{k}\\delta \theta _{k}\\delta \beta _{k} \\delta b _{a{}{k}} \\delta b _{g{}{k}}

\end{bmatrix} \\\nonumber

&+

\begin{bmatrix}

\frac{1}{4}q_{k}\delta t^{2}& v_{01}& \frac{1}{4}q_{k+1}\delta t^{2} & v_{03} & 0 & 0\ 0& \frac{1}{2}\delta t & 0 & \frac{1}{2}\delta t &0 & 0\ \frac{1}{2}q_{k}\delta t& v_{21}& \frac{1}{2}q_{k+1}\delta t & v_{23} & 0 & 0 \0 & 0 & 0 & 0 &\delta t &0 \ 0& 0 &0 & 0 &0 & \delta t

\end{bmatrix}