IMU传感器和预积分

最近看了深蓝学院的VIO课程, 学到了很多, 现将课程内容总结如下. 

惯性测量单元（英文：Inertial measurement unit，简称IMU）是测量物体三轴姿态角(或角速率)以及加速度的装置, 由一个陀螺仪和一个加速度计组成. 

IMU的数学模型

IMU的误差模型

IMU的误差包括确定性误差和随机误差, 确定性误差包括:Bias Scale, 非对齐误差等.

Bias: 理论上，当没有外部作用时，IMU 传感器的输出应该为 0 。但是，实际数据存在一个偏置 b。加速度计 bias 对位姿估计的影响：

verr= bat, perr=bat2/2

Scale: 可以看成是实际数值和传感器输出值之间的比值。

非对齐误差: 多轴 IMU 传感器制作的时候，由于制作工艺的问题，会使得 xyz 轴可能不垂直，如下图所示。

    测量值 = 尺度对齐矩阵 x 真实值 , 如下:

IMU的随机误差包括高斯白噪声 bias随机游走等.

高斯白噪声: IMU 数据连续时间上受到一个均值为 0，方差为 σ，各时刻之间相互独立的高斯过程 n(t).

Bias 随机游走: 通常用维纳过程 (wiener process) 来建模 bias 随时间连续变化的过程，离散时间下称之为随机游走。

加速度计的数学模型

导航系G为东北天(ENU)，g = (0, 0, −9.81)⊤。理论测量值为：

其中amB是测量得到的加速度值, aG是该惯性系下的估计值. 如果考虑高斯白噪声na，bias误差，以及尺度因子Sa，则为：

    通常假设尺度因子为单位矩阵。

陀螺仪的数学模型

考虑尺度因子，高斯白噪声，以及 bias, 陀螺仪的误差模型如下：

低端传感器，考虑加速度对陀螺仪的影响，即 g-灵敏度：

陀螺仪受四种噪声的影响分别如下图所示：

运动模型

运动模型的连续积分

忽略 scale 的影响，只考虑白噪声和 bias 随机游走：

上标 g 表示 gyro，a 表示 acc，w 表示在世界坐标系 world，b 表示imu 机体坐标系 body。IMU 的真实值为 ω, a, 测量值为 ω˜, a˜。P(ose),V(elocity),Q(uaternion)对时间的导数可写成：

根据上面的导数关系，可以从第 i 时刻的 PVQ，通过对 IMU 的测量值进行积分，得到第 j 时刻的 PVQ:

j时刻的位置等于i时刻的位置,加上速度导致的位置变化量,加上加速度导致的位置变化量.

j时刻的速度等于i时刻的速度,加上加速度导致的速度变化量.

Q的变化量如上。

运动模型的离散积分——欧拉法

使用欧拉法，即两个相邻时刻 k 到 k+1 的位姿是用第 k 时刻的测量值 a, ω 来计算。

其中，

运动模型的离散积分——中值法

使用 mid-point 方法，即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算。

其中:

 

 

VIO

机器人定位存在多种方案, 其中IMU和相机定位的优缺点对比:

可以看到, 视觉和 IMU 定位方案存在一定互补性质：IMU 适合计算短时间、快速的运动；视觉适合计算长时间、慢速的运动。可利用视觉定位信息来估计 IMU 的零偏，减少 IMU 由零偏导致的发散和累积误差；反之，IMU 可以为视觉提供快速运动时的定位。

IMU和视觉进行融合定位时, 存在两种方案:

松耦合: 将 IMU 定位与视觉/GNSS 的位姿直接进行融合，融合过程对二者本身不产生影响，作为后处理方式输出。典型方案为卡尔曼滤波器。

紧耦合: 融合过程本身会影响视觉和 IMU 中的参数（如 IMU 的零偏和视觉的尺度）。典型方案为 MSCKF 和非线性优化。

IMU预积分

IMU的测量频率较高, 可达120Hz , 而相机的帧率一般只有30FPS, 若考虑关键帧的化, 频率更低. 如下图所示:

 同时, 紧耦合VIO中对相机位姿和IMU位姿联合优化, 导致与相机帧同时刻的IMU测量值会不断优化. 每次优化更新后，都需要重新进行积分，运算量较大。

从第 i 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:

下面简单的公式将t时刻的积分模型转为预积分模型：

则PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态，而不是相对于世界坐标系的姿态：

预积分量仅仅跟 IMU 测量值有关，将一段时间内的 IMU 数据直接积分起来就得到了预积分量, 记为如下项：

重新整理下 PVQ 的积分公式，有：

预积分误差: 一段时间内 IMU 构建的预积分量作为测量值，对两时刻之间的状态量进行约束:

上面误差中位移，速度，偏置都是直接相减得到。第二项是关于四元数的旋转误差，其中 [·]xyz 表示只取四元数的虚部 (x, y, z) 组成的三维向量。

预积分的离散形式: 这里使用 mid-point 方法，即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算：

预积分量的方差

    假设已知了相邻时刻误差的线性传递方程：

状态量误差为 ηik = [δθik, δvik, δpik]，测量噪声为nk = [ngk, nak]。误差的传递由两部分组成：当前时刻的误差传递给下一时刻，当前时刻测量噪声传递给下一时刻。

协方差矩阵可以通过递推计算得到：

其中，Σn 是测量噪声的协方差矩阵，方差从 i 时刻开始进行递推，Σii = 0。

状态误差线性递推公式的推导

通常对于状态量之间的递推关系是非线性的方程如 xk = f(xk-1, uk-1)，其中状态量为 x，u 为系统的输入量。我们可以用两种方法来推导状态误差传递的线性递推关系：

    • 一种是基于一阶泰勒展开的误差递推方程。

    • 一种是基于误差随时间变化的递推方程。

基于一阶泰勒展开的误差递推方程

令状态量为 x = xˆ + δx，其中，真值为 xˆ，误差为 δx。另外，输入量 u 的噪声为 n。对非线性状态方程进行一阶泰勒展开有：

其中F 是状态量 xk 对状态量 xk-1 的雅克比矩阵，G 是状态量 xk 对输入量 uk-1 的雅克比矩阵。 则非线性系统 xk = f(xk-1, uk-1) 的状态误差的线性递推关系如下：

    用前面一阶泰勒展开的推导方式，我们希望能推导出如下的形式：

F, G 为两个时刻间的协方差传递矩阵, 这里我们直接给出 F, G 的最终形式:

其中: