IMU工作原理、误差及其标定

博主： LukeyAlvin
发布时间：2022 年 04 月 25 日
429 次浏览
暂无评论
9416字数
分类：视觉SLAM

本节主要对IMU的工作原理做详细的介绍，分析IMU中的确定性误差以及随机误差，并给出误差标定的方法，如确定性误差使用六面法标定，随机误差使用艾伦方差标定等。

Ⅰ.旋转运动学

A.线速度与角速度

如图所示，粒子在坐标系中的$z=h$的平面内做圆周运动，坐标为$r=(a\cos\theta,a\sin\theta,h)$，对坐标求导得：

$$ \begin{align*} \dot r&=(-a\dot\theta\sin\theta,a\dot\theta\cos\theta,0)^T\\ &=\begin{bmatrix} 0&-\dot\theta&0\\\dot\theta&0&0\\0&0&0 \end{bmatrix} \begin{bmatrix}a\cos\theta\\a\sin\theta\\h\end{bmatrix}\\ &=w^{\land} r \end{align*}\tag{1} $$

其中$w$是角速度，它是一个矢量，即$w=\begin{bmatrix}0\\0\\ \dot\theta\end{bmatrix}=\dot\theta z$,其中$|\dot\theta|$是角速度的大小。对公式$(1)$取模：

$$ |\dot r|=|w||r|sin\phi=a|\dot\theta| $$

B.旋转坐标系下的运动学

如图，考虑两个坐标系，一个是世界坐标系(惯性坐标系)$W$，它本身是静止不动的；一个是机器人本体坐标系$B$，它本身在做角速度为$w_B$的纯旋转，设质量块在坐标系$B$下做速度为$v_B$，加速度为$a_B$的匀加速直线运动，它在$B$坐标系下坐标为$r_B=(x_1,x_2,x_3)^T$，设从坐标系世界坐标系$W$到是机器人本体坐标系$B$只考虑旋转的时候，旋转矩阵$R_{WB}$。

这里定义一些变量：

①设物体在坐标系$B$下做速度为$v_B$，加速度为$a_B$，对应在坐标系$W$下的速度为$v_W$，加速度为$a_W$
并且设坐标系$B$下的速度和加速度在坐标系$W$下的表示分别为：$v=R_{WB}v_B$，$a=R_{WB}a_B$，一定注意这里的$v\ne v_W,a\ne a_W$
②设坐标轴$B$旋转的角速度为$w_B$，对应在世界坐标系中其旋转的角速度变为$w_W$，且$w_W=R_{WB}w_B$
③设物体在坐标系$B$下的坐标为$r_B$，对应在坐标系$W$下的坐标为$r_W$，且$r_W=R_{WB}r_B$

则对应到世界坐标系下：

$$ r_W(t)=x_1(t)i+x_2(t)j+x_3(t)k=R_{WB}r_B $$

一般简写为：$r_B=x_ie_i$

经过一番定义之后，我们考虑几个问题:

求物体在$W$坐标系下的速度

对时间求导：

$$ \begin{align*} \dot r_W=v_W&=R_{WB}\dot r_B+\dot R_{WB} r_B\\ &=R_{WB}v_B+w_W^{\land} r_W \ \ \ \ 参见公式(3) \\ &=v+w_W^{\land} r_W\Leftrightarrow v=v_W-w_W^{\land} r_W\\ \end{align*}\tag{2} $$

其中$w_W=R_{WB}w_b$表示body坐标系$B$的角速度在世界坐标系$W$下的表示。具体推导参见公式$(3)$.

这里可以明显看出$v\ne v_W$，两者之间相差一个$w_W^{\land} r_W$项。

推导$\dot R_{WB} r_B$，使用右乘扰动模型:

$$ \begin{align*} \dot R_{WB} r_B&=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}\exp([w_B\Delta t]^{\land})r_B-R_{WB}r_B}{\Delta t}\\ &\approx\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}(I+[w_B\Delta t]^{\land})r_B-R_{WB}r_B}{\Delta t}\\ &=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}[w_B\Delta t]^{\land}r_B}{\Delta t}\\ &=\underset{\Delta t \rightarrow0}{lim}\frac{-R_{WB}r_B^{\land}[w_B\Delta t]}{\Delta t}\\ &=-R_{WB}r_B^{\land}w_B\\ &=R_{WB}w_B^{\land}r_B\\ &=(R_{WB}w_B)^{\land}R_{WB}r_B\\ &=w^{\land}_Wr_W \end{align*}\tag{3} $$

求物体在$W$坐标系下的加速度

对时间求导：

