增量式PID算法的STM32實(shí)現(xiàn) 分析比例、積分、微分三個(gè)環(huán)節(jié)

雖然PID不是什么牛逼的東西，但是真心希望以后剛剛接觸這塊的人能盡快進(jìn)入狀態(tài)。特地分享一些自己如何實(shí)現(xiàn)的過(guò)程。
首先說(shuō)說(shuō)增量式PID的公式，這個(gè)關(guān)系到MCU算法公式的書寫，實(shí)際上兩個(gè)公式的寫法是同一個(gè)公式變換來(lái)得，不同的是系數(shù)的差異。
資料上比較多的是：
?

?

還有一種的算法是：
?

?

這里主要介紹第二種，具體會(huì)分析比例、積分、微分三個(gè)環(huán)節(jié)的作用。

硬件部分：
控制系統(tǒng)的控制對(duì)象是4個(gè)空心杯直流電機(jī)，電機(jī)帶光電編碼器，可以反饋轉(zhuǎn)速大小的波形。電機(jī)驅(qū)動(dòng)模塊是普通的L298N模塊。
芯片型號(hào)，STM32F103ZET6
?

軟件部分：
PWM輸出：TIM3，可以直接輸出4路不通占空比的PWM波
PWM捕獲：STM32除了TIM6 TIM7其余的都有捕獲功能，使用TIM1 TIM2 TIM4 TIM5四個(gè)定時(shí)器捕獲四個(gè)反饋信號(hào)
PID的采樣和處理：使用了基本定時(shí)器TIM6，溢出時(shí)間就是我的采樣周期，理論上T越小效果會(huì)越好，這里我取20ms，依據(jù)控制對(duì)象吧，如果控制水溫什么的采樣周期會(huì)是幾秒幾分鐘什么的。

上面的PWM輸出和捕獲關(guān)于定時(shí)器的設(shè)置都有例程，我這里是這樣的：
TIM3輸出四路PWM，在引腳 C 的 GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9輸出
四路捕獲分別是TIM4??TIM1??TIM2??TIM5? ?，對(duì)應(yīng)引腳是：??PB7 PE11 PB3 PA1
高級(jí)定時(shí)器tim1的初始化略不同，它的中斷”名稱“和通用定時(shí)器不同。具體的內(nèi)容，請(qǐng)大家看一下我分享的代碼就明白了。
?

?程序.zip?



主要講解PID部分
準(zhǔn)備部分：先定義PID結(jié)構(gòu)體：

typedef struct 
{
int setpoint;//設(shè)定目標(biāo)
int sum_error;//誤差累計(jì)
float proportion ;//比例常數(shù)
float integral ;//積分常數(shù)
float derivative;//微分常數(shù)
int last_error;//e[-1]
int prev_error;//e[-2]
}PIDtypedef;

復(fù)制代碼

在文件中定義幾個(gè)關(guān)鍵變量：

float??Kp =?  ??0.32??; //比例常數(shù)
float??Ti =?  ?  ?  ?  ?   0.09 ; //積分時(shí)間常數(shù)
float Td =?  ?  ?  ?  ?   0.0028 ;??//微分時(shí)間常數(shù)
#define T?  ?  ?  ?  ?  ?  0.02 //采樣周期
#define Ki?  ??Kp*(T/Ti)?  ?  ??// Kp Ki Kd 三個(gè)主要參數(shù)
#define Kd?  ?  ?  ?  ?   Kp*(Td/T)

復(fù)制代碼

PID.H里面主要的幾個(gè)函數(shù)：

void PIDperiodinit(u16 arr,u16 psc);?  ?  ??//PID 采樣定時(shí)器設(shè)定
void incPIDinit(void);?  ?  ?  ?  ?   //初始化，參數(shù)清零清零
int incPIDcalc(PIDtypedef*PIDx,u16 nextpoint);?  ?  ?  ??//PID計(jì)算
void PID_setpoint(PIDtypedef*PIDx,u16 setvalue);??//設(shè)定 PID預(yù)期值
void PID_set(float pp,float ii,float dd);//設(shè)定PID??kp ki kd三個(gè)參數(shù)
void set_speed(float W1,float W2,float W3,float W4);//設(shè)定四個(gè)電機(jī)的目標(biāo)轉(zhuǎn)速

復(fù)制代碼

