简单理解VIO(一)

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

一、预备知识回顾

1. 场景预设

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

2. 三维刚体运动

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

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

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

3. 四元数

• 四元数解决的问题有两个：
  • 解决了欧拉角旋转奇异的问题
  • 提高了旋转计算的精度
• 四元数的表示，用一个实部和一个虚部来表示：$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_a{w}_b-z_a{z}_b)+(w_a{z}_b+z_a{w}_b)k$
  • 以$yaw=\pi /6$为例，$q_a\otimes q_b=[0.866,0,0,0.500]$，即等于两个三十度相乘。验证方法：四元数在线转换
• 四元数的精妙之处在于，利用复数的乘法，定义了四元数的乘法，以此来表示旋转矩阵的相乘。
• 四元数时间导数
  • WHY：为什么要求时间的导数呢？那四元数是一个关于时间的函数吗？任何系统应该都是与时间相关的，对于移动设备尤其如此 。前面的这个理解不精准，求导的原因与泰勒公式有关，与系统动态有关，见我的另外一篇控制的笔记。
  • HOW：通过角速度，就可以将姿态与时间联系起来
  • WHAT：假设以1°/s的速度逆时针旋转，以当前旋转为局部坐标系，那么时间基准t时刻的角度可以认为是0(因为偏导数是一个向量，与起点在哪里并没有关系)，那么就有（@todo）：
    $$\underset{\delta t->0}{\lim }\frac{q(t+\delta t)-q(t)}{\delta t}=\underset{\delta t->0}{\lim }\frac{q(\delta t)-q(0)}{\delta t}$$
    $$=\underset{\delta t->0}{\lim }\frac{[\cos (\delta t/2),[0,0,1{]}^T\sin (\delta /2)]-[1,0,0,0]}{\delta t}$$
    $$=\underset{\delta t->0}{\lim }[-1/2\sin (\delta t/2),1/2[0,0,1{]}^T\cos (\delta /2)]=[0,\frac{1}{2}w]$$
    这里是我的一种证明方式，我认为是最简单的。
• 一个函数关于四元数的导数，可以通过求导工具来求。或者，使用李代数中求导法则。

4. 李代数
   关于李代数在机器人中的应用，最好的参考文献是2.

作业

1. 使用R或者是q更新，当w足够小时，差异不大。

clc
angles1 = [pi/6, 0, 0];
q1 = quaternion(eul2quat(angles1));
R1 = quat2rotm(q1);
w = [0.01, 0.02, 0.03]';
R2 = R1 * expm(skew(w(1), w(2), w(3)));
q2 = q1 * quaternion([1; 0.5 * w]');
dif = rotm2eul(R2) - quat2eul(q2)
dif =   1.0e-05 *
    0.3524    0.2297    0.1236

参考文献

1. VIO第一讲课件： 链接: https://pan.baidu.com/s/1iqOANw_zmX9ujk_6KsZmBQ 提取码: x269
2. 链接: https://pan.baidu.com/s/1Wh48ra7_5Qyu-G05BnXysA 提取码: i1rh 复制这段内容后打开百度网盘手机App，操作更方便哦