$$ \begin{align*} \ddot r_W =\dot v_W = a_W&=\frac{d(R_{WB}v_B+w_W^{\land}\dot r_W)}{dt}\\ &=R_{WB}\dot v_B+\dot R_{WB} v_B+w_W^{\land}\dot r_W+\dot w_W^{\land}r_W\\ &=a+w^{\land}v+(w^{\land}v+w^{\land}(w^{\land} r_W))+\dot w^{\land}r_W\\ &=a+2w^{\land}v+w^{\land}(w^{\land} r_W)+\dot w^{\land}r_W\\ \end{align*}\tag{4} $$

上式的每一项的推导见下面。这里可以明显看出$a\ne a_W$，两者之间相差三个项，分别是科氏力：$2w^{\land}v$，离心力：$w^{\land}(w^{\land} r_W)$、欧拉力：$\dot w^{\land}r_W$。

第一项：

$$ R_{WB}\dot v_B=R_{WB}a_B=a $$

第二项：

参考公式$(3)$，很容易就可以推出来：

$$ \begin{align*} \dot R_{WB} v_B&=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}\exp([w_B\Delta t]^{\land})v_B-R_{WB}v_B}{\Delta t}\\ &=(R_{WB}w_B)^{\land}R_{WB}r_B\\ &=w^{\land}_Wv \end{align*} $$

第三项：

$$ \begin{align*} w_W^{\land}\dot r_W&=w_W^{\land}(v+w_W^{\land}r_W)\\ &=w_W^{\land}v+w_W^{\land}(w_W^{\land}r_W) \end{align*} $$

整个过程的作用是什么？

我们在已知$B$坐标系下的物体的加速度、速度以及旋转轴的角速度，（这里的B就是我们今天的"IMU"），我们就可以求得对应世界坐标系下的相应的物理量，那么相应的求可以求得世界坐标系下的位姿（速度积分得到位姿）、四元数（根据角速度）。

大篇幅地介绍这个案例，主要是使读者明白，三维世界中的运动物体，在旋转的坐标系下（IMU坐标系）,如何将其状态量转换到世界坐标系下。以及转换之后各个状态量多出来了什么？

Ⅱ.IMU 工作原理

A.加速度计工作原理

测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示:

注意这里的加速度计测量值$a_m$为弹簧拉力对应的加速度，所以有：

$$ a_m=\frac{f}{m}=a-g\tag{5} $$

其中$f $为弹簧拉力，$a$ 为物体在惯性系下的加速度，$g$为重力加速度。

另外，实际的加速度计大都是$MEMS $加速度计，它利用电容或者电阻桥来等原理来测量$ a_m$

其原理可以参考：MEMS 加速度传感器的原理与构造介绍

通常情况下我们假设地球表面为惯性参考系，但是对于高端的测量单元来说，这种精度远远不够（需要考虑地球自转的影响），所以就将惯性参考系的原点记为地球的质心，而位于地球表面的坐标系称为地面参考系或东北天坐标系（ENU坐标系）, 如下图所示。在此坐标系下$g=(0,0,-9.81)^T$

假设 IMU 坐标系就是 ENU 坐标系，$R_{WB }= I$，静止时有$a=0,a_m=-g$，自由落体时有$a=g,a_m=0$

B.陀螺仪测量原理

陀螺仪侦测的是角速度。其工作原理基于科里奥利力的原理：当一个物体在坐标系中直线移动时，假设坐标系做一个旋转，那么在旋转的过程中，物体会感受到一个垂直的力和垂直方向的加速度。

按测量原理分有振动陀螺，光纤陀螺等。

陀螺仪.gif

低端 MEMS 陀螺上一般采用振动陀螺原理，通过测量科氏力（Coriolis force ）来间接得到角速度。MEMS 陀螺仪：一个主动运动轴 + 一个敏感轴，比如，如图示，高速运动的物体速度为$v$，在旋转坐标系下，物体会受到科氏力的影响，我们通过求科氏力就可以求得角速度$w$的大小。

但是实际制作的过程中我们不只是使用一个质量块，而是使用两个质量块，就是音叉陀螺仪，如图所示，叉子的中间为旋转轴，叉子左右两个质量块，做方反的正弦运动，质量块受到的科氏力方向相反。

当两个完全相同的质量块的运动方向相反，但是旋转相同的时候，它们就会受到相反的科氏力以及相同的外部加速度影响力。将两个质量块所受到该方向上的力做差，就会得到两倍的科氏力。

但是为啥要用这么做呢? 一个质量块不行么？

①利用二倍的科氏力对应的电压指数更大，测量也变得更准确；②因为有可能在科氏力方向上物体本身具有一定的加速度，所以利用音叉陀螺仪构成差分模型，可以将两个外部力相互抵消（科氏力方向物体本身的加速度），消除自身的影响，是的获得的科氏力更准。

