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

这个项目包括底层iic驱动、mpu6050驱动、imu支持姿态解决代码stm32标准库和hal库。 以前的代码有错误，现在已经修复了。

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

这部分代码参考了正点原子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_FFO_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;
	
}

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

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;
}

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

首先需要确保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()的时钟相关函数