IMU模型与标定

IMU传感器

旋转运动学

旋转的线速度为：
$$\begin{array}{rl}\dot{\mathbf{r}}& =(-a\dot{\theta }\sin \theta ,a\dot{\theta }\cos \theta ,0{)}^{\top }\\ & =\left[\begin{array}{ccc}0& -\dot{\theta }& 0\\ \dot{\theta }& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}a\cos \theta \\ a\sin \theta \\ h\end{array}\right]\\ & =\mathbf{\omega }\times \mathbf{r}\end{array}\text{\hspace{1em}(1)}$$
取模得到角速度和线速度之间的关系：
$$|\dot{\mathbf{r}}|=|\mathbf{\omega }||\mathbf{r}|\sin \varphi =a|\dot{\theta }|\text{\hspace{1em}(2)}$$
从Body系和Inertial系分别看旋转运动得到的速度关系为：
$$\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+{\left[{\mathbf{R}}_{IB}{\mathbf{\omega }}_b\right]}_{\times }\mathbf{r}\\ & ={\mathbf{R}}_{IB}{\mathbf{v}}_b+\mathbf{\omega }\times \mathbf{r}\end{array}\text{\hspace{1em}(3)}$$

$${\mathbf{v}}_I\equiv {\mathbf{R}}_{IB}{\mathbf{v}}_b+\omega \times \mathbf{r}\iff {\mathbf{R}}_{IB}{\mathbf{v}}_b\equiv {\mathbf{v}}_I-\mathbf{\omega }\times \mathbf{r}\text{\hspace{1em}(4)}$$

对公式（3）中速度求导得到加速度：
$$\begin{array}{rl}{\ddot{\mathbf{r}}}_I& ={\mathbf{R}}_{IB}{\dot{\mathbf{v}}}_B+{\dot{\mathbf{R}}}_{IB}{\mathbf{v}}_B+\mathbf{\omega }\times \dot{\mathbf{r}}+{\left[{\dot{\mathbf{R}}}_{IB}{\mathbf{\omega }}_b+{\mathbf{R}}_{IB}{\dot{\mathbf{\omega }}}_b\right]}_{\times }\mathbf{r}\\ & ={\mathbf{R}}_{IB}{\mathbf{a}}_B+2\mathbf{\omega }\times \mathbf{v}+\mathbf{\omega }\times (\mathbf{\omega }\times \mathbf{r})+\dot{\mathbf{\omega }}\times \mathbf{r}\end{array}\text{\hspace{1em}(5)}$$
对公式（5）进行变形得到：

$$\mathbf{a}={\mathbf{a}}_I-\underset{\text{科}氏力}{2\mathbf{\omega }\times \mathbf{v}}-\underset{\text{欧}拉力}{\dot{\mathbf{\omega }}\times \mathbf{r}}-\underset{\text{离}心力}{\mathbf{\omega }\times (\mathbf{\omega }\times \mathbf{r})}\text{\hspace{1em}(6)}$$
其中：$\mathbf{v}={\mathbf{R}}_{IB}{\mathbf{v}}_b$，$\mathbf{a}={\mathbf{R}}_{IB}{\mathbf{a}}_b$，它们表示body下的速度或加速度在Inertial系下的表示。要区别这几种表示，习惯了找一个真实值，只不过是不同系下的不同表示罢了

测量模型与运动模型

MEMS加速度计

使用电容或者电阻桥来进行测量，注意有可能测出来的加速度计值方向是与实际力相反的，因为是使用弹簧测出的惯性力。

• 加速度计的质量块没有高速运动，不受科氏力影响

陀螺仪

• 振动陀螺仪使用的是科氏力作用来测得旋转，一个主运动轴+一个敏感轴，与运动方向垂直方向（敏感轴方向）受一个科氏力。

• 一般使用两个运动方向相反的质量块来侧科氏力，这样可以抵消加速度的影响。若质量不一致会导致有差别。

IMU误差模型

分为：确定性误差，随机误差。前者可以提前标定，后者建模成高斯分布。

确定性误差

• 偏差bias
• 尺度因子Scale
• 轴偏差（非正交误差）
• Run-to-Run Bias/Scale Factor 即每次上电启动的值都不一样
• In Run (Stability) Bias/Scale Factor 即在运行过程中也会发生变化
• Temperature-Dependent Bias/Scale Factor 即受温度影响的变化值

