>
返回

IMU 内参标定

简介

前两篇博客分别整理了 IMU 的测量原理和误差形成原因,接下来主要关注如何对 IMU 进行标定和校正。

确定误差标定

在之前的博客中,我们总结了 IMU 的误差模型如下所示。这一部分中是对零偏、尺度因子和轴偏差矩阵进行标定,对于噪声(包括测量值中的白噪声和零偏不稳定性)我们无法通过校正来消除,只能通过计算其不确定度来作为我们参考的指标。

$$
\begin{aligned}
    \boldsymbol{a}^O &= \boldsymbol{T}^a\boldsymbol{K}^a(\boldsymbol{a}^S + \boldsymbol{b}^a + \boldsymbol{n}^a) \\
    \boldsymbol{\omega}^O &= \boldsymbol{T}^\omega\boldsymbol{K}^\omega(\boldsymbol{\omega} + \boldsymbol{b}^a + \boldsymbol{n}^a)
\end{aligned}
$$

为了简化计算,我们可以将尺度因子和轴偏差矩阵融合进零偏中先进行计算,标定好尺度因子和轴偏差之后再用起算起零偏即可,即令 $\boldsymbol{B} = \boldsymbol{T}\boldsymbol{K}\boldsymbol{b}$

$$
\begin{aligned}
    \boldsymbol{a}^O &= \boldsymbol{T}^a\boldsymbol{K}^a\boldsymbol{a}^S + \boldsymbol{B}^a \\
    \boldsymbol{\omega}^O &= \boldsymbol{T}^\omega\boldsymbol{K}^\omega\boldsymbol{\omega} + \boldsymbol{B}^g
\end{aligned}
$$

常规解方程方法

这一种方法比较直观,简单的说我们需要解出以下方程组的系数,以加速度计为例:

$$
\begin{aligned}
    \begin{bmatrix}
        a_x^O\\
        a_y^O\\
        a_z^O
    \end{bmatrix} = 
    \begin{bmatrix}
        1 & -\beta_{yz}^a & \beta_{zy}^a \\
        \beta_{xz}^a & 1 & -\beta_{zx}^a \\
        -\beta_{xy}^a & \beta_{yx}^a & 1
    \end{bmatrix}
    \begin{bmatrix}
        s_x^a & 0 & 0 \\
        0 & s_y^a & 0 \\
        0 & 0 & s_z^a
    \end{bmatrix}
    \begin{bmatrix}
        a_x^S\\
        a_y^S\\
        a_z^S
    \end{bmatrix} +
    \begin{bmatrix}
        B_x\\
        B_y\\
        B_z
    \end{bmatrix}
\end{aligned}
$$

如上所示,我们需要求解上述三个联立方程中的 12 个独立参数。因此理论上最少我们只需要获取四组加速度计的数据(互相独立的情况下),联立方程即可。实际中一般有两种思路:

解析法(加速度计)

这个方法中我们需要利用准确的真值和理论值来进行求解,并且需要将上述方程改变一下将真值作为自变量简化计算,如下所示:

$$
\begin{aligned}
    \boldsymbol{a}^S &= (\boldsymbol{T}^a\boldsymbol{K}^a)^{-1}\boldsymbol{a}^O - \boldsymbol{b}^a \\
    &= \boldsymbol{T}'\boldsymbol{a}^O + \boldsymbol{b}'^{a}\\
    &= \begin{bmatrix}
        s_x'^a & \beta'^a_{yz} & \beta'^a_{zy} \\
        \beta'^a_{xz} & s_y'^a & \beta'^a_{zx} \\
        \beta'^a_{xy} & \beta'^a_{yx} & s_z'^a
    \end{bmatrix}\boldsymbol{a}^O+
    \begin{bmatrix}
        b'^a_x\\
        b'^a_y\\
        b'^a_z
    \end{bmatrix}
\end{aligned}
$$

这里注意,实际上有些工具或者论文可能是以这种表示方法作为误差模型,即将真实值作为方程自变量,测量值作为因变量。实际上两者应该互相等效,只需要前后保持一致即可,这里在总结不同方法时为了方便计算可能会转换成不同表示方法。首先将 IMU 水平向上放置,此时理论加速度为:$(0, 0, g)$,代入上述方程有:

