VIO 概述
1.0 结合IMU的融合定位方案
- 自动驾驶中通常用 IMU+GPS/差分 GPS/RTK 的融合定位方案，形成 GNSS-INS 组合导航系统，达到厘米组定位精度；
- 头戴式 AR/VR 头盔则多使用视觉 +IMU 的 VIO 定位系统,形成高帧率定位方案。
2.0 融合方案
2.1 松耦合
将 IMU 定位与视觉/GNSS 的位姿直接进行融合,融合过程对二者本身不产生影响，作为后处理方式输出。典型方案为卡尔曼滤波器。即在每次的融合过程中不会对例如IMU的随机游走或者高斯白噪声进行更新和迭代优化。只是以某种方式将两者的位姿进行了一定的加权融合，最简单的是按照固定的比例，卡尔曼滤波器则是将两者的不确定性也考虑在内，根据不确定性（卡尔曼增益）来进行比例分配。整体框架如下图所示：
上图中，相机和IMU分别估计自身的位姿，然后将两者的结果进行加权的融合即可。是一种典型的松耦合框架。
2.2 紧耦合
融合过程本身会影响视觉和 IMU 中的参数(如 IMU 的零偏和视觉的尺度)。典型方案为 MSCKF 和非线性优化。在这个过程中相机和IMU不虽然也会估计自身的位姿，但是不会直接通过一个简单的融合就进行输出，而是通过非线性优化的方式，将相机和IMU的相对位姿估计作为约束条件，放入到优化问题中，优化问题会对IMU的一些参数，例如随机游走，高斯白噪声等参数进行同时的估计优化，最终通过BA将优化结果进行输出。整体框架如下所示。
2.3 为什么要使用紧耦合
- 单纯凭(单目)视觉或 IMU 都不具备估计 Pose 的能力:视觉存在尺度不确定性、IMU 存在零偏导致漂移；在单目模式下，相机不能够对绝对尺度进行估计，因为在单目相机初始化的时候使用的是基于对极几何约束进行的相对位姿估计，对极几何是尺度等价的，即等式两边同时乘非零常数时，仍然满足约束。
- 松耦合中,视觉内部 BA 没有 IMU 的信息,在整体层面来看不是最优的。
- 紧耦合可以一次性建模所有的运动和测量信息,更容易达到最优。紧耦合的算法中可以将相机的pose，3d点，IMU的pose同时进行优化。
3.0 预备知识
3.1 三维刚体运动
我们通常在机器人/车辆上定义各种坐标系,如：
- 世界坐标系 \(W\);
- IMU 坐标系 \(I\);
- 相机坐标系 \(C\);
坐标系之间变换关系由一个 \(SE(3)\) 给出。如 \(I\) 到 \(W\) 系的变换矩阵为:\(T_{WI}\) :
\[\mathbf{T}_{W I}=\left[\begin{array}{cc} \mathbf{R}_{W I} & \mathbf{t}_{W I} \\ \mathbf{0}^{\top} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \]- \(R_{W I}\) 为 3 × 3 的旋转矩阵,\(t_{W I}\) 为平移向量。
- \(T_{W I}\) 右乘一个 \(I\) 系下(齐次)坐标,将得到该点 \(W\) 系下坐标。
当然这里的 \(I\) 也可以是\(C\)，那么 \(T_{WC}\) 表示的就是，相机坐标系在世界坐标系下的位置和姿态，分别由矩阵中的块元素表示，此时相机坐标系下的一个点，乘以该矩阵即可得到相机坐标系下观察到的点在世界坐标系下的点的位置。此时的坐标是齐次坐标的表示形式。当然\(I\)也是一样的道理。
约定:
- 当某个量表达坐标系的转换关系时,写在右下脚标,例如 \(T_{W B}\) 。
- 当表达矢量在某坐标系中取的坐标时,写在右上角标,如 \(v^w\) 表达速度矢量在 World 系坐标。
- \(I\) 系也称为 Body 系。
- 定义明确时,有时会省略该脚标,我们会直接谈论 \(R, t\) 这样的量。
- 不刻意区分齐次和非齐次坐标,因为在程序中可以自动完成转换,且无歧义。
- 默认以 \(T_{W I}\) 表达并存储 IMU 的定位信息,而不是 \(T_{IW}\) 。二者实际互为逆,存储哪一类区别不大,视习惯而定。
- 同理,\(T_{W I}\) 的平移部分可直接视作 IMU 在世界中的坐标,从而进行绘图或可视化操作。
3.2 四元数
- 旋转矩阵 \(R\) 亦可用四元数 \(q\) 描述。
- 四元数 \(q\) 有一个实部和三个虚部。我们把实部写在前:
其中 \(q_0\) 为实部,\([q_1 , q_2 , q_3 ]^⊤\) 为虚部。因为实部为标量,虚部为矢量,所以也可记为:
\[q=[s,v]^T \]其中 \(s\) 为标量, \(v\) 为虚部的矢量。
四元数之间可以进行乘法运算:
\[\begin{array}{l} \mathbf{q}_{a} \otimes \mathbf{q}_{b}=w_{a} w_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ +\left(w_{a} x_{b}+x_{a} w_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ +\left(w_{a} y_{b}-x_{a} z_{b}+y_{a} w_{b}+z_{a} x_{b}\right) j \\ +\left(w_{a} z_{b}+x_{a} y_{b}-y_{a} x_{b}+z_{a} w_{b}\right) k \\ \end{array} \]或者：
\[\mathbf{q}_{a} \otimes \mathbf{q}_{b}=\left[s_{a} s_{b}-\mathbf{v}_{a}^{\top} \mathbf{v}_{b}, s_{a} \mathbf{v}_{b}+s_{b} \mathbf{v}_{a}+\mathbf{v}_{a} \times \mathbf{v}_{b}\right]^{\top} \]此外,四元数可类似复数,定义加减、模长、逆、共轭等运算。
-
单位四元数可表达任意三维旋转，且无奇异性。
-
四元数和角轴的转换关系:
\[q = \left[\begin{array}{cc} cos \frac{\mathbf{\theta}}{2} \\ \mathbf{u}sin \frac{\mathbf{\theta}}{2} \end{array}\right] \]
假设某个旋转运动的旋转轴为单位向量 \(u\),绕该轴旋转的角度为 \(θ\),那么它对应的单位四元数为:当旋转一段微小时间,即角度趋于 \(0\) 时,容易有:
\[\Delta \mathbf{q}=\left[\begin{array}{c} \cos \frac{\delta \theta}{2} \\ \mathbf{u} \sin \frac{\delta \theta}{2} \end{array}\right] \approx\left[\begin{array}{c} 1 \\ \mathbf{u} \frac{\delta \theta}{2} \end{array}\right]=\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\delta} \boldsymbol{\theta} \end{array}\right] \]
其中 \(δθ\) 的方向表示旋转轴，模长表示旋转角度。注意在等号的时候，\(\theta\) 变成了黑体，变成了矢量，将 \(u\) 包含在内了。
-
角速度可以表示为，也就是上面的结果对时间求导，求导结果既包含了大小，也包含了方向。
\[\boldsymbol{\omega}=\lim _{\Delta t \rightarrow 0} \frac{\boldsymbol{\delta} \boldsymbol{\theta}}{\Delta t}\\ \] -
四元数对时间的导数：
\[\begin{array}{l} \begin{aligned} \dot{\mathbf{q}} & \triangleq \lim _{\Delta t \rightarrow 0} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \\ & =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}-\mathbf{q}}{\Delta t} \\ & =\lim _{\Delta t \rightarrow 0} \frac{\mathbf{q} \otimes\left(\left[\begin{array}{c} 1 \\ \frac{1}{2} \boldsymbol{\delta} \boldsymbol{\theta} \end{array}\right]-\left[\begin{array}{l} 1 \\ \mathbf{0} \end{array}\right]\right)}{\Delta t} \\ & =\mathbf{q} \otimes\left[\begin{array}{c} 0 \\ \frac{1}{2} \boldsymbol{\omega} \end{array}\right] \end{aligned} \end{array} \] -
除了利用四元数求导,亦可利用李代数进行旋转求导。使用旋转矩阵 \(R\) 时,角速度为 \(ω\),那么 \(R\) 相对于时间的导数可写作:
\[\dot{R}=R\omega ^{\wedge} \]该式被称为泊松公式(Possion’s equation)，其中\(∧\)为反对称矩阵算子:
\[\omega ^ \wedge = \begin{bmatrix} 0 & -\omega _3 & \omega _2\\ \omega _3& 0& -\omega _1\\ -\omega_2& \omega _1 &0 \end{bmatrix} \]