IMU传感器与误差模型

IMU传感器模型与误差模型

1 旋转运动学-刚体坐标系

​ 定义几个常用的坐标系，b代表本体body坐标系，也就是IMU所在的坐标系；I代表世界world坐标系(惯性系inertial)；有些论文会这样描述the rotation from the inertial frame to the body frame，所以要搞清楚这些坐标系之间的关系。

​ 质量块在body坐标系下的坐标为：
$${\mathbf{r}}_B=(x_1,x_2,x_3{)}^{\top }$$
​ 把这个坐标旋转到惯性系（世界系）
$${\mathbf{r}}_I(t)=x_1(t)\mathbf{i}+x_2(t)\mathbf{j}+x_3(t)\mathbf{k}={\mathbf{R}}_{IB}{\mathbf{r}}_B$$
① 位置的导数：速度

  对位置求导，得到惯性系下的速度$v_I$
$$\begin{array}{rl}& {\dot{\mathbf{r}}}_I={\mathbf{R}}_{IB}{\dot{\mathbf{r}}}_B+{\dot{\mathbf{R}}}_{IB}{\mathbf{r}}_B\\ & ={\mathbf{R}}_{IB}{\dot{\mathbf{r}}}_B+[{\mathbf{R}}_{IB}{\omega }_b{]}_{\times }{\mathbf{r}}_I\\ & ={\mathbf{R}}_{IB}{\mathbf{v}}_B+\mathbf{\omega }\times {\mathbf{r}}_I\\ & ={\mathbf{v}}_I\end{array}$$
  其中，对旋转矩阵R的导数右下面推出，这里用到了泰勒展开和$R{p}^{\wedge }{R}^{\mathrm{T}}=(Rp{)}^{\wedge }$这一条性质。最后一行因为反对称矩阵也是叉积。
$$\begin{array}{rl}{\dot{\mathbf{R}}}_{IB}{\mathbf{r}}_B& =\underset{\Delta t\to 0}{\lim }\frac{{\mathbf{R}}_{IB}\exp ([{\omega }_{B{B}^{\prime }}\Delta t{]}^{\wedge }){\mathbf{r}}_B-{\mathbf{R}}_{IB}{\mathbf{r}}_B}{\Delta t}\\ & \approx ({\mathbf{R}}_{IB}(I+[{\omega }_{B{B}^{\prime }}\Delta t{]}^{\wedge }){\mathbf{r}}_B-R_{IB}{r}_B)/\Delta t\\ & \approx {\mathbf{R}}_{IB}({\omega }_{B{B}^{\prime }}{)}^{\wedge }{r}_B\\ & =({\mathbf{R}}_{IB}{\omega }_{B{B}^{\prime }}{)}^{\wedge }{\mathbf{R}}_{IB}{\mathbf{r}}_B=\mathbf{\omega }\times {\mathbf{r}}_I\end{array}$$

  这里有一个非常重要的结论，${\mathbf{r}}_I(t)={\mathbf{R}}_{IB}{\mathbf{r}}_B$,但是$v_I!={\mathbf{R}}_{IB}{\mathbf{v}}_B$,也就是说，任一个点在两个坐标系下的速度矢量都是不同的，不是同一个矢量在不同坐标系下的表达！下面加速度同理。
$$\begin{array}{rl}& {\mathbf{v}}_I\equiv {\mathbf{R}}_{IB}{\mathbf{v}}_B+\omega \times {\mathbf{r}}_I\iff {\mathbf{R}}_{IB}{\mathbf{v}}_B\equiv {\mathbf{v}}_I-\omega \times {\mathbf{r}}_I\end{array}$$

② 速度的导数：加速度

