三、测速（物理模型 建立数学模型 传递函数 PD算法）

        假设小车在上面直立控制调节下已经能够保持平衡了，但是由于安装误差，传感器实际测量的角度与车模角度有偏差，因此小车实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，小车就会朝倾斜的方向加速前进。控制速度只要通过控制小车的倾角就可以实现了。具体实现需要解决三个问题： 

    （1）如何测量小车速度？ 

    （2）如何通过小车直立控制实现小车倾角的改变？ 

    （3）如何根据速度误差控制小车倾角？ 

        第一个问题可以通过安装在电机输出轴上的霍尔测速来测量得到小车的车轮速度。如图所示。利用控制单片机的外部中断IO口在不间断测速，速度为脉冲信号的个数可以反映电机的转速。

        第二个问题可以通过角度控制给定值来解决。给定小车直立控制的设定值，在角度控制调节下，小车将会自动维持在一个角度。通过前面小车直立控制算法可以知道，小车倾角最终是跟踪重力加速度Z轴的角度。因此小车的倾角给定值与重力加速度Z轴角度相减，便可以最终决定小车的倾角。

        第三个问题分析起来相对比较困难，远比直观进行速度负反馈分析复杂。首先对一个简单例子进行分析。假设小车开始保持静止，然后增加给定速度，为此需要小车往前倾斜以便获得加速度。在小车直立控制下，为了能够有一个往前的倾斜角度，车轮需要往后运动，这样会引起车轮速度下降（因为车轮往负方向运动了）。由于负反馈，使得小车往前倾角需要更大。如此循环，小车很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。 

        为什么负反馈控制在这儿失灵了呢？原来在直立控制下的小车速度与小车倾角之间传递函数具有非最小相位特性（在此省略了分析），在反馈控制下容易造成系统的不稳定性。

        为了保证系统稳定，往往取的小车倾角控制时间常数Tz很大。这样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的震荡现象。这个现象在实际参数整定的时候可以观察到。那么如何消除速度控制过程中的震荡呢？ 

        要解决控制震荡问题，在前面的小车角度控制中已经有了经验，那就是在控制反馈中增加速度微分控制。但由于车轮的速度反馈信号中往往存在着噪声，对速度进行微分运算会进一步加大噪声的影响。为此需要对上面控制方法进行改进。原系统中倾角调整过程时间常数往往很大，因此可以将该系统近似为一个积分环节。将原来的微分环节和这个积分环节合并，形成一个比例控制环节。这样可以保持系统控制传递函数不变，同时避免了微分计算。

        但在控制反馈中，只是使用反馈信号的比例和微分，没有利误差积分，所以最终这个速度控制是有残差的控制。但是直接引入误差积分控制环节，会增加系统的复杂度，为此就不再增加积分控制，而是通过与角度控制相结合后在进行改进。

        要求小车在原地停止，速度为0。但是由于采用的是比例控制，如果此时陀螺仪有漂移，或者加速度传感器安装有误差，最终小车倾角不会最终调整到0，小车会朝着倾斜的方向恒速运行下去。注意此时车模不会像没有速度控制那样加速运行了，但是速度不会最终为0。为了消除这个误差，可以将小车倾角设定量直接积分补偿在角度控制输出中，这样就会彻底消除速度控制误差。第二点，由于加入了速度控制，它可以补偿陀螺仪和重力加速度的漂移和误差。所以此时重力加速度传感器实际上没有必要了。