PID處理過(guò)程：
岔開一下：這里我控制的是電機(jī)的轉(zhuǎn)速w，實(shí)際上電機(jī)的反饋波形的頻率f、電機(jī)轉(zhuǎn)速w、控制信號(hào)PWM的占空比a三者是大致線性的正比的關(guān)系，這里強(qiáng)調(diào)這個(gè)的目的是
因?yàn)闃侵髟谇捌谝恢备悴欢铱刂频霓D(zhuǎn)速怎么和TIM4輸出的PWM的占空比聯(lián)系起來(lái)，后來(lái)想清楚里面的聯(lián)系之后通過(guò)公式把各個(gè)系數(shù)算出來(lái)了。

正題：控制流程是這樣的，首先我設(shè)定我需要的車速（對(duì)應(yīng)四個(gè)輪子的轉(zhuǎn)速），然后PID就是開始響應(yīng)了，它先采樣電機(jī)轉(zhuǎn)速，得到偏差值E，帶入PID計(jì)算公式，得到調(diào)整量也就是最終更改了PWM的占空比，不斷調(diào)節(jié)，直到轉(zhuǎn)速在穩(wěn)態(tài)的一個(gè)小范圍上下浮動(dòng)。
上面講到的“得到調(diào)整量”就是增量PID的公式：

int incPIDcalc(PIDtypedef *PIDx,u16 nextpoint)
{
int iError,iincpid;
iError=PIDx->setpoint-nextpoint;??//當(dāng)前誤差
/*iincpid=?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ??//增量計(jì)算
PIDx->proportion*iError?  ?  ?  ?  ?   //e[k]項(xiàng)
-PIDx->integral*PIDx->last_error?  ?  ?   //e[k-1]
+PIDx->derivative*PIDx->prev_error;//e[k-2]
*/
iincpid=?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?  ?   //增量計(jì)算
PIDx->proportion*(iError-PIDx->last_error)
+PIDx->integral*iError
+PIDx->derivative*(iError-2*PIDx->last_error+PIDx->prev_error);

PIDx->prev_error=PIDx->last_error; //存儲(chǔ)誤差，便于下次計(jì)算
PIDx->last_error=iError;
return(iincpid) ;
}

復(fù)制代碼

注釋掉的是第一種寫法，沒注釋的是第二種以Kp KI kd為系數(shù)的寫法，實(shí)際結(jié)果是一樣的。
處理過(guò)程放在了TIM6，溢出周期時(shí)間就是是PID里面采樣周期（區(qū)分于反饋信號(hào)的采樣，反饋信號(hào)采樣是1M的頻率）
相關(guān)代碼：

void TIM6_IRQHandler(void)?  ?  ??//?  ?  ??采樣時(shí)間到，中斷處理函數(shù)
{?  ?  ?   
   ?  ? 
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)//更新中斷
   ?  ??{
   ?  ??frequency1=1000000/period_TIM4?  ?  ??; //通過(guò)捕獲的波形的周期算出頻率
   ?  ??frequency2=1000000/period_TIM1?  ?  ??;
   ?  ??frequency3=1000000/period_TIM2?  ?  ??;
   ?  ??frequency4=1000000/period_TIM5?  ?  ??;
/********PID1處理**********/
   ?  ??PID1.sum_error+=(incPIDcalc(&PID1,frequency1));?  ?  ?  //計(jì)算增量并累加 
   ?   pwm1=PID1.sum_error*4.6875??;?  //pwm1 代表將要輸出PWM的占空比
   ?  ?   frequency1=0; //清零
   ??period_TIM4=0;
/********PID2處理**********/
   ?  ?  PID2.sum_error+=(incPIDcalc(&PID2,frequency2));?  ?  ?  //計(jì)算增量并累加??Y=Y+Y'?  ?  ?  ?  ?   
   ?  ?  pwm2=PID2.sum_error*4.6875 ;?  //將要輸出PWM的占空比 
   ?  ??frequency2=0;
   ?  ??period_TIM1=0;
/********PID3處理**********/
   ?  ?  PID3.sum_error+=(incPIDcalc(&PID3,frequency3));?  ?  ?   //常規(guī)PID控制
   ?  ??pwm3=PID3.sum_error*4.6875 ;?  //將要輸出PWM的占空比
   ?  ??frequency3=0;
   ?  ??period_TIM2=0;
/********PID4處理**********/
   ?  ?  ?  PID4.sum_error+=(incPIDcalc(&PID4,frequency4));?  ?  ?  //計(jì)算增量并累加
   ?  ?  pwm4=PID4.sum_error*4.6875 ;?  //將要輸出PWM的占空比 
   ?  ??frequency4=0;
   ?  ??period_TIM5=0; 
   ?  ?   }

