控制算法|惯性测量单元IMU基础 C|数字图像处理

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 一维的情况
【控制算法|惯性测量单元IMU基础】陀螺仪测量的角速度为：