平衡小车之陀螺仪调参

一、平衡小车

平衡小车作为控制进阶，成为每个学习PID控制算法者的必经之路，可难可易，找到方法则非常容易，找不到方法可能盲调数天也没有收获。

要让小车平衡，我认为需要掌握的技能有几点：学会如何搭建小车的机械，找到小车的平衡点，知道一些基本的驱动原理及PWM的控制，对PID控制算法了解会运用（角度环，速度换，转向环），学会看波形，会分析现象并调参。只要掌握以上几点，小车就能稳稳当当的立起。

本篇平衡小车的方案主要是通过陀螺仪MPU6050传感器检测角度实时得到反馈并进行。本篇重点主要分析如何在调参之前确定什么样的波形才是好的。因此对PID如何调参不进行描述。

二、mpu6050介绍

mpu6050具有3轴角速度感测器(陀螺仪)和3轴加速器。因此可以达到移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移。 

MCU通过IIC接口来控制MPU6050，MPU6050是通过SCL和SDA引脚来连接MCU的IIC接口。

对mpu6050来说，加速度计对四轴或小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。

三、陀螺仪调参

加速度计：当测试小车时，人们常常需要测量车辆的动态运动以及小车相对于道路的倾角。我们可以通过加速度计来获得车辆转弯、加速或者制动时产生的冲击力，但是，除非车辆在进行上述运动时保持水平，否侧测试结果是不准确的。

误差分析：比如加速度计测量车辆的制动力，但车辆是向前倾斜的，测量结果中就会有重力分量。大多数倾斜传感器把重力方向当作参考方向。重力是一种加速度，并且由于小车倾斜程度的不同而不断变化。制动、加速和转弯时，车辆会产生加速度。此时的加速度既包含重力加速度又包含运动加速度。然而当进行倾斜测量时，我们只需要得到重力加速度；当进行小车动力测量时，却又只想得到运动加速度。因此此时的加速度将存在极大的分量误差。

角速度：通过测量绕小车重心的旋转，角速度传感器有助于纠正车俩向前倾斜带来的不利影响。不幸的是，角速度传感器有其自身的缺陷。它测量的旋转速度，不是旋转角度，而是通过不断积分得到角度。当旋转速度的测量出现偏差，积分后所得的角度将会有很大的偏差。

数据融合：角速度和加速度的缺点可以相互弥补。因此可以结合角速度值和加速度值，进行互补滤波或卡尔曼滤波，得到准确的角度值。

加速度数据选择：三轴加速度——X轴，Y轴，Z轴。

如图所示，一般车辆前进运动方向X轴，重心方向Z轴是我们需要提取的信息。而Y轴一般不会有大变动。但一般我们安装好MPU6050后并不清楚哪个轴是哪个数据（传感器最好安装在车辆重心，尽量减少旋转加速度对测量带来的不利影响）。因此我们需要通过观察上位机波形确定哪些数据是有效的。

(原文件名:page 1.jpg) 引用图片

调试方法：    

               1.将加速度的三轴数值打印到上位机 ，观察波形。
               2.周期性环绕电机轴，摆动车身。
               3.找出其中两个数据变化大的轴（类似正弦波的图像 ，另一条曲线则几乎没有什么变化）
               4 .找出上述的两个正弦变化的曲线，根据tanα 公式可求得角度值 （即tanα = 对边/临边）

角速度数据选择：三轴角速度——X轴，Y轴，Z轴。

同理，我们需要筛选出对我们有意义的轴信息。

调试方法：
               1.将角速度的三轴数值打印到上位机 ，观察波形。
               2.先大幅度旋转小车，找出有变化的曲线（正弦） 一般只有一条。
               3.定位选出的曲线，平稳旋转车子，观察曲线变化，理论角速度应为固定值，因此应无大变化。
               4.静止立住小车，读取其固定值，此值为角速度零飘值，应手动减去。

至此 ，原始数据调试完成。

滤波原理：滤掉小车运动时由于电机旋转引起角度的抖动而使角度波动的干扰值。互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。

即短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，作为互补。

滤波作用：加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。

例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率

什么时候开始进行数据融合？

  1.根据上述理论，先将加速度、角速度计算出的角度打印到上位机，绕着电机轴旋转应该是两个正弦波
  2.这时候会发现这个波形并不是重合的，因此我们需要把两个波形大致重合
  3.角速度的曲线一般比较大，因此需要乘上一个系数 压缩到加速度的范围  

  4.将加速度或者角速度计算出的角度 通过加减上下平移使波形基本重合即可

   Angle_SpeedOld=atan(Acc_X*1.0/ (1.0*Acc_Z) )*180/3.14-39;  
   Angle_Speed=(-Gyro_Y*1.0) * 0.0585;
   Angle+=(Angle_Speed*0.005);  

   AngleLast=Angle_SpeedOld*(1-K)+K*(Angle);

其中，Angle_SpeedOld为通过加速度算出的角度，-39为偏移量；Angle_Speed为角速度，0.0585是对角速度的衰减量（重合参数）；Angle是通过对角速度进行积分得到的角度；AngleLast是对加速度算出的角度和角速度算出的角度进行互补滤波，K为比例系数。

至此 ，角度调试完成。