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°內。