惯性测量单元IMU基础

IMU由三部分组成：加速度计、磁力计、陀螺仪。
加速度计就是测量三个方向的加速度：

磁力计就是测量磁场北极的方位：

陀螺仪测量的就是绕三个方向旋转速度：

一、加速度计

1.1 一维模型

加速度计的一维模型类似于弹簧质量模型，如下：

静态下，达到平衡：

这个时候加速度计测量到的加速度为重力加速度：

如果在运动的情况下：

则平衡方程变为：

这个时候加速度计测量的加速度为重力加速度和运动加速度之和：

按照这个分析，加速度计在自由落体时测量到的加速度为零。
如果想要通过加速度计测量得到运动加速度，那么应该将测量到的加速度减去重力加速度：

即：

加上硬件的测量误差：

积分得到：

从而可以计算运动的位移：

但是通过这种方式测量位移，误差会随着时间急剧增加。

1.2 二维或三维加速度计

当加速度计的y轴与重力方向重合时，二维加速度计坐标系如下：

那么在x、y方向测量的加速度分别如下：

但是通常情况下，y轴与重力方向不重合，如下图所示：

这个时候测量得到的x、y轴加速度分别如下：

需要知道方位角度θ，才能减去重力项，从而得到我们真正想要的运动加速度。
如果方位角度不准确，不仅不会消除重力，还会引入误差。

以手机为例，假设手机处于静止状态，那么：

则由上述公式可得：

加速度计就变成了一个角度传感器。这时需要加速度计处于静止状态下去估算旋转角度。但是如果需要测量三维旋转，仅靠加速度计是不够的。
加速度计还可以用来测量距离。
如果通过对加速度进行双重积分的方式估算距离，会导致严重的测量错误。

其中n表示测量误差，误差会随着时间的增加大量积累，导致估算的距离严重错误。
然而由于加速度计可以很好的跟踪步态，所以可以根据步数和步长，再加上罗盘测量的方位，就可以估算出距离：
Distance = step_count * step_size

加速度计总结

测量重力加速度和直线加速度；
2D情况下，在静止状态下，测量到的重力可以用于估算旋转角度；
通过对加速度进行双重积分来估算距离，会导致误差严重积累；而用加速度计计算步数比较准确；

二、磁力计

2.1 二维磁力计

在2D情况下，如图所示，当磁场北极刚好处于磁力计y轴，则磁力计测量值为：

如果磁力计的坐标偏转了一个方向θ，如图所示，则磁力计测量得到的值为：

在3D情况下也是同样的原理，3D磁力计的测量输出值取决于旋转角度。
在2D情况下，磁力计可以用于估算旋转角度：

其中，Mx是磁力计x轴的测量输出值，M是磁场北极强度，是已知值。
然而，在3D情况下，单靠磁力计不足以估算三维旋转角度。

2.2 三维旋转

如下图所示，在三维情况下，X-Y-Z表示磁力计本地坐标系，East-North-Up表示的是世界坐标系，三维旋转就是测量的世界坐标系和磁力计本地坐标系之间的旋转角度关系。

下图表示了，三维旋转矩阵的含义，三维旋转矩阵可以充分的表示世界坐标和本地坐标之间的旋转关系。

现在的问题是如何来测量这个3X3旋转矩阵？

通过重力和磁场确定旋转矩阵
最关键的一点是要使用全局已知的参考向量，并且能在本地坐标系中测量到。有两个向量可以做到：重力和磁场北极。
如果采用重力向量，那么根据加速度传感器可以得到如下公式：

如果采用磁场北极向量，那么根据磁力计可以得到如下公式：

由上面两组公式可以得到6个方程，但是有9个未知数（33旋转矩阵R）。这9个未知数并非都是独立变量，旋转矩阵的每一列都是单位正交向量。所以根据上面两组公式足以解算出旋转矩阵R。
结论就是：可以用加速度计和磁力计确定旋转矩阵。
由yaw, pitch, roll确定旋转矩阵
如下式所示，33的旋转矩阵可以用三维的yaw， pitch, roll来表示，如图所示。

所以通过估计yaw, pitch, roll可以确定方向。
加速度计方程
加速度计测量的加速度方程如下：

其中ax, ay, az是由加速度计测量输出的各方向的加速度。也可以写成：

需要注意的是，加速度计的测量输出与yaw角无关，所以上式应该写成：

所以通过加速度计无法估算yaw角度。
上式继续写成：

于是通过这个方程就可以估算出pitch和roll。
磁力计方程
磁力计方程如下：

也可以写成这样的形式：

注意，pitch和roll通过加速度计已经确定了。于是就可以通过上式确定yaw的大小。

知道了yaw, pitch, roll的值，就能确定三维旋转矩阵，也就是三维方向。

三、陀螺仪（Gyroscope）

陀螺仪是用来测量角速度的，通过对角速度进行积分就可以知道角度。

3.1 一维的情况

陀螺仪测量的角速度为：

也就是说：

但是测量是有误差的，加上误差得到的方程为：

两边进行积分，就可以得到测量的角度：

可以明显看出来，误差累计非常严重，如图所示：

3.2 用陀螺仪估算三维旋转

陀螺仪可以测量三维三个方向上的旋转角速度，通过转换就能得到两个时刻之间的相对旋转矩阵，于是可以按这种思路通过陀螺仪估算三维旋转矩阵：

如果只用陀螺仪对旋转矩阵进行估算的话，需要一系列这样的方程：

其中绿色部分是陀螺仪的测量结果，要注意，每次的测量结果都是有误差的。于是最终的旋转矩阵测量结果误差会随着时间推移而积累。

四、两种测量旋转矩阵的方法总结

4.1 通过陀螺仪

测量误差会随着时间积累：

4.2 通过加速度计和磁力计

最大的优点就是不会有累积误差，因为不需要积分。
缺点是，加速度计测量重力时，会被线性运动给破坏掉：

所以用加速度计只能在静态下测量旋转矩阵。
另外一个问题是，磁力计容易受到磁场干扰。

鉴于上述两种方法各有优劣，是否能用加速度计和磁力计来校正陀螺仪的累积误差呢？答案是可以的。

通常使用陀螺仪测量旋转矩阵，会有误差积累导致测量结果漂移。
在静态以及没有磁场干扰的情况下，通过加速度计和磁力计将误差进行一次reset，就可以减少误差累计而导致测量结果漂移的情况，如图所示。