但是，实际上，两个质量块不可能完全一致，也就是说陀螺仪的测量可能会受到外部加速度的影响，即常称的 G-sensitivity，一般的IMU手册里面都会有这个指数，它的含义就是告诉你IMU受加速度影响的系数有多大。

Ⅲ.IMU 中的误差及其标定

误差的分类：加速度计和陀螺仪的误差可以分为：确定性误差以及随机的误差，确定性的误差一般是事先通过标定确定，但是随机误差通常情况下假设噪声服从的是高斯分布。

A.确定性误差与标定

1.确定性误差

Bias

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

$$ v_{err}=b_at,\ \ \ p_{err}=\frac{1}{2}b_at^2 $$

这里的$p_{err}$指的是位移，因为有时间$t$的存在，为了区分，将位移定义为$p$.

Scale(刻度系数误差)

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

Nonorthogonality/Misalignment Errors(安装误差)

特别的在多轴的IMU传感器中，由于制作工艺的问题，有可能对导致$xyz$轴并不是严格意义的正交，如下图所示，$z$轴并不是严格意义上与$xOy$平面垂直，则会影响对应的$x,y$上的分量。

加上之前的scale的影响：

$$ \begin{align*} scale+Misalignment:&\\ &\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} \end{align*} $$

2.确定性误差的标定

六面法标定加速度计和陀螺仪

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

因为静止状态下，向上和向下所得到IMU的示数相加正好抵消掉了重力的影响，所以两次测量相加得到两倍的bias；

向上和向下所得到IMU的示数相减可以抵消bias;

$$ b=\frac{l^{up}_f+l^{down}_f}{2}\\ S=\frac{l^{up}_f-l^{down}_f}{2g}\tag{6} $$

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

考虑轴间误差的时候，实际加速度和测量值之间的关系为：

当考虑轴间的误差的时候，就变成$L = S · a + b$，其中$S$为$3\times3$的矩阵，

$$ \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}\tag{7} $$

水平放置$6$面，就会得到$6$个$a$向量，加速度的理论值为

$$ a_1=\begin{bmatrix} g\\0\\0 \end{bmatrix}, a_2=\begin{bmatrix} -g\\0\\0 \end{bmatrix}, a_3=\begin{bmatrix} 0\\g\\0 \end{bmatrix}, a_4=\begin{bmatrix} 0\\-g\\0 \end{bmatrix}, a_5=\begin{bmatrix} 0\\0\\g \end{bmatrix}, a_6=\begin{bmatrix} 0\\0\\-g \end{bmatrix} $$

对应的$L$为$L=\begin{bmatrix}l_1&l_2&l_3&l_4&l_5&l_6\end{bmatrix}$

在得知六组数据$(a_1,l_1)...(a_6.l_6)$，我们就可以利用最小二乘就可以得出$S$和$b$的具体值。线性最小二乘可以参考：线性最小二乘

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

温度相关的参数标定

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

两种标定方法：

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

B.随机误差与标定

1.随机误差

高斯白噪声

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

$$ \begin{align*} &E[n(t)]\equiv0\\ &E[n(t_1)n(t_2)]=\sigma^2\delta(t_1-t_2) \end{align*}\tag{8} $$

其中$\delta()$表示狄拉克函数。

实际上，IMU 传感器获取的数据为离散采样，离散和连续高斯白噪声的方差之间存在如下转换关系：

定义$n_d[k]$

$$ \begin{align*} n_d[k]\triangleq n(t_0+t_1)≃\frac{1}{\Delta t}\int^{t_0+\Delta t}_{t_0}{n(\tau)}dt \end{align*} $$

对其求方差$Var(X)=E[(X-\mu)^2]$,已知均值为$0$，$X=n_d[k]$：

$$ \begin{align*} E(n_d[k]^2)&=E(\frac{1}{\Delta t^2}\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{n(\tau)}{n(t)}d\tau dt)\\ &=E(\frac{\sigma^2}{\Delta t^2}\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{\delta(t-\tau)}d\tau dt)\\ &=E(\frac{\sigma^2}{\Delta t}) \end{align*}\tag{9} $$

对应相等，可得：

$$ n_d[k]=\sigma_dw[k]\tag{10} $$

其中，

$$ w[k]\sim N(0,1)\\ \sigma_d=\sigma\frac{1}{\sqrt{\Delta t} }\tag{11} $$

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

Bias 随机游走

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

$$ \dot b(t)=n(t)=\sigma_bw(t)\tag{12} $$

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

同样，离散和连续之间的转换：

定义$b_d[k]$

$$ b_d[k]\triangleq b(t_0)+\int^{t_0+\Delta t}_{t_0}{n(t)}dt $$

求方差：

$$ \begin{align*} E((b_d[k]-b_d[k-1])^2)&=E(\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{n(t)}{n(\tau)}d\tau dt)\\ &=E(\sigma_b^2\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}\delta (t-\tau)d\tau dt)\\ &=E(\sigma_b^2\Delta t) \end{align*}\tag{13} $$

