一、 PID公式
二、 PID应用的必要性
1. 四驱小车运动
| 左边两个驱动轮和右边两个驱动轮的速度相同 | 直线 |
| 右边轮子的速度大于左边轮子的速度 | 左偏 |
| 右边轮子的速度小于左边轮子的速度 | 右偏 |
2. 产生多种运动的原因
小车的4个电机，减速箱以及车轮在物理层面上存在误差，虽然给4个轮子通了相同pwm（电流），但是却没办法保证运行时车子的4个轮子的转速相同。
我们只知道通过改变电流让电机旋转起来，却不知道轮子的实际转速，所以没法保证小车跑直线。这就是开环控制。
3. 解决方案分析
小车跑偏就是因为小车现在的控制系统是开环的，只能用pwm控制电机让它旋转，并不知道轮子的实际速度是多少。
只有知道了方向是否发生了偏转，才能进一步对小车的运动方向进行控制调整。例如，往左偏了，我们就把右边的轮子速度降低一点。如果往右边偏了，我们就把边的轮子速度升高一点。
4. 解决方案
引入反馈信号：如果我们能得知车子的运动方向，就可以对车子的方向加以纠正，控制小车跑直线（闭环控制）。
（1）给小车安装电子陀螺仪
用它来测量小车运行的方向信号，从而控制单片机pwm的输出值（input）去修正电机的工作电流，从而修改轮子的实际速度（output），从而纠正小车的跑偏的问题，从而使用PID技术实现方向对小车方向的闭环控制。
闭环比开环多了反馈信号feedback。我们这里的feedback信号选用了电子陀螺仪的偏转角（yaw值）。根据闭环控制，设计一个小车的实际控制方案，例如，保持左边的轮子速度不变，当车子向右发生偏转时，我就加快右边轮子的转速。当车子向左偏的时候，我就降低右边轮子的转速。
（2）确定反馈信号量和控制量之间的比例关系
| 方向没有发生改变 | 反馈为0 | 输出变化为0 ，不需要改变输出值 |
| 车子往右偏 | 改变右轮的速度 | 输入变化是？输出应该改变？ |
| 车子往左偏 | 改变左轮的速度 | 输入变化是？输出应该改变？ |
输入信号和输出信号存在一种比例关系，即传感器的值yaw和驱动电机的pwm的值存在一种比例关系(比例P)。方向偏的越多，那么右轮的pwm应该越大，方向偏的越少，右轮的pwm应该越少。方向偏的多少（偏差error）就是[yaw的目标值]-[yaw的当前值]。
出现的问题原因：左轮输入的Input和右轮驱动输入的Input是一样的，但是由于左轮和右轮的执行器有差异，导致左轮的Output1和右轮的Output2的值不一样。于是我们保持左轮依然使用开环系统，而把右轮系统改成可以调整转速的闭环系统。这样使得输入右轮系统的数值就不仅仅是Input了，还增加了电子陀螺仪反馈回来的偏转角（jaw）的误差值error。把这两个数值通过控制器处理后，再输入到执行器，让右轮最终产生的Output2等于左轮的Output1。
（3）在系统的Controller部分引用PID比例部分
pwm=error*Kp+input
error=yaw的目标值-yaw的当前值
根据error的值的大小设定一个比例系数Kp，Kp是一个系数常数，需要根据系统的性能手动设定。然后加上原来input，形成新的pwm值。
| 当小车发生右偏时 | 当小车发生左偏时 |
| 偏转角测量出来的当前测量值就会跟初始方向产生一个误差error | 偏转值跟初始方向同样产生一个error |
| 假设初始方向是1 | |
| 第一次测量测出来是0.2 | 第一次测量测出来是1.2 |
| error = 1-0.2=0.8，pwm就不等于input了，而是input+0.8Kp | error = 1 - 1.2 = - 0.2，pwm就不等于input了，而是input-0.2Kp |
| 由于0.8Kp是一个正数，所以最总pwm也就是升高了 | 由于-0.2Kp是一个负数，所以最终pwm也就是降低了 |
| 右轮的速度就会增加，小车开始往左，纠正小车右偏 | 右轮的速度就会减小，小车开始往右偏，纠正小车左偏 |
因为电子陀螺仪的采样率大概为100ms一次。所以需要在计算机里每100ms 采样一次得到yaw的值：error = [yaw的目标值]-[yaw的当前值]。新的pwm=error*Kp+input，用来驱动右轮的电机。
Kp值的选取：只有在Kp选取到一个合适的值，小车才能保持直线。
| Kp过大 | 震荡，小车会摇摆行驶 |
| Kp过小 | 纠正的不够，产生稳态误差 |
稳态误差产生的原因：纠正的过程中，当前的测量值会跟目标值越来越小，那么error就会变得很小，使得error*Kp 也会变得很小； error*Kp 由于过小就会失去纠正的作用，离目标值始终存在一个误差，称之为稳态误差。
（4）在系统的Controller部分引用PID积分部分
问题：合适的Kp是很难找的，我们可以引入E*Ki（误差的积分）。当 error*Kp 部分的量纠正不够的时候，可以靠 I 部分的量（对每100ms测量出来的误差进行累加）来补充上去。
假设：a. 开始向右偏转，只要不向左偏转，I 值是一直增加的。b. P 的输出一开始是比较猛的，但是当越来越靠近目标。c. 当 P 因误差 error 过小失去调整作用时，可以用 I 积分部分来弥补比例部分的稳态误差。
pwm = error * Kp + E * Ki + input，
E = 每次测量的error 的累加和，系统每100ms测量一次jaw的值，每次测量的jaw的值和jaw的目标值有一个差值，我称之为err,把每次测量的err累加在一起就是E。
| E*Ki 过大 | 会出现积分饱和，系统依然会振荡 |
| E*Ki 过小 | 纠正不够 |
（5）在系统的Controller部分引用PID微分部分
我们可以通过限制 I 的最大值的方法，来进行限制。也可以再增加一个微分量 D*Kd，Kd为系数常数。每100ms测量一次jaw偏转角，而D指的就是这一次的误差和上一次测量的误差之间的差值，是一个反映误差纠正快慢的量，重点是负值。
在图上看，就是图像变化陡峭的部分。当Input加上这个值以后，系统可以有效抑制运行过程中变化过快的部分，比如振荡期。而对变化缓慢的部分，比如稳定期，微分部分几乎就不起作用。有点像起一个柔和的缓冲作用。
pwm = error * Kp + E * Ki + D * Kd + input,
D=当前一次测量的error - 上一次测量的error，每100ms测量一次jaw，并计算出err，D反应的是系统的err变化率。所以err变化剧烈的区域，D会对总的输出值进行抑制，防止剧烈的振荡，而稳定之后的区域，D对输出值的影响就很小了。
三、程序编写
1. 核心公式
pwm = error * Kp + E * Ki + D * Kd + input
2. 相关参数及说明
| pwm | 控制左边轮子和右边轮子的速度 | 在650-899的范围内，pwm的值越大，轮子的速度越快。低于650时 小车不运动了。 |
| E | 每次测量的error加到一起 | 电子陀螺仪PDU6050测量的偏转角值为jaw。按下小车启动按键时，测量到的 jaw 就为目标值。之后每100ms测量出来的 jaw 值为当前值，用目标值减去当前值为每次测量的误差error |
| D | 0 | / |
最后每100ms 计算一次公式 pwm = error * Kp + E*Ki + D * Kd + input ，把计算出来的pwm值作为右轮的电机驱动的参数，代入电机的驱动程序里，根据实际场景调整一下系统参数Kp，Ki。