TIM_SetCompare(pwm1,pwm2,pwm3,pwm4);?  ?  ?  ?   //重新設(shè)定PWM值

TIM_ClearITPendingBit(TIM6, TIM_IT_Update); //清除中斷標(biāo)志位?  ?  ?  ?  ?   
}

復(fù)制代碼

?

TIM_SetCompare （）函數(shù)：

?

上面幾個(gè)代碼是PID實(shí)現(xiàn)的關(guān)鍵部分

還有整定過(guò)程：
辦法有不少，這里用的是先KP，再TI，再TD，在微調(diào)。其他的辦法特別是有個(gè)尼古拉斯法我發(fā)現(xiàn)不適合我這個(gè)控制對(duì)象。
先Kp，就是消除積分和微分部分的影響，這里我糾結(jié)過(guò)到底是讓Ti 等于一個(gè)很大的值讓Ki=Kp*(T/Ti)里面的KI接近零，還是直接定義KI=0，TI=0.
然后發(fā)現(xiàn)前者沒法找到KP使系統(tǒng)震蕩的臨界值，第二個(gè)辦法可以得到預(yù)期的效果：即KP大了會(huì)產(chǎn)生震蕩，小了會(huì)讓系統(tǒng)穩(wěn)定下來(lái)，當(dāng)然這個(gè)時(shí)候是有穩(wěn)態(tài)誤差的。
隨后把積分部分加進(jìn)去，KI=Kp*(T/Ti)這個(gè)公式用起來(lái)，并且不斷調(diào)節(jié)TI 。TI太大系統(tǒng)穩(wěn)定時(shí)間比較長(zhǎng)。
然后加上Kd? ?? ???=Kp*(Td/T)，對(duì)于系統(tǒng)響應(yīng)比較滯后的情況效果好像好一些，我這里的電機(jī)反映挺快的，所以Td值很小。
最后就是幾個(gè)參數(shù)調(diào)節(jié)一下，讓波形好看一點(diǎn)。這里的波形實(shí)際反映的是采集回來(lái)的轉(zhuǎn)速值，用STM32的DAC功能輸出和轉(zhuǎn)速對(duì)應(yīng)的電壓，用示波器采集的。
最后的波形是這樣的：
?

?
?

?

PID控制算法的C語(yǔ)言實(shí)現(xiàn)一 PID算法原理

?? 最近兩天在考慮一般控制算法的C語(yǔ)言實(shí)現(xiàn)問(wèn)題，發(fā)現(xiàn)網(wǎng)絡(luò)上尚沒有一套完整的比較體系的講解。于是總結(jié)了幾天，整理一套思路分享給大家。

?? 在工業(yè)應(yīng)用中PID及其衍生算法是應(yīng)用最廣泛的算法之一，是當(dāng)之無(wú)愧的萬(wàn)能算法，如果能夠熟練掌握PID算法的設(shè)計(jì)與實(shí)現(xiàn)過(guò)程，對(duì)于一般的研發(fā)人員來(lái)講，應(yīng)該是足夠應(yīng)對(duì)一般研發(fā)問(wèn)題了，而難能可貴的是，在我所接觸的控制算法當(dāng)中，PID控制算法又是最簡(jiǎn)單，最能體現(xiàn)反饋思想的控制算法，可謂經(jīng)典中的經(jīng)典。經(jīng)典的未必是復(fù)雜的，經(jīng)典的東西常常是簡(jiǎn)單的，而且是最簡(jiǎn)單的，想想牛頓的力學(xué)三大定律吧，想想愛因斯坦的質(zhì)能方程吧，何等的簡(jiǎn)單！簡(jiǎn)單的不是原始的，簡(jiǎn)單的也不是落后的，簡(jiǎn)單到了美的程度。先看看PID算法的一般形式：

?? PID的流程簡(jiǎn)單到了不能再簡(jiǎn)單的程度，通過(guò)誤差信號(hào)控制被控量，而控制器本身就是比例、積分、微分三個(gè)環(huán)節(jié)的加和。這里我們規(guī)定（在t時(shí)刻）：

?? 1.輸入量為rin(t);

?? 2.輸出量為rout(t);