所以：

$$ b_d[k]=b_d[k-1]+\sigma_{bd}w[k]\tag{14} $$

其中：

$$ w[k]\sim N(0,1)\\ \sigma_{bd}=\sigma_b \sqrt{\Delta t}\tag{15} $$

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

2.随机误差的标定

艾伦方差标定

在统计学中描述随机变量的两个经典参数是均值和方差，早期在定量表征原子钟的频率稳定度时采用的就是经典方差方法。1996 年，学者 D.W.Allan 在分析铯原子钟频标的频率稳定度时发现经典方差随着时间的增长而发散，为了解决该问题，提出了一种新的评定方法，后来称为艾伦方差。由于惯性器件也具有振荡器的特征，Allan 方差分析也被广泛应用于惯性器件的随机误差建模，IEEE 标准中就将 Allan 方差方法引入到了激光陀螺的建模分析。

具体的流程如下：

①保持传感器绝对静止获取数据。

②对数据进行分段，设定时间段的时长，如下图所示。

③将传感器数据按照时间段进行平均。

④计算方差，绘制艾伦曲线。

得到的艾伦曲线如下图所示：

从艾伦方差曲线中可以辨识出 IMU 的五种噪声，分别为：量化噪声、角度随机游走、零偏不稳定性噪声，角速率随机游走，速率斜坡，一般在 IMU 噪声辨识中用的比较多的是中间 3 种。

🧐 本文作者：LukeyAlvin
😉 本文链接：https://lukeyalvin.site/archives/68.html
😊 版权说明：本博客所有文章除特别声明外，均默认采用 CC BY-NC-SA 4.0 许可协议。

最后修改：2022 年 06 月 10 日

赏杯咖啡

发表评论取消回复

评论 *

私密评论

名称 *

🎲

邮箱 *

地址

momo
好可爱的个站
xhc
我可能没有说清楚问题，就是约等于符号那个式子，是上一个式子省...
xhc
你好同学，请问一下，关于R_bc 的偏导求解中（文中公式 J...
LynxCatTheThird
麻烦更新一下信息，谢谢名称: LynxCatTheThird ...
gfd
挺好，谢谢分享kfls

ch5_使用OpenCV操作图像
浏览次数: 157
IMU工作原理、误差及其标定
浏览次数: 430
C++数字转字符串 to_string()
浏览次数: 16
滑动窗口算法
浏览次数: 184
蚁群算法概述
浏览次数: 147

IMU工作原理、误差及其标定

LukeyAlvin • 2022 年 04 月 25 日

