如何在STM32上實(shí)現(xiàn)增量PID控制

ID:363309 · 發(fā)表于 2018-7-2 16:36

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

還有一種的算法是：

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

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

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

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

程序.zip (2.51 KB, 下載次數(shù): 122)

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

正題：控制流程是這樣的，首先我設(shè)定我需要的車速（對應(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è)小范圍上下浮動。
上面講到的“得到調(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; //存儲誤差，便于下次計(jì)算
PIDx->last_error=iError;
return(iincpid) ;
}

復(fù)制代碼

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

void TIM6_IRQHandler(void) // 采樣時(shí)間到，中斷處理函數(shù)
{
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)//更新中斷
{
frequency1=1000000/period_TIM4 ; //通過捕獲的波形的周期算出頻率
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ù)制代碼

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

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