$$
\begin{aligned}
    a^S_x &= \beta'^a_{zy}g+b'^a_x \\
    a^S_y &= \beta'^a_{zx}g+b'^a_x \\
    a^S_z &= s_z'^ag+b'^a_x
\end{aligned}
$$

同理,将 IMU 倒立放置,此时理论加速度为:$(0, 0, -g)$,代入为:

$$
\begin{aligned}
    a'^S_x &= -\beta'^a_{zy}g+b'^a_x \\
    a'^S_y &= -\beta'^a_{zx}g+b'^a_x \\
    a'^S_z &= -s_z'^ag+b'^a_x
\end{aligned}
$$

两组方程式联立即可求出 6 个独立参数,对于其他参数,可以参考这个做法将对应轴方向或者向下放置求解即可。这个方法要求加速度计必须得放平,对平台有一定的要求。

线性最小二乘(加速度计)

线性最小二乘的方法则不依赖于某个特定角度,将误差模型整理成以下形式:

$$
\begin{aligned}
&\begin{aligned}
    a_x^O &= s_x^aa_x^S -\beta_{yz}^aa_y^S + \beta^a_{zy}a_z^S + b_x^a \\
    a_y^O &= \beta_{xz}^aa_x^S -s_y^aa_y^S + -\beta_{zx}^aa_z^S + b_y^a \\
    a_z^O &= -\beta_{xy}^aa_x^S -\beta_{yx}^aa_y^S + s_z^aa_z^S + b_z^a
\end{aligned}\\
&\Downarrow
\\
\begin{bmatrix}
    a_x^O\\
    a_y^O\\
    a_z^O
\end{bmatrix} &= 
\begin{bmatrix}
    a_x^S & a_y^S & a_z^S & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
    0 & 0 & 0 & 0 & a_x^S & a_y^S & a_z^S & 1 & 0 & 0 & 0 & 0 \\
    0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & a_x^S & a_y^S & a_z^S & 1
\end{bmatrix}
\begin{bmatrix}
    s_x^a\\
    -\beta_{yz}^a\\
    \beta^a_{zy}\\
    b_x^a\\
    \beta_{xz}^a\\
    -s_y^a\\
    -\beta_{zx}^a\\
    b_y^a\\
    -\beta_{xy}^a\\
    -\beta_{yx}^a\\
    s_z^a\\
    b_z^a
\end{bmatrix}\\
&\Downarrow\\
\boldsymbol{b} &= \boldsymbol{A}\boldsymbol{x}
\end{aligned}
$$

通过这种方法我们将问题转换成一个典型的线性最小二乘问题:$\boldsymbol{A}\boldsymbol{x} = \boldsymbol{b}$,其最优解为:$\tilde{\boldsymbol{x}} = (\boldsymbol{A}^T\boldsymbol{A})^{-1}\boldsymbol{A}^T\boldsymbol{b}$

因此,将 IMU 用不同朝向获取一系列数据取得上述 $\boldsymbol{A}$$\boldsymbol{b}$ 求解即可,另外上述构建的方程只是其中最简单的一种,将参数排列顺序改变可以获得不同系数矩阵。

解析法(陀螺仪)

对于陀螺仪标定,大致思路和加速度计一致,但有以下两点区别:

  • 不同于加速度计,陀螺仪没办法使用重力加速度作为真值,因此只能使用高精度转台来提供所需要的真值,而由于转台的角速度精度通常不如角度精度高,因此通常对角度进行积分得到角度作为真值
  • 陀螺的角速度除了要考虑转台提供的角速度,还需要考虑地球自转的影响,通常的做法时通过构建特定的方程将地球转速抵消

同样为了化简计算,采用如下误差模型,这里假设地球转速对陀螺仪真值的影响为 $\boldsymbol{\omega}_e^i = (\omega_{ex}^i, \omega_{ey}^i, \omega_{ez})$

$$
\begin{aligned}
    \boldsymbol{\omega}^S &= (\boldsymbol{T}^g\boldsymbol{K}^g)^{-1}(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i) - \boldsymbol{b}^g \\
    &= \boldsymbol{T}'(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i) + \boldsymbol{b}'^{g}\\
    &= \begin{bmatrix}
        s_x'^g & \beta'^g_{yz} & \beta'^g_{zy} \\
        \beta'^g_{xz} & s_y'^a & \beta'^g_{zx} \\
        \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g
    \end{bmatrix}(\boldsymbol{\omega}^O + \boldsymbol{\omega}_e^i)+
    \begin{bmatrix}
        b'^g_x\\
        b'^g_y\\
        b'^g_z
    \end{bmatrix}
