一、 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。

*EKi 过大**	会出现积分饱和，系统依然会振荡
*EKi 过小**	纠正不够

（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。