本节主要对IMU的工作原理做详细的介绍，分析IMU中的确定性误差以及随机误差，并给出误差标定的方法，如确定性误差使用六面法标定，随机误差使用艾伦方差标定等。<h1>Ⅰ.旋转运动学</h1><h2>A.线速度与角速度</h2>如图所示，粒子在坐标系中的$z=h$的平面内做圆周运动，坐标为$r=(a\cos\theta,a\sin\theta,h)$，对坐标求导得：$$
\begin{align*}
\dot r&amp;=(-a\dot\theta\sin\theta,a\dot\theta\cos\theta,0)^T\\
&amp;=\begin{bmatrix}
0&amp;-\dot\theta&amp;0\\\dot\theta&amp;0&amp;0\\0&amp;0&amp;0
\end{bmatrix}
\begin{bmatrix}a\cos\theta\\a\sin\theta\\h\end{bmatrix}\\
&amp;=w^{\land} r
\end{align*}\tag{1}
$$其中$w$是角速度，它是一个矢量，即$w=\begin{bmatrix}0\\0\\ \dot\theta\end{bmatrix}=\dot\theta z$,其中$|\dot\theta|$是角速度的大小。对公式$(1)$取模：$$
|\dot r|=|w||r|sin\phi=a|\dot\theta|
$$<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422093936869.png" alt="image-20220422093936869" style="zoom:50%;" style=""><h2>B.旋转坐标系下的运动学</h2>如图，考虑两个坐标系，一个是世界坐标系(惯性坐标系)$W$，它本身是静止不动的；一个是机器人本体坐标系$B$，它本身在做角速度为$w_B$的纯旋转，设质量块在坐标系$B$下做速度为$v_B$，加速度为$a_B$的匀加速直线运动，它在$B$坐标系下坐标为$r_B=(x_1,x_2,x_3)^T$，设从坐标系世界坐标系$W$到是机器人本体坐标系$B$只考虑旋转的时候，旋转矩阵$R_{WB}$。<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422155242434.png" alt="image-20220422155242434" title="image-20220422155242434"style="">这里定义一些变量：<blockquote>①设物体在坐标系$B$下做速度为$v_B$，加速度为$a_B$，对应在坐标系$W$下的速度为$v_W$，加速度为$a_W$并且设坐标系$B$下的速度和加速度在坐标系$W$下的表示分别为：$v=R_{WB}v_B$，$a=R_{WB}a_B$，一定注意这里的$v\ne v_W,a\ne a_W$②设坐标轴$B$旋转的角速度为$w_B$，对应在世界坐标系中其旋转的角速度变为$w_W$，且$w_W=R_{WB}w_B$③设物体在坐标系$B$下的坐标为$r_B$，对应在坐标系$W$下的坐标为$r_W$，且$r_W=R_{WB}r_B$</blockquote>则对应到世界坐标系下：$$
r_W(t)=x_1(t)i+x_2(t)j+x_3(t)k=R_{WB}r_B
$$一般简写为：$r_B=x_ie_i$经过一番定义之后，我们考虑几个问题:<ul><li>求物体在$W$坐标系下的速度</li></ul>对时间求导：$$
\begin{align*}
\dot r_W=v_W&amp;=R_{WB}\dot r_B+\dot R_{WB} r_B\\
&amp;=R_{WB}v_B+w_W^{\land} r_W \ \ \ \ 参见公式(3) \\ 
&amp;=v+w_W^{\land} r_W\Leftrightarrow v=v_W-w_W^{\land} r_W\\
\end{align*}\tag{2}
$$其中$w_W=R_{WB}w_b$表示body坐标系$B$的角速度在世界坐标系$W$下的表示。具体推导参见公式$(3)$.这里可以明显看出$v\ne v_W$，两者之间相差一个$w_W^{\land} r_W$项。<hr>推导$\dot R_{WB} r_B$，使用右乘扰动模型:$$
\begin{align*}
\dot R_{WB} r_B&amp;=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}\exp([w_B\Delta t]^{\land})r_B-R_{WB}r_B}{\Delta t}\\
&amp;\approx\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}(I+[w_B\Delta t]^{\land})r_B-R_{WB}r_B}{\Delta t}\\
&amp;=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}[w_B\Delta t]^{\land}r_B}{\Delta t}\\
&amp;=\underset{\Delta t \rightarrow0}{lim}\frac{-R_{WB}r_B^{\land}[w_B\Delta t]}{\Delta t}\\
&amp;=-R_{WB}r_B^{\land}w_B\\ 
&amp;=R_{WB}w_B^{\land}r_B\\
&amp;=(R_{WB}w_B)^{\land}R_{WB}r_B\\
&amp;=w^{\land}_Wr_W
\end{align*}\tag{3}
$$<hr><ul><li>求物体在$W$坐标系下的加速度</li></ul>对时间求导：$$
\begin{align*}
\ddot r_W =\dot v_W = a_W&amp;=\frac{d(R_{WB}v_B+w_W^{\land}\dot r_W)}{dt}\\
&amp;=R_{WB}\dot v_B+\dot R_{WB} v_B+w_W^{\land}\dot r_W+\dot w_W^{\land}r_W\\
&amp;=a+w^{\land}v+(w^{\land}v+w^{\land}(w^{\land} r_W))+\dot w^{\land}r_W\\
&amp;=a+2w^{\land}v+w^{\land}(w^{\land} r_W)+\dot w^{\land}r_W\\
\end{align*}\tag{4}
$$上式的每一项的推导见下面。这里可以明显看出$a\ne a_W$，两者之间相差三个项，分别是科氏力：$2w^{\land}v$，离心力：$w^{\land}(w^{\land} r_W)$、欧拉力：$\dot w^{\land}r_W$。<hr><ul><li>第一项：</li></ul>$$
R_{WB}\dot v_B=R_{WB}a_B=a
$$<ul><li>第二项：</li></ul>参考公式$(3)$，很容易就可以推出来：$$
\begin{align*}
\dot R_{WB} v_B&amp;=\underset{\Delta t \rightarrow0}{lim}\frac{R_{WB}\exp([w_B\Delta t]^{\land})v_B-R_{WB}v_B}{\Delta t}\\

