一、MPU6050的原理分析。

要想知道MPU6050工作原理，得先了解下面俩个传感器：

①陀螺仪传感器：

陀螺仪的原理就是，一个旋转物体的旋转轴所指的方向在不受外力影响时，是不会改变的。人们根据这个道理，用它来保持方向。然后用多种方法读取轴所指示的方向，并自动将数据信号传给控制系统。我们骑自行车其实也是利用了这个原理。*转得越快越不容易倒，因为车轴有一股保持水平的力量。现代陀螺仪可以精确地确定运动物体的方位的仪器，它在现代航空，航海，航天和国防工业中广泛使用的一种惯性导航仪器。传统的惯性陀螺仪主要部分有机械式的陀螺仪，而机械式的陀螺仪对工艺结构的要求很高。70年代提出了现代光纤陀螺仪的基本设想，到八十年代以后，光纤陀螺仪就得到了非常迅速的发展，激光谐振陀螺仪也有了很大的发展。光纤陀螺仪具有结构紧凑，灵敏度高，工作可靠。光纤陀螺仪在很多的领域已经完全取代了机械式的传统的陀螺仪，成为现代导航仪器中的关键部件。光纤陀螺仪同时发展的除了环式激光陀螺仪外。

②加速度传感器：

加速度传感器是一种能够测量加速度的传感器。通常由质量块、阻尼器、弹性元件、敏感元件和适调电路等部分组成。传感器在加速过程中，通过对质量块所受惯性力的测量，利用牛顿第二定律获得加速度值。根据传感器敏感元件的不同，常见的加速度传感器包括电容式、电感式、应变式、压阻式、压电式等。

其实说简单点，在mpu6050中我们用陀螺仪传感器测角度，用加速度传感器测加速度。

  

   MPU-60X0 :

            MPU-60X0是全球首例9轴运动处理传感器。它集成了3轴MEMS陀螺仪， 3轴MEMS 加速度计，以及一个可扩展的数字运动处理器 DMP（DigitalMotion Processor），可用 I2C 接口连接一个第三方的数字传感器，比如磁力计。扩展之后就可以通过其 I2C 或 SPI 接口 输出一个 9 轴的信号（SPI 接口仅在 MPU-6000 可用）。MPU-60X0 也可以通过其 I2C 接口 连接非惯性的数字传感器，比如压力传感器。 

                 

          MPU-60X0 对陀螺仪和加速度计分别用了三个 16 位的 ADC，将其测量的模拟量转化 为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的， 陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4， ±8，±16g。 一个片上 1024 字节的 FIFO，有助于降低系统功耗。 和所有设备寄存器之间的通信采用 400kHz 的 I2C 接口或 1MHz 的 SPI 接口（SPI 仅 MPU-6000 可用）。对于需要高速传输的应用，对寄存器的读取和中断可用 20MHz 的 SPI。 另外，片上还内嵌了一个温度传感器和在工作环境下仅有±1%变动的振荡器。 芯片尺寸 4×4×0.9mm，采用 QFN 封装（无引线方形封装），可承受最大 10000g 的冲 击，并有可编程的低通滤波器。 关于电源，MPU-60X0 可支持 VDD 范围 2.5V±5%，3.0V±5%，或 3.3V±5%。另外 MPU-6050 还有一个 VLOGIC 引脚，用来为 I2C 输出提供逻辑电平。VLOGIC 电压可取 1.8±5%或者 VDD。

          数字运动处理器（DMP）:

 DMP 从陀螺仪、加速度计以及外接的传感器接收并处理数据，处理结果可以从 DMP 寄存器读出，或通过 FIFO 缓冲。DMP 有权使用 MPU 的一个外部引脚产生中断。

2、数据传输:

        如果要写 MPU-60X0 寄存器，主设备除了发出开始标志（S）和地址位，还要加一个 R/W 位，0 为写，1 为读。在第 9 个时钟周期（高电平时），MPU-60X0 产生应答信号。然 后主设备开始传送寄存器地址（RA），接到应答后，开始传送寄存器数据，然后仍然要有应 答信号，依次类推。

单字节写时序：

多字节写时序：

如果要读取 MPU-60X0 寄存器的值，首先由主设备产生开始信号（S），然后发送从设 备地址位和一个写数据位，然后发送寄存器地址，才能开始读寄存器。紧接着，收到应答信 号后，主设备再发一个开始信号，然后发送从设备地址位和一个读数据位。然后，作为从设 备的 MPU-60X0 产生应答信号并开始发送寄存器数据。通信以主设备产生的拒绝应答信号 （NACK）和结束标志（P）结束。拒绝应答信号（NACK）产生定义为 SDA 数据在第 9 个 时钟周期一直为高。 

二、MPU6050姿态融合

