IMU误差模型和校准

参考文献

1 Tedaldi D, Pretto A, Menegatti E. A robust and easy to implement method for IMU calibration without external equipments[J]. 2014:3042-3049.
2 https://github.com/ethz-asl/kalibr
3 Allan Variance: Noise Analysis for Gyroscopes
4 厉宽宽, 陈允芳, 程敏,等. MEMS-IMU随机误差的Allan方差分析[J]. 全球定位系统, 2016,41(6):102-106.
5 https://bitbucket.org/alberto_pretto/imu_tk

1 IMU的误差来源

  IMU的误差来主要来自于三部分，包括噪声(Bias and Noise)、尺度因子(Scale errors)和轴偏差(Axis misalignments)。加速度计和陀螺仪的测量模型可以用式(1.1)和式(1.2)表达。
\[a^{B}=T^{a}K^{a}(a^{S}+b^{a}+\nu ^{a}) \tag{1.1}\]

\[ \nonumber w^{B}=T^{g}K^{g}(w^{S}+b^{g}+\nu ^{g}) \tag{1.2} \]
  其中上标\(a\) 表示加速度计，\(g\) 表示陀螺仪，\(B\) 表示正交的参考坐标系，\(S\) 表示非正交的选准坐标系。\(T\) 表示轴偏差的变换矩阵，\(K\) 表示尺度因子，\(a^{S},w^{S}\) 表示真值，\(b,\nu\) 分别表示Bias和白噪声。
  下面就这三部分的误差的具体来源进行简要的说明。

1.1 IMU噪声模型(Bias and Noise)

  这里对IMU的噪声模型进行重新的定义，去除其误差模型中的轴偏差和尺度因子，可以将IMU的噪声模型写为，
\[ \nonumber \tilde{\omega }(t)=\omega （t）+b(t)+n(t) \tag{1.3} \]
其中 \(n(t)\) 表示高斯白噪声，\(b(t)\) 表示随机游走噪声。下面就这两种噪声的具体形式和来源展开说明。

1. 高斯白噪声
     服从高斯分布的一种白噪声，其一阶矩为常数，二阶距随时间会发生变化。一般来说这种噪声是由AD转换器引起的一种外部噪声。因为连续的高斯白噪声服从如下条件(参考An Introduction to Stochastic PDEs中Example 3.56 (White noise) )，
   \[ \nonumber E(n(t))\equiv 0 \\ \nonumber E(n(t_{1})n(t_{2}))=\sigma _{g}^{2}\delta (t_{1}-t_{2}) \tag{1.4} \]
   其中 \(\delta(t)\) 表示狄拉克函数。\(\sigma _{g}\) 位高斯白噪声的方差，值越大，表示噪声程度越大。将其进一步离散化后，得到，
   \[ \nonumber n_{d}[k]=\sigma _{gd}\omega [k] \tag{1.5} \]
   其中 \(\omega [k]\sim N(0,1)\), \(\sigma _{gd}=\sigma _{g}\frac{1}{\sqrt{\triangle t}}\)。
   证明如下，
   \[ \nonumber n_{d}[k]=n(t_{0}+\Delta t)\simeq\frac{1}{\Delta t}\int_{t_{0}}^{t_{0}+\Delta t}n(\tau)d\tau \]
   \[ \begin{align} \nonumber E(n_{d}[k]^{2})&=E(\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) \\ \nonumber &=\frac{\sigma_{g}^{2}}{\Delta t^{2}}\int_{t_{0}}^{t_{0}+\Delta t}\int_{t_{0}}^{t_{0}+\Delta t}\delta (t-\tau)d\tau dt \\ \nonumber &=\frac{\sigma_{g}^{2}}{\Delta t} \end{align} \]
   则有 \(\sigma _{gd}^{2}=\frac{\sigma_{g}^{2}}{\Delta t}\)，即 \(\sigma _{gd}=\frac{\sigma_{g}}{\sqrt{\Delta t}}\)

