陀螺仪姿态角解算——深挖MPU6050_角速度解算姿态

和陀螺仪的孽缘真的是剪不断理还乱，在研究过程中和它斗智斗勇，所遇到过的问题真是层出不穷，有很多的坑要注意，下面让我来向大家细细唠唠！！也算是回馈csdn了~~

陀螺仪是一种用于测量角速度或旋转速率的传感器。它通常用于惯性测量单元（IMU）或飞行控制系统中，用于测量飞行器、汽车或其他设备的姿态、方向或旋转速率。但是陀螺仪不能直接获取角度数据，他直接输出的是角速度，常常配合加速度计解算姿态角，但不是所有陀螺仪解算都有库的，可能需要自己用算法来融合角速度和加速度数据，解算姿态角，但在解算之前，其实真正容易被忽略的是陀螺仪初始化中的问题，和陀螺仪磋磨良久，积怨深厚，决定发文和大家深挖一下初始化和解算中出现的一些问题：

1.初始化

不知道有没有童鞋和我一样,刚使用陀螺仪的时候直接调用他的库然后开始用算法姿态解算或者使用DMP，但其实某些情况下，陀螺仪的初始化内容也是要改的，即它的库也要改，以32的MPU6050.h里的初始化代码为例：

void MPU6050_Init(void)
{
	//在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错
	Delay(100);
	MPU6050_WriteReg(MPU6050_RA_PWR_MGMT_1, 0x00);	    //解除休眠状态
	MPU6050_WriteReg(MPU6050_RA_SMPLRT_DIV , 0x07);	    //陀螺仪采样率
	MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);         //设置数字低通滤波器滤波周期                                
	MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x10);	//配置加速度传感器工作在16G模式
	MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);    //设置陀螺仪自检及测量范围
	Delay(200);
}

陀螺仪的初始化命令主要有以上5条，在将这5条初始化指令前，陀螺仪在初始化前有两个非常重要的注意事项，请大家一定要注意！！！否则可能导致初始化失败，程序卡死！！！

1.延时

有时候陀螺仪初始化失败有可能就是因为延时太短，我这里一开始是设置为5ms，然后初始化失败了，改成100ms就正常了，不管是库里面还是在调用初始化代码前，建议最好加个延时；

2.上电复位

系统上电复位后，一定要保持系统的静止或平稳，陀螺仪上电复位过程中出现明显晃动的话，会导致陀螺仪数据不准，无限往上加；

3.解算周期

陀螺仪解算角度一般有两种方式，一种是直接用现成的库，调用函数即可解算角度，另一种是使用滤波算法（互补滤波、卡尔曼滤波）或者四元组法来解算角度，但无论内种方式，解算角度都应该有固定的周期以便实时更新解算数据，同时要保证解算的周期要合理，不能过大或过小，但周期该如何确定呢？这就需要看初始数据（角速度、角速度）的采样频率了，而他们都需要通过初始化代码设置。

4.初始化代码讲解 

1.MPU6050_WriteReg（参数1，参数2）

mpu6050所有写入命令都是操作寄存器地址的，参数1输入的是陀螺仪寄存器地址的宏定义，参数2输入的是要赋给寄存器的值，通过mpu6050.h文件，我们可以查看到这些宏对应的寄存器地址，再通过寄存器地址就可以查看到操作的是哪个寄存器，初始化代码中除了第一行的解除休眠，都可能需要改动，下面我来结合寄存器细细分析：

2.MPU6050_RA_SMPLRT_DIV

这个宏对应的寄存器地址为0X19,也就是陀螺仪采样率分频寄存器，他用来设置陀螺仪的采样周期，这里要结合姿态角解算的周期进行调整，如果采样周期慢于解算的周期，那么姿态角更新就有可能出现滞后和紊乱的情况，或者反过来说，解算的周期不能小于陀螺仪的采样周期；

寄存器部分的内容介绍引用自博主Wu__La链接如下：超详细陀螺仪MPU6050模块输出姿态角(有完整版源码)_Wu__La的博客-CSDN博客