姿态角（Euler角）pitch yaw roll
飞行器的姿态角并不是指哪个角度，是三个角度的统称。
它们是：俯仰、滚转、偏航。你可以想象是飞机围绕XYZ三个轴分别转动形成的夹角。

地面坐标系（earth-surface inertial reference frame）Sg--------OXgYgZg
<ignore_js_op> 
①在地面上选一点Og
②使Xg轴在水平面内并指向某一方向
③Zg轴垂直于地面并指向地心(重力方向)
④Yg轴在水平面内垂直于Xg轴，其指向按右手定则确定

机体坐标系(Aircraft-body coordinate frame)Sb-------OXYZ
<ignore_js_op> 

①原点O取在飞机质心处，坐标系与飞机固连
②x轴在飞机对称平面内并平行于飞机的设计轴线指向机头
③y轴垂直于飞机对称平面指向机身右方
④z轴在飞机对称平面内，与x轴垂直并指向机身下方

欧拉角/姿态角（Euler Angle）
<ignore_js_op> 
<ignore_js_op> 

机体坐标系与地面坐标系的关系是三个Euler角，反应了飞机相对地面的姿态。
俯仰角θ（pitch）：机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
<ignore_js_op> 

偏航角ψ（yaw）：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。
<ignore_js_op> 

滚转角Φ（roll）：机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。
<ignore_js_op>

 

 

首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。

一、四元数法

关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。

        虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的（http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735），DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。

#include<math.h>

#include "stm32f10x.h"

//---------------------------------------------------------------------------------------------------

// 变量定义

 

#define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计

#define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接

#define halfT 0.001f                // 采样周期的一半

 

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素，代表估计方向

float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差

 

float Yaw,Pitch,Roll;  //偏航角，俯仰角，翻滚角

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

        float norm;

        float vx, vy, vz;

        float ex, ey, ez;  

 

        // 测量正常化

        norm = sqrt(ax*ax + ay*ay + az*az);      

        ax = ax / norm;                   //单位化

        ay = ay / norm;

        az = az / norm;      

 

        // 估计方向的重力

        vx = 2*(q1*q3 - q0*q2);

        vy = 2*(q0*q1 + q2*q3);

        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

 

        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

        ex = (ay*vz - az*vy);

        ey = (az*vx - ax*vz);

        ez = (ax*vy - ay*vx);

 

        // 积分误差比例积分增益

        exInt = exInt + ex*Ki;

        eyInt = eyInt + ey*Ki;

        ezInt = ezInt + ez*Ki;

 

        // 调整后的陀螺仪测量

        gx = gx + Kp*ex + exInt;

        gy = gy + Kp*ey + eyInt;

        gz = gz + Kp*ez + ezInt;

 

        // 整合四元数率和正常化

        q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

        q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

        q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

        q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  

 

        // 正常化四元

        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

        q0 = q0 / norm;

        q1 = q1 / norm;

        q2 = q2 / norm;

        q3 = q3 / norm;

 

        Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数

        Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv

        //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值，注掉

}

      要注意的的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。

二、一阶互补算法

       MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan()  可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：

//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意：dt的取值为滤波器采样时间

float angle;

 

angle_ax=atan(ax/az)*57.3;     //加速度得到的角度

gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度

Pitch = yijiehubu(angle_ax,gy);

 

float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);

     return angle;

}

    互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：

//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意：dt的取值为滤波器采样时间

float angle_P,angle_R;

float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);

         return angle_P;

}

 

float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);

         return angle_R;

}

单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。

三、卡尔曼滤波

      其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么5个公式等等的，我也不太懂，就不写了，感兴趣的话可以上网查。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。

#include<math.h>

#include "stm32f10x.h"

#include "Kalman_Filter.h"

//卡尔曼滤波参数与函数

float dt=0.001;//注意：dt的取值为kalman滤波器采样时间

float angle, angle_dot;//角度和角速度

float P[2][2] = {{ 1, 0 },

                 { 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

 

//卡尔曼滤波

float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

{

        angle+=(gyro_m-q_bias) * dt;

        angle_err = angle_m - angle;

        Pdot[0]=Q_angle - P[0][1] - P[1][0];

        Pdot[1]=- P[1][1];

        Pdot[2]=- P[1][1];

        Pdot[3]=Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

        PCt_0 = C_0 * P[0][0];

        PCt_1 = C_0 * P[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;

        K_1 = PCt_1 / E;

        t_0 = PCt_0;

        t_1 = C_0 * P[0][1];

        P[0][0] -= K_0 * t_0;

        P[0][1] -= K_0 * t_1;

        P[1][0] -= K_1 * t_0;

        P[1][1] -= K_1 * t_1;

        angle += K_0 * angle_err; //最优角度

        q_bias += K_1 * angle_err;

        angle_dot = gyro_m-q_bias;//最优角速度

 

        return angle；

}

      作个总结：三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。