2. 随机游走(这里指零偏Bias)
     随机游走是一个离散模型，可以把它看做是一种布朗运动，或者将其称之为维纳过程。该模型可以看做是高斯白噪声的积分。该噪声参数一般是由传感器的内部构造、温度等变化量综合影响下的结果。
   \[ \nonumber \dot{b_{g}(t)}=\sigma _{bg}\omega (t) \tag{1.6} \]
   其中 \(\omega (t)\) 是单位的高斯白噪声，在上面提到了。如果把随机游走噪声离散化，可以写作：
   \[ \nonumber b_{d}[k]=b_{d}[k-1]+\sigma _{bgd}\omega [k] \tag{1.7} \]
   其中 \(\omega [k]\sim (0,1)\)， \(\sigma _{bgd}=\sigma _{bg}\sqrt{\bigtriangleup t}\)
   证明如下：
   \[ \nonumber b_{d}[k] \simeq b(t_{0})+\int^{t_{0}+\Delta t}_{t_{0}}n(t)dt \]
   \[ \begin{align} \nonumber 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)dt\tau) \\ \nonumber &=\sigma_{bg}^{2}\int^{t_{0}+\Delta t}_{t_{0}}\int^{t_{0}+\Delta t}_{t_{0}}\delta(t-\tau)dtd\tau \\ \nonumber &=\sigma_{bg}^{2} \Delta t \end{align} \]
   则有 \(\sigma_{bgd}^{2}=\sigma_{bg}^{2}\Delta t\)，即\(\sigma_{bgd}=\sigma_{bg}\sqrt{\Delta t}\)
   由随机游走的分布可以看出，随机游走都是在上一次的噪声的基础之上叠加了一个高斯噪声，所以下一步永远都是随机的。
     误差模型部分的内容和推导参考Kalibr wiki和郑凡博客。

   1.2 IMU尺度因子

     其实在这个地方上说IMU的尺度因子不太恰当，应该将其称之为__尺度误差。__这部分的误差来自于传感器的数字信号向物理量转换的误差，比如说AD向加速度/角速度转换的时候的误差。尺度误差的表达比较简单，
   \[ \begin{align} \nonumber K^{a}&= \begin{bmatrix} s^{a}_{x} & 0&0 \\\nonumber 0 &s^{a}_{x} &0 \\\nonumber 0& 0&s^{a}_{x} \end{bmatrix} \\\nonumber K^{g}&= \begin{bmatrix} s^{g}_{x} & 0&0 \\\nonumber 0 &s^{g}_{x} &0 \\\nonumber 0& 0&s^{g}_{x} \end{bmatrix} \end{align} \tag{1.8} \]

1.3 IMU轴向误差

  一般情况下，加速度计的坐标系AF和陀螺仪的坐标系GF都不是正交的坐标系，但我们正常使用的时候都是默认测量量是在正交坐标系下的，所以就需要一个变换矩阵将测量量从非正交坐标系 AF\GF下转到正交坐标系BF下，将其称之为机体坐标系。如图1所示，虚线为定义的参考正交坐标系，实线为实际的旋转坐标系(当然了，角度偏差不会这么大☺)。

图1 轴偏差示意图
  轴向误差一般还伴随着跨轴误差，这两部分一般不加以区分，就放在一起了。所以这部分的误差就是一个变换矩阵，求取实际的旋转坐标系(AF/GF)到参考坐标系(BF)的转换。为了计算变换矩阵，将三个轴偏角进行进一步的分解，如图2所示。将每个轴向的偏差角沿着另外两个轴分解即可得到。

图2 轴偏角分解示意图
进一步得到变换矩阵
\[ \nonumber s^{B}=Ts^{A}, \qquad T=\begin{bmatrix} 1 & -\beta_{yz} & \beta_{zy} \\ \beta_{xz} & 1 & -\beta{zx} \\ \beta_{xy} & \beta_{yx} & 1 \end{bmatrix} \tag{1.9} \]