尺度因子和非对称误差为：
$$\left[\begin{array}{l}{l}_{ax}\\ l_{ay}\\ l_{az}\end{array}\right]=\left[\begin{array}{lll}{s}_{xx}& m_{xy}& m_{xz}\\ m_{yx}& s_{yy}& m_{yz}\\ m_{zx}& m_{zy}& s_{zz}\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]\text{\hspace{1em}(7)}$$

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

对于加速度计；

三轴分别朝上朝下放置一段时间，如果考虑为都是正交的，相加相减就可以得到：
$$\begin{array}{rl}b& =\frac{l_f^{up}+l_f^{down}}{2}\\ S& =\frac{l_f^{up}-l_f^{down}}{2\cdot g}\end{array}\text{\hspace{1em}(8)}$$
考虑轴间误差时，实际值与测量值之间的关系为
$$\left[\begin{array}{l}{l}_{ax}\\ l_{ay}\\ l_{az}\end{array}\right]=\left[\begin{array}{lll}{s}_{xx}& m_{xy}& m_{xz}\\ m_{yx}& s_{yy}& m_{yz}\\ m_{zx}& m_{zy}& s_{zz}\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{c}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]\text{\hspace{1em}(9)}$$
变为齐次坐标得到：
$$\left[\begin{array}{l}{l}_{ax}\\ l_{ay}\\ l_{az}\end{array}\right]=\left[\begin{array}{lllc}{s}_{xx}& m_{xy}& m_{xz}& b_{ax}\\ m_{yx}& s_{yy}& m_{yz}& b_{ay}\\ m_{zx}& m_{zy}& s_{zz}& b_{az}\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ 1\end{array}\right]\text{\hspace{1em}(10)}$$
然后再进行转置，标准的加速度值已知，利用最小二乘求解12个参数即可。

对于陀螺仪；

也可以使用六面法，需要高精度转台提供真实值，六个值分别为绕各个轴的顺时针和逆时针的角速度。

温度相关

进行温度补偿，获得不同温度时的bias和scale值，绘制成曲线。

• soak 方法：控制不同的恒温值，测得传感器数据。
• ramp 方法：记录一段时间内线性升温和降温时传感器数据。

随机误差

高斯白噪声建模成均值为0，方差$\sigma$的各时刻独立的高斯过程，一般是由AD转换器引起的外部噪声。
$$\begin{array}{l}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}\text{\hspace{1em}(11)}$$
其中$\delta ()$表示狄拉克函数。

离散的数据噪声，与连续的高斯白噪声方差之间的转换关系为：

$$\begin{gathered} 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 \\ \begin{array}{rl}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}\text{\hspace{1em}(12)} \end{gathered}$$
所以离散的高斯白噪声为：
$$n_d[k]={\sigma }_{d}w[k]\text{\hspace{1em}(13)}$$
其中：$w[k]\sim \mathcal{N}(0,1)$，${\sigma }_d=\sigma \frac{1}{\sqrt{\Delta t}}$

