简单理解VIO(二)

本章介绍IMU传感器
本文涉及的代码见： Github。

一、旋转运动学

• 1、线速度与角速度
  粒子在$z=0$的平面运动（假设全局坐标系为家里的房间, 场景与简单理解VIO（一）中的照片类似），以$a=1$为半径的圆上进行圆周运动，坐标为：$r=(\cos \theta ,\sin \theta ,0)$.
  对$\theta$求导得：
  $$\dot{r}=[-\dot{\theta }\sin \theta ,\dot{\theta }\cos \theta ,0{]}^T=[\begin{array}{ccc}0& -\dot{\theta }& 0\\ \dot{\theta }& 0& 0\\ 0& 0& 0\end{array}][\begin{array}{c}\cos \theta \\ \sin \theta \\ 0\end{array}]$$
  其中，$\dot{\theta }为角速度w$，后面写成的矩阵形式只是为了将偏导数与角度分开。
• 2、旋转坐标系下的运动学
  站立面朝小车，人为body坐标系，假设此时全局坐标系即为0时刻人的坐标系，body坐标系匀速逆时针旋转1°/s. 在全局中，小车静止不动.在body坐标系下，小车的坐标：
  $$r_B=[\cos \theta ,-\sin \theta ,0]$$
  在全局坐标系下，小车的坐标等于
  $$r_I=R_{IB}{r}_B$$
  因为body本身在旋转，因此$R_{IB}$也是变化的, 用四元数表示为$q_{IB}=[\cos (\theta /2),[0,0,1]\sin (\theta /2)]$, $r_B$也是变化的（理论上是变化的，这里为了简单前面说小车相对静止）. 如果对上式求偏导，可以得到
  $$\dot{r_I}=R_{IB}\dot{r_B}+\dot{R_{IB}}{r}_B$$
  这里具体公式并不重要，但是有一个比较重要的结论：body坐标系下，质量块的速度或者角速度，转换到全局坐标系下去，不是直接$R_{IB}\dot{r_B}$, 还受到了其他坐标系本身的影响.
  再求二阶导，就可以得到质量块加速度之间的关系。此时多出来的部分，就是科氏力，离心力。这个地方用得最广的是障碍物运动估计，如：无人车的姿态为$R_{IB}$, 在该坐标系下观察到前方车辆的速度$\dot{r_B}$，在障碍物建模过程中，我们需要求的是障碍物在全局坐标系下的速度$\dot{r_I}$

二、IMU测量模型及运动模型

1、MEMS加速度计工作原理

加速度测量最基本的原理是: $a=f/m$. 即是牛顿第二定律: 加速度与作用力成正比，与物体质量成反比，那即可通过测量力，质量一般可以提前测得，得出加速度大小。
常见的测量力的方式：

1. 弹簧
2. 电容电阻

2、陀螺仪测量原理

常见的测量方式有两种：

1. 光纤陀螺
2. 震动陀螺

一般用的MEMS器件，都是震动陀螺。有两个关键点：

1. 通过测量科氏力，求得角速度的。
2. 通过两个震动音叉，抵消其他力的影响。

三、IMU误差模型

误差分为两类：确定性误差(bias, scale, misalign)，随机游走误差(噪声)。

1、六面法标定加速度

当考虑二维情况时，只需要对xy轴进行处理。

1.1、bias与scale

水平静止放置四面就可以得到bias，scale.

1.2、轴间误差

水平放置四个面的时候，加速度的理论值可以知道，这样就可以联立方程组，构建误差方程
$$y=z-h(x)$$
其中，x是bias, scale, 轴间误差等。这里利用的是重力加速度，其实也可以使用水平方向加速度为0来处理，可能精度会略低一些，此时就只用放置一面就可以了。即：“一面法标定角速度计”, 如下：
$$[\begin{array}{c}0\\ 0\end{array}]=[\begin{array}{c}{s}_x,m_{xy}\\ m_{yx},s_y\end{array}][\begin{array}{c}{a}_x\\ a_y\end{array}]+[\begin{array}{c}{b}_x\\ b_y\end{array}]$$
一共6个变量，观测3组实验数据即可求解。

四、运动模型离散时间处理

中间的桥梁是泰勒展开

五、IMU数据仿真

1. 地图生成

把自己家房子做了个如下的结构图，生成了一个简单的地图：

2. 速度生成

位置对于时间的导数，就是速度。
$$x=m(t+n)$$
$$y=c\sin (ax+b)+d$$
其中m,n,a,b,c,d是常数.
$$x^{\prime }=m$$
$$y^{\prime }=c\cos (ax+b)a{x}^{\prime }=a\ast c\ast m\ast \cos (ax+b)$$

3. 加速度生成

位置对于时间的二阶导数，就是加速度。
$$x^{\prime \prime }=0$$ $$y^{\prime \prime }=-a\ast c\ast m\ast \sin (ax+b)a{x}^{\prime }=-a^2\ast c\ast m^2\sin (ax+b)$$
这是全局坐标系下的加速度 $a^G=[x^{\prime \prime },y^{\prime \prime }{]}^T$，需要将其转换为局部坐标系：
$$R^T{a}^G=a^L$$

4. 角速度生成

角度对于时间的导数，就是角速度。斜率k与角度之间的关系
$$k=\tan (\theta )$$
而k就是y对于x的导数：
$$k=a\ast c\ast \cos (ax+b)$$
因此：$\theta =\arctan (k)$
$${\theta }^{\prime }=-\frac{1}{1+k^2}{a}^{2}cm\sin (ax+b)$$

六、代码

1. 生成IMU的数据
2. 利用KF进行积分，得到轨迹
   简单制作了一个GUI，使用Matlab可以运行，代码

七、参考文献

1: 第二节IMU传感器