2 IMU校准

  IMU在校准过程中，加速度计和陀螺仪是分开校准的，一般是先校准加速度计，然后利用准确的加速度计信息再来校准陀螺仪。整个校准过程的流程如下图所示，

st=>start: 静止放置IMU T秒
op1=>operation: 旋转IMU使其保持不同的姿态
op2=>operation: 在某种姿态下，至少保持静态t秒
cond=>condition: 是否旋转IMU N次
e=>end: 用下面的算法估计误差参数st->op1->op2->cond
cond(yes)->e
cond(no)->op1

图3 IMU校准流程图
其中，初始化时间 \(T\)一般取50s，旋转后保持静态时间\(t\) 取\(1\sim4\)s，旋转次数\(N\) 取\(36\sim50\) 次。一般来说，这个旋转次数越多越好，至少旋转次数要大于要求解的参数的个数，这样才能避免奇异性。

2.1 静态检测

  因为在加速度和陀螺仪的校准过程中，常常需要传感器处于静止状态，所以在校准的过程中也就需要判断传感器是否处于静止状态。判断依据也很简单，
\[ \nonumber \varsigma(t_{w}) = \sqrt{[var_{t_{w}}(a^{t}_{x})]^{2}+var_{t_{w}}(a^{t}_{y})]^{2}+var_{t_{w}}(a^{t}_{z})]^{2}} \tag{2.1} \]
其中 \(var_{t_{w}}(a^{t})\) 表示加速度 \(a^{t}\)在时间段 \(t_{w}\)内的方差。
判断的时候，只需将\(\varsigma(t_{w})\) 和 阈值 \(\varsigma(T_{init})\)比较即可，小于则静态，反之动态。一般取 \(T_{init}\)为50s。

2.2 加速度计校准

  加速度计的校准过程一般都是将加速度计静止，然后根据测量值的二范数等于当地重力加速度这一规则校准的。一般会用上一小节提到的静态检测算法在采样序列上挑选出静态的测量片段，然后按照下述内容进行校准。

2.2.1 加速度的常规校准方法

  这里的加速度校准是指校准前面小节提到的所有误差，因为不知道怎么这个方法，所以就叫做“常规校准方法”了☺。因为下一小节要讲六面矫正法。常规方法的校准还是作为了一个优化问题来求解的。
  在加速度校准的时候,为了进一步将变换矩阵\(T\) 简单化，我们假设坐标系BF的x轴和坐标系AF的x轴重合，且BF的y轴处于AF的x轴和y轴的平面上。所以变换矩阵可以进一步写为，
\[ T^{a}=\begin{bmatrix} 1 & -\alpha_{yz} & \alpha_{zy} \\ 0 & 1 & -\alpha_{zx} \\ 0 & 0 & 1 \end{bmatrix} \tag{2.2} \]
那么待求解参数(优化参数)变为，
\[ \theta^{acc}=[\alpha_{yz},\alpha_{zy},\alpha_{zx},s^{a}_{x},s^{a}_{y},s^{a}_{z},b^{a}_{x},b^{a}_{y},b^{a}_{z}] \tag{2.3} \]
定义状态方程，
\[ a^{O}=h(a^{S},\theta^{acc})=T^{a}K^{a}(a^{S}+b^{a}) \tag{2.4} \]
因为在采集加速度计读数的时候取得时一个小窗口内的平均值，所以忽略了高斯白噪声。进而有了优化的代价函数。
\[ L(\theta ^{acc})=\sum^{M}_{k=1}(||g||^{2}-||h(a^{S}_{k},\theta^{acc})||^{2})^{2} \tag{2.5} \]
一般会有多组\(M\) 组较为明显的、稳定的旋转量来放入代价函数中求解待求解参数，然后选取残差最小的一组所对应的参数即可。

2.2.2 六面法校准加速度计

  建立加速度计的校准模型，