$$\begin{array}{rl}\ddot{\mathbf{r}}& ={\mathbf{R}}_{IB}{\dot{\mathbf{v}}}_B+{\dot{\mathbf{R}}}_{IB}{\mathbf{v}}_B+\mathbf{\omega }\times {\dot{\mathbf{r}}}_I+[{\dot{\mathbf{R}}}_{IB}{\mathbf{\omega }}_b+{\mathbf{R}}_{IB}{\dot{\mathbf{\omega }}}_b{]}_{\times }{\mathbf{r}}_I\\ & ={\mathbf{R}}_{IB}{\dot{\mathbf{v}}}_B+{\dot{\mathbf{R}}}_{IB}{\mathbf{v}}_B+\mathbf{\omega }\times (\mathbf{v}+\mathbf{\omega }\times {\mathbf{r}}_I)+[{\mathbf{R}}_{IB}{\mathbf{\omega }}_b{]}_{\times }{\mathbf{r}}_I\\ & ={\mathbf{R}}_{IB}{\mathbf{a}}_B+2\mathbf{\omega }\times \mathbf{v}+\mathbf{\omega }\times (\mathbf{\omega }\times {\mathbf{r}}_I)+\dot{\mathbf{\omega }}\times {\mathbf{r}}_I\\ \Rightarrow \mathbf{a}& ={\mathbf{a}}_I-\underset{\text{科氏力}}{2\mathbf{\omega }\times \mathbf{v}}-\underset{\text{欧拉力}}{\dot{\mathbf{\omega }}\times {\mathbf{r}}_I}-\underset{\text{离心力}}{\mathbf{\omega }\times ({\mathbf{\omega }}_I\times {\mathbf{r}}_I)}\end{array}$$

③ 关于速度、加速度、角速度在不同坐标系间关系

  实际上，如果把IMU放在车辆中心，我们可以忽略掉科氏力等的影响，认为IMU所测的加速度即车体系加速度，即世界系下加速度矢量转换到车辆坐标系下的结果。

  总体来讲，速度和加速度都是世界系下矢量转换到车辆坐标系下的结果，而不是车辆坐标系中的矢量，因为对于车辆自身坐标系来说，其速度一直为0，更不可能有加速度了！

  注意IMU测量的加速度和角速度都是body系下的变量！比如下面误差模型中的左侧的测量值，右上角b表示body系下变量！（这个是我一开始疑惑的地方，IMU测量的数据到底是那个系下的变量，主要是把模型和实际搞反）

  至于为什么在下面的误差模型中，只是变换了加速度，而没有变换角速度。因为实际中，我们得到的是左边的测量值，而不是右边的值！我们实际使用的两个变量，角速度不需要变换到世界系下！
$$\begin{array}{rl}{\tilde{\mathbf{\omega }}}^b& ={\omega }^b+{\mathbf{b}}_g+{\eta }_g\\ {\tilde{\mathbf{a}}}^b& ={\mathbf{R}}_{bw}({\mathbf{a}}^w-{\mathbf{g}}^w)+{\mathbf{b}}_a+{\eta }_a\end{array}$$
  在运动系统中，角速度与加速度是如何测量的。这种思路是仿真的思路，或者说是描述已知系统的思路。也就是说，我们可以先假设物体发生了什么运动，再去考虑这种运动下应该产生什么样的 IMU 测量。但是，实际情况是反过来的。我们能够读取到 IMU 传感器的读数，但必须根据 IMU 和其他传感器的读数来推断系统的运动，而不能直接获取系统的各种速度、加速度信息。

  我们要用测量得到的IMU数据去进行航迹推算，下面的式子忽略了噪声
$$\begin{array}{rl}{\dot{\mathbf{R}}}_{wb}& ={\mathbf{R}}_{wb}({\tilde{\mathbf{\omega }}}^b-{\mathbf{b}}_g{)}^{\wedge },\text{或 }\dot{\mathbf{q}}=\mathbf{q}\left[0,\frac{1}{2}\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g\right)\right]\\ {\dot{\mathbf{p}}}^w& ={\mathbf{v}}^w,\\ {\dot{\mathbf{v}}}^w& ={\mathbf{R}}_{wb}({\tilde{\mathbf{a}}}^b-{\mathbf{b}}_a)+{\mathbf{g}}^w=a^w.\end{array}$$
  通过欧拉法进行积分（省略了上下标），除此以外，还有中值积分、梯形积分以及高阶插值法-龙格库塔法（比如MSCKF）。
