MPU6050加速度转角度原理

MPU6050加速度转角度原理

最近一直在研究四轴飞行器的姿态解算，每次看完过一段时间就忘记了，所以把每个部分写成博客记录下来，方便自己查阅，也加深印象。这里只讨论加速度与角度的转换，陀螺仪与姿态融合这里不讨论。

加速度计顾名思义就是用来测量物体的加速度值，我们可以通过上面这个图片通俗的了解一下3轴加速度计，上图用一个盒子形状的立方体来做模型

如果这个盒子形状的立方体处于一个没有引力场的地方，球会保持在盒子中间，就像人在外太空处于一个漂浮的状态一样，那么在这样一种状态下，这个盒子形状的立方体六个壁面感受到的压力都是0，但是如果我们突然将立法体像左侧移动（我们加快加速，产生1g的值，1g = 9.8米/秒平方），这是灰球打在了X-壁面上，X+和X-这两个壁面就相当于三维坐标系中的X轴，此时灰球打在X-面上产生了-1g的压力值。如果反过来向右侧移动球打在X+壁面则产生+1g的压力值（图片如下）

那么如果我们把这个盒子放在地球上，那么灰球会落在Z-这个壁面上，并为它施加一个1g的加速度压力（因为地球的重力加速的规定为1g），这种情况下，我们不动这个盒子，但任然可以在Z-这个壁面上读到-1g加速度值，如下图

至此我们已经知道了单个轴上加速度输出的由来，下面我们来分析三轴的，三轴加速度计真正的价值是它可以同时检测到三个轴上的惯性力，看下面这个图片，我们将这个盒子翻滚45度，这时球会触及到两个壁面：Z-和X-

此时X和Z轴都受到了-0.71g的加速度压力，那么这个-0.71这个值是不是任意的呢？并不是，它们实际上是一个近似SQRT(1/2)。要知道当盒子只受重力场影响时，各个轴上的重力加速度平方和为1g，也就是
X平方 + Y平方+ Z平方 = 1g（勾股定理），为什么是SQRT(1/2),因为是翻滚了45°，此时只有X轴和Z轴收到了重力加速度的影响，而且是各占一半，所以是1/2。

看上面这个模型，这是一个新的模型代替了上面那个盒子立方体。试想一下，在新的模型中每个轴都是垂直于盒子壁面的。向量R是加速度计测量的力矢量。接受时，Rx,Ry,Rz,为R在X，Y，Z上的矢量投影他们的关系为：RX平方 + RY平方+ RZ平方 = R平方
可以验证：（-SQRT(1/2)）^2 + 0 ^2 + (-SQRT(1/2)) ^2 = 1g
通过简单的迭代以 R = 1时，接收 Rx = SQRT(1/2)时，Ry = 0，Rz = SQRT(1/2)
经过以上的理论分析，值Rx,Ry,Rz,实际上是呈线性关系对应到真正的现实加速度计的X轴 Y轴 Z轴

在此提出一个问题,加速度计如何将这些信息告诉我们?
目前市面上的加速度计从输出上区分为两种,一种是数字的,另一种是模拟的. MINIAHRS使用的是MPU6050三轴加速度计,是12C接口的数字传感器.通过特定的命令可以配置加速度的量程,并将内部ADC的转换结果读出来现在,我们有我们的加速度计的读数,以LSB为单位的,它仍然不是g(9.8米/秒平方),需要最后的转换,我们要知道加速度计灵敏度,通常表示为LSB/g。比方说当我们选择2g的量程时,对应的灵敏度=16384LSB/G。为了得到最终的力值,单位为g,我们用下面的公式RX= ADCRX/灵敏度也就是说当x轴的计数为 ADCRX时,那么对应的加速度值就是( ADCRX/16384)g

回到加速度向量模型，将角度符合补上，如下图

方向余弦
我们感兴趣的是向量R和X、Y、Z轴之间的角度,将它们定义为 Axr Ayr Azr.可以看到由R和Rx组成的直角三角形：
COS(Axr)=RX/R依此类推：
COS(Ayr)=RY/R
COS (AZ)=RZ/R
可以得到R=SQRT(RX平方+RY平方+RZ平方)
我们发现当使用 arccos()反余弦：
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
这三个公式通常被称为方向余弦。你可以轻松地验证：
SQRT(cosX平方 + cosY平方 + cosZ平方) = 1；
这个属性可以让我们在计算时不用担心R的模长的影响，很多时候我们只对惯性矢量的方向感兴趣，对矢量进行规范化对简化程序运算很有意义

重力向量
我们暂且从理论分析回到现实的传感器输出中,当水平放置MPU6050,只有Z轴感受到重力向量,它将输出1g。对应的ADC值就是16384(29的量程)。此时,R就是重力向量Rx=0. Ry=0. Rz = R
满足
R平方 = RX平方 +RY平方 + RZ平方 得到重力向量与各个轴的夹角
Axr arccos(RX/R)=90度
Ayr=arccos(RY/R)=90度
Azr= arccos(RZ/R)=0度