MPU6050姿态解算——Mahony互补滤波

引入

DMP与互补滤波数据对比

Long time no see，首先放一张图，黄色的是DMP获取的Pitch角数据，紫红色的是互补滤波解算的Pitch角数据（人为晃动陀螺仪增加了干扰）。

DMP与互补滤波的优缺点分析

DMP	互补滤波
不需要了解姿态解算知识，直接调用库函数，操作简单	需要掌握一定的姿态解算知识
解算速度慢，最大只能到200Hz	速度快，而且随着单片机主频提高会更快
真实性差	真实性较好

从上面的曲线图结合我实际的干扰，在我大范围晃动陀螺仪的时候，DMP的角度变化还是很小，感觉是为了滤波而滤波，数据的真实性不可靠。而且在动态曲线观察的时候，互补滤波明显比DMP得到的的数据更快，时效性也🆗。
当然这也不意味着DMP一无是处，在对数据要求没那么高的情况下还是适用的。
接下来就分析一下互补滤波是怎么通过陀螺仪的原始数据解算出较为准确的姿态角儿~。

Mahony互补滤波算法分析

概述

陀螺仪一般为六轴或者九轴构成，六轴就是三轴角速度计加上三轴加速度计，九轴就是在六轴的情况下再加上一个三轴的磁力计，而应用最广泛的MPU6050就是一个六轴陀螺仪。
有一种现象很普遍也包括我自己，就是在没有用过陀螺仪之前，总以为陀螺仪可以直接得到角度数据，实际上并不然，陀螺仪（在下文中就代表MPU6050等六轴陀螺仪）只能得到角速度数据和角速度数据，从而间接得到角度。
可能数理基础不错的小伙伴会想到，对角速度积分不就成了角度吗？没错，确实是这样，但是陀螺仪存在误差，假设陀螺仪有一个0.1°/s的误差，一分钟之后误差就到了6°，一个小时后就是360°，所以原始数据并不是可靠的。
互补滤波就是对误差进行一个补偿，但是并不是用这种积分的方法，而是一个神奇的算法。

欧拉角描述方向余弦矩阵

在用欧拉角描述方向余弦矩阵之前先来了解一下坐标系的变换。注意是坐标系的变换而不是坐标。这篇文章就推导的十分详细

空间中任意坐标的旋转，等效于依次绕三个坐标轴的定轴旋转(不用看图的旋转方向，单纯解释这句话)。

以下三个定轴旋转都是在右手坐标系下的正方向旋转：

1. 绕Z轴旋转ψ角，有

2. 绕Y轴旋转θ角，有

3. 绕轴转动γ角，有

4. 整体坐标系转换

本文采用北东地坐标系为地理坐标系，并以地理坐标系作为导航坐标系，所以b系到t系的转换矩阵也是b系到n系的转换矩阵。b到n的转换矩阵被称为姿态矩阵或方向余弦矩阵。

由姿态矩阵可以求解出载体的横滚角γ、俯仰角θ和航向角ψ的主值，即：

这样就解算出了欧拉角，但是用欧拉角描述的方向余弦矩阵伴随着大量的三角运算，对单片机的处理不友好，会影响解算速度，而且会出现一个欧拉角死锁问题点这里，故采用接下来的四元数做解算。

四元数描述方向余弦矩阵

顺口提一句，四元数的发明者就是互补滤波的发明者叫威廉姆·汉密顿，懂为啥要用四元素结算了吧。当然是开个玩笑，四元数也是一种表达姿态的形式，在姿态解算中它避免了大量的三角运算，处理速度快。

用四元数描述上面的用欧拉角描述的姿态矩阵(在阅读开源程序代码的时候一定要注意，可能这个矩阵的不同，那是因为旋转方向和顺序不一样，还要注意看清下标，是哪个系转换到哪个系)：

由上面欧拉角的主值公式可以得到欧拉角与四元数的关系，这样就通过四元数来表示了方向余弦矩阵和欧拉角。

求解四元数

把四元数表示的姿态矩阵对时间求微分(其中做左边一列q上面带个小点表示微分，ω就是各轴的角速度)：

然后采用龙格库塔法解微分方程(我还不会推，直接用结论)：

下标为t+▲t是当前四元数，右边下标t为上个周期四元数。

传感器数据融合

在概述中我们分析了陀螺仪中角速度测量可能存在一个误差，这个误差在短时间内可以忽略，但是随着时间的累积，误差会越来越大。陀螺仪中的加速度计对高频信号敏感，在振动环境中高频干扰大。

陀螺仪得到的角速度数据短期内可信，长期由于积分误差不可信。加速度计得到的加速度数据，短期内由于高频抖动干扰不可信，长期可信。

拿出我们的中学二年级知识，地球上一切物体的重力加速度都是9.8m/s^2，我们用地理坐标系的重力加速度g，乘以一个从地理坐标系n到机体坐标系b的姿态转换矩阵，可以推导出机体坐标系的理论重力加速度v。

