前言
第一次接触PID应该是机器人大赛的时候，导师让我用PID控制寻迹机器人，不过那个时候是真的超级笨，很明显，PID没有做出来，然后被导师各种鄙视。不过能够感觉到那个时候的导师是真的期望能够有所作为，可惜那个时候基础弱爆了很菜，什么都不会；不过并不后悔当初做法，在自习室里呆了3年，中间虽然有波折，给自己定的方向也换了好几次，不过那个时候的学习方法和读完一本本厚厚的专业书籍给予了我今天无比的自信。
结构安排
PID算是一个在工业中应用很广泛的一个控制算法，这里结合一个例子和程序，简要的介绍下这个算法。
本来打算用C++写这个程序，不过C++没有找到合适的可视化工具，这里就使用Python3吧！
应该是分为3个部分吧：第一部分放公式（为了严谨，但不影响理解），第二部分是叙述应用场景，第三部分是一个与应用场景比较匹配的程序。
离散PID公式
PID的公式网上一搜一大堆。
位置PID：
增量PID：
应用场景和PID究竟解决了什么问题
1）人脸识别算法在图像中检测并识别到了一个人脸，但甲方爸爸还希望这个人脸始终处于图像的中心，那么我们如何控制摄像头上的电机保证上述需求呢？
2）项目中需要无人机或者无人车能够尽快和稳定到达的位置，当已知当前位置与目标位置的座标后，我们如何控制电机或者调用速度接口实现上述需求呢？
3）有这么一个一直漏水的游泳池，不过房东希望这个游泳池的水量能够一直保持在1m，我们如何控制阀门解决这个问题呢？
位置PID程序说明
比例控制：
以游泳池为例子，误差是当前水位与期望水位的差值，控制量是放水阀门的大小。
import numpy as np
from matplotlib import pyplot as plt
times = 21 #迭代次数
x = np.arange(0, times, 1) #X轴
target = np.ones(times, dtype=np.int32) #目标位置是1米
current = 0.2 * np.ones(times, dtype=np.int32) #当前位置
kp = 0.5
for time in range(1, times-1):
print("index:", end='')
print("%-3d:" % time, end=' ')
print("target:", 1, end=' ')
print("current:", end=' ')
print('%-10.3f' % round(current[time], 3), end=' ')
error = target[time] - current[time]
print("error:", end=' ')
print('%-10.3f' % round(error, 3), end=' ')
control = kp * error
current[time+1] = current[time] + control
print("control:", end=' ')
print("%-10.3f" %round(control, 3))
plt.title("PID")
plt.plot(x, current, 'o-g')
plt.plot(x, target, 'p-r')
plt.show()
kp=0.5时的输出：
index:1 : target: 1 current: 0.2 error: 0.8 control: 0.4
index:2 : target: 1 current: 0.6 error: 0.4 control: 0.2
index:3 : target: 1 current: 0.8 error: 0.2 control: 0.1
index:4 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:5 : target: 1 current: 0.9 error: 0.1 control: 0.0
index:6 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:7 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:8 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:9 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:10 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:11 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:12 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:13 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:14 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:15 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:16 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:17 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:18 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:19 : target: 1 current: 1.0 error: 0.0 control: 0.0
kp=1.5时的输出：
index:1 : target: 1 current: 0.2 error: 0.8 control: 1.2
index:2 : target: 1 current: 1.4 error: -0.4 control: -0.6
index:3 : target: 1 current: 0.8 error: 0.2 control: 0.3
index:4 : target: 1 current: 1.1 error: -0.1 control: -0.1
index:5 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:6 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:7 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:8 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:9 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:10 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:11 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:12 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:13 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:14 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:15 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:16 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:17 : target: 1 current: 1.0 error: 0.0 control: 0.0
index:18 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:19 : target: 1 current: 1.0 error: 0.0 control: 0.0
kp=3时的输出：
index:1 : target: 1 current: 0.2 error: 0.8 control: 2.4
index:2 : target: 1 current: 2.6 error: -1.6 control: -4.8
index:3 : target: 1 current: -2.2 error: 3.2 control: 9.6
index:4 : target: 1 current: 7.4 error: -6.4 control: -19.2
index:5 : target: 1 current: -11.8 error: 12.8 control: 38.4
index:6 : target: 1 current: 26.6 error: -25.6 control: -76.8
index:7 : target: 1 current: -50.2 error: 51.2 control: 153.6
index:8 : target: 1 current: 103.4 error: -102.4 control: -307.2
index:9 : target: 1 current: -203.8 error: 204.8 control: 614.4
index:10 : target: 1 current: 410.6 error: -409.6 control: -1228.8
index:11 : target: 1 current: -818.2 error: 819.2 control: 2457.6
index:12 : target: 1 current: 1639.4 error: -1638.4 control: -4915.2
index:13 : target: 1 current: -3275.8 error: 3276.8 control: 9830.4
index:14 : target: 1 current: 6554.6 error: -6553.6 control: -19660.8
index:15 : target: 1 current: -13106.2 error: 13107.2 control: 39321.6
index:16 : target: 1 current: 26215.4 error: -26214.4 control: -78643.2
index:17 : target: 1 current: -52427.8 error: 52428.8 control: 157286.4
index:18 : target: 1 current: 104858.6 error: -104857.6 control: -314572.8
index:19 : target: 1 current: -209714.2 error: 209715.2 control: 629145.6
从上面可以看出，kp不能设置的比较大；另外kp足够小，那么肯定是收敛的，不过可能调整时间比较长。
接下来，我们来看下有扰动的情况，假如在每个控制周期内，游泳池会漏掉0.1米的水量，程序如下：
import numpy as np
from matplotlib import pyplot as plt
times = 21 #迭代次数
x = np.arange(0, times, 1) #X轴
target = np.ones(times, dtype=np.int32) #目标位置是1米
current = 0.2 * np.ones(times, dtype=np.int32) #当前位置
kp = 1.5
for time in range(1, times-1):
print("index:", end='')
print("%-3d:" % time, end=' ')
print("target:", 1, end=' ')
print("current:", end=' ')
print('%-10.1f' % round(current[time], 3), end=' ')
error = target[time] - current[time]
print("error:", end=' ')
print('%-10.1f' % round(error, 3), end=' ')
control = kp * error
current[time+1] = current[time] + control - 0.1
print("control:", end=' ')
print("%-10.1f" %round(control, 3))
plt.title("PID")
plt.plot(x, current, 'o-g')
plt.plot(x, target, 'p-r')
plt.show()
在kp=1.5输出如下：
index:1 : target: 1 current: 0.2 error: 0.8 control: 1.2
index:2 : target: 1 current: 1.3 error: -0.3 control: -0.5
index:3 : target: 1 current: 0.8 error: 0.2 control: 0.4
index:4 : target: 1 current: 1.0 error: -0.0 control: -0.0
index:5 : target: 1 current: 0.9 error: 0.1 control: 0.2
index:6 : target: 1 current: 1.0 error: 0.0 control: 0.1
index:7 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:8 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:9 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:10 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:11 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:12 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:13 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:14 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:15 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:16 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:17 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:18 : target: 1 current: 0.9 error: 0.1 control: 0.1
index:19 : target: 1 current: 0.9 error: 0.1 control: 0.1
从上图可以看到，存在着一个稳态误差。
如何消除稳态误差呢，此时就需要引入积分控制：稳态误差可以认为是整个迭代过程的误差之和，它们应该为零的。
比例积分控制
import numpy as np
from matplotlib import pyplot as plt
times = 21 #迭代次数
x = np.arange(0, times, 1) #X轴
target = np.ones(times, dtype=np.int32) #目标位置是1米
current = 0.2 * np.ones(times, dtype=np.int32) #当前位置
kp = 1.5
ki = 0.1;
errorSum = 0
for time in range(1, times-1):
print("index:", end='')
print("%-3d:" % time, end=' ')
print("target:", 1, end=' ')
print("current:", end=' ')
print('%-8.1f' % round(current[time], 3), end=' ')
error = target[time] - current[time]
errorSum = errorSum + error
print("error:", end=' ')
print('%-8.1f' % round(error, 3), end=' ')
print("errorSum:", end=' ')
print('%-8.1f' % round(errorSum, 3), end=' ')
control = kp * error + ki * errorSum
current[time+1] = current[time] + control - 0.1
print("control:", end=' ')
print("%-8.1f" %round(control, 3))
plt.title("PID")
plt.plot(x, current, 'o-g')
plt.plot(x, target, 'p-r')
plt.show()
ki=0.1的输出如下：
index:1 : target: 1 current: 0.2 error: 0.8 errorSum: 0.8 control: 1.3
index:2 : target: 1 current: 1.4 error: -0.4 errorSum: 0.4 control: -0.5
index:3 : target: 1 current: 0.8 error: 0.2 errorSum: 0.7 control: 0.4
index:4 : target: 1 current: 1.1 error: -0.1 errorSum: 0.6 control: -0.1
index:5 : target: 1 current: 0.9 error: 0.1 errorSum: 0.7 control: 0.2
index:6 : target: 1 current: 1.0 error: -0.0 errorSum: 0.7 control: 0.1
index:7 : target: 1 current: 1.0 error: 0.0 errorSum: 0.7 control: 0.1
index:8 : target: 1 current: 1.0 error: 0.0 errorSum: 0.7 control: 0.1
index:9 : target: 1 current: 1.0 error: 0.0 errorSum: 0.7 control: 0.1
index:10 : target: 1 current: 1.0 error: 0.0 errorSum: 0.7 control: 0.1
index:11 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:12 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:13 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:14 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:15 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:16 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:17 : target: 1 current: 1.0 error: 0.0 errorSum: 0.8 control: 0.1
index:18 : target: 1 current: 1.0 error: 0.0 errorSum: 0.9 control: 0.1
index:19 : target: 1 current: 1.0 error: 0.0 errorSum: 0.9 control: 0.1
ki=0.5的输出如下：
index:1 : target: 1 current: 0.2 error: 0.8 errorSum: 0.8 control: 1.6
index:2 : target: 1 current: 1.7 error: -0.7 errorSum: 0.1 control: -1.0
index:3 : target: 1 current: 0.6 error: 0.4 errorSum: 0.5 control: 0.8
index:4 : target: 1 current: 1.4 error: -0.3 errorSum: 0.1 control: -0.5
index:5 : target: 1 current: 0.8 error: 0.2 errorSum: 0.3 control: 0.5
index:6 : target: 1 current: 1.2 error: -0.2 errorSum: 0.2 control: -0.2
index:7 : target: 1 current: 0.9 error: 0.1 errorSum: 0.3 control: 0.3
index:8 : target: 1 current: 1.1 error: -0.1 errorSum: 0.2 control: -0.0
index:9 : target: 1 current: 0.9 error: 0.1 errorSum: 0.2 control: 0.2
index:10 : target: 1 current: 1.0 error: -0.0 errorSum: 0.2 control: 0.0
index:11 : target: 1 current: 1.0 error: 0.0 errorSum: 0.2 control: 0.1
index:12 : target: 1 current: 1.0 error: -0.0 errorSum: 0.2 control: 0.1
index:13 : target: 1 current: 1.0 error: 0.0 errorSum: 0.2 control: 0.1
index:14 : target: 1 current: 1.0 error: -0.0 errorSum: 0.2 control: 0.1
index:15 : target: 1 current: 1.0 error: 0.0 errorSum: 0.2 control: 0.1
index:16 : target: 1 current: 1.0 error: -0.0 errorSum: 0.2 control: 0.1
index:17 : target: 1 current: 1.0 error: 0.0 errorSum: 0.2 control: 0.1
index:18 : target: 1 current: 1.0 error: -0.0 errorSum: 0.2 control: 0.1
index:19 : target: 1 current: 1.0 error: 0.0 errorSum: 0.2 control: 0.1
ki=1.5的输出如下：
index:1 : target: 1 current: 0.2 error: 0.8 errorSum: 0.8 control: 2.4
index:2 : target: 1 current: 2.5 error: -1.5 errorSum: -0.7 control: -3.3
index:3 : target: 1 current: -0.9 error: 1.9 errorSum: 1.2 control: 4.7
index:4 : target: 1 current: 3.6 error: -2.6 errorSum: -1.4 control: -6.2
index:5 : target: 1 current: -2.6 error: 3.6 errorSum: 2.1 control: 8.6
index:6 : target: 1 current: 5.9 error: -4.9 errorSum: -2.8 control: -11.6
index:7 : target: 1 current: -5.7 error: 6.7 errorSum: 4.0 control: 16.0
index:8 : target: 1 current: 10.2 error: -9.2 errorSum: -5.2 control: -21.6
index:9 : target: 1 current: -11.6 error: 12.6 errorSum: 7.3 control: 29.8
index:10 : target: 1 current: 18.1 error: -17.1 errorSum: -9.8 control: -40.5
index:11 : target: 1 current: -22.4 error: 23.4 errorSum: 13.6 control: 55.5
index:12 : target: 1 current: 33.0 error: -32.0 errorSum: -18.4 control: -75.6
index:13 : target: 1 current: -42.7 error: 43.7 errorSum: 25.3 control: 103.5
index:14 : target: 1 current: 60.7 error: -59.7 errorSum: -34.4 control: -141.1
index:15 : target: 1 current: -80.5 error: 81.5 errorSum: 47.1 control: 193.0
index:16 : target: 1 current: 112.4 error: -111.4 errorSum: -64.2 control: -263.5
index:17 : target: 1 current: -151.2 error: 152.2 errorSum: 87.9 control: 360.1
index:18 : target: 1 current: 208.9 error: -207.9 errorSum: -119.9 control: -491.7
index:19 : target: 1 current: -282.9 error: 283.9 errorSum: 164.0 control: 671.9
从上图可以看出，ki太大可能会导致振荡。
比例积分微分控制
考虑刹车这个场景。驾驶车辆时，当发现前面有红灯时，为了使得行车平稳，基本上提前几十米就放松油门并踩刹车了。当车辆离停车线非常近的时候，则使劲踩刹车使车辆停下来。以上过程可以看做一个加入微分的控制策略。
微分控制是为了让你在快要达到目标时令控制量降低而不至于超调。
举个更容易说明的小例子：
1）距离停车线50米速度为10m/s，执行时间为1s，则在距离停车线40米假设速度为8m/s，此时系数为1的话，那么微分项-10；
2）距离停车线50米速度为5m/s，执行时间为1s，则在距离停车线45米假设速度为4m/s，此时系数为1的话，那么微分项-5；
因此越解决目标控制量越小，则微分项越小
另外不过这个参数设计不好容易引起高频振荡。
import numpy as np
from matplotlib import pyplot as plt
times = 21 #迭代次数
x = np.arange(0, times, 1) #X轴
target = np.ones(times, dtype=np.int32) #目标位置是1米
current = 0.2 * np.ones(times, dtype=np.int32) #当前位置
kp = 0.5
ki = 0.1
kd = 0.1
errorSum = 0
currentError = 0
previousError = 0
for time in range(1, times-1):
print("index:", end='')
print("%-3d:" % time, end=' ')
print("target:", 1, end=' ')
print("current:", end=' ')
print('%-5.1f' % round(current[time], 3), end=' ')
currentError = target[time] - current[time]
errorSum = errorSum + currentError
print("currentError:", end=' ')
print('%-5.1f' % round(currentError, 3), end=' ')
print("errorSum:", end=' ')
print('%-5.1f' % round(errorSum, 3), end=' ')
print("errorerror:", end=' ')
print('%-5.1f' % round(currentError - previousError, 3), end=' ')
control = kp * currentError + ki * errorSum + kd * (currentError - previousError)
current[time+1] = current[time] + control - 0.1
print("control:", end=' ')
print("%-5.3f" %round(control, 3))
previousError = currentError
plt.title("PID")
plt.plot(x, current, 'o-g')
plt.plot(x, target, 'p-r')
plt.show()
输出：
index:1 : target: 1 current: 0.2 currentError: 0.8 errorSum: 0.8 errorerror: 0.8 control: 0.560
index:2 : target: 1 current: 0.7 currentError: 0.3 errorSum: 1.1 errorerror: -0.5 control: 0.238
index:3 : target: 1 current: 0.8 currentError: 0.2 errorSum: 1.3 errorerror: -0.1 control: 0.221
index:4 : target: 1 current: 0.9 currentError: 0.1 errorSum: 1.4 errorerror: -0.1 control: 0.170
index:5 : target: 1 current: 1.0 currentError: 0.0 errorSum: 1.4 errorerror: -0.1 control: 0.141
index:6 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.4 errorerror: -0.0 control: 0.120
index:7 : target: 1 current: 1.1 currentError: -0.1 errorSum: 1.4 errorerror: -0.0 control: 0.107
index:8 : target: 1 current: 1.1 currentError: -0.1 errorSum: 1.3 errorerror: -0.0 control: 0.099
index:9 : target: 1 current: 1.1 currentError: -0.1 errorSum: 1.2 errorerror: 0.0 control: 0.095
index:10 : target: 1 current: 1.1 currentError: -0.1 errorSum: 1.2 errorerror: 0.0 control: 0.092
index:11 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.1 errorerror: 0.0 control: 0.092
index:12 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.1 errorerror: 0.0 control: 0.092
index:13 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.1 errorerror: 0.0 control: 0.093
index:14 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.094
index:15 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.095
index:16 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.096
index:17 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.097
index:18 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.098
index:19 : target: 1 current: 1.0 currentError: -0.0 errorSum: 1.0 errorerror: 0.0 control: 0.099
最后
关于增量PID根据公式，参考上面很轻松就可以修改出来，这里就不再写了。