文章目录
0、介绍1，理论分析1.1 MPU60501.2 Mahony算法原理2，代码实现1.1 MPU6050初始化及数据读取1.2 Mahony算法c语言实现1.3 将代码移植到你的工程1.3.1、移植iic1.3.2、在程序中调用3，补充项目地址：/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.2gadc转换精度为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);//唤醒MPU6050MPU_Set_Gyro_Fsr(3);//陀螺仪量程+-2000MPU_Set_Accel_Fsr(0);//加速度计量程+-2gMPU_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);//关闭FIFOMPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);//关闭INTres=MPU_Read_Byte(MPU_DEVICE_ID_REG);//读取设备id，AD0引脚接地 故id应该为0x68if(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_tsum = 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;//gyrogyro[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; //accacc[1] = ay;acc[2] = az;}
计算角速度偏移：当mpu初始化完成且保持静止时，在三轴的角速度上有一个非0的偏移，需要计算出这个偏移，在后面的角度更新过程中减去这个偏移，这样角度才不会累加。
static void MPU6500_Init_Offset(void){unsigned int i;int16_t temp[3],temp2[3];int32_ttempgx=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 readyfor(i=0;i<100;i++){delay_ms(10);MPU6500_readGyro_Acc(temp,temp2);}//calculate the average of imu data as offsetfor(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/0516164833222.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 MahonyMahonyAHRSupdateIMU(Values[0] * PI/180, Values[1] * PI/180, Values[2] * PI/180,Values[3], Values[4], Values[5]);//save Quaternionq[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 quaternionimu.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/0517203510626.jpg)]
所以由四元数得到的机体坐标系下的重力向量为：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-LNKc9nHC-1652242386447)(picture/0516234611509.jpg)]
由于加速度计测的是在机体坐标系下的重力向量，故将两个向量作叉积，即可得到他们之间的误差。
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-sTmQ0A3d-1652242386448)(picture/051623534746.jpg)]
四元数更新方程：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-U8Uv7zZE-1652242386448)(picture/051618002479.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 KiintegralFBy += twoKi * halfey * (1.0f / sampleFreq);integralFBz += twoKi * halfez * (1.0f / sampleFreq);//反馈到角速度gx += integralFBx;// apply integral feedbackgy += integralFBy;gz += integralFBz;}else {integralFBx = 0.0f;// prevent integral windupintegralFBy = 0.0f;integralFBz = 0.0f;}// 对误差作比例运算并反馈gx += twoKp * halfex;gy += twoKp * halfey;gz += twoKp * halfez;}// 计算1/2 dtgx *= (0.5f * (1.0f / sampleFreq));// pre-multiply common factorsgy *= (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 将代码移植到你的工程
项目地址：/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_HALvoid 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_GPIOBRCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);GPIO_InitStructure.GPIO_Pin = IIC_SCL_Pin;GPIO_Init(IIC_SCL_GPIO_Port, &GPIO_InitStructure);// fix RCC_AHB1Periph_GPIOBRCC_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);}#elsevoid 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);
如果觉得《stm32 MPU6050 姿态解算 Mahony互补滤波算法》对你有帮助，请点赞、收藏，并留下你的观点哦！
