stm32 MPU6050 姿态解算 Mahony互补滤波算法

项目地址： https://gitee.com/killerp/mpu6050_-mahony

0、介绍

本项目包括底层iic驱动、mpu6050驱动、imu姿态解算代码，支持stm32标准库和hal库。
之前的代码存在错误，现在已修复。

1，理论分析

1.1 MPU6050

MPU6050是一个集成了陀螺仪和加速度计的传感器，它能输出在直角坐标系下的x，y，z轴的角速度和加速度数据。

陀螺仪输出的格式为：绕x轴的旋转角速度，绕y轴的角速度，绕z轴的角速度（分别称为roll角速度，pitch角速度和yaw角速度）。

加速度计输出的格式为：x轴的加速度，y轴的加速度，z轴的加速度。

另外还需要关注传感器的其他参数如：

• 陀螺仪的量程：eg.±2000dps
• 加速度计的量程：eg.2g
• adc转换精度为16bit
• 传感器采样率4-1000hz：eg.1000hz

我们从MPU6050那就得到了陀螺仪数据gx,gy,gz，加速度数据az,ay,az

螺仪转换精度2^16=65536,65536/{2000-(-2000)}=16.4,实际1°等于adc值16.4

采样率就是数据的更新率，也就是我们每次读取数据的频率。

1.2 Mahony算法原理

参考另一篇文章：基于Manony滤波算法的姿态解算

2，代码实现

1.1 MPU6050初始化及数据读取

该部分代码参考了正点原子的MPU6050例程；主要修改以下初始化代码

/*
* MPU6050模块:绕x轴为roll，绕y轴为pitch，绕z轴为yaw
*/
uint8_t MPU_Init(void)
{ 
	uint8_t res;
	IIC_Init();	//初始化IIC总线
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050	
	//等待复位完成
  	delay_ms(100);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050
	MPU_Set_Gyro_Fsr(3);	//陀螺仪量程+-2000		
	MPU_Set_Accel_Fsr(0);	//加速度计量程+-2g				
	MPU_Set_Rate(1000);		//1khz采样率				
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//关闭IIC主模式
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//关闭INT
	res=MPU_Read_Byte(MPU_DEVICE_ID_REG);	//读取设备id，AD0引脚接地 故id应该为0x68
	if(res==MPU_ADDR)
	{
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置x轴为时钟
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//开启陀螺仪加速度计
		MPU_Set_Rate(1000);						
 	}else return 1;
	return 0;
}

重新编写一个读取mpu6050数据的函数。使我们读取的数据是经过平均滤波的数据。

使用6个FIFO队列对数据(gx,gy,gz,ax,ay,az)进行缓存，每次读取一次数据，就将数据入队，并计算队列的平均值，对原始数据进行平滑滤波。

#define Buf_SIZE  10	//队列长度，越大，平滑性越高

int16_t  MPU6500_FIFO[6][Buf_SIZE];	//6个FIFO队列；0-2：陀螺仪数据；3-5：加速度计数据	

int16_t lastAx,lastAy,lastAz,lastGx,lastGy,lastGz;

static uint8_t Wr_Index = 0;	//当前FIFO的写入下标

//将val入队
static void MPU6500_NewVal(int16_t* buf,int16_t val) {
  	buf[Wr_Index] = val;
}

//计算FIFO中的平均值
static int16_t MPU6500_GetAvg(int16_t* buf)
{
  	int i;
	int32_t	sum = 0;
	for(i=0;i<Buf_SIZE;i++)
		sum += buf[i];
	sum = sum / Buf_SIZE;
	return (int16_t)sum;
}

