# mpu6050_Mahony **Repository Path**: killerp/mpu6050_-mahony ## Basic Information - **Project Name**: mpu6050_Mahony - **Description**: No description available - **Primary Language**: Unknown - **License**: Apache-2.0 - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 24 - **Forks**: 11 - **Created**: 2021-05-17 - **Last Updated**: 2025-11-20 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README ## 0，目录 本项目包括底层iic驱动、mpu6050驱动、imu姿态解算代码，以stm32标准库为例。 ## 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滤波算法的姿态解算](https://blog.csdn.net/weixin_44821644/article/details/108542178) ## 2，代码实现 ### 1.1 MPU6050初始化及数据读取 **该部分代码参考了正点原子的MPU6050例程；主要修改以下初始化代码** ```c /* * 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)进行缓存，每次读取一次数据，就将数据入队，并计算队列的平均值，对原始数据进行平滑滤波。 ```c #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= 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]，与四元数旋转矩阵的转置矩阵相乘，表示机体坐标系下的重力分量。 ![在这里插入图片描述](picture/20210517203510626.jpg) 所以由四元数得到的机体坐标系下的重力向量为： ![在这里插入图片描述](picture/20210516234611509.jpg) **由于加速度计测的是在机体坐标系下的重力向量，故将两个向量作叉积，即可得到他们之间的误差。** ![在这里插入图片描述](picture/2021051623534746.jpg) **四元数更新方程：** ![在这里插入图片描述](picture/2021051618002479.jpg) **代码中的 sampleFreq 即执行姿态解算的频率，这里用定时器，以500HZ的频率调用get_angle();** ```c 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](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:** ```c //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()的时钟相关函数 ```c #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及其数据 ```c if(IMU_Init()!=0) { printf("MPU6050 Init fail\r\n"); return -1; } ``` 然后设置一个定时器，令其产生周期性中断，中断产生的频率要和Mahony.h的宏一样： ```c #define IMU_Update_Freq 100.0f //frequency in Hz must equal to the frequency of IMU_Update() ``` ```c TIM3_Int_Init(100-1,8400-1); //10ms = (1000*840)/84 us = 10ms == 100hz = IMU_Update_Freq ``` 在定时器3中断处理函数中执行IMU_Update()更新姿态角： ```c 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); ![在这里插入图片描述](picture/20210516235902902.png)