前言
第一次接觸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根據公式，參考上面很輕鬆就可以修改出來，這裏就不再寫了。