MPU6050如何通过对角速度进行惯性积分计算水平旋转角度(Yaw角)

前几天研究学习了下MPU6050的姿态计算。Roll和Pitch角很容易得出，但是Yaw角就莫法直接得出。
因为重力加速度的Z轴，在相对地平面东西南北旋转时并无变化，因此只能靠Z轴的角速度来惯性累计估算旋转角度。注意，这里只能算出旋转的相对偏移，正北方在某个位置只能加磁力计得出。
MPU6050仅靠Gryo角加速度来计算水平旋转时的Yaw偏航角度，关键是首先要去除零偏。

重点是先要去除零偏，MPU6050的芯片即使静止放置不动，Gryo角加速度的数据也会不停偏移，必须要除掉这个偏移值。
再就是积分算法重点，确保积分算法的采样率正确。MPU6050初始化数据采集是什么采样率，取数据就得保证这个采样率，最好接上中断脚，用中断读取。
如果采样数据丢多了，角加速度根据积分算法累加出来的角度就会差很多。

要注意单片机读取问题，很多亲拿出来的数据漂的厉害，要么零偏没有去除，要么就是滤波算法采样有问题，读取数据的采样率不够，跟积分算法带入的dt积分时间参数对不上。
我STC单片机跑得慢，主频只有30mHz，测试取一次数据要3-4ms左右，设置的分频是0x7，采样速率=1k/(1+7)=125Hz，间隔8ms读一次采样，加上发送的延迟，勉强够。

下面演示实际代码
首先拿到IMU在XYZ三轴上的角速度数据

/* IMU Data */
float gyroX, gyroY, gyroZ;
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];

gyroX /= 16.384f; // 我的MPU6050初始化的±2000LSB量程 (32768 / 2000) = 16.384f
gyroY /= 16.384f;
gyroZ /= 16.384f;

这里一开始让IMU静止不动，用1秒钟时间统计零偏，
我的采样率是125Hz，dt间隔时间是8ms，也就是统计125次采样做积分来确定零偏值：

static const float dt = 8.0 / 1000;  // 间隔8ms
static const ZERO_OFFSET_COUN (1 / dt) // 1/8=125次每秒

static int g_GetZeroOffset = 0;
static float gyroX_offset = 0.0f, gyroY_offset = 0.0f, gyroZ_offset = 0.0f;

// 间隔8ms一次采样，统计125次，共1秒时间
if (g_GetZeroOffset++ < ZERO_OFFSET_COUN)
{
	gyroX_offset += gyroX * dt;  // 每次8%积分，累计加权125次
	gyroY_offset += gyroY * dt;
	gyroZ_offset += gyroZ * dt;
}

// 除去零偏
gyroX -= gyroX_offset;
gyroY -= gyroY_offset;
gyroZ -= gyroZ_offset;

然后就是靠惯性乘积去拿到XYZ三轴的旋转偏移角度

static float integralX = 0.0f, integralY = 0.0f, integralZ = 0.0f;
if (g_GetZeroOffset > ZERO_OFFSET_COUN) // 统计完零偏后开始累计偏角
{
	integralX += gyroX * dt;  // 每次8%权重累计偏转角度
	integralY += gyroY * dt;
	integralZ += gyroZ * dt;
	// 360°一个循环
	if (integralX > 360)
		integralX -= 360;
	if (integralX < -360)
		integralX += 360;
	if (integralY > 360)
		integralY -= 360;
	if (integralY < -360)
		integralY += 360;

	if (integralZ > 360)
		integralZ -= 360;
	if (integralZ < -360)
		integralZ += 360;
}

这里integralZ就是Yaw角，物体相对地平面东南西北旋转的角度啦。
板子芯片正面朝天空摆放，逆时针转是正，顺时针转是负。
integralX和integralY分别对应Roll和Pitch角的偏移。

去除零偏后，测试板子静止摆放不动几分钟，积分的角度在零点几°范围来回摆。
静止摆10分钟的漂移也只有约1°内。几分钟的使用，按1°取值的精度足够用。
如果需要Yaw角长时间保持不飘，还是需要加磁力计。
如果只要Roll和Pitch角的数据，则没必要使用角速度，可以用重力加速度计算出来的姿态角做个互补滤波或者卡尔曼滤波即可（上一篇有讲解）。

另外，我看网上很多同志说漂移严重，应该很多都是没去除零偏，或者积分运算的dt值，跟实际的数据采样取值不符合，一般都是采样丢数据了。我这里来回旋转的精度没法精确测试，手拿板子旋转90°，再摆回去，来回转四五次，误差目测在1°内。