?? 3.偏差量為err(t)=rin(t)-rout(t);

?? pid的控制規(guī)律為

?? 理解一下這個(gè)公式，主要從下面幾個(gè)問(wèn)題著手，為了便于理解，把控制環(huán)境具體一下：

?? 1.規(guī)定這個(gè)流程是用來(lái)為直流電機(jī)調(diào)速的;

?? 2.輸入量rin(t)為電機(jī)轉(zhuǎn)速預(yù)定值;

?? 3.輸出量rout(t)為電機(jī)轉(zhuǎn)速實(shí)際值;

?? 4.執(zhí)行器為直流電機(jī);

?? 5.傳感器為光電碼盤，假設(shè)碼盤為10線;

?? 6.直流電機(jī)采用PWM調(diào)速 轉(zhuǎn)速用單位 轉(zhuǎn)/min 表示;

? 不難看出以下結(jié)論：

?? 1.輸入量rin（t）為電機(jī)轉(zhuǎn)速預(yù)定值（轉(zhuǎn)/min）;

?? 2. 輸出量rout(t)為電機(jī)轉(zhuǎn)速實(shí)際值（轉(zhuǎn)/min）;

?? 3.偏差量為預(yù)定值和實(shí)際值之差（轉(zhuǎn)/min）;

?? 那么以下幾個(gè)問(wèn)題需要弄清楚：

?? 1.通過(guò)PID環(huán)節(jié)之后的 U(k) 是什么值呢？

? ?2.通過(guò)調(diào)節(jié) PWM 的電壓占空比來(lái)調(diào)節(jié)電機(jī)的轉(zhuǎn)速。

?? 3.那么U(k)與控制電機(jī)的 PWM 之間存在怎樣的聯(lián)系呢？

?

看到有不少人問(wèn)到底如何讓UK值與PWM占空比值對(duì)應(yīng)，進(jìn)而實(shí)現(xiàn)占空比輸出和輸出控制電壓對(duì)應(yīng)。

（注意，我這里討論的前提是輸出控制的是電壓，不是PWM方波。PWM輸出后要經(jīng)過(guò)濾波整形再輸出控制。）

前提條件：

輸出電壓控制電壓范圍是0-10V。

給定、反饋、輸出電壓采樣輸入電壓范圍是0-5V（經(jīng)過(guò)運(yùn)放）。

使用單片機(jī)AD為10位AD芯片。

那么10位AD芯片電壓采集得到的數(shù)據(jù)范圍就是0-1024。

PWM為 8位可調(diào)占空比方波，0對(duì)應(yīng)輸出占空比為0的方波，255對(duì)應(yīng)輸出占空比100%的方波，127對(duì)應(yīng)輸出50%的方波。

比如當(dāng)前給定是2.5V，反饋電壓是1V。（KP,KI,KD等系數(shù)略，關(guān)于PID算法的整數(shù)實(shí)現(xiàn)我在前文中有論述如何實(shí)現(xiàn)）。

那么經(jīng)過(guò)AD采樣

1、給定2.5V對(duì)應(yīng)為 512

2、反饋1V對(duì)應(yīng)為 205

假定經(jīng)過(guò)PID計(jì)算得到的UK為400

也就意味著輸出電壓應(yīng)當(dāng)為（400*（UPWM峰值電壓））/1024

那么UK對(duì)應(yīng)的PWM占空比是多少呢？

我們知道，UK=1024對(duì)應(yīng)占空比為100，也就是PWM的占空比系數(shù)為255?？芍?，PWM系數(shù) = UK/4;

那么400就應(yīng)當(dāng)對(duì)應(yīng)系數(shù) 400/4=100。

也就是輸出電壓=400*10/1024=3.9V

同時(shí)，由于采樣精度以及PWM輸出占空比精度控制的問(wèn)題，將導(dǎo)致輸出電壓和期望值不是那么線性，所以，我在項(xiàng)目?jī)?nèi)加入了輸出電壓采樣的控制。

采樣AD輸入為0-5V，所以，對(duì)于輸出0-10V有一個(gè)縮小的比例。

輸出10V則采樣值對(duì)應(yīng)為255

輸出5V則采樣之對(duì)應(yīng)127

可知，3.9V對(duì)應(yīng)AD結(jié)果為97

采樣輸出電壓值，可以針對(duì)性的調(diào)整一下占空比輸出，從而得到誤差允許范圍內(nèi)的一個(gè)控制輸出電壓。