1.根據我控制演算法類文章中關於PID的理論的一些描述,同時也根據網路上一些其他的PID文章,以及自己最近一個專案的實踐後,總結了幾套基於C語言的PID演算法,由於網路中很少有人進行分享完整的PID演算法實現,我這裡分享下。
(1)標頭檔案,定義pid的結構體,類的概念,包含pid的屬性和方法
#ifndef __PID_H_ #define __PID_H_ #include <stdint.h> typedef struct _pid { int16_t set_value; // 給定值,rin(k) int16_t actual_value; // 實際值,反饋值,rout(k) int16_t err; // 偏差值,rin(k) - rout(k) int16_t err_last; // 上一次偏差值,rin(k - 1) - rout(k - 1) int16_t err_last_last; // 上一次上一次的偏差值,rin(k - 2) - rout(k - 2) float kp; // 比例係數 float ki; // 積分系數 float kd; // 微分系數 float uk; // pid公式運算結果值 float incremental_value; // 增量值 float integral_value; // 積分值 float umax; // uk的上限值,抗積分飽和用 float umin; // uk的下限值,抗積分飽和用 int16_t err_up_value; // 偏差上限值,積分分離用 int16_t ki_k; // 積分的再次乘機係數,積分分離用 float out_value; // float(*position_type)(struct _pid *ppid); // 位置型PID演算法,無積分分離、無抗積分飽和 float(*incremental_type)(struct _pid *ppid); // 增量型PID演算法 float(*integral_separation_type)(struct _pid *ppid); // 積分分離PID演算法 float(*int_sep_anti_sat_type)(struct _pid *ppid); // 積分分離 + 抗積分飽和PID演算法 }_pid_t; _pid_t *pid_create(void); extern _pid_t *pg_pid; #endif
(2).c檔案,包含標頭檔案中4個PID演算法的實現,包含位置型PID演算法、增量型PID演算法、積分分離PID演算法、積分分離+抗飽和PID演算法
#include <stdlib.h> #include <string.h> #include "pid.h" #include "FreeRTOS.h" #include "task.h" #include "queue.h" #include "portmacro.h" #include "semphr.h" #include "Debug.h" /************************************* * * Funciton Name : pid_position_type * Function :位置型PID演算法,無積分分離、無抗積分飽和 * * @author :why * @note : 積分分離:能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分 * : 抗積分飽和:能解決執行器到達極限值時,但uk還在增大,使得系統進入飽和,進入飽和區的時間越長,當系統出現反向偏差,解決這個反向偏差的時間就會越長,此時系統會像失控一樣。 * :進入抗積分飽和,當uk超過上限時,積分向只累加負偏差,當uk超過下限時,積分項只累加正偏差。 * × @param :pid * * * @return : uk *************************************/ static float pid_position_type_f(struct _pid *ppid) { ppid->err = ppid->set_value - ppid->actual_value; // 偏差 ppid->integral_value += ppid->err; // 積分累加 ppid->uk = ppid->kp * ppid->err + ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last); ppid->err_last = ppid->err; return (ppid->uk); } /************************************* * * Funciton Name : pid_incremental_type_f * Function :增量型PID演算法 * * @author :why * @note : 相較於位置型,因為與3個偏差相關,增強了系統穩定性 * × @param :pid * * * @return : uk + 增量值 *************************************/ static float pid_incremental_type_f(struct _pid *ppid) { ppid->err = ppid->set_value - ppid->actual_value; // 偏差 //ppid->integral_value += ppid->err; // 積分累加 ppid->incremental_value = ppid->kp * ( ppid->err - ppid->err_last) + ppid->ki * ppid->err + ppid->kd * (ppid->err - 2 * ppid->err_last + ppid->err_last_last); ppid->uk += ppid->incremental_value; ppid->err_last_last = ppid->err_last; ppid->err_last = ppid->err; return (ppid->uk); } /************************************* * * Funciton Name : pid_integral_separation_type_f * Function :積分分離PID演算法 * * @author :why * @note : 能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分 * × @param :pid * * * @return : uk *************************************/ static float pid_integral_separation_type_f(struct _pid *ppid) { ppid->err = ppid->set_value - ppid->actual_value; // 偏差 // 誤差過大去除積分效果 if ( abs(ppid->err) > ppid->err_up_value ) { ppid->ki_k = 0; } else { ppid->ki_k = 1; ppid->integral_value += ppid->err; // 積分累加 } ppid->uk = ppid->kp * ppid->err + ppid->ki_k * ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last); ppid->err_last = ppid->err; return (ppid->uk); } /************************************* * * Funciton Name : pid_int_sep_anti_sat_type_f * Function :積分分離 + 抗積分飽和PID演算法 * * @author :why * @note : 能解決初期系統偏差值大,累加到積分項中,引起系統超調、振盪問題。積分是為了消除靜態誤差的,應在誤差在小範圍時引入積分 * : 抗積分飽和:能解決執行器到達極限值時,但uk還在增大,使得系統進入飽和,進入飽和區的時間越長,當系統出現反向偏差,解決這個反向偏差的時間就會越長,此時系統會像失控一樣。 * :進入抗積分飽和,當uk超過上限時,積分向只累加負偏差,當uk超過下限時,積分項只累加正偏差。 * × @param :pid * * * @return : uk *************************************/ static float pid_int_sep_anti_sat_type_f(struct _pid *ppid) { ppid->err = ppid->set_value - ppid->actual_value; // 偏差 Debug("ppid->err = %d\r\n", ppid->err); if ( ppid->out_value > ppid->umax ) // 抗積分飽和 { // 誤差過大去除積分效果 if ( abs(ppid->err) > ppid->err_up_value ) // 積分分離 { ppid->ki_k = 0; } else { ppid->ki_k = 1; if ( ppid->err < 0 ) { ppid->integral_value += ppid->err; // 積分累加 } } } else if ( ppid->out_value < ppid->umin ) // 抗積分飽和 { // 誤差過大去除積分效果 if ( abs(ppid->err) > ppid->err_up_value ) // 積分分離 { ppid->ki_k = 0; } else { ppid->ki_k = 1; if ( ppid->err > 0 ) { ppid->integral_value += ppid->err; // 積分累加 } } } else { // 誤差過大去除積分效果 if ( abs(ppid->err) > ppid->err_up_value ) { ppid->ki_k = 0; } else { ppid->ki_k = 1; ppid->integral_value += ppid->err; // 積分累加 } } ppid->uk = ppid->kp * ppid->err + ppid->ki_k * ppid->ki * ppid->integral_value + ppid->kd * (ppid->err - ppid->err_last); ppid->err_last = ppid->err; // if ( ppid->uk >= 0.07 && ppid->uk <= 1 ) // { // ppid->uk = 1; // } // // if ( ppid->uk <= -0.07 && ppid->uk >= -1 ) // { // ppid->uk = -1; // } return (ppid->uk); } /************************************* * * Funciton Name : pid_init * Function : pid例項構造 * * @author :why * * *************************************/ static void pid_init(_pid_t *ppid) { ppid->kp = 0.2; ppid->ki = 0.05; ppid->kd = 0; ppid->umax = 4000; ppid->umin = 0; ppid->position_type = pid_position_type_f; ppid->incremental_type = pid_incremental_type_f; ppid->integral_separation_type = pid_integral_separation_type_f; ppid->int_sep_anti_sat_type = pid_int_sep_anti_sat_type_f; } /************************************* * * Funciton Name : pid_create * Function : pid例項建立 * @author : why * * @return : 返回pid例項 * *************************************/ _pid_t *pid_create(void) { _pid_t *ppid = (_pid_t *)pvPortMalloc(sizeof(_pid_t)); memset(ppid, 0, sizeof(_pid_t)); pid_init(ppid); return ppid; }
(3)PID的實現演算法有了,但還是要根據實際情況進行除錯選取最適合的PID演算法以及修改可能存在的不恰當的位置。