四轴—MPU6050

准大二生为了理奥杯（只是院级比赛- -！）准备制作微型四轴从MPU6050入手在网上查资料的过程中发现资料有些混乱，现总结以供人参考

MPU6050

MPU6050是6轴（可以另配磁力计为9轴）传感器：
1.测量三轴加速度的加速度计
2.测量三轴角速度的传感器陀螺仪
3.测量三轴磁力的磁力计（需要另配）
就下来就大致讲解加速度计和陀螺仪的原理和各自的特性

1. 加速度计
   
   球放在地面上Z轴读数-1g现让其右旋45°
   
   可以读到X和Z轴读数为-0.71g这是近似SQRT（1/2）的.要知道只受重力场时，x^2+Y^2+z^2 =1g
   下面换一个模型来表达
   
   通过三轴传感器我们可以知道R在三轴上的分量命名为Rx Ry Rz由此易得：
   R²=Rx²+Ry²+Rz² →R=SQRT（Rx²+Ry²+Rz² ）
   Axr = arccos(RX / R)
   Ayr = arccos(RY / R)
   Azr = arccos(RZ / R)
   在这之前 我们先来看看更有用的公式：
   cosX = cos(Axr) = Rx / R
   cosY = cos(Ayr) = Ry / R
   cosZ = cos(Azr) = Rz / R
   这三个公式通常被称为方向余弦。你可以轻松地验证：
   SQRT（cos²X+ COS²Y+ cos²Z）= 1
   这个公式可以避免矢量模的运算，算的都只是矢量的单位向量，因为我们只对矢量的方向有兴趣。

2. 陀螺仪
   
   MPU6050带有三个陀螺仪，每个陀螺仪各自负责检测相应轴的转动速度，也就是检测围绕各个轴转动的速度。像三轴的陀螺仪将同时检测 X Y Z轴的旋转。
   由上面这个模型图，首先我们定义：
   Rxz - 是R向量在XZ平面上的投影
   Ryz - 是R向量在XY平面上的投影
   Rxz和Rz所形成的直角三角形，利用勾股定理，我们得到：
   Rxz² = Rx²+ Rz²，和同样：
   Ryz² = Ry ² + Rz²
   同时我们将定义Z轴和Rxz 、Ryz之间的夹角
   Axz - Rxz和Z轴间的夹角
   Ayz - Ryz和Z轴间的夹角
   现在看看,从这个模型中,陀螺仪测量什么?
   上述的说明,已经知道陀螺仪测量角度的变化率.为了解释这一点，让我们假设，我们已经测量围绕Y轴的旋转角（这将是Axz角）在时刻t0，我们将其定义为Axz0，接下来，我们测量这个角度是在稍后的时间t1是Axz1。变化率将被计算如下：
   RateAxz =（Axz1 - Axz0）/（t1 - t0）
   如果Axz单位是度，并以秒为时间单位，那么RateAxz将以deg / s表示。

理解
1.陀螺仪
测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。陀螺仪就是内部有一个陀螺，它的轴由于陀螺效应始终与初始方向平行，这样就可以通过与初始方向的偏差计算出旋转方向和角度。传感器MPU6050实际上是一个结构非常精密的芯片，内部包含超微小的陀螺。
如果这个世界是理想的，美好的，那我们的问题到此就解决了，从理论上讲只用陀螺仪是可以完成姿态导航的任务的。只需要对3个轴的陀螺仪角速度进行积分，得到3个方向上的旋转角度，也就是姿态数据。这也就是说的快速融合。不过很遗憾，现实是残酷的，由于误差噪声等的存在，对陀螺仪积分并不能够得到完全准确的姿态，尤其是运转一段时间以后，积分误差的累加会让得到的姿态和实际的相差甚远。那么哪些原因会使陀螺仪得到的姿态结果不准确呢？下面列举除常见的几种：
1.零点漂移
假设陀螺仪固定不动，理想角速度值是0dps(degree per second)，但是存在零点漂移，例如有一个偏置0.1dps加在上面，于是测量出来是0.1dps，积分一秒之后，得到的角度是0.1度，1分钟之后是6度，还能忍受，一小时之后是360度，转了一圈，也就是说，陀螺仪在短时间内有很大的参考价值。
2.白噪声
电信号的测量中，一定会带有白噪声，陀螺仪数据的测量也不例外。所以获得的陀螺仪数据中也会带有白噪声，而且这种白噪声会随着积分而累加。
3.温度/加速度影响
陀螺仪是一个温度和加速度敏感的元器件。例如对于加速度，多轴飞行器中的马达一般会带来较强烈的振动，一旦减震控制不好，就会在飞行过程中产生很大的加速度，必会带来陀螺输出的变化，引入误差。这就是在陀螺仪数据手册上常见的deg/sec/g指标。
4.积分误差
对陀螺仪角速度的积分是离散的，长时间的积分会出现漂移的情况。所以要考虑积分误差的问题。
正是由于陀螺仪测量姿态存在这么多的误差，所以我们必须要使用其它传感器辅助校正，其中最重要的就是下面的加速度传感器。