//读取经过滤波的陀螺仪，加速度数据
void MPU6500_readGyro_Acc(int16_t *gyro,int16_t *acc)
{
	static short buf[6];	//缓存原始数据：0-2：陀螺仪数据；3-5：加速度计数据	
	static int16_t gx,gy,gz;
	static int16_t ax,ay,az;
	
	//正点原子的库函数，读取传感器原始数据
	MPU_Get_Gyroscope(&buf[0],&buf[1],&buf[2]);	
	MPU_Get_Accelerometer(&buf[3],&buf[4],&buf[5]);
	
	//将原始数据入队
	MPU6500_NewVal(&MPU6500_FIFO[0][0],buf[0]);
	MPU6500_NewVal(&MPU6500_FIFO[1][0],buf[1]);
	MPU6500_NewVal(&MPU6500_FIFO[2][0],buf[2]);

	MPU6500_NewVal(&MPU6500_FIFO[3][0],buf[3]);
	MPU6500_NewVal(&MPU6500_FIFO[4][0],buf[4]);
	MPU6500_NewVal(&MPU6500_FIFO[5][0],buf[5]);
	
	//更新FIFO入口指针
	Wr_Index = (Wr_Index + 1) % Buf_SIZE;	

	//计算队列平均值
	gx =  MPU6500_GetAvg(&MPU6500_FIFO[4][0]);
	gy =  MPU6500_GetAvg(&MPU6500_FIFO[5][0]);
	gz =  MPU6500_GetAvg(&MPU6500_FIFO[6][0]);
	
	//陀螺仪数据要减去偏移量
	gyro[0] = gx - imu.Roll_offset;	//gyro
	gyro[1] = gy - imu.Pitch_offset;
	gyro[2] = gz - imu.Yaw_offset;
		

	ax = 	MPU6500_GetAvg(&MPU6500_FIFO[0][0]);
	ay = 	MPU6500_GetAvg(&MPU6500_FIFO[1][0]);
	az = 	MPU6500_GetAvg(&MPU6500_FIFO[2][0]);
				
	acc[0] = ax; //acc
	acc[1] = ay;
	acc[2] = az;	
}

计算角速度偏移：当mpu初始化完成且保持静止时，在三轴的角速度上有一个非0的偏移，需要计算出这个偏移，在后面的角度更新过程中减去这个偏移，这样角度才不会累加。

static void MPU6500_Init_Offset(void)
{
	unsigned int i;
	int16_t temp[3],temp2[3];
	int32_t	tempgx=0,tempgy=0,tempgz=0;
	int32_t tempax=0,tempay=0,tempaz=0;
	imu.Pitch_offset = 0;
	imu.Roll_offset = 0;
	imu.Yaw_offset = 0;
	
	//wait the mpu to be ready
	for(i=0;i<100;i++){
  		delay_ms(10);
		MPU6500_readGyro_Acc(temp,temp2);
	}
	
	//calculate the average of imu data as offset
 	for(i=0;i<OFFSET_CONUT;i++){
		delay_ms(10);
		MPU6500_readGyro_Acc(temp,temp2);
		tempgx += temp[0];
		tempgy += temp[1];
		tempgz += temp[2];
		
		tempax += temp2[0];
		tempay += temp2[1];
		tempaz += temp2[2];
		
	}
	
	imu.Pitch_offset = tempgy/OFFSET_CONUT;
	imu.Roll_offset = tempgx/OFFSET_CONUT;
	imu.Yaw_offset = tempgz/OFFSET_CONUT;
	
}

1.2 Mahony算法c语言实现

首先将陀螺仪的数据转换成角度，这里封装成一个函数

static void Get_IMU_Values(float *values)
{
	int16_t gyro[3],acc[3];
	
	MPU6500_readGyro_Acc(&gyro[0],&acc[0]);
	
	for(int i=0;i<3;i++)
	{
		values[i]=((float) gyro[i])/16.4f;	//gyro range +-2000; adc accuracy 2^16=65536; 65536/4000=16.4;
		values[3+i]=(float) acc[i];
	}
	
}

然后编写函数实现计算姿态角的功能，使用四元数计算姿态角的公式在理论分析中推导：

其中α为绕x轴旋转角即roll，β为绕y轴旋转角即pitch，γ为绕z轴旋转角即yaw。a,b,c,d即q0,q1,q2,q3.

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Z5ppFChD-1652242386445)(picture/20210516164833222.png)]

