引用文章
首先需要提一点关键的，**如果是采用卡尔曼滤波或者互补滤波，尽可能把采样时间调小，这样可以增加滤波的效果。建议采样时间不要高于10ms，起初我在测试的时候采样时间设置为20ms，结果卡尔曼滤波收敛速度非常慢，也就是旋转小车让角度突变的情况下，OLED上显示的角度值会超过实际值，然后再慢慢回来接近实际值。**包括PID控制也一样，毕竟我们所使用的都是将这些数学线性公式转换成离散形式的公式，采样时间越短，效果当然也就越接近原本数学公式所体现的效果了。
采样时间包括姿态传感器的采样时间。MPU6050的采样频率一般设为200Hz，这样采样时间就是5ms了，可以达到一个不错的效果，再用上MPU6050的数据中断，每隔5ms给MCU发一个中断信号，然后在这个中断服务函数中读取数据，处理数据。
其次，在进行姿态融合滤波时，要注意数据极性(或者说矢量方向)，通常我们都是将加速度计2个轴取反正切值计算出的角度和陀螺仪积分计算出的角度进行滤波融合。这时候就必须保证由加速度计计算出的角度和陀螺仪计算出的角度极性必须相同。通常我们都是以加速度计算出的角度极性为准，确保陀螺仪计算的角度极性与其相同。下面这张图是我从MPU6050数据手册上截下来的，并定义了一下小车前后的方向，接下来我会通过2个例子说明极性的问题。
①假设我的小车用的是Y轴的倾角，水平时Z轴的加速度固定为g，也就是重力加速度，Y轴加速度即为0，计算出的角度也是0。如果此时小车往后转动，那么此时重力和Z轴就不重合了，重力就可以被分解为Z轴和Y轴上两个互相垂直的矢量，这样通过arctan(y/z)就可以计算出Y轴当前的倾角了（单位：弧度）。注意，此时Z轴和Y轴上的加速度都是负值，负负得正，最终计算出的角度是正的。
接下来再来看看陀螺仪计算的角度，由于我们使用Y轴的倾角，那么对应的角速度也就是X轴的角速度**，因为我们Y轴为倾角往后转动小车时，就是在绕X轴运动。从MPU6050的坐标系可以看出，以Y轴为倾角往后转动时，X轴的角速度方向是正的，也就是直接对陀螺仪输出的数据（量程转换后）进行积分得到的角度也是正的，这与上一步加速度计算出的角度极性相同，因此可以直接传入进行滤波。**
②那如果小车以X轴为倾角转动呢，假设此时小车往后转动，那么使用加速度计算角度和刚才是一样的，都是负负得正（大家可以自己思考一下哈）。但是陀螺仪计算的角度呢，往后转动小车，在Y轴上的角速度对比MPU6050的坐标系，可以发现角速度的方向是负的，那么通过积分得到的角度也就是负的，这就需要在姿态融合滤波前，将其取相反数，得到与加速度计算出的角度一样的极性进行下面的姿态融合滤波。
如果极性没有对应的话，比如加速度计算的角度是正的，而陀螺仪计算的角度是负的，那么在滤波的时候，我们短时间内是相信陀螺仪的，这样角度就会比实际值小，随着时间的增加，我们又更相信加速度计，因此加速度计算的角度会不断地进行修正，因此静止几秒后，滤波后的角度才会慢慢接近实际值。其实归根结底就是要根据MPU6050的坐标系来处理数据啦。
在对陀螺仪积分计算角度时，要先进行量程转换，比如我的MPU6050陀螺仪量程设置为±2000dps，那么读出陀螺仪原始数据时需要除以16.4(65536/4000)，才能得到角速度值。而通过加速度取反正切值计算出的数据单位是弧度，要转换成角度还需要乘以(180 / π)。
在读出陀螺仪的原始数据时，最好能减去零点漂移。在静止的时候，陀螺仪读出的数据一般不为0，而是一个接近0的数值，比如可能是-31，那么在进行一切计算前，给陀螺仪读出的数据加上31(减去-31)，这样也可以确保后面进行PID运算时效果更好。具体的零点漂移数值就需要通过显示屏或者上位机来观察了。
通过以下3个环的控制最终会得出三个输出值，这三个输出值按以下关系计算的出最终的电机PWM占空比比较值：
平衡控制是平衡小车最简单最基本的控制，它不需要积分运算，代码如下（示例）：
注意平衡控制的微分项最好是陀螺仪输出的原始数据，而不是经过量程转换或者滤波后的数据，这样在角速度很小的时候才能更好地进行超调抑制，因此前面所说的减去零点漂移就很重要了。
平衡控制的参数也需要注意极性，这和前面的姿态融合滤波是一样的，必须保证角度angle和角速度gyro极性是一样的。如果不一样(比如：一正一负)，就需要在传入参数时对其中一个取相反数，一般是改变角速度gyro极性和角度angle极性相同，否则微分项就会起反效果了(加强超调和震荡)。
①在整定之前，必须保证前面所有的数据处理都没有问题，即旋转小车使小车倾角突变时，滤波后得到的角度不会出现很明显的滞后和超调（然后再慢慢被加速度修正），旋转角度突变值比较大时，滞后或者超调1~2°是正常的，毕竟是离散化公式，这就体现了采样时间的重要性。
②确定平衡的角度，需要通过显示屏或者上位机进行观察确定，至少精确到小数点后一位，比如我的小车是-2.7度左右平衡。
③计算比例系数Kp的范围，参考以下公式：
(10表示10度时最大速度)。
比如我的pwm最大占空比比较值是7200，那么Kp范围就是0~720。
④给一个较小的Kp值以便确定Kp的极性，比如50。如果向前倾斜小车，小车轮子会一直往前转动，小车往后倾斜则轮子往后转动，说明Kp极性正确。否则Kp极性就是反的，应该是-50。
⑤慢慢增大Kp值，直到小车倒下时有一个明显的回复力，表现出来就是小车在平衡角度附近低频震荡，合适的Kp值一般不会超过Kp范围的50%。
⑥确定Kd的范围。参考以下公式：
(2000表示陀螺仪输出的最大原始值，这是我的平台上观测得到的，尽管最大32767，但实际使用时基本不会超过2000)
⑦将Kp设为0，确定Kd的极性，给一个较小的Kd值以便确定Kd的极性，比如0.5,。如果旋转小车，小车轮子会向相同的方向转动，说明Kd极性正确。否则Kd极性就是反的，应该是-0.5。
⑧将Kp设置为先前调好的值，慢慢增大Kd，直到小车的低频抖动消失，但注意Kd值不可过大，否则小车会出现高频抖动，此时非常容易烧电机驱动芯片。合适的Kd值一般不会超过Kd范围的40%。
至此，平衡环就完成了，因为目前我们没有引入与速度相关的控制，因此小车在平衡的同时会往一个方向加速，这是平衡环在起作用，为了不让小车倒下，但是一直加速最终还是会倒下的，而此时电机转速可能会达到最大，因此有必要加上2个条件做保护，1是当小车倾角大于一定值时，关闭电机；2是对pwm进行限幅，一般限制在60%的占空比即可。
平衡小车的速度控制最基本功能是为了抵消平衡环产生的速度，让小车保持平衡时的同时转速为0(理论值)。**速度的控制关键点在于消除静差，因此需要引入积分运算，而不需要微分运算。**代码如下（示例）：
这个代码是参考平衡小车之家的，下面说一些注意事项。
速度PI控制是正反馈，所谓正反馈就是小车的电机有一定转速时，编码器数值的绝对值大于0，PI运算后的值会更大，这个值如果作用到电机PWM占空比上，会让电机转的更快，然后编码器数值的绝对值变得更大，反复运算，直到PWM最大占空比。至于为什么要是正反馈，这个后面再说。
在进行参数整定时，要注意电机编码器的类型，一般有2种，霍尔编码器和光电编码器，霍尔编码器精度低，在参数整定时，如果是霍尔编码器，Kp、Ki值会比较大，而对于光电编码器，Kp、Ki值就不需要很大了。
①在整定之前，需要保证平衡环已经稳定，判断稳定的方法是，让小车保持在平衡角度附近，左右推动小车，感觉不到抖动即可。
②确定Kp和Ki的范围，参考以下公式： 
(这里的2500是在平衡环的作用下加速时达到的两路编码器绝对值之和。注意如果是霍尔编码电机，大概在100~200之间，具体需要根据大家的平台观测。我这里用的是光电编码器，以下参数也以光电编码器的为例)
③注释掉平衡环以便确定Kp和Ki的极性，它们极性一般都是相同的，比如设定Kp为2.0，Ki为0.01，那么当轻轻转动小车轮子时，电机会开始加速，直到最大转速，说明极性正确，因为速度控制要是正反馈。如果不是这种效果，查看Kp、Ki极性是否正确或者2路编码器极性是否相同。
④加入之前注释掉的平衡环，慢慢增大Kp的值，Ki则取对于Kp的1/200，这一步慢慢调试就可以得到不错的效果，但注意Kp不要太大了，否则会引起抖动（超调）。一般合适的Kp值不会超过Kp范围的60%。
需要注意的是，速度环是用来修正平衡环的，如果拿起小车，平衡环不会起作用（没有修正角度），那么速度环会因为正反馈而使电机达到最大转速。这就需要写一个算法进行判断小车是否被拿起了，如果被拿起则需要关闭电机，且清除速度环的积分。直到检测到小车被放下之后才开启电机。
至此，速度环就完成了，这一步需要注意很多数据的极性，还有参数的范围，一但这些确定了，那么速度环还是非常好调的。
关于控制小车前进后退的话，理解了速度环的原理就很简单了，速度环利用比例和积分运算来修正平衡环产生的速度，如果要控制小车前进或后退的话，那么只需要给比例项或者积分项加上或减去一个值，这样速度环就不会修正速度到0了，而是一个非0值，这样就能控制小车前进后退了。
小车在前进后退或者直立时，由于两边的电机电气属性可能不完全相同，导致一边转的快一些，这样小车就会“走歪掉”或者“原地慢慢转圈圈”。这时候就需要引入转向环控制了。代码如下（示例）：
转向控制可以有多种方法，可以使用电机编码器、Z轴角速度，或者两者结合，这里我只使用了Z轴角速度。
转向控制可以只用比例P来控制，比例项即Z轴角速度原始数据。但考虑到需要遥控转向，而且这里使用比例控制或者微分控制没什么区别，都是一个系数乘以一个偏差，因此将Z轴角速度作为微分项，而比例项则是用于遥控控制小车转向。
①先整定微分系数，确定Kd的范围，参考以下公式(和平衡环的Kd一样)：
②注释掉平衡环和速度环，以便确定Kd的极性，比如Kd设为0.5，那么将小车轮子接触地面且绕Z轴旋转小车时，小车轮子会产生一个扭矩抵抗Z轴角速度的变化（转起来比较吃力），那么说明Kd极性正确。如果小车反而加速转动（转起来很轻松），则说明极性反了。
③加上注释掉的平衡环和速度环，慢慢调大Kd的值，直到小车直立或者前进后退时有一个较好的抑制转向效果。合适的Kd值一般不会超过Kd范围的60%。
④比例项则是用来控制遥控小车转向的角速度的，注意转向时要让微分项失效，即让Kd为0。比例项越大则控制小车转的速度越快。
这种方法我测试并不能使小车在直立时有一个很好的抑制转向的效果，只能说较好，如果要更好的效果，还需要结合电机编码器。
首先Balance_Pwm就没什么好说的了，用于修正角度让小车处于平衡状态。 对于Velocity_Pwm先前说是正反馈，电机转的越快，通过PI运算后这个值会越大，因此这里是减去，因为在平衡环的作用下，如果小车往一边加速度，那么Balance_Pwm会越来越大的，Balance_Pwm越来越大意味着电机转速越快，这时候Velocity_Pwm就会对它进行修正，让电机最终的PWM值不至于一直增大。 而Turn_Pwm看前面的符号就很直观了，小车直立时，Balance_Pwm和Velocity_Pwm都是接近0的，这时候如果控制小车转向，那么Turn_Pwm必然是一个比较大的值，代入式子后计算出2个电机的PWM比较值，最终会有一个电机的转向是向后，另一个向前，这样就达到了小车原地转向的效果。如果边前进边转向的话，那就是一个电机转的快，一个转的慢，就达到便前进边转向的效果了。