2.加速度
低频特性好，可以测量低速的静态加速度。在我们的飞行器上，就是对重力加速度g（也就是前面说的静态加速度）的测量和分析，其它瞬间加速度可以忽略。记住这一点对姿态解算融合理解非常重要。当我们把加速度计拿在手上随意转动时，我们看的是重力加速度在三个轴上的分量值。加速度计在自由落体时，其输出为0。为什么会这样呢？这里涉及到加速度计的设计原理：加速度计测量加速度是通过比力来测量，而不是通过加速度。加速度计仅仅测量的是重力加速度，3轴加速度计输出重力加速度在加速度计所在机体坐标系3个轴上的分量大小。重力加速度的方向和大小是固定的。通过这种关系，可以得到加速度计所在平面与地面的角度关系.
加速度计若是绕着重力加速度的轴转动，则测量值不会改变，也就是说加速度计无法感知这种水平旋转。

3.有关陀螺仪和加速度计和关系，姿态解算融合的原理，再把下面这个比喻放到这里一遍。

机体好似一条船，姿态就是航向（船头的方位），重力是灯塔，陀螺（角速度积分）是舵手，加速度计是瞭望手。舵手负责估计和把稳航向，他相信自己，本来船向北开的，就一定会一直往北开，觉得转了90度弯，那就会往东开。当然如果舵手很牛逼，也许能估计很准确，维持很长时间。不过只信任舵手，肯定会迷路，所以一般都有瞭望手来观察误差。

瞭望手根据地图灯塔方位和船的当前航向，算出灯塔理论上应该在船的X方位。然而看到实际灯塔在船的Y方位，那肯定船的当前航向有偏差了，偏差就是ERR=X-Y。舵手收到瞭望手给的ERR报告，觉得可靠，那就听个90%ERR，觉得天气不好、地图误差大，那就听个10%ERR，根据这个来纠正估算航向。

姿态融合算法

一.DMP
由官方提供基于MSP430的姿态结算,正点原子论坛上有移植到stm32上的程序实测效果很好。

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

//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float dt=0.001;//注意：dt的取值为滤波器采样时间
float angle;

angle_ax=atan(ax/az)*57.3;     //加速度得到的角度
gyro_m=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度
Pitch = yijiehubu(angle_ax,gyro_m);

float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度
{
     angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
     return angle;
}

互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：

这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。就是，短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过滤波在哪。因为加速度计不仅能够感受到静态的加速度，还会感受到动态的加速度，而我们真正需要的是静态加速度，这里就需要对动态加速度进行滤波。可以考虑使用数字低通滤波器。而角速度传感器可以预知角度的变化，但是其中的低频信号对系统没有用处，应该滤掉，所以相应的，在综合求倾角的时候要加上一个数字高通滤波器。可以通过以下公式去计算角度：
angle = a × ( angle + gyro × dt ) + (1 -a ) × ( X _ acc)
例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率

公式：Angle = (0.98)(Angle + Gyro dt) + (0.02)*(Acc);

1、其中Angle+Gyro*dt是积分部分。（角度+角速度*取样周期）。
2、(0.98)(Angle + Gyro dt),是高通部分，作用于陀螺仪上。
3、(0.02)*(Acc)是低通部分，作用于加速度计。
4、其中0.98,0.02是可以根据自己的情况修改的。公式原样应该是Angle = (a)(Angle + Gyro dt) + (1-a)*(x_acc);
时间常数t=(a*dt)/(1-a) , 得到a=t/(t+dt)。dt为取样周期，t为时间常数自己设定，一般小于1。
比如取样周期为100HZ，也就是0.01s,那么dt=0.01。时间常数设为0.5（对于不同的系统都不一样，自己试验），那么a=0.5/(0.5+0.01)=0.9804。
那么Angle = (0.9804)(Angle + Gyro dt) + (0.0196)*(Acc);
5、对于第4点，如果觉得有问题，我觉得还可以这么理解：陀螺仪采集回来的总是有滞后的，但是加速度计总是超前的，两者是需要综合的，我们相信陀螺仪多一点，相信加速度计少一点，是采集回来的角度可以更好的适应我们的系统。

三.卡尔曼滤波
其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。卡尔曼原理以及什么5个公式等等的，我也不太懂，就不写了，感兴趣的话可以上网查。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。

#include
#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；
}