$$\begin{array}{rl}& \mathbf{R}(t+\Delta t)=\mathbf{R}(t)\mathrm{Exp}\left((\tilde{\omega }-{\mathbf{b}}_g)\Delta t\right),\text{或 }q(t+\Delta t)=\mathbf{q}(t)\lfloor 1,\frac{1}{2}\left(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g\right)\Delta t\rfloor ,\\ & \mathbf{p}(t+\Delta t)=\mathbf{p}(t)+\mathbf{v}\Delta t+\frac{1}{2}\left(\mathbf{R}(t)(\tilde{\mathbf{a}}-{\mathbf{b}}_a)\right)\Delta t^2+\frac{1}{2}\mathbf{g}\Delta t^2,\\ & \mathbf{v}(t+\Delta t)=\mathbf{v}(t)+\mathbf{R}(t)(\tilde{\mathbf{a}}-{\mathbf{b}}_a)\Delta t+\mathbf{g}\Delta t.\end{array}$$
​ 上式也可以进一步累积，比如从 i 时刻一直递推到 j 时刻
$$\begin{array}{rl}& R_j=R_i\prod _{k=i}^{j-1}\mathrm{Exp}\left(\left({\tilde{\omega }}_k-b_{g,k}\right)\Delta t\right)\text{或 }{\mathbf{q}}_j={\mathbf{q}}_i\prod _{k=i}^{j-1}\left[1,\frac{1}{2}\left({\tilde{\omega }}_k-{\mathbf{b}}_{g,k}\right)\Delta t\right],\\ & {\mathbf{p}}_j={\mathbf{p}}_k+\sum _{k=i}^{j-1}\left[{\mathbf{v}}_k\Delta t+\frac{1}{2}\mathbf{g}\Delta t^2\right]+\frac{1}{2}\sum _{k=i}^{j-1}{\mathbf{R}}_k\left({\mathbf{a}}_k-{\mathbf{b}}_{a,k}\right)\Delta t^2,\\ & {\mathbf{v}}_j={\mathbf{v}}_i+\sum _{k=i}^{j-1}\left[{\mathbf{R}}_k\left({\tilde{\mathbf{a}}}_k-{\mathbf{b}}_{a,k}\right)\Delta t+\mathbf{g}\Delta t\right].\end{array}$$

2 IMU工作原理

  IMU由陀螺仪+加速度计组成，绝⼤多数场景下认为他们是同⼀坐标系，使用时认为他们是捷联的（就是相对固定的）

2.1 加速度计工作原理

  测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示，加速度计测量值$a_m$为弹簧拉力对应的加速度，$f$ 弹簧拉力，$a$ 物体在惯性系下的加速度，$g$ 重力加速度。

$${\mathbf{a}}_m=\frac{\mathbf{f}}{m}=\mathbf{a}-\mathbf{g}$$
  加速度计经常使用的坐标系：东北天坐标系 (ENU)

  静止时，合力为0
$$\begin{array}{rl}\mathbf{a}& =0\\ {\mathbf{a}}_m& =-\mathbf{g}\end{array}$$
  自由落体时：合力即重力，所以$a=g$
$$\begin{array}{rl}\mathbf{a}& =\mathbf{g}\\ {\mathbf{a}}_m& =0\end{array}$$

2.2 陀螺仪测量原理

  陀螺仪主要用来测量物体的旋转角速度，按测量原理分有振动陀螺，光纤陀螺等。MEMS 陀螺仪：一个主动运动轴 + 一个敏感轴。在旋转坐标系中，运动的物体受到科氏力作用

3 IMU误差模型

  一般来讲，IMU存在以下的两种误差：确定性误差和随机误差。值得注意，确定性误差bias会随时间发生随机的改变！也就是形成随机误差中的bias随机游走，随机游走的本质是指导数为高斯过程的随机过程！

3.1 确定性误差

3.1.1 Bias

  理论上，当没有外部作用时，$IMU$ 传感器的输出应该为 0。但是，实际数据存在一个偏置 $b$。加速度计 $bias$ 对位姿估计的影响：
$${\mathbf{v}}_{err}={\mathbf{b}}_{a}t,{\mathbf{p}}_{err}=\frac{1}{2}{\mathbf{b}}_a{t}^2$$

3.1.2 Scale

  真实值和传感器测量值之间存在尺度关系，但不为1.

  工艺问题导致的误差

  scale + Misalignment（尺度 + 失真）

  对角线元素表示尺度，其它的表示由于xyz轴不垂直给其他轴产生的失真影响。
[ l a x l a y l a z ] = [ s x x m x y m x z m y x s y y m y z m z x m z y s z z ] [ a x a y a z ] \begin{bmatrix}l_{ax}\\l_{ay}\\l_{az}\end{bmatrix}=\begin{bmatrix}s_{xx}&m_{xy}&m_{xz}\\m_{yx}&s_{yy}&m_{yz}\\m_{zx}&m_{zy}&s_{zz}\end{bmatrix}\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} ​lax​lay​laz​​ ​= ​sxx​myx​mzx​​mxy​syy​mzy​​mxz​myz​szz​​ ​ ​ax​ay​az​​ ​

