1. 位置式 PID（用于位置環(huán)）
測量位置就是通過 stm32 去采集編碼器的脈沖數(shù)據(jù)，通過脈沖計算出位置（角度）。目標位置和測量位置之間做差這個就是目前系統(tǒng)的偏差。送入 PID 控制器進行計算輸出，然后再經(jīng)過電機驅(qū)動的功率放大控制電機的轉(zhuǎn)動去減小偏差， 最終達到目標位置的過程。
（1）公式
（2）代碼
pid.c
typedef struct PID {
float Kp; // Proportional Const P系數(shù)
float Ki; // Integral Const I系數(shù)
float Kd; // Derivative Const D系數(shù)
float PrevError ; // Error[-2]
float LastError; // Error[-1]
float Error; // Error[0 ]
float DError; //pid->Error - pid->LastError
float SumError; // Sums of Errors
float output;
float Integralmax; //積分項的最大值
float outputmax; //輸出項的最大值
} PID;
//為了防止積分項過度累積，引入積分項的限幅是一種常見的做法。
//限制積分項的幅值可以防止積分項過度增加，從而限制了系統(tǒng)的累積誤差。這樣可以避免系統(tǒng)過度響應或者不穩(wěn)定。
float abs_limit(float value, float ABS_MAX) //積分限幅，設置最大值。
{
if(value > ABS_MAX)
value = ABS_MAX;
if(value< -ABS_MAX)
value = -ABS_MAX;
return value;
}
//函數(shù)里傳入指針，修改時會修改指針里的值。
float PID_Position_Calc(PID *pid, float Target_val, float Actual_val) //位置式PID
{
pid->Error = Target_val - Actual_val; //與pid P系數(shù)相乘。比例誤差值 當前差值=目標值-實際值
pid->SumError += pid->Error; //與pid I系數(shù)相乘。穩(wěn)態(tài)誤差值 誤差相加作為誤差總和，給積分項
pid->DError = pid->Error - pid->LastError; //與pid D系數(shù)相乘。 微分項-消除震蕩
pid->output = pid->Kp* pid->Error +
abs_limit( pid->Ki* pid->SumError, pid->Integralmax ) +
pid->Kd* pid->DError ;
pid->LastError = pid->Error; //更新誤差
//限制輸出最大值，防止出現(xiàn)突發(fā)意外。輸出outputmax的最大值
if(pid->output > pid->outputmax ) pid->output = pid->outputmax;
if(pid->output < - pid->outputmax ) pid->output = -pid->outputmax;
return pid->output ; //輸出為pwm值
}
//PID初始化
void PID_Init(PID *pid, float Kp , float Ki , float Kd , float Limit_value)
{
pid->Kp= Kp;
pid->Ki= Ki;
pid->Kd= Kd;
pid->PrevError =pid->LastError = pid->Error =pid->SumError= pid->output = 0;
pid->Integralmax = pid->outputmax = Limit_value;
} （3）使用代碼
#include "sys.h"
PID postion_pid;
float Encoder_Speed =0;
float Position =0;
float Speed=0;
float Target_val =500;
int main()
{
Time2_Init(10000-1,7200-1); //定時器2用于定時 10000*7200/72 = 1s
Encoder_Init(); //定時器4的編碼器
Motor_PWM_Init(7200-1,0); //定時器1，初始化pwm輸出
PID_Init(&postion_pid, 1.0, 0, 1.0, 7000);
while(1)
{
}
}
//---- 獲得電機的脈沖
int16_t Encoder_Get(void)
{
int16_t Temp;
Temp = TIM_GetCounter(TIM4); //獲取編碼器當前值
TIM_SetCounter(TIM4, 0); //將編碼器計數(shù)器清0
return Temp;
}
//設置pwm
void Set_Pwm(int motor_pwm)
{
TIM_SetCompare4(TIM1, motor_pwm);
}
void MotorControl(void)
{
Encoder_Speed = Encoder_Get();//1.獲取電機1s的脈沖數(shù)。即1s獲取的脈沖數(shù)。即速度
Position +=Encoder_Speed ; //累計實際脈沖數(shù)。與時間無關。即總路程
Speed=PID_Position_Calc(&postion_pid, Target_val , Position);//2.輸入增量式PID計算
Set_Pwm(Speed); //3.PWM輸出給電機
//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1); /*4.給上位機通道2發(fā)送實際的電機速度值*/
}
void TIM2_IRQHandler(void) //定時器中斷函數(shù)，1s進一次中斷
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
{
MotorControl();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}
}2. 增量式 PID（用于速度環(huán)）
增量式 PID 也稱速度環(huán) PID，速度閉環(huán)控制就是根據(jù)單位時間獲取的脈沖數(shù)測量電機的速度信息，并與目標值進行比較，得到控制偏差，然后通過對偏差的比例、積分、微分進行控制，使偏差趨向于零的過程。
（1）公式
（2）代碼
typedef struct PID {
float Kp; // Proportional Const P系數(shù)
float Ki; // Integral Const I系數(shù)
float Kd; // Derivative Const D系數(shù)
float PrevError ; // Error[-2]
float LastError; // Error[-1]
float Error; // Error[0 ]
float DError; //pid->Error - pid->LastError
float SumError; // Sums of Errors
float output;
float Integralmax; //積分項的最大值
float outputmax; //輸出項的最大值
} PID;
float PID_Incremental_Calc(PID *pid, float Target_val, float Actual_val)
{
pid->Error = Target_val- Actual_val;
pid->output += pid->Kp* ( pid->Error - pid->LastError )+
pid->Ki* pid->Error +
pid->Kd* ( pid->Error + pid->PrevError - 2*pid->LastError);
pid->PrevError = pid->LastError;
pid->LastError = pid->Error;
if(pid->output > pid->outputmax ) pid->output = pid->outputmax;
if(pid->output < - pid->outputmax ) pid->output = -pid->outputmax;
return pid->output ; //輸出為pwm值
}
//PID初始化
void PID_Init(PID *pid, float Kp , float Ki , float Kd , float Limit_value)
{
pid->Kp= Kp;
pid->Ki= Ki;
pid->Kd= Kd;
pid->PrevError =pid->LastError = pid->Error =pid->SumError= pid->output = 0;
pid->Integralmax = pid->outputmax = Limit_value;
} （3）使用代碼
#include "sys.h"
PID speedpid;
float Encoder_Speed =0;
float Target_val =500; //目標1s的脈沖數(shù)
float Speed=0;//實際速度
int main()
{
Time2_Init(10000-1,7200-1); //定時器2用于定時 10000*7200/72 = 1s
Encoder_Init(); //定時器4的編碼器
Motor_PWM_Init(7200-1,0); //定時器1，初始化pwm輸出
PID_Init(&speedpid, 1.0, 0, 1.0, 7000);
while(1)
{
}
}
//獲得電機的脈沖
int16_t Encoder_Get(void)
{
int16_t Temp;
Temp = TIM_GetCounter(TIM4); //獲取編碼器當前值
TIM_SetCounter(TIM4, 0); //將編碼器計數(shù)器清0
return Temp;
}
//設置pwm
void Set_Pwm(int motor_pwm)
{
TIM_SetCompare4(TIM1, motor_pwm);
}
void MotorControl(void)
{
Encoder_Speed = Encoder_Get();//1.獲取電機1s的脈沖數(shù)。即1s獲取的脈沖數(shù)。即速度。
Speed=PID_Incremental_Calc(&speedpid,Target_val ,Encoder_Speed);//2.輸入增量式PID計算
Set_Pwm(Speed); //3.PWM輸出給電機
//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1); /*4.給上位機通道2發(fā)送實際的電機速度值*/
}
void TIM2_IRQHandler(void) //定時器中斷函數(shù)，1s進一次中斷
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
{
MotorControl();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}
}3. 串級 PID
（1）位置環(huán)–速度環(huán)（用于控制電機）
利用位置式 pid 的方法將位置環(huán)和速度環(huán)組合在一起使用。位置環(huán)的輸出作為速度環(huán)的輸入。位置環(huán)的輸出作為速度環(huán)的目標期望值。這意味著位置環(huán)的輸出被視為速度環(huán)應該追蹤的目標位置。速度環(huán)的任務是根據(jù)當前位置和目標位置之間的偏差來生成控制輸出，使系統(tǒng)盡可能快地接近目標位置。速度環(huán)將根據(jù)當前速度和目標速度之間的差異來調(diào)整電機的輸出，以便使實際速度接近目標速度。
簡易代碼
將目標位置和實際位置傳入位置環(huán) PID 中，計算出期望轉(zhuǎn)速。然后通過期望轉(zhuǎn)速與實際轉(zhuǎn)速傳入速度環(huán) PID 中計算出對應的 pwm，然后通過 pwm 去控制電機。
#include "stdio.h"
PID postion_pid;
PID speed_pid;
float Encoder_Speed =0;
float Target_val =500; //目標總的脈沖數(shù)
float Speed=0;//實際速度
float Position =0;
int main(void)
{
Time2_Init(10000-1,7200-1); //定時器2用于定時 10000*7200/72 = 1s，如果覺得時間太長可以縮短一些
Encoder_Init(); //定時器4的編碼器
Motor_PWM_Init(7200-1,0); //定時器1，初始化pwm輸出
// 初始化PID控制器
PID_Init(&postion_pid, 1.0, 0.1, 0.01, 300); // PID參數(shù)根據(jù)實際情況調(diào)整
PID_Init(&speed_pid, 1.0, 0.1, 0.01, 300); // PID參數(shù)根據(jù)實際情況調(diào)整
while (1)
{
}
}
//獲得電機的脈沖
int16_t Encoder_Get(void)
{
int16_t Temp;
Temp = TIM_GetCounter(TIM4); //獲取編碼器當前值
TIM_SetCounter(TIM4, 0); //將編碼器計數(shù)器清0
return Temp;
}
//設置pwm
void Set_Pwm(int motor_pwm)
{
TIM_SetCompare4(TIM1, motor_pwm);
}
void MotorControl(void)
{
Encoder_Speed = Encoder_Get(); //1.獲取電機1s的脈沖數(shù)。即1s獲取的脈沖數(shù)。即速度
Position +=Encoder_Speed ; //累計實際脈沖數(shù)。與時間無關。即總路程
Speed=PID_Position_Calc(&postion_pid, Target_val , Position);//2.輸入位置式PID計算
Speed=PID_Incremental_Calc(&speedpid,Speed, Encoder_Speed);//2.輸入增量式PID計算
Set_Pwm(Speed); //3.PWM輸出給電機
//set_computer_value(SEND_FACT_CMD, CURVES_CH2, &Encoder_Speed, 1); /*4.給上位機通道2發(fā)送實際的電機速度值*/
}
void TIM2_IRQHandler(void) //定時器中斷函數(shù)，1s進一次中斷
{
if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)
{
MotorControl();
TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
}
}（2）位置環(huán)–位置環(huán)（用于控制舵機）
因為舵機沒有編碼器，無法獲取實際速度，所以我們可以使用兩個位置環(huán)來進行串級 pid 的使用，這樣更加精準。兩個位置環(huán)的實際值輸入都為距離值。第一個位置環(huán)的輸出作為第二個位置環(huán)的目標值輸入。
實際舉例：假設我們使用舵機來進行目標追蹤。則第一個位置環(huán)的實際值輸入：當前坐標-上次坐標的差值，目標值為 0。將這兩個值傳入位置環(huán)計算的輸出作為第二個位置環(huán)的目標值，第二個位置環(huán)的實際值可以傳入：當前位置和攝像頭中心點位置的差值。計算第二個位置環(huán)的輸出。將其作為 pwm 值輸入定時器通道去控制舵機。