Mpu6050——学习笔记

A 引入

A.1 模块和单片机型号说明

模块:mpu6050
单片机:stm32f103c8t6
函数库:标准库

当然，这只是一个大学生的学习笔记

B 模块介绍

MPU6050是一个整合型的六轴运动处理模块，内部整合了3轴的陀螺仪和3轴的加速度计，还有一个温度传感器

常见的姿态传感器：

六轴（加速度计+陀螺仪），其中加速度计测量的三轴的加速度，陀螺仪测量的是三轴的角速度
九轴（加速度计+陀螺仪+磁力计）磁力计用来解决六轴中偏航角的漂移，其中磁力计测量的时候三轴的磁场强度
十轴（加速度计+陀螺仪+磁力计+气压计）其中气压计用来测量高度

B.1 引脚定义

陀螺仪引脚定义

引脚编号	定义	说明
1	VCC	电源引脚，接3.3V或5V电源
2	GND	接地引脚
3	SCL	I2C时钟线
4	SDA	I2C数据线
5	XDA	辅助I2C数据线（通常未使用）
6	XCL	辅助I2C时钟线（通常未使用）
7	INT	中断引脚，用于输出中断信号

MPU6050 通常使用 I2C 通信，通过操作内部的寄存器来配置 MPU6050 的工作模式并进行数据的读写。其中，XDA 和 XCL 引脚可以外接磁力计，用于扩展为九轴传感器，从而解决六轴传感器在偏航角测量中存在的漂移问题。
这种扩展方式可以显著提高姿态解算的精度，特别是在需要精确航向角的应用场景中。

C 原理介绍

C.1 姿态角介绍

姿态角是描述物体在三维空间中姿态的一组角度参数，通常包括三个角度：偏航角（Yaw）、俯仰角（Pitch）和横滚角（Roll）。它们分别对应物体绕垂直轴、横轴和纵轴的旋转角度，又称欧拉角。

如下图，以飞机为水平面，飞机朝向为x轴，机翼朝向为y轴，垂直方向为z轴

偏航角（Yaw）
偏航角描述物体绕垂直轴（通常是Z轴）的旋转角度，反映物体的航向变化。偏航角在六轴传感器中容易受到漂移影响，通常需要通过外接磁力计来校正。
俯仰角（Pitch）
俯仰角描述物体绕横轴（通常是Y轴）的旋转角度，反映物体的前后倾斜程度。加速度计和陀螺仪的结合可以较为准确地测量俯仰角。
横滚角（Roll）
横滚角描述物体绕纵轴（通常是X轴）的旋转角度，反映物体的左右倾斜程度。与俯仰角类似，横滚角也可以通过加速度计和陀螺仪的融合来测量。

在 MPU6050 中，通过加速度计和陀螺仪的原始数据，结合姿态解算算法（如卡尔曼滤波或互补滤波），可以计算出物体的姿态角。这些角度广泛应用于无人机、机器人、运动追踪等领域，用于实现姿态控制和导航。

C.2 加速度计

加速度计实际上是一个弹簧测力计，根据牛顿第二定律 F=ma，想要测量加速度 a，只需要找一个单位质量的物体，测量它所受的力 F 就行了。
在 MPU6050 中，加速度计通过检测内部微机械结构（MEMS）的位移来测量加速度。当模块受到外力作用时，内部的质量块会发生微小的位移，这种位移会通过电容变化或压电效应转换为电信号，从而计算出加速度值。

MPU6050 的加速度计可以测量三个方向（X、Y、Z）的加速度，后面表示为：ax，ay，az

//在静态情况下（基本只有重力加速度）
roll  =  atan2 (ay,az)
pitch = -atan2 (ax,az)
//但是在水平状态下，无法测量yaw（偏航角）

当然，如果想增加稳定性可以使用下面的公式，对线性加速度稍鲁棒，误差较小

横滚角（Roll）
横滚角描述物体绕 X 轴的旋转角度：
俯仰角（Pitch）
俯仰角描述物体绕 Y 轴的旋转角度：

加速度计具有静态稳定性，不具有动态稳定性
因为假设在向x轴正方向加速的时候，加速度计在z轴和x轴上都有读数，但是它无法判断这是因为倾斜带来的还是加速带来的，而且也会有振动噪声产生，使结果不稳定

C.3 陀螺仪

陀螺仪是一种利用角动量守恒原理来测量角速度的传感器。它可以检测物体绕三个轴（X、Y、Z）的旋转运动。在 MPU6050 中，陀螺仪通过微机械结构（MEMS）检测角速度，并输出对应的电信号。

需要注意的是，MPU6050 测量的是角速度（单位通常为 °/s），而不是角度。
如果需要得到角度值，可以通过对角速度进行积分来计算。

然而，由于积分过程中可能会累积误差（产生漂移），通常需要结合加速度计的数据，通过姿态解算算法（如卡尔曼滤波或互补滤波）来提高精度。

MPU6050 的加速度计可以测量三个方向（X、Y、Z）的角速度，后面表示为：gx，gy，gz

