前文對PID算法離散化和增量式PID算法原理進行來探索，之後又使用Matlab進行了仿真實驗，對PID三個參數又有了更深入的認識，接下來我們來使用C語言進行PID算法實現，並且結合控制電機的項目來深入學習。
1、PID 算法C 語言原代碼
先貼上一種常見的比較通用的C語言增量式PID算法吧
typedef struct PID
{
intSetPoint; //設定目標 DesiredValue
longSumError; //誤差累計
doubleProportion; //比例常數Proportional Const
doubleIntegral; //積分常數 IntegralConst
doubleDerivative; //微分常數Derivative Const
intLastError; //Error[-1]
intPrevError; //Error[-2]
} PID;
static PID sPID;
static PID *sptr = &sPID;
/
*=============================================================
=======
InitializePID Structure PID 參數初始化
=============================================================
======*/
void IncPIDInit(void)
{
sptr->SumError= 0;
sptr->LastError= 0; //Error[-1]
sptr->PrevError= 0; //Error[-2]
sptr->Proportion= 0; //比例常數Proportional Const
sptr->Integral= 0; //積分常數IntegralConst
sptr->Derivative= 0; //微分常數Derivative Const
sptr->SetPoint= 0;
}
/
*=============================================================
======= 增量式PID 計算部分
=============================================================
=======*/
int IncPIDCalc(int NextPoint)
{
registerint iError, iIncpid; //當前誤差
iError= sptr->SetPoint - NextPoint; //增量計算
iIncpid= sptr->Proportion * iError //E[k]項
-sptr->Integral * sptr->LastError //E[k-1]項
+sptr->Derivative * sptr->PrevError; //E[k-2]項
//存儲誤差，用於下次計算
sptr->PrevError= sptr->LastError;
sptr->LastError= iError;
//返回增量值
return(iIncpid);
}2、PID 整定口訣
參數整定找最佳， 從小到大順序查。
先是比例後積分， 最後再把微分加。
曲線振盪很頻繁， 比例度盤要放大。
曲線漂浮繞大彎， 比例度盤往小扳。
曲線偏離回覆慢， 積分時間往下降。
曲線波動週期長， 積分時間再加長。
曲線振盪頻率快， 先把微分降下來。
動差大來波動慢， 微分時間應加長。
理想曲線兩個波， 前高後低四比一。
一看二調多分析， 調節質量不會低。
說實話整定口訣對於初學者來說，其實根本就看不懂，只有從實際整定過程中才能慢慢發覺其中的奧祕。
3、項目原理
學到這裏對PID算法的理解只是停留在理論，那麼我們來結合實際看看，這裏我參考的是平衡小車之家的資料，不得不說確實淺顯易懂，先從電機的簡單控制開始理解吧！
3.1直流電機
簡單來說就是，把+和-分別接到電池的正極和負極，電機即可轉動；
如果是把的+和-分別接到電池的負極和正極，則電機會反方向轉動。電機的轉速可以理解爲和外接的電壓是正相關的（實際是由電樞電流決定）。
3.2.減速器
一般直流電機的轉速都是一分鐘幾千上萬轉的，所以一般需要安裝減速器。減速器是一種相對精密的機械零件，使用它的目的是降低轉速，增加轉矩。減速後的直流電機力矩增大、可控性更強。按照傳動級數不同可分爲單級和多級減速器；按照傳動類型可分爲齒輪減速器、蝸桿減速器和行星齒輪減速器。
3.3電機驅動
要實現電機調試和換向功能，我們可以使用單片機實現的，但是單片機IO 的帶負載能力較弱，而直流電機是大電流感性負載，所以我們需要功率放大器件，在這裏，我們選擇了 TB6612FNG。
TB6612FNG 是東芝半導體公司生產的一款直流電機驅動器件，它具有大電流MOSFET-H橋結構，雙通道電路輸出，可同時驅動 2 個電機。也許大家更熟悉被用爛的L298N，其實這兩者的使用基本一致的。而且，相比 L298N 的熱耗性和外圍二極管續流電路，它無需外加散熱片，外圍電路簡單，只需外接電源濾波電容就可以直接驅動電機，利於減小系統尺寸。對於 PWM 信號輸入頻率範圍，高達 100 kHz 的頻率更是足以滿足我們大部分的需求了。
3.4編碼器
編碼器是一種將角位移或者角速度轉換成一連串電數字脈衝的旋轉式傳感器，我們可以通過編碼器測量到底位移或者速度信息。編碼器從輸出數據類型上分，可以分爲增量式編碼器和絕對式編碼器。
從編碼器檢測原理上來分，還可以分爲光學式、磁式、感應式、電容式。常見的是光電編碼器（光學式）和霍爾編碼器（磁式）。
這裏使用增量式輸出的霍爾編碼器。編碼器有 AB 相輸出，所以不僅可以測速，還可以辨別轉向。根據上圖的接線說明可以看到，我們只需給編碼器電源5V 供電，在電機轉動的時候即可通過 AB 相輸出方波信號。編碼器自帶了上拉電阻，所以無需外部上拉，可以直接連接到單片機IO讀取。
那麼單片機如何採集編碼器數據？
因爲編碼器輸出的是標準的方波，所以我們可以使用單片機（STM32 STM8 51等）直接讀取。在軟件中的處理方法是分兩種，自帶編碼器接口的單片機如STM32，可以直接使用硬件計數。而沒有編碼器接口的單片機如 51 單片機，可以通過外部中斷讀取，比如把編碼器 A 相輸出接到單片機的外部中斷輸入口，這樣就可通過跳變沿觸發中斷，然後在對應的外部中斷服務函數裏面，通過 B 相的電平來確定正反轉。如當 A 相來一個跳變沿的時候，如果 B 相是高電平就認爲是正轉，低電平就認爲是反轉。
4、電機速度閉環控制
4.1原理
速度閉環控制就是根據單位時間獲取的脈衝數測量電機的速度信息，並與目標值進行比較，得到控制偏差，然後通過對偏差的比例、積分、微分進行控制，使偏差趨向於零的過程。
需要說明的是，這裏速度控制 20ms 一次，一般建議 10ms 或者 5ms，因爲在這裏電機是使用 USB 供電，速度比較慢，20ms 可以延長獲取速度的單位時間，提高編碼器的採值。
首先由於需要知道速度，所以一般都需要帶編碼器的電機，編碼器輸出有ab相，可以通過單片機定時器的捕獲模式來得到速度，之後在單片機內部進行PID算法的運算，得到輸出所需要的速度，通過控制佔空比來輸出PWM波，控制電機的速度，這裏用的主控是STM32c8t6。
4.2核心代碼
/**************************************************************************
函數功能：增量PI控制器
入口參數：編碼器測量值，目標速度
返回 值：電機PWM
根據增量式離散PID公式
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此類推
pwm代表增量輸出
在我們的速度控制閉環系統裏面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI (int Encoder,int Target)
{
float Kp=20,Ki=30;
static int Bias,Pwm,Last_bias; //相關內部變量的定義。
Bias=Encoder-Target; //求出速度偏差，由測量值減去目標值。
Pwm+=Kp*(Bias-Last_bias)+Ki*Bias; //使用增量 PI 控制器求出電機 PWM。
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量輸出
}這裏可以看到使用的是增量式比例積分控制器，Kp和Ki的值在函數中臨時設置，完全按照公式編寫，簡單易懂。
4.3定時控制
int Target_velocity=50; //設定速度控制的目標速度爲50個脈衝每10ms
int TIM3_IRQHandler(void)
{
if(TIM3->SR&0X0001)//10ms定時中斷
{
TIM3->SR&=~(1<<0); //===清除定時器1中斷標誌位
Encoder=Read_Encoder(2); //===讀取編碼器的值，M法測速，輸出爲每10ms的脈衝數
Led_Flash(100); //===LED閃爍;指示單片機正常運行
Moto1=Incremental_PI(Encoder,Target_velocity); //===速度PI控制器
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto1); //===賦值給PWM寄存器
}
return 0;
} 這裏控制週期設定的是每10ms控制一次，設置在10ms的中斷中進行，得到控制量後，在經過簡單的賦值和去絕對值來輸出給驅動的PWM控制器。
4.4其他代碼
/**************************************************************************
函數功能：賦值給PWM寄存器
入口參數：PWM
返回 值：無
**************************************************************************/
void Set_Pwm(int moto1)
{
if(moto1>0) AIN2=1, AIN1=0;
else AIN2=0, AIN1=1;
PWMA=myabs(moto1);
}
/**************************************************************************
函數功能：限制PWM賦值
入口參數：無
返回 值：無
**************************************************************************/
void Xianfu_Pwm(void)
{
int Amplitude=7100; //===PWM滿幅是7200 限制在7100
if(Moto1<-Amplitude) Moto1=-Amplitude;
if(Moto1>Amplitude) Moto1=Amplitude;
}
/**************************************************************************
函數功能：絕對值函數
入口參數：int
返回 值：unsigned int
**************************************************************************/
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}主函數
int main(void)
{
Stm32_Clock_Init(9); //系統時鐘設置
...
MiniBalance_PWM_Init(7199,0); //=====初始化PWM 10KHZ 高頻可以防止電機低頻時的尖叫聲
Encoder_Init_TIM2(); //初始化編碼器
Timer3_Init(99,7199); //=====10MS進一次中斷服務函數，中斷服務函數在control.c
while(1);
}5、電機位置閉環控制
位置閉環控制就是根據編碼器的脈衝累加測量電機的位置信息，並與目標值進行比較，得到控制偏差，然後通過對偏差的比例、積分、微分進行控制，使偏差趨向於零的過程。
5.1核心代碼
/**************************************************************************
函數功能：位置式PID控制器
入口參數：編碼器測量位置信息，目標位置
返回 值：電機PWM
根據位置式離散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累積和;其中k爲1,2,,k;
pwm代表輸出
**************************************************************************/
int Position_PID (int Encoder,int Target)
{
float Position_KP=80,Position_KI=0.1,Position_KD=500;
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target; //求出速度偏差，由測量值減去目標值。
Integral_bias+=Bias; //求出偏差的積分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm; //增量輸出
}
這裏採用稍微複雜一點的PID控制，相比之前的速度控制多了個微分環節，但是由於是位置式控制，所以代碼較爲簡單，容易理解。
5.2控制中斷函數
int Target_position=11000; //初始值是10000，目標值是11000
int TIM3_IRQHandler(void)
{
if(TIM3->SR&0X0001)//10ms定時中斷
{
TIM3->SR&=~(1<<0); //===清除定時器1中斷標誌位
Encoder=Read_Encoder(2); //===讀取編碼器的位置數據 初始值是10000
Led_Flash(100); //===LED閃爍;指示單片機正常運行
Moto1=Position_PID(Encoder,Target_position); //===位置PID控制器
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto1); //===賦值給PWM寄存器
}
return 0;
}
其他代碼和上面類似
6.參數整定
首先我們需要明確我們的控制目標，也就是滿足控制系統的 3 個要求：
- 穩定性
- 快速性
- 準確性
具體的評估指標有最大超調量、上升時間、靜差等。
最大超調量是響應曲線的最大峯值與穩態值的差，是評估系統穩定性的一個重要指標；上升時間是指響應曲線從原始工作狀態出發，第一次到達輸出穩態值所需的時間，是評估系統快速性的一個重要指標；靜差是被控量的穩定值與給定值之差，一般用於衡量系統的準確性，具體可以參考前文的講解。
在實踐生產工程中，不同的控制系統對控制器效果的要求不一樣。比如平衡車、倒立擺對系統的快速性要求很高，響應太慢會導致系統失控。智能家居里面的門窗自動開合系統，對快速性要求就不高，但是對穩定性和準確性的要求就很高，所以需要嚴格控制系統的超調量和靜差。所以 PID 參數在不同的控制系統中是不一樣的。只要我們理解了每個 PID 參數的作用，我們就可以應對工程中的各種項目的 PID 參數整定了。
一般而言，一個控制系統的控制難度，一般取決於系統的轉動慣量和對響應速度的要求等。轉動慣量越小、對響應速度要求越低，PID 參數就越不敏感。比如現在我們控制電機轉 90°，需要嚴格控制超調量、和靜差。但是對響應速度無要求。因爲電機處於輕載的情況下，轉動慣量很小，這是一個很容易完成的工作。根據上面的理論分析和實踐，因爲響應速度無要求，一般 P 應該給小一點，然後加大系統的阻尼防止超調，也就是 D 參數儘量大，另外因爲 P 值較小，應該加入I 控制減小靜差。
7.總結
調試裝置的過程中會遇到各種各樣的問題，硬件隨外界環境會不斷的出現變化，會干擾我們的調試以及運行結果。整定出能適應各種環境的參數，必須對每個環境都加以測試，綜合得出最合適的參數。所以在圖形化的調試整定過程能夠快速直觀的得出結論。面對更復雜的情況如平衡車或者四旋翼飛行器，多個傳感器採集的數據在複雜的情況下還要考慮濾波，限幅，權重等問題，這些將在後續文章中進行總結。
增量式PID速度調節代碼已經上傳，位置式的類似可以參考改動https://download.csdn.net/download/kilotwo/10350922