误差补偿

由上图可以看出，陀螺仪测出角速度解算出四元数，再由四元素推导出理论重力加速度v，而加速度计测出实际重力加速度a又称gb(b系下的重力加速度)，如果陀螺仪测出的角速度完全可靠，那么实际重力加速度gb应该是等于理论重力加速度vb的，但是很不幸，角速度数据存在误差，vb和gb就不会重合。

灵魂来了
用一个叉积来描述这个误差的大小

几何意义表示：

叉积公式(两种表达形式，第二种在程序里体现)：

1. aXb = |a|*|b|*sin θ
2. 对于向量a（x1， y1）， b（x2， y2），叉乘公式为x1 * y2 - x2 * y1

小角近似(sin θ的值与θ的值在θ很小的时侯很相近)：

在上面得到了理论重力加速度与实际重力加速度的误差erroor，这个error是一个实际的角度误差，我们用加速度计的值间接得出了误差变化的角度相关值，现在借助PID中的PI思想将这个误差补偿，这样就通过加速度计间接的得到了角度误差值(KP=1.5，KI=0.005，是哪篇论文研究过我忘记了，参数可调，按实际来)：

偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系：

得到准确的角速度后就可以用四元数反解出正确可靠的欧拉角了。

STM32程序实例

#include "imu.h"
#include "math.h"
#include "mpu6050.h"
#include "stm32f10x.h"

#define Acc_Gain 0.0001220f			//加速度转换单位(初始化加速度计量程+-4g，由于mpu6050的数据寄存器是16位的，LSBa = 2*4 / 65535.0)
#define Gyro_Gain 0.0609756f		//角速度转换为角度(LSBg = 2000*2 / 65535)
#define Gyro_Gr 0.0010641f			//角速度转换成弧度(3.1415 / 180 * LSBg)
#define G 9.80665f					// m/s^2

extern short ax,ay,az;
extern short gx,gy,gz;


static float invSqrt(float x) 		//快速计算 1/Sqrt(x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}


void Prepare_Data(void)
{
	MPU_Get_Accelerometer(&ax,&ay,&az);			//获取加速度计原始值
	MPU_Get_Gyroscope(&gx,&gy,&gz);				//获取角速度原始值
	
	//将加速度原始AD值转换为m/s^2
	ax = (float)ax * Acc_Gain * G;
	ay = (float)ay * Acc_Gain * G;
	az = (float)az * Acc_Gain * G;
	
	//将陀螺仪AD值转换为 弧度/s
	gx = (float)gx * Gyro_Gr;
	gy = (float)gy * Gyro_Gr;
	gz = (float)gz * Gyro_Gr;
}



#define Kp 1.50f
#define Ki 0.005f
#define halfT 0.0025f						//计算周期的一半，单位s

extern float Yaw,Pitch,Roll;				//我要给其他文件用所以定义了extern，不用管
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;		//四元数
float exInt = 0, eyInt = 0, ezInt = 0;		//叉积计算误差的累计积分


void Imu_Update(float gx,float gy,float gz,float ax,float ay,float az)
{
	u8 i;
	float vx,vy,vz;							//实际重力加速度
	float ex,ey,ez;							//叉积计算的误差
	float norm;
	
 	float q0q0 = q0*q0;
 	float q0q1 = q0*q1;
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
 	float q1q2 = q1*q2;
 	float q1q3 = q1*q3;
	float q2q2 = q2*q2;
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;
	
	if(ax*ay*az == 0)
		return;
	
	//加速度计测量的重力方向(机体坐标系)
	norm = invSqrt(ax*ax + ay*ay + az*az);			//之前这里写成invSqrt(ax*ax + ay+ay + az*az)是错误的，现在修改过来了
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;
	
	//四元数推出的实际重力方向(机体坐标系)
	vx = 2*(q1q3 - q0q2);												
  	vy = 2*(q0q1 + q2q3);
  	vz = q0q0 - q1q1 - q2q2 + q3q3;
	
	//叉积误差
	ex = (ay*vz - az*vy);
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);
	
	//叉积误差积分为角速度
	exInt = exInt + ex * Ki;
	eyInt = eyInt + ey * Ki;
	ezInt = ezInt + ez * Ki;
	
	//角速度补偿
	gx = gx + Kp*ex + exInt;
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;
	
	//更新四元数
  	q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  	q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  	q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  	q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;	
	
	//单位化四元数
  	norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  	q0 = q0 * norm;
  	q1 = q1 * norm;
  	q2 = q2 * norm;  
  	q3 = q3 * norm;
	
	//四元数反解欧拉角
	Yaw = atan2(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)* 57.3f;
	Pitch = -asin(2.f * (q1q3 - q0q2))* 57.3f;
	Roll = atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3)* 57.3f;
}

MPU6050姿态解算——Mahony互补滤波_acc_gain * g-CSDN博客