随机游走可以把它看做是一种布朗运动，或者将其称之为维纳过程。该模型可以看做是高斯白噪声的积分。该噪声参数一般是由传感器的内部构造、温度等变化量综合影响下的结果。建模成导数为高斯分布的噪声。
$$\dot{b}(t)=n(t)={\sigma }_{b}w(t)\text{\hspace{1em}(14)}$$
同样离散的和连续的之间转换为：
$$\begin{array}{rl}{b}_d[k]\triangleq & b\left(t_0\right)+{\int }_{t_0}^{t_0+\Delta t}n(t)dt\\ E(\left(b_d[k]-b_d[k-1{]}^2\right)& =E\left({\int }_{t_0}^{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}\text{\hspace{1em}(15)}$$
即：
$$b_d[k]=b_d[k-1]+{\sigma }_{bd}w[k]\text{\hspace{1em}(16)}$$
其中：$w[k]\sim \mathcal{N}(0,1)$，${\sigma }_{bd}={\sigma }_b\sqrt{\Delta t}$

随机误差的标定方法——Allan方差法

具体流程如下：

1. 将陀螺仪静止放置时间$T$ ,单个采样周期为 ${\tau }_0$ ，共有$N$组采样值。
2. 计算单次采样输出角度$\theta$ 和 平均因子$m$，$m$要尽量取得均匀。

$$\begin{array}{rl}\theta (n)& ={\int }^{{\tau }_0}\Omega (n)dt\\ m& =\mathrm{Rand}\left(1,\frac{N-1}{2}\right)\end{array}\text{\hspace{1em}(17)}$$

3. 计算Allan方差，当$m$取不同的值的时候会有不同的Allan方差值。

$${\theta }^2(\tau )=\frac{1}{2{\tau }^2(N-2m)}\sum _{k=1}^{N-2m}({\theta }_{K+2m}-2{\theta }_{K+m}+{\theta }_K{)}^2\text{\hspace{1em}(18)}$$

其中：$\tau =m{\tau }_0$

4. 一般在绘制Allan方差曲线的时候使用的是Allan方差的平方根，将公式（18）开根号。

使用最小二乘对计算出来的Allan方差曲线进行拟合，得到如下的示意图。

按照上表中的值进行对应求得相应的噪声值，因为画出的曲线图是log-log对数图，因此斜率分别就是$\tau$的次幂。

认为这些噪声源是相互独立的，则Allan方差是各类型误差的平方和：
$$\begin{array}{rl}{\sigma }_{\text{ total }}^2(\tau )& ={\sigma }_Q^2(\tau )+{\sigma }_N^2(\tau )+{\sigma }_B^2(\tau )+{\sigma }_K^2(\tau )+{\sigma }_R^2(\tau )\\ & =\frac{3Q^2}{{\tau }^2}+\frac{N^2}{\tau }+\frac{2B^2}{\pi }\ln 2+\frac{K^2\tau }{3}+\frac{R^2{\tau }^2}{2}\end{array}\text{\hspace{1em}(19)}$$
把公式（19）整理下：
$${\sigma }_{\mathrm{t}\mathrm{o}\mathrm{t}\mathrm{a}\mathrm{l}}^2(\tau )=\sum _{i=-2}^2{C}_i{\tau }^i\text{\hspace{1em}(20)}$$
其中：$i=(-2,-1,0,1,2)$

使用最小二乘方法对公式（20）与公式（18）计算出来的值进行拟合，得到$C_i$值，然后得到相应的误差系数：
$$\{\begin{array}{l}Q=\frac{\sqrt{C_{-2}}}{3600\sqrt{3}}\left({}^{\circ }\right)\\ N=\frac{\sqrt{C_{-1}}}{60}\left({}^{\circ }\right)/\sqrt{h}\\ B=\frac{\sqrt{C_0}}{0.664}\left({}^{\circ }\right)/h\\ K=60\sqrt{3C_1}\left[\left({}^{\circ }\right)\cdot h^{-1}\right]/\sqrt{h}\\ R=3600\sqrt{2C_2}\left[\left({}^{\circ }\right)\cdot h^{-1}\right]/h\end{array}\text{\hspace{1em}(21)}$$
其中陀螺仪的数据数据转为 deg/h，$\tau$还是sec单位，加速度计还是m/s² 。

imu_utils其中就是使用ceres求解最小二乘得到相应的系数，公式（18）到公式（21）的过程，注意其中的单位转换。

有一点不明白的是这几个误差系数是单独作用在每一段么？代码中测试发现就是令$\tau$就是求得值。画出来的就是总的Allan方差曲线，为什么拟合出来可以单独代表其中一部分的噪声？公式（19）怎么获得单独的噪声？

找到一种解释：1s 量级的平均时间范围内加速度计的误差主要是速度随机游走, 在 100s 以上的范围内主要是零偏不稳定性. 对于更长的平均时间而言, 限于采集数据的长度, 用于 Allan 方差分析的独立子集个数有限, 因此其置信程度很低。

也就是说它确实是分段的，因此可以直接拟合得到。试了下为什么可以直接赋值1计算，是因为其他的太小了，忽略不计。因为次幂的不同，因此取不同时间时，噪声的大小差别很大，因此就直接忽略了，因此可以看做分段的。

数学模型

加速度计

$${\mathbf{a}}_m^B={\mathbf{S}}_a{\mathbf{T}}_a{\mathbf{R}}_{BG}\left({\mathbf{a}}^G-{\mathbf{g}}^G\right)+{\mathbf{n}}_a+{\mathbf{b}}_a\text{\hspace{1em}(22)}$$

陀螺仪

$${\mathbf{\omega }}_m^B={\mathbf{S}}_g{\mathbf{T}}_g{\mathbf{\omega }}^B+{\mathbf{s}}_{ga}{\mathbf{a}}^B+{\mathbf{n}}_g+{\mathbf{b}}_g\text{\hspace{1em}(23)}$$

$\mathbf{S}$是尺度因子，$\mathbf{T}$是非对称误差，$\mathbf{n}$是高斯白噪声，$\mathbf{b}$是零偏随机游走，${\mathbf{s}}_{ga}$是加速度计对陀螺仪的影响。

VIO中模型

VIO中忽略尺度因子和轴偏差，只考虑白噪声和bias随机游走。
$$\begin{array}{rl}{\tilde{\omega }}^b& ={\omega }^b+{\mathbf{b}}^g+{\mathbf{n}}^g\\ {\tilde{\mathbf{a}}}^b& ={\mathbf{q}}_{bw}\left({\mathbf{a}}^w+{\mathbf{g}}^w\right)+{\mathbf{b}}^a+{\mathbf{n}}^a\end{array}\text{\hspace{1em}(24)}$$
这里的**$g$加号减号**注意就行了，和$g$定义的正负，加速度计输出的加速度正负都有关系。

运动模型

参考预积分或者VINS的笔记就好了。

离散积分方法

$$\begin{array}{rl}{\mathbf{p}}_{w{b}_{k+1}}& ={\mathbf{p}}_{w{b}_k}+{\mathbf{v}}_k^w\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2\\ {\mathbf{v}}_{k+1}^w& ={\mathbf{v}}_k^w+\mathbf{a}\Delta t\\ {\mathbf{q}}_{w{b}_{k+1}}& ={\mathbf{q}}_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\end{array}\text{\hspace{1em}(25)}$$

对于非线性函数有以下的方法。

欧拉法

$$\begin{gathered} {\mathbf{X}}_{n+1}={\mathbf{X}}_n+\mathrm{K}\Delta t \\ K=f^{\prime }(t_n,{\mathbf{X}}_n)\text{\hspace{1em}(26)} \end{gathered}$$

其中公式（24）中
$$\begin{array}{rl}\mathbf{a}& ={\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\\ \mathbf{\omega }& ={\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\end{array}$$

中值法（mid-point)

$$\begin{array}{rl}{\mathbf{X}}_{n+1}& ={\mathbf{X}}_n+{\mathrm{K}}_2\Delta t\\ {\mathrm{K}}_2& =f^{\prime }(t_n+\frac{1}{2}\Delta t,{\mathbf{X}}_n+{\mathrm{K}}_1\frac{\Delta t}{2})\\ {\mathrm{K}}_1& =f^{\prime }(t_n,{\mathbf{X}}_n)\end{array}\text{\hspace{1em}(27)}$$

其中公式（24）中的
$$\begin{array}{rl}\mathbf{a}& =\frac{1}{2}\left[{\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w+{\mathbf{q}}_{w{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\right]\\ \mathbf{\omega }& =\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]\end{array}$$

四阶龙格库塔（RK4）

龙格库塔法的微分方程中必须有原来的量，即$\dot{y}=f(t,y)$形式才适用。
$$\begin{array}{rl}{\mathbf{X}}_{n+1}& ={\mathbf{X}}_n+\frac{\Delta t}{6}({\mathrm{K}}_1+2{\mathrm{K}}_2+2{\mathrm{K}}_3+{\mathrm{K}}_4)\\ {\mathrm{K}}_1& =f^{\prime }(t_n,{\mathbf{X}}_n)\\ {\mathrm{K}}_2& =f^{\prime }(t_n+\frac{\Delta t}{2},{\mathbf{X}}_n+\frac{\Delta t}{2}{\mathrm{K}}_1)\\ {\mathrm{K}}_3& =f^{\prime }(t_n+\frac{\Delta t}{2},{\mathbf{X}}_n+\frac{\Delta t}{2}{\mathrm{K}}_2)\\ {\mathrm{K}}_4& =f^{\prime }(t_n+\Delta t,{\mathbf{X}}_n+\Delta t{\mathrm{K}}_3)\end{array}\text{\hspace{1em}(28)}$$
其中公式（24）中的
$$\begin{array}{rl}\mathbf{a}& =\frac{1}{2}\left[{\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w+{\mathbf{q}}_{w{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\right]\end{array}$$

template 
 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 coefficient
  computeOmegaSkew( omega0, omega_skew );
  k1 = _T(0.5)*omega_skew*quat;
  // Second Runge-Kutta coefficient
  tmp_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 coefficient
  tmp_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);
}

总的标定流程

以IMU_tk为例，直接用本科毕设论文里的吧，有些符号可能不一样，比如考虑陀螺仪轴和加速度计轴不垂直、不重合的轴向误差。