MPU6050单轴双浆姿态平衡PID调戏之姿态解算

最近看群里部分小伙伴对于6050如何计算姿态比较感兴趣，自己也想对这段时间的学习做个总结，这次来说说姿态计算的方法，其实还不懂其中数学原理，正好自己也当作学习。

解算姿态的前提

一定要确认MPU6050的加速度计：AX, AY, AZ，陀螺仪：GX, GY, GZ的数据没有问题。比如一下几种：

1.初始化后，静止平放的时候，数据跳变非常频繁，而且幅度比较大，例如：

AX:100 100 100 103  91 98   99  120  20000 100 100 102 

其中20000这个数字经常出现，其他各种也是如此，有人说这个模块原始数据就是毛刺很多，做个滤波就可以了，我把初始化代码调了一个月也没能将数据稳定下来，而且用上面的数据，融合算出来的姿态跳变也非常快。不稳定。

如果出现以上问题，可以考虑几个方面：代码问题，没有正确初始化；IIC驱动的问题（我的就是这个）。

加速度计，陀螺仪

加速度计，我的理解，这是检查X,Y,Z轴受力的。所以原始数据经过转化后，应该得到的是各个轴的受力情况，理想状态下，平放静止的时候，输出应该是， X=0, Y=0, Z=重力，单位是g（就是重力加速度9.8那个）。就是说，加速度计检查的是运动的状态，转化成各轴的受力，经过对各轴的矢量进行三角函数的操作，也可以知道静止状态下的角度。如果运动起来，瞬时的计算角度误差比较大。

陀螺仪，这是测算角速度的，单位：度/每秒。就是计算绕X,Y,Z轴的三个角速度。平放静止状态下，各轴理想都是0。但是当模块进行运动的时候，各轴就开始有速度体现了。

角速度能反映物体的圆周运动状态，加速度能能反映物体的静止和直线运动状态，两者互补就能识别很复杂的运动状态，如四轴飞行器的姿态、人行走的步态、空中鼠标的位移量。这就是等下要用的互补滤波算法。

Mahony的互补滤波姿态解算

数学原理我不懂，这里只做应用介绍。首先找一份完整的Mahony代码，网上有蛮多的。这里感谢各位前辈的奉献。

void Mahony_update_9X(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);//结合地磁计可以用这个
void Mahony_update_6X(float gx, float gy, float gz, float ax, float ay, float az);
void Mahony_Init(float sampleFrequency);
void Mahony_computeAngles(void);
float getRoll(void);
float getPitch(void);
float getYaw(void);

用到的代码就以上几个。使用流程如下。

	mpu.readAccelData(accelCount);
	accelCount[0] -= ACCEL_OFFSET_X;
	accelCount[1] -= ACCEL_OFFSET_Y;
	accelCount[2] += ACCEL_OFFSET_Z;
	ax = (float)(accelCount[0] )*aRes; //转换aRes= 1/16384
	ay = (float)(accelCount[1] )*aRes; 
	az = (float)(accelCount[2] )*aRes; 
    mpu.readGyroData(gyroCount);
	gyroCount[0] -= GYRO_OFFSET_X;
	gyroCount[1] -= GYRO_OFFSET_Y;
	gyroCount[2] -= GYRO_OFFSET_Z;
	gx = (float)(gyroCount[0] )*gRes ; //转换 gRes =  2000.0 / 32768.0
	gy = (float)(gyroCount[1] )*gRes ;  
	gz = (float)(gyroCount[2] )*gRes ; 
	Mahony_update_6X(gx, gy, gz, ax, ay, az);
	Mahony_computeAngles();
    getRoll();//获取翻滚角

以上便是解算的过程，至于原理等，还不清楚如何，计划在接下来的计划中，再细细体会。