MPU6050学习笔记（1）：初识IMU

IMU（Inertial measurement unit），中文翻译为惯性测量单元，是一种能够测量物体三轴加速度和角速度进而解算出物体姿态的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。为了提高可靠性，还可以为每个轴配备更多的传感器，例如地磁传感器。
MPU6050属于IMU的一种，包含了三个单轴的加速度计和三个单轴的陀螺仪。
本文将简单介绍加速度计和陀螺仪，以及如何利用他们的数据进行姿态解算。
第一部分：加速度计
我们可以把加速度计想象成一个圆球在一个方盒里面，虽然这个模型并非一个MEMS传感器的真实构造，但用它来解释与加速度计相关的问题相当有效。
假设这个盒子不在重力场中，圆球处于盒子的正中央，一切东西都处于无重力状态，如下图所示：

image.png

在上面的图中你可以看到我们给每个轴分配了一对墙（我们移除了Y+以此来观察里面的情况）。设想每面墙都能感测压力。如果我们突然把盒子向左移动（加速度为1g=9.8m/s^2），那么球会撞上X-墙。然后我们检测球撞击墙面产生的压力，X轴输出值为-1g，如下图：

image.png

请注意加速度计检测到得力的方向与它本身加速度的方向是相反的。在这个模型中你应该知道了加速度计是通过间接测量力对一个墙面的作用来测量加速度的，在实际应用中，可能通过弹簧等装置来测量力。这个力可以是加速度引起的，但在下面的例子中，我们会发现它不一定是加速度引起的。如果我们把模型放在地球上，球会落在Z-墙面上并对其施加一个1g的力，如下图：

image.png

在这种情况下盒子没有移动但我们任然读取到Z轴有-1g的值。球在墙壁上施加的压力是由引力造成的。引用这个例子只是为了说明加速度计的本质是检测力而非加速度。只是加速度所引起的惯性力正好能被加速度计的检测装置所捕获。
到目前为止，我们已经分析了单轴的加速度计输出，这是使用单轴加速度计所能得到的。三轴加速度计的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型，并将盒子向右旋转45度。现在球会与两个面接触：Z-和X-，见下图：

image.png

0.71g这个值是不是任意的，它们实际上是1/2的平方根的近似值。我们介绍加速度计的下一个模型时这一点会更清楚。
在上一个模型中我们引入了重力并旋转了盒子。在最后的两个例子中我们分析了盒子在两种情况下的输出值，力矢量保持不变。虽然这有助于理解加速度计是怎么和外部力相互作用的，但如果我们将坐标系换为加速度的三个轴并想象矢量力在周围旋转，这会更方便计算。

image.png

请看看在上面的模型，我保留了轴的颜色，以便你的思维能更好的从上一个模型转到新的模型中。想象新模型中每个轴都分别垂直于原模型中各自的墙面。矢量R是加速度计所检测的矢量（它可能是重力或上面例子中惯性力的合成）。RX，RY，RZ是矢量R在X，Y，Z上的投影。请注意下列关系：
R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2（公式1）
此公式等价于三维空间勾股定理。
还记得我刚才说的1/2的平方根0.71不是个随机值吧。如果你把它们代回上式，回顾一下重力加速度是1g，那我们就能验证：1 ^ 2 =（SQRT（1/2））^ 2 + 0 ^ 2 +（SQRT（1/2））^ 2在公式1中简单的取代： R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2)。
经过一大段的理论序言后，我们和实际的加速度计很靠近了。实际中，我们可以从加速度计中获得与RX，RY，RZ线性相关的值，至于如何转化成RX，RY，RZ，不同的仪器方法可能不同，这边不一一细讲。
在我们得到了惯性力矢量的三个分量，如果设备除了重力外不受任何外力影响，那我们就可以认为这个方向就是重力矢量的方向。如果你想计算设备相对于地面的倾角，可以计算这个矢量和Z轴之间的夹角。如果你对每个轴的倾角都感兴趣，你可以把这个结果分为两个分量：X轴、Y轴倾角，这可以通过计算重力矢量和X、Y轴的夹角得到。让我们回到我们的上一个加速度模型，再加一些标注上去：

image.png

