简单理解VIO(一)

随着ALOT（AI+Lot）技术的发展，任务移动的模块在不久的未来都将配备MEMS IMU，以确定设备的位置与姿态，做出更智能的决策与反应。以手机为例，从iPhone4开始，后续的手机均配备了三轴陀螺仪，从小米2开始就配备IMU，相信未来不管是旗舰机还是普通手机，配备IMU都将是基本配置，就像手机配备照相的镜头一样。
但是由于IMU本身只能提供局部信息且与环境无关，仅依赖IMU并不能为移动设备提供定姿定位服务，使用IMU与视觉的融合，可以为移动设备提供稳定高频的定位信息。

预备知识回顾

场景预设

为了更好的简化复杂的逻辑，我们假设机器人在屋里的平面上移动。

三维刚体运动

可以用一个比较形象的例子来说明，假设人的眼睛为视觉，人体的陀螺仪（@todo）为惯性设备，那需要定义三个坐标系：

• 世界坐标系W，原点在一楼
• IMU坐标系I; 原点在后脑勺（小脑部分）
• 相机坐标系C；原点在左眼

坐标系之间的变换关系是一个旋转与平移，由 $T_{WI} \in SE(3)$ 给出，如从I到W的变换矩阵为 $T_{WI}$, 坐标系转换关系：
$P^{W} = T_{WI} * P^{I}$
其中 $P$为齐次坐标形式，当表示为正常坐标形式时：
$P^{W} = R_{WI}P^{I} + t_{WI}$

四元数

• 四元数解决的问题有两个：
  • 解决了欧拉角旋转奇异的问题
  • 提高了旋转计算的精度
• 四元数的表示，用一个实部和一个虚部来表示： $q = [q_0, q_1, q_2, q_3]$. 当我们只有yaw角（z轴）时，例如逆时针旋转30°，用欧拉角表示为[0, 0, $\pi/6$]. 对应的四元数为 $q = [0.966, 0, 0, 0.259]$. 其中x轴，y轴都是0.
• 四元数之间可以进行乘法运算(以yaw角转动为例)：
  • $q_a \otimes q_b = (w_aw_b - z_az_b) + (w_az_b + z_aw_b)k$
  • 以 $yaw = \pi/6$为例， $q_a \otimes q_b = [0.866, 0, 0, 0.500]$，即等于两个三十度相乘。验证方法：四元数在线转换
• 四元数的精妙之处在于，利用复数的乘法，定义了四元数的乘法，以此来表示旋转矩阵的相乘。
• 四元数时间导数
  • WHY：为什么要求时间的导数呢？那四元数是一个关于时间的函数吗？任何系统应该都是与时间相关的，对于移动设备尤其如此。
  • HOW：通过角速度，就可以将姿态与时间联系起来
  • WHAT：假设以1°/s的速度逆时针旋转，以当前旋转为局部坐标系，那么时间基准t时刻的角度可以认为是0(因为偏导数是一个向量，与起点在哪里并没有关系)，那么就有（@todo）：
    $\lim_{\delta t->0} \frac{q(t+\delta t) - q(t)}{\delta t} = \lim_{\delta t->0} \frac{q(\delta t) - q(0)}{\delta t}$
    $= \lim_{\delta t->0} \frac{[\cos(\delta t/2), [0,0,1]^T\sin(\delta /2)] - [1, 0, 0, 0]}{\delta t}$
    $= \lim_{\delta t->0} [-1/2\sin(\delta t/2), 1/2[0,0,1]^T\cos(\delta /2)] = [0, \frac{1}{2}w]$
    这里是我的一种证明方式，我认为是最简单的。