\[ \nonumber A_{t}=\begin{bmatrix} A_{x}\\ A_{y}\\ A_{Z} \end{bmatrix}=R_{[3*3]}\begin{bmatrix} Scale_x &0 &0 \\ 0& Scale_y &0 \\ 0& 0 & Scale_z \end{bmatrix}\begin{bmatrix} A_{mx}-offset_x\\ A_{my}-offset_y\\ A_{mz}-offset_z \end{bmatrix} \tag{2.6} \]
其中 \(A_{t}\) 表示最终的真实值，\(A_{m*}\) 表示测量值， \(offset_xyz\) 表示测量中的零偏， \(R_{[3*3]}\) 表示旋转， \(Scale_x\) 表示尺度缩放。将下式做变换得，
\[ \nonumber \begin{bmatrix} A_{x}\\ A_{y}\\ A_{Z} \end{bmatrix}=\begin{bmatrix} C_{1} & C_{2} & C_{3}\\ C_{4}& C_{5} & C_{6}\\ C_{7}&C_{8} & C_{9} \end{bmatrix}\begin{bmatrix} A_{mx}\\ A_{my}\\ A_{mz} \end{bmatrix}+\begin{bmatrix} C_{off_x}\\ C_{off_y}\\ C_{off_z} \end{bmatrix} \tag{2.7} \]
再将其变为齐次坐标形式
\[ \nonumber \begin{bmatrix} A_{x}\\ A_{y}\\ A_{z} \end{bmatrix}=\begin{bmatrix} C_{1} & C_{2} & C_{3} &C_{off_x} \\ C_{4} & C_{5} & C_{6} & C_{off_y}\\ C_{7}& C_{8} & C_{9} & C_{off_z} \end{bmatrix}\begin{bmatrix} A_{mx}\\ A_{my}\\ A_{mz}\\ 1 \end{bmatrix} \]
再将其转置
\[ \nonumber \begin{bmatrix} A_{mx}& A_{mx} & A_{mx} &1 \end{bmatrix}\begin{bmatrix} C_{1} & C_{4} & C_{7}\\ \nonumber C_{2} & C_{5}& C_{8}\\ C_{3}& C_{6}& C_{9}\\ \nonumber C_{off_x}& C_{off_y} & C_{off_z} \end{bmatrix}=\begin{bmatrix} A_{x} &A_{y} & A_{z} \end{bmatrix} \tag{2.8} \]
这时候就可以最小二乘求解上面的方程了。写作 \(\beta =(X^{T}X)^{-1}X^{T}Y\) ,其中 \(\beta\) 为上面的 \(4*3\) 矩阵，\(X\) 是左边的测量值， \(Y\) 是右边的真值。关于真值我们可以把加速度计按三个轴向6个位置放置（ \(X\) 轴正面朝下： \(\begin{pmatrix} g &0 & 0 \end{pmatrix}\) .....)。这样就对加速度计做了校准，这种方法也称为 六面校准法。

2.3 陀螺仪校准

  陀螺仪的校准有两个部分：Allan方差校准Bias和优化方式求解尺度因子及轴偏差。在校准陀螺仪的时候，要使用到加速度的校准信息，所以加速度校准的好坏关系到整个IMU的校准效果。

2.3.1 Allan方差校准陀螺仪Bias

  在IMU校准的最开始，要将IMU静止搁置时间 \(T_{init}\)(一般取50s)。Allan方差的求取可以参考文献[3]。
  在Allan方差分析中，共有5个噪声参数：量化噪声、角度随机游走、零偏不稳定性、速度随机游走和速度爬升。下面主要分析Allan方差的计算、图像的绘制以及最终噪声的分析。

1. 计算Allan方差

  (1)将陀螺仪静止放置时间\(T\) ,单个采样周期为 \(\tau_{0}\)，共有\(N\) 组采样值。
  (2)计算单次采样输出角度 \(\theta\) 和 平均因子 \(m\)，\(m\) 要尽量取得均匀。
\[ \begin{align} \nonumber \theta(n)&=\int ^{\tau_{0}}\Omega(n)dt \\ \nonumber m &=Rand(1,\frac{N-1}{2}) \end{align} \tag{2.9} \]
  (3)计算Allan方差，当 \(m\) 取不同的值的时候会有不同的Allan方差值。