3.2 确定性误差标定—六面法

3.2.1 六面法标定加速度

  首先确定上面的确定性误差模型，实际值$l$ = 尺度*测量 + 偏差。所以我们在标定的时候，需要求解出这12个变量。
[ l a x l a y l a z ] = [ s x x m x y m x z m y x s y y m y z m z x m z y s z z ] [ a x a y a z ] + [ b a x b a y b a z ] \begin{bmatrix}l_{ax}\\l_{ay}\\l_{az}\end{bmatrix}=\begin{bmatrix}s_{xx}&m_{xy}&m_{xz}\\m_{yx}&s_{yy}&m_{yz}\\m_{zx}&m_{zy}&s_{zz}\end{bmatrix}\begin{bmatrix}a_{x}\\a_{y}\\a_{z}\end{bmatrix}+\begin{bmatrix}b_{ax}\\b_{ay}\\b_{az}\end{bmatrix} ​lax​lay​laz​​ ​= ​sxx​myx​mzx​​mxy​syy​mzy​​mxz​myz​szz​​ ​ ​ax​ay​az​​ ​+ ​bax​bay​baz​​ ​
  六面法的具体原理如下：

3.2.2 六面法标定陀螺仪

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

具体方法同上。

3.3 IMU随机误差

3.3.1 高斯过程

  在高斯过程Gaussian process中，连续输入空间中每个点都是与一个正态分布的随机变量相关联。此外，这些随机变量的每个有限集合都有一个多元正态分布，换句话说他们的任意有限线性组合是一个正态分布。高斯过程的分布是所有那些（无限多个）随机变量的联合分布，正因如此，它是连续域（例如时间或空间）上函数的分布。

  可以把高斯过程看成多元正态分布的无限维广义延伸，每一个采样时刻的变量对应一个维度。

3.3.2 高斯白噪声

  IMU 数据连续时间上受到一个均值为 0，方差为 $\sigma$，各时刻之间相互独立的高斯过程 $n(t)$
$$\begin{array}{rl}& E[n(t)]\equiv 0\\ & E\left[n\left(t_1\right)n\left(t_2\right)\right]={\sigma }^2\delta \left(t_1-t_2\right)\end{array}$$
  但是IMU 传感器获取的数据为离散采样，离散和连续高斯白噪声的方差之间存在如下转换关系：
$$\begin{array}{rl}{n}_d[k]& \triangleq n\left(t_0+\Delta t\right)\simeq \frac{1}{\Delta t}{\int }_{t_0}^{t_0+\Delta t}n(\tau )dt\\ E\left(n_d[k{]}^2\right)& =E\left(\frac{1}{\Delta t^2}{\int }_{t_0}^{t_0+\Delta t}{\int }_{t_0}^{t_0+\Delta t}n(\tau )n(t)d\tau dt\right)\\ & =E\left(\frac{{\sigma }^2}{\Delta t^2}{\int }_{t_0}^{t_0+\Delta t}{\int }_{t_0}^{t_0+\Delta t}\delta (t-\tau )d\tau dt\right)\\ & =E\left(\frac{{\sigma }^2}{\Delta t}\right)\end{array}$$
  所以我们可以推出连续和离散的高斯白噪声标准差相差一个$\frac{1}{\sqrt{\Delta t}}$，其中$w[k]\sim \mathcal{N}(0,1)$
$$n_d[k]={\sigma }_{d}w[k]=\sigma \frac{1}{\sqrt{\Delta t}}w[k]$$

3.3.3 Bias随机游走

  在上面确定性误差bias中，bias会随时间的改变而改变！

  通常用维纳过程(wiener process)来建模 bias 随时间连续变化的过程，离散时间下称之为随机游走，其中 w 是方差为 1 的白噪声。换句话说，随机游走就是导数为高斯过程的随机过程！
$$\dot{b}(t)=n(t)={\sigma }_{b}w(t)$$
  bias 随机游走的噪声标准差从连续时间到离散之间需要乘以$\sqrt{\Delta t}$

$$\begin{array}{rl}{b}_d[k]\triangleq & b\left(t_0\right)+{\int }_{t_0}^{t_0+\Delta t}n(t)dt\\ E\left({\left(b_d[k]-b_d[k-1]\right)}^2\right)& =E\left({\int }_{t_0+\Delta t}^{t_0+\Delta t}{\int }_{t_0}^{t_0+\Delta t}n(t)n(\tau )d\tau dt\right)\\ & =E\left({\sigma }_b^2{\int }_{t_0}^{t_0+\Delta t}{\int }_{t_0}^{t_0+\Delta t}\delta (t-\tau )d\tau dt\right)\\ & =E\left({\sigma }_b^2\Delta t\right)\end{array}$$
  即
