手写VIO学习笔记（二）

一、旋转运动学

body坐标系为IMU坐标系，inertial（惯性）坐标系也称为n系。

质量块在 body 坐标系下的坐标为：

\[{r^b} = {({x_1},{x_2},{x_3})^T}\]

忽略平移，只考虑旋转，则旋转到惯性系下有：

\[{r^i}(t) = {R_{ib}}{r^b}\]

对时间求导有：

其中\(w = {R_{ib}}{w^b}\)，表示body坐标系的角速度在I系下的表示。通过结论可以看出，物体在I系下的运动速度不仅仅是旋转矩阵乘上物体在body坐标系下的速度，还与body坐标系的角速度等因素有关。

补充：

其实就是\({{\dot R}_{ib}} = {R_{ib}}{[{w^b}]_ \times } = {[{R_{ib}}{w^b}]_ \times }{R_{ib}}，其中 {[{w^b}]_ \times }\)代表\({w^b}\)的反对称。

对速度求导得到：

其中，\[{{\dot R}_{ib}}{w^b} = {[{R_{ib}}{w^b}]_ \times }{w^b} = {R_{ib}}{w^b} \times {w^b} = 0\]。\(v = {R_{ib}}{v^b}\)和\(a = {R_{ib}}{a^b}\)，表示物体在body系下的速度或加速度在I系下的表示。

通过结论可以得出在旋转坐标系下观察，运动的物体（运动方向和旋转轴不为同一个轴时）会受到科氏力的作用。

二、IMU测量模型及运动模型

1、MEMS加速度计工作原理

测量原理可以用一个简单的质量块+弹簧+指示计来表示，加速度计测量值\({{\rm{a}}_m}\)为弹簧拉力对应的加速度，

\[{{\rm{a}}_m} = \frac{{\rm{f}}}{m} = {\rm{a}} - g\]

f 弹簧拉力， a 物体在惯性系下的加速度， g 重力加速度（有方向）。

MEMS 加速度计利用电容或者电阻桥来等原理来测量 am。

东北天坐标系 (ENU): \(g = {(0,0, - 9.81)^{\rm{T}}}\)

假设 IMU 坐标系就是 ENU 坐标系，\({R_{ib}} = {\rm{I}}\)，静止时有

\(\begin{array}{l}
{\rm{a = 0}}\\
{{\rm{a}}_m} =  - g
\end{array}\)

自由落体时有

\(\begin{array}{l}
{\rm{a}} = g\\
{{\rm{a}}_m} = 0
\end{array}\)

2、MEMS陀螺仪工作原理

陀螺仪主要用来测量物体的旋转角速度，按测量原理分有振动陀螺，光纤陀螺等。低端 MEMS 陀螺上一般采用振动陀螺原理，通过测量 Coriolisforce （科氏力）来间接得到角速度。

在旋转坐标系中，运动的物体受到科氏力作用。例如MEMS 陀螺仪：一个主动运动轴 + 一个敏感轴（测量科氏力）

此测量方法会受到外部加速度的影响，下面介绍的音叉振动陀螺仪会解决此问题。

音叉振动陀螺仪测量原理

叉子的中间为旋转轴，叉子左右两个质量块，做方向相反的正弦运动，质量块受到的科氏力方向相反。两个质量块测量的科氏力（带方向）相减就可以把外部加速度抵消。

实际上，两个质量块不可能完全一致，也就是说陀螺仪的测量可能会受到外部加速度的影响，即常称的 G-sensitivity。

三、IMU误差模型

1、误差分类

加速度计和陀螺仪的误差可以分为：确定性误差，随机误差。

确定性误差可以事先标定确定，包括： bias, scale ...

随机误差通常假设噪声服从高斯分布，包括：高斯白噪声， bias随机游走..

2、确定性误差

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

\({{\rm{v}}_{err}} = {{\rm{b}}^a}t{{\rm{p}}_{err}} = \frac{1}{2}{{\rm{b}}^a}{t^2}\)

可见，当t非常大时产生的加速度误差和速度误差也会非常大。

scale：可以看成是实际数值和传感器输出值之间的比值

Nonorthogonality/Misalignment Errors：多轴 IMU 传感器制作的时候，由于制作工艺的问题，会使得 xyz 轴可能不垂直，如下图所示：

其中，\({s_{xx}},{s_{yy}},{s_{zz}}\)为尺度因子。由\({l_{ax}} = {s_{xx}}{a_x} + {m_{xy}}{a_y} + {m_{xz}}{a_z}\)可以看出x轴的分量受到y轴和z轴的影响。

其他确定性误差：

Run-to-Run Bias/Scale Factor
In Run (Stability) Bias/Scale Factor
Temperature-Dependent Bias/Scale Factor
......

3、确定性误差的标定

六面法标定加速度计bias和scale

六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间，采集 6 个面的数据完成标定。如果各个轴都是正交的，那很容易得到 bias 和 scale：

其中， l为加速度计某个轴的测量值， g 为当地的重力加速度。

考虑轴间误差的时候，实际加速度和测量值之间的关系为：（\({l_{ax}},{l_{ay}},{l_{az}}\)为测量值，\({a_x},{a_y},{a_z}\)为实际值）

水平静止放置 6 面的时候，加速度的理论值为：
 

对应的测量值矩阵 L ：
 

利用最小二乘就能够把 12 个变量求出来。就是把六个理论值分别当成实际值（水平放置时的加速度就应该是理论值，只是因为加速度计的scale，Misalignment和Bias导致的测量误差），再结合六次的测量值（已知）求出scale，Misalignment和Bias。

六面法标定陀螺仪bias和scale

和加速度计六面法不同的是，陀螺仪的真实值由高精度转台提供，这里的 6六面是指各个轴顺时针和逆时针旋转。