\end{aligned}
$$

先将陀螺仪沿 $z$ 轴正方向以角速度 $\omega$ 旋转,有:

$$
\begin{aligned}
    \omega_x^S = \beta'^g_{zy}\omega + \begin{bmatrix}
        s_x'^g & \beta'^g_{yz} & \beta'^g_{zy}
    \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_x\\
    \omega_x^S = \beta'^g_{zx}\omega + \begin{bmatrix}
        \beta'^g_{xz} & s_y'^a & \beta'^g_{zx}
    \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_y\\
    \omega_x^S = s_z'^g\omega + \begin{bmatrix}
        \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g
    \end{bmatrix}\boldsymbol{\omega}_e^i + b'^g_z
\end{aligned}
$$

对角速度积分,有:

$$
\begin{aligned}
    \theta_x^S = \beta'^g_{zy}\theta + \begin{bmatrix}
        s_x'^g & \beta'^g_{yz} & \beta'^g_{zy}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\
    \theta_x^S = \beta'^g_{zx}\theta + \begin{bmatrix}
        \beta'^g_{xz} & s_y'^a & \beta'^g_{zx}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\
    \theta_x^S = s_z'^g\theta + \begin{bmatrix}
        \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz}
\end{aligned}
$$

同样思路,以角速度 $-\omega$ 旋转(相反方向)并积分,有:

$$
\begin{aligned}
    \theta_x'^S = -\beta'^g_{zy}\theta + \begin{bmatrix}
        s_x'^g & \beta'^g_{yz} & \beta'^g_{zy}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\
    \theta_y'^S = -\beta'^g_{zx}\theta + \begin{bmatrix}
        \beta'^g_{xz} & s_y'^a & \beta'^g_{zx}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\
    \theta_z'^S = -s_z'^g\theta + \begin{bmatrix}
        \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz}
\end{aligned}
$$

两组方程相减,即可求得部分尺度因子和轴偏差参数,对于其他尺度因子和轴偏差参数,只需要将陀螺仪沿其他两轴用同样方式得到即可。注意这里不能像加速度计一样通过相加来获取零偏,有两点原因:地球转速未知、旋转时间过短零产生的角度太小,如果直接计算误差会很大。这里我们通过另一种方法求解:

假设地球沿自转轴自转角速度为 $\omega_e$,将其投影至东北天坐标系(正切与地球地球表面)的角速度为:

$$
\boldsymbol{\omega}^n = 
\begin{bmatrix}
    \omega^n_x \\
    \omega^n_y \\
    \omega^n_z \\
\end{bmatrix} =
\begin{bmatrix}
    0 \\
    \omega_e\cos{\phi} \\
    \omega_e\sin{\phi}
\end{bmatrix}
$$

其中 $\phi$ 为 ENU 系原点所在的纬度。由 ENU 系至 IMU 系还需要一次旋转,这个旋转矩阵是未知的,设为 $\boldsymbol{R}$,则当陀螺仪处以某一位置,上述地球转速对陀螺仪形成的额外角速度为:

$$
\boldsymbol{\omega}^i_e = \boldsymbol{R}\begin{bmatrix}
    \omega^n_x \\
    \omega^n_y \\
    \omega^n_z \\
\end{bmatrix}
$$

此时若我们将 陀螺仪沿其 $z$ 轴旋转 180 度,这个角速度为:

$$
\begin{aligned}
    \boldsymbol{\omega}'^i_e &= \boldsymbol{R}_\pi\boldsymbol{R}\begin{bmatrix}
    \omega^n_x \\
    \omega^n_y \\
    \omega^n_z \\
\end{bmatrix}\\
    &= \begin{bmatrix}
        \cos{\pi} & \sin{\pi} & 0\\
        -\sin{\pi} & \cos{\pi} & 0\\
        0 & 0 & 1
    \end{bmatrix}\boldsymbol{R}\begin{bmatrix}
    \omega^n_x \\
    \omega^n_y \\
    \omega^n_z \\
