一、陀螺仪简介

实验室常用的两种陀螺仪：
mpu6050
icm20602

不管是ICM还是MPU，精度都能达到要求。

陀螺仪是感受空间姿态的传感器，是控制小车平衡，判断和调节姿态的核心元件。
六轴陀螺仪结合了三轴陀螺仪和三轴加速度计，其“六轴”分别为加速度xyz轴，角速度xyz轴，即既能感知角度变化，也能感知加速度变化，这六个量经过运算，可以返回三个姿态角：
俯仰角（车头绕Y轴翘起角度），

航向角（车身绕Z轴旋转角度），

翻滚角（侧轮绕X轴抬起角度）。

陀螺仪在竞速智能车中的主要作用有以下几点：

1. 与转向环串级使用，与Z轴角速度关联，增加转向力度；
2. 调整平衡车姿态，使车身平衡且能正常转向和前进
3. 角度环闭环控制的输入，比如过三叉路打60度角；
4. 判断车身姿态，调整状态。比如感知过环岛的角度变化；感知翻车，进行保护；
5. 温度传感器，电机高温报警（陀螺仪一般离电机较近，自身带有温度传感器）

二、基本的使用方法

六轴陀螺仪结合了三轴角速度计和三轴加速度计。

1.角速度计

角速度计返回的数据实际上是弧度制的角速度，旋转陀螺仪时，返回相应轴的角速度参考量。一般我们要对角速度积分得到角度，用起来直观方便。积分得到的角度是相对角度，开始积分时的姿态即为参考0角度。
对于2ms的积分值为0.000124左右，实际使用时需要调整，将陀螺仪转动一周，计算得到的角度应为360。

/***放于定时器中2ms执行一次***/
fun()
{
	Angle_yaw += GZ * 0.00012480f;
}

积分总会带来误差，随着时间不断增加。另外，陀螺仪还存在零点漂移现象，即车静置时会输出非0的角速度值， 每次上电时需要重新校准偏移值，或者定期校准，后续检测时减去偏移值即可消除此误差。

float GX_Zero = -11.5;//零点偏移值
float GY_Zero = 6;
float GZ_Zero = -1;
......
GX = gx0 - GX_Zero; //去零漂之后的陀螺仪采集值
GY = gy0 - GY_Zero;
GZ = gz0 - GZ_Zero;

2.加速度计

加速计返回的加速度并不是一般意义的加速度。 加速度计相当于一个重锤在中间的弹簧系统，四面八方有弹簧撑着它。平放在桌面时，有的弹簧被拉长，有的被压扁。变化时，不同的弹簧受到不同的压缩，从而侦测出不同方向的力。因此实际上静止不动时也会产生由于重力引起的“加速度”数值。注意这个“加速度”并不稳定，由于车模本身的摆动所产生的加速度会产生很大的干扰信号，因此不宜过于信赖这个数值。

只用加速度计就能得到车身的俯仰角，且是对地的绝对角度。摆动加速度计时，对应轴的重力加速度分量改变，利用反三角函数即可求出角度的变化。但因为加速度计的不稳定，叠加在重力测量信号上使输出信号不能很好的反映车模的倾角。

#include <math.h>
fun()
{
	angle_ratio = (double)(AX / (AZ + 0.1));//+0.1防止分母为0
	Angle_acc = (float)(atan(angle_ratio) * 57.29578049); 
	//加速度计得到的角度  57.3=180/3.14
}

三、更精确的数据处理

因为数据存在误差、不稳定、有零漂，因此需要对数据进行处理。

1.滤波

输入级使用算数平均滤波就可以，取十次求平均一般比较稳定。如果要使用加速度，可以再加一个系数比较小的低通滤波。
角速度记得去掉零点偏移值。

2.角度融合

