MPU6050姿态结算

IMU（Inertial measurement unit），中文翻译为惯性测量单元，是一种能够测量物体三轴加速度和角速度进而解算出物体姿态的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。为了提高可靠性，还可以为每个轴配备更多的传感器，例如地磁传感器。
MPU6050属于IMU的一种，包含了三个单轴的加速度计和三个单轴的陀螺仪。

1. 加速度计原理与姿态解算

2. 陀螺仪原理与姿态解算

3. 融合加速度计和陀螺仪的数据

需要姿态融合的原因

为什么要有同时具备加速度计和陀螺仪。
看了上面的介绍，我们发现单单通过加速度计或者陀螺仪就能解算出物体姿态，为什么IMU要同时具备两种功能相似的仪器呢？

答案很简单：加速度计的数据不是100%准确的。有几个原因，还记加速度计测量的是惯性力，这个力可以由重力引起（理想情况只受重力影响），当也可能由设备的加速度（运动）引起。因此，就算加速度计处于一个相对比较平稳的状态，它对一般的震动和机械噪声很敏感。这就是为什么大部分的IMU系统都需要陀螺仪来使加速度计的输出更平滑。但是怎么办到这点呢？陀螺仪不受噪声影响吗？

陀螺仪也会有噪声，但由于它检测的是旋转，因此对线性机械运动没那么敏感，不过陀螺仪有另外一种问题，比如漂移（当选择停止的时候电压不会回到零速率电压）并且陀螺仪是通过对旋转角度的累加进行姿态解算，所以长时间工作会有极大的误差。然而，通过计算加速度计和陀螺仪的平均值我们能得到一个相对更准确的当前设备的倾角值，这比单独使用加速度计更好。

通过上面的分析我们可以发现，加速度计短时间内精度较差，而长时间稳定性好，陀螺仪短时间内精度高，但会累计误差，只要将两者的数据进行互补，就能解算出正确的姿态。

姿态融合

MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：

• 四元数法

• 一阶互补算法

• 卡尔曼滤波算法。

1. 四元数法

   关于四元数的一些概念和计算就不写上来了，我也不懂。我能告诉你的是：通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。

   ​ 虽然 MPU6050 自带的 DMP库可以直接输出四元数，减轻 STM32 的运算负担， 这里在此没有使用，因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的（http://bbs.elecfans.com/forum.ph … 4&page=1#pid3625735）

   DMP库需要对 I2C 函数进行修改，如 DMP 库中的 I2C 写：i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]))；有4个输入变量，而 STM32 硬件 I2C 的 I2C 写为：void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr)，只有 3 个输入量（这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的），所以必须进行修改，但是改固件库是一件很痛苦的事，你们应该都懂。当然，如果你用模拟 I2C 的话，是容易实现的，网上的 DMP 移植几乎都是基于模拟 I2C 的。

   要注意的的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。

2. 一阶互补算法

   MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan() 可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：

   //一阶互补滤波
   float K1 =0.1; // 对加速度计取值的权重
   float dt=0.001;//注意：dt的取值为滤波器采样时间
   float angle_P,angle_R;
    
   //angle_ax=atan(ax/az)*57.3;     //加速度得到的角度
   //gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度
   Pitch = yijiehubu_P(angle_ax,gy);
   Roll = yijiehubu_R(angle_ax,gy);
   
   float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度
   {
        angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
            return angle_P;
   }
    
   float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度
   {
        angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
            return angle_R;
   }

   互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法。单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。

3. 卡尔曼滤波

   其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。

   #include<math.h>
   #include "stm32f10x.h"
   #include "Kalman_Filter.h"
    
   //卡尔曼滤波参数与函数
   float dt=0.001;//注意：dt的取值为kalman滤波器采样时间
   float angle, angle_dot;//角度和角速度
   float P[2][2] = {{ 1, 0 },
                    { 0, 1 }};
   float Pdot[4] ={ 0,0,0,0};
   float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度
   float R_angle=0.5 ,C_0 = 1;
   float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
    
   //卡尔曼滤波
   float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy 
   {
           angle+=(gyro_m-q_bias) * dt;
           angle_err = angle_m - angle;
           Pdot[0]=Q_angle - P[0][1] - P[1][0];
           Pdot[1]=- P[1][1];
           Pdot[2]=- P[1][1];
           Pdot[3]=Q_gyro;
           P[0][0] += Pdot[0] * dt;
           P[0][1] += Pdot[1] * dt;
           P[1][0] += Pdot[2] * dt;
           P[1][1] += Pdot[3] * dt;
           PCt_0 = C_0 * P[0][0];
           PCt_1 = C_0 * P[1][0];
           E = R_angle + C_0 * PCt_0;
           K_0 = PCt_0 / E;
           K_1 = PCt_1 / E;
           t_0 = PCt_0;
           t_1 = C_0 * P[0][1];
           P[0][0] -= K_0 * t_0;
           P[0][1] -= K_0 * t_1;
           P[1][0] -= K_1 * t_0;
           P[1][1] -= K_1 * t_1;
           angle += K_0 * angle_err; //最优角度
           q_bias += K_1 * angle_err;
           angle_dot = gyro_m-q_bias;//最优角速度
    
           return angle；
   
   }

三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。

陀螺仪漂移的解决方法

横滚角和俯仰角的偏差可以通过卡尔曼滤波或者一阶互补滤波即可消除漂移量.

但是航向角存在的偏差很难由自身来校准,这是由于:
Z轴在静止状态下没有漂移，运动情况下有累积误差。因为Z轴角度是通过对角速度积分计算出来的，没有观测量滤波，所以漂移是不可避免的。X、Y轴的角度是根据重力加速度分量计算，X，Y轴是有重力场滤波，所以不会有漂移。Z轴只能用短时间内的相对测量量。 就是相邻两次的角度差来计算转过的角度。

可以采用带地磁的9轴陀螺仪来降低漂移量,但是仍然无法完全解决这个问题.而且地磁计对周围磁场的变化敏感,多磁场和多电机的工作环境都会导致地磁计不准.