\end{bmatrix} \\
    &= \begin{bmatrix}
        -1 & 0 & 0\\
        0 & -1 & 0\\
        0 & 0 & 1
    \end{bmatrix}\boldsymbol{R}\begin{bmatrix}
    \omega^n_x \\
    \omega^n_y \\
    \omega^n_z \\
\end{bmatrix}\\
&= \boldsymbol{R}\begin{bmatrix}
    -\omega^n_x \\
    -\omega^n_y \\
    \omega^n_z \\
\end{bmatrix}
\end{aligned}
$$

可以发现此时我们将 $x, y$ 轴上的地球角速度转变成其相反数,沿着这个思路,我们先将陀螺仪在某一位置停留一段时间,对其测量角速度(只包含零偏和地球自转角速度)进行积分,有:

$$
\begin{aligned}
    \theta_x'^S =  \begin{bmatrix}
        s_x'^g & \beta'^g_{yz} & \beta'^g_{zy}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bx}\\
    \theta_y'^S =  \begin{bmatrix}
        \beta'^g_{xz} & s_y'^a & \beta'^g_{zx}
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{by}\\
    \theta_z'^S =  \begin{bmatrix}
        \beta'^g_{xy} & \beta'^g_{yx} & s_z'^g
    \end{bmatrix}\boldsymbol{\theta}_e^i + \theta'^g_{bz}
\end{aligned}
$$

这里由于 $\beta\theta$ 是一个二阶小量所以可以省略,上式化简为:

$$
\begin{aligned}
    \theta_x^S &= s_x'^g\theta_{ex}^i + \theta'^g_{bx}\\
    \theta_y^S &= s_y'^g\theta_{ey}^i + \theta'^g_{by}\\
    \theta_z^S &= s_z'^g\theta_{ez}^i + \theta'^g_{bz}
\end{aligned}
$$

将陀螺仪转过 180 度再停留一段时间,对测量角速度积分得:

$$
\begin{aligned}
    \theta_x'^S &= -s_x'^g\theta_{ex}^i + \theta'^g_{bx}\\
    \theta_y'^S &= -s_y'^g\theta_{ey}^i + \theta'^g_{by}\\
    \theta_z'^S &= s_z'^g\theta_{ez}^i + \theta'^g_{bz}
\end{aligned}
$$

两组方程相加即可获得 $x, y$ 轴的零偏,接下来以其他两轴作为转轴旋转以同样方式即可得到 $z$ 轴的零偏。自此我们标定了陀螺仪的零偏,标度因子和轴偏差参数。

线性最小二乘法(陀螺仪)

线性最小二乘法跟加速度计的类似,将误差模型转化为 $\boldsymbol{Ax = b}$ 的形式再获取多组数据求解即可,地球对转速的影响同样可以作为待估计参数进行标定。

优化方法

除了用常规的解方程的思想去标定各个参数以外,我们还可以用优化的思路求解。

优化方法标定加速度计内参

前面我们假设加速度计的三个轴都有一定的轴偏差,这里我们使加速度计的 $x$ 轴和理想正交系的 $x$ 轴重合,并使加速度计的 $y$ 轴在理想正交系的 $x, y$ 轴形成的平面上,这个条件是可以通过旋转理想正交系满足的。此时轴偏差参数 $\beta_{xz}, \beta_{xy}, \beta_{yx}$ 变成 0,轴偏差矩阵为:

$$
\boldsymbol{T}^a=\begin{bmatrix}
        1 & -\beta_{yz}^a & \beta_{zy}^a \\
        0 & 1 & -\beta_{zx}^a \\
        0 & 0 & 1
\end{bmatrix}
$$

因此,加速度计中需要标定的参数为:

$$
\boldsymbol{\theta}^a = \begin{bmatrix}
    \beta_{yz}^a & \beta_{zy}^a & \beta_{zx}^a & s_x^a & s_y^a & s_z^a & b_x^a & b_y^a & b_z^a 
\end{bmatrix}
$$

我们知道当 IMU 静止的时候,它输出的理论值的大小是当地的重力加速度大小,因此可以以这个作为优化条件,在不同角度下将 IMU 静置一段时间,采集 M 组数据,将测量值的大小和理论值的大小作为残差进行 LM 优化,即:

