『imu_tk』工具标定IMU的基本原理

『imu_tk』工具标定IMU的基本原理

IMU误差模型

• 对于加速度计和陀螺仪，定义正交系AOF(GOF)和非正交系AF(GF)：
  • AOF(GOF)系的x轴与AF(GF)系x轴重合
  • AOF(GOF)系的y轴与AF(GF)系的xy平面重合
• 同时，定义正交系BF为载体系，非正交系S为传感器系。在旋转的角度较小时，可以定义非正交系中的测量$s^S$到正交系B的变换为：
  $$s^B=T{s}^S,T=\left[\begin{array}{ccc}1& -{\beta }_{yz}& {\beta }_{zy}\\ {\beta }_{xz}& 1& -{\beta }_{zx}\\ -{\beta }_{xy}& {\beta }_{yx}& 1\end{array}\right]$$
  其中，${\beta }_{ij}$代表S系中的$i$轴绕BF系中$j$轴的旋转。
  

对于IMU标定，可以分为加速度计标定和陀螺仪标定两部分。其中，二者待标定参数的形式上是相同的。（其中，上标$s$代表非正交系中的向量，上标$o$代表正交系中的向量）

• 加速度计模型 $(acc)$：
  $$a^O=T^a{K}^a(a^S+b^a+v^a)$$
• 陀螺仪模型 $(gyro)$：
  $$w^O=T^g{K}^g(w^S+b^g+v^g)$$
• 对于加速度计和陀螺仪的每个轴上都有比例因子误差和交轴耦合误差，分别定义为$K^g,K^a$ 和 $T^g,T^a$。对于二者的量测噪声$v^g,v^a$，可以看做是高斯白噪声，由于本文采用静止区间取测量均值的方法进行计算，故可直接忽略该项。

其中，
$$T^g=\left[\begin{array}{ccc}1& -{\gamma }_{yz}& {\gamma }_{zy}\\ {\gamma }_{xz}& 1& -{\gamma }_{zx}\\ -{\gamma }_{xy}& {\gamma }_{yx}& 1\end{array}\right]，K^g=\left[\begin{array}{ccc}{s}_x^g& 0& 0\\ 0& s_y^g& 0\\ 0& 0& s_z^g\end{array}\right]$$
我们定义AOF系与BF系完全重合，故 $T^a$ 可以简化为：
$$T^a=\left[\begin{array}{ccc}1& -{\alpha }_{yz}& {\alpha }_{zy}\\ 0& 1& -{\alpha }_{zx}\\ 0& 0& 1\end{array}\right]，K^a=\left[\begin{array}{ccc}{s}_x^a& 0& 0\\ 0& s_y^a& 0\\ 0& 0& s_z^a\end{array}\right]$$

利用优化的方法求解

对于加速度计，我们定义损失函数：（$g$是当地重力加速度）
$$L({\theta }^{acc})=\sum _{k=1}^M(||\mathbf{g}|{|}^2-||{\mathbf{a}}^O|{|}^2{)}^2$$
对于陀螺仪，我们定义损失函数：
$$L({\theta }^{gyro})=\sum _{k=2}^M||{\mathbf{u}}_{a,k}-{\mathbf{u}}_{g,k}|{|}^2$$
其中，$M$代表记录的$M$个方向，${\mathbf{u}}_{g,k}=\psi [w_i^S,{\mathbf{u}}_{a,k-1}]$定义为由$k-1$时刻的重力加速度向量和$k-1$到$k$时刻区间内陀螺仪积分计算出的$k$时刻重力加速度方向（由于校准加速度计在前，此时可认为其测量值无偏）。本文采用$Levenberg-Marquardt$算法最小化损失函数。

校准步骤

参数设置

• 文中推荐 $N$ 取 $36\to 50$ 个位置数据
• 设定初始静止区间长度 $T_{init}$，文中推荐值为 $50s$
• 设定间隔区间长度 $T_{wait}$， 推荐取值 $1\to 4s$
• 设定时间窗$win\_size$大小，一般设置为静止区间长度的一半

步骤

1. 根据时间戳，提取从$t_0$到$t_0+T_{init}$的数据序列
2. 计算该序列数据的方差作为初始阈值$nor{m}_{th}$，乘以一个系数$t{h}_{mult}$后作为当前阈值，根据当前阈值提取静态区间
3. 根据当前提取出的静态区间校准加速度计
4. 根据校准后的加速度计和动态区间陀螺仪的积分校准陀螺仪

校准结果

• 提取出的静态区间如图所示：
  
  局部放大后：
  
  可以看到，静止区间提取的都很恰当，如果提取不完整或者有其他问题，酌情修改$win\_size$和间隔区间大小即可。

• 去除$bias$后的陀螺仪数据如下：
  
  局部放大后：
  
  经过四阶龙格库塔法积分后，即可得到$\Delta t$时间内IMU旋转角度，并根据$t$时刻的$a_t^O$计算出$t+1$时刻的$a^{\prime }$，通过迭代求解，使得$a^{\prime }$趋近于$a_{t+1}^O$。

残差

一般来说，算法收敛后，加速度计和陀螺仪$cost$值都至少应该在$10^{-3}$数量级才能保证校准精度。我实际校准MPU6050，是可以保证这一点的。

附上论文中校准后的参数：

我自己校准的结果：

Accelerometers calibration: Better calibration obtained using threshold multiplier 8 with residual 0.000701741
Misalignment Matrix
           1 -9.40579e-05  -0.00345677
           0            1   0.00620255
          -0            0            1
Scale Matrix
 1.00179        0        0
       0  1.00076        0
       0        0 0.979337
Bias Vector
 0.286974
-0.244268
-0.869412

Accelerometers calibration: inverse scale factors:
0.998216
0.999239
1.0211

Gyroscopes calibration: residual 0.000793112
Misalignment Matrix
         1 0.00171194 0.00134399
 0.0022529          1   -0.01433
0.00430149 -0.0174916          1
Scale Matrix
1.00817       0       0
      0  1.0098       0
      0       0  1.0088
Bias Vector
 -0.149787
-0.0270851
-0.0147985

Gyroscopes calibration: inverse scale factors:
0.991895
0.990297
0.991278

由于我采集的时候陀螺仪单位是$°/s$，而程序中默认单位是$rad/s$，所以在读取数据的时候我将数据从角度制转化成了弧度制，因而这里的bias单位是$rad/s$

注意事项

1. 静止区间内一定要保证IMU处于静止状态
2. 动态区间要尽量保证数据平滑