\[ \nonumber \theta^{2}(\tau)=\frac{1}{2\tau^{2}(N-2m)} \sum^{N-2m}_{k=1}(\theta_{K+2m}-2\theta_{K+m}+\theta_{K}) \tag{2.10} \]
其中，\(\tau=m\tau_{0}\).
  (4)一般在绘制Allan方差曲线的时候使用的是Allan方差的平方根，所以将式(2.6)中的计算结果去平方根即可。

2.拟合Allan方差曲线

  用最小二乘法对计算出的Allan方差曲线进行拟合，最终绘制出的Allan方差的曲线图应该是如下图所示，

图4 Allan方差曲线示意图
  可以借助表1对Allan方差曲线做进一步的分析
表1 Allan标准差与常见的噪声的对应关系

结合图3和表1可知，不同段的Allan方差曲线代表了不同的误差参数，我们要求解的零偏噪声(Bias)对应的曲线段的斜率就是0。

3.计算各部分噪声参数

  在使用Allan方差分析惯性器件的误差的时候，可以认为惯性器件输出数据的随机部分是由特定的噪声源产生的。在保证测试环境稳定定情况下，可以认为各噪声源是独立的，那么计算出的Allan方差就是各部分误差的平方和，
\[ \begin{align} \nonumber \sigma^{2}_{total}(\tau)&=\sigma^{2}_{Q}(\tau)+\sigma^{2}_{N}(\tau)+\sigma^{2}_{B}(\tau)+\sigma^{2}_{K}(\tau)+\sigma^{2}_{R}(\tau) \\ \nonumber &=\frac{3Q^{2}}{\tau^{2}}+\frac{N^{2}}{\tau}+\frac{2B^{2}}{\pi}ln2+\frac{K^{2}\tau}{3}+\frac{R^{2}\tau^{2}}{2} \end{align} \tag{2.10} \]
将式(2.10)整理可得，
\[ \sigma^{2}_{total}(\tau)=\sum^{2}_{i=-2}C_{i}\tau^{i} \tag{2.11} \]
根据第一步中得到的 \(\tau\) 和 Allan方差值，依据最小二乘法进行拟合，进而得到\(C_{i}\) 的值，最终可以得到相应的误差系数，
\[ \nonumber \left\{\begin{matrix} Q=\frac{ \sqrt{C_{-2}}}{3600 \sqrt{3}}(°)\\ \nonumber N=\frac{\sqrt{C_{-1}}}{60}(°)\sqrt{h}\\ \nonumber B=\frac{\sqrt{C_{0}}}{0.664}(°)h\\ \nonumber K=60\sqrt{3C_{1}}[(°)\cdot h^{-1}]/\sqrt{h}\\ \nonumber R=3600\sqrt{2C_{2}}[(°)\cdot h^{-1}]/h \end{matrix}\right. \tag{2.12} \]
以上就是通过Allan方差分析得到陀螺仪Bias的过程，一般要采集好几个小时的数据。如果仅需要，零偏参数，则在初始放置的50s左右的时间就足够了。

2.3.2 优化方式求解尺度因子及轴偏差

  陀螺仪的剩余参数在校准的时候，挑选的是加速度计校准过程中的静态采样片段夹杂的动态片段。刚好可以使用校准过的静态片段测量值的平均值作为初始的重力向量和旋转完成后的实际重力向量。
  在已知陀螺仪的Bias之后，还需要校准的参数有：
\[ \nonumber \theta ^{gyro}=[\gamma_{yz},\gamma_{zy},\gamma_{xz},\gamma_{zx},\gamma_{xy},\gamma_{yx},s^{g}_{x},s^{g}_{y},s^{g}_{z}] \tag{2.13} \]
其他参数的校准的思路也比较简单，即“陀螺仪任意转动，积分得到的角度和加速度的测量值求得的角度做比较即可”这里把加速度的测量值当做了参考值，所以前面加速度的校准是非常重要的。具体的校准过程，如下所述。
  设由加速度计测量值得到的一个初始的加速度向量 \(u_{a,k-1}\)， \(n\)个陀螺仪测量值 \(w_{i}\)，则可以得到陀螺仪积分得到的旋转矩阵旋转之后新的重力向量。 \(u_{g,k}\)，整个过程描述为，
\[ \nonumber u_{g,k}=\Psi[w^{S}_{i},u_{a,k-1}] \tag{2.14} \]
按照上面的描述，进一步得到代价函数，
\[ \nonumber L(\theta ^{gyro})=\sum^{M}_{k=2}||u_{a,k}-u_{g,k}||^{2} \tag{2.15} \]
其中 \(u_{a,k}\)为旋转结束之后，由加速度的测量值得到的实际的重力向量。
  在式(2.14)中会涉及到离散时间的陀螺仪积分问题，这里选择使用四阶龙格库塔法(Runge-Kutta, RK4n)。具体的积分过程描述如下。
  定义四元数的导数(局部干扰得到)
\[ \nonumber f(q,t)=\dot{q}=\frac{1}{2}\Omega(w(t))q \]
则RK4的具体过程为，
\[ \nonumber q_{k+1}=q{k}+\Delta t\frac{1}{6}(k_{1}+2k_{2}+2k_{3}+k_{4}) \tag{2.16a} \]\[ \nonumber k_{i}=f(q^{(i)},t_{k}+c_{i}\Delta t) \tag{2.16b} \]\[ \nonumber q^{(i)}=q_{k} \tag{2.16c} \]\[ \nonumber q^{(i)}=q_{k}+\Delta t \sum^{i-1}_{j=1}a_{ij}k_{j} \tag{2.16d} \]
其中
\[ \begin{align} \nonumber c_{1}=0, \quad c_{2}=\frac{1}{2}, \quad c_{3}=\frac{1}{2}, \quad c_{4} = 1 \\ \nonumber a_{21}=\frac{1}{2}, \qquad a_{31}=0, \qquad a_{41}=0, \\ \nonumber a_{32}=\frac{1}{2}, \qquad a_{42}=0, \qquad a_{43}=1 \end{align} \]
最后要将 \(q_{k+1}\)归一化。
上述积分过程的C++代码如下(出自[5])：

template <typename _T> inline void imu_tk::quatIntegrationStepRK4(const Eigen::Matrix< _T, 4, 1> &quat,const Eigen::Matrix< _T, 3, 1> &omega0,const Eigen::Matrix< _T, 3, 1> &omega1,const _T &dt, Eigen::Matrix< _T, 4, 1> &quat_res )
{Eigen::Matrix< _T, 3, 1> omega01 = _T(0.5)*( omega0 + omega1 );Eigen::Matrix< _T, 4, 1> k1, k2, k3, k4, tmp_q;Eigen::Matrix< _T, 4, 4> omega_skew;// First Runge-Kutta coefficientcomputeOmegaSkew( omega0, omega_skew );k1 = _T(0.5)*omega_skew*quat;// Second Runge-Kutta coefficienttmp_q = quat + _T(0.5)*dt*k1;computeOmegaSkew( omega01, omega_skew );k2 = _T(0.5)*omega_skew*tmp_q;// Third Runge-Kutta coefficient (same omega skew as second coeff.)tmp_q = quat + _T(0.5)*dt*k2;k3 = _T(0.5)*omega_skew*tmp_q;// Forth Runge-Kutta coefficienttmp_q = quat + dt*k3;computeOmegaSkew( omega1, omega_skew );k4 = _T(0.5)*omega_skew*tmp_q;_T mult1 = _T(1.0)/_T(6.0), mult2 = _T(1.0)/_T(3.0);quat_res = quat + dt*(mult1*k1 + mult2*k2 + mult2*k3 + mult1*k4);normalizeQuaternion(quat_res);
}