这位博主有介绍了一下换算关系，有兴趣的可以去看看，参数2的值就是配置分频寄存器的8位，默认情况下寄存器配置为0x07

带入计算公式，采样频率 = 陀螺仪输出频率 / (1+0x07)=1KHZ或125HZ(这里就看输出频率了，输出频率的值可能为8KHZ或者1KHZ，主要受数字滤波器影响，而下一条初始化指令就是配置滤波器，让我们接下来往下看)

ps：加速度采样率固定为1KHZ，无法分频

3.MPU6050_RA_CONFIG

陀螺仪的输出信号通常是模拟信号，其幅度和频率与被测量的角速度相关。然而，陀螺仪输出信号也可能包含一些噪音和干扰。为了提高测量的精确性和减小噪音的影响，陀螺仪自带的数字滤波器来减小噪音。

配置寄存器（0X1A）的低3位的值即DLP_CFG[2:0],决定了是否开启数字滤波器和陀螺仪输出采样率：当DLP_CFG[2:0]的值为0或7时滤波器被禁用，且将陀螺仪输出采样率配置为8KHZ，其余值时默认开启数字滤波器，且将陀螺仪输出采样率配置为1KHZ；同时也配置着陀螺仪输出信号的带宽（截止频率）。如下表格所示，不同的值对应不同的带宽，带宽定义了陀螺仪输出信号能够传递的频率范围。所以带宽越高灵敏度和响应速度越高。

较高的带宽设置会减少滤波效果，使得陀螺仪输出的信号更加接近原始数据，可以实现更快的响应速度，适用于需要快速变化和高频旋转的应用场景，但也会将更多的噪声传递到输出中。较低的带宽设置会增加滤波效果，使得陀螺仪输出的信号更加平滑，减少噪声的影响，可以获得更稳定和可靠的输出，适用于需要高精度和低噪声的应用场景，但会降低陀螺仪的响应速度和灵敏度。

带宽的选择要结合实际，但如果配置的值是DLP_CFG[2:0]非0和7也要注意到此时陀螺仪的输出采样率为1KHZ，若配置分频为0x07，角速度采样频率 = 陀螺仪输出频率 / (1+0x07)=125HZ，加速度采样频率仍然为1KHZ，此时单位时间内的加速度的数据获取是有重叠的。总体取下，系统对陀螺仪的采样时间就应该在1/125=8ms上下，否则可能会导致数据失准。以次类推

ps1：使用数字滤波器的优缺点：

有滤波器噪声小，数据更准，比较不飘，但会使角速度频率下降，同时因为滤波器的采样计算需要时间，会导致延时，比如配置

MPU6050_WriteReg(MPU6050_RA_CONFIG , 0x06);     

滤波器延时为19ms/18.6ms，加速度和角速度采样频率再快也没用，最终输出数据的频率还是看滤波器的延时周期。

ps2：陀螺仪对外输出数据的频率：

未开启数字滤波器时，角速度数据输出频率根据分频，加速度固定为1KHZ，开启数字滤波器时，延时周期就是最终的数据输出周期。

4.MPU6050_RA_ACCEL_CONFIG

加速度配置寄存器（0X1C）

MPU6050_WriteReg(MPU6050_RA_ACCEL_CONFIG , 0x10);    //配置加速度传感器工作在16G模式（+-8g就是16g）

//0x00、0x01、0x10、0x11 分别对应 +-2g、+-4g、+-8g、+-16g     

5.MPU6050_RA_ACCEL_CONFIG

MPU6050_WriteReg(MPU6050_RA_GYRO_CONFIG, 0x18);    //设置陀螺仪自检及角速度测量范围为+-2000°/s

//0x18、0x10、0x08、0x00 分别对应+-2000°/s、+-1000°/s、+-500°/s、+-250°/s

新人博主首次扣字，想说的太多太多liao，先立个flag，下次咱继续唠陀螺仪姿态解算和dmp解算姿态角，更完之后想唠一下这届的电赛@@