查看:
1844
|
回复:
0
|
陀螺仪和加速度计矫正是如何实现的?
|
|
发表于2018-10-30 23:08:12
|
显示全部楼层
1#
电梯直达
陀螺仪和加速度计矫正是如何实现的?(APingSci出品)
一、目的
获得准确的姿态角数据
二、情况
我们通过模拟I2C从MPU6050读取寄存器所获得的原始数据有两种不同程度的偏差:
1、刚上电的时候,陀螺仪的加速度计数值普遍过大,在等待一段时间后变化趋于正常。
2、当稳定下来后,并非是准确的数值,有着个位数范围内的偏差。
从DMP获得的欧拉角:
1、刚上电的时候,俯仰角,横滚角和偏航角的数值呈直线递增,三个角稳定在2,2,6左右的角度数据。
2、约30秒后,三个角分别为2,6,2
显然,这样的姿态角数据是无法进行UAV的姿态控制的,必须进行校正陀螺仪。
三、实现方法(姿态解算和获取原始数据暂不讲)
uint8_t MPU6050_OffSet(INT16_XYZ value,INT16_XYZ *offset,uint16_t sensivity)
{
static int32_t tempgx=0,tempgy=0,tempgz=0;
static uint16_t cnt_a=0;//使用static修饰的局部变量,表明次变量具有静态存储周期,也就是说该函数执行完后不释放内存
if(cnt_a==0)
{
value.X=0;
value.Y=0;
value.Z=0;
tempgx = 0;
tempgy = 0;
tempgz = 0;
cnt_a = 1;
sensivity = 0;
offset->X = 0;
offset->Y = 0;
offset->Z = 0;
}
tempgx += value.X;
tempgy += value.Y;
tempgz += value.Z-sensivity ;//加速度计校准 sensivity 等于 MPU6050初始化时设置的灵敏度值(8196LSB/g);陀螺仪校准 sensivity = 0;
if(cnt_a==200) //200个数值求平均
{
offset->X=tempgx/cnt_a;
offset->Y=tempgy/cnt_a;
offset->Z=tempgz/cnt_a;
cnt_a = 0;
return 1;
}
cnt_a++;
return 0;
}
大体上就是累加,然后求平均值。上面的代码,首先定义了cnt_a变量,是为了累加计数,而且是静态的变量,当首次校准的时候将原始数据、偏移数据和灵敏度都清零,然后,在cnt_a累加计数到200次的时候,再将偏移的数值取平均。。
|
|