//arcsin函数
float safe_asin(float v)
{
	if (isnan(v)) {
		return 0.0f;
	}
	if (v >= 1.0f) {
		return PI/2;
	}
	if (v <= -1.0f) {
		return -PI/2;
	}
	return asin(v);
}
void IMU_Update(void)
{
	static float q[4];
	float Values[6];	
	Get_IMU_Values(Values);	
	
	//change angle to radian,the calculate the imu with Mahony
	MahonyAHRSupdateIMU(Values[0] * PI/180, Values[1] * PI/180, Values[2] * PI/180,
 	Values[3], Values[4], Values[5]);		
	
	//save Quaternion
	q[0] = q0;
	q[1] = q1;
	q[2] = q2;
	q[3] = q3;
	
	imu.ax = Values[3];
	imu.ay = Values[4];
	imu.az = Values[5];

	imu.Pitch_v = Values[0];
	imu.Roll_v = Values[1];
	imu.Yaw_v = Values[2];

	//calculate the imu angle with quaternion
	imu.Roll = (atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;	
	imu.Pitch = -safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;
	imu.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 180/PI; // yaw

}

代码中MahonyAHRSupdateIMU()函数实现的就是四元数的更新算法。

逻辑上，首先用加速度计校准陀螺仪，方式是通过计算当前四元数姿态下的重力分量，与加速度计的重力分量作叉积，得到误差。
对误差作P(比例)和I(积分)运算后加到陀螺仪角速度上。最终由角速度计算新的四元数。

使用到的公式有：

四元数重力分量计算：

四元数旋转矩阵的转置表示：从地理坐标系转换到机体坐标系的旋转。重力向量设为[0,0,1]，与四元数旋转矩阵的转置矩阵相乘，表示机体坐标系下的重力分量。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-iQsHsgCJ-1652242386447)(picture/20210517203510626.jpg)]

所以由四元数得到的机体坐标系下的重力向量为：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LNKc9nHC-1652242386447)(picture/20210516234611509.jpg)]
由于加速度计测的是在机体坐标系下的重力向量，故将两个向量作叉积，即可得到他们之间的误差。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sTmQ0A3d-1652242386448)(picture/2021051623534746.jpg)]

四元数更新方程：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-U8Uv7zZE-1652242386448)(picture/2021051618002479.jpg)]

代码中的 sampleFreq 即执行姿态解算的频率，这里用定时器，以500HZ的频率调用get_angle();

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;	
	float halfvx, halfvy, halfvz;	//1/2 重力分量
	float halfex, halfey, halfez;	//1/2 重力误差
	float qa, qb, qc;

	//加速度数据有效时才进行校准
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		//对加速度数据归一化
		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= recipNorm;
		ay *= recipNorm;
		az *= recipNorm;

		// 由四元数计算重力分量
		halfvx = q1 * q3 - q0 * q2;
		halfvy = q0 * q1 + q2 * q3;
		halfvz = q0 * q0 - 0.5f + q3 * q3;

		// 将四元数重力分量 与 加速度计重力分量 作叉积 得到误差
		halfex = (ay * halfvz - az * halfvy);
		halfey = (az * halfvx - ax * halfvz);
		halfez = (ax * halfvy - ay * halfvx);

		// 使用积分？
		if(twoKi > 0.0f) {
			//对误差作积分
			integralFBx += twoKi * halfex * (1.0f / sampleFreq);	// integral error scaled by Ki
			integralFBy += twoKi * halfey * (1.0f / sampleFreq);
			integralFBz += twoKi * halfez * (1.0f / sampleFreq);
			//反馈到角速度
			gx += integralFBx;	// apply integral feedback
			gy += integralFBy;
			gz += integralFBz;
		}
		else {
			integralFBx = 0.0f;	// prevent integral windup
			integralFBy = 0.0f;
			integralFBz = 0.0f;
		}

		// 对误差作比例运算并反馈
		gx += twoKp * halfex;
		gy += twoKp * halfey;
		gz += twoKp * halfez;
	}

	// 计算1/2 dt
	gx *= (0.5f * (1.0f / sampleFreq));		// pre-multiply common factors
	gy *= (0.5f * (1.0f / sampleFreq));
	gz *= (0.5f * (1.0f / sampleFreq));
	qa = q0;
	qb = q1;
	qc = q2;
	// 更新四元数
	q0 += (-qb * gx - qc * gy - q3 * gz);
	q1 += (qa * gx + qc * gz - q3 * gy);
	q2 += (qa * gy - qb * gz + q3 * gx);
	q3 += (qa * gz + qb * gy - qc * gx);

	// 四元数归一化
	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= recipNorm;
	q1 *= recipNorm;
	q2 *= recipNorm;
	q3 *= recipNorm;
}

