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();//获取翻滚角

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

你可能感兴趣的:(树梅派)