IMU传感器和预积分

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

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

图片1.jpg

IMU的数学模型

IMU的误差模型

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

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

verr= bat, perr=bat2/2

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

图片2.jpg

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

图片3.jpg

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

图片4.jpg


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

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

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


加速度计的数学模型

图片5.jpg 

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

图片6.png

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

图片7.png 

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

陀螺仪的数学模型

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

图片8.png 

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

图片9.png 

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

图片10.jpg



运动模型

运动模型的连续积分

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

图片11.png 

上标 g 表示 gyroa 表示 accw 表示在世界坐标系 worldb 表示imu 机体坐标系 bodyIMU 的真实值为 ω, a, 测量值为 ω˜, a˜。P(ose),V(elocity),Q(uaternion)对时间的导数可写成:

图片12.png 

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

图片13.png 

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

图片14.png 

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

图片15.png

Q的变化量如上。

 

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

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

图片16.png 

其中,

图片17.png

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

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

图片18.png 

其中:

 图片19.png

 

 

VIO

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

图片20.jpg 

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

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

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

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

 

 

 

IMU预积分

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

图片21.jpg 

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

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

图片22.jpg 

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

图片23.jpg

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

图片24.jpg 

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

图片25.jpg 

重新整理下 PVQ 的积分公式,有:

图片26.jpg 

 

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

图片27.jpg 

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

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

图片28.jpg 

 

 

预积分量的方差

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

图片29.jpg 

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

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

图片30.jpg 

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

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

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

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

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

 

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

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

图片31.jpg 

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

图片32.jpg 


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

图片33.jpg 

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

图片34.jpg 

其中:

图片35.jpg 

 











下一篇:

首页 所有文章 机器人 计算机视觉 自然语言处理 机器学习 编程随笔 关于