我们感兴趣的角度是向量R和X，Y，Z轴之间的夹角，那就令这些角度为Axr，Ayr，Azr。观察由R和Rx组成的直角三角形：
cos(Axr) = Rx / R , 类似的：
cos(Ayr) = Ry / R
cos(Azr) = Rz / R
从公式1我们可以推导出 R = SQRT( Rx^2 + Ry^2 + Rz^2)
通过arccos()函数（cos()的反函数）我们可以计算出所需的角度：
Axr = arccos(Rx/R)
Ayr = arccos(Ry/R)
Azr = arccos(Rz/R)
我们再介绍几个很常用的公式：
cosX = cos(Axr) = Rx / R
cosY = cos(Ayr) = Ry / R
cosZ = cos(Azr) = Rz / R
这三个公式通常称作方向余弦，它主要表达了单位向量（长度为1的向量）和R向量具有相同的方向。你可以很容易地验证：SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1这是个很好的性质，因为它避免了我们一直检测R向量的模（长度）。通常如果我们只是对惯性力的方向感兴趣，那标准化模长以简化其他计算是个明智的选择。

第二部分：陀螺仪

image.png

陀螺仪的每个通道检测一个轴的旋转。例如，一个2轴陀螺仪检测绕X和Y轴的旋转。为了用数字来表达这些旋转，我们先引进一些符号。首先我们定义：
Rxz – 惯性力矢量R在XZ平面上的投影
Ryz – 惯性力矢量R在YZ平面的上投影
在由Rxz和Rz组成的直角三角形中，运用勾股定理可得：
Rxz^2 = Rx^2 + Rz^2 ，同样：
Ryz^2 = Ry^2 + Rz^2
同时注意：
R^2 = Rxz^2 + Ry^2 ，这个公式可以公式1和上面的公式推导出来，也可由R和Ryz所组成的直角三角形推导出来
R ^ 2 = Ryz ^ 2 + RX ^ 2
相反，我们按如下方法定义Z轴和Rxz、Ryz向量所成的夹角：
AXZ - Rxz（矢量R在XZ平面的投影）和Z轴所成的夹角
AYZ - Ryz（矢量R在YZ平面的投影）和Z轴所成夹角
现在我们离陀螺仪要测量的东西又近了一步。陀螺仪测量上面定义的角度的变化率。换句话说，它会输出一个与上面这些角度变化率线性相关的值。为了解释这一点，我们先假设在t0时刻，我们已测得绕Y轴旋转的角度（也就是Axz），定义为Axz0，之后在t1时刻我们再次测量这个角度，得到Axz1。角度变化率按下面方法计算：
RateAxz = (Axz1 – Axz0) / (t1 – t0).
如果用度来表示角度，秒来表示时间，那这个值的单位就是 度/秒。这就是陀螺仪检测的东西。
同样，实际中陀螺仪不会直接给出三轴角速度的值而是与三轴角速度线性相关的值，转化因仪器不同而不同，这里也不细讲。
得到了角速度，再根据采样频率，我们就能算出物体从上一时刻到这一时刻旋转的角度，从而解算出物体的姿态。

第三部分：为什么要有同时具备加速度计和陀螺仪。
看了上面的介绍，我们发现单单通过加速度计或者陀螺仪就能解算出物体姿态，为什么IMU要同时具备两种功能相似的仪器呢？
仔细看第一部分会发现，我们所举的例子都是在不受力或者只受重力的情况下，当物体在加减速的情况下，三轴的加速度所组合的加速度的方向并不在重力加速度的方法上，此时单纯利用加速度计进行姿态解算会有极大的误差。
而陀螺仪进行姿态解算主要使用的是角速度x时间，但是这个角速度只能表示采样时的角速度，所以计算出来的旋转角度是两次采样间旋转角度的近似值，陀螺仪是通过对旋转角度的累加进行姿态解算，所以长时间工作会有极大的误差。
那这样说IMU其实也无法解算出准确的姿态？非也！
通过上面的分析我们可以发现，加速度计短时间内精度较差，而长时间稳定性好，陀螺仪短时间内精度高，但会累计误差，只要将两者的数据进行互补，就能解算出正确的姿态。
下一节我们将介绍一些数据互补的算法。