横滚角（Roll）
俯仰角（Pitch）
偏航角（Yaw）

：时间间隔，单位为秒，通常由 MPU6050 的采样频率决定。

陀螺仪具有动态稳定性，不具有静态稳定性
因为陀螺仪可以准确测量物体的角速度，特别是在动态运动中表现出色。
但是，如果传感器静止下来，则没有加速度，便无法通过积分获得当前的姿态，尤其如果环境中还存在噪声，角速度会因为噪声而无法完全归零，经过积分会产生偏移

所以当加速度计和陀螺仪一起使用的时候，通过互补滤波，卡尔曼滤波等，就可以得到相对稳定的姿态角

D 驱动代码

D.1 如何用I2C读取MPU6050原始值

MPU6050使用I2C协议通信，我们可以看它的寄存器手册知道如何配置采样率量程等参数，或者是读取角速度加速度等数据

举个栗子:

//假设我们I2C已经配置好了
void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
 MyI2C_Start();                               //I2C起始
 MyI2C_SendByte(MPU6050_ADDRESS);             //发送从机地址，读写位为0，表示即将写入
 MyI2C_ReceiveAck();                          //接收应答
 MyI2C_SendByte(RegAddress);                  //发送寄存器地址
 MyI2C_ReceiveAck();                          //接收应答
 MyI2C_SendByte(Data);                        //发送要写入寄存器的数据
 MyI2C_ReceiveAck();                          //接收应答
 MyI2C_Stop();                                //I2C终止
}

uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
 uint8_t Data;//定义接受的数据储存位置
 
 MyI2C_Start();                             //I2C起始
 MyI2C_SendByte(MPU6050_ADDRESS);           //发送从机地址，读写位为0，表示即将写入
 MyI2C_ReceiveAck();                        //接收应答
 MyI2C_SendByte(RegAddress);                //发送寄存器地址
 MyI2C_ReceiveAck();                        //接收应答
 
 MyI2C_Start();                             //I2C重复起始
 MyI2C_SendByte(MPU6050_ADDRESS | 0x01);    //发送从机地址，读写位为1，表示即将读取
 MyI2C_ReceiveAck();                        //接收应答
 Data = MyI2C_ReceiveByte();                //接收指定寄存器的数据
 MyI2C_SendAck(1);                          //发送应答，给从机非应答，终止从机的数据输出
 MyI2C_Stop();                              //I2C终止
 
 return Data;//返回读取到的值
}

我们可以看到手册中的角速度值由0X3B和0X3C的高8位和低8位组成，所以假设我们要获得角速度的时候，要读取两次值，然后移位拼接得到真正的原始值，而16位二进制表示有符号的范围是-32768~32767，所以如果换算成+-250°/s 就可以直接除以131，同理，如果读取的加速度，假设要换算成+-2g，就可以直接除以16384

下面是手册中关于如何利用原始值换算成我们平常用的值

D.2 如何配置MPU6050


//下面只是初始化的框架，具体函数内容和参数可以根据手册更改
void MPU6050_Init(void)
{ 
    u8 res; 
    MPU6050_IIC_IO_Init(); // 初始化 IIC 总线
    mpu6050_write_reg(PWR_MGMT_1, 0X80); // 复位 MPU6050
    delay_ms(100);
    mpu6050_write_reg(PWR_MGMT_1, 0X00); // 唤醒 MPU6050
    MPU_Set_Gyro_Fsr(3); // 设置陀螺仪量程为 ±2000dps
    MPU_Set_Accel_Fsr(0); // 设置加速度计量程为 ±2g
    MPU_Set_Rate(100); // 设置采样率为 100Hz
    mpu6050_write_reg(MPU_INT_EN_REG, 0X00); // 关闭所有中断
    mpu6050_write_reg(MPU_USER_CTRL_REG, 0X00); // 关闭 I2C 主模式
    mpu6050_write_reg(MPU_FIFO_EN_REG, 0X00); // 关闭 FIFO
    mpu6050_write_reg(MPU_INTBP_CFG_REG, 0X80); // 配置 INT 引脚为低电平有效
    res = mpu6050_read_reg(MPU_DEVICE_ID_REG); // 读取设备 ID
    if (res == MPU_ADDR) // 检查设备 ID 是否正确
    {
        mpu6050_write_reg(PWR_MGMT_1, 0X01); // 设置时钟源为 PLL X 轴参考
        mpu6050_write_reg(PWR_MGMT_2, 0X00); // 启用加速度计和陀螺仪
        MPU_Set_Rate(100); // 再次设置采样率为 100Hz
    }
}

D.3 如何利用原始值解算姿态角

下面先介绍不使用DMP解算姿态角的方法，在前面我们已经可以通过加速度计和陀螺仪，得到ax，ay，az，gx，gy，gz，然后计算出2个roll，2个pitch和1个yaw，然后我们可以通过互补滤波来融合两组值来得到一个粗略的结果

互补滤波公式：

互补滤波是一种简单有效的姿态解算方法，通过结合加速度计和陀螺仪的数据，利用两者的优点来计算稳定的姿态角。其公式如下：