温度相关的参数标定

目的：这个标定的主要目的是对传感器估计的 bias 和 scale 进行温度补偿，获取不同温度时 bias 和 scale 的值，绘制成曲线。

两种标定方法：

soak method: 控制恒温室的温度值，然后读取传感器数值进行标定。
ramp method：记录一段时间内线性升温和降温时传感器的数据来进行标定。

4、随机误差

高斯白噪声

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

其中 δ() 表示狄拉克函数，当\({t_1} = {t_2}\)时，\(\delta ({t_1} - {t_2}) = 1\)，当\({t_1} \ne {t_2}\)时，\(\delta ({t_1} - {t_2}) = 0\)，体现了各时刻之间的相互独立。

Bias随机游走

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

其中 w 是方差为 1 的白噪声。

引出问题：实际上， IMU 传感器获取的数据为离散采样，离散和连续高斯白噪声存在何种关系？下面来探讨：

高斯白噪声的离散化

假设有一个单轴角速度信号收到高斯白噪声和 bias 的影响，建模如下：
 

当传感器采集信号时，假设采样时间段内信息为常数：
 

即：

只考虑高斯白噪声的积分，将连续的高斯白噪声积分变成离散化的高斯白噪声：
 

协方差为（狄拉克函数的积分为1）：

即

其中

也就是说高斯白噪声的连续时间到离散时间之间差一个 \(\frac{1}{{\sqrt {\Delta t} }}\), \(\sqrt {\Delta t} \)是传感器的采样时间。

bias随机游走的离散化

由此公式可以得出：

由此可得离散化下的 bias 协方差
 

 

 

 

 

由于

所以有

即

bias 随机游走的噪声方差从连续时间到离散之间需要乘以 \(\sqrt {\Delta t} \)。

IMU随机误差的标定

艾伦方差标定（random walk noise），Allan方差是20世纪60年代由美国国家标准局的David Allan提出的，它是一种基于时域的分析方法，具体的流程如下:

1. 保持传感器绝对静止获取数据。
2. 对数据进行分段，设定时间段的时长，如下图所示。
 

3. 将传感器数据按照时间段进行平均。
4. 计算方差，绘制艾伦曲线。
得到的艾伦曲线如下图所示：

该曲线上，m=-0.5的直线对应t=1时刻的值即为高斯白噪声的方差，m=0.5的直线对应t=3时刻的值即为bias随机游走的方差。

Allan方差的验证：

制作一个仿真 IMU 数据集。设定，加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015.。加速度 bias 的随机游走噪声设定为 5e-4，陀螺仪的 bias 随机游走噪声设定为 5e-5。
加速度的艾伦方差曲线如下：
 

5、IMU数学模型

加速度计数学模型：

导航系（建立在地表的一个特定坐标系） G 为东北天，body系B为IMU坐标系。\({{\rm{g}}^G} = {(0,0, - 9.81)^{\rm{T}}}\)。理论测量值为：

如果考虑高斯白噪声， bias，以及尺度因子，则为：

通常假设尺度因子为单位矩阵（三个轴正交）。

陀螺仪数学模型：

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

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

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

四、运动模型离散时间处理

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

上标 g 表示 gyro，a 表示 acc， w 表示在世界坐标系 world，b表示imu机体坐标系body。 IMU的真实值为 ω, a, 测量值为 ω˜, a˜。（特别强调，此处的\({{\rm{g}}^w}\)前面为+，而前面为-，此处的g为+9.81，所以为加号，只是个人习惯而已）

P(ose),V(elocity),Q(uaternion) 对时间的导数可写成：

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

1、运动模型的离散积分---欧拉法

四元数时间求导：

所以，

\[\begin{array}{l}
{q_{w{b_{k + 1}}}} = {q_{w{b_k}}} + {{\dot q}_{w{b_k}}}\delta t = {q_{w{b_k}}} + {q_{w{b_k}}} \otimes \left( \begin{array}{l}
0\\
\frac{1}{2}w\delta t
\end{array} \right)\\
 = {q_{w{b_k}}} \otimes \left( \begin{array}{l}
1\\
0
\end{array} \right) + {q_{w{b_k}}} \otimes \left( \begin{array}{l}
0\\
\frac{1}{2}w\delta t
\end{array} \right) = {q_{w{b_k}}} \otimes \left( \begin{array}{l}
1\\
\frac{1}{2}w\delta t
\end{array} \right)
\end{array}\]

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

其中，

2、运动模型的离散积分---中值法

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

其中，

五、IMU数据仿真

1、仿真思路：

思路 1：指定轨迹方程，求一阶导得到速度, 角速度，求二阶导得到加速度。
思路 2：已有 pose 轨迹，不知道方程，利用 B-Spline 产生 IMU数据。

2、旋转基础知识

旋转积分的几种方式

四元数形式：

SO3形式：

欧拉角形式：

其中\(\vartheta  = {({\psi _{roll}},{\theta _{pitch}},{\phi _{yaw}})^{\rm{T}}}\)，\({E_{wb}}\)表示将 IMU body 坐标系下的角速
度转换成欧拉角速度a。

3、欧拉角

问题：inertial frame 下的一个点旋转到 body 坐标系，用欧拉角如何表示？仿真数据中旋转矩阵用欧拉角来表示很方便。

step 1. 绕着惯性坐标系的 z 轴旋转，得到新的坐标系 b1。

step 2. 绕着新坐标系 b1 的 y 轴旋转得到坐标系 b2

step 3. 绕着新坐标系 b2 的 x 轴旋转得到坐标系 b3, b3 就是我们的body 坐标系。

综合起来，得到：

问题：inertial frame 下的欧拉角速度怎么转换到body坐标系下呢？

euler rate to body rate：

公式取逆就能得到， body rate to euler rate 的变换：