最近，在某社團的要求下，自學了PID算法。學完后，深切地感受到PID算法之強大。PID算法應用廣泛，比如加熱器、平衡車、無人機等等，是自動控制理論中比較容易理解但十分重要的算法。
下面是博主學習過程中所做的筆記，筆記后面提供了4種編程語言的仿真代碼（C, C++, Python, Matlab)，使實現方式更加靈活，同時增強對PID的理解。(文章較長，可點擊右側目錄選擇性閱讀）
PID算法學習筆記
參考：PID基礎入門教程
一、位式控制算法
1.1 位式控制算法原理
位式控制算法，通過比較SV(設定值)和PV(當前值)，輸出高低電平給執行部件，執行部件（如開關）通過執行/停止來控制目標（如加熱器），控制對象通過傳感器將當前值反饋給控制算法，如圖1。
圖1 位式控制算法簡單應用
1.2 位式控制算法特點
位式控制算法具有如下特點：
（1）輸出信號一般只有兩種狀態(LOW / HIGH)。
（2）通過比較SV和PV的值來產生OUT值，比如PV < SV輸出高電平，PV > SV輸出低電平。
（3）只比較控制對當前的狀態值。
1.3 位式控制算法缺陷
位式控制算法的缺陷：
（1）輸出信號單一，缺乏包容性。
（2）僅僅活在當下，沒有回顧歷史和展望未來。
二、PID控制算法
2.1 PID算法原理
PID控制算法，通過分析PV與SV的偏差值（包括當前偏差、歷史偏差、最近偏差），輸出值（PWM）經過執行器轉換后應用於控制對象，控制對象通過傳感器將PV反饋給PID，通過硬件寄存器等記錄偏差值，以便PID隨時調用，如圖2。
圖2 PID控制算法簡單應用
假設從“0”時刻到 k 時刻，傳感器獲取的狀態值分別為
${X_{0}, X_{1}, X_{2}, ..., X_{k-1}, X_{k}}$
2.2 PID比例控制
在2.1的條件下，設偏差值 Ek 為設定值與當前值之差，即
${E_{k}=SV-X_{k}}$
實際應用中的偏差值存在如下3種情況
分析以上三種情況，不同情況下算法將輸出不同值。比如算法在Ek > 0時輸出較高的值，以促進當前值接近設定值。而實際應用中，控制對象的狀態偏差值一般不能直接作為PWM輸出值，需要進行一定比例的放大或縮小，以提高控制靈敏度。因此輸出值滿足關系式
${P_{out}=K_{p}\cdot E_{k}+OUT_{0}}$ ①
其中，POUT為輸出值，一般與PWM有關。Kp 為比例系數，對偏差值Ek 進行一定比例的放大或縮小。OUT0 是當偏差值為 0 時，算法的輸出值，防止負載失控。分析公式可知，偏差值Ek 越大，輸出值越大，當前值接近設定值的速度越快，當前值超過設定值時，Ek < 0, 算法輸出負值，當前值減小。往復循環，直到當前值穩定在設定值的誤差允許范圍內。
2.3 PID積分控制
在2.2的條件下，將歷史偏差相加，其和為
${S_{k}=E_{0}+E{1}+E_{2}+...+E_{k-1}+E_{k}}$
實際應用中的歷史偏差值之和存在如下3種情況
不同情況下輸出值應不同，分析以上情況，可以令輸出值滿足關系式
${I_{out}=K_{p}\cdot S_{k}+OUT_{0}}$ ②
其中Iout為輸出值，Kp為比例系數，Sk為歷史偏差和，OUT0為初始值。通過上述算法，可以對控制對象的歷史狀態值進行評估，根據歷史狀態判斷輸出值的大小。這種方法比較局限，因為當歷史值較多時，當前值的變化將很難引起輸出值改變，因此積分控制一般不會從0開始啟動，當當前值接近設定值時才開啟積分控制，以減少參考的歷史值。
2.4 PID微分控制
在2.2的條件下，將最近兩次偏差值相減，其差為
${D_{k}=E_{k}-E_{k-1}}$
實際應用中的最近偏差值之差存在如下3種情況
不同情況下輸出值應不同，分析以上情況，可以令輸出值滿足關系式
${D_{out}=K_{p}\cdot D_{k}+OUT_{0}}$ ③
其中Dout為輸出值，Kp為比例系數，Sk為歷史偏差和，OUT0為初始值。通過最近偏差值之差，判斷偏差值的變化趨勢，預測未來的偏差值大小，從而輸出對應的PWM。
2.5 PID算法模型
根據以上分析，每種控制算法均有較大局限。因此綜合①②③算法，令輸出值為
${PID_{out_{k}}=P_{out}+I_{out}+D_{out}}$
代入①②③關系式，並進行簡單歸並，得到關系式
${PID_{out_{k}}=K_{p}\cdot \left ( E_{k}+S_{k}+D_{k}\right )+OUT_{0}}$ ④
分析上式中 Sk , Dk 的值，假設T為計算周期，即每次運行算法的時間間隔，Ti為積分時間常數，用於控制積分算法對輸出值的影響因數，Td為微分時間常數，用於控制微分算法對輸出值的影響。
綜上，積分控制Sk滿足關系式
${S_{k}=\frac{1}{T_{i}}\cdot\sum_{k=0}^{k}E_{k} \cdot T}$ ⑤
微分控制Dk滿足關系式
${D_{k}=T_{d}\cdot\frac{\left ( E_{k}-E_{k-1}\right )}{T}}$ ⑥
綜合④⑤⑥，並進行簡單的歸並處理后，得到PID的輸出關系式
${PID_{out_{k}}=P\left (K_{p}\cdot E_{k} \right )+I\left (K_{p}\cdot \frac{T}{T_{i}}\cdot \sum_{k=0}^{k}\cdot E_{k} \right )+D\left [K_{p}\cdot \frac{T_{d}}{T}\cdot\left(E_{k}-E_{k-1}\right) \right ]}$ ⑦
其中P，I，D分別表示比例，積分，微分控制。通過調整 Kp ，Ti ，Td 的值來調整P， I，D對輸出值的影響權重，從而使當前值更快接近並穩定在設定值誤差允許范圍內。
上述算法有一個明顯的特點，即計算結果輸出為PWM值，直接控制負載。因此又被稱為“位置式PID算法“
2.6 增量式PID算法
實際應用中，大部分控制系統具有記憶功能，可以記錄每個時刻狀態值，因此為了減小累加產生的運算負擔，可以采用計算“增量”的方式來輸出控制信號。
增量式PID算法的特點是只計算增加（減小）值，歷史值加上增加值即為輸出值，滿足關系式
${\Delta PID_{out}=PID_{out_{k}}-PID_{out_{k-1}}}$ ⑧
代入④關系式，
${\Delta PID_{out}=P\left [K_{p}\cdot \left ( E_{k}-E_{k-1}\right ) \right ]+I\left (K_{p}\cdot \frac{T}{T_{i}}\cdot E_{k} \right )+D\left [K_{p}\cdot \frac{T_{d}}{T}\cdot \left ( E_{k}-2E_{k-1}-E_{k-2}\right ) \right ]}$ ⑨
對比⑧式和⑦式，⑧式運算量更小。因此對於有記憶功能的硬件系統，可以使用增量式PID算法，以減少運算，提升性能。
PID仿真實驗
一、問題
既然是仿真實驗，那就應該以模擬解決生活中的問題為主，為了進行比較具體，但不復雜的仿真實驗，博主絞盡腦汁，終於構造了下面這個題目。
在《機甲大師》動漫中，主角“單單”擁有一架語音遙控的雙旋翼無人機，名叫“KAKA"。如圖1，動漫第一集5:35左右，KAKA在追蹤飛盤時，突然受海風影響，飛行姿態偏離水平位置。性能超高的KAKA通過內部傳感器測得偏角后，迅速調整姿態，恢復水平。請對這一情形進行建模分析。
圖1 被海風影響的KAKA
二、解答
分析題目，需要對KAKA“恢復姿態”這一現象進行分析。圍繞這個問題，下面以“建立物理模型→建立數學模型→算法仿真”進行逐步分析。
2.1 建立物理模型
首先簡化問題，將KAKA看作剛性直桿。如圖2.1，一質量為2m，長度為2r的剛性直桿，兩端垂直固定一個不計質量的直流電機。直桿可以繞中心自由旋轉，初始位置相對水平線偏離θ角。
為了使直桿恢復水平位置，改變右端電機轉速，產生“額外”升力F。
根據以上參數，在理想情況下，可以得到直桿的合外力矩M滿足
${M = F\cdot r}$ ①
轉動慣量J滿足
${J=\frac{2}{3}mr^{2}}$ ②
由剛體軸轉動定理
${M=J\cdot \alpha}$ ③
其中α為角加速度，滿足關系式
${\alpha =\frac{\mathrm{d^2}\theta }{\mathrm{d} t^2}}$ ④
其中t為時間，聯立①②③④，求解微分方程可得到關系式
${\theta _{t}=\frac{3\cdot F}{4\cdot m\cdot r}\cdot t^2}$ ⑤
其中θ為直桿在力合外力F的作用下，經過時間t后轉動的角度。
2.2 建立數學模型
設${T}$為計算周期，在⑤式的條件下，令${t=T}$，在${T}$時間內直桿轉動角度滿足關系式
${\theta _{T} =\frac{3\cdot F}{4\cdot m\cdot r}\cdot T^2}$ ⑥
假設${F}$隨時間的變化周期為${T}$，那么經過${t}$時間后，${F}$變化${n}$次，直桿轉動角度滿足
${\theta =\frac{3\cdot T^2}{4\cdot m\cdot r}\cdot\sum_{n=0}^{n} F_{n}}$ ⑦
直桿與水平線的當前偏差角${E_{k}}$滿足
${E_{k}=\theta_{0}-\frac{3\cdot T^2}{4\cdot m\cdot r}\cdot\sum_{n=0}^{k} F_{n}}$ ⑧
上式即為直桿在恢復水平位置過程中，在合外力F作用下，當前偏角對於時間的函數。
參考實際情況，由於合外力以T為周期發生變化，該偏角函數曲線應如滿足圖2.2
圖2.2 （預測）直桿偏角對於時間的變化曲線
分析上圖曲線，發現其變化趨勢可以用PID算法進行擬合。
2.3 PID算法仿真
分析關系式⑧，其中${T}$可以看作采樣周期，${F}$為算法輸出值，${E_{k}}$為當前偏差角。應用位置式${PID}$算法，設比例系數為${K_{p}}$，積分時間常數為${T_{i}}$，微分時間常數為${T_{d}}$，輸出值滿足PID算法關系式
${F_{out_{k}}=K_{p}\cdot E_{k} +K_{p}\cdot \frac{T}{T_{i}}\cdot \sum_{k=0}^{k}\cdot E_{k} + K_{p}\cdot \frac{T_{d}}{T}\cdot E_{k}-E_{k-1}}$ ⑨
分析⑧式，為了簡化計算，不妨令
${m = 0.3}$, ${r=0.1}$, ${\theta _{0}=\frac{\pi}{3}}$ （${SI}$）
則當前偏差角滿足
${E_{k}=\frac{\pi}{3}-25\cdot T^2\cdot\sum_{n=0}^{k} F_{out_{n}}}$ ⑩
綜上，可以假設如表2.3中的參數
表2.3 PID參數
下面，在上述模型條件下，用5種編程語言(Matlab, C, C++, Python)進行算法仿真。
1) Matlab
比較方便，可以先通過Matlb仿真確定PID系數
源碼：
clear,clc,close all % 清屏
syms x
SV = 0; % 設定值，角（弧）度 0 (rad)
T = 0.01; % 計算周期/采樣周期
Kp = 50; % 比例系數
Ti = 5; % 積分時間常數
Td = 0.05; % 微分時間常數
E = []; % 歷史偏差
Fout = []; % 輸出值，升力
E(1) = pi ./3; % 初始角度 π/3 (rad)
for i = 1:1:3 % 繪制3種比較曲線
if i == 2;
Kp = 50; % 比例系數
Ti = 0.05; % 積分時間常數
Td = 0.05; % 微分時間常數
E = []; % 歷史偏差
Fout = []; % 輸出值，升力
E(1) = pi ./3; % 初始角度 π/3 (rad)
end % if i ==2
if i ==3;
Kp = 50; % 比例系數
Ti = 5; % 積分時間常數
Td = 0.005; % 微分時間常數
E = []; % 歷史偏差
Fout = []; % 輸出值，升力
E(1) = pi ./3; % 初始角度 π/3 (rad)
end % if i ==3
for t = 0:0.01:3; % 計算300次
k = round(t*100 + 2); % 當前指數
E(k) = E(1) - 25*(T^2)*sum(Fout); % 獲取當前值
%#### 核心，PID計算輸出值 ####%
if k>2;
if E(k) != 0;
Fout(k) = Kp*(E(k) + (T./Ti)*sum(E) + (Td./T)*(E(k)-E(k-1)));
end % end if E(k) !=0
end % end if k>2
%#############################%
k++; % 當前指數+1
end % end for 計算400次
hold on
plot(E) % 顯示數據圖
end % for 繪制3種比較曲線
legend('PID(50, 5, 0.05)','PID(50, 0.05, 0.05)','PID(20, 5, 0.005)')
hold on
plot([0,300],[0,0],'--'); % 顯示參考線，斜率0，截距0
運行結果
2) C語言
1 /**@file main.c 2 * @brief 位置式PID C語言算法仿真 3 * @author BROSY 4 * @copyright CSU | BROSY 5 ********************************************************************************/ 6 7 8 /************************************************************************************* 9 注：以便查閱，我將所有函數和聲明都放在main.c中，進行項目實踐時，再設計文件架構 10 *************************************************************************************/ 11 12 13 #include<stdio.h> 14 #define PI (3.1416) 15 16 typedef struct { 17 const int SV = 0; // 設定值（弧度rad) 18 19 double InitVal; //初始偏差值 20 double T; // 采樣周期 21 double Kp; // 比例系數 22 double Ti; // 積分時間常數 23 double Td; // 微分時間常數 24 double Ek; //當前偏差 25 double SumEk; //歷史偏差之和 26 double Ek_1; //上次偏差 27 double SumFout; // 輸出值之和 28 }PID_Structure; 29 30 /** 31 @brief 位置式PID輸出函數 32 @param [in] PID結構體 33 @return 算法輸出值（額外升力） 34 */ 35 double PID_OUT(PID_Structure* PID) 36 { 37 double Fout; 38 Fout = PID->Kp * (PID->Ek 39 + (PID->T / PID->Ti) * PID->SumEk 40 + (PID->Td / PID->T) * (PID->Ek - PID->Ek_1)); 41 42 return Fout; // 輸出值（額外升力） 43 } 44 45 46 /** 47 @brief 獲取當前偏差值 48 @param [in] PID結構體, 歷史輸出值（數組） 49 @return kaka當前狀態偏差值 50 */ 51 double GetCurrE(PID_Structure PID) 52 { 53 double Ek; 54 Ek = PID.InitVal - 25 * (PID.T * PID.T) * PID.SumFout; 55 return Ek; 56 } 57 58 int main() 59 { 60 PID_Structure PID; // 創建PID 61 62 PID.InitVal = PI / 3; 63 PID.T = 0.01; 64 PID.Kp = 50; 65 PID.Ti = 5; 66 PID.Td = 0.005; 67 PID.Ek = 0; 68 PID.Ek_1 = 0; 69 PID.SumFout = 0; 70 PID.SumEk = 0; 71 72 // 計算400次 73 for (int i = 0; i < 400; i++) 74 { 75 if (i > 0) 76 { 77 PID.Ek_1 = PID.Ek; // 獲取k-1的偏差值 78 } 79 PID.Ek = GetCurrE(PID); // 獲取當前偏差值 80 PID.SumEk += PID.Ek; // 歷史偏差之和 81 82 printf("%f\n", PID.Ek); 83 if (PID.Ek != 0 && i > 0) // 誤差 84 { 85 PID.SumFout += PID_OUT(&PID); // 獲取輸出值之和 86 87 } 88 else 89 { 90 PID.SumFout += 0; // 儲存輸出值 91 } 92 } 93 94 }
將輸出結果導入到excel中並繪制曲線：
3) C++
1 /**@file main.cpp 2 * @brief 位置式PID C語言算法仿真 3 * @author BROSY 4 * @copyright CSU | BROSY 5 ********************************************************************************/ 6 7 #include "PID.h" 8 9 int main() 10 { 11 PID* pid[3]; // 創建PID 12 13 14 pid[0] = new PID(50, 5, 0.05); // 初始化PID1 15 pid[1] = new PID(50, 0.05, 0.05); // 初始化PID2 16 pid[2] = new PID(50, 5, 0.005); // 初始化PID3 17 18 for (int i = 0; i < 3; i++) 19 { 20 pid[i]->Loop(400);// 計算400次 21 delete pid[i]; // 釋放內存 22 } 23 }
1 #include "PID.h" 2 #include <iostream> 3 /** 4 @brief 初始化PID參數 5 @param [in] P I D系數 6 只設置P I D的系數，其余默認 7 */ 8 PID::PID(double P, double I, double D) 9 { 10 Kp = P; 11 Ti = I; 12 Td = D; 13 14 InitVal = (3.1426)/3; // 初始偏差值π/3 15 T = 0.01; // 采樣周期 16 Ek = 0; //當前偏差 17 SumEk = 0; //歷史偏差之和 18 Ek_1 = 0; //上次偏差 19 SumFout = 0; // 輸出值之和 20 } 21 22 /** 23 @brief 位置式PID輸出函數 24 @return 算法輸出值（額外升力） 25 */ 26 double PID::PID_OUT() 27 { 28 double Fout; 29 30 Fout = Kp * (Ek 31 + (T / Ti) * SumEk 32 + (Td / T) * (Ek - Ek_1)); 33 34 return Fout; // 輸出值（額外升力） 35 } 36 37 38 /** 39 @brief 獲取當前偏差值 40 @return kaka當前狀態偏差值 41 */ 42 double PID::GetCurrE() 43 { 44 double Ek; 45 Ek = InitVal - 25 * (T * T) * SumFout; 46 return Ek; 47 } 48 49 /** 50 @brief 循環計算並輸出值 51 @param [in] 計算次數 52 */ 53 void PID::Loop(int times) 54 { 55 std::cout << "計算次數：" << times << std::endl; 56 std::cout << "P = " << Kp << std::endl; 57 std::cout << "I = " << Ti << std::endl; 58 std::cout << "D = " << Td << std::endl<<std::endl; 59 60 for (int i = 0; i < times; i++) 61 { 62 if (i > 0) 63 { 64 Ek_1 = Ek; // 獲取k-1的偏差值 65 } 66 Ek = GetCurrE(); // 獲取當前偏差值 67 SumEk += Ek; // 歷史偏差之和 68 69 std::cout << Ek << std::endl; 70 if (Ek != 0 && i > 0) // 誤差 71 { 72 SumFout += PID_OUT(); // 獲取輸出值之和 73 74 } 75 else 76 { 77 SumFout += 0; // 儲存輸出值 78 } 79 } 80 }
#pragma once class PID { private: const int SV = 0; // 設定值（弧度rad) double InitVal; //初始偏差值 double T; // 采樣周期 double Kp; // 比例系數 double Ti; // 積分時間常數 double Td; // 微分時間常數 double Ek; //當前偏差 double SumEk; //歷史偏差之和 double Ek_1; //上次偏差 double SumFout; // 輸出值之和 public: PID(double P, double I, double D); // PID初始化，只輸入PID系數，其余默認 double PID_OUT(); // PID算法核心，計算輸出值 double GetCurrE(); // 獲取當前值 void Loop(int times); // 循環計算輸入計算次數 };
將輸出結果導入到excel中並繪制曲線：
4) Python
1 import matplotlib.pyplot as plt # 導入繪圖庫 2 import numpy as np 3 4 ''' 5 @brief 位置式PID輸出函數 6 @param [in] PID結構體 7 @return 算法輸出值（額外升力） 8 ''' 9 10 11 def pid_out(): 12 f_out = Kp * (Ek 13 + (T / Ti) * sum_Ek 14 + (Td / T) * (Ek - Ek_1)) 15 return f_out 16 17 18 ''' 19 @brief 獲取當前偏差值 20 @param [in] PID結構體, 歷史輸出值（數組） 21 @return kaka當前狀態偏差值 22 ''' 23 24 25 def get_curr_e(): 26 ek = init_val - 25 * (T ** 2) * sum_f_out 27 return ek 28 29 30 sv = 0.0 # 設定值 31 init_val = (3.1416) / 3 # 初始值 32 T = 0.01 # 采樣周期 33 times = 300 # 計算次數 34 e = np.zeros(times) 35 for t in range(3): 36 Ek = 0.0 # 當前偏差 37 sum_Ek = 0.0 # 歷史偏差之和 38 Ek_1 = 0.0 # 上一次偏差 39 sum_f_out = 0.0 # 輸出值之和（升力） 40 41 if t == 0: 42 Kp = 50 # 比例系數 43 Ti = 5 # 積分時間常數 44 Td = 0.05 # 微分時間常數 45 if t == 1: 46 Kp = 50 # 比例系數 47 Ti = 0.05 # 積分時間常數 48 Td = 0.05 # 微分時間常數 49 if t == 2: 50 Kp = 50 # 比例系數 51 Ti = 5 # 積分時間常數 52 Td = 0.005 # 微分時間常數 53 54 ''' 55 @brief 循環計算並輸出值 56 @param [in] 計算次數 57 ''' 58 for i in range(times): 59 if i > 0: 60 Ek_1 = Ek 61 62 Ek = get_curr_e() # 獲取當前值 63 sum_Ek = sum_Ek + Ek # 獲取歷史值之和 64 65 e[i] = Ek # 儲存當前值，方便后面繪圖 66 67 if Ek != 0 and i > 0: 68 sum_f_out = sum_f_out + pid_out() # 獲取輸出值之和 69 70 plt.plot(e, label='PID({0}, {1}, {2})'.format(Kp, Ti, Td)) # 畫曲線圖，顯示PID圖例 71 72 plt.plot(np.zeros(times + 25), label='SV', linestyle='--') # 設定值 73 plt.legend() # 顯示圖例 74 75 plt.show()
下載源碼
鏈接：PID仿真源碼 密碼：hhh