Mpu6050 工作原理 一、加速度和陀螺仪原理 加速度计主要是测量物体运动的加速度，陀螺仪主要测量物体转动的角速度。 二、加速度测量 在开始之前，不知大家是否还记得加速度具有合成定理？如果不记得可以先大概了解 一下，其实简单的举个例子来说就是重力加速度可以理解成是由 x,y,z 三个方向的加速度共同作用 的结果。反过来说就是重力加速度可以分解成 x,y,z 三个方向的加速度。 加速度计可以测量某一时刻 x,y,z 三个方向的加速度值。而自平衡小车利用加速度计测出重力 加速度在 x,y,z 轴的分量，然后利用各个方向的分量与重力加速度的比值来计算出小车大致的倾角。 其实在自平衡小车上非静止的时候，加速度计测出的结果并不是非常精确。因为大家在高中物理的 时候都学过，物体时刻都会受到地球的万有引力作用产生一个向下的重力加速度，而小车在动态时， 受电机的作用肯定有一个前进或者后退方向的作用力，而加速度计测出的结果是，重力加速度与小 车运动加速度合成得到一个总的加速度在三个方向上的分量。 不过我们暂时不考虑电机作用产生的运动加速度对测量结果的影响。因为我们要先把复杂的 事情分解成一个个简单的事情来分析，这样才能看到成果，才会有信心继续。 下边我们就开始分析从加速度得到角度的方法。如下图，把加速度计平放，分别画出 xyz 轴 的方向。这三个轴就是我们后边分析所要用到的坐标系。如图一 

把 mpu6050 安装在自平衡车上时也是这样的水平安装在小车底盘上的，假设两个车轮安装时车轴和 y 轴在一条直线上。那么小车摆动时，参考水平面就是桌面，并且车轴（y 轴）与桌面始终是平行的， 小车摆动和移动过程中 y 轴与桌面的夹角是不会发生变化的，一直是 0 度。发生变化的是 x 轴与桌面 的夹角以及 z 轴与桌面的夹角，而且桌面与 x 轴 z 轴夹角变化度数是一样的。所以我们只需要计算出 x 轴和 z 轴中任意一个轴的夹角就可以反映出小车的倾斜的情况了。如图二 为了方便分析，由于 y 轴与桌面夹角始终不变，我们从 y 轴的方向俯看下去，那么这个问题 就会简化成只有 x 轴和 z 轴的二维关系。假设某一时刻小车上加速度计（mpu6050）处于如下 状态，下图是我们看到简化后的模型。如图三 

在这个图中，y 轴已经简化和坐标系的原点 o 重合在了一起。我们来看看如何计算出小车 的倾斜角，也就是与桌面的夹角 a。（上图 g 是重力加速度，gx、gz 分别是 g 在 x 轴和 z 轴的 分量。） 由于重力加速度是垂直于水平面的，得到：∠a+∠b=90°，∠b+∠d=90°于是轻 松的就可以得出：∠a=∠d。 所以计算出角度 d 就等效于计算出了 x 轴与桌面的夹角 a。根据正弦定理得出：sin（d）=gx/g。 得到这个公式可是还是得不到想要的角度，因为需要计算反正弦，而反正弦在单片机里不是 很好计算。 为了得到角度，于是又查了相关资料，原来在角度较小的情况下，角度的正弦与 角度对应的弧度成线性关系。如图四： 

这个图 x 轴是角度，取值范围是 0~90 度，有三个函数曲线，分别是： y = sin(x) y = x*π/180 正弦曲线 弧度 y = 0.92*x*π/180 乘以一个 0.92 系数的弧度 从图上可以看出，当角度范围是 0~29 度时，对于自平衡车来说摆动范围在-29~29 度之内 sinx = x*3.14/180 可用来代替 y = sinx；如果超过这个范围，小车姿态也无法调整。当然有时候 也会担心-29~29 度的摇摆范围还是无法满足需求，那可以给上边的公式乘一个系数，得到如 下公式：sinx = k*x*3.14/180。从上边的函数对比图可以看出，当系数取 0.92 时，角度范 围可以扩大到-45~45 度。 经过这一系列的分析，终于得到角度换算方法： 由 sin(d) = gx/g sin(d) = k*d*3.14/180 ​ 得到： gx/g = k*d*3.14/180 那么角度就可以通过如下公式计算出： d = 180*gx/(k*g*3.14) 而 gx 可以从加速度计里读出来，所以这下角度就可以轻松得到了。而之前也说过这个角度不 是很精确，但是至少可以反映出角度变化的趋势。不过可以通过卡尔曼滤波等算法把加速度 计读出的角度和陀螺仪读出的角度结合起来，使小车的角度更加准确。 三、陀螺仪 通过陀螺仪来测量角度就很简单了，因为陀螺仪读出的是角速度，大家都知道， 角速度乘以时间，就是转过的角度。把每次计算出的角度做累加就会等到当前所在位置的角 度。如图五 

假设最初陀螺仪是与桌面平行，单片机每 t ms 读一次陀螺仪的角速度，当读了三次角速度以 后 z 轴转到上图的位置，则在这段时间中转过的角度为 a： ∠a = ∠1+∠2+∠3 假设从陀螺仪读出的角速度为 w，那总角度为： ∠a = (w1*t1+w2*t2+w3*t3)/1000 假设经过 n 次，那么总的角度如下： ∠a = (w1*t1+w2*t2+w3*t3+…+w(n)*t(n))/1000 实际上这就是一个积分过程。其实这种计算出来的角度也存在一定的误差，而且总的角度是 经过多次相加得到的，这样误差就会越积累越大，最终导致计算出的角度与实际角度相差很 大。于是也可以使用卡尔曼滤波把加速度计读出的角度结合在一起，使计算出的角度更准确。