角速度积分得到的角度比较稳，但会随时间慢慢漂移且只反映变化量；加速度取反三角函数得到的角度不稳，但跟随真实角度，且是对地的绝对角度。
平衡车整个过程都要用到角度这个变量，单纯角速度积分带来的漂移偏差不可忽视，而且参考点不好确定，所以需要用角度融合，将两者得出的角度进行滤波融合，取长补短，就能得到比较可靠的角度。

角度融合滤波的方法比较常用的有：互补滤波、二阶互补滤波、卡尔曼滤波、清华滤波、四元数滤波。
以下为互补滤波程序

/***互补滤波角度计算***/
void AngleGet(void
{
        float dt = 0.0001249;//Gy 2ms时间积分系数
        double angle_ratio;//加速度比值
        Anglefiltering();//入口滤波，算数平均

         /***以下为加速度计取反正切得到角度***/
        angle_ratio=((double)ax)/(az+0.1);
        Angle_acc=(float)atan(angle_ratio)*57.29578049;//加速度计得到的角
        if(Angle_acc > 89)
          Angle_acc = 89;
        if(Angle_acc < -89)
          Angle_acc = -89;
        
         /***以下为角速度计积分，同融合加速度，得到角度***/
        Gy = (float)(gy)
        GY = Gy -GY_Zero; //去零漂之后的陀螺仪采集值

        Angle = (float)(Angle-(float)(GY * dt));
        Angle = Angle + (Angle_acc-Angle)*0.001; 
		//相当于Angle = Angle*(1-0.00105) + Angle_acc*0.001
}

最后两条语句是最关键的部分，简单解释一下：
当前值（陀螺仪）＝上次值＋角速度微分值
当前值（融合）＝当前值（陀螺仪）*(1-a) - 当前值（加速度)*a （a为权重）

陀螺仪积分得到的角度稳定，加速度得到的角度最接近真实值
以陀螺仪积分算出来的角度为主导，用加速度得到的角度作纠正，防止角度误差随时间不断累计

“a”要调到什么状态呢？让角度既要跟随真实角度，不能太滞后，又不能有太大波动

互补滤波的调试过程可以看这个视频
https://www.bilibili.com/video/BV1Y541177rD。
不管是互补滤波还是四元数滤波，拿到后都需要重新调参，车不一样参数也不一样。
车刚开始启动时要注意，如果用融合算法得到角度值，会有从值比较小的角速度积分，逐渐过渡到真实值的过程，持续时间大概在0.3s到1s，因此刚启动的这段时间用融合角度不合适，而只用加速度取反三角函数得到的角度更真实。

/***以下函数放于定时器中2ms执行一次***/
void Angle_get(void)
{
  static int angle_start_flag = 0;//刚开始，单独使用加速度标志
  double angle_ratio = 0; //加速度比值
  static float Angle_acc_last = 0;

	*******（滤波过程略）

	/***以下为刚开始时的加速度角度单独处理***/
  {
    Angle_acc_last = Angle_acc;
    angle_ratio = (double)(a_x / (a_z + 0.1));
    Angle_acc = (float)(atan(angle_ratio) * 57.29578049); //加速度计得到的角度

    if (angle_start_flag == 0) //低通限幅滤波
    {
      Angle_acc_last = Angle_acc;
    }
    Angle_acc = 0.5 * Angle_acc + 0.5 * Angle_acc_last;
    if (Angle_acc > 89)
      Angle_acc = 89;
    if (Angle_acc < -89)
      Angle_acc = -89;
  }

	*******（融合过程略）

  if (angle_start_flag < 300) //刚开始运行的0.6s时，取加速度轴得到的角度
  {
    Angle = Angle_acc;//加速度得到的角度
    angle_start_flag++;
  }
  else
  {
    Angle = pitch;//融合后的结果
  }
}

若只是暂时利用角度处理别的东西，比如用来计算入环岛360度角度，入三叉60度角度，用角速度积分足够了。 计算周期为2ms的积分系数大概是0.0124，慢跑跟剧烈晃动的跑积分出来的角度值有小偏差，因此不要太过信赖这个角度。