$$b_d[k]=b_d[k-1]+{\sigma }_{bd}w[k]$$
  其中：
$$\begin{array}{l}w[k]\sim \mathcal{N}(0,1)\\ {\sigma }_{bd}={\sigma }_b\sqrt{\Delta t}\end{array}$$

3.3.4 总结

  因为 IMU传感器按照固定频率进行采样，不妨设每次采样间隔为 ∆t，那么对于噪声来说，陀螺仪和加速度计的离散测量噪声可以简化地描述为
$$\begin{array}{rl}{\eta }_g(k)& \sim \mathcal{N}(0,\frac{1}{\Delta t}\mathrm{Cov}({\eta }_g)),\\ {\eta }_a(k)& \sim \mathcal{N}(0,\frac{1}{\Delta t}\mathrm{Cov}({\eta }_a)).\end{array}$$
  对于零偏部分，则可以写：
$$\begin{array}{rl}{\mathbf{b}}_g(k+1)-{\mathbf{b}}_g(k)& \sim \mathcal{N}(0,\Delta t\mathrm{Cov}({\mathbf{b}}_g)),\\ {\mathbf{b}}_a(k+1)-{\mathbf{b}}_a(k)& \sim \mathcal{N}(0,\Delta t\mathrm{Cov}({\mathbf{b}}_a)).\end{array}$$

  ${\sigma }_g,{\sigma }_a$ 的参数来表达 IMU 的噪声标准差，用${\sigma }_{b}g,{\sigma }_{b}a$参数来表达零偏游走的标准差，下面表明了离散与连续之间的转换关系（一般标定都是连续，程序中使用离散需要进行转换）
$$\begin{array}{rlr}{\sigma }_g(k)& =\frac{1}{\sqrt{\Delta t}}{\sigma }_g,{\sigma }_a(k)& =\frac{1}{\sqrt{\Delta t}}{\sigma }_a,\\ {\sigma }_{bg}(k)& =\sqrt{\Delta t}{\sigma }_{bg},{\sigma }_{ba}(k)& =\sqrt{\Delta t}{\sigma }_{ba}.\end{array}$$
  离散时间的噪声和零偏是直接加在被测的物理量上的，很容易确定它们的物理单位
$${\sigma }_g(k)\to \frac{rad}{s},{\sigma }_a(k)\to \frac{m}{s^2},{\sigma }_{bg}(k)\to \frac{rad}{s},{\sigma }_{ba}(k)\to \frac{m}{s^2}.$$
  连续时间的方差需要在离散方差上乘或除以一个开方时间单位
$${\sigma }_g\to \frac{rad}{\sqrt{s}},{\sigma }_a\to \frac{m}{s\sqrt{s}},{\sigma }_{bg}\to \frac{rad}{s\sqrt{s}},{\sigma }_{ba}\to \frac{m}{s^2\sqrt{s}}.$$

3.5 IMU误差模型总结

  $S_a$为尺度，$n_a$为高斯白噪声， $b_a$为偏差（随机游走的确定性偏差）
$${\mathbf{a}}_m^B={\mathbf{S}}_a{\mathbf{R}}_{BG}\left({\mathbf{a}}^G-{\mathbf{g}}^G\right)+{\eta }_a+{\mathbf{b}}_a$$

  低端IMU会受到科氏力等影响，所以还要考虑加速度$a^B$对陀螺仪的影响。
$${\omega }_m^B={\mathbf{S}}_g{\omega }^B+{\mathbf{s}}_{ga}{\mathbf{a}}^B+{\eta }_g+{\mathbf{b}}_g$$

• 简化的IMU误差模型

  很多时候都会忽略尺度的影响，以及如果IMU安装在车体中心，就可以忽略科氏力等的影响！
$$\begin{array}{rl}{\tilde{\mathbf{\omega }}}^b& ={\omega }^b+{\mathbf{b}}_g+{\eta }_g\\ {\tilde{\mathbf{a}}}^b& ={\mathbf{R}}_{bw}({\mathbf{a}}^w-{\mathbf{g}}^w)+{\mathbf{b}}_a+{\eta }_a\end{array}$$