一、本文章是我在学习江科大平衡车后的理解，硬件驱动层是采用江科大的库。

二、姿态解算过程

1. 获取原始数据

MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&GZ); //获取原始数据（延时比较严重，需要修改底层）

平衡车项目对延时要求比较高，如果使用的是软件IIC，因此需要修改底层，将延时注释，对于主频72MHZ可以将IIC翻转电平的delay延时注释掉，但对于更高主频的单片机还是需要加一定的延时，以免超过IIC通讯要求的最大速率。

 2. 计算加速度计解算的角度（AngleAcc)

AngleAcc = - atan2(AY, AZ) * 57.29578f;  // 180/3.1415926

注意需要引用math头文件。AY还是AX于mpu6050放置的方向有关系。atan2函数相较于atan函数（-90到90)能达到-180到180角度取值的范围。加速度计是依靠重力分量进行计算的，atan函数能得到俯仰角弧度值（带pai的），再/3.1415926*180得到角度。

 3. 陀螺仪解算得到的角度

AngleGyro =  AngleGyro - GX * 0.00061035f; //2000 / 32768 * 0.01 (dt)

GX还是GY同样与mpu6050放置有关系，其实就是小车往前往后转动时对应的陀螺仪的角速度。

陀螺仪满量程寄存器配置为+-2000°/s，而GX变量类型为int16_t（-32767-32768），因此需要转换成对应的角速度，然后再乘dt(计算周期）即为角度。

4.记得确定俩角度的正负，一般往前为正，往后为负

5.互补滤波姿态解算

由上面可知，加速度计是利用重力分量进行解算的，静态稳而动态不稳，陀螺仪是利用角速度积累来计算的，动态稳但静态容易漂移，因为静态时GY不等于0。而且初始时小车不管时躺地上还是数竖直起来角度都为0，因为是角度是角速度累计得到的，不具有绝对角度，这是陀螺仪的第二个缺点。

因此采用互补滤波融合二者优点。

AngleGyro =  Angle - GX * 0.00061035f;   // 2000 / 32768 * 0.01 (dt) ; 0.01（dt）为中断周期10ms，dt变了滤波系数也要变
float Alpha = 0.98;  //滤波系数（10ms中断一般0.98-0.99)  
Angle = (1- Alpha) * AngleAcc +  Alpha * AngleGyro; 

注意陀螺仪下次积累是在滤波后的角度基础上进行的，相当于每次计算时阿尔法拉陀螺仪一把，让其往加速度计偏移，抑制漂移，每计算一次拉一下。因此计算周期间隔越大，陀螺仪漂移的越厉害，阿尔法应当越大，拉陀螺仪的力就越强。同时每次初始化如果角度不在0，陀螺仪还是从0开始，但是因为阿尔法的作用，陀螺仪会快速往角速度偏移，变为绝对角度。因此陀螺仪的两个缺点都优化了，这就是互补滤波的作用。

6.最后再校准两处

GX += 50;//陀螺仪校准，抑制零飘
AngleAcc -= 0.5f; //平衡车机械零点校准

第一使GY在小车静置时为0，抑制零飘，第二处是使小车处于平衡时的位置时（即机械零点）角度为0。

7.以下是完整代码

//  ---mpu6050互补滤波姿态解算---
MPU6050_GetData(&AX,&AY,&AZ,&GX,&GY,&GZ); //获取原始数据（延时比较严重，需要修改底层）
GX += 50;//陀螺仪校准，抑制零飘
AngleAcc = - atan2(AY, AZ) * 57.29578f;  // 180/3.1415926
AngleAcc -= 0.5f; //平衡车机械零点校准
AngleGyro =  Angle - GX * 0.00061035f;   // 2000 / 32768 * 0.01 (dt) ; 0.01（dt）为中断周期10ms，dt变了滤波系数也要变
float Alpha = 0.98;  //滤波系数（10ms中断一般0.98-0.99)  
Angle = (1- Alpha) * AngleAcc +  Alpha * AngleGyro;