1.3 将代码移植到你的工程

项目地址：https://gitee.com/killerp/mpu6050_-mahony，下载代码，并将include和src目录下的文件复制到你的项目。

1.3.1、移植iic

首先需要确保iic驱动能正常使用，如果使用的是HAL库，需要定义宏IMU_USE_HAL。

将代码中的gpio引脚修改成你的项目的iic引脚。例如，本项目使用stm32的gpio模拟iic，其实现是在myiic.h和myiic.c，需要修改如下：

本例子使用GPIOB_12作为SDA，GPIOB_13作为SCL

myiic.h:

//change gpio
#define IIC_SCL_GPIO_Port GPIOB
#define IIC_SDA_GPIO_Port GPIOB
#define IIC_SCL_Pin_Num 13
#define IIC_SDA_Pin_Num 12

myiic.c:

修改对应的IIC_Init()的时钟相关函数

#ifndef IMU_USE_HAL

void IIC_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;

	// fix RCC_AHB1Periph_GPIOB
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
	GPIO_InitStructure.GPIO_Pin = IIC_SCL_Pin;
	GPIO_Init(IIC_SCL_GPIO_Port, &GPIO_InitStructure);

	// fix RCC_AHB1Periph_GPIOB
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
	GPIO_InitStructure.GPIO_Pin = IIC_SDA_Pin;
	GPIO_Init(IIC_SDA_GPIO_Port, &GPIO_InitStructure);

	IIC_SCL(1);
	IIC_SDA(1);
}
#else
void IIC_Init(void)
{
	GPIO_InitTypeDef GPIO_Initure;
	GPIO_Initure.Mode = GPIO_MODE_OUTPUT_PP;
	GPIO_Initure.Pull = GPIO_PULLUP;
	GPIO_Initure.Speed = GPIO_SPEED_HIGH;

	// fix __HAL_RCC_GPIOB_CLK_ENABLE
	__HAL_RCC_GPIOB_CLK_ENABLE();
	GPIO_Initure.Pin = IIC_SCL_Pin;
	HAL_GPIO_Init(IIC_SCL_GPIO_Port, &GPIO_Initure);

	// fix __HAL_RCC_GPIOB_CLK_ENABLE
	__HAL_RCC_GPIOB_CLK_ENABLE();
	GPIO_Initure.Pin = IIC_SDA_Pin;
	HAL_GPIO_Init(IIC_SDA_GPIO_Port, &GPIO_Initure);

	IIC_SCL(1);
	IIC_SDA(1);
}

注意其中的delay.h是需要你自己实现延时函数。

1.3.2、在程序中调用

在主函数中执行下面函数，初始化MPU6050及其数据

	if(IMU_Init()!=0)
	{
	     printf("MPU6050 Init fail\r\n");
		return -1;
	}

然后设置一个定时器，令其产生周期性中断，中断产生的频率要和Mahony.h的宏一样：

#define IMU_Update_Freq 100.0f   //frequency in Hz must equal to the frequency of IMU_Update() 

TIM3_Int_Init(100-1,8400-1);    //10ms = (1000*840)/84 us = 10ms == 100hz = IMU_Update_Freq

在定时器3中断处理函数中执行IMU_Update()更新姿态角：

void TIM3_IRQHandler(void)
{
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET) 
	{
		IMU_Update();
	}
	TIM_ClearITPendingBit(TIM3,TIM_IT_Update);  
}

假设出现了角度不断增加的情况，请查看角速度值是否为0，若明显大于0，可能是MPU6500_Init_Offset()函数得到的偏移值不准确。

3，补充

由于加速度计对水平方向的旋转无能为力，故用此程序得到的yaw角数据会一直漂移，无法得到校准；通常的解决方法是增加一个磁场传感器，来获得一个准确的水平方向角来校准陀螺仪的漂移。MPU6050支持扩展一个IIC接口到磁场传感器，可通过配置MPU6050的IIC MASTER 来读取磁场传感器的数据。

在Mahony中提供了包含磁场数据的融合函数：

void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);