&amp;=(R_{WB}w_B)^{\land}R_{WB}r_B\\
&amp;=w^{\land}_Wv
\end{align*}
$$<ul><li>第三项：</li></ul>$$
\begin{align*}
w_W^{\land}\dot r_W&amp;=w_W^{\land}(v+w_W^{\land}r_W)\\
&amp;=w_W^{\land}v+w_W^{\land}(w_W^{\land}r_W)
\end{align*}
$$<hr>整个过程的作用是什么？我们在已知$B$坐标系下的物体的加速度、速度以及旋转轴的角速度，（这里的B就是我们今天的"IMU"），我们就可以求得对应世界坐标系下的相应的物理量，那么相应的求可以求得世界坐标系下的位姿（速度积分得到位姿）、四元数（根据角速度）。大篇幅地介绍这个案例，主要是使读者明白，三维世界中的运动物体，在旋转的坐标系下（IMU坐标系）,如何将其状态量转换到世界坐标系下。以及转换之后各个状态量多出来了什么？<h1>Ⅱ.IMU 工作原理</h1><h2>A.加速度计工作原理</h2>测量原理可以用一个简单的质量块 + 弹簧 + 指示计来表示:<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422191601477.png" alt="image-20220422191601477.png" style="zoom:33%;" style="">注意这里的加速度计测量值$a_m$为弹簧拉力对应的加速度，所以有：$$
a_m=\frac{f}{m}=a-g\tag{5}
$$其中$f $为弹簧拉力，$a$ 为物体在惯性系下的加速度，$g$为重力加速度。另外，实际的加速度计大都是$MEMS $加速度计，它利用电容或者电阻桥来等原理来测量$ a_m$其原理可以参考：<a class="no-external-link" href="https://m.elecfans.com/article/596137.html" target="_blank">MEMS 加速度传感器的原理与构造介绍</a>通常情况下我们假设地球表面为惯性参考系，但是对于高端的测量单元来说，这种精度远远不够（需要考虑地球自转的影响），所以就将惯性参考系的原点记为地球的质心，而位于地球表面的坐标系称为地面参考系或东北天坐标系（ENU坐标系）, 如下图所示。在此坐标系下$g=(0,0,-9.81)^T$<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422192532908.png" alt="image-20220422192532908.png" style="zoom:33%;" style="">假设 IMU 坐标系就是 ENU 坐标系，$R_{WB }= I$，静止时有$a=0,a_m=-g$，自由落体时有$a=g,a_m=0$<h2>B.陀螺仪测量原理</h2>陀螺仪侦测的是角速度。其工作原理基于科里奥利力的原理：当一个物体在坐标系中直线移动时，假设坐标系做一个旋转，那么在旋转的过程中，物体会感受到一个垂直的力和垂直方向的加速度。按测量原理分有振动陀螺，光纤陀螺等。<img src="/images/Blog/2-IMU运动学模型以及误差模型/陀螺仪.gif" alt="陀螺仪.gif" title="陀螺仪.gif"style="">低端 MEMS 陀螺上一般采用振动陀螺原理，通过测量 科氏力（Coriolis force ）来间接得到角速度。MEMS 陀螺仪：一个主动运动轴 + 一个敏感轴，比如，如图示，高速运动的物体速度为$v$，在旋转坐标系下，物体会受到科氏力的影响，我们通过求科氏力就可以求得角速度$w$的大小。<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422195814350.png" alt="image-20220422195814350.png" style="zoom:33%;" style="">但是实际制作的过程中我们不只是使用一个质量块，而是使用两个质量块，就是音叉陀螺仪，如图所示，叉子的中间为旋转轴，叉子左右两个质量块，做方反的正弦运动，质量块受到的科氏力方向相反。<img src="/images/Blog/2-IMU运动学模型以及误差模型/20210905220130.png" alt="20210905220130.png" style="zoom: 33%;" style="">当两个完全相同的质量块的运动方向相反，但是旋转相同的时候，它们就会受到相反的科氏力以及相同的外部加速度影响力。将两个质量块所受到该方向上的力做差，就会得到两倍的科氏力。但是为啥要用这么做呢? 一个质量块不行么？①利用二倍的科氏力对应的电压指数更大，测量也变得更准确；②因为有可能在科氏力方向上物体本身具有一定的加速度，所以利用音叉陀螺仪构成差分模型，可以将两个外部力相互抵消（科氏力方向物体本身的加速度），消除自身的影响，是的获得的科氏力更准。但是，实际上，两个质量块不可能完全一致，也就是说陀螺仪的测量可能会受到外部加速度的影响，即常称的 G-sensitivity，一般的IMU手册里面都会有这个指数，它的含义就是告诉你IMU受加速度影响的系数有多大。<h1>Ⅲ.IMU 中的误差及其标定</h1>误差的分类：加速度计和陀螺仪的误差可以分为：确定性误差以及随机的误差，确定性的误差一般是事先通过标定确定，但是随机误差通常情况下假设噪声服从的是高斯分布。<h2>A.确定性误差与标定</h2><h3>1.确定性误差</h3><ul><li>Bias</li></ul>理论上，当没有外部作用时，IMU 传感器的输出应该为$0$。但是，实际数据存在一个偏置$ b$。加速度计 bias 对位姿估计的影响：$$
v_{err}=b_at,\ \ \ p_{err}=\frac{1}{2}b_at^2
$$这里的$p_{err}$指的是位移，因为有时间$t$的存在，为了区分，将位移定义为$p$.<ul><li>Scale(刻度系数误差)</li></ul>scale 可以看成是实际数值和传感器输出值之间的比值。<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422204953295.png" alt="image-20220422204953295.png" style="zoom:33%;" style=""><ul><li>Nonorthogonality/Misalignment Errors(安装误差)</li></ul>特别的在多轴的IMU传感器中，由于制作工艺的问题，有可能对导致$xyz$轴并不是严格意义的正交，如下图所示，$z$轴并不是严格意义上与$xOy$平面垂直，则会影响对应的$x,y$上的分量。<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220422204127003.png" alt="image-20220422204127003.png" style="zoom:33%;" style="">加上之前的scale的影响：$$
\begin{align*}
scale+Misalignment:&amp;\\
&amp;\begin{bmatrix}
l_{ax}\\l_{ay}\\l_{az}
\end{bmatrix}=
\begin{bmatrix}
s_{xx}&amp;m_{xy}&amp;m_{xz}\\
m_{yx}&amp;s_{yy}&amp;m_{yz}\\
m_{zx}&amp;m_{zy}&amp;s_{zz}
\end{bmatrix}
\begin{bmatrix}
a_{x}\\a_{y}\\a_{z}
\end{bmatrix}
\end{align*}
$$<h3>2.确定性误差的标定</h3><h4>六面法标定加速度计和陀螺仪</h4>六面法是指将加速度计的 3 个轴分别朝上或者朝下水平放置一段时间，采集 6 个面的数据完成标定。 如果各个轴都是正交的，那很容易得到 bias 和 scale：因为静止状态下，向上和向下所得到IMU的示数相加正好抵消掉了重力的影响，所以两次测量相加得到两倍的bias；向上和向下所得到IMU的示数相减可以抵消bias;$$
b=\frac{l^{up}_f+l^{down}_f}{2}\\
S=\frac{l^{up}_f-l^{down}_f}{2g}\tag{6}
$$其中，$l $为加速度计某个轴的测量值，$g$ 为当地的重力加速度考虑轴间误差的时候，实际加速度和测量值之间的关系为：当考虑轴间的误差的时候，就变成$L = S · a + b$，其中$S$为$3\times3$的矩阵，$$
\begin{bmatrix}
l_{ax}\\l_{ay}\\l_{az}
\end{bmatrix}=
\begin{bmatrix}
s_{xx}&amp;m_{xy}&amp;m_{xz}\\
m_{yx}&amp;s_{yy}&amp;m_{yz}\\
m_{zx}&amp;m_{zy}&amp;s_{zz}
\end{bmatrix}
\begin{bmatrix}
a_{x}\\a_{y}\\a_{z}
\end{bmatrix}+
\begin{bmatrix}
b_{ax}\\b_{ay}\\b_{az}
\end{bmatrix}\tag{7}
$$水平放置$6$面，就会得到$6$个$a$向量，加速度的理论值为$$
a_1=\begin{bmatrix}
g\\0\\0
\end{bmatrix},
a_2=\begin{bmatrix}
-g\\0\\0
\end{bmatrix},
a_3=\begin{bmatrix}
0\\g\\0
\end{bmatrix},
a_4=\begin{bmatrix}
0\\-g\\0
\end{bmatrix},
a_5=\begin{bmatrix}
0\\0\\g
\end{bmatrix},
a_6=\begin{bmatrix}
0\\0\\-g
\end{bmatrix}
$$对应的$L$为$L=\begin{bmatrix}l_1&amp;l_2&amp;l_3&amp;l_4&amp;l_5&amp;l_6\end{bmatrix}$在得知六组数据$(a_1,l_1)...(a_6.l_6)$，我们就可以利用最小二乘就可以得出$S$和$b$的具体值。线性最小二乘可以参考：<a class="no-external-link" href="https://lukeyalvin.top/2022/04/04/%E4%BB%80%E4%B9%88%E6%98%AF%E6%9C%80%E5%B0%8F%E4%BA%8C%E4%B9%98%E6%B3%95/" target="_blank">线性最小二乘</a>同理陀螺仪的六面法标定与其类似，和加速度计六面法不同的是，陀螺仪的真实值由高精度转台提供，这里的$6 $面是指各个轴顺时针和逆时针旋转。<h4>温度相关的参数标定</h4>目的：这个标定的主要目的是对传感器估计的 bias 和 scale 进行温度补偿，获取不同温度时 bias 和 scale 的值，绘制成曲线。两种标定方法：<ul><li>soak method：控制恒温室的温度值，然后读取传感器数值进行标定。</li><li>ramp method：记录一段时间内线性升温和降温时传感器的数据来进行标定。</li></ul><h2>B.随机误差与标定</h2><h3>1.随机误差</h3><h4>高斯白噪声</h4>IMU 数据连续时间上受到一个均值为$ 0$，方差为$\sigma$，各时刻之间相互独立的高斯过程$ n(t)$：$$
\begin{align*}
&amp;E[n(t)]\equiv0\\
&amp;E[n(t_1)n(t_2)]=\sigma^2\delta(t_1-t_2)
\end{align*}\tag{8}
$$其中$\delta()$表示狄拉克函数。实际上，IMU 传感器获取的数据为离散采样，离散和连续高斯白噪声的方差之间存在如下转换关系：定义$n_d[k]$$$
\begin{align*}
n_d[k]\triangleq n(t_0+t_1)≃\frac{1}{\Delta t}\int^{t_0+\Delta t}_{t_0}{n(\tau)}dt
\end{align*}
$$对其求方差$Var(X)=E[(X-\mu)^2]$,已知均值为$0$，$X=n_d[k]$：$$
\begin{align*}
E(n_d[k]^2)&amp;=E(\frac{1}{\Delta t^2}\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{n(\tau)}{n(t)}d\tau dt)\\
&amp;=E(\frac{\sigma^2}{\Delta t^2}\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{\delta(t-\tau)}d\tau dt)\\
&amp;=E(\frac{\sigma^2}{\Delta t})
\end{align*}\tag{9}
$$对应相等，可得：$$
n_d[k]=\sigma_dw[k]\tag{10}
$$其中，$$
w[k]\sim N(0,1)\\
\sigma_d=\sigma\frac{1}{\sqrt{\Delta t} }\tag{11}
$$也就是说高斯白噪声的连续时间到离散时间之间差一个$\frac{1}{\sqrt{\Delta t} }$ ，$\sqrt{\Delta t} $是传感器的采样时间。<h4>Bias 随机游走</h4>通常用维纳过程 (wiener process) 来建模 bias 随时间连续变化的过程，离散时间下称之为随机游走。$$
\dot b(t)=n(t)=\sigma_bw(t)\tag{12}
$$其中$ w $是方差为$ 1 $的白噪声。同样，离散和连续之间的转换：定义$b_d[k]$$$
b_d[k]\triangleq b(t_0)+\int^{t_0+\Delta t}_{t_0}{n(t)}dt
$$求方差：$$
\begin{align*}
E((b_d[k]-b_d[k-1])^2)&amp;=E(\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}{n(t)}{n(\tau)}d\tau dt)\\
&amp;=E(\sigma_b^2\int^{t_0+\Delta t}_{t_0}\int^{t_0+\Delta t}_{t_0}\delta (t-\tau)d\tau dt)\\
&amp;=E(\sigma_b^2\Delta t)
\end{align*}\tag{13}
$$所以：$$
b_d[k]=b_d[k-1]+\sigma_{bd}w[k]\tag{14}
$$其中：$$
w[k]\sim N(0,1)\\
\sigma_{bd}=\sigma_b \sqrt{\Delta t}\tag{15}
$$bias 随机游走的噪声方差从连续时间到离散之间需要乘以$\sqrt{\Delta t}$<h3>2.随机误差的标定</h3><h4>艾伦方差标定</h4><blockquote>在统计学中描述随机变量的两个经典参数是均值和方差，早期在定量表征原子钟的频率稳定度时采用的就是经典方差方法。1996 年，学者 D.W.Allan 在分析铯原子钟频标的频率稳定度时发现经典方差随着时间的增长而发散，为了解决该问题，提出了一种新的评定方法，后来称为艾伦方差。由于惯性器件也具有振荡器的特征，Allan 方差分析也被广泛应用于惯性器件的随机误差建模，IEEE 标准中就将 Allan 方差方法引入到了激光陀螺的建模分析。</blockquote>具体的流程如下：①保持传感器绝对静止获取数据。②对数据进行分段，设定时间段的时长，如下图所示。<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220423162306358.png" alt="image-20220423162306358" style="zoom:50%;" style="">③将传感器数据按照时间段进行平均。④计算方差，绘制艾伦曲线。得到的艾伦曲线如下图所示：<img src="/images/Blog/2-IMU运动学模型以及误差模型/image-20220423162352964.png" alt="image-20220423162352964" style="zoom: 33%;" style="">从艾伦方差曲线中可以辨识出 IMU 的五种噪声，分别为：量化噪声、角度随机游走、零偏不稳定性噪声，角速率随机游走，速率斜坡，一般在 IMU 噪声辨识中用的比较多的是中间 3 种。

IMU工作原理、误差及其标定

Ⅰ.旋转运动学

A.线速度与角速度

B.旋转坐标系下的运动学

Ⅱ.IMU 工作原理

A.加速度计工作原理

B.陀螺仪测量原理

Ⅲ.IMU 中的误差及其标定