横滚角（Roll）
俯仰角（Pitch）
偏航角（Yaw）

参数说明：

、、：上一时刻的姿态角。
、、：陀螺仪在 X、Y、Z 轴上的角速度，单位通常为 °/s。
：时间间隔，单位为秒，通常由 MPU6050 的采样频率决定。
：滤波系数，范围为，用于平衡加速度计和陀螺仪的权重。通常取值接近 0.98。

注释事项：

偏航角（Yaw）的计算通常需要磁力计来校正，因为加速度计无法提供水平状态下的偏航角信息。
滤波系数的选择需要根据实际应用场景调整，较大的值会更依赖陀螺仪，较小的值会更依赖加速度计。

滤波系数 α 的取值：

滤波系数可以通过以下公式粗略计算：

其中：

：陀螺仪的时间常数，单位为秒，粗略直接取0.1
：采样时间间隔，单位为秒。

对面上面这个粗略的计算方法，可以直接在stm32中计算实现，但是结果会有很大的误差和抖动，所以官方给我提供了一个方法：
DMP是mpu6050中传感器内部集成的硬件模块（Digital Motion Processor，数字运动处理器），它具有运算能力，通过获得原始数据，可能使用卡尔曼滤波和四元数解算姿态角，可以让我们获得更精准的结果，并且节省主控的资源

D.4 如何使用DMP库读取解算后的姿态角

D.4.1 移植DMP库

ps:咳咳，我自己没移植成功，还是先借用别人移植好的吧

下面提供下载官方移植源码的方式：

打开官网：https://invensense.tdk.com/
找到相关固件界面
滑到最下面找到eMD5.1.3或eMD6.12
点击之后会让你登录，选择注册，注意会发送给你的邮箱check邮件，找不到的可以在垃圾邮箱里面翻一下
然后重新上面的流程就可以下载了，如果下载速度过慢可以试试魔法
下载完之后在文件中选中下面这几个文件，就可以开始移植了
参考可以看MSP430的驱动，就是下图文件

D.4.1 使用DMP库

跳过最难的配置dmp环节,下面是或得姿态角的关键代码


// 获取 DMP 解算后的姿态角（注意：需要先初始化 DMP 并正确配置）
// pitch: 俯仰角，单位：0.1°，范围：-90.0° <---> +90.0°
// roll: 横滚角，单位：0.1°，范围：-180.0° <---> +180.0°
// yaw: 偏航角，单位：0.1°，范围：-180.0° <---> +180.0°
// 返回值：0 表示成功，1 表示读取 FIFO 数据失败，2 表示未检测到四元数数据
u8 MPU6050_DMP_Get_Data(float *pitch, float *roll, float *yaw) // 每 5ms 读取一次，200Hz 采样率
{
    float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 初始化四元数
    unsigned long sensor_timestamp;                  // 传感器时间戳
    short gyro[3], accel[3], sensors;                // 陀螺仪、加速度计数据和传感器标志
    unsigned char more;                              // FIFO 中是否有更多数据
    long quat[4];                                    // 四元数数据（Q30 格式）

    // 从 FIFO 中读取数据（包括陀螺仪、加速度计和四元数）
    if (dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more)) 
        return 1; // 如果读取失败，返回 1

    /* 
     * 陀螺仪和加速度计数据以芯片坐标系和硬件单位写入 FIFO。
     * 这种行为确保 dmp_read_fifo 和 mpu_read_fifo 的输出一致。
     */

    // 检查是否包含四元数数据
    if (sensors & INV_WXYZ_QUAT) 
    {
        // 将 Q30 格式的四元数转换为浮点数
        q0 = quat[0] / q30;
        q1 = quat[1] / q30;
        q2 = quat[2] / q30;
        q3 = quat[3] / q30;

        // 使用四元数计算俯仰角（pitch）、横滚角（roll）和偏航角（yaw）
        *pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // pitch = arcsin(-2 * q1 * q3 + 2 * q0 * q2)
        *roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll = atan2(...)
        *yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw = atan2(...)
    }
    else 
        return 2; // 如果未检测到四元数数据，返回 2

    return 0; // 成功返回 0
}

这个函数通过 dmp_read_fifo 函数从 FIFO 中读取陀螺仪、加速度计和四元数数据，然后根据公式计算出姿态角，然后调用这个函数在我们需用使用的模块即可

总体的流程就是：

  graph TD;
    A[配置DMP] --> B[DMP读取原始值]
    B --> C[DMP进行融合计算]
    C --> D[DMP输出四元数]
    D --> E[主控读取四元数]
    E --> F[主控计算出姿态角]
    F --> G[使用计算的姿态角]

E 如何外接磁力计

ps：还在学习ing

参考文献

铁头山羊stm32入门教程6.4 MPU6050(上)

MPU6050 6轴姿态传感器的分析与使用（一）

文件地址

完成后上传

留言

有问题请指出,你可以选择以下方式：

在下方评论区留言

邮箱留言

Mouse_blog