$$
\begin{aligned}
    \boldsymbol{L}(\boldsymbol{\theta}^a) &= \sum_{k = 1}^M(||\boldsymbol{g}||^2 - ||h(\boldsymbol{a}_k^S, \boldsymbol{\theta}^a)||^2)^2 \\
    h(\boldsymbol{a}_k^S, \boldsymbol{\theta}^a) &= \boldsymbol{T}^a\boldsymbol{K}^a(\boldsymbol{a}^S + \boldsymbol{b}^a)
\end{aligned}
$$

优化方法标定陀螺仪内参

对于陀螺仪的内参,首先用优化方法标定标度因子和轴偏差,这里我们先不考虑零偏的影响,零偏在下一部分随机误差标定中再说明。不考虑零偏的情况下,这里我们要优化的参数为:

$$
\boldsymbol{\theta}^a = \begin{bmatrix}
    \beta_{yz}^g & \beta_{zy}^g & \beta_{zx}^g & \beta^g_{xz} & \beta^g_{xy} & \beta^g_{yx} & s_x^g & s_y^g & s_z^g
\end{bmatrix}
$$

标定陀螺仪内参需要用到加速度计测量值作为参考真值,因此需要先标定好加速度计内参再标定陀螺仪。具体如下:

对于一次加速度计测量我们可以获取其得到的重力方向的分量 $\boldsymbol{u}_{a, k - 1}$, 在这之后旋转 IMU 获取 n 组角速度读数,并对角速度计求解 IMU 的姿态变化,并计算得到重力方向的分量:$\boldsymbol{u}_g = \Psi(\omega_i^S, \boldsymbol{u}_{a, k - 1})$,并且此时我们从加速度计测量值也可以得到一个重力分量:$\boldsymbol{u}_{a, k}$,通过这两个比较可以构建残差并进行 LM 优化:

$$
\boldsymbol{L}(\boldsymbol{\theta}^a) = \sum_{k = 1}^M(\boldsymbol{u}_{a, k} - \Psi(\omega_i^S, \boldsymbol{u}_{a, k - 1}))^2
$$

关于如何从离散角速度积分得到姿态变化,论文作者采用了四阶龙格库塔法,具体可以参考论文。

滤波方法

除了以上两种思路以外,还可以采用滤波的方法来标定误差。之后补上过程。

随机误差标定

在误差分析中,我们提到了除了标度因子和轴偏差以外,还有一部分我们没办法通过标定去除的误差,即测量值中的高斯白噪声以及对高斯白噪声积分得到的随机游走,这里面我们一般通过 Allan 方差对其不确定度进行确定。Allan 方差能够标定的误差不确定度包括:

  • (角度/速度)随机游走
  • (角速度/加速度)随机游走
  • 零偏不稳定性

Allan 方差标定的原理主要是利用功率谱的一些相关知识,由于我也不太熟悉所以这里不做深入,只总结了以下标定过程和怎么获取标定结果。过程如下:

  • 将 IMU 静置一段时间,先按照周期 T 采集一段数据,对这组数据可以计算出其方差
  • 将采集到的数据每两个进行平均,相当于我们 以 2T 周期采集了另一组数据,同样可以计算出相应的方差
  • 用同样的方法获取不同周期下的方差数据,绘制方差-周期曲线(log-log 形式)

以下图为例,图中绘制 x, y, z 三个型号的陀螺仪的艾伦方差曲线

不确定度参数获取方式如下,找到下面各个参数对应的位置与 $\tau = 10^0 = 1$ 的交点,再将对应的 Y 值转化为对应的不确定度即可:

  • 量化噪声(quantization noise):斜率为 -1,转换关系:$y = \sqrt{3}Q$
  • 白噪声/(角度/速度)随机游走(white noise/ (angle/velocity) random walk): 斜率为 -1/2,转换关系:$y = N$
  • 零偏不稳定性(bias instablity):斜率为 0(曲线最低点),转换关系:$y = \frac{2B}{3}$
  • (角速度/加速度)随机游走(angle velocity/acceralation random walk):斜率为 1/2,转换关系:$y = \frac{K}{\sqrt{3}}$
  • 速率斜坡(rate ramp):斜率为 1,转换关系:$y = \frac{R}{\sqrt{2}}$

参考资料

Built with Hugo
Theme Stack designed by Jimmy